navigator component: using memory introspection

source/Navigation/components/Navigator/Navigator.cpp

@@ -52,6 +52,7 @@
 #include "Navigation/libraries/factories/NavigationStackFactory.h"
 #include "Navigation/libraries/server/Navigator.h"
 #include "Navigation/libraries/server/execution/PlatformUnitExecutor.h"
+#include "Navigation/libraries/server/introspection/MemoryIntrospector.h"
 #include "Navigation/libraries/util/util.h"
 
 std::vector<armarx::nav::core::Pose>
@@ -65,7 +66,11 @@ convert(const std::vector<Eigen::Matrix4f>& wps)
     return p;
 }
 
-armarx::nav::components::Navigator::Navigator() = default;
+armarx::nav::components::Navigator::Navigator() : writerGlobPlan(memoryNameSystem)
+{
+}
+
+
 armarx::nav::components::Navigator::~Navigator() = default;
 
 void
@@ -92,11 +97,14 @@ armarx::nav::components::Navigator::onConnectComponent()
     scene.staticScene = staticScene();
 
     executor = server::PlatformUnitExecutor(platformUnit);
+
     introspector = server::ArvizIntrospector(getArvizClient(), scene.robot);
 
     // TODO dynamic scene
     // TODO memory
     // TODO param (10)
+    writerGlobPlan.connect();
+
 
     robotStateUpdateTask = new PeriodicTask<Navigator>(
         this, &Navigator::updateRobot, 10, false, "RobotStateUpdateTask");
@@ -105,6 +113,7 @@ armarx::nav::components::Navigator::onConnectComponent()
     navRemoteGui = std::make_unique<NavigatorRemoteGui>(remoteGui, *this);
     navRemoteGui->enable();
 
+
     initialized = true;
 }
 
@@ -115,6 +124,8 @@ armarx::nav::components::Navigator::onReconnectComponent()
 
     // TODO not in all cases meaningful
     //resume();
+    writerGlobPlan.connect();
+
 
     navRemoteGui->enable();
 }
@@ -161,11 +172,16 @@ armarx::nav::components::Navigator::createConfig(const aron::data::AronDictPtr&
 
     server::GeneralConfig generalConfig;
 
+    memoryIntrospectors.emplace_back(
+        std::make_unique<server::MemoryIntrospector>(writerGlobPlan, callerId));
+
+
     navigators.emplace(callerId,
                        server::NavigatorConfig{.stack = std::move(stack),
                                                .scene = &scene,
                                                .executor = &executor.value(),
-                                               .introspector = &(introspector.value()),
+                                               //    .introspector = &(introspector.value()),
+                                               .introspector = &(*memoryIntrospectors.back()),
                                                .general = generalConfig});
 }
 
@@ -269,6 +285,8 @@ armarx::nav::components::Navigator::createPropertyDefinitions()
     // Add an optionalproperty.
     // def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
 
+    writerGlobPlan.registerPropertyDefinitions(def);
+
     return def;
 }
 

source/Navigation/components/Navigator/Navigator.h

@@ -41,7 +41,10 @@
 #include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
 #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
 #include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
+#include <RobotAPI/libraries/armem/client/ComponentPlugin.h>
 
+#include "Navigation/libraries/memory/client/stack_result/Writer.h"
+#include "Navigation/libraries/server/introspection/MemoryIntrospector.h"
 #include <Navigation/components/Navigator/NavigatorInterface.h>
 #include <Navigation/components/Navigator/RemoteGui.h>
 #include <Navigation/libraries/core/types.h>
@@ -68,7 +71,8 @@ namespace armarx::nav::components
         virtual public NavigatorInterface,
         virtual public ObjectPoseClientPluginUser,
         virtual public RobotStateComponentPluginUser,
-        virtual public ArVizComponentPluginUser
+        virtual public ArVizComponentPluginUser,
+        virtual public armem::client::ComponentPluginUser
     {
 
     public:
@@ -160,5 +164,10 @@ namespace armarx::nav::components
 
         // TODO maybe as optional, but requires some effort
         std::unique_ptr<NavigatorRemoteGui> navRemoteGui;
+
+        mem::client::stack_result::glob_plan::Writer writerGlobPlan;
+
+        // unique_ptr to avoid dangling refs
+        std::vector<std::unique_ptr<server::MemoryIntrospector>> memoryIntrospectors;
     };
 } // namespace armarx::nav::components

source/Navigation/components/Navigator/NavigatorInterface.ice

@@ -26,6 +26,8 @@
 
 // ArmarX
 #include <ArmarXCore/interface/serialization/Eigen.ice>
+
+#include <RobotAPI/interface/armem/client/MemoryListenerInterface.ice>
 #include <RobotAPI/interface/aron.ice>
 
 
@@ -36,19 +38,22 @@ module armarx
         module components
         {
 
-            interface NavigatorInterface
-            {
+            interface NavigatorInterface extends armem::client::MemoryListenerInterface {
+
                 void createConfig(aron::data::AronDict config, string callerId);
+
                 void moveTo(Eigen::Matrix4fSeq waypoints, string navigationFrame, string callerId);
-                void moveTowards(Eigen::Vector3f direction, string navigationFrame, string callerId);
+                void
+                moveTowards(Eigen::Vector3f direction, string navigationFrame, string callerId);
+
                 idempotent void pause(string callerId);
                 idempotent void resume(string callerId);
                 idempotent void stop(string callerId);
                 idempotent void stopAll();
+
                 bool isPaused(string callerId);
                 bool isStopped(string callerId);
             };
-
         };
     };
 };