First version rtabmap_launch working

rtabmap_legacy/src/CameraNode.cpp

@@ -0,0 +1,293 @@
+/*
+Copyright (c) 2010-2016, 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 <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <cv_bridge/cv_bridge.h>
+#include <std_msgs/Empty.h>
+#include <image_transport/image_transport.h>
+#include <std_srvs/Empty.h>
+
+#include <rtabmap/core/CameraRGB.h>
+#include <rtabmap/core/CameraThread.h>
+#include <rtabmap/core/CameraEvent.h>
+
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UEventsHandler.h>
+#include <rtabmap/utilite/UEventsManager.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UFile.h>
+
+#include <dynamic_reconfigure/server.h>
+#include <rtabmap_legacy/CameraConfig.h>
+
+class CameraWrapper : public UEventsHandler
+{
+public:
+	// Usb device like a Webcam
+	CameraWrapper(int usbDevice = 0,
+			      float imageRate = 0,
+				  unsigned int imageWidth = 0,
+				  unsigned int imageHeight = 0) :
+		cameraThread_(0),
+		camera_(0),
+		frameId_("camera")
+	{
+		ros::NodeHandle nh;
+		ros::NodeHandle pnh("~");
+		pnh.param("frame_id", frameId_, frameId_);
+		image_transport::ImageTransport it(nh);
+		rosPublisher_ = it.advertise("image", 1);
+		startSrv_ = nh.advertiseService("start_camera", &CameraWrapper::startSrv, this);
+		stopSrv_ = nh.advertiseService("stop_camera", &CameraWrapper::stopSrv, this);
+		UEventsManager::addHandler(this);
+	}
+
+	virtual ~CameraWrapper()
+	{
+		if(cameraThread_)
+		{
+			cameraThread_->join(true);
+			delete cameraThread_;
+		}
+	}
+
+	bool init()
+	{
+		if(cameraThread_)
+		{
+			return cameraThread_->camera()->init();
+		}
+		return false;
+	}
+
+	void start()
+	{
+		if(cameraThread_)
+		{
+			cameraThread_->start();
+		}
+	}
+
+	bool startSrv(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+	{
+		ROS_INFO("Camera started...");
+		if(cameraThread_)
+		{
+			cameraThread_->start();
+		}
+		return true;
+	}
+
+	bool stopSrv(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+	{
+		ROS_INFO("Camera stopped...");
+		if(cameraThread_)
+		{
+			cameraThread_->kill();
+		}
+		return true;
+	}
+
+	void setParameters(int deviceId, double frameRate, const std::string & path, bool pause)
+	{
+		ROS_INFO("Parameters changed: deviceId=%d, path=%s frameRate=%f pause=%s",
+				deviceId, path.c_str(), frameRate, pause?"true":"false");
+		if(cameraThread_)
+		{
+			rtabmap::CameraVideo * videoCam = dynamic_cast<rtabmap::CameraVideo *>(camera_);
+			rtabmap::CameraImages * imagesCam = dynamic_cast<rtabmap::CameraImages *>(camera_);
+
+			if(imagesCam)
+			{
+				// images
+				if(!path.empty() && UDirectory::getDir(path+"/").compare(UDirectory::getDir(imagesCam->getPath())) == 0)
+				{
+					imagesCam->setImageRate(frameRate);
+					if(pause && !cameraThread_->isPaused())
+					{
+						cameraThread_->join(true);
+					}
+					else if(!pause && cameraThread_->isPaused())
+					{
+						cameraThread_->start();
+					}
+				}
+				else
+				{
+					delete cameraThread_;
+					cameraThread_ = 0;
+				}
+			}
+			else if(videoCam)
+			{
+				if(!path.empty() && path.compare(videoCam->getFilePath()) == 0)
+				{
+					// video
+					videoCam->setImageRate(frameRate);
+					if(pause && !cameraThread_->isPaused())
+					{
+						cameraThread_->join(true);
+					}
+					else if(!pause && cameraThread_->isPaused())
+					{
+						cameraThread_->start();
+					}
+				}
+				else if(path.empty() &&
+						videoCam->getFilePath().empty() &&
+						videoCam->getUsbDevice() == deviceId)
+				{
+					// usb device
+					videoCam->setImageRate(frameRate);
+					if(pause && !cameraThread_->isPaused())
+					{
+						cameraThread_->join(true);
+					}
+					else if(!pause && cameraThread_->isPaused())
+					{
+						cameraThread_->start();
+					}
+				}
+				else
+				{
+					delete cameraThread_;
+					cameraThread_ = 0;
+				}
+			}
+			else
+			{
+				ROS_ERROR("Wrong camera type ?!?");
+				delete cameraThread_;
+				cameraThread_ = 0;
+			}
+		}
+
+		if(!cameraThread_)
+		{
+			if(!path.empty() && UDirectory::exists(path))
+			{
+				//images
+				camera_ = new rtabmap::CameraImages(path, frameRate);
+			}
+			else if(!path.empty() && UFile::exists(path))
+			{
+				//video
+				camera_ = new rtabmap::CameraVideo(path, false, frameRate);
+			}
+			else
+			{
+				if(!path.empty() && !UDirectory::exists(path) && !UFile::exists(path))
+				{
+					ROS_ERROR("Path \"%s\" does not exist (or you don't have the permissions to read)... falling back to usb device...", path.c_str());
+				}
+				//usb device
+				camera_ = new rtabmap::CameraVideo(deviceId, false, frameRate);
+			}
+			cameraThread_ = new rtabmap::CameraThread(camera_);
+			init();
+			if(!pause)
+			{
+				start();
+			}
+		}
+	}
+
+protected:
+	virtual bool handleEvent(UEvent * event)
+	{
+		if(event->getClassName().compare("CameraEvent") == 0)
+		{
+			rtabmap::CameraEvent * e = (rtabmap::CameraEvent*)event;
+			const cv::Mat & image = e->data().imageRaw();
+			if(!image.empty() && image.depth() == CV_8U)
+			{
+				cv_bridge::CvImage img;
+				if(image.channels() == 1)
+				{
+					img.encoding = sensor_msgs::image_encodings::MONO8;
+				}
+				else
+				{
+					img.encoding = sensor_msgs::image_encodings::BGR8;
+				}
+				img.image = image;
+				sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
+				rosMsg->header.frame_id = frameId_;
+				rosMsg->header.stamp = ros::Time::now();
+				rosPublisher_.publish(rosMsg);
+			}
+		}
+		return false;
+	}
+
+private:
+	image_transport::Publisher rosPublisher_;
+	rtabmap::CameraThread * cameraThread_;
+	rtabmap::Camera * camera_;
+	ros::ServiceServer startSrv_;
+	ros::ServiceServer stopSrv_;
+	std::string frameId_;
+};
+
+CameraWrapper * camera = 0;
+void callback(rtabmap_legacy::CameraConfig &config, uint32_t level)
+{
+	if(camera)
+	{
+		camera->setParameters(config.device_id, config.frame_rate, config.video_or_images_path, config.pause);
+	}
+}
+
+int main(int argc, char** argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	//ULogger::setLevel(ULogger::kDebug);
+	ULogger::setEventLevel(ULogger::kWarning);
+
+	ros::init(argc, argv, "camera");
+
+	ros::NodeHandle nh("~");
+
+	camera = new CameraWrapper(); // webcam device 0
+
+	dynamic_reconfigure::Server<rtabmap_legacy::CameraConfig> server;
+	dynamic_reconfigure::Server<rtabmap_legacy::CameraConfig>::CallbackType f;
+	f = boost::bind(&callback, boost::placeholders::_1, boost::placeholders::_2);
+	server.setCallback(f);
+
+	ros::spin();
+
+	//cleanup
+	if(camera)
+	{
+		delete camera;
+	}
+
+	return 0;
+}

rtabmap_legacy/src/StereoCameraNode.cpp

@@ -0,0 +1,113 @@
+/*
+Copyright (c) 2010-2016, 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 <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/core/CameraStereo.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap_conversions/MsgConversion.h>
+#include <image_transport/image_transport.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <cv_bridge/cv_bridge.h>
+
+int main(int argc, char** argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kInfo);
+
+	ros::init(argc, argv, "uvc_stereo_camera");
+
+	ros::NodeHandle pnh("~");
+	double rate = 0.0;
+	std::string id = "camera";
+	std::string frameId = "camera_link";
+	double scale = 1.0;
+	pnh.param("rate", rate, rate);
+	pnh.param("camera_id", id, id);
+	pnh.param("frame_id", frameId, frameId);
+	pnh.param("scale", scale, scale);
+
+	rtabmap::CameraStereoVideo camera(0, false, rate);
+
+	if(camera.init(UDirectory::homeDir() + "/.ros/camera_info", id))
+	{
+		ros::NodeHandle nh;
+		ros::NodeHandle left_nh(nh, "left");
+		ros::NodeHandle right_nh(nh, "right");
+		image_transport::ImageTransport left_it(left_nh);
+		image_transport::ImageTransport right_it(right_nh);
+
+		image_transport::Publisher imageLeftPub = left_it.advertise(left_nh.resolveName("image_raw"), 1);
+		image_transport::Publisher imageRightPub = right_it.advertise(right_nh.resolveName("image_raw"), 1);
+		ros::Publisher infoLeftPub = left_nh.advertise<sensor_msgs::CameraInfo>(left_nh.resolveName("camera_info"), 1);
+		ros::Publisher infoRightPub = right_nh.advertise<sensor_msgs::CameraInfo>(right_nh.resolveName("camera_info"), 1);
+
+		while(ros::ok())
+		{
+			rtabmap::SensorData data = camera.takeImage();
+			
+			ros::Time currentTime = ros::Time::now();
+
+			cv_bridge::CvImage imageLeft;
+			imageLeft.header.frame_id = frameId;
+			imageLeft.header.stamp = currentTime;
+			imageLeft.encoding = "bgr8";
+			cv::resize(data.imageRaw(), imageLeft.image, cv::Size(0,0), scale, scale, CV_INTER_AREA);
+			imageLeftPub.publish(imageLeft.toImageMsg());
+
+			cv_bridge::CvImage imageRight;
+			imageRight.header = imageLeft.header;
+			imageRight.encoding = "mono8";
+			cv::resize(data.rightRaw(), imageRight.image, cv::Size(0,0), scale, scale, CV_INTER_AREA);
+			imageRightPub.publish(imageRight.toImageMsg());
+
+			if(data.stereoCameraModels().size())
+			{
+				sensor_msgs::CameraInfo infoLeft, infoRight;
+				rtabmap_conversions::cameraModelToROS(data.stereoCameraModels()[0].left().scaled(scale), infoLeft);
+				rtabmap_conversions::cameraModelToROS(data.stereoCameraModels()[0].right().scaled(scale), infoRight);
+				infoLeft.header = imageLeft.header;
+				infoRight.header = imageLeft.header;
+				infoLeftPub.publish(infoLeft);
+				infoRightPub.publish(infoRight);
+			}
+			else
+			{
+				ROS_ERROR("No calibration loaded!");
+			}
+
+			ros::spinOnce();
+		}
+	}
+	else
+	{
+		ROS_ERROR("Could not initialize the camera!");
+	}
+
+	return 0;
+}

rtabmap_legacy/src/nodelets/data_odom_sync.cpp

@@ -0,0 +1,130 @@
+/*
+Copyright (c) 2010-2016, 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.hpp"
+#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>
+#include <nav_msgs/Odometry.h>
+
+namespace rtabmap_legacy
+{
+
+class DataOdomSyncNodelet : public nodelet::Nodelet
+{
+public:
+	//Constructor
+	DataOdomSyncNodelet():
+		sync_(0)
+	{
+	}
+
+	virtual ~DataOdomSyncNodelet()
+	{
+		delete sync_;
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle& nh = getNodeHandle();
+		ros::NodeHandle& private_nh = getPrivateNodeHandle();
+
+		ros::NodeHandle rgb_nh(nh, "rgb");
+		ros::NodeHandle depth_nh(nh, "depth");
+		ros::NodeHandle rgb_pnh(private_nh, "rgb");
+		ros::NodeHandle depth_pnh(private_nh, "depth");
+		image_transport::ImageTransport rgb_it(rgb_nh);
+		image_transport::ImageTransport depth_it(depth_nh);
+		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+		image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+
+		int queueSize = 10;
+		private_nh.param("queue_size", queueSize, queueSize);
+
+		sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_, odom_sub_);
+		sync_->registerCallback(boost::bind(&DataOdomSyncNodelet::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+
+		image_sub_.subscribe(rgb_it, rgb_nh.resolveName("image_in"), 1, hintsRgb);
+		image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image_in"), 1, hintsDepth);
+		info_sub_.subscribe(rgb_nh, "camera_info_in", 1);
+		odom_sub_.subscribe(nh, "odom_in", 1);
+
+		imagePub_ = rgb_it.advertise("image_out", 1);
+		imageDepthPub_ = depth_it.advertise("image_out", 1);
+		infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 1);
+		odomPub_ = nh.advertise<nav_msgs::Odometry>("odom_out", 1);
+	};
+
+	void callback(const sensor_msgs::ImageConstPtr& image,
+			const sensor_msgs::ImageConstPtr& imageDepth,
+			const sensor_msgs::CameraInfoConstPtr& camInfo,
+			const nav_msgs::OdometryConstPtr & odom)
+	{
+		if(imagePub_.getNumSubscribers())
+		{
+			imagePub_.publish(image);
+		}
+		if(imageDepthPub_.getNumSubscribers())
+		{
+			imageDepthPub_.publish(imageDepth);
+		}
+		if(infoPub_.getNumSubscribers())
+		{
+			infoPub_.publish(camInfo);
+		}
+		if(odomPub_.getNumSubscribers())
+		{
+			odomPub_.publish(odom);
+		}
+	}
+
+	image_transport::Publisher imagePub_;
+	image_transport::Publisher imageDepthPub_;
+	ros::Publisher infoPub_;
+	ros::Publisher odomPub_;
+
+	image_transport::SubscriberFilter image_sub_;
+	image_transport::SubscriberFilter image_depth_sub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
+	message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, nav_msgs::Odometry> MySyncPolicy;
+	message_filters::Synchronizer<MySyncPolicy> * sync_;
+
+};
+
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_legacy::DataOdomSyncNodelet, nodelet::Nodelet);
+}

rtabmap_legacy/src/nodelets/data_throttle.cpp

@@ -0,0 +1,242 @@
+/*
+Copyright (c) 2010-2016, 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.hpp"
+#include "nodelet/nodelet.h"
+
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <sensor_msgs/CameraInfo.h>
+
+#include <cv_bridge/cv_bridge.h>
+
+#include <rtabmap/core/util2d.h>
+
+namespace rtabmap_legacy
+{
+
+class DataThrottleNodelet : public nodelet::Nodelet
+{
+public:
+	//Constructor
+	DataThrottleNodelet():
+		rate_(0),
+		approxSync_(0),
+		exactSync_(0),
+		decimation_(1)
+	{
+	}
+
+	virtual ~DataThrottleNodelet()
+	{
+		if(approxSync_)
+		{
+			delete approxSync_;
+		}
+		if(exactSync_)
+		{
+			delete exactSync_;
+		}
+	}
+
+private:
+	ros::Time last_update_;
+	double rate_;
+	virtual void onInit()
+	{
+		ros::NodeHandle& nh = getNodeHandle();
+		ros::NodeHandle& private_nh = getPrivateNodeHandle();
+
+		ros::NodeHandle rgb_nh(nh, "rgb");
+		ros::NodeHandle depth_nh(nh, "depth");
+		ros::NodeHandle rgb_pnh(private_nh, "rgb");
+		ros::NodeHandle depth_pnh(private_nh, "depth");
+		image_transport::ImageTransport rgb_it(rgb_nh);
+		image_transport::ImageTransport depth_it(depth_nh);
+		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+		image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+
+		int queueSize = 10;
+		bool approxSync = true;
+		double approxSyncMaxInterval = 0.0;
+		if(private_nh.getParam("max_rate", rate_))
+		{
+			NODELET_WARN("\"max_rate\" is now known as \"rate\".");
+		}
+		private_nh.param("rate", rate_, rate_);
+		private_nh.param("queue_size", queueSize, queueSize);
+		private_nh.param("approx_sync", approxSync, approxSync);
+		private_nh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
+		private_nh.param("decimation", decimation_, decimation_);
+		ROS_ASSERT(decimation_ >= 1);
+		NODELET_INFO("rate=%f Hz", rate_);
+		NODELET_INFO("decimation=%d", decimation_);
+		NODELET_INFO("approx_sync = %s", approxSync?"true":"false");
+		if(approxSync)
+			NODELET_INFO("approx_sync_max_interval = %f", approxSyncMaxInterval);
+
+		if(approxSync)
+		{
+			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_);
+			if(approxSyncMaxInterval > 0.0)
+				approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			approxSync_->registerCallback(boost::bind(&DataThrottleNodelet::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+		}
+		else
+		{
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_);
+			exactSync_->registerCallback(boost::bind(&DataThrottleNodelet::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
+		}
+
+		image_sub_.subscribe(rgb_it, rgb_nh.resolveName("image_in"), 1, hintsRgb);
+		image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image_in"), 1, hintsDepth);
+		info_sub_.subscribe(rgb_nh, "camera_info_in", 1);
+
+		imagePub_ = rgb_it.advertise("image_out", 1);
+		imageDepthPub_ = depth_it.advertise("image_out", 1);
+		infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 1);
+	};
+
+	void callback(const sensor_msgs::ImageConstPtr& image,
+			const sensor_msgs::ImageConstPtr& imageDepth,
+			const sensor_msgs::CameraInfoConstPtr& camInfo)
+	{
+		if (rate_ > 0.0)
+		{
+			NODELET_DEBUG("update set to %f", rate_);
+			if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now())
+			{
+				NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
+				return;
+			}
+		}
+		else
+			NODELET_DEBUG("rate unset continuing");
+
+		last_update_ = ros::Time::now();
+
+		double rgbStamp = image->header.stamp.toSec();
+		double depthStamp = imageDepth->header.stamp.toSec();
+		double infoStamp = camInfo->header.stamp.toSec();
+
+		if(infoPub_.getNumSubscribers())
+		{
+			if(decimation_ > 1)
+			{
+				sensor_msgs::CameraInfo info = *camInfo;
+				info.height /= decimation_;
+				info.width /= decimation_;
+				info.roi.height /= decimation_;
+				info.roi.width /= decimation_;
+				info.K[2]/=float(decimation_); // cx
+				info.K[5]/=float(decimation_); // cy
+				info.K[0]/=float(decimation_); // fx
+				info.K[4]/=float(decimation_); // fy
+				info.P[2]/=float(decimation_); // cx
+				info.P[6]/=float(decimation_); // cy
+				info.P[0]/=float(decimation_); // fx
+				info.P[5]/=float(decimation_); // fy
+				infoPub_.publish(info);
+			}
+			else
+			{
+				infoPub_.publish(camInfo);
+			}
+		}
+		if(imagePub_.getNumSubscribers())
+		{
+			if(decimation_ > 1)
+			{
+				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
+				cv_bridge::CvImage out;
+				out.header = imagePtr->header;
+				out.encoding = imagePtr->encoding;
+				out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
+				imagePub_.publish(out.toImageMsg());
+			}
+			else
+			{
+				imagePub_.publish(image);
+			}
+		}
+
+		if(imageDepthPub_.getNumSubscribers())
+		{
+			if(decimation_ > 1)
+			{
+				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageDepth);
+				cv_bridge::CvImage out;
+				out.header = imagePtr->header;
+				out.encoding = imagePtr->encoding;
+				out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
+				imageDepthPub_.publish(out.toImageMsg());
+			}
+			else
+			{
+				imageDepthPub_.publish(imageDepth);
+			}
+		}
+
+		if( rgbStamp != image->header.stamp.toSec() ||
+			depthStamp != imageDepth->header.stamp.toSec())
+		{
+			NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
+					"sure the node publishing the topics doesn't override the same data after publishing them. A "
+					"solution is to use this node within another nodelet manager. Stamps: "
+					"rgb=%f->%f depth=%f->%f",
+					rgbStamp, image->header.stamp.toSec(),
+					depthStamp, imageDepth->header.stamp.toSec());
+		}
+	}
+
+	image_transport::Publisher imagePub_;
+	image_transport::Publisher imageDepthPub_;
+	ros::Publisher infoPub_;
+
+	image_transport::SubscriberFilter image_sub_;
+	image_transport::SubscriberFilter image_depth_sub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
+
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
+	message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
+	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
+
+	int decimation_;
+
+};
+
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_legacy::DataThrottleNodelet, nodelet::Nodelet);
+}

rtabmap_legacy/src/nodelets/obstacles_detection_old.cpp

@@ -0,0 +1,357 @@
+/*
+Copyright (c) 2010-2016, 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.hpp>
+#include <nodelet/nodelet.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <tf/transform_listener.h>
+
+#include <sensor_msgs/PointCloud2.h>
+
+#include <rtabmap_conversions/MsgConversion.h>
+
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/core/util3d_filtering.h"
+#include "rtabmap/core/util3d_mapping.h"
+#include "rtabmap/core/util3d_transforms.h"
+
+namespace rtabmap_legacy
+{
+
+class ObstaclesDetectionOld : public nodelet::Nodelet
+{
+public:
+	ObstaclesDetectionOld() :
+		frameId_("base_link"),
+		normalKSearch_(20),
+		groundNormalAngle_(M_PI_4),
+		clusterRadius_(0.05),
+		minClusterSize_(20),
+		maxObstaclesHeight_(0.0), // if<=0.0 -> disabled
+		maxGroundHeight_(0.0),    // if<=0.0 -> disabled, used only if detect_flat_obstacles is true
+		segmentFlatObstacles_(false),
+		waitForTransform_(false),
+		optimizeForCloseObjects_(false),
+		projVoxelSize_(0.01)
+	{}
+
+	virtual ~ObstaclesDetectionOld()
+	{}
+
+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("normal_k", normalKSearch_, normalKSearch_);
+		pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_);
+		if(pnh.hasParam("normal_estimation_radius") && !pnh.hasParam("cluster_radius"))
+		{
+			NODELET_WARN("Parameter \"normal_estimation_radius\" has been renamed "
+					 "to \"cluster_radius\"! Your value is still copied to "
+					 "corresponding parameter. Instead of normal radius, nearest neighbors count "
+					 "\"normal_k\" is used instead (default 20).");
+			pnh.param("normal_estimation_radius", clusterRadius_, clusterRadius_);
+		}
+		else
+		{
+			pnh.param("cluster_radius", clusterRadius_, clusterRadius_);
+		}
+		pnh.param("min_cluster_size", minClusterSize_, minClusterSize_);
+		pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_);
+		pnh.param("max_ground_height", maxGroundHeight_, maxGroundHeight_);
+		pnh.param("detect_flat_obstacles", segmentFlatObstacles_, segmentFlatObstacles_);
+		pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
+		pnh.param("optimize_for_close_objects", optimizeForCloseObjects_, optimizeForCloseObjects_);
+		pnh.param("proj_voxel_size", projVoxelSize_, projVoxelSize_);
+
+		cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetectionOld::callback, this);
+
+		groundPub_ = nh.advertise<sensor_msgs::PointCloud2>("ground", 1);
+		obstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("obstacles", 1);
+		projObstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("proj_obstacles", 1);
+	}
+
+
+
+	void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
+	{
+		ros::WallTime time = ros::WallTime::now();
+
+		if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0 && projObstaclesPub_.getNumSubscribers() == 0)
+		{
+			// no one wants the results
+			return;
+		}
+
+		rtabmap::Transform localTransform;
+		try
+		{
+			if(waitForTransform_)
+			{
+				if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
+				{
+					NODELET_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
+					return;
+				}
+			}
+			tf::StampedTransform tmp;
+			tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp);
+			localTransform = rtabmap_conversions::transformFromTF(tmp);
+		}
+		catch(tf::TransformException & ex)
+		{
+			NODELET_ERROR("%s",ex.what());
+			return;
+		}
+
+		pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::fromROSMsg(*cloudMsg, *originalCloud);
+
+		//Common variables for all strategies
+		pcl::IndicesPtr ground, obstacles;
+		pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloudWithoutFlatSurfaces(new pcl::PointCloud<pcl::PointXYZ>);
+
+		if(originalCloud->size())
+		{
+			originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform);
+			if(maxObstaclesHeight_ > 0)
+			{
+				// std::numeric_limits<float>::lowest() exists only for c++11
+				originalCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::min(), maxObstaclesHeight_);
+			}
+
+			if(originalCloud->size())
+			{
+				if(!optimizeForCloseObjects_)
+				{
+					// This is the default strategy
+					pcl::IndicesPtr flatObstacles(new std::vector<int>);
+					rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
+							originalCloud,
+							ground,
+							obstacles,
+							normalKSearch_,
+							groundNormalAngle_,
+							clusterRadius_,
+							minClusterSize_,
+							segmentFlatObstacles_,
+							maxGroundHeight_,
+							&flatObstacles);
+
+					if(groundPub_.getNumSubscribers() &&
+							ground.get() && ground->size())
+					{
+						pcl::copyPointCloud(*originalCloud, *ground, *groundCloud);
+					}
+
+					if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) &&
+							obstacles.get() && obstacles->size())
+					{
+						// remove flat obstacles from obstacles
+						std::set<int> flatObstaclesSet;
+						if(projObstaclesPub_.getNumSubscribers())
+						{
+							flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end());
+						}
+
+						obstaclesCloud->resize(obstacles->size());
+						obstaclesCloudWithoutFlatSurfaces->resize(obstacles->size());
+
+						int oi=0;
+						for(unsigned int i=0; i<obstacles->size(); ++i)
+						{
+							obstaclesCloud->points[i] = originalCloud->at(obstacles->at(i));
+							if(flatObstaclesSet.size() == 0 ||
+							   flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end())
+							{
+								obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i];
+								obstaclesCloudWithoutFlatSurfaces->points[oi].z = 0;
+								++oi;
+							}
+
+						}
+
+						obstaclesCloudWithoutFlatSurfaces->resize(oi);
+						if(obstaclesCloudWithoutFlatSurfaces->size() && projVoxelSize_ > 0.0)
+						{
+							obstaclesCloudWithoutFlatSurfaces = rtabmap::util3d::voxelize(obstaclesCloudWithoutFlatSurfaces, projVoxelSize_);
+						}
+					}
+				}
+				else
+				{
+					// in this case optimizeForCloseObject_ is true:
+					// we divide the floor point cloud into two subsections, one for all potential floor points up to 1m
+					// one for potential floor points further away than 1m.
+					// For the points at closer range, we use a smaller normal estimation radius and ground normal angle,
+					// which allows to detect smaller objects, without increasing the number of false positive.
+					// For all other points, we use a bigger normal estimation radius (* 3.) and tolerance for the
+					// grond normal angle (* 2.).
+
+					pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud_near = rtabmap::util3d::passThrough(originalCloud, "x", std::numeric_limits<int>::min(), 1.);
+					pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud_far = rtabmap::util3d::passThrough(originalCloud, "x", 1., std::numeric_limits<int>::max());
+
+					// Part 1: segment floor and obstacles near the robot
+					rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
+							originalCloud_near,
+							ground,
+							obstacles,
+							normalKSearch_,
+							groundNormalAngle_,
+							clusterRadius_,
+							minClusterSize_,
+							segmentFlatObstacles_,
+							maxGroundHeight_);
+
+					if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
+					{
+						pcl::copyPointCloud(*originalCloud_near, *ground, *groundCloud);
+						ground->clear();
+					}
+
+					if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && obstacles.get() && obstacles->size())
+					{
+						pcl::copyPointCloud(*originalCloud_near, *obstacles, *obstaclesCloud);
+						obstacles->clear();
+					}
+
+					// Part 2: segment floor and obstacles far from the robot
+					rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
+							originalCloud_far,
+							ground,
+							obstacles,
+							normalKSearch_,
+							2.*groundNormalAngle_,
+							3.*clusterRadius_,
+							minClusterSize_,
+							segmentFlatObstacles_,
+							maxGroundHeight_);
+
+					if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
+					{
+						pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud2 (new pcl::PointCloud<pcl::PointXYZ>);
+						pcl::copyPointCloud(*originalCloud_far, *ground, *groundCloud2);
+						*groundCloud += *groundCloud2;
+					}
+
+
+					if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && obstacles.get() && obstacles->size())
+					{
+						pcl::PointCloud<pcl::PointXYZ>::Ptr obstacles2(new pcl::PointCloud<pcl::PointXYZ>);
+						pcl::copyPointCloud(*originalCloud_far, *obstacles, *obstacles2);
+						*obstaclesCloud += *obstacles2;
+					}
+				}
+
+				if(!localTransform.isIdentity())
+				{
+					//transform back in topic frame
+					rtabmap::Transform localTransformInv = localTransform.inverse();
+					if(groundCloud->size())
+					{
+						groundCloud = rtabmap::util3d::transformPointCloud(groundCloud, localTransformInv);
+					}
+					if(obstaclesCloud->size())
+					{
+						obstaclesCloud = rtabmap::util3d::transformPointCloud(obstaclesCloud, localTransformInv);
+					}
+				}
+			}
+		}
+
+		if(groundPub_.getNumSubscribers())
+		{
+			sensor_msgs::PointCloud2 rosCloud;
+			pcl::toROSMsg(*groundCloud, rosCloud);
+			rosCloud.header = cloudMsg->header;
+
+			//publish the message
+			groundPub_.publish(rosCloud);
+		}
+
+		if(obstaclesPub_.getNumSubscribers())
+		{
+			sensor_msgs::PointCloud2 rosCloud;
+			pcl::toROSMsg(*obstaclesCloud, rosCloud);
+			rosCloud.header = cloudMsg->header;
+
+			//publish the message
+			obstaclesPub_.publish(rosCloud);
+		}
+
+		if(projObstaclesPub_.getNumSubscribers())
+		{
+			sensor_msgs::PointCloud2 rosCloud;
+			pcl::toROSMsg(*obstaclesCloudWithoutFlatSurfaces, rosCloud);
+			rosCloud.header.stamp = cloudMsg->header.stamp;
+			rosCloud.header.frame_id = frameId_;
+
+			//publish the message
+			projObstaclesPub_.publish(rosCloud);
+		}
+
+		NODELET_DEBUG("Obstacles segmentation time = %f s", (ros::WallTime::now() - time).toSec());
+	}
+
+private:
+	std::string frameId_;
+	int normalKSearch_;
+	double groundNormalAngle_;
+	double clusterRadius_;
+	int minClusterSize_;
+	double maxObstaclesHeight_;
+	double maxGroundHeight_;
+	bool segmentFlatObstacles_;
+	bool waitForTransform_;
+	bool optimizeForCloseObjects_;
+	double projVoxelSize_;
+
+	tf::TransformListener tfListener_;
+
+	ros::Publisher groundPub_;
+	ros::Publisher obstaclesPub_;
+	ros::Publisher projObstaclesPub_;
+
+	ros::Subscriber cloudSub_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_legacy::ObstaclesDetectionOld, nodelet::Nodelet);
+}
+
+

rtabmap_legacy/src/nodelets/stereo_throttle.cpp

@@ -0,0 +1,270 @@
+/*
+Copyright (c) 2010-2016, 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.hpp"
+#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>
+
+#include <cv_bridge/cv_bridge.h>
+
+#include <rtabmap/core/util2d.h>
+
+namespace rtabmap_legacy
+{
+
+class StereoThrottleNodelet : public nodelet::Nodelet
+{
+public:
+	//Constructor
+	StereoThrottleNodelet():
+		rate_(0),
+		approxSync_(0),
+		exactSync_(0),
+		decimation_(1)
+	{
+	}
+
+	virtual ~StereoThrottleNodelet()
+	{
+		if(approxSync_)
+		{
+			delete approxSync_;
+		}
+		if(exactSync_)
+		{
+			delete exactSync_;
+		}
+	}
+
+private:
+	ros::Time last_update_;
+	double 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;
+		double approxSyncMaxInterval = 0.0;
+		pnh.param("approx_sync", approxSync, approxSync);
+		pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
+		pnh.param("rate", rate_, rate_);
+		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("decimation", decimation_, decimation_);
+		ROS_ASSERT(decimation_ >= 1);
+		NODELET_INFO("rate=%f Hz", rate_);
+		NODELET_INFO("decimation=%d", decimation_);
+		NODELET_INFO("approx_sync = %s", approxSync?"true":"false");
+		if(approxSync)
+			NODELET_INFO("approx_sync_max_interval = %f", approxSyncMaxInterval);
+
+		if(approxSync)
+		{
+			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
+			if(approxSyncMaxInterval>0.0)
+				approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
+			approxSync_->registerCallback(boost::bind(&StereoThrottleNodelet::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
+		}
+		else
+		{
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
+			exactSync_->registerCallback(boost::bind(&StereoThrottleNodelet::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_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 (rate_ > 0.0)
+		{
+			NODELET_DEBUG("update set to %f", rate_);
+			if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now())
+			{
+				NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
+				return;
+			}
+		}
+		else
+			NODELET_DEBUG("rate unset continuing");
+
+		last_update_ = ros::Time::now();
+
+		double leftStamp = imageLeft->header.stamp.toSec();
+		double rightStamp = imageRight->header.stamp.toSec();
+		double leftInfoStamp = camInfoLeft->header.stamp.toSec();
+		double rightInfoStamp = camInfoRight->header.stamp.toSec();
+
+		if(infoLeftPub_.getNumSubscribers())
+		{
+			if(decimation_ > 1)
+			{
+				sensor_msgs::CameraInfo info = *camInfoLeft;
+				info.height /= decimation_;
+				info.width /= decimation_;
+				info.roi.height /= decimation_;
+				info.roi.width /= decimation_;
+				info.K[2]/=float(decimation_); // cx
+				info.K[5]/=float(decimation_); // cy
+				info.K[0]/=float(decimation_); // fx
+				info.K[4]/=float(decimation_); // fy
+				info.P[2]/=float(decimation_); // cx
+				info.P[6]/=float(decimation_); // cy
+				info.P[0]/=float(decimation_); // fx
+				info.P[5]/=float(decimation_); // fy
+				info.P[3]/=float(decimation_); // Tx
+				infoLeftPub_.publish(info);
+			}
+			else
+			{
+				infoLeftPub_.publish(camInfoLeft);
+			}
+		}
+		if(infoRightPub_.getNumSubscribers())
+		{
+			if(decimation_ > 1)
+			{
+				sensor_msgs::CameraInfo info = *camInfoRight;
+				info.height /= decimation_;
+				info.width /= decimation_;
+				info.roi.height /= decimation_;
+				info.roi.width /= decimation_;
+				info.K[2]/=float(decimation_); // cx
+				info.K[5]/=float(decimation_); // cy
+				info.K[0]/=float(decimation_); // fx
+				info.K[4]/=float(decimation_); // fy
+				info.P[2]/=float(decimation_); // cx
+				info.P[6]/=float(decimation_); // cy
+				info.P[0]/=float(decimation_); // fx
+				info.P[5]/=float(decimation_); // fy
+				info.P[3]/=float(decimation_); // Tx
+				infoRightPub_.publish(info);
+			}
+			else
+			{
+				infoRightPub_.publish(camInfoRight);
+			}
+		}
+
+
+		if(imageLeftPub_.getNumSubscribers())
+		{
+			if(decimation_ > 1)
+			{
+				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft);
+				cv_bridge::CvImage out;
+				out.header = imagePtr->header;
+				out.encoding = imagePtr->encoding;
+				out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
+				imageLeftPub_.publish(out.toImageMsg());
+			}
+			else
+			{
+				imageLeftPub_.publish(imageLeft);
+			}
+		}
+		if(imageRightPub_.getNumSubscribers())
+		{
+			if(decimation_ > 1)
+			{
+				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageRight);
+				cv_bridge::CvImage out;
+				out.header = imagePtr->header;
+				out.encoding = imagePtr->encoding;
+				out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
+				imageRightPub_.publish(out.toImageMsg());
+			}
+			else
+			{
+				imageRightPub_.publish(imageRight);
+			}
+		}
+
+		if( leftStamp != imageLeft->header.stamp.toSec() ||
+			rightStamp != imageRight->header.stamp.toSec())
+		{
+			NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
+					"sure the node publishing the topics doesn't override the same data after publishing them. A "
+					"solution is to use this node within another nodelet manager. Stamps: "
+					"left%f->%f right=%f->%f",
+					leftStamp, imageLeft->header.stamp.toSec(),
+					rightStamp, imageRight->header.stamp.toSec());
+		}
+	}
+
+	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_;
+
+	int decimation_;
+
+};
+
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_legacy::StereoThrottleNodelet, nodelet::Nodelet);
+}

rtabmap_legacy/src/nodelets/undistort_depth.cpp

@@ -0,0 +1,121 @@
+/*
+Copyright (c) 2010-2016, 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.hpp>
+#include <nodelet/nodelet.h>
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber.h>
+#include <image_transport/publisher.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+
+#include "rtabmap/core/clams/discrete_depth_distortion_model.h"
+#include "rtabmap/utilite/UConversion.h"
+
+namespace rtabmap_legacy
+{
+
+class UndistortDepth : public nodelet::Nodelet
+{
+public:
+	UndistortDepth()
+	{}
+
+	virtual ~UndistortDepth()
+	{
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		std::string modelPath;
+		pnh.param("model", modelPath, modelPath);
+
+		if(modelPath.empty())
+		{
+			NODELET_ERROR("undistort_depth: \"model\" parameter should be set!");
+		}
+
+		model_.load(modelPath);
+		if(!model_.isValid())
+		{
+			NODELET_ERROR("Loaded distortion model from \"%s\" is not valid!", modelPath.c_str());
+		}
+		else
+		{
+			image_transport::ImageTransport it(nh);
+			sub_ = it.subscribe("depth", 1, &UndistortDepth::callback, this);
+			pub_ = it.advertise(uFormat("%s_undistorted", nh.resolveName("depth").c_str()), 1);
+		}
+	}
+
+	void callback(const sensor_msgs::ImageConstPtr& depth)
+	{
+		if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
+		   depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 &&
+		   depth->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0)
+		{
+			NODELET_ERROR("Input type depth=32FC1,16UC1,MONO16");
+			return;
+		}
+
+		if(pub_.getNumSubscribers())
+		{
+			if(depth->width == model_.getWidth() && depth->width == model_.getWidth())
+			{
+				cv_bridge::CvImagePtr imageDepthPtr = cv_bridge::toCvCopy(depth);
+				model_.undistort(imageDepthPtr->image);
+				pub_.publish(imageDepthPtr->toImageMsg());
+			}
+			else
+			{
+				NODELET_ERROR("Input depth image size (%dx%d) and distortion model "
+						"size (%dx%d) don't match! Cannot undistort image.",
+						depth->width, depth->height,
+						model_.getWidth(), model_.getHeight());
+			}
+		}
+	}
+
+private:
+	clams::DiscreteDepthDistortionModel model_;
+	image_transport::Publisher pub_;
+	image_transport::Subscriber sub_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_legacy::UndistortDepth, nodelet::Nodelet);
+}
+