From 580e9fb76aa94ce0fc44c380c59510b4fe92921c Mon Sep 17 00:00:00 2001
From: ARMAR-6 <armar6@needs_you.com>
Date: Wed, 3 May 2023 18:15:37 +0200
Subject: [PATCH 1/2] Add object information in form of an ObjectID to
 ActionHypothesis

---
 .../armarx/manipulation/core/ActionHypothesis.cpp  | 13 +++++++++----
 source/armarx/manipulation/core/ActionHypothesis.h |  9 +++++++--
 .../manipulation/core/aron/ActionHypothesis.xml    |  4 ++++
 .../armarx/manipulation/core/aron_conversions.cpp  | 14 +++++++++++++-
 4 files changed, 33 insertions(+), 7 deletions(-)

diff --git a/source/armarx/manipulation/core/ActionHypothesis.cpp b/source/armarx/manipulation/core/ActionHypothesis.cpp
index ad6aaa86..50626d5d 100644
--- a/source/armarx/manipulation/core/ActionHypothesis.cpp
+++ b/source/armarx/manipulation/core/ActionHypothesis.cpp
@@ -23,6 +23,7 @@
 #include "ActionHypothesis.h"
 
 #include <memory>
+#include <utility>
 
 #include <RobotAPI/libraries/armem_robot/types.h>
 #include <RobotAPI/libraries/core/FramedPose.h>
@@ -40,13 +41,15 @@ namespace armarx::manipulation::core
                                        long obsTime,
                                        std::optional<arondto::Handedness> handed,
                                        std::optional<float> existCert,
-                                       std::optional<std::size_t> segmentLabel) :
+                                       std::optional<std::size_t> segmentLabel,
+                                       std::optional<armarx::ObjectID> object) :
         affordance(std::make_unique<Affordance>(aff)),
         framedPose(std::make_unique<FramedUncertainPose>(pose)),
         handedness(std::move(handed)),
         existenceCertainty(existCert),
         observationTime(obsTime),
-        segmentLabel(segmentLabel)
+        segmentLabel(segmentLabel),
+        objectID(object)
     {
     }
 
@@ -57,13 +60,15 @@ namespace armarx::manipulation::core
                                        std::optional<Eigen::Matrix6f> poseUncertainty,
                                        std::optional<arondto::Handedness> handed,
                                        std::optional<float> existCert,
-                                       std::optional<std::size_t> segmentLabel) :
+                                       std::optional<std::size_t> segmentLabel,
+                                       std::optional<armarx::ObjectID> object) :
         affordance(std::make_unique<Affordance>(actionType, robotDescription)),
         framedPose(std::make_unique<FramedUncertainPose>(pose, std::move(poseUncertainty))),
         handedness(std::move(handed)),
         existenceCertainty(existCert),
         observationTime(obsTime),
-        segmentLabel(segmentLabel)
+        segmentLabel(segmentLabel),
+        objectID(std::move(object))
     {
     }
 
diff --git a/source/armarx/manipulation/core/ActionHypothesis.h b/source/armarx/manipulation/core/ActionHypothesis.h
index 14f2054a..b24509d8 100644
--- a/source/armarx/manipulation/core/ActionHypothesis.h
+++ b/source/armarx/manipulation/core/ActionHypothesis.h
@@ -25,6 +25,8 @@
 #include <optional>
 #include <utility>
 
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+
 #include <armarx/manipulation/core/aron/ActionHypothesis.aron.generated.h>
 
 namespace Eigen
@@ -58,14 +60,16 @@ namespace armarx
                              std::optional<Eigen::Matrix6f> poseUncertainty = std::nullopt,
                              std::optional<arondto::Handedness> handed = std::nullopt,
                              std::optional<float> existCert = std::nullopt,
-                             std::optional<std::size_t> segmentLabel = std::nullopt);
+                             std::optional<std::size_t> segmentLabel = std::nullopt,
+                             std::optional<armarx::ObjectID> object = std::nullopt);
 
             ActionHypothesis(const Affordance& aff,
                              const FramedUncertainPose& pose,
                              long obsTime,
                              std::optional<arondto::Handedness> handed = std::nullopt,
                              std::optional<float> existCert = std::nullopt,
-                             std::optional<std::size_t> segmentLabel = std::nullopt);
+                             std::optional<std::size_t> segmentLabel = std::nullopt,
+                             std::optional<armarx::ObjectID> object = std::nullopt);
             ActionHypothesis(const ActionHypothesis&);
             ActionHypothesis& operator=(const ActionHypothesis&);
             ActionHypothesis(ActionHypothesis&&);
@@ -93,6 +97,7 @@ namespace armarx
 
             // if the ActionHypothesis was generated on a point cloud segment, its ID is stored here.
             std::optional<std::size_t> segmentLabel;
+            std::optional<armarx::ObjectID> objectID;
         };
     } // namespace manipulation::core
 
diff --git a/source/armarx/manipulation/core/aron/ActionHypothesis.xml b/source/armarx/manipulation/core/aron/ActionHypothesis.xml
index 4f0746f7..5b2a1338 100644
--- a/source/armarx/manipulation/core/aron/ActionHypothesis.xml
+++ b/source/armarx/manipulation/core/aron/ActionHypothesis.xml
@@ -4,6 +4,7 @@
         <Include include="armarx/manipulation/core/aron/Affordance.xml" />
         <Include include="armarx/manipulation/core/aron/FramedUncertainPose.xml" />
         <Include include="armarx/manipulation/core/aron/Handedness.xml" />
+        <PackagePath package="RobotAPI" path="libraries/ArmarXObjects/aron/ObjectID.xml" />
     </AronIncludes>
 
     <GenerateTypes>
@@ -27,6 +28,9 @@
             <ObjectChild key='segmentLabel'>
                 <float optional="true" />
             </ObjectChild>
+            <ObjectChild key='objectID'>
+                <armarx::arondto::ObjectID optional="true"/>
+            </ObjectChild>
         </Object>
     </GenerateTypes>
 </AronTypeDefinition>
diff --git a/source/armarx/manipulation/core/aron_conversions.cpp b/source/armarx/manipulation/core/aron_conversions.cpp
index a574d10c..f6f3dd1c 100644
--- a/source/armarx/manipulation/core/aron_conversions.cpp
+++ b/source/armarx/manipulation/core/aron_conversions.cpp
@@ -27,6 +27,7 @@
 #include <RobotAPI/libraries/armem_robot/aron_conversions.h>
 #include <RobotAPI/libraries/armem_robot/types.h>
 #include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
 
 #include <armarx/manipulation/core/ActionHypothesis.h>
 #include <armarx/manipulation/core/Affordance.h>
@@ -108,12 +109,18 @@ namespace armarx::manipulation::core
     ActionHypothesis
     fromAron(const arondto::ActionHypothesis& dto)
     {
+        std::optional<armarx::ObjectID> objID;
+        if (dto.objectID.has_value())
+        {
+            objID.emplace();
+            fromAron(dto.objectID, objID);
+        }
         return {fromAron(dto.affordance),
                 fromAron(dto.framedPose),
                 dto.observationTime,
                 dto.handedness,
                 dto.existenceCertainty,
-                dto.segmentLabel};
+                dto.segmentLabel, objID};
     }
 
     arondto::ExecutableAction
@@ -248,6 +255,11 @@ namespace armarx::manipulation::core
         dto.observationTime = bo.observationTime;
         dto.handedness = bo.handedness;
         dto.segmentLabel = bo.segmentLabel;
+        if (bo.objectID.has_value())
+        {
+            dto.objectID.emplace();
+            toAron(dto.objectID, bo.objectID);
+        }
     }
 
     void
-- 
GitLab


From 89704e55cae49bf0fb68ac05f52ee37033a4c0f9 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Wed, 3 May 2023 20:27:17 +0200
Subject: [PATCH 2/2] Cover objectID in copy constructor

---
 source/armarx/manipulation/core/ActionHypothesis.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/source/armarx/manipulation/core/ActionHypothesis.cpp b/source/armarx/manipulation/core/ActionHypothesis.cpp
index 50626d5d..0e133d2b 100644
--- a/source/armarx/manipulation/core/ActionHypothesis.cpp
+++ b/source/armarx/manipulation/core/ActionHypothesis.cpp
@@ -78,7 +78,8 @@ namespace armarx::manipulation::core
         handedness(hypothesis.handedness),
         existenceCertainty(hypothesis.existenceCertainty),
         observationTime(hypothesis.observationTime),
-        segmentLabel(hypothesis.segmentLabel)
+        segmentLabel(hypothesis.segmentLabel),
+        objectID(hypothesis.objectID)
     {
     }
 
@@ -91,6 +92,7 @@ namespace armarx::manipulation::core
         existenceCertainty = hypothesis.existenceCertainty;
         observationTime = hypothesis.observationTime;
         segmentLabel = hypothesis.segmentLabel;
+        objectID = hypothesis.objectID;
         return *this;
     }
 
-- 
GitLab