i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-05 17:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Updated against latest changes from 0.11. Added localMap features to OdomInfo msg. Updated rgbdslam_datasets.launch.

Browse Source
This commit is contained in:
matlabbe
2016-03-09 17:31:57 -05:00
parent d712efe455
commit a4dfe5a21e
8 changed files with 82 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/OdometryROS.cpp
+7 -8
View File
@@ -369,15 +369,14 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
//set velocity
if(previousStamp_.isValid())
{
float dt = 1.0f/(stamp - previousStamp_).toSec();
float x,y,z,roll,pitch,yaw;
odometry_->previousTransform().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
odom.twist.twist.linear.x = x*dt;
odom.twist.twist.linear.y = y*dt;
odom.twist.twist.linear.z = z*dt;
odom.twist.twist.angular.x = roll*dt;
odom.twist.twist.angular.y = pitch*dt;
odom.twist.twist.angular.z = yaw*dt;
odometry_->previousVelocityTransform().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
odom.twist.twist.linear.x = x;
odom.twist.twist.linear.y = y;
odom.twist.twist.linear.z = z;
odom.twist.twist.angular.x = roll;
odom.twist.twist.angular.y = pitch;
odom.twist.twist.angular.z = yaw;
}
previousStamp_ = stamp;