diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp index f5ae8c13b383662411bc4d33399cc87ec3048a79..b8fb2887e5c1c6c129da78cbf6b41cd313750732 100644 --- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp +++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp @@ -68,46 +68,18 @@ namespace armarx::armem::articulated_object const auto resultCoreInstanceSegmentName = memoryWriter.addSegment(properties.coreInstanceSegmentName, properties.providerName); - - armem::MemoryID refId = armem::MemoryID(resultCoreClassSegment.segmentID); - - armem::MemoryID id; - id.setCoreSegmentID(refId); // listen to all provider segments! - - updateKnownObjects(); - memoryNameSystem.subscribe(id, this, &Writer::updateKnownObjects); - } - - void - Writer::updateKnownObject(const armem::MemoryID& snapshotId) - { - arondto::RobotDescription aronArticulatedObjectDescription; - // aronArticulatedObjectDescription.fromAron(snapshotId.ent); - - // TODO(fabian.reister): implement } - void - Writer::updateKnownObjects(const armem::MemoryID& subscriptionID, - const std::vector<armem::MemoryID>& snapshotIDs) - { - ARMARX_INFO << "New objects available!"; - updateKnownObjects(); - } - - void - Writer::updateKnownObjects() - { - knownObjects = queryDescriptions(Time::Now()); - - ARMARX_INFO << "Known articulated objects " << simox::alg::get_keys(knownObjects); - } std::optional<armem::MemoryID> Writer::storeOrGetClass(const ArticulatedObject& obj) const { ARMARX_TRACE; + // key: name of object: RobotDescription::name + const std::unordered_map<std::string, MemoryID> knownObjects = queryDescriptions(Time::Now()); + ARMARX_VERBOSE << "Known articulated objects " << simox::alg::get_keys(knownObjects); + const auto objectId = knownObjects.find(obj.description.name); // check if exists @@ -172,9 +144,6 @@ namespace armarx::armem::articulated_object return std::nullopt; } - // update cache (TODO: likely remove this) - knownObjects[obj.description.name] = updateResult.snapshotID; - return updateResult.snapshotID; } @@ -276,7 +245,7 @@ namespace armarx::armem::articulated_object if (not classId) { ARMARX_WARNING << "Could not get class id for object " << obj.description.name << "! " - << "Known classes are " << simox::alg::get_keys(knownObjects); + << "Known classes are " << simox::alg::get_keys(queryDescriptions(Time::Now())); return false; } @@ -333,7 +302,7 @@ namespace armarx::armem::articulated_object } std::unordered_map<std::string, armem::MemoryID> - Writer::queryDescriptions(const armem::Time& timestamp) + Writer::queryDescriptions(const armem::Time& timestamp) const { // Query all entities from provider. armem::client::query::Builder qb; @@ -343,7 +312,7 @@ namespace armarx::armem::articulated_object .coreSegments().withName(properties.coreClassSegmentName) .providerSegments().all() .entities().all() - .snapshots().latest(); // TODO beforeTime(timestamp); + .snapshots().beforeTime(timestamp); // clang-format on const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput()); diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h index 568bec35dad164f665cd41f9067d92b16693e380..c31f5ce72eb4c95038b82e01b3ea73641352544d 100644 --- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h +++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h @@ -59,14 +59,9 @@ namespace armarx::armem::articulated_object private: std::optional<armem::MemoryID> storeOrGetClass(const ArticulatedObject& obj) const; - void updateKnownObjects(const armem::MemoryID& subscriptionID, - const std::vector<armem::MemoryID>& snapshotIDs); - void updateKnownObjects(); - void updateKnownObject(const armem::MemoryID& snapshotId); - // TODO duplicate std::unordered_map<std::string, armem::MemoryID> - queryDescriptions(const armem::Time& timestamp); + queryDescriptions(const armem::Time& timestamp) const; std::optional<robot_state::description::RobotDescription> getRobotDescription(const armarx::armem::wm::Memory& memory) const; std::unordered_map<std::string, armem::MemoryID> @@ -89,9 +84,6 @@ namespace armarx::armem::articulated_object armem::client::Reader memoryReader; mutable std::mutex memoryReaderMutex; - - // key: name of object: RobotDescription::name - mutable std::unordered_map<std::string, MemoryID> knownObjects; };