i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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); distanceToCurrentGoal = currentPose.getDistanceSquared(iter->second);
} }
} }
if(distanceToCurrentGoal > 0.0f) if(distanceToCurrentGoal > 0.0f)
{ {
if(_pathStuckDistance > 0.0f && if(distanceToCurrentGoal >= _pathStuckDistance)
distanceToCurrentGoal >= _pathStuckDistance)
{ {
// we are not approaching the goal // we are not approaching the goal
isStuck = true; isStuck = true;
} if(_pathStuckDistance == 0.0f)
else {
{ _pathStuckDistance = distanceToCurrentGoal;
_pathStuckDistance = distanceToCurrentGoal; }
} }
} }
else else