Added point_cloud_aggregator node

CMakeLists.txt

@@ -220,7 +220,7 @@ SET(rtabmap_plugins_lib_src
     src/nodelets/pointcloud_to_depthimage.cpp 
     src/nodelets/obstacles_detection.cpp
 #   src/nodelets/obstacles_detection_old.cpp
-#   src/nodelets/point_cloud_aggregator.cpp
+    src/nodelets/point_cloud_aggregator.cpp
 #   src/nodelets/point_cloud_assembler.cpp
 #   src/nodelets/undistort_depth.cpp
 #   src/nodelets/imu_to_tf.cpp
@@ -284,6 +284,7 @@ rclcpp_components_register_nodes(rtabmap_plugins "rtabmap_ros::PointCloudXYZ")
 rclcpp_components_register_nodes(rtabmap_plugins "rtabmap_ros::PointCloudXYZRGB")
 rclcpp_components_register_nodes(rtabmap_plugins "rtabmap_ros::PointCloudToDepthImage")
 rclcpp_components_register_nodes(rtabmap_plugins "rtabmap_ros::ObstaclesDetection")
+rclcpp_components_register_nodes(rtabmap_plugins "rtabmap_ros::PointCloudAggregator")
 
 rclcpp_components_register_nodes(rtabmap_sync "rtabmap_ros::CoreWrapper")
 
@@ -340,6 +341,11 @@ ament_target_dependencies(rtabmap_obstacles_detection ${Libraries})
 target_link_libraries(rtabmap_obstacles_detection rtabmap_plugins ${RTABMap_LIBRARIES})
 set_target_properties(rtabmap_obstacles_detection PROPERTIES OUTPUT_NAME "obstacles_detection")
 
+add_executable(rtabmap_point_cloud_aggregator src/PointCloudAggregatorNode.cpp)
+ament_target_dependencies(rtabmap_point_cloud_aggregator ${Libraries})
+target_link_libraries(rtabmap_point_cloud_aggregator rtabmap_plugins ${RTABMap_LIBRARIES})
+set_target_properties(rtabmap_point_cloud_aggregator PROPERTIES OUTPUT_NAME "point_cloud_aggregator")
+
 #add_executable(rtabmap_map_optimizer src/MapOptimizerNode.cpp)
 #ament_target_dependencies(rtabmap_map_optimizer rtabmap_ros)
 #set_target_properties(rtabmap_map_optimizer PROPERTIES OUTPUT_NAME "map_optimizer")
@@ -572,6 +578,7 @@ install(TARGETS
    rtabmap_point_cloud_xyz
    rtabmap_point_cloud_xyzrgb
    rtabmap_obstacles_detection
+   rtabmap_point_cloud_aggregator
 #   rtabmap_camera
    rtabmap_rgbd_sync
    rtabmap_stereo_sync

include/rtabmap_ros/point_cloud_aggregator.hpp

@@ -0,0 +1,102 @@
+/*
+Copyright (c) 2010-2019, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap_ros/visibility.h>
+#include "rclcpp/rclcpp.hpp"
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <sensor_msgs/PointCloud2.h>
+
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/exact_time.h>
+
+namespace rtabmap_ros
+{
+
+/**
+ * Nodelet used to merge point clouds from different sensors into a single
+ * assembled cloud. If fixed_frame_id is set and approx_sync is true,
+ * the clouds are adjusted to include the displacement of the robot
+ * in the output cloud.
+ */
+class PointCloudAggregator : public rclcpp::Node
+{
+public:
+	RTABMAP_ROS_PUBLIC
+	explicit PointCloudAggregator(const rclcpp::NodeOptions & options);
+
+	virtual ~PointCloudAggregator();
+
+private:
+	void clouds4_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_1,
+						 const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_2,
+						 const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_3,
+						 const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_4);
+	void clouds3_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_1,
+	                     const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_2,
+	                     const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_3);
+	void clouds2_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_1,
+						 const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_2);
+	void combineClouds(const std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> & cloudMsgs);
+
+	std::thread * warningThread_;
+	bool callbackCalled_;
+
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> ExactSync4Policy;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> ApproxSync4Policy;
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> ExactSync3Policy;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> ApproxSync3Policy;
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> ExactSync2Policy;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> ApproxSync2Policy;
+	message_filters::Synchronizer<ExactSync4Policy>* exactSync4_;
+	message_filters::Synchronizer<ApproxSync4Policy>* approxSync4_;
+	message_filters::Synchronizer<ExactSync3Policy>* exactSync3_;
+	message_filters::Synchronizer<ApproxSync3Policy>* approxSync3_;
+	message_filters::Synchronizer<ExactSync2Policy>* exactSync2_;
+	message_filters::Synchronizer<ApproxSync2Policy>* approxSync2_;
+	message_filters::Subscriber<sensor_msgs::msg::PointCloud2> cloudSub_1_;
+	message_filters::Subscriber<sensor_msgs::msg::PointCloud2> cloudSub_2_;
+	message_filters::Subscriber<sensor_msgs::msg::PointCloud2> cloudSub_3_;
+	message_filters::Subscriber<sensor_msgs::msg::PointCloud2> cloudSub_4_;
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudPub_;
+
+	std::string frameId_;
+	std::string fixedFrameId_;
+
+
+	std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
+	std::shared_ptr<tf2_ros::TransformListener> tfListener_;
+};
+
+}
+

src/PointCloudAggregatorNode.cpp

@@ -0,0 +1,38 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap_ros/point_cloud_aggregator.hpp"
+
+int main(int argc, char **argv)
+{
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<rtabmap_ros::PointCloudAggregator>(rclcpp::NodeOptions()));
+	rclcpp::shutdown();
+	return 0;
+}
+
+

src/nodelets/point_cloud_aggregator.cpp

@@ -25,44 +25,132 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include <ros/ros.h>
+#include <rtabmap_ros/point_cloud_aggregator.hpp>
-#include <pluginlib/class_list_macros.h>
-#include <nodelet/nodelet.h>
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
-#include <pcl/io/pcd_io.h>
-
-#include <pcl_ros/transforms.h>
-
-#include <tf/transform_listener.h>
-
-#include <sensor_msgs/PointCloud2.h>
-
-#include <image_transport/image_transport.h>
-#include <image_transport/subscriber_filter.h>
-
-#include <message_filters/sync_policies/approximate_time.h>
-#include <message_filters/subscriber.h>
-#include <message_filters/sync_policies/exact_time.h>
 
 #include <rtabmap_ros/MsgConversion.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/ULogger.h>
+
+namespace pcl_ros {
+
+void
+transformPointCloud (
+		const Eigen::Matrix4f &transform,
+		const sensor_msgs::msg::PointCloud2 &in,
+        sensor_msgs::msg::PointCloud2 &out)
+{
+  // Get X-Y-Z indices
+  int x_idx = pcl::getFieldIndex (in, "x");
+  int y_idx = pcl::getFieldIndex (in, "y");
+  int z_idx = pcl::getFieldIndex (in, "z");
+
+  if (x_idx == -1 || y_idx == -1 || z_idx == -1)
+  {
+    UERROR ("Input dataset has no X-Y-Z coordinates! Cannot convert to Eigen format.");
+    return;
+  }
+
+  if (in.fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
+      in.fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
+      in.fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32)
+  {
+	  UERROR ("X-Y-Z coordinates not floats. Currently only floats are supported.");
+    return;
+  }
+
+  // Check if distance is available
+  int dist_idx = pcl::getFieldIndex (in, "distance");
+
+  // Copy the other data
+  if (&in != &out)
+  {
+    out.header = in.header;
+    out.height = in.height;
+    out.width  = in.width;
+    out.fields = in.fields;
+    out.is_bigendian = in.is_bigendian;
+    out.point_step   = in.point_step;
+    out.row_step     = in.row_step;
+    out.is_dense     = in.is_dense;
+    out.data.resize (in.data.size ());
+    // Copy everything as it's faster than copying individual elements
+    memcpy (&out.data[0], &in.data[0], in.data.size ());
+  }
+
+  Eigen::Array4i xyz_offset (in.fields[x_idx].offset, in.fields[y_idx].offset, in.fields[z_idx].offset, 0);
+
+  for (size_t i = 0; i < in.width * in.height; ++i)
+  {
+    Eigen::Vector4f pt (*(float*)&in.data[xyz_offset[0]], *(float*)&in.data[xyz_offset[1]], *(float*)&in.data[xyz_offset[2]], 1);
+    Eigen::Vector4f pt_out;
+
+    bool max_range_point = false;
+    int distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset;
+    float* distance_ptr = (dist_idx < 0 ? NULL : (float*)(&in.data[distance_ptr_offset]));
+    if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2]))
+    {
+      if (distance_ptr==NULL || !std::isfinite(*distance_ptr))  // Invalid point
+      {
+        pt_out = pt;
+      }
+      else  // max range point
+      {
+        pt[0] = *distance_ptr;  // Replace x with the x value saved in distance
+        pt_out = transform * pt;
+        max_range_point = true;
+        //std::cout << pt[0]<<","<<pt[1]<<","<<pt[2]<<" => "<<pt_out[0]<<","<<pt_out[1]<<","<<pt_out[2]<<"\n";
+      }
+    }
+    else
+    {
+      pt_out = transform * pt;
+    }
+
+    if (max_range_point)
+    {
+      // Save x value in distance again
+      *(float*)(&out.data[distance_ptr_offset]) = pt_out[0];
+      pt_out[0] = std::numeric_limits<float>::quiet_NaN();
+    }
+
+    memcpy (&out.data[xyz_offset[0]], &pt_out[0], sizeof (float));
+    memcpy (&out.data[xyz_offset[1]], &pt_out[1], sizeof (float));
+    memcpy (&out.data[xyz_offset[2]], &pt_out[2], sizeof (float));
+
+
+    xyz_offset += in.point_step;
+  }
+
+  // Check if the viewpoint information is present
+  int vp_idx = pcl::getFieldIndex (in, "vp_x");
+  if (vp_idx != -1)
+  {
+    // Transform the viewpoint info too
+    for (size_t i = 0; i < out.width * out.height; ++i)
+    {
+      float *pstep = (float*)&out.data[i * out.point_step + out.fields[vp_idx].offset];
+      // Assume vp_x, vp_y, vp_z are consecutive
+      Eigen::Vector4f vp_in (pstep[0], pstep[1], pstep[2], 1);
+      Eigen::Vector4f vp_out = transform * vp_in;
+
+      pstep[0] = vp_out[0];
+      pstep[1] = vp_out[1];
+      pstep[2] = vp_out[2];
+    }
+  }
+}
+
+}
 
 namespace rtabmap_ros
 {
 
-/**
+PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options) :
- * Nodelet used to merge point clouds from different sensors into a single
+	Node("pointcloud_to_depthimage", options),
- * assembled cloud. If fixed_frame_id is set and approx_sync is true,
- * the clouds are adjusted to include the displacement of the robot
- * in the output cloud.
- */
-class PointCloudAggregator : public nodelet::Nodelet
-{
-public:
-	PointCloudAggregator() :
 	warningThread_(0),
 	callbackCalled_(false),
 	exactSync4_(0),
@@ -71,9 +159,112 @@ public:
 	approxSync3_(0),
 	exactSync2_(0),
 	approxSync2_(0)
-	{}
+{
 
-	virtual ~PointCloudAggregator()
+	tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
+	//auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
+	//	this->get_node_base_interface(),
+	//	this->get_node_timers_interface());
+	//tfBuffer_->setCreateTimerInterface(timer_interface);
+	tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
+
+	int queueSize = 5;
+	int count = 2;
+	bool approx=true;
+	queueSize = this->declare_parameter("queue_size", queueSize);
+	frameId_ = this->declare_parameter("frame_id", frameId_);
+	fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
+	approx = this->declare_parameter("approx_sync", approx);
+	count = this->declare_parameter("count", count);
+
+	cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("combined_cloud", 1);
+
+	cloudSub_1_.subscribe(this, "cloud1", rmw_qos_profile_sensor_data);
+	cloudSub_2_.subscribe(this, "cloud2", rmw_qos_profile_sensor_data);
+
+	std::string subscribedTopicsMsg;
+	if(count == 4)
+	{
+		cloudSub_3_.subscribe(this, "cloud3", rmw_qos_profile_sensor_data);
+		cloudSub_4_.subscribe(this, "cloud4", rmw_qos_profile_sensor_data);
+		if(approx)
+		{
+			approxSync4_ = new message_filters::Synchronizer<ApproxSync4Policy>(ApproxSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
+			approxSync4_->registerCallback(std::bind(&rtabmap_ros::PointCloudAggregator::clouds4_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
+		}
+		else
+		{
+			exactSync4_ = new message_filters::Synchronizer<ExactSync4Policy>(ExactSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
+			exactSync4_->registerCallback(std::bind(&rtabmap_ros::PointCloudAggregator::clouds4_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
+		}
+		subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s",
+				get_name(),
+				approx?"approx":"exact",
+				cloudSub_1_.getTopic().c_str(),
+				cloudSub_2_.getTopic().c_str(),
+				cloudSub_3_.getTopic().c_str(),
+				cloudSub_4_.getTopic().c_str());
+	}
+	else if(count == 3)
+	{
+		cloudSub_3_.subscribe(this, "cloud3", rmw_qos_profile_sensor_data);
+		if(approx)
+		{
+			approxSync3_ = new message_filters::Synchronizer<ApproxSync3Policy>(ApproxSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
+			approxSync3_->registerCallback(std::bind(&rtabmap_ros::PointCloudAggregator::clouds3_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+		}
+		else
+		{
+			exactSync3_ = new message_filters::Synchronizer<ExactSync3Policy>(ExactSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
+			exactSync3_->registerCallback(std::bind(&rtabmap_ros::PointCloudAggregator::clouds3_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+		}
+		subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s",
+				this->get_name(),
+				approx?"approx":"exact",
+				cloudSub_1_.getTopic().c_str(),
+				cloudSub_2_.getTopic().c_str(),
+				cloudSub_3_.getTopic().c_str());
+	}
+	else
+	{
+		if(approx)
+		{
+			approxSync2_ = new message_filters::Synchronizer<ApproxSync2Policy>(ApproxSync2Policy(queueSize), cloudSub_1_, cloudSub_2_);
+			approxSync2_->registerCallback(std::bind(&rtabmap_ros::PointCloudAggregator::clouds2_callback, this, std::placeholders::_1, std::placeholders::_2));
+		}
+		else
+		{
+			exactSync2_ = new message_filters::Synchronizer<ExactSync2Policy>(ExactSync2Policy(queueSize), cloudSub_1_, cloudSub_2_);
+			exactSync2_->registerCallback(std::bind(&rtabmap_ros::PointCloudAggregator::clouds2_callback, this, std::placeholders::_1, std::placeholders::_2));
+		}
+		subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s",
+				this->get_name(),
+				approx?"approx":"exact",
+				cloudSub_1_.getTopic().c_str(),
+				cloudSub_2_.getTopic().c_str());
+	}
+
+
+	warningThread_ = new std::thread([&](){
+		rclcpp::Rate r(1.0/5.0);
+		while(!callbackCalled_)
+		{
+			r.sleep();
+			if(!callbackCalled_)
+			{
+				RCLCPP_WARN(this->get_logger(), "%s: Did not receive data since 5 seconds! Make sure the input topics are "
+						"published (\"$ rostopic hz my_topic\") and the timestamps in their "
+						"header are set. %s%s",
+						this->get_name(),
+						approx?"":"Parameter \"approx_sync\" is false, which means that input "
+							"topics should have all the exact timestamp for the callback to be called.",
+							subscribedTopicsMsg.c_str());
+			}
+		}
+	});
+}
+
+PointCloudAggregator::~PointCloudAggregator()
 {
 	delete exactSync4_;
 	delete approxSync4_;
@@ -90,98 +281,12 @@ public:
 	}
 }
 
-private:
+void PointCloudAggregator::clouds4_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_1,
-	virtual void onInit()
+					 const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_2,
+					 const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_3,
+					 const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_4)
 {
-		ros::NodeHandle & nh = getNodeHandle();
+	std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> clouds;
-		ros::NodeHandle & pnh = getPrivateNodeHandle();
-
-		int queueSize = 5;
-		int count = 2;
-		bool approx=true;
-		pnh.param("queue_size", queueSize, queueSize);
-		pnh.param("frame_id", frameId_, frameId_);
-		pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
-		pnh.param("approx_sync", approx, approx);
-		pnh.param("count", count, count);
-
-		cloudSub_1_.subscribe(nh, "cloud1", 1);
-		cloudSub_2_.subscribe(nh, "cloud2", 1);
-
-		std::string subscribedTopicsMsg;
-		if(count == 4)
-		{
-			cloudSub_3_.subscribe(nh, "cloud3", 1);
-			cloudSub_4_.subscribe(nh, "cloud4", 1);
-			if(approx)
-			{
-				approxSync4_ = new message_filters::Synchronizer<ApproxSync4Policy>(ApproxSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
-				approxSync4_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds4_callback, this, _1, _2, _3, _4));
-			}
-			else
-			{
-				exactSync4_ = new message_filters::Synchronizer<ExactSync4Policy>(ExactSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
-				exactSync4_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds4_callback, this, _1, _2, _3, _4));
-			}
-			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s",
-					getName().c_str(),
-					approx?"approx":"exact",
-					cloudSub_1_.getTopic().c_str(),
-					cloudSub_2_.getTopic().c_str(),
-					cloudSub_3_.getTopic().c_str(),
-					cloudSub_4_.getTopic().c_str());
-		}
-		else if(count == 3)
-		{
-			cloudSub_3_.subscribe(nh, "cloud3", 1);
-			if(approx)
-			{
-				approxSync3_ = new message_filters::Synchronizer<ApproxSync3Policy>(ApproxSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
-				approxSync3_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds3_callback, this, _1, _2, _3));
-			}
-			else
-			{
-				exactSync3_ = new message_filters::Synchronizer<ExactSync3Policy>(ExactSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
-				exactSync3_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds3_callback, this, _1, _2, _3));
-			}
-			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s",
-					getName().c_str(),
-					approx?"approx":"exact",
-					cloudSub_1_.getTopic().c_str(),
-					cloudSub_2_.getTopic().c_str(),
-					cloudSub_3_.getTopic().c_str());
-		}
-		else
-		{
-			if(approx)
-			{
-				approxSync2_ = new message_filters::Synchronizer<ApproxSync2Policy>(ApproxSync2Policy(queueSize), cloudSub_1_, cloudSub_2_);
-				approxSync2_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds2_callback, this, _1, _2));
-			}
-			else
-			{
-				exactSync2_ = new message_filters::Synchronizer<ExactSync2Policy>(ExactSync2Policy(queueSize), cloudSub_1_, cloudSub_2_);
-				exactSync2_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds2_callback, this, _1, _2));
-			}
-			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s",
-					getName().c_str(),
-					approx?"approx":"exact",
-					cloudSub_1_.getTopic().c_str(),
-					cloudSub_2_.getTopic().c_str());
-		}
-
-		cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("combined_cloud", 1);
-
-		warningThread_ = new boost::thread(boost::bind(&PointCloudAggregator::warningLoop, this, subscribedTopicsMsg, approx));
-		NODELET_INFO("%s", subscribedTopicsMsg.c_str());
-	}
-
-	void clouds4_callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg_1,
-						 const sensor_msgs::PointCloud2ConstPtr & cloudMsg_2,
-						 const sensor_msgs::PointCloud2ConstPtr & cloudMsg_3,
-						 const sensor_msgs::PointCloud2ConstPtr & cloudMsg_4)
-	{
-		std::vector<sensor_msgs::PointCloud2ConstPtr> clouds;
 	clouds.push_back(cloudMsg_1);
 	clouds.push_back(cloudMsg_2);
 	clouds.push_back(cloudMsg_3);
@@ -189,39 +294,44 @@ private:
 
 	combineClouds(clouds);
 }
-	void clouds3_callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg_1,
+void PointCloudAggregator::clouds3_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_1,
-	                     const sensor_msgs::PointCloud2ConstPtr & cloudMsg_2,
+					 const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_2,
-	                     const sensor_msgs::PointCloud2ConstPtr & cloudMsg_3)
+					 const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_3)
 {
-		std::vector<sensor_msgs::PointCloud2ConstPtr> clouds;
+	std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> clouds;
 	clouds.push_back(cloudMsg_1);
 	clouds.push_back(cloudMsg_2);
 	clouds.push_back(cloudMsg_3);
 
 	combineClouds(clouds);
 }
-	void clouds2_callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg_1,
+void PointCloudAggregator::clouds2_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_1,
-						 const sensor_msgs::PointCloud2ConstPtr & cloudMsg_2)
+					 const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_2)
 {
-		std::vector<sensor_msgs::PointCloud2ConstPtr> clouds;
+	std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> clouds;
 	clouds.push_back(cloudMsg_1);
 	clouds.push_back(cloudMsg_2);
 
 	combineClouds(clouds);
 }
-	void combineClouds(const std::vector<sensor_msgs::PointCloud2ConstPtr> & cloudMsgs)
+void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> & cloudMsgs)
 {
 	callbackCalled_ = true;
-		ROS_ASSERT(cloudMsgs.size() > 1);
+	UASSERT(cloudMsgs.size() > 1);
-		if(cloudPub_.getNumSubscribers())
+	if(cloudPub_->get_subscription_count())
 	{
 		pcl::PCLPointCloud2 output;
 
 		std::string frameId = frameId_;
 		if(!frameId.empty() && frameId.compare(cloudMsgs[0]->header.frame_id) != 0)
 		{
-				sensor_msgs::PointCloud2 tmp;
+			sensor_msgs::msg::PointCloud2 tmp;
-				pcl_ros::transformPointCloud(frameId, *cloudMsgs[0], tmp, tfListener_);
+			rtabmap::Transform t = rtabmap_ros::getTransform(frameId, cloudMsgs[0]->header.frame_id, cloudMsgs[0]->header.stamp, *tfBuffer_, 0.1);
+			if(t.isNull())
+			{
+				return;
+			}
+			pcl_ros::transformPointCloud(t.toEigen4f(), *cloudMsgs[0], tmp);
 			pcl_conversions::toPCL(tmp, output);
 		}
 		else
@@ -243,7 +353,7 @@ private:
 						fixedFrameId_, //fixedFrame
 						cloudMsgs[i]->header.stamp, //stampSource
 						cloudMsgs[0]->header.stamp, //stampTarget
-							tfListener_,
+						*tfBuffer_,
 						0.1);
 				notsync = true;
 			}
@@ -251,11 +361,12 @@ private:
 			pcl::PCLPointCloud2 cloud2;
 			if(frameId.compare(cloudMsgs[i]->header.frame_id) != 0)
 			{
-					sensor_msgs::PointCloud2 tmp;
+				sensor_msgs::msg::PointCloud2 tmp;
-					pcl_ros::transformPointCloud(frameId, *cloudMsgs[i], tmp, tfListener_);
+				rtabmap::Transform t = rtabmap_ros::getTransform(frameId, cloudMsgs[i]->header.frame_id, cloudMsgs[i]->header.stamp, *tfBuffer_, 0.1);
+				pcl_ros::transformPointCloud(t.toEigen4f(), *cloudMsgs[i], tmp);
 				if(!cloudDisplacement.isNull())
 				{
-						sensor_msgs::PointCloud2 tmp2;
+					sensor_msgs::msg::PointCloud2 tmp2;
 					pcl_ros::transformPointCloud(cloudDisplacement.toEigen4f(), tmp, tmp2);
 					pcl_conversions::toPCL(tmp2, cloud2);
 				}
@@ -269,7 +380,7 @@ private:
 			{
 				if(!cloudDisplacement.isNull())
 				{
-						sensor_msgs::PointCloud2 tmp;
+					sensor_msgs::msg::PointCloud2 tmp;
 					pcl_ros::transformPointCloud(cloudDisplacement.toEigen4f(), *cloudMsgs[i], tmp);
 					pcl_conversions::toPCL(tmp, cloud2);
 				}
@@ -284,60 +395,19 @@ private:
 			output = tmp_output;
 		}
 
-			sensor_msgs::PointCloud2 rosCloud;
+		sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
-			pcl_conversions::moveFromPCL(output, rosCloud);
+		pcl_conversions::moveFromPCL(output, *rosCloud);
-			rosCloud.header.stamp = cloudMsgs[0]->header.stamp;
+		rosCloud->header.stamp = cloudMsgs[0]->header.stamp;
-			rosCloud.header.frame_id = frameId;
+		rosCloud->header.frame_id = frameId;
-			cloudPub_.publish(rosCloud);
+		cloudPub_->publish(std::move(rosCloud));
 	}
 }
 
-	void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
-	{
-		ros::Duration r(5.0);
-		while(!callbackCalled_)
-		{
-			r.sleep();
-			if(!callbackCalled_)
-			{
-				ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
-						"published (\"$ rostopic hz my_topic\") and the timestamps in their "
-						"header are set. %s%s",
-						getName().c_str(),
-						approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
-							"topics should have all the exact timestamp for the callback to be called.",
-						subscribedTopicsMsg.c_str());
-			}
-		}
 }
 
-	boost::thread * warningThread_;
+#include "rclcpp_components/register_node_macro.hpp"
-	bool callbackCalled_;
-
-	typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ExactSync4Policy;
-	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ApproxSync4Policy;
-	typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ExactSync3Policy;
-	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ApproxSync3Policy;
-	typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ExactSync2Policy;
-	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> ApproxSync2Policy;
-	message_filters::Synchronizer<ExactSync4Policy>* exactSync4_;
-	message_filters::Synchronizer<ApproxSync4Policy>* approxSync4_;
-	message_filters::Synchronizer<ExactSync3Policy>* exactSync3_;
-	message_filters::Synchronizer<ApproxSync3Policy>* approxSync3_;
-	message_filters::Synchronizer<ExactSync2Policy>* exactSync2_;
-	message_filters::Synchronizer<ApproxSync2Policy>* approxSync2_;
-	message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_1_;
-	message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_2_;
-	message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_3_;
-	message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_4_;
-
-	ros::Publisher cloudPub_;
-
-	std::string frameId_;
-	std::string fixedFrameId_;
-	tf::TransformListener tfListener_;
-};
-
-PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudAggregator, nodelet::Nodelet);
-}
 
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_ros::PointCloudAggregator)