Added "cancel_goal" service

src/CoreWrapper.cpp

@@ -375,6 +375,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	getProjMapSrv_ = nh.advertiseService("get_proj_map", &CoreWrapper::getProjMapCallback, this);
 	publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this);
 	setGoalSrv_ = nh.advertiseService("set_goal", &CoreWrapper::setGoalCallback, this);
+	cancelGoalSrv_ = nh.advertiseService("cancel_goal", &CoreWrapper::cancelGoalCallback, this);
 	setLabelSrv_ = nh.advertiseService("set_label", &CoreWrapper::setLabelCallback, this);
 	listLabelsSrv_ = nh.advertiseService("list_labels", &CoreWrapper::listLabelsCallback, this);
 #ifdef WITH_OCTOMAP
@@ -1849,6 +1850,23 @@ bool CoreWrapper::setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ro
 	return true;
 }
 
+bool CoreWrapper::cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)
+{
+	if(rtabmap_.getPath().size())
+	{
+		ROS_WARN("Goal cancelled!");
+		rtabmap_.clearPath();
+		currentMetricGoal_.setNull();
+		latestNodeWasReached_ = false;
+		if(mbClient_.isServerConnected())
+		{
+			mbClient_.cancelGoal();
+		}
+	}
+
+	return true;
+}
+
 bool CoreWrapper::setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res)
 {
 	if(rtabmap_.labelLocation(req.node_id, req.node_label))

src/CoreWrapper.h

@@ -197,6 +197,7 @@ private:
 	bool getGridMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
 	bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&);
 	bool setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res);
+	bool cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
 	bool setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res);
 	bool listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtabmap_ros::ListLabels::Response& res);
 #ifdef WITH_OCTOMAP
@@ -372,6 +373,7 @@ private:
 	ros::ServiceServer getGridMapSrv_;
 	ros::ServiceServer publishMapDataSrv_;
 	ros::ServiceServer setGoalSrv_;
+	ros::ServiceServer cancelGoalSrv_;
 	ros::ServiceServer setLabelSrv_;
 	ros::ServiceServer listLabelsSrv_;
 #ifdef WITH_OCTOMAP

src/GuiWrapper.cpp

@@ -53,6 +53,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap_ros/MsgConversion.h"
 #include "rtabmap_ros/GetMap.h"
+#include "rtabmap_ros/SetGoal.h"
 
 #include "PreferencesDialogROS.h"
 
@@ -361,6 +362,23 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
 				processRequestedMap(getMapSrv.response.data);
 			}
 		}
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdGoal)
+		{
+			rtabmap_ros::SetGoal setGoalSrv;
+			setGoalSrv.request.node_id = cmdEvent->getInt();
+			setGoalSrv.request.node_label = cmdEvent->getStr();
+			if(!ros::service::call("set_goal", setGoalSrv))
+			{
+				ROS_ERROR("Can't call \"set_goal\" service");
+			}
+		}
+		else if(cmd == rtabmap::RtabmapEventCmd::kCmdCancelGoal)
+		{
+			if(!ros::service::call("cancel_goal", emptySrv))
+			{
+				ROS_ERROR("Can't call \"cancel_goal\" service");
+			}
+		}
 		else
 		{
 			ROS_WARN("Not handled command (%d)...", cmd);

src/GuiWrapper.h

@@ -213,6 +213,7 @@ private:
 
 	message_filters::Subscriber<rtabmap_ros::Info> infoTopic_;
 	message_filters::Subscriber<rtabmap_ros::MapData> mapDataTopic_;
+	ros::Subscriber globalPathTopic_;
 
 	ros::Subscriber defaultSub_; // odometry only
 	std::vector<image_transport::SubscriberFilter*> imageSubs_;