Added "Cancel goal" action

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -76,7 +76,8 @@ public:
 			kCmdPublishTOROGraphLocal, // params: optimized
 			kCmdTriggerNewMap,
 			kCmdPause,
-			kCmdGoal}; // params: label or location ID
+			kCmdGoal, // params: label or location ID
+			kCmdCancelGoal};
 public:
 	RtabmapEventCmd(Cmd cmd, const std::string & strValue = "", int intValue = 0, const ParametersMap & parameters = ParametersMap()) :
 			UEvent(0),

corelib/include/rtabmap/core/RtabmapThread.h

@@ -74,7 +74,8 @@ public:
 		kStatePublishingTOROGraphGlobal,
 		kStateTriggeringMap,
 		kStateAddingUserData,
-		kStateSettingGoal
+		kStateSettingGoal,
+		kStateCancellingGoal
 	};
 
 public:

corelib/src/RtabmapThread.cpp

@@ -286,6 +286,9 @@ void RtabmapThread::mainLoop()
 		}
 		this->post(new RtabmapGlobalPathEvent(id, _rtabmap->getPath()));
 		break;
+	case kStateCancellingGoal:
+		_rtabmap->clearPath();
+		break;
 	default:
 		UFATAL("Invalid state !?!?");
 		break;
@@ -492,6 +495,11 @@ void RtabmapThread::handleEvent(UEvent* event)
 			param.insert(ParametersPair("goal_id", uNumber2Str(rtabmapEvent->getInt())));
 			pushNewState(kStateSettingGoal, param);
 		}
+		else if(cmd == RtabmapEventCmd::kCmdCancelGoal)
+		{
+			ULOGGER_DEBUG("CMD_CANCEL_GOAL");
+			pushNewState(kStateCancellingGoal);
+		}
 		else
 		{
 			UWARN("Cmd %d unknown!", cmd);

guilib/include/rtabmap/gui/MainWindow.h

@@ -156,6 +156,7 @@ private slots:
 	void dumpTheMemory();
 	void dumpThePrediction();
 	void sendGoal();
+	void cancelGoal();
 	void downloadAllClouds();
 	void downloadPoseGraph();
 	void clearTheCache();

guilib/src/MainWindow.cpp

@@ -294,6 +294,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(_ui->actionDump_the_memory, SIGNAL(triggered()), this, SLOT(dumpTheMemory()));
 	connect(_ui->actionDump_the_prediction_matrix, SIGNAL(triggered()), this, SLOT(dumpThePrediction()));
 	connect(_ui->actionSend_goal, SIGNAL(triggered()), this, SLOT(sendGoal()));
+	connect(_ui->actionCancel_goal, SIGNAL(triggered()), this, SLOT(cancelGoal()));
 	connect(_ui->actionClear_cache, SIGNAL(triggered()), this, SLOT(clearTheCache()));
 	connect(_ui->actionAbout, SIGNAL(triggered()), _aboutDialog , SLOT(exec()));
 	connect(_ui->actionPrint_loop_closure_IDs_to_console, SIGNAL(triggered()), this, SLOT(printLoopClosureIds()));
@@ -3870,6 +3871,12 @@ void MainWindow::sendGoal()
 	}
 }
 
+void MainWindow::cancelGoal()
+{
+	UINFO("Cancelling goal...");
+	this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdCancelGoal));
+}
+
 void MainWindow::downloadAllClouds()
 {
 	QStringList items;

guilib/src/ui/mainWindow.ui

@@ -27,7 +27,7 @@
      <x>0</x>
      <y>0</y>
      <width>1012</width>
-     <height>22</height>
+     <height>25</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -64,6 +64,7 @@
      <addaction name="actionExport_poses_txt"/>
      <addaction name="actionPrint_loop_closure_IDs_to_console"/>
      <addaction name="actionSend_goal"/>
+     <addaction name="actionCancel_goal"/>
     </widget>
     <addaction name="actionDownload_all_clouds"/>
     <addaction name="actionDownload_graph"/>
@@ -1209,6 +1210,11 @@
     <string>Export poses (*.txt)...</string>
    </property>
   </action>
+  <action name="actionCancel_goal">
+   <property name="text">
+    <string>Cancel goal</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>