i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-05 01:27:46 +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
corelib/src/Rtabmap.cpp
+20 -11
View File
@@ -3527,17 +3527,6 @@ bool Rtabmap::computePath(const Transform & targetPose)
} }
UINFO("Time getting links = %fs", timer.ticks()); UINFO("Time getting links = %fs", timer.ticks());
int nearestId = rtabmap::graph::findNearestNode(nodes, targetPose);
UINFO("Nearest node found=%d ,%fs", nearestId, timer.ticks());
if(nearestId > 0)
{
if(_localRadius != 0.0f && targetPose.getDistance(nodes.at(nearestId)) > _localRadius)
{
UWARN("Cannot plan farther than %f m from the graph! (distance=%f m from node %d)",
_localRadius, targetPose.getDistance(nodes.at(nearestId)), nearestId);
}
else
{
int currentNode = 0; int currentNode = 0;
if(_memory->isIncremental()) if(_memory->isIncremental())
{ {
@@ -3558,6 +3547,26 @@ bool Rtabmap::computePath(const Transform & targetPose)
currentNode = graph::findNearestNode(_optimizedPoses, _lastLocalizationPose); 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)
{
if(_localRadius != 0.0f && targetPose.getDistance(nodes.at(nearestId)) > _localRadius)
{
UWARN("Cannot plan farther than %f m from the graph! (distance=%f m from node %d)",
_localRadius, targetPose.getDistance(nodes.at(nearestId)), nearestId);
}
else
{
UINFO("Computing path from location %d to %d", currentNode, nearestId); UINFO("Computing path from location %d to %d", currentNode, nearestId);
UTimer timer; UTimer timer;
_path = uListToVector(rtabmap::graph::computePath(nodes, links, currentNode, nearestId)); _path = uListToVector(rtabmap::graph::computePath(nodes, links, currentNode, nearestId));