i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37: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
+10 -7
View File
@@ -3618,14 +3618,17 @@ 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)
{ {