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: Detect if the pose is reachable from the current node before looking for the nearest one

Browse Source
This commit is contained in:
matlabbe
2016-07-08 19:44:44 -04:00
parent 61f6e5ff79
commit b3207d6402
1 changed files with 30 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
corelib/src/Rtabmap.cpp
View File

@@ -3527,7 +3527,36 @@ bool Rtabmap::computePath(const Transform & targetPose)
}
UINFO("Time getting links = %fs", timer.ticks());
int nearestId = rtabmap::graph::findNearestNode(nodes, targetPose);
int currentNode = 0;
if(_memory->isIncremental())
{
if(!_memory->getLastWorkingSignature())
{
UWARN("Working memory is empty... cannot compute a path");
return false;
}
currentNode = _memory->getLastWorkingSignature()->id();
}
else
{
if(_lastLocalizationPose.isNull() || _optimizedPoses.size() == 0)
{
UWARN("Last localization pose is null... cannot compute a path");
return false;
}
currentNode = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose);
}
int nearestId;
if(!_lastLocalizationPose.isNull() && _lastLocalizationPose.getDistance(targetPose) < _localRadius)
{
// target can be reached from the current node
nearestId = currentNode;
}
else
{
nearestId = rtabmap::graph::findNearestNode(nodes, targetPose);
}
UINFO("Nearest node found=%d ,%fs", nearestId, timer.ticks());
if(nearestId > 0)
{
@@ -3538,26 +3567,6 @@ bool Rtabmap::computePath(const Transform & targetPose)
}
else
{
int currentNode = 0;
if(_memory->isIncremental())
{
if(!_memory->getLastWorkingSignature())
{
UWARN("Working memory is empty... cannot compute a path");
return false;
}
currentNode = _memory->getLastWorkingSignature()->id();
}
else
{
if(_lastLocalizationPose.isNull() || _optimizedPoses.size() == 0)
{
UWARN("Last localization pose is null... cannot compute a path");
return false;
}
currentNode = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose);
}
UINFO("Computing path from location %d to %d", currentNode, nearestId);
UTimer timer;
_path = uListToVector(rtabmap::graph::computePath(nodes, links, currentNode, nearestId));