diff --git a/source/RobotAPI/libraries/armem_objects/aron/ObjectInstance.xml b/source/RobotAPI/libraries/armem_objects/aron/ObjectInstance.xml index adcaa6b389a838e5d105e4fa1de4808c90ae67a9..774c6e0d5d32734a16d7fdc918f491d19cfbfa39 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 fb1414b1cb889c1842b71e6426050c6d6c37b33c..d831d5dee213b69da6eb29a063e466d30456d867 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 941f5a76d9f1b9d6ae9811843da6847a3a0863d8..4a1e697427e6e64c07320f17cc12988954f6ba10 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;