Implement MemoryPlotter core functionality

source/RobotAPI/components/armem/client/MemoryPlotter/MemoryPlotter.cpp

@@ -25,9 +25,15 @@
 #include <ArmarXCore/core/time/Frequency.h>
 #include <ArmarXCore/core/time/Metronome.h>
 
+#include <RobotAPI/libraries/armem/core/error/mns.h>
+#include <RobotAPI/libraries/aron/core/data/variant/Variant.h>
+#include <RobotAPI/libraries/aron/core/data/variant/primitive/All.h>
+#include <RobotAPI/libraries/aron/core/data/visitor/variant/VariantVisitor.h>
+
 namespace armarx
 {
 
+
     armarx::PropertyDefinitionsPtr
     MemoryPlotter::createPropertyDefinitions()
     {
@@ -38,6 +44,10 @@ namespace armarx
         return defs;
     }
 
+    MemoryPlotter::MemoryPlotter()
+    {
+    }
+
     std::string
     MemoryPlotter::getDefaultName() const
     {
@@ -47,6 +57,30 @@ namespace armarx
     void
     MemoryPlotter::onInitComponent()
     {
+        DebugObserverComponentPluginUser::setDebugObserverBatchModeEnabled(true);
+
+        armem::MemoryID robotEntityId{"RobotState", "Proprioception", "Armar7", "Armar7"};
+
+        std::vector<std::string> jointNames{
+            "Neck_1_Yaw",
+            "Neck_2_Hemisphere_A",
+            "Neck_3_Hemisphere_B",
+        };
+
+        for (const std::string& jointName : jointNames)
+        {
+            std::vector<std::string> attributes{
+                "position",
+                "velocity",
+            };
+            for (const std::string& attribute : attributes)
+            {
+                properties.plottedValues.push_back(MemoryValueID{
+                    .entityID = robotEntityId,
+                    .aronPath = {{"joints", attribute, jointName}},
+                });
+            }
+        }
     }
 
     void
@@ -55,7 +89,7 @@ namespace armarx
         createRemoteGuiTab();
         RemoteGui_startRunningTask();
 
-        task = new RunningTask<MemoryPlotter>(this, &MemoryPlotter::run);
+        task = new RunningTask<MemoryPlotter>(this, &MemoryPlotter::runLoop);
         task->start();
     }
 
@@ -71,16 +105,168 @@ namespace armarx
     }
 
     void
-    MemoryPlotter::run()
+    MemoryPlotter::runLoop()
     {
         Frequency frequency = Frequency::Hertz(30);
         Metronome metronome(frequency);
         while (not task->isStopped())
         {
+            loopOnce();
             metronome.waitForNextTick();
         }
     }
 
+    class Visitor : public aron::data::ConstVariantVisitor
+    {
+    public:
+        Visitor(armarx::plugins::DebugObserverComponentPlugin& debugObserver) :
+            debugObserver{debugObserver}
+        {
+        }
+
+        // void visitAronVariant(const data::DictPtr&) override;
+        // void visitAronVariant(const data::ListPtr&) override;
+        // void visitAronVariant(const data::NDArrayPtr&) override;
+        void
+        visitAronVariant(const aron::data::IntPtr& v) override
+        {
+            setDatafield(v->getValue());
+        }
+
+        void
+        visitAronVariant(const aron::data::LongPtr& v) override
+        {
+            setDatafield(v->getValue());
+        }
+
+        void
+        visitAronVariant(const aron::data::FloatPtr& v) override
+        {
+            setDatafield(v->getValue());
+        }
+
+        void
+        visitAronVariant(const aron::data::DoublePtr& v) override
+        {
+            setDatafield(v->getValue());
+        }
+
+        void
+        visitAronVariant(const aron::data::BoolPtr& v) override
+        {
+            setDatafield(v->getValue());
+        }
+
+        void
+        visitAronVariant(const aron::data::StringPtr& v) override
+        {
+            setDatafield(v->getValue());
+        }
+
+        template <class ValueT>
+        void
+        setDatafield(const ValueT& value)
+        {
+            debugObserver.setDebugObserverDatafield(channelName, datafieldName, value);
+            ++count;
+        }
+
+        armarx::plugins::DebugObserverComponentPlugin& debugObserver;
+        std::string channelName;
+        std::string datafieldName;
+
+        int count = 0;
+    };
+
+    void
+    MemoryPlotter::loopOnce()
+    {
+        Visitor visitor(getDebugObserverComponentPlugin());
+
+        std::stringstream log;
+        for (const MemoryValueID& valueId : properties.plottedValues)
+        {
+            armem::client::Reader* reader = getReader(valueId.entityID, log);
+            if (reader == nullptr)
+            {
+                // Log is written in getReader.
+                continue;
+            }
+
+            std::optional<armem::wm::EntitySnapshot> snapshot =
+                reader->getLatestSnapshotIn(valueId.entityID);
+
+            if (not snapshot.has_value())
+            {
+                log << "\nDid not find snapshot in entity " << valueId.entityID << ".";
+                continue;
+            }
+
+            aron::data::DictPtr data = snapshot->getInstance(0).data();
+            aron::data::VariantPtr valueVariant = data->navigateAbsolute(valueId.aronPath);
+            if (not valueVariant)
+            {
+                log << "\nDid not find " << valueId.aronPath.toString() << " in entity snapshot "
+                    << snapshot->id() << ".";
+                continue;
+            }
+
+
+            auto makeDatafieldName = [](const aron::Path& path) -> std::string
+            {
+                // The first element is always "ARON->", which we can discard for readability.
+                std::string str = path.toString();
+                str =
+                    simox::alg::remove_prefix(str, path.getRootIdentifier() + path.getDelimeter());
+                str = simox::alg::replace_all(str, path.getDelimeter(), ">");
+                return str;
+            };
+
+            visitor.channelName = simox::alg::replace_all(valueId.entityID.str(), "/", ">");
+            visitor.datafieldName = makeDatafieldName(valueId.aronPath);
+
+            aron::data::visit(visitor, valueVariant);
+        }
+
+        ARMARX_INFO << "Sending debug observer batch with " << visitor.count << " values ...";
+        sendDebugObserverBatch();
+
+        if (not log.str().empty())
+        {
+            ARMARX_INFO << "Encountered issues while sending memory values to the debug observer "
+                           "for plotting: "
+                        << log.str();
+        }
+    }
+
+    armem::client::Reader*
+    MemoryPlotter::getReader(const armem::MemoryID& memoryID, std::stringstream& log)
+    {
+        armem::MemoryID key = memoryID.getProviderSegmentID();
+
+        auto it = memoryReaders.find(key);
+        if (it != memoryReaders.end())
+        {
+            return &it->second;
+        }
+        else
+        {
+            armem::client::Reader reader;
+            try
+            {
+                reader = memoryNameSystem().getReader(key);
+            }
+            catch (armem::error::CouldNotResolveMemoryServer& e)
+            {
+                log << "\n" << e.what();
+                return nullptr;
+            }
+
+            auto [it, _] = memoryReaders.emplace(key, reader);
+            return &it->second;
+        }
+    }
+
     void
     MemoryPlotter::createRemoteGuiTab()
     {

source/RobotAPI/components/armem/client/MemoryPlotter/MemoryPlotter.h

@@ -22,6 +22,7 @@
 
 #pragma once
 
+
 #include <ArmarXCore/core/Component.h>
 #include <ArmarXCore/interface/observers/ObserverInterface.h>
 #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
@@ -37,6 +38,13 @@
 namespace armarx
 {
 
+
+    struct MemoryValueID
+    {
+        armem::MemoryID entityID;
+        aron::Path aronPath;
+    };
+
     /**
      * @defgroup Component-MemoryPlotter MemoryPlotter
      * @ingroup RobotAPI-Components
@@ -55,6 +63,8 @@ namespace armarx
         virtual public armarx::LightweightRemoteGuiComponentPluginUser
     {
     public:
+        MemoryPlotter();
+
         /// @see armarx::ManagedIceObject::getDefaultName()
         std::string getDefaultName() const override;
 
@@ -72,15 +82,19 @@ namespace armarx
         void onDisconnectComponent() override;
         void onExitComponent() override;
 
-        void run();
+        void runLoop();
+        void loopOnce();
+
+        armem::client::Reader* getReader(const armem::MemoryID& memoryID, std::stringstream& log);
 
 
     private:
         struct Properties
         {
+            std::vector<MemoryValueID> plottedValues;
         };
 
-        Properties p;
+        Properties properties;
 
         std::map<armem::MemoryID, armem::client::Reader> memoryReaders;