Merge pull request #1 from introlab/master

latest changes

.github/workflows/ros1.yml

@@ -0,0 +1,72 @@
+name: ros1
+
+on:
+  push:
+    branches: [ master ]
+  pull_request:
+    branches: [ master ]
+
+env:
+  # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.)
+  BUILD_TYPE: Release
+
+jobs:
+  build:
+    # The CMake configure and build commands are platform agnostic and should work equally
+    # well on Windows or Mac.  You can convert this to a matrix build if you need
+    # cross-platform coverage.
+    # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
+    name: Build on ros ${{ matrix.ros_distro }} and ${{ matrix.os }}
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        os: [ubuntu-20.04, ubuntu-18.04]
+        include:
+        - os: ubuntu-20.04
+          ros_distro: 'noetic'
+        - os: ubuntu-18.04
+          ros_distro: 'melodic'
+
+    steps:   
+    - uses: ros-tooling/[email protected]
+      with:
+        required-ros-distributions: ${{ matrix.ros_distro }}
+    
+    - name: Install dependencies
+      run: |
+        sudo apt-get update
+        sudo apt-get -y install ros-${{ matrix.ros_distro }}-rtabmap-ros
+        sudo apt-get -y remove ros-${{ matrix.ros_distro }}-rtabmap
+        
+    - uses: actions/checkout@v2
+      with:
+        repository: 'introlab/rtabmap'
+        path: 'rtabmap'
+        
+    - name: Setup catkin workspace
+      run: | 
+        source /opt/ros/${{ matrix.ros_distro }}/setup.bash
+        mkdir -p ${{github.workspace}}/catkin_ws/src
+        cd ${{github.workspace}}/catkin_ws/src
+        catkin_init_workspace
+        cd ..
+        catkin_make
+        
+    - uses: actions/checkout@v2
+      with:
+        path: 'catkin_ws/src/rtabmap_ros'
+        
+    - name: Build latest rtabmap library
+      run: |
+        source /opt/ros/${{ matrix.ros_distro }}/setup.bash
+        cd ${{github.workspace}}/rtabmap
+        cmake -B build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DCMAKE_INSTALL_PREFIX=${{github.workspace}}/catkin_ws/devel
+        cmake --build build --config ${{env.BUILD_TYPE}} --target install
+
+    - name: caktkin_make
+      run: |
+        source /opt/ros/${{ matrix.ros_distro }}/setup.bash
+        source ${{github.workspace}}/catkin_ws/devel/setup.bash
+        cd ${{github.workspace}}/catkin_ws
+        catkin_make
+        catkin_make install     

.travis.yml

@@ -1,74 +0,0 @@
-sudo: true
-language: cpp
-
-compiler:
-  - gcc
-
-matrix:
-  include:    
-    - dist: bionic
-      install:
-        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu bionic main" > /etc/apt/sources.list.d/ros-latest.list'
-        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-        - sudo apt-get update
-        - sudo apt-get install dpkg
-        - sudo apt-get -y install ros-melodic-rtabmap-ros
-        - sudo apt-get -y remove ros-melodic-rtabmap
-
-      script: 
-        - source /opt/ros/melodic/setup.bash
-        - export PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages
-        - cd ..
-        - mkdir -p catkin_ws/src
-        - cd catkin_ws/src
-        - catkin_init_workspace
-        - cd ..
-        - catkin_make
-        - cd ..
-        - mv rtabmap_ros catkin_ws/src/.
-        - git clone https://github.com/introlab/rtabmap.git
-        - cd rtabmap
-        - if [ "$TRAVIS_BRANCH" = "devel" ]; then git checkout devel; fi
-        - mkdir -p build && cd build
-        - cmake -DCMAKE_INSTALL_PREFIX=~/build/introlab/catkin_ws/devel .. 
-        - make
-        - make install
-        - cd ../../catkin_ws
-        - source devel/setup.bash
-        - catkin_make
-        - catkin_make install
-        
-    - dist: focal
-      install:
-        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros-latest.list'
-        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-        - sudo apt-get update
-        - sudo apt-get install dpkg
-        - sudo apt-get -y install ros-noetic-rtabmap-ros
-        - sudo apt-get -y remove ros-noetic-rtabmap
-
-      script: 
-        - source /opt/ros/noetic/setup.bash
-        - cd ..
-        - mkdir -p catkin_ws/src
-        - cd catkin_ws/src
-        - catkin_init_workspace
-        - cd ..
-        - catkin_make
-        - cd ..
-        - mv rtabmap_ros catkin_ws/src/.
-        - git clone https://github.com/introlab/rtabmap.git
-        - cd rtabmap
-        - if [ "$TRAVIS_BRANCH" = "devel" ]; then git checkout devel; fi
-        - mkdir -p build && cd build
-        - cmake -DCMAKE_INSTALL_PREFIX=~/build/introlab/catkin_ws/devel .. 
-        - make
-        - make install
-        - cd ../../catkin_ws
-        - source devel/setup.bash
-        - catkin_make
-        - catkin_make install
-
-notifications:
-  email:
-    - [email protected]

CMakeLists.txt

@@ -31,7 +31,7 @@ find_package(find_object_2d)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.20.13 REQUIRED)
+find_package(RTABMap 0.20.14 REQUIRED)
 
 find_package(OpenCV REQUIRED)
 
@@ -46,6 +46,19 @@ IF(WIN32)
 add_compile_options(-bigobj)
 ENDIF(WIN32)
 
+# kinetic issue, rtabmap now requires at least c++11
+if("$ENV{ROS_DISTRO}" STREQUAL "kinetic")
+include(CheckCXXCompilerFlag)
+CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
+CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+IF(COMPILER_SUPPORTS_CXX14)
+  set(CMAKE_CXX_STANDARD 14)
+ELSEIF(COMPILER_SUPPORTS_CXX11)
+  set(CMAKE_CXX_STANDARD 11)
+ENDIF()
+endif()
+
+
 option(RTABMAP_SYNC_MULTI_RGBD "Build with multi RGBD camera synchronization support"  OFF)
 option(RTABMAP_SYNC_USER_DATA "Build with input user data support"  OFF)
 MESSAGE(STATUS "RTABMAP_SYNC_MULTI_RGBD = ${RTABMAP_SYNC_MULTI_RGBD}")
@@ -103,6 +116,7 @@ add_message_files(
    Point3f.msg
    Goal.msg
    RGBDImage.msg
+   RGBDImages.msg
    UserData.msg
    GPS.msg
    Path.msg
@@ -124,6 +138,9 @@ add_message_files(
    GetNodeData.srv
    GetNodesInRadius.srv
    LoadDatabase.srv
+   DetectMoreLoopClosures.srv
+   GlobalBundleAdjustment.srv
+   CleanupLocalGrids.srv
  )
 
 ## Generate added messages and services with any dependencies listed here
@@ -200,6 +217,7 @@ SET(rtabmap_sync_lib_src
    src/impl/CommonDataSubscriberStereo.cpp
    src/impl/CommonDataSubscriberRGB.cpp
    src/impl/CommonDataSubscriberRGBD.cpp
+   src/impl/CommonDataSubscriberRGBDX.cpp
    src/impl/CommonDataSubscriberScan.cpp
    src/impl/CommonDataSubscriberOdom.cpp
    src/CoreWrapper.cpp # we put CoreWrapper here instead of plugins lib to avoid long compilation time on plugins lib
@@ -240,6 +258,7 @@ SET(rtabmap_plugins_lib_src
    src/nodelets/point_cloud_assembler.cpp
    src/nodelets/undistort_depth.cpp
    src/nodelets/imu_to_tf.cpp
+   src/nodelets/rgbdx_sync.cpp
 )
 
 IF(${cv_bridge_VERSION_MAJOR} GREATER 1 OR ${cv_bridge_VERSION_MINOR} GREATER 10)
@@ -322,6 +341,10 @@ add_executable(rtabmap_rgbd_sync src/RGBDSyncNode.cpp)
 target_link_libraries(rtabmap_rgbd_sync ${Libraries})
 set_target_properties(rtabmap_rgbd_sync PROPERTIES OUTPUT_NAME "rgbd_sync")
 
+add_executable(rtabmap_rgbdx_sync src/RGBDXSyncNode.cpp)
+target_link_libraries(rtabmap_rgbdx_sync ${Libraries})
+set_target_properties(rtabmap_rgbdx_sync PROPERTIES OUTPUT_NAME "rgbdx_sync")
+
 add_executable(rtabmap_stereo_sync src/StereoSyncNode.cpp)
 target_link_libraries(rtabmap_stereo_sync ${Libraries})
 set_target_properties(rtabmap_stereo_sync PROPERTIES OUTPUT_NAME "stereo_sync")
@@ -399,6 +422,10 @@ add_executable(rtabmap_pointcloud_to_depthimage src/PointCloudToDepthImageNode.c
 target_link_libraries(rtabmap_pointcloud_to_depthimage ${Libraries})
 set_target_properties(rtabmap_pointcloud_to_depthimage PROPERTIES OUTPUT_NAME "pointcloud_to_depthimage")
 
+add_executable(rtabmap_point_cloud_aggregator src/PointCloudAggregatorNode.cpp)
+target_link_libraries(rtabmap_point_cloud_aggregator ${Libraries})
+set_target_properties(rtabmap_point_cloud_aggregator PROPERTIES OUTPUT_NAME "rtabmap_point_cloud_aggregator")
+
 add_executable(rtabmap_point_cloud_assembler src/PointCloudAssemblerNode.cpp)
 target_link_libraries(rtabmap_point_cloud_assembler ${Libraries})
 set_target_properties(rtabmap_point_cloud_assembler PROPERTIES OUTPUT_NAME "point_cloud_assembler")
@@ -555,6 +582,7 @@ install(TARGETS
    rtabmap_point_cloud_assembler
    rtabmap_camera
    rtabmap_rgbd_sync
+   rtabmap_rgbdx_sync
    rtabmap_rgbd_relay
    rtabmap_wifi_signal_sub
    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

README.md

@@ -1,4 +1,4 @@
-rtabmap_ros [![Build Status](https://travis-ci.org/introlab/rtabmap_ros.svg?branch=master)](https://travis-ci.org/introlab/rtabmap_ros)
+rtabmap_ros [![Build Status](https://github.com/introlab/rtabmap_ros/actions/workflows/ros1.yml/badge.svg)](https://github.com/introlab/rtabmap_ros/actions/workflows/ros1.yml)
 ===========
 
 RTAB-Map's ROS package.

include/rtabmap_ros/CommonDataSubscriber.h

@@ -46,6 +46,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <nav_msgs/Odometry.h>
 
 #include <rtabmap_ros/RGBDImage.h>
+#include <rtabmap_ros/RGBDImages.h>
 #include <rtabmap_ros/UserData.h>
 #include <rtabmap_ros/OdomInfo.h>
 #include <rtabmap_ros/ScanDescriptor.h>
@@ -176,6 +177,17 @@ private:
 			bool subscribeOdomInfo,
 			int queueSize,
 			bool approxSync);
+	void setupRGBDXCallbacks(
+			ros::NodeHandle & nh,
+			ros::NodeHandle & pnh,
+			bool subscribeOdom,
+			bool subscribeUserData,
+			bool subscribeScan2d,
+			bool subscribeScan3d,
+			bool subscribeScanDesc,
+			bool subscribeOdomInfo,
+			int queueSize,
+			bool approxSync);
 #ifdef RTABMAP_SYNC_MULTI_RGBD
 	void setupRGBD2Callbacks(
 			ros::NodeHandle & nh,
@@ -278,6 +290,8 @@ private:
 	//for rgbd callback
 	ros::Subscriber rgbdSub_;
 	std::vector<message_filters::Subscriber<rtabmap_ros::RGBDImage>*> rgbdSubs_;
+	ros::Subscriber rgbdXSubOnly_;
+	message_filters::Subscriber<rtabmap_ros::RGBDImages> rgbdXSub_;
 
 	//stereo callback
 	image_transport::SubscriberFilter imageRectLeft_;
@@ -419,6 +433,36 @@ private:
 	DATA_SYNCS4(rgbdOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
 #endif
 
+	// X RGBD
+	void rgbdXCallback(const rtabmap_ros::RGBDImagesConstPtr&);
+	DATA_SYNCS2(rgbdXScan2d, rtabmap_ros::RGBDImages, sensor_msgs::LaserScan);
+	DATA_SYNCS2(rgbdXScan3d, rtabmap_ros::RGBDImages, sensor_msgs::PointCloud2)
+	DATA_SYNCS2(rgbdXScanDesc, rtabmap_ros::RGBDImages, rtabmap_ros::ScanDescriptor);
+	DATA_SYNCS2(rgbdXInfo, rtabmap_ros::RGBDImages, rtabmap_ros::OdomInfo);
+
+	// X RGBD + Odom
+	DATA_SYNCS2(rgbdXOdom, nav_msgs::Odometry, rtabmap_ros::RGBDImages);
+	DATA_SYNCS3(rgbdXOdomScan2d, nav_msgs::Odometry, rtabmap_ros::RGBDImages, sensor_msgs::LaserScan);
+	DATA_SYNCS3(rgbdXOdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImages, sensor_msgs::PointCloud2);
+	DATA_SYNCS3(rgbdXOdomScanDesc, nav_msgs::Odometry, rtabmap_ros::RGBDImages, rtabmap_ros::ScanDescriptor);
+	DATA_SYNCS3(rgbdXOdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImages, rtabmap_ros::OdomInfo);
+
+#ifdef RTABMAP_SYNC_USER_DATA
+	// X RGBD + User Data
+	DATA_SYNCS2(rgbdXData, rtabmap_ros::UserData, rtabmap_ros::RGBDImages);
+	DATA_SYNCS3(rgbdXDataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, sensor_msgs::LaserScan);
+	DATA_SYNCS3(rgbdXDataScan3d, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, sensor_msgs::PointCloud2);
+	DATA_SYNCS3(rgbdXDataScanDesc, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, rtabmap_ros::ScanDescriptor);
+	DATA_SYNCS3(rgbdXDataInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, rtabmap_ros::OdomInfo);
+
+	// X RGBD + Odom + User Data
+	DATA_SYNCS3(rgbdXOdomData, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImages);
+	DATA_SYNCS4(rgbdXOdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, sensor_msgs::LaserScan);
+	DATA_SYNCS4(rgbdXOdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, sensor_msgs::PointCloud2);
+	DATA_SYNCS4(rgbdXOdomDataScanDesc, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, rtabmap_ros::ScanDescriptor);
+	DATA_SYNCS4(rgbdXOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImages, rtabmap_ros::OdomInfo);
+#endif
+
 #ifdef RTABMAP_SYNC_MULTI_RGBD
 	// 2 RGBD
 	DATA_SYNCS2(rgbd2, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);

include/rtabmap_ros/CommonDataSubscriberDefines.h

@@ -105,18 +105,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 	if(PREFIX##ExactSync_) delete PREFIX##ExactSync_;
 
 // Sync declarations
-#define SYNC_DECL2(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1) \
+#define SYNC_DECL2(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1) \
 		if(APPROX) \
 		{ \
 			PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
 					PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1); \
-			PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2)); \
+			PREFIX##ApproximateSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2)); \
 		} \
 		else \
 		{ \
 			PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
 					PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1); \
-			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2)); \
+			PREFIX##ExactSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2)); \
 		} \
 		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n   %s", \
 				name_.c_str(), \
@@ -124,18 +124,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 				SUB0.getTopic().c_str(), \
 				SUB1.getTopic().c_str());
 
-#define SYNC_DECL3(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2) \
+#define SYNC_DECL3(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2) \
 		if(APPROX) \
 		{ \
 			PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
 					PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2); \
-			PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3)); \
+			PREFIX##ApproximateSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3)); \
 		} \
 		else \
 		{ \
 			PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
 					PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2); \
-			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3)); \
+			PREFIX##ExactSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3)); \
 		} \
 		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n   %s \\\n   %s", \
 				name_.c_str(), \
@@ -144,18 +144,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 				SUB1.getTopic().c_str(), \
 				SUB2.getTopic().c_str());
 
-#define SYNC_DECL4(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3) \
+#define SYNC_DECL4(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3) \
 		if(APPROX) \
 		{ \
 			PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
 					PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3); \
-			PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4)); \
+			PREFIX##ApproximateSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4)); \
 		} \
 		else \
 		{ \
 			PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
 					PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3); \
-			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4)); \
+			PREFIX##ExactSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4)); \
 		} \
 		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n   %s \\\n   %s \\\n   %s", \
 				name_.c_str(), \
@@ -165,18 +165,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 				SUB2.getTopic().c_str(), \
 				SUB3.getTopic().c_str());
 
-#define SYNC_DECL5(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4) \
+#define SYNC_DECL5(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4) \
 		if(APPROX) \
 		{ \
 			PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
 					PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4); \
-			PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5)); \
+			PREFIX##ApproximateSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5)); \
 		} \
 		else \
 		{ \
 			PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
 					PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4); \
-			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5)); \
+			PREFIX##ExactSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5)); \
 		} \
 		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n   %s \\\n   %s \\\n   %s \\\n   %s", \
 				name_.c_str(), \
@@ -187,18 +187,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 				SUB3.getTopic().c_str(), \
 				SUB4.getTopic().c_str());
 
-#define SYNC_DECL6(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5) \
+#define SYNC_DECL6(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5) \
 		if(APPROX) \
 		{ \
 			PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
 					PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5); \
-			PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6)); \
+			PREFIX##ApproximateSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6)); \
 		} \
 		else \
 		{ \
 			PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
 					PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5); \
-			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6)); \
+			PREFIX##ExactSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6)); \
 		} \
 		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n   %s \\\n   %s \\\n   %s \\\n   %s \\\n   %s", \
 				name_.c_str(), \
@@ -210,18 +210,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 				SUB4.getTopic().c_str(), \
 				SUB5.getTopic().c_str());
 
-#define SYNC_DECL7(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6) \
+#define SYNC_DECL7(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6) \
 		if(APPROX) \
 		{ \
 			PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
 					PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6); \
-			PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7)); \
+			PREFIX##ApproximateSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7)); \
 		} \
 		else \
 		{ \
 			PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
 					PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6); \
-			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7)); \
+			PREFIX##ExactSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7)); \
 		} \
 		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n   %s \\\n   %s \\\n   %s \\\n   %s \\\n   %s \\\n   %s", \
 				name_.c_str(), \
@@ -234,18 +234,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 				SUB5.getTopic().c_str(), \
 				SUB6.getTopic().c_str());
 
-#define SYNC_DECL8(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6, SUB7) \
+#define SYNC_DECL8(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6, SUB7) \
 		if(APPROX) \
 		{ \
 			PREFIX##ApproximateSync_ = new message_filters::Synchronizer<PREFIX##ApproximateSyncPolicy>( \
 					PREFIX##ApproximateSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6, SUB7); \
-			PREFIX##ApproximateSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7, _8)); \
+			PREFIX##ApproximateSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7, _8)); \
 		} \
 		else \
 		{ \
 			PREFIX##ExactSync_ = new message_filters::Synchronizer<PREFIX##ExactSyncPolicy>( \
 					PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6, SUB7); \
-			PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7, _8)); \
+			PREFIX##ExactSync_->registerCallback(boost::bind(&CLASS::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6, _7, _8)); \
 		} \
 		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s \\\n   %s \\\n   %s \\\n   %s \\\n   %s \\\n   %s \\\n   %s \\\n   %s", \
 				name_.c_str(), \

include/rtabmap_ros/CoreWrapper.h

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <std_msgs/Empty.h>
 #include <std_msgs/Int32.h>
+#include "std_msgs/Int32MultiArray.h"
 #include <sensor_msgs/NavSatFix.h>
 #include <nav_msgs/GetMap.h>
 #include <nav_msgs/GetPlan.h>
@@ -63,6 +64,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_ros/AddLink.h"
 #include "rtabmap_ros/GetNodesInRadius.h"
 #include "rtabmap_ros/LoadDatabase.h"
+#include "rtabmap_ros/DetectMoreLoopClosures.h"
+#include "rtabmap_ros/GlobalBundleAdjustment.h"
+#include "rtabmap_ros/CleanupLocalGrids.h"
 
 #include "MapsManager.h"
 
@@ -162,6 +166,7 @@ private:
 	void tagDetectionsAsyncCallback(const apriltag_ros::AprilTagDetectionArray & tagDetections);
 #endif
 	void imuAsyncCallback(const sensor_msgs::ImuConstPtr & tagDetections);
+	void republishNodeDataCallback(const std_msgs::Int32MultiArray::ConstPtr& msg);
 	void interOdomCallback(const nav_msgs::OdometryConstPtr & msg);
 	void interOdomInfoCallback(const nav_msgs::OdometryConstPtr & msg1, const rtabmap_ros::OdomInfoConstPtr & msg2);
 
@@ -196,6 +201,9 @@ private:
 	bool loadDatabaseCallback(rtabmap_ros::LoadDatabase::Request&, rtabmap_ros::LoadDatabase::Response&);
 	bool triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool detectMoreLoopClosuresCallback(rtabmap_ros::DetectMoreLoopClosures::Request&, rtabmap_ros::DetectMoreLoopClosures::Response&);
+	bool globalBundleAdjustmentCallback(rtabmap_ros::GlobalBundleAdjustment::Request&, rtabmap_ros::GlobalBundleAdjustment::Response&);
+	bool cleanupLocalGridsCallback(rtabmap_ros::CleanupLocalGrids::Request&, rtabmap_ros::CleanupLocalGrids::Response&);
 	bool setModeLocalizationCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
@@ -235,6 +243,7 @@ private:
 	void goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback);
 	void publishLocalPath(const ros::Time & stamp);
 	void publishGlobalPath(const ros::Time & stamp);
+	void republishMaps();
 
 private:
 	rtabmap::Rtabmap rtabmap_;
@@ -312,6 +321,9 @@ private:
 	ros::ServiceServer loadDatabaseSrv_;
 	ros::ServiceServer triggerNewMapSrv_;
 	ros::ServiceServer backupDatabase_;
+	ros::ServiceServer detectMoreLoopClosuresSrv_;
+	ros::ServiceServer globalBundleAdjustmentSrv_;
+	ros::ServiceServer cleanupLocalGridsSrv_;
 	ros::ServiceServer setModeLocalizationSrv_;
 	ros::ServiceServer setModeMappingSrv_;
 	ros::ServiceServer setLogDebugSrv_;
@@ -356,10 +368,11 @@ private:
 	ros::Subscriber gpsFixAsyncSub_;
 	rtabmap::GPS gps_;
 	ros::Subscriber tagDetectionsSub_;
-	std::map<int, geometry_msgs::PoseWithCovarianceStamped> tags_;
+	std::map<int, std::pair<geometry_msgs::PoseWithCovarianceStamped, float> > tags_; // id, <pose, size>
 	ros::Subscriber imuSub_;
 	std::map<double, rtabmap::Transform> imus_;
 	std::string imuFrameId_;
+	ros::Subscriber republishNodeDataSub_;
 
 	ros::Subscriber interOdomSub_;
 	std::list<std::pair<nav_msgs::Odometry, rtabmap_ros::OdomInfo> > interOdoms_;
@@ -377,6 +390,8 @@ private:
 	bool alreadyRectifiedImages_;
 	bool twoDMapping_;
 	ros::Time previousStamp_;
+	std::set<int> nodesToRepublish_;
+	int maxNodesRepublished_;
 };
 
 }

include/rtabmap_ros/GuiWrapper.h

@@ -117,6 +117,7 @@ private:
 	rtabmap::MainWindow * mainWindow_;
 	std::string cameraNodeName_;
 	double lastOdomInfoUpdateTime_;
+	std::string rtabmapNodeName_;
 
 	// odometry subscription stuffs
 	std::string frameId_;

include/rtabmap_ros/MsgConversion.h

@@ -74,6 +74,7 @@ rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg, bool ig
 
 void toCvCopy(const rtabmap_ros::RGBDImage & image, cv_bridge::CvImagePtr & rgb, cv_bridge::CvImagePtr & depth);
 void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth);
+void toCvShare(const rtabmap_ros::RGBDImage & image, const boost::shared_ptr<void const>& trackedObject, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth);
 void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_ros::RGBDImage & msg, const std::string & sensorFrameId);
 rtabmap::SensorData rgbdImageFromROS(const rtabmap_ros::RGBDImageConstPtr & image);
 
@@ -175,7 +176,7 @@ cv::Mat userDataFromROS(const rtabmap_ros::UserData & dataMsg);
 void userDataToROS(const cv::Mat & data, rtabmap_ros::UserData & dataMsg, bool compress);
 
 rtabmap::Landmarks landmarksFromROS(
-		const std::map<int, geometry_msgs::PoseWithCovarianceStamped> & tags,
+		const std::map<int, std::pair<geometry_msgs::PoseWithCovarianceStamped, float> > & tags,
 		const std::string & frameId,
 		const std::string & odomFrameId,
 		const ros::Time & odomStamp,

include/rtabmap_ros/PreferencesDialogROS.h

@@ -36,7 +36,7 @@ using namespace rtabmap;
 class PreferencesDialogROS : public PreferencesDialog
 {
 public:
-	PreferencesDialogROS(const QString & configFile);
+	PreferencesDialogROS(const QString & configFile, const std::string & rtabmapNodeName);
 	virtual ~PreferencesDialogROS();
 
 	virtual QString getIniFilePath() const;
@@ -52,6 +52,7 @@ protected:
 
 private:
 	QString configFile_;
+	std::string rtabmapNodeName_;
 };
 
 #endif /* PREFERENCESDIALOGROS_H_ */

launch/demo/demo_husky.launch

@@ -82,11 +82,13 @@
     
     <!-- 2D LiDAR -->
     <arg name="subscribe_scan"  value="$(arg lidar2d)" />
-    <arg name="scan_topic"      value="/scan" />
+    <arg     if="$(arg lidar2d)" name="scan_topic"  value="/scan" />
+    <arg unless="$(arg lidar2d)" name="scan_topic"  value="/scan_not_used" />
     
     <!-- 3D LiDAR -->
     <arg name="subscribe_scan_cloud"  value="$(arg lidar3d)" />
-    <arg name="scan_cloud_topic"      value="/velodyne_points" />
+    <arg     if="$(arg lidar3d)" name="scan_cloud_topic" value="/velodyne_points" />
+    <arg unless="$(arg lidar3d)" name="scan_cloud_topic" value="/scan_cloud_not_used" />
     
     <!-- If camera is used -->
     <arg name="depth"             value="$(eval camera and not depth_from_lidar)" />

launch/rtabmap.launch

@@ -51,6 +51,7 @@
   <arg name="gdb"                     default="false"/>         <!-- Launch nodes in gdb for debugging (apt install xterm gdb) -->
   <arg     if="$(arg gdb)" name="launch_prefix" default="xterm -e gdb -q -ex run --args"/>
   <arg unless="$(arg gdb)" name="launch_prefix" default=""/>
+  <arg name="clear_params"            default="true"/>
   <arg name="output"                  default="screen"/>        <!-- Control node output (screen or log) -->
   <arg name="publish_tf_map"          default="true"/>
 
@@ -83,10 +84,14 @@
   <arg name="compressed"              default="false"/>         <!-- If you want to subscribe to compressed image topics -->
   <arg name="rgb_image_transport"     default="compressed"/>    <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
   <arg name="depth_image_transport"   default="compressedDepth"/>  <!-- Depth compatible types: compressedDepth (see "rosrun image_transport list_transports") -->
+  
+  <arg name="gen_cloud"               default="false"/> <!-- only works with depth image and if not subscribing to scan_cloud topic-->
+  <arg name="gen_cloud_decimation"    default="4"/>
+  <arg name="gen_cloud_voxel"         default="0.05"/>
    
   <arg name="subscribe_scan"          default="false"/>
   <arg name="scan_topic"              default="/scan"/>
-  <arg name="subscribe_scan_cloud"    default="false"/>
+  <arg name="subscribe_scan_cloud"    default="$(arg gen_cloud)"/>
   <arg name="scan_cloud_topic"        default="/scan_cloud"/>
   <arg name="subscribe_scan_descriptor" default="false"/>
   <arg name="scan_descriptor_topic"   default="/scan_descriptor"/>
@@ -119,9 +124,12 @@
   <arg name="use_odom_features"        default="false"/>
   
   <arg name="scan_cloud_assembling"              default="false"/>
-  <arg name="scan_cloud_assembling_time"         default="1"/>
+  <arg name="scan_cloud_assembling_time"         default="1"/>      <!-- max_clouds and time should not be set at the same time -->
+  <arg name="scan_cloud_assembling_max_clouds"   default="0"/>      <!-- max_clouds and time should not be set at the same time -->
   <arg name="scan_cloud_assembling_fixed_frame"  default=""/>
   <arg name="scan_cloud_assembling_voxel_size"   default="0.05"/>
+  <arg name="scan_cloud_assembling_range_min"    default="0.0"/>    <!-- 0=disabled -->
+  <arg name="scan_cloud_assembling_range_max"    default="0.0"/>    <!-- 0=disabled -->
   <arg name="scan_cloud_assembling_noise_radius"   default="0.0"/>    <!-- 0=disabled -->
   <arg name="scan_cloud_assembling_noise_min_neighbors"   default="5"/>
   
@@ -159,7 +167,7 @@
       <group if="$(arg rgbd_sync)">
         <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
         <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
-        <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="$(arg output)">
+        <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" clear_params="$(arg clear_params)" output="$(arg output)">
           <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
           <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
           <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
@@ -179,7 +187,7 @@
       <group if="$(arg rgbd_sync)">
         <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
         <node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
-        <node pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_ros/stereo_sync" output="$(arg output)">
+        <node pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_ros/stereo_sync" clear_params="$(arg clear_params)" output="$(arg output)">
           <remap from="left/image_rect"   to="$(arg left_image_topic_relay)"/>
           <remap from="right/image_rect"  to="$(arg right_image_topic_relay)"/>
           <remap from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
@@ -193,7 +201,7 @@
     
     <group unless="$(arg rgbd_sync)">
        <group if="$(arg subscribe_rgbd)">
-         <node name="republish_rgbd_image"  type="rgbd_relay" pkg="rtabmap_ros">
+         <node name="republish_rgbd_image"  type="rgbd_relay" pkg="rtabmap_ros" clear_params="$(arg clear_params)">
            <remap     if="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)/compressed"/>
            <remap     if="$(arg compressed)" from="$(arg rgbd_topic)/compressed_relay" to="$(arg rgbd_topic_relay)"/>
            <remap unless="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)"/>
@@ -201,13 +209,23 @@
          </node>
       </group>
     </group>
+    
+    <node if="$(arg gen_cloud)" pkg="nodelet" type="nodelet" name="gen_cloud_from_depth" args="standalone rtabmap_ros/point_cloud_xyz" clear_params="$(arg clear_params)" output="$(arg output)">
+      <remap from="depth/image"       to="$(arg depth_topic_relay)"/>
+      <remap from="depth/camera_info" to="$(arg camera_info_topic)"/>
+      <remap from="cloud"             to="$(arg scan_cloud_topic)" />
+
+      <param name="decimation"  type="double" value="$(arg gen_cloud_decimation)"/>
+      <param name="voxel_size"  type="double" value="$(arg gen_cloud_voxel)"/>
+      <param name="approx_sync" type="bool"   value="false"/>
+    </node>
 
     <!-- Visual odometry -->
     <group unless="$(arg icp_odometry)">
       <group if="$(arg visual_odometry)">
       
         <!-- RGB-D Odometry -->
-        <node unless="$(arg stereo)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
+        <node unless="$(arg stereo)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
           <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
           <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
           <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
@@ -235,7 +253,7 @@
         </node>
 
         <!-- Stereo Odometry -->
-        <node if="$(arg stereo)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
+        <node if="$(arg stereo)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
           <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
           <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
           <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
@@ -266,7 +284,7 @@
     </group>
     
     <!-- ICP Odometry -->
-    <node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
+    <node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
       <remap from="scan"                   to="$(arg scan_topic)"/>
       <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
       <remap from="odom"                   to="$(arg odom_topic)"/>
@@ -289,21 +307,24 @@
       <param name="max_update_rate"             type="double" value="$(arg odom_max_rate)"/>
     </node>
     
-    <node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen">
+    <node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" clear_params="$(arg clear_params)" output="$(arg output)">
       <remap     if="$(arg scan_cloud_filtered)" from="cloud" to="odom_filtered_input_scan"/>
       <remap unless="$(arg scan_cloud_filtered)" from="cloud" to="$(arg scan_cloud_topic)"/>
 
       <remap from="odom"            to="$(arg odom_topic)"/>
       <param name="assembling_time" type="double" value="$(arg scan_cloud_assembling_time)"/>
+      <param name="max_clouds"      type="int"    value="$(arg scan_cloud_assembling_max_clouds)"/>
       <param name="fixed_frame_id"  type="string" value="$(arg scan_cloud_assembling_fixed_frame)"/>
       <param name="voxel_size"      type="double" value="$(arg scan_cloud_assembling_voxel_size)"/>
+      <param name="range_min"       type="double" value="$(arg scan_cloud_assembling_range_min)"/>
+      <param name="range_max"       type="double" value="$(arg scan_cloud_assembling_range_max)"/>
       <param name="noise_radius"        type="double" value="$(arg scan_cloud_assembling_noise_radius)"/>
       <param name="noise_min_neighbors" type="int"    value="$(arg scan_cloud_assembling_noise_min_neighbors)"/>
     </node>
   
     <!-- Visual SLAM (robot side) -->
     <!-- args: "delete_db_on_start" and "udebug" -->
-    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
+    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" clear_params="$(arg clear_params)" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
       <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
       <param unless="$(arg stereo)" name="subscribe_depth"  type="bool" value="$(arg depth)"/>
       <param name="subscribe_rgb"        type="bool"   value="$(arg subscribe_rgb)"/>
@@ -331,7 +352,7 @@
       <param name="approx_sync"          type="bool"   value="$(eval approx_sync and not use_odom_features)"/>
       <param name="config_path"          type="string" value="$(arg cfg)"/>
       <param name="queue_size"           type="int" value="$(arg queue_size)"/>
-      <param name="scan_cloud_max_points"     type="int" value="$(arg scan_cloud_max_points)"/>
+      <param if="$(eval not scan_cloud_filtered and not scan_cloud_assembling)" name="scan_cloud_max_points" type="int" value="$(arg scan_cloud_max_points)"/>
       <param name="landmark_linear_variance"   type="double" value="$(arg tag_linear_variance)"/>
       <param name="landmark_angular_variance"  type="double" value="$(arg tag_angular_variance)"/> 
       <param name="gen_depth"                  type="bool"   value="$(arg gen_depth)" />
@@ -371,7 +392,7 @@
     </node>
   
     <!-- Visualisation RTAB-Map -->
-    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
+    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" clear_params="$(arg clear_params)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
       <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
       <param unless="$(arg stereo)" name="subscribe_depth"  type="bool" value="$(arg depth)"/>
       <param name="subscribe_rgb"        type="bool"   value="$(arg subscribe_rgb)"/>
@@ -412,7 +433,7 @@
   
   <!-- Visualization RVIZ -->
   <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
-  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" output="$(arg output)">
+  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" clear_params="$(arg clear_params)" output="$(arg output)">
     <remap if="$(arg stereo)" from="left/image"        to="$(arg left_image_topic_relay)"/>
     <remap if="$(arg stereo)" from="right/image"       to="$(arg right_image_topic_relay)"/>
     <remap if="$(arg stereo)" from="left/camera_info"  to="$(arg left_camera_info_topic)"/>

msg/RGBDImages.msg

@@ -0,0 +1,4 @@
+
+Header header
+
+rtabmap_ros/RGBDImage[] rgbd_images

nodelet_plugins.xml

@@ -128,6 +128,14 @@
     </description>
   </class>
   
+  <class name="rtabmap_ros/rgbdx_sync" 
+         type="rtabmap_ros::RGBDXSync" 
+         base_class_type="nodelet::Nodelet">
+    <description>
+      This is my nodelet.
+    </description>
+  </class>
+  
   <class name="rtabmap_ros/rgbd_relay" 
          type="rtabmap_ros::RGBDRelay" 
          base_class_type="nodelet::Nodelet">

package.xml

@@ -44,6 +44,7 @@
   <build_depend>find_object_2d</build_depend>
   <build_depend>message_generation</build_depend>
   <build_depend>pluginlib</build_depend>
+  <build_depend>apriltag_ros</build_depend>
 
   <run_depend>cv_bridge</run_depend>
   <run_depend>roscpp</run_depend>
@@ -57,6 +58,7 @@
   <run_depend>visualization_msgs</run_depend>
   <run_depend>rosgraph_msgs</run_depend>
   <run_depend>image_transport</run_depend>
+  <run_depend>theora_image_transport</run_depend>
   <run_depend>compressed_depth_image_transport</run_depend>
   <run_depend>compressed_image_transport</run_depend>
   <run_depend>tf</run_depend>
@@ -79,6 +81,7 @@
   <run_depend>find_object_2d</run_depend>
   <run_depend>message_runtime</run_depend>
   <run_depend>pluginlib</run_depend>
+  <run_depend>apriltag_ros</run_depend>
 
   <build_depend>libpcl-all-dev</build_depend>
 

src/CommonDataSubscriber.cpp

@@ -163,6 +163,34 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
 		SYNC_INIT(rgbdOdomDataScanDesc),
 		SYNC_INIT(rgbdOdomDataInfo),
 #endif
+		// X RGBD
+		SYNC_INIT(rgbdXScan2d),
+		SYNC_INIT(rgbdXScan3d),
+		SYNC_INIT(rgbdXScanDesc),
+		SYNC_INIT(rgbdXInfo),
+
+		// X RGBD + Odom
+		SYNC_INIT(rgbdXOdom),
+		SYNC_INIT(rgbdXOdomScan2d),
+		SYNC_INIT(rgbdXOdomScan3d),
+		SYNC_INIT(rgbdXOdomScanDesc),
+		SYNC_INIT(rgbdXOdomInfo),
+
+#ifdef RTABMAP_SYNC_USER_DATA
+		// X RGBD + User Data
+		SYNC_INIT(rgbdXData),
+		SYNC_INIT(rgbdXDataScan2d),
+		SYNC_INIT(rgbdXDataScan3d),
+		SYNC_INIT(rgbdXDataScanDesc),
+		SYNC_INIT(rgbdXDataInfo),
+
+		// X RGBD + Odom + User Data
+		SYNC_INIT(rgbdXOdomData),
+		SYNC_INIT(rgbdXOdomDataScan2d),
+		SYNC_INIT(rgbdXOdomDataScan3d),
+		SYNC_INIT(rgbdXOdomDataScanDesc),
+		SYNC_INIT(rgbdXOdomDataInfo),
+#endif
 
 #ifdef RTABMAP_SYNC_MULTI_RGBD
 		// 2 RGBD
@@ -438,11 +466,6 @@ void CommonDataSubscriber::setupCallbacks(
 		pnh.param("approx_sync", approxSync_, approxSync_);
 	}
 
-	if(rgbdCameras <= 0 && subscribedToRGBD_)
-	{
-		rgbdCameras = 1;
-	}
-
 	ROS_INFO("%s: subscribe_depth = %s", name.c_str(), subscribedToDepth_?"true":"false");
 	ROS_INFO("%s: subscribe_rgb = %s", name.c_str(), subscribedToRGB_?"true":"false");
 	ROS_INFO("%s: subscribe_stereo = %s", name.c_str(), subscribedToStereo_?"true":"false");
@@ -496,9 +519,30 @@ void CommonDataSubscriber::setupCallbacks(
 	}
 	else if(subscribedToRGBD_)
 	{
-#ifdef RTABMAP_SYNC_MULTI_RGBD
-		if(rgbdCameras == 6)
+		if(rgbdCameras == 0)
 		{
+			setupRGBDXCallbacks(
+					nh,
+					pnh,
+					subscribedToOdom_,
+					subscribeUserData,
+					subscribeScan2d,
+					subscribeScan3d,
+					subscribeScanDesc,
+					subscribeOdomInfo,
+					queueSize_,
+					approxSync_);
+		}
+#ifdef RTABMAP_SYNC_MULTI_RGBD
+		else if(rgbdCameras >= 6)
+		{
+			if(rgbdCameras > 6)
+			{
+				ROS_ERROR("Cannot synchronize more than 6 rgbd topics (rgbd_cameras is set to %d). Set "
+						"rgbd_cameras=0 to use RGBDImages interface instead, then "
+						"synchronize RGBDImage topics yourself.", rgbdCameras);
+			}
+
 			setupRGBD6Callbacks(
 					nh,
 					pnh,
@@ -570,7 +614,10 @@ void CommonDataSubscriber::setupCallbacks(
 #else
 		if(rgbdCameras>1)
 		{
-			ROS_FATAL("Cannot synchronize more than 1 rgbd topic (rtabmap_ros has been built without RTABMAP_SYNC_MULTI_RGBD option)");
+			ROS_FATAL("Cannot synchronize more than 1 rgbd topic (rtabmap_ros has "
+					"been built without RTABMAP_SYNC_MULTI_RGBD option). Set rgbd_cameras=0 to "
+					"use RGBDImages interface instead without recompiling with RTABMAP_SYNC_MULTI_RGBD, "
+					"but you will have to synchronize RGBDImage topics yourself.");
 		}
 #endif
 		else
@@ -749,6 +796,35 @@ CommonDataSubscriber::~CommonDataSubscriber()
 	SYNC_DEL(rgbdOdomDataInfo);
 #endif
 
+	// X RGBD
+	SYNC_DEL(rgbdXScan2d);
+	SYNC_DEL(rgbdXScan3d);
+	SYNC_DEL(rgbdXScanDesc);
+	SYNC_DEL(rgbdXInfo);
+
+	// X RGBD + Odom
+	SYNC_DEL(rgbdXOdom);
+	SYNC_DEL(rgbdXOdomScan2d);
+	SYNC_DEL(rgbdXOdomScan3d);
+	SYNC_DEL(rgbdXOdomScanDesc);
+	SYNC_DEL(rgbdXOdomInfo);
+
+#ifdef RTABMAP_SYNC_USER_DATA
+	// X RGBD + User Data
+	SYNC_DEL(rgbdXData);
+	SYNC_DEL(rgbdXDataScan2d);
+	SYNC_DEL(rgbdXDataScan3d);
+	SYNC_DEL(rgbdXDataScanDesc);
+	SYNC_DEL(rgbdXDataInfo);
+
+	// X RGBD + Odom + User Data
+	SYNC_DEL(rgbdXOdomData);
+	SYNC_DEL(rgbdXOdomDataScan2d);
+	SYNC_DEL(rgbdXOdomDataScan3d);
+	SYNC_DEL(rgbdXOdomDataScanDesc);
+	SYNC_DEL(rgbdXOdomDataInfo);
+#endif
+
 #ifdef RTABMAP_SYNC_MULTI_RGBD
 	// 2 RGBD
 	SYNC_DEL(rgbd2);
@@ -912,24 +988,6 @@ CommonDataSubscriber::~CommonDataSubscriber()
 		delete rgbdSubs_[i];
 	}
 	rgbdSubs_.clear();
-
-	//clear params
-	ros::NodeHandle pnh("~");
-	pnh.deleteParam("subscribe_depth");
-	pnh.deleteParam("subscribe_laserScan");
-	pnh.deleteParam("subscribe_scan");
-	pnh.deleteParam("subscribe_scan_cloud");
-	pnh.deleteParam("subscribe_stereo");
-	pnh.deleteParam("subscribe_rgb");
-	pnh.deleteParam("subscribe_rgbd");
-	pnh.deleteParam("subscribe_odom_info");
-	pnh.deleteParam("subscribe_user_data");
-	pnh.deleteParam("odom_frame_id");
-	pnh.deleteParam("rgbd_cameras");
-	pnh.deleteParam("depth_cameras");
-	pnh.deleteParam("queue_size");
-	pnh.deleteParam("approx_sync");
-	pnh.deleteParam("stereo_approx_sync");
 }
 
 void CommonDataSubscriber::warningLoop()

src/CoreWrapper.cpp

@@ -60,6 +60,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/DBDriver.h>
 #include <rtabmap/core/Registration.h>
 #include <rtabmap/core/Graph.h>
+#include <rtabmap/core/Optimizer.h>
 
 #ifdef WITH_OCTOMAP_MSGS
 #ifdef RTABMAP_OCTOMAP
@@ -124,7 +125,8 @@ CoreWrapper::CoreWrapper() :
 		alreadyRectifiedImages_(Parameters::defaultRtabmapImagesAlreadyRectified()),
 		twoDMapping_(Parameters::defaultRegForce3DoF()),
 		previousStamp_(0),
-		mbClient_(0)
+		mbClient_(0),
+		maxNodesRepublished_(2)
 {
 	char * rosHomePath = getenv("ROS_HOME");
 	std::string workingDir = rosHomePath?rosHomePath:UDirectory::homeDir()+"/.ros";
@@ -187,6 +189,7 @@ void CoreWrapper::onInit()
 	pnh.param("wait_for_transform_duration",  waitForTransformDuration_, waitForTransformDuration_);
 	pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_);
 	pnh.param("use_saved_map", useSavedMap_, useSavedMap_);
+	pnh.param("max_nodes_republished", maxNodesRepublished_, maxNodesRepublished_);
 	pnh.param("gen_scan",            genScan_, genScan_);
 	pnh.param("gen_scan_max_depth",  genScanMaxDepth_, genScanMaxDepth_);
 	pnh.param("gen_scan_min_depth",  genScanMinDepth_, genScanMinDepth_);
@@ -610,6 +613,7 @@ void CoreWrapper::onInit()
 		Parameters::parse(parameters_, Parameters::kRegForce3DoF(), twoDMapping_);
 	}
 
+	paused_ = pnh.param("is_rtabmap_paused", paused_);
 	if(paused_)
 	{
 		NODELET_WARN("Node paused... don't forget to call service \"resume\" to start rtabmap.");
@@ -639,7 +643,7 @@ void CoreWrapper::onInit()
 
 	if(rtabmap_.getMemory())
 	{
-		if(useSavedMap_ && !rtabmap_.getMemory()->isIncremental())
+		if(useSavedMap_)
 		{
 			float xMin, yMin, gridCellSize;
 			cv::Mat map = rtabmap_.getMemory()->load2DMap(xMin, yMin, gridCellSize);
@@ -680,6 +684,9 @@ void CoreWrapper::onInit()
 	loadDatabaseSrv_ = nh.advertiseService("load_database", &CoreWrapper::loadDatabaseCallback, this);
 	triggerNewMapSrv_ = nh.advertiseService("trigger_new_map", &CoreWrapper::triggerNewMapCallback, this);
 	backupDatabase_ = nh.advertiseService("backup", &CoreWrapper::backupDatabaseCallback, this);
+	detectMoreLoopClosuresSrv_ = nh.advertiseService("detect_more_loop_closures", &CoreWrapper::detectMoreLoopClosuresCallback, this);
+	globalBundleAdjustmentSrv_ = nh.advertiseService("global_bundle_adjustment", &CoreWrapper::globalBundleAdjustmentCallback, this);
+	cleanupLocalGridsSrv_ = nh.advertiseService("cleanup_local_grids", &CoreWrapper::cleanupLocalGridsCallback, this);
 	setModeLocalizationSrv_ = nh.advertiseService("set_mode_localization", &CoreWrapper::setModeLocalizationCallback, this);
 	setModeMappingSrv_ = nh.advertiseService("set_mode_mapping", &CoreWrapper::setModeMappingCallback, this);
 	getNodeDataSrv_ = nh.advertiseService("get_node_data", &CoreWrapper::getNodeDataCallback, this);
@@ -801,11 +808,10 @@ void CoreWrapper::onInit()
 		}
 	}
 
-	// set public parameters
-	nh.setParam("is_rtabmap_paused", paused_);
+	// set private parameters
 	for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
 	{
-		nh.setParam(iter->first, iter->second);
+		pnh.setParam(iter->first, iter->second);
 	}
 
 	userDataAsyncSub_ = nh.subscribe("user_data_async", 1, &CoreWrapper::userDataAsyncCallback, this);
@@ -815,6 +821,7 @@ void CoreWrapper::onInit()
 	tagDetectionsSub_ = nh.subscribe("tag_detections", 1, &CoreWrapper::tagDetectionsAsyncCallback, this);
 #endif
 	imuSub_ = nh.subscribe("imu", 100, &CoreWrapper::imuAsyncCallback, this);
+	republishNodeDataSub_ = nh.subscribe("republish_node_data", 100, &CoreWrapper::republishNodeDataCallback, this);
 }
 
 CoreWrapper::~CoreWrapper()
@@ -828,13 +835,6 @@ CoreWrapper::~CoreWrapper()
 
 	this->saveParameters(configPath_);
 
-	ros::NodeHandle nh;
-	for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
-	{
-		nh.deleteParam(iter->first);
-	}
-	nh.deleteParam("is_rtabmap_paused");
-
 	printf("rtabmap: Saving database/long-term memory... (located at %s)\n", databasePath_.c_str());
 	if(rtabmap_.getMemory())
 	{
@@ -1912,9 +1912,9 @@ void CoreWrapper::process(
 					else if(twoDMapping_)
 					{
 						// If 2d mapping, make sure all diagonal values of the covariance that even not used are not null.
-						covariance.at<double>(2,2) = covariance.at<double>(2,2)!=0?covariance.at<double>(2,2):1;
-						covariance.at<double>(3,3) = covariance.at<double>(3,3)!=0?covariance.at<double>(3,3):1;
-						covariance.at<double>(4,4) = covariance.at<double>(4,4)!=0?covariance.at<double>(4,4):1;
+						covariance.at<double>(2,2) = uIsFinite(covariance.at<double>(2,2)) && covariance.at<double>(2,2)!=0?covariance.at<double>(2,2):1;
+						covariance.at<double>(3,3) = uIsFinite(covariance.at<double>(3,3)) && covariance.at<double>(3,3)!=0?covariance.at<double>(3,3):1;
+						covariance.at<double>(4,4) = uIsFinite(covariance.at<double>(4,4)) && covariance.at<double>(4,4)!=0?covariance.at<double>(4,4):1;
 					}
 
 					SensorData interData(cv::Mat(), cv::Mat(), CameraModel(), -1, rtabmap_ros::timestampFromROS(iter->first.header.stamp));
@@ -2088,9 +2088,9 @@ void CoreWrapper::process(
 		else if(twoDMapping_)
 		{
 			// If 2d mapping, make sure all diagonal values of the covariance that even not used are not null.
-			covariance.at<double>(2,2) = covariance.at<double>(2,2)!=0?covariance.at<double>(2,2):1;
-			covariance.at<double>(3,3) = covariance.at<double>(3,3)!=0?covariance.at<double>(3,3):1;
-			covariance.at<double>(4,4) = covariance.at<double>(4,4)!=0?covariance.at<double>(4,4):1;
+			covariance.at<double>(2,2) = uIsFinite(covariance.at<double>(2,2)) && covariance.at<double>(2,2)!=0?covariance.at<double>(2,2):1;
+			covariance.at<double>(3,3) = uIsFinite(covariance.at<double>(3,3)) && covariance.at<double>(3,3)!=0?covariance.at<double>(3,3):1;
+			covariance.at<double>(4,4) = uIsFinite(covariance.at<double>(4,4)) && covariance.at<double>(4,4)!=0?covariance.at<double>(4,4):1;
 		}
 
 		std::map<std::string, float> externalStats;
@@ -2454,7 +2454,9 @@ void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_ros::AprilTagDetecti
 						warned = true;
 					}
 				}
-				uInsert(tags_, std::make_pair(tagDetections.detections[i].id[0], p));
+				uInsert(tags_,
+						std::make_pair(tagDetections.detections[i].id[0],
+								std::make_pair(p, tagDetections.detections[i].size.size()==1?(float)tagDetections.detections[i].size[0]:0.0f)));
 			}
 		}
 	}
@@ -2496,6 +2498,26 @@ void CoreWrapper::imuAsyncCallback(const sensor_msgs::ImuConstPtr & msg)
 	}
 }
 
+void CoreWrapper::republishNodeDataCallback(const std_msgs::Int32MultiArray::ConstPtr& msg)
+{
+	if(maxNodesRepublished_>0)
+	{
+		nodesToRepublish_.insert(msg->data.begin(), msg->data.end());
+	}
+	else
+	{
+		static bool warned = false;
+		if(!warned)
+		{
+			NODELET_WARN("A node is requesting some node data "
+					"to be republished after the next update, "
+					"but parameter \"max_nodes_republished\" is not over 0, "
+					"ignoring the call. This warning is only printed once.");
+			warned = true;
+		}
+	}
+}
+
 void CoreWrapper::interOdomCallback(const nav_msgs::OdometryConstPtr & msg)
 {
 	if(!paused_)
@@ -2717,29 +2739,29 @@ void CoreWrapper::goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg)
 
 bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
 {
-	ros::NodeHandle nh;
+	ros::NodeHandle pnh("~");
 	for(rtabmap::ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
 	{
 		std::string vStr;
 		bool vBool;
 		int vInt;
 		double vDouble;
-		if(nh.getParam(iter->first, vStr))
+		if(pnh.getParam(iter->first, vStr))
 		{
 			NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
 			iter->second = vStr;
 		}
-		else if(nh.getParam(iter->first, vBool))
+		else if(pnh.getParam(iter->first, vBool))
 		{
 			NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
 			iter->second = uBool2Str(vBool);
 		}
-		else if(nh.getParam(iter->first, vInt))
+		else if(pnh.getParam(iter->first, vInt))
 		{
 			NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
 			iter->second = uNumber2Str(vInt).c_str();
 		}
-		else if(nh.getParam(iter->first, vDouble))
+		else if(pnh.getParam(iter->first, vDouble))
 		{
 			NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
 			iter->second = uNumber2Str(vDouble).c_str();
@@ -2806,6 +2828,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
 	mapToOdomMutex_.lock();
 	mapToOdom_.setIdentity();
 	mapToOdomMutex_.unlock();
+	nodesToRepublish_.clear();
 
 	return true;
 }
@@ -2820,8 +2843,8 @@ bool CoreWrapper::pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
 	{
 		paused_ = true;
 		NODELET_INFO("rtabmap: paused!");
-		ros::NodeHandle nh;
-		nh.setParam("is_rtabmap_paused", true);
+		ros::NodeHandle pnh("~");
+		pnh.setParam("is_rtabmap_paused", true);
 	}
 	return true;
 }
@@ -2836,8 +2859,8 @@ bool CoreWrapper::resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Emp
 	{
 		paused_ = false;
 		NODELET_INFO("rtabmap: resumed!");
-		ros::NodeHandle nh;
-		nh.setParam("is_rtabmap_paused", false);
+		ros::NodeHandle pnh("~");
+		pnh.setParam("is_rtabmap_paused", false);
 	}
 	return true;
 }
@@ -2895,6 +2918,7 @@ bool CoreWrapper::loadDatabaseCallback(rtabmap_ros::LoadDatabase::Request& req,
 	mapToOdomMutex_.lock();
 	mapToOdom_.setIdentity();
 	mapToOdomMutex_.unlock();
+	nodesToRepublish_.clear();
 
 	// Open new database
 	databasePath_ = newDatabasePath;
@@ -3010,6 +3034,7 @@ bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Em
 	globalPose_.header.stamp = ros::Time(0);
 	gps_ = rtabmap::GPS();
 	tags_.clear();
+	nodesToRepublish_.clear();
 
 	NODELET_INFO("Backup: Saving \"%s\" to \"%s\"...", databasePath_.c_str(), (databasePath_+".back").c_str());
 	UFile::copy(databasePath_, databasePath_+".back");
@@ -3022,6 +3047,207 @@ bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Em
 	return true;
 }
 
+void CoreWrapper::republishMaps()
+{
+	ros::Time stamp = ros::Time::now();
+	mapsManager_.publishMaps(rtabmap_.getLocalOptimizedPoses(), stamp, mapFrameId_);
+
+	if(mapDataPub_.getNumSubscribers())
+	{
+		rtabmap_ros::MapDataPtr msg(new rtabmap_ros::MapData);
+		msg->header.stamp = stamp;
+		msg->header.frame_id = mapFrameId_;
+
+		rtabmap_ros::mapDataToROS(
+			rtabmap_.getLocalOptimizedPoses(),
+			rtabmap_.getLocalConstraints(),
+			std::map<int, Signature>(),
+			rtabmap_.getMapCorrection(),
+			*msg);
+
+		mapDataPub_.publish(msg);
+	}
+
+	if(mapGraphPub_.getNumSubscribers())
+	{
+		rtabmap_ros::MapGraphPtr msg(new rtabmap_ros::MapGraph);
+		msg->header.stamp = stamp;
+		msg->header.frame_id = mapFrameId_;
+
+		rtabmap_ros::mapGraphToROS(
+		rtabmap_.getLocalOptimizedPoses(),
+		rtabmap_.getLocalConstraints(),
+		rtabmap_.getMapCorrection(),
+		*msg);
+
+		mapGraphPub_.publish(msg);
+	}
+}
+
+bool CoreWrapper::detectMoreLoopClosuresCallback(rtabmap_ros::DetectMoreLoopClosures::Request& req, rtabmap_ros::DetectMoreLoopClosures::Response& res)
+{
+	NODELET_WARN("Detect more loop closures service called");
+
+	UTimer timer;
+	float clusterRadiusMax = 1;
+	float clusterRadiusMin = 0;
+	float clusterAngle = 0;
+	int iterations = 1;
+	bool intraSession = true;
+	bool interSession = true;
+	if(req.cluster_radius_max > 0.0f)
+	{
+		clusterRadiusMax = req.cluster_radius_max;
+	}
+	if(req.cluster_radius_min >= 0.0f)
+	{
+		clusterRadiusMin = req.cluster_radius_min;
+	}
+	if(req.cluster_angle >= 0.0f)
+	{
+		clusterAngle = req.cluster_angle;
+	}
+	if(req.iterations >= 1.0f)
+	{
+		iterations = (int)req.iterations;
+	}
+	if(req.intra_only)
+	{
+		interSession = false;
+	}
+	else if(req.inter_only)
+	{
+		intraSession = false;
+	}
+	NODELET_WARN("Post-Processing service called: Detecting more loop closures "
+			"(max radius=%f, min radius=%f, angle=%f, iterations=%d, intra=%s, inter=%s)...",
+			clusterRadiusMax,
+			clusterRadiusMin,
+			clusterAngle,
+			iterations,
+			intraSession?"true":"false",
+			interSession?"true":"false");
+	res.detected = rtabmap_.detectMoreLoopClosures(
+			clusterRadiusMax,
+			clusterAngle*M_PI/180.0,
+			iterations,
+			intraSession,
+			interSession,
+			0,
+			clusterRadiusMin);
+	if(res.detected<0)
+	{
+		NODELET_ERROR("Post-Processing: Detecting more loop closures failed!");
+	}
+	else
+	{
+		NODELET_WARN("Post-Processing: Detected %d loop closures! (%fs)", res.detected, timer.ticks());
+
+		if(res.detected>0)
+		{
+			republishMaps();
+		}
+		return true;
+	}
+	return false;
+}
+
+bool CoreWrapper::cleanupLocalGridsCallback(rtabmap_ros::CleanupLocalGrids::Request& req, rtabmap_ros::CleanupLocalGrids::Response& res)
+{
+	NODELET_WARN("Cleanup local grids service called");
+	UTimer timer;
+	int radius = 1;
+	bool filterScans = false;
+	if(req.radius > 1.0f)
+	{
+		radius = (int)req.radius;
+	}
+	filterScans = req.filter_scans;
+	float xMin, yMin, gridCellSize;
+	cv::Mat map = mapsManager_.getGridMap(xMin, yMin, gridCellSize);
+	if(map.empty())
+	{
+		NODELET_ERROR("Post-Processing: Cleanup local grids failed! There is no optimized map.");
+		return false;
+	}
+	std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
+	NODELET_WARN("Post-Processing: Cleanup local grids... (radius=%d, filter scans=%s)",
+			radius,
+			filterScans?"true":"false");
+	res.modified = rtabmap_.cleanupLocalGrids(poses, map, xMin, yMin, gridCellSize, radius, filterScans);
+	if(res.modified<0)
+	{
+		NODELET_ERROR("Post-Processing: Cleanup local grids failed!");
+	}
+	else
+	{
+		if(filterScans)
+		{
+			NODELET_WARN("Post-Processing: %d grids and scans modified! (%fs)", res.modified, timer.ticks());
+		}
+		else
+		{
+			NODELET_WARN("Post-Processing: %d grids modified! (%fs)", res.modified, timer.ticks());
+		}
+		if(res.modified > 0)
+		{
+			// We should update MapsManager's cache with the modifications
+			mapsManager_.clear();
+			mapsManager_.set2DMap(map, xMin, yMin, gridCellSize, rtabmap_.getLocalOptimizedPoses(), rtabmap_.getMemory());
+
+			republishMaps();
+		}
+		return true;
+	}
+
+	return false;
+}
+bool CoreWrapper::globalBundleAdjustmentCallback(rtabmap_ros::GlobalBundleAdjustment::Request& req, rtabmap_ros::GlobalBundleAdjustment::Response& res)
+{
+	NODELET_WARN("Global bundle adjustment service called");
+
+	UTimer timer;
+	int optimizer = (int)Optimizer::kTypeG2O; // g2o
+	int iterations = Parameters::defaultOptimizerIterations();
+	float pixelVariance = Parameters::defaultg2oPixelVariance();
+	bool rematchFeatures = true;
+	Parameters::parse(parameters_, Parameters::kOptimizerIterations(), iterations);
+	Parameters::parse(parameters_, Parameters::kg2oPixelVariance(), pixelVariance);
+	if(req.type == 1.0f)
+	{
+		optimizer = (int)Optimizer::kTypeCVSBA;
+	}
+	if(req.iterations >= 1.0f)
+	{
+		iterations = req.iterations;
+	}
+	if(req.pixel_variance > 0.0f)
+	{
+		pixelVariance = req.pixel_variance;
+	}
+	rematchFeatures = !req.voc_matches;
+
+	NODELET_WARN("Post-Processing: Global Bundle Adjustment... "
+			"(Optimizer=%s, iterations=%d, pixel variance=%f, rematch=%s)...",
+			optimizer==Optimizer::kTypeG2O?"g2o":"cvsba",
+			iterations,
+			pixelVariance,
+			rematchFeatures?"true":"false");
+	bool success = rtabmap_.globalBundleAdjustment((Optimizer::Type)optimizer, iterations, pixelVariance, rematchFeatures);
+	if(!success)
+	{
+		NODELET_ERROR("Post-Processing: Global Bundle Adjustment failed!");
+	}
+	else
+	{
+		NODELET_WARN("Post-Processing: Global Bundle Adjustment... done! (%fs)", timer.ticks());
+		republishMaps();
+		return true;
+	}
+
+	return false;
+}
+
 bool CoreWrapper::setModeLocalizationCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
 {
 	NODELET_INFO("rtabmap: Set localization mode");
@@ -3836,6 +4062,58 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
 		{
 			signatures.insert(std::make_pair(stats.getLastSignatureData().id(), stats.getLastSignatureData()));
 		}
+
+		if(nodesToRepublish_.size() && !rtabmap_.getLastLocalizationPose().isNull())
+		{
+			// Republish data from closest nodes of the current localization
+			std::map<int, Transform> nodesOnly(rtabmap_.getLocalOptimizedPoses().lower_bound(1), rtabmap_.getLocalOptimizedPoses().end());
+			int id = rtabmap::graph::findNearestNode(nodesOnly, rtabmap_.getLastLocalizationPose());
+			if(id>0)
+			{
+				std::map<int, int> ids = rtabmap_.getMemory()->getNeighborsId(id, 0, 0, false, false, true);
+				std::map<int, int> missingIds;
+				for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
+				{
+					if(nodesToRepublish_.find(iter->first) != nodesToRepublish_.end())
+					{
+						missingIds.insert(std::make_pair(iter->second, iter->first));
+					}
+				}
+
+				if(nodesToRepublish_.size() != missingIds.size())
+				{
+					// remove requested nodes not anymore in the graph
+					for(std::set<int>::iterator iter=nodesToRepublish_.begin(); iter!=nodesToRepublish_.end();)
+					{
+						if(ids.find(*iter) == ids.end())
+						{
+							iter = nodesToRepublish_.erase(iter);
+						}
+						else
+						{
+							++iter;
+						}
+					}
+				}
+
+				int loaded = 0;
+				std::stringstream stream;
+				for(std::map<int, int>::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<maxNodesRepublished_; ++iter)
+				{
+					signatures.insert(std::make_pair(iter->second, rtabmap_.getMemory()->getNodeData(iter->second, true, true, true, true)));
+					nodesToRepublish_.erase(iter->second);
+					++loaded;
+					stream << iter->second << " ";
+				}
+				if(loaded)
+				{
+					NODELET_WARN("Republishing data of requested node(s) %sfrom \"%s\" input topic (max_nodes_republished=%d)",
+							stream.str().c_str(),
+							republishNodeDataSub_.getTopic().c_str(),
+							maxNodesRepublished_);
+				}
+			}
+		}
 		rtabmap_ros::mapDataToROS(
 			stats.poses(),
 			stats.constraints(),

src/GuiWrapper.cpp

@@ -72,7 +72,8 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 		odomSensorSync_(false),
 		maxOdomUpdateRate_(10),
 		cameraNodeName_(""),
-		lastOdomInfoUpdateTime_(0)
+		lastOdomInfoUpdateTime_(0),
+		rtabmapNodeName_("rtabmap")
 {
 	ros::NodeHandle nh;
 	ros::NodeHandle pnh("~");
@@ -93,14 +94,18 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 
 	configFile.replace('~', QDir::homePath());
 
+	pnh.param("rtabmap", rtabmapNodeName_, rtabmapNodeName_);
+
 	ROS_INFO("rtabmapviz: Using configuration from \"%s\"", configFile.toStdString().c_str());
 	uSleep(500);
-	prefDialog_ = new PreferencesDialogROS(configFile);
+	prefDialog_ = new PreferencesDialogROS(configFile, rtabmapNodeName_);
 	mainWindow_ = new MainWindow(prefDialog_);
 	mainWindow_->setWindowTitle(mainWindow_->windowTitle()+" [ROS]");
 	mainWindow_->show();
+
 	bool paused = false;
-	nh.param("is_rtabmap_paused", paused, paused);
+	ros::NodeHandle rnh(rtabmapNodeName_);
+	rnh.param("is_rtabmap_paused", paused, paused);
 	mainWindow_->setMonitoringState(paused);
 
 	// To receive odometry events
@@ -249,13 +254,13 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
 		const rtabmap::ParametersMap & defaultParameters = rtabmap::Parameters::getDefaultParameters();
 		rtabmap::ParametersMap parameters = ((rtabmap::ParamEvent *)anEvent)->getParameters();
 		bool modified = false;
-		ros::NodeHandle nh;
+		ros::NodeHandle rnh(rtabmapNodeName_);
 		for(rtabmap::ParametersMap::iterator i=parameters.begin(); i!=parameters.end(); ++i)
 		{
 			//save only parameters with valid names
 			if(defaultParameters.find((*i).first) != defaultParameters.end())
 			{
-				nh.setParam((*i).first, (*i).second);
+				rnh.setParam((*i).first, (*i).second);
 				modified = true;
 			}
 			else if((*i).first.find('/') != (*i).first.npos)

src/MsgConversion.cpp

@@ -163,38 +163,43 @@ void toCvCopy(const rtabmap_ros::RGBDImage & image, cv_bridge::CvImagePtr & rgb,
 
 void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth)
 {
-	if(!image->rgb.data.empty())
+	toCvShare(*image, image, rgb, depth);
+}
+
+void toCvShare(const rtabmap_ros::RGBDImage & image, const boost::shared_ptr<void const>& trackedObject, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth)
+{
+	if(!image.rgb.data.empty())
 	{
-		rgb = cv_bridge::toCvShare(image->rgb, image);
+		rgb = cv_bridge::toCvShare(image.rgb, trackedObject);
 	}
-	else if(!image->rgb_compressed.data.empty())
+	else if(!image.rgb_compressed.data.empty())
 	{
 #ifdef CV_BRIDGE_HYDRO
 		ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
 #else
-		rgb = cv_bridge::toCvCopy(image->rgb_compressed);
+		rgb = cv_bridge::toCvCopy(image.rgb_compressed);
 #endif
 	}
 
-	if(!image->depth.data.empty())
+	if(!image.depth.data.empty())
 	{
-		depth = cv_bridge::toCvShare(image->depth, image);
+		depth = cv_bridge::toCvShare(image.depth, trackedObject);
 	}
-	else if(!image->depth_compressed.data.empty())
+	else if(!image.depth_compressed.data.empty())
 	{
-		if(image->depth_compressed.format.compare("jpg")==0)
+		if(image.depth_compressed.format.compare("jpg")==0)
 		{
 #ifdef CV_BRIDGE_HYDRO
 			ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
 #else
-			depth = cv_bridge::toCvCopy(image->depth_compressed);
+			depth = cv_bridge::toCvCopy(image.depth_compressed);
 #endif
 		}
 		else
 		{
 			cv_bridge::CvImagePtr ptr = boost::make_shared<cv_bridge::CvImage>();
-			ptr->header = image->depth_compressed.header;
-			ptr->image = rtabmap::uncompressImage(image->depth_compressed.data);
+			ptr->header = image.depth_compressed.header;
+			ptr->image = rtabmap::uncompressImage(image.depth_compressed.data);
 			ROS_ASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1);
 			ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
 			depth = ptr;
@@ -803,6 +808,25 @@ rtabmap::CameraModel cameraModelFromROS(
 			D.at<double>(0,4) = camInfo.D[2];
 			D.at<double>(0,5) = camInfo.D[3];
 		}
+		else if(camInfo.D.size()>8)
+		{
+			bool zerosAfter8 = true;
+			for(size_t i=8; i<camInfo.D.size() && zerosAfter8; ++i)
+			{
+				if(camInfo.D[i] != 0.0)
+				{
+					zerosAfter8 = false;
+				}
+			}
+			static bool warned = false;
+			if(!zerosAfter8 && !warned)
+			{
+				ROS_WARN("Camera info conversion: Distortion model is larger than 8, coefficients after 8 are ignored. This message is only shown once.");
+				warned = true;
+			}
+			D = cv::Mat(1, 8, CV_64FC1);
+			memcpy(D.data, camInfo.D.data(), D.cols*sizeof(double));
+		}
 		else
 		{
 			D = cv::Mat(1, camInfo.D.size(), CV_64FC1);
@@ -1561,7 +1585,7 @@ void userDataToROS(const cv::Mat & data, rtabmap_ros::UserData & dataMsg, bool c
 }
 
 rtabmap::Landmarks landmarksFromROS(
-		const std::map<int, geometry_msgs::PoseWithCovarianceStamped> & tags,
+		const std::map<int, std::pair<geometry_msgs::PoseWithCovarianceStamped, float> > & tags,
 		const std::string & frameId,
 		const std::string & odomFrameId,
 		const ros::Time & odomStamp,
@@ -1572,7 +1596,7 @@ rtabmap::Landmarks landmarksFromROS(
 {
 	//tag detections
 	rtabmap::Landmarks landmarks;
-	for(std::map<int, geometry_msgs::PoseWithCovarianceStamped>::const_iterator iter=tags.begin(); iter!=tags.end(); ++iter)
+	for(std::map<int, std::pair<geometry_msgs::PoseWithCovarianceStamped, float> >::const_iterator iter=tags.begin(); iter!=tags.end(); ++iter)
 	{
 		if(iter->first <=0)
 		{
@@ -1581,19 +1605,19 @@ rtabmap::Landmarks landmarksFromROS(
 		}
 		rtabmap::Transform baseToCamera = rtabmap_ros::getTransform(
 				frameId,
-				iter->second.header.frame_id,
-				iter->second.header.stamp,
+				iter->second.first.header.frame_id,
+				iter->second.first.header.stamp,
 				listener,
 				waitForTransform);
 
 		if(baseToCamera.isNull())
 		{
 			ROS_ERROR("Cannot transform tag pose from \"%s\" frame to \"%s\" frame!",
-					iter->second.header.frame_id.c_str(), frameId.c_str());
+					iter->second.first.header.frame_id.c_str(), frameId.c_str());
 			continue;
 		}
 
-		rtabmap::Transform baseToTag = baseToCamera * transformFromPoseMsg(iter->second.pose.pose);
+		rtabmap::Transform baseToTag = baseToCamera * transformFromPoseMsg(iter->second.first.pose.pose);
 
 		if(!baseToTag.isNull())
 		{
@@ -1601,7 +1625,7 @@ rtabmap::Landmarks landmarksFromROS(
 			rtabmap::Transform correction = rtabmap_ros::getTransform(
 					frameId,
 					odomFrameId,
-					iter->second.header.stamp,
+					iter->second.first.header.stamp,
 					odomStamp,
 					listener,
 					waitForTransform);
@@ -1615,14 +1639,14 @@ rtabmap::Landmarks landmarksFromROS(
 						"If odometry is small since it received the tag pose and "
 						"covariance is large, this should not be a problem.");
 			}
-			cv::Mat covariance = cv::Mat(6,6, CV_64FC1, (void*)iter->second.pose.covariance.data()).clone();
+			cv::Mat covariance = cv::Mat(6,6, CV_64FC1, (void*)iter->second.first.pose.covariance.data()).clone();
 			if(covariance.empty() || !uIsFinite(covariance.at<double>(0,0)) || covariance.at<double>(0,0)<=0.0f)
 			{
 				covariance = cv::Mat::eye(6,6,CV_64FC1);
 				covariance(cv::Range(0,3), cv::Range(0,3)) *= defaultLinVariance;
 				covariance(cv::Range(3,6), cv::Range(3,6)) *= defaultAngVariance;
 			}
-			landmarks.insert(std::make_pair(iter->first, rtabmap::Landmark(iter->first, baseToTag, covariance)));
+			landmarks.insert(std::make_pair(iter->first, rtabmap::Landmark(iter->first, iter->second.second, baseToTag, covariance)));
 		}
 	}
 	return landmarks;

src/OdometryROS.cpp

@@ -96,14 +96,6 @@ OdometryROS::~OdometryROS()
 		warningThread_->join();
 		delete warningThread_;
 	}
-	ros::NodeHandle & pnh = getPrivateNodeHandle();
-	if(pnh.ok())
-	{
-		for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
-		{
-			pnh.deleteParam(iter->first);
-		}
-	}
 
 	delete odometry_;
 }
@@ -332,6 +324,12 @@ void OdometryROS::onInit()
 		}
 	}
 
+	// set private parameters
+	for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
+	{
+		pnh.setParam(iter->first, iter->second);
+	}
+
 	Parameters::parse(parameters_, Parameters::kOdomResetCountdown(), resetCountdown_);
 	parameters_.at(Parameters::kOdomResetCountdown()) = "0"; // use modified reset countdown here
 

src/PointCloudAggregatorNode.cpp

@@ -0,0 +1,42 @@
+/*
+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 "ros/ros.h"
+#include "nodelet/loader.h"
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "point_cloud_aggregator");
+
+	nodelet::Loader nodelet;
+	nodelet::V_string nargv;
+	nodelet::M_string remap(ros::names::getRemappings());
+	std::string nodelet_name = ros::this_node::getName();
+	nodelet.load(nodelet_name, "rtabmap_ros/point_cloud_aggregator", remap, nargv);
+	ros::spin();
+	return 0;
+}

src/PreferencesDialogROS.cpp

@@ -41,8 +41,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 using namespace rtabmap;
 
-PreferencesDialogROS::PreferencesDialogROS(const QString & configFile) :
-		configFile_(configFile)
+PreferencesDialogROS::PreferencesDialogROS(const QString & configFile, const std::string & rtabmapNodeName) :
+		configFile_(configFile),
+		rtabmapNodeName_(rtabmapNodeName)
 {
 
 }
@@ -84,7 +85,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
 		path = filePath;
 	}
 
-	ros::NodeHandle nh;
+	ros::NodeHandle rnh(rtabmapNodeName_);
 	ROS_INFO("rtabmapviz: %s", this->getParamMessage().toStdString().c_str());
 	bool validParameters = true;
 	int readCount = 0;
@@ -108,7 +109,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
 		double stamp = UTimer::now();
 		std::string tmp;
 		bool warned = false;
-		while(!nh.getParam(Parameters::kRtabmapDetectionRate(),tmp) && UTimer::now()-stamp < 5.0)
+		while(!rnh.getParam(Parameters::kRtabmapDetectionRate(),tmp) && UTimer::now()-stamp < 5.0)
 		{
 			if(!warned)
 			{
@@ -146,7 +147,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
 		else
 		{
 			std::string value;
-			if(nh.getParam(i->first,value))
+			if(rnh.getParam(i->first,value))
 			{
 				//backward compatibility
 				if(i->first.compare(Parameters::kIcpStrategy()) == 0)

src/RGBDXSyncNode.cpp

@@ -0,0 +1,47 @@
+/*
+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 "ros/ros.h"
+#include "nodelet/loader.h"
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "rgbdx_sync");
+
+	nodelet::V_string nargv;
+	for(int i=1;i<argc;++i)
+	{
+		nargv.push_back(argv[i]);
+	}
+
+	nodelet::Loader nodelet;
+	nodelet::M_string remap(ros::names::getRemappings());
+	std::string nodelet_name = ros::this_node::getName();
+	nodelet.load(nodelet_name, "rtabmap_ros/rgbdx_sync", remap, nargv);
+	ros::spin();
+	return 0;
+}

src/impl/CommonDataSubscriberDepth.cpp

@@ -498,11 +498,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL7(depthOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+				SYNC_DECL7(CommonDataSubscriber, depthOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL6(depthOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
+				SYNC_DECL6(CommonDataSubscriber, depthOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
 			}
 		}
 		else if(subscribeScan2d)
@@ -513,11 +513,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL7(depthOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+				SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL6(depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+				SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
 			}
 		}
 		else if(subscribeScan3d)
@@ -528,22 +528,22 @@ void CommonDataSubscriber::setupDepthCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL7(depthOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+				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(nh, "odom_info", queueSize);
-			SYNC_DECL6(depthOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
+			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 
@@ -560,11 +560,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL6(depthOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+				SYNC_DECL6(CommonDataSubscriber, depthOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL5(depthOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
+				SYNC_DECL5(CommonDataSubscriber, depthOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
 			}
 		}
 		else if(subscribeScan2d)
@@ -575,11 +575,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL6(depthOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+				SYNC_DECL6(CommonDataSubscriber, depthOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL5(depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+				SYNC_DECL5(CommonDataSubscriber, depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
 			}
 		}
 		else if(subscribeScan3d)
@@ -590,22 +590,22 @@ void CommonDataSubscriber::setupDepthCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL6(depthOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+				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(nh, "odom_info", queueSize);
-			SYNC_DECL5(depthOdomInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
+			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
@@ -622,11 +622,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL6(depthDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+				SYNC_DECL6(CommonDataSubscriber, depthDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL5(depthDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
+				SYNC_DECL5(CommonDataSubscriber, depthDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
 			}
 		}
 		else if(subscribeScan2d)
@@ -638,11 +638,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL6(depthDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+				SYNC_DECL6(CommonDataSubscriber, depthDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL5(depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+				SYNC_DECL5(CommonDataSubscriber, depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
 			}
 		}
 		else if(subscribeScan3d)
@@ -653,22 +653,22 @@ void CommonDataSubscriber::setupDepthCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL6(depthDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+				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(nh, "odom_info", queueSize);
-			SYNC_DECL5(depthDataInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
+			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
@@ -682,11 +682,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL5(depthScanDescInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+				SYNC_DECL5(CommonDataSubscriber, depthScanDescInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL4(depthScanDesc, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
+				SYNC_DECL4(CommonDataSubscriber, depthScanDesc, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
 			}
 		}
 		else if(subscribeScan2d)
@@ -697,11 +697,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL5(depthScan2dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+				SYNC_DECL5(CommonDataSubscriber, depthScan2dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL4(depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+				SYNC_DECL4(CommonDataSubscriber, depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
 			}
 		}
 		else if(subscribeScan3d)
@@ -712,22 +712,22 @@ void CommonDataSubscriber::setupDepthCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL5(depthScan3dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+				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(nh, "odom_info", queueSize);
-			SYNC_DECL4(depthInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
+			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

@@ -89,11 +89,11 @@ void CommonDataSubscriber::setupOdomCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL3(odomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, odomInfoSub_);
+				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 
@@ -102,7 +102,7 @@ void CommonDataSubscriber::setupOdomCallbacks(
 		{
 			subscribedToOdomInfo_ = true;
 			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-			SYNC_DECL2(odomInfo, approxSync, queueSize, odomSub_, odomInfoSub_);
+			SYNC_DECL2(CommonDataSubscriber, odomInfo, approxSync, queueSize, odomSub_, odomInfoSub_);
 		}
 	}
 	else

src/impl/CommonDataSubscriberRGB.cpp

@@ -492,11 +492,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL6(rgbOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+				SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL5(rgbOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
+				SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
 			}
 		}
 		else if(subscribeScan2d)
@@ -507,11 +507,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL6(rgbOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+				SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL5(rgbOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
+				SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
 			}
 		}
 		else if(subscribeScan3d)
@@ -522,22 +522,22 @@ void CommonDataSubscriber::setupRGBCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL6(rgbOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+				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(nh, "odom_info", queueSize);
-			SYNC_DECL5(rgbOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
+			SYNC_DECL5(CommonDataSubscriber, rgbOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL4(rgbOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_);
+			SYNC_DECL4(CommonDataSubscriber, rgbOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_);
 		}
 	}
 	else
@@ -554,11 +554,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL5(rgbOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+				SYNC_DECL5(CommonDataSubscriber, rgbOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL4(rgbOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_);
+				SYNC_DECL4(CommonDataSubscriber, rgbOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_);
 			}
 		}
 		else if(subscribeScan2d)
@@ -569,11 +569,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL5(rgbOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+				SYNC_DECL5(CommonDataSubscriber, rgbOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL4(rgbOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_);
+				SYNC_DECL4(CommonDataSubscriber, rgbOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_);
 			}
 		}
 		else if(subscribeScan3d)
@@ -584,22 +584,22 @@ void CommonDataSubscriber::setupRGBCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL5(rgbOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+				SYNC_DECL5(CommonDataSubscriber, rgbOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL4(rgbOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_);
+				SYNC_DECL4(CommonDataSubscriber, rgbOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_);
 			}
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
 			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-			SYNC_DECL4(rgbOdomInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
+			SYNC_DECL4(CommonDataSubscriber, rgbOdomInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL3(rgbOdom, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_);
+			SYNC_DECL3(CommonDataSubscriber, rgbOdom, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_);
 		}
 	}
 #ifdef RTABMAP_SYNC_USER_DATA
@@ -616,11 +616,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL5(rgbDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+				SYNC_DECL5(CommonDataSubscriber, rgbDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL4(rgbDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
+				SYNC_DECL4(CommonDataSubscriber, rgbDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
 			}
 		}
 		else if(subscribeScan2d)
@@ -632,11 +632,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL5(rgbDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+				SYNC_DECL5(CommonDataSubscriber, rgbDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL4(rgbDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
+				SYNC_DECL4(CommonDataSubscriber, rgbDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
 			}
 		}
 		else if(subscribeScan3d)
@@ -647,22 +647,22 @@ void CommonDataSubscriber::setupRGBCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL5(rgbDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+				SYNC_DECL5(CommonDataSubscriber, rgbDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL4(rgbDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
+				SYNC_DECL4(CommonDataSubscriber, rgbDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
 			}
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
 			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-			SYNC_DECL4(rgbDataInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
+			SYNC_DECL4(CommonDataSubscriber, rgbDataInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL3(rgbData, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_);
+			SYNC_DECL3(CommonDataSubscriber, rgbData, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_);
 		}
 	}
 #endif
@@ -676,11 +676,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL4(rgbScanDescInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
+				SYNC_DECL4(CommonDataSubscriber, rgbScanDescInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL3(rgbScanDesc, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_);
+				SYNC_DECL3(CommonDataSubscriber, rgbScanDesc, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_);
 			}
 		}
 		else if(subscribeScan2d)
@@ -691,11 +691,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL4(rgbScan2dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+				SYNC_DECL4(CommonDataSubscriber, rgbScan2dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL3(rgbScan2d, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_);
+				SYNC_DECL3(CommonDataSubscriber, rgbScan2d, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_);
 			}
 		}
 		else if(subscribeScan3d)
@@ -706,22 +706,22 @@ void CommonDataSubscriber::setupRGBCallbacks(
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL4(rgbScan3dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+				SYNC_DECL4(CommonDataSubscriber, rgbScan3dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL3(rgbScan3d, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_);
+				SYNC_DECL3(CommonDataSubscriber, rgbScan3d, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_);
 			}
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
 			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-			SYNC_DECL3(rgbInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, odomInfoSub_);
+			SYNC_DECL3(CommonDataSubscriber, rgbInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL2(rgb, approxSync, queueSize, imageSub_, cameraInfoSub_);
+			SYNC_DECL2(CommonDataSubscriber, rgb, approxSync, queueSize, imageSub_, cameraInfoSub_);
 		}
 	}
 }

src/impl/CommonDataSubscriberRGBD.cpp

@@ -565,7 +565,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 					subscribedToOdomInfo_ = false;
 					ROS_WARN("subscribe_odom_info ignored...");
 				}
-				SYNC_DECL4(rgbdOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
+				SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
 			}
 			else if(subscribeScan2d)
 			{
@@ -576,7 +576,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 					subscribedToOdomInfo_ = false;
 					ROS_WARN("subscribe_odom_info ignored...");
 				}
-				SYNC_DECL4(rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
+				SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
 			}
 			else if(subscribeScan3d)
 			{
@@ -587,17 +587,17 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 					subscribedToOdomInfo_ = false;
 					ROS_WARN("subscribe_odom_info ignored...");
 				}
-				SYNC_DECL4(rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
+				SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
 			}
 			else if(subscribeOdomInfo)
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL4(rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
+				SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL3(rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]));
+				SYNC_DECL3(CommonDataSubscriber, rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]));
 			}
 		}
 		else
@@ -614,7 +614,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 					subscribedToOdomInfo_ = false;
 					ROS_WARN("subscribe_odom_info ignored...");
 				}
-				SYNC_DECL3(rgbdOdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanDescSub_);
+				SYNC_DECL3(CommonDataSubscriber, rgbdOdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanDescSub_);
 			}
 			else if(subscribeScan2d)
 			{
@@ -625,7 +625,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 					subscribedToOdomInfo_ = false;
 					ROS_WARN("subscribe_odom_info ignored...");
 				}
-				SYNC_DECL3(rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
+				SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
 			}
 			else if(subscribeScan3d)
 			{
@@ -636,17 +636,17 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 					subscribedToOdomInfo_ = false;
 					ROS_WARN("subscribe_odom_info ignored...");
 				}
-				SYNC_DECL3(rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
+				SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
 			}
 			else if(subscribeOdomInfo)
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL3(rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
+				SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL2(rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]));
+				SYNC_DECL2(CommonDataSubscriber, rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]));
 			}
 		}
 #ifdef RTABMAP_SYNC_USER_DATA
@@ -662,7 +662,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 					subscribedToOdomInfo_ = false;
 					ROS_WARN("subscribe_odom_info ignored...");
 				}
-				SYNC_DECL3(rgbdDataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
+				SYNC_DECL3(CommonDataSubscriber, rgbdDataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
 			}
 			else if(subscribeScan2d)
 			{
@@ -673,7 +673,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 					subscribedToOdomInfo_ = false;
 					ROS_WARN("subscribe_odom_info ignored...");
 				}
-				SYNC_DECL3(rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
+				SYNC_DECL3(CommonDataSubscriber, rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
 			}
 			else if(subscribeScan3d)
 			{
@@ -684,17 +684,17 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 					subscribedToOdomInfo_ = false;
 					ROS_WARN("subscribe_odom_info ignored...");
 				}
-				SYNC_DECL3(rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
+				SYNC_DECL3(CommonDataSubscriber, rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
 			}
 			else if(subscribeOdomInfo)
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL3(rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
+				SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL2(rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]));
+				SYNC_DECL2(CommonDataSubscriber, rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]));
 			}
 		}
 #endif
@@ -709,7 +709,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 					subscribedToOdomInfo_ = false;
 					ROS_WARN("subscribe_odom_info ignored...");
 				}
-				SYNC_DECL2(rgbdScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), scanDescSub_);
+				SYNC_DECL2(CommonDataSubscriber, rgbdScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), scanDescSub_);
 			}
 			else if(subscribeScan2d)
 			{
@@ -720,7 +720,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 					subscribedToOdomInfo_ = false;
 					ROS_WARN("subscribe_odom_info ignored...");
 				}
-				SYNC_DECL2(rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
+				SYNC_DECL2(CommonDataSubscriber, rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
 			}
 			else if(subscribeScan3d)
 			{
@@ -731,13 +731,13 @@ void CommonDataSubscriber::setupRGBDCallbacks(
 					subscribedToOdomInfo_ = false;
 					ROS_WARN("subscribe_odom_info ignored...");
 				}
-				SYNC_DECL2(rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
+				SYNC_DECL2(CommonDataSubscriber, rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
 			}
 			else if(subscribeOdomInfo)
 			{
 				subscribedToOdomInfo_ = true;
 				odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-				SYNC_DECL2(rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_);
+				SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_);
 			}
 			else
 			{

src/impl/CommonDataSubscriberRGBD2.cpp

@@ -374,7 +374,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL5(rgbd2OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
+			SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -385,7 +385,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL5(rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+			SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -396,17 +396,17 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL5(rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+			SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
 			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-			SYNC_DECL5(rgbd2OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
+			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 
@@ -423,7 +423,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL4(rgbd2OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
+			SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -434,7 +434,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL4(rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+			SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -445,17 +445,17 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL4(rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+			SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
 			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-			SYNC_DECL4(rgbd2OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
+			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
@@ -471,7 +471,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL4(rgbd2DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
+			SYNC_DECL4(CommonDataSubscriber, rgbd2DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -482,7 +482,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL4(rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+			SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -493,17 +493,17 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL4(rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+			SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
 			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-			SYNC_DECL4(rgbd2DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
+			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
@@ -518,7 +518,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL3(rgbd2ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
+			SYNC_DECL3(CommonDataSubscriber, rgbd2ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -529,7 +529,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL3(rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+			SYNC_DECL3(CommonDataSubscriber, rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -540,17 +540,17 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL3(rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+			SYNC_DECL3(CommonDataSubscriber, rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
 			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-			SYNC_DECL3(rgbd2Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
+			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

@@ -461,7 +461,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL6(rgbd3OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
+			SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -472,7 +472,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL6(rgbd3OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
+			SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -483,17 +483,17 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL6(rgbd3OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
+			SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
 			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-			SYNC_DECL6(rgbd3OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
+			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]));
+			SYNC_DECL5(CommonDataSubscriber, rgbd3OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
 		}
 	}
 	else
@@ -510,7 +510,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL5(rgbd3OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
+			SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -521,7 +521,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL5(rgbd3OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
+			SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -532,17 +532,17 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL5(rgbd3OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
+			SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
 			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-			SYNC_DECL5(rgbd3OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
+			SYNC_DECL5(CommonDataSubscriber, rgbd3OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL4(rgbd3Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
+			SYNC_DECL4(CommonDataSubscriber, rgbd3Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
 		}
 	}
 #ifdef RTABMAP_SYNC_USER_DATA
@@ -558,7 +558,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL5(rgbd3DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);		}
+			SYNC_DECL5(CommonDataSubscriber, rgbd3DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);		}
 		else if(subscribeScan2d)
 		{
 			subscribedToScan2d_ = true;
@@ -568,7 +568,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL5(rgbd3DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
+			SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -579,17 +579,17 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL5(rgbd3DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
+			SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
 			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-			SYNC_DECL5(rgbd3DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
+			SYNC_DECL5(CommonDataSubscriber, 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_DECL4(CommonDataSubscriber, rgbd3Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
 		}
 	}
 #endif
@@ -604,7 +604,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL4(rgbd3ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
+			SYNC_DECL4(CommonDataSubscriber, rgbd3ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -615,7 +615,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL4(rgbd3Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
+			SYNC_DECL4(CommonDataSubscriber, rgbd3Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -626,17 +626,17 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL4(rgbd3Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
+			SYNC_DECL4(CommonDataSubscriber, rgbd3Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
 			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-			SYNC_DECL4(rgbd3Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
+			SYNC_DECL4(CommonDataSubscriber, rgbd3Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL3(rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
+			SYNC_DECL3(CommonDataSubscriber, rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
 		}
 	}
 }

src/impl/CommonDataSubscriberRGBD4.cpp

@@ -429,7 +429,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL7(rgbd4OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
+			SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -440,7 +440,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL7(rgbd4OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
+			SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -451,17 +451,17 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL7(rgbd4OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
+			SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
 			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-			SYNC_DECL7(rgbd4OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
+			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]));
+			SYNC_DECL6(CommonDataSubscriber, rgbd4OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
 		}
 	}
 	else
@@ -478,7 +478,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL6(rgbd4OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
+			SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -489,7 +489,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL6(rgbd4OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
+			SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -500,17 +500,17 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL6(rgbd4OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
+			SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
 			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-			SYNC_DECL6(rgbd4OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
+			SYNC_DECL6(CommonDataSubscriber, 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]));
+			SYNC_DECL5(CommonDataSubscriber, rgbd4Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
 		}
 	}
 #ifdef RTABMAP_SYNC_USER_DATA
@@ -526,7 +526,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL6(rgbd4DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
+			SYNC_DECL6(CommonDataSubscriber, rgbd4DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -537,7 +537,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL6(rgbd4DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
+			SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -548,17 +548,17 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL6(rgbd4DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
+			SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
 			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-			SYNC_DECL6(rgbd4DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
+			SYNC_DECL6(CommonDataSubscriber, 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_DECL5(CommonDataSubscriber, rgbd4Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
 		}
 	}
 #endif
@@ -573,7 +573,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL5(rgbd4ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
+			SYNC_DECL5(CommonDataSubscriber, rgbd4ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -584,7 +584,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL5(rgbd4Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
+			SYNC_DECL5(CommonDataSubscriber, rgbd4Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -595,17 +595,17 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL5(rgbd4Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
+			SYNC_DECL5(CommonDataSubscriber, rgbd4Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
 			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-			SYNC_DECL5(rgbd4Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
+			SYNC_DECL5(CommonDataSubscriber, rgbd4Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL4(rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
+			SYNC_DECL4(CommonDataSubscriber, rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
 		}
 	}
 }

src/impl/CommonDataSubscriberRGBD5.cpp

@@ -282,7 +282,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL7(rgbd5OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
+			SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -293,7 +293,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL7(rgbd5OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
+			SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -304,17 +304,17 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL7(rgbd5OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
+			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(nh, "odom_info", queueSize);
-			SYNC_DECL7(rgbd5OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
+			SYNC_DECL7(CommonDataSubscriber, rgbd5OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL6(rgbd5Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
+			SYNC_DECL6(CommonDataSubscriber, rgbd5Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
 		}
 	}
 	else
@@ -328,7 +328,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL6(rgbd5ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
+			SYNC_DECL6(CommonDataSubscriber, rgbd5ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -339,7 +339,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL6(rgbd5Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
+			SYNC_DECL6(CommonDataSubscriber, rgbd5Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -350,17 +350,17 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL6(rgbd5Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
+			SYNC_DECL6(CommonDataSubscriber, rgbd5Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
 		}
 		else if(subscribeOdomInfo)
 		{
 			subscribedToOdomInfo_ = true;
 			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-			SYNC_DECL6(rgbd5Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
+			SYNC_DECL6(CommonDataSubscriber, rgbd5Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL5(rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
+			SYNC_DECL5(CommonDataSubscriber, rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
 		}
 	}
 }

src/impl/CommonDataSubscriberRGBD6.cpp

@@ -299,7 +299,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL8(rgbd6OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
+			SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -310,7 +310,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL8(rgbd6OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
+			SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -321,17 +321,17 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL8(rgbd6OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
+			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(nh, "odom_info", queueSize);
-			SYNC_DECL8(rgbd6OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
+			SYNC_DECL8(CommonDataSubscriber, rgbd6OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL7(rgbd6Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
+			SYNC_DECL7(CommonDataSubscriber, rgbd6Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
 		}
 	}
 	else
@@ -345,7 +345,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL7(rgbd6ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
+			SYNC_DECL7(CommonDataSubscriber, rgbd6ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
 		}
 		else if(subscribeScan2d)
 		{
@@ -356,7 +356,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL7(rgbd6Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
+			SYNC_DECL7(CommonDataSubscriber, rgbd6Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
 		}
 		else if(subscribeScan3d)
 		{
@@ -367,17 +367,17 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
 				subscribedToOdomInfo_ = false;
 				ROS_WARN("subscribe_odom_info ignored...");
 			}
-			SYNC_DECL7(rgbd6Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
+			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(nh, "odom_info", queueSize);
-			SYNC_DECL7(rgbd6Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
+			SYNC_DECL7(CommonDataSubscriber, rgbd6Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
 		}
 		else
 		{
-			SYNC_DECL6(rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
+			SYNC_DECL6(CommonDataSubscriber, rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
 		}
 	}
 }

src/impl/CommonDataSubscriberRGBDX.cpp

@@ -0,0 +1,535 @@
+/*
+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::CameraInfo> cameraInfoMsgs; \
+		std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs; \
+		std::vector<std::vector<rtabmap_ros::KeyPoint> > localKeyPoints; \
+		std::vector<std::vector<rtabmap_ros::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::RGBDImagesConstPtr& imagesMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXScan2dCallback(
+		const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXScan3dCallback(
+		const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXScanDescCallback(
+		const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr 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::RGBDImagesConstPtr& imagesMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::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::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImagesConstPtr& imagesMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXOdomScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXOdomScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXOdomScanDescCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr 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::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::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::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImagesConstPtr& imagesMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXDataScan2dCallback(
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXDataScan3dCallback(
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXDataScanDescCallback(
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr 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::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::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::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImagesConstPtr& imagesMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXOdomDataScan2dCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXOdomDataScan3dCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::LaserScan scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+void CommonDataSubscriber::rgbdXOdomDataScanDescCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
+		const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::OdomInfoConstPtr 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::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImagesConstPtr& imagesMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::LaserScan scanMsg; // Null
+	sensor_msgs::PointCloud2 scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
+}
+#endif
+
+void CommonDataSubscriber::setupRGBDXCallbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeScanDesc,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup rgbdX callback");
+
+	rgbdXSub_.subscribe(nh, "rgbd_images", queueSize);
+#ifdef RTABMAP_SYNC_USER_DATA
+	if(subscribeOdom && subscribeUserData)
+	{
+		odomSub_.subscribe(nh, "odom", queueSize);
+		userDataSub_.subscribe(nh, "user_data", queueSize);
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			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(nh, "odom", queueSize);
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScanDesc, approxSync, queueSize, odomSub_, rgbdXSub_, scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan2d, approxSync, queueSize, odomSub_, rgbdXSub_, scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan3d, approxSync, queueSize, odomSub_, rgbdXSub_, scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			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(nh, "user_data", queueSize);
+		if(subscribeScanDesc)
+		{
+			subscribedToScanDescriptor_ = true;
+			scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL3(CommonDataSubscriber, rgbdXDataScanDesc, approxSync, queueSize, userDataSub_, rgbdXSub_, scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan2d, approxSync, queueSize, userDataSub_, rgbdXSub_, scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan3d, approxSync, queueSize, userDataSub_, rgbdXSub_, scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			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(nh, "scan_descriptor", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL2(CommonDataSubscriber, rgbdXScanDesc, approxSync, queueSize, rgbdXSub_, scanDescSub_);
+		}
+		else if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL2(CommonDataSubscriber, rgbdXScan2d, approxSync, queueSize, rgbdXSub_, scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = false;
+				ROS_WARN("subscribe_odom_info ignored...");
+			}
+			SYNC_DECL2(CommonDataSubscriber, rgbdXScan3d, approxSync, queueSize, rgbdXSub_, scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
+			SYNC_DECL2(CommonDataSubscriber, rgbdXInfo, approxSync, queueSize, rgbdXSub_, odomInfoSub_);
+		}
+		else
+		{
+			rgbdXSubOnly_ = nh.subscribe("rgbd_images", queueSize, &CommonDataSubscriber::rgbdXCallback, this);
+
+			subscribedTopicsMsg_ =
+					uFormat("\n%s subscribed to:\n   %s",
+					ros::this_node::getName().c_str(),
+					rgbdXSubOnly_.getTopic().c_str());
+		}
+	}
+}
+
+} /* namespace rtabmap_ros */

src/impl/CommonDataSubscriberScan.cpp

@@ -310,11 +310,11 @@ void CommonDataSubscriber::setupScanCallbacks(
 				{
 					subscribedToOdomInfo_ = true;
 					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-					SYNC_DECL4(odomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_);
+					SYNC_DECL4(CommonDataSubscriber, odomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_);
 				}
 				else
 				{
-					SYNC_DECL3(odomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_);
+					SYNC_DECL3(CommonDataSubscriber, odomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_);
 				}
 			}
 			else if(scan2dTopic)
@@ -323,11 +323,11 @@ void CommonDataSubscriber::setupScanCallbacks(
 				{
 					subscribedToOdomInfo_ = true;
 					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-					SYNC_DECL4(odomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, scanSub_, odomInfoSub_);
+					SYNC_DECL4(CommonDataSubscriber, odomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, scanSub_, odomInfoSub_);
 				}
 				else
 				{
-					SYNC_DECL3(odomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, scanSub_);
+					SYNC_DECL3(CommonDataSubscriber, odomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, scanSub_);
 				}
 			}
 			else
@@ -336,11 +336,11 @@ void CommonDataSubscriber::setupScanCallbacks(
 				{
 					subscribedToOdomInfo_ = true;
 					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-					SYNC_DECL4(odomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_);
+					SYNC_DECL4(CommonDataSubscriber, odomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_);
 				}
 				else
 				{
-					SYNC_DECL3(odomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_);
+					SYNC_DECL3(CommonDataSubscriber, odomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_);
 				}
 			}
 		}
@@ -356,11 +356,11 @@ void CommonDataSubscriber::setupScanCallbacks(
 				{
 					subscribedToOdomInfo_ = true;
 					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-					SYNC_DECL3(odomScanDescInfo, approxSync, queueSize, odomSub_, scanDescSub_, odomInfoSub_);
+					SYNC_DECL3(CommonDataSubscriber, odomScanDescInfo, approxSync, queueSize, odomSub_, scanDescSub_, odomInfoSub_);
 				}
 				else
 				{
-					SYNC_DECL2(odomScanDesc, approxSync, queueSize, odomSub_, scanDescSub_);
+					SYNC_DECL2(CommonDataSubscriber, odomScanDesc, approxSync, queueSize, odomSub_, scanDescSub_);
 				}
 			}
 			else if(scan2dTopic)
@@ -369,11 +369,11 @@ void CommonDataSubscriber::setupScanCallbacks(
 				{
 					subscribedToOdomInfo_ = true;
 					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-					SYNC_DECL3(odomScan2dInfo, approxSync, queueSize, odomSub_, scanSub_, odomInfoSub_);
+					SYNC_DECL3(CommonDataSubscriber, odomScan2dInfo, approxSync, queueSize, odomSub_, scanSub_, odomInfoSub_);
 				}
 				else
 				{
-					SYNC_DECL2(odomScan2d, approxSync, queueSize, odomSub_, scanSub_);
+					SYNC_DECL2(CommonDataSubscriber, odomScan2d, approxSync, queueSize, odomSub_, scanSub_);
 				}
 			}
 			else
@@ -382,11 +382,11 @@ void CommonDataSubscriber::setupScanCallbacks(
 				{
 					subscribedToOdomInfo_ = true;
 					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-					SYNC_DECL3(odomScan3dInfo, approxSync, queueSize, odomSub_, scan3dSub_, odomInfoSub_);
+					SYNC_DECL3(CommonDataSubscriber, odomScan3dInfo, approxSync, queueSize, odomSub_, scan3dSub_, odomInfoSub_);
 				}
 				else
 				{
-					SYNC_DECL2(odomScan3d, approxSync, queueSize, odomSub_, scan3dSub_);
+					SYNC_DECL2(CommonDataSubscriber, odomScan3d, approxSync, queueSize, odomSub_, scan3dSub_);
 				}
 			}
 		}
@@ -401,11 +401,11 @@ void CommonDataSubscriber::setupScanCallbacks(
 				{
 					subscribedToOdomInfo_ = true;
 					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-					SYNC_DECL3(dataScanDescInfo, approxSync, queueSize, userDataSub_, scanDescSub_, odomInfoSub_);
+					SYNC_DECL3(CommonDataSubscriber, dataScanDescInfo, approxSync, queueSize, userDataSub_, scanDescSub_, odomInfoSub_);
 				}
 				else
 				{
-					SYNC_DECL2(dataScanDesc, approxSync, queueSize, userDataSub_, scanDescSub_);
+					SYNC_DECL2(CommonDataSubscriber, dataScanDesc, approxSync, queueSize, userDataSub_, scanDescSub_);
 				}
 			}
 			else if(scan2dTopic)
@@ -418,7 +418,7 @@ void CommonDataSubscriber::setupScanCallbacks(
 				}
 				else
 				{
-					SYNC_DECL2(dataScan2d, approxSync, queueSize, userDataSub_, scanSub_);
+					SYNC_DECL2(CommonDataSubscriber, dataScan2d, approxSync, queueSize, userDataSub_, scanSub_);
 				}
 			}
 			else
@@ -427,11 +427,11 @@ void CommonDataSubscriber::setupScanCallbacks(
 				{
 					subscribedToOdomInfo_ = true;
 					odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-					SYNC_DECL3(dataScan3dInfo, approxSync, queueSize, userDataSub_, scan3dSub_, odomInfoSub_);
+					SYNC_DECL3(CommonDataSubscriber, dataScan3dInfo, approxSync, queueSize, userDataSub_, scan3dSub_, odomInfoSub_);
 				}
 				else
 				{
-					SYNC_DECL2(dataScan3d, approxSync, queueSize, userDataSub_, scan3dSub_);
+					SYNC_DECL2(CommonDataSubscriber, dataScan3d, approxSync, queueSize, userDataSub_, scan3dSub_);
 				}
 			}
 		}
@@ -442,15 +442,15 @@ void CommonDataSubscriber::setupScanCallbacks(
 			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
 			if(scanDescTopic)
 			{
-				SYNC_DECL2(scanDescInfo, approxSync, queueSize, scanDescSub_, odomInfoSub_);
+				SYNC_DECL2(CommonDataSubscriber, scanDescInfo, approxSync, queueSize, scanDescSub_, odomInfoSub_);
 			}
 			else if(scan2dTopic)
 			{
-				SYNC_DECL2(scan2dInfo, approxSync, queueSize, scanSub_, odomInfoSub_);
+				SYNC_DECL2(CommonDataSubscriber, scan2dInfo, approxSync, queueSize, scanSub_, odomInfoSub_);
 			}
 			else
 			{
-				SYNC_DECL2(scan3dInfo, approxSync, queueSize, scan3dSub_, odomInfoSub_);
+				SYNC_DECL2(CommonDataSubscriber, scan3dInfo, approxSync, queueSize, scan3dSub_, odomInfoSub_);
 			}
 		}
 	}

src/impl/CommonDataSubscriberStereo.cpp

@@ -121,11 +121,11 @@ void CommonDataSubscriber::setupStereoCallbacks(
 		{
 			subscribedToOdomInfo_ = true;
 			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-			SYNC_DECL6(stereoOdomInfo, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
+			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
@@ -134,11 +134,11 @@ void CommonDataSubscriber::setupStereoCallbacks(
 		{
 			subscribedToOdomInfo_ = true;
 			odomInfoSub_.subscribe(nh, "odom_info", queueSize);
-			SYNC_DECL5(stereoInfo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
+			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_);
 		}
 	}
 }

src/nodelets/icp_odometry.cpp

@@ -535,7 +535,7 @@ private:
 					"cloud is not dense, for convenience it will be set to %d (%dx%d)",
 					scanCloudMaxPoints_, cloudMsg.width, cloudMsg.height);
 		}
-		else if(cloudMsg.height > 1 && scanCloudMaxPoints_ != cloudMsg.height * cloudMsg.width)
+		else if(cloudMsg.height > 1 && scanCloudMaxPoints_ < cloudMsg.height * cloudMsg.width)
 		{
 			NODELET_WARN("IcpOdometry: \"scan_cloud_max_points\" is set to %d but input "
 					"cloud is not dense and has a size of %d (%dx%d), setting to this later size.",

src/nodelets/point_cloud_assembler.cpp

@@ -51,6 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_ros/OdomInfo.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/Version.h>
 
 namespace rtabmap_ros
 {
@@ -391,12 +392,52 @@ private:
 					sensor_msgs::PointCloud2 rosCloud;
 					if(voxelSize_>0.0)
 					{
-						pcl::VoxelGrid<pcl::PCLPointCloud2> filter;
-						filter.setLeafSize(voxelSize_, voxelSize_, voxelSize_);
-						filter.setInputCloud(assembled);
-						pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
-						filter.filter(*output);
-						assembled = output;
+						// estimate if there would be an overflow
+						int x_idx=-1, y_idx=-1, z_idx=-1;
+						for (std::size_t d = 0; d < assembled->fields.size (); ++d)
+						{
+							if (assembled->fields[d].name.compare("x")==0)
+								x_idx = d;
+							if (assembled->fields[d].name.compare("y")==0)
+								y_idx = d;
+							if (assembled->fields[d].name.compare("z")==0)
+								z_idx = d;
+						}
+						bool overflow = false;
+						if(x_idx>=0 && y_idx>=0 && z_idx>=0) {
+							Eigen::Vector4f min_p, max_p;
+							pcl::getMinMax3D(assembled, x_idx, y_idx, z_idx, min_p, max_p);
+							float inverseVoxelSize = 1.0f/voxelSize_;
+							std::int64_t dx = static_cast<std::int64_t>((max_p[0] - min_p[0]) * inverseVoxelSize)+1;
+							std::int64_t dy = static_cast<std::int64_t>((max_p[1] - min_p[1]) * inverseVoxelSize)+1;
+							std::int64_t dz = static_cast<std::int64_t>((max_p[2] - min_p[2]) * inverseVoxelSize)+1;
+
+							if ((dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()))
+							{
+								overflow = true;
+							}
+						}
+						if(overflow)
+						{
+							rtabmap::LaserScan scan = rtabmap::util3d::laserScanFromPointCloud(*assembled);
+							scan = rtabmap::util3d::commonFiltering(scan, 1, 0, 0, voxelSize_);
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+							std::uint64_t stamp = assembled->header.stamp;
+#else
+							pcl::uint64_t stamp = assembled->header.stamp;
+#endif
+							assembled = rtabmap::util3d::laserScanToPointCloud2(scan);
+							assembled->header.stamp = stamp;
+						}
+						else
+						{
+							pcl::VoxelGrid<pcl::PCLPointCloud2> filter;
+							filter.setLeafSize(voxelSize_, voxelSize_, voxelSize_);
+							filter.setInputCloud(assembled);
+							pcl::PCLPointCloud2Ptr output(new pcl::PCLPointCloud2);
+							filter.filter(*output);
+							assembled = output;
+						}
 					}
 					if(noiseRadius_>0.0 && noiseMinNeighbors_>0)
 					{

src/nodelets/rgbdx_sync.cpp

@@ -0,0 +1,307 @@
+/*
+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 <ros/ros.h>
+#include <pluginlib/class_list_macros.h>
+#include <nodelet/nodelet.h>
+
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/subscriber.h>
+
+#include <boost/thread.hpp>
+
+#include "rtabmap_ros/RGBDImages.h"
+#include "rtabmap_ros/CommonDataSubscriber.h"
+
+namespace rtabmap_ros
+{
+
+class RGBDXSync : public nodelet::Nodelet
+{
+public:
+	RGBDXSync() :
+		warningThread_(0),
+		callbackCalled_(false),
+		SYNC_INIT(rgbd2),
+		SYNC_INIT(rgbd3),
+		SYNC_INIT(rgbd4),
+		SYNC_INIT(rgbd5),
+		SYNC_INIT(rgbd6),
+		SYNC_INIT(rgbd7),
+		SYNC_INIT(rgbd8)
+	{}
+
+	virtual ~RGBDXSync()
+	{
+		SYNC_DEL(rgbd2);
+
+		if(warningThread_)
+		{
+			callbackCalled_=true;
+			warningThread_->join();
+			delete warningThread_;
+		}
+	}
+
+private:
+
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int queueSize = 10;
+		bool approxSync = true;
+		int rgbdCameras = 2;
+		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
+
+		NODELET_INFO("%s: approx_sync  = %s", getName().c_str(), approxSync?"true":"false");
+		NODELET_INFO("%s: queue_size   = %d", getName().c_str(), queueSize);
+		NODELET_INFO("%s: rgbd_cameras = %d", getName().c_str(), rgbdCameras);
+
+		rgbdImagesPub_ = nh.advertise<rtabmap_ros::RGBDImages>("rgbd_images", 1);
+
+		ROS_ASSERT(rgbdCameras>=2 && rgbdCameras<=8);
+
+		rgbdSubs_.resize(rgbdCameras);
+		for(int i=0; i<rgbdCameras; ++i)
+		{
+			rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
+			rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize);
+		}
+
+		std::string name_ = this->getName();
+		std::string subscribedTopicsMsg_;
+		if(rgbdCameras==2)
+		{
+			SYNC_DECL2(RGBDXSync, rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
+		}
+		else if(rgbdCameras==3)
+		{
+			SYNC_DECL3(RGBDXSync, rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
+		}
+		else if(rgbdCameras==4)
+		{
+			SYNC_DECL4(RGBDXSync, rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
+		}
+		else if(rgbdCameras==5)
+		{
+			SYNC_DECL5(RGBDXSync, rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
+		}
+		else if(rgbdCameras==6)
+		{
+			SYNC_DECL6(RGBDXSync, rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
+		}
+		else if(rgbdCameras==7)
+		{
+			SYNC_DECL7(RGBDXSync, rgbd7, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]));
+		}
+		else if(rgbdCameras==8)
+		{
+			SYNC_DECL8(RGBDXSync, rgbd8, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]), (*rgbdSubs_[7]));
+		}
+
+		warningThread_ = new boost::thread(boost::bind(&RGBDXSync::warningLoop, this, subscribedTopicsMsg_, approxSync));
+		NODELET_INFO("%s", subscribedTopicsMsg_.c_str());
+	}
+
+	void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
+	{
+		ros::Duration r(5.0);
+		while(!callbackCalled_)
+		{
+			r.sleep();
+			if(!callbackCalled_)
+			{
+				ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
+						"published (\"$ rostopic hz my_topic\") and the timestamps in their "
+						"header are set. %s%s",
+						getName().c_str(),
+						approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
+							"topics should have all the exact timestamp for the callback to be called.",
+						subscribedTopicsMsg.c_str());
+			}
+		}
+	}
+
+	DATA_SYNCS2(rgbd2, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
+	DATA_SYNCS3(rgbd3, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
+	DATA_SYNCS4(rgbd4, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
+	DATA_SYNCS5(rgbd5, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
+	DATA_SYNCS6(rgbd6, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
+	DATA_SYNCS7(rgbd7, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
+	DATA_SYNCS8(rgbd8, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
+
+private:
+	boost::thread * warningThread_;
+	bool callbackCalled_;
+
+	ros::Publisher rgbdImagesPub_;
+
+	std::vector<message_filters::Subscriber<rtabmap_ros::RGBDImage>*> rgbdSubs_;
+};
+
+
+void RGBDXSync::rgbd2Callback(
+		  const rtabmap_ros::RGBDImageConstPtr& image0,
+		  const rtabmap_ros::RGBDImageConstPtr& image1)
+{
+	callbackCalled_ = true;
+	rtabmap_ros::RGBDImages output;
+	output.header = image0->header;
+	output.rgbd_images.resize(2);
+	output.rgbd_images[0]=(*image0);
+	output.rgbd_images[1]=(*image1);
+	rgbdImagesPub_.publish(output);
+}
+
+void RGBDXSync::rgbd3Callback(
+		  const rtabmap_ros::RGBDImageConstPtr& image0,
+		  const rtabmap_ros::RGBDImageConstPtr& image1,
+		  const rtabmap_ros::RGBDImageConstPtr& image2)
+{
+	callbackCalled_ = true;
+	rtabmap_ros::RGBDImages output;
+	output.header = image0->header;
+	output.rgbd_images.resize(3);
+	output.rgbd_images[0]=(*image0);
+	output.rgbd_images[1]=(*image1);
+	output.rgbd_images[2]=(*image2);
+	rgbdImagesPub_.publish(output);
+}
+
+void RGBDXSync::rgbd4Callback(
+		  const rtabmap_ros::RGBDImageConstPtr& image0,
+		  const rtabmap_ros::RGBDImageConstPtr& image1,
+		  const rtabmap_ros::RGBDImageConstPtr& image2,
+		  const rtabmap_ros::RGBDImageConstPtr& image3)
+{
+	callbackCalled_ = true;
+	rtabmap_ros::RGBDImages output;
+	output.header = image0->header;
+	output.rgbd_images.resize(4);
+	output.rgbd_images[0]=(*image0);
+	output.rgbd_images[1]=(*image1);
+	output.rgbd_images[2]=(*image2);
+	output.rgbd_images[3]=(*image3);
+	rgbdImagesPub_.publish(output);
+}
+
+void RGBDXSync::rgbd5Callback(
+		  const rtabmap_ros::RGBDImageConstPtr& image0,
+		  const rtabmap_ros::RGBDImageConstPtr& image1,
+		  const rtabmap_ros::RGBDImageConstPtr& image2,
+		  const rtabmap_ros::RGBDImageConstPtr& image3,
+		  const rtabmap_ros::RGBDImageConstPtr& image4)
+{
+	callbackCalled_ = true;
+	rtabmap_ros::RGBDImages output;
+	output.header = image0->header;
+	output.rgbd_images.resize(5);
+	output.rgbd_images[0]=(*image0);
+	output.rgbd_images[1]=(*image1);
+	output.rgbd_images[2]=(*image2);
+	output.rgbd_images[3]=(*image3);
+	output.rgbd_images[4]=(*image4);
+	rgbdImagesPub_.publish(output);
+}
+
+void RGBDXSync::rgbd6Callback(
+		  const rtabmap_ros::RGBDImageConstPtr& image0,
+		  const rtabmap_ros::RGBDImageConstPtr& image1,
+		  const rtabmap_ros::RGBDImageConstPtr& image2,
+		  const rtabmap_ros::RGBDImageConstPtr& image3,
+		  const rtabmap_ros::RGBDImageConstPtr& image4,
+		  const rtabmap_ros::RGBDImageConstPtr& image5)
+{
+	callbackCalled_ = true;
+	rtabmap_ros::RGBDImages output;
+	output.header = image0->header;
+	output.rgbd_images.resize(6);
+	output.rgbd_images[0]=(*image0);
+	output.rgbd_images[1]=(*image1);
+	output.rgbd_images[2]=(*image2);
+	output.rgbd_images[3]=(*image3);
+	output.rgbd_images[4]=(*image4);
+	output.rgbd_images[5]=(*image5);
+	rgbdImagesPub_.publish(output);
+}
+
+void RGBDXSync::rgbd7Callback(
+		  const rtabmap_ros::RGBDImageConstPtr& image0,
+		  const rtabmap_ros::RGBDImageConstPtr& image1,
+		  const rtabmap_ros::RGBDImageConstPtr& image2,
+		  const rtabmap_ros::RGBDImageConstPtr& image3,
+		  const rtabmap_ros::RGBDImageConstPtr& image4,
+		  const rtabmap_ros::RGBDImageConstPtr& image5,
+		  const rtabmap_ros::RGBDImageConstPtr& image6)
+{
+	callbackCalled_ = true;
+	rtabmap_ros::RGBDImages output;
+	output.header = image0->header;
+	output.rgbd_images.resize(7);
+	output.rgbd_images[0]=(*image0);
+	output.rgbd_images[1]=(*image1);
+	output.rgbd_images[2]=(*image2);
+	output.rgbd_images[3]=(*image3);
+	output.rgbd_images[4]=(*image4);
+	output.rgbd_images[5]=(*image5);
+	output.rgbd_images[6]=(*image6);
+	rgbdImagesPub_.publish(output);
+}
+
+void RGBDXSync::rgbd8Callback(
+		  const rtabmap_ros::RGBDImageConstPtr& image0,
+		  const rtabmap_ros::RGBDImageConstPtr& image1,
+		  const rtabmap_ros::RGBDImageConstPtr& image2,
+		  const rtabmap_ros::RGBDImageConstPtr& image3,
+		  const rtabmap_ros::RGBDImageConstPtr& image4,
+		  const rtabmap_ros::RGBDImageConstPtr& image5,
+		  const rtabmap_ros::RGBDImageConstPtr& image6,
+		  const rtabmap_ros::RGBDImageConstPtr& image7)
+{
+	callbackCalled_ = true;
+	rtabmap_ros::RGBDImages output;
+	output.header = image0->header;
+	output.rgbd_images.resize(8);
+	output.rgbd_images[0]=(*image0);
+	output.rgbd_images[1]=(*image1);
+	output.rgbd_images[2]=(*image2);
+	output.rgbd_images[3]=(*image3);
+	output.rgbd_images[4]=(*image4);
+	output.rgbd_images[5]=(*image5);
+	output.rgbd_images[6]=(*image6);
+	output.rgbd_images[7]=(*image7);
+	rgbdImagesPub_.publish(output);
+}
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDXSync, nodelet::Nodelet);
+}
+

src/rviz/MapCloudDisplay.cpp

@@ -57,6 +57,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Graph.h>
 #include <rtabmap_ros/MsgConversion.h>
 #include <rtabmap_ros/GetMap.h>
+#include <std_msgs/Int32MultiArray.h>
 
 
 namespace rtabmap_ros
@@ -90,7 +91,8 @@ MapCloudDisplay::MapCloudDisplay()
     new_xyz_transformer_(false),
     new_color_transformer_(false),
     needs_retransform_(false),
-    transformer_class_loader_(NULL)
+    transformer_class_loader_(NULL),
+	current_map_updated_(false)
 {
 	//QIcon icon;
 	//this->setIcon(icon);
@@ -186,6 +188,8 @@ MapCloudDisplay::MapCloudDisplay()
 	node_filtering_angle_->setMin( 0.0f );
 	node_filtering_angle_->setMax( 359.0f );
 
+	download_namespace = new rviz::StringProperty("Download namespace", "rtabmap", "Namespace used to call Download services below", this, SLOT( downloadNamespaceChanged() ), this);
+
 	download_map_ = new rviz::BoolProperty( "Download map", false,
 										 "Download the optimized global map using rtabmap/GetMap service. This will force to re-create all clouds.",
 										 this, SLOT( downloadMap() ), this );
@@ -194,6 +198,8 @@ MapCloudDisplay::MapCloudDisplay()
 											 "Download the optimized global graph (without cloud data) using rtabmap/GetMap service.",
 											 this, SLOT( downloadGraph() ), this );
 
+	downloadNamespaceChanged();
+
 	// PointCloudCommon sets up a callback queue with a thread for each
 	// instance.  Use that for processing incoming messages.
 	update_nh_.setCallbackQueue( &cbqueue_ );
@@ -385,6 +391,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 	{
 		boost::mutex::scoped_lock lock(current_map_mutex_);
 		current_map_ = poses;
+		current_map_updated_ = true;
 	}
 }
 
@@ -527,50 +534,71 @@ void MapCloudDisplay::updateCloudParameters()
 	// do nothing... only take effect on next generated clouds
 }
 
+void MapCloudDisplay::downloadMap(bool graphOnly)
+{
+	rtabmap_ros::GetMap getMapSrv;
+	getMapSrv.request.global = false;
+	getMapSrv.request.optimized = true;
+	getMapSrv.request.graphOnly = graphOnly;
+	std::string rtabmapNs = download_namespace->getStdString();
+	std::string srvName = update_nh_.resolveName(uFormat("%s/get_map_data", rtabmapNs.c_str()));
+	QMessageBox * messageBox = new QMessageBox(
+			QMessageBox::NoIcon,
+			tr("Calling \"%1\" service...").arg(srvName.c_str()),
+			tr("Downloading the map... please wait (rviz could become gray!)"),
+			QMessageBox::NoButton);
+	messageBox->setAttribute(Qt::WA_DeleteOnClose, true);
+	messageBox->show();
+	QApplication::processEvents();
+	uSleep(100); // hack make sure the text in the QMessageBox is shown...
+	QApplication::processEvents();
+	if(!ros::service::call(srvName, getMapSrv))
+	{
+		ROS_ERROR("MapCloudDisplay: Cannot call \"%s\" service. "
+				  "Tip: if rtabmap node is not in \"%s\" namespace, you can "
+				  "change the \"Download namespace\" option.",
+				  srvName.c_str(),
+				  rtabmapNs.c_str());
+		messageBox->setText(tr("MapCloudDisplay: Cannot call \"%1\" service. "
+				  "Tip: if rtabmap node is not in \"%2\" namespace, you can "
+				  "change the \"Download namespace\" option.").
+				  arg(srvName.c_str()).arg(rtabmapNs.c_str()));
+	}
+	else if(graphOnly)
+	{
+		messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(getMapSrv.response.data.graph.poses.size()));
+		QApplication::processEvents();
+		processMapData(getMapSrv.response.data);
+		messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.graph.poses.size()));
+
+		QTimer::singleShot(1000, messageBox, SLOT(close()));
+	}
+	else
+	{
+		messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
+				.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
+		QApplication::processEvents();
+		this->reset();
+		processMapData(getMapSrv.response.data);
+		messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!")
+				.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
+
+		QTimer::singleShot(1000, messageBox, SLOT(close()));
+	}
+}
+
+void MapCloudDisplay::downloadNamespaceChanged()
+{
+	std::string rtabmapNs = download_namespace->getStdString();
+	std::string topicName = update_nh_.resolveName(uFormat("%s/republish_node_data", rtabmapNs.c_str()));
+	republishNodeDataPub_ = update_nh_.advertise<std_msgs::Int32MultiArray>(topicName, 1);
+}
+
 void MapCloudDisplay::downloadMap()
 {
 	if(download_map_->getBool())
 	{
-		rtabmap_ros::GetMap getMapSrv;
-		getMapSrv.request.global = true;
-		getMapSrv.request.optimized = true;
-		getMapSrv.request.graphOnly = false;
-		ros::NodeHandle nh;
-		QMessageBox * messageBox = new QMessageBox(
-				QMessageBox::NoIcon,
-				tr("Calling \"%1\" service...").arg(nh.resolveName("rtabmap/get_map_data").c_str()),
-				tr("Downloading the map... please wait (rviz could become gray!)"),
-				QMessageBox::NoButton);
-		messageBox->setAttribute(Qt::WA_DeleteOnClose, true);
-		messageBox->show();
-		QApplication::processEvents();
-		uSleep(100); // hack make sure the text in the QMessageBox is shown...
-		QApplication::processEvents();
-		if(!ros::service::call("rtabmap/get_map_data", getMapSrv))
-		{
-			ROS_ERROR("MapCloudDisplay: Can't call \"%s\" service. "
-					  "Tip: if rtabmap node is not in rtabmap namespace, you can remap the service "
-					  "to \"get_map_data\" in the launch "
-					  "file like: <remap from=\"rtabmap/get_map_data\" to=\"get_map_data\"/>.",
-					  nh.resolveName("rtabmap/get_map_data").c_str());
-			messageBox->setText(tr("MapCloudDisplay: Can't call \"%1\" service. "
-					  "Tip: if rtabmap node is not in rtabmap namespace, you can remap the service "
-					  "to \"get_map_data\" in the launch "
-					  "file like: <remap from=\"rtabmap/get_map_data\" to=\"get_map_data\"/>.").
-					  arg(nh.resolveName("rtabmap/get_map_data").c_str()));
-		}
-		else
-		{
-			messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
-					.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
-			QApplication::processEvents();
-			this->reset();
-			processMapData(getMapSrv.response.data);
-			messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!")
-					.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
-
-			QTimer::singleShot(1000, messageBox, SLOT(close()));
-		}
+		downloadMap(false);
 		download_map_->blockSignals(true);
 		download_map_->setBool(false);
 		download_map_->blockSignals(false);
@@ -589,43 +617,7 @@ void MapCloudDisplay::downloadGraph()
 {
 	if(download_graph_->getBool())
 	{
-		rtabmap_ros::GetMap getMapSrv;
-		getMapSrv.request.global = true;
-		getMapSrv.request.optimized = true;
-		getMapSrv.request.graphOnly = true;
-		ros::NodeHandle nh;
-		QMessageBox * messageBox = new QMessageBox(
-				QMessageBox::NoIcon,
-				tr("Calling \"%1\" service...").arg(nh.resolveName("rtabmap/get_map_data").c_str()),
-				tr("Downloading the graph... please wait (rviz could become gray!)"),
-				QMessageBox::NoButton);
-		messageBox->setAttribute(Qt::WA_DeleteOnClose, true);
-		messageBox->show();
-		QApplication::processEvents();
-		uSleep(100); // hack make sure the text in the QMessageBox is shown...
-		QApplication::processEvents();
-		if(!ros::service::call("rtabmap/get_map_data", getMapSrv))
-		{
-			ROS_ERROR("MapCloudDisplay: Can't call \"%s\" service. "
-					  "Tip: if rtabmap node is not in rtabmap namespace, you can remap the service "
-					  "to \"get_map_data\" in the launch "
-					  "file like: <remap from=\"rtabmap/get_map_data\" to=\"get_map_data\"/>.",
-					  nh.resolveName("rtabmap/get_map_data").c_str());
-			messageBox->setText(tr("MapCloudDisplay: Can't call \"%1\" service. "
-					  "Tip: if rtabmap node is not in rtabmap namespace, you can remap the service "
-					  "to \"get_map_data\" in the launch "
-					  "file like: <remap from=\"rtabmap/get_map_data\" to=\"get_map_data\"/>.").
-					  arg(nh.resolveName("rtabmap/get_map_data").c_str()));
-		}
-		else
-		{
-			messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(getMapSrv.response.data.graph.poses.size()));
-			QApplication::processEvents();
-			processMapData(getMapSrv.response.data);
-			messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.graph.poses.size()));
-
-			QTimer::singleShot(1000, messageBox, SLOT(close()));
-		}
+		downloadMap(true);
 		download_graph_->blockSignals(true);
 		download_graph_->setBool(false);
 		download_graph_->blockSignals(false);
@@ -728,6 +720,7 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
 		boost::mutex::scoped_lock lock(current_map_mutex_);
 		if(!current_map_.empty())
 		{
+			std::vector<int> missingNodes;
 			for (std::map<int, rtabmap::Transform>::iterator it=current_map_.begin(); it != current_map_.end(); ++it)
 			{
 				std::map<int, CloudInfoPtr>::iterator cloudInfoIt = cloud_infos_.find(it->first);
@@ -759,9 +752,15 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
 					}
 					else
 					{
-						ROS_ERROR("MapCloudDisplay: Could not update pose of node %d", it->first);
+						ROS_ERROR("MapCloudDisplay: Could not update pose of node %d (cannot transform pose in target frame id \"%s\", set fixed frame in global options to \"%s\")",
+								it->first,
+								cloudInfoIt->second->message_->header.frame_id.c_str(),
+								cloudInfoIt->second->message_->header.frame_id.c_str());
 					}
-
+				}
+				else if(it->first>0 && current_map_updated_)
+				{
+					missingNodes.push_back(it->first);
 				}
 			}
 			//hide not used clouds
@@ -786,7 +785,15 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
 					++iter;
 				}
 			}
+
+			if(!missingNodes.empty())
+			{
+				std_msgs::Int32MultiArray msg;
+				msg.data = missingNodes;
+				republishNodeDataPub_.publish(msg);
+			}
 		}
+		current_map_updated_ = false;
 	}
 	if(lastCloudAdded>0)
 	{
@@ -808,6 +815,7 @@ void MapCloudDisplay::reset()
 	{
 		boost::mutex::scoped_lock lock(current_map_mutex_);
 		current_map_.clear();
+		current_map_updated_ = false;
 	}
 	MFDClass::reset();
 }

src/rviz/MapCloudDisplay.h

@@ -119,6 +119,7 @@ public:
 	rviz::FloatProperty* cloud_filter_ceiling_height_;
 	rviz::FloatProperty* node_filtering_radius_;
 	rviz::FloatProperty* node_filtering_angle_;
+	rviz::StringProperty * download_namespace;
 	rviz::BoolProperty* download_map_;
 	rviz::BoolProperty* download_graph_;
 
@@ -134,6 +135,7 @@ private Q_SLOTS:
 	void setXyzTransformerOptions( EnumProperty* prop );
 	void setColorTransformerOptions( EnumProperty* prop );
 	void updateCloudParameters();
+	void downloadNamespaceChanged();
 	void downloadMap();
 	void downloadGraph();
 
@@ -145,6 +147,7 @@ protected:
 	virtual void processMessage( const rtabmap_ros::MapDataConstPtr& cloud );
 
 private:
+	void downloadMap(bool graphOnly);
 	void processMapData(const rtabmap_ros::MapData& map);
 
 	/**
@@ -165,6 +168,7 @@ private:
 private:
 	ros::AsyncSpinner spinner_;
 	ros::CallbackQueue cbqueue_;
+	ros::Publisher republishNodeDataPub_;
 
 	std::map<int, CloudInfoPtr> cloud_infos_;
 
@@ -173,6 +177,7 @@ private:
 
 	std::map<int, rtabmap::Transform> current_map_;
 	boost::mutex current_map_mutex_;
+	bool current_map_updated_;
 
 	int lastCloudAdded_;
 

srv/CleanupLocalGrids.srv

@@ -0,0 +1,24 @@
+#  Cleanup local grids service
+# 
+#     Clear empty space from local occupancy grids 
+#     (and laser scans) based on the current optimized global 2d grid map.
+#     If the map needs to be regenerated in the future (e.g., when
+#     we re-use the map in SLAM mode), removed obstacles won't reappear.
+#     Use this with care and only when you know that the map doesn't have errors, 
+#     otherwise some real obstacles/walls may be cleared if there is too much 
+#     drift in the map.
+#
+
+# Radius in cells around empty cell without obstacles to clear underlying obstacles, default 1 cell if not set.
+int32 radius
+
+# Filter also the scans, default false if not set.
+# The filtered laser scans will be used for localization,
+# so if dynamic obstacles have been removed, localization won't try to
+# match them anymore. Filtering the laser scans cannot be reverted, 
+# but grids can (see DatabaseViewer->Edit menu).
+bool filter_scans
+
+---
+# return the number of grids or scans modified, -1 if there is an error
+int32 modified

srv/DetectMoreLoopClosures.srv

@@ -0,0 +1,27 @@
+# Detect more loop closures service
+#
+#     Based on the current optimized graph, 
+#     this process will try to find more nodes corresponding with each 
+#     other, and thus finding more loop closures to add to graph.
+# 
+
+# Cluster radius (m),     default 1 m if not set
+float32 cluster_radius_max
+
+# Cluster radius min (m), default 0 m if not set
+float32 cluster_radius_min
+
+# Cluster angle (deg),    default 0 deg if not set
+float32 cluster_angle
+
+# Iterations,             default 1 if not set
+int32 iterations
+
+# Add only intra session loop closures
+bool intra_only
+
+# Add only inter session loop closures
+bool inter_only
+---
+# return the number of loop closures detected, or -1 if it failed.
+int32 detected

srv/GlobalBundleAdjustment.srv

@@ -0,0 +1,22 @@
+# Global Bundle Adjustment service
+#
+#       Perform global bundle adjustment. Note that as soon as the map 
+#       is modified again, the graph is re-optimized the standard way (without SBA).
+#       It then makes only sense to use this after a mapping run (and after a call 
+#       to /rtabmap/pause) when you know that the robot will restart in localization 
+#       mode the next time, or at the beginning of the localization session.
+#
+
+# Optimizer type (0=g2o, 1=CVSBA), default 0
+int32 type
+
+# Iterations,        default 0 (use Optimizer/Iterations already loaded in the node)
+int32 iterations
+
+# Pixel variance,    default 0 (use g2o/PixelVariance already loaded in the node)
+float32 pixel_variance
+
+# Use vocabulary matches, default false (rematch all features between frames)
+bool voc_matches
+---
+# return false if failure