diff --git a/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.cpp b/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.cpp index ca365080d21ef1ca6eab5c779d50231e4f1b007a..de1a50c49b2ba21bf256c74dfca3886fa1b12dc9 100644 --- a/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.cpp +++ b/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.cpp @@ -36,10 +36,10 @@ namespace armarx::armem::obj::clazz { armem::client::query::Builder builder; auto& entities = builder.coreSegments() - .withName(properties().coreSegmentName) - .providerSegments() - .all() - .entities(); + .withName(properties().coreSegmentName) + .providerSegments() + .all() + .entities(); for (const ObjectID& objectID : objectIDs) { @@ -67,6 +67,40 @@ namespace armarx::armem::obj::clazz return objectClasses; } + std::map<ObjectID, armem::clazz::ObjectClass> + ClassReader::getAllObjectClasses() + { + armem::client::query::Builder builder; + + // clang-format off + builder + .coreSegments().withName(properties().coreSegmentName) + .providerSegments().all() + .entities().all() + .snapshots().latest(); + // clang-format on + + const armem::client::QueryResult result = memoryReader().query(builder); + if (not result.success) + { + throw armem::error::QueryFailed(result.errorMessage); + } + + std::map<ObjectID, armem::clazz::ObjectClass> objectClasses; + + result.memory.forEachInstance( + [&objectClasses](const armem::wm::EntityInstance& instance) -> bool + { + const ObjectID classID = ObjectID::FromString(instance.id().entityName); + auto aron = instance.dataAs<armarx::armem::arondto::ObjectClass>(); + armarx::armem::clazz::fromAron(aron, objectClasses[classID]); + + return true; + }); + + return objectClasses; + } + std::string ClassReader::propertyPrefix() const { diff --git a/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.h b/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.h index 8dd50018a7483d7ce645475dd49f3dd840fe8f75..4fe1943895372fd26c1e7fda7a59dd5b77c42ee6 100644 --- a/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.h +++ b/source/RobotAPI/libraries/armem_objects/client/class/ClassReader.h @@ -46,6 +46,8 @@ namespace armarx::armem::obj::clazz std::map<armarx::ObjectID, armem::clazz::ObjectClass> getObjectClasses(const std::vector<armarx::ObjectID>& objectIDs); + std::map<armarx::ObjectID, armem::clazz::ObjectClass> getAllObjectClasses(); + protected: std::string propertyPrefix() const final; Properties defaultProperties() const final; diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp index 1b4b0dd2ea571562291ddffcdf70b1c438b15fef..d100459622099a65dded88c216950cae1ff1841f 100644 --- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp +++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp @@ -10,6 +10,7 @@ #include <ArmarXCore/core/exceptions/local/ExpressionException.h> #include <ArmarXCore/core/logging/Logging.h> #include <ArmarXCore/core/time/Clock.h> +#include <ArmarXCore/core/time/Duration.h> #include "RobotAPI/libraries/armem_robot_state/client/common/constants.h" #include "RobotAPI/libraries/armem_robot_state/common/localization/types.h" @@ -49,11 +50,14 @@ namespace armarx::armem::robot_state { transformReader.connect(memoryNameSystem); + armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(200)); + // Wait for the memory to become available and add it as dependency. ARMARX_INFO << "RobotReader: Waiting for memory '" << constants::memoryName << "' ..."; try { - memoryReader = memoryNameSystem.useReader(constants::memoryName); + // memoryReader = memoryNameSystem.useReader(constants::memoryName); + memoryReader = transformReader.getMemoryReader(); ARMARX_INFO << "RobotReader: Connected to memory '" << constants::memoryName << "'"; } catch (const armem::error::CouldNotResolveMemoryServer& e) diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h index 37dc53c5c190e576af8fcc13d2d12772b719d921..184fae2481188bbcc6e6f73a72c1dc47fa42d647 100644 --- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h +++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h @@ -58,6 +58,8 @@ namespace armarx::armem::robot_state::client::localization void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def) override; + armem::client::Reader getMemoryReader(){return memoryReader;} + private: armem::client::Reader memoryReader; mutable std::mutex memoryReaderMutex;