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 e278bcd05fb60f8ee45f10c6aaef3a2a9a310194..270b9d30257ccaf3253f7d169a222e043251b354 100644 --- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp +++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp @@ -167,6 +167,7 @@ namespace armarx::armem::server::robot_state::proprioception up.entityID.coreSegmentName = ::armarx::armem::robot_state::constants::proprioceptionCoreSegment; up.referencedTime = data.timestamp; + up.arrivedTime = data.timestampArrived; up.instancesData = {data.proprioception}; } diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h index 8d904f249029cf9fd5ce010657cb4324f3ba5551..2cd2307421175d4e1feed29be5096be99d56d355 100644 --- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h +++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h @@ -13,6 +13,7 @@ namespace armarx::armem::server::robot_state::proprioception struct RobotUnitData { Time timestamp; + Time timestampArrived; aron::data::DictPtr proprioception; aron::data::DictPtr exteroception; }; 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 a0178f32fe86eed0406c09d862de7e7a9db6b0e0..8a9ca2aa1e762790194cd2af64271207541a5596 100644 --- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp +++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp @@ -81,11 +81,14 @@ namespace armarx::armem::server::robot_state::proprioception { ARMARX_CHECK_NOT_NULL(converterProprioception); + RobotUnitData result; std::optional<RobotUnitDataStreaming::TimeStep> data; { auto start = std::chrono::high_resolution_clock::now(); data = fetchLatestData(); + result.timestampArrived = armarx::DateTime::Now(); + auto duration = std::chrono::duration_cast<std::chrono::microseconds>( std::chrono::high_resolution_clock::now() - start); if (debugObserver) @@ -102,7 +105,6 @@ namespace armarx::armem::server::robot_state::proprioception ARMARX_DEBUG << "RobotUnitReader: Converting data current timestep to commit"; auto start = std::chrono::high_resolution_clock::now(); - RobotUnitData result; if (converterProprioception != nullptr) {