diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp index 75d412a201933a07089ab6433f757533a11fd77d..75e2e9481025981a20f5764e6cf4cd2931a473ce 100644 --- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp +++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp @@ -168,30 +168,40 @@ namespace armarx::armem::server::robot_state::localization } - bool Segment::storeTransform(const armarx::armem::robot_state::Transform& transform) + bool Segment::commitTransform(const armarx::armem::robot_state::Transform& transform) { - const armem::Time& timestamp = transform.header.timestamp; + Commit commit; + commit.add(makeUpdate(transform)); - const MemoryID providerID = - coreSegment->id().withProviderSegmentName(transform.header.agent); - if (not coreSegment->hasProviderSegment(providerID.providerSegmentName)) - { - coreSegment->addProviderSegment(providerID.providerSegmentName, arondto::Transform::toAronType()); - } + const armem::CommitResult result = iceMemory.commit(commit); + return result.allSuccess(); + } + + bool Segment::commitTransformLocking(const armarx::armem::robot_state::Transform& transform) + { Commit commit; + commit.add(makeUpdate(transform)); - EntityUpdate& update = commit.updates.emplace_back(); - update.entityID = - providerID.withEntityName(transform.header.parentFrame + "," + transform.header.frame); + const armem::CommitResult result = iceMemory.commitLocking(commit); + return result.allSuccess(); + } + + + EntityUpdate Segment::makeUpdate(const armarx::armem::robot_state::Transform& transform) const + { + const armem::Time& timestamp = transform.header.timestamp; + const MemoryID providerID = coreSegment->id().withProviderSegmentName(transform.header.agent); + + EntityUpdate update; + update.entityID = providerID.withEntityName(transform.header.parentFrame + "," + transform.header.frame); update.timeArrived = update.timeCreated = update.timeSent = timestamp; arondto::Transform aronTransform; toAron(aronTransform, transform); update.instancesData = {aronTransform.toAron()}; - const armem::CommitResult result = iceMemory.commit(commit); - return result.allSuccess(); + return update; } } // namespace armarx::armem::server::robot_state::localization diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h index d1ce5de2208f6fbcd32a99a8837710fcfba80f31..d2f8eef82a2cb6b2a9530bc0d646cef5653e8f21 100644 --- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h +++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h @@ -36,6 +36,7 @@ namespace armarx::armem { + class EntityUpdate; namespace server { class MemoryToIceAdapter; @@ -75,7 +76,14 @@ namespace armarx::armem::server::robot_state::localization RobotFramePoseMap getRobotFramePoses(const armem::Time& timestamp) const; RobotFramePoseMap getRobotFramePosesLocking(const armem::Time& timestamp) const; - bool storeTransform(const armarx::armem::robot_state::Transform& transform); + bool commitTransform(const armarx::armem::robot_state::Transform& transform); + bool commitTransformLocking(const armarx::armem::robot_state::Transform& transform); + + + private: + + EntityUpdate makeUpdate(const armarx::armem::robot_state::Transform& transform) const; + private: