ros-pkg: added disparity_odom_sync nodelet

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1917 f169173b-cf89-36c8-b27e-44dbe73f0c83
2026-10-04 00:37:46 +08:00 · 2014-10-23 17:02:13 +00:00
parent dcb4f3e928
commit 0c003c6cc2
4 changed files with 133 additions and 6 deletions
@@ -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);
+}