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

Added tf2_ros dependency

Browse Source
This commit is contained in:
Mathieu Labbe
2015-05-14 00:42:21 -04:00
parent 2b46da2773
commit 550655c887
12 changed files with 104 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/OdometryROS.cpp
+10 -7
View File
@@ -316,12 +316,15 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
//*********************
// Update odometry
//*********************
tf::Transform poseTF;
rtabmap_ros::transformToTF(pose, poseTF);
geometry_msgs::TransformStamped poseMsg;
poseMsg.child_frame_id = frameId_;
poseMsg.header.frame_id = odomFrameId_;
poseMsg.header.stamp = header.stamp;
rtabmap_ros::transformToGeometryMsg(pose, poseMsg.transform);
if(publishTf_)
{
tfBroadcaster_.sendTransform( tf::StampedTransform (poseTF, header.stamp, odomFrameId_, frameId_));
tfBroadcaster_.sendTransform(poseMsg);
}
if(odomPub_.getNumSubscribers())
@@ -333,10 +336,10 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
odom.child_frame_id = frameId_;
//set the position
odom.pose.pose.position.x = poseTF.getOrigin().x();
odom.pose.pose.position.y = poseTF.getOrigin().y();
odom.pose.pose.position.z = poseTF.getOrigin().z();
tf::quaternionTFToMsg(poseTF.getRotation().normalized(), odom.pose.pose.orientation);
odom.pose.pose.position.x = poseMsg.transform.translation.x;
odom.pose.pose.position.y = poseMsg.transform.translation.y;
odom.pose.pose.position.z = poseMsg.transform.translation.z;
odom.pose.pose.orientation = poseMsg.transform.rotation;
//set covariance
odom.pose.covariance.at(0) = info.variance; // xx