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

republish node data: fixed order of which ones are published first

Browse Source
This commit is contained in:
matlabbe
2022-04-27 19:15:22 -04:00
parent 88ca603b99
commit 2c135f0e0f
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+3 -3
View File
@@ -4159,8 +4159,8 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
int id = rtabmap::graph::findNearestNode(nodesOnly, rtabmap_.getLastLocalizationPose()); int id = rtabmap::graph::findNearestNode(nodesOnly, rtabmap_.getLastLocalizationPose());
if(id>0) if(id>0)
{ {
std::map<int, int> ids = rtabmap_.getMemory()->getNeighborsId(id, 0, 0, false, false, true); std::map<int, int> ids = rtabmap_.getMemory()->getNeighborsId(id, 0, 0, true, false, true);
std::map<int, int> missingIds; std::multimap<int, int> missingIds;
for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter) for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{ {
if(nodesToRepublish_.find(iter->first) != nodesToRepublish_.end()) if(nodesToRepublish_.find(iter->first) != nodesToRepublish_.end())
@@ -4187,7 +4187,7 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
int loaded = 0; int loaded = 0;
std::stringstream stream; std::stringstream stream;
for(std::map<int, int>::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<maxNodesRepublished_; ++iter) for(std::multimap<int, int>::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<maxNodesRepublished_; ++iter)
{ {
signatures.insert(std::make_pair(iter->second, rtabmap_.getMemory()->getNodeData(iter->second, true, true, true, true))); signatures.insert(std::make_pair(iter->second, rtabmap_.getMemory()->getNodeData(iter->second, true, true, true, true)));
nodesToRepublish_.erase(iter->second); nodesToRepublish_.erase(iter->second);