fixing armem: ArticulatedObjectReader, RobotReader (changes to memory data types ...)

source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp

@@ -43,12 +43,15 @@ namespace armarx::armem::articulated_object
             return nullptr;
         }
 
+        ARMARX_DEBUG << "Object " << name << " available";
+
         auto obj =
             VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(it->xml.serialize().path),
                                              VirtualRobot::RobotIO::eStructure);
 
         if (not obj)
         {
+            ARMARX_WARNING << "Failed to load object: " << name;
             return nullptr;
         }
 
@@ -58,4 +61,4 @@ namespace armarx::armem::articulated_object
         return obj;
     }
 
-} // namespace armarx::armem::articulated_object
\ No newline at end of file
+} // namespace armarx::armem::articulated_object

source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp

@@ -2,7 +2,9 @@
 
 #include <mutex>
 #include <optional>
+#include <Eigen/src/Geometry/Transform.h>
 
+#include "RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h"
 #include <ArmarXCore/core/PackagePath.h>
 #include <ArmarXCore/core/logging/Logging.h>
 
@@ -188,7 +190,7 @@ namespace armarx::armem::articulated_object
         // clang-format off
         qb
         .coreSegments().withName(properties.coreClassSegmentName)
-        .providerSegments().all() // TODO(fabian.reister): think about this: which authority is trustworthy?
+        .providerSegments().withName(properties.providerName)
         .entities().all() // withName(name)
         .snapshots().latest();
         // clang-format on
@@ -231,11 +233,38 @@ namespace armarx::armem::articulated_object
             return std::nullopt;
         }
 
-        return getRobotState(qResult.memory);
+        return getArticulatedObjectState(qResult.memory);
+    }
+
+
+    std::optional<robot::RobotState> convertToRobotState(const armem::wm::EntityInstance& instance)
+    {
+        armarx::armem::arondto::ObjectInstance aronObjectInstance;
+        try
+        {
+            aronObjectInstance.fromAron(instance.data());
+        }
+        catch (...)
+        {
+            ARMARX_WARNING << "Conversion to ObjectInstance failed!";
+            return std::nullopt;
+        }
+
+        objpose::ObjectPose objectPose;
+        objpose::fromAron(aronObjectInstance.pose, objectPose);
+
+        robot::RobotState robotState
+        {
+            .timestamp = objectPose.timestamp,
+            .globalPose = Eigen::Affine3f(objectPose.objectPoseRobot),
+            .jointMap = objectPose.objectJointValues
+        };
+
+        return robotState;
     }
 
     std::optional<robot::RobotState>
-    Reader::getRobotState(const armarx::armem::wm::Memory& memory) const
+    Reader::getArticulatedObjectState(const armarx::armem::wm::Memory& memory) const
     {
         // clang-format off
         const armem::wm::CoreSegment& coreSegment = memory
@@ -247,14 +276,16 @@ namespace armarx::armem::articulated_object
         {
             instance = i;
         });
+        
         if (instance.has_value())
         {
-            return robot::convertRobotState(instance.value());
-        }
-        else
-        {
-            return std::nullopt;
+            return convertToRobotState(instance.value());
+            // return robot::convertRobotState(instance.value());
         }
+        
+        ARMARX_FATAL << "Failed to obtain robot state";
+        return std::nullopt;
+       
     }
 
 

source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h

@@ -66,7 +66,7 @@ namespace armarx::armem::articulated_object
 
     protected:
         std::optional<robot::RobotState>
-        getRobotState(const armarx::armem::wm::Memory& memory) const;
+        getArticulatedObjectState(const armarx::armem::wm::Memory& memory) const;
         std::optional<robot::RobotDescription>
         getRobotDescription(const armarx::armem::wm::Memory& memory) const;
         std::vector<robot::RobotDescription>

source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp

@@ -3,6 +3,7 @@
 #include <mutex>
 #include <optional>
 
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
 #include <ArmarXCore/core/exceptions/LocalException.h>
 #include <ArmarXCore/core/logging/Logging.h>
 #include <ArmarXCore/core/PackagePath.h>
@@ -15,6 +16,7 @@
 #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_robot_state/aron/Proprioception.aron.generated.h>
 
 
 namespace fs = ::std::filesystem;
@@ -172,6 +174,8 @@ namespace armarx::armem::robot_state
         // Query all entities from provider.
         armem::client::query::Builder qb;
 
+        ARMARX_DEBUG << "Querying robot description for robot: " << description;
+
         // clang-format off
         qb
         .coreSegments().withName(properties.proprioceptionCoreSegment)
@@ -276,14 +280,23 @@ namespace armarx::armem::robot_state
         {
             const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
 
-            const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
-            if (not jointState)
-            {
-                // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
-                return;
-            }
+            const auto proprioception = tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
+            ARMARX_CHECK(proprioception.has_value());
+
+            const armarx::armem::prop::arondto::Joints& joints = proprioception->joints;
+            
+
+
+            // const auto jointState = tryCast<::armarx::armem::arondto::JointState>(entityInstance);
+            // if (not jointState)
+            // {
+            //     ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
+            //     return;
+            // }
+
+            jointMap = joints.position;
 
-            jointMap.emplace(jointState->name, jointState->position);
+            // jointMap.emplace(jointState->name, jointState->position);
         });
 
         if (jointMap.empty())