ros-pkg: added disparity to cloud in point_cloud_xyz nodelet

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

launch/azimut3/az3_mapping_robot_stereo_nav.launch

@@ -69,7 +69,7 @@
 	<!-- Run the ROS package stereo_image_proc for image rectification-->
 	<group ns="/stereo_camera" >
 		<!-- HACK: the fps parameter on camera1394stereo_node doesn't work for my camera!?!?
-			 Throttle camera images to 10 Hz () -->
+			 Throttle camera images to 5 Hz (), odometry on azimut cannot run over 5Hz -->
 		<node pkg="nodelet" type="nodelet" name="stereo_throttle" args="standalone rtabmap/stereo_throttle">
 			<remap from="left/image"       to="left/image_raw"/>
 		    <remap from="right/image"      to="right/image_raw"/>
@@ -77,7 +77,7 @@
 		    <remap from="right/camera_info" to="right/camera_info"/>
 		    
 		    <param name="queue_size" type="int" value="10"/>
-		    <param name="rate" type="double" value="10"/>
+		    <param name="rate" type="double" value="5"/>
 		</node>
 	
 		<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
@@ -85,7 +85,14 @@
 			<remap from="left/camera_info"  to="left/camera_info_throttle"/>
 			<remap from="right/image_raw"   to="right/image_raw_throttle"/>
 			<remap from="right/camera_info" to="right/camera_info_throttle"/>
-			<remap from="points2" to="/planner_cloud"/>
+		</node>
+		<node pkg="nodelet" type="nodelet" name="disparity2cloud" args="standalone rtabmap/point_cloud_xyz">
+			<remap from="disparity/image"       to="disparity"/>
+			<remap from="disparity/camera_info" to="right/camera_info_throttle"/>
+			<remap from="cloud"                 to="/planner_cloud"/>
+			
+			<param name="voxel_size" type="double" value="0.05"/>
+			<param name="decimation" type="int" value="4"/>
 		</node>
 	</group>
 	

src/nodelets/point_cloud_xyz.cpp

@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #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>
@@ -62,6 +63,7 @@ public:
 	virtual ~PointCloudXYZ()
 	{
 		delete sync_;
+		delete syncDisparity_;
 	}
 
 private:
@@ -77,12 +79,18 @@ private:
 
 		sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
 		sync_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2));
-		cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
+
+		syncDisparity_ = new message_filters::Synchronizer<MySyncDispPolicy>(MySyncDispPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
+		syncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2));
 
 		image_transport::ImageTransport it(nh);
+		imageDepthSub_.subscribe(it, "depth/image", 1);
+		cameraInfoSub_.subscribe(nh, "depth/camera_info", 1);
 
-		imageDepthSub_.subscribe(it, "depth", 1);
+		disparitySub_.subscribe(nh, "disparity/image", 1);
-		cameraInfoSub_.subscribe(nh, "camera_info", 1);
+		disparityCameraInfoSub_.subscribe(nh, "disparity/camera_info", 1);
+
+		cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
 	}
 
 
@@ -137,6 +145,54 @@ private:
 		}
 	}
 
+	void callbackDisparity(
+			const stereo_msgs::DisparityImageConstPtr& disparityMsg,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo)
+	{
+		if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0)
+		{
+			ROS_ERROR("Input type must be disparity=32FC1");
+			return;
+		}
+
+		// sensor_msgs::image_encodings::TYPE_32FC1
+		cv::Mat disparity(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
+
+		if(cloudPub_.getNumSubscribers())
+		{
+			image_geometry::PinholeCameraModel model;
+			model.fromCameraInfo(*cameraInfo);
+			float cx = model.cx();
+			float cy = model.cy();
+
+			pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
+			pclCloud = rtabmap::util3d::cloudFromDisparity(
+					disparity,
+					cx,
+					cy,
+					disparityMsg->f,
+					disparityMsg->T,
+					decimation_);
+
+			if(voxelSize_ > 0.0)
+			{
+				pclCloud = rtabmap::util3d::voxelize(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);
+		}
+	}
+
 private:
 
 	double voxelSize_;
@@ -146,10 +202,14 @@ private:
 	image_transport::SubscriberFilter imageDepthSub_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
 
+	message_filters::Subscriber<stereo_msgs::DisparityImage> disparitySub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> disparityCameraInfoSub_;
 
-	// without odometry subscription (odometry is computed by this node)
 	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
 	message_filters::Synchronizer<MySyncPolicy> * sync_;
+
+	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

@@ -162,8 +162,6 @@ private:
 	image_transport::SubscriberFilter imageDepthSub_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
 
-
-	// without odometry subscription (odometry is computed by this node)
 	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
 	message_filters::Synchronizer<MySyncPolicy> * sync_;
 };