MapsManager: Make sure maps are cleanup when there is no subscriber. Added rosinfo for memory released when maps are cleared.

src/CoreWrapper.cpp

@@ -2900,6 +2900,8 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 {
 	NODELET_INFO("rtabmap: Publishing map...");
 
+	ros::Time now = ros::Time::now();
+
 	if(mapDataPub_.getNumSubscribers() ||
 	   (!req.graphOnly && mapsManager_.hasSubscribers()) ||
 	   (req.graphOnly && (labelsPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers() || mapPathPub_.getNumSubscribers())))
@@ -2919,7 +2921,6 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 				!req.graphOnly,
 				!req.graphOnly);
 
-		ros::Time now = ros::Time::now();
 		if(mapDataPub_.getNumSubscribers())
 		{
 			rtabmap_ros::MapDataPtr msg(new rtabmap_ros::MapData);
@@ -2997,7 +2998,9 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 			landmarksPub_.publish(msg);
 		}
 
-		if(!req.graphOnly && mapsManager_.hasSubscribers())
+		if(!req.graphOnly)
+		{
+			if(mapsManager_.hasSubscribers())
 			{
 				std::map<int, Transform> filteredPoses(poses.lower_bound(1), poses.end());
 				if(maxMappingNodes_ > 0 && filteredPoses.size()>1)
@@ -3028,6 +3031,12 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 				}
 				mapsManager_.publishMaps(filteredPoses, now, mapFrameId_);
 			}
+			else
+			{
+				// this will cleanup the cache if there are no subscribers
+				mapsManager_.publishMaps(std::map<int, Transform>(), now, mapFrameId_);
+			}
+		}
 
 		bool pubPath = mapPathPub_.getNumSubscribers();
 		if(pubLabels || pubPath)
@@ -3133,6 +3142,11 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 	else
 	{
 		UWARN("No subscribers, don't need to publish!");
+		if(!req.graphOnly)
+		{
+			// this will cleanup the cache if there are no subscribers
+			mapsManager_.publishMaps(std::map<int, Transform>(), now, mapFrameId_);
+		}
 	}
 
 	return true;

src/MapsManager.cpp

@@ -1149,6 +1149,26 @@ void MapsManager::publishMaps(
 	}
 	else if(mapCacheCleanup_)
 	{
+		if(!groundClouds_.empty() || !obstacleClouds_.empty())
+		{
+			size_t totalBytes = 0;
+			for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=groundClouds_.begin();iter!=groundClouds_.end();++iter)
+			{
+				totalBytes += sizeof(int) + iter->second->points.size()*sizeof(pcl::PointXYZRGB);
+			}
+			for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=obstacleClouds_.begin();iter!=obstacleClouds_.end();++iter)
+			{
+				totalBytes += sizeof(int) + iter->second->points.size()*sizeof(pcl::PointXYZRGB);
+			}
+			totalBytes += (assembledGround_->size() + assembledObstacles_->size()) *sizeof(pcl::PointXYZRGB);
+			totalBytes += (assembledGroundPoses_.size() + assembledObstaclePoses_.size()) * 13*sizeof(float);
+			totalBytes += assembledGroundIndex_.indexedFeatures()*assembledGroundIndex_.featuresDim() * sizeof(float);
+			totalBytes += assembledObstacleIndex_.indexedFeatures()*assembledObstacleIndex_.featuresDim() * sizeof(float);
+			ROS_INFO("MapsManager: cleanup point clouds (%ld points, %ld cached clouds, ~%ld MB)...",
+					assembledGround_->size()+assembledObstacles_->size(),
+					groundClouds_.size()+obstacleClouds_.size(),
+					totalBytes/1048576);
+		}
 		assembledGround_->clear();
 		assembledObstacles_->clear();
 		assembledGroundPoses_.clear();
@@ -1325,6 +1345,12 @@ void MapsManager::publishMaps(
 		octoMapEmptySpace_.getNumSubscribers() == 0 &&
 		octoMapProj_.getNumSubscribers() == 0)
 	{
+		if(octomap_->octree()->getNumLeafNodes()>0)
+		{
+			ROS_INFO("MapsManager: cleanup octomap (%ld leaf nodes, ~%ld MB)...",
+					octomap_->octree()->getNumLeafNodes(),
+					octomap_->octree()->memoryUsage()/1048576);
+		}
 		octomap_->clear();
 	}
 
@@ -1490,6 +1516,19 @@ void MapsManager::publishMaps(
 
 	if(!this->hasSubscribers() && mapCacheCleanup_)
 	{
+		if(!gridMaps_.empty())
+		{
+			size_t totalBytes = 0;
+			for(std::map<int, std::pair< std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=gridMaps_.begin(); iter!=gridMaps_.end(); ++iter)
+			{
+				totalBytes+= sizeof(int)+
+						iter->second.first.first.total()*iter->second.first.first.elemSize() +
+						iter->second.first.second.total()*iter->second.first.second.elemSize() +
+						iter->second.second.total()*iter->second.second.elemSize();
+			}
+			totalBytes += gridMapsViewpoints_.size()*sizeof(int) + gridMapsViewpoints_.size() * sizeof(cv::Point3f);
+			ROS_INFO("MapsManager: cleanup %ld grid maps (~%ld MB)...", gridMaps_.size(), totalBytes/1048576);
+		}
 		gridMaps_.clear();
 		gridMapsViewpoints_.clear();
 	}