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

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/OdometryROS.cpp
+10 -10
View File
@@ -302,7 +302,7 @@ void OdometryROS::processArguments(int argc, char * argv[])
}
}
void OdometryROS::processData(const SensorData & data, const std_msgs::Header & header)
void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
{
if(odometry_->getPose().isNull() &&
!groundTruthFrameId_.empty())
@@ -312,13 +312,13 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
{
if(this->waitForTransform())
{
if(!this->tfListener().waitForTransform(groundTruthFrameId_, frameId_, header.stamp, ros::Duration(1)))
if(!this->tfListener().waitForTransform(groundTruthFrameId_, frameId_, stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", groundTruthFrameId_.c_str(), frameId_.c_str());
return;
}
}
this->tfListener().lookupTransform(groundTruthFrameId_, frameId_, header.stamp, initialPose);
this->tfListener().lookupTransform(groundTruthFrameId_, frameId_, stamp, initialPose);
}
catch(tf::TransformException & ex)
{
@@ -345,7 +345,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
geometry_msgs::TransformStamped poseMsg;
poseMsg.child_frame_id = frameId_;
poseMsg.header.frame_id = odomFrameId_;
poseMsg.header.stamp = header.stamp;
poseMsg.header.stamp = stamp;
rtabmap_ros::transformToGeometryMsg(pose, poseMsg.transform);
if(publishTf_)
@@ -357,7 +357,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
{
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = header.stamp; // use corresponding time stamp to image
odom.header.stamp = stamp; // use corresponding time stamp to image
odom.header.frame_id = odomFrameId_;
odom.child_frame_id = frameId_;
@@ -389,7 +389,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
}
sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(cloud, cloudMsg);
cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_;
odomLocalMap_.publish(cloudMsg);
}
@@ -412,7 +412,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(cloud, cloudMsg);
cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_;
odomLastFrame_.publish(cloudMsg);
}
@@ -427,7 +427,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
cloudTransformed = util3d::transformPointCloud(cloud, pose);
sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(*cloudTransformed, cloudMsg);
cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_;
odomLastFrame_.publish(cloudMsg);
}
@@ -440,7 +440,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
//send null pose to notify that odometry is lost
nav_msgs::Odometry odom;
odom.header.stamp = header.stamp; // use corresponding time stamp to image
odom.header.stamp = stamp; // use corresponding time stamp to image
odom.header.frame_id = odomFrameId_;
odom.child_frame_id = frameId_;
@@ -452,7 +452,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
{
rtabmap_ros::OdomInfo infoMsg;
odomInfoToROS(info, infoMsg);
infoMsg.header.stamp = header.stamp; // use corresponding time stamp to image
infoMsg.header.stamp = stamp; // use corresponding time stamp to image
infoMsg.header.frame_id = odomFrameId_;
odomInfoPub_.publish(infoMsg);
}
src/OdometryROS.h
+1 -1
View File
@@ -53,7 +53,7 @@ public:
~OdometryROS();
void processArguments(int argc, char * argv[]);
void processData(const rtabmap::SensorData & data, const std_msgs::Header & header);
void processData(const rtabmap::SensorData & data, const ros::Time & stamp);
bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool resetToPose(rtabmap_ros::ResetPose::Request&, rtabmap_ros::ResetPose::Response&);
src/RGBDOdometryNode.cpp
+22 -8
View File
@@ -188,18 +188,20 @@ public:
}
}
ros::Time stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
tf::StampedTransform localTransform;
try
{
if(this->waitForTransform())
{
if(!this->tfListener().waitForTransform(this->frameId(), image->header.frame_id, image->header.stamp, ros::Duration(1)))
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, image->header.stamp, localTransform);
this->tfListener().lookupTransform(this->frameId(), image->header.frame_id, stamp, localTransform);
}
catch(tf::TransformException & ex)
{
@@ -225,9 +227,9 @@ public:
ptrDepth->image,
rtabmapModel,
0,
rtabmap_ros::timestampFromROS(image->header.stamp));
rtabmap_ros::timestampFromROS(stamp));
this->processData(data, image->header);
this->processData(data, stamp);
}
}
}
@@ -252,6 +254,7 @@ public:
infoMsgs.push_back(cameraInfo);
infoMsgs.push_back(cameraInfo2);
ros::Time higherStamp;
int imageWidth = imageMsgs[0]->width;
int imageHeight = imageMsgs[0]->height;
int cameraCount = imageMsgs.size();
@@ -276,18 +279,29 @@ public:
UASSERT(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight);
UASSERT(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight);
ros::Time stamp = imageMsgs[i]->header.stamp>depthMsgs[i]->header.stamp?imageMsgs[i]->header.stamp:depthMsgs[i]->header.stamp;
if(i == 0)
{
higherStamp = stamp;
}
else if(stamp > higherStamp)
{
higherStamp = stamp;
}
tf::StampedTransform localTransform;
try
{
if(this->waitForTransform())
{
if(!this->tfListener().waitForTransform(this->frameId(), imageMsgs[i]->header.frame_id, imageMsgs[i]->header.stamp, ros::Duration(1)))
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, imageMsgs[i]->header.stamp, localTransform);
this->tfListener().lookupTransform(this->frameId(), imageMsgs[i]->header.frame_id, stamp, localTransform);
}
catch(tf::TransformException & ex)
{
@@ -368,9 +382,9 @@ public:
depth,
cameraModels,
0,
rtabmap_ros::timestampFromROS(image->header.stamp));
rtabmap_ros::timestampFromROS(higherStamp));
this->processData(data, image->header);
this->processData(data, higherStamp);
}
}
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
{