From 5a2afce30b4827a9d13e80490ef5ad25fc1e6971 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 29 Jun 2021 18:03:43 +0200
Subject: [PATCH] Add conversion for ObjectClass/Instance/Pose and
 RobotDescription/State

---
 .../armem_robot/aron_conversions.cpp          | 50 +++++++++++++++++++
 .../libraries/armem_robot/aron_conversions.h  | 16 +++++-
 2 files changed, 65 insertions(+), 1 deletion(-)

diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
index 601f17d65..da72cc16f 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
@@ -1,9 +1,15 @@
 #include "aron_conversions.h"
 
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+
 #include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
 #include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
 #include <RobotAPI/libraries/aron/common/aron_conversions/armarx.h>
 
+
 namespace armarx::armem::robot
 {
 
@@ -59,3 +65,47 @@ namespace armarx::armem::robot
     }
 
 }  // namespace armarx::armem::robot
+
+
+namespace armarx::armem
+{
+
+    void robot::fromAron(const arondto::ObjectClass& dto, RobotDescription& bo)
+    {
+        bo.name = aron::fromAron<armarx::ObjectID>(dto.id).str();
+        fromAron(dto.articulatedSimoxXmlPath, bo.xml);
+    }
+
+    void robot::toAron(arondto::ObjectClass& dto, const RobotDescription& bo)
+    {
+        toAron(dto.id, ObjectID(bo.name));
+        toAron(dto.articulatedSimoxXmlPath, bo.xml);
+    }
+
+
+    void robot::fromAron(const arondto::ObjectInstance& dto, RobotState& bo)
+    {
+        fromAron(dto.pose, bo);
+    }
+
+    void robot::toAron(arondto::ObjectInstance& dto, const RobotState& bo)
+    {
+        toAron(dto.pose, bo);
+    }
+
+
+    void robot::fromAron(const objpose::arondto::ObjectPose& dto, RobotState& bo)
+    {
+        bo.timestamp = dto.timestamp;
+        bo.globalPose = dto.objectPoseGlobal;
+        bo.jointMap = dto.objectJointValues;
+    }
+
+    void robot::toAron(objpose::arondto::ObjectPose& dto, const RobotState& bo)
+    {
+        dto.timestamp = bo.timestamp;
+        dto.objectPoseGlobal = bo.globalPose.matrix();
+        dto.objectJointValues = bo.jointMap;
+    }
+
+}  // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.h b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
index 70e06ce8e..9c124d758 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.h
@@ -1,11 +1,15 @@
 #pragma once
 
-#include <RobotAPI/libraries/armem_robot/types.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
 
+#include <RobotAPI/libraries/armem_robot/types.h>
 #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>
 
+
 namespace armarx::armem::robot
 {
     // TODO move the following
@@ -22,4 +26,14 @@ namespace armarx::armem::robot
     void fromAron(const arondto::RobotState& dto, RobotState& bo);
     void toAron(arondto::RobotState& dto, const RobotState& bo);
 
+
+    void fromAron(const arondto::ObjectClass& dto, RobotDescription& bo);
+    void toAron(arondto::ObjectClass& dto, const RobotDescription& bo);
+
+    void fromAron(const arondto::ObjectInstance& dto, RobotState& bo);
+    void toAron(arondto::ObjectInstance& dto, const RobotState& bo);
+
+    void fromAron(const objpose::arondto::ObjectPose& dto, RobotState& bo);
+    void toAron(objpose::arondto::ObjectPose& dto, const RobotState& bo);
+
 }  // namespace armarx::armem::robot
-- 
GitLab