CommonSubscribers: added RGB-only callbacks, added odom-only callbacks. Odom: Fixed reset_odom service assert.

src/impl/CommonDataSubscriberOdom.cpp

@@ -0,0 +1,113 @@
+/*
+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 {
+
+void CommonDataSubscriber::odomCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg)
+{
+	callbackCalled();
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomInfoCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomDataCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg)
+{
+	callbackCalled();
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::odomDataInfoCallback(
+		const nav_msgs::OdometryConstPtr& odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	callbackCalled();
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
+}
+
+void CommonDataSubscriber::setupOdomCallbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeUserData,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup scan callback");
+
+	if(subscribeUserData || subscribeOdomInfo)
+	{
+		odomSub_.subscribe(nh, "odom", 1);
+
+		if(subscribeUserData)
+		{
+			userDataSub_.subscribe(nh, "user_data", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL3(odomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL2(odomData, approxSync, queueSize, odomSub_, userDataSub_);
+			}
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL2(odomInfo, approxSync, queueSize, odomSub_, odomInfoSub_);
+		}
+	}
+	else
+	{
+		odomSubOnly_ = nh.subscribe("odom", 1, &CommonDataSubscriber::odomCallback, this);
+		subscribedTopicsMsg_ =
+				uFormat("\n%s subscribed to:\n   %s",
+				ros::this_node::getName().c_str(),
+				odomSubOnly_.getTopic().c_str());
+	}
+}
+
+} /* namespace rtabmap_ros */

src/impl/CommonDataSubscriberRGB.cpp

@@ -0,0 +1,532 @@
+/*
+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
+void CommonDataSubscriber::rgbCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		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
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbScan2dCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		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
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbScan3dCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		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
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbInfoCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		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
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbScan2dInfoCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbScan3dInfoCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+
+// RGB + Odom
+void CommonDataSubscriber::rgbOdomCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		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
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		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
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		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
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		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
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomScan2dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomScan3dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+
+// RGB + Depth + User Data
+void CommonDataSubscriber::rgbDataCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		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
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbDataScan2dCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		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
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbDataScan3dCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		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
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbDataInfoCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		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
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbDataScan2dInfoCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbDataScan3dInfoCallback(
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	nav_msgs::OdometryConstPtr odomMsg; // null
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+
+// RGB + Depth + Odom + User Data
+void CommonDataSubscriber::rgbOdomDataCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomDataScan2dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg)
+{
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomDataScan3dCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg)
+{
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomDataInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomDataScan2dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::LaserScanConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+void CommonDataSubscriber::rgbOdomDataScan3dInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::UserDataConstPtr & userDataMsg,
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const sensor_msgs::PointCloud2ConstPtr& scanMsg,
+		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
+}
+
+void CommonDataSubscriber::setupRGBCallbacks(
+		ros::NodeHandle & nh,
+		ros::NodeHandle & pnh,
+		bool subscribeOdom,
+		bool subscribeUserData,
+		bool subscribeScan2d,
+		bool subscribeScan3d,
+		bool subscribeOdomInfo,
+		int queueSize,
+		bool approxSync)
+{
+	ROS_INFO("Setup rgb-only callback");
+
+	std::string rgbPrefix = "rgb";
+	ros::NodeHandle rgb_nh(nh, rgbPrefix);
+	ros::NodeHandle rgb_pnh(pnh, rgbPrefix);
+	image_transport::ImageTransport rgb_it(rgb_nh);
+	image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+
+	imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+	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);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL6(rgbOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(rgbOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
+			}
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL6(rgbOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL5(rgbOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
+			}
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL5(rgbOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL4(rgbOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_);
+		}
+	}
+	else if(subscribeOdom)
+	{
+		odomSub_.subscribe(nh, "odom", 1);
+
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL5(rgbOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(rgbOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_);
+			}
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL5(rgbOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(rgbOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_);
+			}
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL4(rgbOdomInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL3(rgbOdom, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_);
+		}
+	}
+	else if(subscribeUserData)
+	{
+		userDataSub_.subscribe(nh, "user_data", 1);
+
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL5(rgbDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(rgbDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
+			}
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL5(rgbDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL4(rgbDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
+			}
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL4(rgbDataInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL3(rgbData, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_);
+		}
+	}
+	else
+	{
+		if(subscribeScan2d)
+		{
+			subscribedToScan2d_ = true;
+			scanSub_.subscribe(nh, "scan", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL4(rgbScan2dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL3(rgbScan2d, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_);
+			}
+		}
+		else if(subscribeScan3d)
+		{
+			subscribedToScan3d_ = true;
+			scan3dSub_.subscribe(nh, "scan_cloud", 1);
+			if(subscribeOdomInfo)
+			{
+				subscribedToOdomInfo_ = true;
+				odomInfoSub_.subscribe(nh, "odom_info", 1);
+				SYNC_DECL4(rgbScan3dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
+			}
+			else
+			{
+				SYNC_DECL3(rgbScan3d, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_);
+			}
+		}
+		else if(subscribeOdomInfo)
+		{
+			subscribedToOdomInfo_ = true;
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			SYNC_DECL3(rgbInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, odomInfoSub_);
+		}
+		else
+		{
+			SYNC_DECL2(rgb, approxSync, queueSize, imageSub_, cameraInfoSub_);
+		}
+	}
+}
+
+} /* namespace rtabmap_ros */