From 227e646094f60c3d41ef51bfc6213b941a13bad6 Mon Sep 17 00:00:00 2001
From: Fabian Peller <fabian.peller-konrad@kit.edu>
Date: Wed, 15 Nov 2023 15:54:53 +0100
Subject: [PATCH] add features to objectclass
---
.../armem_objects/aron/ObjectClass.xml | 30 +++++++++
.../armem_objects/aron_conversions.cpp | 65 +++++++++++++++++++
.../armem_objects/aron_conversions.h | 34 ++++++----
.../RobotAPI/libraries/armem_objects/types.h | 35 +++++++++-
4 files changed, 150 insertions(+), 14 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
index e2d695429..019987f50 100644
--- a/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
+++ b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
@@ -13,6 +13,30 @@ Core segment type of Object/Class.
</AronIncludes>
<GenerateTypes>
+ <Object name="armarx::armem::arondto::Feature">
+ <ObjectChild key="angle">
+ <float32 />
+ </ObjectChild>
+
+ <ObjectChild key="scale">
+ <float32 />
+ </ObjectChild>
+
+ <ObjectChild key="point2d">
+ <vector2f />
+ </ObjectChild>
+
+ <ObjectChild key="point3d">
+ <vector3f />
+ </ObjectChild>
+
+ <ObjectChild key="feature">
+ <List>
+ <float32 />
+ </List>
+ </ObjectChild>
+ </Object>
+
<Object name="armarx::armem::arondto::ObjectClass">
<ObjectChild key="id">
@@ -62,6 +86,12 @@ Core segment type of Object/Class.
<armarx::arondto::ObjectNames />
</ObjectChild>
+ <ObjectChild key="ivtFeatures">
+ <List>
+ <armarx::armem::arondto::Feature />
+ </List>
+ </ObjectChild>
+
</Object>
</GenerateTypes>
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
index 6b3f3c184..da32c2771 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.cpp
@@ -111,6 +111,71 @@ namespace armarx::armem
} // namespace armarx::armem
+namespace armarx::armem::clazz
+{
+
+ void
+ fromAron(const arondto::Feature& dto, Feature& bo)
+ {
+ bo.angle = dto.angle;
+ bo.scale = dto.scale;
+ bo.feature = dto.feature;
+ bo.point2d = dto.point2d;
+ bo.point3d = dto.point3d;
+ }
+
+ void
+ toAron(arondto::Feature& dto, const Feature& bo)
+ {
+ dto.angle = bo.angle;
+ dto.scale = bo.scale;
+ dto.feature = bo.feature;
+ dto.point2d = bo.point2d;
+ dto.point3d = bo.point3d;
+ }
+
+ void
+ fromAron(const arondto::ObjectClass& dto, ObjectClass& bo)
+ {
+ armarx::fromAron(dto.id, bo.id);
+ armarx::fromAron(dto.simoxXmlPath, bo.simoxXmlPath);
+ armarx::fromAron(dto.articulatedSimoxXmlPath, bo.articulatedSimoxXmlPath);
+ armarx::fromAron(dto.urdfPath, bo.urdfPath);
+ armarx::fromAron(dto.articulatedUrdfPath, bo.articulatedUrdfPath);
+ armarx::fromAron(dto.sdfPath, bo.sdfPath);
+ armarx::fromAron(dto.meshWrlPath, bo.meshWrlPath);
+ armarx::fromAron(dto.meshObjPath, bo.meshObjPath);
+ armarx::fromAron(dto.aabb, bo.aabb);
+ armarx::fromAron(dto.oobb, bo.oobb);
+ bo.ivtFeatures.clear();
+ for (const auto& i : dto.ivtFeatures)
+ {
+ fromAron(i, bo.ivtFeatures.emplace_back());
+ }
+ }
+
+ void
+ toAron(arondto::ObjectClass& dto, const ObjectClass& bo)
+ {
+ armarx::toAron(dto.id, bo.id);
+ armarx::toAron(dto.simoxXmlPath, bo.simoxXmlPath);
+ armarx::toAron(dto.articulatedSimoxXmlPath, bo.articulatedSimoxXmlPath);
+ armarx::toAron(dto.urdfPath, bo.urdfPath);
+ armarx::toAron(dto.articulatedUrdfPath, bo.articulatedUrdfPath);
+ armarx::toAron(dto.sdfPath, bo.sdfPath);
+ armarx::toAron(dto.meshWrlPath, bo.meshWrlPath);
+ armarx::toAron(dto.meshObjPath, bo.meshObjPath);
+ armarx::toAron(dto.aabb, bo.aabb);
+ armarx::toAron(dto.oobb, bo.oobb);
+ dto.ivtFeatures.clear();
+ for (const auto& i : bo.ivtFeatures)
+ {
+ toAron(dto.ivtFeatures.emplace_back(), i);
+ }
+ }
+
+} // namespace armarx::armem::clazz
+
armarx::armem::MemoryID
armarx::armem::obj::makeObjectInstanceMemoryID(const objpose::ObjectPose& objectPose)
{
diff --git a/source/RobotAPI/libraries/armem_objects/aron_conversions.h b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
index e27ed16c4..b61164a5d 100644
--- a/source/RobotAPI/libraries/armem_objects/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_objects/aron_conversions.h
@@ -1,11 +1,10 @@
#pragma once
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-
-#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/Marker.aron.generated.h>
-
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/types.h>
namespace armarx::armem
@@ -18,19 +17,32 @@ namespace armarx::armem
/* Attachments */
- void fromAron(const arondto::attachment::AgentDescription& dto, attachment::AgentDescription& bo);
+ void fromAron(const arondto::attachment::AgentDescription& dto,
+ attachment::AgentDescription& bo);
void toAron(arondto::attachment::AgentDescription& dto, const attachment::AgentDescription& bo);
- void fromAron(const arondto::attachment::ObjectAttachment& dto, attachment::ObjectAttachment& bo);
+ void fromAron(const arondto::attachment::ObjectAttachment& dto,
+ attachment::ObjectAttachment& bo);
void toAron(arondto::attachment::ObjectAttachment& dto, const attachment::ObjectAttachment& bo);
- void fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto, attachment::ArticulatedObjectAttachment& bo);
- void toAron(arondto::attachment::ArticulatedObjectAttachment& dto, const attachment::ArticulatedObjectAttachment& bo);
+ void fromAron(const arondto::attachment::ArticulatedObjectAttachment& dto,
+ attachment::ArticulatedObjectAttachment& bo);
+ void toAron(arondto::attachment::ArticulatedObjectAttachment& dto,
+ const attachment::ArticulatedObjectAttachment& bo);
+
+ void fromAron(const arondto::Marker& dto, marker::Marker& bo);
+ void toAron(arondto::Marker& dto, const marker::Marker& bo);
+} // namespace armarx::armem
+
+namespace armarx::armem::clazz
+{
+ void fromAron(const armarx::armem::arondto::Feature& dto, Feature& bo);
+ void toAron(armarx::armem::arondto::Feature& dto, const Feature& bo);
- void fromAron(const arondto::Marker& dto, marker::Marker&bo);
- void toAron(arondto::Marker& dto, const marker::Marker&bo);
-} // namespace armarx::armem
+ void fromAron(const armarx::armem::arondto::ObjectClass& dto, ObjectClass& bo);
+ void toAron(armarx::armem::arondto::ObjectClass& dto, const ObjectClass& bo);
+} // namespace armarx::armem::clazz
#include <RobotAPI/libraries/armem/core/MemoryID.h>
@@ -38,4 +50,4 @@ namespace armarx::armem::obj
{
/// Make a Memory ID for the object instance snapshot representing this pose.
MemoryID makeObjectInstanceMemoryID(const objpose::ObjectPose& objectPose);
-}
+} // namespace armarx::armem::obj
diff --git a/source/RobotAPI/libraries/armem_objects/types.h b/source/RobotAPI/libraries/armem_objects/types.h
index 79fe9ac95..598c78ca9 100644
--- a/source/RobotAPI/libraries/armem_objects/types.h
+++ b/source/RobotAPI/libraries/armem_objects/types.h
@@ -23,6 +23,8 @@
#include <Eigen/Geometry>
+#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/Time.h>
@@ -31,7 +33,6 @@
#include "aron_forward_declarations.h"
-
namespace armarx::armem
{
struct ObjectInstance
@@ -40,7 +41,36 @@ namespace armarx::armem
MemoryID classID;
MemoryID sourceID;
};
-}
+} // namespace armarx::armem
+
+namespace armarx::armem::clazz
+{
+ struct Feature
+ {
+ float angle;
+ float scale;
+ Eigen::Vector2f point2d;
+ Eigen::Vector3f point3d;
+ std::vector<float> feature;
+ };
+
+ struct ObjectClass
+ {
+ armarx::ObjectID id;
+ armarx::PackagePath simoxXmlPath;
+ armarx::PackagePath articulatedSimoxXmlPath;
+ armarx::PackagePath urdfPath;
+ armarx::PackagePath articulatedUrdfPath;
+ armarx::PackagePath sdfPath;
+ armarx::PackagePath articulatedSdfPath;
+ armarx::PackagePath meshWrlPath;
+ armarx::PackagePath meshObjPath;
+ simox::AxisAlignedBoundingBox aabb;
+ simox::OrientedBoxf oobb;
+ // TODO NAMES
+ std::vector<Feature> ivtFeatures;
+ };
+} // namespace armarx::armem::clazz
namespace armarx::armem::attachment
{
@@ -110,7 +140,6 @@ namespace armarx::armem::articulated_object
using ArticulatedObjects = armarx::armem::robot::Robots;
} // namespace armarx::armem::articulated_object
-
namespace armarx::armem::marker
{
class Marker
--
GitLab