From b44b8ca73204a91fae754a938f895a5c59a5a814 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Wed, 25 Nov 2015 15:46:56 -0500
Subject: [PATCH] fixed last goal not reached in localization mode

---
 CMakeLists.txt      | 2 +-
 src/CoreWrapper.cpp | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index c74742dd..60878203 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,7 +17,7 @@ find_package(octomap_ros)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.10.11 REQUIRED)
+find_package(RTABMap 0.10.12 REQUIRED)
 
 find_package(OpenCV REQUIRED)
 
diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp
index eee467f4..84341e19 100644
--- a/src/CoreWrapper.cpp
+++ b/src/CoreWrapper.cpp
@@ -1291,7 +1291,7 @@ void CoreWrapper::process(
 						if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size())
 						{
 							if(latestNodeWasReached_ ||
-							   rtabmap_.getLocalOptimizedPoses().rbegin()->second.getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius() ||
+							   rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius() ||
 							   rtabmap_.getPathTransformToGoal().getNorm() < rtabmap_.getGoalReachedRadius())
 							{
 								latestNodeWasReached_ = true;
@@ -1411,7 +1411,7 @@ void CoreWrapper::goalCommonCallback(
 				// Adjust the target pose relative to last node
 				if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size())
 				{
-					if(rtabmap_.getLocalOptimizedPoses().rbegin()->second.getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius())
+					if(rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius())
 					{
 						latestNodeWasReached_ = true;
 						currentMetricGoal_ *= rtabmap_.getPathTransformToGoal();