ros-pkg: added stereo_throttle plugin

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

CMakeLists.txt

@@ -119,6 +119,7 @@ set_property(
 ## Declare a cpp library
 add_library(rtabmap_ros
    src/nodelets/data_throttle.cpp
+   src/nodelets/stereo_throttle.cpp
    src/nodelets/data_odom_sync.cpp
    src/nodelets/point_cloud_xyzrgb.cpp 
    src/nodelets/disparity_to_depth.cpp 

launch/tests/test_stereo_mapping.launch

@@ -9,7 +9,7 @@
 	    <param name="camera_info_url_left" value="" />
 	    <param name="camera_info_url_right" value="" />
   	</node>
-  	
+    	
   	<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"
@@ -17,7 +17,22 @@
   	
   	<!-- Run the ROS package stereo_image_proc -->
 	<group ns="/stereo_camera" >
+		<!-- Throttle camera images to 10 Hz -->
+		<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"/>
+		    <remap from="left/camera_info" to="left/camera_info"/>
+		    <remap from="right/camera_info" to="right/camera_info"/>
+		    
+		    <param name="queue_size" type="int" value="10"/>
+		    <param name="rate" type="double" value="10"/>
+		</node>
+	
 		<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
+		<remap from="left/image_raw" to="left/image_raw_throttle"/>
+		<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"/>
 		</node>
 	</group>
 
@@ -28,14 +43,16 @@
 		<param name="base_link_frame_id" value="base_link"/>
 		<param name="odom_frame_id" value="/odom"/>
 		<param name="ref_frame_change_method" value="1"/>
+		<remap from="left/camera_info" to="left/camera_info_throttle" />
+	    <remap from="right/camera_info" to="right/camera_info_throttle" />
 	</node>
 	
 	<!--
 	<node pkg="fovis_ros" type="fovis_stereo_odometer" name="stereo_odometer" >
 	    <remap from="/stereo/left/image" to="/stereo_camera/left/image_rect" />
 	    <remap from="/stereo/right/image" to="/stereo_camera/right/image_rect" />
-	    <remap from="/stereo/left/camera_info" to="/stereo_camera/left/camera_info" />
+	    <remap from="/stereo/left/camera_info" to="/stereo_camera/left/camera_info_throttle" />
-	    <remap from="/stereo/right/camera_info" to="/stereo_camera/right/camera_info" />
+	    <remap from="/stereo/right/camera_info" to="/stereo_camera/right/camera_info_throttle" />
 	    <remap from="odometry" to="stereo_odometer/odometry" />
     </node>
     -->	
@@ -43,8 +60,8 @@
     <node pkg="rtabmap" type="stereo_odometry" name="stereo_odometer" output="screen">
   	  	<remap from="left/image_rect"       to="/stereo_camera/left/image_rect"/>
   	  	<remap from="right/image_rect"      to="/stereo_camera/right/image_rect"/>
-  	  	<remap from="left/camera_info"      to="/stereo_camera/left/camera_info"/>
+  	  	<remap from="left/camera_info"      to="/stereo_camera/left/camera_info_throttle"/>
-  	  	<remap from="right/camera_info"     to="/stereo_camera/right/camera_info"/>
+  	  	<remap from="right/camera_info"     to="/stereo_camera/right/camera_info_throttle"/>
 	  	<remap from="odom"                  to="/stereo_odometer/odometry"/>
     
   	  	<param name="frame_id" type="string" value="base_link"/>
@@ -66,8 +83,8 @@
 	
       <remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
       <remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
-      <remap from="left/camera_info" to="/stereo_camera/left/camera_info"/>
+      <remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
-      <remap from="right/camera_info" to="/stereo_camera/right/camera_info"/>
+      <remap from="right/camera_info" to="/stereo_camera/right/camera_info_throttle"/>
       
       <remap from="odom" to="/stereo_odometer/odometry"/>
       
@@ -100,7 +117,7 @@
       <param name="queue_size" type="int" value="30"/>
     
       <remap from="rgb/image" to="/stereo_camera/left/image_rect_color"/>
-      <remap from="rgb/camera_info" to="/stereo_camera/left/camera_info"/>
+      <remap from="rgb/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
       <remap from="depth/image" to="/odom_depth"/>
       
       <remap from="odom" to="/stereo_odometer/odometry"/>
@@ -111,27 +128,5 @@
   <!-- RVIZ -->
   
   <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/camera_info_in" to="/stereo_camera/left/camera_info"/>
-    <remap from="odom_in"             to="/stereo_odometer/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"/>
-    
-    <param name="queue_size" type="int" value="30"/>
-  </node>
-  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap/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="queue_size" type="int" value="10"/>
-  </node>
   
 </launch>

nodelet_plugins.xml

@@ -7,6 +7,14 @@
     </description>
   </class>
   
+  <class name="rtabmap/stereo_throttle" 
+         type="rtabmap::StereoThrottleNodelet" 
+         base_class_type="nodelet::Nodelet">
+    <description>
+      This is my nodelet.
+    </description>
+  </class>
+  
   <class name="rtabmap/data_odom_sync" 
          type="rtabmap::DataOdomSyncNodelet" 
          base_class_type="nodelet::Nodelet">

src/CoreWrapper.h

@@ -45,7 +45,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <sensor_msgs/CameraInfo.h>
 #include <sensor_msgs/LaserScan.h>
 #include <nav_msgs/Odometry.h>
-#include <stereo_msgs/DisparityImage.h>
 
 #include <rtabmap/core/Statistics.h>
 #include <rtabmap/core/Parameters.h>

src/nodelets/stereo_throttle.cpp

@@ -0,0 +1,174 @@
+/*
+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 <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <sensor_msgs/CameraInfo.h>
+
+namespace rtabmap
+{
+
+class StereoThrottleNodelet : public nodelet::Nodelet
+{
+public:
+	//Constructor
+	StereoThrottleNodelet():
+		max_update_rate_(0),
+		approxSync_(0),
+		exactSync_(0)
+	{
+	}
+
+	virtual ~StereoThrottleNodelet()
+	{
+		if(approxSync_)
+		{
+			delete approxSync_;
+		}
+		if(exactSync_)
+		{
+			delete exactSync_;
+		}
+	}
+
+private:
+	ros::Time last_update_;
+	double max_update_rate_;
+	virtual void onInit()
+	{
+		ros::NodeHandle& nh = getNodeHandle();
+		ros::NodeHandle& pnh = getPrivateNodeHandle();
+
+		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);
+
+		int queueSize = 5;
+		bool approxSync = false;
+		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("rate", max_update_rate_, max_update_rate_);
+		pnh.param("queue_size", queueSize, queueSize);
+		ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
+
+		if(max_update_rate_ == 0.0)
+		{
+			ROS_WARN("Parameter \"rate\" (Hz) is not set!");
+		}
+
+		if(approxSync)
+		{
+			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
+			approxSync_->registerCallback(boost::bind(&StereoThrottleNodelet::callback, this, _1, _2, _3, _4));
+		}
+		else
+		{
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
+			exactSync_->registerCallback(boost::bind(&StereoThrottleNodelet::callback, this, _1, _2, _3, _4));
+		}
+
+		imageLeft_.subscribe(left_it, left_nh.resolveName("image"), 1, hintsLeft);
+		imageRight_.subscribe(right_it, right_nh.resolveName("image"), 1, hintsRight);
+		cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
+		cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
+
+		imageLeftPub_ = left_it.advertise(left_nh.resolveName("image")+"_throttle", 1);
+		imageRightPub_ = right_it.advertise(right_nh.resolveName("image")+"_throttle", 1);
+		infoLeftPub_ = left_nh.advertise<sensor_msgs::CameraInfo>(left_nh.resolveName("camera_info")+"_throttle", 1);
+		infoRightPub_ = right_nh.advertise<sensor_msgs::CameraInfo>(right_nh.resolveName("camera_info")+"_throttle", 1);
+	};
+
+	void callback(const sensor_msgs::ImageConstPtr& imageLeft,
+			const sensor_msgs::ImageConstPtr& imageRight,
+			const sensor_msgs::CameraInfoConstPtr& camInfoLeft,
+			const sensor_msgs::CameraInfoConstPtr& camInfoRight)
+	{
+		if (max_update_rate_ > 0.0)
+		{
+			NODELET_DEBUG("update set to %f", max_update_rate_);
+			if ( last_update_ + ros::Duration(1.0/max_update_rate_) > ros::Time::now())
+			{
+				NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
+				return;
+			}
+		}
+		else
+			NODELET_DEBUG("update_rate unset continuing");
+
+		last_update_ = ros::Time::now();
+
+		if(imageLeftPub_.getNumSubscribers())
+		{
+			imageLeftPub_.publish(imageLeft);
+		}
+		if(imageRightPub_.getNumSubscribers())
+		{
+			imageRightPub_.publish(imageRight);
+		}
+		if(infoLeftPub_.getNumSubscribers())
+		{
+			infoLeftPub_.publish(camInfoLeft);
+		}
+		if(infoRightPub_.getNumSubscribers())
+		{
+			infoRightPub_.publish(camInfoRight);
+		}
+	}
+
+	image_transport::Publisher imageLeftPub_;
+	image_transport::Publisher imageRightPub_;
+	ros::Publisher infoLeftPub_;
+	ros::Publisher infoRightPub_;
+
+	image_transport::SubscriberFilter imageLeft_;
+	image_transport::SubscriberFilter imageRight_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
+	message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
+	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
+
+};
+
+
+PLUGINLIB_EXPORT_CLASS(rtabmap::StereoThrottleNodelet, nodelet::Nodelet);
+}