i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 08:47:45 +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
+4 -1
View File
@@ -3618,7 +3618,9 @@ void Rtabmap::updateGoalIndex()
_pathStuckIterations); _pathStuckIterations);
_pathStuckCount = 0; _pathStuckCount = 0;
_pathUnreachableNodes.insert(_pathGoalIndex); _pathUnreachableNodes.insert(_pathGoalIndex);
// select previous one // select previous reachable one
while(_pathUnreachableNodes.find(_pathGoalIndex) != _pathUnreachableNodes.end())
{
if(_pathGoalIndex == 0 || --_pathGoalIndex <= _pathCurrentIndex) if(_pathGoalIndex == 0 || --_pathGoalIndex <= _pathCurrentIndex)
{ {
// plan failed! // plan failed!
@@ -3627,6 +3629,7 @@ void Rtabmap::updateGoalIndex()
return; return;
} }
} }
}
else if(!sameGoalIndex || !sameCurrentIndex) else if(!sameGoalIndex || !sameCurrentIndex)
{ {
_pathStuckCount = 0; _pathStuckCount = 0;