diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp index 676a83e722e2356942252814675e7bcbac839edc..4230394aa823c96e2382fd73af68d9663513b989 100644 --- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp +++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp @@ -63,22 +63,21 @@ namespace armarx::armem::client::robot_state::localization { ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions"; - def->optional(properties.localizationSegment, - propertyPrefix + "localizationSegment", + def->optional(properties.coreSegmentID.memoryName, propertyPrefix + "MemoryName"); + def->optional(properties.coreSegmentID.coreSegmentName, + propertyPrefix + "LocalizationSegmentName", "Name of the localization memory core segment to use."); - - def->optional(properties.memoryName, propertyPrefix + "Memory"); } void TransformWriter::connect() { // Wait for the memory to become available and add it as dependency. - ARMARX_IMPORTANT << "TransformWriter: Waiting for memory '" << properties.memoryName + ARMARX_IMPORTANT << "TransformWriter: Waiting for memory '" << properties.coreSegmentID.memoryName << "' ..."; try { - memoryWriter = memoryNameSystem.useWriter(properties.memoryName); - ARMARX_IMPORTANT << "TransformWriter: Connected to memory '" << properties.memoryName << "'"; + memoryWriter = memoryNameSystem.useWriter(properties.coreSegmentID); + ARMARX_IMPORTANT << "TransformWriter: Connected to memory for '" << properties.coreSegmentID << "'"; } catch (const armem::error::CouldNotResolveMemoryServer& e) { @@ -91,50 +90,19 @@ namespace armarx::armem::client::robot_state::localization { std::lock_guard g{memoryWriterMutex}; - ARMARX_DEBUG << "Trying to create core segment + provider segment"; - - const auto providerId = [&]() -> std::optional<armem::MemoryID> - { - try - { - const auto result = - memoryWriter.addSegment(properties.localizationSegment, transform.header.agent); - - if (not result.success) - { - ARMARX_WARNING << "Could not obtain provider id! Reason: " << result.errorMessage; - return std::nullopt; - } - - return armem::MemoryID(result.segmentID); - } - catch (...) - { - ARMARX_WARNING << "Could not obtain provider id!"; - return std::nullopt; - } - }(); - - if (not providerId) - { - return false; - } - + const MemoryID providerId = properties.coreSegmentID.withProviderSegmentName(transform.header.agent); // const auto& timestamp = transform.header.timestamp; - const auto timestamp = IceUtil::Time::now(); // FIXME remove - - - const auto entityID = providerId->withEntityName(transform.header.parentFrame + "," + - transform.header.frame).withTimestamp(timestamp); + const MemoryID entityID = providerId.withEntityName( + transform.header.parentFrame + "," + transform.header.frame); + const Time timestamp = Time::now(); // FIXME remove armem::EntityUpdate update; - update.entityID = entityID; + update.entityID = entityID; + update.timeCreated = timestamp; arondto::Transform aronTransform; toAron(aronTransform, transform); - update.instancesData = {aronTransform.toAron()}; - update.timeCreated = timestamp; ARMARX_DEBUG << "Committing " << update << " at time " << transform.header.timestamp; armem::EntityUpdateResult updateResult = memoryWriter.commit(update); diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h index 299c79b69222accb8290a7064229a9dfe59e8572..e553b073464e5fcf1444e14a4fef48e2034e808c 100644 --- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h +++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h @@ -61,6 +61,7 @@ namespace armarx::armem::client::robot_state::localization bool commitTransform(const ::armarx::armem::robot_state::Transform& transform) override; + private: armem::client::MemoryNameSystem& memoryNameSystem; @@ -70,8 +71,7 @@ namespace armarx::armem::client::robot_state::localization // Properties struct Properties { - std::string memoryName = "RobotState"; - std::string localizationSegment = "Localization"; + MemoryID coreSegmentID { "RobotState", "Localization" }; } properties; const std::string propertyPrefix = "mem.robot_state.";