i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 16:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
matlabbe
2020-08-19 14:14:56 -04:00
parent cbad3e37b6
commit c5c0e0807d
2 changed files with 74 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+35 -21
View File
@@ -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(maxMappingNodes_ > 0 && filteredPoses.size()>1)
if(mapsManager_.hasSubscribers())
{
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> filteredPoses(poses.lower_bound(1), poses.end());
if(maxMappingNodes_ > 0 && filteredPoses.size()>1)
{
std::map<int, Transform>::iterator pter = filteredPoses.find(*iter);
if(pter != filteredPoses.end())
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)
{
nearestPoses.insert(*pter);
std::map<int, Transform>::iterator pter = filteredPoses.find(*iter);
if(pter != filteredPoses.end())
{
nearestPoses.insert(*pter);
}
}
}
}
if(signatures.size())
{
filteredPoses = mapsManager_.updateMapCaches(
filteredPoses,
rtabmap_.getMemory(),
false,
false,
signatures);
if(signatures.size())
{
filteredPoses = mapsManager_.updateMapCaches(
filteredPoses,
rtabmap_.getMemory(),
false,
false,
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
+39
View File
@@ -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();
}