From 3f264804fa8acf0fa563009a772a5b1d56cb5d55 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Fri, 23 Apr 2021 17:30:28 +0200
Subject: [PATCH] Add class (memory) ID to instance
---
.../armem_objects/aron/ObjectInstance.xml | 3 ++
.../armem_objects/server/instance/Segment.cpp | 38 ++++++++++++++++---
.../armem_objects/server/instance/Segment.h | 3 ++
3 files changed, 38 insertions(+), 6 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_objects/aron/ObjectInstance.xml b/source/RobotAPI/libraries/armem_objects/aron/ObjectInstance.xml
index adcaa6b38..774c6e0d5 100644
--- a/source/RobotAPI/libraries/armem_objects/aron/ObjectInstance.xml
+++ b/source/RobotAPI/libraries/armem_objects/aron/ObjectInstance.xml
@@ -19,6 +19,9 @@ Core segment type of Object/Instance.
<armarx::objpose::arondto::ObjectPose />
</ObjectChild>
+ <ObjectChild key="classID">
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
<ObjectChild key="sourceID">
<armarx::armem::arondto::MemoryID />
</ObjectChild>
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index fb1414b1c..d831d5dee 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -2,17 +2,19 @@
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include <RobotAPI/libraries/core/Pose.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/Visitor.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
-#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/armem/core/Visitor.h>
-#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <ArmarXCore/core/time/TimeUtil.h>
@@ -181,6 +183,11 @@ namespace armarx::armem::server::obj::instance
arondto::ObjectInstance dto;
toAron(dto, pose);
+ // Search for object class.
+ if (auto instance = findClassInstance(pose.objectID))
+ {
+ toAron(dto.classID, instance->id());
+ }
update.instancesData.push_back(dto.toAron());
}
@@ -640,6 +647,25 @@ namespace armarx::armem::server::obj::instance
}
+ std::optional<EntityInstance> Segment::findClassInstance(const ObjectID& objectID)
+ {
+ const ObjectID classID = { objectID.dataset(), objectID.className() };
+ try
+ {
+ for (const auto& [_, provSeg] : iceMemory.memory->getCoreSegment("Class"))
+ {
+ return provSeg.getEntity(classID.str()).getLatestSnapshot().getInstance(0);
+ }
+ return std::nullopt;
+ }
+ catch (const armem::error::ArMemError&)
+ {
+ // Some segment or entity did not exist.
+ return std::nullopt;
+ }
+ }
+
+
void Segment::RemoteGui::setup(const Segment& data)
{
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index 941f5a76d..4a1e69742 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -128,6 +128,9 @@ namespace armarx::armem::server::obj::instance
bool commitAttachedPose);
+ std::optional<EntityInstance> findClassInstance(const ObjectID& objectID);
+
+
friend struct DetachVisitor;
--
GitLab