i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +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

17
corelib/src/Rtabmap.cpp
View File

@@ -3618,14 +3618,17 @@ void Rtabmap::updateGoalIndex()
_pathStuckIterations);
_pathStuckCount = 0;
_pathUnreachableNodes.insert(_pathGoalIndex);
// select previous one
if(_pathGoalIndex == 0 || --_pathGoalIndex <= _pathCurrentIndex)
// select previous reachable one
while(_pathUnreachableNodes.find(_pathGoalIndex) != _pathUnreachableNodes.end())
{
// plan failed!
UERROR("No upcoming nodes on the path are reachable! Aborting the plan...");
this->clearPath(-1);
return;
}
if(_pathGoalIndex == 0 || --_pathGoalIndex <= _pathCurrentIndex)
{
// plan failed!
UERROR("No upcoming nodes on the path are reachable! Aborting the plan...");
this->clearPath(-1);
return;
}
}
}
else if(!sameGoalIndex || !sameCurrentIndex)
{