diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h
index 675f5242..3e446f8c 100644
--- a/corelib/include/rtabmap/core/Rtabmap.h
+++ b/corelib/include/rtabmap/core/Rtabmap.h
@@ -137,6 +137,8 @@ public:
 	std::vector<std::pair<int, Transform> > getPathNextPoses() const;
 	std::vector<int> getPathNextNodes() const;
 	int getPathCurrentGoalId() const;
+	unsigned int getPathCurrentIndex() const {return _pathCurrentIndex;}
+	unsigned int getPathCurrentGoalIndex() const {return _pathGoalIndex;}
 	const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
 
 	std::map<int, Transform> getForwardWMPoses(int fromId, int maxNearestNeighbors, float radius, int maxDiffID) const;
diff --git a/guilib/include/rtabmap/gui/GraphViewer.h b/guilib/include/rtabmap/gui/GraphViewer.h
index 12f35062..0674cd5f 100644
--- a/guilib/include/rtabmap/gui/GraphViewer.h
+++ b/guilib/include/rtabmap/gui/GraphViewer.h
@@ -62,7 +62,7 @@ public:
 	void updatePosterior(const std::map<int, float> & posterior);
 	void updateLocalPath(const std::vector<int> & localPath);
 	void setGlobalPath(const std::vector<std::pair<int, Transform> > & globalPath);
-	void setCurrentGoalID(int id);
+	void setCurrentGoalID(int id, const Transform & pose = Transform());
 	void setLocalRadius(float radius);
 	void clearGraph();
 	void clearMap();
diff --git a/guilib/src/GraphViewer.cpp b/guilib/src/GraphViewer.cpp
index aea24dbf..c17764c0 100644
--- a/guilib/src/GraphViewer.cpp
+++ b/guilib/src/GraphViewer.cpp
@@ -129,6 +129,15 @@ public:
 		_poseB = poseB;
 	}
 
+	const Transform & getPoseA() const
+	{
+		return _poseA;
+	}
+	const Transform & getPoseB() const
+	{
+		return _poseB;
+	}
+
 	Link::Type linkType() const {return _type;}
 	bool isInterSession() const {return _interSession;}
 	int from() const {return _from;}
@@ -515,7 +524,7 @@ void GraphViewer::setGlobalPath(const std::vector<std::pair<int, Transform> > &
 	}
 }
 
-void GraphViewer::setCurrentGoalID(int id)
+void GraphViewer::setCurrentGoalID(int id, const Transform & pose)
 {
 	NodeItem * node = _nodeItems.value(id, 0);
 	if(node)
@@ -524,7 +533,18 @@ void GraphViewer::setCurrentGoalID(int id)
 	}
 	else
 	{
-		UWARN("Curent goal %d not found in the graph", id);
+		UWARN("Current goal %d not found in the graph", id);
+	}
+
+	if(!pose.isNull() && _globalPathLinkItems.size() && _globalPathLinkItems.contains(id))
+	{
+		// transform the global path in the goal referential
+		const LinkItem * oldPose = _globalPathLinkItems.value(id);
+		Transform t = pose * oldPose->getPoseA().inverse();
+		for(QMultiMap<int, LinkItem*>::iterator iter=_globalPathLinkItems.begin(); iter!=_globalPathLinkItems.end(); ++iter)
+		{
+			iter.value()->setPoses(t*iter.value()->getPoseA(), t*iter.value()->getPoseB());
+		}
 	}
 }
 
diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp
index b0721221..f4a2cdad 100644
--- a/guilib/src/MainWindow.cpp
+++ b/guilib/src/MainWindow.cpp
@@ -1311,7 +1311,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		// update current goal id
 		if(stat.currentGoalId() > 0)
 		{
-			_ui->graphicsView_graphView->setCurrentGoalID(stat.currentGoalId());
+			_ui->graphicsView_graphView->setCurrentGoalID(stat.currentGoalId(), uValue(stat.poses(), stat.currentGoalId(), Transform()));
 		}
 
 		UDEBUG("");
@@ -5799,6 +5799,9 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->actionDownload_all_clouds->setEnabled(false);
 		_ui->actionDownload_graph->setEnabled(false);
 		_ui->menuSelect_source->setEnabled(false);
+		_ui->actionLabel_current_location->setEnabled(false);
+		_ui->actionSend_goal->setEnabled(false);
+		_ui->actionCancel_goal->setEnabled(false);
 		_ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(false);
 		_ui->actionTrigger_a_new_map->setEnabled(false);
 		_ui->doubleSpinBox_stats_imgRate->setEnabled(true);
@@ -5845,6 +5848,9 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->actionDownload_all_clouds->setEnabled(true);
 		_ui->actionDownload_graph->setEnabled(true);
 		_ui->menuSelect_source->setEnabled(true);
+		_ui->actionLabel_current_location->setEnabled(true);
+		_ui->actionSend_goal->setEnabled(true);
+		_ui->actionCancel_goal->setEnabled(true);
 		_ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(true);
 		_ui->actionTrigger_a_new_map->setEnabled(true);
 		_ui->doubleSpinBox_stats_imgRate->setEnabled(true);
@@ -5880,6 +5886,9 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->actionDownload_all_clouds->setEnabled(false);
 		_ui->actionDownload_graph->setEnabled(false);
 		_ui->menuSelect_source->setEnabled(false);
+		_ui->actionLabel_current_location->setEnabled(true);
+		_ui->actionSend_goal->setEnabled(true);
+		_ui->actionCancel_goal->setEnabled(true);
 		_ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(false);
 		_ui->actionTrigger_a_new_map->setEnabled(true);
 		_ui->doubleSpinBox_stats_imgRate->setEnabled(true);
@@ -5971,6 +5980,9 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->actionDownload_all_clouds->setEnabled(true);
 		_ui->actionDownload_graph->setEnabled(true);
 		_ui->actionTrigger_a_new_map->setEnabled(true);
+		_ui->actionLabel_current_location->setEnabled(true);
+		_ui->actionSend_goal->setEnabled(true);
+		_ui->actionCancel_goal->setEnabled(true);
 		_ui->statusbar->showMessage(tr("Monitoring..."));
 		_state = newState;
 		_elapsedTime->start();
@@ -5990,6 +6002,9 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->actionDownload_all_clouds->setEnabled(true);
 		_ui->actionDownload_graph->setEnabled(true);
 		_ui->actionTrigger_a_new_map->setEnabled(true);
+		_ui->actionLabel_current_location->setEnabled(true);
+		_ui->actionSend_goal->setEnabled(true);
+		_ui->actionCancel_goal->setEnabled(true);
 		_ui->statusbar->showMessage(tr("Monitoring paused..."));
 		_state = newState;
 		_oneSecondTimer->stop();
diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui
index f87bf20d..ff2ffa94 100644
--- a/guilib/src/ui/mainWindow.ui
+++ b/guilib/src/ui/mainWindow.ui
@@ -72,9 +72,6 @@
      <addaction name="actionGenerate_map"/>
      <addaction name="menuExport_poses"/>
      <addaction name="actionPrint_loop_closure_IDs_to_console"/>
-     <addaction name="separator"/>
-     <addaction name="actionSend_goal"/>
-     <addaction name="actionCancel_goal"/>
     </widget>
     <addaction name="actionDownload_all_clouds"/>
     <addaction name="actionDownload_graph"/>
@@ -214,6 +211,8 @@
     <addaction name="actionTrigger_a_new_map"/>
     <addaction name="separator"/>
     <addaction name="actionLabel_current_location"/>
+    <addaction name="actionSend_goal"/>
+    <addaction name="actionCancel_goal"/>
    </widget>
    <widget class="QMenu" name="menuWindow">
     <property name="title">