From b7d811bb24f51dee9a5ee47b30fb4a84160e0ad0 Mon Sep 17 00:00:00 2001
From: Fabian Peller <fabian.peller-konrad@kit.edu>
Date: Fri, 12 May 2023 23:25:49 +0200
Subject: [PATCH] remove polling possibility
---
.../components/RobotHealth/RobotHealth.cpp | 383 +++++++++++-------
.../components/RobotHealth/RobotHealth.h | 161 ++++----
.../RobotHealth/RobotHealthDummy.cpp | 68 ++--
.../components/RobotHealth/RobotHealthDummy.h | 25 +-
.../components/RobotHealthInterface.ice | 36 +-
5 files changed, 394 insertions(+), 279 deletions(-)
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
index 0e4b7f72a..5364b5de5 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
@@ -20,245 +20,323 @@
* GNU General Public License
*/
-#include <ArmarXCore/util/CPPUtility/trace.h>
-
#include "RobotHealth.h"
#include <boost/regex.hpp>
+#include <ArmarXCore/core/time/ice_conversions.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
namespace armarx
{
- void RobotHealth::onInitComponent()
+ void
+ RobotHealth::onInitComponent()
{
ARMARX_TRACE;
- monitorHealthTask = new PeriodicTask<RobotHealth>(this, &RobotHealth::monitorHealthTaskClb, 10);
- defaultMaximumCycleTimeWarn = getProperty<int>("MaximumCycleTimeWarnMS").getValue();
- defaultMaximumCycleTimeErr = getProperty<int>("MaximumCycleTimeErrMS").getValue();
- usingTopic(getProperty<std::string>("RobotHealthTopicName").getValue());
- reportErrorsWithSpeech = getProperty<bool>("ReportErrorsWithSpeech").getValue();
- speechMinimumReportInterval = getProperty<int>("SpeechMinimumReportInterval").getValue();
-
- ARMARX_TRACE;
- //robotUnitRequired = getProperty<bool>("RobotUnitRequired").getValue();
- /*if(robotUnitRequired)
- {
- ARMARX_IMPORTANT << "RobotUnit is required";
- usingProxy(getProperty<std::string>("RobotUnitName").getValue());
- }*/
+ monitorUpdateHealthTask =
+ new PeriodicTask<RobotHealth>(this, &RobotHealth::monitorHealthUpdateTaskClb, 10);
+ monitorAggregatedHealthTask =
+ new PeriodicTask<RobotHealth>(this, &RobotHealth::monitorAggregatedHealthTaskClb, 10);
- //usingProxy(getProperty<std::string>("RemoteGuiName").getValue());
+ defaultMaximumCycleTimeWarn =
+ armarx::core::time::Duration::MilliSeconds(p.maximumCycleTimeWarnMS);
+ defaultMaximumCycleTimeErr =
+ armarx::core::time::Duration::MilliSeconds(p.maximumCycleTimeErrMS);
setDebugObserverBatchModeEnabled(true);
}
-
- void RobotHealth::onConnectComponent()
+ void
+ RobotHealth::onConnectComponent()
{
ARMARX_TRACE;
- emergencyStopTopicPrx = getTopic<EmergencyStopListenerPrx>(getProperty<std::string>("EmergencyStopTopicName").getValue());
- //remoteGuiPrx = getProxy<RemoteGuiInterfacePrx>(getProperty<std::string>("RemoteGuiName").getValue());
- aggregatedRobotHealthTopicPrx = getTopic<AggregatedRobotHealthInterfacePrx>(getProperty<std::string>("AggregatedRobotHealthTopicName").getValue());
- textToSpeechTopic = getTopic<TextListenerInterfacePrx>(getProperty<std::string>("TextToSpeechTopicName").getValue());
- lastSpeechOutput = TimeUtil::GetTime();
- /*if(robotUnitRequired)
- {
- robotUnitPrx = getTopic<RobotUnitInterfacePrx>(getProperty<std::string>("RobotUnitName").getValue());
- robotUnitPrx->enableHeartbeat();
- }*/
-
- monitorHealthTask->start();
+ monitorUpdateHealthTask->start();
}
- void RobotHealth::monitorHealthTaskClb()
+ void
+ RobotHealth::monitorHealthUpdateTaskClb()
{
- ARMARX_TRACE;
- long now = TimeUtil::GetTime().toMicroSeconds();
- bool hasNewErr = false;
+ std::shared_lock lock(updateMutex);
- RobotHealthState healthState = RobotHealthState::HealthOK;
+ ARMARX_TRACE;
+ auto now = armarx::core::time::DateTime::Now();
- for (size_t i = 0; i < validEntries; i++)
+ for (size_t i = 0; i < updateEntries.size(); i++)
{
ARMARX_TRACE;
- Entry& e = entries.at(i);
- long delta = std::max(now - e.lastUpdate, e.lastDelta);
- if (e.required && delta > e.maximumCycleTimeErr)
- {
- healthState = RobotHealthState::HealthError;
- }
+ UpdateEntry& e = updateEntries.at(i);
+
+ std::unique_lock elock(e.mutex);
if (!e.enabled)
{
continue;
}
- ARMARX_TRACE;
+ auto delta = now - e.history.back();
+
+ ARMARX_TRACE;
if (delta > e.maximumCycleTimeErr)
{
ARMARX_TRACE;
- healthState = RobotHealthState::HealthError;
- if (e.isRunning)
+
+ if (e.state != HealthError) // not already in error state
{
ARMARX_TRACE;
- ARMARX_ERROR << deactivateSpam(0.1, e.name) << "Component " << e.name << " has died.";
- if (reportErrorsWithSpeech && (TimeUtil::GetTime() - lastSpeechOutput).toSecondsDouble() > speechMinimumReportInterval)
- {
- ARMARX_TRACE;
- lastSpeechOutput = TimeUtil::GetTime();
-
- std::string name;
-
- boost::smatch m;
- boost::regex regex("^[a-zA-Z]+");
- if (boost::regex_search(e.name, m, regex))
- {
- if (m.empty())
- {
- name = "Whatever";
- }
- else
- {
- name = m[0].str();
- }
- }
-
- textToSpeechTopic->reportText("Oh no! Component " + name + " is no longer running.");
- }
- hasNewErr = true;
- e.isRunning = false;
+ ARMARX_ERROR << deactivateSpam(0.1, e.name) << "Component " << e.name
+ << " has died.";
+ e.state = HealthError;
}
}
- else if (e.isRunning && delta > e.maximumCycleTimeWarn)
+ else if (delta > e.maximumCycleTimeWarn)
{
ARMARX_TRACE;
- ARMARX_WARNING << deactivateSpam(0.1, e.name) << "Component " << e.name << " is running too slow.";
- if (healthState == RobotHealthState::HealthOK)
+ ARMARX_WARNING << deactivateSpam(0.1, e.name) << "Component " << e.name
+ << " is running too slow.";
+
+ e.state = HealthWarning;
+ }
+ }
+
+ reportDebugObserver();
+ }
+
+ void
+ RobotHealth::monitorAggregatedHealthTaskClb()
+ {
+ RobotHealthState overallHealthState = RobotHealthState::HealthOK;
+
+ {
+ std::shared_lock lock(updateMutex);
+
+ // get aggregated status
+ for (size_t i = 0; i < updateEntries.size(); i++)
+ {
+ ARMARX_TRACE;
+ UpdateEntry& e = updateEntries.at(i);
+
+ std::shared_lock elock(e.mutex);
+
+ if (e.required)
+ {
+ if (e.state == HealthWarning && overallHealthState != HealthError)
+ {
+ overallHealthState = RobotHealthState::HealthWarning;
+ }
+ else if (e.state == HealthError)
+ {
+ overallHealthState = RobotHealthState::HealthError;
+ }
+ }
+ else
{
- healthState = RobotHealthState::HealthWarning;
+ if (e.state != HealthOK && overallHealthState != HealthError)
+ {
+ overallHealthState = RobotHealthState::HealthWarning;
+ }
}
}
-
}
- if (hasNewErr)
+
+ if (overallHealthState == HealthError)
{
ARMARX_TRACE;
- emergencyStopTopicPrx->reportEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
+ p.emergencyStopTopicPrx->reportEmergencyStopState(
+ EmergencyStopState::eEmergencyStopActive);
}
ARMARX_TRACE;
- aggregatedRobotHealthTopicPrx->aggregatedHeartbeat(healthState);
- /*if(allRequiredRunning && robotUnitPrx)
- {
- try
- {
- robotUnitPrx->sendHeartbeat();
- }
- catch(...)
- {}
- }*/
-
- reportDebugObserver();
+ p.aggregatedRobotHealthTopicPrx->aggregatedHeartbeat(overallHealthState);
}
- void RobotHealth::onDisconnectComponent()
+ void
+ RobotHealth::onDisconnectComponent()
{
//robotUnitPrx = nullptr;
- monitorHealthTask->stop();
+ monitorUpdateHealthTask->stop();
}
-
- void RobotHealth::onExitComponent()
+ void
+ RobotHealth::onExitComponent()
{
-
}
- armarx::PropertyDefinitionsPtr RobotHealth::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ RobotHealth::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new RobotHealthPropertyDefinitions(
- getConfigIdentifier()));
+ auto defs = armarx::PropertyDefinitionsPtr(
+ new armarx::ComponentPropertyDefinitions(getConfigIdentifier()));
+
+
+ defs->topic(p.emergencyStopTopicPrx,
+ "The name of the topic over which changes of the emergencyStopState are sent.");
+ defs->topic<RobotHealthInterfacePrx>(p.robotHealthTopicName,
+ "Name of the RobotHealth topic");
+
+ defs->topic(p.aggregatedRobotHealthTopicPrx, "Name of the AggregatedRobotHealthTopic");
+
+ defs->optional(p.maximumCycleTimeWarnMS,
+ "Default value of the maximum cycle time for warnings");
+ defs->optional(p.maximumCycleTimeErrMS,
+ "Default value of the maximum cycle time for error");
+
+ return defs;
}
- RobotHealth::Entry& RobotHealth::findOrCreateEntry(const std::string& name)
+ std::pair<bool, RobotHealth::UpdateEntry&>
+ RobotHealth::findOrCreateUpdateEntry(const std::string& name)
{
ARMARX_TRACE;
- for (size_t i = 0; i < entries.size(); i++)
{
- if (entries.at(i).name == name)
+ std::shared_lock lock(updateMutex);
+
+ for (size_t i = 0; i < updateEntries.size(); i++)
{
- return entries.at(i);
+ if (updateEntries.at(i).name == name)
+ {
+ return {false, updateEntries.at(i)};
+ }
}
+
+ // Todo upgrade lock?
}
- std::unique_lock lock(mutex);
- ARMARX_INFO << "registering '" << name << "'";
- entries.emplace_back(name, defaultMaximumCycleTimeWarn * 1000, defaultMaximumCycleTimeErr * 1000);
- validEntries++;
- return entries.back();
- }
+ std::unique_lock lock(updateMutex);
+ ARMARX_INFO << "registering for updates: '" << name << "'";
+ updateEntries.emplace_back(name, defaultMaximumCycleTimeWarn, defaultMaximumCycleTimeErr);
- void RobotHealth::heartbeat(const std::string& componentName, const RobotHealthHeartbeatArgs& args, const Ice::Current&)
+ return {true, updateEntries.back()};
+ }
+
+ void
+ RobotHealth::signUp(const std::string& componentName,
+ const RobotHealthHeartbeatArgs& args,
+ const Ice::Current& current)
{
ARMARX_TRACE;
- Entry& e = findOrCreateEntry(componentName);
- long now = TimeUtil::GetTime().toMicroSeconds();
- e.maximumCycleTimeWarn = args.maximumCycleTimeWarningMS * 1000;
- e.maximumCycleTimeErr = args.maximumCycleTimeErrorMS * 1000;
- if (!e.isRunning)
+ if (args->mode == POLLING)
{
- ARMARX_INFO << "'" << componentName << "' is now alive and running";
- e.lastDelta = 0;
+ // TODO
}
else
{
- e.lastDelta = now - e.lastUpdate;
- e.history.at(e.historyIndex) = e.lastDelta;
- e.historyIndex = (e.historyIndex + 1) % e.history.size();
+ auto argsCasted = RobotHealthUpdateHeartbeatArgsPtr::dynamicCast(args);
+
+ auto e = findOrCreateUpdateEntry(componentName);
+
+ std::unique_lock lock(e.second.mutex);
+
+ if (e.first)
+ {
+ ARMARX_INFO << "Newly registering component " << componentName << " for updates.";
+ }
+ else
+ {
+ ARMARX_INFO << "Updating the config for component " << componentName
+ << " for updates.";
+ }
+
+ armarx::core::time::fromIce(argsCasted->maximumCycleTimeWarning,
+ e.second.maximumCycleTimeWarn);
+ armarx::core::time::fromIce(argsCasted->maximumCycleTimeError,
+ e.second.maximumCycleTimeErr);
+
+ e.second.required = args->required;
+ e.second.description = args->description;
+ e.second.enabled = true;
+ e.second.state = HealthOK;
+ e.second.history.clear();
}
+ }
+
+ void
+ RobotHealth::heartbeat(const std::string& componentName,
+ const core::time::dto::DateTime& referenceTime,
+ const Ice::Current&)
+ {
ARMARX_TRACE;
- e.lastUpdate = now;
- e.isRunning = true;
- e.enabled = true;
+ auto e = findOrCreateUpdateEntry(componentName);
+
+ std::unique_lock lock(e.second.mutex);
+
+ if (e.first) // newly created
+ {
+ ARMARX_WARNING << "Attention. Component " << componentName
+ << " was not signed up for heartbeat. Creating a heartbeat with default "
+ "config... Thus, this component is by default NOT REQUIRED.";
+ }
+
+ if (!e.second.enabled)
{
- std::unique_lock lock(e.messageMutex);
- e.message = args.message;
+ ARMARX_WARNING << "Attention. Component " << componentName
+ << " was not enabled. The heartbeat updated again enabled this "
+ "component for updates using the latest config found. ";
+ e.second.enabled = true;
}
+
+ auto now = armarx::core::time::DateTime::Now();
}
- void armarx::RobotHealth::unregister(const std::string& componentName, const Ice::Current&)
+ void
+ armarx::RobotHealth::unregister(const std::string& componentName, const Ice::Current&)
{
ARMARX_TRACE;
- Entry& e = findOrCreateEntry(componentName);
- e.isRunning = false;
- e.enabled = false;
- ARMARX_INFO << "unregistering " << componentName;
+ bool found = false;
+ {
+ std::shared_lock lock(pollingMutex);
+
+ for (size_t i = 0; i < updateEntries.size(); i++)
+ {
+ auto& e = updateEntries.at(i);
+ if (e.name == componentName)
+ {
+ std::unique_lock elock(e.mutex);
+ e.enabled = false;
+ break;
+ }
+ }
+ // Todo upgrade lock?
+ }
+
+ if (found)
+ {
+ ARMARX_INFO << "Component " << componentName << " disabled from heartbeat";
+ }
+ else
+ {
+ ARMARX_INFO << "Could not unregister component " << componentName << ". Not found.";
+ }
}
- std::string RobotHealth::getSummary(const Ice::Current&)
+ RobotHealthInfo
+ RobotHealth::getSummary(const Ice::Current&)
{
ARMARX_TRACE;
- long now = TimeUtil::GetTime().toMicroSeconds();
+ auto now = armarx::core::time::DateTime::Now();
+
std::stringstream ss;
- for (size_t i = 0; i < validEntries; i++)
+ for (size_t i = 0; i < updateEntries.size(); i++)
{
ARMARX_TRACE;
- Entry& e = entries.at(i);
- long delta = std::max(now - e.lastUpdate, e.lastDelta);
+ UpdateEntry& e = updateEntries.at(i);
+ auto delta = now - e.history.back();
- long minDelta = delta;
- long maxDelta = delta;
+ auto minDelta = delta;
+ auto maxDelta = delta;
for (size_t i = 1; i < e.history.size(); i++)
{
- long historicalDelta = e.history.at(i);
- if (historicalDelta < 0)
+ auto historicalDelta = e.history.at(i);
+
+ if (minDelta > historicalDelta)
+ {
+ }
+
+ if (maxDelta < historicalDelta)
{
- break;
}
- minDelta = std::min(minDelta, historicalDelta);
- maxDelta = std::max(maxDelta, historicalDelta);
}
ss << e.name;
@@ -285,31 +363,34 @@ namespace armarx
{
ss << ": ok";
}
- ss << " (" << (delta / 1000) << "ms, min: " << (minDelta / 1000) << "ms, max: " << (maxDelta / 1000) << "ms)";
+ ss << " (" << (delta / 1000) << "ms, min: " << (minDelta / 1000)
+ << "ms, max: " << (maxDelta / 1000) << "ms)";
}
ARMARX_TRACE;
- std::unique_lock lock(e.messageMutex);
+ std::unique_lock lock(e.mutex);
if (e.message.size())
{
ss << " - " << e.message;
}
ss << "\n";
-
}
return ss.str();
}
- void RobotHealth::reportDebugObserver()
+ void
+ RobotHealth::reportDebugObserver()
{
- for(const auto& entry: entries)
+ for (const auto& entry : updateEntries)
{
setDebugObserverDatafield("RobotHealth_" + entry.name + "_lastDelta", entry.lastDelta);
- setDebugObserverDatafield("RobotHealth_" + entry.name + "_maximumCycleTimeWarn", entry.maximumCycleTimeWarn);
- setDebugObserverDatafield("RobotHealth_" + entry.name + "_maximumCycleTimeErr", entry.maximumCycleTimeErr);
+ setDebugObserverDatafield("RobotHealth_" + entry.name + "_maximumCycleTimeWarn",
+ entry.maximumCycleTimeWarn);
+ setDebugObserverDatafield("RobotHealth_" + entry.name + "_maximumCycleTimeErr",
+ entry.maximumCycleTimeErr);
}
sendDebugObserverBatch();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.h b/source/RobotAPI/components/RobotHealth/RobotHealth.h
index 3da12c179..a47b7a7d0 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.h
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.h
@@ -22,54 +22,28 @@
#pragma once
-#include <RobotAPI/interface/components/RobotHealthInterface.h>
-#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-#include <RobotAPI/interface/speech/SpeechInterface.h>
-
-#include <ArmarXGui/interface/RemoteGuiInterface.h>
+#include <array>
+#include <atomic>
+#include <deque>
+#include <mutex>
-#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/Duration.h>
+#include <ArmarXCore/core/time/Frequency.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/interface/components/EmergencyStopInterface.h>
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
-#include <atomic>
-#include <mutex>
+#include <ArmarXGui/interface/RemoteGuiInterface.h>
+
+#include <RobotAPI/interface/components/RobotHealthInterface.h>
+#include <RobotAPI/interface/speech/SpeechInterface.h>
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
namespace armarx
{
- /**
- * @class RobotHealthPropertyDefinitions
- * @brief
- */
- class RobotHealthPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
- {
- public:
- RobotHealthPropertyDefinitions(std::string prefix):
- armarx::ComponentPropertyDefinitions(prefix)
- {
- defineOptionalProperty<std::string>("EmergencyStopTopicName", "EmergencyStop", "The name of the topic over which changes of the emergencyStopState are sent.");
- defineOptionalProperty<std::string>("RobotHealthTopicName", "RobotHealthTopic", "Name of the RobotHealth topic");
- defineOptionalProperty<bool>("ReportErrorsWithSpeech", true, "");
- defineOptionalProperty<std::string>("TextToSpeechTopicName", "TextToSpeech", "Name of the TextToSpeech topic");
- defineOptionalProperty<int>("MaximumCycleTimeWarnMS", 50, "Default value of the maximum cycle time for warnings");
- defineOptionalProperty<int>("MaximumCycleTimeErrMS", 100, "Default value of the maximum cycle time for error");
- defineOptionalProperty<std::string>("AggregatedRobotHealthTopicName", "AggregatedRobotHealthTopic", "Name of the AggregatedRobotHealthTopic");
- defineOptionalProperty<std::string>("RequiredComponents", "", "Comma separated list of required components");
-
- defineOptionalProperty<int>("SpeechMinimumReportInterval", 60, "Time that has to pass between reported messages in seconds.");
-
- //defineOptionalProperty<std::string>("RemoteGuiName", "RemoteGuiProvider", "Name of the remote GUI provider");
- //defineOptionalProperty<std::string>("RobotUnitName", "Armar6Unit", "Name of the RobotUnit");
- //defineOptionalProperty<bool>("RobotUnitRequired", true, "Wait for RobotUnit");
-
- //defineRequiredProperty<std::string>("PropertyName", "Description");
- //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
- }
- };
-
/**
* @defgroup Component-RobotHealth RobotHealth
* @ingroup RobotAPI-Components
@@ -90,34 +64,12 @@ namespace armarx
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "RobotHealth";
}
- struct Entry
- {
- Entry(std::string name, long maximumCycleTimeWarn, long maximumCycleTimeErr)
- : name(name), maximumCycleTimeWarn(maximumCycleTimeWarn), maximumCycleTimeErr(maximumCycleTimeErr), history(100, -1)
- {}
- Entry(Entry&&) = default;
- Entry& operator=(Entry&&) = default;
-
- std::string name;
- long maximumCycleTimeWarn;
- long maximumCycleTimeErr;
- long lastUpdate = 0;
- long lastDelta = 0;
- RobotHealthState state = HealthOK;
- std::string message = "";
- bool isRunning = false;
- bool required = false;
- bool enabled = true;
- std::mutex messageMutex;
- std::vector<long> history;
- size_t historyIndex = 0;
- };
-
protected:
/**
* @see armarx::ManagedIceObject::onInitComponent()
@@ -144,32 +96,73 @@ namespace armarx
*/
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
- void monitorHealthTaskClb();
- Entry& findOrCreateEntry(const std::string& name);
+ private:
+ struct UpdateEntry
+ {
+ UpdateEntry(const std::string& name,
+ const armarx::core::time::Duration& maximumCycleTimeWarn,
+ const armarx::core::time::Duration& maximumCycleTimeErr) :
+ name(name),
+ maximumCycleTimeWarn(maximumCycleTimeWarn),
+ maximumCycleTimeErr(maximumCycleTimeErr)
+ {
+ }
- void reportDebugObserver();
+ std::string name;
+ RobotHealthState state = HealthOK;
+ std::string description = "";
+
+ bool required = false;
+ bool enabled = true;
- std::mutex mutex;
- std::deque<Entry> entries;
- std::atomic_size_t validEntries {0};
- PeriodicTask<RobotHealth>::pointer_type monitorHealthTask;
- int defaultMaximumCycleTimeWarn;
- int defaultMaximumCycleTimeErr;
- EmergencyStopListenerPrx emergencyStopTopicPrx;
- //RobotUnitInterfacePrx robotUnitPrx;
- //bool robotUnitRequired;
- RemoteGuiInterfacePrx remoteGuiPrx;
- AggregatedRobotHealthInterfacePrx aggregatedRobotHealthTopicPrx;
- TextListenerInterfacePrx textToSpeechTopic;
- bool reportErrorsWithSpeech;
- int speechMinimumReportInterval;
- IceUtil::Time lastSpeechOutput;
+ mutable std::shared_mutex mutex;
+ std::deque<armarx::core::time::DateTime> history;
+
+ armarx::core::time::Duration maximumCycleTimeWarn;
+ armarx::core::time::Duration maximumCycleTimeErr;
+ };
+
+ void monitorHealthUpdateTaskClb();
+ void monitorAggregatedHealthTaskClb();
+
+ std::pair<bool, UpdateEntry&> findOrCreateUpdateEntry(const std::string& name);
+
+ void reportDebugObserver();
// RobotHealthInterface interface
public:
- void heartbeat(const std::string& componentName, const RobotHealthHeartbeatArgs& args, const Ice::Current&) override;
void unregister(const std::string& componentName, const Ice::Current&) override;
- std::string getSummary(const Ice::Current&) override;
+ void signUp(const std::string& componentName,
+ const RobotHealthHeartbeatArgs& args,
+ const Ice::Current& current) override;
+ void heartbeat(const std::string& componentName,
+ const std::string& message,
+ const core::time::dto::DateTime& referenceTime,
+ const Ice::Current& current) override;
+
+ // RobotHealthComponentInterface interface
+ public:
+ RobotHealthInfo getSummary(const Ice::Current& current) override;
+
+ private:
+ mutable std::shared_mutex updateMutex;
+
+ std::deque<UpdateEntry> updateEntries;
+ PeriodicTask<RobotHealth>::pointer_type monitorUpdateHealthTask;
+ PeriodicTask<RobotHealth>::pointer_type monitorAggregatedHealthTask;
+
+ armarx::core::time::Duration defaultMaximumCycleTimeWarn;
+ armarx::core::time::Duration defaultMaximumCycleTimeErr;
+
+ struct Properties
+ {
+ EmergencyStopListenerPrx emergencyStopTopicPrx;
+ std::string robotHealthTopicName = "RobotHealthTopic";
+ AggregatedRobotHealthInterfacePrx aggregatedRobotHealthTopicPrx;
+ RemoteGuiInterfacePrx remoteGuiPrx;
+ long maximumCycleTimeWarnMS = 50;
+ long maximumCycleTimeErrMS = 100;
+ } p;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
index b239fdd07..416a43da0 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
+++ b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
@@ -21,26 +21,35 @@
*/
#include "RobotHealthDummy.h"
+
#include <time.h>
+
#include <thread>
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+
namespace armarx
{
- void RobotHealthDummy::onInitComponent()
+ void
+ RobotHealthDummy::onInitComponent()
{
dummyTask = new RunningTask<RobotHealthDummy>(this, &RobotHealthDummy::runTask);
getProperty(sleepmode, "SleepMode");
}
-
- void RobotHealthDummy::onConnectComponent()
+ void
+ RobotHealthDummy::onConnectComponent()
{
- robotHealthTopicPrx = getTopic<RobotHealthInterfacePrx>(getProperty<std::string>("RobotHealthTopicName").getValue());
+ robotHealthTopicPrx = getTopic<RobotHealthInterfacePrx>(
+ getProperty<std::string>("RobotHealthTopicName").getValue());
dummyTask->start();
}
-#define NANOSECS_PER_SEC 1000000000
- int RobotHealthDummy::NanoSleep(long nanosec)
+#define NANOSECS_PER_SEC 1000000000
+
+ int
+ RobotHealthDummy::NanoSleep(long nanosec)
{
struct timespec ts;
ts.tv_sec = nanosec / NANOSECS_PER_SEC;
@@ -48,48 +57,54 @@ namespace armarx
return nanosleep(&ts, nullptr); // jitter up to +100ms!
}
- void RobotHealthDummy::sleepwait(long microseconds)
+ void
+ RobotHealthDummy::sleepwait(long microseconds)
{
long start = TimeUtil::GetTime().toMicroSeconds();
auto end = start + microseconds;
do
{
NanoSleep(1000);
- }
- while (TimeUtil::GetTime().toMicroSeconds() < end);
+ } while (TimeUtil::GetTime().toMicroSeconds() < end);
}
- void RobotHealthDummy::yieldwait(long microseconds)
+
+ void
+ RobotHealthDummy::yieldwait(long microseconds)
{
long start = TimeUtil::GetTime().toMicroSeconds();
auto end = start + microseconds;
do
{
std::this_thread::yield();
- }
- while (TimeUtil::GetTime().toMicroSeconds() < end); // jitter up to +25ms...
+ } while (TimeUtil::GetTime().toMicroSeconds() < end); // jitter up to +25ms...
}
- void RobotHealthDummy::busydwait(long microseconds)
+
+ void
+ RobotHealthDummy::busydwait(long microseconds)
{
long start = TimeUtil::GetTime().toMicroSeconds();
auto end = start + microseconds;
do
{
;
- }
- while (TimeUtil::GetTime().toMicroSeconds() < end);
+ } while (TimeUtil::GetTime().toMicroSeconds() < end);
}
- void RobotHealthDummy::runTask()
+ void
+ RobotHealthDummy::runTask()
{
-
+ robotHealthTopicPrx->signUp(getName(), RobotHealthHeartbeatArgs());
ARMARX_INFO << "starting rinning task";
while (!dummyTask->isStopped())
{
long beforeTopicCall = TimeUtil::GetTime().toMicroSeconds();
//ARMARX_INFO << "send heartbeat";
- robotHealthTopicPrx->heartbeat(getName(), RobotHealthHeartbeatArgs());
+
+ 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();
if (sleepmode == "nanosleep")
{
@@ -126,23 +141,24 @@ namespace armarx
}
}
-
- void RobotHealthDummy::onDisconnectComponent()
+ void
+ RobotHealthDummy::onDisconnectComponent()
{
//ARMARX_IMPORTANT << "onDisconnectComponent";
dummyTask->stop();
}
-
- void RobotHealthDummy::onExitComponent()
+ void
+ RobotHealthDummy::onExitComponent()
{
//ARMARX_IMPORTANT << "onExitComponent";
dummyTask->stop();
}
- armarx::PropertyDefinitionsPtr RobotHealthDummy::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ RobotHealthDummy::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new RobotHealthDummyPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new RobotHealthDummyPropertyDefinitions(getConfigIdentifier()));
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h
index cb9d46377..cfd8d369d 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h
+++ b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.h
@@ -24,10 +24,10 @@
#include <ArmarXCore/core/Component.h>
-#include <RobotAPI/interface/components/RobotHealthInterface.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <RobotAPI/interface/components/RobotHealthInterface.h>
namespace armarx
{
@@ -35,15 +35,18 @@ namespace armarx
* @class RobotHealthDummyPropertyDefinitions
* @brief
*/
- class RobotHealthDummyPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class RobotHealthDummyPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- RobotHealthDummyPropertyDefinitions(std::string prefix):
+ RobotHealthDummyPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("RobotHealthTopicName", "RobotHealthTopic", "Name of the RobotHealth topic");
- defineOptionalProperty<std::string>("SleepMode", "nanosleep", "Which sleep function to use: nanosleep, sleepwait, yieldwait, busywait");
+ defineOptionalProperty<std::string>(
+ "RobotHealthTopicName", "RobotHealthTopic", "Name of the RobotHealth topic");
+ defineOptionalProperty<std::string>(
+ "SleepMode",
+ "nanosleep",
+ "Which sleep function to use: nanosleep, sleepwait, yieldwait, busywait");
//defineRequiredProperty<std::string>("PropertyName", "Description");
//defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
@@ -61,14 +64,14 @@ namespace armarx
*
* Detailed description of class RobotHealthDummy.
*/
- class RobotHealthDummy :
- virtual public armarx::Component
+ class RobotHealthDummy : virtual public armarx::Component
{
public:
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "RobotHealthDummy";
}
@@ -77,6 +80,7 @@ namespace armarx
void yieldwait(long microseconds);
void busydwait(long microseconds);
void sleepwait(long microseconds);
+
protected:
/**
* @see armarx::ManagedIceObject::onInitComponent()
@@ -109,6 +113,5 @@ namespace armarx
RunningTask<RobotHealthDummy>::pointer_type dummyTask;
std::string sleepmode;
-
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/interface/components/RobotHealthInterface.ice b/source/RobotAPI/interface/components/RobotHealthInterface.ice
index bbad059d5..9a0ce888f 100644
--- a/source/RobotAPI/interface/components/RobotHealthInterface.ice
+++ b/source/RobotAPI/interface/components/RobotHealthInterface.ice
@@ -22,33 +22,55 @@
#pragma once
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/core/time.ice>
+
module armarx
{
enum RobotHealthState
{
HealthOK, HealthWarning, HealthError
- };
+ };´
struct RobotHealthHeartbeatArgs
{
- int maximumCycleTimeWarningMS = 100;
- int maximumCycleTimeErrorMS = 200;
- string message;
+ string componentName;
+ bool required;
+ string description;
+ Ice::StringList tags;
+
+ armarx::core::time::dto::Duration maximumCycleTimeWarning;
+ armarx::core::time::dto::Duration maximumCycleTimeError;
};
interface RobotHealthInterface
{
- void heartbeat(string componentName, RobotHealthHeartbeatArgs args);
- void unregister(string componentName);
+ idempotent void require(Ice::StringSeq namesOrTags);
+ idempotent void unrequire(Ice::StringSeq namesOrTags);
+
+ idempotent void signUp(RobotHealthHeartbeatArgs args);
+
+ idempotent void heartbeat(string componentName, string message, armarx::core::time::dto::DateTime referenceTime);
+ idempotent void unregister(string componentName);
};
+ // Used by the RobotHealth to send an overall status update ehich e.g. the emergency stop listens to.
interface AggregatedRobotHealthInterface
{
void aggregatedHeartbeat(RobotHealthState overallHealthState);
};
+ struct RobotHealthInfoEntry
+ {
+ string componentName;
+ RobotHealthState state;
+ string message;
+ };
+
+ dictionary<string, RobotHealthInfoEntry> RobotHealthInfo;
+
interface RobotHealthComponentInterface extends RobotHealthInterface
{
- string getSummary();
+ RobotHealthInfo getSummary();
};
};
--
GitLab