From 29d7187f486a9464261556a1ec8f50d7959e84c9 Mon Sep 17 00:00:00 2001
From: armar-user <armar6@kit>
Date: Fri, 7 Jan 2022 11:48:13 +0100
Subject: [PATCH] navigator: stop events
---
.../navigation/components/Navigator/Navigator.cpp | 12 ++++++++++++
source/armarx/navigation/server/Navigator.cpp | 8 ++++++++
2 files changed, 20 insertions(+)
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index 6532665f..512837bf 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/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
diff --git a/source/armarx/navigation/server/Navigator.cpp b/source/armarx/navigation/server/Navigator.cpp
index 0f610336..6bac92f2 100644
--- a/source/armarx/navigation/server/Navigator.cpp
+++ b/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.
}
--
GitLab