diff --git a/scenarios/RobotHealthTest/RobotHealthTest.scx b/scenarios/RobotHealthTest/RobotHealthTest.scx
index c0dde0a3b95ecb6a6155a07f818b0343badd6e6d..68dad197bc70f86a5cdef0161121f48d0d312800 100644
--- a/scenarios/RobotHealthTest/RobotHealthTest.scx
+++ b/scenarios/RobotHealthTest/RobotHealthTest.scx
@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="utf-8"?>
<scenario name="RobotHealthTest" creation="2018-11-30.11:42:03" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
- <application name="RobotHealthApp" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="RobotHealthApp" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="RobotHealthDummyApp" instance="HealthDummy1" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RobotHealthDummyApp" instance="HealthDummy2" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/RobotHealthTest/config/RobotHealthApp.cfg b/scenarios/RobotHealthTest/config/RobotHealthApp.cfg
index b91501c1a405672cec4ce8b06ea9bcf83f758c4b..ebffea693fd85455a4f16d02ab490a8cad798bc3 100644
--- a/scenarios/RobotHealthTest/config/RobotHealthApp.cfg
+++ b/scenarios/RobotHealthTest/config/RobotHealthApp.cfg
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
@@ -109,20 +109,12 @@
# ArmarX.RemoteHandlesDeletionTimeout = 3000
-# ArmarX.RobotHealth.AggregatedRobotHealthTopicName: Name of the AggregatedRobotHealthTopic
+# ArmarX.RobotHealth.DebugObserverTopicName: Name of the topic the DebugObserver listens on
# Attributes:
-# - Default: AggregatedRobotHealthTopic
+# - Default: DebugObserver
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RobotHealth.AggregatedRobotHealthTopicName = AggregatedRobotHealthTopic
-
-
-# ArmarX.RobotHealth.EmergencyStopTopicName: The name of the topic over which changes of the emergencyStopState are sent.
-# Attributes:
-# - Default: EmergencyStop
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.RobotHealth.EmergencyStopTopicName = EmergencyStop
+# ArmarX.RobotHealth.DebugObserverTopicName = DebugObserver
# ArmarX.RobotHealth.EnableProfiling: enable profiler which is used for logging performance events
@@ -159,37 +151,36 @@
# ArmarX.RobotHealth.MinimumLoggingLevel = Undefined
-# ArmarX.RobotHealth.ObjectName: Name of IceGrid well-known object
+# ArmarX.RobotHealth.Name of the AggregatedRobotHealthTopic: Name of the `AggregatedRobotHealth` topic to publish data to.
# Attributes:
-# - Default: ""
+# - Default: AggregatedRobotHealthTopic
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RobotHealth.ObjectName = ""
+# ArmarX.RobotHealth.Name of the AggregatedRobotHealthTopic = AggregatedRobotHealthTopic
-# ArmarX.RobotHealth.ReportErrorsWithSpeech:
+# ArmarX.RobotHealth.ObjectName: Name of IceGrid well-known object
# Attributes:
-# - Default: true
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.RobotHealth.ReportErrorsWithSpeech = true
+# ArmarX.RobotHealth.ObjectName = ""
-# ArmarX.RobotHealth.RequiredComponents: Comma separated list of required components
+# ArmarX.RobotHealth.RequiredTags: Tags that should be requested.
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RobotHealth.RequiredComponents = ""
+# ArmarX.RobotHealth.RequiredTags = ""
-# ArmarX.RobotHealth.RobotHealthTopicName: Name of the RobotHealth topic
+# ArmarX.RobotHealth.RobotHealthTopic: Name of the RobotHealth topic
# Attributes:
# - Default: RobotHealthTopic
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RobotHealth.RobotHealthTopicName = RobotHealthTopic
+# ArmarX.RobotHealth.RobotHealthTopic = RobotHealthTopic
# ArmarX.RobotHealth.RobotUnitName: No Description
@@ -208,20 +199,12 @@ ArmarX.RobotHealth.RobotUnitName = RobotUnitSimulation
ArmarX.RobotHealth.RobotUnitRequired = 0
-# ArmarX.RobotHealth.SpeechMinimumReportInterval: Time that has to pass between reported messages in seconds.
+# ArmarX.RobotHealth.The name of the topic over which changes of the emergencyStopState are sent.: Name of the `EmergencyStop` topic to publish data to.
# Attributes:
-# - Default: 60
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.RobotHealth.SpeechMinimumReportInterval = 60
-
-
-# ArmarX.RobotHealth.TextToSpeechTopicName: Name of the TextToSpeech topic
-# Attributes:
-# - Default: TextToSpeech
+# - Default: EmergencyStop
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RobotHealth.TextToSpeechTopicName = TextToSpeech
+# ArmarX.RobotHealth.The name of the topic over which changes of the emergencyStopState are sent. = EmergencyStop
# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
diff --git a/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy1.cfg b/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy1.cfg
index 5df0ff4cfd0fd82e8c0c7b9155be4e797fd14959..0f14e83d287e44accc961037ba069f3ea77dce46 100644
--- a/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy1.cfg
+++ b/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy1.cfg
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
diff --git a/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy2.cfg b/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy2.cfg
index 60d2ee3f291cbdb09a97eb89500b1fd7426e4a72..e157c2704c9191a48e3daccf14a545f125e72c51 100644
--- a/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy2.cfg
+++ b/scenarios/RobotHealthTest/config/RobotHealthDummyApp.HealthDummy2.cfg
@@ -18,7 +18,7 @@
# ArmarX.ApplicationName = ""
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
@@ -148,7 +148,7 @@ ArmarX.RobotHealthDummy.ObjectName = HealthDummy2
# - Default: nanosleep
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RobotHealthDummy.SleepMode = nanosleep
+ArmarX.RobotHealthDummy.SleepMode = std::this_thread::sleep_for
# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
index e028bc55648f02446b5980cc00124950d4564219..34894b6ea9c449d13f47af00d2c012a47f764f1d 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
+++ b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
@@ -80,7 +80,7 @@ namespace armarx
}
void
- RobotHealthDummy::busydwait(long microseconds)
+ RobotHealthDummy::busywait(long microseconds)
{
long start = TimeUtil::GetTime().toMicroSeconds();
auto end = start + microseconds;
@@ -95,51 +95,60 @@ namespace armarx
{
auto args = RobotHealthHeartbeatArgs();
args.identifier = getName();
+ armarx::core::time::toIce(args.maximumCycleTimeError, armarx::core::time::Duration::MilliSeconds(1000));
+ armarx::core::time::toIce(args.maximumCycleTimeWarning, armarx::core::time::Duration::MilliSeconds(500));
robotHealthTopicPrx->signUp(args);
ARMARX_INFO << "starting rinning task";
while (!dummyTask->isStopped())
{
- long beforeTopicCall = TimeUtil::GetTime().toMicroSeconds();
+ auto beforeTopicCall = armarx::core::time::DateTime::Now();
//ARMARX_INFO << "send heartbeat";
armarx::core::time::dto::DateTime now;
armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
robotHealthTopicPrx->heartbeat(getName(), now);
- long afterTopicCall = TimeUtil::GetTime().toMicroSeconds();
+ auto afterTopicCall = armarx::core::time::DateTime::Now();
+
if (sleepmode == "nanosleep")
{
- NanoSleep(10 * 1000 * 1000);
+ NanoSleep(10 * 1000 * 1000); // wait 10 milliseconds
}
else if (sleepmode == "sleepwait")
{
- sleepwait(10 * 1000);
+ sleepwait(10 * 1000); // wait 10 milliseconds
}
else if (sleepmode == "yieldwait")
{
- yieldwait(10 * 1000);
+ yieldwait(10 * 1000); // wait 10 milliseconds
}
else if (sleepmode == "busywait")
{
- busydwait(10 * 1000);
+ busywait(10 * 1000); // wait 10 milliseconds
+ }
+ else if (sleepmode == "std::this_thread::sleep_for")
+ {
+ std::this_thread::sleep_for(std::chrono::microseconds(10 * 1000));
}
else
{
throw LocalException("Unknown sleepmode.");
}
- long afterSleep = TimeUtil::GetTime().toMicroSeconds();
- long topicCallDelay = afterTopicCall - beforeTopicCall;
- long sleepDelay = afterSleep - afterTopicCall;
- if (sleepDelay > 20000)
+ auto afterSleep = armarx::core::time::DateTime::Now();
+ auto topicCallDelay = afterTopicCall - beforeTopicCall;
+ auto sleepDelay = afterSleep - afterTopicCall;
+ if (sleepDelay.toMicroSeconds() > 20000)
{
- ARMARX_IMPORTANT << sleepmode << ": " << sleepDelay << "us";
+ ARMARX_IMPORTANT << sleepmode << " took long: " << sleepDelay << "us";
}
- if (topicCallDelay > 1000)
+ if (topicCallDelay.toMicroSeconds() > 1000)
{
- ARMARX_IMPORTANT << "topic: " << topicCallDelay << "us";
+ ARMARX_IMPORTANT << "topic took long: " << topicCallDelay << "us";
}
}
+
+ robotHealthTopicPrx->unregister(args.identifier);
}
void
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h
index cfd8d369d0335768c2e85dd735f1d31d1e30e60d..a841720145b96f925c427443f3998475620509bb 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h
+++ b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h
@@ -78,7 +78,7 @@ namespace armarx
int NanoSleep(long nanosec);
void yieldwait(long microseconds);
- void busydwait(long microseconds);
+ void busywait(long microseconds);
void sleepwait(long microseconds);
protected:
diff --git a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
index 76112d64cfa0788fadfbcc5e86f923b8bf1d978b..efe2d01c13731c5c32ba876a3837b2e1816ea41a 100644
--- a/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
+++ b/source/RobotAPI/libraries/armem_grasping/client/KnownGraspCandidateReader.cpp
@@ -67,7 +67,7 @@ namespace armarx::armem::grasping::known_grasps
{ instance = &i; });
if (instance == nullptr)
{
- ARMARX_WARNING << "No entity snapshots found";
+ ARMARX_VERBOSE << "No entity snapshots found";
return std::nullopt;
}
return armem::grasping::arondto::KnownGraspInfo::FromAron(instance->data());
@@ -88,7 +88,7 @@ namespace armarx::armem::grasping::known_grasps
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
- ARMARX_INFO << "Lookup result in reader: " << qResult;
+ ARMARX_VERBOSE << "Lookup result in reader: " << qResult;
if (not qResult.success) /* c++20 [[unlikely]] */
{
diff --git a/source/RobotAPI/libraries/armem_robot/client/interfaces.h b/source/RobotAPI/libraries/armem_robot/client/interfaces.h
index 4975e53e597f54b3f451c7075e368f00f692d343..64d68ed6a3549fa7b0e6fa5745a1cc5731158a38 100644
--- a/source/RobotAPI/libraries/armem_robot/client/interfaces.h
+++ b/source/RobotAPI/libraries/armem_robot/client/interfaces.h
@@ -12,10 +12,10 @@ namespace armarx::armem::robot
public:
virtual ~ReaderInterface() = default;
- virtual bool synchronize(Robot& obj, const armem::Time& timestamp) = 0;
+ virtual bool synchronize(Robot& obj, const armem::Time& timestamp) const = 0;
- virtual Robot get(const RobotDescription& description, const armem::Time& timestamp) = 0;
- virtual std::optional<Robot> get(const std::string& name, const armem::Time& timestamp) = 0;
+ virtual Robot get(const RobotDescription& description, const armem::Time& timestamp) const = 0;
+ virtual std::optional<Robot> get(const std::string& name, const armem::Time& timestamp) const = 0;
};
class WriterInterface
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 7699417369cb2dae5d6cc59b8a1d7fabcd0b9251..bfb85d68f00e31832119278e0b155a54810ea424 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -66,7 +66,7 @@ namespace armarx::armem::robot_state
}
std::optional<robot::Robot>
- RobotReader::get(const std::string& name, const armem::Time& timestamp)
+ RobotReader::get(const std::string& name, const armem::Time& timestamp) const
{
const auto description = queryDescription(name, timestamp);
@@ -80,7 +80,7 @@ namespace armarx::armem::robot_state
}
robot::Robot
- RobotReader::get(const robot::RobotDescription& description, const armem::Time& timestamp)
+ RobotReader::get(const robot::RobotDescription& description, const armem::Time& timestamp) const
{
robot::Robot robot{.description = description,
.instance = "", // TODO(fabian.reister):
@@ -106,7 +106,7 @@ namespace armarx::armem::robot_state
}
bool
- RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp)
+ RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp) const
{
const auto tsStartFunctionInvokation = armem::Time::Now();
@@ -136,7 +136,7 @@ namespace armarx::armem::robot_state
}
std::optional<robot::RobotDescription>
- RobotReader::queryDescription(const std::string& name, const armem::Time& timestamp)
+ RobotReader::queryDescription(const std::string& name, const armem::Time& timestamp) const
{
const auto sanitizedTimestamp = timestamp.isValid() ? timestamp : Clock::Now();
@@ -184,7 +184,7 @@ namespace armarx::armem::robot_state
std::optional<robot::RobotState>
RobotReader::queryState(const robot::RobotDescription& description,
- const armem::Time& timestamp)
+ const armem::Time& timestamp) const
{
const auto jointMap = queryJointState(description, timestamp);
if (not jointMap)
@@ -800,7 +800,7 @@ namespace armarx::armem::robot_state
}
std::vector<robot::RobotDescription>
- RobotReader::queryDescriptions(const armem::Time& timestamp)
+ RobotReader::queryDescriptions(const armem::Time& timestamp) const
{
const auto sanitizedTimestamp = timestamp.isValid() ? timestamp : Clock::Now();
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 ee1274fab20cadb62231bc7fdd685c408104e705..3a225855858d830e4cc85183689f2c67a2afff5b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -51,20 +51,20 @@ namespace armarx::armem::robot_state
virtual void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
- [[nodiscard]] bool synchronize(robot::Robot& obj, const armem::Time& timestamp) override;
+ [[nodiscard]] bool synchronize(robot::Robot& obj, const armem::Time& timestamp) const override;
std::optional<robot::Robot> get(const std::string& name,
- const armem::Time& timestamp) override;
+ const armem::Time& timestamp) const override;
robot::Robot get(const robot::RobotDescription& description,
- const armem::Time& timestamp) override;
+ const armem::Time& timestamp) const override;
std::optional<robot::RobotDescription> queryDescription(const std::string& name,
- const armem::Time& timestamp);
+ const armem::Time& timestamp) const;
- std::vector<robot::RobotDescription> queryDescriptions(const armem::Time& timestamp);
+ std::vector<robot::RobotDescription> queryDescriptions(const armem::Time& timestamp) const;
std::optional<robot::RobotState> queryState(const robot::RobotDescription& description,
- const armem::Time& timestamp);
+ const armem::Time& timestamp) const;
std::optional<robot::RobotState::JointMap>
queryJointState(const robot::RobotDescription& description,
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 416585111bfbd96cca816211285faf9ea3715792..0e972f79138fbfc973dbab543206d94965227f03 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -32,7 +32,7 @@ namespace armarx::armem::robot_state
RobotReader::registerPropertyDefinitions(def);
}
- bool VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot, const armem::Time& timestamp)
+ bool VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot, const armem::Time& timestamp) const
{
// const static auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
// const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
index a80d9b02871c65998c75c3ee5522ef4f71af9a06..7a2f39c17939d30b26ef75057e1c9d302b0f68a8 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -47,7 +47,7 @@ namespace armarx::armem::robot_state
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def) override;
[[nodiscard]] bool synchronizeRobot(VirtualRobot::Robot& robot,
- const armem::Time& timestamp);
+ const armem::Time& timestamp) const;
[[nodiscard]] VirtualRobot::RobotPtr
getRobot(const std::string& name,
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h b/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h
index 69cabb5ba160f3abef84d331b971c4146d866c29..b5444266d3a2bfbc09e2cc151755eb90402e511a 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h
+++ b/source/RobotAPI/libraries/aron/core/data/variant/detail/PrimitiveVariant.h
@@ -45,7 +45,7 @@ namespace armarx::aron::data::detail
using ValueType = ValueT;
public:
- using Base::SpecializedVariantBase;
+ using typename Base::SpecializedVariantBase;
PrimitiveVariant(const ValueT& v,
const data::Descriptor descriptor,