Merge branch 'master' of https://github.com/introlab/rtabmap_ros into kinetic-devel

.travis.yml

@@ -0,0 +1,46 @@
+sudo: true
+dist: trusty
+language: cpp
+
+compiler:
+  - gcc
+  - clang
+
+addons:
+  apt:
+    packages:
+      - cmake
+      - libopencv-dev
+      - libqt4-dev
+      - libsqlite3-dev
+
+install:
+    - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty 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 -y install ros-indigo-ros-base libpcl-1.7-all libfreenect-dev ros-indigo-libg2o ros-indigo-octomap libopenni2-dev ros-indigo-costmap-2d ros-indigo-octomap-ros ros-indigo-rviz ros-indigo-cv-bridge ros-indigo-move-base-msgs ros-indigo-costmap-2d ros-indigo-image-geometry ros-indigo-message-filters ros-indigo-image-transport ros-indigo-eigen-conversions ros-indigo-stereo-msgs ros-indigo-nav-msgs ros-indigo-sensor-msgs ros-indigo-tf-conversions ros-indigo-laser-geometry ros-indigo-pcl-conversions ros-indigo-pcl-ros ros-indigo-dynamic-reconfigure ros-indigo-nodelet
+
+script: 
+    - source /opt/ros/indigo/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
+    - 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
+
+notifications:
+  email:
+    - [email protected]

CMakeLists.txt

@@ -18,7 +18,7 @@ find_package(rviz)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.11.8 REQUIRED)
+find_package(RTABMap 0.11.13 REQUIRED)
 
 find_package(OpenCV REQUIRED)
 
@@ -72,6 +72,8 @@ add_message_files(
    Point2f.msg
    Point3f.msg
    Goal.msg
+   RGBDImage.msg
+   UserData.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -151,6 +153,8 @@ SET(Libraries
 SET(rtabmap_ros_lib_src
    src/nodelets/rgbd_odometry.cpp
    src/nodelets/stereo_odometry.cpp
+   src/nodelets/rgbdicp_odometry.cpp
+   src/nodelets/icp_odometry.cpp
    src/nodelets/data_throttle.cpp
    src/nodelets/stereo_throttle.cpp
    src/nodelets/data_odom_sync.cpp
@@ -158,10 +162,19 @@ SET(rtabmap_ros_lib_src
    src/nodelets/point_cloud_xyz.cpp
    src/nodelets/disparity_to_depth.cpp 
    src/nodelets/obstacles_detection.cpp
+   src/nodelets/obstacles_detection_old.cpp
    src/nodelets/point_cloud_aggregator.cpp
-   src/nodelets/OdometryROS.cpp
+   src/nodelets/undistort_depth.cpp
+   src/nodelets/rgbd_sync.cpp
+   src/OdometryROS.cpp
    src/MsgConversion.cpp
    src/MapsManager.cpp
+   src/CommonDataSubscriber.cpp
+   src/CoreWrapper.cpp
+   src/impl/CommonDataSubscriberDepth.cpp
+   src/impl/CommonDataSubscriberStereo.cpp
+   src/impl/CommonDataSubscriberRGBD.cpp
+   src/impl/CommonDataSubscriberRGBD2.cpp
 )   
 
 # If costmap_2d is found, add the plugin
@@ -259,8 +272,8 @@ IF(Qt5_FOUND)
 ENDIF(Qt5_FOUND)
 add_dependencies(rtabmap_ros ${${PROJECT_NAME}_EXPORTED_TARGETS})
 
-add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp)
-target_link_libraries(rtabmap rtabmap_ros ${Libraries})
+add_executable(rtabmap src/CoreNode.cpp)
+target_link_libraries(rtabmap ${Libraries})
 
 add_executable(rgbd_odometry src/RGBDOdometryNode.cpp)
 target_link_libraries(rgbd_odometry ${Libraries})
@@ -268,6 +281,12 @@ target_link_libraries(rgbd_odometry ${Libraries})
 add_executable(stereo_odometry src/StereoOdometryNode.cpp)
 target_link_libraries(stereo_odometry ${Libraries})
 
+add_executable(rgbdicp_odometry src/RGBDICPOdometryNode.cpp)
+target_link_libraries(rgbdicp_odometry ${Libraries})
+
+add_executable(icp_odometry src/ICPOdometryNode.cpp)
+target_link_libraries(icp_odometry ${Libraries})
+
 add_executable(map_optimizer src/MapOptimizerNode.cpp)
 target_link_libraries(map_optimizer rtabmap_ros ${Libraries})
 
@@ -318,6 +337,7 @@ install(TARGETS
    rtabmap 
    rtabmapviz 
    rgbd_odometry 
+   rgbdicp_odometry 
    stereo_odometry
    map_assembler
    map_optimizer
@@ -332,6 +352,7 @@ install(TARGETS
    rtabmap_ros 
    rtabmap 
    rgbd_odometry 
+   rgbdicp_odometry 
    stereo_odometry
    map_assembler
    map_optimizer
@@ -392,3 +413,4 @@ ENDIF(costmap_2d_FOUND)
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
+

README.md

@@ -1,4 +1,4 @@
-rtabmap_ros
+rtabmap_ros [![Build Status](https://travis-ci.org/introlab/rtabmap_ros.svg?branch=master)](https://travis-ci.org/introlab/rtabmap_ros)
 ===========
 
 RTAB-Map's ROS package.
@@ -35,7 +35,7 @@ $ export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/ros/kinetic/lib/x86_64-linux-gnu
 ```
 
 ### Build from source
-This section shows how to install RTAB-Map ros-pkg on **ROS Hydro/Indigo/Jade/Kinetic** (Catkin build). RTAB-Map works only with the PCL 1.7, which is the default version installed with ROS Hydro/Indigo/Jade/Kinetic (**Fuerte and Groovy are not supported**).
+This section shows how to install RTAB-Map ros-pkg on **ROS Hydro/Indigo/Jade/Kinetic** (Catkin build). RTAB-Map works only with the PCL >=1.7, which is the default version installed with ROS Hydro/Indigo/Jade/Kinetic (**Fuerte and Groovy are not supported**).
 
  * The next instructions assume that you have set up your ROS workspace using this [tutorial](http://wiki.ros.org/catkin/Tutorials/create_a_workspace). I will use kinetic prefix for convenience, but it should work with Hydro, Indigo and Jade. The workspace path is `~/catkin_ws` and your `~/.bashrc` contains:
  
@@ -57,7 +57,7 @@ $ sudo apt-get remove ros-kinetic-rtabmap
 $ sudo apt-get install libqt4-dev libpcl-1.7-all-dev libdc1394-dev ros-kinetic-openni-launch ros-kinetic-openni2-launch ros-kinetic-freenect-launch ros-kinetic-costmap-2d ros-kinetic-octomap-ros ros-kinetic-g2o ros-kinetic-rviz ros-kinetic-cv-bridge
 ```
 
-  * [GTSAM](https://collab.cc.gatech.edu/borg/gtsam): Follow installation instructions from [here](https://collab.cc.gatech.edu/borg/gtsam/#quickstart). RTAB-Map needs latest version from source (`git clone https://bitbucket.org/gtborg/gtsam.git`), it will not build with 3.2.1.
+  * [GTSAM](https://collab.cc.gatech.edu/borg/gtsam): Follow installation instructions from [here](https://collab.cc.gatech.edu/borg/gtsam/#quickstart). RTAB-Map needs latest version from source (`git clone https://bitbucket.org/gtborg/gtsam.git`), it will **not build** with 3.2.1.
   
   * [cvsba](http://www.uco.es/investiga/grupos/ava/node/39): Follow installation instructions from [here](http://www.uco.es/investiga/grupos/ava/node/39). Their installation is not standard CMake, you need these extra steps so RTAB-Map can find it:
     ```bash
@@ -94,6 +94,7 @@ $ git pull origin master
 $ cd build
 $ make
 $ make install
+# Do "sudo make install" if you installed rtabmap in "/usr/local"
 
 $ roscd rtabmap_ros
 $ git pull origin master

include/rtabmap_ros/CommonDataSubscriber.h

@@ -0,0 +1,264 @@
+/*
+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.
+*/
+
+#ifndef INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBER_H_
+#define INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBER_H_
+
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <cv_bridge/cv_bridge.h>
+
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/LaserScan.h>
+
+#include <nav_msgs/Odometry.h>
+
+#include <rtabmap_ros/RGBDImage.h>
+#include <rtabmap_ros/UserData.h>
+#include <rtabmap_ros/OdomInfo.h>
+#include <rtabmap_ros/CommonDataSubscriberDefines.h>
+
+#include <boost/thread.hpp>
+
+namespace rtabmap_ros {
+
+class CommonDataSubscriber {
+public:
+	CommonDataSubscriber(bool gui);
+	virtual ~CommonDataSubscriber();
+
+	bool isSubscribedToDepth() const  {return subscribedToDepth_;}
+	bool isSubscribedToStereo() const {return subscribedToStereo_;}
+	bool isSubscribedToRGBD() const   {return subscribedToRGBD_;}
+	bool isSubscribedToScan2d() const {return subscribedToScan2d_;}
+	bool isSubscribedToScan3d() const {return subscribedToScan3d_;}
+	bool isSubscribedToOdomInfo() const {return subscribedToOdomInfo_;}
+	bool isDataSubscribed() const {return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD();}
+	int getQueueSize() const {return queueSize_;}
+
+protected:
+	void setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::string & name);
+	virtual void commonDepthCallback(
+				const nav_msgs::OdometryConstPtr & odomMsg,
+				const rtabmap_ros::UserDataConstPtr & userDataMsg,
+				const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
+				const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
+				const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
+				const sensor_msgs::LaserScanConstPtr& scanMsg,
+				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) = 0;
+	virtual void commonStereoCallback(
+				const nav_msgs::OdometryConstPtr & odomMsg,
+				const sensor_msgs::ImageConstPtr& leftImageMsg,
+				const sensor_msgs::ImageConstPtr& rightImageMsg,
+				const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+				const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+				const sensor_msgs::LaserScanConstPtr& scanMsg,
+				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg) = 0;
+
+	void commonSingleDepthCallback(
+				const nav_msgs::OdometryConstPtr & odomMsg,
+				const rtabmap_ros::UserDataConstPtr & userDataMsg,
+				const cv_bridge::CvImageConstPtr & imageMsg,
+				const cv_bridge::CvImageConstPtr & depthMsg,
+				const sensor_msgs::CameraInfo & cameraInfoMsg,
+				const sensor_msgs::LaserScanConstPtr& scanMsg,
+				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
+
+private:
+	void warningLoop();
+	void callbackCalled() {callbackCalled_ = true;}
+	void setupDepthCallbacks(
+			ros::NodeHandle & nh,
+			ros::NodeHandle & pnh,
+			bool subscribeOdom,
+			bool subscribeUserData,
+			bool subscribeScan2d,
+			bool subscribeScan3d,
+			bool subscribeOdomInfo,
+			int queueSize,
+			bool approxSync);
+	void setupStereoCallbacks(
+			ros::NodeHandle & nh,
+			ros::NodeHandle & pnh,
+			bool subscribeOdom,
+			bool subscribeOdomInfo,
+			int queueSize,
+			bool approxSync);
+	void setupRGBDCallbacks(
+			ros::NodeHandle & nh,
+			ros::NodeHandle & pnh,
+			bool subscribeOdom,
+			bool subscribeUserData,
+			bool subscribeScan2d,
+			bool subscribeScan3d,
+			bool subscribeOdomInfo,
+			int queueSize,
+			bool approxSync);
+	void setupRGBD2Callbacks(
+			ros::NodeHandle & nh,
+			ros::NodeHandle & pnh,
+			bool subscribeOdom,
+			bool subscribeUserData,
+			bool subscribeScan2d,
+			bool subscribeScan3d,
+			bool subscribeOdomInfo,
+			int queueSize,
+			bool approxSync);
+
+protected:
+	std::string subscribedTopicsMsg_;
+	int queueSize_;
+
+private:
+	bool approxSync_;
+	boost::thread* warningThread_;
+	bool callbackCalled_;
+	bool subscribedToDepth_;
+	bool subscribedToStereo_;
+	bool subscribedToRGBD_;
+	bool subscribedToScan2d_;
+	bool subscribedToScan3d_;
+	bool subscribedToOdomInfo_;
+	std::string name_;
+
+	//for depth callback
+	image_transport::SubscriberFilter imageSub_;
+	image_transport::SubscriberFilter imageDepthSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+
+	//for rgbd callback
+	ros::Subscriber rgbdSub_;
+	std::vector<message_filters::Subscriber<rtabmap_ros::RGBDImage>*> rgbdSubs_;
+
+	//stereo callback
+	image_transport::SubscriberFilter imageRectLeft_;
+	image_transport::SubscriberFilter imageRectRight_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
+
+	message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
+	message_filters::Subscriber<rtabmap_ros::UserData> userDataSub_;
+	message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
+	message_filters::Subscriber<sensor_msgs::PointCloud2> scan3dSub_;
+	message_filters::Subscriber<rtabmap_ros::OdomInfo> odomInfoSub_;
+
+	// RGB + Depth
+	DATA_SYNCS3(depth, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo);
+	DATA_SYNCS4(depthScan2d, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
+	DATA_SYNCS4(depthScan3d, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2);
+	DATA_SYNCS4(depthInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
+
+	// RGB + Depth + Odom
+	DATA_SYNCS4(depthOdom, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo);
+	DATA_SYNCS5(depthOdomScan2d, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
+	DATA_SYNCS5(depthOdomScan3d, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2);
+	DATA_SYNCS5(depthOdomInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
+
+	// RGB + Depth + User Data
+	DATA_SYNCS4(depthData, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo);
+	DATA_SYNCS5(depthDataScan2d, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
+	DATA_SYNCS5(depthDataScan3d, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2);
+	DATA_SYNCS5(depthDataInfo, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
+
+	// RGB + Depth + Odom + User Data
+	DATA_SYNCS5(depthOdomData, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo);
+	DATA_SYNCS6(depthOdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan);
+	DATA_SYNCS6(depthOdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2);
+	DATA_SYNCS6(depthOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
+
+	// Stereo
+	DATA_SYNCS4(stereo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo);
+	DATA_SYNCS5(stereoInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
+
+	// Stereo + Odom
+	DATA_SYNCS5(stereoOdom, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo);
+	DATA_SYNCS6(stereoOdomInfo, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo, rtabmap_ros::OdomInfo);
+
+	// 1 RGBD
+	void rgbdCallback(const rtabmap_ros::RGBDImageConstPtr&);
+	DATA_SYNCS2(rgbdScan2d, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
+	DATA_SYNCS2(rgbdScan3d, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS2(rgbdInfo, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
+
+	// 1 RGBD + Odom
+	DATA_SYNCS2(rgbdOdom, nav_msgs::Odometry, rtabmap_ros::RGBDImage);
+	DATA_SYNCS3(rgbdOdomScan2d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
+	DATA_SYNCS3(rgbdOdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS3(rgbdOdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
+
+	// 1 RGBD + User Data
+	DATA_SYNCS2(rgbdData, rtabmap_ros::UserData, rtabmap_ros::RGBDImage);
+	DATA_SYNCS3(rgbdDataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
+	DATA_SYNCS3(rgbdDataScan3d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS3(rgbdDataInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
+
+	// 1 RGBD + Odom + User Data
+	DATA_SYNCS3(rgbdOdomData, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage);
+	DATA_SYNCS4(rgbdOdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
+	DATA_SYNCS4(rgbdOdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS4(rgbdOdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
+
+	// 2 RGBD
+	DATA_SYNCS2(rgbd2, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
+	DATA_SYNCS3(rgbd2Scan2d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
+	DATA_SYNCS3(rgbd2Scan3d, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS3(rgbd2Info, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
+
+	// 2 RGBD + Odom
+	DATA_SYNCS3(rgbd2Odom, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
+	DATA_SYNCS4(rgbd2OdomScan2d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
+	DATA_SYNCS4(rgbd2OdomScan3d, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS4(rgbd2OdomInfo, nav_msgs::Odometry, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
+
+	// 2 RGBD + User Data
+	DATA_SYNCS3(rgbd2Data, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
+	DATA_SYNCS4(rgbd2DataScan2d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
+	DATA_SYNCS4(rgbd2DataScan3d, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS4(rgbd2DataInfo, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
+
+	// 2 RGBD + Odom + User Data
+	DATA_SYNCS4(rgbd2OdomData, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
+	DATA_SYNCS5(rgbd2OdomDataScan2d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::LaserScan);
+	DATA_SYNCS5(rgbd2OdomDataScan3d, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, sensor_msgs::PointCloud2);
+	DATA_SYNCS5(rgbd2OdomDataInfo, nav_msgs::Odometry, rtabmap_ros::UserData, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::OdomInfo);
+
+};
+
+} /* namespace rtabmap_ros */
+
+#endif /* INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBER_H_ */

include/rtabmap_ros/CommonDataSubscriberDefines.h

@@ -0,0 +1,195 @@
+/*
+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.
+*/
+
+#ifndef INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_
+#define INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_
+
+#include <rtabmap/utilite/UConversion.h>
+
+#define DATA_SYNC2(PREFIX, SYNC_NAME, MSG0, MSG1) \
+	typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1> PREFIX##SYNC_NAME##SyncPolicy; \
+	message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
+
+#define DATA_SYNCS2(PREFIX, MSG0, MSG1) \
+		DATA_SYNC2(PREFIX, Approximate, MSG0, MSG1) \
+		DATA_SYNC2(PREFIX, Exact, MSG0, MSG1) \
+		void PREFIX##Callback(const MSG0##ConstPtr&, const MSG1##ConstPtr&);
+
+#define DATA_SYNC3(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2) \
+		typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2> PREFIX##SYNC_NAME##SyncPolicy; \
+		message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
+
+#define DATA_SYNCS3(PREFIX, MSG0, MSG1, MSG2) \
+		DATA_SYNC3(PREFIX, Approximate, MSG0, MSG1, MSG2) \
+		DATA_SYNC3(PREFIX, Exact, MSG0, MSG1, MSG2) \
+		void PREFIX##Callback(const MSG0##ConstPtr&, const MSG1##ConstPtr&, const MSG2##ConstPtr&); \
+
+#define DATA_SYNC4(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3) \
+		typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2, MSG3> PREFIX##SYNC_NAME##SyncPolicy; \
+		message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
+
+#define DATA_SYNCS4(PREFIX, MSG0, MSG1, MSG2, MSG3) \
+		DATA_SYNC4(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3) \
+		DATA_SYNC4(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3) \
+		void PREFIX##Callback(const MSG0##ConstPtr&, const MSG1##ConstPtr&, const MSG2##ConstPtr&, const MSG3##ConstPtr&); \
+
+#define DATA_SYNC5(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3, MSG4) \
+		typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2, MSG3, MSG4> PREFIX##SYNC_NAME##SyncPolicy; \
+		message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
+
+#define DATA_SYNCS5(PREFIX, MSG0, MSG1, MSG2, MSG3, MSG4) \
+		DATA_SYNC5(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3, MSG4) \
+		DATA_SYNC5(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3, MSG4) \
+		void PREFIX##Callback(const MSG0##ConstPtr&, const MSG1##ConstPtr&, const MSG2##ConstPtr&, const MSG3##ConstPtr&, const MSG4##ConstPtr&); \
+
+#define DATA_SYNC6(PREFIX, SYNC_NAME, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \
+		typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1, MSG2, MSG3, MSG4, MSG5> PREFIX##SYNC_NAME##SyncPolicy; \
+		message_filters::Synchronizer<PREFIX##SYNC_NAME##SyncPolicy> * PREFIX##SYNC_NAME##Sync_;
+
+#define DATA_SYNCS6(PREFIX, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \
+		DATA_SYNC6(PREFIX, Approximate, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \
+		DATA_SYNC6(PREFIX, Exact, MSG0, MSG1, MSG2, MSG3, MSG4, MSG5) \
+		void PREFIX##Callback(const MSG0##ConstPtr&, const MSG1##ConstPtr&, const MSG2##ConstPtr&, const MSG3##ConstPtr&, const MSG4##ConstPtr&, const MSG5##ConstPtr&); \
+
+// Constructor
+#define SYNC_INIT(PREFIX) \
+	PREFIX##ApproximateSync_(0), \
+	PREFIX##ExactSync_(0)
+
+// Destructor
+#define SYNC_DEL(PREFIX) \
+	if(PREFIX##ApproximateSync_) delete PREFIX##ApproximateSync_; \
+	if(PREFIX##ExactSync_) delete PREFIX##ExactSync_;
+
+// Sync declarations
+#define SYNC_DECL2(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)); \
+		} \
+		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)); \
+		} \
+		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s", \
+				name_.c_str(), \
+				APPROX?"approx":"exact", \
+				SUB0.getTopic().c_str(), \
+				SUB1.getTopic().c_str());
+
+#define SYNC_DECL3(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)); \
+		} \
+		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)); \
+		} \
+		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s", \
+				name_.c_str(), \
+				APPROX?"approx":"exact", \
+				SUB0.getTopic().c_str(), \
+				SUB1.getTopic().c_str(), \
+				SUB2.getTopic().c_str());
+
+#define SYNC_DECL4(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)); \
+		} \
+		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)); \
+		} \
+		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s", \
+				name_.c_str(), \
+				APPROX?"approx":"exact", \
+				SUB0.getTopic().c_str(), \
+				SUB1.getTopic().c_str(), \
+				SUB2.getTopic().c_str(), \
+				SUB3.getTopic().c_str());
+
+#define SYNC_DECL5(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)); \
+		} \
+		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)); \
+		} \
+		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s,\n   %s", \
+				name_.c_str(), \
+				approxSync?"approx":"exact", \
+				SUB0.getTopic().c_str(), \
+				SUB1.getTopic().c_str(), \
+				SUB2.getTopic().c_str(), \
+				SUB3.getTopic().c_str(), \
+				SUB4.getTopic().c_str());
+
+#define SYNC_DECL6(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)); \
+		} \
+		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)); \
+		} \
+		subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s", \
+				name_.c_str(), \
+				APPROX?"approx":"exact", \
+				SUB0.getTopic().c_str(), \
+				SUB1.getTopic().c_str(), \
+				SUB2.getTopic().c_str(), \
+				SUB3.getTopic().c_str(), \
+				SUB4.getTopic().c_str(), \
+				SUB5.getTopic().c_str());
+
+
+#endif /* INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_ */

include/rtabmap_ros/CoreWrapper.h

@@ -0,0 +1,264 @@
+/*
+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.
+*/
+
+#ifndef COREWRAPPER_H_
+#define COREWRAPPER_H_
+
+
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+
+#include <std_srvs/Empty.h>
+
+#include <tf/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+
+#include <std_msgs/Empty.h>
+#include <std_msgs/Int32.h>
+#include <nav_msgs/GetMap.h>
+
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Rtabmap.h>
+
+#include "rtabmap_ros/GetMap.h"
+#include "rtabmap_ros/ListLabels.h"
+#include "rtabmap_ros/PublishMap.h"
+#include "rtabmap_ros/SetGoal.h"
+#include "rtabmap_ros/SetLabel.h"
+#include "rtabmap_ros/Goal.h"
+#include "rtabmap_ros/CommonDataSubscriber.h"
+
+#include "MapsManager.h"
+
+#ifdef WITH_OCTOMAP_ROS
+#include <octomap_msgs/GetOctomap.h>
+#endif
+
+#include <actionlib/client/simple_action_client.h>
+#include <move_base_msgs/MoveBaseAction.h>
+#include <move_base_msgs/MoveBaseActionGoal.h>
+#include <move_base_msgs/MoveBaseActionResult.h>
+#include <move_base_msgs/MoveBaseActionFeedback.h>
+#include <actionlib_msgs/GoalStatusArray.h>
+typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
+
+namespace rtabmap {
+class StereoDense;
+}
+
+namespace rtabmap_ros {
+
+class CoreWrapper : public CommonDataSubscriber, public nodelet::Nodelet
+{
+public:
+	CoreWrapper();
+	virtual ~CoreWrapper();
+
+private:
+
+	virtual void onInit();
+
+	bool odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg);
+	bool odomTFUpdate(const ros::Time & stamp); // TF odom
+
+	virtual void commonDepthCallback(
+				const nav_msgs::OdometryConstPtr & odomMsg,
+				const rtabmap_ros::UserDataConstPtr & userDataMsg,
+				const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
+				const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
+				const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
+				const sensor_msgs::LaserScanConstPtr& scanMsg,
+				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
+	void commonDepthCallbackImpl(
+			const std::string & odomFrameId,
+			const rtabmap_ros::UserDataConstPtr & userDataMsg,
+			const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
+			const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
+			const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
+			const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+			const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
+	virtual void commonStereoCallback(
+				const nav_msgs::OdometryConstPtr & odomMsg,
+				const sensor_msgs::ImageConstPtr& leftImageMsg,
+				const sensor_msgs::ImageConstPtr& rightImageMsg,
+				const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+				const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+				const sensor_msgs::LaserScanConstPtr& scanMsg,
+				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+				const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
+
+	void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
+
+	void goalCommonCallback(int id, const std::string & label, const rtabmap::Transform & pose, const ros::Time & stamp, double * planningTime = 0);
+	void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
+	void goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg);
+	void updateGoal(const ros::Time & stamp);
+
+	void process(
+			const ros::Time & stamp,
+			const rtabmap::SensorData & data,
+			const rtabmap::Transform & odom = rtabmap::Transform(),
+			const std::string & odomFrameId = "",
+			float odomRotationalVariance = 1.0,
+			float odomTransitionalVariance = 1.0);
+
+	bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Empty::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&);
+	bool setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool getMapDataCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res);
+	bool getMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
+	bool getProjMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
+	bool getGridMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
+	bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&);
+	bool setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res);
+	bool cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
+	bool setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res);
+	bool listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtabmap_ros::ListLabels::Response& res);
+#ifdef WITH_OCTOMAP_ROS
+	bool octomapBinaryCallback(octomap_msgs::GetOctomap::Request  &req, octomap_msgs::GetOctomap::Response &res);
+	bool octomapFullCallback(octomap_msgs::GetOctomap::Request  &req, octomap_msgs::GetOctomap::Response &res);
+#endif
+
+	void loadParameters(const std::string & configFile, rtabmap::ParametersMap & parameters);
+	void saveParameters(const std::string & configFile);
+
+	void publishLoop(double tfDelay, double tfTolerance);
+
+	void publishStats(const ros::Time & stamp);
+	void publishCurrentGoal(const ros::Time & stamp);
+	void goalDoneCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result);
+	void goalActiveCb();
+	void goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback);
+	void publishLocalPath(const ros::Time & stamp);
+	void publishGlobalPath(const ros::Time & stamp);
+
+private:
+	rtabmap::Rtabmap rtabmap_;
+	bool paused_;
+	rtabmap::Transform lastPose_;
+	ros::Time lastPoseStamp_;
+	bool lastPoseIntermediate_;
+	float rotVariance_;
+	float transVariance_;
+	rtabmap::Transform currentMetricGoal_;
+	bool latestNodeWasReached_;
+	rtabmap::ParametersMap parameters_;
+
+	std::string frameId_;
+	std::string odomFrameId_;
+	std::string mapFrameId_;
+	std::string groundTruthFrameId_;
+	std::string groundTruthBaseFrameId_;
+	std::string configPath_;
+	std::string databasePath_;
+	bool waitForTransform_;
+	double waitForTransformDuration_;
+	bool useActionForGoal_;
+	bool genScan_;
+	double genScanMaxDepth_;
+	double genScanMinDepth_;
+	int scanCloudMaxPoints_;
+	int scanCloudNormalK_;
+
+	rtabmap::Transform mapToOdom_;
+	boost::mutex mapToOdomMutex_;
+
+	MapsManager mapsManager_;
+
+	ros::Publisher infoPub_;
+	ros::Publisher mapDataPub_;
+	ros::Publisher mapGraphPub_;
+	ros::Publisher labelsPub_;
+
+	//Planning stuff
+	ros::Subscriber goalSub_;
+	ros::Subscriber goalNodeSub_;
+	ros::Publisher nextMetricGoalPub_;
+	ros::Publisher goalReachedPub_;
+	ros::Publisher globalPathPub_;
+	ros::Publisher localPathPub_;
+
+	tf2_ros::TransformBroadcaster tfBroadcaster_;
+	tf::TransformListener tfListener_;
+
+	ros::ServiceServer updateSrv_;
+	ros::ServiceServer resetSrv_;
+	ros::ServiceServer pauseSrv_;
+	ros::ServiceServer resumeSrv_;
+	ros::ServiceServer triggerNewMapSrv_;
+	ros::ServiceServer backupDatabase_;
+	ros::ServiceServer setModeLocalizationSrv_;
+	ros::ServiceServer setModeMappingSrv_;
+	ros::ServiceServer setLogDebugSrv_;
+	ros::ServiceServer setLogInfoSrv_;
+	ros::ServiceServer setLogWarnSrv_;
+	ros::ServiceServer setLogErrorSrv_;
+	ros::ServiceServer getMapDataSrv_;
+	ros::ServiceServer getProjMapSrv_;
+	ros::ServiceServer getMapSrv_;
+	ros::ServiceServer getGridMapSrv_;
+	ros::ServiceServer publishMapDataSrv_;
+	ros::ServiceServer setGoalSrv_;
+	ros::ServiceServer cancelGoalSrv_;
+	ros::ServiceServer setLabelSrv_;
+	ros::ServiceServer listLabelsSrv_;
+#ifdef WITH_OCTOMAP_ROS
+	ros::ServiceServer octomapBinarySrv_;
+	ros::ServiceServer octomapFullSrv_;
+#endif
+
+	MoveBaseClient mbClient_;
+
+	boost::thread* transformThread_;
+	bool tfThreadRunning_;
+
+	// for loop closure detection only
+	image_transport::Subscriber defaultSub_;
+
+	bool stereoToDepth_;
+	bool odomSensorSync_;
+	float rate_;
+	bool createIntermediateNodes_;
+	ros::Time time_;
+	ros::Time previousStamp_;
+};
+
+}
+
+#endif /* COREWRAPPER_H_ */
+

include/rtabmap_ros/GuiWrapper.h

@@ -0,0 +1,128 @@
+/*
+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.
+*/
+
+#ifndef GUIWRAPPER_H_
+#define GUIWRAPPER_H_
+
+#include <ros/ros.h>
+#include "rtabmap_ros/Info.h"
+#include "rtabmap_ros/MapData.h"
+#include "rtabmap_ros/OdomInfo.h"
+#include "rtabmap_ros/Goal.h"
+#include "rtabmap/utilite/UEventsHandler.h"
+#include "rtabmap/core/Transform.h"
+
+#include <tf/transform_listener.h>
+
+#include <geometry_msgs/TwistStamped.h>
+#include <nav_msgs/Path.h>
+#include <std_msgs/Bool.h>
+
+#include <rtabmap_ros/CommonDataSubscriber.h>
+
+namespace rtabmap
+{
+	class MainWindow;
+}
+
+class QApplication;
+
+namespace rtabmap_ros {
+
+class GuiWrapper : public UEventsHandler, public CommonDataSubscriber
+{
+public:
+	GuiWrapper(int & argc, char** argv);
+	virtual ~GuiWrapper();
+
+protected:
+	virtual void handleEvent(UEvent * anEvent);
+
+private:
+	void infoMapCallback(const rtabmap_ros::InfoConstPtr & infoMsg, const rtabmap_ros::MapDataConstPtr & mapMsg);
+	void goalPathCallback(const rtabmap_ros::GoalConstPtr & goalMsg, const nav_msgs::PathConstPtr & pathMsg);
+	void goalReachedCallback(const std_msgs::BoolConstPtr & value);
+
+	virtual void commonDepthCallback(
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const rtabmap_ros::UserDataConstPtr & userDataMsg,
+			const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
+			const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
+			const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
+			const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+			const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
+	virtual void commonStereoCallback(
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const sensor_msgs::ImageConstPtr& leftImageMsg,
+			const sensor_msgs::ImageConstPtr& rightImageMsg,
+			const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+			const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+			const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
+
+	void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg);
+
+	void processRequestedMap(const rtabmap_ros::MapData & map);
+
+private:
+	rtabmap::MainWindow * mainWindow_;
+	std::string cameraNodeName_;
+	double lastOdomInfoUpdateTime_;
+
+	// odometry subscription stuffs
+	std::string frameId_;
+	std::string odomFrameId_;
+	bool waitForTransform_;
+	double waitForTransformDuration_;
+	bool odomSensorSync_;
+	tf::TransformListener tfListener_;
+
+	message_filters::Subscriber<rtabmap_ros::Info> infoTopic_;
+	message_filters::Subscriber<rtabmap_ros::MapData> mapDataTopic_;
+
+	message_filters::Subscriber<rtabmap_ros::Goal> goalTopic_;
+	message_filters::Subscriber<nav_msgs::Path> pathTopic_;
+	ros::Subscriber goalReachedTopic_;
+
+	ros::Subscriber defaultSub_; // odometry only
+
+	typedef message_filters::sync_policies::ExactTime<
+			rtabmap_ros::Info,
+			rtabmap_ros::MapData> MyInfoMapSyncPolicy;
+	message_filters::Synchronizer<MyInfoMapSyncPolicy> * infoMapSync_;
+
+	typedef message_filters::sync_policies::ExactTime<
+			rtabmap_ros::Goal,
+			nav_msgs::Path> MyGoalPathSyncPolicy;
+	message_filters::Synchronizer<MyGoalPathSyncPolicy> * goalPathSync_;
+};
+
+}
+
+#endif /* GUIWRAPPER_H_ */

include/rtabmap_ros/MapsManager.h

@@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define MAPSMANAGER_H_
 
 #include <rtabmap/core/Signature.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/FlannIndex.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <ros/time.h>
@@ -37,15 +39,19 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap {
 class OctoMap;
 class Memory;
+class OccupancyGrid;
 
 }  // namespace rtabmap
 
 class MapsManager {
 public:
-	MapsManager(bool usePublicNamespace);
+	MapsManager();
 	virtual ~MapsManager();
+	void init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::string & name, bool usePublicNamespace);
 	void clear();
 	bool hasSubscribers() const;
+	void backwardCompatibilityParameters(ros::NodeHandle & pnh, rtabmap::ParametersMap & parameters) const;
+	void setParameters(const rtabmap::ParametersMap & parameters);
 
 	std::map<int, rtabmap::Transform> getFilteredPoses(
 			const std::map<int, rtabmap::Transform> & poses);
@@ -53,10 +59,7 @@ public:
 	std::map<int, rtabmap::Transform> updateMapCaches(
 			const std::map<int, rtabmap::Transform> & poses,
 			const rtabmap::Memory * memory,
-			bool updateCloud,
-			bool updateProj,
 			bool updateGrid,
-			bool updateScan,
 			bool updateOctomap,
 			const std::map<int, rtabmap::Signature> & signatures = std::map<int, rtabmap::Signature>());
 
@@ -65,52 +68,33 @@ public:
 			const ros::Time & stamp,
 			const std::string & mapFrameId);
 
-	cv::Mat generateProjMap(
-			const std::map<int, rtabmap::Transform> & filteredPoses,
-			float & xMin,
-			float & yMin,
-			float & gridCellSize);
-
 	cv::Mat generateGridMap(
 			const std::map<int, rtabmap::Transform> & filteredPoses,
 			float & xMin,
 			float & yMin,
 			float & gridCellSize);
 
-	rtabmap::OctoMap * getOctomap() const {return octomap_;}
+	const rtabmap::OctoMap * getOctomap() const {return octomap_;}
+	const rtabmap::OccupancyGrid * getOccupancyGrid() const {return occupancyGrid_;}
 
 private:
 	// mapping stuff
-	int cloudDecimation_;
-	double cloudMaxDepth_;
-	double cloudMinDepth_;
-	double cloudVoxelSize_;
-	double cloudFloorCullingHeight_;
-	double cloudCeilingCullingHeight_;
 	bool cloudOutputVoxelized_;
-	bool cloudFrustumCulling_;
-	double cloudNoiseFilteringRadius_;
-	int cloudNoiseFilteringMinNeighbors_;
-	int scanDecimation_;
-	double scanVoxelSize_;
-	bool scanOutputVoxelized_;
-	double projMaxGroundAngle_;
-	int projMinClusterSize_;
-	double projMaxObstaclesHeight_;
-	double projMaxGroundHeight_;
-	bool projDetectFlatObstacles_;
-	bool projMapFrame_;
+	bool cloudSubtractFiltering_;
+	int cloudSubtractFilteringMinNeighbors_;
 	double gridCellSize_;
+	bool gridIncremental_;
 	double gridSize_;
 	bool gridEroded_;
-	bool gridUnknownSpaceFilled_;
-	double gridMaxUnknownSpaceFilledRange_;
+	double footprintRadius_;
 	double mapFilterRadius_;
 	double mapFilterAngle_;
 	bool mapCacheCleanup_;
 	bool negativePosesIgnored_;
 
 	ros::Publisher cloudMapPub_;
+	ros::Publisher cloudGroundPub_;
+	ros::Publisher cloudObstaclesPub_;
 	ros::Publisher projMapPub_;
 	ros::Publisher gridMapPub_;
 	ros::Publisher scanMapPub_;
@@ -120,15 +104,27 @@ private:
 	ros::Publisher octoMapEmptySpace_;
 	ros::Publisher octoMapProj_;
 
-	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds_;
-	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans_;
-	std::map<int, std::vector<rtabmap::CameraModel> > cameraModels_;
-	std::map<int, std::pair<cv::Mat, cv::Mat> > projMaps_; // <ground, obstacles>
+	std::map<int, rtabmap::Transform> assembledGroundPoses_;
+	std::map<int, rtabmap::Transform> assembledObstaclePoses_;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledObstacles_;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledGround_;
+	rtabmap::FlannIndex assembledGroundIndex_;
+	rtabmap::FlannIndex assembledObstacleIndex_;
+	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > groundClouds_;
+	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > obstacleClouds_;
+
+	std::map<int, rtabmap::Transform> gridPoses_;
+	cv::Mat gridMap_;
 	std::map<int, std::pair<cv::Mat, cv::Mat> > gridMaps_; // <ground, obstacles>
+	std::map<int, cv::Point3f> gridMapsViewpoints_;
+
+	rtabmap::OccupancyGrid * occupancyGrid_;
 
 	rtabmap::OctoMap * octomap_;
 	int octomapTreeDepth_;
-	bool octomapGroundIsObstacle_;
+	double octomapOccupancyThr_;
+
+	rtabmap::ParametersMap parameters_;
 };
 
 #endif /* MAPSMANAGER_H_ */

include/rtabmap_ros/MsgConversion.h

@@ -29,12 +29,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define MSGCONVERSION_H_
 
 #include <tf/tf.h>
+#include <tf/transform_listener.h>
 #include <geometry_msgs/Transform.h>
 #include <geometry_msgs/Pose.h>
 #include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/LaserScan.h>
+#include <sensor_msgs/Image.h>
 
 #include <opencv2/opencv.hpp>
 #include <opencv2/features2d/features2d.hpp>
+#include <cv_bridge/cv_bridge.h>
 
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/Link.h>
@@ -52,6 +56,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_ros/NodeData.h>
 #include <rtabmap_ros/OdomInfo.h>
 #include <rtabmap_ros/Info.h>
+#include <rtabmap_ros/RGBDImage.h>
+#include <rtabmap_ros/UserData.h>
 
 namespace rtabmap_ros {
 
@@ -64,6 +70,9 @@ rtabmap::Transform transformFromGeometryMsg(const geometry_msgs::Transform & msg
 void transformToPoseMsg(const rtabmap::Transform & transform, geometry_msgs::Pose & msg);
 rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg);
 
+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);
+
 // copy data
 void compressedMatToBytes(const cv::Mat & compressed, std::vector<unsigned char> & bytes);
 cv::Mat compressedMatFromBytes(const std::vector<unsigned char> & bytes, bool copy = true);
@@ -137,8 +146,78 @@ void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg);
 void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg);
 
+cv::Mat userDataFromROS(const rtabmap_ros::UserData & dataMsg);
+void userDataToROS(const cv::Mat & data, rtabmap_ros::UserData & dataMsg, bool compress);
+
 inline double timestampFromROS(const ros::Time & stamp) {return double(stamp.sec) + double(stamp.nsec)/1000000000.0;}
 
+// common stuff
+rtabmap::Transform getTransform(
+		const std::string & fromFrameId,
+		const std::string & toFrameId,
+		const ros::Time & stamp,
+		tf::TransformListener & listener,
+		double waitForTransform);
+
+
+// get moving transform accordingly to a fixed frame. For example get
+// transform of /base_link between two stamps accordingly to /odom frame.
+rtabmap::Transform getTransform(
+		const std::string & sourceTargetFrame,
+		const std::string & fixedFrame,
+		const ros::Time & stampSource,
+		const ros::Time & stampTarget,
+		tf::TransformListener & listener,
+		double waitForTransform);
+
+bool convertRGBDMsgs(
+		const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
+		const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
+		const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
+		const std::string & frameId,
+		const std::string & odomFrameId,
+		const ros::Time & odomStamp,
+		cv::Mat & rgb,
+		cv::Mat & depth,
+		std::vector<rtabmap::CameraModel> & cameraModels,
+		tf::TransformListener & listener,
+		double waitForTransform);
+
+bool convertStereoMsg(
+		const sensor_msgs::ImageConstPtr& leftImageMsg,
+		const sensor_msgs::ImageConstPtr& rightImageMsg,
+		const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+		const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+		const std::string & frameId,
+		const std::string & odomFrameId,
+		const ros::Time & odomStamp,
+		cv::Mat & left,
+		cv::Mat & right,
+		rtabmap::StereoCameraModel & stereoModel,
+		tf::TransformListener & listener,
+		double waitForTransform);
+
+bool convertScanMsg(
+		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+		const std::string & frameId,
+		const std::string & odomFrameId,
+		const ros::Time & odomStamp,
+		cv::Mat & scan,
+		rtabmap::Transform & scanLocalTransform,
+		tf::TransformListener & listener,
+		double waitForTransform);
+
+bool convertScan3dMsg(
+		const sensor_msgs::PointCloud2ConstPtr & scan3dMsg,
+		const std::string & frameId,
+		const std::string & odomFrameId,
+		const ros::Time & odomStamp,
+		int scanCloudNormalK,
+		cv::Mat & scan,
+		rtabmap::Transform & scanLocalTransform,
+		tf::TransformListener & listener,
+		double waitForTransform);
+
 }
 
 #endif /* MSGCONVERSION_H_ */

include/rtabmap_ros/OdometryROS.h

@@ -41,6 +41,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/SensorData.h>
 #include <rtabmap/core/Parameters.h>
 
+#include <boost/thread.hpp>
+
 namespace rtabmap {
 class Odometry;
 }
@@ -51,7 +53,7 @@ class OdometryROS : public nodelet::Nodelet
 {
 
 public:
-	OdometryROS(bool stereo);
+	OdometryROS(bool stereoParams, bool visParams, bool icpParams);
 	virtual ~OdometryROS();
 
 	void processData(const rtabmap::SensorData & data, const ros::Time & stamp);
@@ -68,25 +70,32 @@ public:
 	const std::string & frameId() const {return frameId_;}
 	const std::string & odomFrameId() const {return odomFrameId_;}
 	const rtabmap::ParametersMap & parameters() const {return parameters_;}
-	const tf::TransformListener & tfListener() const {return tfListener_;}
 	bool isPaused() const {return paused_;}
-	bool isOdometryF2M() const;
 	rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
 
 protected:
+	void startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync);
+	void callbackCalled() {callbackCalled_ = true;}
+
 	virtual void flushCallbacks() = 0;
+	tf::TransformListener & tfListener() {return tfListener_;}
 
 private:
+	void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync);
 	virtual void onInit();
 	virtual void onOdomInit() = 0;
+	virtual void updateParameters(rtabmap::ParametersMap & parameters) {}
 
 private:
 	rtabmap::Odometry * odometry_;
+	boost::thread * warningThread_;
+	bool callbackCalled_;
 
 	// parameters
 	std::string frameId_;
 	std::string odomFrameId_;
 	std::string groundTruthFrameId_;
+	std::string guessFrameId_;
 	bool publishTf_;
 	bool waitForTransform_;
 	double waitForTransformDuration_;
@@ -97,6 +106,7 @@ private:
 	ros::Publisher odomPub_;
 	ros::Publisher odomInfoPub_;
 	ros::Publisher odomLocalMap_;
+	ros::Publisher odomLocalScanMap_;
 	ros::Publisher odomLastFrame_;
 	ros::ServiceServer resetSrv_;
 	ros::ServiceServer resetToPoseSrv_;
@@ -112,7 +122,9 @@ private:
 	bool paused_;
 	int resetCountdown_;
 	int resetCurrentCount_;
-	bool stereo_;
+	bool stereoParams_;
+	bool visParams_;
+	bool icpParams_;
 };
 
 }

launch/azimut3/az3_mapping_robot_stereo_nav.launch

@@ -28,7 +28,7 @@
     	  <rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
      	  <rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
     	  <rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params_3d.yaml" command="load" />
-    	  <rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml" command="load" />
+    	  <rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml" command="load" ns="global_costmap"/>
     	  <rosparam file="$(find rtabmap_ros)/launch/azimut3/config/base_local_planner_params.yaml" command="load" />
    		</node>
    		

launch/config/demo_stereo_outdoor.rviz

@@ -180,7 +180,7 @@ Visualization Manager:
       Draw Behind: false
       Enabled: true
       Name: Map
-      Topic: /rtabmap/proj_map
+      Topic: /rtabmap/grid_map
       Value: true
     - Class: rtabmap_ros/Info
       Enabled: true

launch/data_recorder.launch

@@ -5,7 +5,8 @@
   <arg name="subscribe_depth"      default="true"/>
   <arg name="subscribe_stereo"     default="false"/>
   <arg name="subscribe_scan"       default="false"/>
-  <arg name="stereo_approx_sync"   default="false"/>
+  <arg     if="$(arg subscribe_stereo)" name="approx_sync"  default="false"/>
+  <arg unless="$(arg subscribe_stereo)" name="approx_sync"  default="true"/>
   
   <arg name="frame_id"      default="camera_link"/>
   <arg name="odom_frame_id" default=""/>        <!-- use topic when not set, otherwise use TF if set -->
@@ -42,6 +43,7 @@
 	<param name="RGBD/ProximityBySpace"   type="string" value="false"/> 
 	<param name="RGBD/LinearUpdate"       type="string" value="0"/> 
 	<param name="RGBD/AngularUpdate"      type="string" value="0"/>
+	<param name="RGBD/CreateOccupancyGrid" type="string" value="false"/> 
 	<param unless="$(arg subscribe_odometry)" name="RGBD/Enabled" type="string" value="false"/>
 	
 	<param name="Rtabmap/DetectionRate"   type="string" value="$(arg max_rate)"/>
@@ -52,7 +54,7 @@
 	<param name="subscribe_scan"      type="bool"   value="$(arg subscribe_scan)"/>
 	<param name="subscribe_stereo"    type="bool"   value="$(arg subscribe_stereo)"/>
 	<param name="queue_size"          type="int"    value="$(arg queue_size)"/>
-	<param name="stereo_approx_sync"  type="bool"    value="$(arg stereo_approx_sync)"/>
+	<param name="approx_sync"         type="bool"   value="$(arg approx_sync)"/>
 	
 	<!-- Hack to use a fake odom_frame_id=frame_id if subscribe_odometry = false -->
 	<param if="$(arg subscribe_odometry)" name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>

launch/demo/demo_multi-session_mapping.launch

@@ -6,6 +6,7 @@
    <!-- Choose visualization -->
   <arg name="rviz" default="false" />
   <arg name="rtabmapviz" default="true" />
+  <arg name="rtabmap_args" default="" />
 
   <param name="use_sim_time" type="bool" value="True"/>
 
@@ -13,7 +14,7 @@
   
   <!-- SLAM (robot side) -->
   <!-- args: "delete_db_on_start" and "udebug" -->
-  <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="">
+  <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
 	<param name="frame_id" type="string" value="base_footprint"/>
 	
 	<param name="subscribe_depth" type="bool" value="true"/>
@@ -33,7 +34,7 @@
 	
 	<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
 	<param name="RGBD/NeighborLinkRefining"  type="string" value="false"/> 
-	<param name="RGBD/ProximityBySpace"      type="string" value="false"/> 
+	<param name="RGBD/ProximityBySpace"      type="string" value="false"/> <!-- Referred paper did only global loop closure detection -->
 	<param name="RGBD/ProximityByTime"       type="string" value="false"/> 
 	<param name="RGBD/OptimizeFromGraphEnd"  type="string" value="true"/> 
 	<param name="Reg/Strategy"               type="string" value="1"/>
@@ -49,8 +50,12 @@
 	<param name="Bayes/FullPredictionUpdate" type="string" value="true"/>
 	<param name="Kp/DetectorStrategy"        type="string" value="0"/>   <!-- use SURF -->
 	<param name="Kp/MaxFeatures"             type="string" value="400"/>
-	<param name="Optimizer/Slam2D"           type="string" value="true"/>
 	<param name="Reg/Force3DoF"              type="string" value="true"/> 
+	<param name="RGBD/OptimizeMaxError"      type="string" value="0.25"/>
+        <param name="Optimizer/Strategy"         type="string" value="0"/>  <!-- TORO is the most stable for multi-session mapping -->
+        <param name="Optimizer/Iterations"       type="string" value="100"/> 
+        <param name="Kp/IncrementalFlann"        type="string" value="false"/>  <!-- Referred paper didn't use incremental FLANN -->
+        <param name="Grid/FromDepth"             type="string" value="false"/>
   </node>
   
   <!-- Visualisation RTAB-Map -->

launch/demo/demo_robot_mapping.launch

@@ -33,19 +33,18 @@
   	  <remap from="depth/image"     to="/data_throttled_image_depth"/>
   	  <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
   	
-      <param name="rgb/image_transport"   type="string" value="compressed"/>
-      <param name="depth/image_transport" type="string" value="compressedDepth"/>
+	  <param name="rgb/image_transport"   type="string" value="compressed"/>
+	  <param name="depth/image_transport" type="string" value="compressedDepth"/>
 	
 	  <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
 	  <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>  <!-- Do odometry correction with consecutive laser scans -->
 	  <param name="RGBD/ProximityBySpace"     type="string" value="true"/>  <!-- Local loop closure detection (using estimated position) with locations in WM -->
 	  <param name="RGBD/ProximityByTime"      type="string" value="false"/> <!-- Local loop closure detection with locations in STM -->
-	  <param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
 	  <param name="Reg/Strategy"              type="string" value="1"/>     <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
 	  <param name="Vis/InlierDistance"        type="string" value="0.1"/>   <!-- 3D visual words correspondence distance -->
 	  <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> <!-- Optimize graph from initial node so /map -> /odom transform will be generated -->
-	  <param name="Optimizer/Slam2D"          type="string" value="true"/>
 	  <param name="Reg/Force3DoF"             type="string" value="true"/>
+	  <param name="Grid/FromDepth"            type="string" value="false"/>
 	  
 	  <!-- localization mode -->
 	  <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>

launch/demo/demo_stereo_outdoor.launch

@@ -77,6 +77,8 @@
 
          <!-- RTAB-Map's parameters -->
          <param name="Rtabmap/TimeThr"                   type="string" value="700"/>
+         <param name="Grid/DepthDecimation"              type="string" value="4"/>
+         <param name="Grid/FlatObstacleDetected"         type="string" value="true"/>
          <param name="Kp/MaxFeatures"                    type="string" value="200"/>
          <param name="Kp/MaxDepth"                       type="string" value="10"/>
          <param name="Kp/DetectorStrategy"               type="string" value="0"/>   <!-- use SURF -->

launch/demo/demo_two_kinects.launch

@@ -1,7 +1,7 @@
 
 <launch>
    
-  <!-- Multi-cameras demo  with 2 Kinects -->
+  <!-- Multi-cameras demo with 2 Kinects -->
    
   <!-- Cameras -->
   <include file="$(find freenect_launch)/launch/freenect.launch">
@@ -15,7 +15,7 @@
     <arg name="device_id" value="#2" />
   </include>
 
-  <!-- Frames: Kinects are placed at 90 degrees -->
+  <!-- Frames: Kinects are placed at 90 degrees, clockwise -->
   <node pkg="tf" type="static_transform_publisher" name="base_to_camera1_tf"
       args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera1_link 100" />
   <node pkg="tf" type="static_transform_publisher" name="base_to_camera2_tf"
@@ -47,20 +47,32 @@
    <arg name="odom_info_data"  default="true" />
    <arg name="wait_for_transform"  default="true" />
    
+   <!-- sync rgb/depth images per camera -->
+   <group ns="camera1">
+    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera1_nodelet_manager">
+      <remap from="rgb/image"       to="rgb/image_rect_color"/>
+      <remap from="depth/image"     to="depth_registered/image_raw"/>
+      <remap from="rgb/camera_info" to="rgb/camera_info"/>
+    </node>
+   </group>
+   <group ns="camera2">
+    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera2_nodelet_manager">
+      <remap from="rgb/image"       to="rgb/image_rect_color"/>
+      <remap from="depth/image"     to="depth_registered/image_raw"/>
+      <remap from="rgb/camera_info" to="rgb/camera_info"/>
+    </node>
+   </group>
+        
   <group ns="rtabmap">
   
     <!-- Odometry -->
     <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
-      <remap from="rgb0/image"       to="/camera1/rgb/image_rect_color"/>
-      <remap from="depth0/image"     to="/camera1/depth_registered/image_raw"/>
-      <remap from="rgb0/camera_info" to="/camera1/rgb/camera_info"/>
-      
-      <remap from="rgb1/image"       to="/camera2/rgb/image_rect_color"/>
-      <remap from="depth1/image"     to="/camera2/depth_registered/image_raw"/>
-      <remap from="rgb1/camera_info" to="/camera2/rgb/camera_info"/>
+      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
+      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>
 	  
+	  <param name="subscribe_rgbd"           type="bool"   value="true"/>
 	  <param name="frame_id"                 type="string" value="base_link"/>
-	  <param name="depth_cameras"            type="int"    value="2"/>
+	  <param name="rgbd_cameras"             type="int"    value="2"/>
 	  <param name="wait_for_transform"       type="bool"   value="$(arg wait_for_transform)"/>
 	  <param name="Odom/Strategy"            type="string" value="$(arg strategy)"/> 
 	  <param name="Vis/FeatureType"         type="string" value="$(arg feature)"/>  
@@ -75,65 +87,38 @@
     <!-- Visual SLAM (robot side) -->
     <!-- args: "delete_db_on_start" and "udebug" -->
     <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
-      <param name="subscribe_depth"  type="bool"   value="true"/>
-      <param name="depth_cameras"    type="int"    value="2"/>
+      <param name="subscribe_depth"  type="bool"   value="false"/>
+      <param name="subscribe_rgbd"   type="bool"   value="true"/>
+      <param name="rgbd_cameras"    type="int"    value="2"/>
 	  <param name="frame_id"         type="string" value="base_link"/>
 	  <param name="gen_scan"         type="bool"   value="true"/>
 	  <param name="wait_for_transform" type="bool"   value="$(arg wait_for_transform)"/>
+	  <param name="map_negative_poses_ignored" type="bool"   value="true"/>
 	
-      <remap from="rgb0/image"       to="/camera1/rgb/image_rect_color"/>
-      <remap from="depth0/image"     to="/camera1/depth_registered/image_raw"/>
-      <remap from="rgb0/camera_info" to="/camera1/rgb/camera_info"/>
-     
-      <remap from="rgb1/image"       to="/camera2/rgb/image_rect_color"/>
-      <remap from="depth1/image"     to="/camera2/depth_registered/image_raw"/>
-      <remap from="rgb1/camera_info" to="/camera2/rgb/camera_info"/>
+      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
+      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>
 
+      <param name="Grid/FromDepth"     type="string" value="false"/>
       <param name="Vis/MinInliers"     type="string" value="10"/>
       <param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>
     </node>
   
     <!-- Visualisation RTAB-Map -->
     <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
-      <param name="subscribe_depth"     type="bool"   value="true"/>
+      <param name="subscribe_depth"  type="bool"   value="false"/>
+      <param name="subscribe_rgbd"   type="bool"   value="true"/>
       <param name="subscribe_odom_info" type="bool"   value="$(arg odom_info_data)"/>
       <param name="frame_id"            type="string" value="base_link"/>
-      <param name="depth_cameras"       type="int"    value="2"/>
+      <param name="rgbd_cameras"       type="int"    value="2"/>
       <param name="wait_for_transform"  type="bool"   value="$(arg wait_for_transform)"/>
     
-      <remap from="rgb0/image"       to="/camera1/rgb/image_rect_color"/>
-      <remap from="depth0/image"     to="/camera1/depth_registered/image_raw"/>
-      <remap from="rgb0/camera_info" to="/camera1/rgb/camera_info"/>
-     
-      <remap from="rgb1/image"       to="/camera2/rgb/image_rect_color"/>
-      <remap from="depth1/image"     to="/camera2/depth_registered/image_raw"/>
-      <remap from="rgb1/camera_info" to="/camera2/rgb/camera_info"/>
+      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
+      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>
     </node>
   
   </group>
   
   <!-- Visualization RVIZ -->
   <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
-  <!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
-  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>
-  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
-    <remap from="rgb/image_in"       to="camera1/rgb/image_rect_color"/>
-    <remap from="depth/image_in"     to="camera1/depth_registered/image_raw"/>
-    <remap from="rgb/camera_info_in" to="camera1/rgb/camera_info"/>
-    <remap from="odom_in"            to="rtabmap/odom"/>
-    
-    <remap from="rgb/image_out"       to="data_odom_sync/image"/>
-    <remap from="depth/image_out"     to="data_odom_sync/depth"/>
-    <remap from="rgb/camera_info_out" to="data_odom_sync/camera_info"/>
-    <remap from="odom_out"            to="odom_sync"/>
-  </node>
-  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
-    <remap from="rgb/image"       to="data_odom_sync/image"/>
-    <remap from="depth/image"     to="data_odom_sync/depth"/>
-    <remap from="rgb/camera_info" to="data_odom_sync/camera_info"/>
-    <remap from="cloud"           to="voxel_cloud" />
-
-    <param name="voxel_size" type="double" value="0.01"/>
-  </node>
 
 </launch>

launch/rgbd_mapping.launch

@@ -15,8 +15,8 @@
   <arg name="localization"            default="false"/>
   
   <!-- Corresponding config files -->
-  <arg name="rtabmapviz_cfg"          default="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" />
-  <arg name="rviz_cfg"                default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
+  <arg name="rtabmapviz_cfg"          default="~/.ros/rtabmap_gui.ini" />
+  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
   
   <arg name="frame_id"                default="camera_link"/>   <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
   <arg name="database_path"           default="~/.ros/rtabmap.db"/>
@@ -37,6 +37,7 @@
   
   <arg name="visual_odometry"         default="true"/>          <!-- Generate visual odometry -->
   <arg name="odom_topic"              default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
+  <arg name="odom_frame_id"           default=""/>              <!-- If set, TF is used to get odometry instead of the topic -->
    
   <arg name="namespace"               default="rtabmap"/>
   <arg name="wait_for_transform"      default="0.2"/>
@@ -69,6 +70,7 @@
    
     <arg name="visual_odometry"         value="$(arg visual_odometry)"/>          
     <arg name="odom_topic"              value="$(arg odom_topic)"/>    
+    <arg name="odom_frame_id"           value="$(arg odom_frame_id)"/>         
     <arg name="odom_args"               value="$(arg rtabmap_args)"/>
   </include>
 

launch/rtabmap.launch

@@ -27,7 +27,7 @@
   <!-- Corresponding config files -->
   <arg name="cfg"                     default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
   <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
-  <arg name="rviz_cfg"                default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
+  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
   
   <arg name="frame_id"                default="camera_link"/>     <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
   <arg name="namespace"               default="rtabmap"/>
@@ -36,7 +36,10 @@
   <arg name="wait_for_transform"      default="0.2"/>
   <arg name="rtabmap_args"            default=""/>              <!-- delete_db_on_start, udebug -->
   <arg name="launch_prefix"           default=""/>              <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
-  <arg name="approx_sync"             default="true"/>         <!-- if timestamps of the input topics are not synchronized -->
+  
+  <!-- if timestamps of the input topics are synchronized using approximate or exact time policy-->
+  <arg     if="$(arg stereo)" name="approx_sync"  default="false"/>
+  <arg unless="$(arg stereo)" name="approx_sync"  default="true"/>         
    
   <!-- RGB-D related topics -->
   <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
@@ -51,9 +54,8 @@
   <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
   
   <arg name="compressed"              default="false"/>         <!-- If you want to subscribe to compressed image topics -->
-                                                                <!-- For depth_topic, "compressedDepth" image_transport is used. --> 
-                                                                <!-- For rgb_topic, see "rgb_image_transport" argument. -->
   <arg name="rgb_image_transport"     default="compressed"/>    <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
+  <arg name="depth_image_transport"   default="compressedDepth"/>  <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
    
   <arg name="subscribe_scan"          default="false"/>
   <arg name="scan_topic"              default="/scan"/>
@@ -63,6 +65,7 @@
    
   <arg name="visual_odometry"         default="true"/>          <!-- Launch rtabmap visual odometry node -->
   <arg name="odom_topic"              default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
+  <arg name="odom_frame_id"           default=""/>              <!-- If set, TF is used to get odometry instead of the topic -->
   <arg name="odom_args"               default="$(arg rtabmap_args)"/>
   
   <!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
@@ -81,7 +84,7 @@
     <!-- RGB-D Odometry -->
     <group unless="$(arg stereo)">
       <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="compressedDepth in:=$(arg depth_topic) raw out:=$(arg depth_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 if="$(arg visual_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen" args="$(arg odom_args)" launch-prefix="$(arg launch_prefix)">
         <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
@@ -124,6 +127,7 @@
       <param name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
       <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
       <param name="frame_id"             type="string" value="$(arg frame_id)"/>
+      <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
       <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
       <param name="database_path"        type="string" value="$(arg database_path)"/>
       <param name="approx_sync"          type="bool"   value="$(arg approx_sync)"/>
@@ -158,8 +162,10 @@
       <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
       <param name="subscribe_odom_info"  type="bool"   value="$(arg visual_odometry)"/>
       <param name="frame_id"             type="string" value="$(arg frame_id)"/>
+      <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
       <param name="wait_for_transform_duration" type="double"   value="$(arg wait_for_transform)"/>
       <param name="queue_size"           type="int" value="$(arg queue_size)"/>
+      <param name="approx_sync"                 type="bool"   value="$(arg approx_sync)"/>
     
       <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
       <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
@@ -178,7 +184,7 @@
   </group>
   
   <!-- Visualization RVIZ -->
-  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="$(arg rviz_cfg)"/>
+  <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">
     <remap from="left/image"        to="$(arg left_image_topic_relay)"/>
     <remap from="right/image"       to="$(arg right_image_topic_relay)"/>

launch/stereo_mapping.launch

@@ -15,8 +15,8 @@
   <arg name="localization"            default="false"/>
   
   <!-- Corresponding config files -->
-  <arg name="rtabmapviz_cfg"          default="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" />
-  <arg name="rviz_cfg"                default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
+  <arg name="rtabmapviz_cfg"          default="$(find rtabmap_ros)/launch/config/rgbd_gui.ini" />
+  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
   
   <arg name="frame_id"                default="base_link"/>     <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
   <arg name="database_path"           default="~/.ros/rtabmap.db"/>
@@ -39,12 +39,12 @@
   
   <arg name="visual_odometry"         default="true"/>          <!-- Generate visual odometry -->
   <arg name="odom_topic"              default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
+  <arg name="odom_frame_id"           default=""/>              <!-- If set, TF is used to get odometry instead of the topic -->
   
   <arg name="namespace"               default="rtabmap"/>
   <arg name="wait_for_transform"      default="0.2"/>
   
   <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
-    <arg name="rgbd"                    value="false"/>
     <arg name="stereo"                  value="true"/>
     <arg name="rtabmapviz"              value="$(arg rtabmapviz)" /> 
     <arg name="rviz"                    value="$(arg rviz)" />
@@ -76,6 +76,7 @@
    
     <arg name="visual_odometry"         value="$(arg visual_odometry)"/>          
     <arg name="odom_topic"              value="$(arg odom_topic)"/>       
+    <arg name="odom_frame_id"           value="$(arg odom_frame_id)"/>   
     <arg name="odom_args"               value="$(arg rtabmap_args)"/>
   </include>
 

launch/tests/rgbdslam_datasets.launch

@@ -62,6 +62,8 @@
       <param name="RGBD/LoopClosureReextractFeatures" type="string" value="false"/>
       <param name="Mem/RawDescriptorsKept" type="string" value="true"/>
       <param name="Kp/DetectorStrategy" type="string" value="0"/>
+      <param name="RGBD/CreateOccupancyGrid" type="string" value="false"/>
+      <param name="Rtabmap/CreateIntermediateNodes" type="string" value="true"/>
 
       <param name="frame_id" type="string" value="kinect"/>
       <param name="ground_truth_frame_id" type="string" value="world"/> 

launch/tests/sensor_fusion_kinect_brick.launch

@@ -1,6 +1,7 @@
 <launch>
 
   <arg name="localization"      default="false"/>
+  <arg name="uid"/>
 
   <!-- Kinect: -->
   <include file="$(find freenect_launch)/launch/freenect.launch">
@@ -12,7 +13,7 @@
   <node pkg="imu_brick" type="imu_brick_node" name="imu_brick">
       <param name="frame_id"                       value="imu_link"/>
       <param name="period_ms"                      value="10"/>
-      <param name="uid"              type="string" value="6xDEo7"/>
+      <param name="uid"              type="string" value="$(arg uid)"/>
       <param name="cov_orientation"  type="double" value="0.0005"/>
       <param name="cov_velocity"     type="double" value="0.00025"/>
       <param name="cov_acceleration" type="double" value="0.1"/>

launch/tests/test_icp_odometry.launch

@@ -0,0 +1,53 @@
+
+<launch>
+
+  <!-- We test here ICP odometry using a guess from visual odometry -->
+  
+  <arg name="rgbd"  default="false"/>
+   
+  <include file="$(find freenect_launch)/launch/freenect.launch" >
+    <arg name="depth_registration" value="true"/>
+    <arg name="data_skip" value="3"/>
+  </include>
+  
+  <group ns="camera"> 
+    <node pkg="nodelet" type="nodelet" name="points_xyz" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
+      <remap from="depth/image"       to="depth_registered/image_raw"/>
+      <remap from="depth/camera_info" to="depth_registered/camera_info"/>
+      <remap from="cloud"             to="/voxel_cloud" />
+
+      <param name="voxel_size" type="double" value="0.05"/>
+      <param name="decimation" type="int" value="8"/>
+      
+      <param name="Odom/AlignWithGround"  type="string" value="true"/>
+    </node>
+
+    <node if="$(arg rgbd)" pkg="nodelet" type="nodelet" name="rgbdicp_odometry" args="load rtabmap_ros/rgbdicp_odometry camera_nodelet_manager">
+      <remap from="scan_cloud"        to="/voxel_cloud"/>
+      <remap from="depth/image"       to="depth_registered/image_raw"/>
+      <remap from="rgb/camera_info"   to="rgb/camera_info"/>
+      <remap from="rgb/image"         to="rgb/image_rect_mono"/>
+	  
+      <param name="frame_id"        type="string" value="camera_link"/>   
+      <param name="scan_cloud_normal_k" type="int" value="10"/>
+      <param name="subscribe_scan_cloud" type="bool" value="true"/>   
+     
+      <param name="Icp/PointToPlane"  type="string" value="true"/>
+      <param name="Icp/VoxelSize"     type="string" value="0"/>
+    </node>
+    <node unless="$(arg rgbd)" pkg="nodelet" type="nodelet" name="icp_odometry" args="load rtabmap_ros/icp_odometry camera_nodelet_manager">
+      <remap from="scan_cloud"      to="/voxel_cloud"/>
+	  
+      <param name="frame_id"        type="string" value="camera_link"/>   
+      <param name="scan_cloud_normal_k" type="int" value="10"/>   
+     
+      <param name="Icp/PointToPlane"  type="string" value="true"/>
+      <param name="Icp/VoxelSize"     type="string" value="0"/>
+      <param name="Odom/GuessMotion"  type="string" value="true"/>
+    </node>
+
+  </group>
+  
+  <!-- Visualization RVIZ -->
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
+</launch>

launch/tests/test_map_optimizer.launch

@@ -0,0 +1,24 @@
+<launch>
+   <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
+     <arg name="rtabmap_args" value="
+                                     --delete_db_on_start 
+                                     --RGBD/OptimizeMaxError 0 
+                                     --Optimizer/Iterations 0
+                                     --RGBD/ProximityBySpace false"/>
+     <arg name="rtabmapviz" value="false"/>
+  </include>
+  
+  <group ns="rtabmap">
+     <node pkg="rtabmap_ros" type="map_optimizer" name="map_optimizer"/>
+     
+     <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
+        <remap from="mapData" to="mapData_optimized"/>
+        <param name="frame_id" value="camera_link"/>
+        <param name="subscribe_depth" value="false"/>
+     </node>
+     
+     <node pkg="rtabmap_ros" type="map_assembler" name="map_assembler">
+        <remap from="mapData" to="mapData_optimized"/>
+     </node>
+  </group>  
+</launch>

launch/tests/test_obstacles_detection.launch

@@ -1,8 +1,6 @@
 <launch>
 
 <!-- Use stereo_outdoorA.bag for testing -->
-<arg name="optimize_for_close_objects" default="false" />
-
 <include file="$(find rtabmap_ros)/launch/demo/demo_stereo_outdoor.launch"/>
 
 <group ns="/stereo_camera" >
@@ -23,7 +21,6 @@
 		<param name="wait_for_transform" type="bool" value="true"/>
 		<param name="min_cluster_size" type="int" value="20"/>
 		<param name="max_obstacles_height" type="double" value="0.0"/>
-		<param name="optimize_for_close_objects" type="bool" value="$(arg optimize_for_close_objects)"/>
     </node>
 </group>
 

launch/tests/test_rgbd_image.launch

@@ -0,0 +1,48 @@
+
+<launch>  
+
+  <arg name="compressed" default="false"/>
+
+  <group ns="camera">
+    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera_nodelet_manager">
+      <remap from="rgb/image"       to="rgb/image_rect_color"/>
+      <remap from="depth/image"     to="depth_registered/image_raw"/>
+      <remap from="rgb/camera_info" to="rgb/camera_info"/>
+    </node>
+  </group>
+
+   <!-- Nodes -->
+  <group ns="rtabmap">
+  
+      <!-- RGB-D Odometry -->
+      <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
+        <remap from="rgb/image"       to="/camera/rgb/image_rect_color"/>
+        <remap from="depth/image"     to="/camera/depth_registered/image_raw"/>
+        <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
+        
+	    <param name="Odom/AlignWithGround"  type="string" value="true"/>
+        <param name="frame_id"         type="string" value="camera_link"/>
+      </node>
+    
+    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
+      <param name="subscribe_depth"       type="bool"   value="false"/>
+      <param name="subscribe_rgbd"       type="bool"   value="true"/>
+      <param name="frame_id"             type="string" value="camera_link"/>
+      <param name="approx_sync"          type="string" value="false"/>
+      <remap     if="$(arg compressed)"    from="rgbd_image"   to="/camera/rgbd_image/compressed"/>
+      <remap unless="$(arg compressed)"    from="rgbd_image"   to="/camera/rgbd_image"/>
+    </node>
+  
+    <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
+      <param name="subscribe_depth"      type="bool"   value="false"/>
+      <param name="subscribe_rgbd"       type="bool"   value="true"/>
+      <param name="frame_id"             type="string" value="camera_link"/>
+      <param name="approx_sync"          type="string" value="false"/>
+      <remap     if="$(arg compressed)"    from="rgbd_image"   to="/camera/rgbd_image/compressed"/>
+      <remap unless="$(arg compressed)"    from="rgbd_image"   to="/camera/rgbd_image"/>
+      <param name="subscribe_odom_info"  type="bool"   value="true"/>
+    </node>
+  
+  </group>
+
+</launch>

launch/tests/test_rtabmap_nodelets.launch

@@ -0,0 +1,49 @@
+
+<launch>
+
+  <!-- Kinect: -->
+  <include file="$(find freenect_launch)/launch/freenect.launch">
+    <arg name="depth_registration" value="true" />
+  </include>
+  
+  <arg name="frame_id"                default="camera_link"/> 
+  <arg name="rtabmap_args"            default=""/>                <!-- delete_db_on_start, udebug -->  
+  <arg name="odom_args"               default="$(arg rtabmap_args)"/>
+   
+  <!-- RGB-D related topics -->
+  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
+  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
+  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
+ 
+  <group ns="camera"> 
+  
+    <!-- Use RGBD synchronization -->
+    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera_nodelet_manager">
+      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
+      <remap from="depth/image"     to="$(arg depth_topic)"/>
+      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
+    </node>
+  
+    <!-- RGB-D Odometry -->
+      <node pkg="nodelet" type="nodelet" name="rgbd_odometry" args="load rtabmap_ros/rgbd_odometry camera_nodelet_manager $(arg odom_args)">
+        <param name="frame_id"        type="string" value="$(arg frame_id)"/>
+        <param name="subscribe_depth" type="bool"   value="false"/>
+        <param name="subscribe_rgbd"  type="bool"   value="true"/>
+      </node>
+  
+    <!-- RTAB-Map -->
+    <node pkg="nodelet" type="nodelet" name="rtabmap" args="load rtabmap_ros/rtabmap camera_nodelet_manager $(arg rtabmap_args)">
+      <param name="frame_id"        type="string" value="$(arg frame_id)"/>
+      <param name="subscribe_depth" type="bool"   value="false"/>
+      <param name="subscribe_rgbd"  type="bool"   value="true"/>
+      <param name="approx_sync"     type="bool"   value="false"/>
+    </node>
+  
+    <!-- Visualisation -->
+    <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
+      <param name="frame_id" type="string" value="$(arg frame_id)"/>
+    </node>
+    
+  </group>
+
+</launch>

launch/tests/test_undistort_depth.launch

@@ -0,0 +1,20 @@
+
+<launch>
+
+  <!-- Kinect: -->
+  <include file="$(find openni2_launch)/launch/openni2.launch">
+    <arg name="depth_registration" value="true" />
+  </include>
+  
+  <arg name="depth"  default="/camera/depth_registered/image_raw" />
+  <arg name="model"  default="$(find rtabmap_ros)/launch/calibration/distortion_model_PS1080.bin" /> <!-- XTION Live Pro -->
+ 
+  <group ns="camera"> 
+    <!-- Undistort depth image -->
+    <node pkg="nodelet" type="nodelet" name="undistort" args="load rtabmap_ros/undistort_depth camera_nodelet_manager">
+      <remap from="depth" to="$(arg depth)"/>
+      <param name="model" value="$(arg model)"/>
+    </node>   
+  </group>
+
+</launch>

msg/NodeData.msg

@@ -24,6 +24,8 @@ float32[] fx
 float32[] fy
 float32[] cx
 float32[] cy
+float32[] width
+float32[] height
 float32 baseline
 # local transform (/base_link -> /camera_link)
 geometry_msgs/Transform[] localTransform
@@ -33,13 +35,25 @@ geometry_msgs/Transform[] localTransform
 uint8[] laserScan
 int32 laserScanMaxPts
 float32 laserScanMaxRange
+geometry_msgs/Transform laserScanLocalTransform
 
 # compressed user data
 # use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
 uint8[] userData
 
+# compressed occupancy grid
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] grid_ground
+uint8[] grid_obstacles
+float32 grid_cell_size
+Point3f grid_view_point
+
 # std::multimap<wordId, cv::Keypoint>
 # std::multimap<wordId, pcl::PointXYZ>
 int32[] wordIds
 KeyPoint[] wordKpts
 sensor_msgs/PointCloud2 wordPts
+
+# compressed descriptors
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] descriptors

msg/OdomInfo.msg

@@ -6,7 +6,8 @@ Header header
 #    bool lost;
 #    int matches;
 #    int inliers;
-#    float variance;
+#    float varianceLin;
+#    float varianceAng;
 #    int features;
 #    int localMapSize;
 #    float time;
@@ -27,9 +28,11 @@ Header header
 bool lost
 int32 matches
 int32 inliers
-float32 variance
+float32 varianceLin
+float32 varianceAng
 int32 features
 int32 localMapSize
+int32 localScanMapSize
 float32 timeEstimation
 float32 timeParticleFiltering
 float32 stamp
@@ -52,3 +55,7 @@ int32[] cornerInliers
 geometry_msgs/Transform transform
 geometry_msgs/Transform transformFiltered
 
+# compressed local scan map data
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] localScanMap
+

msg/RGBDImage.msg

@@ -0,0 +1,12 @@
+
+Header header
+
+sensor_msgs/CameraInfo cameraInfo
+
+# Raw
+sensor_msgs/Image rgb
+sensor_msgs/Image depth
+
+# Compressed
+sensor_msgs/CompressedImage rgbCompressed
+sensor_msgs/CompressedImage depthCompressed

msg/UserData.msg

@@ -0,0 +1,12 @@
+
+Header header
+
+# OpenCV matrix containing the user data. A matrix of type CV_8UC1 
+# with 1 row is considered to be compressed (with rtabmap::compressData() method).
+# If you have one dimension unsigned 8 bits uncompressed data, make sure to transpose it
+# (to have multiple rows instead of multiple columns) in order to be detected as
+# not compressed.
+uint32 rows
+uint32 cols
+uint32 type
+uint8[] data

nodelet_plugins.xml

@@ -1,5 +1,13 @@
 <library path="lib/librtabmap_ros"> 
 
+  <class name="rtabmap_ros/rtabmap" 
+         type="rtabmap_ros::CoreWrapper" 
+         base_class_type="nodelet::Nodelet">
+    <description>
+      This is my nodelet.
+    </description>
+  </class>
+
   <class name="rtabmap_ros/rgbd_odometry" 
          type="rtabmap_ros::RGBDOdometry" 
          base_class_type="nodelet::Nodelet">
@@ -16,6 +24,22 @@
     </description>
   </class>
   
+  <class name="rtabmap_ros/rgbdicp_odometry" 
+         type="rtabmap_ros::RGBDICPOdometry" 
+         base_class_type="nodelet::Nodelet">
+    <description>
+      This is my nodelet.
+    </description>
+  </class>
+  
+  <class name="rtabmap_ros/icp_odometry" 
+         type="rtabmap_ros::ICPOdometry" 
+         base_class_type="nodelet::Nodelet">
+    <description>
+      This is my nodelet.
+    </description>
+  </class>
+
   <class name="rtabmap_ros/data_throttle" 
          type="rtabmap_ros::DataThrottleNodelet" 
          base_class_type="nodelet::Nodelet">
@@ -72,6 +96,14 @@
     </description>
   </class>
   
+  <class name="rtabmap_ros/obstacles_detection_old" 
+         type="rtabmap_ros::ObstaclesDetectionOld" 
+         base_class_type="nodelet::Nodelet">
+    <description>
+      This is my nodelet.
+    </description>
+  </class>
+
   <class name="rtabmap_ros/point_cloud_aggregator" 
          type="rtabmap_ros::PointCloudAggregator" 
          base_class_type="nodelet::Nodelet">
@@ -80,4 +112,21 @@
     </description>
   </class>
   
+  <class name="rtabmap_ros/rgbd_sync" 
+         type="rtabmap_ros::RGBDSync" 
+         base_class_type="nodelet::Nodelet">
+    <description>
+      This is my nodelet.
+    </description>
+  </class>
+  
+  <class name="rtabmap_ros/undistort_depth" 
+         type="rtabmap_ros::UndistortDepth" 
+         base_class_type="nodelet::Nodelet">
+    <description>
+      This is my nodelet.
+    </description>
+  </class>
+
 </library>
+

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap_ros</name>
-  <version>0.11.8</version>
+  <version>0.11.13</version>
   <description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

src/CommonDataSubscriber.cpp

@@ -0,0 +1,411 @@
+/*
+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>
+
+namespace rtabmap_ros {
+
+CommonDataSubscriber::CommonDataSubscriber(bool gui) :
+		queueSize_(10),
+		approxSync_(true),
+		warningThread_(0),
+		callbackCalled_(false),
+		subscribedToDepth_(!gui),
+		subscribedToStereo_(false),
+		subscribedToRGBD_(false),
+		subscribedToScan2d_(false),
+		subscribedToScan3d_(false),
+		subscribedToOdomInfo_(false),
+
+		// RGB + Depth
+		SYNC_INIT(depth),
+		SYNC_INIT(depthScan2d),
+		SYNC_INIT(depthScan3d),
+		SYNC_INIT(depthInfo),
+
+		// RGB + Depth + Odom
+		SYNC_INIT(depthOdom),
+		SYNC_INIT(depthOdomScan2d),
+		SYNC_INIT(depthOdomScan3d),
+		SYNC_INIT(depthOdomInfo),
+
+		// RGB + Depth + User Data
+		SYNC_INIT(depthData),
+		SYNC_INIT(depthDataScan2d),
+		SYNC_INIT(depthDataScan3d),
+		SYNC_INIT(depthDataInfo),
+
+		// RGB + Depth + Odom + User Data
+		SYNC_INIT(depthOdomData),
+		SYNC_INIT(depthOdomDataScan2d),
+		SYNC_INIT(depthOdomDataScan3d),
+		SYNC_INIT(depthOdomDataInfo),
+
+		// Stereo
+		SYNC_INIT(stereo),
+		SYNC_INIT(stereoInfo),
+
+		// Stereo + Odom
+		SYNC_INIT(stereoOdom),
+		SYNC_INIT(stereoOdomInfo),
+
+		// 1 RGBD
+		SYNC_INIT(rgbdScan2d),
+		SYNC_INIT(rgbdScan3d),
+		SYNC_INIT(rgbdInfo),
+
+		// 1 RGBD + Odom
+		SYNC_INIT(rgbdOdom),
+		SYNC_INIT(rgbdOdomScan2d),
+		SYNC_INIT(rgbdOdomScan3d),
+		SYNC_INIT(rgbdOdomInfo),
+
+		// 1 RGBD + User Data
+		SYNC_INIT(rgbdData),
+		SYNC_INIT(rgbdDataScan2d),
+		SYNC_INIT(rgbdDataScan3d),
+		SYNC_INIT(rgbdDataInfo),
+
+		// 1 RGBD + Odom + User Data
+		SYNC_INIT(rgbdOdomData),
+		SYNC_INIT(rgbdOdomDataScan2d),
+		SYNC_INIT(rgbdOdomDataScan3d),
+		SYNC_INIT(rgbdOdomDataInfo),
+
+		// 2 RGBD
+		SYNC_INIT(rgbd2),
+		SYNC_INIT(rgbd2Scan2d),
+		SYNC_INIT(rgbd2Scan3d),
+		SYNC_INIT(rgbd2Info),
+
+		// 2 RGBD + Odom
+		SYNC_INIT(rgbd2Odom),
+		SYNC_INIT(rgbd2OdomScan2d),
+		SYNC_INIT(rgbd2OdomScan3d),
+		SYNC_INIT(rgbd2OdomInfo),
+
+		// 2 RGBD + User Data
+		SYNC_INIT(rgbd2Data),
+		SYNC_INIT(rgbd2DataScan2d),
+		SYNC_INIT(rgbd2DataScan3d),
+		SYNC_INIT(rgbd2DataInfo),
+
+		// 2 RGBD + Odom + User Data
+		SYNC_INIT(rgbd2OdomData),
+		SYNC_INIT(rgbd2OdomDataScan2d),
+		SYNC_INIT(rgbd2OdomDataScan3d),
+		SYNC_INIT(rgbd2OdomDataInfo)
+
+{
+}
+
+void CommonDataSubscriber::setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::string & name)
+{
+	bool subscribeScan2d = false;
+	bool subscribeScan3d = false;
+	bool subscribeOdomInfo = false;
+	bool subscribeUserData = false;
+	int rgbdCameras = 1;
+	name_ = name;
+
+	// ROS related parameters (private)
+	pnh.param("subscribe_depth",     subscribedToDepth_, subscribedToDepth_);
+	if(pnh.getParam("subscribe_laserScan", subscribeScan2d) && subscribeScan2d)
+	{
+		ROS_WARN("rtabmap: \"subscribe_laserScan\" parameter is deprecated, use \"subscribe_scan\" instead. The scan topic is still subscribed.");
+	}
+	pnh.param("subscribe_scan",      subscribeScan2d, subscribeScan2d);
+	pnh.param("subscribe_scan_cloud", subscribeScan3d, subscribeScan3d);
+	pnh.param("subscribe_stereo",    subscribedToStereo_, subscribedToStereo_);
+	pnh.param("subscribe_rgbd",      subscribedToRGBD_, subscribedToRGBD_);
+	pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
+	pnh.param("subscribe_user_data", subscribeUserData, subscribeUserData);
+	if(subscribedToDepth_ && subscribedToStereo_)
+	{
+		ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false.");
+		subscribedToDepth_ = false;
+	}
+	if(subscribedToDepth_ && subscribedToRGBD_)
+	{
+		ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_rgbd cannot be true at the same time. Parameter subscribe_depth is set to false.");
+		subscribedToDepth_ = false;
+	}
+	if(subscribedToStereo_ && subscribedToRGBD_)
+	{
+		ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_rgbd cannot be true at the same time. Parameter subscribe_stereo is set to false.");
+		subscribedToStereo_ = false;
+	}
+	if(subscribeScan2d && subscribeScan3d)
+	{
+		ROS_WARN("rtabmap: Parameters subscribe_scan and subscribe_scan_cloud cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
+		subscribeScan3d = false;
+	}
+	if(subscribeScan2d || subscribeScan3d)
+	{
+		if(!subscribedToDepth_ && !subscribedToStereo_ && !subscribedToRGBD_)
+		{
+			ROS_WARN("When subscribing to laser scan, you should subscribe to depth, stereo or rgbd too. Subscribing to depth by default...");
+			subscribedToDepth_ = true;
+		}
+	}
+	if(subscribedToStereo_)
+	{
+		approxSync_ = false; // default for stereo: exact sync
+	}
+
+	std::string odomFrameId;
+	pnh.getParam("odom_frame_id", odomFrameId);
+	pnh.param("rgbd_cameras",       rgbdCameras, rgbdCameras);
+	if(pnh.hasParam("depth_cameras"))
+	{
+		ROS_ERROR("\"depth_cameras\" parameter doesn't exist anymore. It is replaced by \"rgbd_cameras\" used when \"subscribe_rgbd\" is true.");
+	}
+	pnh.param("queue_size",          queueSize_, queueSize_);
+	if(pnh.hasParam("stereo_approx_sync") && !pnh.hasParam("approx_sync"))
+	{
+		ROS_WARN("Parameter \"stereo_approx_sync\" has been renamed "
+				 "to \"approx_sync\"! Your value is still copied to "
+				 "corresponding parameter.");
+		pnh.param("stereo_approx_sync", approxSync_, approxSync_);
+	}
+	else
+	{
+		pnh.param("approx_sync", approxSync_, approxSync_);
+	}
+
+	if(rgbdCameras <= 0 && subscribedToRGBD_)
+	{
+		rgbdCameras = 1;
+	}
+
+	ROS_INFO("%s: queue_size    = %d", name.c_str(), queueSize_);
+	ROS_INFO("%s: rgbd_cameras = %d", name.c_str(), rgbdCameras);
+	ROS_INFO("%s: approx_sync   = %s", name.c_str(), approxSync_?"true":"false");
+
+	bool subscribeOdom = odomFrameId.empty();
+	if(subscribedToDepth_)
+	{
+		setupDepthCallbacks(
+				nh,
+				pnh,
+				subscribeOdom,
+				subscribeUserData,
+				subscribeScan2d,
+				subscribeScan3d,
+				subscribeOdomInfo,
+				queueSize_,
+				approxSync_);
+	}
+	else if(subscribedToStereo_)
+	{
+		setupStereoCallbacks(
+				nh,
+				pnh,
+				subscribeOdom,
+				subscribeOdomInfo,
+				queueSize_,
+				approxSync_);
+	}
+	else if(subscribedToRGBD_)
+	{
+		if(rgbdCameras == 2)
+		{
+			setupRGBD2Callbacks(
+					nh,
+					pnh,
+					subscribeOdom,
+					subscribeUserData,
+					subscribeScan2d,
+					subscribeScan3d,
+					subscribeOdomInfo,
+					queueSize_,
+					approxSync_);
+		}
+		else
+		{
+			setupRGBDCallbacks(
+					nh,
+					pnh,
+					subscribeOdom,
+					subscribeUserData,
+					subscribeScan2d,
+					subscribeScan3d,
+					subscribeOdomInfo,
+					queueSize_,
+					approxSync_);
+		}
+	}
+	if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_)
+	{
+		warningThread_ = new boost::thread(boost::bind(&CommonDataSubscriber::warningLoop, this));
+		ROS_INFO("%s", subscribedTopicsMsg_.c_str());
+	}
+}
+
+CommonDataSubscriber::~CommonDataSubscriber()
+{
+	if(warningThread_)
+	{
+		callbackCalled();
+		warningThread_->join();
+		delete warningThread_;
+	}
+
+	// RGB + Depth
+	SYNC_DEL(depth);
+	SYNC_DEL(depthScan2d);
+	SYNC_DEL(depthScan3d);
+	SYNC_DEL(depthInfo);
+
+	// RGB + Depth + Odom
+	SYNC_DEL(depthOdom);
+	SYNC_DEL(depthOdomScan2d);
+	SYNC_DEL(depthOdomScan3d);
+	SYNC_DEL(depthOdomInfo);
+
+	// RGB + Depth + User Data
+	SYNC_DEL(depthData);
+	SYNC_DEL(depthDataScan2d);
+	SYNC_DEL(depthDataScan3d);
+	SYNC_DEL(depthDataInfo);
+
+	// RGB + Depth + Odom + User Data
+	SYNC_DEL(depthOdomData);
+	SYNC_DEL(depthOdomDataScan2d);
+	SYNC_DEL(depthOdomDataScan3d);
+	SYNC_DEL(depthOdomDataInfo);
+
+	// Stereo
+	SYNC_DEL(stereo);
+	SYNC_DEL(stereoInfo);
+
+	// Stereo + Odom
+	SYNC_DEL(stereoOdom);
+	SYNC_DEL(stereoOdomInfo);
+
+	// 1 RGBD
+	SYNC_DEL(rgbdScan2d);
+	SYNC_DEL(rgbdScan3d);
+	SYNC_DEL(rgbdInfo);
+
+	// 1 RGBD + Odom
+	SYNC_DEL(rgbdOdom);
+	SYNC_DEL(rgbdOdomScan2d);
+	SYNC_DEL(rgbdOdomScan3d);
+	SYNC_DEL(rgbdOdomInfo);
+
+	// 1 RGBD + User Data
+	SYNC_DEL(rgbdData);
+	SYNC_DEL(rgbdDataScan2d);
+	SYNC_DEL(rgbdDataScan3d);
+	SYNC_DEL(rgbdDataInfo);
+
+	// 1 RGBD + Odom + User Data
+	SYNC_DEL(rgbdOdomData);
+	SYNC_DEL(rgbdOdomDataScan2d);
+	SYNC_DEL(rgbdOdomDataScan3d);
+	SYNC_DEL(rgbdOdomDataInfo);
+
+	// 2 RGBD
+	SYNC_DEL(rgbd2);
+	SYNC_DEL(rgbd2Scan2d);
+	SYNC_DEL(rgbd2Scan3d);
+	SYNC_DEL(rgbd2Info);
+
+	// 2 RGBD + Odom
+	SYNC_DEL(rgbd2Odom);
+	SYNC_DEL(rgbd2OdomScan2d);
+	SYNC_DEL(rgbd2OdomScan3d);
+	SYNC_DEL(rgbd2OdomInfo);
+
+	// 2 RGBD + User Data
+	SYNC_DEL(rgbd2Data);
+	SYNC_DEL(rgbd2DataScan2d);
+	SYNC_DEL(rgbd2DataScan3d);
+	SYNC_DEL(rgbd2DataInfo);
+
+	// 2 RGBD + Odom + User Data
+	SYNC_DEL(rgbd2OdomData);
+	SYNC_DEL(rgbd2OdomDataScan2d);
+	SYNC_DEL(rgbd2OdomDataScan3d);
+	SYNC_DEL(rgbd2OdomDataInfo);
+
+	for(unsigned int i=0; i<rgbdSubs_.size(); ++i)
+	{
+		delete rgbdSubs_[i];
+	}
+	rgbdSubs_.clear();
+}
+
+void CommonDataSubscriber::warningLoop()
+{
+	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. If topics are coming from different computers, make sure "
+					"the clocks of the computers are synchronized (\"ntpdate\"). %s%s",
+					name_.c_str(),
+					approxSync_?
+							uFormat("If topics are not published at the same rate, you could increase \"queue_size\" parameter (current=%d).", queueSize_).c_str():
+							"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());
+		}
+	}
+}
+
+void CommonDataSubscriber::commonSingleDepthCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const cv_bridge::CvImageConstPtr & imageMsg,
+		const cv_bridge::CvImageConstPtr & depthMsg,
+		const sensor_msgs::CameraInfo & cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	std::vector<cv_bridge::CvImageConstPtr> imageMsgs;
+	std::vector<cv_bridge::CvImageConstPtr> depthMsgs;
+	std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs;
+	if(imageMsg.get())
+	{
+		imageMsgs.push_back(imageMsg);
+	}
+	if(depthMsg.get())
+	{
+		depthMsgs.push_back(depthMsg);
+	}
+	cameraInfoMsgs.push_back(cameraInfoMsg);
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+
+} /* namespace rtabmap_ros */

src/CoreNode.cpp

@@ -25,12 +25,14 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "CoreWrapper.h"
+#include "ros/ros.h"
+#include <rtabmap/core/Parameters.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/core/Version.h>
+#include "nodelet/loader.h"
 
 int main(int argc, char** argv)
 {
@@ -41,19 +43,14 @@ int main(int argc, char** argv)
 
 	ros::init(argc, argv, "rtabmap");
 
-	bool deleteDbOnStart = false;
+	nodelet::V_string nargv;
 	for(int i=1;i<argc;++i)
 	{
-		if(strcmp(argv[i], "--delete_db_on_start") == 0)
-		{
-			deleteDbOnStart = true;
-		}
-		else if(strcmp(argv[i], "--params") == 0 || strcmp(argv[i], "--params-all") == 0)
+		if(strcmp(argv[i], "--params") == 0 || strcmp(argv[i], "--params-all") == 0)
 		{
 			rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
-			uInsert(parameters,
-					std::make_pair(rtabmap::Parameters::kRtabmapWorkingDirectory(),
-					UDirectory::homeDir()+"/.ros")); // change default to ~/.ros
+			uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRGBDCreateOccupancyGrid(), "true")); // default true in ROS
+			uInsert(parameters,	rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapWorkingDirectory(), UDirectory::homeDir()+"/.ros")); // change default to ~/.ros
 
 			if(strcmp(argv[i], "--params") == 0)
 			{
@@ -86,16 +83,15 @@ int main(int argc, char** argv)
 					 "argument \"--params\" is detected!");
 			exit(0);
 		}
+		nargv.push_back(argv[i]);
 	}
 
-	rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argc, argv);
-
-	CoreWrapper * rtabmap = new CoreWrapper(deleteDbOnStart, parameters);
-
+	nodelet::Loader nodelet;
+	nodelet::M_string remap(ros::names::getRemappings());
+	std::string nodelet_name = ros::this_node::getName();
+	nodelet.load(nodelet_name, "rtabmap_ros/rtabmap", remap, nargv);
 	ROS_INFO("rtabmap %s started...", RTABMAP_VERSION);
 	ros::spin();
 
-	delete rtabmap;
-
 	return 0;
 }

src/CoreWrapper.h

@@ -1,487 +0,0 @@
-/*
-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.
-*/
-
-#ifndef COREWRAPPER_H_
-#define COREWRAPPER_H_
-
-
-#include <ros/ros.h>
-
-#include <std_srvs/Empty.h>
-
-#include <tf/transform_listener.h>
-#include <tf2_ros/transform_broadcaster.h>
-
-#include <std_msgs/Empty.h>
-#include <std_msgs/Int32.h>
-#include <sensor_msgs/PointCloud2.h>
-#include <sensor_msgs/Image.h>
-#include <sensor_msgs/CameraInfo.h>
-#include <sensor_msgs/LaserScan.h>
-#include <nav_msgs/Odometry.h>
-#include <nav_msgs/GetMap.h>
-
-#include <rtabmap/core/Parameters.h>
-#include <rtabmap/core/Rtabmap.h>
-
-#include "rtabmap_ros/GetMap.h"
-#include "rtabmap_ros/ListLabels.h"
-#include "rtabmap_ros/PublishMap.h"
-#include "rtabmap_ros/SetGoal.h"
-#include "rtabmap_ros/SetLabel.h"
-#include "rtabmap_ros/Goal.h"
-
-#include "MapsManager.h"
-
-#include <message_filters/subscriber.h>
-#include <message_filters/synchronizer.h>
-#include <message_filters/sync_policies/approximate_time.h>
-#include <message_filters/sync_policies/exact_time.h>
-
-#include <image_transport/image_transport.h>
-#include <image_transport/subscriber_filter.h>
-
-#ifdef WITH_OCTOMAP_ROS
-#include <octomap_msgs/GetOctomap.h>
-#endif
-
-#include <actionlib/client/simple_action_client.h>
-#include <move_base_msgs/MoveBaseAction.h>
-#include <move_base_msgs/MoveBaseActionGoal.h>
-#include <move_base_msgs/MoveBaseActionResult.h>
-#include <move_base_msgs/MoveBaseActionFeedback.h>
-#include <actionlib_msgs/GoalStatusArray.h>
-typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
-
-class CoreWrapper
-{
-public:
-	CoreWrapper(bool deleteDbOnStart, const rtabmap::ParametersMap & parameters);
-	virtual ~CoreWrapper();
-
-private:
-	void setupCallbacks(
-			bool subscribeDepth,
-			bool subscribeScan2d,
-			bool subscribeScan3d,
-			bool subscribeStereo,
-			int queueSize,
-			bool approxSync,
-			int depthCameras);
-	void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
-
-	bool commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg);
-	bool commonOdomTFUpdate(const ros::Time & stamp); // TF odom
-	rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
-
-	void commonDepthCallback(
-				const std::string & odomFrameId,
-				const sensor_msgs::ImageConstPtr& imageMsg,
-				const sensor_msgs::ImageConstPtr& depthMsg,
-				const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
-				const sensor_msgs::LaserScanConstPtr& scanMsg,
-				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg);
-	void commonDepthCallback(
-				const std::string & odomFrameId,
-				const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
-				const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
-				const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
-				const sensor_msgs::LaserScanConstPtr& scanMsg,
-				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg);
-	void commonStereoCallback(
-				const std::string & odomFrameId,
-				const sensor_msgs::ImageConstPtr& leftImageMsg,
-				const sensor_msgs::ImageConstPtr& rightImageMsg,
-				const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
-				const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
-				const sensor_msgs::LaserScanConstPtr& scanMsg,
-				const sensor_msgs::PointCloud2ConstPtr& scan3dMsg);
-
-	// with odom msg
-	void depthCallback(
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& imageDepthMsg,
-			const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
-	void depthScanCallback(
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& imageDepthMsg,
-			const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
-			const sensor_msgs::LaserScanConstPtr& scanMsg);
-	void depthScan3dCallback(
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& imageDepthMsg,
-			const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
-			const sensor_msgs::PointCloud2ConstPtr& scanMsg);
-	void stereoCallback(
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg);
-	void stereoScanCallback(
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
-			const sensor_msgs::LaserScanConstPtr& scanMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg);
-	void stereoScan3dCallback(
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
-			const sensor_msgs::PointCloud2ConstPtr& scanMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg);
-	void depth2Callback(
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& image1Msg,
-			const sensor_msgs::ImageConstPtr& imageDepth1Msg,
-			const sensor_msgs::CameraInfoConstPtr& camInfo1Msg,
-			const sensor_msgs::ImageConstPtr& image2Msg,
-			const sensor_msgs::ImageConstPtr& imageDept2hMsg,
-			const sensor_msgs::CameraInfoConstPtr& camInfo2Msg);
-
-	// without odom, when TF is used for odom
-	void depthTFCallback(
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& imageDepthMsg,
-			const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
-	void depthScanTFCallback(
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& imageDepthMsg,
-			const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
-			const sensor_msgs::LaserScanConstPtr& scanMsg);
-	void depthScan3dTFCallback(
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& imageDepthMsg,
-			const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
-			const sensor_msgs::PointCloud2ConstPtr& scanMsg);
-	void stereoTFCallback(
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg);
-	void stereoScanTFCallback(
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
-			const sensor_msgs::LaserScanConstPtr& scanMsg);
-	void stereoScan3dTFCallback(
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
-			const sensor_msgs::PointCloud2ConstPtr& scanMsg);
-
-	void goalCommonCallback(int id, const std::string & label, const rtabmap::Transform & pose, const ros::Time & stamp, double * planningTime = 0);
-	void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
-	void goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg);
-	void updateGoal(const ros::Time & stamp);
-
-	void process(
-			const ros::Time & stamp,
-			const rtabmap::SensorData & data,
-			const rtabmap::Transform & odom = rtabmap::Transform(),
-			const std::string & odomFrameId = "",
-			float odomRotationalVariance = 1.0,
-			float odomTransitionalVariance = 1.0);
-
-	bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
-	bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
-	bool pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
-	bool resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
-	bool triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
-	bool backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Empty::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&);
-	bool setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
-	bool setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
-	bool setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
-	bool getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res);
-	bool getProjMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
-	bool getGridMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
-	bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&);
-	bool setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res);
-	bool cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
-	bool setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res);
-	bool listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtabmap_ros::ListLabels::Response& res);
-#ifdef WITH_OCTOMAP_ROS
-	bool octomapBinaryCallback(octomap_msgs::GetOctomap::Request  &req, octomap_msgs::GetOctomap::Response &res);
-	bool octomapFullCallback(octomap_msgs::GetOctomap::Request  &req, octomap_msgs::GetOctomap::Response &res);
-#endif
-
-	rtabmap::ParametersMap loadParameters(const std::string & configFile);
-	void saveParameters(const std::string & configFile);
-
-	void publishLoop(double tfDelay, double tfTolerance);
-
-	void publishStats(const ros::Time & stamp);
-	void publishCurrentGoal(const ros::Time & stamp);
-	void goalDoneCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result);
-	void goalActiveCb();
-	void goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback);
-	void publishLocalPath(const ros::Time & stamp);
-	void publishGlobalPath(const ros::Time & stamp);
-
-private:
-	rtabmap::Rtabmap rtabmap_;
-	bool paused_;
-	rtabmap::Transform lastPose_;
-	ros::Time lastPoseStamp_;
-	bool lastPoseIntermediate_;
-	float rotVariance_;
-	float transVariance_;
-	rtabmap::Transform currentMetricGoal_;
-	bool latestNodeWasReached_;
-	rtabmap::ParametersMap parameters_;
-
-	std::string frameId_;
-	std::string mapFrameId_;
-	std::string odomFrameId_;
-	std::string groundTruthFrameId_;
-	std::string configPath_;
-	std::string databasePath_;
-	bool waitForTransform_;
-	double waitForTransformDuration_;
-	bool useActionForGoal_;
-	bool genScan_;
-	double genScanMaxDepth_;
-	double genScanMinDepth_;
-	int scanCloudMaxPoints_;
-	int scanCloudNormalK_;
-	bool flipScan_;
-
-	rtabmap::Transform mapToOdom_;
-	boost::mutex mapToOdomMutex_;
-
-	MapsManager mapsManager_;
-
-	ros::Publisher infoPub_;
-	ros::Publisher mapDataPub_;
-	ros::Publisher mapGraphPub_;
-	ros::Publisher labelsPub_;
-
-	//Planning stuff
-	ros::Subscriber goalSub_;
-	ros::Subscriber goalNodeSub_;
-	ros::Publisher nextMetricGoalPub_;
-	ros::Publisher goalReachedPub_;
-	ros::Publisher globalPathPub_;
-	ros::Publisher localPathPub_;
-
-	// for loop closure detection only
-	image_transport::Subscriber defaultSub_;
-
-	//for depth callback
-	std::vector<image_transport::SubscriberFilter*> imageSubs_;
-	std::vector<image_transport::SubscriberFilter*> imageDepthSubs_;
-	std::vector<message_filters::Subscriber<sensor_msgs::CameraInfo>*> cameraInfoSubs_;
-
-	//stereo callback
-	image_transport::SubscriberFilter imageRectLeft_;
-	image_transport::SubscriberFilter imageRectRight_;
-	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
-	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
-
-	message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
-	message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
-	message_filters::Subscriber<sensor_msgs::PointCloud2> scan3dSub_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::LaserScan> MyDepthScanSyncPolicy;
-	message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::PointCloud2> MyDepthScan3dSyncPolicy;
-	message_filters::Synchronizer<MyDepthScan3dSyncPolicy> * depthScan3dSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthSyncPolicy;
-	message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
-	typedef message_filters::sync_policies::ExactTime<
-			sensor_msgs::Image,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthExactSyncPolicy;
-	message_filters::Synchronizer<MyDepthExactSyncPolicy> * depthExactSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::LaserScan,
-			nav_msgs::Odometry> MyStereoScanSyncPolicy;
-	message_filters::Synchronizer<MyStereoScanSyncPolicy> * stereoScanSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::PointCloud2,
-			nav_msgs::Odometry> MyStereoScan3dSyncPolicy;
-	message_filters::Synchronizer<MyStereoScan3dSyncPolicy> * stereoScan3dSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo,
-			nav_msgs::Odometry> MyStereoApproxSyncPolicy;
-	message_filters::Synchronizer<MyStereoApproxSyncPolicy> * stereoApproxSync_;
-
-	typedef message_filters::sync_policies::ExactTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo,
-			nav_msgs::Odometry> MyStereoExactSyncPolicy;
-	message_filters::Synchronizer<MyStereoExactSyncPolicy> * stereoExactSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepth2SyncPolicy;
-	message_filters::Synchronizer<MyDepth2SyncPolicy> * depth2Sync_;
-
-	// without odom, when TF is used for odom
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::LaserScan> MyDepthScanTFSyncPolicy;
-	message_filters::Synchronizer<MyDepthScanTFSyncPolicy> * depthScanTFSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::PointCloud2> MyDepthScan3dTFSyncPolicy;
-	message_filters::Synchronizer<MyDepthScan3dTFSyncPolicy> * depthScan3dTFSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthTFSyncPolicy;
-	message_filters::Synchronizer<MyDepthTFSyncPolicy> * depthTFSync_;
-	typedef message_filters::sync_policies::ExactTime<
-				sensor_msgs::Image,
-				sensor_msgs::Image,
-				sensor_msgs::CameraInfo> MyDepthTFExactSyncPolicy;
-	message_filters::Synchronizer<MyDepthTFExactSyncPolicy> * depthTFExactSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::LaserScan> MyStereoScanTFSyncPolicy;
-	message_filters::Synchronizer<MyStereoScanTFSyncPolicy> * stereoScanTFSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::PointCloud2> MyStereoScan3dTFSyncPolicy;
-	message_filters::Synchronizer<MyStereoScan3dTFSyncPolicy> * stereoScan3dTFSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoApproxTFSyncPolicy;
-	message_filters::Synchronizer<MyStereoApproxTFSyncPolicy> * stereoApproxTFSync_;
-
-	typedef message_filters::sync_policies::ExactTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoExactTFSyncPolicy;
-	message_filters::Synchronizer<MyStereoExactTFSyncPolicy> * stereoExactTFSync_;
-
-	tf2_ros::TransformBroadcaster tfBroadcaster_;
-	tf::TransformListener tfListener_;
-
-	ros::ServiceServer updateSrv_;
-	ros::ServiceServer resetSrv_;
-	ros::ServiceServer pauseSrv_;
-	ros::ServiceServer resumeSrv_;
-	ros::ServiceServer triggerNewMapSrv_;
-	ros::ServiceServer backupDatabase_;
-	ros::ServiceServer setModeLocalizationSrv_;
-	ros::ServiceServer setModeMappingSrv_;
-	ros::ServiceServer setLogDebugSrv_;
-	ros::ServiceServer setLogInfoSrv_;
-	ros::ServiceServer setLogWarnSrv_;
-	ros::ServiceServer setLogErrorSrv_;
-	ros::ServiceServer getMapDataSrv_;
-	ros::ServiceServer getProjMapSrv_;
-	ros::ServiceServer getGridMapSrv_;
-	ros::ServiceServer publishMapDataSrv_;
-	ros::ServiceServer setGoalSrv_;
-	ros::ServiceServer cancelGoalSrv_;
-	ros::ServiceServer setLabelSrv_;
-	ros::ServiceServer listLabelsSrv_;
-#ifdef WITH_OCTOMAP_ROS
-	ros::ServiceServer octomapBinarySrv_;
-	ros::ServiceServer octomapFullSrv_;
-#endif
-
-	MoveBaseClient mbClient_;
-
-	boost::thread* transformThread_;
-
-	float rate_;
-	bool createIntermediateNodes_;
-	ros::Time time_;
-	ros::Time previousStamp_;
-};
-
-#endif /* COREWRAPPER_H_ */
-

src/GuiNode.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "GuiWrapper.h"
+#include "rtabmap_ros/GuiWrapper.h"
 #include "rtabmap/utilite/ULogger.h"
 
 #include <QApplication>
@@ -54,7 +54,7 @@ int main(int argc, char** argv)
 	app = new QApplication(argc, argv);
 	app->connect( app, SIGNAL( lastWindowClosed() ), app, SLOT( quit() ) );
 
-	GuiWrapper * gui = new GuiWrapper(argc, argv);
+	rtabmap_ros::GuiWrapper * gui = new rtabmap_ros::GuiWrapper(argc, argv);
 
 	// Catch ctrl-c to close the gui
 	// (Place this after QApplication's constructor)

src/GuiWrapper.h

@@ -1,492 +0,0 @@
-/*
-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.
-*/
-
-#ifndef GUIWRAPPER_H_
-#define GUIWRAPPER_H_
-
-#include <ros/ros.h>
-#include "rtabmap_ros/Info.h"
-#include "rtabmap_ros/MapData.h"
-#include "rtabmap_ros/OdomInfo.h"
-#include "rtabmap_ros/Goal.h"
-#include "rtabmap/utilite/UEventsHandler.h"
-#include "rtabmap/core/Transform.h"
-
-#include <tf/transform_listener.h>
-
-#include <geometry_msgs/TwistStamped.h>
-#include <sensor_msgs/PointCloud2.h>
-#include <sensor_msgs/Image.h>
-#include <sensor_msgs/CameraInfo.h>
-#include <sensor_msgs/LaserScan.h>
-#include <nav_msgs/Odometry.h>
-#include <nav_msgs/Path.h>
-#include <std_msgs/Bool.h>
-
-#include <message_filters/subscriber.h>
-#include <message_filters/synchronizer.h>
-#include <message_filters/sync_policies/approximate_time.h>
-#include <message_filters/sync_policies/exact_time.h>
-
-#include <image_transport/image_transport.h>
-#include <image_transport/subscriber_filter.h>
-
-namespace rtabmap
-{
-	class MainWindow;
-}
-
-class QApplication;
-
-class GuiWrapper : public UEventsHandler
-{
-public:
-	GuiWrapper(int & argc, char** argv);
-	virtual ~GuiWrapper();
-
-protected:
-	virtual void handleEvent(UEvent * anEvent);
-
-private:
-	void infoMapCallback(const rtabmap_ros::InfoConstPtr & infoMsg, const rtabmap_ros::MapDataConstPtr & mapMsg);
-	void goalPathCallback(const rtabmap_ros::GoalConstPtr & goalMsg, const nav_msgs::PathConstPtr & pathMsg);
-	void goalReachedCallback(const std_msgs::BoolConstPtr & value);
-
-	void setupCallbacks(
-			bool subscribeDepth,
-			bool subscribeLaserScan2d,
-			bool subscribeLaserScan3d,
-			bool subscribeOdomInfo,
-			bool subscribeStereo,
-			int queueSize,
-			int depthCameras);
-
-	void commonDepthCallback(
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& depthMsg,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
-			const sensor_msgs::LaserScanConstPtr& scan2dMsg,
-			const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
-			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
-	void commonDepthCallback(
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
-			const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
-			const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
-			const sensor_msgs::LaserScanConstPtr& scan2dMsg,
-			const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
-			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
-	void commonStereoCallback(
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
-			const sensor_msgs::LaserScanConstPtr& scan2dMsg,
-			const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
-			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
-
-	void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg);
-
-	// With odom msg
-	void depthCallback(
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& imageDepthMsg,
-			const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
-	void depth2Callback(
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& image1Msg,
-			const sensor_msgs::ImageConstPtr& depth1Msg,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg,
-			const sensor_msgs::ImageConstPtr& image2Msg,
-			const sensor_msgs::ImageConstPtr& depth2Msg,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg);
-	void depthOdomInfoCallback(
-			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& depthMsg,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg);
-	void depthOdomInfo2Callback(
-			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& image1Msg,
-			const sensor_msgs::ImageConstPtr& depth1Msg,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg,
-			const sensor_msgs::ImageConstPtr& image2Msg,
-			const sensor_msgs::ImageConstPtr& depth2Msg,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg);
-	void depthScanCallback(
-			const sensor_msgs::LaserScanConstPtr& scanMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& imageMsg,
-		    const sensor_msgs::ImageConstPtr& imageDepthMsg,
-		    const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
-	void depthScanOdomInfoCallback(
-			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
-			const sensor_msgs::LaserScanConstPtr& scanMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& imageDepthMsg,
-			const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
-	void depthScan3dCallback(
-			const sensor_msgs::PointCloud2ConstPtr& scanMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& imageDepthMsg,
-			const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
-	void depthScan3dOdomInfoCallback(
-			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
-			const sensor_msgs::PointCloud2ConstPtr& scanMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& imageDepthMsg,
-			const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
-
-	void stereoScanCallback(
-			const sensor_msgs::LaserScanConstPtr& scanMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
-	void stereoScanOdomInfoCallback(
-			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
-			const sensor_msgs::LaserScanConstPtr& scanMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
-	void stereoScan3dCallback(
-			const sensor_msgs::PointCloud2ConstPtr& scanMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
-	void stereoScan3dOdomInfoCallback(
-			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
-			const sensor_msgs::PointCloud2ConstPtr& scanMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
-	void stereoOdomInfoCallback(
-			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
-	void stereoCallback(
-			const nav_msgs::OdometryConstPtr & odomMsg,
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
-
-	// with TF
-	void depthTFCallback(const sensor_msgs::ImageConstPtr& imageMsg,
-					   const sensor_msgs::ImageConstPtr& imageDepthMsg,
-					   const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
-	void depthOdomInfoTFCallback(
-			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& depthMsg,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg);
-	void depthScanTFCallback(
-			const sensor_msgs::LaserScanConstPtr& scanMsg,
-			const sensor_msgs::ImageConstPtr& imageMsg,
-		    const sensor_msgs::ImageConstPtr& imageDepthMsg,
-		    const sensor_msgs:: CameraInfoConstPtr& camInfoMsg);
-	void depthScan3dTFCallback(
-			const sensor_msgs::PointCloud2ConstPtr& scanMsg,
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const sensor_msgs::ImageConstPtr& imageDepthMsg,
-			const sensor_msgs:: CameraInfoConstPtr& camInfoMsg);
-
-	void stereoScanTFCallback(
-			const sensor_msgs::LaserScanConstPtr& scanMsg,
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
-	void stereoScan3dTFCallback(
-			const sensor_msgs::PointCloud2ConstPtr& scanMsg,
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
-	void stereoOdomInfoTFCallback(
-			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
-	void stereoTFCallback(
-			const sensor_msgs::ImageConstPtr& leftImageMsg,
-			const sensor_msgs::ImageConstPtr& rightImageMsg,
-			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
-			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
-
-	void processRequestedMap(const rtabmap_ros::MapData & map);
-	rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
-
-private:
-	rtabmap::MainWindow * mainWindow_;
-	std::string cameraNodeName_;
-	double lastOdomInfoUpdateTime_;
-
-	// odometry subscription stuffs
-	std::string frameId_;
-	std::string odomFrameId_;
-	bool waitForTransform_;
-	double waitForTransformDuration_;
-	tf::TransformListener tfListener_;
-
-	message_filters::Subscriber<rtabmap_ros::Info> infoTopic_;
-	message_filters::Subscriber<rtabmap_ros::MapData> mapDataTopic_;
-
-	message_filters::Subscriber<rtabmap_ros::Goal> goalTopic_;
-	message_filters::Subscriber<nav_msgs::Path> pathTopic_;
-	ros::Subscriber goalReachedTopic_;
-
-	ros::Subscriber defaultSub_; // odometry only
-	std::vector<image_transport::SubscriberFilter*> imageSubs_;
-	std::vector<image_transport::SubscriberFilter*> imageDepthSubs_;
-	std::vector<message_filters::Subscriber<sensor_msgs::CameraInfo>* > cameraInfoSubs_;
-	message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
-	message_filters::Subscriber<rtabmap_ros::OdomInfo> odomInfoSub_;
-	message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
-	message_filters::Subscriber<sensor_msgs::PointCloud2> scan3dSub_;
-
-	image_transport::SubscriberFilter imageRectLeft_;
-	image_transport::SubscriberFilter imageRectRight_;
-	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
-	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
-
-	typedef message_filters::sync_policies::ExactTime<
-			rtabmap_ros::Info,
-			rtabmap_ros::MapData> MyInfoMapSyncPolicy;
-	message_filters::Synchronizer<MyInfoMapSyncPolicy> * infoMapSync_;
-
-	typedef message_filters::sync_policies::ExactTime<
-			rtabmap_ros::Goal,
-			nav_msgs::Path> MyGoalPathSyncPolicy;
-	message_filters::Synchronizer<MyGoalPathSyncPolicy> * goalPathSync_;
-
-	// with odom msg
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::LaserScan,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthScanSyncPolicy;
-	message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			rtabmap_ros::OdomInfo,
-			sensor_msgs::LaserScan,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthScanOdomInfoSyncPolicy;
-	message_filters::Synchronizer<MyDepthScanOdomInfoSyncPolicy> * depthScanOdomInfoSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::PointCloud2,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthScan3dSyncPolicy;
-	message_filters::Synchronizer<MyDepthScan3dSyncPolicy> * depthScan3dSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			rtabmap_ros::OdomInfo,
-			sensor_msgs::PointCloud2,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthScan3dOdomInfoSyncPolicy;
-	message_filters::Synchronizer<MyDepthScan3dOdomInfoSyncPolicy> * depthScan3dOdomInfoSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthSyncPolicy;
-	message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			rtabmap_ros::OdomInfo,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthOdomInfoSyncPolicy;
-	message_filters::Synchronizer<MyDepthOdomInfoSyncPolicy> * depthOdomInfoSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoSyncPolicy;
-	message_filters::Synchronizer<MyStereoSyncPolicy> * stereoSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::LaserScan,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoScanSyncPolicy;
-	message_filters::Synchronizer<MyStereoScanSyncPolicy> * stereoScanSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			rtabmap_ros::OdomInfo,
-			sensor_msgs::LaserScan,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoScanOdomInfoSyncPolicy;
-	message_filters::Synchronizer<MyStereoScanOdomInfoSyncPolicy> * stereoScanOdomInfoSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::PointCloud2,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoScan3dSyncPolicy;
-	message_filters::Synchronizer<MyStereoScan3dSyncPolicy> * stereoScan3dSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			rtabmap_ros::OdomInfo,
-			sensor_msgs::PointCloud2,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoScan3dOdomInfoSyncPolicy;
-	message_filters::Synchronizer<MyStereoScan3dOdomInfoSyncPolicy> * stereoScan3dOdomInfoSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			rtabmap_ros::OdomInfo,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoOdomInfoSyncPolicy;
-	message_filters::Synchronizer<MyStereoOdomInfoSyncPolicy> * stereoOdomInfoSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepth2SyncPolicy;
-	message_filters::Synchronizer<MyDepth2SyncPolicy> * depth2Sync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			rtabmap_ros::OdomInfo,
-			nav_msgs::Odometry,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthOdomInfo2SyncPolicy;
-	message_filters::Synchronizer<MyDepthOdomInfo2SyncPolicy> * depthOdomInfo2Sync_;
-
-	// with odom TF
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::LaserScan,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthScanTFSyncPolicy;
-	message_filters::Synchronizer<MyDepthScanTFSyncPolicy> * depthScanTFSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::PointCloud2,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthScan3dTFSyncPolicy;
-	message_filters::Synchronizer<MyDepthScan3dTFSyncPolicy> * depthScan3dTFSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthTFSyncPolicy;
-	message_filters::Synchronizer<MyDepthTFSyncPolicy> * depthTFSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			rtabmap_ros::OdomInfo,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo> MyDepthOdomInfoTFSyncPolicy;
-	message_filters::Synchronizer<MyDepthOdomInfoTFSyncPolicy> * depthOdomInfoTFSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoTFSyncPolicy;
-	message_filters::Synchronizer<MyStereoTFSyncPolicy> * stereoTFSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::LaserScan,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoScanTFSyncPolicy;
-	message_filters::Synchronizer<MyStereoScanTFSyncPolicy> * stereoScanTFSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::PointCloud2,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoScan3dTFSyncPolicy;
-	message_filters::Synchronizer<MyStereoScan3dTFSyncPolicy> * stereoScan3dTFSync_;
-
-	typedef message_filters::sync_policies::ApproximateTime<
-			rtabmap_ros::OdomInfo,
-			sensor_msgs::Image,
-			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::CameraInfo> MyStereoOdomInfoTFSyncPolicy;
-	message_filters::Synchronizer<MyStereoOdomInfoTFSyncPolicy> * stereoOdomInfoTFSync_;
-};
-
-#endif /* GUIWRAPPER_H_ */

src/ICPOdometryNode.cpp

@@ -0,0 +1,78 @@
+/*
+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"
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/core/Parameters.h>
+
+int main(int argc, char **argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+	ros::init(argc, argv, "icp_odometry");
+
+	// process "--params" argument
+	nodelet::V_string nargv;
+	for(int i=1;i<argc;++i)
+	{
+		if(strcmp(argv[i], "--params") == 0)
+		{
+			rtabmap::ParametersMap parametersOdom = rtabmap::Parameters::getDefaultOdometryParameters(false, false, true);
+			for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
+			{
+				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
+				std::cout <<
+						str <<
+						std::setw(60 - str.size()) <<
+						" [" <<
+						rtabmap::Parameters::getDescription(iter->first).c_str() <<
+						"]" <<
+						std::endl;
+			}
+			ROS_WARN("Node will now exit after showing default odometry parameters because "
+					 "argument \"--params\" is detected!");
+			exit(0);
+		}
+		else if(strcmp(argv[i], "--udebug") == 0)
+		{
+			ULogger::setLevel(ULogger::kDebug);
+		}
+		else if(strcmp(argv[i], "--uinfo") == 0)
+		{
+			ULogger::setLevel(ULogger::kInfo);
+		}
+		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/icp_odometry", remap, nargv);
+	ros::spin();
+	return 0;
+}

src/MapAssemblerNode.cpp

@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <ros/ros.h>
 #include "rtabmap_ros/MapData.h"
 #include "rtabmap_ros/MsgConversion.h"
-#include "MapsManager.h"
+#include "rtabmap_ros/MapsManager.h"
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_filtering.h>
@@ -49,12 +49,13 @@ class MapAssembler
 {
 
 public:
-	MapAssembler() :
-		mapsManager_(false)
+	MapAssembler()
 	{
 		ros::NodeHandle pnh("~");
-
 		ros::NodeHandle nh;
+
+		mapsManager_.init(nh, pnh, ros::this_node::getName(), false);
+
 		mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this);
 
 		// private service
@@ -99,9 +100,6 @@ public:
 				0,
 				false,
 				false,
-				false,
-				false,
-				false,
 				nodes_);
 
 		mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id);

src/MapOptimizerNode.cpp

@@ -84,7 +84,7 @@ public:
 		parameters.insert(ParametersPair(Parameters::kOptimizerEpsilon(), uNumber2Str(epsilon)));
 		parameters.insert(ParametersPair(Parameters::kOptimizerIterations(), uNumber2Str(iterations)));
 		parameters.insert(ParametersPair(Parameters::kOptimizerRobust(), uBool2Str(robust)));
-		parameters.insert(ParametersPair(Parameters::kOptimizerSlam2D(), uBool2Str(slam2d)));
+		parameters.insert(ParametersPair(Parameters::kRegForce3DoF(), uBool2Str(slam2d)));
 		parameters.insert(ParametersPair(Parameters::kOptimizerVarianceIgnored(), uBool2Str(ignoreVariance)));
 		optimizer_ = Optimizer::create(parameters);
 
@@ -254,10 +254,10 @@ public:
 			{
 				optimizedPoses = poses;
 			}
-			else if(poses.size() || constraints.size())
+			else if(poses.size() == 0 && constraints.size())
 			{
-				ROS_ERROR("map_optimizer: Poses=%d and edges=%d (poses must "
-					   "not be null if there are edges, and edges must be null if poses <= 1)",
+				ROS_ERROR("map_optimizer: Poses=%d and edges=%d: poses must "
+					   "not be null if there are edges.",
 					  (int)poses.size(), (int)constraints.size());
 			}
 

src/MsgConversion.cpp

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <zlib.h>
 #include <ros/ros.h>
 #include <rtabmap/core/util3d.h>
+#include <rtabmap/core/Compression.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <pcl_conversions/pcl_conversions.h>
@@ -38,6 +39,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <tf_conversions/tf_eigen.h>
 #include <image_geometry/pinhole_camera_model.h>
 #include <image_geometry/stereo_camera_model.h>
+#include <sensor_msgs/image_encodings.h>
+#include <laser_geometry/laser_geometry.h>
+#include <rtabmap/core/util3d_surface.h>
 
 namespace rtabmap_ros {
 
@@ -114,6 +118,78 @@ rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg)
 	return rtabmap::Transform::fromEigen3d(tfPose);
 }
 
+void toCvCopy(const rtabmap_ros::RGBDImage & image, cv_bridge::CvImagePtr & rgb, cv_bridge::CvImagePtr & depth)
+{
+	if(!image.rgb.data.empty())
+	{
+		rgb = cv_bridge::toCvCopy(image.rgb);
+	}
+	else if(!image.rgbCompressed.data.empty())
+	{
+		rgb = cv_bridge::toCvCopy(image.rgbCompressed);
+	}
+	else
+	{
+		// empty
+		rgb = boost::make_shared<cv_bridge::CvImage>();
+	}
+
+	if(!image.depth.data.empty())
+	{
+		depth = cv_bridge::toCvCopy(image.depth);
+	}
+	else if(!image.depthCompressed.data.empty())
+	{
+		cv_bridge::CvImagePtr ptr = boost::make_shared<cv_bridge::CvImage>();
+		ptr->header = image.depthCompressed.header;
+		ptr->image = rtabmap::uncompressImage(image.depthCompressed.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;
+	}
+	else
+	{
+		// empty
+		depth = boost::make_shared<cv_bridge::CvImage>();
+	}
+}
+
+void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth)
+{
+	if(!image->rgb.data.empty())
+	{
+		rgb = cv_bridge::toCvShare(image->rgb, image);
+	}
+	else if(!image->rgbCompressed.data.empty())
+	{
+		rgb = cv_bridge::toCvCopy(image->rgbCompressed);
+	}
+	else
+	{
+		// empty
+		rgb = boost::make_shared<cv_bridge::CvImage>();
+	}
+
+	if(!image->depth.data.empty())
+	{
+		depth = cv_bridge::toCvShare(image->depth, image);
+	}
+	else if(!image->depthCompressed.data.empty())
+	{
+		cv_bridge::CvImagePtr ptr = boost::make_shared<cv_bridge::CvImage>();
+		ptr->header = image->depthCompressed.header;
+		ptr->image = rtabmap::uncompressImage(image->depthCompressed.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;
+	}
+	else
+	{
+		// empty
+		depth = boost::make_shared<cv_bridge::CvImage>();
+	}
+}
+
 void compressedMatToBytes(const cv::Mat & compressed, std::vector<unsigned char> & bytes)
 {
 	UASSERT(compressed.empty() || compressed.type() == CV_8UC1);
@@ -331,16 +407,42 @@ rtabmap::CameraModel cameraModelFromROS(
 		const sensor_msgs::CameraInfo & camInfo,
 		const rtabmap::Transform & localTransform)
 {
-	image_geometry::PinholeCameraModel model;
-	model.fromCameraInfo(camInfo);
+	cv::Mat D;
+	if(camInfo.D.size())
+	{
+		D = cv::Mat(1, camInfo.D.size(), CV_64FC1);
+		memcpy(D.data, camInfo.D.data(), D.cols*sizeof(double));
+	}
+
+	cv:: Mat K;
+	UASSERT(camInfo.K.empty() || camInfo.K.size() == 9);
+	if(!camInfo.K.empty())
+	{
+		K = cv::Mat(3, 3, CV_64FC1);
+		memcpy(K.data, camInfo.K.elems, 9*sizeof(double));
+	}
+
+	cv:: Mat R;
+	UASSERT(camInfo.R.empty() || camInfo.R.size() == 9);
+	if(!camInfo.R.empty())
+	{
+		R = cv::Mat(3, 3, CV_64FC1);
+		memcpy(R.data, camInfo.R.elems, 9*sizeof(double));
+	}
+
+	cv:: Mat P;
+	UASSERT(camInfo.P.empty() || camInfo.P.size() == 12);
+	if(!camInfo.P.empty())
+	{
+		P = cv::Mat(3, 4, CV_64FC1);
+		memcpy(P.data, camInfo.P.elems, 12*sizeof(double));
+	}
+
 	return rtabmap::CameraModel(
-			model.fx(),
-			model.fy(),
-			model.cx(),
-			model.cy(),
-			localTransform,
-			0.0,
-			cv::Size(model.fullResolution().width, model.fullResolution().height));
+			"ros",
+			cv::Size(camInfo.width, camInfo.height),
+			K, D, R, P,
+			localTransform);
 }
 void cameraModelToROS(
 		const rtabmap::CameraModel & model,
@@ -400,16 +502,11 @@ rtabmap::StereoCameraModel stereoCameraModelFromROS(
 		const sensor_msgs::CameraInfo & rightCamInfo,
 		const rtabmap::Transform & localTransform)
 {
-	image_geometry::StereoCameraModel model;
-	model.fromCameraInfo(leftCamInfo, rightCamInfo);
 	return rtabmap::StereoCameraModel(
-			model.left().fx(),
-			model.left().fy(),
-			model.left().cx(),
-			model.left().cy(),
-			model.baseline(),
-			localTransform,
-			cv::Size(model.left().fullResolution().width, model.left().fullResolution().height));
+			"ros",
+			cameraModelFromROS(leftCamInfo, localTransform),
+			cameraModelFromROS(rightCamInfo, localTransform),
+			rtabmap::Transform());
 }
 
 void mapDataFromROS(
@@ -504,11 +601,14 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 	//Features stuff...
 	std::multimap<int, cv::KeyPoint> words;
 	std::multimap<int, cv::Point3f> words3D;
+	std::multimap<int, cv::Mat> wordsDescriptors;
 	pcl::PointCloud<pcl::PointXYZ> cloud;
+	cv::Mat descriptors;
 	if(msg.wordPts.data.size() &&
 	   msg.wordPts.height*msg.wordPts.width == msg.wordIds.size())
 	{
 		pcl::fromROSMsg(msg.wordPts, cloud);
+		descriptors = rtabmap::uncompressData(msg.descriptors);
 	}
 
 	for(unsigned int i=0; i<msg.wordIds.size() && i<msg.wordKpts.size(); ++i)
@@ -520,6 +620,10 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 		{
 			words3D.insert(std::make_pair(wordId, cv::Point3f(cloud[i].x, cloud[i].y, cloud[i].z)));
 		}
+		if(i < descriptors.rows)
+		{
+			wordsDescriptors.insert(std::make_pair(wordId, descriptors.row(i).clone()));
+		}
 	}
 
 	if(words3D.size() && words3D.size() != words.size())
@@ -533,9 +637,11 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 	{
 		// stereo model
 		if(msg.fx.size() == 1 &&
-		   msg.fy.size() == 1,
-		   msg.cx.size() == 1,
-		   msg.cy.size() == 1,
+		   msg.fy.size() == 1 &&
+		   msg.cx.size() == 1 &&
+		   msg.cy.size() == 1 &&
+		   msg.width.size() == 1 &&
+		   msg.height.size() == 1 &&
 		   msg.localTransform.size() == 1)
 		{
 			stereoModel = rtabmap::StereoCameraModel(
@@ -544,7 +650,8 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 					msg.cx[0],
 					msg.cy[0],
 					msg.baseline,
-					transformFromGeometryMsg(msg.localTransform[0]));
+					transformFromGeometryMsg(msg.localTransform[0]),
+					cv::Size(msg.width[0], msg.height[0]));
 		}
 	}
 	else
@@ -563,7 +670,9 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 						msg.fy[i],
 						msg.cx[i],
 						msg.cy[i],
-						transformFromGeometryMsg(msg.localTransform[i])));
+						transformFromGeometryMsg(msg.localTransform[i]),
+						0.0,
+						cv::Size(msg.width[i], msg.height[i])));
 			}
 		}
 	}
@@ -579,8 +688,10 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 			stereoModel.isValidForProjection()?
 				rtabmap::SensorData(
 					compressedMatFromBytes(msg.laserScan),
-					msg.laserScanMaxPts,
-					msg.laserScanMaxRange,
+					rtabmap::LaserScanInfo(
+							msg.laserScanMaxPts,
+							msg.laserScanMaxRange,
+							transformFromGeometryMsg(msg.laserScanLocalTransform)),
 					compressedMatFromBytes(msg.image),
 					compressedMatFromBytes(msg.depth),
 					stereoModel,
@@ -589,8 +700,10 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 					compressedMatFromBytes(msg.userData)):
 				rtabmap::SensorData(
 					compressedMatFromBytes(msg.laserScan),
-					msg.laserScanMaxPts,
-					msg.laserScanMaxRange,
+					rtabmap::LaserScanInfo(
+							msg.laserScanMaxPts,
+							msg.laserScanMaxRange,
+							transformFromGeometryMsg(msg.laserScanLocalTransform)),
 					compressedMatFromBytes(msg.image),
 					compressedMatFromBytes(msg.depth),
 					models,
@@ -599,6 +712,12 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 					compressedMatFromBytes(msg.userData)));
 	s.setWords(words);
 	s.setWords3(words3D);
+	s.setWordsDescriptors(wordsDescriptors);
+	s.sensorData().setOccupancyGrid(
+			compressedMatFromBytes(msg.grid_ground),
+			compressedMatFromBytes(msg.grid_obstacles),
+			msg.grid_cell_size,
+			point3fFromROS(msg.grid_view_point));
 	return s;
 }
 void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg)
@@ -615,8 +734,13 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 	compressedMatToBytes(signature.sensorData().depthOrRightCompressed(), msg.depth);
 	compressedMatToBytes(signature.sensorData().laserScanCompressed(), msg.laserScan);
 	compressedMatToBytes(signature.sensorData().userDataCompressed(), msg.userData);
-	msg.laserScanMaxPts = signature.sensorData().laserScanMaxPts();
-	msg.laserScanMaxRange = signature.sensorData().laserScanMaxRange();
+	compressedMatToBytes(signature.sensorData().gridGroundCellsCompressed(), msg.grid_ground);
+	compressedMatToBytes(signature.sensorData().gridObstacleCellsCompressed(), msg.grid_obstacles);
+	point3fToROS(signature.sensorData().gridViewPoint(), msg.grid_view_point);
+	msg.grid_cell_size = signature.sensorData().gridCellSize();
+	msg.laserScanMaxPts = signature.sensorData().laserScanInfo().maxPoints();
+	msg.laserScanMaxRange = signature.sensorData().laserScanInfo().maxRange();
+	transformToGeometryMsg(signature.sensorData().laserScanInfo().localTransform(), msg.laserScanLocalTransform);
 	msg.baseline = 0;
 	if(signature.sensorData().cameraModels().size())
 	{
@@ -624,6 +748,8 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 		msg.fy.resize(signature.sensorData().cameraModels().size());
 		msg.cx.resize(signature.sensorData().cameraModels().size());
 		msg.cy.resize(signature.sensorData().cameraModels().size());
+		msg.width.resize(signature.sensorData().cameraModels().size());
+		msg.height.resize(signature.sensorData().cameraModels().size());
 		msg.localTransform.resize(signature.sensorData().cameraModels().size());
 		for(unsigned int i=0; i<signature.sensorData().cameraModels().size(); ++i)
 		{
@@ -631,6 +757,8 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 			msg.fy[i] = signature.sensorData().cameraModels()[i].fy();
 			msg.cx[i] = signature.sensorData().cameraModels()[i].cx();
 			msg.cy[i] = signature.sensorData().cameraModels()[i].cy();
+			msg.width[i] = signature.sensorData().cameraModels()[i].imageWidth();
+			msg.height[i] = signature.sensorData().cameraModels()[i].imageHeight();
 			transformToGeometryMsg(signature.sensorData().cameraModels()[i].localTransform(), msg.localTransform[i]);
 		}
 	}
@@ -640,6 +768,8 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 		msg.fy.push_back(signature.sensorData().stereoCameraModel().left().fy());
 		msg.cx.push_back(signature.sensorData().stereoCameraModel().left().cx());
 		msg.cy.push_back(signature.sensorData().stereoCameraModel().left().cy());
+		msg.width.push_back(signature.sensorData().stereoCameraModel().left().imageWidth());
+		msg.height.push_back(signature.sensorData().stereoCameraModel().left().imageHeight());
 		msg.baseline = signature.sensorData().stereoCameraModel().baseline();
 		msg.localTransform.resize(1);
 		transformToGeometryMsg(signature.sensorData().stereoCameraModel().left().localTransform(), msg.localTransform[0]);
@@ -677,6 +807,42 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 				(int)signature.getWords().size(),
 				(int)signature.getWords3().size());
 	}
+
+	if(signature.getWordsDescriptors().size() && signature.getWordsDescriptors().size() == signature.getWords().size())
+	{
+		cv::Mat descriptors(
+				signature.getWordsDescriptors().size(),
+				signature.getWordsDescriptors().begin()->second.cols,
+				signature.getWordsDescriptors().begin()->second.type());
+		index = 0;
+		bool valid = true;
+		for(std::multimap<int, cv::Mat>::const_iterator jter=signature.getWordsDescriptors().begin();
+			jter!=signature.getWordsDescriptors().end() && valid;
+			++jter)
+		{
+			if(jter->second.cols == descriptors.cols &&
+				jter->second.type() == descriptors.type())
+			{
+				jter->second.copyTo(descriptors.row(index++));
+			}
+			else
+			{
+				valid = false;
+				ROS_ERROR("Some descriptors have different type/size! Cannot copy them...");
+			}
+		}
+
+		if(valid)
+		{
+			msg.descriptors = rtabmap::compressData(descriptors);
+		}
+	}
+	else if(signature.getWordsDescriptors().size())
+	{
+		ROS_ERROR("Words and descriptors must have the same size (%d vs %d)!",
+				(int)signature.getWords().size(),
+				(int)signature.getWordsDescriptors().size());
+	}
 }
 
 rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData & msg)
@@ -711,8 +877,10 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
 	info.features = msg.features;
 	info.inliers = msg.inliers;
 	info.localMapSize = msg.localMapSize;
+	info.localScanMapSize = msg.localScanMapSize;
 	info.timeEstimation = msg.timeEstimation;
-	info.variance = msg.variance;
+	info.varianceLin = msg.varianceLin;
+	info.varianceAng = msg.varianceAng;
 	info.timeParticleFiltering =  msg.timeParticleFiltering;
 	info.stamp = msg.stamp;
 	info.interval = msg.interval;
@@ -742,6 +910,8 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
 		info.localMap.insert(std::make_pair(msg.localMapKeys[i], point3fFromROS(msg.localMapValues[i])));
 	}
 
+	info.localScanMap = rtabmap::uncompressData(msg.localScanMap);
+
 	return info;
 }
 
@@ -752,8 +922,10 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
 	msg.features = info.features;
 	msg.inliers = info.inliers;
 	msg.localMapSize = info.localMapSize;
+	msg.localScanMapSize = info.localScanMapSize;
 	msg.timeEstimation = info.timeEstimation;
-	msg.variance = info.variance;
+	msg.varianceLin = info.varianceLin;
+	msg.varianceAng = info.varianceAng;
 	msg.timeParticleFiltering =  info.timeParticleFiltering;
 	msg.stamp = info.stamp;
 	msg.interval = info.interval;
@@ -777,6 +949,492 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
 	msg.localMapKeys = uKeys(info.localMap);
 	points3fToROS(uValues(info.localMap), msg.localMapValues);
 
+	msg.localScanMap = rtabmap::compressData(info.localScanMap);
+}
+
+cv::Mat userDataFromROS(const rtabmap_ros::UserData & dataMsg)
+{
+	cv::Mat data;
+	if(!dataMsg.data.empty())
+	{
+		if(dataMsg.cols > 0 && dataMsg.rows > 0 && dataMsg.type >= 0)
+		{
+			data = cv::Mat(dataMsg.rows, dataMsg.cols, dataMsg.type, (void*)dataMsg.data.data()).clone();
+		}
+		else
+		{
+			if(dataMsg.cols != (int)dataMsg.data.size() || dataMsg.rows != 1 || dataMsg.type != CV_8UC1)
+			{
+				ROS_ERROR("cols, rows and type fields of the UserData msg "
+						"are not correctly set (cols=%d, rows=%d, type=%d)! We assume that the data "
+						"is compressed (cols=%d, rows=1, type=%d(CV_8UC1)).",
+						dataMsg.cols, dataMsg.rows, dataMsg.type, (int)dataMsg.data.size(), CV_8UC1);
+
+			}
+			data = cv::Mat(1, dataMsg.data.size(), CV_8UC1, (void*)dataMsg.data.data()).clone();
+		}
+	}
+	return data;
+}
+void userDataToROS(const cv::Mat & data, rtabmap_ros::UserData & dataMsg, bool compress)
+{
+	if(!data.empty())
+	{
+		if(compress)
+		{
+			dataMsg.data = rtabmap::compressData(data);
+			dataMsg.rows = 1;
+			dataMsg.cols = dataMsg.data.size();
+			dataMsg.type = CV_8UC1;
+		}
+		else
+		{
+			dataMsg.data.resize(data.step[0] * data.rows); // use step for non-contiguous matrices
+			memcpy(dataMsg.data.data(), data.data, dataMsg.data.size());
+			dataMsg.rows = data.rows;
+			dataMsg.cols = data.cols;
+			dataMsg.type = data.type();
+		}
+	}
+}
+
+rtabmap::Transform getTransform(
+		const std::string & fromFrameId,
+		const std::string & toFrameId,
+		const ros::Time & stamp,
+		tf::TransformListener & listener,
+		double waitForTransform)
+{
+	// TF ready?
+	rtabmap::Transform transform;
+	try
+	{
+		if(waitForTransform > 0.0 && !stamp.isZero())
+		{
+			//if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
+			std::string errorMsg;
+			if(!listener.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransform), ros::Duration(0.01), &errorMsg))
+			{
+				ROS_WARN("Could not get transform from %s to %s after %f seconds (for stamp=%f)! Error=\"%s\".",
+						fromFrameId.c_str(), toFrameId.c_str(), waitForTransform, stamp.toSec(), errorMsg.c_str());
+				return transform;
+			}
+		}
+
+		tf::StampedTransform tmp;
+		listener.lookupTransform(fromFrameId, toFrameId, stamp, tmp);
+		transform = rtabmap_ros::transformFromTF(tmp);
+	}
+	catch(tf::TransformException & ex)
+	{
+		ROS_WARN("%s",ex.what());
+	}
+	return transform;
+}
+
+// get moving transform accordingly to a fixed frame. For example get
+// transform between moving /base_link between two stamps accordingly to /odom frame.
+rtabmap::Transform getTransform(
+		const std::string & sourceTargetFrame,
+		const std::string & fixedFrame,
+		const ros::Time & stampSource,
+		const ros::Time & stampTarget,
+		tf::TransformListener & listener,
+		double waitForTransform)
+{
+	// TF ready?
+	rtabmap::Transform transform;
+	try
+	{
+		ros::Time stamp = stampSource>stampTarget?stampSource:stampTarget;
+		if(waitForTransform > 0.0 && !stamp.isZero())
+		{
+			std::string errorMsg;
+			if(!listener.waitForTransform(sourceTargetFrame, fixedFrame, stamp, ros::Duration(waitForTransform), ros::Duration(0.01), &errorMsg))
+			{
+				ROS_WARN("Could not get transform from %s to %s accordingly to %s after %f seconds (for stamps=%f -> %f)! Error=\"%s\".",
+						sourceTargetFrame.c_str(), sourceTargetFrame.c_str(), fixedFrame.c_str(), waitForTransform, stampSource.toSec(), stampTarget.toSec(), errorMsg.c_str());
+				return transform;
+			}
+		}
+
+		tf::StampedTransform tmp;
+		listener.lookupTransform(sourceTargetFrame, stampTarget, sourceTargetFrame, stampSource, fixedFrame, tmp);
+		transform = rtabmap_ros::transformFromTF(tmp);
+	}
+	catch(tf::TransformException & ex)
+	{
+		ROS_WARN("%s",ex.what());
+	}
+	return transform;
+}
+
+bool convertRGBDMsgs(
+		const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
+		const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
+		const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
+		const std::string & frameId,
+		const std::string & odomFrameId,
+		const ros::Time & odomStamp,
+		cv::Mat & rgb,
+		cv::Mat & depth,
+		std::vector<rtabmap::CameraModel> & cameraModels,
+		tf::TransformListener & listener,
+		double waitForTransform)
+{
+	UASSERT(imageMsgs.size()>0 &&
+			imageMsgs.size() == depthMsgs.size() &&
+			imageMsgs.size() == cameraInfoMsgs.size());
+
+	int imageWidth = imageMsgs[0]->image.cols;
+	int imageHeight = imageMsgs[0]->image.rows;
+	int cameraCount = imageMsgs.size();
+	for(unsigned int i=0; i<imageMsgs.size(); ++i)
+	{
+		if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
+			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
+			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
+			!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
+			 depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
+			 depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
+		{
+			ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
+			return false;
+		}
+
+		UASSERT_MSG(imageMsgs[i]->image.cols == imageWidth && imageMsgs[i]->image.rows == imageHeight,
+				uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
+						imageWidth,
+						imageMsgs[i]->image.cols,
+						imageHeight,
+						imageMsgs[i]->image.rows).c_str());
+		UASSERT_MSG(depthMsgs[i]->image.cols == imageWidth && depthMsgs[i]->image.rows == imageHeight,
+				uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
+						imageWidth,
+						depthMsgs[i]->image.cols,
+						imageHeight,
+						depthMsgs[i]->image.rows).c_str());
+
+		rtabmap::Transform localTransform = rtabmap_ros::getTransform(frameId, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp, listener, waitForTransform);
+		if(localTransform.isNull())
+		{
+			ROS_ERROR("TF of received depth image %d at time %fs is not set!", i, depthMsgs[i]->header.stamp.toSec());
+			return false;
+		}
+		// sync with odometry stamp
+		if(!odomFrameId.empty() && odomStamp != depthMsgs[i]->header.stamp)
+		{
+			rtabmap::Transform sensorT = getTransform(
+					frameId,
+					odomFrameId,
+					odomStamp,
+					depthMsgs[i]->header.stamp,
+					listener,
+					waitForTransform);
+			if(sensorT.isNull())
+			{
+				ROS_WARN("Could not get odometry value for depth image stamp (%fs). Latest odometry "
+						"stamp is %fs. The depth image pose will not be synchronized with odometry.", depthMsgs[i]->header.stamp.toSec(), odomStamp.toSec());
+			}
+			else
+			{
+				//ROS_WARN("RGBD correction = %s (time diff=%fs)", sensorT.prettyPrint().c_str(), fabs(depthMsgs[i]->header.stamp.toSec()-odomStamp.toSec()));
+				localTransform = sensorT * localTransform;
+			}
+		}
+
+		cv_bridge::CvImageConstPtr ptrImage = imageMsgs[i];
+		if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
+		   imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+		   imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0)
+		{
+			// do nothing
+		}
+		else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+		{
+			ptrImage = cv_bridge::cvtColor(imageMsgs[i], "mono8");
+		}
+		else
+		{
+			ptrImage = cv_bridge::cvtColor(imageMsgs[i], "bgr8");
+		}
+		cv_bridge::CvImageConstPtr ptrDepth = depthMsgs[i];
+		cv::Mat subDepth = ptrDepth->image;
+
+		// initialize
+		if(rgb.empty())
+		{
+			rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
+		}
+		if(depth.empty())
+		{
+			depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type());
+		}
+
+		if(ptrImage->image.type() == rgb.type())
+		{
+			ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
+		}
+		else
+		{
+			ROS_ERROR("Some RGB images are not the same type!");
+			return false;
+		}
+
+		if(subDepth.type() == depth.type())
+		{
+			subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
+		}
+		else
+		{
+			ROS_ERROR("Some Depth images are not the same type!");
+			return false;
+		}
+
+		cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfoMsgs[i], localTransform));
+	}
+	return true;
+}
+
+bool convertStereoMsg(
+		const sensor_msgs::ImageConstPtr& leftImageMsg,
+		const sensor_msgs::ImageConstPtr& rightImageMsg,
+		const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+		const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+		const std::string & frameId,
+		const std::string & odomFrameId,
+		const ros::Time & odomStamp,
+		cv::Mat & left,
+		cv::Mat & right,
+		rtabmap::StereoCameraModel & stereoModel,
+		tf::TransformListener & listener,
+		double waitForTransform)
+{
+	UASSERT(leftImageMsg.get() && rightImageMsg.get());
+	UASSERT(leftCamInfoMsg.get() && rightCamInfoMsg.get());
+
+	if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+		leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
+		leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+		leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
+		!(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+		rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
+		rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+		rightImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
+	{
+		ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8");
+		return false;
+	}
+
+	if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+	   leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+	{
+		left = cv_bridge::toCvCopy(leftImageMsg, "mono8")->image;
+	}
+	else
+	{
+		left = cv_bridge::toCvCopy(leftImageMsg, "bgr8")->image;
+	}
+	right = cv_bridge::toCvCopy(rightImageMsg, "mono8")->image;
+
+	rtabmap::Transform localTransform = getTransform(frameId, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, listener, waitForTransform);
+	if(localTransform.isNull())
+	{
+		return false;
+	}
+	// sync with odometry stamp
+	if(!odomFrameId.empty() && odomStamp != leftImageMsg->header.stamp)
+	{
+		rtabmap::Transform sensorT = getTransform(
+				frameId,
+				odomFrameId,
+				odomStamp,
+				leftImageMsg->header.stamp,
+				listener,
+				waitForTransform);
+		if(sensorT.isNull())
+		{
+			ROS_WARN("Could not get odometry value for stereo msg stamp (%fs). Latest odometry "
+					"stamp is %fs. The stereo image pose will not be synchronized with odometry.", leftImageMsg->header.stamp.toSec(), odomStamp.toSec());
+		}
+		else
+		{
+			localTransform = sensorT * localTransform;
+		}
+	}
+
+	stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform);
+
+	if(stereoModel.baseline() > 10.0)
+	{
+		static bool shown = false;
+		if(!shown)
+		{
+			ROS_WARN("Detected baseline (%f m) is quite large! Is your "
+					 "right camera_info P(0,3) correctly set? Note that "
+					 "baseline=-P(0,3)/P(0,0). You may need to calibrate your camera. "
+					 "This warning is printed only once.",
+					 stereoModel.baseline());
+			shown = true;
+		}
+	}
+	return true;
+}
+
+bool convertScanMsg(
+		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+		const std::string & frameId,
+		const std::string & odomFrameId,
+		const ros::Time & odomStamp,
+		cv::Mat & scan,
+		rtabmap::Transform & scanLocalTransform,
+		tf::TransformListener & listener,
+		double waitForTransform)
+{
+	// make sure the frame of the laser is updated too
+	rtabmap::Transform tmpT = getTransform(
+			odomFrameId.empty()?frameId:odomFrameId,
+			scan2dMsg->header.frame_id,
+			scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment),
+			listener,
+			waitForTransform);
+	if(tmpT.isNull())
+	{
+		return false;
+	}
+
+	scanLocalTransform = getTransform(
+			frameId,
+			scan2dMsg->header.frame_id,
+			scan2dMsg->header.stamp,
+			listener,
+			waitForTransform);
+	if(scanLocalTransform.isNull())
+	{
+		return false;
+	}
+
+	//transform in frameId_ frame
+	sensor_msgs::PointCloud2 scanOut;
+	laser_geometry::LaserProjection projection;
+	projection.transformLaserScanToPointCloud(odomFrameId.empty()?frameId:odomFrameId, *scan2dMsg, scanOut, listener);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::fromROSMsg(scanOut, *pclScan);
+
+	//transform back in laser frame
+	rtabmap::Transform laserToOdom = getTransform(
+			scan2dMsg->header.frame_id,
+			odomFrameId.empty()?frameId:odomFrameId,
+			scan2dMsg->header.stamp,
+			listener,
+			waitForTransform);
+	if(laserToOdom.isNull())
+	{
+		return false;
+	}
+
+	// sync with odometry stamp
+	if(!odomFrameId.empty() && odomStamp != scan2dMsg->header.stamp)
+	{
+		rtabmap::Transform sensorT = getTransform(
+				frameId,
+				odomFrameId,
+				odomStamp,
+				scan2dMsg->header.stamp,
+				listener,
+				waitForTransform);
+		if(sensorT.isNull())
+		{
+			ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry "
+					"stamp is %fs. The laser scan pose will not be synchronized with odometry.", scan2dMsg->header.stamp.toSec(), odomStamp.toSec());
+		}
+		else
+		{
+			//ROS_WARN("scan correction = %s (time diff=%fs)", sensorT.prettyPrint().c_str(), fabs(scan2dMsg->header.stamp.toSec()-odomStamp.toSec()));
+			scanLocalTransform = sensorT * scanLocalTransform;
+		}
+	}
+	scan = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom); // put back in laser frame
+	return true;
+}
+
+bool convertScan3dMsg(
+		const sensor_msgs::PointCloud2ConstPtr & scan3dMsg,
+		const std::string & frameId,
+		const std::string & odomFrameId,
+		const ros::Time & odomStamp,
+		int scanCloudNormalK,
+		cv::Mat & scan,
+		rtabmap::Transform & scanLocalTransform,
+		tf::TransformListener & listener,
+		double waitForTransform)
+{
+	bool containNormals = false;
+	for(unsigned int i=0; i<scan3dMsg->fields.size(); ++i)
+	{
+		if(scan3dMsg->fields[i].name.compare("normal_x") == 0)
+		{
+			containNormals = true;
+			break;
+		}
+	}
+
+	scanLocalTransform = getTransform(frameId, scan3dMsg->header.frame_id, scan3dMsg->header.stamp, listener, waitForTransform);
+	if(scanLocalTransform.isNull())
+	{
+		ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", scan3dMsg->header.stamp.toSec());
+		return false;
+	}
+
+	// sync with odometry stamp
+	if(!odomFrameId.empty() && odomStamp != scan3dMsg->header.stamp)
+	{
+		rtabmap::Transform sensorT = getTransform(
+				frameId,
+				odomFrameId,
+				odomStamp,
+				scan3dMsg->header.stamp,
+				listener,
+				waitForTransform);
+		if(sensorT.isNull())
+		{
+			ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry "
+					"stamp is %fs. The 3d laser scan pose will not be synchronized with odometry.", scan3dMsg->header.stamp.toSec(), odomStamp.toSec());
+		}
+		else
+		{
+			scanLocalTransform = sensorT * scanLocalTransform;
+		}
+	}
+
+	if(containNormals)
+	{
+		pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
+		pcl::fromROSMsg(*scan3dMsg, *pclScan);
+
+		scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
+	}
+	else
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::fromROSMsg(*scan3dMsg, *pclScan);
+
+		if(scanCloudNormalK > 0)
+		{
+			//compute normals
+			pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclScan, scanCloudNormalK);
+			pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
+			pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+			scan = rtabmap::util3d::laserScanFromPointCloud(*pclScanNormal);
+		}
+		else
+		{
+			scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
+		}
+	}
+	return true;
 }
 
 }

src/OdometryROS.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "OdometryROS.h"
+#include "rtabmap_ros/OdometryROS.h"
 
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/image_encodings.h>
@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Rtabmap.h>
 #include <rtabmap/core/OdometryF2M.h>
 #include <rtabmap/core/OdometryF2F.h>
+#include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/Memory.h>
 #include <rtabmap/core/Signature.h>
@@ -55,11 +56,14 @@ using namespace rtabmap;
 
 namespace rtabmap_ros {
 
-OdometryROS::OdometryROS(bool stereo) :
+OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
 	odometry_(0),
+	warningThread_(0),
+	callbackCalled_(false),
 	frameId_("base_link"),
 	odomFrameId_("odom"),
 	groundTruthFrameId_(""),
+	guessFrameId_(""),
 	publishTf_(true),
 	waitForTransform_(true),
 	waitForTransformDuration_(0.1), // 100 ms
@@ -68,13 +72,21 @@ OdometryROS::OdometryROS(bool stereo) :
 	paused_(false),
 	resetCountdown_(0),
 	resetCurrentCount_(0),
-	stereo_(stereo)
+	stereoParams_(stereoParams),
+	visParams_(visParams),
+	icpParams_(icpParams)
 {
 
 }
 
 OdometryROS::~OdometryROS()
 {
+	if(warningThread_)
+	{
+		callbackCalled();
+		warningThread_->join();
+		delete warningThread_;
+	}
 	ros::NodeHandle & pnh = getPrivateNodeHandle();
 	if(pnh.ok())
 	{
@@ -95,6 +107,7 @@ void OdometryROS::onInit()
 	odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1);
 	odomInfoPub_ = nh.advertise<rtabmap_ros::OdomInfo>("odom_info", 1);
 	odomLocalMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_map", 1);
+	odomLocalScanMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_scan_map", 1);
 	odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
 
 	Transform initialPose = Transform::getIdentity();
@@ -112,10 +125,13 @@ void OdometryROS::onInit()
 	pnh.param("config_path", configPath, configPath);
 	pnh.param("publish_null_when_lost", publishNullWhenLost_, publishNullWhenLost_);
 	pnh.param("guess_from_tf", guessFromTf_, guessFromTf_);
+	pnh.param("guess_frame_id", guessFrameId_, frameId_);
 
-	if(publishTf_ && guessFromTf_)
+	if(publishTf_ && guessFromTf_ && guessFrameId_.compare(frameId_) == 0)
 	{
-		NODELET_WARN( "\"publish_tf\" and \"guess_from_tf\" cannot be used at the same time. \"guess_from_tf\" is disabled.");
+		NODELET_WARN( "\"publish_tf\" and \"guess_from_tf\" cannot be used "
+				"at the same time if \"guess_frame_id\" and \"frame_id\" "
+				"are the same frame (value=\"%s\"). \"guess_from_tf\" is disabled.", frameId_.c_str());
 		guessFromTf_ = false;
 	}
 
@@ -159,7 +175,7 @@ void OdometryROS::onInit()
 
 
 	//parameters
-	parameters_ = Parameters::getDefaultOdometryParameters(stereo_);
+	parameters_ = Parameters::getDefaultOdometryParameters(stereoParams_, visParams_, icpParams_);
 	if(!configPath.empty())
 	{
 		if(UFile::exists(configPath.c_str()))
@@ -267,6 +283,9 @@ void OdometryROS::onInit()
 
 	Parameters::parse(parameters_, Parameters::kOdomResetCountdown(), resetCountdown_);
 	parameters_.at(Parameters::kOdomResetCountdown()) = "0"; // use modified reset countdown here
+
+	this->updateParameters(parameters_);
+
 	odometry_ = Odometry::create(parameters_);
 	if(!initialPose.isIdentity())
 	{
@@ -286,6 +305,31 @@ void OdometryROS::onInit()
 	onOdomInit();
 }
 
+void OdometryROS::startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync)
+{
+	warningThread_ = new boost::thread(boost::bind(&OdometryROS::warningLoop, this, subscribedTopicsMsg, approxSync));
+	NODELET_INFO("%s", subscribedTopicsMsg.c_str());
+}
+
+void OdometryROS::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());
+		}
+	}
+}
+
 Transform OdometryROS::getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const
 {
 	// TF ready?
@@ -295,10 +339,11 @@ Transform OdometryROS::getTransform(const std::string & fromFrameId, const std::
 		if(waitForTransform_ && !stamp.isZero() && waitForTransformDuration_ > 0.0)
 		{
 			//if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
-			if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_)))
+			std::string errorMsg;
+			if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_), ros::Duration(0.01), &errorMsg))
 			{
-				NODELET_WARN( "odometry: Could not get transform from %s to %s (stamp=%f) after %f seconds (\"wait_for_transform_duration\"=%f)!",
-						fromFrameId.c_str(), toFrameId.c_str(), stamp.toSec(), waitForTransformDuration_, waitForTransformDuration_);
+				NODELET_WARN( "odometry: Could not get transform from %s to %s (stamp=%f) after %f seconds (\"wait_for_transform_duration\"=%f)! Error=\"%s\"",
+						fromFrameId.c_str(), toFrameId.c_str(), stamp.toSec(), waitForTransformDuration_, waitForTransformDuration_, errorMsg.c_str());
 				return transform;
 			}
 		}
@@ -336,8 +381,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 	Transform guess;
 	if(guessFromTf_)
 	{
-		Transform previousPose = this->getTransform(odomFrameId_, frameId_, ros::Time(odometry_->previousStamp()));
-		Transform pose = this->getTransform(odomFrameId_, frameId_, stamp);
+		Transform previousPose = this->getTransform(odomFrameId_, guessFrameId_, ros::Time(odometry_->previousStamp()));
+		Transform pose = this->getTransform(odomFrameId_, guessFrameId_, stamp);
 		if(!previousPose.isNull() && !pose.isNull())
 		{
 			guess = previousPose.inverse() * pose;
@@ -352,6 +397,11 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 			}
 			NODELET_WARN( "TF Guess %s", guess.prettyPrint().c_str());*/
 		}
+		else
+		{
+			ROS_ERROR("\"guess_from_tf\" is true, but guess cannot be computed between frames \"%s\" -> \"%s\". Aborting odometry update...", odomFrameId_.c_str(), guessFrameId_.c_str());
+			return;
+		}
 	}
 
 	// process data
@@ -393,12 +443,12 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 
 			//set covariance
 			// libviso2 uses approximately vel variance * 2
-			odom.pose.covariance.at(0) = info.variance*2;  // xx
-			odom.pose.covariance.at(7) = info.variance*2;  // yy
-			odom.pose.covariance.at(14) = info.variance*2; // zz
-			odom.pose.covariance.at(21) = info.variance*2; // rr
-			odom.pose.covariance.at(28) = info.variance*2; // pp
-			odom.pose.covariance.at(35) = info.variance*2; // yawyaw
+			odom.pose.covariance.at(0) = info.varianceLin*2;  // xx
+			odom.pose.covariance.at(7) = info.varianceLin*2;  // yy
+			odom.pose.covariance.at(14) = info.varianceLin*2; // zz
+			odom.pose.covariance.at(21) = info.varianceAng*2; // rr
+			odom.pose.covariance.at(28) = info.varianceAng*2; // pp
+			odom.pose.covariance.at(35) = info.varianceAng*2; // yawyaw
 
 			//set velocity
 			bool setTwist = !odometry_->previousVelocityTransform().isNull();
@@ -414,19 +464,19 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 				odom.twist.twist.angular.z = yaw;
 			}
 
-			odom.twist.covariance.at(0) = setTwist?info.variance:BAD_COVARIANCE;  // xx
-			odom.twist.covariance.at(7) = setTwist?info.variance:BAD_COVARIANCE;  // yy
-			odom.twist.covariance.at(14) = setTwist?info.variance:BAD_COVARIANCE; // zz
-			odom.twist.covariance.at(21) = setTwist?info.variance:BAD_COVARIANCE; // rr
-			odom.twist.covariance.at(28) = setTwist?info.variance:BAD_COVARIANCE; // pp
-			odom.twist.covariance.at(35) = setTwist?info.variance:BAD_COVARIANCE; // yawyaw
+			odom.twist.covariance.at(0) = setTwist?info.varianceLin:BAD_COVARIANCE;  // xx
+			odom.twist.covariance.at(7) = setTwist?info.varianceLin:BAD_COVARIANCE;  // yy
+			odom.twist.covariance.at(14) = setTwist?info.varianceLin:BAD_COVARIANCE; // zz
+			odom.twist.covariance.at(21) = setTwist?info.varianceAng:BAD_COVARIANCE; // rr
+			odom.twist.covariance.at(28) = setTwist?info.varianceAng:BAD_COVARIANCE; // pp
+			odom.twist.covariance.at(35) = setTwist?info.varianceAng:BAD_COVARIANCE; // yawyaw
 
 			//publish the message
 			odomPub_.publish(odom);
 		}
 
 		// local map / reference frame
-		if(odomLocalMap_.getNumSubscribers() && dynamic_cast<OdometryF2M*>(odometry_))
+		if(odomLocalMap_.getNumSubscribers() && odometry_->getType() == Odometry::kTypeF2M)
 		{
 			pcl::PointCloud<pcl::PointXYZ> cloud;
 			const std::multimap<int, cv::Point3f> & map = ((OdometryF2M*)odometry_)->getMap().getWords3();
@@ -443,7 +493,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 
 		if(odomLastFrame_.getNumSubscribers())
 		{
-			if(dynamic_cast<OdometryF2M*>(odometry_))
+			// check which type of Odometry is using
+			if(odometry_->getType() == Odometry::kTypeF2M) // If it's Frame to Map Odometry
 			{
 				const std::multimap<int, cv::Point3f> & words3  = ((OdometryF2M*)odometry_)->getLastFrame().getWords3();
 				if(words3.size())
@@ -463,10 +514,10 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 					odomLastFrame_.publish(cloudMsg);
 				}
 			}
-			else
+			else if(odometry_->getType() == Odometry::kTypeF2F) // if Using Frame to Frame Odometry
 			{
-				//Frame to Frame
 				const Signature & refFrame = ((OdometryF2F*)odometry_)->getRefFrame();
+
 				if(refFrame.getWords3().size())
 				{
 					pcl::PointCloud<pcl::PointXYZ> cloud;
@@ -482,8 +533,29 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 					cloudMsg.header.frame_id = odomFrameId_;
 					odomLastFrame_.publish(cloudMsg);
 				}
+			}else{
+				NODELET_ERROR("ERROR, Wrong Type of Odometry, Shouldn't happen");
 			}
 		}
+
+		if(odomLocalScanMap_.getNumSubscribers() && !info.localScanMap.empty())
+		{
+			sensor_msgs::PointCloud2 cloudMsg;
+			if(info.localScanMap.channels() == 6)
+			{
+				pcl::PointCloud<pcl::PointNormal>::Ptr cloud = util3d::laserScanToPointCloudNormal(info.localScanMap);
+				pcl::toROSMsg(*cloud, cloudMsg);
+			}
+			else
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(info.localScanMap);
+				pcl::toROSMsg(*cloud, cloudMsg);
+			}
+
+			cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
+			cloudMsg.header.frame_id = odomFrameId_;
+			odomLocalScanMap_.publish(cloudMsg);
+		}
 	}
 	else if(publishNullWhenLost_)
 	{
@@ -544,12 +616,22 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 		odomInfoPub_.publish(infoMsg);
 	}
 
-	NODELET_INFO( "Odom: quality=%d, std dev=%fm, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.variance), (ros::WallTime::now()-time).toSec());
-}
+	if(visParams_)
+	{
+		if(icpParams_)
+		{
+			NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.inliers, info.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.varianceLin), pose.isNull()?0.0f:std::sqrt(info.varianceAng), (ros::WallTime::now()-time).toSec());
+		}
+		else
+		{
+			NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.varianceLin), pose.isNull()?0.0f:std::sqrt(info.varianceAng), (ros::WallTime::now()-time).toSec());
+		}
+	}
+	else
+	{
+		NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.varianceLin), pose.isNull()?0.0f:std::sqrt(info.varianceAng), (ros::WallTime::now()-time).toSec());
+	}
 
-bool OdometryROS::isOdometryF2M() const
-{
-	return dynamic_cast<OdometryF2M*>(odometry_) != 0;
 }
 
 bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)

src/PreferencesDialogROS.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "PreferencesDialogROS.h"
+#include "rtabmap_ros/PreferencesDialogROS.h"
 #include <rtabmap/core/Parameters.h>
 #include <QDir>
 #include <QFileInfo>

src/RGBDICPOdometryNode.cpp

@@ -0,0 +1,78 @@
+/*
+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"
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/core/Parameters.h>
+
+int main(int argc, char **argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+	ros::init(argc, argv, "rgbdicp_odometry");
+
+	// process "--params" argument
+	nodelet::V_string nargv;
+	for(int i=1;i<argc;++i)
+	{
+		if(strcmp(argv[i], "--params") == 0)
+		{
+			rtabmap::ParametersMap parametersOdom = rtabmap::Parameters::getDefaultOdometryParameters(false, true, true);
+			for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
+			{
+				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
+				std::cout <<
+						str <<
+						std::setw(60 - str.size()) <<
+						" [" <<
+						rtabmap::Parameters::getDescription(iter->first).c_str() <<
+						"]" <<
+						std::endl;
+			}
+			ROS_WARN("Node will now exit after showing default odometry parameters because "
+					 "argument \"--params\" is detected!");
+			exit(0);
+		}
+		else if(strcmp(argv[i], "--udebug") == 0)
+		{
+			ULogger::setLevel(ULogger::kDebug);
+		}
+		else if(strcmp(argv[i], "--uinfo") == 0)
+		{
+			ULogger::setLevel(ULogger::kInfo);
+		}
+		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/rgbdicp_odometry", remap, nargv);
+	ros::spin();
+	return 0;
+}

src/impl/CommonDataSubscriberDepth.cpp

@@ -0,0 +1,369 @@
+/*
+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>
+
+namespace rtabmap_ros {
+
+// RGB + Depth
+void CommonDataSubscriber::depthCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthScan2dCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthScan3dCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthInfoCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+
+// RGB + Depth + Odom
+void CommonDataSubscriber::depthOdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+
+// RGB + Depth + User Data
+void CommonDataSubscriber::depthDataCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthDataScan2dCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthDataScan3dCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthDataInfoCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+
+// RGB + Depth + Odom + User Data
+void CommonDataSubscriber::depthOdomDataCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomDataScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomDataScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::depthOdomDataInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+
+void CommonDataSubscriber::setupDepthCallbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup depth callback");
+
+	std::string rgbPrefix = "rgb";
+	std::string depthPrefix = "depth";
+	ros::NodeHandle rgb_nh(nh, rgbPrefix);
+	ros::NodeHandle depth_nh(nh, depthPrefix);
+	ros::NodeHandle rgb_pnh(pnh, rgbPrefix);
+	ros::NodeHandle depth_pnh(pnh, depthPrefix);
+	image_transport::ImageTransport rgb_it(rgb_nh);
+	image_transport::ImageTransport depth_it(depth_nh);
+	image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+	image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+
+	imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+	imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+	cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+
+	if(subscribeOdom && subscribeUserData)
+	{
+		odomSub_.subscribe(nh, "odom", 1);
+		userDataSub_.subscribe(nh, "user_data", 1);
+
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+			SYNC_DECL6(depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			SYNC_DECL6(depthOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL6(depthOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL5(depthOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
+		}
+	}
+	else if(subscribeOdom)
+	{
+		odomSub_.subscribe(nh, "odom", 1);
+
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+			SYNC_DECL5(depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			SYNC_DECL5(depthOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL5(depthOdomInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL4(depthOdom, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
+		}
+	}
+	else if(subscribeUserData)
+	{
+		userDataSub_.subscribe(nh, "user_data", 1);
+
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+			SYNC_DECL5(depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			SYNC_DECL5(depthDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL5(depthDataInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL4(depthData, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
+		}
+	}
+	else
+	{
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+			SYNC_DECL4(depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			SYNC_DECL4(depthScan3d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL4(depthInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL3(depth, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_);
+		}
+	}
+}
+
+} /* namespace rtabmap_ros */

src/impl/CommonDataSubscriberRGBD.cpp

@@ -0,0 +1,387 @@
+/*
+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_ros/MsgConversion.h>
+#include <cv_bridge/cv_bridge.h>
+
+namespace rtabmap_ros {
+
+// 1 RGBD camera
+void CommonDataSubscriber::rgbdCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdScan2dCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdScan3dCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdInfoCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+
+// 1 RGBD camera + Odom
+void CommonDataSubscriber::rgbdOdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdOdomScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdOdomScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdOdomInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+
+// 1 RGBD camera + User Data
+void CommonDataSubscriber::rgbdDataCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdDataScan2dCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdDataScan3dCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdDataInfoCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+
+// 1 RGBD camera + Odom + User Data
+void CommonDataSubscriber::rgbdOdomDataCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdOdomDataScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdOdomDataScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbdOdomDataInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	cv_bridge::CvImageConstPtr rgb, depth;
+	rtabmap_ros::toCvShare(image1Msg, rgb, depth);
+
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
+}
+
+void CommonDataSubscriber::setupRGBDCallbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup rgbd callback");
+
+	if(subscribeOdom || subscribeUserData || subscribeScan2d || subscribeScan3d || subscribeOdomInfo)
+	{
+		rgbdSubs_.resize(1);
+		rgbdSubs_[0] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
+		rgbdSubs_[0]->subscribe(nh, "rgbd_image", 1);
+
+
+		if(subscribeOdom && subscribeUserData)
+		{
+			odomSub_.subscribe(nh, "odom", 1);
+			userDataSub_.subscribe(nh, "user_data", 1);
+			if(subscribeScan2d)
+			{
+				subscribedToScan2d_ = true;
+				scanSub_.subscribe(nh, "scan", 1);
+				SYNC_DECL4(rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
+			}
+			else if(subscribeScan3d)
+			{
+				subscribedToScan3d_ = true;
+				scan3dSub_.subscribe(nh, "scan_cloud", 1);
+				SYNC_DECL4(rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
+			}
+			else if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL4(rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL3(rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]));
+			}
+		}
+		else if(subscribeOdom)
+		{
+			odomSub_.subscribe(nh, "odom", 1);
+			if(subscribeScan2d)
+			{
+				subscribedToScan2d_ = true;
+				scanSub_.subscribe(nh, "scan", 1);
+				SYNC_DECL3(rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
+			}
+			else if(subscribeScan3d)
+			{
+				subscribedToScan3d_ = true;
+				scan3dSub_.subscribe(nh, "scan_cloud", 1);
+				SYNC_DECL3(rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
+			}
+			else if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL3(rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL2(rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]));
+			}
+		}
+		else if(subscribeUserData)
+		{
+			userDataSub_.subscribe(nh, "user_data", 1);
+			if(subscribeScan2d)
+			{
+				subscribedToScan2d_ = true;
+				scanSub_.subscribe(nh, "scan", 1);
+				SYNC_DECL3(rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
+			}
+			else if(subscribeScan3d)
+			{
+				subscribedToScan3d_ = true;
+				scan3dSub_.subscribe(nh, "scan_cloud", 1);
+				SYNC_DECL3(rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
+			}
+			else if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL3(rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL2(rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]));
+			}
+		}
+		else
+		{
+			if(subscribeScan2d)
+			{
+				subscribedToScan2d_ = true;
+				scanSub_.subscribe(nh, "scan", 1);
+				SYNC_DECL2(rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
+			}
+			else if(subscribeScan3d)
+			{
+				subscribedToScan3d_ = true;
+				scan3dSub_.subscribe(nh, "scan_cloud", 1);
+				SYNC_DECL2(rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
+			}
+			else if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL2(rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_);
+			}
+			else
+			{
+				ROS_FATAL("Not supposed to be here!");
+			}
+		}
+	}
+	else
+	{
+		rgbdSub_ = nh.subscribe("rgbd_image", 1, &CommonDataSubscriber::rgbdCallback, this);
+
+		subscribedTopicsMsg_ =
+				uFormat("\n%s subscribed to:\n   %s",
+				ros::this_node::getName().c_str(),
+				rgbdSub_.getTopic().c_str());
+	}
+}
+
+} /* namespace rtabmap_ros */

src/impl/CommonDataSubscriberRGBD2.cpp

@@ -0,0 +1,386 @@
+/*
+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_ros/MsgConversion.h>
+#include <cv_bridge/cv_bridge.h>
+
+namespace rtabmap_ros {
+
+#define IMAGE_CONVERSION() \
+		callbackCalled(); \
+		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2); \
+		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2); \
+		rtabmap_ros::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
+		rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
+		std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
+		cameraInfoMsgs.push_back(image1Msg->cameraInfo); \
+		cameraInfoMsgs.push_back(image2Msg->cameraInfo);
+
+// 2 RGBD
+void CommonDataSubscriber::rgbd2Callback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2Scan2dCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2Scan3dCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2InfoCallback(
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+
+// 2 RGBD + Odom
+void CommonDataSubscriber::rgbd2OdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2OdomScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2OdomScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2OdomInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+
+// 2 RGBD + User Data
+void CommonDataSubscriber::rgbd2DataCallback(
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2DataScan2dCallback(
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2DataScan3dCallback(
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2DataInfoCallback(
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+
+// 2 RGBD + Odom + User Data
+void CommonDataSubscriber::rgbd2OdomDataCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2OdomDataScan2dCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2OdomDataScan3dCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbd2OdomDataInfoCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr& userDataMsg,
+		const rtabmap_ros::RGBDImageConstPtr& image1Msg,
+		const rtabmap_ros::RGBDImageConstPtr& image2Msg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	IMAGE_CONVERSION();
+
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
+}
+
+void CommonDataSubscriber::setupRGBD2Callbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup rgbd2 callback");
+
+	rgbdSubs_.resize(2);
+	for(int i=0; i<2; ++i)
+	{
+		rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
+		rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), 1);
+	}
+	if(subscribeOdom && subscribeUserData)
+	{
+		odomSub_.subscribe(nh, "odom", 1);
+		userDataSub_.subscribe(nh, "user_data", 1);
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+			SYNC_DECL5(rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			SYNC_DECL5(rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL5(rgbd2OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL4(rgbd2OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
+		}
+	}
+	else if(subscribeOdom)
+	{
+		odomSub_.subscribe(nh, "odom", 1);
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+			SYNC_DECL4(rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			SYNC_DECL4(rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL4(rgbd2OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL3(rgbd2Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
+		}
+	}
+	else if(subscribeUserData)
+	{
+		userDataSub_.subscribe(nh, "user_data", 1);
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+			SYNC_DECL4(rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			SYNC_DECL4(rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL4(rgbd2DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL3(rgbd2Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
+		}
+	}
+	else
+	{
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+			SYNC_DECL3(rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			SYNC_DECL3(rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL3(rgbd2Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL2(rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
+		}
+	}
+}
+
+} /* namespace rtabmap_ros */

src/impl/CommonDataSubscriberStereo.cpp

@@ -0,0 +1,142 @@
+/*
+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>
+
+namespace rtabmap_ros {
+
+// Stereo
+void CommonDataSubscriber::stereoCallback(
+		const sensor_msgs::ImageConstPtr& leftImageMsg,
+	   const sensor_msgs::ImageConstPtr& rightImageMsg,
+	   const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+	   const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg)
+{
+	callbackCalled();
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonStereoCallback(odomMsg, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::stereoInfoCallback(
+		const sensor_msgs::ImageConstPtr& leftImageMsg,
+	   const sensor_msgs::ImageConstPtr& rightImageMsg,
+	   const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+	   const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+	   const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	commonStereoCallback(odomMsg, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+
+// Stereo + Odom
+void CommonDataSubscriber::stereoOdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& leftImageMsg,
+		const sensor_msgs::ImageConstPtr& rightImageMsg,
+		const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+		const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg)
+{
+	callbackCalled();
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonStereoCallback(odomMsg, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::stereoOdomInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& leftImageMsg,
+		const sensor_msgs::ImageConstPtr& rightImageMsg,
+		const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+		const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+		const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg)
+{
+	callbackCalled();
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonStereoCallback(odomMsg, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+
+void CommonDataSubscriber::setupStereoCallbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeOdom,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup stereo callback");
+
+	ros::NodeHandle left_nh(nh, "left");
+	ros::NodeHandle right_nh(nh, "right");
+	ros::NodeHandle left_pnh(pnh, "left");
+	ros::NodeHandle right_pnh(pnh, "right");
+	image_transport::ImageTransport left_it(left_nh);
+	image_transport::ImageTransport right_it(right_nh);
+	image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
+	image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
+
+	imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft);
+	imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
+	cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
+	cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
+
+	if(subscribeOdom)
+	{
+		odomSub_.subscribe(nh, "odom", 1);
+
+		if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL6(stereoOdomInfo, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL5(stereoOdom, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+		}
+	}
+	else
+	{
+		if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL5(stereoInfo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL4(stereo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+		}
+	}
+}
+
+} /* namespace rtabmap_ros */

src/nodelets/icp_odometry.cpp

@@ -0,0 +1,198 @@
+/*
+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/OdometryROS.h>
+
+#include <pluginlib/class_list_macros.h>
+#include <nodelet/nodelet.h>
+
+#include <laser_geometry/laser_geometry.h>
+
+#include <sensor_msgs/LaserScan.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include "rtabmap_ros/MsgConversion.h"
+
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_surface.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+
+using namespace rtabmap;
+
+namespace rtabmap_ros
+{
+
+class ICPOdometry : public rtabmap_ros::OdometryROS
+{
+public:
+	ICPOdometry() :
+		OdometryROS(false, false, true),
+		scanCloudMaxPoints_(0),
+		scanCloudNormalK_(0)
+	{
+	}
+
+	virtual ~ICPOdometry()
+	{
+	}
+
+private:
+
+	virtual void onOdomInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		pnh.param("scan_cloud_max_points",  scanCloudMaxPoints_, scanCloudMaxPoints_);
+		pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_);
+
+		scan_sub_ = nh.subscribe("scan", 1, &ICPOdometry::callbackScan, this);
+		cloud_sub_ = nh.subscribe("scan_cloud", 1, &ICPOdometry::callbackCloud, this);
+	}
+
+	virtual void updateParameters(ParametersMap & parameters)
+	{
+		//make sure we are using Reg/Strategy=0
+		ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
+		if(iter != parameters.end() && iter->second.compare("0") != 0)
+		{
+			ROS_WARN("ICP odometry works only with \"Reg/Strategy\"=1. Ignoring value %s.", iter->second.c_str());
+		}
+		uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "1"));
+	}
+
+	void callbackScan(const sensor_msgs::LaserScanConstPtr& scanMsg)
+	{
+		// make sure the frame of the laser is updated too
+		Transform localScanTransform = getTransform(this->frameId(),
+				scanMsg->header.frame_id,
+				scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment));
+		if(localScanTransform.isNull())
+		{
+			ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec());
+			return;
+		}
+
+		//transform in frameId_ frame
+		sensor_msgs::PointCloud2 scanOut;
+		laser_geometry::LaserProjection projection;
+		projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener());
+		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::fromROSMsg(scanOut, *pclScan);
+
+		cv::Mat scan = util3d::laserScan2dFromPointCloud(*pclScan);
+
+		rtabmap::SensorData data(
+				scan,
+				LaserScanInfo((int)scanMsg->ranges.size(), scanMsg->range_max, localScanTransform),
+				cv::Mat(),
+				cv::Mat(),
+				CameraModel(),
+				0,
+				rtabmap_ros::timestampFromROS(scanMsg->header.stamp));
+
+		this->processData(data, scanMsg->header.stamp);
+	}
+
+	void callbackCloud(const sensor_msgs::PointCloud2ConstPtr& cloudMsg)
+	{
+		cv::Mat scan;
+		bool containNormals = false;
+		for(unsigned int i=0; i<cloudMsg->fields.size(); ++i)
+		{
+			if(cloudMsg->fields[i].name.compare("normal_x") == 0)
+			{
+				containNormals = true;
+				break;
+			}
+		}
+
+		Transform localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp);
+		if(localScanTransform.isNull())
+		{
+			ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg->header.stamp.toSec());
+			return;
+		}
+
+		if(containNormals)
+		{
+			pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
+			pcl::fromROSMsg(*cloudMsg, *pclScan);
+			scan = util3d::laserScanFromPointCloud(*pclScan);
+		}
+		else
+		{
+			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+			pcl::fromROSMsg(*cloudMsg, *pclScan);
+
+			if(scanCloudNormalK_ > 0)
+			{
+				//compute normals
+				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanCloudNormalK_);
+				pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
+				pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+				scan = util3d::laserScanFromPointCloud(*pclScanNormal);
+			}
+			else
+			{
+				scan = util3d::laserScanFromPointCloud(*pclScan);
+			}
+		}
+
+		rtabmap::SensorData data(
+				scan,
+				LaserScanInfo(scanCloudMaxPoints_, 0, localScanTransform),
+				cv::Mat(),
+				cv::Mat(),
+				CameraModel(),
+				0,
+				rtabmap_ros::timestampFromROS(cloudMsg->header.stamp));
+
+		this->processData(data, cloudMsg->header.stamp);
+	}
+
+protected:
+	virtual void flushCallbacks()
+	{
+		// flush callbacks
+	}
+
+private:
+	ros::Subscriber scan_sub_;
+	ros::Subscriber cloud_sub_;
+	int scanCloudMaxPoints_;
+	int scanCloudNormalK_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ICPOdometry, nodelet::Nodelet);
+
+}

src/nodelets/obstacles_detection.cpp

@@ -36,28 +36,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <tf/transform_listener.h>
 
 #include <sensor_msgs/PointCloud2.h>
-#include <sensor_msgs/Image.h>
-#include <sensor_msgs/image_encodings.h>
-#include <sensor_msgs/CameraInfo.h>
-#include <stereo_msgs/DisparityImage.h>
-
-#include <image_transport/image_transport.h>
-#include <image_transport/subscriber_filter.h>
-
-#include <image_geometry/pinhole_camera_model.h>
-
-#include <message_filters/sync_policies/approximate_time.h>
-#include <message_filters/subscriber.h>
-
-#include <cv_bridge/cv_bridge.h>
-#include <opencv2/highgui/highgui.hpp>
 
 #include <rtabmap_ros/MsgConversion.h>
 
-#include "rtabmap/core/util3d.h"
-#include "rtabmap/core/util3d_filtering.h"
-#include "rtabmap/core/util3d_mapping.h"
-#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/OccupancyGrid.h"
+#include "rtabmap/utilite/UStl.h"
 
 namespace rtabmap_ros
 {
@@ -67,51 +50,156 @@ class ObstaclesDetection : public nodelet::Nodelet
 public:
 	ObstaclesDetection() :
 		frameId_("base_link"),
-		normalKSearch_(20),
-		groundNormalAngle_(M_PI_4),
-		clusterRadius_(0.05),
-		minClusterSize_(20),
-		maxObstaclesHeight_(0.0), // if<=0.0 -> disabled
-		maxGroundHeight_(0.0),    // if<=0.0 -> disabled, used only if detect_flat_obstacles is true
-		segmentFlatObstacles_(false),
-		waitForTransform_(false),
-		optimizeForCloseObjects_(false),
-		projVoxelSize_(0.01)
+		waitForTransform_(false)
 	{}
 
 	virtual ~ObstaclesDetection()
 	{}
 
 private:
+
+	void parameterMoved(
+			ros::NodeHandle & nh,
+			const std::string & rosName,
+			const std::string & parameterName,
+			rtabmap::ParametersMap & parameters)
+	{
+		if(nh.hasParam(rosName))
+		{
+			rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid");
+			rtabmap::ParametersMap::const_iterator iter =gridParameters.find(parameterName);
+			if(iter != gridParameters.end())
+			{
+				NODELET_ERROR("obstacles_detection: Parameter \"%s\" has moved from "
+						 "rtabmap_ros to rtabmap library. Use "
+						 "parameter \"%s\" instead. The value is still "
+						 "copied to new parameter name.",
+						 rosName.c_str(),
+						 parameterName.c_str());
+				std::string type = rtabmap::Parameters::getType(parameterName);
+				if(type.compare("float") || type.compare("double"))
+				{
+					double v = uStr2Double(iter->second);
+					nh.getParam(rosName, v);
+					parameters.insert(rtabmap::ParametersPair(parameterName, uNumber2Str(v)));
+				}
+				else if(type.compare("int") || type.compare("unsigned int"))
+				{
+					int v = uStr2Int(iter->second);
+					nh.getParam(rosName, v);
+					parameters.insert(rtabmap::ParametersPair(parameterName, uNumber2Str(v)));
+				}
+				else
+				{
+					NODELET_ERROR("Not handled type \"%s\" for parameter \"%s\"", type.c_str(), parameterName.c_str());
+				}
+			}
+			else
+			{
+				NODELET_ERROR("Parameter \"%s\" not found in default parameters.", parameterName.c_str());
+			}
+		}
+	}
+
 	virtual void onInit()
 	{
+		ROS_DEBUG("_"); // not sure why, but all NODELET_*** log are not shown if a normal ROS_*** is not called!?
 		ros::NodeHandle & nh = getNodeHandle();
 		ros::NodeHandle & pnh = getPrivateNodeHandle();
 
 		int queueSize = 10;
 		pnh.param("queue_size", queueSize, queueSize);
 		pnh.param("frame_id", frameId_, frameId_);
-		pnh.param("normal_k", normalKSearch_, normalKSearch_);
-		pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_);
-		if(pnh.hasParam("normal_estimation_radius") && !pnh.hasParam("cluster_radius"))
-		{
-			NODELET_WARN("Parameter \"normal_estimation_radius\" has been renamed "
-					 "to \"cluster_radius\"! Your value is still copied to "
-					 "corresponding parameter. Instead of normal radius, nearest neighbors count "
-					 "\"normal_k\" is used instead (default 20).");
-			pnh.param("normal_estimation_radius", clusterRadius_, clusterRadius_);
-		}
-		else
-		{
-			pnh.param("cluster_radius", clusterRadius_, clusterRadius_);
-		}
-		pnh.param("min_cluster_size", minClusterSize_, minClusterSize_);
-		pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_);
-		pnh.param("max_ground_height", maxGroundHeight_, maxGroundHeight_);
-		pnh.param("detect_flat_obstacles", segmentFlatObstacles_, segmentFlatObstacles_);
+		pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
 		pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
-		pnh.param("optimize_for_close_objects", optimizeForCloseObjects_, optimizeForCloseObjects_);
-		pnh.param("proj_voxel_size", projVoxelSize_, projVoxelSize_);
+
+		if(pnh.hasParam("optimize_for_close_objects"))
+		{
+			NODELET_ERROR("\"optimize_for_close_objects\" parameter doesn't exist "
+					"anymore. Use rtabmap_ros/obstacles_detection_old nodelet to use "
+					"the old interface.");
+		}
+
+		rtabmap::ParametersMap parameters;
+
+		// Backward compatibility
+		for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=rtabmap::Parameters::getRemovedParameters().begin();
+			iter!=rtabmap::Parameters::getRemovedParameters().end();
+			++iter)
+		{
+			std::string vStr;
+			if(pnh.getParam(iter->first, vStr))
+			{
+				if(iter->second.first)
+				{
+					// can be migrated
+					uInsert(parameters, rtabmap::ParametersPair(iter->second.second, vStr));
+					NODELET_ERROR("obstacles_detection: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
+							iter->first.c_str(), iter->second.second.c_str(), vStr.c_str());
+				}
+				else
+				{
+					if(iter->second.second.empty())
+					{
+						NODELET_ERROR("obstacles_detection: Parameter \"%s\" doesn't exist anymore!",
+								iter->first.c_str());
+					}
+					else
+					{
+						NODELET_ERROR("obstacles_detection: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
+								iter->first.c_str(), iter->second.second.c_str());
+					}
+				}
+			}
+		}
+
+		rtabmap::ParametersMap gridParameters2 = rtabmap::Parameters::getDefaultParameters();
+		rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid");
+		for(rtabmap::ParametersMap::iterator iter=gridParameters.begin(); iter!=gridParameters.end(); ++iter)
+		{
+			std::string vStr;
+			bool vBool;
+			int vInt;
+			double vDouble;
+			if(pnh.getParam(iter->first, vStr))
+			{
+				NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
+				iter->second = vStr;
+			}
+			else if(pnh.getParam(iter->first, vBool))
+			{
+				NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
+				iter->second = uBool2Str(vBool);
+			}
+			else if(pnh.getParam(iter->first, vDouble))
+			{
+				NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
+				iter->second = uNumber2Str(vDouble);
+			}
+			else if(pnh.getParam(iter->first, vInt))
+			{
+				NODELET_INFO("obstacles_detection: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
+				iter->second = uNumber2Str(vInt);
+			}
+		}
+		uInsert(parameters, gridParameters);
+		parameterMoved(pnh, "proj_voxel_size", rtabmap::Parameters::kGridCellSize(), parameters);
+		parameterMoved(pnh, "ground_normal_angle", rtabmap::Parameters::kGridMaxGroundAngle(), parameters);
+		parameterMoved(pnh, "min_cluster_size", rtabmap::Parameters::kGridMinClusterSize(), parameters);
+		parameterMoved(pnh, "normal_estimation_radius", rtabmap::Parameters::kGridClusterRadius(), parameters);
+		parameterMoved(pnh, "cluster_radius", rtabmap::Parameters::kGridClusterRadius(), parameters);
+		parameterMoved(pnh, "max_obstacles_height", rtabmap::Parameters::kGridMaxObstacleHeight(), parameters);
+		parameterMoved(pnh, "max_ground_height", rtabmap::Parameters::kGridMaxGroundHeight(), parameters);
+		parameterMoved(pnh, "detect_flat_obstacles", rtabmap::Parameters::kGridFlatObstacleDetected(), parameters);
+		parameterMoved(pnh, "normal_k", rtabmap::Parameters::kGridNormalK(), parameters);
+
+		UASSERT(uContains(parameters, rtabmap::Parameters::kGridMapFrameProjection()));
+		if(uStr2Bool(parameters.at(rtabmap::Parameters::kGridMapFrameProjection())) && mapFrameId_.empty())
+		{
+			NODELET_ERROR("obstacles_detection: Parameter \"%s\" is true but map_frame_id is not set!", rtabmap::Parameters::kGridMapFrameProjection().c_str());
+		}
+
+		grid_.parseParameters(parameters);
 
 		cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this);
 
@@ -153,8 +241,32 @@ private:
 			return;
 		}
 
-		pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
-		pcl::fromROSMsg(*cloudMsg, *originalCloud);
+		rtabmap::Transform pose = rtabmap::Transform::getIdentity();
+		if(!mapFrameId_.empty())
+		{
+			try
+			{
+				if(waitForTransform_)
+				{
+					if(!tfListener_.waitForTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, ros::Duration(1)))
+					{
+						NODELET_ERROR("Could not get transform from %s to %s after 1 second!", mapFrameId_.c_str(), frameId_.c_str());
+						return;
+					}
+				}
+				tf::StampedTransform tmp;
+				tfListener_.lookupTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, tmp);
+				pose = rtabmap_ros::transformFromTF(tmp);
+			}
+			catch(tf::TransformException & ex)
+			{
+				NODELET_ERROR("%s",ex.what());
+				return;
+			}
+		}
+
+		pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::fromROSMsg(*cloudMsg, *inputCloud);
 
 		//Common variables for all strategies
 		pcl::IndicesPtr ground, obstacles;
@@ -162,135 +274,69 @@ private:
 		pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
 		pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloudWithoutFlatSurfaces(new pcl::PointCloud<pcl::PointXYZ>);
 
-		if(originalCloud->size())
+		if(inputCloud->size())
 		{
-			originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform);
-			if(maxObstaclesHeight_ > 0)
-			{
-				// std::numeric_limits<float>::lowest() exists only for c++11
-				originalCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::min(), maxObstaclesHeight_);
-			}
+			inputCloud = rtabmap::util3d::transformPointCloud(inputCloud, localTransform);
 
-			if(originalCloud->size())
+			pcl::IndicesPtr flatObstacles(new std::vector<int>);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = grid_.segmentCloud<pcl::PointXYZ>(
+					inputCloud,
+					pcl::IndicesPtr(new std::vector<int>),
+					pose,
+					cv::Point3f(localTransform.x(), localTransform.y(), localTransform.z()),
+					ground,
+					obstacles,
+					&flatObstacles);
+
+			if(cloud->size() && (ground->size() || obstacles->size()))
 			{
-				if(!optimizeForCloseObjects_)
+				if(groundPub_.getNumSubscribers() &&
+						ground.get() && ground->size())
 				{
-					// This is the default strategy
-					pcl::IndicesPtr flatObstacles(new std::vector<int>);
-					rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
-							originalCloud,
-							ground,
-							obstacles,
-							normalKSearch_,
-							groundNormalAngle_,
-							clusterRadius_,
-							minClusterSize_,
-							segmentFlatObstacles_,
-							maxGroundHeight_,
-							&flatObstacles);
-
-					if(groundPub_.getNumSubscribers() &&
-							ground.get() && ground->size())
-					{
-						pcl::copyPointCloud(*originalCloud, *ground, *groundCloud);
-					}
-
-					if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) &&
-							obstacles.get() && obstacles->size())
-					{
-						// remove flat obstacles from obstacles
-						std::set<int> flatObstaclesSet;
-						if(projObstaclesPub_.getNumSubscribers())
-						{
-							flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end());
-						}
-
-						obstaclesCloud->resize(obstacles->size());
-						obstaclesCloudWithoutFlatSurfaces->resize(obstacles->size());
-
-						int oi=0;
-						for(unsigned int i=0; i<obstacles->size(); ++i)
-						{
-							obstaclesCloud->points[i] = originalCloud->at(obstacles->at(i));
-							if(flatObstaclesSet.size() == 0 ||  
-							   flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end())
-							{
-								obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i];
-								obstaclesCloudWithoutFlatSurfaces->points[oi].z = 0;
-								++oi;
-							}
-
-						}
-
-						obstaclesCloudWithoutFlatSurfaces->resize(oi);
-						if(obstaclesCloudWithoutFlatSurfaces->size() && projVoxelSize_ > 0.0)
-						{
-							obstaclesCloudWithoutFlatSurfaces = rtabmap::util3d::voxelize(obstaclesCloudWithoutFlatSurfaces, projVoxelSize_);
-						}
-					}
+					pcl::copyPointCloud(*cloud, *ground, *groundCloud);
 				}
-				else
+
+				if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) &&
+						obstacles.get() && obstacles->size())
 				{
-					// in this case optimizeForCloseObject_ is true:
-					// we divide the floor point cloud into two subsections, one for all potential floor points up to 1m
-					// one for potential floor points further away than 1m.
-					// For the points at closer range, we use a smaller normal estimation radius and ground normal angle,
-					// which allows to detect smaller objects, without increasing the number of false positive.
-					// For all other points, we use a bigger normal estimation radius (* 3.) and tolerance for the
-					// grond normal angle (* 2.).
-
-					pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud_near = rtabmap::util3d::passThrough(originalCloud, "x", std::numeric_limits<int>::min(), 1.);
-					pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud_far = rtabmap::util3d::passThrough(originalCloud, "x", 1., std::numeric_limits<int>::max());
-
-					// Part 1: segment floor and obstacles near the robot
-					rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
-							originalCloud_near,
-							ground,
-							obstacles,
-							normalKSearch_,
-							groundNormalAngle_,
-							clusterRadius_,
-							minClusterSize_,
-							segmentFlatObstacles_,
-							maxGroundHeight_);
-
-					if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
+					// remove flat obstacles from obstacles
+					std::set<int> flatObstaclesSet;
+					if(projObstaclesPub_.getNumSubscribers())
 					{
-						pcl::copyPointCloud(*originalCloud_near, *ground, *groundCloud);
-						ground->clear();
+						flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end());
 					}
 
-					if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && obstacles.get() && obstacles->size())
+					obstaclesCloud->resize(obstacles->size());
+					obstaclesCloudWithoutFlatSurfaces->resize(obstacles->size());
+
+					int oi=0;
+					for(unsigned int i=0; i<obstacles->size(); ++i)
 					{
-						pcl::copyPointCloud(*originalCloud_near, *obstacles, *obstaclesCloud);
-						obstacles->clear();
+						obstaclesCloud->points[i] = cloud->at(obstacles->at(i));
+						if(flatObstaclesSet.size() == 0 ||
+						   flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end())
+						{
+							obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i];
+							obstaclesCloudWithoutFlatSurfaces->points[oi].z = 0;
+							++oi;
+						}
+
 					}
 
-					// Part 2: segment floor and obstacles far from the robot
-					rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
-							originalCloud_far,
-							ground,
-							obstacles,
-							normalKSearch_,
-							2.*groundNormalAngle_,
-							3.*clusterRadius_,
-							minClusterSize_,
-							segmentFlatObstacles_,
-							maxGroundHeight_);
+					obstaclesCloudWithoutFlatSurfaces->resize(oi);
+				}
 
-					if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
+				if(!localTransform.isIdentity())
+				{
+					//transform back in topic frame
+					rtabmap::Transform localTransformInv = localTransform.inverse();
+					if(groundCloud->size())
 					{
-						pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud2 (new pcl::PointCloud<pcl::PointXYZ>);
-						pcl::copyPointCloud(*originalCloud_far, *ground, *groundCloud2);
-						*groundCloud += *groundCloud2;
+						groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, localTransformInv);
 					}
-
-
-					if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && obstacles.get() && obstacles->size())
+					if(obstaclesCloud->size())
 					{
-						pcl::PointCloud<pcl::PointXYZ>::Ptr obstacles2(new pcl::PointCloud<pcl::PointXYZ>);
-						pcl::copyPointCloud(*originalCloud_far, *obstacles, *obstacles2);
-						*obstaclesCloud += *obstacles2;
+						obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, localTransformInv);
 					}
 				}
 			}
@@ -300,8 +346,7 @@ private:
 		{
 			sensor_msgs::PointCloud2 rosCloud;
 			pcl::toROSMsg(*groundCloud, rosCloud);
-			rosCloud.header.stamp = cloudMsg->header.stamp;
-			rosCloud.header.frame_id = frameId_;
+			rosCloud.header = cloudMsg->header;
 
 			//publish the message
 			groundPub_.publish(rosCloud);
@@ -311,8 +356,7 @@ private:
 		{
 			sensor_msgs::PointCloud2 rosCloud;
 			pcl::toROSMsg(*obstaclesCloud, rosCloud);
-			rosCloud.header.stamp = cloudMsg->header.stamp;
-			rosCloud.header.frame_id = frameId_;
+			rosCloud.header = cloudMsg->header;
 
 			//publish the message
 			obstaclesPub_.publish(rosCloud);
@@ -334,16 +378,10 @@ private:
 
 private:
 	std::string frameId_;
-	int normalKSearch_;
-	double groundNormalAngle_;
-	double clusterRadius_;
-	int minClusterSize_;
-	double maxObstaclesHeight_;
-	double maxGroundHeight_;
-	bool segmentFlatObstacles_;
+	std::string mapFrameId_;
 	bool waitForTransform_;
-	bool optimizeForCloseObjects_;
-	double projVoxelSize_;
+
+	rtabmap::OccupancyGrid grid_;
 
 	tf::TransformListener tfListener_;
 
@@ -357,3 +395,4 @@ private:
 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ObstaclesDetection, nodelet::Nodelet);
 }
 
+

src/nodelets/obstacles_detection_old.cpp

@@ -0,0 +1,357 @@
+/*
+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 <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <tf/transform_listener.h>
+
+#include <sensor_msgs/PointCloud2.h>
+
+#include <rtabmap_ros/MsgConversion.h>
+
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/util3d_filtering.h"
+#include "rtabmap/core/util3d_mapping.h"
+#include "rtabmap/core/util3d_transforms.h"
+
+namespace rtabmap_ros
+{
+
+class ObstaclesDetectionOld : public nodelet::Nodelet
+{
+public:
+	ObstaclesDetectionOld() :
+		frameId_("base_link"),
+		normalKSearch_(20),
+		groundNormalAngle_(M_PI_4),
+		clusterRadius_(0.05),
+		minClusterSize_(20),
+		maxObstaclesHeight_(0.0), // if<=0.0 -> disabled
+		maxGroundHeight_(0.0),    // if<=0.0 -> disabled, used only if detect_flat_obstacles is true
+		segmentFlatObstacles_(false),
+		waitForTransform_(false),
+		optimizeForCloseObjects_(false),
+		projVoxelSize_(0.01)
+	{}
+
+	virtual ~ObstaclesDetectionOld()
+	{}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int queueSize = 10;
+		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("frame_id", frameId_, frameId_);
+		pnh.param("normal_k", normalKSearch_, normalKSearch_);
+		pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_);
+		if(pnh.hasParam("normal_estimation_radius") && !pnh.hasParam("cluster_radius"))
+		{
+			NODELET_WARN("Parameter \"normal_estimation_radius\" has been renamed "
+					 "to \"cluster_radius\"! Your value is still copied to "
+					 "corresponding parameter. Instead of normal radius, nearest neighbors count "
+					 "\"normal_k\" is used instead (default 20).");
+			pnh.param("normal_estimation_radius", clusterRadius_, clusterRadius_);
+		}
+		else
+		{
+			pnh.param("cluster_radius", clusterRadius_, clusterRadius_);
+		}
+		pnh.param("min_cluster_size", minClusterSize_, minClusterSize_);
+		pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_);
+		pnh.param("max_ground_height", maxGroundHeight_, maxGroundHeight_);
+		pnh.param("detect_flat_obstacles", segmentFlatObstacles_, segmentFlatObstacles_);
+		pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
+		pnh.param("optimize_for_close_objects", optimizeForCloseObjects_, optimizeForCloseObjects_);
+		pnh.param("proj_voxel_size", projVoxelSize_, projVoxelSize_);
+
+		cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetectionOld::callback, this);
+
+		groundPub_ = nh.advertise<sensor_msgs::PointCloud2>("ground", 1);
+		obstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("obstacles", 1);
+		projObstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("proj_obstacles", 1);
+	}
+
+
+
+	void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
+	{
+		ros::WallTime time = ros::WallTime::now();
+
+		if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0 && projObstaclesPub_.getNumSubscribers() == 0)
+		{
+			// no one wants the results
+			return;
+		}
+
+		rtabmap::Transform localTransform;
+		try
+		{
+			if(waitForTransform_)
+			{
+				if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
+				{
+					NODELET_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
+					return;
+				}
+			}
+			tf::StampedTransform tmp;
+			tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp);
+			localTransform = rtabmap_ros::transformFromTF(tmp);
+		}
+		catch(tf::TransformException & ex)
+		{
+			NODELET_ERROR("%s",ex.what());
+			return;
+		}
+
+		pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::fromROSMsg(*cloudMsg, *originalCloud);
+
+		//Common variables for all strategies
+		pcl::IndicesPtr ground, obstacles;
+		pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloudWithoutFlatSurfaces(new pcl::PointCloud<pcl::PointXYZ>);
+
+		if(originalCloud->size())
+		{
+			originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform);
+			if(maxObstaclesHeight_ > 0)
+			{
+				// std::numeric_limits<float>::lowest() exists only for c++11
+				originalCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::min(), maxObstaclesHeight_);
+			}
+
+			if(originalCloud->size())
+			{
+				if(!optimizeForCloseObjects_)
+				{
+					// This is the default strategy
+					pcl::IndicesPtr flatObstacles(new std::vector<int>);
+					rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
+							originalCloud,
+							ground,
+							obstacles,
+							normalKSearch_,
+							groundNormalAngle_,
+							clusterRadius_,
+							minClusterSize_,
+							segmentFlatObstacles_,
+							maxGroundHeight_,
+							&flatObstacles);
+
+					if(groundPub_.getNumSubscribers() &&
+							ground.get() && ground->size())
+					{
+						pcl::copyPointCloud(*originalCloud, *ground, *groundCloud);
+					}
+
+					if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) &&
+							obstacles.get() && obstacles->size())
+					{
+						// remove flat obstacles from obstacles
+						std::set<int> flatObstaclesSet;
+						if(projObstaclesPub_.getNumSubscribers())
+						{
+							flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end());
+						}
+
+						obstaclesCloud->resize(obstacles->size());
+						obstaclesCloudWithoutFlatSurfaces->resize(obstacles->size());
+
+						int oi=0;
+						for(unsigned int i=0; i<obstacles->size(); ++i)
+						{
+							obstaclesCloud->points[i] = originalCloud->at(obstacles->at(i));
+							if(flatObstaclesSet.size() == 0 ||
+							   flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end())
+							{
+								obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i];
+								obstaclesCloudWithoutFlatSurfaces->points[oi].z = 0;
+								++oi;
+							}
+
+						}
+
+						obstaclesCloudWithoutFlatSurfaces->resize(oi);
+						if(obstaclesCloudWithoutFlatSurfaces->size() && projVoxelSize_ > 0.0)
+						{
+							obstaclesCloudWithoutFlatSurfaces = rtabmap::util3d::voxelize(obstaclesCloudWithoutFlatSurfaces, projVoxelSize_);
+						}
+					}
+				}
+				else
+				{
+					// in this case optimizeForCloseObject_ is true:
+					// we divide the floor point cloud into two subsections, one for all potential floor points up to 1m
+					// one for potential floor points further away than 1m.
+					// For the points at closer range, we use a smaller normal estimation radius and ground normal angle,
+					// which allows to detect smaller objects, without increasing the number of false positive.
+					// For all other points, we use a bigger normal estimation radius (* 3.) and tolerance for the
+					// grond normal angle (* 2.).
+
+					pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud_near = rtabmap::util3d::passThrough(originalCloud, "x", std::numeric_limits<int>::min(), 1.);
+					pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud_far = rtabmap::util3d::passThrough(originalCloud, "x", 1., std::numeric_limits<int>::max());
+
+					// Part 1: segment floor and obstacles near the robot
+					rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
+							originalCloud_near,
+							ground,
+							obstacles,
+							normalKSearch_,
+							groundNormalAngle_,
+							clusterRadius_,
+							minClusterSize_,
+							segmentFlatObstacles_,
+							maxGroundHeight_);
+
+					if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
+					{
+						pcl::copyPointCloud(*originalCloud_near, *ground, *groundCloud);
+						ground->clear();
+					}
+
+					if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && obstacles.get() && obstacles->size())
+					{
+						pcl::copyPointCloud(*originalCloud_near, *obstacles, *obstaclesCloud);
+						obstacles->clear();
+					}
+
+					// Part 2: segment floor and obstacles far from the robot
+					rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
+							originalCloud_far,
+							ground,
+							obstacles,
+							normalKSearch_,
+							2.*groundNormalAngle_,
+							3.*clusterRadius_,
+							minClusterSize_,
+							segmentFlatObstacles_,
+							maxGroundHeight_);
+
+					if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
+					{
+						pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud2 (new pcl::PointCloud<pcl::PointXYZ>);
+						pcl::copyPointCloud(*originalCloud_far, *ground, *groundCloud2);
+						*groundCloud += *groundCloud2;
+					}
+
+
+					if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && obstacles.get() && obstacles->size())
+					{
+						pcl::PointCloud<pcl::PointXYZ>::Ptr obstacles2(new pcl::PointCloud<pcl::PointXYZ>);
+						pcl::copyPointCloud(*originalCloud_far, *obstacles, *obstacles2);
+						*obstaclesCloud += *obstacles2;
+					}
+				}
+
+				if(!localTransform.isIdentity())
+				{
+					//transform back in topic frame
+					rtabmap::Transform localTransformInv = localTransform.inverse();
+					if(groundCloud->size())
+					{
+						groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, localTransformInv);
+					}
+					if(obstaclesCloud->size())
+					{
+						obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, localTransformInv);
+					}
+				}
+			}
+		}
+
+		if(groundPub_.getNumSubscribers())
+		{
+			sensor_msgs::PointCloud2 rosCloud;
+			pcl::toROSMsg(*groundCloud, rosCloud);
+			rosCloud.header = cloudMsg->header;
+
+			//publish the message
+			groundPub_.publish(rosCloud);
+		}
+
+		if(obstaclesPub_.getNumSubscribers())
+		{
+			sensor_msgs::PointCloud2 rosCloud;
+			pcl::toROSMsg(*obstaclesCloud, rosCloud);
+			rosCloud.header = cloudMsg->header;
+
+			//publish the message
+			obstaclesPub_.publish(rosCloud);
+		}
+
+		if(projObstaclesPub_.getNumSubscribers())
+		{
+			sensor_msgs::PointCloud2 rosCloud;
+			pcl::toROSMsg(*obstaclesCloudWithoutFlatSurfaces, rosCloud);
+			rosCloud.header.stamp = cloudMsg->header.stamp;
+			rosCloud.header.frame_id = frameId_;
+
+			//publish the message
+			projObstaclesPub_.publish(rosCloud);
+		}
+
+		NODELET_DEBUG("Obstacles segmentation time = %f s", (ros::WallTime::now() - time).toSec());
+	}
+
+private:
+	std::string frameId_;
+	int normalKSearch_;
+	double groundNormalAngle_;
+	double clusterRadius_;
+	int minClusterSize_;
+	double maxObstaclesHeight_;
+	double maxGroundHeight_;
+	bool segmentFlatObstacles_;
+	bool waitForTransform_;
+	bool optimizeForCloseObjects_;
+	double projVoxelSize_;
+
+	tf::TransformListener tfListener_;
+
+	ros::Publisher groundPub_;
+	ros::Publisher obstaclesPub_;
+	ros::Publisher projObstaclesPub_;
+
+	ros::Subscriber cloudSub_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ObstaclesDetectionOld, nodelet::Nodelet);
+}
+
+

src/nodelets/rgbd_odometry.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "OdometryROS.h"
+#include <rtabmap_ros/OdometryROS.h>
 
 #include <pluginlib/class_list_macros.h>
 #include <nodelet/nodelet.h>
@@ -49,6 +49,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util2d.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
 
 using namespace rtabmap;
 
@@ -59,16 +60,18 @@ class RGBDOdometry : public rtabmap_ros::OdometryROS
 {
 public:
 	RGBDOdometry() :
-		OdometryROS(false),
+		OdometryROS(false, true, false),
 		approxSync_(0),
 		exactSync_(0),
-		sync2_(0),
+		approxSync2_(0),
+		exactSync2_(0),
 		queueSize_(5)
 	{
 	}
 
 	virtual ~RGBDOdometry()
 	{
+		rgbdSub_.shutdown();
 		if(approxSync_)
 		{
 			delete approxSync_;
@@ -77,9 +80,13 @@ public:
 		{
 			delete exactSync_;
 		}
-		if(sync2_)
+		if(approxSync2_)
 		{
-			delete sync2_;
+			delete approxSync2_;
+		}
+		if(exactSync2_)
+		{
+			delete exactSync2_;
 		}
 	}
 
@@ -90,66 +97,65 @@ private:
 		ros::NodeHandle & nh = getNodeHandle();
 		ros::NodeHandle & pnh = getPrivateNodeHandle();
 
-		int depthCameras = 1;
+		int rgbdCameras = 1;
 		bool approxSync = true;
+		bool subscribeRGBD = false;
 		pnh.param("approx_sync", approxSync, approxSync);
 		pnh.param("queue_size", queueSize_, queueSize_);
-		pnh.param("depth_cameras", depthCameras, depthCameras);
-		if(depthCameras <= 0)
+		pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
+		if(pnh.hasParam("depth_cameras"))
 		{
-			depthCameras = 1;
+			ROS_ERROR("\"depth_cameras\" parameter doesn't exist anymore. It is replaced by \"rgbd_cameras\" with the \"rgbd_image\" input topics. \"subscribe_rgbd\" should be also set to true.");
 		}
-		if(depthCameras > 2)
+		pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
+		if(rgbdCameras <= 0)
+		{
+			rgbdCameras = 1;
+		}
+		if(rgbdCameras > 2)
 		{
 			NODELET_FATAL("Only 2 cameras maximum supported yet.");
 		}
 
-		if(depthCameras == 2)
+		std::string subscribedTopicsMsg;
+		if(subscribeRGBD)
 		{
-			ros::NodeHandle rgb0_nh(nh, "rgb0");
-			ros::NodeHandle depth0_nh(nh, "depth0");
-			ros::NodeHandle rgb0_pnh(pnh, "rgb0");
-			ros::NodeHandle depth0_pnh(pnh, "depth0");
-			image_transport::ImageTransport rgb0_it(rgb0_nh);
-			image_transport::ImageTransport depth0_it(depth0_nh);
-			image_transport::TransportHints hintsRgb0("raw", ros::TransportHints(), rgb0_pnh);
-			image_transport::TransportHints hintsDepth0("raw", ros::TransportHints(), depth0_pnh);
+			if(rgbdCameras == 2)
+			{
+				rgbd_image1_sub_.subscribe(nh, "rgbd_image0", 1);
+				rgbd_image2_sub_.subscribe(nh, "rgbd_image1", 1);
 
-			image_mono_sub_.subscribe(rgb0_it, rgb0_nh.resolveName("image"), 1, hintsRgb0);
-			image_depth_sub_.subscribe(depth0_it, depth0_nh.resolveName("image"), 1, hintsDepth0);
-			info_sub_.subscribe(rgb0_nh, "camera_info", 1);
+				if(approxSync)
+				{
+					approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
+							MyApproxSync2Policy(queueSize_),
+							rgbd_image1_sub_,
+							rgbd_image2_sub_);
+					approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2));
+				}
+				else
+				{
+					exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
+							MyExactSync2Policy(queueSize_),
+							rgbd_image1_sub_,
+							rgbd_image2_sub_);
+					exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2));
+				}
+				subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s",
+						getName().c_str(),
+						approxSync?"approx":"exact",
+						rgbd_image1_sub_.getTopic().c_str(),
+						rgbd_image2_sub_.getTopic().c_str());
+			}
+			else
+			{
+				rgbdSub_ = nh.subscribe("rgbd_image", queueSize_, &RGBDOdometry::callbackRGBD, this);
 
-			ros::NodeHandle rgb1_nh(nh, "rgb1");
-			ros::NodeHandle depth1_nh(nh, "depth1");
-			ros::NodeHandle rgb1_pnh(pnh, "rgb1");
-			ros::NodeHandle depth1_pnh(pnh, "depth1");
-			image_transport::ImageTransport rgb1_it(rgb1_nh);
-			image_transport::ImageTransport depth1_it(depth1_nh);
-			image_transport::TransportHints hintsRgb1("raw", ros::TransportHints(), rgb1_pnh);
-			image_transport::TransportHints hintsDepth1("raw", ros::TransportHints(), depth1_pnh);
-
-			image_mono2_sub_.subscribe(rgb1_it, rgb1_nh.resolveName("image"), 1, hintsRgb1);
-			image_depth2_sub_.subscribe(depth1_it, depth1_nh.resolveName("image"), 1, hintsDepth1);
-			info2_sub_.subscribe(rgb1_nh, "camera_info", 1);
-
-			NODELET_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
-					ros::this_node::getName().c_str(),
-					image_mono_sub_.getTopic().c_str(),
-					image_depth_sub_.getTopic().c_str(),
-					info_sub_.getTopic().c_str(),
-					image_mono2_sub_.getTopic().c_str(),
-					image_depth2_sub_.getTopic().c_str(),
-					info2_sub_.getTopic().c_str());
-
-			sync2_ = new message_filters::Synchronizer<MySync2Policy>(
-					MySync2Policy(queueSize_),
-					image_mono_sub_,
-					image_depth_sub_,
-					info_sub_,
-					image_mono2_sub_,
-					image_depth2_sub_,
-					info2_sub_);
-			sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6));
+				subscribedTopicsMsg =
+						uFormat("\n%s subscribed to:\n   %s",
+						getName().c_str(),
+						rgbdSub_.getTopic().c_str());
+			}
 		}
 		else
 		{
@@ -177,13 +183,136 @@ private:
 				exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
 			}
 
-			NODELET_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s",
-					ros::this_node::getName().c_str(),
+			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s",
+					getName().c_str(),
 					approxSync?"approx":"exact",
 					image_mono_sub_.getTopic().c_str(),
 					image_depth_sub_.getTopic().c_str(),
 					info_sub_.getTopic().c_str());
 		}
+		this->startWarningThread(subscribedTopicsMsg, approxSync);
+	}
+
+	virtual void updateParameters(ParametersMap & parameters)
+	{
+		//make sure we are using Reg/Strategy=0
+		ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
+		if(iter != parameters.end() && iter->second.compare("0") != 0)
+		{
+			ROS_WARN("RGBD odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str());
+		}
+		uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
+	}
+
+	void commonCallback(
+				const std::vector<cv_bridge::CvImageConstPtr> & rgbImages,
+				const std::vector<cv_bridge::CvImageConstPtr> & depthImages,
+				const std::vector<sensor_msgs::CameraInfo>& cameraInfos)
+	{
+		ROS_ASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size());
+		ros::Time higherStamp;
+		int imageWidth = rgbImages[0]->image.cols;
+		int imageHeight = rgbImages[0]->image.rows;
+		int cameraCount = rgbImages.size();
+		cv::Mat rgb;
+		cv::Mat depth;
+		pcl::PointCloud<pcl::PointXYZ> scanCloud;
+		std::vector<CameraModel> cameraModels;
+		for(unsigned int i=0; i<rgbImages.size(); ++i)
+		{
+			if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+				 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+				 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
+				 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+				 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
+				!(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
+				 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
+				 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
+			{
+				NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
+				return;
+			}
+			UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight,
+					uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
+							imageWidth,
+							rgbImages[i]->image.cols,
+							imageHeight,
+							rgbImages[i]->image.rows).c_str());
+			UASSERT_MSG(depthImages[i]->image.cols == imageWidth && depthImages[i]->image.rows == imageHeight,
+					uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
+							imageWidth,
+							depthImages[i]->image.cols,
+							imageHeight,
+							depthImages[i]->image.rows).c_str());
+
+			ros::Time stamp = rgbImages[i]->header.stamp>depthImages[i]->header.stamp?rgbImages[i]->header.stamp:depthImages[i]->header.stamp;
+
+			if(i == 0)
+			{
+				higherStamp = stamp;
+			}
+			else if(stamp > higherStamp)
+			{
+				higherStamp = stamp;
+			}
+
+			Transform localTransform = getTransform(this->frameId(), rgbImages[i]->header.frame_id, stamp);
+			if(localTransform.isNull())
+			{
+				return;
+			}
+
+			cv_bridge::CvImageConstPtr ptrImage = rgbImages[i];
+			if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
+			   rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
+			{
+				ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
+			}
+
+			cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
+			cv::Mat subDepth = ptrDepth->image;
+
+			// initialize
+			if(rgb.empty())
+			{
+				rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
+			}
+			if(depth.empty())
+			{
+				depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type());
+			}
+
+			if(ptrImage->image.type() == rgb.type())
+			{
+				ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
+			}
+			else
+			{
+				NODELET_ERROR("Some RGB images are not the same type!");
+				return;
+			}
+
+			if(subDepth.type() == depth.type())
+			{
+				subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
+			}
+			else
+			{
+				NODELET_ERROR("Some Depth images are not the same type!");
+				return;
+			}
+
+			cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfos[i], localTransform));
+		}
+
+		rtabmap::SensorData data(
+				rgb,
+				depth,
+				cameraModels,
+				0,
+				rtabmap_ros::timestampFromROS(higherStamp));
+
+		this->processData(data, higherStamp);
 	}
 
 	void callback(
@@ -191,179 +320,52 @@ private:
 			const sensor_msgs::ImageConstPtr& depth,
 			const sensor_msgs::CameraInfoConstPtr& cameraInfo)
 	{
+		callbackCalled();
 		if(!this->isPaused())
 		{
-			if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
-				 image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
-				 image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
-				 image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
-				 image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
-			   !(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
-				 depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
-				 depth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))
-			{
-				NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 "
-						  "recommended) and image_depth=16UC1,32FC1,mono16. Types detected: %s %s",
-						image->encoding.c_str(), depth->encoding.c_str());
-				return;
-			}
+			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
+			std::vector<cv_bridge::CvImageConstPtr> depthMsgs(1);
+			std::vector<sensor_msgs::CameraInfo> infoMsgs;
+			imageMsgs[0] = cv_bridge::toCvShare(image);
+			depthMsgs[0] = cv_bridge::toCvShare(depth);
+			infoMsgs.push_back(*cameraInfo);
 
-			ros::Time stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
-
-			Transform localTransform = getTransform(this->frameId(), image->header.frame_id, stamp);
-			if(localTransform.isNull())
-			{
-				return;
-			}
-
-			if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
-			{
-				rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform);
-				cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
-				cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth);
-
-				rtabmap::SensorData data(
-						ptrImage->image,
-						ptrDepth->image,
-						rtabmapModel,
-						0,
-						rtabmap_ros::timestampFromROS(stamp));
-
-				this->processData(data, stamp);
-			}
+			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
 		}
 	}
 
-	void callback2(
-			const sensor_msgs::ImageConstPtr& image,
-			const sensor_msgs::ImageConstPtr& depth,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfo,
-			const sensor_msgs::ImageConstPtr& image2,
-			const sensor_msgs::ImageConstPtr& depth2,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfo2)
+	void callbackRGBD(
+			const rtabmap_ros::RGBDImageConstPtr& image)
 	{
+		callbackCalled();
 		if(!this->isPaused())
 		{
-			std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
-			std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
-			std::vector<sensor_msgs::CameraInfoConstPtr> infoMsgs;
-			imageMsgs.push_back(image);
-			imageMsgs.push_back(image2);
-			depthMsgs.push_back(depth);
-			depthMsgs.push_back(depth2);
-			infoMsgs.push_back(cameraInfo);
-			infoMsgs.push_back(cameraInfo2);
+			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
+			std::vector<cv_bridge::CvImageConstPtr> depthMsgs(1);
+			std::vector<sensor_msgs::CameraInfo> infoMsgs;
+			rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
+			infoMsgs.push_back(image->cameraInfo);
 
-			ros::Time higherStamp;
-			int imageWidth = imageMsgs[0]->width;
-			int imageHeight = imageMsgs[0]->height;
-			int cameraCount = imageMsgs.size();
-			cv::Mat rgb;
-			cv::Mat depth;
-			pcl::PointCloud<pcl::PointXYZ> scanCloud;
-			std::vector<CameraModel> cameraModels;
-			for(unsigned int i=0; i<imageMsgs.size(); ++i)
-			{
-				if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
-					 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
-					 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
-					 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
-					 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
-					!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
-					 depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
-					 depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
-				{
-					NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
-					return;
-				}
-				UASSERT_MSG(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight,
-						uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
-								imageWidth,
-								imageMsgs[i]->width,
-								imageHeight,
-								imageMsgs[i]->height).c_str());
-				UASSERT_MSG(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight,
-						uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
-								imageWidth,
-								depthMsgs[i]->width,
-								imageHeight,
-								depthMsgs[i]->height).c_str());
+			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		}
+	}
 
-				ros::Time stamp = imageMsgs[i]->header.stamp>depthMsgs[i]->header.stamp?imageMsgs[i]->header.stamp:depthMsgs[i]->header.stamp;
+	void callbackRGBD2(
+			const rtabmap_ros::RGBDImageConstPtr& image,
+			const rtabmap_ros::RGBDImageConstPtr& image2)
+	{
+		callbackCalled();
+		if(!this->isPaused())
+		{
+			std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2);
+			std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2);
+			std::vector<sensor_msgs::CameraInfo> infoMsgs;
+			rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
+			rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
+			infoMsgs.push_back(image->cameraInfo);
+			infoMsgs.push_back(image2->cameraInfo);
 
-				if(i == 0)
-				{
-					higherStamp = stamp;
-				}
-				else if(stamp > higherStamp)
-				{
-					higherStamp = stamp;
-				}
-
-				Transform localTransform = getTransform(this->frameId(), imageMsgs[i]->header.frame_id, stamp);
-				if(localTransform.isNull())
-				{
-					return;
-				}
-
-				cv_bridge::CvImageConstPtr ptrImage;
-				if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
-				{
-					ptrImage = cv_bridge::toCvShare(imageMsgs[i]);
-				}
-				else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
-				   imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
-				{
-					ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");
-				}
-				else
-				{
-					ptrImage = cv_bridge::toCvShare(imageMsgs[i], "bgr8");
-				}
-				cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]);
-				cv::Mat subDepth = ptrDepth->image;
-
-				// initialize
-				if(rgb.empty())
-				{
-					rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
-				}
-				if(depth.empty())
-				{
-					depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type());
-				}
-
-				if(ptrImage->image.type() == rgb.type())
-				{
-					ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
-				}
-				else
-				{
-					NODELET_ERROR("Some RGB images are not the same type!");
-					return;
-				}
-
-				if(subDepth.type() == depth.type())
-				{
-					subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
-				}
-				else
-				{
-					NODELET_ERROR("Some Depth images are not the same type!");
-					return;
-				}
-
-				cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*infoMsgs[i], localTransform));
-			}
-
-			rtabmap::SensorData data(
-					rgb,
-					depth,
-					cameraModels,
-					0,
-					rtabmap_ros::timestampFromROS(higherStamp));
-
-			this->processData(data, higherStamp);
+			this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
 		}
 	}
 
@@ -383,18 +385,23 @@ protected:
 			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
 			exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
 		}
-		if(sync2_)
+		if(approxSync2_)
 		{
-			delete sync2_;
-			sync2_ = new message_filters::Synchronizer<MySync2Policy>(
-					MySync2Policy(queueSize_),
-					image_mono_sub_,
-					image_depth_sub_,
-					info_sub_,
-					image_mono2_sub_,
-					image_depth2_sub_,
-					info2_sub_);
-			sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6));
+			delete approxSync2_;
+			approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
+					MyApproxSync2Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_);
+			approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2));
+		}
+		if(exactSync2_)
+		{
+			delete exactSync2_;
+			exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
+					MyExactSync2Policy(queueSize_),
+					rgbd_image1_sub_,
+					rgbd_image2_sub_);
+			exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2));
 		}
 	}
 
@@ -402,15 +409,19 @@ private:
 	image_transport::SubscriberFilter image_mono_sub_;
 	image_transport::SubscriberFilter image_depth_sub_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
-	image_transport::SubscriberFilter image_mono2_sub_;
-	image_transport::SubscriberFilter image_depth2_sub_;
-	message_filters::Subscriber<sensor_msgs::CameraInfo> info2_sub_;
+
+	ros::Subscriber rgbdSub_;
+	message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image1_sub_;
+	message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image2_sub_;
+
 	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
 	message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
-	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
 	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
-	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySync2Policy;
-	message_filters::Synchronizer<MySync2Policy> * sync2_;
+	typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyApproxSync2Policy;
+	message_filters::Synchronizer<MyApproxSync2Policy> * approxSync2_;
+	typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync2Policy;
+	message_filters::Synchronizer<MyExactSync2Policy> * exactSync2_;
 	int queueSize_;
 };
 

src/nodelets/rgbd_sync.cpp

@@ -0,0 +1,165 @@
+/*
+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 <sensor_msgs/Image.h>
+#include <sensor_msgs/CompressedImage.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/CameraInfo.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/subscriber.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+
+#include "rtabmap_ros/RGBDImage.h"
+
+#include "rtabmap/core/Compression.h"
+
+namespace rtabmap_ros
+{
+
+class RGBDSync : public nodelet::Nodelet
+{
+public:
+	RGBDSync() :
+		approxSyncDepth_(0),
+		exactSyncDepth_(0)
+	{}
+
+	virtual ~RGBDSync()
+	{
+		if(approxSyncDepth_)
+			delete approxSyncDepth_;
+		if(exactSyncDepth_)
+			delete exactSyncDepth_;
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int queueSize = 10;
+		bool approxSync = true;
+		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("queue_size", queueSize, queueSize);
+
+		NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
+
+		rgbdImagePub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image", 1);
+		rgbdImageCompressedPub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image/compressed", 1);
+
+		if(approxSync)
+		{
+			approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
+			approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3));
+		}
+		else
+		{
+			exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
+			exactSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3));
+		}
+
+		ros::NodeHandle rgb_nh(nh, "rgb");
+		ros::NodeHandle depth_nh(nh, "depth");
+		ros::NodeHandle rgb_pnh(pnh, "rgb");
+		ros::NodeHandle depth_pnh(pnh, "depth");
+		image_transport::ImageTransport rgb_it(rgb_nh);
+		image_transport::ImageTransport depth_it(depth_nh);
+		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+		image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+
+		imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+		imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+		cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+	}
+
+	void callback(
+			  const sensor_msgs::ImageConstPtr& image,
+			  const sensor_msgs::ImageConstPtr& depth,
+			  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
+	{
+		if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
+		{
+			rtabmap_ros::RGBDImage msg;
+			msg.header.frame_id = cameraInfo->header.frame_id;
+			msg.header.stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
+			msg.cameraInfo = *cameraInfo;
+
+			if(rgbdImageCompressedPub_.getNumSubscribers())
+			{
+				rtabmap_ros::RGBDImage msgCompressed = msg;
+
+				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
+				imagePtr->toCompressedImageMsg(msgCompressed.rgbCompressed, cv_bridge::JPG);
+
+				cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
+				ROS_ASSERT(imageDepthPtr->image.type() == CV_32FC1 || imageDepthPtr->image.type() == CV_16UC1);
+				msgCompressed.depthCompressed.header = imageDepthPtr->header;
+				msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
+				msgCompressed.depthCompressed.format = "png";
+
+				rgbdImageCompressedPub_.publish(msgCompressed);
+			}
+
+			if(rgbdImagePub_.getNumSubscribers())
+			{
+				msg.rgb = *image;
+				msg.depth = *depth;
+				rgbdImagePub_.publish(msg);
+			}
+		}
+	}
+
+private:
+	ros::Publisher rgbdImagePub_;
+	ros::Publisher rgbdImageCompressedPub_;
+
+	image_transport::SubscriberFilter imageSub_;
+	image_transport::SubscriberFilter imageDepthSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
+	message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
+
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
+	message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDSync, nodelet::Nodelet);
+}
+

src/nodelets/rgbdicp_odometry.cpp

@@ -0,0 +1,406 @@
+/*
+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/OdometryROS.h>
+
+#include <pluginlib/class_list_macros.h>
+#include <nodelet/nodelet.h>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <image_geometry/stereo_camera_model.h>
+#include <laser_geometry/laser_geometry.h>
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/LaserScan.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include "rtabmap_ros/MsgConversion.h"
+
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_surface.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+
+using namespace rtabmap;
+
+namespace rtabmap_ros
+{
+
+class RGBDICPOdometry : public rtabmap_ros::OdometryROS
+{
+public:
+	RGBDICPOdometry() :
+		OdometryROS(false, true, true),
+		approxScanSync_(0),
+		exactScanSync_(0),
+		approxCloudSync_(0),
+		exactCloudSync_(0),
+		queueSize_(5),
+		scanCloudMaxPoints_(0),
+		scanCloudNormalK_(0)
+	{
+	}
+
+	virtual ~RGBDICPOdometry()
+	{
+		if(approxScanSync_)
+		{
+			delete approxScanSync_;
+		}
+		if(exactScanSync_)
+		{
+			delete exactScanSync_;
+		}
+		if(approxCloudSync_)
+		{
+			delete approxCloudSync_;
+		}
+		if(exactCloudSync_)
+		{
+			delete exactCloudSync_;
+		}
+	}
+
+private:
+
+	virtual void onOdomInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		bool approxSync = true;
+		bool subscribeScanCloud = false;
+		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("queue_size", queueSize_, queueSize_);
+		pnh.param("subscribe_scan_cloud", subscribeScanCloud, subscribeScanCloud);
+		pnh.param("scan_cloud_max_points",  scanCloudMaxPoints_, scanCloudMaxPoints_);
+		pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_);
+
+		ros::NodeHandle rgb_nh(nh, "rgb");
+		ros::NodeHandle depth_nh(nh, "depth");
+		ros::NodeHandle rgb_pnh(pnh, "rgb");
+		ros::NodeHandle depth_pnh(pnh, "depth");
+		image_transport::ImageTransport rgb_it(rgb_nh);
+		image_transport::ImageTransport depth_it(depth_nh);
+		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+		image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+
+		image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+		image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+		info_sub_.subscribe(rgb_nh, "camera_info", 1);
+
+		std::string subscribedTopicsMsg;
+		if(subscribeScanCloud)
+		{
+			cloud_sub_.subscribe(nh, "scan_cloud", 1);
+			if(approxSync)
+			{
+				approxCloudSync_ = new message_filters::Synchronizer<MyApproxCloudSyncPolicy>(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
+				approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4));
+			}
+			else
+			{
+				exactCloudSync_ = new message_filters::Synchronizer<MyExactCloudSyncPolicy>(MyExactCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
+				exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4));
+			}
+
+			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s, \n   %s",
+					getName().c_str(),
+					approxSync?"approx":"exact",
+					image_mono_sub_.getTopic().c_str(),
+					image_depth_sub_.getTopic().c_str(),
+					info_sub_.getTopic().c_str(),
+					cloud_sub_.getTopic().c_str());
+		}
+		else
+		{
+			scan_sub_.subscribe(nh, "scan", 1);
+			if(approxSync)
+			{
+				approxScanSync_ = new message_filters::Synchronizer<MyApproxScanSyncPolicy>(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
+				approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4));
+			}
+			else
+			{
+				exactScanSync_ = new message_filters::Synchronizer<MyExactScanSyncPolicy>(MyExactScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
+				exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4));
+			}
+
+			subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s, \n   %s",
+					getName().c_str(),
+					approxSync?"approx":"exact",
+					image_mono_sub_.getTopic().c_str(),
+					image_depth_sub_.getTopic().c_str(),
+					info_sub_.getTopic().c_str(),
+					scan_sub_.getTopic().c_str());
+		}
+		this->startWarningThread(subscribedTopicsMsg, approxSync);
+	}
+
+	virtual void updateParameters(ParametersMap & parameters)
+	{
+		//make sure we are using Reg/Strategy=0
+		ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
+		if(iter != parameters.end() && iter->second.compare("0") != 0)
+		{
+			ROS_WARN("RGBDICP odometry works only with \"Reg/Strategy\"=2. Ignoring value %s.", iter->second.c_str());
+		}
+		uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2"));
+	}
+
+	void callback(
+			const sensor_msgs::ImageConstPtr& image,
+			const sensor_msgs::ImageConstPtr& depth,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo)
+	{
+		sensor_msgs::LaserScanConstPtr scanMsg;
+		sensor_msgs::PointCloud2ConstPtr cloudMsg;
+		callbackCommon(image, depth, cameraInfo, scanMsg, cloudMsg);
+	}
+
+	void callbackScan(
+			const sensor_msgs::ImageConstPtr& image,
+			const sensor_msgs::ImageConstPtr& depth,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo,
+			const sensor_msgs::LaserScanConstPtr& scanMsg)
+	{
+		sensor_msgs::PointCloud2ConstPtr cloudMsg;
+		callbackCommon(image, depth, cameraInfo, scanMsg, cloudMsg);
+	}
+
+	void callbackCloud(
+			const sensor_msgs::ImageConstPtr& image,
+			const sensor_msgs::ImageConstPtr& depth,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo,
+			const sensor_msgs::PointCloud2ConstPtr& cloudMsg)
+	{
+		sensor_msgs::LaserScanConstPtr scanMsg;
+		callbackCommon(image, depth, cameraInfo, scanMsg, cloudMsg);
+	}
+
+	void callbackCommon(
+			const sensor_msgs::ImageConstPtr& image,
+			const sensor_msgs::ImageConstPtr& depth,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo,
+			const sensor_msgs::LaserScanConstPtr& scanMsg,
+			const sensor_msgs::PointCloud2ConstPtr& cloudMsg)
+	{
+		callbackCalled();
+		if(!this->isPaused())
+		{
+			if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+				 image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+				 image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
+				 image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+				 image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
+			   !(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
+				 depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
+				 depth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))
+			{
+				NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 "
+						  "recommended) and image_depth=16UC1,32FC1,mono16. Types detected: %s %s",
+						image->encoding.c_str(), depth->encoding.c_str());
+				return;
+			}
+
+			// use the highest stamp to make sure that there will be no future interpolation required when synchronized with another node
+			ros::Time stamp = image->header.stamp > depth->header.stamp? image->header.stamp : depth->header.stamp;
+			if(scanMsg.get() != 0)
+			{
+				if(stamp < scanMsg->header.stamp)
+				{
+					stamp = scanMsg->header.stamp;
+				}
+			}
+			else if(cloudMsg.get() != 0)
+			{
+				if(stamp < cloudMsg->header.stamp)
+				{
+					stamp = cloudMsg->header.stamp;
+				}
+			}
+
+			Transform localTransform = getTransform(this->frameId(), image->header.frame_id, stamp);
+			if(localTransform.isNull())
+			{
+				return;
+			}
+
+			if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
+			{
+				rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform);
+				cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
+				cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth);
+
+				cv::Mat scan;
+				Transform localScanTransform = Transform::getIdentity();
+				if(scanMsg.get() != 0)
+				{
+					// make sure the frame of the laser is updated too
+					localScanTransform = getTransform(this->frameId(),
+							scanMsg->header.frame_id,
+							scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment));
+					if(localScanTransform.isNull())
+					{
+						ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec());
+						return;
+					}
+
+					//transform in frameId_ frame
+					sensor_msgs::PointCloud2 scanOut;
+					laser_geometry::LaserProjection projection;
+					projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener());
+					pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+					pcl::fromROSMsg(scanOut, *pclScan);
+
+					scan = util3d::laserScan2dFromPointCloud(*pclScan);
+				}
+				else if(cloudMsg.get() != 0)
+				{
+					bool containNormals = false;
+					for(unsigned int i=0; i<cloudMsg->fields.size(); ++i)
+					{
+						if(cloudMsg->fields[i].name.compare("normal_x") == 0)
+						{
+							containNormals = true;
+							break;
+						}
+					}
+					localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp);
+					if(localScanTransform.isNull())
+					{
+						ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg->header.stamp.toSec());
+						return;
+					}
+
+					if(containNormals)
+					{
+						pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
+						pcl::fromROSMsg(*cloudMsg, *pclScan);
+						scan = util3d::laserScanFromPointCloud(*pclScan);
+					}
+					else
+					{
+						pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+						pcl::fromROSMsg(*cloudMsg, *pclScan);
+
+						if(scanCloudNormalK_ > 0)
+						{
+							//compute normals
+							pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanCloudNormalK_);
+							pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
+							pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+							scan = util3d::laserScanFromPointCloud(*pclScanNormal);
+						}
+						else
+						{
+							scan = util3d::laserScanFromPointCloud(*pclScan);
+						}
+					}
+				}
+
+				rtabmap::SensorData data(
+						scan,
+						LaserScanInfo(
+								scanMsg.get() != 0?(int)scanMsg->ranges.size():cloudMsg.get() != 0?scanCloudMaxPoints_:0,
+								scanMsg.get() != 0?scanMsg->range_max:0,
+								localScanTransform),
+						ptrImage->image,
+						ptrDepth->image,
+						rtabmapModel,
+						0,
+						rtabmap_ros::timestampFromROS(stamp));
+
+				this->processData(data, stamp);
+			}
+		}
+	}
+
+protected:
+	virtual void flushCallbacks()
+	{
+		// flush callbacks
+		if(approxScanSync_)
+		{
+			delete approxScanSync_;
+			approxScanSync_ = new message_filters::Synchronizer<MyApproxScanSyncPolicy>(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
+			approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4));
+		}
+		if(exactScanSync_)
+		{
+			delete exactScanSync_;
+			exactScanSync_ = new message_filters::Synchronizer<MyExactScanSyncPolicy>(MyExactScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
+			exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4));
+		}
+		if(approxCloudSync_)
+		{
+			delete approxCloudSync_;
+			approxCloudSync_ = new message_filters::Synchronizer<MyApproxCloudSyncPolicy>(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
+			approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4));
+		}
+		if(exactCloudSync_)
+		{
+			delete exactCloudSync_;
+			exactCloudSync_ = new message_filters::Synchronizer<MyExactCloudSyncPolicy>(MyExactCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
+			exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4));
+		}
+	}
+
+private:
+	image_transport::SubscriberFilter image_mono_sub_;
+	image_transport::SubscriberFilter image_depth_sub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
+	message_filters::Subscriber<sensor_msgs::LaserScan> scan_sub_;
+	message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan> MyApproxScanSyncPolicy;
+	message_filters::Synchronizer<MyApproxScanSyncPolicy> * approxScanSync_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan> MyExactScanSyncPolicy;
+	message_filters::Synchronizer<MyExactScanSyncPolicy> * exactScanSync_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2> MyApproxCloudSyncPolicy;
+	message_filters::Synchronizer<MyApproxCloudSyncPolicy> * approxCloudSync_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2> MyExactCloudSyncPolicy;
+	message_filters::Synchronizer<MyExactCloudSyncPolicy> * exactCloudSync_;
+	int queueSize_;
+	int scanCloudMaxPoints_;
+	int scanCloudNormalK_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDICPOdometry, nodelet::Nodelet);
+
+}

src/nodelets/stereo_odometry.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "OdometryROS.h"
+#include "rtabmap_ros/OdometryROS.h"
 #include "pluginlib/class_list_macros.h"
 #include "nodelet/nodelet.h"
 
@@ -47,6 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UStl.h>
 
 using namespace rtabmap;
 
@@ -57,7 +58,7 @@ class StereoOdometry : public rtabmap_ros::OdometryROS
 {
 public:
 	StereoOdometry() :
-		rtabmap_ros::OdometryROS(true),
+		rtabmap_ros::OdometryROS(true, true, false),
 		approxSync_(0),
 		exactSync_(0),
 		queueSize_(5)
@@ -85,7 +86,6 @@ private:
 		bool approxSync = false;
 		pnh.param("approx_sync", approxSync, approxSync);
 		pnh.param("queue_size", queueSize_, queueSize_);
-		NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
 
 		ros::NodeHandle left_nh(nh, "left");
 		ros::NodeHandle right_nh(nh, "right");
@@ -113,13 +113,25 @@ private:
 		}
 
 
-		NODELET_INFO("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s",
-				ros::this_node::getName().c_str(),
+		std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s",
+				getName().c_str(),
 				approxSync?"approx":"exact",
 				imageRectLeft_.getTopic().c_str(),
 				imageRectRight_.getTopic().c_str(),
 				cameraInfoLeft_.getTopic().c_str(),
 				cameraInfoRight_.getTopic().c_str());
+		this->startWarningThread(subscribedTopicsMsg, approxSync);
+	}
+
+	virtual void updateParameters(ParametersMap & parameters)
+	{
+		//make sure we are using Reg/Strategy=0
+		ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
+		if(iter != parameters.end() && iter->second.compare("0") != 0)
+		{
+			ROS_WARN("Stereo odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str());
+		}
+		uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
 	}
 
 	void callback(
@@ -128,6 +140,7 @@ private:
 			const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
 			const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
 	{
+		callbackCalled();
 		if(!this->isPaused())
 		{
 			if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||

src/nodelets/undistort_depth.cpp

@@ -0,0 +1,123 @@
+/*
+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 <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber.h>
+#include <image_transport/publisher.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+
+#include "rtabmap/core/clams/discrete_depth_distortion_model.h"
+#include "rtabmap/utilite/UConversion.h"
+
+namespace rtabmap_ros
+{
+
+class UndistortDepth : public nodelet::Nodelet
+{
+public:
+	UndistortDepth()
+	{}
+
+	virtual ~UndistortDepth()
+	{
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int queueSize = 10;
+		std::string modelPath;
+		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("model", modelPath, modelPath);
+
+		if(modelPath.empty())
+		{
+			NODELET_ERROR("undistort_depth: \"model\" parameter should be set!");
+		}
+
+		model_.load(modelPath);
+		if(!model_.isValid())
+		{
+			NODELET_ERROR("Loaded distortion model from \"%s\" is not valid!", modelPath.c_str());
+		}
+		else
+		{
+			image_transport::ImageTransport it(nh);
+			sub_ = it.subscribe("depth", queueSize, &UndistortDepth::callback, this);
+			pub_ = it.advertise(uFormat("%s_undistorted", nh.resolveName("depth").c_str()), 1);
+		}
+	}
+
+	void callback(const sensor_msgs::ImageConstPtr& depth)
+	{
+		if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
+		   depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 &&
+		   depth->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0)
+		{
+			NODELET_ERROR("Input type depth=32FC1,16UC1,MONO16");
+			return;
+		}
+
+		if(pub_.getNumSubscribers())
+		{
+			if(depth->width == model_.getWidth() && depth->width == model_.getWidth())
+			{
+				cv_bridge::CvImagePtr imageDepthPtr = cv_bridge::toCvCopy(depth);
+				model_.undistort(imageDepthPtr->image);
+				pub_.publish(imageDepthPtr->toImageMsg());
+			}
+			else
+			{
+				NODELET_ERROR("Input depth image size (%dx%d) and distortion model "
+						"size (%dx%d) don't match! Cannot undistort image.",
+						depth->width, depth->height,
+						model_.getWidth(), model_.getHeight());
+			}
+		}
+	}
+
+private:
+	clams::DiscreteDepthDistortionModel model_;
+	image_transport::Publisher pub_;
+	image_transport::Subscriber sub_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_ros::UndistortDepth, nodelet::Nodelet);
+}
+

src/rviz/MapCloudDisplay.cpp

@@ -272,58 +272,60 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 	for(unsigned int i=0; i<map.nodes.size() && i<map.nodes.size(); ++i)
 	{
 		int id = map.nodes[i].id;
-		if(poses.find(id) != poses.end() &&
-		   cloud_infos_.find(id) == cloud_infos_.end())
+
+		// Always refresh the cloud if there are data
+		rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(map.nodes[i]);
+		if(!s.sensorData().imageCompressed().empty() &&
+		   !s.sensorData().depthOrRightCompressed().empty() &&
+		   (s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValidForProjection()))
 		{
-			// Cloud not added to RVIZ, add it!
-			rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(map.nodes[i]);
-			if(!s.sensorData().imageCompressed().empty() &&
-			   !s.sensorData().depthOrRightCompressed().empty() &&
-			   (s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValidForProjection()))
+			cv::Mat image, depth;
+			s.sensorData().uncompressData(&image, &depth, 0);
+
+
+			if(!s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty())
 			{
-				cv::Mat image, depth;
-				s.sensorData().uncompressData(&image, &depth, 0);
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+				pcl::IndicesPtr validIndices(new std::vector<int>);
+				cloud = rtabmap::util3d::cloudRGBFromSensorData(
+						s.sensorData(),
+						cloud_decimation_->getInt(),
+						cloud_max_depth_->getFloat(),
+						cloud_min_depth_->getFloat(),
+						validIndices.get());
 
-
-				if(!s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty())
+				if(cloud_voxel_size_->getFloat())
 				{
-					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-					pcl::IndicesPtr validIndices(new std::vector<int>);
-					cloud = rtabmap::util3d::cloudRGBFromSensorData(
-							s.sensorData(),
-							cloud_decimation_->getInt(),
-							cloud_max_depth_->getFloat(),
-							cloud_min_depth_->getFloat(),
-							validIndices.get());
+					cloud = rtabmap::util3d::voxelize(cloud, validIndices, cloud_voxel_size_->getFloat());
+				}
 
-					if(cloud_voxel_size_->getFloat())
+				if(cloud->size())
+				{
+					if(cloud_filter_floor_height_->getFloat() > 0.0f || cloud_filter_ceiling_height_->getFloat() > 0.0f)
 					{
-						cloud = rtabmap::util3d::voxelize(cloud, validIndices, cloud_voxel_size_->getFloat());
+						// convert in /odom frame
+						cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose());
+						cloud = rtabmap::util3d::passThrough(cloud, "z",
+								cloud_filter_floor_height_->getFloat()>0.0f?cloud_filter_floor_height_->getFloat():-999.0f,
+								cloud_filter_ceiling_height_->getFloat()>0.0f && (cloud_filter_floor_height_->getFloat()<=0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f);
+						// convert back in /base_link frame
+						cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose().inverse());
 					}
 
-					if(cloud->size())
+					sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
+					pcl::toROSMsg(*cloud, *cloudMsg);
+					cloudMsg->header = map.header;
+
+					CloudInfoPtr info(new CloudInfo);
+					info->message_ = cloudMsg;
+					info->pose_ = rtabmap::Transform::getIdentity();
+					info->id_ = id;
+
+					if (transformCloud(info, true))
 					{
-						if(cloud_filter_floor_height_->getFloat() > 0.0f || cloud_filter_ceiling_height_->getFloat() > 0.0f)
-						{
-							cloud = rtabmap::util3d::passThrough(cloud, "z",
-									cloud_filter_floor_height_->getFloat()>0.0f?cloud_filter_floor_height_->getFloat():-999.0f,
-									cloud_filter_ceiling_height_->getFloat()>0.0f && (cloud_filter_floor_height_->getFloat()<=0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f);
-						}
-
-						sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
-						pcl::toROSMsg(*cloud, *cloudMsg);
-						cloudMsg->header = map.header;
-
-						CloudInfoPtr info(new CloudInfo);
-						info->message_ = cloudMsg;
-						info->pose_ = rtabmap::Transform::getIdentity();
-						info->id_ = id;
-
-						if (transformCloud(info, true))
-						{
-							boost::mutex::scoped_lock lock(new_clouds_mutex_);
-							new_cloud_infos_.insert(std::make_pair(id, info));
-						}
+						boost::mutex::scoped_lock lock(new_clouds_mutex_);
+						new_cloud_infos_.erase(id);
+						new_cloud_infos_.insert(std::make_pair(id, info));
 					}
 				}
 			}
@@ -485,7 +487,7 @@ void MapCloudDisplay::downloadMap()
 		ros::NodeHandle nh;
 		QMessageBox * messageBox = new QMessageBox(
 				QMessageBox::NoIcon,
-				tr("Calling \"%1\" service...").arg(nh.resolveName("rtabmap/get_map").c_str()),
+				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);
@@ -493,18 +495,18 @@ void MapCloudDisplay::downloadMap()
 		QApplication::processEvents();
 		uSleep(100); // hack make sure the text in the QMessageBox is shown...
 		QApplication::processEvents();
-		if(!ros::service::call("rtabmap/get_map", getMapSrv))
+		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\" in the launch "
-					  "file like: <remap from=\"rtabmap/get_map\" to=\"get_map\"/>.",
-					  nh.resolveName("rtabmap/get_map").c_str());
+					  "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\" in the launch "
-					  "file like: <remap from=\"rtabmap/get_map\" to=\"get_map\"/>.").
-					  arg(nh.resolveName("rtabmap/get_map").c_str()));
+					  "file like: <remap from=\"rtabmap/get_map_data\" to=\"get_map_data\"/>.").
+					  arg(nh.resolveName("rtabmap/get_map_data").c_str()));
 		}
 		else
 		{
@@ -633,6 +635,7 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
 				cloud_info->scene_node_->attachObject( cloud_info->cloud_.get() );
 				cloud_info->scene_node_->setVisible(false);
 
+				cloud_infos_.erase(it->first);
 				cloud_infos_.insert(*it);
 			}