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,36 +2998,44 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 			landmarksPub_.publish(msg);
 		}
 
-		if(!req.graphOnly && mapsManager_.hasSubscribers())
+		if(!req.graphOnly)
 		{
-			std::map<int, Transform> filteredPoses(poses.lower_bound(1), poses.end());
+			if(mapsManager_.hasSubscribers())
-			if(maxMappingNodes_ > 0 && filteredPoses.size()>1)
 			{
-				std::map<int, Transform> nearestPoses;
+				std::map<int, Transform> filteredPoses(poses.lower_bound(1), poses.end());
-				std::vector<int> nodes = graph::findNearestNodes(filteredPoses, filteredPoses.rbegin()->second, maxMappingNodes_);
+				if(maxMappingNodes_ > 0 && filteredPoses.size()>1)
-				for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
 				{
-					std::map<int, Transform>::iterator pter = filteredPoses.find(*iter);
+					std::map<int, Transform> nearestPoses;
-					if(pter != filteredPoses.end())
+					std::vector<int> nodes = graph::findNearestNodes(filteredPoses, filteredPoses.rbegin()->second, maxMappingNodes_);
+					for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
 					{
-						nearestPoses.insert(*pter);
+						std::map<int, Transform>::iterator pter = filteredPoses.find(*iter);
+						if(pter != filteredPoses.end())
+						{
+							nearestPoses.insert(*pter);
+						}
 					}
 				}
-			}
+				if(signatures.size())
-			if(signatures.size())
+				{
-			{
+					filteredPoses = mapsManager_.updateMapCaches(
-				filteredPoses = mapsManager_.updateMapCaches(
+							filteredPoses,
-						filteredPoses,
+							rtabmap_.getMemory(),
-						rtabmap_.getMemory(),
+							false,
-						false,
+							false,
-						false,
+							signatures);
-						signatures);
+				}
+				else
+				{
+					filteredPoses = mapsManager_.getFilteredPoses(filteredPoses);
+				}
+				mapsManager_.publishMaps(filteredPoses, now, mapFrameId_);
 			}
 			else
 			{
-				filteredPoses = mapsManager_.getFilteredPoses(filteredPoses);
+				// this will cleanup the cache if there are no subscribers
+				mapsManager_.publishMaps(std::map<int, Transform>(), now, mapFrameId_);
 			}
-			mapsManager_.publishMaps(filteredPoses, now, mapFrameId_);
 		}
 
 		bool pubPath = mapPathPub_.getNumSubscribers();
@@ -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();
 	}