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

using latest stamp between input image topics for visual odometry (so less " Lookup would require extrapolation into the future." warnings show up)

Browse Source
This commit is contained in:
matlabbe
2015-07-17 17:29:10 -04:00
parent eddf7a757d
commit 135bdb1954
4 changed files with 39 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/StereoOdometryNode.cpp
+6 -4
View File
@@ -132,19 +132,21 @@ public:
return;
}
ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp;
tf::StampedTransform localTransform;
try
{
if(this->waitForTransform())
{
if(!this->tfListener().waitForTransform(this->frameId(), imageRectLeft->header.frame_id, imageRectLeft->header.stamp, ros::Duration(1)))
if(!this->tfListener().waitForTransform(this->frameId(), imageRectLeft->header.frame_id, stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", this->frameId().c_str(), imageRectLeft->header.frame_id.c_str());
return;
}
}
this->tfListener().lookupTransform(this->frameId(), imageRectLeft->header.frame_id, imageRectLeft->header.stamp, localTransform);
this->tfListener().lookupTransform(this->frameId(), imageRectLeft->header.frame_id, stamp, localTransform);
}
catch(tf::TransformException & ex)
{
@@ -185,9 +187,9 @@ public:
ptrImageRight->image,
stereoModel,
0,
rtabmap_ros::timestampFromROS(imageRectLeft->header.stamp));
rtabmap_ros::timestampFromROS(stamp));
this->processData(data, imageRectLeft->header);
this->processData(data, stamp);
}
else
{