robotstatememory: changed field name of streamed data: removing e.g...

robotstatememory: changed field name of streamed data: removing e.g 'sens.Platform' as it prevents direct casting (e.g. JointState)

source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp

@@ -31,6 +31,7 @@
 
 // Header
 #include "ArmarXCore/core/logging/Logging.h"
+#include "RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.h"
 #include "RobotAPI/libraries/core/Pose.h"
 
 // Simox
@@ -50,6 +51,7 @@
 #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
 {
@@ -59,8 +61,10 @@ namespace armarx::armem::server::robot_state
           proprioceptionSegment(server::ComponentPluginUser::iceMemory,
                                 server::ComponentPluginUser::workingMemoryMutex),
           localizationSegment(server::ComponentPluginUser::iceMemory,
-                              server::ComponentPluginUser::workingMemoryMutex)
+                              server::ComponentPluginUser::workingMemoryMutex),
+          commonVisu(descriptionSegment, proprioceptionSegment, localizationSegment)
     {
+
     }
 
     RobotStateMemory::~RobotStateMemory() = default;
@@ -77,6 +81,7 @@ namespace armarx::armem::server::robot_state
         descriptionSegment.defineProperties(defs);
         proprioceptionSegment.defineProperties(defs);
         localizationSegment.defineProperties(defs);
+        commonVisu.defineProperties(defs);
 
         return defs;
     }
@@ -94,6 +99,7 @@ namespace armarx::armem::server::robot_state
         descriptionSegment.init();
         proprioceptionSegment.init();
         localizationSegment.init();
+        commonVisu.init();
 
         robotUnitMemoryBatchSize = std::max((unsigned int) 1, robotUnitMemoryBatchSize);
         robotUnitPollFrequency = std::clamp(robotUnitPollFrequency, 1, ROBOT_UNIT_MAXIMUM_FREQUENCY);
@@ -130,8 +136,9 @@ namespace armarx::armem::server::robot_state
 
         localizationSegment.connect(getArvizClient());
 
-        commonVisu = std::make_unique<Visu>(getArvizClient(), descriptionSegment, proprioceptionSegment, localizationSegment);
-        commonVisu->init();
+        commonVisu.connect(
+            getArvizClient()
+        );
 
         cfg.loggingNames.emplace_back(robotUnitSensorPrefix);
         handler = make_shared<RobotUnitDataStreamingReceiver>(this, getRobotUnit(), cfg);
@@ -171,8 +178,11 @@ namespace armarx::armem::server::robot_state
         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())
             {
@@ -180,13 +190,18 @@ namespace armarx::armem::server::robot_state
             }
             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;
@@ -200,6 +215,9 @@ namespace armarx::armem::server::robot_state
                 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;
@@ -320,9 +338,9 @@ namespace armarx::armem::server::robot_state
                     ARMARX_INFO << " found odometry data.";
 
                     const RobotUnitData::RobotUnitDataGroup& timestep = it->second;
-                    const float relPosX = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("sens.Platform.relativePositionX"))->getValue();
-                    const float relPosY = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("sens.Platform.relativePositionY"))->getValue();
-                    const float relOrientation = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("sens.Platform.relativePositionRotation"))->getValue();
+                    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

source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h

@@ -44,10 +44,10 @@
 #include <RobotAPI/libraries/armem_robot_state/server/description/Segment.h>
 #include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
 #include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
+#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
 
 namespace armarx::armem::server::robot_state
 {
-    class Visu;
 
     /**
      * @defgroup Component-RobotSensorMemory RobotSensorMemory
@@ -116,7 +116,7 @@ namespace armarx::armem::server::robot_state
         localization::Segment localizationSegment;
 
         // Joint visu for all segments => robot pose and configuration
-        std::unique_ptr<Visu> commonVisu;
+        Visu commonVisu;
 
         // RobotUnit stuff
         RobotUnitDataStreaming::DataStreamingDescription keys;