From b3207d64024c41a245ff7aeb15c2c751e72504b9 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Fri, 8 Jul 2016 19:44:44 -0400
Subject: [PATCH] PLanning: Detect if the pose is reachable from the current
 node before looking for the nearest one

---
 corelib/src/Rtabmap.cpp | 51 ++++++++++++++++++++++++-----------------
 1 file changed, 30 insertions(+), 21 deletions(-)

diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp
index d46ded00..6903eb76 100644
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -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));