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

ros1: Migrating tf to tf2 (#1425)

Browse Source 
* Migrating tf to tf2

* Added ci action to test PR on ros1

* updated dev container with nvidia working

* backward compatibility with topics having frame_id with leading slash not allowed with tf2

* backward compatibility of leading slash for other tf2 buffers

* updated comment
This commit is contained in:
matlabbe
2026-05-23 15:08:21 -07:00
committed by GitHub
parent f81536f508
commit a6921845b6
30 changed files with 311 additions and 247 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_odom/include/rtabmap_odom/OdometryROS.h
+5 -3
View File
@@ -31,8 +31,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <tf2_ros/transform_listener.h>
#include <std_srvs/Empty.h>
#include <std_msgs/Header.h>
@@ -84,7 +85,7 @@ protected:
void initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic = "");
virtual void flushCallbacks() = 0;
tf::TransformListener & tfListener() {return tfListener_;}
tf2_ros::Buffer & tfBuffer() {return tfBuffer_;}
double waitForTransformDuration() const {return waitForTransform_?waitForTransformDuration_:0.0;}
rtabmap::Transform velocityGuess() const;
ros::Time previousStamp() const {return previousStamp_;}
@@ -142,7 +143,8 @@ private:
ros::ServiceServer setLogWarnSrv_;
ros::ServiceServer setLogErrorSrv_;
tf2_ros::TransformBroadcaster tfBroadcaster_;
tf::TransformListener tfListener_;
tf2_ros::Buffer tfBuffer_;
tf2_ros::TransformListener tfListener_;
ros::Subscriber imuSub_;
// Safe-threading
rtabmap_odom/src/OdometryROS.cpp
+6 -5
View File
@@ -74,6 +74,7 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
waitForTransformDuration_(0.1), // 100 ms
publishNullWhenLost_(true),
publishCompressedSensorData_(false),
tfListener_(tfBuffer_),
paused_(false),
resetCountdown_(0),
resetCurrentCount_(0),
@@ -429,7 +430,7 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
if(this->frameId().compare(msg->header.frame_id) != 0)
{
localTransform = rtabmap_conversions::getTransform(this->frameId(), msg->header.frame_id, msg->header.stamp, this->tfListener(), this->waitForTransformDuration());
localTransform = rtabmap_conversions::getTransform(this->frameId(), msg->header.frame_id, msg->header.stamp, this->tfBuffer(), this->waitForTransformDuration());
}
if(localTransform.isNull())
{
@@ -647,7 +648,7 @@ void OdometryROS::processData()
if(!groundTruthFrameId_.empty())
{
groundTruth = rtabmap_conversions::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, header.stamp, this->tfListener(), this->waitForTransformDuration());
groundTruth = rtabmap_conversions::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, header.stamp, this->tfBuffer(), this->waitForTransformDuration());
if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
{
@@ -695,7 +696,7 @@ void OdometryROS::processData()
else
{
// Check TF to see if sensor fusion is used (e.g., the output of robot_localization)
Transform tfPose = rtabmap_conversions::getTransform(odomFrameId_, frameId_, header.stamp, this->tfListener(), this->waitForTransformDuration());
Transform tfPose = rtabmap_conversions::getTransform(odomFrameId_, frameId_, header.stamp, this->tfBuffer(), this->waitForTransformDuration());
if(tfPose.isNull())
{
NODELET_WARN( "Odometry automatically reset to latest computed pose!");
@@ -719,7 +720,7 @@ void OdometryROS::processData()
Transform guessCurrentPose;
if(!guessFrameId_.empty())
{
guessCurrentPose = rtabmap_conversions::getTransform(guessFrameId_, frameId_, header.stamp, this->tfListener(), this->waitForTransformDuration());
guessCurrentPose = rtabmap_conversions::getTransform(guessFrameId_, frameId_, header.stamp, this->tfBuffer(), this->waitForTransformDuration());
Transform previousPose = guessPreviousPose_;
if(guessPreviousPose_.isNull())
@@ -1068,7 +1069,7 @@ void OdometryROS::processData()
else
{
// Check TF to see if sensor fusion is used (e.g., the output of robot_localization)
Transform tfPose = rtabmap_conversions::getTransform(odomFrameId_, frameId_, header.stamp, this->tfListener(), this->waitForTransformDuration());
Transform tfPose = rtabmap_conversions::getTransform(odomFrameId_, frameId_, header.stamp, this->tfBuffer(), this->waitForTransformDuration());
if(tfPose.isNull())
{
NODELET_WARN( "Odometry automatically reset to latest computed pose!");
rtabmap_odom/src/nodelets/icp_odometry.cpp
+8 -8
View File
@@ -357,7 +357,7 @@ private:
Transform localScanTransform = rtabmap_conversions::getTransform(this->frameId(),
scanMsg->header.frame_id,
scanMsg->header.stamp,
this->tfListener(),
this->tfBuffer(),
this->waitForTransformDuration());
if(localScanTransform.isNull())
{
@@ -377,7 +377,7 @@ private:
guessFrameId().empty()?frameId():guessFrameId(),
scanMsg->header.stamp,
scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment),
this->tfListener(),
this->tfBuffer(),
this->waitForTransformDuration());
if(tmpT.isNull())
{
@@ -388,7 +388,7 @@ private:
guessFrameId().empty()?frameId():guessFrameId(),
*scanMsg,
scanOut,
this->tfListener(),
this->tfBuffer(),
-1.0,
laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);
@@ -405,7 +405,7 @@ private:
}
sensor_msgs::PointCloud2 scanOutDeskewed;
if(!pcl_ros::transformPointCloud(scanMsg->header.frame_id, scanOut, scanOutDeskewed, this->tfListener()))
if(!pcl_ros::transformPointCloud(scanMsg->header.frame_id, scanOut, scanOutDeskewed, this->tfBuffer()))
{
ROS_ERROR("Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
(guessFrameId().empty()?frameId():guessFrameId()).c_str(), scanMsg->header.frame_id.c_str(), scanMsg->header.stamp.toSec());
@@ -622,7 +622,7 @@ private:
*cloudMsg = *pointCloudMsg;
}
Transform localScanTransform = rtabmap_conversions::getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp, this->tfListener(), this->waitForTransformDuration());
Transform localScanTransform = rtabmap_conversions::getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp, this->tfBuffer(), this->waitForTransformDuration());
if(localScanTransform.isNull())
{
ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg->header.stamp.toSec());
@@ -634,7 +634,7 @@ private:
if(!guessFrameId().empty())
{
// deskew with TF
if(!rtabmap_conversions::deskew(*pointCloudMsg, *cloudMsg, guessFrameId(), tfListener(), waitForTransformDuration(), deskewingSlerp_))
if(!rtabmap_conversions::deskew(*pointCloudMsg, *cloudMsg, guessFrameId(), tfBuffer(), waitForTransformDuration(), deskewingSlerp_))
{
ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
return;
@@ -650,7 +650,7 @@ private:
{
// transform in base frame
cloudInBaseFrame.reset(new sensor_msgs::PointCloud2);
if(!pcl_ros::transformPointCloud(frameId(), *pointCloudMsg, *cloudInBaseFrame, this->tfListener()))
if(!pcl_ros::transformPointCloud(frameId(), *pointCloudMsg, *cloudInBaseFrame, this->tfBuffer()))
{
ROS_ERROR("Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
pointCloudMsg->header.frame_id.c_str(), frameId().c_str(), pointCloudMsg->header.stamp.toSec());
@@ -669,7 +669,7 @@ private:
if(!alreadyInBaseFrame)
{
// put back in scan frame
if(!pcl_ros::transformPointCloud(pointCloudMsg->header.frame_id.c_str(), *cloudDeskewed, *cloudMsg, this->tfListener()))
if(!pcl_ros::transformPointCloud(pointCloudMsg->header.frame_id.c_str(), *cloudDeskewed, *cloudMsg, this->tfBuffer()))
{
ROS_ERROR("Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
frameId().c_str(), pointCloudMsg->header.frame_id.c_str(), pointCloudMsg->header.stamp.toSec());
rtabmap_odom/src/nodelets/rgbd_odometry.cpp
+1 -1
View File
@@ -487,7 +487,7 @@ private:
higherStamp = stamp;
}
Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), rgbImages[i]->header.frame_id, stamp, this->tfListener(), this->waitForTransformDuration());
Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), rgbImages[i]->header.frame_id, stamp, this->tfBuffer(), this->waitForTransformDuration());
if(localTransform.isNull())
{
return;
rtabmap_odom/src/nodelets/rgbdicp_odometry.cpp
+4 -4
View File
@@ -294,7 +294,7 @@ private:
}
}
Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), image->header.frame_id, stamp, this->tfListener(), this->waitForTransformDuration());
Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), image->header.frame_id, stamp, this->tfBuffer(), this->waitForTransformDuration());
if(localTransform.isNull())
{
return;
@@ -330,7 +330,7 @@ private:
localScanTransform = rtabmap_conversions::getTransform(this->frameId(),
scanMsg->header.frame_id,
scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment),
this->tfListener(),
this->tfBuffer(),
this->waitForTransformDuration());
if(localScanTransform.isNull())
{
@@ -341,7 +341,7 @@ private:
//transform in frameId_ frame
sensor_msgs::PointCloud2 scanOut;
laser_geometry::LaserProjection projection;
projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener());
projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfBuffer());
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(scanOut, *pclScan);
pclScan->is_dense = true;
@@ -396,7 +396,7 @@ private:
}
}
}
localScanTransform = rtabmap_conversions::getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp, this->tfListener(), this->waitForTransformDuration());
localScanTransform = rtabmap_conversions::getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp, this->tfBuffer(), this->waitForTransformDuration());
if(localScanTransform.isNull())
{
ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg->header.stamp.toSec());
rtabmap_odom/src/nodelets/stereo_odometry.cpp
+3 -3
View File
@@ -465,7 +465,7 @@ private:
higherStamp = stamp;
}
Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), leftImages[i]->header.frame_id, stamp, this->tfListener(), this->waitForTransformDuration());
Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), leftImages[i]->header.frame_id, stamp, this->tfBuffer(), this->waitForTransformDuration());
if(localTransform.isNull())
{
return;
@@ -530,7 +530,7 @@ private:
rightCameraInfos[i].header.frame_id,
leftCameraInfos[i].header.frame_id,
leftCameraInfos[i].header.stamp,
this->tfListener(),
this->tfBuffer(),
this->waitForTransformDuration());
if(stereoTransform.isNull())
{
@@ -564,7 +564,7 @@ private:
leftCameraInfos[i].header.frame_id,
rightCameraInfos[i].header.frame_id,
leftCameraInfos[i].header.stamp,
this->tfListener(),
this->tfBuffer(),
this->waitForTransformDuration());
if(!stereoTransform.isNull() && stereoTransform.x()>0)