ros1: Migrating tf to tf2 (#1425)

* 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

rtabmap_util/src/nodelets/imu_to_tf.cpp

@@ -30,8 +30,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <nodelet/nodelet.h>
 #include <sensor_msgs/Imu.h>
 #include <tf/transform_broadcaster.h>
+#include <tf/transform_datatypes.h>
 #include <tf/LinearMath/Matrix3x3.h>
-#include <tf/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
 
 namespace rtabmap_util
 {
@@ -41,6 +43,7 @@ class ImuToTF : public nodelet::Nodelet
 public:
 	ImuToTF() :
 		fixedFrameId_("odom"),
+		tfListener_(tfBuffer_),
 		waitForTransformDuration_(0.1)
 	{}
 
@@ -78,23 +81,21 @@ private:
 		{
 			try
 			{
-				std::string errorMsg;
-				if(!tfListener_.waitForTransform(baseFrameId_, msg->header.frame_id, msg->header.stamp, ros::Duration(waitForTransformDuration_), ros::Duration(0.01), &errorMsg))
-				{
-					NODELET_ERROR("Could not get transform from %s to %s after %f seconds (for stamp=%f)! Error=\"%s\".",
-							baseFrameId_.c_str(), msg->header.frame_id.c_str(), 0.1, msg->header.stamp.toSec(), errorMsg.c_str());
-					return;
-				}
-
-				tf::StampedTransform tmp;
-				tfListener_.lookupTransform(baseFrameId_, msg->header.frame_id, msg->header.stamp, tmp);
+				geometry_msgs::TransformStamped tmpMsg;
+				tmpMsg = tfBuffer_.lookupTransform(
+						!baseFrameId_.empty()&&baseFrameId_.at(0)=='/'?baseFrameId_.substr(1):baseFrameId_,
+						!msg->header.frame_id.empty()&&msg->header.frame_id.at(0)=='/'?msg->header.frame_id.substr(1):msg->header.frame_id,
+						msg->header.stamp,
+						ros::Duration(waitForTransformDuration_));
+				tf::Transform tmp;
+				tf::transformMsgToTF(tmpMsg.transform, tmp);
 				tf::Quaternion q;
 				q.setRPY(0.0,0.0,tf::getYaw(tmp.getRotation()));
 				tf::Transform t = tf::Transform(q)*st*tmp.inverse(); // base_frame orientation
 				st.setRotation(t.getRotation());
 				st.child_frame_id_ = baseFrameId_;
 			}
-			catch(tf::TransformException & ex)
+			catch(tf2::TransformException & ex)
 			{
 				NODELET_ERROR("(getting transform %s -> %s) %s", baseFrameId_.c_str(), msg->header.frame_id.c_str(), ex.what());
 				return;
@@ -114,7 +115,8 @@ private:
 	tf::TransformBroadcaster pub_;
 	std::string fixedFrameId_;
 	std::string baseFrameId_;
-	tf::TransformListener tfListener_;
+	tf2_ros::Buffer tfBuffer_;
+	tf2_ros::TransformListener tfListener_;
 	double waitForTransformDuration_;
 };
 

rtabmap_util/src/nodelets/lidar_deskewing.cpp

@@ -3,7 +3,8 @@
 #include <pluginlib/class_list_macros.hpp>
 #include <nodelet/nodelet.h>
 
-#include <tf/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
 
 #include <sensor_msgs/PointCloud2.h>
 #include <sensor_msgs/LaserScan.h>
@@ -31,12 +32,13 @@ public:
 
 	virtual ~LidarDeskewing()
 	{
+		delete tfListener_;
 	}
 
 private:
 	virtual void onInit()
 	{
-		tfListener_ = new tf::TransformListener();
+		tfListener_ = new tf2_ros::TransformListener(tfBuffer_);
 		ros::NodeHandle & nh = getNodeHandle();
 		ros::NodeHandle & pnh = getPrivateNodeHandle();
 
@@ -67,7 +69,7 @@ private:
 				fixedFrameId_,
 				msg->header.stamp,
 				msg->header.stamp + ros::Duration().fromSec(msg->ranges.size()*msg->time_increment),
-				*tfListener_,
+				tfBuffer_,
 				waitForTransformDuration_);
 		if(tmpT.isNull())
 		{
@@ -76,10 +78,10 @@ private:
 
 		sensor_msgs::PointCloud2 scanOut;
 		laser_geometry::LaserProjection projection;
-		projection.transformLaserScanToPointCloud(fixedFrameId_, *msg, scanOut, *tfListener_);
+		projection.transformLaserScanToPointCloud(fixedFrameId_, *msg, scanOut, tfBuffer_);
 
 		sensor_msgs::PointCloud2 scanOutDeskewed;
-		if(!pcl_ros::transformPointCloud(msg->header.frame_id, scanOut, scanOutDeskewed, *tfListener_))
+		if(!pcl_ros::transformPointCloud(msg->header.frame_id, scanOut, scanOutDeskewed, tfBuffer_))
 		{
 			ROS_ERROR("Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
 					fixedFrameId_.c_str(), msg->header.frame_id.c_str(), msg->header.stamp.toSec());
@@ -91,7 +93,7 @@ private:
 	void callbackCloud(const sensor_msgs::PointCloud2ConstPtr & msg)
 	{
 		sensor_msgs::PointCloud2 msgDeskewed;
-		if(rtabmap_conversions::deskew(*msg, msgDeskewed, fixedFrameId_, *tfListener_, waitForTransformDuration_, slerp_))
+		if(rtabmap_conversions::deskew(*msg, msgDeskewed, fixedFrameId_, tfBuffer_, waitForTransformDuration_, slerp_))
 		{
 			pubCloud_.publish(msgDeskewed);
 		}
@@ -112,7 +114,8 @@ private:
 	std::string fixedFrameId_;
 	double waitForTransformDuration_;
 	bool slerp_;
-	tf::TransformListener * tfListener_;
+	tf2_ros::Buffer tfBuffer_;
+	tf2_ros::TransformListener * tfListener_;
 };
 
 PLUGINLIB_EXPORT_CLASS(rtabmap_util::LidarDeskewing, nodelet::Nodelet);

rtabmap_util/src/nodelets/obstacles_detection.cpp

@@ -35,7 +35,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/filters/filter.h>
 #include <rtabmap/core/LocalGridMaker.h>
 
-#include <tf/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
 
 #include <sensor_msgs/PointCloud2.h>
 
@@ -53,7 +54,8 @@ public:
 		frameId_("base_link"),
 		waitForTransform_(false),
 		mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection()),
-		warned_(false)
+		warned_(false),
+		tfListener_(tfBuffer_)
 	{}
 
 	virtual ~ObstaclesDetection()
@@ -248,19 +250,15 @@ private:
 		rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
 		try
 		{
-			if(waitForTransform_)
-			{
-				if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
-				{
-					NODELET_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
-					return;
-				}
-			}
-			tf::StampedTransform tmp;
-			tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp);
-			localTransform = rtabmap_conversions::transformFromTF(tmp);
+			geometry_msgs::TransformStamped tmp;
+			tmp = tfBuffer_.lookupTransform(
+					!frameId_.empty()&&frameId_.at(0)=='/'?frameId_.substr(1):frameId_,
+					!cloudMsg->header.frame_id.empty()&&cloudMsg->header.frame_id.at(0)=='/'?cloudMsg->header.frame_id.substr(1):cloudMsg->header.frame_id,
+					cloudMsg->header.stamp,
+					ros::Duration(waitForTransform_?1.0:0.0));
+			localTransform = rtabmap_conversions::transformFromGeometryMsg(tmp.transform);
 		}
-		catch(tf::TransformException & ex)
+		catch(tf2::TransformException & ex)
 		{
 			NODELET_ERROR("%s",ex.what());
 			return;
@@ -271,19 +269,15 @@ private:
 		{
 			try
 			{
-				if(waitForTransform_)
-				{
-					if(!tfListener_.waitForTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, ros::Duration(1)))
-					{
-						NODELET_ERROR("Could not get transform from %s to %s after 1 second!", mapFrameId_.c_str(), frameId_.c_str());
-						return;
-					}
-				}
-				tf::StampedTransform tmp;
-				tfListener_.lookupTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, tmp);
-				pose = rtabmap_conversions::transformFromTF(tmp);
+				geometry_msgs::TransformStamped tmp;
+				tmp = tfBuffer_.lookupTransform(
+						!mapFrameId_.empty()&&mapFrameId_.at(0)=='/'?mapFrameId_.substr(1):mapFrameId_,
+						!frameId_.empty()&&frameId_.at(0)=='/'?frameId_.substr(1):frameId_,
+						cloudMsg->header.stamp,
+						ros::Duration(waitForTransform_?1.0:0.0));
+				pose = rtabmap_conversions::transformFromGeometryMsg(tmp.transform);
 			}
-			catch(tf::TransformException & ex)
+			catch(tf2::TransformException & ex)
 			{
 				NODELET_ERROR("%s",ex.what());
 				return;
@@ -443,7 +437,8 @@ private:
 	bool mapFrameProjection_;
 	bool warned_;
 
-	tf::TransformListener tfListener_;
+	tf2_ros::Buffer tfBuffer_;
+	tf2_ros::TransformListener tfListener_;
 
 	ros::Publisher groundPub_;
 	ros::Publisher obstaclesPub_;

rtabmap_util/src/nodelets/point_cloud_aggregator.cpp

@@ -36,7 +36,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <pcl_ros/transforms.h>
 
-#include <tf/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
 
 #include <sensor_msgs/PointCloud2.h>
 
@@ -73,7 +74,8 @@ public:
 		exactSync2_(0),
 		approxSync2_(0),
 		waitForTransformDuration_(0.1),
-		xyzOutput_(false)
+		xyzOutput_(false),
+		tfListener_(tfBuffer_)
 	{}
 
 	virtual ~PointCloudAggregator()
@@ -250,7 +252,7 @@ private:
 			if(!frameId.empty() && frameId.compare(cloudMsgs[0]->header.frame_id) != 0)
 			{
 				sensor_msgs::PointCloud2 tmp;
-				pcl_ros::transformPointCloud(frameId, *cloudMsgs[0], tmp, tfListener_);
+				pcl_ros::transformPointCloud(frameId, *cloudMsgs[0], tmp, tfBuffer_);
 				pcl_conversions::toPCL(tmp, *output);
 			}
 			else
@@ -307,7 +309,7 @@ private:
 							fixedFrameId_, //fixedFrame
 							cloudMsgs[0]->header.stamp, //stampTarget
 							cloudMsgs[i]->header.stamp, //stampSource
-							tfListener_,
+							tfBuffer_,
 							waitForTransformDuration_);
 				}
 
@@ -315,7 +317,7 @@ private:
 				if(frameId.compare(cloudMsgs[i]->header.frame_id) != 0)
 				{
 					sensor_msgs::PointCloud2 tmp;
-					pcl_ros::transformPointCloud(frameId, *cloudMsgs[i], tmp, tfListener_);
+					pcl_ros::transformPointCloud(frameId, *cloudMsgs[i], tmp, tfBuffer_);
 					if(!cloudDisplacement.isNull())
 					{
 						sensor_msgs::PointCloud2 tmp2;
@@ -468,7 +470,8 @@ private:
 	std::string fixedFrameId_;
 	double waitForTransformDuration_;
 	bool xyzOutput_;
-	tf::TransformListener tfListener_;
+	tf2_ros::Buffer tfBuffer_;
+	tf2_ros::TransformListener tfListener_;
 };
 
 PLUGINLIB_EXPORT_CLASS(rtabmap_util::PointCloudAggregator, nodelet::Nodelet);

rtabmap_util/src/nodelets/point_cloud_assembler.cpp

@@ -38,7 +38,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <pcl_ros/transforms.h>
 
-#include <tf/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
 
 #include <nav_msgs/Odometry.h>
 #include <sensor_msgs/PointCloud2.h>
@@ -87,7 +88,8 @@ public:
 		noiseMinNeighbors_(5),
 		removeZ_(false),
 		fixedFrameId_("odom"),
-		frameId_("")
+		frameId_(""),
+		tfListener_(tfBuffer_)
 	{}
 
 	virtual ~PointCloudAssembler()
@@ -316,7 +318,7 @@ private:
 						fixedFrameId_, //fromFrame
 						cloudMsg->header.frame_id, //toFrame
 						cloudMsg->header.stamp,
-						tfListener_,
+						tfBuffer_,
 						waitForTransformDuration_);
 
 				if(pose.isNull())
@@ -475,7 +477,7 @@ private:
 								fixedFrameId_, //fromFrame
 								frameId_, //toFrame
 								cloudMsg->header.stamp,
-								tfListener_,
+								tfBuffer_,
 								waitForTransformDuration_);
 						if(t.isNull())
 						{
@@ -582,7 +584,8 @@ private:
 	bool removeZ_;
 	std::string fixedFrameId_;
 	std::string frameId_;
-	tf::TransformListener tfListener_;
+	tf2_ros::Buffer tfBuffer_;
+	tf2_ros::TransformListener tfListener_;
 	rtabmap::Transform previousPose_;
 
 	std::list<pcl::PCLPointCloud2::Ptr> clouds_;

rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp

@@ -52,6 +52,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <message_filters/sync_policies/exact_time.h>
 #include <message_filters/subscriber.h>
 
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
 namespace rtabmap_util
 {
 
@@ -87,7 +90,7 @@ public:
 private:
 	virtual void onInit()
 	{
-		listener_ = new tf::TransformListener();
+		listener_ = new tf2_ros::TransformListener(tfBuffer_);
 
 		ros::NodeHandle & nh = getNodeHandle();
 		ros::NodeHandle & pnh = getPrivateNodeHandle();
@@ -179,7 +182,7 @@ private:
 						fixedFrameId_,
 						pointCloud2Msg->header.stamp,
 						cameraInfoMsg->header.stamp,
-						*listener_,
+						tfBuffer_,
 						waitForTransform_);
 			}
 
@@ -192,7 +195,7 @@ private:
 					pointCloud2Msg->header.frame_id,
 					cameraInfoMsg->header.frame_id,
 					cameraInfoMsg->header.stamp,
-					*listener_,
+					tfBuffer_,
 					waitForTransform_);
 
 			if(cloudToCamera.isNull())
@@ -306,7 +309,8 @@ private:
 	message_filters::Subscriber<sensor_msgs::PointCloud2> pointCloudSub_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
 	std::string fixedFrameId_;
-	tf::TransformListener * listener_;
+	tf2_ros::Buffer tfBuffer_;
+	tf2_ros::TransformListener * listener_;
 	double waitForTransform_;
 	int fillHolesSize_;
 	double fillHolesError_;