From 0b5d6c84b12497c43f09d7b35ba63ade0e51d166 Mon Sep 17 00:00:00 2001
From: Mathieu Labbe <matlabbe@gmail.com>
Date: Mon, 4 May 2015 18:35:29 -0400
Subject: [PATCH] Aborting path planned on "last node not found" error

---
 corelib/src/Rtabmap.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp
index ed9766de..0ca383b2 100644
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -3099,7 +3099,8 @@ void Rtabmap::updateGoalIndex()
 		if(_memory->getLastWorkingSignature() == 0 ||
 		   !uContains(_optimizedPoses, _memory->getLastWorkingSignature()->id()))
 		{
-			UERROR("Last node is null in memory or not in optimized poses");
+			UERROR("Last node is null in memory or not in optimized poses. Aborting the plan...");
+			this->clearPath();
 			return;
 		}