ros-pkg: Added obstacles_detection nodelet

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1923 f169173b-cf89-36c8-b27e-44dbe73f0c83

CMakeLists.txt

@@ -124,6 +124,7 @@ add_library(rtabmap_ros
    src/nodelets/point_cloud_xyzrgb.cpp 
    src/nodelets/point_cloud_xyz.cpp
    src/nodelets/disparity_to_depth.cpp 
+   src/nodelets/obstacles_detection.cpp
    src/MsgConversion.cpp
    src/OdometryROS.cpp
    src/rviz/MapCloudDisplay.cpp

launch/tests/test_stereo_mapping_bag.launch

@@ -118,10 +118,13 @@
   <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap)/launch/config/rgbd.rviz"/>
   <node pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>
   <node pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap/data_odom_sync standalone_nodelet">
-    <remap from="rgb/image_in"       to="/stereo_camera/left/image_rect_color"/>
-    <remap from="depth/image_in"     to="/odom_depth"/>
+    <remap from="rgb/image_in"       to="/stereo_camera/left/image_rect"/>
+    <remap from="depth/image_in"     to="/stereo_camera/depth"/>
     <remap from="rgb/camera_info_in" to="/stereo_camera/left/camera_info"/>
-    <remap from="odom_in"             to="/stereo_odometer/odom"/>
+    <remap from="odom_in"             to="/stereo_odometer/odometry"/>
+    
+    <param name="rgb/image_transport" type="string" value="compressed"/>
+    <param name="depth/image_transport" type="string" value="compressedDepth"/>
     
     <remap from="rgb/image_out"       to="data_odom_sync/image"/>
     <remap from="depth/image_out"     to="data_odom_sync/depth"/>
@@ -137,7 +140,21 @@
     <remap from="cloud"           to="voxel_cloud" />
     
     <param name="queue_size" type="int" value="10"/>
+    <param name="decimation" type="int" value="2"/>
   </node>
   
+  <node pkg="nodelet" type="nodelet" name="points_xyz" args="load rtabmap/point_cloud_xyz standalone_nodelet">
+    <remap from="depth/image"     to="data_odom_sync/depth"/>
+    <remap from="depth/camera_info" to="data_odom_sync/camera_info"/>
+    <remap from="cloud"           to="cloudXYZ" />
+    
+    <param name="queue_size" type="int" value="10"/>
+    <param name="decimation" type="int" value="4"/>
+    <param name="max_depth" type="double" value="4"/>
+    <param name="voxel_size" type="double" value="0.05"/>
+  </node>
+  <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap/obstacles_detection standalone_nodelet">
+    <remap from="cloud" to="cloudXYZ"/>
+  </node>
   
 </launch>

nodelet_plugins.xml

@@ -46,4 +46,12 @@
       This is my nodelet.
     </description>
   </class>
+  
+  <class name="rtabmap/obstacles_detection" 
+         type="rtabmap::ObstaclesDetection" 
+         base_class_type="nodelet::Nodelet">
+    <description>
+      This is my nodelet.
+    </description>
+  </class>
 </library>

src/nodelets/obstacles_detection.cpp

@@ -0,0 +1,224 @@
+/*
+Copyright (c) 2010-2014, 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/tf.h>
+#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/MsgConversion.h>
+
+#include "rtabmap/core/util3d.h"
+
+namespace rtabmap
+{
+
+class ObstaclesDetection : public nodelet::Nodelet
+{
+public:
+	ObstaclesDetection() :
+		frameId_("base_link"),
+		mapFrameId_("odom"),
+		normalEstimationRadius_(0.05),
+		groundNormalAngle_(M_PI_4),
+		minClusterSize_(20),
+		maxObstaclesHeight_(0)
+	{}
+
+	virtual ~ObstaclesDetection()
+	{}
+
+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("map_frame_id", mapFrameId_, mapFrameId_);
+		pnh.param("normal_estimation_radius", normalEstimationRadius_, normalEstimationRadius_);
+		pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_);
+		pnh.param("min_cluster_size", minClusterSize_, minClusterSize_);
+		pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_);
+
+		cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this);
+
+		groundPub_ = nh.advertise<sensor_msgs::PointCloud2>("ground", 1);
+		obstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("obstacles", 1);
+		obstaclesToMap2dPub_ = nh.advertise<sensor_msgs::PointCloud2>("obstacles_2d", 1);
+	}
+
+
+
+	void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
+	{
+		if(groundPub_.getNumSubscribers() || obstaclesPub_.getNumSubscribers() || obstaclesToMap2dPub_.getNumSubscribers())
+		{
+			Transform localTransform, fixedToBaseTransform;
+			try
+			{
+				tf::StampedTransform tmp;
+				if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
+				{
+					ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
+					return;
+				}
+				tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp);
+				localTransform = transformFromTF(tmp);
+
+				if(obstaclesToMap2dPub_.getNumSubscribers())
+				{
+					if(!tfListener_.waitForTransform(mapFrameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
+					{
+						ROS_WARN("Could not get transform from %s to %s after 1 second!", mapFrameId_.c_str(), cloudMsg->header.frame_id.c_str());
+						return;
+					}
+					tfListener_.lookupTransform(mapFrameId_, frameId_, cloudMsg->header.stamp, tmp);
+					fixedToBaseTransform = transformFromTF(tmp);
+				}
+			}
+			catch(tf::TransformException & ex)
+			{
+				ROS_WARN("%s",ex.what());
+				return;
+			}
+
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+			pcl::fromROSMsg(*cloudMsg, *cloud);
+			pcl::IndicesPtr ground, obstacles;
+			if(cloud->size())
+			{
+				cloud = util3d::transformPointCloud<pcl::PointXYZ>(cloud, localTransform);
+
+				if(maxObstaclesHeight_ > 0)
+				{
+					cloud = util3d::passThrough<pcl::PointXYZ>(cloud, "z", std::numeric_limits<int>::min(), maxObstaclesHeight_);
+				}
+				util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud,
+						ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_);
+			}
+
+			pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
+			if(groundPub_.getNumSubscribers() && ground->size())
+			{
+				pcl::copyPointCloud(*cloud, *ground, *groundCloud);
+			}
+			pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesToMap2dCloud(new pcl::PointCloud<pcl::PointXYZ>);
+			if((obstaclesPub_.getNumSubscribers() || obstaclesToMap2dPub_.getNumSubscribers()) && obstacles->size())
+			{
+				pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
+
+				if(obstaclesToMap2dPub_.getNumSubscribers())
+				{
+					//force 2d
+					float x,y,z, roll,pitch,yaw;
+					fixedToBaseTransform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+					Transform transform2d(x,y, 0, 0,0,yaw);
+					obstaclesToMap2dCloud = util3d::transformPointCloud<pcl::PointXYZ>(obstaclesCloud, transform2d);
+				}
+			}
+
+			if(groundPub_.getNumSubscribers())
+			{
+				sensor_msgs::PointCloud2 rosCloud;
+				pcl::toROSMsg(*groundCloud, rosCloud);
+				rosCloud.header.stamp = cloudMsg->header.stamp;
+				rosCloud.header.frame_id = frameId_;
+
+				//publish the message
+				groundPub_.publish(rosCloud);
+			}
+
+			if(obstaclesPub_.getNumSubscribers())
+			{
+				sensor_msgs::PointCloud2 rosCloud;
+				pcl::toROSMsg(*obstaclesCloud, rosCloud);
+				rosCloud.header.stamp = cloudMsg->header.stamp;
+				rosCloud.header.frame_id = frameId_;
+
+				//publish the message
+				obstaclesPub_.publish(rosCloud);
+			}
+			if(obstaclesToMap2dPub_.getNumSubscribers())
+			{
+				sensor_msgs::PointCloud2 rosCloud;
+				pcl::toROSMsg(*obstaclesToMap2dCloud, rosCloud);
+				rosCloud.header.stamp = cloudMsg->header.stamp;
+				rosCloud.header.frame_id = mapFrameId_;
+
+				//publish the message
+				obstaclesToMap2dPub_.publish(rosCloud);
+			}
+		}
+	}
+
+private:
+	std::string frameId_;
+	std::string mapFrameId_;
+	double normalEstimationRadius_;
+	double groundNormalAngle_;
+	int minClusterSize_;
+	double maxObstaclesHeight_;
+
+	tf::TransformListener tfListener_;
+
+	ros::Publisher groundPub_;
+	ros::Publisher obstaclesPub_;
+	ros::Publisher obstaclesToMap2dPub_;
+
+	ros::Subscriber cloudSub_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap::ObstaclesDetection, nodelet::Nodelet);
+}
+

src/nodelets/point_cloud_xyz.cpp

@@ -45,6 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <image_geometry/pinhole_camera_model.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>
@@ -59,16 +60,27 @@ class PointCloudXYZ : public nodelet::Nodelet
 {
 public:
 	PointCloudXYZ() :
+		maxDepth_(0.0),
 		voxelSize_(0.0),
 		decimation_(1),
 		noiseFilterRadius_(0.0),
-		noiseFilterMinNeighbors_(5)
+		noiseFilterMinNeighbors_(5),
+		approxSyncDepth_(0),
+		approxSyncDisparity_(0),
+		exactSyncDepth_(0),
+		exactSyncDisparity_(0)
 	{}
 
 	virtual ~PointCloudXYZ()
 	{
-		delete sync_;
-		delete syncDisparity_;
+		if(approxSyncDepth_)
+			delete approxSyncDepth_;
+		if(approxSyncDisparity_)
+			delete approxSyncDisparity_;
+		if(exactSyncDepth_)
+			delete exactSyncDepth_;
+		if(exactSyncDisparity_)
+			delete exactSyncDisparity_;
 	}
 
 private:
@@ -78,21 +90,40 @@ private:
 		ros::NodeHandle & pnh = getPrivateNodeHandle();
 
 		int queueSize = 10;
+		bool approxSync = true;
+		pnh.param("approx_sync", approxSync, approxSync);
 		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("max_depth", maxDepth_, maxDepth_);
 		pnh.param("voxel_size", voxelSize_, voxelSize_);
 		pnh.param("decimation", decimation_, decimation_);
 		pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
 		pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
+		ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
 
-		sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
-		sync_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2));
+		if(approxSync)
+		{
+			approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
+			approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2));
 
-		syncDisparity_ = new message_filters::Synchronizer<MySyncDispPolicy>(MySyncDispPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
-		syncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2));
+			approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityDispPolicy>(MyApproxSyncDisparityDispPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
+			approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2));
+		}
+		else
+		{
+			exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
+			exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2));
 
-		image_transport::ImageTransport it(nh);
-		imageDepthSub_.subscribe(it, "depth/image", 1);
-		cameraInfoSub_.subscribe(nh, "depth/camera_info", 1);
+			exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityDispPolicy>(MyExactSyncDisparityDispPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
+			exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2));
+		}
+
+		ros::NodeHandle depth_nh(nh, "depth");
+		ros::NodeHandle depth_pnh(pnh, "depth");
+		image_transport::ImageTransport depth_it(depth_nh);
+		image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+
+		imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+		cameraInfoSub_.subscribe(depth_nh, "camera_info", 1);
 
 		disparitySub_.subscribe(nh, "disparity/image", 1);
 		disparityCameraInfoSub_.subscribe(nh, "disparity/camera_info", 1);
@@ -100,8 +131,6 @@ private:
 		cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
 	}
 
-
-
 	void callback(
 			  const sensor_msgs::ImageConstPtr& depth,
 			  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
@@ -133,30 +162,7 @@ private:
 					fy,
 					decimation_);
 
-			if(noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
-			{
-				pcl::IndicesPtr indices = util3d::radiusFiltering<pcl::PointXYZ>(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
-				pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
-				pcl::copyPointCloud(*pclCloud, *indices, *tmp);
-				pclCloud = tmp;
-			}
-
-			if(voxelSize_ > 0.0)
-			{
-				pclCloud = util3d::voxelize<pcl::PointXYZ>(pclCloud, voxelSize_);
-			}
-
-			//*********************
-			// Publish Map
-			//*********************
-
-			sensor_msgs::PointCloud2 rosCloud;
-			pcl::toROSMsg(*pclCloud, rosCloud);
-			rosCloud.header.stamp = depth->header.stamp;
-			rosCloud.header.frame_id = depth->header.frame_id;
-
-			//publish the message
-			cloudPub_.publish(rosCloud);
+			processAndPublish(pclCloud, depth->header);
 		}
 	}
 
@@ -197,35 +203,42 @@ private:
 					disparityMsg->T,
 					decimation_);
 
-			if(noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
-			{
-				pcl::IndicesPtr indices = util3d::radiusFiltering<pcl::PointXYZ>(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
-				pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
-				pcl::copyPointCloud(*pclCloud, *indices, *tmp);
-				pclCloud = tmp;
-			}
-
-			if(voxelSize_ > 0.0)
-			{
-				pclCloud = util3d::voxelize<pcl::PointXYZ>(pclCloud, voxelSize_);
-			}
-
-			//*********************
-			// Publish Map
-			//*********************
-
-			sensor_msgs::PointCloud2 rosCloud;
-			pcl::toROSMsg(*pclCloud, rosCloud);
-			rosCloud.header.stamp = disparityMsg->header.stamp;
-			rosCloud.header.frame_id = disparityMsg->header.frame_id;
-
-			//publish the message
-			cloudPub_.publish(rosCloud);
+			processAndPublish(pclCloud, disparityMsg->header);
 		}
 	}
 
+	void processAndPublish(pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, const std_msgs::Header & header)
+	{
+		if(pclCloud->size() && maxDepth_ > 0)
+		{
+			pclCloud = util3d::passThrough<pcl::PointXYZ>(pclCloud, "z", 0, maxDepth_);
+		}
+
+		if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
+		{
+			pcl::IndicesPtr indices = util3d::radiusFiltering<pcl::PointXYZ>(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
+			pcl::copyPointCloud(*pclCloud, *indices, *tmp);
+			pclCloud = tmp;
+		}
+
+		if(pclCloud->size() && voxelSize_ > 0.0)
+		{
+			pclCloud = util3d::voxelize<pcl::PointXYZ>(pclCloud, voxelSize_);
+		}
+
+		sensor_msgs::PointCloud2 rosCloud;
+		pcl::toROSMsg(*pclCloud, rosCloud);
+		rosCloud.header.stamp = header.stamp;
+		rosCloud.header.frame_id = header.frame_id;
+
+		//publish the message
+		cloudPub_.publish(rosCloud);
+	}
+
 private:
 
+	double maxDepth_;
 	double voxelSize_;
 	int decimation_;
 	double noiseFilterRadius_;
@@ -239,11 +252,18 @@ private:
 	message_filters::Subscriber<stereo_msgs::DisparityImage> disparitySub_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> disparityCameraInfoSub_;
 
-	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
-	message_filters::Synchronizer<MySyncPolicy> * sync_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
+	message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
+
+	typedef message_filters::sync_policies::ApproximateTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyApproxSyncDisparityDispPolicy;
+	message_filters::Synchronizer<MyApproxSyncDisparityDispPolicy> * approxSyncDisparity_;
+
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
+	message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
+
+	typedef message_filters::sync_policies::ExactTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyExactSyncDisparityDispPolicy;
+	message_filters::Synchronizer<MyExactSyncDisparityDispPolicy> * exactSyncDisparity_;
 
-	typedef message_filters::sync_policies::ApproximateTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MySyncDispPolicy;
-	message_filters::Synchronizer<MySyncDispPolicy> * syncDisparity_;
 };
 
 PLUGINLIB_EXPORT_CLASS(rtabmap::PointCloudXYZ, nodelet::Nodelet);

src/nodelets/point_cloud_xyzrgb.cpp

@@ -138,10 +138,6 @@ private:
 				pclCloud = rtabmap::util3d::voxelize<pcl::PointXYZRGB>(pclCloud, voxelSize_);
 			}
 
-			//*********************
-			// Publish Map
-			//*********************
-
 			sensor_msgs::PointCloud2 rosCloud;
 			pcl::toROSMsg(*pclCloud, rosCloud);
 			rosCloud.header.stamp = image->header.stamp;