From 135bdb1954dc4ecf157e10f0295b01c342a05881 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 17 Jul 2015 17:29:10 -0400 Subject: [PATCH] using latest stamp between input image topics for visual odometry (so less " Lookup would require extrapolation into the future." warnings show up) --- src/OdometryROS.cpp | 20 ++++++++++---------- src/OdometryROS.h | 2 +- src/RGBDOdometryNode.cpp | 30 ++++++++++++++++++++++-------- src/StereoOdometryNode.cpp | 10 ++++++---- 4 files changed, 39 insertions(+), 23 deletions(-) diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index a5bd4d0a..4cb0d2f0 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -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); } diff --git a/src/OdometryROS.h b/src/OdometryROS.h index a5aa7a66..6e5281cf 100644 --- a/src/OdometryROS.h +++ b/src/OdometryROS.h @@ -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&); diff --git a/src/RGBDOdometryNode.cpp b/src/RGBDOdometryNode.cpp index 0239f91a..90e05327 100644 --- a/src/RGBDOdometryNode.cpp +++ b/src/RGBDOdometryNode.cpp @@ -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); } } diff --git a/src/StereoOdometryNode.cpp b/src/StereoOdometryNode.cpp index 8ab75ec3..a429e63c 100644 --- a/src/StereoOdometryNode.cpp +++ b/src/StereoOdometryNode.cpp @@ -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 {