navigator: stop events

source/armarx/navigation/components/Navigator/Navigator.cpp

@@ -362,11 +362,23 @@ namespace armarx::navigation::components
     void
     components::Navigator::stopAll(const Ice::Current&)
     {
+        ARMARX_IMPORTANT << "stopAll()";
+
         ARMARX_TRACE;
         for (auto& [_, navigator] : navigators)
         {
             navigator.stop();
         }
+
+        const core::GoalReachedEvent evt{{.timestamp = scene.timeServer->now()},
+                                         {.pose = core::Pose(scene.robot->getGlobalPose())}};
+
+        for (auto& [callerId, memoryPublisher] : memoryPublishers)
+        {
+            // TODO use different event, e.g. generic stopped event
+            // TODO check if navigator was stopped
+            memoryPublisher->goalReached(evt);
+        }
     }
 
     bool

source/armarx/navigation/server/Navigator.cpp

@@ -685,6 +685,8 @@ namespace armarx::navigation::server
         ARMARX_TRACE;
         ARMARX_CHECK_NOT_EMPTY(waypoints);
 
+        ARMARX_INFO << "Request to move from " << config.scene->robot->getGlobalPose() << " to " << waypoints.back().matrix();
+
         // first we check if we are already at the goal position
         goalReachedMonitor =
             GoalReachedMonitor(waypoints.back(), *config.scene, GoalReachedMonitorConfig());
@@ -692,6 +694,11 @@ namespace armarx::navigation::server
         if (goalReachedMonitor->goalReached())
         {
             ARMARX_INFO << "Already at goal position. Robot won't move.";
+
+            srv.publisher->goalReached(
+                core::GoalReachedEvent{{.timestamp = config.scene->timeServer->now()},
+                                       {.pose = core::Pose(config.scene->robot->getGlobalPose())}});
+
             return;
         }
 
@@ -1032,6 +1039,7 @@ namespace armarx::navigation::server
         ARMARX_INFO << "Stopping.";
 
         pause();
+        // FIXME: send stop event
         // TODO: Reset own state.
     }