From fd60312153c90d5c2402a3ee3390117c1b8f389c Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 22 Mar 2022 14:15:26 +0100
Subject: [PATCH] Update usage of time in armem_robot_state
---
.../armem_robot_state/aron_conversions.cpp | 8 ++++----
.../armem_robot_state/client/common/RobotReader.cpp | 13 +++++++------
.../armem_robot_state/client/common/RobotReader.h | 10 +++++-----
.../client/common/VirtualRobotReader.cpp | 6 ++++--
.../client/localization/TransformWriter.cpp | 2 +-
.../armem_robot_state/server/common/Visu.cpp | 2 +-
.../server/description/Segment.cpp | 2 +-
.../server/proprioception/RobotUnitReader.cpp | 2 +-
.../server/proprioception/Segment.cpp | 8 ++++----
9 files changed, 28 insertions(+), 25 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index 03a578fab..30f3a489c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
@@ -2,15 +2,15 @@
#include <string>
-#include <IceUtil/Time.h>
-
#include <ArmarXCore/core/logging/Logging.h>
+
#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+
namespace armarx::armem
{
@@ -38,7 +38,7 @@ namespace armarx::armem
aron::toAron(dto.parentFrame, bo.parentFrame);
aron::toAron(dto.frame, bo.frame);
aron::toAron(dto.agent, bo.agent);
- dto.timestamp = bo.timestamp;
+ toAron(dto.timestamp, bo.timestamp);
}
void
@@ -47,7 +47,7 @@ namespace armarx::armem
aron::fromAron(dto.parentFrame, bo.parentFrame);
aron::fromAron(dto.frame, bo.frame);
aron::fromAron(dto.agent, bo.agent);
- bo.timestamp = dto.timestamp;
+ fromAron(dto.timestamp, bo.timestamp);
}
/* JointState */
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index b3dee3440..3c59b2980 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -9,6 +9,7 @@
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/Clock.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
@@ -94,13 +95,13 @@ namespace armarx::armem::robot_state
}
void
- RobotReader::setSyncTimeout(const armem::Time& duration)
+ RobotReader::setSyncTimeout(const armem::Duration& duration)
{
syncTimeout = duration;
}
void
- RobotReader::setSleepAfterSyncFailure(const armem::Time& duration)
+ RobotReader::setSleepAfterSyncFailure(const armem::Duration& duration)
{
ARMARX_CHECK_NONNEGATIVE(duration.toMicroSeconds());
sleepAfterFailure = duration;
@@ -109,8 +110,9 @@ namespace armarx::armem::robot_state
bool
RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp)
{
- const auto tsStartFunctionInvokation = armem::Time::now();
+ const auto tsStartFunctionInvokation = armem::Time::Now();
+ Clock clock;
while (true)
{
auto state = queryState(obj.description, timestamp);
@@ -120,7 +122,7 @@ namespace armarx::armem::robot_state
ARMARX_VERBOSE << "Could not synchronize object " << obj.description.name;
// if the syncTime is non-positive there will be no retry
- const auto elapsedTime = armem::Time::now() - tsStartFunctionInvokation;
+ const auto elapsedTime = armem::Time::Now() - tsStartFunctionInvokation;
if (elapsedTime > syncTimeout)
{
ARMARX_WARNING << "Could not synchronize object " << obj.description.name;
@@ -128,8 +130,7 @@ namespace armarx::armem::robot_state
}
ARMARX_INFO << "Retrying to query robot state after failure";
- std::this_thread::sleep_for(
- std::chrono::microseconds(sleepAfterFailure.toMicroSeconds()));
+ clock.waitFor(sleepAfterFailure);
}
obj.config = std::move(*state);
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index 46b0924fc..6682fcd79 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -24,7 +24,7 @@
#include <mutex>
#include <optional>
-#include "RobotAPI/libraries/armem/core/Time.h"
+#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem_robot/client/interfaces.h>
@@ -81,8 +81,8 @@ namespace armarx::armem::robot_state
queryPlatformState(const robot::RobotDescription& description,
const armem::Time& timestamp) const;
- void setSyncTimeout(const armem::Time& duration);
- void setSleepAfterSyncFailure(const armem::Time& duration);
+ void setSyncTimeout(const armem::Duration& duration);
+ void setSleepAfterSyncFailure(const armem::Duration& duration);
enum class Hand
{
@@ -101,8 +101,8 @@ namespace armarx::armem::robot_state
protected:
// by default, no timeout mechanism
- armem::Time syncTimeout = armem::Time::microSeconds(0);
- armem::Time sleepAfterFailure = armem::Time::microSeconds(0);
+ armem::Duration syncTimeout = armem::Duration::MicroSeconds(0);
+ armem::Duration sleepAfterFailure = armem::Duration::MicroSeconds(0);
private:
std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory,
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
index 5ef7564f8..b59eaad14 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -11,6 +11,7 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/time/Clock.h>
namespace armarx::armem::robot_state
@@ -91,6 +92,8 @@ namespace armarx::armem::robot_state
const VirtualRobot::RobotIO::RobotDescription& loadMode,
const bool blocking)
{
+ Clock clock;
+
while (blocking)
{
VirtualRobot::RobotPtr robot = getRobot(name, timestamp, loadMode);
@@ -100,8 +103,7 @@ namespace armarx::armem::robot_state
}
ARMARX_INFO << "Retrying to query robot after failure";
- std::this_thread::sleep_for(
- std::chrono::microseconds(sleepAfterFailure.toMicroSeconds()));
+ clock.waitFor(sleepAfterFailure);
}
ARMARX_WARNING << "Failed to get synchronized robot `" << name << "`";
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 ba34586a1..932f55b0a 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
@@ -94,7 +94,7 @@ namespace armarx::armem::client::robot_state::localization
// const auto& timestamp = transform.header.timestamp;
const MemoryID entityID = providerId.withEntityName(
transform.header.parentFrame + "," + transform.header.frame);
- const Time timestamp = Time::now(); // FIXME remove
+ const Time timestamp = Time::Now(); // FIXME remove
armem::EntityUpdate update;
update.entityID = entityID;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
index 67d537d19..e68119aff 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -123,7 +123,7 @@ namespace armarx::armem::server::robot_state
{
if (p.enabled)
{
- const Time timestamp = Time::now();
+ const Time timestamp = Time::Now();
ARMARX_DEBUG << "Visu task at " << armem::toStringMilliSeconds(timestamp);
try
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
index 185356425..f6cba1d8c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -44,7 +44,7 @@ namespace armarx::armem::server::robot_state::description
void Segment::commitRobotDescription(const robot::RobotDescription& robotDescription)
{
- const Time now = TimeUtil::GetTime();
+ const Time now = Time::Now();
const MemoryID providerID = segmentPtr->id().withProviderSegmentName(robotDescription.name);
segmentPtr->addProviderSegment(providerID.providerSegmentName, arondto::RobotDescription::ToAronType());
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 12638d84e..f50c4e561 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
@@ -89,7 +89,7 @@ namespace armarx::armem::server::robot_state::proprioception
RobotUnitData result;
result.proprioception = converter->convert(data.value(), description);
- result.timestamp = Time::microSeconds(data->timestampUSec);
+ result.timestamp = Time(Duration::MicroSeconds(data->timestampUSec));
auto stop = std::chrono::high_resolution_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
index 9bf3f1e81..ca9e43ed9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -78,7 +78,7 @@ namespace armarx::armem::server::robot_state::proprioception
RobotJointPositionMap jointMap;
int i = 0;
- Duration tFindData = Duration::milliSeconds(0), tReadJointPositions = Duration::milliSeconds(0);
+ Duration tFindData = Duration::MilliSeconds(0), tReadJointPositions = Duration::MilliSeconds(0);
TIMING_START(tProcessEntities)
segmentPtr->forEachEntity([&](const wm::Entity & entity)
{
@@ -97,8 +97,8 @@ namespace armarx::armem::server::robot_state::proprioception
data = snapshot->findInstanceData();
}
- TIMING_END_COMMENT_STREAM(_tFindData, "tFindData " + std::to_string(i), ARMARX_DEBUG)
- tFindData += _tFindData;
+ TIMING_END_COMMENT_STREAM(_tFindData, "tFindData " + std::to_string(i), ARMARX_DEBUG);
+ tFindData += Duration::MicroSeconds(_tFindData.toMicroSeconds());
}
if (data)
{
@@ -107,7 +107,7 @@ namespace armarx::armem::server::robot_state::proprioception
jointMap.emplace(entity.id().providerSegmentName, readJointPositions(*data));
TIMING_END_COMMENT_STREAM(_tReadJointPositions, "tReadJointPositions " + std::to_string(i), ARMARX_DEBUG)
- tReadJointPositions += _tReadJointPositions;
+ tReadJointPositions += Duration::MicroSeconds(_tReadJointPositions.toMicroSeconds());
}
++i;
});
--
GitLab