odometry streaming

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

@@ -6,6 +6,7 @@
 #include <ArmarXCore/core/time/TimeUtil.h>
 #include "ArmarXCore/core/logging/Logging.h"
 
+#include "RobotAPI/libraries/armem_robot_state/types.h"
 #include "RobotAPI/libraries/aron/common/aron_conversions.h"
 
 #include <RobotAPI/libraries/armem/core/aron_conversions.h>
@@ -18,9 +19,12 @@
 
 #include "RobotAPI/libraries/armem_robot/robot_conversions.h"
 #include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
 
 #include <RobotAPI/libraries/armem_objects/aron_conversions.h>
 
+#include <RobotAPI/libraries/armem/util/util.h>
+
 
 namespace armarx::armem::server::robot_state::proprioception
 {
@@ -45,7 +49,7 @@ namespace armarx::armem::server::robot_state::proprioception
         ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
 
         ARMARX_INFO << "Adding core segment '" <<  p.coreSegment << "'";
-        coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegment); // TODO , arondto::Robot::toInitialAronType());
+        coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegment);
         coreSegment->setMaxHistorySize(p.maxHistorySize);
     }
 
@@ -83,32 +87,31 @@ namespace armarx::armem::server::robot_state::proprioception
         robotUnitProviderID.memoryName = iceMemory.workingMemory->id().memoryName;
         robotUnitProviderID.coreSegmentName = p.coreSegment;
         robotUnitProviderID.providerSegmentName = providerSegmentName;
-
-        // this->visu = std::make_unique<Visu>(arviz, *this);
     }
 
     std::unordered_map<std::string, std::map<std::string, float>> Segment::getRobotJointPositions() const
     {
         std::unordered_map<std::string, std::map<std::string, float>> jointMap;
 
-        // for (const auto& [provName, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
-        // {
-        //     for (const auto& [entityName, entity] :  provSeg.entities())
-        //     {
-        //         const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
-        //         const auto description = articulated_object::convertRobotDescription(entityInstance);
+        for (const auto& [robotName, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
+        {
+            for (const auto& [name, entity] :  provSeg.entities())
+            {
+                const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
 
-        //         if (not description)
-        //         {
-        //             ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
-        //             continue;
-        //         }
+                const auto jointState = tryCast<armarx::armem::arondto::JointState>(entityInstance);
 
-        //         ARMARX_INFO << "Key is " << armem::MemoryID(entity.id());
+                if (not jointState)
+                {
+                    // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
+                    continue;
+                }
+
+                jointMap[robotName].emplace(jointState->name, jointState->position);
+            }
+        }
 
-        //         objects.emplace(armem::MemoryID(entity.id()), *description);
-        //     }
-        // }
+        ARMARX_INFO << deactivateSpam(10) <<  "Number of known robot joint maps: " << jointMap.size();
 
         return jointMap;
     }