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

Added "wait_for_transform_duration" (default 0.1 s) parameter to rtabmap, rtabmapviz and odometry nodes

Browse Source
This commit is contained in:
matlabbe
2015-08-14 15:01:51 -04:00
parent cc5203127f
commit 0dd1d5721b
11 changed files with 105 additions and 131 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/RGBDOdometryNode.cpp
+6 -32
View File
@@ -190,22 +190,9 @@ public:
ros::Time stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
tf::StampedTransform localTransform;
try
Transform localTransform = getTransform(this->frameId(), image->header.frame_id, stamp);
if(localTransform.isNull())
{
if(this->waitForTransform())
{
if(!this->tfListener().waitForTransform(this->frameId(), image->header.frame_id, stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", this->frameId().c_str(), image->header.frame_id.c_str());
return;
}
}
this->tfListener().lookupTransform(this->frameId(), image->header.frame_id, stamp, localTransform);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
@@ -218,7 +205,7 @@ public:
model.fy(),
model.cx(),
model.cy(),
rtabmap_ros::transformFromTF(localTransform));
localTransform);
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depth);
@@ -290,22 +277,9 @@ public:
higherStamp = stamp;
}
tf::StampedTransform localTransform;
try
Transform localTransform = getTransform(this->frameId(), imageMsgs[i]->header.frame_id, stamp);
if(localTransform.isNull())
{
if(this->waitForTransform())
{
if(!this->tfListener().waitForTransform(this->frameId(), imageMsgs[i]->header.frame_id, stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", this->frameId().c_str(), imageMsgs[i]->header.frame_id.c_str());
return;
}
}
this->tfListener().lookupTransform(this->frameId(), imageMsgs[i]->header.frame_id, stamp, localTransform);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
@@ -374,7 +348,7 @@ public:
model.fy(),
model.cx(),
model.cy(),
rtabmap_ros::transformFromTF(localTransform)));
localTransform));
}
rtabmap::SensorData data(