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..5c4580c4c8c5d4540b0867365c657807d0502a7d 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,
@@ -70,10 +67,14 @@ namespace armarx::armem::server::robot_state::proprioception
{
while (isRunning())
{
- // if (debugObserver)
- // {
- // debugObserver->setDebugObserverDatafield("RobotStateWriter | Queue Size", dataBuffer.size());
- // }
+ if (debugObserver)
+ {
+ // This locks the queue, but I did not find an interface to lock the queue,
+ // get the size and wait_pull().
+ size_t dataBufferSize = dataBuffer.size();
+ debugObserver->setDebugObserverDatafield("RobotStateWriter | Queue Size",
+ static_cast<long>(dataBufferSize));
+ }
RobotUnitData robotUnitData;
if (const auto status = dataBuffer.wait_pull(robotUnitData);
@@ -88,10 +89,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 +108,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 +163,26 @@ 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.arrivedTime = data.timestampArrived;
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 +210,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/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 34260f51e36951f7b9a0a68369cf8317e8a4dedc..ec7a56751f1ca8393f218941a27e4041190d1c96 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>(
@@ -80,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)
@@ -101,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)
{
@@ -143,6 +146,9 @@ namespace armarx::armem::server::robot_state::proprioception
debugObserver->setDebugObserverDatafield(
"RobotUnitReader | RT Timestamp Since Last Iteration",
data.back().timesSinceLastIterationUSec);
+ debugObserver->setDebugObserverDatafield(
+ "RobotUnitReader | Timestamp Arrived in RSM",
+ armarx::DateTime::Now().toMicroSecondsSinceEpoch());
}
}
if (data.empty())