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