Merged master to ros2. ros2: Uniformized qos of all subscribers and publishers.

2026-10-05 09:17:47 +08:00 · 2021-10-04 19:29:19 -04:00
parent ccf152d877 1ecce15878
commit af1c7edeef
121 changed files with 10854 additions and 3478 deletions
@@ -55,8 +55,11 @@ ICPOdometry::ICPOdometry(const rclcpp::NodeOptions & options) :
 	scanRangeMax_(0),
 	scanVoxelSize_(0.0),
 	scanNormalK_(0),
-	scanNormalRadius_(0.0)
-	//plugin_loader_("rtabmap_ros", "rtabmap_ros::PluginInterface")
+	scanNormalRadius_(0.0),
+	scanNormalGroundUp_(0.0),
+	//plugin_loader_("rtabmap_ros", "rtabmap_ros::PluginInterface"),
+	scanReceived_(false),
+	cloudReceived_(false)
 {
 	OdometryROS::init(false, false, true);
 }
@@ -75,6 +78,7 @@ void ICPOdometry::onOdomInit()
 	scanVoxelSize_ = this->declare_parameter("scan_voxel_size", scanVoxelSize_);
 	scanNormalK_ = this->declare_parameter("scan_normal_k", scanNormalK_);
 	scanNormalRadius_ = this->declare_parameter("scan_normal_radius", scanNormalRadius_);
+	scanNormalGroundUp_ = this->declare_parameter("scan_normal_ground_up", scanNormalGroundUp_);

 	/*if (pnh.hasParam("plugins"))
 	{
@@ -112,11 +116,12 @@ void ICPOdometry::onOdomInit()
 	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_voxel_size        = %f m", scanVoxelSize_);
 	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_k          = %d", scanNormalK_);
 	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_radius     = %f m", scanNormalRadius_);
+	RCLCPP_INFO(this->get_logger(), "IcpOdometry: scan_normal_ground_up  = %f", scanNormalGroundUp_);

-	scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::SensorDataQoS(), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1));
-	cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::SensorDataQoS(), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1));
+	scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>("scan", 5, std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1));
+	cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", 5, std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1));

-	filtered_scan_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>("odom_filtered_input_scan", rclcpp::SensorDataQoS());
+	filtered_scan_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>("odom_filtered_input_scan", 1);
 }

 void ICPOdometry::updateParameters(ParametersMap & parameters)
@@ -226,11 +231,34 @@ void ICPOdometry::updateParameters(ParametersMap & parameters)
 				scanNormalRadius_ = value;
 			}
 		}
+		iter = parameters.find(Parameters::kIcpPointToPlaneGroundNormalsUp());
+		if(iter != parameters.end())
+		{
+			float value = uStr2Float(iter->second);
+			if(value != 0.0f)
+			{
+				if(!this->has_parameter("scan_normal_ground_up"))
+				{
+					RCLCPP_WARN(get_logger(), "IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_ground_up\" for convenience.", iter->second.c_str(), iter->first.c_str());
+					scanNormalGroundUp_ = value;
+				}
+			}
+		}
 	}
 }

 void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scanMsg)
 {
+	if(cloudReceived_)
+	{
+		RCLCPP_ERROR(this->get_logger(), "%s is already receiving clouds on \"%s\", but also "
+				"just received a scan on \"%s\". Both subscribers cannot be "
+				"used at the same time! Disabling scan subscriber.",
+				get_name(), cloud_sub_->get_topic_name(), scan_sub_->get_topic_name());
+		scan_sub_.reset();
+		return;
+	}
+	scanReceived_ = true;
 	if(this->isPaused())
 	{
 		return;
@@ -251,24 +279,77 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 	sensor_msgs::msg::PointCloud2 scanOut;
 	laser_geometry::LaserProjection projection;
 	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;

-	cv::Mat scan;
+	bool hasIntensity = false;
+	for(unsigned int i=0; i<scanOut.fields.size(); ++i)
+	{
+		if(scanOut.fields[i].name.compare("intensity") == 0)
+		{
+			if(scanOut.fields[i].datatype == sensor_msgs::msg::PointField::FLOAT32)
+			{
+				hasIntensity = true;
+			}
+			else
+			{
+				static bool warningShown = false;
+				if(!warningShown)
+				{
+					RCLCPP_WARN(get_logger(), "The input scan cloud has an \"intensity\" field "
+							"but the datatype (%d) is not supported. Intensity will be ignored. "
+							"This message is only shown once.", scanOut.fields[i].datatype);
+					warningShown = true;
+				}
+			}
+		}
+	}
+
+	pcl::PointCloud<pcl::PointXYZI>::Ptr pclScanI(new pcl::PointCloud<pcl::PointXYZI>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+
+	if(hasIntensity)
+	{
+		pcl::fromROSMsg(scanOut, *pclScanI);
+		pclScanI->is_dense = true;
+	}
+	else
+	{
+		pcl::fromROSMsg(scanOut, *pclScan);
+		pclScan->is_dense = true;
+	}
+
+	LaserScan scan;
 	int maxLaserScans = (int)scanMsg->ranges.size();
-	if(pclScan->size())
+	if(!pclScan->empty() || !pclScanI->empty())
 	{
 		if(scanDownsamplingStep_ > 1)
 		{
-			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
+			if(hasIntensity)
+			{
+				pclScanI = util3d::downsample(pclScanI, scanDownsamplingStep_);
+			}
+			else
+			{
+				pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
+			}
 			maxLaserScans /= scanDownsamplingStep_;
 		}
 		if(scanVoxelSize_ > 0.0f)
 		{
-			float pointsBeforeFiltering = (float)pclScan->size();
-			pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
-			float ratio = float(pclScan->size()) / pointsBeforeFiltering;
+			float pointsBeforeFiltering;
+			float pointsAfterFiltering;
+			if(hasIntensity)
+			{
+				pointsBeforeFiltering = (float)pclScanI->size();
+				pclScanI = util3d::voxelize(pclScanI, scanVoxelSize_);
+				pointsAfterFiltering = (float)pclScanI->size();
+			}
+			else
+			{
+				pointsBeforeFiltering = (float)pclScan->size();
+				pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
+				pointsAfterFiltering = (float)pclScan->size();
+			}
+			float ratio = pointsAfterFiltering / pointsBeforeFiltering;
 			maxLaserScans = int(float(maxLaserScans) * ratio);
 		}
 		if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
@@ -277,51 +358,88 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 			pcl::PointCloud<pcl::Normal>::Ptr normals;
 			if(scanVoxelSize_ > 0.0f)
 			{
-				normals = util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
+				if(hasIntensity)
+				{
+					normals = util3d::computeNormals2D(pclScanI, scanNormalK_, scanNormalRadius_);
+				}
+				else
+				{
+					normals = util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
+				}
 			}
 			else
 			{
-				normals = util3d::computeFastOrganizedNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
+				if(hasIntensity)
+				{
+					normals = util3d::computeFastOrganizedNormals2D(pclScanI, scanNormalK_, scanNormalRadius_);
+				}
+				else
+				{
+					normals = util3d::computeFastOrganizedNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
+				}
 			}
-			pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
-			pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
-			scan = util3d::laserScan2dFromPointCloud(*pclScanNormal).data();
-
-			if(filtered_scan_pub_->get_subscription_count())
+			pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanINormal;
+			pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal;
+			if(hasIntensity)
 			{
-				sensor_msgs::msg::PointCloud2::UniquePtr msg(new sensor_msgs::msg::PointCloud2);
-				pcl::toROSMsg(*pclScanNormal, *msg);
-				msg->header = scanMsg->header;
-				filtered_scan_pub_->publish(std::move(msg));
+				pclScanINormal.reset(new pcl::PointCloud<pcl::PointXYZINormal>);
+				pcl::concatenateFields(*pclScanI, *normals, *pclScanINormal);
+				scan = util3d::laserScan2dFromPointCloud(*pclScanINormal);
+			}
+			else
+			{
+				pclScanNormal.reset(new pcl::PointCloud<pcl::PointNormal>);
+				pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+				scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
 			}
 		}
 		else
 		{
-			scan = util3d::laserScan2dFromPointCloud(*pclScan).data();
-
-			if(filtered_scan_pub_->get_subscription_count())
+			if(hasIntensity)
 			{
-				sensor_msgs::msg::PointCloud2::UniquePtr msg(new sensor_msgs::msg::PointCloud2);
-				pcl::toROSMsg(*pclScan, *msg);
-				msg->header = scanMsg->header;
-				filtered_scan_pub_->publish(std::move(msg));
+				scan = util3d::laserScan2dFromPointCloud(*pclScanI);
+			}
+			else
+			{
+				scan = util3d::laserScan2dFromPointCloud(*pclScan);
 			}
 		}
 	}

+	if(scanRangeMin_ > 0 || scanRangeMax_ > 0)
+	{
+		scan = util3d::rangeFiltering(scan, scanRangeMin_, scanRangeMax_);
+	}
+
 	rtabmap::SensorData data(
-			LaserScan::backwardCompatibility(scan, maxLaserScans, scanMsg->range_max, localScanTransform),
+			LaserScan(scan,
+					maxLaserScans,
+					scanRangeMax_>0&&scanRangeMax_<scanMsg->range_max?scanRangeMax_:scanMsg->range_max,
+					localScanTransform),
 			cv::Mat(),
 			cv::Mat(),
 			CameraModel(),
 			0,
 			rtabmap_ros::timestampFromROS(scanMsg->header.stamp));

-	this->processData(data, scanMsg->header.stamp);
+	this->processData(data, scanMsg->header);
 }

 void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr pointCloudMsg)
 {
+	UASSERT_MSG(pointCloudMsg->data.size() == pointCloudMsg->row_step*pointCloudMsg->height,
+			uFormat("data=%d row_step=%d height=%d", pointCloudMsg->data.size(), pointCloudMsg->row_step, pointCloudMsg->height).c_str());
+
+	if(scanReceived_)
+	{
+		RCLCPP_ERROR(this->get_logger(), "%s is already receiving scans on \"%s\", but also "
+				"just received a cloud on \"%s\". Both subscribers cannot be "
+				"used at the same time! Disabling cloud subscriber.",
+				this->get_name(), scan_sub_->get_topic_name(), cloud_sub_->get_topic_name());
+		cloud_sub_.reset();
+		return;
+	}
+	cloudReceived_ = true;
 	if(this->isPaused())
 	{
 		return;
@@ -348,21 +466,36 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 			}
 		}
 	}
-	else*/
+	else */
 	{
-	cloudMsg = *pointCloudMsg;
+		cloudMsg = *pointCloudMsg;
 	}

-	cv::Mat scan;
-	bool containNormals = false;
-	if(scanVoxelSize_ == 0.0f)
+	LaserScan scan;
+	bool hasNormals = false;
+	bool hasIntensity = false;
+	for(unsigned int i=0; i<cloudMsg.fields.size(); ++i)
 	{
-		for(unsigned int i=0; i<cloudMsg.fields.size(); ++i)
+		if(scanVoxelSize_ == 0.0f && cloudMsg.fields[i].name.compare("normal_x") == 0)
 		{
-			if(cloudMsg.fields[i].name.compare("normal_x") == 0)
+			hasNormals = true;
+		}
+		if(cloudMsg.fields[i].name.compare("intensity") == 0)
+		{
+			if(cloudMsg.fields[i].datatype == sensor_msgs::msg::PointField::FLOAT32)
 			{
-				containNormals = true;
-				break;
+				hasIntensity = true;
+			}
+			else
+			{
+				static bool warningShown = false;
+				if(!warningShown)
+				{
+					RCLCPP_WARN(this->get_logger(), "The input scan cloud has an \"intensity\" field "
+							"but the datatype (%d) is not supported. Intensity will be ignored. "
+							"This message is only shown once.", cloudMsg.fields[i].datatype);
+					warningShown = true;
+				}
 			}
 		}
 	}
@@ -380,23 +513,93 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 				"cloud is not dense, for convenience it will be set to %d (%dx%d)",
 				scanCloudMaxPoints_, cloudMsg.width, cloudMsg.height);
 	}
+	else if(cloudMsg.height > 1 && scanCloudMaxPoints_ < int(cloudMsg.height * cloudMsg.width))
+	{
+		RCLCPP_WARN(this->get_logger(), "IcpOdometry: \"scan_cloud_max_points\" is set to %d but input "
+				"cloud is not dense and has a size of %d (%dx%d), setting to this later size.",
+				scanCloudMaxPoints_, cloudMsg.width *cloudMsg.height, cloudMsg.width, cloudMsg.height);
+		scanCloudMaxPoints_ = cloudMsg.width *cloudMsg.height;
+	}
 	int maxLaserScans = scanCloudMaxPoints_;
-	if(containNormals)
+
+	if(hasNormals && hasIntensity)
+	{
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZINormal>);
+		pcl::fromROSMsg(cloudMsg, *pclScan);
+		if(pclScan->size() && scanDownsamplingStep_ > 1)
+		{
+			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
+			if(pclScan->height>1)
+			{
+				maxLaserScans = pclScan->height * pclScan->width;
+			}
+			else
+			{
+				maxLaserScans /= scanDownsamplingStep_;
+			}
+		}
+		scan = util3d::laserScanFromPointCloud(*pclScan);
+	}
+	else if(hasNormals)
 	{
 		pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
 		pcl::fromROSMsg(cloudMsg, *pclScan);
 		if(pclScan->size() && scanDownsamplingStep_ > 1)
 		{
 			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
-			maxLaserScans /= scanDownsamplingStep_;
+			if(pclScan->height>1)
+			{
+				maxLaserScans = pclScan->height * pclScan->width;
+			}
+			else
+			{
+				maxLaserScans /= scanDownsamplingStep_;
+			}
 		}
-		scan = util3d::laserScanFromPointCloud(*pclScan).data();
-		if(filtered_scan_pub_->get_subscription_count())
+		scan = util3d::laserScanFromPointCloud(*pclScan);
+	}
+	else if(hasIntensity)
+	{
+		pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
+		pcl::fromROSMsg(cloudMsg, *pclScan);
+		if(pclScan->size() && scanDownsamplingStep_ > 1)
 		{
-			sensor_msgs::msg::PointCloud2::UniquePtr msg(new sensor_msgs::msg::PointCloud2);
-			pcl::toROSMsg(*pclScan, *msg);
-			msg->header = cloudMsg.header;
-			filtered_scan_pub_->publish(std::move(msg));
+			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
+			if(pclScan->height>1)
+			{
+				maxLaserScans = pclScan->height * pclScan->width;
+			}
+			else
+			{
+				maxLaserScans /= scanDownsamplingStep_;
+			}
+		}
+		if(!pclScan->is_dense)
+		{
+			pclScan = util3d::removeNaNFromPointCloud(pclScan);
+		}
+
+		if(pclScan->size())
+		{
+			if(scanVoxelSize_ > 0.0f)
+			{
+				float pointsBeforeFiltering = (float)pclScan->size();
+				pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
+				float ratio = float(pclScan->size()) / pointsBeforeFiltering;
+				maxLaserScans = int(float(maxLaserScans) * ratio);
+			}
+			if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
+			{
+				//compute normals
+				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_);
+				pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointXYZINormal>);
+				pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+				scan = util3d::laserScanFromPointCloud(*pclScanNormal);
+			}
+			else
+			{
+				scan = util3d::laserScanFromPointCloud(*pclScan);
+			}
 		}
 	}
 	else
@@ -406,7 +609,15 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 		if(pclScan->size() && scanDownsamplingStep_ > 1)
 		{
 			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
-			maxLaserScans /= scanDownsamplingStep_;
+			if(pclScan->height>1)
+			{
+				maxLaserScans = pclScan->height * pclScan->width;
+			}
+			else
+			{
+				maxLaserScans /= scanDownsamplingStep_;
+			}
+
 		}
 		if(!pclScan->is_dense)
 		{
@@ -428,36 +639,27 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_);
 				pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
 				pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
-				scan = util3d::laserScanFromPointCloud(*pclScanNormal).data();
-
-				if(filtered_scan_pub_->get_subscription_count())
-				{
-					sensor_msgs::msg::PointCloud2::UniquePtr msg(new sensor_msgs::msg::PointCloud2);
-					pcl::toROSMsg(*pclScanNormal, *msg);
-					msg->header = cloudMsg.header;
-					filtered_scan_pub_->publish(std::move(msg));
-				}
+				scan = util3d::laserScanFromPointCloud(*pclScanNormal);
 			}
 			else
 			{
-				scan = util3d::laserScanFromPointCloud(*pclScan).data();
-
-				if(filtered_scan_pub_->get_subscription_count())
-				{
-					sensor_msgs::msg::PointCloud2::UniquePtr msg(new sensor_msgs::msg::PointCloud2);
-					pcl::toROSMsg(*pclScan, *msg);
-					msg->header = cloudMsg.header;
-					filtered_scan_pub_->publish(std::move(msg));
-				}
+				scan = util3d::laserScanFromPointCloud(*pclScan);
 			}
 		}
 	}

-	LaserScan laserScan = LaserScan::backwardCompatibility(scan, maxLaserScans, 0, localScanTransform);
+	LaserScan laserScan(scan,
+			maxLaserScans,
+			0,
+			localScanTransform);
 	if(scanRangeMin_ > 0 || scanRangeMax_ > 0)
 	{
 		laserScan = util3d::rangeFiltering(laserScan, scanRangeMin_, scanRangeMax_);
 	}
+	if(!laserScan.isEmpty() && laserScan.hasNormals() && !laserScan.is2d() && scanNormalGroundUp_)
+	{
+		laserScan = util3d::adjustNormalsToViewPoint(laserScan, Eigen::Vector3f(0,0,10), (float)scanNormalGroundUp_);
+	}

 	rtabmap::SensorData data(
 			laserScan,
@@ -467,7 +669,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 			0,
 			rtabmap_ros::timestampFromROS(cloudMsg.header.stamp));

-	this->processData(data, cloudMsg.header.stamp);
+	this->processData(data, cloudMsg.header);
 }

 void ICPOdometry::flushCallbacks()
@@ -475,6 +677,17 @@ void ICPOdometry::flushCallbacks()
 	// flush callbacks
 }

+void ICPOdometry::postProcessData(const SensorData & data, const std_msgs::msg::Header & header) const
+{
+	if(filtered_scan_pub_->get_subscription_count())
+	{
+		sensor_msgs::msg::PointCloud2::UniquePtr msg(new sensor_msgs::msg::PointCloud2);
+		pcl_conversions::fromPCL(*rtabmap::util3d::laserScanToPointCloud2(data.laserScanRaw()), *msg);
+		msg->header = header;
+		filtered_scan_pub_->publish(std::move(msg));
+	}
+}
+
 }

 #include "rclcpp_components/register_node_macro.hpp"
@@ -88,7 +88,8 @@ private:

 				tf::StampedTransform tmp;
 				tfListener_.lookupTransform(msg->header.frame_id, baseFrameId_, msg->header.stamp, tmp);
-				st *= tmp;
+				tf::Transform t = tmp.inverse()*st*tmp;
+				st.setRotation(t.getRotation());
 				st.child_frame_id_ = baseFrameId_;
 			}
 			catch(tf::TransformException & ex)
@@ -49,8 +49,6 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) :
 	ULogger::setType(ULogger::kTypeConsole);
 	ULogger::setLevel(ULogger::kWarning);

-	int queueSize = 10;
-	queueSize = this->declare_parameter("queue_size", queueSize);
 	frameId_ = this->declare_parameter("frame_id", frameId_);
 	mapFrameId_ = this->declare_parameter("map_frame_id", mapFrameId_);
 	waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
@@ -78,11 +76,11 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) :
 	tfBuffer_ = std::make_shared< tf2_ros::Buffer >(this->get_clock());
 	tfListener_ = std::make_shared< tf2_ros::TransformListener >(*tfBuffer_);

-	cloudSub_ = create_subscription<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::SensorDataQoS(), std::bind(&ObstaclesDetection::callback, this, std::placeholders::_1));
+	cloudSub_ = create_subscription<sensor_msgs::msg::PointCloud2>("cloud", 5, std::bind(&ObstaclesDetection::callback, this, std::placeholders::_1));

-	groundPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("ground", 1);
-	obstaclesPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("obstacles", 1);
-	projObstaclesPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("proj_obstacles", 1);
+	groundPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("ground", 5);
+	obstaclesPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("obstacles", 5);
+	projObstaclesPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("proj_obstacles", 5);
 }


@@ -102,6 +100,7 @@ void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstShar
 	if(localTransform.isNull())
 	{
 		RCLCPP_ERROR(this->get_logger(), "Failed to get transform between %s and %s frames", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
+		return;
 	}

 	rtabmap::Transform pose = rtabmap::Transform::getIdentity();
@@ -115,6 +114,9 @@ void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstShar
 		}
 	}

+	UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height,
+			uFormat("data=%d row_step=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->height).c_str());
+
 	pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::fromROSMsg(*cloudMsg, *inputCloud);
 	if(inputCloud->isOrganized())
@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_ros/MsgConversion.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/core/util3d_filtering.h>

 namespace rtabmap_ros
 {
@@ -47,9 +48,9 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
 	exactSync3_(0),
 	approxSync3_(0),
 	exactSync2_(0),
-	approxSync2_(0)
+	approxSync2_(0),
+	waitForTransform_(0.1)
 {
-
 	tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
 	//auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
 	//	this->get_node_base_interface(),
@@ -65,17 +66,18 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
 	fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
 	approx = this->declare_parameter("approx_sync", approx);
 	count = this->declare_parameter("count", count);
+	waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);

 	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);
+	cloudSub_1_.subscribe(this, "cloud1");
+	cloudSub_2_.subscribe(this, "cloud2");

 	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);
+		cloudSub_3_.subscribe(this, "cloud3");
+		cloudSub_4_.subscribe(this, "cloud4");
 		if(approx)
 		{
 			approxSync4_ = new message_filters::Synchronizer<ApproxSync4Policy>(ApproxSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
@@ -96,7 +98,7 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
 	}
 	else if(count == 3)
 	{
-		cloudSub_3_.subscribe(this, "cloud3", rmw_qos_profile_sensor_data);
+		cloudSub_3_.subscribe(this, "cloud3");
 		if(approx)
 		{
 			approxSync3_ = new message_filters::Synchronizer<ApproxSync3Policy>(ApproxSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
@@ -209,23 +211,23 @@ void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::Poi
 	UASSERT(cloudMsgs.size() > 1);
 	if(cloudPub_->get_subscription_count())
 	{
-		pcl::PCLPointCloud2 output;
+		pcl::PCLPointCloud2::Ptr output(new pcl::PCLPointCloud2);

 		std::string frameId = frameId_;
 		if(!frameId.empty() && frameId.compare(cloudMsgs[0]->header.frame_id) != 0)
 		{
 			sensor_msgs::msg::PointCloud2 tmp;
-			rtabmap::Transform t = rtabmap_ros::getTransform(frameId, cloudMsgs[0]->header.frame_id, cloudMsgs[0]->header.stamp, *tfBuffer_, 0.1);
+			rtabmap::Transform t = rtabmap_ros::getTransform(frameId, cloudMsgs[0]->header.frame_id, cloudMsgs[0]->header.stamp, *tfBuffer_, waitForTransform_);
 			if(t.isNull())
 			{
 				return;
 			}
 			rtabmap_ros::transformPointCloud(t.toEigen4f(), *cloudMsgs[0], tmp);
-			pcl_conversions::toPCL(tmp, output);
+			pcl_conversions::toPCL(tmp, *output);
 		}
 		else
 		{
-			pcl_conversions::toPCL(*cloudMsgs[0], output);
+			pcl_conversions::toPCL(*cloudMsgs[0], *output);
 			frameId = cloudMsgs[0]->header.frame_id;
 		}

@@ -242,26 +244,25 @@ void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::Poi
 						cloudMsgs[i]->header.stamp, //stampSource
 						cloudMsgs[0]->header.stamp, //stampTarget
 						*tfBuffer_,
-						0.1);
+						waitForTransform_);
 			}

-			pcl::PCLPointCloud2 cloud2;
+			pcl::PCLPointCloud2::Ptr cloud2(new pcl::PCLPointCloud2);
 			if(frameId.compare(cloudMsgs[i]->header.frame_id) != 0)
 			{
 				sensor_msgs::msg::PointCloud2 tmp;
-				rtabmap::Transform t = rtabmap_ros::getTransform(frameId, cloudMsgs[i]->header.frame_id, cloudMsgs[i]->header.stamp, *tfBuffer_, 0.1);
+				rtabmap::Transform t = rtabmap_ros::getTransform(frameId, cloudMsgs[i]->header.frame_id, cloudMsgs[i]->header.stamp, *tfBuffer_, waitForTransform_);
 				rtabmap_ros::transformPointCloud(t.toEigen4f(), *cloudMsgs[i], tmp);
 				if(!cloudDisplacement.isNull())
 				{
 					sensor_msgs::msg::PointCloud2 tmp2;
 					rtabmap_ros::transformPointCloud(cloudDisplacement.toEigen4f(), tmp, tmp2);
-					pcl_conversions::toPCL(tmp2, cloud2);
+					pcl_conversions::toPCL(tmp2, *cloud2);
 				}
 				else
 				{
-					pcl_conversions::toPCL(tmp, cloud2);
+					pcl_conversions::toPCL(tmp, *cloud2);
 				}
-
 			}
 			else
 			{
@@ -269,21 +270,33 @@ void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::Poi
 				{
 					sensor_msgs::msg::PointCloud2 tmp;
 					rtabmap_ros::transformPointCloud(cloudDisplacement.toEigen4f(), *cloudMsgs[i], tmp);
-					pcl_conversions::toPCL(tmp, cloud2);
+					pcl_conversions::toPCL(tmp, *cloud2);
 				}
 				else
 				{
-					pcl_conversions::toPCL(*cloudMsgs[i], cloud2);
+					pcl_conversions::toPCL(*cloudMsgs[i], *cloud2);
 				}
 			}

-			pcl::PCLPointCloud2 tmp_output;
-			pcl::concatenatePointCloud(output, cloud2, tmp_output);
+			if(!cloud2->is_dense)
+			{
+				// remove nans
+				cloud2 = rtabmap::util3d::removeNaNFromPointCloud(cloud2);
+			}
+
+			pcl::PCLPointCloud2::Ptr tmp_output(new pcl::PCLPointCloud2);
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+			pcl::concatenate(*output, *cloud2, *tmp_output);
+#else
+			pcl::concatenatePointCloud(*output, *cloud2, *tmp_output);
+#endif
+			//Make sure row_step is the sum of both
+			tmp_output->row_step = tmp_output->width * tmp_output->point_step;
 			output = tmp_output;
 		}

 		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.frame_id = frameId;
 		cloudPub_->publish(std::move(rosCloud));
@@ -32,10 +32,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl_conversions/pcl_conversions.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/radius_outlier_removal.h>

 #include <rtabmap_ros/MsgConversion.h>
+#include <rtabmap_ros/msg/odom_info.hpp>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/Version.h>

 namespace rtabmap_ros
 {
@@ -45,15 +48,23 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
 	warningThread_(0),
 	callbackCalled_(false),
 	exactSync_(0),
+	exactInfoSync_(0),
 	maxClouds_(0),
 	skipClouds_(0),
 	cloudsSkipped_(0),
+	circularBuffer_(false),
+	linearUpdate_(0),
+	angularUpdate_(0),
 	assemblingTime_(0),
-	waitForTransformDuration_(0.1),
+	waitForTransform_(0.1),
 	rangeMin_(0),
 	rangeMax_(0),
 	voxelSize_(0),
-	fixedFrameId_("odom")
+	noiseRadius_(0),
+	noiseMinNeighbors_(5),
+	removeZ_(false),
+	fixedFrameId_("odom"),
+	frameId_("")
 {
 	tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
 	//auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
@@ -63,66 +74,109 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
 	tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);

 	int queueSize = 5;
+	bool subscribeOdomInfo = false;

 	queueSize = this->declare_parameter("queue_size", queueSize);
 	fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
+	frameId_ = this->declare_parameter("frame_id", frameId_);
 	maxClouds_ = this->declare_parameter("max_clouds", maxClouds_);
 	assemblingTime_ = this->declare_parameter("assembling_time", assemblingTime_);
 	skipClouds_ = this->declare_parameter("skip_clouds", skipClouds_);
-	waitForTransformDuration_ = this->declare_parameter("wait_for_transform", waitForTransformDuration_);
+	circularBuffer_ = this->declare_parameter("circular_buffer", circularBuffer_);
+	linearUpdate_ = this->declare_parameter("linear_update", linearUpdate_);
+	angularUpdate_ = this->declare_parameter("angular_update", angularUpdate_);
+	waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
 	rangeMin_ = this->declare_parameter("range_min", rangeMin_);
 	rangeMax_ = this->declare_parameter("range_max", rangeMax_);
 	voxelSize_ = this->declare_parameter("voxel_size", voxelSize_);
-	UASSERT(maxClouds_>0 || assemblingTime_ >0.0);
+	noiseRadius_ = this->declare_parameter("noise_radius", noiseRadius_);
+	noiseMinNeighbors_ = this->declare_parameter("noise_min_neighbors", noiseMinNeighbors_);
+	removeZ_ = this->declare_parameter("remove_z", removeZ_);
+	subscribeOdomInfo = this->declare_parameter("subscribe_odom_info", subscribeOdomInfo);
+
+	RCLCPP_INFO(this->get_logger(), "%s: queue_size=%d", get_name(), queueSize);
+	RCLCPP_INFO(this->get_logger(), "%s: fixed_frame_id=%s", get_name(), fixedFrameId_.c_str());
+	RCLCPP_INFO(this->get_logger(), "%s: frame_id=%s", get_name(), frameId_.c_str());
+	RCLCPP_INFO(this->get_logger(), "%s: max_clouds=%d", get_name(), maxClouds_);
+	RCLCPP_INFO(this->get_logger(), "%s: assembling_time=%fs", get_name(), assemblingTime_);
+	RCLCPP_INFO(this->get_logger(), "%s: skip_clouds=%d", get_name(), skipClouds_);
+	RCLCPP_INFO(this->get_logger(), "%s: circular_buffer=%s", get_name(), circularBuffer_?"true":"false");
+	RCLCPP_INFO(this->get_logger(), "%s: linear_update=%f m", get_name(), linearUpdate_);
+	RCLCPP_INFO(this->get_logger(), "%s: angular_update=%f rad", get_name(), angularUpdate_);
+	RCLCPP_INFO(this->get_logger(), "%s: wait_for_transform=%f", get_name(), waitForTransform_);
+	RCLCPP_INFO(this->get_logger(), "%s: range_min=%f", get_name(), rangeMin_);
+	RCLCPP_INFO(this->get_logger(), "%s: range_max=%f", get_name(), rangeMax_);
+	RCLCPP_INFO(this->get_logger(), "%s: voxel_size=%fm", get_name(), voxelSize_);
+	RCLCPP_INFO(this->get_logger(), "%s: noise_radius=%fm", get_name(), noiseRadius_);
+	RCLCPP_INFO(this->get_logger(), "%s: noise_min_neighbors=%d", get_name(), noiseMinNeighbors_);
+	RCLCPP_INFO(this->get_logger(), "%s: remove_z=%s", get_name(), removeZ_?"true":"false");
+
+	if(maxClouds_==0 && assemblingTime_ ==0.0)
+	{
+		RCLCPP_ERROR(get_logger(), "point_cloud_assembler: max_cloud or assembling_time parameters should be set!");
+		exit(-1);
+	}

 	cloudsSkipped_ = skipClouds_;

 	cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("assembled_cloud", 1);

-	std::string subscribedTopicsMsg;
 	if(!fixedFrameId_.empty())
 	{
-		cloudSub_ = create_subscription<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::SensorDataQoS(), std::bind(&PointCloudAssembler::callbackCloud, this, std::placeholders::_1));
-		subscribedTopicsMsg = uFormat("\n%s subscribed to %s",
+		cloudSub_ = create_subscription<sensor_msgs::msg::PointCloud2>("cloud", 5, std::bind(&PointCloudAssembler::callbackCloud, this, std::placeholders::_1));
+		subscribedTopicsMsg_ = uFormat("\n%s subscribed to %s",
 							get_name(),
 							cloudSub_->get_topic_name());
 	}
+	else if(subscribeOdomInfo)
+	{
+		syncCloudSub_.subscribe(this, "cloud");
+		syncOdomSub_.subscribe(this, "odom");
+		syncOdomInfoSub_.subscribe(this, "odom_info");
+		exactInfoSync_ = new message_filters::Synchronizer<syncInfoPolicy>(syncInfoPolicy(queueSize), syncCloudSub_, syncOdomSub_, syncOdomInfoSub_);
+		exactInfoSync_->registerCallback(std::bind(&rtabmap_ros::PointCloudAssembler::callbackCloudOdomInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (exact sync):\n   %s,\n   %s",
+							get_name(),
+							syncCloudSub_.getTopic().c_str(),
+							syncOdomSub_.getTopic().c_str(),
+							syncOdomInfoSub_.getTopic().c_str());
+	}
 	else
 	{
-		syncCloudSub_.subscribe(this, "cloud", rmw_qos_profile_sensor_data);
-		syncOdomSub_.subscribe(this, "odom", rmw_qos_profile_sensor_data);
+		syncCloudSub_.subscribe(this, "cloud");
+		syncOdomSub_.subscribe(this, "odom");
 		exactSync_ = new message_filters::Synchronizer<syncPolicy>(syncPolicy(queueSize), syncCloudSub_, syncOdomSub_);
 		exactSync_->registerCallback(std::bind(&rtabmap_ros::PointCloudAssembler::callbackCloudOdom, this, std::placeholders::_1, std::placeholders::_2));
-		subscribedTopicsMsg = uFormat("\n%s subscribed to (exact sync):\n   %s,\n   %s",
+		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (exact sync):\n   %s,\n   %s",
 							get_name(),
 							syncCloudSub_.getTopic().c_str(),
 							syncOdomSub_.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",
-								get_name(),
-								subscribedTopicsMsg.c_str());
-					}
-				}
-			});
-
 	}

-	RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.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",
+						get_name(),
+						subscribedTopicsMsg_.c_str());
+			}
+		}
+	});
+
+	RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg_.c_str());
 }

 PointCloudAssembler::~PointCloudAssembler()
 {
 	delete exactSync_;
+	delete exactInfoSync_;

 	if(warningThread_)
 	{
@@ -150,83 +204,299 @@ void PointCloudAssembler::callbackCloudOdom(
 	}
 }

+sensor_msgs::msg::PointCloud2 removeField(const sensor_msgs::msg::PointCloud2 & input, const std::string & field)
+{
+	sensor_msgs::msg::PointCloud2 output;
+	int offset = 0;
+	std::vector<int> inputFieldIndex;
+	for(size_t i=0; i<input.fields.size(); ++i)
+	{
+		if(input.fields[i].name.compare(field) == 0)
+		{
+			continue;
+		}
+		else
+		{
+			sensor_msgs::msg::PointField outputField = input.fields[i];
+			outputField.offset = offset;
+			offset += outputField.count * sizeOfPointField(outputField.datatype);
+			output.fields.push_back(outputField);
+			inputFieldIndex.push_back(i);
+		}
+	}
+	output.header = input.header;
+	output.height = input.height;
+	output.width = input.width;
+	output.is_bigendian = input.is_bigendian;
+	output.is_dense = input.is_dense;
+	output.point_step = offset;
+	output.row_step = output.width * output.point_step;
+	output.data.resize(output.height*output.row_step);
+	int total = output.height*output.width;
+	for(int i=0; i<total; ++i)
+	{
+		// for each point, copy fields
+		int oi = i*output.point_step;
+		int pi = i*input.point_step;
+		for(size_t j=0;j<output.fields.size(); ++j)
+		{
+			memcpy(&output.data[oi + output.fields[j].offset],
+				   &input.data[pi + input.fields[inputFieldIndex[j]].offset],
+				   output.fields[j].count * sizeOfPointField(output.fields[j].datatype));
+		}
+	}
+	return output;
+}
+
+void PointCloudAssembler::callbackCloudOdomInfo(
+			const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg,
+			const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+			const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	callbackCalled_ = true;
+	rtabmap::Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
+	if(!odom.isNull())
+	{
+		if(odomInfoMsg->key_frame_added)
+		{
+			fixedFrameId_ = odomMsg->header.frame_id;
+			callbackCloud(cloudMsg);
+		}
+		else
+		{
+			RCLCPP_INFO(this->get_logger(), "Skipping non keyframe...");
+		}
+	}
+	else
+	{
+		RCLCPP_WARN(this->get_logger(), "Reseting point cloud assembler as null odometry has been received.");
+		clouds_.clear();
+	}
+}
+
 void PointCloudAssembler::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg)
 {
 	if(cloudPub_->get_subscription_count())
 	{
+		UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height,
+						uFormat("data=%d row_step=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->height).c_str());
+
 		if(skipClouds_<=0 || cloudsSkipped_ >= skipClouds_)
 		{
 			cloudsSkipped_ = 0;

-			sensor_msgs::msg::PointCloud2::SharedPtr cpy(new sensor_msgs::msg::PointCloud2);
-			*cpy = *cloudMsg;
-			clouds_.push_back(cpy);
+			rtabmap::Transform pose = rtabmap_ros::getTransform(
+					fixedFrameId_, //fromFrame
+					cloudMsg->header.frame_id, //toFrame
+					cloudMsg->header.stamp,
+					*tfBuffer_,
+					waitForTransform_);

-			if(  ((int)clouds_.size() >= maxClouds_ && maxClouds_ > 0)
-				||
-				 (timestampFromROS((*cpy).header.stamp) >= timestampFromROS(clouds_[0]->header.stamp) + assemblingTime_ && assemblingTime_ > 0.0))
+			if(pose.isNull())
+			{
+				RCLCPP_ERROR(get_logger(), "Cloud not transform all clouds! Resetting...");
+				clouds_.clear();
+				return;
+			}
+
+			bool isMoving = true;
+			if(!previousPose_.isNull() && (linearUpdate_>0 || angularUpdate_>0))
+			{
+				rtabmap::Transform delta = previousPose_.inverse()*pose;
+				float roll, pitch, yaw;
+				delta.getEulerAngles(roll, pitch, yaw);
+				isMoving = fabs(delta.x()) > linearUpdate_ ||
+								fabs(delta.y()) > linearUpdate_ ||
+								fabs(delta.z()) > linearUpdate_ ||
+								(angularUpdate_>0.0f && (
+									fabs(roll) > angularUpdate_ ||
+									fabs(pitch) > angularUpdate_ ||
+									fabs(yaw) > angularUpdate_));
+			}
+
+			pcl::PCLPointCloud2::Ptr newCloud(new pcl::PCLPointCloud2);
+			if(rangeMin_ > 0.0 || rangeMax_ > 0.0 || voxelSize_ > 0.0f)
+			{
+				pcl_conversions::toPCL(*cloudMsg, *newCloud);
+				rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(*newCloud);
+				scan = rtabmap::util3d::commonFiltering(scan, 1, rangeMin_, rangeMax_, voxelSize_);
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+				std::uint64_t stamp = newCloud->header.stamp;
+#else
+				pcl::uint64_t stamp = newCloud->header.stamp;
+#endif
+				newCloud = rtabmap::util3d::laserScanToPointCloud2(scan, pose);
+				newCloud->header.stamp = stamp;
+			}
+			else
+			{
+				sensor_msgs::msg::PointCloud2 output;
+				transformPointCloud(pose.toEigen4f(), *cloudMsg, output);
+				pcl_conversions::toPCL(output, *newCloud);
+			}
+
+			if(!newCloud->is_dense)
+			{
+				// remove nans
+				newCloud = rtabmap::util3d::removeNaNFromPointCloud(newCloud);
+			}
+
+			clouds_.push_back(newCloud);
+
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+			bool reachedMaxSize =
+					((int)clouds_.size() >= maxClouds_ && maxClouds_ > 0)
+					||
+					((*newCloud).header.stamp >= clouds_.front()->header.stamp + static_cast<std::uint64_t>(assemblingTime_*1000000.0) && assemblingTime_ > 0.0);
+#else
+			bool reachedMaxSize =
+					((int)clouds_.size() >= maxClouds_ && maxClouds_ > 0)
+					||
+					((*newCloud).header.stamp >= clouds_.front()->header.stamp + static_cast<pcl::uint64_t>(assemblingTime_*1000000.0) && assemblingTime_ > 0.0);
+#endif
+
+			if( circularBuffer_ || reachedMaxSize )
 			{
 				pcl::PCLPointCloud2Ptr assembled(new pcl::PCLPointCloud2);
-				pcl_conversions::toPCL(*clouds_.back(), *assembled);
-
-				for(size_t i=0; i<clouds_.size()-1; ++i)
+				for(std::list<pcl::PCLPointCloud2::Ptr>::iterator iter=clouds_.begin(); iter!=clouds_.end(); ++iter)
 				{
-					rtabmap::Transform t = rtabmap_ros::getTransform(
-							clouds_[i]->header.frame_id, //sourceTargetFrame
-							fixedFrameId_, //fixedFrame
-							clouds_[i]->header.stamp, //stampSource
-							clouds_.back()->header.stamp, //stampTarget
-							*tfBuffer_,
-							waitForTransformDuration_);
-
-					if(t.isNull())
+					if(assembled->data.empty())
 					{
-						RCLCPP_ERROR(this->get_logger(), "Cloud not transform all clouds! Resetting...");
-						clouds_.clear();
-						return;
-					}
-
-					pcl::PCLPointCloud2Ptr assembledTmp(new pcl::PCLPointCloud2);
-					if(rangeMin_ > 0.0 || rangeMax_ > 0.0)
-					{
-						pcl::PCLPointCloud2 output2;
-						pcl_conversions::toPCL(*clouds_[i], output2);
-						rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(output2);
-						if(rangeMin_ > 0.0 || rangeMax_ > 0.0)
-						{
-							scan = rtabmap::util3d::rangeFiltering(scan, rangeMin_, rangeMax_);
-						}
-						pcl::concatenatePointCloud(*assembled, *rtabmap::util3d::laserScanToPointCloud2(scan, t), *assembledTmp);
+						*assembled = *(*iter);
 					}
 					else
 					{
-						sensor_msgs::msg::PointCloud2 output;
-						rtabmap_ros::transformPointCloud(t.toEigen4f(), *clouds_[i], output);
-						pcl::PCLPointCloud2 output2;
-						pcl_conversions::toPCL(output, output2);
-						pcl::concatenatePointCloud(*assembled, output2, *assembledTmp);
+						pcl::PCLPointCloud2Ptr assembledTmp(new pcl::PCLPointCloud2);
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+						pcl::concatenate(*assembled, *(*iter), *assembledTmp);
+#else
+						pcl::concatenatePointCloud(*assembled, *(*iter), *assembledTmp);
+#endif
+						//Make sure row_step is the sum of both
+						assembledTmp->row_step = assembled->row_step + (*iter)->row_step;
+						assembled = assembledTmp;
 					}
-
-					assembled = assembledTmp;
 				}

 				sensor_msgs::msg::PointCloud2 rosCloud;
 				if(voxelSize_>0.0)
 				{
-					pcl::VoxelGrid<pcl::PCLPointCloud2> filter;
-					filter.setLeafSize(voxelSize_, voxelSize_, voxelSize_);
+					// estimate if there would be an overflow
+					int x_idx=-1, y_idx=-1, z_idx=-1;
+					for (std::size_t d = 0; d < assembled->fields.size (); ++d)
+					{
+						if (assembled->fields[d].name.compare("x")==0)
+							x_idx = d;
+						if (assembled->fields[d].name.compare("y")==0)
+							y_idx = d;
+						if (assembled->fields[d].name.compare("z")==0)
+							z_idx = d;
+					}
+					bool overflow = false;
+					if(x_idx>=0 && y_idx>=0 && z_idx>=0) {
+						Eigen::Vector4f min_p, max_p;
+						pcl::getMinMax3D(assembled, x_idx, y_idx, z_idx, min_p, max_p);
+						float inverseVoxelSize = 1.0f/voxelSize_;
+						std::int64_t dx = static_cast<std::int64_t>((max_p[0] - min_p[0]) * inverseVoxelSize)+1;
+						std::int64_t dy = static_cast<std::int64_t>((max_p[1] - min_p[1]) * inverseVoxelSize)+1;
+						std::int64_t dz = static_cast<std::int64_t>((max_p[2] - min_p[2]) * inverseVoxelSize)+1;
+
+						if ((dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()))
+						{
+							overflow = true;
+						}
+					}
+					if(overflow)
+					{
+						rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(*assembled);
+						scan = rtabmap::util3d::commonFiltering(scan, 1, 0, 0, voxelSize_);
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+						std::uint64_t stamp = assembled->header.stamp;
+#else
+						pcl::uint64_t stamp = assembled->header.stamp;
+#endif
+						assembled = rtabmap::util3d::laserScanToPointCloud2(scan);
+						assembled->header.stamp = stamp;
+					}
+					else
+					{
+						pcl::VoxelGrid<pcl::PCLPointCloud2> filter;
+						filter.setLeafSize(voxelSize_, voxelSize_, voxelSize_);
+						filter.setInputCloud(assembled);
+						pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
+						filter.filter(*output);
+						assembled = output;
+					}
+				}
+				if(noiseRadius_>0.0 && noiseMinNeighbors_>0)
+				{
+					pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2> filter;
+					filter.setRadiusSearch(noiseRadius_);
+					filter.setMinNeighborsInRadius(noiseMinNeighbors_);
 					filter.setInputCloud(assembled);
 					pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
 					filter.filter(*output);
-					pcl_conversions::moveFromPCL(*output, rosCloud);
+					assembled = output;
+				}
+				pcl_conversions::moveFromPCL(*assembled, rosCloud);
+				rtabmap::Transform t = pose;
+				if(!frameId_.empty())
+				{
+					// transform in target frame_id instead of sensor frame
+					t = rtabmap_ros::getTransform(
+							fixedFrameId_, //fromFrame
+							frameId_, //toFrame
+							cloudMsg->header.stamp,
+							*tfBuffer_,
+							waitForTransform_);
+					if(t.isNull())
+					{
+						RCLCPP_ERROR(this->get_logger(), "Cloud not transform back assembled clouds in target frame \"%s\"! Resetting...", frameId_.c_str());
+						clouds_.clear();
+						return;
+					}
+				}
+				transformPointCloud(t.toEigen4f().inverse(), rosCloud, rosCloud);
+
+				if(removeZ_)
+				{
+					rosCloud = removeField(rosCloud, "z");
+				}
+
+				rosCloud.header = cloudMsg->header;
+				if(!frameId_.empty())
+				{
+					rosCloud.header.frame_id = frameId_;
+				}
+				cloudPub_->publish(rosCloud);
+				if(circularBuffer_)
+				{
+					if(!isMoving)
+					{
+						clouds_.pop_back();
+					}
+					else
+					{
+						previousPose_ = pose;
+						if(reachedMaxSize)
+						{
+							clouds_.pop_front();
+						}
+					}
 				}
 				else
 				{
-					pcl_conversions::moveFromPCL(*assembled, rosCloud);
+					clouds_.clear();
+					previousPose_.setNull();
 				}
-				rosCloud.header = cloudMsg->header;
-				cloudPub_->publish(rosCloud);
-				clouds_.clear();
+			}
+			else if(!isMoving)
+			{
+				clouds_.pop_back();
+			}
+			else
+			{
+				previousPose_ = pose;
 			}
 		}
 		else
@@ -133,11 +133,11 @@ PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) :
 	cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 1);

 	image_transport::TransportHints hints(this);
-	imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rmw_qos_profile_sensor_data);
-	cameraInfoSub_.subscribe(this, "depth/camera_info", rmw_qos_profile_sensor_data);
+	imageDepthSub_.subscribe(this, "depth/image", hints.getTransport());
+	cameraInfoSub_.subscribe(this, "depth/camera_info");

-	disparitySub_.subscribe(this, "disparity/image", rmw_qos_profile_sensor_data);
-	disparityCameraInfoSub_.subscribe(this, "disparity/camera_info", rmw_qos_profile_sensor_data);
+	disparitySub_.subscribe(this, "disparity/image");
+	disparityCameraInfoSub_.subscribe(this, "disparity/camera_info");
 }

 PointCloudXYZ::~PointCloudXYZ()
@@ -240,10 +240,11 @@ void PointCloudXYZ::processAndPublish(pcl::PointCloud<pcl::PointXYZ>::Ptr & pclC
 	if(indices->size() && voxelSize_ > 0.0)
 	{
 		pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_);
+		pclCloud->is_dense = true;
 	}

 	// Do radius filtering after voxel filtering ( a lot faster)
-	if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
+	if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
 	{
 		if(pclCloud->is_dense)
 		{
@@ -259,7 +260,7 @@ void PointCloudXYZ::processAndPublish(pcl::PointCloud<pcl::PointXYZ>::Ptr & pclC
 	}

 	sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
-	if(pclCloud->size() && (normalK_ > 0 || normalRadius_ > 0.0f))
+if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && (normalK_ > 0 || normalRadius_ > 0.0f))
 	{
 		//compute normals
 		pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
@@ -130,7 +130,7 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) :

 	cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 1);

-	rgbdImageSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::SensorDataQoS(), std::bind(&PointCloudXYZRGB::rgbdImageCallback, this, std::placeholders::_1));
+	rgbdImageSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", 5, std::bind(&PointCloudXYZRGB::rgbdImageCallback, this, std::placeholders::_1));

 	if(approxSync)
 	{
@@ -157,16 +157,16 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) :
 	}

 	image_transport::TransportHints hints(this);
-	imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rmw_qos_profile_sensor_data);
-	imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rmw_qos_profile_sensor_data);
-	cameraInfoSub_.subscribe(this, "rgb/camera_info", rmw_qos_profile_sensor_data);
+	imageSub_.subscribe(this, "rgb/image", hints.getTransport());
+	imageDepthSub_.subscribe(this, "depth/image", hints.getTransport());
+	cameraInfoSub_.subscribe(this, "rgb/camera_info");

-	imageDisparitySub_.subscribe(this, "disparity", rmw_qos_profile_sensor_data);
+	imageDisparitySub_.subscribe(this, "disparity");

-	imageLeft_.subscribe(this, "left/image", hints.getTransport(), rmw_qos_profile_sensor_data);
-	imageRight_.subscribe(this, "right/image", hints.getTransport(), rmw_qos_profile_sensor_data);
-	cameraInfoLeft_.subscribe(this, "left/camera_info", rmw_qos_profile_sensor_data);
-	cameraInfoRight_.subscribe(this, "right/camera_info", rmw_qos_profile_sensor_data);
+	imageLeft_.subscribe(this, "left/image", hints.getTransport());
+	imageRight_.subscribe(this, "right/image", hints.getTransport());
+	cameraInfoLeft_.subscribe(this, "left/camera_info");
+	cameraInfoRight_.subscribe(this, "right/camera_info");
 }

 PointCloudXYZRGB::~PointCloudXYZRGB()
@@ -407,10 +407,11 @@ void PointCloudXYZRGB::processAndPublish(
 	if(indices->size() && voxelSize_ > 0.0)
 	{
 		pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_);
+		pclCloud->is_dense = true;
 	}

 	// Do radius filtering after voxel filtering ( a lot faster)
-	if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
+	if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
 	{
 		if(pclCloud->is_dense)
 		{
@@ -426,7 +427,7 @@ void PointCloudXYZRGB::processAndPublish(
 	}

 	sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
-	if(pclCloud->size() && (normalK_ > 0 || normalRadius_ > 0.0f))
+	if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && (normalK_ > 0 || normalRadius_ > 0.0f))
 	{
 		//compute normals
 		pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
@@ -91,6 +91,7 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio
 	image_transport::ImageTransport it(node);
 	depthImage16Pub_ = it.advertise("image_raw", 1); // 16 bits unsigned in mm
 	depthImage32Pub_ = it.advertise("image", 1);// 32 bits float in meters
+	pointCloudTransformedPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud_transformed", 1);

 	if(approx)
 	{
@@ -104,8 +105,8 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio
 		exactSync_->registerCallback(std::bind(&PointCloudToDepthImage::callback, this, std::placeholders::_1, std::placeholders::_2));
 	}

-	pointCloudSub_.subscribe(this, "cloud", rmw_qos_profile_sensor_data);
-	cameraInfoSub_.subscribe(this, "camera_info", rmw_qos_profile_sensor_data);
+	pointCloudSub_.subscribe(this, "cloud");
+	cameraInfoSub_.subscribe(this, "camera_info");
 }

 PointCloudToDepthImage::~PointCloudToDepthImage()
@@ -130,8 +131,8 @@ void PointCloudToDepthImage::callback(
 			cloudDisplacement = rtabmap_ros::getTransform(
 					pointCloud2Msg->header.frame_id,
 					fixedFrameId_,
-					pointCloud2Msg->header.stamp,
 					cameraInfoMsg->header.stamp,
+					pointCloud2Msg->header.stamp,
 					*tfBuffer_,
 					waitForTransform_);
 		}
@@ -153,7 +154,7 @@ void PointCloudToDepthImage::callback(
 			return;
 		}

-		rtabmap::Transform localTransform = cloudDisplacement.inverse()*cloudToCamera;
+		rtabmap::Transform localTransform = cloudDisplacement*cloudToCamera;

 		rtabmap::CameraModel model = rtabmap_ros::cameraModelFromROS(*cameraInfoMsg, localTransform);

@@ -172,6 +173,9 @@ void PointCloudToDepthImage::callback(
 			}
 		}

+		UASSERT_MSG(pointCloud2Msg->data.size() == pointCloud2Msg->row_step*pointCloud2Msg->height,
+						uFormat("data=%d row_step=%d height=%d", pointCloud2Msg->data.size(), pointCloud2Msg->row_step, pointCloud2Msg->height).c_str());
+
 		pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
 		pcl_conversions::toPCL(*pointCloud2Msg, *cloud);

@@ -192,6 +196,14 @@ void PointCloudToDepthImage::callback(
 				{
 					depthImage.image = rtabmap::util2d::fillDepthHoles(depthImage.image, fillHolesSize_, fillHolesError_);
 				}
+
+				if(pointCloudTransformedPub_->get_subscription_count()>0)
+				{
+					sensor_msgs::msg::PointCloud2 pointCloud2Out;
+					transformPointCloud(model.localTransform().inverse().toEigen4f(), *pointCloud2Msg, pointCloud2Out);
+					pointCloud2Out.header = cameraInfoMsg->header;
+					pointCloudTransformedPub_->publish(pointCloud2Out);
+				}
 			}
 		}

@@ -0,0 +1,181 @@
+/*
+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/rgb_sync.hpp"
+
+#include <sensor_msgs/msg/compressed_image.hpp>
+#include <sensor_msgs/image_encodings.hpp>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+
+#include "rtabmap/core/Compression.h"
+#include "rtabmap/core/util2d.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap_ros/MsgConversion.h"
+
+namespace rtabmap_ros
+{
+
+RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
+	Node("rgbd_sync", options),
+	compressedRate_(0),
+	warningThread_(0),
+	callbackCalled_(false),
+	approxSync_(0),
+	exactSync_(0)
+{
+	int queueSize = 10;
+	bool approxSync = true;
+	approxSync = this->declare_parameter("approx_sync", approxSync);
+	queueSize = this->declare_parameter("queue_size", queueSize);
+	compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_);
+
+	RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
+	RCLCPP_INFO(this->get_logger(), "%s: queue_size  = %d", get_name(), queueSize);
+	RCLCPP_INFO(this->get_logger(), "%s: compressed_rate = %f", get_name(), compressedRate_);
+
+	rgbdImagePub_ = this->create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image", 1);
+	rgbdImageCompressedPub_ = this->create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image/compressed", 1);
+
+	if(approxSync)
+	{
+		approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageSub_, cameraInfoSub_);
+		approxSync_->registerCallback(std::bind(&RGBSync::callback, this, std::placeholders::_1, std::placeholders::_2));
+	}
+	else
+	{
+		exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageSub_, cameraInfoSub_);
+		exactSync_->registerCallback(std::bind(&RGBSync::callback, this, std::placeholders::_1, std::placeholders::_2));
+	}
+
+	image_transport::TransportHints hints(this);
+	imageSub_.subscribe(this, "rgb/image", hints.getTransport());
+	cameraInfoSub_.subscribe(this, "rgb/camera_info");
+
+	subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s",
+						get_name(),
+						approxSync?"approx":"exact",
+						imageSub_.getSubscriber().getTopic().c_str(),
+						cameraInfoSub_.getSubscriber()->get_topic_name());
+
+	RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg_.c_str());
+
+	warningThread_ = new std::thread([&](){
+		rclcpp::Rate r(1/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(),
+						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());
+			}
+		}
+	});
+}
+
+
+RGBSync::~RGBSync()
+{
+	if(approxSync_)
+		delete approxSync_;
+	if(exactSync_)
+		delete exactSync_;
+
+	if(warningThread_)
+	{
+		callbackCalled_=true;
+		warningThread_->join();
+		delete warningThread_;
+	}
+}
+
+void RGBSync::callback(
+		  const sensor_msgs::msg::Image::ConstSharedPtr image,
+		  const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
+{
+	callbackCalled_ = true;
+	if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count())
+	{
+		double stamp = timestampFromROS(image->header.stamp);
+
+		rtabmap_ros::msg::RGBDImage msg;
+		msg.header.frame_id = cameraInfo->header.frame_id;
+		msg.header.stamp = image->header.stamp;
+		msg.rgb_camera_info = *cameraInfo;
+
+		if(rgbdImageCompressedPub_->get_subscription_count())
+		{
+			bool publishCompressed = true;
+			if (compressedRate_ > 0.0)
+			{
+				if ( lastCompressedPublished_ + rclcpp::Duration(1.0/compressedRate_) > now())
+				{
+					RCLCPP_DEBUG(this->get_logger(), "throttle last update at %f skipping", lastCompressedPublished_.seconds());
+					publishCompressed = false;
+				}
+			}
+
+			if(publishCompressed)
+			{
+				lastCompressedPublished_ = now();
+
+				rtabmap_ros::msg::RGBDImage msgCompressed = msg;
+
+				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
+				imagePtr->toCompressedImageMsg(msgCompressed.rgb_compressed, cv_bridge::JPG);
+
+				rgbdImageCompressedPub_->publish(msgCompressed);
+			}
+		}
+
+		if(rgbdImagePub_->get_subscription_count())
+		{
+			msg.rgb = *image;
+			rgbdImagePub_->publish(msg);
+		}
+
+		if( stamp != timestampFromROS(image->header.stamp))
+		{
+			RCLCPP_ERROR(this->get_logger(), "Input stamps changed between the beginning and the end of the callback! Make "
+					"sure the node publishing the topics doesn't override the same data after publishing them. A "
+					"solution is to use this node within another nodelet manager. Stamps: "
+					"%f->%f",
+					stamp, timestampFromROS(image->header.stamp));
+		}
+	}
+}
+
+}
+
@@ -53,7 +53,11 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) :
 		approxSync3_(0),
 		exactSync3_(0),
 		approxSync4_(0),
-		exactSync4_(0)
+		exactSync4_(0),
+		approxSync5_(0),
+		exactSync5_(0),
+		queueSize_(5),
+		keepColor_(false)
 {
 	OdometryROS::init(false, true, false);
 }
@@ -76,35 +80,43 @@ void RGBDOdometry::onOdomInit()
 	bool approxSync = true;
 	bool subscribeRGBD = false;
 	approxSync = this->declare_parameter("approx_sync", approxSync);
+	queueSize_ = this->declare_parameter("queue_size", queueSize_);
 	subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);
 	rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras);
 	if(rgbdCameras <= 0)
 	{
 		rgbdCameras = 1;
 	}
-	if(rgbdCameras > 4)
+	if(rgbdCameras > 5)
 	{
-		RCLCPP_FATAL(this->get_logger(), "Only 4 cameras maximum supported yet.");
+		RCLCPP_FATAL(this->get_logger(), "Only 5 cameras maximum supported yet.");
 	}
+	keepColor_ = this->declare_parameter("keep_color", keepColor_);

 	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync    = %s", approxSync?"true":"false");
+	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: queue_size     = %d", queueSize_);
 	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
 	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgbd_cameras   = %d", rgbdCameras);
+	RCLCPP_INFO(this->get_logger(), "RGBDOdometry: keep_color     = %s", keepColor_?"true":"false");

 	std::string subscribedTopicsMsg;
 	if(subscribeRGBD)
 	{
 		if(rgbdCameras >= 2)
 		{
-			rgbd_image1_sub_.subscribe(this, "rgbd_image0", rmw_qos_profile_sensor_data);
-			rgbd_image2_sub_.subscribe(this, "rgbd_image1", rmw_qos_profile_sensor_data);
+			rgbd_image1_sub_.subscribe(this, "rgbd_image0");
+			rgbd_image2_sub_.subscribe(this, "rgbd_image1");
 			if(rgbdCameras >= 3)
 			{
-				rgbd_image3_sub_.subscribe(this, "rgbd_image2", rmw_qos_profile_sensor_data);
+				rgbd_image3_sub_.subscribe(this, "rgbd_image2");
 			}
 			if(rgbdCameras >= 4)
 			{
-				rgbd_image4_sub_.subscribe(this, "rgbd_image3", rmw_qos_profile_sensor_data);
+				rgbd_image4_sub_.subscribe(this, "rgbd_image3");
+			}
+			if(rgbdCameras >= 5)
+			{
+				rgbd_image5_sub_.subscribe(this, "rgbd_image4");
 			}

 			if(rgbdCameras == 2)
@@ -112,7 +124,7 @@ void RGBDOdometry::onOdomInit()
 				if(approxSync)
 				{
 					approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
-							MyApproxSync2Policy(queueSize()),
+							MyApproxSync2Policy(queueSize_),
 							rgbd_image1_sub_,
 							rgbd_image2_sub_);
 					approxSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -120,7 +132,7 @@ void RGBDOdometry::onOdomInit()
 				else
 				{
 					exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
-							MyExactSync2Policy(queueSize()),
+							MyExactSync2Policy(queueSize_),
 							rgbd_image1_sub_,
 							rgbd_image2_sub_);
 					exactSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -136,7 +148,7 @@ void RGBDOdometry::onOdomInit()
 				if(approxSync)
 				{
 					approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
-							MyApproxSync3Policy(queueSize()),
+							MyApproxSync3Policy(queueSize_),
 							rgbd_image1_sub_,
 							rgbd_image2_sub_,
 							rgbd_image3_sub_);
@@ -145,7 +157,7 @@ void RGBDOdometry::onOdomInit()
 				else
 				{
 					exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
-							MyExactSync3Policy(queueSize()),
+							MyExactSync3Policy(queueSize_),
 							rgbd_image1_sub_,
 							rgbd_image2_sub_,
 							rgbd_image3_sub_);
@@ -163,7 +175,7 @@ void RGBDOdometry::onOdomInit()
 				if(approxSync)
 				{
 					approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
-							MyApproxSync4Policy(queueSize()),
+							MyApproxSync4Policy(queueSize_),
 							rgbd_image1_sub_,
 							rgbd_image2_sub_,
 							rgbd_image3_sub_,
@@ -173,7 +185,7 @@ void RGBDOdometry::onOdomInit()
 				else
 				{
 					exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
-							MyExactSync4Policy(queueSize()),
+							MyExactSync4Policy(queueSize_),
 							rgbd_image1_sub_,
 							rgbd_image2_sub_,
 							rgbd_image3_sub_,
@@ -188,10 +200,43 @@ void RGBDOdometry::onOdomInit()
 						rgbd_image3_sub_.getSubscriber()->get_topic_name(),
 						rgbd_image4_sub_.getSubscriber()->get_topic_name());
 			}
+			else if(rgbdCameras == 5)
+			{
+				if(approxSync)
+				{
+					approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
+							MyApproxSync5Policy(queueSize_),
+							rgbd_image1_sub_,
+							rgbd_image2_sub_,
+							rgbd_image3_sub_,
+							rgbd_image4_sub_,
+															rgbd_image5_sub_);
+					approxSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
+				}
+				else
+				{
+					exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
+							MyExactSync5Policy(queueSize_),
+							rgbd_image1_sub_,
+							rgbd_image2_sub_,
+							rgbd_image3_sub_,
+							rgbd_image4_sub_,
+															rgbd_image5_sub_);
+					exactSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
+				}
+				subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n  %s \\\n  %s \\\n   %s \\\n   %s",
+						get_name(),
+						approxSync?"approx":"exact",
+						rgbd_image1_sub_.getSubscriber()->get_topic_name(),
+						rgbd_image2_sub_.getSubscriber()->get_topic_name(),
+						rgbd_image3_sub_.getSubscriber()->get_topic_name(),
+						rgbd_image4_sub_.getSubscriber()->get_topic_name(),
+						rgbd_image5_sub_.getSubscriber()->get_topic_name());
+			}
 		}
 		else
 		{
-			rgbdSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::SensorDataQoS(), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1));
+			rgbdSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", queueSize_, std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1));

 			subscribedTopicsMsg =
 					uFormat("\n%s subscribed to:\n   %s",
@@ -202,18 +247,18 @@ void RGBDOdometry::onOdomInit()
 	else
 	{
 		image_transport::TransportHints hints(this);
-		image_mono_sub_.subscribe(this, "rgb/image", hints.getTransport(), rmw_qos_profile_sensor_data);
-		image_depth_sub_.subscribe(this, "depth/image", hints.getTransport(), rmw_qos_profile_sensor_data);
-		info_sub_.subscribe(this, "rgb/camera_info", rmw_qos_profile_sensor_data);
+		image_mono_sub_.subscribe(this, "rgb/image", hints.getTransport());
+		image_depth_sub_.subscribe(this, "depth/image", hints.getTransport());
+		info_sub_.subscribe(this, "rgb/camera_info");

 		if(approxSync)
 		{
-			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize()), image_mono_sub_, image_depth_sub_, info_sub_);
+			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
 			approxSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 		}
 		else
 		{
-			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize()), image_mono_sub_, image_depth_sub_, info_sub_);
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
 			exactSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 		}

@@ -268,9 +313,8 @@ void RGBDOdometry::commonCallback(
 	int depthHeight = depthImages[0]->image.rows;

 	UASSERT_MSG(
-		imageWidth % depthWidth == 0 && imageHeight % depthHeight == 0 &&
-		imageWidth/depthWidth == imageHeight/depthHeight,
-		uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
+			imageWidth/depthWidth == imageHeight/depthHeight,
+			uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());

 	int cameraCount = rgbImages.size();
 	cv::Mat rgb;
@@ -331,7 +375,14 @@ void RGBDOdometry::commonCallback(
 		if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
 		   rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
 		{
-			ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
+			if(keepColor_ && rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
+			{
+				ptrImage = cv_bridge::cvtColor(rgbImages[i], "bgr8");
+			}
+			else
+			{
+				ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
+			}
 		}

 		cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
@@ -377,7 +428,10 @@ void RGBDOdometry::commonCallback(
 			0,
 			rtabmap_ros::timestampFromROS(higherStamp));

-	this->processData(data, higherStamp);
+	std_msgs::msg::Header header;
+	header.stamp = higherStamp;
+	header.frame_id = rgbImages.size()==1?rgbImages[0]->header.frame_id:"";
+	this->processData(data, header);
 }

 void RGBDOdometry::callback(
@@ -481,26 +535,54 @@ void RGBDOdometry::callbackRGBD4(
 	}
 }

+void RGBDOdometry::callbackRGBD5(
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5)
+{
+	callbackCalled();
+	if(!this->isPaused())
+	{
+		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(5);
+		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(5);
+		std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
+		rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
+		rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
+		rtabmap_ros::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
+		rtabmap_ros::toCvShare(image4, imageMsgs[3], depthMsgs[3]);
+		rtabmap_ros::toCvShare(image5, imageMsgs[4], depthMsgs[4]);
+		infoMsgs.push_back(image->rgb_camera_info);
+		infoMsgs.push_back(image2->rgb_camera_info);
+		infoMsgs.push_back(image3->rgb_camera_info);
+		infoMsgs.push_back(image4->rgb_camera_info);
+		infoMsgs.push_back(image5->rgb_camera_info);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+	}
+}
+
 void RGBDOdometry::flushCallbacks()
 {
 	// flush callbacks
 	if(approxSync_)
 	{
 		delete approxSync_;
-		approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize()), image_mono_sub_, image_depth_sub_, info_sub_);
+		approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
 		approxSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 	}
 	if(exactSync_)
 	{
 		delete exactSync_;
-		exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize()), image_mono_sub_, image_depth_sub_, info_sub_);
+		exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
 		exactSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 	}
 	if(approxSync2_)
 	{
 		delete approxSync2_;
 		approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
-				MyApproxSync2Policy(queueSize()),
+				MyApproxSync2Policy(queueSize_),
 				rgbd_image1_sub_,
 				rgbd_image2_sub_);
 		approxSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -509,7 +591,7 @@ void RGBDOdometry::flushCallbacks()
 	{
 		delete exactSync2_;
 		exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
-				MyExactSync2Policy(queueSize()),
+				MyExactSync2Policy(queueSize_),
 				rgbd_image1_sub_,
 				rgbd_image2_sub_);
 		exactSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
@@ -518,7 +600,7 @@ void RGBDOdometry::flushCallbacks()
 	{
 		delete approxSync3_;
 		approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
-				MyApproxSync3Policy(queueSize()),
+				MyApproxSync3Policy(queueSize_),
 				rgbd_image1_sub_,
 				rgbd_image2_sub_,
 				rgbd_image3_sub_);
@@ -528,7 +610,7 @@ void RGBDOdometry::flushCallbacks()
 	{
 		delete exactSync3_;
 		exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
-				MyExactSync3Policy(queueSize()),
+				MyExactSync3Policy(queueSize_),
 				rgbd_image1_sub_,
 				rgbd_image2_sub_,
 				rgbd_image3_sub_);
@@ -538,7 +620,7 @@ void RGBDOdometry::flushCallbacks()
 	{
 		delete approxSync4_;
 		approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
-				MyApproxSync4Policy(queueSize()),
+				MyApproxSync4Policy(queueSize_),
 				rgbd_image1_sub_,
 				rgbd_image2_sub_,
 				rgbd_image3_sub_,
@@ -549,13 +631,37 @@ void RGBDOdometry::flushCallbacks()
 	{
 		delete exactSync4_;
 		exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
-				MyExactSync4Policy(queueSize()),
+				MyExactSync4Policy(queueSize_),
 				rgbd_image1_sub_,
 				rgbd_image2_sub_,
 				rgbd_image3_sub_,
 				rgbd_image4_sub_);
 		exactSync4_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
 	}
+	if(approxSync5_)
+	{
+		delete approxSync5_;
+		approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
+				MyApproxSync5Policy(queueSize_),
+				rgbd_image1_sub_,
+				rgbd_image2_sub_,
+				rgbd_image3_sub_,
+				rgbd_image4_sub_,
+				rgbd_image5_sub_);
+		approxSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
+	}
+	if(exactSync5_)
+	{
+		delete exactSync5_;
+		exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
+				MyExactSync5Policy(queueSize_),
+				rgbd_image1_sub_,
+				rgbd_image2_sub_,
+				rgbd_image3_sub_,
+				rgbd_image4_sub_,
+				rgbd_image5_sub_);
+		exactSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
+	}
 }

 }
@@ -51,7 +51,7 @@ RGBDRelay::RGBDRelay(const rclcpp::NodeOptions & options) :
 	compress_ = this->declare_parameter("compress", compress_);
 	uncompress_ = this->declare_parameter("uncompress", uncompress_);

-	rgbdImageSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::SensorDataQoS(), std::bind(&RGBDRelay::callback, this, std::placeholders::_1));
+	rgbdImageSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", 5, std::bind(&RGBDRelay::callback, this, std::placeholders::_1));
 	rgbdImagePub_ = create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image_relay", 1);
 }

@@ -70,8 +70,12 @@ void RGBDRelay::callback(const rtabmap_ros::msg::RGBDImage::SharedPtr input) con
 		output->header = input->header;
 		output->rgb_camera_info = input->rgb_camera_info;
 		output->depth_camera_info = input->depth_camera_info;
+		output->key_points = input->key_points;
+		output->points = input->points;
+		output->descriptors = input->descriptors;
+		output->global_descriptor = input->global_descriptor;

-		rtabmap::StereoCameraModel stereoModel;// = stereoCameraModelFromROS(input->rgb_camera_info, input->depth_camera_info, rtabmap::Transform::getIdentity());
+		rtabmap::StereoCameraModel stereoModel = stereoCameraModelFromROS(input->rgb_camera_info, input->depth_camera_info, rtabmap::Transform::getIdentity());

 		if(compress_)
 		{
@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/highgui/highgui.hpp>

 #include "rtabmap/core/Compression.h"
+#include "rtabmap/core/util2d.h"
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap_ros/MsgConversion.h"

@@ -43,7 +44,9 @@ namespace rtabmap_ros
 RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
 	Node("rgbd_sync", options),
 	depthScale_(1.0),
+	decimation_(1),
 	compressedRate_(0),
+	warningThread_(0),
 	callbackCalled_(false),
 	approxSyncDepth_(0),
 	exactSyncDepth_(0)
@@ -53,11 +56,18 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
 	approxSync = this->declare_parameter("approx_sync", approxSync);
 	queueSize = this->declare_parameter("queue_size", queueSize);
 	depthScale_ = this->declare_parameter("depth_scale", depthScale_);
+	decimation_ = this->declare_parameter("decimation", decimation_);
 	compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_);

+	if(decimation_<1)
+	{
+		decimation_ = 1;
+	}
+
 	RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
 	RCLCPP_INFO(this->get_logger(), "%s: queue_size  = %d", get_name(), queueSize);
 	RCLCPP_INFO(this->get_logger(), "%s: depth_scale = %f", get_name(), depthScale_);
+	RCLCPP_INFO(this->get_logger(), "%s: decimation = %d", get_name(), decimation_);
 	RCLCPP_INFO(this->get_logger(), "%s: compressed_rate = %f", get_name(), compressedRate_);

 	rgbdImagePub_ = this->create_publisher<rtabmap_ros::msg::RGBDImage>("rgbd_image", 1);
@@ -75,9 +85,9 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
 	}

 	image_transport::TransportHints hints(this);
-	imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rmw_qos_profile_sensor_data);
-	imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rmw_qos_profile_sensor_data);
-	cameraInfoSub_.subscribe(this, "rgb/camera_info", rmw_qos_profile_sensor_data);
+	imageSub_.subscribe(this, "rgb/image", hints.getTransport());
+	imageDepthSub_.subscribe(this, "depth/image", hints.getTransport());
+	cameraInfoSub_.subscribe(this, "rgb/camera_info");

 	subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s",
 						get_name(),
@@ -131,8 +141,44 @@ void RGBDSync::callback(
 		rtabmap_ros::msg::RGBDImage::UniquePtr msg(new rtabmap_ros::msg::RGBDImage);
 		msg->header.frame_id = cameraInfo->header.frame_id;
 		msg->header.stamp = rgbStamp>depthStamp?image->header.stamp:depth->header.stamp;
-		msg->rgb_camera_info = *cameraInfo;
-		msg->depth_camera_info = *cameraInfo;
+		if(decimation_>1 && !(depth->width % decimation_ == 0 && depth->height % decimation_ == 0))
+		{
+			RCLCPP_WARN(this->get_logger(), "Decimation of depth images should be exact (decimation=%d, size=(%d,%d))! "
+				   "Images won't be resized.", decimation_, depth->width, depth->height);
+			decimation_ = 1;
+		}
+		if(decimation_>1)
+		{
+			rtabmap::CameraModel model = rtabmap_ros::cameraModelFromROS(*cameraInfo);
+			sensor_msgs::msg::CameraInfo info;
+			rtabmap_ros::cameraModelToROS(model.scaled(1.0f/float(decimation_)), info);
+			info.header = cameraInfo->header;
+			msg->rgb_camera_info = info;
+			msg->depth_camera_info = info;
+		}
+		else
+		{
+			msg->rgb_camera_info = *cameraInfo;
+			msg->depth_camera_info = *cameraInfo;
+		}
+
+		cv::Mat rgbMat;
+		cv::Mat depthMat;
+		cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
+		cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
+		rgbMat = imagePtr->image;
+		depthMat = imageDepthPtr->image;
+
+		if(decimation_>1)
+		{
+			rgbMat = rtabmap::util2d::decimate(rgbMat, decimation_);
+			depthMat = rtabmap::util2d::decimate(depthMat, decimation_);
+		}
+
+		if(depthScale_ != 1.0)
+		{
+			depthMat*=depthScale_;
+		}

 		if(rgbdImageCompressedPub_->get_subscription_count())
 		{
@@ -151,21 +197,19 @@ void RGBDSync::callback(
 				lastCompressedPublished_ = now();

 				rtabmap_ros::msg::RGBDImage::UniquePtr msgCompressed(new rtabmap_ros::msg::RGBDImage);
-				*msgCompressed = *msg;
+				msgCompressed->header = msg->header;
+				msgCompressed->rgb_camera_info = msg->rgb_camera_info;
+				msgCompressed->depth_camera_info = msg->depth_camera_info;

-				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
-				imagePtr->toCompressedImageMsg(msgCompressed->rgb_compressed, cv_bridge::JPG);
+				cv_bridge::CvImage cvImg;
+				cvImg.header = image->header;
+				cvImg.image = rgbMat;
+				cvImg.encoding = image->encoding;
+				cvImg.toCompressedImageMsg(msgCompressed->rgb_compressed, cv_bridge::JPG);

-				cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
 				msgCompressed->depth_compressed.header = imageDepthPtr->header;
-				if(depthScale_ != 1.0)
-				{
-					msgCompressed->depth_compressed.data = rtabmap::compressImage(imageDepthPtr->image*depthScale_, ".png");
-				}
-				else
-				{
-					msgCompressed->depth_compressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
-				}
+				msgCompressed->depth_compressed.data = rtabmap::compressImage(depthMat, ".png");
+
 				msgCompressed->depth_compressed.format = "png";

 				rgbdImageCompressedPub_->publish(std::move(msgCompressed));
@@ -174,17 +218,18 @@ void RGBDSync::callback(

 		if(rgbdImagePub_->get_subscription_count())
 		{
-			msg->rgb = *image;
-			if(depthScale_ != 1.0)
-			{
-				cv_bridge::CvImagePtr imageDepthPtr = cv_bridge::toCvCopy(depth);
-				imageDepthPtr->image*=depthScale_;
-				msg->depth = *imageDepthPtr->toImageMsg();
-			}
-			else
-			{
-				msg->depth = *depth;
-			}
+			cv_bridge::CvImage cvImg;
+			cvImg.header = image->header;
+			cvImg.image = rgbMat;
+			cvImg.encoding = image->encoding;
+			cvImg.toImageMsg(msg->rgb);
+
+			cv_bridge::CvImage cvDepth;
+			cvDepth.header = depth->header;
+			cvDepth.image = depthMat;
+			cvDepth.encoding = depth->encoding;
+			cvDepth.toImageMsg(msg->depth);
+
 			rgbdImagePub_->publish(std::move(msg));
 		}

@@ -73,6 +73,7 @@ public:
 		approxCloudSync_(0),
 		exactCloudSync_(0),
 		queueSize_(5),
+		keepColor_(false),
 		scanCloudMaxPoints_(0),
 		scanVoxelSize_(0.0),
 		scanNormalK_(0),
@@ -122,6 +123,7 @@ private:
 			pnh.param("scan_cloud_normal_k", scanNormalK_, scanNormalK_);
 		}
 		pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_);
+		pnh.param("keep_color", keepColor_, keepColor_);

 		NODELET_INFO("RGBDIcpOdometry: approx_sync           = %s", approxSync?"true":"false");
 		NODELET_INFO("RGBDIcpOdometry: queue_size            = %d", queueSize_);
@@ -130,6 +132,7 @@ private:
 		NODELET_INFO("RGBDIcpOdometry: scan_voxel_size       = %f", scanVoxelSize_);
 		NODELET_INFO("RGBDIcpOdometry: scan_normal_k         = %d", scanNormalK_);
 		NODELET_INFO("RGBDIcpOdometry: scan_normal_radius    = %f", scanNormalRadius_);
+		NODELET_INFO("RGBDIcpOdometry: keep_color            = %s", keepColor_?"true":"false");

 		ros::NodeHandle rgb_nh(nh, "rgb");
 		ros::NodeHandle depth_nh(nh, "depth");
@@ -181,7 +184,7 @@ private:
 				exactScanSync_->registerCallback(std::bind(&RGBDICPOdometry::callbackScan, 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",
+			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n   %s \\\n   %s \\\n   %s",
 					getName().c_str(),
 					approxSync?"approx":"exact",
 					image_mono_sub_.getSubscriber().getTopic().c_str(),
@@ -277,10 +280,13 @@ private:
 			if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
 			{
 				rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform);
-				cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
+				cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image,
+						image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
+						image->encoding.compare(sensor_msgs::image_encodings::MONO8)==0?"":
+								keepColor_ && image->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0?"bgr8":"mono8");
 				cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth);

-				cv::Mat scan;
+				LaserScan scan;
 				Transform localScanTransform = Transform::getIdentity();
 				int maxLaserScans = 0;
 				if(scanMsg.get() != 0)
@@ -337,6 +343,10 @@ private:
 				}
 				else if(cloudMsg.get() != 0)
 				{
+					UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height,
+							uFormat("data=%d row_step=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->height).c_str());
+
+
 					bool containNormals = false;
 					if(scanVoxelSize_ == 0.0f)
 					{
@@ -402,7 +412,7 @@ private:
 				}

 				rtabmap::SensorData data(
-						LaserScan::backwardCompatibility(scan,
+						LaserScan(scan,
 								scanMsg.get() != 0 || cloudMsg.get() != 0?maxLaserScans:0,
 								scanMsg.get() != 0?scanMsg->range_max:0,
 								localScanTransform),
@@ -412,7 +422,10 @@ private:
 						0,
 						rtabmap_ros::timestampFromROS(stamp));

-				this->processData(data, stamp);
+				std_msgs::Header header;
+				header.stamp = stamp;
+				header.frame_id = image->header.frame_id;
+				this->processData(data, header);
 			}
 		}
 	}
@@ -462,6 +475,7 @@ private:
 	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2> MyExactCloudSyncPolicy;
 	message_filters::Synchronizer<MyExactCloudSyncPolicy> * exactCloudSync_;
 	int queueSize_;
+	bool keepColor_;
 	int scanCloudMaxPoints_;
 	double scanVoxelSize_;
 	int scanNormalK_;
@@ -0,0 +1,274 @@
+/*
+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/rgbdx_sync.hpp>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+
+namespace rtabmap_ros
+{
+
+RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
+	Node("rgbd_sync", options),
+	SYNC_INIT(rgbd2),
+	SYNC_INIT(rgbd3),
+	SYNC_INIT(rgbd4),
+	SYNC_INIT(rgbd5),
+	SYNC_INIT(rgbd6),
+	SYNC_INIT(rgbd7),
+	SYNC_INIT(rgbd8),
+	warningThread_(0),
+	callbackCalled_(false)
+{
+	int queueSize = 10;
+	bool approxSync = true;
+	int rgbdCameras = 2;
+	approxSync = this->declare_parameter("approx_sync", approxSync);
+	queueSize = this->declare_parameter("queue_size", queueSize);
+	rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras);
+
+	RCLCPP_INFO(this->get_logger(), "%s: approx_sync  = %s", get_name(), approxSync?"true":"false");
+	RCLCPP_INFO(this->get_logger(), "%s: queue_size   = %d", get_name(), queueSize);
+	RCLCPP_INFO(this->get_logger(), "%s: rgbd_cameras = %d", get_name(), rgbdCameras);
+
+	rgbdImagesPub_ = this->create_publisher<rtabmap_ros::msg::RGBDImages>("rgbd_images", 1);
+
+	UASSERT(rgbdCameras>=2 && rgbdCameras<=8);
+
+	rgbdSubs_.resize(rgbdCameras);
+	for(int i=0; i<rgbdCameras; ++i)
+	{
+		rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::msg::RGBDImage>;
+		rgbdSubs_[i]->subscribe(this, uFormat("rgbd_image%d", i));
+	}
+
+	std::string name_ = get_name();
+	std::string subscribedTopicsMsg_;
+	if(rgbdCameras==2)
+	{
+		SYNC_DECL2(RGBDXSync, rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
+	}
+	else if(rgbdCameras==3)
+	{
+		SYNC_DECL3(RGBDXSync, rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
+	}
+	else if(rgbdCameras==4)
+	{
+		SYNC_DECL4(RGBDXSync, rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
+	}
+	else if(rgbdCameras==5)
+	{
+		SYNC_DECL5(RGBDXSync, rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
+	}
+	else if(rgbdCameras==6)
+	{
+		SYNC_DECL6(RGBDXSync, rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
+	}
+	else if(rgbdCameras==7)
+	{
+		SYNC_DECL7(RGBDXSync, rgbd7, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]));
+	}
+	else if(rgbdCameras==8)
+	{
+		SYNC_DECL8(RGBDXSync, rgbd8, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]), (*rgbdSubs_[7]));
+	}
+
+	RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg_.c_str());
+
+	warningThread_ = new std::thread([&](){
+		rclcpp::Rate r(1/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(),
+						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());
+			}
+		}
+	});
+}
+
+RGBDXSync::~RGBDXSync()
+{
+	SYNC_DEL(rgbd2);
+	SYNC_DEL(rgbd3);
+	SYNC_DEL(rgbd4);
+	SYNC_DEL(rgbd5);
+	SYNC_DEL(rgbd6);
+	SYNC_DEL(rgbd7);
+	SYNC_DEL(rgbd8);
+
+	if(warningThread_)
+	{
+		callbackCalled_=true;
+		warningThread_->join();
+		delete warningThread_;
+	}
+}
+
+void RGBDXSync::rgbd2Callback(
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image0,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1)
+{
+	callbackCalled_ = true;
+	rtabmap_ros::msg::RGBDImages output;
+	output.header = image0->header;
+	output.rgbd_images.resize(2);
+	output.rgbd_images[0]=(*image0);
+	output.rgbd_images[1]=(*image1);
+	rgbdImagesPub_->publish(output);
+}
+
+void RGBDXSync::rgbd3Callback(
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image0,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2)
+{
+	callbackCalled_ = true;
+	rtabmap_ros::msg::RGBDImages output;
+	output.header = image0->header;
+	output.rgbd_images.resize(3);
+	output.rgbd_images[0]=(*image0);
+	output.rgbd_images[1]=(*image1);
+	output.rgbd_images[2]=(*image2);
+	rgbdImagesPub_->publish(output);
+}
+
+void RGBDXSync::rgbd4Callback(
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image0,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3)
+{
+	callbackCalled_ = true;
+	rtabmap_ros::msg::RGBDImages output;
+	output.header = image0->header;
+	output.rgbd_images.resize(4);
+	output.rgbd_images[0]=(*image0);
+	output.rgbd_images[1]=(*image1);
+	output.rgbd_images[2]=(*image2);
+	output.rgbd_images[3]=(*image3);
+	rgbdImagesPub_->publish(output);
+}
+
+void RGBDXSync::rgbd5Callback(
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image0,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4)
+{
+	callbackCalled_ = true;
+	rtabmap_ros::msg::RGBDImages output;
+	output.header = image0->header;
+	output.rgbd_images.resize(5);
+	output.rgbd_images[0]=(*image0);
+	output.rgbd_images[1]=(*image1);
+	output.rgbd_images[2]=(*image2);
+	output.rgbd_images[3]=(*image3);
+	output.rgbd_images[4]=(*image4);
+	rgbdImagesPub_->publish(output);
+}
+
+void RGBDXSync::rgbd6Callback(
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image0,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5)
+{
+	callbackCalled_ = true;
+	rtabmap_ros::msg::RGBDImages output;
+	output.header = image0->header;
+	output.rgbd_images.resize(6);
+	output.rgbd_images[0]=(*image0);
+	output.rgbd_images[1]=(*image1);
+	output.rgbd_images[2]=(*image2);
+	output.rgbd_images[3]=(*image3);
+	output.rgbd_images[4]=(*image4);
+	output.rgbd_images[5]=(*image5);
+	rgbdImagesPub_->publish(output);
+}
+
+void RGBDXSync::rgbd7Callback(
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image0,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image6)
+{
+	callbackCalled_ = true;
+	rtabmap_ros::msg::RGBDImages output;
+	output.header = image0->header;
+	output.rgbd_images.resize(7);
+	output.rgbd_images[0]=(*image0);
+	output.rgbd_images[1]=(*image1);
+	output.rgbd_images[2]=(*image2);
+	output.rgbd_images[3]=(*image3);
+	output.rgbd_images[4]=(*image4);
+	output.rgbd_images[5]=(*image5);
+	output.rgbd_images[6]=(*image6);
+	rgbdImagesPub_->publish(output);
+}
+
+void RGBDXSync::rgbd8Callback(
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image0,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image6,
+		  const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image7)
+{
+	callbackCalled_ = true;
+	rtabmap_ros::msg::RGBDImages output;
+	output.header = image0->header;
+	output.rgbd_images.resize(8);
+	output.rgbd_images[0]=(*image0);
+	output.rgbd_images[1]=(*image1);
+	output.rgbd_images[2]=(*image2);
+	output.rgbd_images[3]=(*image3);
+	output.rgbd_images[4]=(*image4);
+	output.rgbd_images[5]=(*image5);
+	output.rgbd_images[6]=(*image6);
+	output.rgbd_images[7]=(*image7);
+	rgbdImagesPub_->publish(output);
+}
+
+}
+
@@ -49,7 +49,9 @@ namespace rtabmap_ros
 StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) :
 		rtabmap_ros::OdometryROS("stereo_odometry", options),
 		approxSync_(0),
-		exactSync_(0)
+		exactSync_(0),
+		queueSize_(5),
+		keepColor_(false)
 {
 	OdometryROS::init(true, true, false);
 }
@@ -65,15 +67,19 @@ void StereoOdometry::onOdomInit()
 	bool approxSync = false;
 	bool subscribeRGBD = false;
 	approxSync = this->declare_parameter("approx_sync", approxSync);
+	queueSize_ = this->declare_parameter("queue_size", queueSize_);
 	subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);
+	keepColor_ = this->declare_parameter("keep_color", keepColor_);

 	RCLCPP_INFO(this->get_logger(), "StereoOdometry: approx_sync = %s", approxSync?"true":"false");
+	RCLCPP_INFO(this->get_logger(), "StereoOdometry: queue_size  = %d", queueSize_);
 	RCLCPP_INFO(this->get_logger(), "StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
+	RCLCPP_INFO(this->get_logger(), "StereoOdometry: keep_color     = %s", keepColor_?"true":"false");

 	std::string subscribedTopicsMsg;
 	if(subscribeRGBD)
 	{
-		rgbdSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::SensorDataQoS(), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1));
+		rgbdSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", 5, std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1));

 		subscribedTopicsMsg =
 				uFormat("\n%s subscribed to:\n   %s",
@@ -83,23 +89,22 @@ void StereoOdometry::onOdomInit()
 	else
 	{
 		image_transport::TransportHints hints(this);
-		imageRectLeft_.subscribe(this, "left/image_rect", hints.getTransport(), rmw_qos_profile_sensor_data);
-		imageRectRight_.subscribe(this, "right/image_rect", hints.getTransport(), rmw_qos_profile_sensor_data);
-		cameraInfoLeft_.subscribe(this, "left/camera_info", rmw_qos_profile_sensor_data);
-		cameraInfoRight_.subscribe(this, "right/camera_info", rmw_qos_profile_sensor_data);
+		imageRectLeft_.subscribe(this, "left/image_rect", hints.getTransport());
+		imageRectRight_.subscribe(this, "right/image_rect", hints.getTransport());
+		cameraInfoLeft_.subscribe(this, "left/camera_info");
+		cameraInfoRight_.subscribe(this, "right/camera_info");

 		if(approxSync)
 		{
-			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize()), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
 			approxSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
 		}
 		else
 		{
-			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize()), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
 			exactSync_->registerCallback(std::bind(&StereoOdometry::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(),
 				approxSync?"approx":"exact",
@@ -132,13 +137,15 @@ void StereoOdometry::callback(
 	callbackCalled();
 	if(!this->isPaused())
 	{
-		if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+		if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+			 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
 			 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
 			 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
 			 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
 			 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
 			 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
-			!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+			!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+			  imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
 			  imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
 			  imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
 			  imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
@@ -180,6 +187,38 @@ void StereoOdometry::callback(

 			rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform, stereoTransform);

+			if(stereoModel.baseline() == 0 && alreadyRectified)
+			{
+				stereoTransform = getTransform(
+						cameraInfoLeft->header.frame_id,
+						cameraInfoRight->header.frame_id,
+						cameraInfoLeft->header.stamp,
+						tfBuffer(),
+						waitForTransform());
+
+				if(!stereoTransform.isNull() && stereoTransform.x()>0)
+				{
+					static bool warned = false;
+					if(!warned)
+					{
+						RCLCPP_WARN(this->get_logger(), "Right camera info doesn't have Tx set but we are assuming that stereo images are already rectified (see %s parameter). While not "
+								"recommended, we used TF to get the baseline (%s->%s = %fm) for convenience (e.g., D400 ir stereo issue). It is preferred to feed "
+								"a valid right camera info if stereo images are already rectified. This message is only printed once...",
+								rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str(),
+								cameraInfoRight->header.frame_id.c_str(), cameraInfoLeft->header.frame_id.c_str(), stereoTransform.x());
+						warned = true;
+					}
+					stereoModel = rtabmap::StereoCameraModel(
+							stereoModel.left().fx(),
+							stereoModel.left().fy(),
+							stereoModel.left().cx(),
+							stereoModel.left().cy(),
+							stereoTransform.x(),
+							stereoModel.localTransform(),
+							stereoModel.left().imageSize());
+				}
+			}
+
 			if(alreadyRectified && stereoModel.baseline() <= 0)
 			{
 				RCLCPP_ERROR(this->get_logger(), "The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
@@ -200,8 +239,13 @@ void StereoOdometry::callback(
 				}
 			}

-			cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft, "mono8");
-			cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight, "mono8");
+			cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft,
+					imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
+					imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8)==0?"":
+						keepColor_ && imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0?"bgr8":"mono8");
+			cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight,
+					imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
+					imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8)==0?"":"mono8");

 			UTimer stepTimer;
 			//
@@ -213,7 +257,10 @@ void StereoOdometry::callback(
 					0,
 					rtabmap_ros::timestampFromROS(stamp));

-			this->processData(data, stamp);
+			std_msgs::msg::Header header;
+			header.stamp = stamp;
+			header.frame_id = imageRectLeft->header.frame_id;
+			this->processData(data, header);
 		}
 		else
 		{
@@ -231,13 +278,15 @@ void StereoOdometry::callbackRGBD(
 		cv_bridge::CvImageConstPtr imageRectLeft, imageRectRight;
 		rtabmap_ros::toCvShare(image, imageRectLeft, imageRectRight);

-		if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+		if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+			 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
 			 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
 			 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
 			 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
 			 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
 			 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
-			!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+			!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+			  imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
 			  imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
 			  imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
 			  imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
@@ -259,8 +308,59 @@ void StereoOdometry::callbackRGBD(

 		if(!imageRectLeft->image.empty() && !imageRectRight->image.empty())
 		{
+			bool alreadyRectified = true;
+			Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
+			rtabmap::Transform stereoTransform;
+			if(!alreadyRectified)
+			{
+				stereoTransform = getTransform(
+						image->depth_camera_info.header.frame_id,
+						image->rgb_camera_info.header.frame_id,
+						image->rgb_camera_info.header.stamp,
+						tfBuffer(),
+						waitForTransform());
+				if(stereoTransform.isNull())
+				{
+					RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get TF between the two cameras!", Parameters::kRtabmapImagesAlreadyRectified().c_str());
+					return;
+				}
+			}
+
 			rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(image->rgb_camera_info, image->depth_camera_info, localTransform);
-			if(stereoModel.baseline() <= 0)
+
+			if(stereoModel.baseline() == 0 && alreadyRectified)
+			{
+				stereoTransform = getTransform(
+						image->rgb_camera_info.header.frame_id,
+						image->depth_camera_info.header.frame_id,
+						image->rgb_camera_info.header.stamp,
+						tfBuffer(),
+						waitForTransform());
+
+				if(!stereoTransform.isNull() && stereoTransform.x()>0)
+				{
+					static bool warned = false;
+					if(!warned)
+					{
+						RCLCPP_WARN(get_logger(), "Right camera info doesn't have Tx set but we are assuming that stereo images are already rectified (see %s parameter). While not "
+								"recommended, we used TF to get the baseline (%s->%s = %fm) for convenience (e.g., D400 ir stereo issue). It is preferred to feed "
+								"a valid right camera info if stereo images are already rectified. This message is only printed once...",
+								rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str(),
+								image->depth_camera_info.header.frame_id.c_str(), image->rgb_camera_info.header.frame_id.c_str(), stereoTransform.x());
+						warned = true;
+					}
+					stereoModel = rtabmap::StereoCameraModel(
+							stereoModel.left().fx(),
+							stereoModel.left().fy(),
+							stereoModel.left().cx(),
+							stereoModel.left().cy(),
+							stereoTransform.x(),
+							stereoModel.localTransform(),
+							stereoModel.left().imageSize());
+				}
+			}
+
+			if(alreadyRectified && stereoModel.baseline() <= 0)
 			{
 				RCLCPP_FATAL(this->get_logger(), "The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
 						  "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline());
@@ -280,8 +380,25 @@ void StereoOdometry::callbackRGBD(
 				}
 			}

-			cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::cvtColor(imageRectLeft, "mono8");
-			cv_bridge::CvImagePtr ptrImageRight = cv_bridge::cvtColor(imageRectRight, "mono8");
+			cv_bridge::CvImageConstPtr ptrImageLeft = imageRectLeft;
+			if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
+			   imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
+			{
+				if(keepColor_ && imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
+				{
+					ptrImageLeft = cv_bridge::cvtColor(imageRectLeft, "bgr8");
+				}
+				else
+				{
+					ptrImageLeft = cv_bridge::cvtColor(imageRectLeft, "mono8");
+				}
+			}
+			cv_bridge::CvImageConstPtr ptrImageRight = imageRectRight;
+			if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
+			   imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
+			{
+				ptrImageRight = cv_bridge::cvtColor(imageRectRight, "mono8");
+			}

 			UTimer stepTimer;
 			//
@@ -293,7 +410,10 @@ void StereoOdometry::callbackRGBD(
 					0,
 					rtabmap_ros::timestampFromROS(stamp));

-			this->processData(data, stamp);
+			std_msgs::msg::Header header;
+			header.stamp = stamp;
+			header.frame_id = image->header.frame_id;
+			this->processData(data, header);
 		}
 		else
 		{
@@ -308,13 +428,13 @@ void StereoOdometry::flushCallbacks()
 	if(approxSync_)
 	{
 		delete approxSync_;
-		approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize()), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+		approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
 		approxSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
 	}
 	if(exactSync_)
 	{
 		delete exactSync_;
-		exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize()), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+		exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
 		exactSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
 	}
 }
@@ -73,10 +73,10 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
 	}

 	image_transport::TransportHints hints(this);
-	imageLeftSub_.subscribe(this, "left/image_rect", hints.getTransport(), rmw_qos_profile_sensor_data);
-	imageRightSub_.subscribe(this, "right/image_rect", hints.getTransport(), rmw_qos_profile_sensor_data);
-	cameraInfoLeftSub_.subscribe(this, "left/camera_info", rmw_qos_profile_sensor_data);
-	cameraInfoRightSub_.subscribe(this, "right/camera_info", rmw_qos_profile_sensor_data);
+	imageLeftSub_.subscribe(this, "left/image_rect", hints.getTransport());
+	imageRightSub_.subscribe(this, "right/image_rect", hints.getTransport());
+	cameraInfoLeftSub_.subscribe(this, "left/camera_info");
+	cameraInfoRightSub_.subscribe(this, "right/camera_info");

 	subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s",
 						get_name(),