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

data_player: fixed published TF stamp synchronization

Browse Source
This commit is contained in:
matlabbe
2015-10-28 15:56:30 -04:00
parent 470958c28e
commit fc9c3aa162
1 changed files with 3 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/DbPlayerNode.cpp
+3 -5
View File
@@ -257,8 +257,6 @@ int main(int argc, char** argv)
// publish transforms first // publish transforms first
if(publishTf) if(publishTf)
{ {
ros::Time tfExpiration = time + ros::Duration(rate>0?1.0/rate:acquisitionTime);
rtabmap::Transform localTransform; rtabmap::Transform localTransform;
if(odom.data().cameraModels().size() == 1) if(odom.data().cameraModels().size() == 1)
{ {
@@ -273,7 +271,7 @@ int main(int argc, char** argv)
geometry_msgs::TransformStamped baseToCamera; geometry_msgs::TransformStamped baseToCamera;
baseToCamera.child_frame_id = cameraFrameId; baseToCamera.child_frame_id = cameraFrameId;
baseToCamera.header.frame_id = frameId; baseToCamera.header.frame_id = frameId;
baseToCamera.header.stamp = tfExpiration; baseToCamera.header.stamp = time;
rtabmap_ros::transformToGeometryMsg(localTransform, baseToCamera.transform); rtabmap_ros::transformToGeometryMsg(localTransform, baseToCamera.transform);
tfBroadcaster.sendTransform(baseToCamera); tfBroadcaster.sendTransform(baseToCamera);
} }
@@ -283,7 +281,7 @@ int main(int argc, char** argv)
geometry_msgs::TransformStamped odomToBase; geometry_msgs::TransformStamped odomToBase;
odomToBase.child_frame_id = frameId; odomToBase.child_frame_id = frameId;
odomToBase.header.frame_id = odomFrameId; odomToBase.header.frame_id = odomFrameId;
odomToBase.header.stamp = tfExpiration; odomToBase.header.stamp = time;
rtabmap_ros::transformToGeometryMsg(odom.pose(), odomToBase.transform); rtabmap_ros::transformToGeometryMsg(odom.pose(), odomToBase.transform);
tfBroadcaster.sendTransform(odomToBase); tfBroadcaster.sendTransform(odomToBase);
} }
@@ -293,7 +291,7 @@ int main(int argc, char** argv)
geometry_msgs::TransformStamped baseToLaserScan; geometry_msgs::TransformStamped baseToLaserScan;
baseToLaserScan.child_frame_id = scanFrameId; baseToLaserScan.child_frame_id = scanFrameId;
baseToLaserScan.header.frame_id = frameId; baseToLaserScan.header.frame_id = frameId;
baseToLaserScan.header.stamp = tfExpiration; baseToLaserScan.header.stamp = time;
rtabmap_ros::transformToGeometryMsg(rtabmap::Transform(0,0,scanHeight,0,0,0), baseToLaserScan.transform); rtabmap_ros::transformToGeometryMsg(rtabmap::Transform(0,0,scanHeight,0,0,0), baseToLaserScan.transform);
tfBroadcaster.sendTransform(baseToLaserScan); tfBroadcaster.sendTransform(baseToLaserScan);
} }