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

src/impl/CommonDataSubscriberDepth.cpp

@@ -37,8 +37,8 @@ void CommonDataSubscriber::depthCallback(
 {
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
@@ -50,9 +50,9 @@ void CommonDataSubscriber::depthScan2dCallback(
 {
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthScan3dCallback(
 		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
@@ -62,9 +62,25 @@ void CommonDataSubscriber::depthScan3dCallback(
 {
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthScanDescCallback(
+		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg)
+{
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 void CommonDataSubscriber::depthInfoCallback(
 		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
@@ -74,8 +90,8 @@ void CommonDataSubscriber::depthInfoCallback(
 {
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthScan2dInfoCallback(
@@ -87,8 +103,8 @@ void CommonDataSubscriber::depthScan2dInfoCallback(
 {
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthScan3dInfoCallback(
 		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
@@ -99,8 +115,24 @@ void CommonDataSubscriber::depthScan3dInfoCallback(
 {
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthScanDescInfoCallback(
+		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 
 // RGB + Depth + Odom
@@ -111,8 +143,8 @@ void CommonDataSubscriber::depthOdomCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
 {
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
@@ -124,9 +156,9 @@ void CommonDataSubscriber::depthOdomScan2dCallback(
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthOdomScan3dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -136,9 +168,25 @@ void CommonDataSubscriber::depthOdomScan3dCallback(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
 {
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomScanDescCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg)
+{
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 void CommonDataSubscriber::depthOdomInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -148,8 +196,8 @@ void CommonDataSubscriber::depthOdomInfoCallback(
 		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthOdomScan2dInfoCallback(
@@ -161,8 +209,8 @@ void CommonDataSubscriber::depthOdomScan2dInfoCallback(
 		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthOdomScan3dInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -173,10 +221,27 @@ void CommonDataSubscriber::depthOdomScan3dInfoCallback(
 		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomScanDescInfoCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 
+#ifdef RTABMAP_SYNC_USER_DATA
 // RGB + Depth + User Data
 void CommonDataSubscriber::depthDataCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -185,8 +250,8 @@ void CommonDataSubscriber::depthDataCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
 {
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
@@ -198,9 +263,9 @@ void CommonDataSubscriber::depthDataScan2dCallback(
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthDataScan3dCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -210,9 +275,25 @@ void CommonDataSubscriber::depthDataScan3dCallback(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
 {
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthDataScanDescCallback(
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg)
+{
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 void CommonDataSubscriber::depthDataInfoCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -222,8 +303,8 @@ void CommonDataSubscriber::depthDataInfoCallback(
 		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthDataScan2dInfoCallback(
@@ -235,8 +316,8 @@ void CommonDataSubscriber::depthDataScan2dInfoCallback(
 		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthDataScan3dInfoCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -247,8 +328,24 @@ void CommonDataSubscriber::depthDataScan3dInfoCallback(
 		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthDataScanDescInfoCallback(
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 
 // RGB + Depth + Odom + User Data
@@ -259,8 +356,8 @@ void CommonDataSubscriber::depthOdomDataCallback(
 		const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
 {
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
@@ -272,9 +369,9 @@ void CommonDataSubscriber::depthOdomDataScan2dCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthOdomDataScan3dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -284,9 +381,25 @@ void CommonDataSubscriber::depthOdomDataScan3dCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
 {
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomDataScanDescCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg)
+{
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 void CommonDataSubscriber::depthOdomDataInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -296,8 +409,8 @@ void CommonDataSubscriber::depthOdomDataInfoCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthOdomDataScan2dInfoCallback(
@@ -309,8 +422,8 @@ void CommonDataSubscriber::depthOdomDataScan2dInfoCallback(
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
 		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::depthOdomDataScan3dInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -321,9 +434,26 @@ void CommonDataSubscriber::depthOdomDataScan3dInfoCallback(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
 		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
 }
+void CommonDataSubscriber::depthOdomDataScanDescInfoCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
+}
+#endif
 
 void CommonDataSubscriber::setupDepthCallbacks(
 		rclcpp::Node& node,
@@ -331,6 +461,7 @@ void CommonDataSubscriber::setupDepthCallbacks(
 		bool subscribeUserData,
 		bool subscribeScan2d,
 		bool subscribeScan3d,
+		bool subscribeScanDesc,
 		bool subscribeOdomInfo,
 		int queueSize,
 		bool approxSync)
@@ -338,188 +469,254 @@ void CommonDataSubscriber::setupDepthCallbacks(
 	RCLCPP_INFO(node.get_logger(), "Setup depth callback");
 
 	image_transport::TransportHints hints(&node);
-	imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rmw_qos_profile_sensor_data);
-	imageDepthSub_.subscribe(&node, "depth/image", hints.getTransport(), rmw_qos_profile_sensor_data);
-	cameraInfoSub_.subscribe(&node, "rgb/camera_info", rmw_qos_profile_sensor_data);
+	imageSub_.subscribe(&node, "rgb/image", hints.getTransport());
+	imageDepthSub_.subscribe(&node, "depth/image", hints.getTransport());
+	cameraInfoSub_.subscribe(&node, "rgb/camera_info");
 	
+#ifdef RTABMAP_SYNC_USER_DATA
 	if(subscribeOdom && subscribeUserData)
 	{
-		odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
-		userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
+		odomSub_.subscribe(&node, "odom");
+		userDataSub_.subscribe(&node, "user_data");
 
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
 		{
-			subscribedToScan2d_ = true;
-			scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
 			if(subscribeOdomInfo)
 			{
 				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL7(depthOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL7(CommonDataSubscriber, depthOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL6(depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+				SYNC_DECL6(CommonDataSubscriber, depthOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
 			}
 		}
 		else if(subscribeScan3d)
 		{
 			subscribedToScan3d_ = true;
-			scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
+			scan3dSub_.subscribe(&node, "scan_cloud");
 			if(subscribeOdomInfo)
 			{
 				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL7(depthOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL6(depthOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+				SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
 			}
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL6(depthOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL6(CommonDataSubscriber, depthOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL5(depthOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
+			SYNC_DECL5(CommonDataSubscriber, depthOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
 		}
 	}
-	else if(subscribeOdom)
+	else 
+#endif
+	if(subscribeOdom)
 	{
-		odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
+		odomSub_.subscribe(&node, "odom");
 
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
 		{
-			subscribedToScan2d_ = true;
-			scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
 			if(subscribeOdomInfo)
 			{
 				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL6(depthOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL6(CommonDataSubscriber, depthOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL5(depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+				SYNC_DECL5(CommonDataSubscriber, depthOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL6(CommonDataSubscriber, depthOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(CommonDataSubscriber, depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
 			}
 		}
 		else if(subscribeScan3d)
 		{
 			subscribedToScan3d_ = true;
-			scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
+			scan3dSub_.subscribe(&node, "scan_cloud");
 			if(subscribeOdomInfo)
 			{
 				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL6(depthOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL6(CommonDataSubscriber, depthOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL5(depthOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+				SYNC_DECL5(CommonDataSubscriber, depthOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
 			}
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL5(depthOdomInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL5(CommonDataSubscriber, depthOdomInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL4(depthOdom, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
+			SYNC_DECL4(CommonDataSubscriber, depthOdom, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
 		}
 	}
+#ifdef RTABMAP_SYNC_USER_DATA
 	else if(subscribeUserData)
 	{
-		userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
+		userDataSub_.subscribe(&node, "user_data");
 
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
 		{
-			subscribedToScan2d_ = true;
-			scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
 
 			if(subscribeOdomInfo)
 			{
 				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL6(depthDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL6(CommonDataSubscriber, depthDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL5(depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+				SYNC_DECL5(CommonDataSubscriber, depthDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL6(CommonDataSubscriber, depthDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(CommonDataSubscriber, depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
 			}
 		}
 		else if(subscribeScan3d)
 		{
 			subscribedToScan3d_ = true;
-			scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
+			scan3dSub_.subscribe(&node, "scan_cloud");
 			if(subscribeOdomInfo)
 			{
 				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL6(depthDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL6(CommonDataSubscriber, depthDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL5(depthDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+				SYNC_DECL5(CommonDataSubscriber, depthDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
 			}
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL5(depthDataInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL5(CommonDataSubscriber, depthDataInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL4(depthData, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
+			SYNC_DECL4(CommonDataSubscriber, depthData, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
 		}
 	}
+#endif
 	else
 	{
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
 		{
-			subscribedToScan2d_ = true;
-			scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
 			if(subscribeOdomInfo)
 			{
 				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL5(depthScan2dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL5(CommonDataSubscriber, depthScanDescInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL4(depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+				SYNC_DECL4(CommonDataSubscriber, depthScanDesc, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL5(CommonDataSubscriber, depthScan2dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(CommonDataSubscriber, depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
 			}
 		}
 		else if(subscribeScan3d)
 		{
 			subscribedToScan3d_ = true;
-			scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
+			scan3dSub_.subscribe(&node, "scan_cloud");
 			if(subscribeOdomInfo)
 			{
 				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL5(depthScan3dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL5(CommonDataSubscriber, depthScan3dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL4(depthScan3d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+				SYNC_DECL4(CommonDataSubscriber, depthScan3d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
 			}
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL4(depthInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL4(CommonDataSubscriber, depthInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL3(depth, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_);
+			SYNC_DECL3(CommonDataSubscriber, depth, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_);
 		}
 	}
 }

src/impl/CommonDataSubscriberOdom.cpp

@@ -47,6 +47,7 @@ void CommonDataSubscriber::odomInfoCallback(
 	sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
 	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
 }
+#ifdef RTABMAP_SYNC_USER_DATA
 void CommonDataSubscriber::odomDataCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg)
@@ -64,6 +65,7 @@ void CommonDataSubscriber::odomDataInfoCallback(
 	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
 	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
 }
+#endif
 
 void CommonDataSubscriber::setupOdomCallbacks(
 		rclcpp::Node& node,
@@ -76,32 +78,35 @@ void CommonDataSubscriber::setupOdomCallbacks(
 
 	if(subscribeUserData || subscribeOdomInfo)
 	{
-		odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
+		odomSub_.subscribe(&node, "odom");
 
+#ifdef RTABMAP_SYNC_USER_DATA
 		if(subscribeUserData)
 		{
-			userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
+			userDataSub_.subscribe(&node, "user_data");
 			if(subscribeOdomInfo)
 			{
 				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL3(odomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, odomInfoSub_);
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL3(CommonDataSubscriber, odomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL2(odomData, approxSync, queueSize, odomSub_, userDataSub_);
+				SYNC_DECL2(CommonDataSubscriber, odomData, approxSync, queueSize, odomSub_, userDataSub_);
 			}
 		}
-		else if(subscribeOdomInfo)
+		else 
+#endif
+		if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL2(odomInfo, approxSync, queueSize, odomSub_, odomInfoSub_);
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL2(CommonDataSubscriber, odomInfo, approxSync, queueSize, odomSub_, odomInfoSub_);
 		}
 	}
 	else
 	{
-		odomSubOnly_ = node.create_subscription<nav_msgs::msg::Odometry>("odom", rclcpp::SensorDataQoS(), std::bind(&CommonDataSubscriber::odomCallback, this, std::placeholders::_1));
+		odomSubOnly_ = node.create_subscription<nav_msgs::msg::Odometry>("odom", 5, std::bind(&CommonDataSubscriber::odomCallback, this, std::placeholders::_1));
 		subscribedTopicsMsg_ =
 				uFormat("\n%s subscribed to:\n   %s",
 				node.get_name(),

src/impl/CommonDataSubscriberRGB.cpp

@@ -36,8 +36,8 @@ void CommonDataSubscriber::rgbCallback(
 {
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
@@ -49,10 +49,10 @@ void CommonDataSubscriber::rgbScan2dCallback(
 {
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbScan3dCallback(
 		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
@@ -61,10 +61,26 @@ void CommonDataSubscriber::rgbScan3dCallback(
 {
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbScanDescCallback(
+		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg)
+{
+	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
+	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
+	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 void CommonDataSubscriber::rgbInfoCallback(
 		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
@@ -73,8 +89,8 @@ void CommonDataSubscriber::rgbInfoCallback(
 {
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
@@ -86,9 +102,9 @@ void CommonDataSubscriber::rgbScan2dInfoCallback(
 {
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbScan3dInfoCallback(
 		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
@@ -98,9 +114,25 @@ void CommonDataSubscriber::rgbScan3dInfoCallback(
 {
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbScanDescInfoCallback(
+		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
+	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 
 // RGB + Odom
@@ -110,8 +142,8 @@ void CommonDataSubscriber::rgbOdomCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
 {
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
@@ -123,10 +155,10 @@ void CommonDataSubscriber::rgbOdomScan2dCallback(
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbOdomScan3dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -135,10 +167,26 @@ void CommonDataSubscriber::rgbOdomScan3dCallback(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
 {
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomScanDescCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg)
+{
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 void CommonDataSubscriber::rgbOdomInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -147,8 +195,8 @@ void CommonDataSubscriber::rgbOdomInfoCallback(
 		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
@@ -160,9 +208,9 @@ void CommonDataSubscriber::rgbOdomScan2dInfoCallback(
 		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbOdomScan3dInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -172,11 +220,28 @@ void CommonDataSubscriber::rgbOdomScan3dInfoCallback(
 		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomScanDescInfoCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 
+#ifdef RTABMAP_SYNC_USER_DATA
 // RGB + Depth + User Data
 void CommonDataSubscriber::rgbDataCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -184,8 +249,8 @@ void CommonDataSubscriber::rgbDataCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
 {
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
@@ -197,10 +262,10 @@ void CommonDataSubscriber::rgbDataScan2dCallback(
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbDataScan3dCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -209,10 +274,26 @@ void CommonDataSubscriber::rgbDataScan3dCallback(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
 {
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
-	sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbDataScanDescCallback(
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg)
+{
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 void CommonDataSubscriber::rgbDataInfoCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -221,8 +302,8 @@ void CommonDataSubscriber::rgbDataInfoCallback(
 		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
-	sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
@@ -234,9 +315,9 @@ void CommonDataSubscriber::rgbDataScan2dInfoCallback(
 		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbDataScan3dInfoCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -246,9 +327,25 @@ void CommonDataSubscriber::rgbDataScan3dInfoCallback(
 		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
-	sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbDataScanDescInfoCallback(
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 
 // RGB + Depth + Odom + User Data
@@ -258,8 +355,8 @@ void CommonDataSubscriber::rgbOdomDataCallback(
 		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
 {
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
@@ -271,10 +368,10 @@ void CommonDataSubscriber::rgbOdomDataScan2dCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbOdomDataScan3dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -283,10 +380,26 @@ void CommonDataSubscriber::rgbOdomDataScan3dCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
 {
-	sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomDataScanDescCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg)
+{
+	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
 }
 void CommonDataSubscriber::rgbOdomDataInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -295,8 +408,8 @@ void CommonDataSubscriber::rgbOdomDataInfoCallback(
 		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
 		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
-	sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
 	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
@@ -308,9 +421,9 @@ void CommonDataSubscriber::rgbOdomDataScan2dInfoCallback(
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
 		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::rgbOdomDataScan3dInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -320,10 +433,27 @@ void CommonDataSubscriber::rgbOdomDataScan3dInfoCallback(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
 		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
-	sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	cv_bridge::CvImageConstPtr depthMsg;// Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
 }
+void CommonDataSubscriber::rgbOdomDataScanDescInfoCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
+		const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptor;
+	if(!scanMsg->global_descriptor.data.empty())
+	{
+		globalDescriptor.push_back(scanMsg->global_descriptor);
+	}
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
+}
+#endif
 
 void CommonDataSubscriber::setupRGBCallbacks(
 		rclcpp::Node& node,
@@ -331,6 +461,7 @@ void CommonDataSubscriber::setupRGBCallbacks(
 		bool subscribeUserData,
 		bool subscribeScan2d,
 		bool subscribeScan3d,
+		bool subscribeScanDesc,
 		bool subscribeOdomInfo,
 		int queueSize,
 		bool approxSync)
@@ -338,187 +469,253 @@ void CommonDataSubscriber::setupRGBCallbacks(
 	RCLCPP_INFO(node.get_logger(), "Setup rgb-only callback");
 
 	image_transport::TransportHints hints(&node);
-	imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rmw_qos_profile_sensor_data);
-	cameraInfoSub_.subscribe(&node, "rgb/camera_info", rmw_qos_profile_sensor_data);
+	imageSub_.subscribe(&node, "rgb/image", hints.getTransport());
+	cameraInfoSub_.subscribe(&node, "rgb/camera_info");
 
+#ifdef RTABMAP_SYNC_USER_DATA
 	if(subscribeOdom && subscribeUserData)
 	{
-		odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
-		userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
+		odomSub_.subscribe(&node, "odom");
+		userDataSub_.subscribe(&node, "user_data");
 
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
 		{
-			subscribedToScan2d_ = true;
-			scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
 			if(subscribeOdomInfo)
 			{
 				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL6(rgbOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL5(rgbOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
+				SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
 			}
 		}
 		else if(subscribeScan3d)
 		{
 			subscribedToScan3d_ = true;
-			scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
+			scan3dSub_.subscribe(&node, "scan_cloud");
 			if(subscribeOdomInfo)
 			{
 				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL6(rgbOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL5(rgbOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
+				SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
 			}
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL5(rgbOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL5(CommonDataSubscriber, rgbOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL4(rgbOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_);
-		}
-	}
-	else if(subscribeOdom)
-	{
-		odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
-
-		if(subscribeScan2d)
-		{
-			subscribedToScan2d_ = true;
-			scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
-			if(subscribeOdomInfo)
-			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL5(rgbOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL4(rgbOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_);
-			}
-		}
-		else if(subscribeScan3d)
-		{
-			subscribedToScan3d_ = true;
-			scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
-			if(subscribeOdomInfo)
-			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL5(rgbOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL4(rgbOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_);
-			}
-		}
-		else if(subscribeOdomInfo)
-		{
-			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL4(rgbOdomInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
-		}
-		else
-		{
-			SYNC_DECL3(rgbOdom, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_);
-		}
-	}
-	else if(subscribeUserData)
-	{
-		userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
-
-		if(subscribeScan2d)
-		{
-			subscribedToScan2d_ = true;
-			scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
-
-			if(subscribeOdomInfo)
-			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL5(rgbDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL4(rgbDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
-			}
-		}
-		else if(subscribeScan3d)
-		{
-			subscribedToScan3d_ = true;
-			scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
-			if(subscribeOdomInfo)
-			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL5(rgbDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL4(rgbDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
-			}
-		}
-		else if(subscribeOdomInfo)
-		{
-			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL4(rgbDataInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
-		}
-		else
-		{
-			SYNC_DECL3(rgbData, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_);
+			SYNC_DECL4(CommonDataSubscriber, rgbOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_);
 		}
 	}
 	else
+#endif		
+	if(subscribeOdom)
 	{
-		if(subscribeScan2d)
+		odomSub_.subscribe(&node, "odom");
+
+		if(subscribeScanDesc)
 		{
-			subscribedToScan2d_ = true;
-			scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
 			if(subscribeOdomInfo)
 			{
 				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL4(rgbScan2dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL5(CommonDataSubscriber, rgbOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL3(rgbScan2d, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_);
+				SYNC_DECL4(CommonDataSubscriber, rgbOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL5(CommonDataSubscriber, rgbOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(CommonDataSubscriber, rgbOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_);
 			}
 		}
 		else if(subscribeScan3d)
 		{
 			subscribedToScan3d_ = true;
-			scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
+			scan3dSub_.subscribe(&node, "scan_cloud");
 			if(subscribeOdomInfo)
 			{
 				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL4(rgbScan3dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL5(CommonDataSubscriber, rgbOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL3(rgbScan3d, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_);
+				SYNC_DECL4(CommonDataSubscriber, rgbOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_);
 			}
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL3(rgbInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, odomInfoSub_);
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL4(CommonDataSubscriber, rgbOdomInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL2(rgb, approxSync, queueSize, imageSub_, cameraInfoSub_);
+			SYNC_DECL3(CommonDataSubscriber, rgbOdom, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_);
+		}
+	}
+#ifdef RTABMAP_SYNC_USER_DATA
+	else if(subscribeUserData)
+	{
+		userDataSub_.subscribe(&node, "user_data");
+
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
+
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL5(CommonDataSubscriber, rgbDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(CommonDataSubscriber, rgbDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL5(CommonDataSubscriber, rgbDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(CommonDataSubscriber, rgbDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
+			}
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(&node, "scan_cloud");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL5(CommonDataSubscriber, rgbDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(CommonDataSubscriber, rgbDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
+			}
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL4(CommonDataSubscriber, rgbDataInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL3(CommonDataSubscriber, rgbData, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_);
+		}
+	}
+#endif
+	else
+	{
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL4(CommonDataSubscriber, rgbScanDescInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL3(CommonDataSubscriber, rgbScanDesc, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_);
+			}
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL4(CommonDataSubscriber, rgbScan2dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL3(CommonDataSubscriber, rgbScan2d, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_);
+			}
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(&node, "scan_cloud");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL4(CommonDataSubscriber, rgbScan3dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL3(CommonDataSubscriber, rgbScan3d, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_);
+			}
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL3(CommonDataSubscriber, rgbInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL2(CommonDataSubscriber, rgb, approxSync, queueSize, imageSub_, cameraInfoSub_);
 		}
 	}
 }

src/impl/CommonDataSubscriberRGBD.cpp

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap_ros/CommonDataSubscriber.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
 #include <rtabmap_ros/MsgConversion.h>
 #include <cv_bridge/cv_bridge.h>
 
@@ -41,10 +42,21 @@ void CommonDataSubscriber::rgbdCallback(
 
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdScan2dCallback(
 		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -55,9 +67,20 @@ void CommonDataSubscriber::rgbdScan2dCallback(
 
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			*scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdScan3dCallback(
 		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -68,9 +91,43 @@ void CommonDataSubscriber::rgbdScan3dCallback(
 
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, *scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
+}
+void CommonDataSubscriber::rgbdScanDescCallback(
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdInfoCallback(
 		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -81,35 +138,20 @@ void CommonDataSubscriber::rgbdInfoCallback(
 
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbdScan2dInfoCallback(
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	cv_bridge::CvImageConstPtr rgb, depth;
-	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 
-	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbdScan3dInfoCallback(
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	cv_bridge::CvImageConstPtr rgb, depth;
-	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
 
-	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 
 // 1 RGBD camera + Odom
@@ -121,10 +163,21 @@ void CommonDataSubscriber::rgbdOdomCallback(
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdOdomScan2dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -135,9 +188,20 @@ void CommonDataSubscriber::rgbdOdomScan2dCallback(
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			*scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdOdomScan3dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -148,9 +212,47 @@ void CommonDataSubscriber::rgbdOdomScan3dCallback(
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, *scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
+}
+void CommonDataSubscriber::rgbdOdomScanDescCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdOdomInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -161,37 +263,23 @@ void CommonDataSubscriber::rgbdOdomInfoCallback(
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbdOdomScan2dInfoCallback(
-		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	cv_bridge::CvImageConstPtr rgb, depth;
-	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 
-	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbdOdomScan3dInfoCallback(
-		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	cv_bridge::CvImageConstPtr rgb, depth;
-	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
 
-	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 
+#ifdef RTABMAP_SYNC_USER_DATA
 // 1 RGBD camera + User Data
 void CommonDataSubscriber::rgbdDataCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -201,10 +289,21 @@ void CommonDataSubscriber::rgbdDataCallback(
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdDataScan2dCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -215,9 +314,20 @@ void CommonDataSubscriber::rgbdDataScan2dCallback(
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			*scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdDataScan3dCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -228,9 +338,47 @@ void CommonDataSubscriber::rgbdDataScan3dCallback(
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, *scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
+}
+void CommonDataSubscriber::rgbdDataScanDescCallback(
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdDataInfoCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -240,36 +388,21 @@ void CommonDataSubscriber::rgbdDataInfoCallback(
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
-	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbdDataScan2dInfoCallback(
-		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	cv_bridge::CvImageConstPtr rgb, depth;
-	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 
-	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbdDataScan3dInfoCallback(
-		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	cv_bridge::CvImageConstPtr rgb, depth;
-	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
 
-	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 
 // 1 RGBD camera + Odom + User Data
@@ -281,10 +414,21 @@ void CommonDataSubscriber::rgbdOdomDataCallback(
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdOdomDataScan2dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -295,9 +439,20 @@ void CommonDataSubscriber::rgbdOdomDataScan2dCallback(
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			*scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdOdomDataScan3dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -308,9 +463,47 @@ void CommonDataSubscriber::rgbdOdomDataScan3dCallback(
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
+
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg ,rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, *scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
+}
+void CommonDataSubscriber::rgbdOdomDataScanDescCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+
+	commonSingleDepthCallback(odomMsg, userDataMsg ,rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
 void CommonDataSubscriber::rgbdOdomDataInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -321,36 +514,21 @@ void CommonDataSubscriber::rgbdOdomDataInfoCallback(
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
 
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbdOdomDataScan2dInfoCallback(
-		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
-		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	cv_bridge::CvImageConstPtr rgb, depth;
-	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
+	std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs;
+	if(!image1Msg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
+	}
 
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbdOdomDataScan3dInfoCallback(
-		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
-		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	cv_bridge::CvImageConstPtr rgb, depth;
-	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
-
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
+			depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
+			scanMsg, scan3dMsg, odomInfoMsg,
+			globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
+			rtabmap::uncompressData(image1Msg->descriptors));
 }
+#endif
 
 void CommonDataSubscriber::setupRGBDCallbacks(
 		rclcpp::Node& node,
@@ -358,6 +536,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 		bool subscribeUserData,
 		bool subscribeScan2d,
 		bool subscribeScan3d,
+		bool subscribeScanDesc,
 		bool subscribeOdomInfo,
 		int queueSize,
 		bool approxSync)
@@ -368,179 +547,195 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 	{
 		rgbdSubs_.resize(1);
 		rgbdSubs_[0] = new message_filters::Subscriber<rtabmap_ros::msg::RGBDImage>;
-		rgbdSubs_[0]->subscribe(&node, "rgbd_image", rmw_qos_profile_sensor_data);
-
+		rgbdSubs_[0]->subscribe(&node, "rgbd_image");
 
+#ifdef RTABMAP_SYNC_USER_DATA
 		if(subscribeOdom && subscribeUserData)
 		{
-			odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
-			userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
-			if(subscribeScan2d)
+			odomSub_.subscribe(&node, "odom");
+			userDataSub_.subscribe(&node, "user_data");
+			if(subscribeScanDesc)
 			{
-				subscribedToScan2d_ = true;
-				scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
+				subscribedToScanDescriptor_ = true;
+				scanDescSub_.subscribe(&node, "scan_descriptor");
 				if(subscribeOdomInfo)
 				{
-					subscribedToOdomInfo_ = true;
-					odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-					SYNC_DECL5(rgbdOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
+					subscribedToOdomInfo_ = false;
+					RCLCPP_WARN(node.get_logger(),  "subscribe_odom_info ignored...");
 				}
-				else
+				SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
+			}
+			else if(subscribeScan2d)
+			{
+				subscribedToScan2d_ = true;
+				scanSub_.subscribe(&node, "scan");
+				if(subscribeOdomInfo)
 				{
-					SYNC_DECL4(rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
+					subscribedToOdomInfo_ = false;
+					RCLCPP_WARN(node.get_logger(),  "subscribe_odom_info ignored...");
 				}
+				SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
 			}
 			else if(subscribeScan3d)
 			{
 				subscribedToScan3d_ = true;
-				scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
+				scan3dSub_.subscribe(&node, "scan_cloud");
 				if(subscribeOdomInfo)
 				{
-					subscribedToOdomInfo_ = true;
-					odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-					SYNC_DECL5(rgbdOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
-				}
-				else
-				{
-					SYNC_DECL4(rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
+					subscribedToOdomInfo_ = false;
+					RCLCPP_WARN(node.get_logger(),  "subscribe_odom_info ignored...");
 				}
+				SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
 			}
 			else if(subscribeOdomInfo)
 			{
 				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL4(rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL3(rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]));
-			}
-		}
-		else if(subscribeOdom)
-		{
-			odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
-			if(subscribeScan2d)
-			{
-				subscribedToScan2d_ = true;
-				scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
-				if(subscribeOdomInfo)
-				{
-					subscribedToOdomInfo_ = true;
-					odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-					SYNC_DECL4(rgbdOdomScan2dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
-				}
-				else
-				{
-					SYNC_DECL3(rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
-				}
-			}
-			else if(subscribeScan3d)
-			{
-				subscribedToScan3d_ = true;
-				scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
-				if(subscribeOdomInfo)
-				{
-					subscribedToOdomInfo_ = true;
-					odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-					SYNC_DECL4(rgbdOdomScan3dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
-				}
-				else
-				{
-					SYNC_DECL3(rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
-				}
-			}
-			else if(subscribeOdomInfo)
-			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL3(rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL2(rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]));
-			}
-		}
-		else if(subscribeUserData)
-		{
-			userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
-			if(subscribeScan2d)
-			{
-				subscribedToScan2d_ = true;
-				scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
-				if(subscribeOdomInfo)
-				{
-					subscribedToOdomInfo_ = true;
-					odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-					SYNC_DECL4(rgbdDataScan2dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
-				}
-				else
-				{
-					SYNC_DECL3(rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
-				}
-			}
-			else if(subscribeScan3d)
-			{
-				subscribedToScan3d_ = true;
-				scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
-				if(subscribeOdomInfo)
-				{
-					subscribedToOdomInfo_ = true;
-					odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-					SYNC_DECL4(rgbdDataScan3dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
-				}
-				else
-				{
-					SYNC_DECL3(rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
-				}
-			}
-			else if(subscribeOdomInfo)
-			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL3(rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL2(rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]));
+				SYNC_DECL3(CommonDataSubscriber, rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]));
 			}
 		}
 		else
+#endif			
+		if(subscribeOdom)
 		{
-			if(subscribeScan2d)
+			odomSub_.subscribe(&node, "odom");
+			if(subscribeScanDesc)
 			{
-				subscribedToScan2d_ = true;
-				scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
+				subscribedToScanDescriptor_ = true;
+				scanDescSub_.subscribe(&node, "scan_descriptor");
 				if(subscribeOdomInfo)
 				{
-					subscribedToOdomInfo_ = true;
-					odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-					SYNC_DECL3(rgbdScan2dInfo, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
+					subscribedToOdomInfo_ = false;
+					RCLCPP_WARN(node.get_logger(),  "subscribe_odom_info ignored...");
 				}
-				else
+				SYNC_DECL3(CommonDataSubscriber, rgbdOdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanDescSub_);
+			}
+			else if(subscribeScan2d)
+			{
+				subscribedToScan2d_ = true;
+				scanSub_.subscribe(&node, "scan");
+				if(subscribeOdomInfo)
 				{
-					SYNC_DECL2(rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
+					subscribedToOdomInfo_ = false;
+					RCLCPP_WARN(node.get_logger(),  "subscribe_odom_info ignored...");
 				}
+				SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
 			}
 			else if(subscribeScan3d)
 			{
 				subscribedToScan3d_ = true;
-				scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
+				scan3dSub_.subscribe(&node, "scan_cloud");
 				if(subscribeOdomInfo)
 				{
-					subscribedToOdomInfo_ = true;
-					odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-					SYNC_DECL3(rgbdScan3dInfo, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
-				}
-				else
-				{
-					SYNC_DECL2(rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
+					subscribedToOdomInfo_ = false;
+					RCLCPP_WARN(node.get_logger(),  "subscribe_odom_info ignored...");
 				}
+				SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
 			}
 			else if(subscribeOdomInfo)
 			{
 				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL2(rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_);
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL2(CommonDataSubscriber, rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]));
+			}
+		}
+#ifdef RTABMAP_SYNC_USER_DATA
+		else if(subscribeUserData)
+		{
+			userDataSub_.subscribe(&node, "user_data");
+			if(subscribeScanDesc)
+			{
+				subscribedToScanDescriptor_ = true;
+				scanDescSub_.subscribe(&node, "scan_descriptor");
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = false;
+					RCLCPP_WARN(node.get_logger(),  "subscribe_odom_info ignored...");
+				}
+				SYNC_DECL3(CommonDataSubscriber, rgbdDataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
+			}
+			else if(subscribeScan2d)
+			{
+				subscribedToScan2d_ = true;
+				scanSub_.subscribe(&node, "scan");
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = false;
+					RCLCPP_WARN(node.get_logger(),  "subscribe_odom_info ignored...");
+				}
+				SYNC_DECL3(CommonDataSubscriber, rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
+			}
+			else if(subscribeScan3d)
+			{
+				subscribedToScan3d_ = true;
+				scan3dSub_.subscribe(&node, "scan_cloud");
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = false;
+					RCLCPP_WARN(node.get_logger(),  "subscribe_odom_info ignored...");
+				}
+				SYNC_DECL3(CommonDataSubscriber, rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
+			}
+			else if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL2(CommonDataSubscriber, rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]));
+			}
+		}
+#endif
+		else
+		{
+			if(subscribeScanDesc)
+			{
+				subscribedToScanDescriptor_ = true;
+				scanDescSub_.subscribe(&node, "scan_descriptor");
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = false;
+					RCLCPP_WARN(node.get_logger(),  "subscribe_odom_info ignored...");
+				}
+				SYNC_DECL2(CommonDataSubscriber, rgbdScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), scanDescSub_);
+			}
+			else if(subscribeScan2d)
+			{
+				subscribedToScan2d_ = true;
+				scanSub_.subscribe(&node, "scan");
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = false;
+					RCLCPP_WARN(node.get_logger(),  "subscribe_odom_info ignored...");
+				}
+				SYNC_DECL2(CommonDataSubscriber, rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
+			}
+			else if(subscribeScan3d)
+			{
+				subscribedToScan3d_ = true;
+				scan3dSub_.subscribe(&node, "scan_cloud");
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = false;
+					RCLCPP_WARN(node.get_logger(),  "subscribe_odom_info ignored...");
+				}
+				SYNC_DECL2(CommonDataSubscriber, rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
+			}
+			else if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(&node, "odom_info");
+				SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_);
 			}
 			else
 			{
@@ -550,7 +745,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 	}
 	else
 	{
-		rgbdSub_ = node.create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::SensorDataQoS(), std::bind(&CommonDataSubscriber::rgbdCallback, this, std::placeholders::_1));
+		rgbdSub_ = node.create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", 5, std::bind(&CommonDataSubscriber::rgbdCallback, this, std::placeholders::_1));
 
 		subscribedTopicsMsg_ =
 				uFormat("\n%s subscribed to:\n   %s",

src/impl/CommonDataSubscriberRGBD2.cpp

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap_ros/CommonDataSubscriber.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
 #include <rtabmap_ros/MsgConversion.h>
 #include <cv_bridge/cv_bridge.h>
 
@@ -38,9 +39,25 @@ namespace rtabmap_ros {
 		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2); \
 		rtabmap_ros::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
 		rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
+		if(!depthMsgs[0].get()) \
+			depthMsgs.clear(); \
 		std::vector<sensor_msgs::msg::CameraInfo> cameraInfoMsgs; \
 		cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
-		cameraInfoMsgs.push_back(image2Msg->rgb_camera_info);
+		cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
+		std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs; \
+		std::vector<std::vector<rtabmap_ros::msg::KeyPoint> > localKeyPoints; \
+		std::vector<std::vector<rtabmap_ros::msg::Point3f> > localPoints3d; \
+		std::vector<cv::Mat> localDescriptors; \
+		if(!image1Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image1Msg->global_descriptor); \
+		if(!image2Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image2Msg->global_descriptor); \
+		localKeyPoints.push_back(image1Msg->key_points); \
+		localKeyPoints.push_back(image2Msg->key_points); \
+		localPoints3d.push_back(image1Msg->points); \
+		localPoints3d.push_back(image2Msg->points); \
+		localDescriptors.push_back(rtabmap::uncompressData(image1Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image2Msg->descriptors));
 
 // 2 RGBD
 void CommonDataSubscriber::rgbd2Callback(
@@ -51,10 +68,10 @@ void CommonDataSubscriber::rgbd2Callback(
 
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2Scan2dCallback(
 		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -65,9 +82,9 @@ void CommonDataSubscriber::rgbd2Scan2dCallback(
 
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2Scan3dCallback(
 		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -78,9 +95,25 @@ void CommonDataSubscriber::rgbd2Scan3dCallback(
 
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2ScanDescCallback(
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2InfoCallback(
 		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -89,39 +122,12 @@ void CommonDataSubscriber::rgbd2InfoCallback(
 {
 	IMAGE_CONVERSION();
 
-	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
-void CommonDataSubscriber::rgbd2Scan2dInfoCallback(
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbd2Scan3dInfoCallback(
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-
 // 2 RGBD + Odom
 void CommonDataSubscriber::rgbd2OdomCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -131,10 +137,10 @@ void CommonDataSubscriber::rgbd2OdomCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2OdomScan2dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -145,9 +151,9 @@ void CommonDataSubscriber::rgbd2OdomScan2dCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2OdomScan3dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -158,9 +164,25 @@ void CommonDataSubscriber::rgbd2OdomScan3dCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2OdomScanDescCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2OdomInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -170,38 +192,13 @@ void CommonDataSubscriber::rgbd2OdomInfoCallback(
 {
 	IMAGE_CONVERSION();
 
-	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbd2OdomScan2dInfoCallback(
-		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbd2OdomScan3dInfoCallback(
-		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 
+#ifdef RTABMAP_SYNC_USER_DATA
 // 2 RGBD + User Data
 void CommonDataSubscriber::rgbd2DataCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -211,10 +208,10 @@ void CommonDataSubscriber::rgbd2DataCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2DataScan2dCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -225,9 +222,9 @@ void CommonDataSubscriber::rgbd2DataScan2dCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2DataScan3dCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -238,9 +235,25 @@ void CommonDataSubscriber::rgbd2DataScan3dCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2DataScanDescCallback(
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2DataInfoCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -250,36 +263,10 @@ void CommonDataSubscriber::rgbd2DataInfoCallback(
 {
 	IMAGE_CONVERSION();
 
-	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbd2DataScan2dInfoCallback(
-		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbd2DataScan3dInfoCallback(
-		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 
 // 2 RGBD + Odom + User Data
@@ -291,10 +278,10 @@ void CommonDataSubscriber::rgbd2OdomDataCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2OdomDataScan2dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -305,9 +292,9 @@ void CommonDataSubscriber::rgbd2OdomDataScan2dCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2OdomDataScan3dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -318,9 +305,25 @@ void CommonDataSubscriber::rgbd2OdomDataScan3dCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd2OdomDataScanDescCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd2OdomDataInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -331,36 +334,11 @@ void CommonDataSubscriber::rgbd2OdomDataInfoCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbd2OdomDataScan2dInfoCallback(
-		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
-		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbd2OdomDataScan3dInfoCallback(
-		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
-		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
+#endif
 
 void CommonDataSubscriber::setupRGBD2Callbacks(
 		rclcpp::Node& node,
@@ -368,6 +346,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 		bool subscribeUserData,
 		bool subscribeScan2d,
 		bool subscribeScan3d,
+		bool subscribeScanDesc,
 		bool subscribeOdomInfo,
 		int queueSize,
 		bool approxSync)
@@ -378,182 +357,199 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 	for(int i=0; i<2; ++i)
 	{
 		rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::msg::RGBDImage>;
-		rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rmw_qos_profile_sensor_data);
+		rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i));
 	}
+#ifdef RTABMAP_SYNC_USER_DATA
 	if(subscribeOdom && subscribeUserData)
 	{
-		odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
-		userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
-		if(subscribeScan2d)
+		odomSub_.subscribe(&node, "odom");
+		userDataSub_.subscribe(&node, "user_data");
+		if(subscribeScanDesc)
 		{
-			subscribedToScan2d_ = true;
-			scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL6(rgbd2OdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
-			else
+			SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
 			{
-				SYNC_DECL5(rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
+			SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
 			subscribedToScan3d_ = true;
-			scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
+			scan3dSub_.subscribe(&node, "scan_cloud");
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL6(rgbd2OdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL5(rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
+			SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL5(rgbd2OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL4(rgbd2OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
+			SYNC_DECL4(CommonDataSubscriber, rgbd2OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
 		}
 	}
-	else if(subscribeOdom)
+	else 
+#endif
+	if(subscribeOdom)
 	{
-		odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
-		if(subscribeScan2d)
+		odomSub_.subscribe(&node, "odom");
+		if(subscribeScanDesc)
 		{
-			subscribedToScan2d_ = true;
-			scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL5(rgbd2OdomScan2dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
-			else
+			SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
 			{
-				SYNC_DECL4(rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
+			SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
 			subscribedToScan3d_ = true;
-			scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
+			scan3dSub_.subscribe(&node, "scan_cloud");
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL5(rgbd2OdomScan3dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL4(rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
+			SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL4(rgbd2OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL4(CommonDataSubscriber, rgbd2OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL3(rgbd2Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
+			SYNC_DECL3(CommonDataSubscriber, rgbd2Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
 		}
 	}
+#ifdef RTABMAP_SYNC_USER_DATA
 	else if(subscribeUserData)
 	{
-		userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
-		if(subscribeScan2d)
+		userDataSub_.subscribe(&node, "user_data");
+		if(subscribeScanDesc)
 		{
-			subscribedToScan2d_ = true;
-			scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL5(rgbd2DataScan2dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
-			else
+			SYNC_DECL4(CommonDataSubscriber, rgbd2DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
 			{
-				SYNC_DECL4(rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
+			SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
 			subscribedToScan3d_ = true;
-			scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
+			scan3dSub_.subscribe(&node, "scan_cloud");
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL5(rgbd2DataScan3dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL4(rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
+			SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL4(rgbd2DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL4(CommonDataSubscriber, rgbd2DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL3(rgbd2Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
+			SYNC_DECL3(CommonDataSubscriber, rgbd2Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
 		}
 	}
+#endif
 	else
 	{
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
 		{
-			subscribedToScan2d_ = true;
-			scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL4(rgbd2Scan2dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
-			else
+			SYNC_DECL3(CommonDataSubscriber, rgbd2ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
 			{
-				SYNC_DECL3(rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
+			SYNC_DECL3(CommonDataSubscriber, rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
 			subscribedToScan3d_ = true;
-			scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
+			scan3dSub_.subscribe(&node, "scan_cloud");
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL4(rgbd2Scan3dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL3(rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
+			SYNC_DECL3(CommonDataSubscriber, rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL3(rgbd2Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL3(CommonDataSubscriber, rgbd2Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL2(rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
+			SYNC_DECL2(CommonDataSubscriber, rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
 		}
 	}
 }

src/impl/CommonDataSubscriberRGBD3.cpp

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap_ros/CommonDataSubscriber.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
 #include <rtabmap_ros/MsgConversion.h>
 #include <cv_bridge/cv_bridge.h>
 
@@ -39,10 +40,31 @@ namespace rtabmap_ros {
 		rtabmap_ros::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
 		rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
 		rtabmap_ros::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
+		if(!depthMsgs[0].get()) \
+			depthMsgs.clear(); \
 		std::vector<sensor_msgs::msg::CameraInfo> cameraInfoMsgs; \
 		cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
 		cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
-		cameraInfoMsgs.push_back(image3Msg->rgb_camera_info);
+		cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
+		std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs; \
+		std::vector<std::vector<rtabmap_ros::msg::KeyPoint> > localKeyPoints; \
+		std::vector<std::vector<rtabmap_ros::msg::Point3f> > localPoints3d; \
+		std::vector<cv::Mat> localDescriptors; \
+		if(!image1Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image1Msg->global_descriptor); \
+		if(!image2Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image2Msg->global_descriptor); \
+		if(!image3Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image3Msg->global_descriptor); \
+		localKeyPoints.push_back(image1Msg->key_points); \
+		localKeyPoints.push_back(image2Msg->key_points); \
+		localKeyPoints.push_back(image3Msg->key_points); \
+		localPoints3d.push_back(image1Msg->points); \
+		localPoints3d.push_back(image2Msg->points); \
+		localPoints3d.push_back(image3Msg->points); \
+		localDescriptors.push_back(rtabmap::uncompressData(image1Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image2Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image3Msg->descriptors));
 
 // 3 RGBD
 void CommonDataSubscriber::rgbd3Callback(
@@ -54,10 +76,13 @@ void CommonDataSubscriber::rgbd3Callback(
 
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3Scan2dCallback(
 		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -69,9 +94,12 @@ void CommonDataSubscriber::rgbd3Scan2dCallback(
 
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, *scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3Scan3dCallback(
 		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -83,9 +111,32 @@ void CommonDataSubscriber::rgbd3Scan3dCallback(
 
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3ScanDescCallback(
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
+			scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3InfoCallback(
 		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -95,39 +146,14 @@ void CommonDataSubscriber::rgbd3InfoCallback(
 {
 	IMAGE_CONVERSION();
 
-	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbd3Scan2dInfoCallback(
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
-		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbd3Scan3dInfoCallback(
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
-		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 
 // 2 RGBD + Odom
@@ -140,10 +166,13 @@ void CommonDataSubscriber::rgbd3OdomCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3OdomScan2dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -155,9 +184,12 @@ void CommonDataSubscriber::rgbd3OdomScan2dCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, *scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3OdomScan3dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -169,9 +201,32 @@ void CommonDataSubscriber::rgbd3OdomScan3dCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3OdomScanDescCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
+			scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3OdomInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -182,40 +237,16 @@ void CommonDataSubscriber::rgbd3OdomInfoCallback(
 {
 	IMAGE_CONVERSION();
 
-	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbd3OdomScan2dInfoCallback(
-		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
-		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbd3OdomScan3dInfoCallback(
-		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
-		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 
+#ifdef RTABMAP_SYNC_USER_DATA
 // 2 RGBD + User Data
 void CommonDataSubscriber::rgbd3DataCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -226,10 +257,13 @@ void CommonDataSubscriber::rgbd3DataCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3DataScan2dCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -241,9 +275,12 @@ void CommonDataSubscriber::rgbd3DataScan2dCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, *scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3DataScan3dCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -255,9 +292,32 @@ void CommonDataSubscriber::rgbd3DataScan3dCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3DataScanDescCallback(
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
+			scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3DataInfoCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -268,40 +328,14 @@ void CommonDataSubscriber::rgbd3DataInfoCallback(
 {
 	IMAGE_CONVERSION();
 
-	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
-void CommonDataSubscriber::rgbd3DataScan2dInfoCallback(
-		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
-		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbd3DataScan3dInfoCallback(
-		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
-		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-
 // 2 RGBD + Odom + User Data
 void CommonDataSubscriber::rgbd3OdomDataCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -312,10 +346,13 @@ void CommonDataSubscriber::rgbd3OdomDataCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3OdomDataScan2dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -327,9 +364,12 @@ void CommonDataSubscriber::rgbd3OdomDataScan2dCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, *scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3OdomDataScan3dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -341,9 +381,32 @@ void CommonDataSubscriber::rgbd3OdomDataScan3dCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd3OdomDataScanDescCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
+			scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd3OdomDataInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -355,38 +418,14 @@ void CommonDataSubscriber::rgbd3OdomDataInfoCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbd3OdomDataScan2dInfoCallback(
-		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
-		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
-		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbd3OdomDataScan3dInfoCallback(
-		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
-		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
-		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
+			depthMsgs, cameraInfoMsgs, scanMsg,
+			scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
+			localKeyPoints, localPoints3d, localDescriptors);
 }
+#endif
 
 void CommonDataSubscriber::setupRGBD3Callbacks(
 		rclcpp::Node& node,
@@ -394,6 +433,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 		bool subscribeUserData,
 		bool subscribeScan2d,
 		bool subscribeScan3d,
+		bool subscribeScanDescriptor,
 		bool subscribeOdomInfo,
 		int queueSize,
 		bool approxSync)
@@ -404,182 +444,200 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 	for(int i=0; i<3; ++i)
 	{
 		rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::msg::RGBDImage>;
-		rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rmw_qos_profile_sensor_data);
+		rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i));
 	}
+#ifdef RTABMAP_SYNC_USER_DATA
 	if(subscribeOdom && subscribeUserData)
 	{
-		odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
-		userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
-		if(subscribeScan2d)
+		odomSub_.subscribe(&node, "odom");
+		userDataSub_.subscribe(&node, "user_data");
+		if(subscribeScanDescriptor)
 		{
-			subscribedToScan2d_ = true;
-			scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL7(rgbd3OdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_, odomInfoSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
-			else
+			SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
 			{
-				SYNC_DECL6(rgbd3OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
+			SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
 			subscribedToScan3d_ = true;
-			scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
+			scan3dSub_.subscribe(&node, "scan_cloud");
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL7(rgbd3OdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL6(rgbd3OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
+			SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL6(rgbd3OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL5(rgbd3OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
-		}
-	}
-	else if(subscribeOdom)
-	{
-		odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
-		if(subscribeScan2d)
-		{
-			subscribedToScan2d_ = true;
-			scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
-			if(subscribeOdomInfo)
-			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL6(rgbd3OdomScan2dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL5(rgbd3OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
-			}
-		}
-		else if(subscribeScan3d)
-		{
-			subscribedToScan3d_ = true;
-			scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
-			if(subscribeOdomInfo)
-			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL6(rgbd3OdomScan3dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL5(rgbd3OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
-			}
-		}
-		else if(subscribeOdomInfo)
-		{
-			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL5(rgbd3OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
-		}
-		else
-		{
-			SYNC_DECL4(rgbd3Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
-		}
-	}
-	else if(subscribeUserData)
-	{
-		userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
-		if(subscribeScan2d)
-		{
-			subscribedToScan2d_ = true;
-			scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
-			if(subscribeOdomInfo)
-			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL6(rgbd3DataScan2dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL5(rgbd3DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
-			}
-		}
-		else if(subscribeScan3d)
-		{
-			subscribedToScan3d_ = true;
-			scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
-			if(subscribeOdomInfo)
-			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL6(rgbd3DataScan3dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL5(rgbd3DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
-			}
-		}
-		else if(subscribeOdomInfo)
-		{
-			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL5(rgbd3DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
-		}
-		else
-		{
-			SYNC_DECL4(rgbd3Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
+			SYNC_DECL5(CommonDataSubscriber, rgbd3OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
 		}
 	}
 	else
+#endif		
+	if(subscribeOdom)
 	{
-		if(subscribeScan2d)
+		odomSub_.subscribe(&node, "odom");
+		if(subscribeScanDescriptor)
 		{
-			subscribedToScan2d_ = true;
-			scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL5(rgbd3Scan2dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_, odomInfoSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
-			else
+			SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
 			{
-				SYNC_DECL4(rgbd3Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
+			SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
 			subscribedToScan3d_ = true;
-			scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
+			scan3dSub_.subscribe(&node, "scan_cloud");
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL5(rgbd3Scan3dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL4(rgbd3Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
+			SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL4(rgbd3Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL5(CommonDataSubscriber, rgbd3OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL3(rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
+			SYNC_DECL4(CommonDataSubscriber, rgbd3Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
+		}
+	}
+#ifdef RTABMAP_SYNC_USER_DATA
+	else if(subscribeUserData)
+	{
+		userDataSub_.subscribe(&node, "user_data");
+		if(subscribeScanDescriptor)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL5(CommonDataSubscriber, rgbd3DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(&node, "scan_cloud");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL5(CommonDataSubscriber, rgbd3DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL4(CommonDataSubscriber, rgbd3Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
+		}
+	}
+#endif
+	else
+	{
+		if(subscribeScanDescriptor)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL4(CommonDataSubscriber, rgbd3ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL4(CommonDataSubscriber, rgbd3Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(&node, "scan_cloud");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL4(CommonDataSubscriber, rgbd3Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL4(CommonDataSubscriber, rgbd3Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL3(CommonDataSubscriber, rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
 		}
 	}
 }

src/impl/CommonDataSubscriberRGBD4.cpp

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap_ros/CommonDataSubscriber.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
 #include <rtabmap_ros/MsgConversion.h>
 #include <cv_bridge/cv_bridge.h>
 
@@ -40,11 +41,37 @@ namespace rtabmap_ros {
 		rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
 		rtabmap_ros::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
 		rtabmap_ros::toCvShare(image4Msg, imageMsgs[3], depthMsgs[3]); \
+		if(!depthMsgs[0].get()) \
+			depthMsgs.clear(); \
 		std::vector<sensor_msgs::msg::CameraInfo> cameraInfoMsgs; \
 		cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
 		cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
 		cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
-		cameraInfoMsgs.push_back(image4Msg->rgb_camera_info);
+		cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
+		std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs; \
+		std::vector<std::vector<rtabmap_ros::msg::KeyPoint> > localKeyPoints; \
+		std::vector<std::vector<rtabmap_ros::msg::Point3f> > localPoints3d; \
+		std::vector<cv::Mat> localDescriptors; \
+		if(!image1Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image1Msg->global_descriptor); \
+		if(!image2Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image2Msg->global_descriptor); \
+		if(!image3Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image3Msg->global_descriptor); \
+		if(!image4Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image4Msg->global_descriptor); \
+		localKeyPoints.push_back(image1Msg->key_points); \
+		localKeyPoints.push_back(image2Msg->key_points); \
+		localKeyPoints.push_back(image3Msg->key_points); \
+		localKeyPoints.push_back(image4Msg->key_points); \
+		localPoints3d.push_back(image1Msg->points); \
+		localPoints3d.push_back(image2Msg->points); \
+		localPoints3d.push_back(image3Msg->points); \
+		localPoints3d.push_back(image4Msg->points); \
+		localDescriptors.push_back(rtabmap::uncompressData(image1Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image2Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image3Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image4Msg->descriptors));
 
 // 4 RGBD
 void CommonDataSubscriber::rgbd4Callback(
@@ -57,10 +84,10 @@ void CommonDataSubscriber::rgbd4Callback(
 
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4Scan2dCallback(
 		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -73,9 +100,9 @@ void CommonDataSubscriber::rgbd4Scan2dCallback(
 
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4Scan3dCallback(
 		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -88,9 +115,27 @@ void CommonDataSubscriber::rgbd4Scan3dCallback(
 
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4ScanDescCallback(
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4InfoCallback(
 		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -103,39 +148,9 @@ void CommonDataSubscriber::rgbd4InfoCallback(
 
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbd4Scan2dInfoCallback(
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
-		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
-	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbd4Scan3dInfoCallback(
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
-		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
-	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 
 // 2 RGBD + Odom
@@ -149,10 +164,10 @@ void CommonDataSubscriber::rgbd4OdomCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4OdomScan2dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -165,9 +180,9 @@ void CommonDataSubscriber::rgbd4OdomScan2dCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4OdomScan3dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -180,9 +195,27 @@ void CommonDataSubscriber::rgbd4OdomScan3dCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4OdomScanDescCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4OdomInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -195,41 +228,12 @@ void CommonDataSubscriber::rgbd4OdomInfoCallback(
 	IMAGE_CONVERSION();
 
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbd4OdomScan2dInfoCallback(
-		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
-		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbd4OdomScan3dInfoCallback(
-		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
-		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 
+#ifdef RTABMAP_SYNC_USER_DATA
 // 2 RGBD + User Data
 void CommonDataSubscriber::rgbd4DataCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -241,10 +245,10 @@ void CommonDataSubscriber::rgbd4DataCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4DataScan2dCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -257,9 +261,9 @@ void CommonDataSubscriber::rgbd4DataScan2dCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4DataScan3dCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -272,9 +276,27 @@ void CommonDataSubscriber::rgbd4DataScan3dCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4DataScanDescCallback(
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4DataInfoCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -287,39 +309,9 @@ void CommonDataSubscriber::rgbd4DataInfoCallback(
 	IMAGE_CONVERSION();
 
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbd4DataScan2dInfoCallback(
-		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
-		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbd4DataScan3dInfoCallback(
-		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
-		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 
 // 2 RGBD + Odom + User Data
@@ -333,10 +325,10 @@ void CommonDataSubscriber::rgbd4OdomDataCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4OdomDataScan2dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -349,9 +341,9 @@ void CommonDataSubscriber::rgbd4OdomDataScan2dCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4OdomDataScan3dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -364,9 +356,27 @@ void CommonDataSubscriber::rgbd4OdomDataScan3dCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd4OdomDataScanDescCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
 void CommonDataSubscriber::rgbd4OdomDataInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -379,40 +389,11 @@ void CommonDataSubscriber::rgbd4OdomDataInfoCallback(
 {
 	IMAGE_CONVERSION();
 
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbd4OdomDataScan2dInfoCallback(
-		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
-		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
-		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
-}
-void CommonDataSubscriber::rgbd4OdomDataScan3dInfoCallback(
-		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
-		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
-		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
-		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
-{
-	IMAGE_CONVERSION();
-
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
-	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
 }
+#endif
 
 void CommonDataSubscriber::setupRGBD4Callbacks(
 		rclcpp::Node& node,
@@ -420,6 +401,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 		bool subscribeUserData,
 		bool subscribeScan2d,
 		bool subscribeScan3d,
+		bool subscribeScanDesc,
 		bool subscribeOdomInfo,
 		int queueSize,
 		bool approxSync)
@@ -430,182 +412,199 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 	for(int i=0; i<4; ++i)
 	{
 		rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::msg::RGBDImage>;
-		rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rmw_qos_profile_sensor_data);
+		rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i));
 	}
+#ifdef RTABMAP_SYNC_USER_DATA
 	if(subscribeOdom && subscribeUserData)
 	{
-		odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
+		odomSub_.subscribe(&node, "odom");
 		userDataSub_.subscribe(&node, "user_data");
-		if(subscribeScan2d)
+		if(subscribeScanDesc)
 		{
-			subscribedToScan2d_ = true;
-			scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL8(rgbd4OdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_, odomInfoSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
-			else
+			SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
 			{
-				SYNC_DECL7(rgbd4OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
+			SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
 			subscribedToScan3d_ = true;
-			scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
+			scan3dSub_.subscribe(&node, "scan_cloud");
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL8(rgbd4OdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL7(rgbd4OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
+			SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL7(rgbd4OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL6(rgbd4OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
-		}
-	}
-	else if(subscribeOdom)
-	{
-		odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
-		if(subscribeScan2d)
-		{
-			subscribedToScan2d_ = true;
-			scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
-			if(subscribeOdomInfo)
-			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL7(rgbd4OdomScan2dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL6(rgbd4OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
-			}
-		}
-		else if(subscribeScan3d)
-		{
-			subscribedToScan3d_ = true;
-			scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
-			if(subscribeOdomInfo)
-			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL7(rgbd4OdomScan3dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL6(rgbd4OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
-			}
-		}
-		else if(subscribeOdomInfo)
-		{
-			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL6(rgbd4OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
-		}
-		else
-		{
-			SYNC_DECL5(rgbd4Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
-		}
-	}
-	else if(subscribeUserData)
-	{
-		userDataSub_.subscribe(&node, "user_data");
-		if(subscribeScan2d)
-		{
-			subscribedToScan2d_ = true;
-			scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
-			if(subscribeOdomInfo)
-			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL7(rgbd4DataScan2dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL6(rgbd4DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
-			}
-		}
-		else if(subscribeScan3d)
-		{
-			subscribedToScan3d_ = true;
-			scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
-			if(subscribeOdomInfo)
-			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL7(rgbd4DataScan3dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL6(rgbd4DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
-			}
-		}
-		else if(subscribeOdomInfo)
-		{
-			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL6(rgbd4DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
-		}
-		else
-		{
-			SYNC_DECL5(rgbd4Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
+			SYNC_DECL6(CommonDataSubscriber, rgbd4OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
 		}
 	}
 	else
+#endif		
+	if(subscribeOdom)
 	{
-		if(subscribeScan2d)
+		odomSub_.subscribe(&node, "odom");
+		if(subscribeScanDesc)
 		{
-			subscribedToScan2d_ = true;
-			scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL6(rgbd4Scan2dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_, odomInfoSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
-			else
+			SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
 			{
-				SYNC_DECL5(rgbd4Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
+			SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
 			subscribedToScan3d_ = true;
-			scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
+			scan3dSub_.subscribe(&node, "scan_cloud");
 			if(subscribeOdomInfo)
 			{
-				subscribedToOdomInfo_ = true;
-				odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-				SYNC_DECL6(rgbd4Scan3dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_, odomInfoSub_);
-			}
-			else
-			{
-				SYNC_DECL5(rgbd4Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
 			}
+			SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL5(rgbd4Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL6(CommonDataSubscriber, rgbd4OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL4(rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
+			SYNC_DECL5(CommonDataSubscriber, rgbd4Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
+		}
+	}
+#ifdef RTABMAP_SYNC_USER_DATA
+	else if(subscribeUserData)
+	{
+		userDataSub_.subscribe(&node, "user_data");
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL6(CommonDataSubscriber, rgbd4DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(&node, "scan_cloud");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL6(CommonDataSubscriber, rgbd4DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL5(CommonDataSubscriber, rgbd4Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
+		}
+	}
+#endif
+	else
+	{
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL5(CommonDataSubscriber, rgbd4ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL5(CommonDataSubscriber, rgbd4Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(&node, "scan_cloud");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL5(CommonDataSubscriber, rgbd4Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL5(CommonDataSubscriber, rgbd4Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL4(CommonDataSubscriber, rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
 		}
 	}
 }

src/impl/CommonDataSubscriberRGBD5.cpp

@@ -0,0 +1,367 @@
+/*
+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/CommonDataSubscriber.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
+#include <rtabmap_ros/MsgConversion.h>
+#include <cv_bridge/cv_bridge.h>
+
+namespace rtabmap_ros {
+
+#define IMAGE_CONVERSION() \
+		callbackCalled(); \
+		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(5); \
+		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(5); \
+		rtabmap_ros::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
+		rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
+		rtabmap_ros::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
+		rtabmap_ros::toCvShare(image4Msg, imageMsgs[3], depthMsgs[3]); \
+		rtabmap_ros::toCvShare(image5Msg, imageMsgs[4], depthMsgs[4]); \
+		if(!depthMsgs[0].get()) \
+			depthMsgs.clear(); \
+		std::vector<sensor_msgs::msg::CameraInfo> cameraInfoMsgs; \
+		cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image5Msg->rgb_camera_info); \
+		std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs; \
+		std::vector<std::vector<rtabmap_ros::msg::KeyPoint> > localKeyPoints; \
+		std::vector<std::vector<rtabmap_ros::msg::Point3f> > localPoints3d; \
+		std::vector<cv::Mat> localDescriptors; \
+		if(!image1Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image1Msg->global_descriptor); \
+		if(!image2Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image2Msg->global_descriptor); \
+		if(!image3Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image3Msg->global_descriptor); \
+		if(!image4Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image4Msg->global_descriptor); \
+		if(!image5Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image5Msg->global_descriptor); \
+		localKeyPoints.push_back(image1Msg->key_points); \
+		localKeyPoints.push_back(image2Msg->key_points); \
+		localKeyPoints.push_back(image3Msg->key_points); \
+		localKeyPoints.push_back(image4Msg->key_points); \
+		localKeyPoints.push_back(image5Msg->key_points); \
+		localPoints3d.push_back(image1Msg->points); \
+		localPoints3d.push_back(image2Msg->points); \
+		localPoints3d.push_back(image3Msg->points); \
+		localPoints3d.push_back(image4Msg->points); \
+		localPoints3d.push_back(image5Msg->points); \
+		localDescriptors.push_back(rtabmap::uncompressData(image1Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image2Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image3Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image4Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image5Msg->descriptors));
+
+// 5 RGBD
+void CommonDataSubscriber::rgbd5Callback(
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5Msg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5Scan2dCallback(
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5Msg,
+		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5Scan3dCallback(
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5Msg,
+		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5ScanDescCallback(
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5Msg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5InfoCallback(
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5Msg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+
+// 5 RGBD + Odom
+void CommonDataSubscriber::rgbd5OdomCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5Msg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5OdomScan2dCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5Msg,
+		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5OdomScan3dCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5Msg,
+		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5OdomScanDescCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5Msg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd5OdomInfoCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5Msg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+
+void CommonDataSubscriber::setupRGBD5Callbacks(
+		rclcpp::Node& node,
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeScanDesc,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	RCLCPP_INFO(node.get_logger(), "Setup rgbd5 callback");
+
+	rgbdSubs_.resize(5);
+	for(int i=0; i<5; ++i)
+	{
+		rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::msg::RGBDImage>;
+		rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i));
+	}
+	if(subscribeOdom)
+	{
+		odomSub_.subscribe(&node, "odom");
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(&node, "scan_cloud");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL7(CommonDataSubscriber, rgbd5OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL6(CommonDataSubscriber, rgbd5Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
+		}
+	}
+	else
+	{
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL6(CommonDataSubscriber, rgbd5ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL6(CommonDataSubscriber, rgbd5Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(&node, "scan_cloud");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL6(CommonDataSubscriber, rgbd5Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL6(CommonDataSubscriber, rgbd5Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL5(CommonDataSubscriber, rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
+		}
+	}
+}
+
+} /* namespace rtabmap_ros */

src/impl/CommonDataSubscriberRGBD6.cpp

@@ -0,0 +1,384 @@
+/*
+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/CommonDataSubscriber.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
+#include <rtabmap_ros/MsgConversion.h>
+#include <cv_bridge/cv_bridge.h>
+
+namespace rtabmap_ros {
+
+#define IMAGE_CONVERSION() \
+		callbackCalled(); \
+		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(6); \
+		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(6); \
+		rtabmap_ros::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
+		rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
+		rtabmap_ros::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
+		rtabmap_ros::toCvShare(image4Msg, imageMsgs[3], depthMsgs[3]); \
+		rtabmap_ros::toCvShare(image5Msg, imageMsgs[4], depthMsgs[4]); \
+		rtabmap_ros::toCvShare(image6Msg, imageMsgs[5], depthMsgs[5]); \
+		if(!depthMsgs[0].get()) \
+			depthMsgs.clear(); \
+		std::vector<sensor_msgs::msg::CameraInfo> cameraInfoMsgs; \
+		cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image5Msg->rgb_camera_info); \
+		cameraInfoMsgs.push_back(image6Msg->rgb_camera_info); \
+		std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs; \
+		std::vector<std::vector<rtabmap_ros::msg::KeyPoint> > localKeyPoints; \
+		std::vector<std::vector<rtabmap_ros::msg::Point3f> > localPoints3d; \
+		std::vector<cv::Mat> localDescriptors; \
+		if(!image1Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image1Msg->global_descriptor); \
+		if(!image2Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image2Msg->global_descriptor); \
+		if(!image3Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image3Msg->global_descriptor); \
+		if(!image4Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image4Msg->global_descriptor); \
+		if(!image5Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image5Msg->global_descriptor); \
+		if(!image6Msg->global_descriptor.data.empty()) \
+			globalDescriptorMsgs.push_back(image6Msg->global_descriptor); \
+		localKeyPoints.push_back(image1Msg->key_points); \
+		localKeyPoints.push_back(image2Msg->key_points); \
+		localKeyPoints.push_back(image3Msg->key_points); \
+		localKeyPoints.push_back(image4Msg->key_points); \
+		localKeyPoints.push_back(image5Msg->key_points); \
+		localKeyPoints.push_back(image6Msg->key_points); \
+		localPoints3d.push_back(image1Msg->points); \
+		localPoints3d.push_back(image2Msg->points); \
+		localPoints3d.push_back(image3Msg->points); \
+		localPoints3d.push_back(image4Msg->points); \
+		localPoints3d.push_back(image5Msg->points); \
+		localPoints3d.push_back(image6Msg->points); \
+		localDescriptors.push_back(rtabmap::uncompressData(image1Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image2Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image3Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image4Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image5Msg->descriptors)); \
+		localDescriptors.push_back(rtabmap::uncompressData(image6Msg->descriptors));
+
+// 6 RGBD
+void CommonDataSubscriber::rgbd6Callback(
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image6Msg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6Scan2dCallback(
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image6Msg,
+		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6Scan3dCallback(
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image6Msg,
+		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6ScanDescCallback(
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image6Msg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6InfoCallback(
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image6Msg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+
+// 6 RGBD + Odom
+void CommonDataSubscriber::rgbd6OdomCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image6Msg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6OdomScan2dCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image6Msg,
+		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6OdomScan3dCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image6Msg,
+		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6OdomScanDescCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image6Msg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbd6OdomInfoCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image5Msg,
+		const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image6Msg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+
+void CommonDataSubscriber::setupRGBD6Callbacks(
+		rclcpp::Node& node,
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeScanDesc,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	RCLCPP_INFO(node.get_logger(), "Setup rgbd6 callback");
+
+	rgbdSubs_.resize(6);
+	for(int i=0; i<6; ++i)
+	{
+		rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::msg::RGBDImage>;
+		rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i));
+	}
+	if(subscribeOdom)
+	{
+		odomSub_.subscribe(&node, "odom");
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(&node, "scan_cloud");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL8(CommonDataSubscriber, rgbd6OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL7(CommonDataSubscriber, rgbd6Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
+		}
+	}
+	else
+	{
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL7(CommonDataSubscriber, rgbd6ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL7(CommonDataSubscriber, rgbd6Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(&node, "scan_cloud");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL7(CommonDataSubscriber, rgbd6Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL7(CommonDataSubscriber, rgbd6Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL6(CommonDataSubscriber, rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
+		}
+	}
+}
+
+} /* namespace rtabmap_ros */

src/impl/CommonDataSubscriberRGBDX.cpp

@@ -0,0 +1,534 @@
+/*
+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/CommonDataSubscriber.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
+#include <rtabmap_ros/MsgConversion.h>
+#include <cv_bridge/cv_bridge.h>
+
+namespace rtabmap_ros {
+
+#define IMAGE_CONVERSION() \
+		UASSERT(!imagesMsg->rgbd_images.empty()); \
+		callbackCalled(); \
+		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(imagesMsg->rgbd_images.size()); \
+		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(imagesMsg->rgbd_images.size()); \
+		std::vector<sensor_msgs::msg::CameraInfo> cameraInfoMsgs; \
+		std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs; \
+		std::vector<std::vector<rtabmap_ros::msg::KeyPoint> > localKeyPoints; \
+		std::vector<std::vector<rtabmap_ros::msg::Point3f> > localPoints3d; \
+		std::vector<cv::Mat> localDescriptors; \
+		for(size_t i=0; i<imageMsgs.size(); ++i) \
+		{ \
+			rtabmap_ros::toCvShare(imagesMsg->rgbd_images[i], imagesMsg, imageMsgs[i], depthMsgs[i]); \
+			cameraInfoMsgs.push_back(imagesMsg->rgbd_images[i].rgb_camera_info); \
+			if(!imagesMsg->rgbd_images[i].global_descriptor.data.empty()) \
+				globalDescriptorMsgs.push_back(imagesMsg->rgbd_images[i].global_descriptor); \
+			localKeyPoints.push_back(imagesMsg->rgbd_images[i].key_points); \
+			localPoints3d.push_back(imagesMsg->rgbd_images[i].points); \
+			localDescriptors.push_back(rtabmap::uncompressData(imagesMsg->rgbd_images[i].descriptors)); \
+		} \
+		if(!depthMsgs[0].get()) \
+			depthMsgs.clear();
+
+// X RGBD
+void CommonDataSubscriber::rgbdXCallback(
+		const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXScan2dCallback(
+		const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg,
+		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXScan3dCallback(
+		const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg,
+		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXScanDescCallback(
+		const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXInfoCallback(
+		const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+// X RGBD + Odom
+void CommonDataSubscriber::rgbdXOdomCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXOdomScan2dCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg,
+		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXOdomScan3dCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg,
+		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXOdomScanDescCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXOdomInfoCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+
+#ifdef RTABMAP_SYNC_USER_DATA
+// X RGBD + User Data
+void CommonDataSubscriber::rgbdXDataCallback(
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXDataScan2dCallback(
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg,
+		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXDataScan3dCallback(
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg,
+		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXDataScanDescCallback(
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXDataInfoCallback(
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+
+// X RGBD + Odom + User Data
+void CommonDataSubscriber::rgbdXOdomDataCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXOdomDataScan2dCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg,
+		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXOdomDataScan3dCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg,
+		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXOdomDataScanDescCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	if(!scanDescMsg->global_descriptor.data.empty())
+	{
+		globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
+	}
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXOdomDataInfoCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::msg::LaserScan scanMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+#endif
+
+void CommonDataSubscriber::setupRGBDXCallbacks(
+		rclcpp::Node& node,
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeScanDesc,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	RCLCPP_INFO(node.get_logger(), "Setup rgbdX callback");
+
+	rgbdXSub_.subscribe(&node, "rgbd_images");
+#ifdef RTABMAP_SYNC_USER_DATA
+	if(subscribeOdom && subscribeUserData)
+	{
+		odomSub_.subscribe(&node, "odom");
+		userDataSub_.subscribe(&node, "user_data");
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(&node, "scan_cloud");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL3(CommonDataSubscriber, rgbdXOdomData, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_);
+		}
+	}
+	else 
+#endif
+	if(subscribeOdom)
+	{
+		odomSub_.subscribe(&node, "odom");
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScanDesc, approxSync, queueSize, odomSub_, rgbdXSub_, scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan2d, approxSync, queueSize, odomSub_, rgbdXSub_, scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(&node, "scan_cloud");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan3d, approxSync, queueSize, odomSub_, rgbdXSub_, scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL3(CommonDataSubscriber, rgbdXOdomInfo, approxSync, queueSize, odomSub_, rgbdXSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL2(CommonDataSubscriber, rgbdXOdom, approxSync, queueSize, odomSub_, rgbdXSub_);
+		}
+	}
+#ifdef RTABMAP_SYNC_USER_DATA
+	else if(subscribeUserData)
+	{
+		userDataSub_.subscribe(&node, "user_data");
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL3(CommonDataSubscriber, rgbdXDataScanDesc, approxSync, queueSize, userDataSub_, rgbdXSub_, scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan2d, approxSync, queueSize, userDataSub_, rgbdXSub_, scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(&node, "scan_cloud");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan3d, approxSync, queueSize, userDataSub_, rgbdXSub_, scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL3(CommonDataSubscriber, rgbdXDataInfo, approxSync, queueSize, userDataSub_, rgbdXSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL2(CommonDataSubscriber, rgbdXData, approxSync, queueSize, userDataSub_, rgbdXSub_);
+		}
+	}
+#endif
+	else
+	{
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL2(CommonDataSubscriber, rgbdXScanDesc, approxSync, queueSize, rgbdXSub_, scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(&node, "scan");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL2(CommonDataSubscriber, rgbdXScan2d, approxSync, queueSize, rgbdXSub_, scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(&node, "scan_cloud");
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
+			}
+			SYNC_DECL2(CommonDataSubscriber, rgbdXScan3d, approxSync, queueSize, rgbdXSub_, scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL2(CommonDataSubscriber, rgbdXInfo, approxSync, queueSize, rgbdXSub_, odomInfoSub_);
+		}
+		else
+		{
+			rgbdXSubOnly_ = node.create_subscription<rtabmap_ros::msg::RGBDImages>("rgbd_images", 5, std::bind(&CommonDataSubscriber::rgbdXCallback, this, std::placeholders::_1));
+
+			subscribedTopicsMsg_ =
+					uFormat("\n%s subscribed to:\n   %s",
+					node.get_name(),
+					rgbdXSubOnly_->get_topic_name());
+		}
+	}
+}
+
+} /* namespace rtabmap_ros */

src/impl/CommonDataSubscriberScan.cpp

@@ -35,9 +35,9 @@ void CommonDataSubscriber::scan2dCallback(
 	callbackCalled();
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::scan3dCallback(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
@@ -45,9 +45,18 @@ void CommonDataSubscriber::scan3dCallback(
 	callbackCalled();
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::scanDescCallback(
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg)
+{
+	callbackCalled();
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
 void CommonDataSubscriber::scan2dInfoCallback(
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
@@ -56,8 +65,8 @@ void CommonDataSubscriber::scan2dInfoCallback(
 	callbackCalled();
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
-	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::scan3dInfoCallback(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
@@ -66,8 +75,17 @@ void CommonDataSubscriber::scan3dInfoCallback(
 	callbackCalled();
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
-	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::scanDescInfoCallback(
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	callbackCalled();
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
 
 void CommonDataSubscriber::odomScan2dCallback(
@@ -76,9 +94,9 @@ void CommonDataSubscriber::odomScan2dCallback(
 {
 	callbackCalled();
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::odomScan3dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -86,9 +104,18 @@ void CommonDataSubscriber::odomScan3dCallback(
 {
 	callbackCalled();
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomScanDescCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg)
+{
+	callbackCalled();
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
 void CommonDataSubscriber::odomScan2dInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -97,8 +124,8 @@ void CommonDataSubscriber::odomScan2dInfoCallback(
 {
 	callbackCalled();
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
-	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::odomScan3dInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -107,19 +134,29 @@ void CommonDataSubscriber::odomScan3dInfoCallback(
 {
 	callbackCalled();
 	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
-	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomScanDescInfoCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	callbackCalled();
+	rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
 
+#ifdef RTABMAP_SYNC_USER_DATA
 void CommonDataSubscriber::dataScan2dCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
 	callbackCalled();
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::dataScan3dCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -127,9 +164,18 @@ void CommonDataSubscriber::dataScan3dCallback(
 {
 	callbackCalled();
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::dataScanDescCallback(
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg)
+{
+	callbackCalled();
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
 void CommonDataSubscriber::dataScan2dInfoCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -138,8 +184,8 @@ void CommonDataSubscriber::dataScan2dInfoCallback(
 {
 	callbackCalled();
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
-	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::dataScan3dInfoCallback(
 		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -148,8 +194,17 @@ void CommonDataSubscriber::dataScan3dInfoCallback(
 {
 	callbackCalled();
 	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
-	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::dataScanDescInfoCallback(
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	callbackCalled();
+	nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
 
 void CommonDataSubscriber::odomDataScan2dCallback(
@@ -158,9 +213,9 @@ void CommonDataSubscriber::odomDataScan2dCallback(
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
 	callbackCalled();
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::odomDataScan3dCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -168,9 +223,18 @@ void CommonDataSubscriber::odomDataScan3dCallback(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
 {
 	callbackCalled();
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
-	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomDataScanDescCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg)
+{
+	callbackCalled();
+	rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
 }
 void CommonDataSubscriber::odomDataScan2dInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -179,8 +243,8 @@ void CommonDataSubscriber::odomDataScan2dInfoCallback(
 		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
 	callbackCalled();
-	sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
-	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // null
+	commonLaserScanCallback(odomMsg, userDataMsg, *scanMsg, scan3dMsg, odomInfoMsg);
 }
 void CommonDataSubscriber::odomDataScan3dInfoCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -189,13 +253,24 @@ void CommonDataSubscriber::odomDataScan3dInfoCallback(
 		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
 	callbackCalled();
-	sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
-	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
+	commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, *scanMsg, odomInfoMsg);
 }
+void CommonDataSubscriber::odomDataScanDescInfoCallback(
+		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
+		const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
+		const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanMsg,
+		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
+{
+	callbackCalled();
+	commonLaserScanCallback(odomMsg, userDataMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, scanMsg->global_descriptor);
+}
+#endif
 
 void CommonDataSubscriber::setupScanCallbacks(
 		rclcpp::Node& node,
 		bool scan2dTopic,
+		bool scanDescTopic,
 		bool subscribeOdom,
 		bool subscribeUserData,
 		bool subscribeOdomInfo,
@@ -206,33 +281,52 @@ void CommonDataSubscriber::setupScanCallbacks(
 
 	if(subscribeOdom || subscribeUserData || subscribeOdomInfo)
 	{
-		if(scan2dTopic)
+		if(scanDescTopic)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(&node, "scan_descriptor");
+		}
+		else if(scan2dTopic)
 		{
 			subscribedToScan2d_ = true;
-			scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
+			scanSub_.subscribe(&node, "scan");
 		}
 		else
 		{
 			subscribedToScan3d_ = true;
-			scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
+			scan3dSub_.subscribe(&node, "scan_cloud");
 		}
 
+#ifdef RTABMAP_SYNC_USER_DATA
 		if(subscribeOdom && subscribeUserData)
 		{
-			odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
-			userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
+			odomSub_.subscribe(&node, "odom");
+			userDataSub_.subscribe(&node, "user_data");
 
-			if(scan2dTopic)
+			if(scanDescTopic)
 			{
 				if(subscribeOdomInfo)
 				{
 					subscribedToOdomInfo_ = true;
-					odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-					SYNC_DECL4(odomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, scanSub_, odomInfoSub_);
+					odomInfoSub_.subscribe(&node, "odom_info");
+					SYNC_DECL4(CommonDataSubscriber, odomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_);
 				}
 				else
 				{
-					SYNC_DECL3(odomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, scanSub_);
+					SYNC_DECL3(CommonDataSubscriber, odomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_);
+				}
+			}
+			else if(scan2dTopic)
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(&node, "odom_info");
+					SYNC_DECL4(CommonDataSubscriber, odomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, scanSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL3(CommonDataSubscriber, odomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, scanSub_);
 				}
 			}
 			else
@@ -240,86 +334,45 @@ void CommonDataSubscriber::setupScanCallbacks(
 				if(subscribeOdomInfo)
 				{
 					subscribedToOdomInfo_ = true;
-					odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-					SYNC_DECL4(odomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_);
+					odomInfoSub_.subscribe(&node, "odom_info");
+					SYNC_DECL4(CommonDataSubscriber, odomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_);
 				}
 				else
 				{
-					SYNC_DECL3(odomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_);
-				}
-			}
-		}
-		else if(subscribeOdom)
-		{
-			odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
-
-			if(scan2dTopic)
-			{
-				if(subscribeOdomInfo)
-				{
-					subscribedToOdomInfo_ = true;
-					odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-					SYNC_DECL3(odomScan2dInfo, approxSync, queueSize, odomSub_, scanSub_, odomInfoSub_);
-				}
-				else
-				{
-					SYNC_DECL2(odomScan2d, approxSync, queueSize, odomSub_, scanSub_);
-				}
-			}
-			else
-			{
-				if(subscribeOdomInfo)
-				{
-					subscribedToOdomInfo_ = true;
-					odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-					SYNC_DECL3(odomScan3dInfo, approxSync, queueSize, odomSub_, scan3dSub_, odomInfoSub_);
-				}
-				else
-				{
-					SYNC_DECL2(odomScan3d, approxSync, queueSize, odomSub_, scan3dSub_);
-				}
-			}
-		}
-		else if(subscribeUserData)
-		{
-			userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
-
-			if(scan2dTopic)
-			{
-				if(subscribeOdomInfo)
-				{
-					subscribedToOdomInfo_ = true;
-					odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-					SYNC_DECL3(dataScan2dInfo, approxSync, queueSize, userDataSub_, scanSub_, odomInfoSub_);
-				}
-				else
-				{
-					SYNC_DECL2(dataScan2d, approxSync, queueSize, userDataSub_, scanSub_);
-				}
-			}
-			else
-			{
-				if(subscribeOdomInfo)
-				{
-					subscribedToOdomInfo_ = true;
-					odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-					SYNC_DECL3(dataScan3dInfo, approxSync, queueSize, userDataSub_, scan3dSub_, odomInfoSub_);
-				}
-				else
-				{
-					SYNC_DECL2(dataScan3d, approxSync, queueSize, userDataSub_, scan3dSub_);
+					SYNC_DECL3(CommonDataSubscriber, odomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_);
 				}
 			}
 		}
 		else
+#endif			
+		if(subscribeOdom)
 		{
-			if(scan2dTopic)
+			odomSub_.subscribe(&node, "odom");
+
+			if(scanDescTopic)
 			{
 				if(subscribeOdomInfo)
 				{
 					subscribedToOdomInfo_ = true;
-					odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-					SYNC_DECL2(scan2dInfo, approxSync, queueSize, scanSub_, odomInfoSub_);
+					odomInfoSub_.subscribe(&node, "odom_info");
+					SYNC_DECL3(CommonDataSubscriber, odomScanDescInfo, approxSync, queueSize, odomSub_, scanDescSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL2(CommonDataSubscriber, odomScanDesc, approxSync, queueSize, odomSub_, scanDescSub_);
+				}
+			}
+			else if(scan2dTopic)
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(&node, "odom_info");
+					SYNC_DECL3(CommonDataSubscriber, odomScan2dInfo, approxSync, queueSize, odomSub_, scanSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL2(CommonDataSubscriber, odomScan2d, approxSync, queueSize, odomSub_, scanSub_);
 				}
 			}
 			else
@@ -327,18 +380,94 @@ void CommonDataSubscriber::setupScanCallbacks(
 				if(subscribeOdomInfo)
 				{
 					subscribedToOdomInfo_ = true;
-					odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-					SYNC_DECL2(scan3dInfo, approxSync, queueSize, scan3dSub_, odomInfoSub_);
+					odomInfoSub_.subscribe(&node, "odom_info");
+					SYNC_DECL3(CommonDataSubscriber, odomScan3dInfo, approxSync, queueSize, odomSub_, scan3dSub_, odomInfoSub_);
 				}
+				else
+				{
+					SYNC_DECL2(CommonDataSubscriber, odomScan3d, approxSync, queueSize, odomSub_, scan3dSub_);
+				}
+			}
+		}
+#ifdef RTABMAP_SYNC_USER_DATA
+		else if(subscribeUserData)
+		{
+			userDataSub_.subscribe(&node, "user_data");
+
+			if(scanDescTopic)
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(&node, "odom_info");
+					SYNC_DECL3(CommonDataSubscriber, dataScanDescInfo, approxSync, queueSize, userDataSub_, scanDescSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL2(CommonDataSubscriber, dataScanDesc, approxSync, queueSize, userDataSub_, scanDescSub_);
+				}
+			}
+			else if(scan2dTopic)
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(&node, "odom_info");
+					SYNC_DECL3(CommonDataSubscriber, dataScan2dInfo, approxSync, queueSize, userDataSub_, scanSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL2(CommonDataSubscriber, dataScan2d, approxSync, queueSize, userDataSub_, scanSub_);
+				}
+			}
+			else
+			{
+				if(subscribeOdomInfo)
+				{
+					subscribedToOdomInfo_ = true;
+					odomInfoSub_.subscribe(&node, "odom_info");
+					SYNC_DECL3(CommonDataSubscriber, dataScan3dInfo, approxSync, queueSize, userDataSub_, scan3dSub_, odomInfoSub_);
+				}
+				else
+				{
+					SYNC_DECL2(CommonDataSubscriber, dataScan3d, approxSync, queueSize, userDataSub_, scan3dSub_);
+				}
+			}
+		}
+#endif
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(&node, "odom_info");
+			if(scanDescTopic)
+			{
+				SYNC_DECL2(CommonDataSubscriber, scanDescInfo, approxSync, queueSize, scanDescSub_, odomInfoSub_);
+			}
+			else if(scan2dTopic)
+			{
+				SYNC_DECL2(CommonDataSubscriber, scan2dInfo, approxSync, queueSize, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL2(CommonDataSubscriber, scan3dInfo, approxSync, queueSize, scan3dSub_, odomInfoSub_);
 			}
 		}
 	}
 	else
 	{
-		if(scan2dTopic)
+		if(scanDescTopic)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSubOnly_ = node.create_subscription<rtabmap_ros::msg::ScanDescriptor>("scan_descriptor", 5,  std::bind(&CommonDataSubscriber::scanDescCallback, this, std::placeholders::_1));
+			subscribedTopicsMsg_ =
+					uFormat("\n%s subscribed to:\n   %s",
+					node.get_name(),
+					scanDescSubOnly_->get_topic_name());
+		}
+		else if(scan2dTopic)
 		{
 			subscribedToScan2d_ = true;
-			scan2dSubOnly_ = node.create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::SensorDataQoS(), std::bind(&CommonDataSubscriber::scan2dCallback, this, std::placeholders::_1));
+			scan2dSubOnly_ = node.create_subscription<sensor_msgs::msg::LaserScan>("scan", 5, std::bind(&CommonDataSubscriber::scan2dCallback, this, std::placeholders::_1));
 			subscribedTopicsMsg_ =
 					uFormat("\n%s subscribed to:\n   %s",
 					node.get_name(),
@@ -347,7 +476,7 @@ void CommonDataSubscriber::setupScanCallbacks(
 		else
 		{
 			subscribedToScan3d_ = true;
-			scan3dSubOnly_ = node.create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::SensorDataQoS(), std::bind(&CommonDataSubscriber::scan3dCallback, this, std::placeholders::_1));
+			scan3dSubOnly_ = node.create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", 5, std::bind(&CommonDataSubscriber::scan3dCallback, this, std::placeholders::_1));
 			subscribedTopicsMsg_ =
 					uFormat("\n%s subscribed to:\n   %s",
 					node.get_name(),

src/impl/CommonDataSubscriberStereo.cpp

@@ -39,8 +39,8 @@ void CommonDataSubscriber::stereoCallback(
 	callbackCalled();
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // null
+	sensor_msgs::msg::LaserScan scanMsg; // null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
 	commonStereoCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
@@ -54,8 +54,8 @@ void CommonDataSubscriber::stereoInfoCallback(
 	callbackCalled();
 	nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	commonStereoCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
 
@@ -69,8 +69,8 @@ void CommonDataSubscriber::stereoOdomCallback(
 {
 	callbackCalled();
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // null
+	sensor_msgs::msg::LaserScan scanMsg; // null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
 	commonStereoCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
 }
@@ -84,8 +84,8 @@ void CommonDataSubscriber::stereoOdomInfoCallback(
 {
 	callbackCalled();
 	rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
-	sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
-	sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
+	sensor_msgs::msg::LaserScan scan2dMsg; // Null
+	sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
 	commonStereoCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
 }
 
@@ -99,24 +99,24 @@ void CommonDataSubscriber::setupStereoCallbacks(
 	RCLCPP_INFO(node.get_logger(), "Setup stereo callback");
 
 	image_transport::TransportHints hints(&node);
-	imageRectLeft_.subscribe(&node, "left/image_rect", hints.getTransport(), rmw_qos_profile_sensor_data);
-	imageRectRight_.subscribe(&node, "right/image_rect", hints.getTransport(), rmw_qos_profile_sensor_data);
-	cameraInfoLeft_.subscribe(&node, "left/camera_info", rmw_qos_profile_sensor_data);
-	cameraInfoRight_.subscribe(&node, "right/camera_info", rmw_qos_profile_sensor_data);
+	imageRectLeft_.subscribe(&node, "left/image_rect", hints.getTransport());
+	imageRectRight_.subscribe(&node, "right/image_rect", hints.getTransport());
+	cameraInfoLeft_.subscribe(&node, "left/camera_info");
+	cameraInfoRight_.subscribe(&node, "right/camera_info");
 
 	if(subscribeOdom)
 	{
-		odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
+		odomSub_.subscribe(&node, "odom");
 
 		if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL6(stereoOdomInfo, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL6(CommonDataSubscriber, stereoOdomInfo, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL5(stereoOdom, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+			SYNC_DECL5(CommonDataSubscriber, stereoOdom, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
 		}
 	}
 	else
@@ -124,12 +124,12 @@ void CommonDataSubscriber::setupStereoCallbacks(
 		if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
-			odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
-			SYNC_DECL5(stereoInfo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
+			odomInfoSub_.subscribe(&node, "odom_info");
+			SYNC_DECL5(CommonDataSubscriber, stereoInfo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL4(stereo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+			SYNC_DECL4(CommonDataSubscriber, stereo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
 		}
 	}
 }