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/components/armem/server/RobotStateMemory/RobotStateMemory.cpp

@@ -20,13 +20,20 @@
  *             GNU General Public License
  */
 
+#include "RobotStateMemory.h"
+
 // STD
+#include <Eigen/src/Geometry/Transform.h>
+#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
 #include <algorithm>
 #include <iostream>
 #include <fstream>
 
 // Header
-#include "RobotStateMemory.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.h"
+#include "RobotAPI/libraries/core/Pose.h"
 
 // Simox
 #include <SimoxUtility/algorithm/string.h>
@@ -40,19 +47,44 @@
 #include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
 #include <RobotAPI/libraries/aron/core/navigator/type/AllNavigators.h>
 
-namespace armarx
+#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
+
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <memory>
+
+namespace armarx::armem::server::robot_state
 {
     RobotStateMemory::RobotStateMemory()
+        : descriptionSegment(server::ComponentPluginUser::iceMemory,
+                             server::ComponentPluginUser::workingMemoryMutex),
+          proprioceptionSegment(server::ComponentPluginUser::iceMemory,
+                                server::ComponentPluginUser::workingMemoryMutex),
+          localizationSegment(server::ComponentPluginUser::iceMemory,
+                              server::ComponentPluginUser::workingMemoryMutex),
+          commonVisu(descriptionSegment, proprioceptionSegment, localizationSegment)
     {
+
     }
 
+    RobotStateMemory::~RobotStateMemory() = default;
+
+
     armarx::PropertyDefinitionsPtr RobotStateMemory::createPropertyDefinitions()
     {
+        const std::string robotUnitPrefix{sensorValuePrefix};
+
         armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
-        defs->optional(robotUnitSensorPrefix, "SensorValuePrefix", "Prefix of all sensor values");
-        defs->optional(robotUnitMemoryBatchSize, "RobotUnitMemoryBatchSize", "The size of the entity snapshot to send to the memory. Min is 1");
-        defs->optional(robotUnitPollFrequency, "RobotUnitUpdateFrequency", "The frequency in Hz to store values. All other values get discarded. Min is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY));
-        defs->optional(robotUnitConfigPath, "robotUnitConfigPath", "Specify a configuration file to group the sensor values specifically.");
+        defs->optional(robotUnitSensorPrefix, robotUnitPrefix + "SensorValuePrefix", "Prefix of all sensor values");
+        defs->optional(robotUnitMemoryBatchSize, robotUnitPrefix + "MemoryBatchSize", "The size of the entity snapshot to send to the memory. Min is 1");
+        defs->optional(robotUnitPollFrequency, robotUnitPrefix + "UpdateFrequency", "The frequency in Hz to store values. All other values get discarded. Min is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY));
+        defs->optional(robotUnitConfigPath, robotUnitPrefix + "ConfigPath", "Specify a configuration file to group the sensor values specifically.");
+
+        descriptionSegment.defineProperties(defs);
+        proprioceptionSegment.defineProperties(defs);
+        localizationSegment.defineProperties(defs);
+        commonVisu.defineProperties(defs);
 
         return defs;
     }
@@ -66,10 +98,15 @@ namespace armarx
 
     void RobotStateMemory::onInitComponent()
     {
+
+        descriptionSegment.init();
+        proprioceptionSegment.init();
+        localizationSegment.init();
+        commonVisu.init();
+
         robotUnitMemoryBatchSize = std::max((unsigned int) 1, robotUnitMemoryBatchSize);
         robotUnitPollFrequency = std::clamp(robotUnitPollFrequency, 1, ROBOT_UNIT_MAXIMUM_FREQUENCY);
 
-
         Ice::StringSeq includePaths;
         auto packages = armarx::Application::GetProjectDependencies();
         packages.push_back(Application::GetProjectName());
@@ -95,9 +132,16 @@ namespace armarx
 
     void RobotStateMemory::onConnectComponent()
     {
-        setupRobotUnitSegment();
+        descriptionSegment.connect(getArvizClient(), getRobotUnit());
 
-        setupLocalizationCoreSegment();
+        proprioceptionSegment.connect(getArvizClient(), getRobotUnit());
+        toIce(robotUnitProviderID, proprioceptionSegment.getRobotUnitProviderID());
+
+        localizationSegment.connect(getArvizClient());
+
+        commonVisu.connect(
+            getArvizClient()
+        );
 
         cfg.loggingNames.emplace_back(robotUnitSensorPrefix);
         handler = make_shared<RobotUnitDataStreamingReceiver>(this, getRobotUnit(), cfg);
@@ -124,45 +168,6 @@ namespace armarx
     /*************************************************************/
     // RobotUnit Streaming functions
     /*************************************************************/
-    void RobotStateMemory::setupLocalizationCoreSegment()
-    {
-        ARMARX_INFO << "Adding core segment " << localizationCoreSegmentName;
-        workingMemory.addCoreSegments({localizationCoreSegmentName});
-    }
-
-    /*************************************************************/
-    // RobotUnit Streaming functions
-    /*************************************************************/
-    void RobotStateMemory::setupRobotUnitSegment()
-    {
-        ARMARX_INFO << "Adding core segment " << proprioceptionCoreSegmentName;
-        workingMemory.addCoreSegments({proprioceptionCoreSegmentName});
-
-        ARMARX_INFO << "Adding provider segment " << proprioceptionCoreSegmentName << "/" << robotUnitProviderSegmentName;
-        armem::data::AddSegmentInput input;
-        input.coreSegmentName = proprioceptionCoreSegmentName;
-        input.providerSegmentName = robotUnitProviderSegmentName;
-
-        auto encoderEntryType = std::make_shared<aron::typenavigator::ObjectNavigator>("RobotUnitEncoderEntry");
-        auto encoderNameType = std::make_shared<aron::typenavigator::StringNavigator>();
-        auto encoderIterationIDType = std::make_shared<aron::typenavigator::LongNavigator>();
-        encoderEntryType->addMemberType("EncoderGroupName", encoderNameType);
-        encoderEntryType->addMemberType("IterationId", encoderIterationIDType);
-        //auto encoderValueType = std::make_shared<aron::typenavigator::AnyType>();
-        //encoderEntryType->addMemberType("value", encoderValueType);
-
-        auto result = addSegments({input})[0];
-
-        if (!result.success)
-        {
-            ARMARX_ERROR << "Could not add segment " << proprioceptionCoreSegmentName << "/" << robotUnitProviderSegmentName << ". The error message is: " << result.errorMessage;
-        }
-
-        robotUnitProviderID.memoryName = workingMemoryName;
-        robotUnitProviderID.coreSegmentName = proprioceptionCoreSegmentName;
-        robotUnitProviderID.providerSegmentName = robotUnitProviderSegmentName;
-    }
-
     void RobotStateMemory::convertRobotUnitStreamingDataToAron()
     {
         auto& data = handler->getDataBuffer();
@@ -176,8 +181,11 @@ namespace armarx
         auto start = std::chrono::high_resolution_clock::now();
 
         RobotUnitData convertedAndGroupedData;
-        for (const auto& [name, dataEntry] : keys.entries)
+        for (const auto& [nameEntry, dataEntry] : keys.entries)
         {
+            std::string name = nameEntry;
+            std::string jointOrWhateverName;
+
             std::string groupName = "";
             if (configSensorMapping.find(name) != configSensorMapping.end())
             {
@@ -185,13 +193,18 @@ namespace armarx
             }
             else
             {
-                size_t dot_pos = name.find(".", name.find(".") + 1); // find second occurence of "."
-                if (dot_pos == std::string::npos)
+                const size_t first_dot_pos = name.find(".");
+                const size_t second_dot_pos = name.find(".", first_dot_pos + 1); // find second occurence of "."
+                if (second_dot_pos == std::string::npos)
                 {
                     ARMARX_WARNING << "Could not find a groupname for the sensor with name " << name << ". All sensors must be called sens.X.Y where X is the name of the group and Y is the actual sensor. Ignoring this sensor.";
                     continue;
                 }
-                groupName = name.substr(0, dot_pos);
+                groupName = name.substr(0, second_dot_pos);
+
+                jointOrWhateverName = name.substr(first_dot_pos + 1, second_dot_pos - first_dot_pos - 1);
+
+                name = name.substr(second_dot_pos + 1); // remove the groupName, TODO check if +1 is valid
             }
 
             RobotUnitData::RobotUnitDataGroup e;
@@ -205,6 +218,9 @@ namespace armarx
                 auto iterId = std::make_shared<aron::datanavigator::LongNavigator>(currentTimestep.iterationId);
                 dict->addElement("IterationId", iterId);
 
+                const auto nameId = std::make_shared<aron::datanavigator::StringNavigator>(jointOrWhateverName);
+                dict->addElement("name", nameId);
+
                 e.timestamp = currentTimestep.timestampUSec;
                 e.name = groupName;
                 e.data = dict;
@@ -294,7 +310,9 @@ namespace armarx
             auto start = std::chrono::high_resolution_clock::now();
 
             // send batch to memory
-            armem::data::Commit c;
+            armem::data::Commit proprioceptionCommit;
+            armem::data::Commit odometryCommit;
+
             for (unsigned int i = 0; i < robotUnitMemoryBatchSize; ++i)
             {
                 std::lock_guard g{robotUnitDataMutex};
@@ -309,15 +327,55 @@ namespace armarx
                     const auto& timeUSec = encTimestep.timestamp;
                     const auto& aron = encTimestep.data;
 
-                    armem::data::EntityUpdate& update = c.updates.emplace_back();
+                    armem::data::EntityUpdate& update = proprioceptionCommit.updates.emplace_back();
                     update.entityID = entityID;
                     update.timeCreatedMicroSeconds = timeUSec;
                     update.instancesData = {aron->toAronDictPtr()};
                 }
+
+                // odometry pose -> localization segment
+                auto it = convertedAndGroupedData.groups.find("sens.Platform");
+                if (it != convertedAndGroupedData.groups.end())
+                {
+                    ARMARX_DEBUG << " found odometry data.";
+
+                    const RobotUnitData::RobotUnitDataGroup& timestep = it->second;
+                    const float relPosX = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionX"))->getValue();
+                    const float relPosY = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionY"))->getValue();
+                    const float relOrientation = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionRotation"))->getValue();
+
+                    Eigen::Affine3f odometryPose = Eigen::Affine3f::Identity();
+                    odometryPose.translation() << relPosX, relPosY, 0; // TODO set height
+                    odometryPose.linear() = simox::math::rpy_to_mat3f(0.F, 0.F, relOrientation);
+
+                    // const auto timestamp = armem::Time::microSeconds(it->second.timestamp);
+                    const auto timestamp = armem::Time::now();
+
+                    const ::armarx::armem::robot_state::Transform transform
+                    {
+                        .header =
+                        {
+                            .parentFrame = OdometryFrame,
+                            .frame = "root", // TODO: robot root node
+                            .agent = robotUnitProviderID.providerSegmentName,
+                            .timestamp = timestamp
+                        },
+                        .transform = odometryPose
+                    };
+
+                    localizationSegment.storeTransform(transform);
+
+                }
+                else
+                {
+                    ARMARX_INFO << deactivateSpam(1000) << "No odometry data! If you are using a mobile platform this should not have happened";
+                }
+
+
                 robotUnitDataQueue.pop();
             }
 
-            auto results = commit(c);
+            auto results = commit(proprioceptionCommit);
             auto stop = std::chrono::high_resolution_clock::now();
             ARMARX_DEBUG << "RobotUnitData: The total runtime of sending a batch to the memory is: " << std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
 
@@ -442,4 +500,5 @@ namespace armarx
         robotUnitConversionTask->start();
         robotUnitStoringTask->start();
     }
+
 }