From 9822159f4b9a7ede765a7a892964b8f6725e6ad6 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Sun, 19 Jul 2015 20:51:32 -0400
Subject: [PATCH] fixed a warning where no scans are found when calling the
 service to publish the 3D map

---
 src/CoreWrapper.cpp | 14 ++++++++------
 src/MapsManager.cpp | 12 +++++++++++-
 src/MapsManager.h   |  1 +
 3 files changed, 20 insertions(+), 7 deletions(-)

diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp
index 0460371c..09a7dc57 100644
--- a/src/CoreWrapper.cpp
+++ b/src/CoreWrapper.cpp
@@ -1687,7 +1687,9 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 {
 	ROS_INFO("rtabmap: Publishing map...");
 
-	if(mapDataPub_.getNumSubscribers())
+	if(mapDataPub_.getNumSubscribers() ||
+	   (!req.graphOnly && mapsManager_.hasSubscribers()) ||
+	   (req.graphOnly && labelsPub_.getNumSubscribers()))
 	{
 		std::map<int, Transform> poses;
 		std::multimap<int, Link> constraints;
@@ -1714,7 +1716,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 
 		if(poses.size() && poses.size() != signatures.size())
 		{
-			ROS_ERROR("poses and signatures are not the same size!? %d vs %d", (int)poses.size(), (int)signatures.size());
+			ROS_WARN("poses and signatures are not the same size!? %d vs %d", (int)poses.size(), (int)signatures.size());
 		}
 
 		ros::Time now = ros::Time::now();
@@ -1733,7 +1735,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 			mapDataPub_.publish(msg);
 		}
 
-		if(!req.graphOnly)
+		if(!req.graphOnly && mapsManager_.hasSubscribers())
 		{
 			std::map<int, Transform> filteredPoses;
 			if(signatures.size())
@@ -1741,9 +1743,9 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 				filteredPoses = mapsManager_.updateMapCaches(
 						poses,
 						rtabmap_.getMemory(),
-						true,
-						true,
-						true,
+						false,
+						false,
+						false,
 						signatures);
 			}
 			else
diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp
index dfdbe812..98a6b5b9 100644
--- a/src/MapsManager.cpp
+++ b/src/MapsManager.cpp
@@ -94,6 +94,13 @@ void MapsManager::clear()
 	laserScanIncrement_ = 0;
 }
 
+bool MapsManager::hasSubscribers() const
+{
+	return  cloudMapPub_.getNumSubscribers() != 0 ||
+			projMapPub_.getNumSubscribers() != 0 ||
+			gridMapPub_.getNumSubscribers() != 0;
+}
+
 void MapsManager::setLaserScanParameters(
 		float maxRange,
 		float minAngle,
@@ -192,7 +199,10 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 					{
 						// Which data should we decompress?
 						cv::Mat image, depth, scan;
-						data.uncompressData(rgbDepthRequired||data.stereoCameraModel().isValid()?&image:0, rgbDepthRequired||depthRequired?&depth:0, scanRequired?&scan:0);
+						data.uncompressData(
+								(rgbDepthRequired||data.stereoCameraModel().isValid()) ? &image:0,
+								(rgbDepthRequired||depthRequired) ? &depth:0,
+								scanRequired?&scan:0);
 
 						pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
 						pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ;
diff --git a/src/MapsManager.h b/src/MapsManager.h
index cb86d038..93299825 100644
--- a/src/MapsManager.h
+++ b/src/MapsManager.h
@@ -29,6 +29,7 @@ public:
 	MapsManager();
 	virtual ~MapsManager();
 	void clear();
+	bool hasSubscribers() const;
 
 	std::map<int, rtabmap::Transform> getFilteredPoses(
 			const std::map<int, rtabmap::Transform> & poses);