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

planning: fixed a bug making the last pose published again even after stuck detected

Browse Source
This commit is contained in:
matlabbe
2015-09-27 12:01:02 -04:00
parent 88e67f047a
commit 7ea730bb0a
1 changed files with 10 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+9 -6
View File
@@ -3618,13 +3618,16 @@ void Rtabmap::updateGoalIndex()
_pathStuckIterations); _pathStuckIterations);
_pathStuckCount = 0; _pathStuckCount = 0;
_pathUnreachableNodes.insert(_pathGoalIndex); _pathUnreachableNodes.insert(_pathGoalIndex);
// select previous one // select previous reachable one
if(_pathGoalIndex == 0 || --_pathGoalIndex <= _pathCurrentIndex) while(_pathUnreachableNodes.find(_pathGoalIndex) != _pathUnreachableNodes.end())
{ {
// plan failed! if(_pathGoalIndex == 0 || --_pathGoalIndex <= _pathCurrentIndex)
UERROR("No upcoming nodes on the path are reachable! Aborting the plan..."); {
this->clearPath(-1); // plan failed!
return; UERROR("No upcoming nodes on the path are reachable! Aborting the plan...");
this->clearPath(-1);
return;
}
} }
} }
else if(!sameGoalIndex || !sameCurrentIndex) else if(!sameGoalIndex || !sameCurrentIndex)