Update 0.15.4.

rgbd_sync and rtabmap.launch: added depth_scale parameter.
Updated OdomInfo msg with memoryUsage field.
CoreWrapper: supporting GridGlobal/MaxNodes parameter, added "RtabmapROS" statistics.
MapsManager: updated when occupancy grid is updated.

CMakeLists.txt

@@ -18,7 +18,7 @@ find_package(rviz)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.15.3 REQUIRED)
+find_package(RTABMap 0.15.4 REQUIRED)
 
 find_package(OpenCV REQUIRED)
 

include/rtabmap_ros/CoreWrapper.h

@@ -183,6 +183,7 @@ private:
 	rtabmap::Transform lastPublishedMetricGoal_;
 	bool latestNodeWasReached_;
 	rtabmap::ParametersMap parameters_;
+	std::map<std::string, float> rtabmapROSStats_;
 
 	std::string frameId_;
 	std::string odomFrameId_;
@@ -274,6 +275,7 @@ private:
 	bool odomSensorSync_;
 	float rate_;
 	bool createIntermediateNodes_;
+	int maxMappingNodes_;
 	ros::Time time_;
 	ros::Time previousStamp_;
 };

include/rtabmap_ros/MapsManager.h

@@ -68,7 +68,7 @@ public:
 			const ros::Time & stamp,
 			const std::string & mapFrameId);
 
-	cv::Mat generateGridMap(
+	cv::Mat getGridMap(
 			const std::map<int, rtabmap::Transform> & filteredPoses,
 			float & xMin,
 			float & yMin,

launch/rtabmap.launch

@@ -45,8 +45,8 @@
   <arg name="args"                    default=""/>              <!-- delete_db_on_start, udebug -->
   <arg name="rtabmap_args"            default="$(arg args)"/>   <!-- deprecated, use "args" argument -->
   <arg name="launch_prefix"           default=""/>              <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
-  <arg name="output"                  default="screen"/>        <!--Control node output (screen or log)-->
-  
+  <arg name="output"                  default="screen"/>        <!-- Control node output (screen or log) -->
+
   <!-- if timestamps of the input topics are synchronized using approximate or exact time policy-->
   <arg     if="$(arg stereo)" name="approx_sync"  default="false"/>
   <arg unless="$(arg stereo)" name="approx_sync"  default="true"/>         
@@ -68,6 +68,7 @@
   <arg name="approx_rgbd_sync"        default="true"/>          <!-- false=exact synchronization -->
   <arg name="subscribe_rgbd"          default="$(arg rgbd_sync)"/>
   <arg name="rgbd_topic"              default="rgbd_image" />
+  <arg name="depth_scale"             default="1.0" />
   
   <arg name="compressed"              default="false"/>         <!-- If you want to subscribe to compressed image topics -->
   <arg name="rgb_image_transport"     default="compressed"/>    <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
@@ -125,6 +126,7 @@
       <remap from="rgbd_image"      to="$(arg rgbd_topic)"/>
       <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)"/>
     </node>
     
     <!-- Visual odometry -->
@@ -291,7 +293,7 @@
     <remap from="cloud"             to="voxel_cloud" />
 
     <param name="decimation"  type="double" value="2"/>
-    <param name="voxel_size"  type="double" value="0.0"/>
+    <param name="voxel_size"  type="double" value="0.02"/>
     <param name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
   </node>
 

msg/OdomInfo.msg

@@ -22,6 +22,7 @@ float32 timeParticleFiltering
 float32 stamp
 float32 interval
 float32 distanceTravelled
+int32 memoryUsage # MB
 
 geometry_msgs/Transform transform
 geometry_msgs/Transform transformFiltered

package.xml

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

src/CoreWrapper.cpp

@@ -57,6 +57,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/OccupancyGrid.h>
 #include <rtabmap/core/DBDriver.h>
 #include <rtabmap/core/Registration.h>
+#include <rtabmap/core/Graph.h>
 
 #ifdef WITH_OCTOMAP_ROS
 #ifdef RTABMAP_OCTOMAP
@@ -111,6 +112,7 @@ CoreWrapper::CoreWrapper() :
 		odomSensorSync_(false),
 		rate_(Parameters::defaultRtabmapDetectionRate()),
 		createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()),
+		maxMappingNodes_(Parameters::defaultGridGlobalMaxNodes()),
 		time_(ros::Time::now()),
 		previousStamp_(0),
 		mbClient_("move_base", true)
@@ -427,6 +429,14 @@ void CoreWrapper::onInit()
 			NODELET_INFO("Create intermediate nodes");
 		}
 	}
+	if(parameters_.find(Parameters::kGridGlobalMaxNodes()) != parameters_.end())
+	{
+		Parameters::parse(parameters_, Parameters::kGridGlobalMaxNodes(), maxMappingNodes_);
+		if(maxMappingNodes_>0)
+		{
+			NODELET_INFO("Max mapping nodes = %d", maxMappingNodes_);
+		}
+	}
 
 	if(paused_)
 	{
@@ -780,8 +790,8 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
 		if(!odom.isNull())
 		{
 			cv::Mat covariance;
-			float variance = odomMsg->twist.covariance[0];
-			if(variance == BAD_COVARIANCE)
+			double variance = odomMsg->twist.covariance[0];
+			if(variance == BAD_COVARIANCE || variance <= 0.0f)
 			{
 				//use the one of the pose
 				covariance = cv::Mat(6,6,CV_64FC1, (void*)odomMsg->pose.covariance.data()).clone();
@@ -1381,6 +1391,7 @@ void CoreWrapper::process(
 			externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
 			externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
 			externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
+			externalStats.insert(std::make_pair("Odometry/RAM_usage/MB", odomInfo.memoryUsage));
 
 			if(odomInfo.interval>0.0)
 			{
@@ -1395,6 +1406,11 @@ void CoreWrapper::process(
 				odomVelocity[5] = yaw/odomInfo.interval;
 			}
 		}
+		if(rtabmapROSStats_.size())
+		{
+			externalStats.insert(rtabmapROSStats_.begin(), rtabmapROSStats_.end());
+			rtabmapROSStats_.clear();
+		}
 
 		if(rtabmap_.process(data, odom, covariance, odomVelocity, externalStats))
 		{
@@ -1426,7 +1442,41 @@ void CoreWrapper::process(
 					SensorData tmpData = data;
 					tmpData.setId(-1);
 					tmpSignature.insert(std::make_pair(-1, Signature(-1, -1, 0, data.stamp(), "", odom, Transform(), tmpData)));
-					filteredPoses.insert(std::make_pair(-1, rtabmap_.getMapCorrection()*odom));
+					filteredPoses.insert(std::make_pair(-1, mapToOdom_*odom));
+				}
+
+				if(maxMappingNodes_ > 0 && filteredPoses.size()>1)
+				{
+					std::map<int, Transform> nearestPoses;
+					std::vector<int> nodes = graph::findNearestNodes(filteredPoses, mapToOdom_*odom, maxMappingNodes_);
+					for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
+					{
+						std::map<int, Transform>::iterator pter = filteredPoses.find(*iter);
+						if(pter != filteredPoses.end())
+						{
+							nearestPoses.insert(*pter);
+						}
+					}
+					//add negative and make sure those on a planned path are not filtered
+					std::set<int> onPath;
+					if(rtabmap_.getPath().size())
+					{
+						std::vector<int> nextNodes = rtabmap_.getPathNextNodes();
+						onPath.insert(nextNodes.begin(), nextNodes.end());
+					}
+					for(std::map<int, Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter)
+					{
+						if(iter->first < 0 || onPath.find(iter->first) != onPath.end())
+						{
+							nearestPoses.insert(*iter);
+						}
+						else if(onPath.empty())
+						{
+							break;
+						}
+					}
+
+					filteredPoses = nearestPoses;
 				}
 
 				// Update maps
@@ -1528,6 +1578,11 @@ void CoreWrapper::process(
 				timePublishMaps,
 				(int)rtabmap_.getLocalOptimizedPoses().size(),
 				rtabmap_.getWMSize()+rtabmap_.getSTMSize());
+		rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/HasSubscribers/"), mapsManager_.hasSubscribers()?1:0));
+		rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeRtabmap/ms"), timeRtabmap*1000.0f));
+		rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeUpdatingMaps/ms"), timeUpdateMaps*1000.0f));
+		rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimePublishing/ms"), timePublishMaps*1000.0f));
+		rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeTotal/ms"), (timeRtabmap+timeUpdateMaps+timePublishMaps)*1000.0f));
 	}
 	else if(!rtabmap_.isIDsGenerated())
 	{
@@ -1964,9 +2019,24 @@ bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::
 
 bool CoreWrapper::getMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res)
 {
-	std::map<int, rtabmap::Transform> filteredPoses;
+	std::map<int, rtabmap::Transform> filteredPoses = rtabmap_.getLocalOptimizedPoses();
+	if(maxMappingNodes_ > 0 && filteredPoses.size()>1)
+	{
+		std::map<int, Transform> nearestPoses;
+		std::vector<int> nodes = graph::findNearestNodes(filteredPoses, filteredPoses.rbegin()->second, maxMappingNodes_);
+		for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
+		{
+			std::map<int, Transform>::iterator pter = filteredPoses.find(*iter);
+			if(pter != filteredPoses.end())
+			{
+				nearestPoses.insert(*pter);
+			}
+		}
+		filteredPoses = nearestPoses;
+	}
+
 	filteredPoses = mapsManager_.updateMapCaches(
-			rtabmap_.getLocalOptimizedPoses(),
+			filteredPoses,
 			rtabmap_.getMemory(),
 			true,
 			false);
@@ -1974,7 +2044,7 @@ bool CoreWrapper::getMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetM
 	{
 		// create the grid map
 		float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
-		cv::Mat pixels = mapsManager_.generateGridMap(filteredPoses, xMin, yMin, gridCellSize);
+		cv::Mat pixels = mapsManager_.getGridMap(filteredPoses, xMin, yMin, gridCellSize);
 
 		if(!pixels.empty())
 		{
@@ -2072,11 +2142,24 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 
 		if(!req.graphOnly && mapsManager_.hasSubscribers())
 		{
-			std::map<int, Transform> filteredPoses;
+			std::map<int, Transform> filteredPoses = poses;
+			if(maxMappingNodes_ > 0 && poses.size()>1)
+			{
+				std::map<int, Transform> nearestPoses;
+				std::vector<int> nodes = graph::findNearestNodes(filteredPoses, filteredPoses.rbegin()->second, maxMappingNodes_);
+				for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
+				{
+					std::map<int, Transform>::iterator pter = filteredPoses.find(*iter);
+					if(pter != filteredPoses.end())
+					{
+						nearestPoses.insert(*pter);
+					}
+				}
+			}
 			if(signatures.size())
 			{
 				filteredPoses = mapsManager_.updateMapCaches(
-						poses,
+						filteredPoses,
 						rtabmap_.getMemory(),
 						false,
 						false,
@@ -2084,7 +2167,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 			}
 			else
 			{
-				filteredPoses = mapsManager_.getFilteredPoses(poses);
+				filteredPoses = mapsManager_.getFilteredPoses(filteredPoses);
 			}
 			mapsManager_.publishMaps(filteredPoses, now, mapFrameId_);
 		}
@@ -2602,6 +2685,21 @@ bool CoreWrapper::octomapBinaryCallback(
 	res.map.header.stamp = ros::Time::now();
 
 	std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
+	if(maxMappingNodes_ > 0 && poses.size()>1)
+	{
+		std::map<int, Transform> nearestPoses;
+		std::vector<int> nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxMappingNodes_);
+		for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
+		{
+			std::map<int, Transform>::iterator pter = poses.find(*iter);
+			if(pter != poses.end())
+			{
+				nearestPoses.insert(*pter);
+			}
+		}
+		poses = nearestPoses;
+	}
+
 	poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, true);
 
 	const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
@@ -2618,6 +2716,21 @@ bool CoreWrapper::octomapFullCallback(
 	res.map.header.stamp = ros::Time::now();
 
 	std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
+	if(maxMappingNodes_ > 0 && poses.size()>1)
+	{
+		std::map<int, Transform> nearestPoses;
+		std::vector<int> nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxMappingNodes_);
+		for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
+		{
+			std::map<int, Transform>::iterator pter = poses.find(*iter);
+			if(pter != poses.end())
+			{
+				nearestPoses.insert(*pter);
+			}
+		}
+		poses = nearestPoses;
+	}
+
 	poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, true);
 
 	const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();

src/MapsManager.cpp

@@ -64,7 +64,7 @@ MapsManager::MapsManager() :
 		mapFilterRadius_(0.0),
 		mapFilterAngle_(30.0), // degrees
 		mapCacheCleanup_(true),
-		negativePosesIgnored_(false),
+		negativePosesIgnored_(true),
 		negativeScanEmptyRayTracing_(true),
 		assembledObstacles_(new pcl::PointCloud<pcl::PointXYZRGB>),
 		assembledGround_(new pcl::PointCloud<pcl::PointXYZRGB>),
@@ -345,16 +345,25 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 		bool updateOctomap,
 		const std::map<int, rtabmap::Signature> & signatures)
 {
+	bool updateGridCache = updateGrid || updateOctomap;
 	if(!updateGrid && !updateOctomap)
 	{
-		//  all false, udpate only those where we have subscribers
-		updateGrid = this->hasSubscribers();
+		//  all false, update only those where we have subscribers
 		updateOctomap =
 				octoMapPubBin_.getNumSubscribers() != 0 ||
 				octoMapPubFull_.getNumSubscribers() != 0 ||
 				octoMapCloud_.getNumSubscribers() != 0 ||
 				octoMapEmptySpace_.getNumSubscribers() != 0 ||
 				octoMapProj_.getNumSubscribers() != 0;
+
+		updateGrid = projMapPub_.getNumSubscribers() != 0 ||
+				gridMapPub_.getNumSubscribers() != 0;
+
+		updateGridCache = updateOctomap || updateGrid ||
+				cloudMapPub_.getNumSubscribers() != 0 ||
+				cloudObstaclesPub_.getNumSubscribers() != 0 ||
+				cloudGroundPub_.getNumSubscribers() != 0 ||
+				scanMapPub_.getNumSubscribers() != 0;
 	}
 
 #ifndef WITH_OCTOMAP_ROS
@@ -376,7 +385,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 	std::map<int, rtabmap::Transform> filteredPoses;
 
 	// update cache
-	if(updateGrid || updateOctomap)
+	if(updateGridCache)
 	{
 		// filter nodes
 		if(mapFilterRadius_ > 0.0)
@@ -420,7 +429,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 		bool longUpdate = false;
 		if(filteredPoses.size() > 20)
 		{
-			if(updateGrid && gridMaps_.size() < 5)
+			if(updateGridCache && gridMaps_.size() < 5)
 			{
 				ROS_WARN("Many occupancy grids should be loaded (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-gridMaps_.size()));
 				longUpdate = true;
@@ -443,7 +452,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 			if(!iter->second.isNull())
 			{
 				rtabmap::SensorData data;
-				if((updateGrid || updateOctomap) && (iter->first < 0 || !uContains(gridMaps_, iter->first)))
+				if(updateGridCache && (iter->first < 0 || !uContains(gridMaps_, iter->first)))
 				{
 					UDEBUG("Data required for %d", iter->first);
 					std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);
@@ -498,8 +507,8 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 							else
 							{
 								viewPoint = data.gridViewPoint();
-								gridMaps_.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles)));
-								gridMapsViewpoints_.insert(std::make_pair(iter->first, viewPoint));
+								uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
+								uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
 							}
 						}
 						else
@@ -537,8 +546,8 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 							else
 							{
 								viewPoint = data.gridViewPoint();
-								gridMaps_.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles)));
-								gridMapsViewpoints_.insert(std::make_pair(iter->first, viewPoint));
+								uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
+								uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
 							}
 
 							// put back
@@ -549,10 +558,6 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 								occupancyGrid_->parseParameters(parameters);
 							}
 						}
-						if(ground.cols || obstacles.cols)
-						{
-							occupancyGrid_->addToCache(iter->first, ground, obstacles);
-						}
 					}
 					else if(memory)
 					{
@@ -560,12 +565,25 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 					}
 				}
 
+				if(updateGrid &&
+						(iter->first < 0 ||
+						  occupancyGrid_->addedNodes().find(iter->first) == occupancyGrid_->addedNodes().end()))
+				{
+					std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator mter = gridMaps_.find(iter->first);
+					if(mter != gridMaps_.end())
+					{
+						if(!mter->second.first.empty() || !mter->second.second.empty())
+						{
+							occupancyGrid_->addToCache(iter->first, mter->second.first, mter->second.second);
+						}
+					}
+				}
+
 #ifdef WITH_OCTOMAP_ROS
 #ifdef RTABMAP_OCTOMAP
 				if(updateOctomap &&
 						(iter->first < 0 ||
-						  octomap_->addedNodes().empty() ||
-						  iter->first > octomap_->addedNodes().rbegin()->first))
+						  octomap_->addedNodes().find(iter->first) == octomap_->addedNodes().end()))
 				{
 					std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator mter = gridMaps_.find(iter->first);
 					std::map<int, cv::Point3f>::iterator pter = gridMapsViewpoints_.find(iter->first);
@@ -594,6 +612,11 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 			}
 		}
 
+		if(updateGrid)
+		{
+			occupancyGrid_->update(filteredPoses);
+		}
+
 #ifdef WITH_OCTOMAP_ROS
 #ifdef RTABMAP_OCTOMAP
 		if(updateOctomap)
@@ -1141,7 +1164,7 @@ void MapsManager::publishMaps(
 
 		// create the grid map
 		float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
-		cv::Mat pixels = this->generateGridMap(poses, xMin, yMin, gridCellSize);
+		cv::Mat pixels = this->getGridMap(poses, xMin, yMin, gridCellSize);
 
 		if(!pixels.empty())
 		{
@@ -1189,14 +1212,13 @@ void MapsManager::publishMaps(
 	}
 }
 
-cv::Mat MapsManager::generateGridMap(
+cv::Mat MapsManager::getGridMap(
 		const std::map<int, rtabmap::Transform> & poses,
 		float & xMin,
 		float & yMin,
 		float & gridCellSize)
 {
 	gridCellSize = occupancyGrid_->getCellSize();
-	occupancyGrid_->update(poses);
 	return occupancyGrid_->getMap(xMin, yMin);
 }
 

src/MsgConversion.cpp

@@ -919,6 +919,7 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
 	info.stamp = msg.stamp;
 	info.interval = msg.interval;
 	info.distanceTravelled = msg.distanceTravelled;
+	info.memoryUsage = msg.memoryUsage;
 
 	info.type = msg.type;
 
@@ -975,6 +976,7 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
 	msg.stamp = info.stamp;
 	msg.interval = info.interval;
 	msg.distanceTravelled = info.distanceTravelled;
+	msg.memoryUsage = info.memoryUsage;
 
 	msg.type = info.type;
 

src/nodelets/rgbd_sync.cpp

@@ -58,6 +58,7 @@ class RGBDSync : public nodelet::Nodelet
 {
 public:
 	RGBDSync() :
+		depthScale_(1.0),
 		warningThread_(0),
 		callbackCalled_(false),
 		approxSyncDepth_(0),
@@ -89,8 +90,11 @@ private:
 		bool approxSync = true;
 		pnh.param("approx_sync", approxSync, approxSync);
 		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("depth_scale", depthScale_, depthScale_);
 
-		NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
+		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_);
 
 		rgbdImagePub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image", 1);
 		rgbdImageCompressedPub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image/compressed", 1);
@@ -172,7 +176,14 @@ private:
 				cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
 				ROS_ASSERT(imageDepthPtr->image.type() == CV_32FC1 || imageDepthPtr->image.type() == CV_16UC1);
 				msgCompressed.depthCompressed.header = imageDepthPtr->header;
-				msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
+				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);
@@ -181,13 +192,23 @@ private:
 			if(rgbdImagePub_.getNumSubscribers())
 			{
 				msg.rgb = *image;
-				msg.depth = *depth;
+				if(depthScale_ != 1.0)
+				{
+					cv_bridge::CvImagePtr imageDepthPtr = cv_bridge::toCvCopy(depth);
+					imageDepthPtr->image*=depthScale_;
+					msg.depth = *imageDepthPtr->toImageMsg();
+				}
+				else
+				{
+					msg.depth = *depth;
+				}
 				rgbdImagePub_.publish(msg);
 			}
 		}
 	}
 
 private:
+	double depthScale_;
 	boost::thread * warningThread_;
 	bool callbackCalled_;