feature: Architectural design of event handling facilities.

source/Navigation/components/Navigator/Navigator.cpp

@@ -27,6 +27,7 @@
 #include <cmath>
 #include <iterator>
 #include <memory>
+#include <utility>
 
 #include <Eigen/Geometry>
 
@@ -175,14 +176,14 @@ armarx::nav::components::Navigator::createConfig(const aron::data::AronDictPtr&
     memoryIntrospectors.emplace_back(
         std::make_unique<server::MemoryIntrospector>(writerGlobPlan, callerId));
 
-
-    navigators.emplace(callerId,
-                       server::NavigatorConfig{.stack = std::move(stack),
+    navigators.emplace(std::piecewise_construct,
+                       std::forward_as_tuple(callerId),
+                       std::forward_as_tuple(server::NavigatorConfig{.stack = std::move(stack),
                                                .scene = &scene,
-                                               .executor = &executor.value(),
-                                               .introspector = &(introspector.value()),
-                                               //.introspector = &(*memoryIntrospectors.back()),
-                                               .general = generalConfig});
+                                               .general = generalConfig},
+                       server::Services{.executor = &executor.value(),
+                                        .publisher = &publisher,
+                                        .introspector = &(introspector.value())}));
 }
 
 void

source/Navigation/components/Navigator/Navigator.h

@@ -51,6 +51,7 @@
 #include <Navigation/libraries/server/Navigator.h>
 #include <Navigation/libraries/server/execution/PlatformUnitExecutor.h>
 #include <Navigation/libraries/server/introspection/ArvizIntrospector.h>
+#include <Navigation/libraries/server/event_publishing/MemoryPublisher.h>
 
 namespace armarx::nav::components
 {
@@ -155,6 +156,7 @@ namespace armarx::nav::components
 
         core::Scene scene;
         std::optional<server::PlatformUnitExecutor> executor;
+        server::MemoryPublisher publisher; // TODO: Implement.
         std::optional<server::ArvizIntrospector> introspector;
         std::unordered_map<std::string, server::Navigator> navigators;
 

source/Navigation/libraries/client/CMakeLists.txt

@@ -19,14 +19,21 @@ armarx_add_library(
     SOURCES
         ./Navigator.cpp
         ./NavigationStackConfig.cpp
-        ./IceNavigator.cpp
         ./ComponentPlugin.cpp
+        # Services.
+        ./services/MemorySubscriber.cpp
+        ./services/SimpleEventHandler.cpp
+        ./services/IceNavigator.cpp
     HEADERS
         ../client.h
         ./Navigator.h
         ./NavigationStackConfig.h
-        ./IceNavigator.h
         ./ComponentPlugin.h
+        # Services.
+        ./services/EventSubscriptionInterface.h
+        ./services/MemorySubscriber.h
+        ./services/SimpleEventHandler.h
+        ./services/IceNavigator.h
 )
 
 add_library(

source/Navigation/libraries/client/ComponentPlugin.h

@@ -6,8 +6,8 @@
 
 // Navigator
 #include <Navigation/components/Navigator/NavigatorInterface.h>
-#include <Navigation/libraries/client/IceNavigator.h>
 #include <Navigation/libraries/client/Navigator.h>
+#include <Navigation/libraries/client/services/IceNavigator.h>
 
 namespace armarx::nav::client
 {

source/Navigation/libraries/client/Navigator.cpp

@@ -62,13 +62,13 @@ armarx::nav::client::Navigator::stop()
 void
 armarx::nav::client::Navigator::onGoalReached(const std::function<void(void)>& callback)
 {
-    onGoalReached([&callback](const GoalReachedEvent&) { callback(); });
+    onGoalReached([&callback](const server::GoalReachedEvent&) { callback(); });
 }
 
 
 void
 armarx::nav::client::Navigator::onGoalReached(
-    const std::function<void(const GoalReachedEvent&)>& callback)
+    const std::function<void(const server::GoalReachedEvent&)>& callback)
 {
 }
 
@@ -94,6 +94,6 @@ armarx::nav::client::Navigator::waitForStop()
         stopped = navigator->isStopped();
     } while (not stopped);
 
-    StopEvent se = GoalReachedEvent{.pose = core::Pose::Identity()};
+    StopEvent se = server::GoalReachedEvent{.pose = core::Pose::Identity()};
     return se;
 }

source/Navigation/libraries/client/Navigator.h

@@ -31,44 +31,11 @@
 // Navigation
 #include <Navigation/libraries/core/NavigatorInterface.h>
 #include <Navigation/libraries/core/types.h>
+#include <Navigation/libraries/server/event_publishing/EventPublishingInterface.h>
 
 namespace armarx::nav::client
 {
 
-    struct GoalReachedEvent
-    {
-        core::Pose pose;
-    };
-
-    struct WaypointReachedEvent
-    {
-        int waypointId;
-        core::Pose pose;
-    };
-
-    struct SafetyThrottlingTriggeredEvent
-    {
-        core::Pose pose;
-        float throttlingFactor = 1.0;
-        // TODO: Direction where safety-critical obstacle is (or range or whatever...).
-    };
-
-    struct SafetyStopTriggeredEvent
-    {
-        core::Pose pose;
-        // TODO: Direction where safety-critical obstacle is (or range or whatever...).
-    };
-
-    struct UserAbortTriggeredEvent
-    {
-        core::Pose pose;
-    };
-
-    struct InternalErrorEvent
-    {
-        std::string message;
-    };
-
     class StopEvent
     {
 
@@ -81,49 +48,49 @@ namespace armarx::nav::client
         bool
         isGoalReachedEvent() const
         {
-            return std::holds_alternative<GoalReachedEvent>(event);
+            return std::holds_alternative<server::GoalReachedEvent>(event);
         }
 
-        GoalReachedEvent&
+        server::GoalReachedEvent&
         toGoalReachedEvent()
         {
-            return std::get<GoalReachedEvent>(event);
+            return std::get<server::GoalReachedEvent>(event);
         }
 
         bool
         isSafetyStopTriggeredEvent() const
         {
-            return std::holds_alternative<SafetyStopTriggeredEvent>(event);
+            return std::holds_alternative<server::SafetyStopTriggeredEvent>(event);
         }
 
-        SafetyStopTriggeredEvent&
+        server::SafetyStopTriggeredEvent&
         toSafetyStopTriggeredEvent()
         {
-            return std::get<SafetyStopTriggeredEvent>(event);
+            return std::get<server::SafetyStopTriggeredEvent>(event);
         }
 
         bool
         isUserAbortTriggeredEvent() const
         {
-            return std::holds_alternative<UserAbortTriggeredEvent>(event);
+            return std::holds_alternative<server::UserAbortTriggeredEvent>(event);
         }
 
-        UserAbortTriggeredEvent&
+        server::UserAbortTriggeredEvent&
         toUserAbortTriggeredEvent()
         {
-            return std::get<UserAbortTriggeredEvent>(event);
+            return std::get<server::UserAbortTriggeredEvent>(event);
         }
 
         bool
         isInternalErrorEvent() const
         {
-            return std::holds_alternative<InternalErrorEvent>(event);
+            return std::holds_alternative<server::InternalErrorEvent>(event);
         }
 
-        InternalErrorEvent&
+        server::InternalErrorEvent&
         toInternalErrorEvent()
         {
-            return std::get<InternalErrorEvent>(event);
+            return std::get<server::InternalErrorEvent>(event);
         }
 
         operator bool() const
@@ -132,10 +99,10 @@ namespace armarx::nav::client
         }
 
     private:
-        using StopEventVar = std::variant<GoalReachedEvent,
-                                          SafetyStopTriggeredEvent,
-                                          UserAbortTriggeredEvent,
-                                          InternalErrorEvent>;
+        using StopEventVar = std::variant<server::GoalReachedEvent,
+                                          server::SafetyStopTriggeredEvent,
+                                          server::UserAbortTriggeredEvent,
+                                          server::InternalErrorEvent>;
         StopEventVar event;
     };
 
@@ -160,7 +127,7 @@ namespace armarx::nav::client
 
         void onGoalReached(const std::function<void(void)>& callback);
 
-        void onGoalReached(const std::function<void(const GoalReachedEvent&)>& callback);
+        void onGoalReached(const std::function<void(const server::GoalReachedEvent&)>& callback);
 
         void onWaypointReached(const std::function<void(int)>& callback);
 

source/Navigation/libraries/client/services/EventSubscriptionInterface.h

+#pragma once
+
+#include <Navigation/libraries/server/event_publishing/EventPublishingInterface.h>
+
+namespace armarx::nav::client
+{
+
+    class EventSubscriptionInterface
+    {
+
+    public:
+
+        virtual void onGoalReached(const server::GoalReachedEvent&) = 0;
+
+        virtual void onWaypointReached(const server::WaypointReachedEvent&) = 0;
+
+        // Non-API.
+    public:
+
+        virtual ~EventSubscriptionInterface() = default;
+
+    };
+
+}

source/Navigation/libraries/client/IceNavigator.cpp → source/Navigation/libraries/client/services/IceNavigator.cpp

-#include <Navigation/libraries/client/IceNavigator.h>
+#include <Navigation/libraries/client/services/IceNavigator.h>
 
 std::vector<Eigen::Matrix4f>
 convert(const std::vector<armarx::nav::core::Pose>& ps)

source/Navigation/libraries/client/services/MemorySubscriber.h

+#pragma once
+
+#include <Navigation/libraries/client/services/EventSubscriptionInterface.h>
+
+namespace armarx::nav::client
+{
+
+    class MemorySubscriber : virtual public EventSubscriptionInterface
+    {
+
+    public:
+
+        void onGoalReached(const server::GoalReachedEvent&) override
+        {
+            // TODO: Implement.
+        }
+
+        void onWaypointReached(const server::WaypointReachedEvent&) override
+        {
+            // TODO: Implement.
+        }
+
+        // Non-API.
+    public:
+
+        ~MemorySubscriber() override = default;
+
+    };
+
+}

source/Navigation/libraries/client/services/SimpleEventHandler.cpp

+#include <Navigation/libraries/client/services/SimpleEventHandler.h>
+
+
+void
+armarx::nav::client::SimpleEventHandler::onGoalReached(const server::GoalReachedEvent&)
+{
+    // TODO: Implement.
+}
+
+
+void
+armarx::nav::client::SimpleEventHandler::onWaypointReached(const server::WaypointReachedEvent&)
+{
+    // TODO: Implement.
+}
+
+
+void
+armarx::nav::client::SimpleEventHandler::goalReached(const server::GoalReachedEvent&)
+{
+    // TODO: Implement.
+}
+
+
+void
+armarx::nav::client::SimpleEventHandler::waypointReached(const server::WaypointReachedEvent&)
+{
+    // TODO: Implement.
+}

source/Navigation/libraries/client/services/SimpleEventHandler.h

+#pragma once
+
+#include <Navigation/libraries/client/services/EventSubscriptionInterface.h>
+#include <Navigation/libraries/server/event_publishing/EventPublishingInterface.h>
+
+namespace armarx::nav::client
+{
+
+    class SimpleEventHandler :
+            virtual public EventSubscriptionInterface,
+            virtual public server::EventPublishingInterface
+    {
+
+        // EventSubscriptionInterface interface
+    public:
+        void onGoalReached(const server::GoalReachedEvent&) override;
+        void onWaypointReached(const server::WaypointReachedEvent&) override;
+
+        // EventPublishingInterface interface
+    public:
+        void goalReached(const server::GoalReachedEvent&) override;
+        void waypointReached(const server::WaypointReachedEvent&) override;
+
+    private:
+
+        struct {
+            bool todo;
+        } subscriptions;
+
+    };
+
+}

source/Navigation/libraries/server/CMakeLists.txt

@@ -32,6 +32,9 @@ armarx_add_library(
         ./execution/ExecutorInterface.h
         ./execution/PlatformUnitExecutor.h
         ./execution/DummyExecutor.h
+        # Event publishing.
+        ./event_publishing/EventPublishingInterface.h
+        ./event_publishing/MemoryPublisher.h
         # Introspection
         ./introspection/IntrospectorInterface.h
         ./introspection/ArvizIntrospector.h

source/Navigation/libraries/server/Navigator.cpp

@@ -25,10 +25,11 @@ namespace armarx::nav::server
         }
     }
 
-    Navigator::Navigator(const NavigatorConfig& config) : config{config}
+    Navigator::Navigator(const NavigatorConfig& config, const Services& services) : config{config}, srv{services}
     {
         ARMARX_CHECK_NOT_NULL(config.scene) << "The scene must be set!";
-        ARMARX_CHECK_NOT_NULL(config.executor) << "The executor must be set!";
+        ARMARX_CHECK_NOT_NULL(services.executor) << "The executor service must be set!";
+        ARMARX_CHECK_NOT_NULL(services.publisher) << "The publisher service must be set!";
     }
 
     Navigator::~Navigator()
@@ -217,17 +218,17 @@ namespace armarx::nav::server
         ARMARX_INFO << deactivateSpam(1) << "velocity in robot frame "
                     << robotFrameVelocity.linear;
 
-        config.executor->move(robotFrameVelocity);
+        srv.executor->move(robotFrameVelocity);
     }
 
     void
     Navigator::updateIntrospector()
     {
-        ARMARX_CHECK_NOT_NULL(config.introspector);
+        ARMARX_CHECK_NOT_NULL(srv.introspector);
 
         if (res.isValid())
         {
-            config.introspector->onStackResult(res);
+            srv.introspector->onStackResult(res);
         }
     }
 
@@ -312,8 +313,8 @@ namespace armarx::nav::server
 
         executorEnabled.store(false);
 
-        ARMARX_CHECK_NOT_NULL(config.executor);
-        config.executor->move(core::Twist::Zero());
+        ARMARX_CHECK_NOT_NULL(srv.executor);
+        srv.executor->move(core::Twist::Zero());
     }
 
     void
@@ -347,7 +348,7 @@ namespace armarx::nav::server
     }
 
     Navigator::Navigator(Navigator&& other) noexcept :
-        config{std::move(other.config)}, executorEnabled{other.executorEnabled.load()}
+        config{std::move(other.config)}, srv{std::move(other.srv)}, executorEnabled{other.executorEnabled.load()}
     {
     }
 } // namespace armarx::nav::server

source/Navigation/libraries/server/Navigator.h

@@ -42,6 +42,7 @@
 #include <Navigation/libraries/core/NavigatorInterface.h>
 #include <Navigation/libraries/server/NavigationStack.h>
 #include <Navigation/libraries/server/execution/ExecutorInterface.h>
+#include <Navigation/libraries/server/event_publishing/EventPublishingInterface.h>
 #include <Navigation/libraries/server/monitoring/GoalReachedMonitor.h>
 
 namespace armarx::nav::server
@@ -65,17 +66,21 @@ namespace armarx::nav::server
     {
         server::NavigationStack stack;
         core::Scene* const scene;
+        GeneralConfig general;
+    };
+
+    struct Services
+    {
         ExecutorInterface* executor;
+        EventPublishingInterface* publisher;
         IntrospectorInterface* introspector = nullptr;
-
-        GeneralConfig general;
     };
 
     class Navigator : virtual public core::NavigatorInterface
     {
 
     public:
-        Navigator(const NavigatorConfig& config);
+        Navigator(const NavigatorConfig& config, const Services& services);
 
         void moveTo(const std::vector<core::Pose>& waypoints,
                     core::NavigationFrame navigationFrame) override;
@@ -118,6 +123,8 @@ namespace armarx::nav::server
 
         NavigatorConfig config;
 
+        Services srv;
+
         std::atomic_bool executorEnabled = true;
 
         StackResult res;

source/Navigation/libraries/server/event_publishing/EventPublishingInterface.h

+#pragma once
+
+#include <Navigation/libraries/core/types.h>
+
+namespace armarx::nav::server
+{
+
+    struct GoalReachedEvent
+    {
+        core::Pose pose;
+    };
+
+    struct WaypointReachedEvent
+    {
+        int waypointId;
+        core::Pose pose;
+    };
+
+    struct SafetyThrottlingTriggeredEvent
+    {
+        core::Pose pose;
+        float throttlingFactor = 1.0;
+        // TODO: Direction where safety-critical obstacle is (or range or whatever...).
+    };
+
+    struct SafetyStopTriggeredEvent
+    {
+        core::Pose pose;
+        // TODO: Direction where safety-critical obstacle is (or range or whatever...).
+    };
+
+    struct UserAbortTriggeredEvent
+    {
+        core::Pose pose;
+    };
+
+    struct InternalErrorEvent
+    {
+        std::string message;
+    };
+
+    /**
+     * @brief A publisher the server navigator will use to notify others about events.
+     */
+    class EventPublishingInterface
+    {
+
+    public:
+
+        /**
+         * @brief Will be called whenever the navigator reached the goal.
+         */
+        virtual void goalReached(const GoalReachedEvent&) = 0;
+
+        /**
+         * @brief Will be called whenever the navigator reached a user-defined waypoint.
+         */
+        virtual void waypointReached(const WaypointReachedEvent&) = 0;
+
+        // Non-API.
+    public:
+        virtual ~EventPublishingInterface() = default;
+
+    };
+
+} // namespace armarx::nav::server

source/Navigation/libraries/server/event_publishing/MemoryPublisher.h

+#pragma once
+
+#include <Navigation/libraries/server/event_publishing/EventPublishingInterface.h>
+
+namespace armarx::nav::server
+{
+
+    class MemoryPublisher : virtual public EventPublishingInterface
+    {
+
+    public:
+
+        void goalReached(const GoalReachedEvent&) override
+        {
+            // TODO: Implement.
+        }
+
+        void waypointReached(const WaypointReachedEvent&) override
+        {
+            // TODO: Implement.
+        }
+
+        // Non-API.
+    public:
+        ~MemoryPublisher() override = default;
+
+    };
+
+} // namespace armarx::nav::server

source/Navigation/libraries/server/execution/ExecutorInterface.h

@@ -8,6 +8,9 @@ namespace armarx::nav::core
 namespace armarx::nav::server
 {
 
+    /**
+     * @brief An executer the server navigator will use to send its control commands to.
+     */
     class ExecutorInterface
     {
 

source/Navigation/libraries/server/test/serverTest.cpp

@@ -30,6 +30,7 @@
 #include "Navigation/libraries/server/Navigator.h"
 #include "Navigation/libraries/trajectory_control/TrajectoryFollowingController.h"
 #include <Navigation/libraries/server/execution/DummyExecutor.h>
+#include <Navigation/libraries/client/services/SimpleEventHandler.h>
 
 #define BOOST_TEST_MODULE Navigation::ArmarXLibraries::server
 
@@ -62,11 +63,16 @@ BOOST_AUTO_TEST_CASE(testNavigator)
 
     // Executor.
     server::DummyExecutor executor{scene.robot, server::DummyExecutor::Params()};
+    client::SimpleEventHandler eventHandler;
 
     server::GeneralConfig generalConfig;
 
     server::Navigator navigator(server::NavigatorConfig{
-        .stack = stack, .scene = &scene, .executor = &executor, .general = generalConfig});
+        .stack = stack, .scene = &scene, .general = generalConfig},
+                                server::Services{
+                                    .executor = &executor,
+                                    .publisher = &eventHandler
+                                });
     navigator.moveTo(std::vector{goal}, core::NavigationFrame::Absolute);
 
     BOOST_CHECK_EQUAL(true, true);