diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp index 861ff14b97addd9b87fee4a834bd605f20b2b681..a515fe725b51ff8302a129053996e97d6738872c 100644 --- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp +++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.cpp @@ -21,8 +21,8 @@ namespace armarx::armem::obj::instance { - Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) : - memoryNameSystem(memoryNameSystem) + Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem, const objpose::ObjectPoseProviderPrx& objPoseProvider) : + memoryNameSystem(memoryNameSystem), objPoseProvider(objPoseProvider) {} void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) @@ -55,6 +55,16 @@ namespace armarx::armem::obj::instance } } + bool Reader::requestLocalization(const std::string& entityName, const armarx::core::time::Duration& until) + { + auto entityNameParts = GetEntityNameParts(entityName); + + armarx::data::ObjectID requestObject({std::get<0>(entityNameParts), std::get<1>(entityNameParts), std::get<2>(entityNameParts)}); + armarx::objpose::provider::RequestObjectsOutput requestResult = objPoseProvider->requestObjects({{requestObject}, until.toMilliSeconds()}); + + return requestResult.results.at(requestObject).success; + } + /// get the latest object from an memory and cast it to an ObjectInstance std::optional<armarx::armem::arondto::ObjectInstance> Reader::queryObject(const armem::wm::Memory& memory, const armem::Time& timestamp) { diff --git a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h index 825687d5b6bf17acad4515841e9852c9f020dbed..4900004e06517e824053b4ed080de02c42aeb3a2 100644 --- a/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h +++ b/source/RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h @@ -32,23 +32,29 @@ #include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h> +// The object pose provider. As long as the provider is not connected to armem we need the second proxy +#include <RobotAPI/interface/objectpose/ObjectPoseProvider.h> namespace armarx::armem::obj::instance { class Reader { public: - Reader(armem::client::MemoryNameSystem& memoryNameSystem); + Reader(armem::client::MemoryNameSystem& memoryNameSystem, const objpose::ObjectPoseProviderPrx& objPoseProvider); virtual ~Reader() = default; void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def); void connect(); + // localization stuff + bool requestLocalization(const std::string& entityName, const armarx::core::time::Duration& until); + + // query existing instances from the memory std::optional<armem::arondto::ObjectInstance> queryObject(const armem::wm::Memory& memory, const armem::Time&); std::optional<armem::arondto::ObjectInstance> queryObjectByEntityID(const std::string& entityName, const armem::Time&); std::optional<armem::arondto::ObjectInstance> queryObjectByObjectID(const std::string& objectId, const armem::Time&); - // return the class name, e.g. Kitchen/greencup in Kitchen/greencup/0 + // return the dataset/class, e.g. Kitchen/greencup in Kitchen/greencup/0 static std::string GetObjectId(const std::string& s) { auto split = simox::alg::split(s, "/"); @@ -69,7 +75,40 @@ namespace armarx::armem::obj::instance return ""; } - // return the class name, e.g. greencup in Kitchen/greencup/0 + // return all parts of the entity name + static std::tuple<std::string, std::string, std::string> GetEntityNameParts(const std::string& s) + { + std::string dataset = ""; + std::string clazz = ""; + std::string instance = ""; + + auto split = simox::alg::split(s, "/"); + if (simox::alg::starts_with(s, "/")) + { + split.insert(split.begin(), ""); // sanitize + } + + for (auto& e : split) + { + e = simox::alg::replace_all(e, "/", ""); + } + + if (split.size() > 0) + { + dataset = split[0]; + } + if (split.size() > 1) + { + clazz = split[1]; + } + if (split.size() > 2) + { + instance = split[2]; + } + return {dataset, clazz, instance}; + } + + // return the class, e.g. greencup in Kitchen/greencup/0 static std::string GetObjectClassName(const std::string& s) { auto split = simox::alg::split(s, "/"); @@ -133,8 +172,10 @@ namespace armarx::armem::obj::instance const std::string propertyPrefix = "mem.obj.instance."; armem::client::MemoryNameSystem& memoryNameSystem; + objpose::ObjectPoseProviderPrx objPoseProvider; + armem::client::Reader memoryReader; - std::mutex memoryWriterMutex; + mutable std::mutex memoryWriterMutex; };