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

Planning stuck detection updated: now using distance to goal

Browse Source
This commit is contained in:
matlabbe
2016-07-11 17:16:11 -04:00
parent 09795678ef
commit 38c3e3600d
2 changed files with 49 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -246,6 +246,7 @@ private:
unsigned int _pathGoalIndex; unsigned int _pathGoalIndex;
Transform _pathTransformToGoal; Transform _pathTransformToGoal;
int _pathStuckCount; int _pathStuckCount;
float _pathStuckDistance;
}; };

53
corelib/src/Rtabmap.cpp
View File

@@ -128,7 +128,8 @@ Rtabmap::Rtabmap() :
_pathCurrentIndex(0), _pathCurrentIndex(0),
_pathGoalIndex(0), _pathGoalIndex(0),
_pathTransformToGoal(Transform::getIdentity()), _pathTransformToGoal(Transform::getIdentity()),
_pathStuckCount(0) _pathStuckCount(0),
_pathStuckDistance(0.0f)
{ {
} }
@@ -1979,6 +1980,10 @@ bool Rtabmap::process(
nearestId, transform.getNorm(), _proximityFilteringRadius); nearestId, transform.getNorm(), _proximityFilteringRadius);
} }
} }
else
{
UWARN("Local scan matching rejected: %s", info.rejectedMsg.c_str());
}
} }
} }
} }
@@ -3374,6 +3379,7 @@ void Rtabmap::clearPath(int status)
_pathTransformToGoal.setIdentity(); _pathTransformToGoal.setIdentity();
_pathUnreachableNodes.clear(); _pathUnreachableNodes.clear();
_pathStuckCount = 0; _pathStuckCount = 0;
_pathStuckDistance = 0.0f;
if(_memory) if(_memory)
{ {
_memory->removeAllVirtualLinks(); _memory->removeAllVirtualLinks();
@@ -3866,15 +3872,51 @@ void Rtabmap::updateGoalIndex()
sameCurrentIndex = true; sameCurrentIndex = true;
} }
if(sameGoalIndex && sameCurrentIndex && bool isStuck = false;
_pathStuckIterations > 0 && if(sameGoalIndex && sameCurrentIndex && _pathStuckIterations>0)
++_pathStuckCount > _pathStuckIterations) {
float distanceToCurrentGoal = 0.0f;
std::map<int, Transform>::iterator iter = _optimizedPoses.find(_path[_pathGoalIndex].first);
if(iter != _optimizedPoses.end())
{
if(_pathGoalIndex == _pathCurrentIndex &&
_pathGoalIndex == _path.size()-1)
{
distanceToCurrentGoal = currentPose.getDistanceSquared(iter->second*_pathTransformToGoal);
}
else
{
distanceToCurrentGoal = currentPose.getDistanceSquared(iter->second);
}
}
if(distanceToCurrentGoal > 0.0f)
{
if(_pathStuckDistance > 0.0f &&
distanceToCurrentGoal >= _pathStuckDistance)
{
// we are not approaching the goal
isStuck = true;
}
else
{
_pathStuckDistance = distanceToCurrentGoal;
}
}
else
{
// no nodes available, cannot plan
isStuck = true;
}
}
if(isStuck && ++_pathStuckCount > _pathStuckIterations)
{ {
UWARN("Current goal %d not reached since %d iterations (\"RGBD/PlanStuckIterations\"=%d), mark that node as unreachable.", UWARN("Current goal %d not reached since %d iterations (\"RGBD/PlanStuckIterations\"=%d), mark that node as unreachable.",
_path[_pathGoalIndex].first, _path[_pathGoalIndex].first,
_pathStuckCount, _pathStuckCount,
_pathStuckIterations); _pathStuckIterations);
_pathStuckCount = 0; _pathStuckCount = 0;
_pathStuckDistance = 0.0;
_pathUnreachableNodes.insert(_pathGoalIndex); _pathUnreachableNodes.insert(_pathGoalIndex);
// select previous reachable one // select previous reachable one
while(_pathUnreachableNodes.find(_pathGoalIndex) != _pathUnreachableNodes.end()) while(_pathUnreachableNodes.find(_pathGoalIndex) != _pathUnreachableNodes.end())
@@ -3888,9 +3930,10 @@ void Rtabmap::updateGoalIndex()
} }
} }
} }
else if(!sameGoalIndex || !sameCurrentIndex) else if(!isStuck)
{ {
_pathStuckCount = 0; _pathStuckCount = 0;
_pathStuckDistance = 0.0;
} }
} }
} }