From d81a9cef23f3f9e0c830bf89b849530d6a3c1b0a Mon Sep 17 00:00:00 2001
From: Mathieu Labbe <matlabbe@gmail.com>
Date: Fri, 6 Feb 2015 17:39:27 -0500
Subject: [PATCH] updated warning when goal is already reached

---
 src/CoreWrapper.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp
index 83a866ff..59ee61e2 100644
--- a/src/CoreWrapper.cpp
+++ b/src/CoreWrapper.cpp
@@ -1052,12 +1052,12 @@ void CoreWrapper::goalCommonCallback(const std::list<std::pair<int, Transform> >
 	}
 	else
 	{
-		ROS_WARN("Planning: Cannot compute a path!");
+		ROS_WARN("Planning: Goal already reached (RGBD/GoalReachedRadius=%fm).", rtabmap_.getGoalReachedRadius());
 		rtabmap_.clearPath();
 		if(goalReachedPub_.getNumSubscribers())
 		{
 			std_msgs::Bool result;
-			result.data = false;
+			result.data = true;
 			goalReachedPub_.publish(result);
 		}
 	}