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

Reset stuck when moving toward the goal

Browse Source
This commit is contained in:
Mathieu Labbe
2016-07-11 17:40:34 -04:00
parent 38c3e3600d
commit c24079884d
1 changed files with 6 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
corelib/src/Rtabmap.cpp
View File

@@ -3889,17 +3889,17 @@ void Rtabmap::updateGoalIndex()
distanceToCurrentGoal = currentPose.getDistanceSquared(iter->second);
}
}
if(distanceToCurrentGoal > 0.0f)
{
if(_pathStuckDistance > 0.0f &&
distanceToCurrentGoal >= _pathStuckDistance)
if(distanceToCurrentGoal >= _pathStuckDistance)
{
// we are not approaching the goal
isStuck = true;
}
else
{
_pathStuckDistance = distanceToCurrentGoal;
if(_pathStuckDistance == 0.0f)
{
_pathStuckDistance = distanceToCurrentGoal;
}
}
}
else