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

depth/rgb common callback: using camera header for timestamp and frame

Browse Source
This commit is contained in:
matlabbe
2017-04-18 21:54:26 -04:00
parent 7540ed3309
commit a92fac1ea3
1 changed files with 5 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+5 -5
View File
@@ -1142,26 +1142,26 @@ bool convertRGBDMsgs(
depthHeight, depthHeight,
depthMsgs[i]->image.rows).c_str()); depthMsgs[i]->image.rows).c_str());
rtabmap::Transform localTransform = rtabmap_ros::getTransform(frameId, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp, listener, waitForTransform); rtabmap::Transform localTransform = rtabmap_ros::getTransform(frameId, cameraInfoMsgs[i].header.frame_id, cameraInfoMsgs[i].header.stamp, listener, waitForTransform);
if(localTransform.isNull()) if(localTransform.isNull())
{ {
ROS_ERROR("TF of received depth image %d at time %fs is not set!", i, depthMsgs[i]->header.stamp.toSec()); ROS_ERROR("TF of received depth image %d at time %fs is not set!", i, cameraInfoMsgs[i].header.stamp.toSec());
return false; return false;
} }
// sync with odometry stamp // sync with odometry stamp
if(!odomFrameId.empty() && odomStamp != depthMsgs[i]->header.stamp) if(!odomFrameId.empty() && odomStamp != cameraInfoMsgs[i].header.stamp)
{ {
rtabmap::Transform sensorT = getTransform( rtabmap::Transform sensorT = getTransform(
frameId, frameId,
odomFrameId, odomFrameId,
odomStamp, odomStamp,
depthMsgs[i]->header.stamp, cameraInfoMsgs[i].header.stamp,
listener, listener,
waitForTransform); waitForTransform);
if(sensorT.isNull()) if(sensorT.isNull())
{ {
ROS_WARN("Could not get odometry value for depth image stamp (%fs). Latest odometry " ROS_WARN("Could not get odometry value for depth image stamp (%fs). Latest odometry "
"stamp is %fs. The depth image pose will not be synchronized with odometry.", depthMsgs[i]->header.stamp.toSec(), odomStamp.toSec()); "stamp is %fs. The depth image pose will not be synchronized with odometry.", cameraInfoMsgs[i].header.stamp.toSec(), odomStamp.toSec());
} }
else else
{ {