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

Merge branch 'master' of github.com:introlab/rtabmap_ros into 0.11.0

Browse Source 
Conflicts:
	src/CoreWrapper.cpp
	src/CoreWrapper.h
This commit is contained in:
matlabbe
2016-03-09 12:22:27 -05:00
parent 6b5ccf99fb 26c656776c
commit d712efe455
12 changed files with 126 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/OdometryROS.cpp
+17
View File
@@ -366,6 +366,21 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
odom.pose.covariance.at(28) = info.variance; // pp
odom.pose.covariance.at(35) = info.variance; // yawyaw
//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;
}
previousStamp_ = stamp;
//publish the message
odomPub_.publish(odom);
}
@@ -465,6 +480,7 @@ bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
ROS_INFO("visual_odometry: reset odom!");
odometry_->reset();
previousStamp_ = ros::Time();
return true;
}
@@ -473,6 +489,7 @@ bool OdometryROS::resetToPose(rtabmap_ros::ResetPose::Request& req, rtabmap_ros:
Transform pose(req.x, req.y, req.z, req.roll, req.pitch, req.yaw);
ROS_INFO("visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str());
odometry_->reset(pose);
previousStamp_ = ros::Time();
return true;
}