From 9a201f5e2e55c47b2e1103f72bad6a74a0b3e6b6 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Tue, 22 Sep 2015 15:47:10 -0400
Subject: [PATCH] Fixed goal reached topic not sent when move_base has finished

---
 src/CoreWrapper.cpp | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp
index 44cc96c3..0614a5b1 100644
--- a/src/CoreWrapper.cpp
+++ b/src/CoreWrapper.cpp
@@ -1887,6 +1887,12 @@ bool CoreWrapper::cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Em
 		{
 			mbClient_.cancelGoal();
 		}
+		if(goalReachedPub_.getNumSubscribers())
+		{
+			std_msgs::Bool result;
+			result.data = false;
+			goalReachedPub_.publish(result);
+		}
 	}
 
 	return true;
@@ -2126,7 +2132,7 @@ void CoreWrapper::goalDoneCb(const actionlib::SimpleClientGoalState& state,
 			ROS_ERROR("Planning: move_base failed for some reason. Aborting the plan...");
 		}
 
-		if(!ignore && !goalReachedPub_.getNumSubscribers())
+		if(!ignore && goalReachedPub_.getNumSubscribers())
 		{
 			std_msgs::Bool result;
 			result.data = state == actionlib::SimpleClientGoalState::SUCCEEDED;