Added rgbd_relay node/nodelet (can be used exactly like a relay of topic_tools but with ability to uncompress or compress rgbd_image)

CMakeLists.txt

@@ -212,7 +212,7 @@ SET(rtabmap_plugins_lib_src
 )
 
 IF(${cv_bridge_VERSION_MAJOR} GREATER 1 OR ${cv_bridge_VERSION_MINOR} GREATER 10)
-   SET(rtabmap_plugins_lib_src ${rtabmap_plugins_lib_src} src/nodelets/rgbd_sync.cpp src/nodelets/stereo_sync.cpp)
+   SET(rtabmap_plugins_lib_src ${rtabmap_plugins_lib_src} src/nodelets/rgbd_sync.cpp src/nodelets/stereo_sync.cpp src/nodelets/rgbd_relay.cpp)
 ELSE()
    ADD_DEFINITIONS("-DCV_BRIDGE_HYDRO")
 ENDIF()   
@@ -349,6 +349,14 @@ add_executable(rtabmap_icp_odometry src/ICPOdometryNode.cpp)
 target_link_libraries(rtabmap_icp_odometry ${Libraries})
 set_target_properties(rtabmap_icp_odometry PROPERTIES OUTPUT_NAME "icp_odometry")
 
+add_executable(rtabmap_rgbd_sync src/RGBDSyncNode.cpp)
+target_link_libraries(rtabmap_rgbd_sync ${Libraries})
+set_target_properties(rtabmap_rgbd_sync PROPERTIES OUTPUT_NAME "rgbd_sync")
+
+add_executable(rtabmap_rgbd_relay src/RGBDRelayNode.cpp)
+target_link_libraries(rtabmap_rgbd_relay ${Libraries})
+set_target_properties(rtabmap_rgbd_relay PROPERTIES OUTPUT_NAME "rgbd_relay")
+
 add_executable(rtabmap_map_optimizer src/MapOptimizerNode.cpp)
 target_link_libraries(rtabmap_map_optimizer rtabmap_ros)
 set_target_properties(rtabmap_map_optimizer PROPERTIES OUTPUT_NAME "map_optimizer")

launch/rtabmap.launch

@@ -113,6 +113,8 @@
   <arg unless="$(arg compressed)" name="left_image_topic_relay"    default="$(arg left_image_topic)"/>
   <arg if="$(arg compressed)"     name="right_image_topic_relay"   default="$(arg right_image_topic)_relay"/>
   <arg unless="$(arg compressed)" name="right_image_topic_relay"   default="$(arg right_image_topic)"/>
+  <arg if="$(arg rgbd_sync)"      name="rgbd_topic_relay"          default="$(arg rgbd_topic)"/>
+  <arg unless="$(arg rgbd_sync)"  name="rgbd_topic_relay"          default="$(arg rgbd_topic)_relay"/>
 
   <!-- Nodes -->
   <group ns="$(arg namespace)">
@@ -130,7 +132,7 @@
           <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
           <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
           <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
-          <remap from="rgbd_image"      to="$(arg rgbd_topic)"/>
+          <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
           <param name="approx_sync"     type="bool"   value="$(arg approx_rgbd_sync)"/>
           <param name="queue_size"      type="int"    value="$(arg queue_size)"/>
           <param name="depth_scale"     type="double" value="$(arg depth_scale)"/>
@@ -150,7 +152,7 @@
           <remap from="right/image_rect"  to="$(arg right_image_topic_relay)"/>
           <remap from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
           <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
-          <remap from="rgbd_image"      to="$(arg rgbd_topic)"/>
+          <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
           <param name="approx_sync"     type="bool"   value="$(arg approx_rgbd_sync)"/>
           <param name="queue_size"      type="int"    value="$(arg queue_size)"/>
         </node>
@@ -158,7 +160,14 @@
     </group>
     
     <group unless="$(arg rgbd_sync)">
-       <node if="$(arg subscribe_rgbd)" name="republish_rgbd_image"  type="relay" pkg="topic_tools" args="$(arg rgbd_topic) $(arg rgbd_topic)_relay" />
+       <group if="$(arg subscribe_rgbd)">
+         <node name="republish_rgbd_image"  type="rgbd_relay" pkg="rtabmap_ros">
+           <remap     if="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)/compressed"/>
+           <remap     if="$(arg compressed)" from="$(arg rgbd_topic)/compressed_relay" to="$(arg rgbd_topic_relay)"/>
+           <remap unless="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)"/>
+           <param if="$(arg compressed)" name="uncompress" value="true"/>
+         </node>
+      </group>
     </group>
 
     <!-- Visual odometry -->
@@ -170,8 +179,7 @@
           <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
           <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
           <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
-          <remap     if="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)"/>
+          <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
-          <remap unless="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)_relay"/>
           <remap from="odom"            to="$(arg odom_topic)"/>
       
           <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
@@ -194,8 +202,7 @@
           <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
           <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
           <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
-          <remap     if="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)"/>
+          <remap from="rgbd_image"             to="$(arg rgbd_topic_relay)"/>
-          <remap unless="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)_relay"/>
           <remap from="odom"                   to="$(arg odom_topic)"/>
           <remap from="imu"                    to="$(arg imu_topic)"/>
       
@@ -266,8 +273,7 @@
       <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
       <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
       
-      <remap     if="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)"/>
+      <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
-      <remap unless="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)_relay"/>
 	
       <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
       <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
@@ -308,8 +314,7 @@
       <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
       <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
       
-      <remap     if="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)"/>
+      <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
-      <remap unless="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)_relay"/>
       
       <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
       <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
@@ -333,8 +338,7 @@
     <remap unless="$(arg subscribe_rgbd)" from="rgb/image"         to="$(arg rgb_topic_relay)"/>
     <remap unless="$(arg subscribe_rgbd)" from="depth/image"       to="$(arg depth_topic_relay)"/>
     <remap unless="$(arg subscribe_rgbd)" from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
-    <remap     if="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)"/>
+    <remap from="rgbd_image"        to="$(arg rgbd_topic_relay)"/>
-    <remap unless="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)_relay"/>
     <remap from="cloud"             to="voxel_cloud" />
 
     <param name="decimation"  type="double" value="4"/>

nodelet_plugins.xml

@@ -128,6 +128,14 @@
     </description>
   </class>
   
+  <class name="rtabmap_ros/rgbd_relay" 
+         type="rtabmap_ros::RGBDRelay" 
+         base_class_type="nodelet::Nodelet">
+    <description>
+      This is my nodelet.
+    </description>
+  </class>
+  
   <class name="rtabmap_ros/stereo_sync" 
          type="rtabmap_ros::StereoSync" 
          base_class_type="nodelet::Nodelet">

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap_ros</name>
-  <version>0.18.3</version>
+  <version>0.19.0</version>
   <description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

src/RGBDRelayNode.cpp

@@ -0,0 +1,47 @@
+/*
+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 "nodelet/loader.h"
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "rgbd_relay");
+
+	nodelet::V_string nargv;
+	for(int i=1;i<argc;++i)
+	{
+		nargv.push_back(argv[i]);
+	}
+
+	nodelet::Loader nodelet;
+	nodelet::M_string remap(ros::names::getRemappings());
+	std::string nodelet_name = ros::this_node::getName();
+	nodelet.load(nodelet_name, "rtabmap_ros/rgbd_relay", remap, nargv);
+	ros::spin();
+	return 0;
+}

src/RGBDSyncNode.cpp

@@ -0,0 +1,47 @@
+/*
+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 "nodelet/loader.h"
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "rgbd_sync");
+
+	nodelet::V_string nargv;
+	for(int i=1;i<argc;++i)
+	{
+		nargv.push_back(argv[i]);
+	}
+
+	nodelet::Loader nodelet;
+	nodelet::M_string remap(ros::names::getRemappings());
+	std::string nodelet_name = ros::this_node::getName();
+	nodelet.load(nodelet_name, "rtabmap_ros/rgbd_sync", remap, nargv);
+	ros::spin();
+	return 0;
+}

src/nodelets/rgbd_relay.cpp

@@ -0,0 +1,201 @@
+/*
+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.h>
+#include <nodelet/nodelet.h>
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CompressedImage.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/CameraInfo.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/subscriber.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+
+#include <boost/thread.hpp>
+
+#include "rtabmap_ros/RGBDImage.h"
+#include "rtabmap_ros/MsgConversion.h"
+
+#include "rtabmap/core/Compression.h"
+#include "rtabmap/utilite/UConversion.h"
+
+namespace rtabmap_ros
+{
+
+class RGBDRelay : public nodelet::Nodelet
+{
+public:
+	RGBDRelay() :
+		compress_(false),
+		uncompress_(false)
+	{}
+
+	virtual ~RGBDRelay()
+	{
+	}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int queueSize = 10;
+		bool approxSync = true;
+		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("compress", compress_, compress_);
+		pnh.param("uncompress", uncompress_, uncompress_);
+
+		NODELET_INFO("%s: queue_size  = %d", getName().c_str(), queueSize);
+
+		rgbdImageSub_ = nh.subscribe("rgbd_image", 1, &RGBDRelay::callback, this);
+		rgbdImagePub_ = nh.advertise<rtabmap_ros::RGBDImage>(nh.resolveName("rgbd_image") + "_relay", 1);
+	}
+
+	void callback(const rtabmap_ros::RGBDImageConstPtr& input)
+	{
+		if(rgbdImagePub_.getNumSubscribers())
+		{
+			if(!compress_ && !uncompress_)
+			{
+				//just republish it
+				rgbdImagePub_.publish(input);
+				return;
+			}
+
+			rtabmap_ros::RGBDImage output;
+			output.header = input->header;
+			output.rgbCameraInfo = input->rgbCameraInfo;
+			output.depthCameraInfo = input->depthCameraInfo;
+
+			rtabmap::StereoCameraModel stereoModel = stereoCameraModelFromROS(input->rgbCameraInfo, input->depthCameraInfo, rtabmap::Transform::getIdentity());
+
+			if(compress_)
+			{
+				if(!input->rgbCompressed.data.empty())
+				{
+					// already compressed, just copy pointer
+					output.rgbCompressed = input->rgbCompressed;
+				}
+				else if(!input->rgb.data.empty())
+				{
+#ifdef CV_BRIDGE_HYDRO
+					ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
+#else
+					cv_bridge::CvImageConstPtr rgb = cv_bridge::toCvShare(input->rgb, input);
+					rgb->toCompressedImageMsg(output.rgbCompressed, cv_bridge::JPG);
+#endif
+				}
+
+				if(!input->depthCompressed.data.empty())
+				{
+					// already compressed, just copy pointer
+					output.depthCompressed = input->depthCompressed;
+				}
+				else if(!input->depth.data.empty())
+				{
+					if(stereoModel.isValidForProjection())
+					{
+						// right stereo image
+						cv_bridge::CvImageConstPtr imageRightPtr = cv_bridge::toCvShare(input->depth, input);
+						imageRightPtr->toCompressedImageMsg(output.depthCompressed, cv_bridge::JPG);
+					}
+					else
+					{
+						// depth image
+						cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(input->depth, input);
+						output.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
+						output.depthCompressed.format = "png";
+					}
+				}
+			}
+			if(uncompress_)
+			{
+				if(!input->rgb.data.empty())
+				{
+					// already raw, just copy pointer
+					output.rgb = input->rgb;
+				}
+				if(!input->rgbCompressed.data.empty())
+				{
+#ifdef CV_BRIDGE_HYDRO
+					ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
+#else
+					cv_bridge::toCvCopy(input->rgbCompressed)->toImageMsg(output.rgb);
+#endif
+				}
+
+				if(!input->depth.data.empty())
+				{
+					// already raw, just copy pointer
+					output.depth = input->depth;
+				}
+				else if(input->depthCompressed.format.compare("jpg")==0)
+				{
+					// right stereo image
+#ifdef CV_BRIDGE_HYDRO
+					ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
+#else
+					cv_bridge::toCvCopy(input->depthCompressed)->toImageMsg(output.depth);
+#endif
+				}
+				else
+				{
+					// dpeth image
+					cv_bridge::CvImagePtr ptr = boost::make_shared<cv_bridge::CvImage>();
+					ptr->header = input->depthCompressed.header;
+					ptr->image = rtabmap::uncompressImage(input->depthCompressed.data);
+					ROS_ASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1);
+					ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
+					ptr->toImageMsg(output.depth);
+				}
+			}
+
+			rgbdImagePub_.publish(output);
+		}
+	}
+
+private:
+
+	bool compress_;
+	bool uncompress_;
+	ros::Subscriber rgbdImageSub_;
+	ros::Publisher rgbdImagePub_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDRelay, nodelet::Nodelet);
+}
+

src/nodelets/rgbd_sync.cpp

@@ -59,6 +59,7 @@ class RGBDSync : public nodelet::Nodelet
 public:
 	RGBDSync() :
 		depthScale_(1.0),
+		compressedRate_(0),
 		warningThread_(0),
 		callbackCalled_(false),
 		approxSyncDepth_(0),
@@ -91,10 +92,12 @@ private:
 		pnh.param("approx_sync", approxSync, approxSync);
 		pnh.param("queue_size", queueSize, queueSize);
 		pnh.param("depth_scale", depthScale_, depthScale_);
+		pnh.param("compressed_rate", compressedRate_, compressedRate_);
 
 		NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
 		NODELET_INFO("%s: queue_size  = %d", getName().c_str(), queueSize);
 		NODELET_INFO("%s: depth_scale = %f", getName().c_str(), depthScale_);
+		NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_);
 
 		rgbdImagePub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image", 1);
 		rgbdImageCompressedPub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image/compressed", 1);
@@ -173,24 +176,39 @@ private:
 
 			if(rgbdImageCompressedPub_.getNumSubscribers())
 			{
-				rtabmap_ros::RGBDImage msgCompressed = msg;
+				bool publishCompressed = true;
-
+				if (compressedRate_ > 0.0)
-				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
-				imagePtr->toCompressedImageMsg(msgCompressed.rgbCompressed, cv_bridge::JPG);
-
-				cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
-				msgCompressed.depthCompressed.header = imageDepthPtr->header;
-				if(depthScale_ != 1.0)
 				{
-					msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image*depthScale_, ".png");
+					if ( lastCompressedPublished_ + ros::Duration(1.0/compressedRate_) > ros::Time::now())
+					{
+						NODELET_DEBUG("throttle last update at %f skipping", lastCompressedPublished_.toSec());
+						publishCompressed = false;
+					}
 				}
-				else
-				{
-					msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
-				}
-				msgCompressed.depthCompressed.format = "png";
 
-				rgbdImageCompressedPub_.publish(msgCompressed);
+				if(publishCompressed)
+				{
+					lastCompressedPublished_ = ros::Time::now();
+
+					rtabmap_ros::RGBDImage msgCompressed = msg;
+
+					cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
+					imagePtr->toCompressedImageMsg(msgCompressed.rgbCompressed, cv_bridge::JPG);
+
+					cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
+					msgCompressed.depthCompressed.header = imageDepthPtr->header;
+					if(depthScale_ != 1.0)
+					{
+						msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image*depthScale_, ".png");
+					}
+					else
+					{
+						msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
+					}
+					msgCompressed.depthCompressed.format = "png";
+
+					rgbdImageCompressedPub_.publish(msgCompressed);
+				}
 			}
 
 			if(rgbdImagePub_.getNumSubscribers())
@@ -226,9 +244,12 @@ private:
 
 private:
 	double depthScale_;
+	double compressedRate_;
 	boost::thread * warningThread_;
 	bool callbackCalled_;
 
+	ros::Time lastCompressedPublished_;
+
 	ros::Publisher rgbdImagePub_;
 	ros::Publisher rgbdImageCompressedPub_;