diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp index e1469a80d35feecd9cb12914a62b944cabb4ba81..e278bcd05fb60f8ee45f10c6aaef3a2a9a310194 100644 --- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp +++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp @@ -40,7 +40,6 @@ #include <RobotAPI/libraries/aron/core/data/variant/All.h> #include <RobotAPI/libraries/core/FramedPose.h> - namespace armarx::armem::server::robot_state::proprioception { @@ -52,7 +51,6 @@ namespace armarx::armem::server::robot_state::proprioception DebugObserverHelper(Logging::tag.tagName, debugObserverPlugin.getDebugObserver(), true); } - static float toDurationMs(std::chrono::high_resolution_clock::time_point start, std::chrono::high_resolution_clock::time_point end) @@ -61,7 +59,6 @@ namespace armarx::armem::server::robot_state::proprioception return duration.count() / 1000.f; } - void RobotStateWriter::run(float pollFrequency, Queue& dataBuffer, @@ -88,10 +85,12 @@ namespace armarx::armem::server::robot_state::proprioception // Commits lock the core segments. // Proprioception + Exteroception - armem::CommitResult resultProprioception = memory.commitLocking(update.proprioception); + armem::CommitResult resultProprioception = + memory.commitLocking(update.proprioception); ARMARX_DEBUG << deactivateSpam(1) << VAROUT(update.exteroception.updates.size()); - armem::CommitResult resultExteroception = memory.commitLocking(update.exteroception); + armem::CommitResult resultExteroception = + memory.commitLocking(update.exteroception); endProprioception = std::chrono::high_resolution_clock::now(); // Localization @@ -105,13 +104,15 @@ namespace armarx::armem::server::robot_state::proprioception if (not resultProprioception.allSuccess()) { - ARMARX_WARNING << "Could not commit data to proprioception segment in memory. Error message: " + ARMARX_WARNING << "Could not commit data to proprioception segment in memory. " + "Error message: " << resultProprioception.allErrorMessages(); } if (not resultExteroception.allSuccess()) { - ARMARX_WARNING << "Could not commit data to exteroception segment in memory. Error message: " + ARMARX_WARNING << "Could not commit data to exteroception segment in memory. " + "Error message: " << resultExteroception.allErrorMessages(); } @@ -158,21 +159,25 @@ namespace armarx::armem::server::robot_state::proprioception // Send batch to memory Update update; - if(data.proprioception){ + if (data.proprioception) + { armem::EntityUpdate& up = update.proprioception.add(); up.entityID = properties.robotUnitProviderID.withEntityName( properties.robotUnitProviderID.providerSegmentName); - up.entityID.coreSegmentName =::armarx::armem::robot_state::constants::proprioceptionCoreSegment; + up.entityID.coreSegmentName = + ::armarx::armem::robot_state::constants::proprioceptionCoreSegment; up.referencedTime = data.timestamp; up.instancesData = {data.proprioception}; } // Exteroception - if(data.exteroception){ + if (data.exteroception) + { armem::EntityUpdate& up = update.exteroception.add(); up.entityID = properties.robotUnitProviderID.withEntityName( properties.robotUnitProviderID.providerSegmentName); - up.entityID.coreSegmentName = ::armarx::armem::robot_state::constants::exteroceptionCoreSegment; + up.entityID.coreSegmentName = + ::armarx::armem::robot_state::constants::exteroceptionCoreSegment; up.referencedTime = data.timestamp; up.instancesData = {data.exteroception}; } @@ -200,7 +205,6 @@ namespace armarx::armem::server::robot_state::proprioception return update; } - armem::robot_state::Transform RobotStateWriter::getTransform(const aron::data::DictPtr& platformData, const Time& timestamp) const diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp index 34260f51e36951f7b9a0a68369cf8317e8a4dedc..a0178f32fe86eed0406c09d862de7e7a9db6b0e0 100644 --- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp +++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp @@ -56,10 +56,11 @@ namespace armarx::armem::server::robot_state::proprioception if (std::optional<RobotUnitData> commit = fetchAndConvertLatestRobotUnitData()) { - // will lock a mutex - debugObserver->setDebugObserverDatafield("RobotUnitReader | t commitTimestamp [us]", - commit->timestamp.toMicroSecondsSinceEpoch()); + debugObserver->setDebugObserverDatafield( + "RobotUnitReader | t commitTimestamp [us]", + commit->timestamp.toMicroSecondsSinceEpoch()); + // will lock a mutex dataBuffer.push(std::move(commit.value())); } auto duration = std::chrono::duration_cast<std::chrono::microseconds>(