armem_robot(_state): fixing circular dependency

source/RobotAPI/libraries/armem_robot/aron_conversions.cpp

@@ -109,30 +109,5 @@ namespace armarx::armem
         dto.objectJointValues = bo.jointMap;
     }
     
-    void robot::fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo) 
-    {
-        bo.twist.linear.setZero();
-        bo.twist.linear.head<2>() = dto.velocity.head<2>(); // x and y
-
-        bo.twist.angular.setZero();
-        bo.twist.angular.z() = dto.velocity.z(); // yaw
-    }
-    
-    void robot::toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo) 
-    {
-        ARMARX_ERROR << "Not implemented yet.";
-    }
-
-     void robot::fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo) 
-    {
-        bo.force = dto.force;
-        bo.torque = dto.torque;
-    }
-    
-    void robot::toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo) 
-    {
-        dto.force = bo.force;
-        dto.torque = bo.torque;
-    }
 
 }  // namespace armarx::armem

source/RobotAPI/libraries/armem_robot/aron_conversions.h

@@ -8,7 +8,6 @@
 #include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
 #include <RobotAPI/libraries/armem_robot/aron/RobotState.aron.generated.h>
 #include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
 
 
 namespace armarx::armem::robot
@@ -27,12 +26,6 @@ namespace armarx::armem::robot
     void fromAron(const arondto::RobotState& dto, RobotState& bo);
     void toAron(arondto::RobotState& dto, const RobotState& bo);
 
-    void fromAron(const armarx::armem::prop::arondto::Platform& dto, PlatformState& bo);
-    void toAron(armarx::armem::prop::arondto::Platform& dto, const PlatformState& bo);
-
-    void fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, ForceTorque& bo);
-    void toAron(armarx::armem::prop::arondto::ForceTorque& dto, const ForceTorque& bo);
-
     void fromAron(const arondto::ObjectClass& dto, RobotDescription& bo);
     void toAron(arondto::ObjectClass& dto, const RobotDescription& bo);
 

source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp

 #include "aron_conversions.h"
 
-// STL
 #include <string>
 
-// Ice
 #include <IceUtil/Time.h>
 
-// RobotAPI
-#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <ArmarXCore/core/logging/Logging.h>
 #include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
-
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/types.h>
 #include <RobotAPI/libraries/aron/common/aron_conversions.h>
 
-#include "RobotAPI/libraries/armem_robot_state/types.h"
-
 namespace armarx::armem
 {
 
     /* Transform */
 
-    void fromAron(const arondto::Transform& dto, robot_state::Transform& bo)
+    void
+    fromAron(const arondto::Transform& dto, robot_state::Transform& bo)
     {
         fromAron(dto.header, bo.header);
         aron::fromAron(dto.transform, bo.transform);
     }
 
-    void toAron(arondto::Transform& dto, const robot_state::Transform& bo)
+    void
+    toAron(arondto::Transform& dto, const robot_state::Transform& bo)
     {
         toAron(dto.header, bo.header);
         aron::toAron(dto.transform, bo.transform);
@@ -33,7 +32,8 @@ namespace armarx::armem
 
     /* TransformHeader */
 
-    void toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo)
+    void
+    toAron(arondto::TransformHeader& dto, const robot_state::TransformHeader& bo)
     {
         aron::toAron(dto.parentFrame, bo.parentFrame);
         aron::toAron(dto.frame, bo.frame);
@@ -41,7 +41,8 @@ namespace armarx::armem
         dto.timestamp = bo.timestamp;
     }
 
-    void fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo)
+    void
+    fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo)
     {
         aron::fromAron(dto.parentFrame, bo.parentFrame);
         aron::fromAron(dto.frame, bo.frame);
@@ -51,16 +52,49 @@ namespace armarx::armem
 
     /* JointState */
 
-    void fromAron(const arondto::JointState& dto, robot_state::JointState& bo)
+    void
+    fromAron(const arondto::JointState& dto, robot_state::JointState& bo)
     {
         aron::fromAron(dto.name, bo.name);
         aron::fromAron(dto.position, bo.position);
     }
 
-    void toAron(arondto::JointState& dto, const robot_state::JointState& bo)
+    void
+    toAron(arondto::JointState& dto, const robot_state::JointState& bo)
     {
         aron::toAron(dto.name, bo.name);
         aron::toAron(dto.position, bo.position);
     }
 
-}  // namespace armarx::armem
+
+    void
+    fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo)
+    {
+        bo.twist.linear.setZero();
+        bo.twist.linear.head<2>() = dto.velocity.head<2>(); // x and y
+
+        bo.twist.angular.setZero();
+        bo.twist.angular.z() = dto.velocity.z(); // yaw
+    }
+
+    void
+    toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo)
+    {
+        ARMARX_ERROR << "Not implemented yet.";
+    }
+
+    void
+    fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo)
+    {
+        bo.force = dto.force;
+        bo.torque = dto.torque;
+    }
+
+    void
+    toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo)
+    {
+        dto.force = bo.force;
+        dto.torque = bo.torque;
+    }
+
+} // namespace armarx::armem

source/RobotAPI/libraries/armem_robot_state/aron_conversions.h

@@ -21,6 +21,9 @@
 
 #pragma once
 
+#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+
 namespace armarx::armem
 {
     namespace robot_state
@@ -41,6 +44,7 @@ namespace armarx::armem
 
     } // namespace arondto
 
+
     void fromAron(const arondto::Transform& dto, robot_state::Transform& bo);
     void toAron(arondto::Transform& dto, const robot_state::Transform& bo);
 
@@ -50,4 +54,11 @@ namespace armarx::armem
     void fromAron(const arondto::JointState& dto, robot_state::JointState& bo);
     void toAron(arondto::JointState& dto, const robot_state::JointState& bo);
 
-}  // namespace armarx::armem
\ No newline at end of file
+    void fromAron(const armarx::armem::prop::arondto::Platform& dto, robot::PlatformState& bo);
+    void toAron(armarx::armem::prop::arondto::Platform& dto, const robot::PlatformState& bo);
+
+    void fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo);
+    void toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo);
+
+
+}  // namespace armarx::armem

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

@@ -4,11 +4,10 @@
 #include <optional>
 
 #include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "RobotAPI/libraries/armem_robot/types.h"
 #include <ArmarXCore/core/PackagePath.h>
 #include <ArmarXCore/core/exceptions/LocalException.h>
 #include <ArmarXCore/core/logging/Logging.h>
-
-#include "RobotAPI/libraries/armem_robot/types.h"
 #include <RobotAPI/libraries/armem/client/query/Builder.h>
 #include <RobotAPI/libraries/armem/core/Time.h>
 #include <RobotAPI/libraries/armem/core/error.h>
@@ -18,6 +17,7 @@
 #include <RobotAPI/libraries/armem_robot/robot_conversions.h>
 #include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
 #include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
 
 
 namespace fs = ::std::filesystem;
@@ -508,7 +508,7 @@ namespace armarx::armem::robot_state
                 ARMARX_CHECK(proprioception.has_value());
 
                 platformState = robot::PlatformState(); // initialize optional
-                robot::fromAron(proprioception->platform, platformState.value());
+                fromAron(proprioception->platform, platformState.value());
             });
 
         return platformState;
@@ -554,7 +554,7 @@ namespace armarx::armem::robot_state
                 for (const auto& [handName, dtoFt] : proprioception->forceTorque)
                 {
                     robot::ForceTorque forceTorque;
-                    robot::fromAron(dtoFt, forceTorque);
+                    fromAron(dtoFt, forceTorque);
 
                     const auto hand = fromHandName(handName);
                     forceTorques.emplace(hand, forceTorque);
@@ -588,7 +588,7 @@ namespace armarx::armem::robot_state
                 for (const auto& [handName, dtoFt] : proprioception->forceTorque)
                 {
                     robot::ForceTorque forceTorque;
-                    robot::fromAron(dtoFt, forceTorque);
+                    fromAron(dtoFt, forceTorque);
 
                     const auto hand = fromHandName(handName);
                     forceTorques[hand].emplace(entityInstance.id().timestamp, forceTorque);
@@ -612,7 +612,7 @@ namespace armarx::armem::robot_state
         const armem::wm::EntityInstance* instance = nullptr;
         providerSegment.forEachInstance([&instance](const wm::EntityInstance& i)
                                         { instance = &i; });
-        if (!instance)
+        if (instance == nullptr)
         {
             ARMARX_WARNING << "No entity snapshots found";
             return std::nullopt;