ros-pkg: added disparity_odom_sync nodelet

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

CMakeLists.txt

@@ -121,6 +121,7 @@ add_library(rtabmap_ros
    src/nodelets/data_throttle.cpp
    src/nodelets/stereo_throttle.cpp
    src/nodelets/data_odom_sync.cpp
+   src/nodelets/disparity_odom_sync.cpp
    src/nodelets/point_cloud_xyzrgb.cpp 
    src/nodelets/point_cloud_xyz.cpp
    src/nodelets/disparity_to_depth.cpp 

launch/azimut3/az3_mapping_robot_stereo_nav.launch

@@ -61,16 +61,17 @@
   	<!-- TF transforms for the stereo camera -->
   	<arg name="pi/2" value="1.5707963267948966" />
 	<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
-	<node pkg="tf" type="static_transform_publisher" name="camera_base_link"
+	<node pkg="tf" type="static_transform_publisher" name="stereo_camera_base_link"
 	    args="$(arg optical_rotate) stereo_camera_base stereo_camera 100" />  
-	<node pkg="tf" type="static_transform_publisher" name="base_to_camera_base_link"
+	<node pkg="tf" type="static_transform_publisher" name="base_to_stereo_camera_base_link"
 	    args="0.01 0.06 0.90 0 0.2 0 base_link stereo_camera_base 100" />  
 	    
 	<!-- Run the ROS package stereo_image_proc for image rectification-->
 	<group ns="/stereo_camera" >
+	    <node pkg="nodelet" type="nodelet" name="stereo_nodelet"  args="manager"/>
 		<!-- HACK: the fps parameter on camera1394stereo_node doesn't work for my camera!?!?
 			 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">
+		<node pkg="nodelet" type="nodelet" name="stereo_throttle" args="load rtabmap/stereo_throttle stereo_nodelet">
 			<remap from="left/image"       to="left/image_raw"/>
 		    <remap from="right/image"      to="right/image_raw"/>
 		    <remap from="left/camera_info" to="left/camera_info"/>
@@ -86,9 +87,14 @@
 			<remap from="right/image_raw"   to="right/image_raw_throttle"/>
 			<remap from="right/camera_info" to="right/camera_info_throttle"/>
 		</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"/>
+		<node pkg="nodelet" type="nodelet" name="disparity_odom_sync" args="load rtabmap/disparity_odom_sync stereo_nodelet">
+			<remap from="disparity"       to="disparity"/>
+		    <remap from="camera_info"     to="right/camera_info_throttle"/>
+		    <remap from="odom"            to="/stereo_odometer/odometry"/>
+		</node>
+		<node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap/point_cloud_xyz stereo_nodelet">
+			<remap from="disparity/image"       to="disparity_sync"/>
+			<remap from="disparity/camera_info" to="camera_info_sync"/>
 			<remap from="cloud"                 to="/planner_cloud"/>
 			
 			<param name="voxel_size" type="double" value="0.05"/>
@@ -158,6 +164,7 @@
     </node>
     <node pkg="rtabmap" type="map_assembler" name="map_assembler">
     	  <param name="occupancy_grid" type="bool" value="true"/>
+    	  <remap from="grid_projection_map" to="/map"/>
     </node>
  	
   </group>

nodelet_plugins.xml

@@ -22,6 +22,14 @@
       This is my nodelet.
     </description>
   </class>
+  
+  <class name="rtabmap/disparity_odom_sync" 
+         type="rtabmap::DisparityOdomSyncNodelet" 
+         base_class_type="nodelet::Nodelet">
+    <description>
+      This is my nodelet.
+    </description>
+  </class>
     
   <class name="rtabmap/point_cloud_xyzrgb" 
          type="rtabmap::PointCloudXYZRGB" 

src/nodelets/disparity_odom_sync.cpp

@@ -0,0 +1,111 @@
+/*
+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 <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include <stereo_msgs/DisparityImage.h>
+
+#include <sensor_msgs/CameraInfo.h>
+#include <nav_msgs/Odometry.h>
+
+namespace rtabmap
+{
+
+class DisparityOdomSyncNodelet : public nodelet::Nodelet
+{
+public:
+	//Constructor
+	DisparityOdomSyncNodelet():
+		sync_(0)
+	{
+	}
+
+	virtual ~DisparityOdomSyncNodelet()
+	{
+		delete sync_;
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle& nh = getNodeHandle();
+		ros::NodeHandle& private_nh = getPrivateNodeHandle();
+
+		int queueSize = 10;
+		private_nh.param("queue_size", queueSize, queueSize);
+
+		sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), disparitySub_, infoSub_, odomSub_);
+		sync_->registerCallback(boost::bind(&DisparityOdomSyncNodelet::callback, this, _1, _2, _3));
+
+		disparitySub_.subscribe(nh, "disparity", 1);
+		infoSub_.subscribe(nh, "camera_info", 1);
+		odomSub_.subscribe(nh, "odom", 1);
+
+		disparityPub_ = nh.advertise<stereo_msgs::DisparityImage>(nh.resolveName("disparity")+"_sync", 10);
+		infoPub_ = nh.advertise<sensor_msgs::CameraInfo>(nh.resolveName("camera_info")+"_sync", 10);
+		odomPub_ = nh.advertise<nav_msgs::Odometry>(nh.resolveName("odom")+"_sync", 10);
+	};
+
+	void callback(const stereo_msgs::DisparityImageConstPtr& image,
+			const sensor_msgs::CameraInfoConstPtr& camInfo,
+			const nav_msgs::OdometryConstPtr & odom)
+	{
+		if(disparityPub_.getNumSubscribers())
+		{
+			disparityPub_.publish(image);
+		}
+		if(infoPub_.getNumSubscribers())
+		{
+			infoPub_.publish(camInfo);
+		}
+		if(odomPub_.getNumSubscribers())
+		{
+			odomPub_.publish(odom);
+		}
+	}
+
+	ros::Publisher disparityPub_;
+	ros::Publisher infoPub_;
+	ros::Publisher odomPub_;
+
+	message_filters::Subscriber<stereo_msgs::DisparityImage> disparitySub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> infoSub_;
+	message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
+	typedef message_filters::sync_policies::ApproximateTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo, nav_msgs::Odometry> MySyncPolicy;
+	message_filters::Synchronizer<MySyncPolicy> * sync_;
+
+};
+
+
+PLUGINLIB_EXPORT_CLASS(rtabmap::DisparityOdomSyncNodelet, nodelet::Nodelet);
+}