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

rtabmap: Added warnings when scan/depth cannot be synchronized to odom TF, instead of aborting update.

Browse Source
This commit is contained in:
matlabbe
2016-02-02 12:22:23 -05:00
parent edd107f889
commit 622d15ea6a
1 changed files with 15 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+12 -2
View File
@@ -778,6 +778,7 @@ void CoreWrapper::commonDepthCallback(
Transform localTransform = getTransform(frameId_, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp); Transform localTransform = getTransform(frameId_, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp);
if(localTransform.isNull()) if(localTransform.isNull())
{ {
ROS_ERROR("TF of received depth image %d at time %fs is not set, aborting rtabmap update.", i, depthMsgs[i]->header.stamp.toSec());
return; return;
} }
// sync with odometry stamp // sync with odometry stamp
@@ -788,11 +789,15 @@ void CoreWrapper::commonDepthCallback(
Transform sensorT = getTransform(odomFrameId, frameId_, depthMsgs[i]->header.stamp); Transform sensorT = getTransform(odomFrameId, frameId_, depthMsgs[i]->header.stamp);
if(sensorT.isNull()) if(sensorT.isNull())
{ {
return; ROS_WARN("Could not get odometry value for depth image %d stamp (%fs). Latest odometry "
"stamp is %fs. The depth image pose will not be synchronized with odometry.", i, depthMsgs[i]->header.stamp.toSec(), lastPoseStamp_.toSec());
} }
else
{
localTransform = odomT.inverse() * sensorT * localTransform; localTransform = odomT.inverse() * sensorT * localTransform;
} }
} }
}
cv_bridge::CvImageConstPtr ptrImage; cv_bridge::CvImageConstPtr ptrImage;
if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0) if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
@@ -884,6 +889,7 @@ void CoreWrapper::commonDepthCallback(
// make sure the frame of the laser is updated too // make sure the frame of the laser is updated too
if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull()) if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull())
{ {
ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting rtabmap update.", scanMsg->header.stamp.toSec());
return; return;
} }
@@ -902,10 +908,14 @@ void CoreWrapper::commonDepthCallback(
Transform sensorT = getTransform(odomFrameId, frameId_, scanMsg->header.stamp); Transform sensorT = getTransform(odomFrameId, frameId_, scanMsg->header.stamp);
if(sensorT.isNull()) if(sensorT.isNull())
{ {
return; ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry "
"stamp is %fs. The laser scan pose will not be synchronized with odometry.", scanMsg->header.stamp.toSec(), lastPoseStamp_.toSec());
} }
else
{
Transform t = odomT.inverse() * sensorT; Transform t = odomT.inverse() * sensorT;
pclScan = util3d::transformPointCloud(pclScan, t); pclScan = util3d::transformPointCloud(pclScan, t);
}
} }
} }