robot state memory update

Disclaimer: sorry, a bit messy; more changes than initially expected due to changes in base class: MemoryConnector.h (now using a PluginUser class) has been removed. The PR https://gitlab.com/ArmarX/RobotAPI/-/merge_requests/141 should be merged first as I started from there.

Changes to RobotStateMemory:

• The robot state memory now has 3 segments: Localization, Proprioception and Description. The latter stores the robot description, so to say the "information to instantiate a robot model". Currently, I only store a XML file link there, but in the future, it should be the robot nodes and its properties to track changes over time (e.g. installing a new camera, changing hands, ...)
• The robot state memory core segments are now implemented in separate files, each is called Segment.h, similar to the object memory. This segment class has an attached visu class to allow memory-side visualization (to ArViz). Visualization can be turned on and off via properties.
• as the robot state memory and the object memory (the articulated objects in particular) use the same XML Aron definitions, e.g. Robot, RobotDescription and RobotState, there is a new library armem_robot to avoid dependencies between armem_robot_state and armem_objects.

Further changes:

• All my memory client classes, e.g. TransformReader are now refactored: As mentioned, the MemoryConnector class is now gone and replaced by memory Client**ComponentPluginUser classes.

source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp

+#include "Segment.h"
+
+#include <IceUtil/Time.h>
+#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <sstream>
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include "RobotAPI/libraries/armem_robot/types.h"
+#include "RobotAPI/libraries/aron/common/aron_conversions.h"
+
+#include "RobotAPI/libraries/armem/core/MemoryID.h"
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Visitor.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include <thread>
+
+namespace armarx::armem::server::robot_state::description
+{
+
+    Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter,
+                     std::mutex& memoryMutex) :
+        iceMemory(memoryToIceAdapter), memoryMutex(memoryMutex)
+    {
+        Logging::setTag("DescriptionSegment");
+    }
+
+    Segment::~Segment() = default;
+
+    void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+    {
+        defs->optional(p.coreSegment,
+                       prefix + "seg.description.CoreSegment",
+                       "Name of the robot description core segment.");
+        defs->optional(p.maxHistorySize,
+                       prefix + "seg.description.MaxHistorySize",
+                       "Maximal size of object poses history (-1 for infinite).");
+    }
+
+    void Segment::init()
+    {
+        ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
+
+        ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
+
+        coreSegment = &iceMemory.workingMemory->addCoreSegment(
+                          p.coreSegment, arondto::RobotDescription::toInitialAronType());
+        coreSegment->setMaxHistorySize(p.maxHistorySize);
+    }
+
+    void Segment::connect(const viz::Client& arviz, const RobotUnitInterfacePrx& robotUnitPrx)
+    {
+        // this->visu = std::make_unique<Visu>(arviz, *this);
+        robotUnit = robotUnitPrx;
+
+        // store the robot description linked to the robot unit in this segment
+        updateRobotDescription();
+    }
+
+    void Segment::storeRobotDescription(const robot::RobotDescription& robotDescription)
+    {
+        const Time now = TimeUtil::GetTime();
+
+        const MemoryID providerID = coreSegment->id().withProviderSegmentName(robotDescription.name);
+        coreSegment->addProviderSegment(providerID.providerSegmentName, arondto::RobotDescription::toInitialAronType());
+
+        EntityUpdate update;
+        update.entityID = providerID.withEntityName("description");
+        update.timeArrived = update.timeCreated = update.timeSent = now;
+
+        arondto::RobotDescription dto;
+        robot::toAron(dto, robotDescription);
+        update.instancesData =
+        {
+            dto.toAron()
+        };
+
+        Commit commit;
+        commit.updates.push_back(update);
+
+        {
+            // std::lock_guard g{memoryMutex};
+            iceMemory.commit(commit);
+        }
+
+    }
+
+    void Segment::updateRobotDescription()
+    {
+        ARMARX_CHECK_NOT_NULL(robotUnit);
+
+        const auto waitForKinematicUnit = [&]()
+        {
+            while (true)
+            {
+                auto kinematicUnit = robotUnit->getKinematicUnit();
+
+                if (kinematicUnit)
+                {
+                    ARMARX_INFO << "Kinematic unit is now available.";
+                    return kinematicUnit;
+                }
+
+                ARMARX_INFO << "Waiting for kinematic unit ...";
+                std::this_thread::sleep_for(std::chrono::seconds(1));
+            }
+        };
+
+        auto kinematicUnit = waitForKinematicUnit();
+        ARMARX_CHECK_NOT_NULL(kinematicUnit);
+
+        const auto robotName = kinematicUnit->getRobotName();
+
+        const robot::RobotDescription robotDescription
+        {
+            .name = kinematicUnit->getRobotName(),
+            .xml  = {"Armar6RT", kinematicUnit->getRobotFilename()}}; // FIXME
+
+        storeRobotDescription(robotDescription);
+    }
+
+    Segment::RobotDescriptionMap Segment::getRobotDescriptions(const armem::Time& timestamp) const
+    {
+
+        RobotDescriptionMap robotDescriptions;
+
+        {
+            // std::lock_guard g{memoryMutex};
+
+            for (const auto &[_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
+            {
+                for (const auto &[name, entity] : provSeg.entities())
+                {
+                    const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+                    const auto description     = robot::convertRobotDescription(entityInstance);
+
+                    if (not description)
+                    {
+                        ARMARX_WARNING << "Could not convert entity instance to "
+                                       "'RobotDescription'";
+                        continue;
+                    }
+
+                    ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
+
+                    robotDescriptions.emplace(description->name, *description);
+                }
+            }
+        }
+
+        ARMARX_INFO << deactivateSpam(10) <<  "Number of known robot descriptions: " << robotDescriptions.size();
+
+        return robotDescriptions;
+    }
+
+    // std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> Segment::getKnownObjectClasses() const
+    // {
+    //     std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> objects;
+
+    //     for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
+    //     {
+    //         for (const auto& [name, entity] :  provSeg.entities())
+    //         {
+    //             const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
+    //             const auto description = articulated_object::convertRobotDescription(entityInstance);
+
+    //             if (not description)
+    //             {
+    //                 ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
+    //                 continue;
+    //             }
+
+    //             ARMARX_INFO << "Key is " << armem::MemoryID(entity.id());
+
+    //             objects.emplace(armem::MemoryID(entity.id()), *description);
+    //         }
+    //     }
+
+    //     ARMARX_IMPORTANT << "Number of known articulated object classes: " << objects.size();
+
+    //     return objects;
+    // }
+
+    // void Segment::RemoteGui::setup(const Segment& data)
+    // {
+    //     using namespace armarx::RemoteGui::Client;
+
+    //     maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
+    //     maxHistorySize.setRange(1, 1e6);
+    //     infiniteHistory.setValue(data.p.maxHistorySize == -1);
+    //     discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
+
+    //     GridLayout grid;
+    //     int row = 0;
+    //     grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
+    //     row++;
+    //     grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
+    //     row++;
+    //     grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
+    //     row++;
+
+    //     group.setLabel("Data");
+    //     group.addChild(grid);
+    // }
+
+    // void Segment::RemoteGui::update(Segment& data)
+    // {
+    //     if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
+    //         || discardSnapshotsWhileAttached.hasValueChanged())
+    //     {
+    //         std::scoped_lock lock(data.memoryMutex);
+
+    //         if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+    //         {
+    //             data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+    //             if (data.coreSegment)
+    //             {
+    //                 data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
+    //             }
+    //         }
+
+    //         data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
+    //     }
+    // }
+
+} // namespace armarx::armem::server::robot_state::description