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

Fixing map_cache_loaded_on_init loading anyway on first update (#1352)

Browse Source
This commit is contained in:
matlabbe
2025-09-03 16:59:24 -07:00
committed by GitHub
parent e9bed4a6b0
commit deee14e1d2
1 changed files with 22 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_util/src/MapsManager.cpp
+22 -15
View File
@@ -551,32 +551,39 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
filteredPoses.erase(0); filteredPoses.erase(0);
} }
bool fullUpdateNeeded = true;
#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>=23)
fullUpdateNeeded = (updateGrid && occupancyGrid_->fullUpdateNeeded(filteredPoses))
#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
|| (updateOctomap && octomap_->fullUpdateNeeded(filteredPoses))
#endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
|| (updateElevation && elevationMap_->fullUpdateNeeded(filteredPoses))
#endif
;
if(fullUpdateNeeded) {
ROS_INFO("Full occupancy grid map update needed");
}
else {
ROS_DEBUG("Full occupancy grid map update not needed");
}
#endif
bool longUpdate = false; bool longUpdate = false;
UTimer longUpdateTimer; UTimer longUpdateTimer;
if(filteredPoses.size() > 20) if(fullUpdateNeeded && filteredPoses.size() > 20 & localMaps_.size() < 5)
{ {
if(updateGridCache && localMaps_.size() < 5) ROS_WARN("Many occupancy grids should be loaded (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-localMaps_.size()));
{ longUpdate = true;
ROS_WARN("Many occupancy grids should be loaded (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-localMaps_.size()));
longUpdate = true;
}
#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
if(updateOctomap && octomap_->addedNodes().size() < 5)
{
ROS_WARN("Many clouds should be added to octomap (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-octomap_->addedNodes().size()));
longUpdate = true;
}
#endif
} }
bool occupancySavedInDB = memory && uStrNumCmp(memory->getDatabaseVersion(), "0.11.10")>=0?true:false; bool occupancySavedInDB = memory && uStrNumCmp(memory->getDatabaseVersion(), "0.11.10")>=0?true:false;
for(std::map<int, rtabmap::Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter) for(std::map<int, rtabmap::Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter)
{ {
if(!iter->second.isNull()) if(!iter->second.isNull())
{ {
rtabmap::SensorData data; rtabmap::SensorData data;
if(updateGridCache && (iter->first == 0 || !uContains(localMaps_.localGrids(), iter->first))) if(iter->first == 0 || (fullUpdateNeeded && !uContains(localMaps_.localGrids(), iter->first)))
{ {
ROS_DEBUG("Data required for %d", iter->first); ROS_DEBUG("Data required for %d", iter->first);
std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first); std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);