diff --git a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h index 4c4e50c920d87273d701378befd982823e2f00ea..d28d09c74f3f324fab3d9f31dc37548a567ea95e 100644 --- a/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h +++ b/source/RobotAPI/libraries/armem/core/base/CoreSegmentBase.h @@ -109,6 +109,26 @@ namespace armarx::armem::base return getProviderSegment(id.providerSegmentName).getEntity(id); } + const EntityT* findEntity(const MemoryID& id) const override + { + this->_checkContainerName(id.coreSegmentName, this->getKeyString()); + if (id.hasProviderSegmentName()) + { + return getProviderSegment(id.providerSegmentName).findEntity(id); + } + else + { + for (const auto& [_, providerSegment] : this->_container) + { + if (auto entity = providerSegment.findEntity(id)) + { + return entity; + } + } + return nullptr; + } + } + virtual MemoryID update(const EntityUpdate& update) override { this->_checkContainerName(update.entityID.coreSegmentName, this->name()); diff --git a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h index 7bcef9350941e86ea5f3e290afcf70d1c42f1010..53b398a284bc8ebb1151d5266f81ec7a43010cea 100644 --- a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h +++ b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h @@ -103,6 +103,26 @@ namespace armarx::armem::base return getCoreSegment(id.coreSegmentName).getEntity(id); } + const EntityT* findEntity(const MemoryID& id) const override + { + this->_checkContainerName(id.memoryName, this->name()); + if (id.hasCoreSegmentName()) + { + return getCoreSegment(id.providerSegmentName).findEntity(id); + } + else + { + for (const auto& [_, coreSegment] : this->_container) + { + if (auto entity = coreSegment.findEntity(id)) + { + return entity; + } + } + return nullptr; + } + } + /** * @brief Add an empty core segment with the given name. * @param name The core segment name. diff --git a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h index 81be753a74ea9887ce11b7b93b364993679caf23..20b298af557452854fb39b442023e75f6f0a2f2e 100644 --- a/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h +++ b/source/RobotAPI/libraries/armem/core/base/ProviderSegmentBase.h @@ -109,6 +109,20 @@ namespace armarx::armem::base } } + const EntityT* findEntity(const MemoryID& id) const override + { + this->_checkContainerName(id.providerSegmentName, this->getKeyString()); + auto it = this->_container.find(id.entityName); + if (it != this->_container.end()) + { + return &it->second; + } + else + { + return nullptr; + } + } + /** * @brief Updates an entity's history. * diff --git a/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h b/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h index 9e5c869c333196c4ae9f1d32a803458ef34d27b4..09733b648d096c7e33179c2903f3dbe241dca8d2 100644 --- a/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h +++ b/source/RobotAPI/libraries/armem/core/base/detail/EntityContainerBase.h @@ -67,11 +67,26 @@ namespace armarx::armem::base::detail * @return The entity. * @throw An exception deriving from `armem::error::ArMemError` if the entity is missing. */ - virtual _EntityT& getEntity(const MemoryID& id) + virtual EntityT& getEntity(const MemoryID& id) { - return const_cast<_EntityT&>(const_cast<const EntityContainerBase*>(this)->getEntity(id)); + return const_cast<EntityT&>(const_cast<const EntityContainerBase*>(this)->getEntity(id)); } - virtual const _EntityT& getEntity(const MemoryID& id) const = 0; + virtual const EntityT& getEntity(const MemoryID& id) const = 0; + + /** + * @brief Find an entity. + * + * Search for the entity with the given ID and return a pointer to the + * first match. If `id` is underspecified (e.g. no provider segment name), + * search all children until the first match is found. + * + * If no matching entity is found, return `nullptr`. + * + * @param id The entities ID. + * @return A pointer to the first matching entity or `nullptr` if none was found. + */ + virtual const EntityT* findEntity(const MemoryID& id) const = 0; + /** * @brief Retrieve an entity snapshot. @@ -89,7 +104,7 @@ namespace armarx::armem::base::detail virtual const EntitySnapshotT& getEntitySnapshot(const MemoryID& id) const { - const _EntityT& entity = getEntity(id); + const EntityT& entity = getEntity(id); if (id.hasTimestamp()) {