diff --git a/source/RobotAPI/components/GamepadControlUnit/CMakeLists.txt b/source/RobotAPI/components/GamepadControlUnit/CMakeLists.txt
index c3b764a153b15400568f67eb1dc9c2813874b921..8154ff69af2b057c18189c6ad07262e2a6f33142 100644
--- a/source/RobotAPI/components/GamepadControlUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/GamepadControlUnit/CMakeLists.txt
@@ -1,6 +1,6 @@
armarx_component_set_name("GamepadControlUnit")
-set(COMPONENT_LIBS ArmarXCoreInterfaces ArmarXCore ArmarXCoreObservers RobotAPIInterfaces )
+set(COMPONENT_LIBS ArmarXCoreInterfaces ArmarXCore ArmarXCoreObservers RobotAPIComponentPlugins RobotAPIInterfaces )
set(SOURCES GamepadControlUnit.cpp)
set(HEADERS GamepadControlUnit.h)
diff --git a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
index 5227bee38e728999d4a0f7fed6809913144573b6..5113961eeebb95c1af593714fbb1eca93ea84909 100644
--- a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
+++ b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp
@@ -22,15 +22,15 @@
#include "GamepadControlUnit.h"
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/Duration.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
-
-
-
-
namespace armarx
{
- void GamepadControlUnit::onInitComponent()
+ void
+ GamepadControlUnit::onInitComponent()
{
ARMARX_INFO << "oninit GamepadControlUnit";
usingProxy(getProperty<std::string>("PlatformUnitName").getValue());
@@ -41,67 +41,80 @@ namespace armarx
scaleY = getProperty<float>("ScaleY").getValue();
scaleRotation = getProperty<float>("ScaleAngle").getValue();
ARMARX_INFO << "oninit GamepadControlUnit end";
-
-
-
}
-
- void GamepadControlUnit::onConnectComponent()
+ void
+ GamepadControlUnit::onConnectComponent()
{
ARMARX_INFO << "onConnect GamepadControlUnit";
- platformUnitPrx = getProxy<PlatformUnitInterfacePrx>(getProperty<std::string>("PlatformUnitName").getValue());
+ platformUnitPrx = getProxy<PlatformUnitInterfacePrx>(
+ getProperty<std::string>("PlatformUnitName").getValue());
emergencyStop = getProxy<EmergencyStopMasterInterfacePrx>("EmergencyStopMaster");
- }
- void GamepadControlUnit::onDisconnectComponent()
- {
-
+ if (enableHeartBeat)
+ {
+ this->heartbeatPlugin->signUp(armarx::core::time::Duration::MilliSeconds(1000),
+ armarx::core::time::Duration::MilliSeconds(1500),
+ {"Gamepad"},
+ "The GamepadControlUnit");
+ }
}
+ void
+ GamepadControlUnit::onDisconnectComponent()
+ {
+ }
- void GamepadControlUnit::onExitComponent()
+ void
+ GamepadControlUnit::onExitComponent()
{
ARMARX_INFO << "exit GamepadControlUnit";
}
- armarx::PropertyDefinitionsPtr GamepadControlUnit::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ GamepadControlUnit::createPropertyDefinitions()
{
- auto defs = armarx::PropertyDefinitionsPtr(new GamepadControlUnitPropertyDefinitions(
- getConfigIdentifier()));
- defs->topic(robotHealthTopic);
- defs->optional(enableHeartBeat, "EnableHeartBeat", "Flag to enable send a heart beat to the robot healh topic");
- return defs;
+ auto defs = armarx::PropertyDefinitionsPtr(
+ new GamepadControlUnitPropertyDefinitions(getConfigIdentifier()));
+ defs->optional(enableHeartBeat,
+ "EnableHeartBeat",
+ "Flag to enable send a heart beat to the robot healh topic");
+ return defs;
}
- void GamepadControlUnit::reportGamepadState(const std::string& device, const std::string& name, const GamepadData& data, const TimestampBasePtr& timestamp, const Ice::Current& c)
+ void
+ GamepadControlUnit::reportGamepadState(const std::string& device,
+ const std::string& name,
+ const GamepadData& data,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current& c)
{
- // struct GamepadData {
- // float leftStickX;
- // float leftStickY;
- // float rightStickX;
- // float rightStickY;
- // float dPadX;
- // float dPadY;
- // float leftTrigger;
- // float rightTrigger;
- //
- // bool leftButton;
- // bool rightButton;
- // bool backButton;
- // bool startButton;
- // bool xButton;
- // bool yButton;
- // bool aButton;
- // bool bButton;
- // bool theMiddleButton;
- // bool leftStickButton;
- // bool rightStickButton;
- //
- // };
+ // struct GamepadData {
+ // float leftStickX;
+ // float leftStickY;
+ // float rightStickX;
+ // float rightStickY;
+ // float dPadX;
+ // float dPadY;
+ // float leftTrigger;
+ // float rightTrigger;
+ //
+ // bool leftButton;
+ // bool rightButton;
+ // bool backButton;
+ // bool startButton;
+ // bool xButton;
+ // bool yButton;
+ // bool aButton;
+ // bool bButton;
+ // bool theMiddleButton;
+ // bool leftStickButton;
+ // bool rightStickButton;
+ //
+ // };
if (data.leftTrigger > 0)
@@ -116,7 +129,9 @@ namespace armarx
//scales are for the robot axis
if (data.rightTrigger > 0)
{
- platformUnitPrx->move(data.leftStickY * scaleX, data. leftStickX * scaleY, data.rightStickX * scaleRotation);
+ platformUnitPrx->move(data.leftStickY * scaleX,
+ data.leftStickX * scaleY,
+ data.rightStickX * scaleRotation);
}
else
{
@@ -124,64 +139,68 @@ namespace armarx
}
- if (data.leftButton)
- {
-
- if(leftHandTime <= 0.0)
- {
- leftHandTime = IceUtil::Time::now().toMicroSeconds();
- }
- else if ((IceUtil::Time::now().toMicroSeconds() - leftHandTime) > 1000* 1000)
- {
-
- HandUnitInterfacePrx handUnit = getProxy<HandUnitInterfacePrx>("LeftHandUnit");
- if (handUnit)
- {
- std::string shapeName = (leftHandOpen) ? "Close" : "Open";
- handUnit->setShape(shapeName);
- leftHandOpen = !leftHandOpen;
- leftHandTime = 0.0;
- }
- }
- }
- else
- {
- leftHandTime = 0.0;
- }
-
- if(data.rightButton)
- {
-
- if(rightHandTime <= 0.0)
- {
- rightHandTime = IceUtil::Time::now().toMicroSeconds();
- }
- else if ((IceUtil::Time::now().toMicroSeconds() - rightHandTime) > 1000* 1000)
- {
- HandUnitInterfacePrx handUnit = getProxy<HandUnitInterfacePrx>("RightHandUnit");
- if (handUnit)
- {
- std::string shapeName = (rightHandOpen) ? "Close" : "Open";
- handUnit->setShape(shapeName);
- rightHandOpen = !rightHandOpen;
- rightHandTime = 0.0;
- }
- }
- }
- else
- {
- rightHandTime = 0.0;
- }
-
-
- if(enableHeartBeat)
- {
- RobotHealthHeartbeatArgs args;
- args.maximumCycleTimeErrorMS = 1000;
- robotHealthTopic->heartbeat(getDefaultName(), args);
-
- }
-
+ if (data.leftButton)
+ {
+
+ if (leftHandTime <= 0.0)
+ {
+ leftHandTime = IceUtil::Time::now().toMicroSeconds();
+ }
+ else if ((IceUtil::Time::now().toMicroSeconds() - leftHandTime) > 1000 * 1000)
+ {
+
+ HandUnitInterfacePrx handUnit = getProxy<HandUnitInterfacePrx>("LeftHandUnit");
+ if (handUnit)
+ {
+ std::string shapeName = (leftHandOpen) ? "Close" : "Open";
+ handUnit->setShape(shapeName);
+ leftHandOpen = !leftHandOpen;
+ leftHandTime = 0.0;
+ }
+ }
+ }
+ else
+ {
+ leftHandTime = 0.0;
+ }
+
+ if (data.rightButton)
+ {
+
+ if (rightHandTime <= 0.0)
+ {
+ rightHandTime = IceUtil::Time::now().toMicroSeconds();
+ }
+ else if ((IceUtil::Time::now().toMicroSeconds() - rightHandTime) > 1000 * 1000)
+ {
+ HandUnitInterfacePrx handUnit = getProxy<HandUnitInterfacePrx>("RightHandUnit");
+ if (handUnit)
+ {
+ std::string shapeName = (rightHandOpen) ? "Close" : "Open";
+ handUnit->setShape(shapeName);
+ rightHandOpen = !rightHandOpen;
+ rightHandTime = 0.0;
+ }
+ }
+ }
+ else
+ {
+ rightHandTime = 0.0;
+ }
+
+
+ if (enableHeartBeat)
+ {
+ auto now = armarx::core::time::dto::DateTime();
+ armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
+ heartbeatPlugin->heartbeat();
+ }
+
//ARMARX_INFO << "sending targets" << data.leftStickX* scaleX << " " << data.leftStickY* scaleY << " " << data.rightStickX* scaleRotation;
}
-}
+
+ GamepadControlUnit::GamepadControlUnit()
+ {
+ addPlugin(heartbeatPlugin);
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h
index da5945c44c7e289081df35385ea7e75184d69c90..9438f305fe6a96648eb4d6011112ea637a245369 100644
--- a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h
+++ b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h
@@ -30,6 +30,7 @@
#include <RobotAPI/interface/units/HandUnitInterface.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h>
#include <RobotAPI/interface/components/RobotHealthInterface.h>
#include <RobotAPI/interface/units/PlatformUnitInterface.h>
@@ -72,6 +73,8 @@ namespace armarx
virtual public GamepadUnitListener
{
public:
+ GamepadControlUnit();
+
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
@@ -109,10 +112,11 @@ namespace armarx
private:
PlatformUnitInterfacePrx platformUnitPrx;
+ plugins::HeartbeatComponentPlugin* heartbeatPlugin = nullptr;
- bool enableHeartBeat = false;
- RobotHealthInterfacePrx robotHealthTopic;
+
+ bool enableHeartBeat = false;
float scaleX;
float scaleY;
float scaleRotation;
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
index 0e4b7f72ac1cc1f5f9b69b678c2d2f790c479184..0daef1ab73ee13ed22d59090270ffe94a6736c44 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
@@ -20,296 +20,523 @@
* GNU General Public License
*/
-#include <ArmarXCore/util/CPPUtility/trace.h>
-
#include "RobotHealth.h"
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/time/Clock.h"
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <algorithm>
#include <boost/regex.hpp>
+#include <ArmarXCore/core/time/ice_conversions.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
+#include <optional>
namespace armarx
{
- 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();
+ inline const std::string PROPERTY_REQUESTER_ID = "required_default";
+ void
+ RobotHealth::onInitComponent()
+ {
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);
+ defaultMaximumCycleTimeWarn =
+ armarx::core::time::Duration::MilliSeconds(p.maximumCycleTimeWarnMS);
+ defaultMaximumCycleTimeErr =
+ armarx::core::time::Duration::MilliSeconds(p.maximumCycleTimeErrMS);
- //usingProxy(getProperty<std::string>("RemoteGuiName").getValue());
+ const std::vector<std::string> requiredTags = simox::alg::split(p.requiredTags, ",");
+ // a special requester: cannot be changed during runtime.
+ tagsPerRequester[PROPERTY_REQUESTER_ID] = std::set<std::string>(requiredTags.begin(), requiredTags.end());
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)
+ UpdateEntry& e = updateEntries.at(i);
+
+ std::unique_lock elock(e.mutex);
+
+ if (!e.enabled)
{
- healthState = RobotHealthState::HealthError;
+ continue;
}
- if (!e.enabled)
+ if(e.history.empty())
{
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;
+ }
+ else
+ {
+ // If everything is OK (again), update health
+ e.state = HealthOK;
+ }
+ }
+
+ // update aggregated health
+ RobotHealthState overallHealthState = RobotHealthState::HealthOK;
+
+
+ // 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);
+
+ // trim history
+ while (e.history.size() > 20)
+ {
+ e.history.pop_front();
+ }
+
+ if (e.required)
+ {
+ if (e.state == HealthWarning && overallHealthState != HealthError)
+ {
+ overallHealthState = RobotHealthState::HealthWarning;
+ }
+ else if (e.state == HealthError)
{
- healthState = RobotHealthState::HealthWarning;
+ overallHealthState = RobotHealthState::HealthError;
+ }
+ }
+ else
+ {
+ if (e.state != HealthOK && overallHealthState != HealthError)
+ {
+ overallHealthState = RobotHealthState::HealthWarning;
}
}
-
}
- if (hasNewErr)
+
+ if (overallHealthState == HealthError)
{
ARMARX_TRACE;
- emergencyStopTopicPrx->reportEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
+ ARMARX_INFO << "Requesting emergency stop";
+ ARMARX_CHECK_NOT_NULL(p.emergencyStopTopicPrx);
+ p.emergencyStopTopicPrx->reportEmergencyStopState(
+ EmergencyStopState::eEmergencyStopActive);
}
ARMARX_TRACE;
- aggregatedRobotHealthTopicPrx->aggregatedHeartbeat(healthState);
- /*if(allRequiredRunning && robotUnitPrx)
- {
- try
- {
- robotUnitPrx->sendHeartbeat();
- }
- catch(...)
- {}
- }*/
+ ARMARX_CHECK_NOT_NULL(p.aggregatedRobotHealthTopicPrx);
+ p.aggregatedRobotHealthTopicPrx->aggregatedHeartbeat(overallHealthState);
reportDebugObserver();
}
- void RobotHealth::onDisconnectComponent()
+ void
+ RobotHealth::onDisconnectComponent()
{
//robotUnitPrx = nullptr;
- monitorHealthTask->stop();
+ monitorUpdateHealthTask->stop();
}
+ void
+ RobotHealth::onExitComponent()
+ {
+ }
- void RobotHealth::onExitComponent()
+ armarx::PropertyDefinitionsPtr
+ RobotHealth::createPropertyDefinitions()
{
+ auto defs = armarx::PropertyDefinitionsPtr(
+ new armarx::ComponentPropertyDefinitions(getConfigIdentifier()));
+
+
+ defs->topic(p.emergencyStopTopicPrx, "EmergencyStop",
+ "The name of the topic over which changes of the emergencyStopState are sent.");
+ defs->topic<RobotHealthInterfacePrx>(p.robotHealthTopicName, "RobotHealthTopic",
+ "Name of the RobotHealth topic");
+
+ defs->topic(p.aggregatedRobotHealthTopicPrx, "AggregatedRobotHealthTopic", "Name of the AggregatedRobotHealthTopic");
+
+ defs->optional(p.maximumCycleTimeWarnMS, "MaximumCycleTimeWarnMS",
+ "Default value of the maximum cycle time for warnings");
+ defs->optional(p.maximumCycleTimeErrMS, "MaximumCycleTimeErrMS",
+ "Default value of the maximum cycle time for error");
+ defs->optional(p.requiredTags, "RequiredTags", "Tags that should be requested.");
+
+ return defs;
}
- armarx::PropertyDefinitionsPtr RobotHealth::createPropertyDefinitions()
+ RobotHealth::UpdateEntry*
+ RobotHealth::findUpdateEntry(const std::string& name)
{
- return armarx::PropertyDefinitionsPtr(new RobotHealthPropertyDefinitions(
- getConfigIdentifier()));
+ ARMARX_TRACE;
+
+ for (size_t i = 0; i < updateEntries.size(); i++)
+ {
+ if (updateEntries.at(i).name == name)
+ {
+ return &updateEntries.at(i);
+ }
+ }
+
+ return nullptr;
}
- 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)
+ for (size_t i = 0; i < updateEntries.size(); i++)
{
- return entries.at(i);
+ if (updateEntries.at(i).name == name)
+ {
+ return {false, updateEntries.at(i)};
+ }
}
}
- std::unique_lock lock(mutex);
- ARMARX_INFO << "registering '" << name << "'";
- entries.emplace_back(name, defaultMaximumCycleTimeWarn * 1000, defaultMaximumCycleTimeErr * 1000);
- validEntries++;
- return entries.back();
- }
+ ARMARX_INFO << "registering for heartbeat: '" << 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 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)
+ std::unique_lock lockU(updateMutex);
+
+ auto e = findOrCreateUpdateEntry(args.identifier);
+
{
- ARMARX_INFO << "'" << componentName << "' is now alive and running";
- e.lastDelta = 0;
+ std::unique_lock lock(e.second.mutex);
+
+ if (e.first)
+ {
+ ARMARX_INFO << "Newly registering component " << args.identifier << " for updates.";
+ }
+ // else no messsage as components may spam sign up for legacy reasons (when there was no sign up)
+
+ armarx::core::time::fromIce(args.maximumCycleTimeWarning, e.second.maximumCycleTimeWarn);
+ armarx::core::time::fromIce(args.maximumCycleTimeError, e.second.maximumCycleTimeErr);
+
+ e.second.tags = args.tags;
+ // e.second.required = args.requiredByDefault; // FIXME
+ e.second.description = args.description;
+ e.second.enabled = true;
+ e.second.state = HealthOK;
+ e.second.history.clear();
}
- else
+
+ updateRequiredElements();
+ }
+
+ void
+ RobotHealth::heartbeat(const std::string& identifier,
+ const core::time::dto::DateTime& /*referenceTime*/,
+ const Ice::Current& current)
+ {
+ ARMARX_TRACE;
+ ARMARX_VERBOSE << "Finding update entry";
+
+ // We hold a reference to 'o' which is an element in a vector.
+ // If we don't lock until the end of this scope, the vector size might change and 'o' will be invalidated.
+ std::shared_lock lockU(updateMutex);
+ auto* const entry = findUpdateEntry(identifier);
+
+ if (entry == nullptr)
{
- e.lastDelta = now - e.lastUpdate;
- e.history.at(e.historyIndex) = e.lastDelta;
- e.historyIndex = (e.historyIndex + 1) % e.history.size();
+ ARMARX_WARNING << "Attention. Component `" << identifier
+ << "` was not signed up for heartbeat. Ignoring heartbeat for now...";
+ return;
}
- ARMARX_TRACE;
- e.lastUpdate = now;
- e.isRunning = true;
- e.enabled = true;
+
+ ARMARX_CHECK_NOT_NULL(entry);
+
+ std::unique_lock lock(entry->mutex);
+
+ if (not entry->enabled)
{
- std::unique_lock lock(e.messageMutex);
- e.message = args.message;
+ ARMARX_WARNING << deactivateSpam() << "Attention. Component `" << identifier
+ << "` was not enabled. The heartbeat is ignored... ";
+ return;
}
- }
- 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;
+ ARMARX_VERBOSE << "Extending history";
+ // auto now = armarx::core::time::DateTime::Now();
+
+
+ // armarx::core::time::DateTime timestamp;
+ // fromIce(referenceTime, timestamp);
+ const DateTime timestamp = Clock::Now();
+ entry->history.push_back(timestamp);
}
- std::string RobotHealth::getSummary(const Ice::Current&)
+ void
+ armarx::RobotHealth::unregister(const std::string& componentName, const Ice::Current&)
{
ARMARX_TRACE;
- long now = TimeUtil::GetTime().toMicroSeconds();
- std::stringstream ss;
- for (size_t i = 0; i < validEntries; i++)
+ std::shared_lock lock(updateMutex);
+
+ bool found = false;
{
- ARMARX_TRACE;
- Entry& e = entries.at(i);
- long delta = std::max(now - e.lastUpdate, e.lastDelta);
+ std::shared_lock lock(updateMutex);
- long minDelta = delta;
- long maxDelta = delta;
- for (size_t i = 1; i < e.history.size(); i++)
+ for (size_t i = 0; i < updateEntries.size(); i++)
{
- long historicalDelta = e.history.at(i);
- if (historicalDelta < 0)
+ auto& e = updateEntries.at(i);
+ if (e.name == componentName)
{
+ std::unique_lock elock(e.mutex);
+ e.required = false;
+ e.enabled = false;
+ found = true;
break;
}
- minDelta = std::min(minDelta, historicalDelta);
- maxDelta = std::max(maxDelta, historicalDelta);
}
+ // Todo upgrade lock?
+ }
- ss << e.name;
- if (e.required)
+ if (found)
+ {
+ ARMARX_INFO << "Component " << componentName << " disabled from heartbeat";
+ }
+ else
+ {
+ ARMARX_INFO << "Could not unregister component " << componentName << ". Not found.";
+ }
+
+ updateRequiredElements();
+ }
+
+ void
+ armarx::RobotHealth::addRequiredTags(const std::string& requesterIdentifier,
+ const std::vector<std::string>& tags,
+ const Ice::Current& current)
+ {
+ std::shared_lock lock(updateMutex);
+
+ // add newly requested tags
+ for(const auto& tag : tags)
+ {
+ tagsPerRequester[requesterIdentifier].insert(tag);
+ }
+
+ updateRequiredElements();
+ }
+
+ std::set<std::string>
+ armarx::RobotHealth::requestedTags() const
+ {
+ // obtain a list of all tags that are relevant / requested at the moment
+ std::set<std::string> allRequestedTags;
+ for(const auto& tgs: simox::alg::get_values(tagsPerRequester))
+ {
+ for(const auto& tag : tgs)
{
- ss << ", required";
+ allRequestedTags.insert(tag);
}
+ }
+ return allRequestedTags;
+ }
- if (!e.required && !e.enabled)
- {
- ss << ": disabled";
+ // checks whether any element in 'search' is in 'values'
+ bool containsAnyOf(const std::set<std::string>& values, const std::vector<std::string>& searchKeys)
+ {
+ return std::any_of(searchKeys.begin(), searchKeys.end(), [&values](const auto& key){
+ return values.count(key) > 0;
+ });
+ }
+
+ void armarx::RobotHealth::updateRequiredElements()
+ {
+ ARMARX_IMPORTANT << "updateRequiredElements";
+ const std::set<std::string> allRequestedTags = requestedTags();
+
+ for (auto &e : updateEntries) {
+ ARMARX_INFO << e.name;
+
+ e.required = false; // if required, will be set in the following
+
+ std::unique_lock lock(e.mutex);
+
+ if (containsAnyOf(allRequestedTags, e.tags)) {
+ ARMARX_INFO << e.name << " is required.";
+ e.required = true;
+ if (not e.enabled) {
+ ARMARX_WARNING << "You are requiring the disabled component "
+ << e.name << ". Enabling it...";
+ e.enabled = true;
+ }
}
- else
+ }
+ }
+
+ void
+ armarx::RobotHealth::removeRequiredTags(const std::string& requesterIdentifier,
+ const std::vector<std::string>& tags,
+ const Ice::Current& current)
+ {
+ std::shared_lock lock(updateMutex);
+
+ // update the required tags list / map
+ for(const auto& tag : tags)
+ {
+ tagsPerRequester[requesterIdentifier].erase(tag);
+ }
+
+ updateRequiredElements();
+ }
+
+ void
+ armarx::RobotHealth::resetRequiredTags(const Ice::Current& current)
+ {
+ std::shared_lock lock(updateMutex);
+
+ // treat the special provider (see onInitComponent()) appropriately
+ ARMARX_CHECK(tagsPerRequester.count(PROPERTY_REQUESTER_ID) > 0);
+ const auto propertyProviderTags = tagsPerRequester.at(PROPERTY_REQUESTER_ID);
+
+ // clear everything except the special provider
+ tagsPerRequester.clear();
+ tagsPerRequester.emplace(PROPERTY_REQUESTER_ID, propertyProviderTags);
+ }
+
+ RobotHealthInfo
+ RobotHealth::getSummary(const Ice::Current&)
+ {
+ std::shared_lock lock(updateMutex);
+
+ ARMARX_TRACE;
+ RobotHealthInfo ret;
+
+ auto now = armarx::core::time::DateTime::Now();
+
+ armarx::core::time::Duration minDelta;
+ armarx::core::time::Duration maxDelta;
+
+ for (const auto& e : updateEntries)
+ {
+ RobotHealthInfoEntry healthEntry;
+
+ for (size_t i = 1; i < e.history.size(); i++)
{
- if (delta > e.maximumCycleTimeErr)
- {
- ss << ": ERROR";
- }
- else if (delta > e.maximumCycleTimeWarn)
+ auto later = e.history[i];
+ auto pre = e.history[i - 1];
+
+ auto delta = later - pre;
+
+ if (minDelta > delta)
{
- ss << ": warning";
+ minDelta = delta;
}
- else
+
+ if (maxDelta < delta)
{
- ss << ": ok";
+ maxDelta = delta;
}
- ss << " (" << (delta / 1000) << "ms, min: " << (minDelta / 1000) << "ms, max: " << (maxDelta / 1000) << "ms)";
}
- ARMARX_TRACE;
- std::unique_lock lock(e.messageMutex);
- if (e.message.size())
- {
- ss << " - " << e.message;
- }
+ const Duration timeSinceLastUpdate = e.history.empty() ? Duration() : Clock::Now() - e.history.back();
- ss << "\n";
+ healthEntry.identifier = e.name;
+ healthEntry.state = e.state;
+ healthEntry.enabled = e.enabled;
+ healthEntry.required = e.required;
+ toIce(healthEntry.minDelta, minDelta);
+ toIce(healthEntry.maxDelta,maxDelta);
+ toIce(healthEntry.timeSinceLastUpdate, timeSinceLastUpdate);
+ toIce(healthEntry.maximumCycleTimeWarning, e.maximumCycleTimeWarn);
+ toIce(healthEntry.maximumCycleTimeError, e.maximumCycleTimeErr);
+ healthEntry.tags = e.tags;
+ ret.entries[e.name] = healthEntry;
}
- return ss.str();
+
+ const auto tags = requestedTags();
+ ret.activeTags = std::vector<std::string>(tags.begin(), tags.end());
+
+ return ret;
}
- void RobotHealth::reportDebugObserver()
+ std::string
+ RobotHealth::getTopicName(const Ice::Current& current)
{
- for(const auto& entry: entries)
+ return p.robotHealthTopicName;
+ }
+
+ void
+ RobotHealth::reportDebugObserver()
+ {
+ 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);
+ if (entry.history.size() > 1)
+ {
+ auto later = entry.history[entry.history.size() - 1];
+ auto pre = entry.history[entry.history.size() - 2];
+ const Duration delta = later - pre;
+
+ const Duration deltaToNow = Clock::Now() - entry.history.back();
+
+ setDebugObserverDatafield("RobotHealth_" + entry.name + "_lastDelta",
+ delta.toMilliSecondsDouble());
+ setDebugObserverDatafield("RobotHealth_" + entry.name + "_deltaToNow",
+ deltaToNow.toMilliSecondsDouble());
+ setDebugObserverDatafield("RobotHealth_" + entry.name + "_maximumCycleTimeWarn",
+ entry.maximumCycleTimeWarn.toMilliSecondsDouble());
+ setDebugObserverDatafield("RobotHealth_" + entry.name + "_maximumCycleTimeErr",
+ entry.maximumCycleTimeErr.toMilliSecondsDouble());
+ }
}
sendDebugObserverBatch();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.h b/source/RobotAPI/components/RobotHealth/RobotHealth.h
index 3da12c1797ffa0afbc9af7310ec40953ecc51309..899c237647f8c381bab41dd4db35d96dbe6499a4 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,90 @@ 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;
+ std::vector<std::string> tags;
+ RobotHealthState state = HealthOK;
+ std::string description = "";
+
+ bool required = false;
+ bool enabled = false;
+
+ mutable std::shared_mutex mutex;
+ std::deque<armarx::core::time::DateTime> history;
+
+ armarx::core::time::Duration maximumCycleTimeWarn;
+ armarx::core::time::Duration maximumCycleTimeErr;
+ };
+
+ void monitorHealthUpdateTaskClb();
+
+ UpdateEntry* findUpdateEntry(const std::string& name);
+ std::pair<bool, UpdateEntry&> findOrCreateUpdateEntry(const std::string& name);
- 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;
+ 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 RobotHealthHeartbeatArgs& args, const Ice::Current& current) override;
+ void unregister(const std::string& identifier, const Ice::Current&) override;
+
+
+ void addRequiredTags(const std::string& requesterIdentifier,
+ const std::vector<std::string>& tags,
+ const Ice::Current& current) override;
+ void removeRequiredTags(const std::string& requesterIdentifier,
+ const std::vector<std::string>& tags,
+ const Ice::Current& current) override;
+ void resetRequiredTags(const Ice::Current& current) override;
+
+
+ void heartbeat(const std::string& identifier,
+ const core::time::dto::DateTime& referenceTime,
+ const Ice::Current& current) override;
+
+ std::string getTopicName(const Ice::Current& current) override;
+
+ // RobotHealthComponentInterface interface
+ public:
+ RobotHealthInfo getSummary(const Ice::Current& current) override;
+ private:
+ void updateRequiredElements();
+ std::set<std::string> requestedTags() const;
+
+ mutable std::shared_mutex updateMutex;
+
+ std::deque<UpdateEntry> updateEntries;
+
+ std::map<std::string, std::set<std::string>> tagsPerRequester;
+
+ PeriodicTask<RobotHealth>::pointer_type monitorUpdateHealthTask;
+
+ 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;
+
+ std::string requiredTags;
+ } p;
};
-}
+} // namespace armarx
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp
index b239fdd0719716c1e579a803a6a89265168240c3..e028bc55648f02446b5980cc00124950d4564219 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,55 @@ 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()
{
-
-
+ auto args = RobotHealthHeartbeatArgs();
+ args.identifier = getName();
+ robotHealthTopicPrx->signUp(args);
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 +142,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 cb9d46377437764fac06cb4a9535d1adabb5534c..cfd8d369d0335768c2e85dd735f1d31d1e30e60d 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/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
index 52af2c8dd81679509f7aaf965caa91e3b49349eb..2a1f02718bb8b1fa9465445086238a4e04ba92dd 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
@@ -22,17 +22,25 @@
#include "HokuyoLaserUnit.h"
-#include <ArmarXCore/observers/variant/TimestampVariant.h>
-
#include <SimoxUtility/algorithm/string/string_tools.h>
-#include <HokuyoLaserScannerDriver/urg_utils.h>
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/Duration.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
+#include <HokuyoLaserScannerDriver/urg_utils.h>
using namespace armarx;
+HokuyoLaserUnit::HokuyoLaserUnit()
+{
+ addPlugin(heartbeat);
+}
+
-void HokuyoLaserUnit::onInitComponent()
+void
+HokuyoLaserUnit::onInitComponent()
{
offeringTopicFromProperty("RobotHealthTopicName");
offeringTopicFromProperty("DebugObserverName");
@@ -72,19 +80,21 @@ void HokuyoLaserUnit::onInitComponent()
}
catch (std::exception const& ex)
{
- ARMARX_WARNING << "Could not convert port to integer for laser scanner device " << deviceString
- << " (port is " << deviceInfo[1] << ") : " << ex.what();
+ ARMARX_WARNING << "Could not convert port to integer for laser scanner device "
+ << deviceString << " (port is " << deviceInfo[1] << ") : " << ex.what();
continue;
}
}
}
-
-void HokuyoLaserUnit::onConnectComponent()
+void
+HokuyoLaserUnit::onConnectComponent()
{
- robotHealthTopic = getTopic<RobotHealthInterfacePrx>(getProperty<std::string>("RobotHealthTopicName").getValue());
+ ARMARX_TRACE;
+
topic = getTopic<LaserScannerUnitListenerPrx>(topicName);
- debugObserver = getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverName").getValue());
+ debugObserver = getTopic<DebugObserverInterfacePrx>(
+ getProperty<std::string>("DebugObserverName").getValue());
connectedDevices.clear();
for (HokuyoLaserScanDevice& device : devices)
@@ -95,12 +105,21 @@ void HokuyoLaserUnit::onConnectComponent()
device.task = nullptr;
}
+ // connecting devices for first time
if (!device.reconnect())
{
- ARMARX_WARNING << "Not starting task for laser scanner with IP: " << device.ip << ", Port: " << device.port;
+ ARMARX_WARNING << "Not starting task for laser scanner with IP: " << device.ip
+ << ", Port: " << device.port;
continue;
}
+ ARMARX_CHECK_NOT_NULL(heartbeat);
+ heartbeat->signUp(device.ip,
+ armarx::core::time::Duration::MilliSeconds(500),
+ armarx::core::time::Duration::MilliSeconds(800),
+ {"LaserScanner", "Localization"},
+ "HokuyoLaserScanDevice");
+
LaserScannerInfo info;
info.device = device.ip;
info.frame = device.frame;
@@ -115,23 +134,22 @@ void HokuyoLaserUnit::onConnectComponent()
device.lengthData.resize(lengthDataSize);
device.scanTopic = topic;
- device.robotHealthTopic = robotHealthTopic;
+ device.robotHealthPlugin = heartbeat;
device.debugObserver = debugObserver;
connectedDevices.push_back(info);
- ARMARX_INFO << "Connected to " << device.ip << ", " << info.frame << ", "
- << info.minAngle << ", " << info.maxAngle << ", " << info.stepSize;
+ ARMARX_INFO << "Connected to " << device.ip << ", " << info.frame << ", " << info.minAngle
+ << ", " << info.maxAngle << ", " << info.stepSize;
- device.task = new RunningTask<HokuyoLaserScanDevice>(&device, &HokuyoLaserScanDevice::run,
- "HokuyoLaserScanUpdate_" + device.ip);
+ device.task = new RunningTask<HokuyoLaserScanDevice>(
+ &device, &HokuyoLaserScanDevice::run, "HokuyoLaserScanUpdate_" + device.ip);
device.task->start();
}
-
}
-
-void HokuyoLaserUnit::onDisconnectComponent()
+void
+HokuyoLaserUnit::onDisconnectComponent()
{
for (HokuyoLaserScanDevice& device : devices)
{
@@ -148,29 +166,32 @@ void HokuyoLaserUnit::onDisconnectComponent()
}
}
-
-void HokuyoLaserUnit::onExitComponent()
+void
+HokuyoLaserUnit::onExitComponent()
{
-
}
-armarx::PropertyDefinitionsPtr HokuyoLaserUnit::createPropertyDefinitions()
+armarx::PropertyDefinitionsPtr
+HokuyoLaserUnit::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new HokuyoLaserUnitPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(
+ new HokuyoLaserUnitPropertyDefinitions(getConfigIdentifier()));
}
-std::string HokuyoLaserUnit::getReportTopicName(const Ice::Current&) const
+std::string
+HokuyoLaserUnit::getReportTopicName(const Ice::Current&) const
{
return topicName;
}
-LaserScannerInfoSeq HokuyoLaserUnit::getConnectedDevices(const Ice::Current&) const
+LaserScannerInfoSeq
+HokuyoLaserUnit::getConnectedDevices(const Ice::Current&) const
{
return connectedDevices;
}
-bool HokuyoLaserScanDevice::reconnect()
+bool
+HokuyoLaserScanDevice::reconnect()
{
if (connected)
{
@@ -183,8 +204,8 @@ bool HokuyoLaserScanDevice::reconnect()
connected = (ret == 0);
if (!connected)
{
- ARMARX_WARNING << "Could not connect to laser scanner device using URG driver (IP: "
- << ip << ", Port: " << port << ", Error: " << ret << ")";
+ ARMARX_WARNING << "Could not connect to laser scanner device using URG driver (IP: " << ip
+ << ", Port: " << port << ", Error: " << ret << ")";
return false;
}
ret = urg_start_measurement(&urg, URG_DISTANCE, URG_SCAN_INFINITY, 0);
@@ -196,21 +217,24 @@ bool HokuyoLaserScanDevice::reconnect()
}
else
{
- ARMARX_WARNING << "Could not start measurement for laser scanner device using URG driver (IP: "
- << ip << ", Port: " << port << ", Error: " << ret << ")";
+ ARMARX_WARNING
+ << "Could not start measurement for laser scanner device using URG driver (IP: " << ip
+ << ", Port: " << port << ", Error: " << ret << ")";
return false;
}
}
-void HokuyoLaserScanDevice::run()
+void
+HokuyoLaserScanDevice::run()
{
+ ARMARX_TRACE;
while (!task->isStopped())
{
IceUtil::Time time_start = TimeUtil::GetTime();
if (errorCounter > 10)
{
- ARMARX_ERROR << "Device " << ip << " has too many consecutive errors!";
+ ARMARX_ERROR << "Device " << ip << " has too many consecutive errors!";
// assume dead
reconnect();
}
@@ -219,8 +243,9 @@ void HokuyoLaserScanDevice::run()
int lengthDataSize = urg_get_distance(&urg, lengthData.data(), nullptr);
if (lengthDataSize < 0)
{
- ARMARX_WARNING << deactivateSpam(1) << "Could not get measurement for laser scanner (IP: "
- << ip << ", Port: " << port << ", Error: " << lengthDataSize << ")";
+ ARMARX_WARNING << deactivateSpam(1)
+ << "Could not get measurement for laser scanner (IP: " << ip
+ << ", Port: " << port << ", Error: " << lengthDataSize << ")";
errorCounter++;
continue;
}
@@ -236,7 +261,8 @@ void HokuyoLaserScanDevice::run()
// TODO: Extract the min and max valid value for distance into parameters?
if (distance >= 21 && distance <= 30000)
{
- step.angle = angleOffset + (float)urg_index2rad(&urg, stepIndex); // Convert steps to rad
+ step.angle =
+ angleOffset + (float)urg_index2rad(&urg, stepIndex); // Convert steps to rad
step.distance = (float)distance; // Data is already in mm
scan.push_back(step);
}
@@ -256,16 +282,14 @@ void HokuyoLaserScanDevice::run()
}
IceUtil::Time time_topicSensor = TimeUtil::GetTime();
- RobotHealthHeartbeatArgs args;
- args.maximumCycleTimeWarningMS = 500;
- args.maximumCycleTimeErrorMS = 800;
- if (robotHealthTopic)
+ if (robotHealthPlugin != nullptr)
{
- robotHealthTopic->heartbeat(componentName + "_" + ip, args);
+ robotHealthPlugin->heartbeatOnChannel(ip);
}
else
{
- ARMARX_WARNING << "No robot health topic available: IP: " << ip << ", Port: " << port;
+ ARMARX_WARNING << "No robot health topic available: IP: " << ip
+ << ", Port: " << port;
}
IceUtil::Time time_topicHeartbeat = TimeUtil::GetTime();
@@ -274,20 +298,30 @@ void HokuyoLaserScanDevice::run()
StringVariantBaseMap durations;
durations["total_ms"] = new Variant(duration.toMilliSecondsDouble());
- durations["measure_ms"] = new Variant((time_measure - time_start).toMilliSecondsDouble());
- durations["update_ms"] = new Variant((time_update - time_measure).toMilliSecondsDouble());
- durations["topic_sensor_ms"] = new Variant((time_topicSensor - time_update).toMilliSecondsDouble());
- durations["topic_health_ms"] = new Variant((time_topicHeartbeat - time_topicSensor).toMilliSecondsDouble());
- debugObserver->setDebugChannel("LaserScannerDuration_" + simox::alg::replace_all(ip, ".", "_"), durations);
+ durations["measure_ms"] =
+ new Variant((time_measure - time_start).toMilliSecondsDouble());
+ durations["update_ms"] =
+ new Variant((time_update - time_measure).toMilliSecondsDouble());
+ durations["topic_sensor_ms"] =
+ new Variant((time_topicSensor - time_update).toMilliSecondsDouble());
+ durations["topic_health_ms"] =
+ new Variant((time_topicHeartbeat - time_topicSensor).toMilliSecondsDouble());
+ debugObserver->setDebugChannel(
+ "LaserScannerDuration_" + simox::alg::replace_all(ip, ".", "_"), durations);
if (duration.toSecondsDouble() > 0.1)
{
ARMARX_WARNING << "Laserscanner reports are slow"
<< "Total time: " << duration.toMilliSecondsDouble() << "ms\n"
- << "Measure: " << (time_measure - time_start).toMilliSecondsDouble() << "ms \n"
- << "Update: " << (time_update - time_measure).toMilliSecondsDouble() << "ms\n"
- << "TopicSensor: " << (time_topicSensor - time_update).toMilliSecondsDouble() << "ms\n"
- << "TopicHeart: " << (time_topicHeartbeat - time_topicSensor).toMilliSecondsDouble() << "ms\n";
+ << "Measure: "
+ << (time_measure - time_start).toMilliSecondsDouble() << "ms \n"
+ << "Update: "
+ << (time_update - time_measure).toMilliSecondsDouble() << "ms\n"
+ << "TopicSensor: "
+ << (time_topicSensor - time_update).toMilliSecondsDouble() << "ms\n"
+ << "TopicHeart: "
+ << (time_topicHeartbeat - time_topicSensor).toMilliSecondsDouble()
+ << "ms\n";
}
}
}
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
index e9afa8e5b927857a03b171dc615722275d30986f..0b8ad3985e9e3b1825054fe1339cb41a5c59f4cd 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
+++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h
@@ -22,14 +22,17 @@
#pragma once
+#include <vector>
+
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
+
#include <RobotAPI/components/units/SensorActorUnit.h>
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/interface/components/RobotHealthInterface.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h>
#include <HokuyoLaserScannerDriver/urg_sensor.h>
-#include <vector>
namespace armarx
{
@@ -37,19 +40,30 @@ namespace armarx
* @class HokuyoLaserUnitPropertyDefinitions
* @brief
*/
- class HokuyoLaserUnitPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
+ class HokuyoLaserUnitPropertyDefinitions : public armarx::ComponentPropertyDefinitions
{
public:
- HokuyoLaserUnitPropertyDefinitions(std::string prefix):
+ HokuyoLaserUnitPropertyDefinitions(std::string prefix) :
armarx::ComponentPropertyDefinitions(prefix)
{
- defineOptionalProperty<std::string>("LaserScannerTopicName", "LaserScans", "Name of the laser scan topic.");
+ defineOptionalProperty<std::string>(
+ "LaserScannerTopicName", "LaserScans", "Name of the laser scan topic.");
defineOptionalProperty<int>("UpdatePeriod", 25, "Update period for laser scans");
- defineOptionalProperty<float>("AngleOffset", -2.3561944902, "Offset is applied the raw angles before reporting them");
- defineOptionalProperty<std::string>("Devices", "", "List of devices in form of 'IP1,port1,frame1;IP2,port2,frame2;...'");
- defineOptionalProperty<std::string>("RobotHealthTopicName", "RobotHealthTopic", "Name of the RobotHealth topic");
- defineOptionalProperty<std::string>("DebugObserverName", "DebugObserver", "Name of the topic the DebugObserver listens on");
+ defineOptionalProperty<float>("AngleOffset",
+ -2.3561944902,
+ "Offset is applied the raw angles before reporting them");
+ defineOptionalProperty<std::string>(
+ "Devices",
+ "",
+ "List of devices in form of 'IP1,port1,frame1;IP2,port2,frame2;...'");
+
+ defineOptionalProperty<std::string>(
+ "RobotHealthComponentName", "RobotHealth", "Name of the RobotHealth component");
+ defineOptionalProperty<std::string>(
+ "RobotHealthTopicName", "RobotHealthTopic", "Name of the RobotHealth topic");
+ defineOptionalProperty<std::string>("DebugObserverName",
+ "DebugObserver",
+ "Name of the topic the DebugObserver listens on");
}
};
@@ -66,6 +80,7 @@ namespace armarx
bool reconnect();
+
void run();
RunningTask<HokuyoLaserScanDevice>::pointer_type task;
@@ -73,7 +88,7 @@ namespace armarx
std::string componentName;
LaserScannerUnitListenerPrx scanTopic;
- RobotHealthInterfacePrx robotHealthTopic;
+ armarx::plugins::HeartbeatComponentPlugin* robotHealthPlugin;
DebugObserverInterfacePrx debugObserver;
};
@@ -93,10 +108,13 @@ namespace armarx
virtual public armarx::SensorActorUnit
{
public:
+ HokuyoLaserUnit();
+
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "HokuyoLaserUnit";
}
@@ -138,8 +156,8 @@ namespace armarx
float angleOffset = 0.0f;
std::vector<HokuyoLaserScanDevice> devices;
LaserScannerInfoSeq connectedDevices;
- RobotHealthInterfacePrx robotHealthTopic;
DebugObserverInterfacePrx debugObserver;
- };
-}
+ plugins::HeartbeatComponentPlugin* heartbeat = nullptr;
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidget.ui b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidget.ui
index 3c7001134bb60d82eb5017ee2b3d6f86a7bc57ee..2140458dc4fb92ddc0f065178ed4487733d053fa 100644
--- a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidget.ui
+++ b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidget.ui
@@ -1,55 +1,42 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
- <class>GuiHealthClientWidget</class>
- <widget class="QWidget" name="GuiHealthClientWidget">
- <property name="geometry">
- <rect>
- <x>0</x>
- <y>0</y>
- <width>500</width>
- <height>71</height>
- </rect>
- </property>
- <property name="windowTitle">
- <string>GuiHealthClientWidget</string>
- </property>
- <layout class="QGridLayout" name="gridLayout">
- <item row="0" column="1">
- <widget class="QPushButton" name="pushButtonToggleOwnHeartbeats">
- <property name="text">
- <string>Toggle</string>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
- <widget class="QLabel" name="labelState">
- <property name="text">
- <string>TextLabel</string>
- </property>
- </widget>
- </item>
- <item row="0" column="2">
- <spacer name="horizontalSpacer">
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>40</width>
- <height>20</height>
- </size>
- </property>
- </spacer>
- </item>
- <item row="1" column="0" colspan="3">
- <widget class="QLabel" name="labelHealthState">
- <property name="text">
- <string>TextLabel</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- <resources/>
- <connections/>
+ <class>GuiHealthClientWidget</class>
+ <widget class="QWidget" name="GuiHealthClientWidget">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>500</width>
+ <height>71</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>GuiHealthClientWidget</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout">
+
+ <item row="0" column="0" colspan="2">
+ <widget class="QLabel" name="labelTags">
+ <property name="text">
+ <string>...</string>
+ </property>
+ </widget>
+ </item>
+
+ <item row="0" column="2">
+ <widget class="QPushButton" name="pushButtonUnrequireAll">
+ <property name="text">
+ <string>Reset required tags</string>
+ </property>
+ </widget>
+ </item>
+
+ <item row="1" column="0" colspan="3">
+ <widget class="QTableWidget" name="tableHealthState">
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <resources />
+ <connections />
</ui>
diff --git a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
index ca95dd07836e035a04019bd2fd1d176c4ca01256..aa795eb22ff8c96dfcb82b77f2f4f46f1512471a 100644
--- a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
@@ -22,67 +22,95 @@
#include "GuiHealthClientWidgetController.h"
-#include <QLabel>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <qcolor.h>
+#include <qnamespace.h>
+#include <qtablewidget.h>
#include <string>
+
+#include <QLabel>
#include <QPushButton>
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/Duration.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+
namespace armarx
{
+ std::string serializeList(const std::vector<std::string>& l)
+ {
+ std::string s;
+ for(std::size_t i = 0; i < l.size(); i++)
+ {
+ const auto& tag = l.at(i);
+ s += tag;
+
+ if(i != l.size() - 1)
+ {
+ s += ", ";
+ }
+ }
+
+ return s;
+ }
+
GuiHealthClientWidgetController::GuiHealthClientWidgetController()
{
widget.setupUi(getWidget());
- //ARMARX_IMPORTANT << "ctor start";
- healthTimer = new QTimer(this);
- healthTimer->setInterval(10);
- connect(healthTimer, SIGNAL(timeout()), this, SLOT(healthTimerClb()));
- connect(widget.pushButtonToggleOwnHeartbeats, SIGNAL(clicked()), this, SLOT(toggleSendOwnHeartbeats()));
- connect(this, SIGNAL(invokeConnectComponentQt()), this, SLOT(onConnectComponentQt()), Qt::QueuedConnection);
- connect(this, SIGNAL(invokeDisconnectComponentQt()), this, SLOT(onDisconnectComponentQt()), Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(invokeConnectComponentQt()),
+ this,
+ SLOT(onConnectComponentQt()),
+ Qt::QueuedConnection);
+ connect(this,
+ SIGNAL(invokeDisconnectComponentQt()),
+ this,
+ SLOT(onDisconnectComponentQt()),
+ Qt::QueuedConnection);
+
+ connect(widget.pushButtonUnrequireAll,
+ SIGNAL(clicked()),
+ this,
+ SLOT(unrequireAll()));
updateSummaryTimer = new QTimer(this);
updateSummaryTimer->setInterval(100);
connect(updateSummaryTimer, SIGNAL(timeout()), this, SLOT(updateSummaryTimerClb()));
-
- widget.labelState->setText("idle.");
- widget.pushButtonToggleOwnHeartbeats->setText("send own heartbeats");
-
- //ARMARX_IMPORTANT << "ctor end";
}
-
GuiHealthClientWidgetController::~GuiHealthClientWidgetController()
{
-
}
-
- void GuiHealthClientWidgetController::loadSettings(QSettings* settings)
+ void
+ GuiHealthClientWidgetController::loadSettings(QSettings* settings)
{
-
}
- void GuiHealthClientWidgetController::saveSettings(QSettings* settings)
+ void
+ GuiHealthClientWidgetController::saveSettings(QSettings* settings)
{
-
}
-
- void GuiHealthClientWidgetController::onInitComponent()
+ void
+ GuiHealthClientWidgetController::onInitComponent()
{
//ARMARX_IMPORTANT << "onInitComponent";
usingProxy("RobotHealth");
offeringTopic("RobotHealthTopic");
}
- void GuiHealthClientWidgetController::healthTimerClb()
+ void
+ GuiHealthClientWidgetController::healthTimerClb()
{
- RobotHealthHeartbeatArgs rhha;
- rhha.maximumCycleTimeWarningMS = 250;
- rhha.maximumCycleTimeErrorMS = 500;
- robotHealthTopicPrx->heartbeat(getName(), RobotHealthHeartbeatArgs());
+ armarx::core::time::dto::DateTime now;
+ armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
+ robotHealthTopicPrx->heartbeat(getName(), now);
}
- void GuiHealthClientWidgetController::updateSummaryTimerClb()
+
+ void
+ GuiHealthClientWidgetController::updateSummaryTimerClb()
{
//ARMARX_IMPORTANT << "updateSummaryTimerClb";
if (robotHealthComponentPrx)
@@ -90,59 +118,128 @@ namespace armarx
//ARMARX_IMPORTANT << "has robotHealthComponentPrx";
try
{
- //ARMARX_IMPORTANT << "before set text";
- widget.labelHealthState->setText(QString::fromUtf8(robotHealthComponentPrx->getSummary().c_str()));
- //ARMARX_IMPORTANT << "after set text";
+ auto summary = robotHealthComponentPrx->getSummary();
+
+ const std::size_t nCols = 7;
+
+ auto& tableWidget = widget.tableHealthState;
+ tableWidget->setRowCount(summary.entries.size());
+ tableWidget->setColumnCount(nCols);
+
+ const auto summaryVals = simox::alg::get_values(summary.entries);
+
+ tableWidget->setHorizontalHeaderLabels({
+ "identifier", "required", "keeps frequency", "tags", "time since\nlast update [ms]", "warning\nthreshold [ms]", "error\nthreshold [ms]"
+ });
+
+ tableWidget->setColumnWidth(0, 250);
+ tableWidget->setColumnWidth(1, 80);
+ tableWidget->setColumnWidth(2, 120);
+ tableWidget->setColumnWidth(3, 180);
+ tableWidget->setColumnWidth(4, 120);
+ tableWidget->setColumnWidth(5, 120);
+ tableWidget->setColumnWidth(6, 120);
+
+ for(std::size_t i = 0; i < summary.entries.size(); i++)
+ {
+ const auto& entry = summaryVals.at(i);
+
+ std::string stateRepr;
+ QColor color;
+ switch(entry.state)
+ {
+ case HealthOK:
+ stateRepr = "yes";
+ color.setRgb(0, 255, 0); // green
+ break;
+ case HealthWarning:
+ stateRepr = "yes";
+ color.setRgb(255, 165, 0); // orange
+ break;
+ case HealthError:
+ stateRepr = "no";
+ color.setRgb(255, 0, 0); // red
+ break;
+ }
+
+ const std::string timeSinceLastRepr = std::to_string(entry.timeSinceLastUpdate.microSeconds / 1000);
+ const std::string tagsRepr = serializeList(entry.tags);
+
+ tableWidget->setItem(i, 0, new QTableWidgetItem(QString::fromStdString(entry.identifier)));
+
+ auto* requiredItem = new QTableWidgetItem(QString::fromStdString(entry.required ? "yes" : "no"));
+ requiredItem->setTextAlignment(Qt::AlignHCenter | Qt::AlignVCenter);
+ tableWidget->setItem(i, 1, requiredItem);
+
+ auto* stateItem = new QTableWidgetItem(QString::fromStdString(stateRepr));
+ stateItem->setTextAlignment(Qt::AlignHCenter | Qt::AlignVCenter);
+ tableWidget->setItem(i, 2, stateItem);
+ tableWidget->item(i, 2)->setBackgroundColor(color);
+
+ tableWidget->setItem(i, 3, new QTableWidgetItem(QString::fromStdString(tagsRepr)));
+
+ tableWidget->setItem(i, 4, new QTableWidgetItem(QString::fromStdString(timeSinceLastRepr)));
+ tableWidget->setItem(i, 5, new QTableWidgetItem(QString::fromStdString(std::to_string(entry.maximumCycleTimeWarning.microSeconds / 1000))));
+ tableWidget->setItem(i, 6, new QTableWidgetItem(QString::fromStdString(std::to_string(entry.maximumCycleTimeError.microSeconds / 1000))));
+ }
+
+ std::string tagsText = "Active tags: [";
+ {
+ std::sort(summary.activeTags.begin(), summary.activeTags.end());
+
+ tagsText += serializeList(summary.activeTags);
+
+ tagsText += "]";
+ }
+
+ widget.labelTags->setText(QString::fromStdString(tagsText));
}
catch (...)
- {}
+ {
+ }
}
}
- void GuiHealthClientWidgetController::toggleSendOwnHeartbeats()
+ void
+ GuiHealthClientWidgetController::unrequireAll()
{
- if (ownHeartbeatsActive)
- {
- ownHeartbeatsActive = false;
- healthTimer->stop();
- robotHealthTopicPrx->unregister(getName());
- widget.labelState->setText("idle.");
- //widget.pushButtonToggleOwnHeartbeats->setDisabled(true);
- widget.pushButtonToggleOwnHeartbeats->setText("send own heartbeats");
- }
- else
+ ARMARX_INFO << "UnrequireAll.";
+ if (robotHealthComponentPrx)
{
- ownHeartbeatsActive = true;
- healthTimer->start();
- widget.labelState->setText("sending heartbeats...");
- widget.pushButtonToggleOwnHeartbeats->setText("unregister self");
+ try
+ {
+ robotHealthComponentPrx->resetRequiredTags();
+ }
+ catch(...){}
}
-
}
-
- void GuiHealthClientWidgetController::onConnectComponent()
+ void
+ GuiHealthClientWidgetController::onConnectComponent()
{
//ARMARX_IMPORTANT << "onConnectComponent";
robotHealthTopicPrx = getTopic<RobotHealthInterfacePrx>("RobotHealthTopic");
robotHealthComponentPrx = getProxy<RobotHealthComponentInterfacePrx>("RobotHealth");
invokeConnectComponentQt();
}
- void GuiHealthClientWidgetController::onConnectComponentQt()
+
+ void
+ GuiHealthClientWidgetController::onConnectComponentQt()
{
//healthTimer->start();
- //ARMARX_IMPORTANT << "updateSummaryTimer->start";
+ //ARMARX_IMPORTANT <<ame "updateSummaryTimer->start";
updateSummaryTimer->start();
}
-
- void GuiHealthClientWidgetController::onDisconnectComponent()
+ void
+ GuiHealthClientWidgetController::onDisconnectComponent()
{
invokeDisconnectComponentQt();
}
- void GuiHealthClientWidgetController::onDisconnectComponentQt()
+
+ void
+ GuiHealthClientWidgetController::onDisconnectComponentQt()
{
- healthTimer->stop();
updateSummaryTimer->stop();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.h b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.h
index 0fed7c1ae22ad51714624a49388e8fec87a2967d..f5bc6ff28a593ba1d5e58bf1a02aabf42cd29bae 100644
--- a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.h
+++ b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.h
@@ -110,7 +110,7 @@ namespace armarx
void onDisconnectComponentQt();
void healthTimerClb();
void updateSummaryTimerClb();
- void toggleSendOwnHeartbeats();
+ void unrequireAll();
signals:
/* QT signal declarations */
@@ -122,13 +122,9 @@ namespace armarx
* Widget Form
*/
Ui::GuiHealthClientWidget widget;
- bool ownHeartbeatsActive = false;
RobotHealthInterfacePrx robotHealthTopicPrx;
RobotHealthComponentInterfacePrx robotHealthComponentPrx;
- QTimer* healthTimer;
QTimer* updateSummaryTimer;
};
}
-
-
diff --git a/source/RobotAPI/interface/components/RobotHealthInterface.ice b/source/RobotAPI/interface/components/RobotHealthInterface.ice
index bbad059d5a7468428a5a351b07b95e64bc27532b..a42072ae33903969d0de84e5bd7de708d50532a0 100644
--- a/source/RobotAPI/interface/components/RobotHealthInterface.ice
+++ b/source/RobotAPI/interface/components/RobotHealthInterface.ice
@@ -22,8 +22,12 @@
#pragma once
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/core/time.ice>
+
module armarx
{
+
enum RobotHealthState
{
HealthOK, HealthWarning, HealthError
@@ -31,24 +35,65 @@ module armarx
struct RobotHealthHeartbeatArgs
{
- int maximumCycleTimeWarningMS = 100;
- int maximumCycleTimeErrorMS = 200;
- string message;
+ string identifier;
+ // bool requiredByDefault;
+ string description;
+ Ice::StringSeq 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);
+ void signUp(RobotHealthHeartbeatArgs args);
+ void unregister(string identifier);
+
+ void addRequiredTags(string requesterIdentifier, Ice::StringSeq tags);
+ void removeRequiredTags(string requesterIdentifier, Ice::StringSeq tags);
+ void resetRequiredTags();
+
+ void heartbeat(string identifier, armarx::core::time::dto::DateTime referenceTime);
+
+ string getTopicName();
};
+ // 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 identifier;
+ RobotHealthState state;
+
+ // string message;
+ armarx::core::time::dto::Duration minDelta;
+ armarx::core::time::dto::Duration maxDelta;
+ armarx::core::time::dto::Duration timeSinceLastUpdate;
+
+ bool required; //
+ bool enabled;
+
+ armarx::core::time::dto::Duration maximumCycleTimeWarning;
+ armarx::core::time::dto::Duration maximumCycleTimeError;
+
+ Ice::StringSeq tags;
+ };
+
+ dictionary<string, RobotHealthInfoEntry> RobotHealthEntries;
+
+ struct RobotHealthInfo
+ {
+ Ice::StringSeq activeTags;
+ RobotHealthEntries entries;
+ };
+
+
interface RobotHealthComponentInterface extends RobotHealthInterface
{
- string getSummary();
+ RobotHealthInfo getSummary();
};
};
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
index 41b48bdff28d2cdbd2e601ab7e9d6dadcdbc0e2b..b0ad0e37fe12f626c82e0c7d1c6ec0cf320bcead 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
@@ -2,56 +2,120 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/DateTime.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
#include <RobotAPI/interface/components/RobotHealthInterface.h>
namespace armarx::plugins
{
- void HeartbeatComponentPlugin::configureHeartbeatChannel(const std::string& channel,
- const RobotHealthHeartbeatArgs& args)
+
+ void
+ HeartbeatComponentPlugin::signUp(const std::string& identifier,
+ const std::vector<std::string>& tags,
+ const std::string& description)
+ {
+ RobotHealthHeartbeatArgs argsCopy = defaultHeartbeatArgs;
+ argsCopy.description = description;
+ argsCopy.identifier = identifier;
+ argsCopy.tags = tags;
+ signUp(argsCopy);
+ }
+
+ void
+ HeartbeatComponentPlugin::signUp(const std::string& identifier,
+ const armarx::core::time::Duration& warning,
+ const armarx::core::time::Duration& error,
+ const std::vector<std::string>& tags,
+ const std::string& description)
{
- channelHeartbeatConfig.emplace(channel, args);
+ RobotHealthHeartbeatArgs argsCopy = defaultHeartbeatArgs;
+ argsCopy.description = description;
+ argsCopy.tags = tags;
+ argsCopy.identifier = identifier;
+ toIce(argsCopy.maximumCycleTimeWarning, warning);
+ toIce(argsCopy.maximumCycleTimeError, error);
+
+ ARMARX_TRACE;
+ signUp(argsCopy);
}
- void HeartbeatComponentPlugin::configureHeartbeatChannel(const std::string& channel, const std::string& message)
+ void
+ HeartbeatComponentPlugin::signUp(const armarx::core::time::Duration& warning,
+ const armarx::core::time::Duration& error,
+ const std::vector<std::string>& tags,
+ const std::string& description)
{
- auto args = heartbeatArgs;
- args.message = message;
- configureHeartbeatChannel(channel, args);
+ RobotHealthHeartbeatArgs argsCopy = defaultHeartbeatArgs;
+ argsCopy.description = description;
+ argsCopy.tags = tags;
+ toIce(argsCopy.maximumCycleTimeWarning, warning);
+ toIce(argsCopy.maximumCycleTimeError, error);
+ // argsCopy.requiredByDefault = required;
+ signUp(argsCopy);
}
- void HeartbeatComponentPlugin::heartbeat()
+ void
+ HeartbeatComponentPlugin::signUp(const RobotHealthHeartbeatArgs& args)
{
+ ARMARX_TRACE;
+ ARMARX_CHECK_NOT_NULL(rhprx);
+
+ if (args.identifier.empty())
+ {
+ RobotHealthHeartbeatArgs argsCopy = args;
+ argsCopy.identifier = parent().getName();
+ rhprx->signUp(argsCopy);
+ }
+ else
+ {
+ // add component name prefix to identifier
+ RobotHealthHeartbeatArgs argsCopy = args;
+ argsCopy.identifier = parent().getName() + "_" + argsCopy.identifier;
+ rhprx->signUp(argsCopy);
+ }
+ }
+ void
+ HeartbeatComponentPlugin::heartbeat()
+ {
if (robotHealthTopic)
{
- robotHealthTopic->heartbeat(componentName, heartbeatArgs);
- } else
+ armarx::core::time::dto::DateTime now;
+ armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
+ robotHealthTopic->heartbeat(parent().getName(), now);
+ }
+ else
{
ARMARX_WARNING << "No robot health topic available!";
}
}
- void HeartbeatComponentPlugin::heartbeat(const std::string& channel)
+ void
+ HeartbeatComponentPlugin::heartbeatOnChannel(const std::string& channelName)
{
- const auto argsIt = channelHeartbeatConfig.find(channel);
- ARMARX_CHECK(argsIt != channelHeartbeatConfig.end()) << "heartbeat() called for unknown channel '" << channel
- << "'."
- << "You must register the config using configureHeartbeatChannel(channel) first!";
-
- const auto& args = argsIt->second;
-
if (robotHealthTopic)
{
- robotHealthTopic->heartbeat(componentName + "_" + channel, args);
- } else
+ armarx::core::time::dto::DateTime now;
+ armarx::core::time::toIce(now, armarx::core::time::DateTime::Now());
+ robotHealthTopic->heartbeat(parent().getName() + "_" + channelName, now);
+ }
+ else
{
ARMARX_WARNING << "No robot health topic available!";
}
}
- void HeartbeatComponentPlugin::preOnInitComponent()
+ void
+ HeartbeatComponentPlugin::preOnInitComponent()
{
+ // set default args
+ auto warn = armarx::core::time::Duration::MilliSeconds(p.maximumCycleTimeWarningMS);
+ auto err = armarx::core::time::Duration::MilliSeconds(p.maximumCycleTimeErrorMS);
+ armarx::core::time::toIce(defaultHeartbeatArgs.maximumCycleTimeWarning, warn);
+ armarx::core::time::toIce(defaultHeartbeatArgs.maximumCycleTimeError, err);
+ // defaultHeartbeatArgs.requiredByDefault = true;
+
// if (topicName.empty())
// {
// parent<Component>().getProperty(topicName, makePropertyName(topicPropertyName));
@@ -59,33 +123,39 @@ namespace armarx::plugins
// parent<Component>().offeringTopic(topicName);
}
- void HeartbeatComponentPlugin::postOnInitComponent()
+ void
+ HeartbeatComponentPlugin::postOnInitComponent()
{
}
- void HeartbeatComponentPlugin::preOnConnectComponent()
+ void
+ HeartbeatComponentPlugin::postOnConnectComponent()
{
- // robotHealthTopic = parent<Component>().getTopic<RobotHealthInterfacePrx>(topicName);
- componentName = parent<Component>().getName();
+ ARMARX_CHECK_NOT_NULL(rhprx);
+ topicName = rhprx->getTopicName();
+ robotHealthTopic = parent<Component>().getTopic<RobotHealthInterfacePrx>(topicName);
}
- void HeartbeatComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
+ void
+ HeartbeatComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
{
- if (!properties->hasDefinition(makePropertyName(topicPropertyName)))
+ if (!properties->hasDefinition(makePropertyName(healthPropertyName)))
{
- properties->topic(robotHealthTopic, topicName, topicPropertyName,
- "Name of the topic the DebugObserver listens on");
+ properties->component(
+ rhprx, "RobotHealth", healthPropertyName, "Name of the robot health component.");
}
if (not properties->hasDefinition(makePropertyName(maximumCycleTimeWarningMSPropertyName)))
{
- properties->required(heartbeatArgs.maximumCycleTimeWarningMS, maximumCycleTimeWarningMSPropertyName,
+ properties->required(p.maximumCycleTimeWarningMS,
+ maximumCycleTimeWarningMSPropertyName,
"maximum cycle time before warning is emitted");
}
if (not properties->hasDefinition(makePropertyName(maximumCycleTimeErrorMSPropertyName)))
{
- properties->required(heartbeatArgs.maximumCycleTimeErrorMS, maximumCycleTimeErrorMSPropertyName,
+ properties->required(p.maximumCycleTimeErrorMS,
+ maximumCycleTimeErrorMSPropertyName,
"maximum cycle time before error is emitted");
}
}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h
index bbefa079c1d54e71444d9f0e90120f4db0250297..7b3654e84562fdbc5386eedb70e86424701592af 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h
@@ -22,6 +22,8 @@
#pragma once
#include <ArmarXCore/core/ComponentPlugin.h>
+#include <ArmarXCore/core/ManagedIceObject.h>
+#include <ArmarXCore/core/time/Duration.h>
#include <RobotAPI/interface/components/RobotHealthInterface.h>
@@ -34,21 +36,33 @@ namespace armarx::plugins
using ComponentPlugin::ComponentPlugin;
/**
- * @brief Configures a heartbeat subchannel.
- *
- * @param channel Identifier of the heartbeat channel
- * @param args Configuration of this channel's heartbeat properties
+ * @brief register component to heartbeat
*/
- void configureHeartbeatChannel(const std::string& channel, const std::string& message);
+ void signUp(const std::string& channelName = "",
+ const std::vector<std::string>& aliases = {},
+ const std::string& description = "");
/**
- * @brief Configures a heartbeat subchannel.
- *
- * @param channel Identifier of the heartbeat channel
- * @param args Configuration of this channel's heartbeat properties
+ * @brief register component to heartbeat
+ */
+ void signUp(const armarx::core::time::Duration& warning,
+ const armarx::core::time::Duration& error,
+ const std::vector<std::string>& aliases = {},
+ const std::string& description = "");
+
+ /**
+ * @brief register component to heartbeat
*/
- void configureHeartbeatChannel(const std::string& channel,
- const RobotHealthHeartbeatArgs& args);
+ void signUp(const std::string& channelName,
+ const armarx::core::time::Duration& warning,
+ const armarx::core::time::Duration& error,
+ const std::vector<std::string>& aliases = {},
+ const std::string& description = "");
+
+ /**
+ * @brief register component to heartbeat, possibly with different component name
+ */
+ void signUp(const RobotHealthHeartbeatArgs& args);
/**
* @brief Sends out a heartbeat using the default config
@@ -63,35 +77,37 @@ namespace armarx::plugins
*
* @param channel Identifier of the heartbeat channel
*/
- void heartbeat(const std::string& channel);
+ void heartbeatOnChannel(const std::string& channelName);
protected:
void preOnInitComponent() override;
void postOnInitComponent() override;
- void preOnConnectComponent() override;
+ // void preOnConnectComponent() override;
+ void postOnConnectComponent() override;
void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
private:
//! heartbeat topic name (outgoing)
+ RobotHealthInterfacePrx robotHealthTopic;
std::string topicName{"RobotHealthTopic"};
- //! name of this component used as identifier for heartbeats
- std::string componentName;
+ RobotHealthComponentInterfacePrx rhprx;
//
- static constexpr auto topicPropertyName = "heartbeat.TopicName";
+ static constexpr auto healthPropertyName = "heartbeat.ComponentName";
static constexpr auto maximumCycleTimeWarningMSPropertyName =
"heartbeat.maximumCycleTimeWarningMS";
static constexpr auto maximumCycleTimeErrorMSPropertyName =
"heartbeat.maximumCycleTimeErrorMS";
- RobotHealthInterfacePrx robotHealthTopic;
+ struct Properties
+ {
+ long maximumCycleTimeWarningMS = 50;
+ long maximumCycleTimeErrorMS = 100;
+ } p;
//! default config used in heartbeat(), set via properties
- RobotHealthHeartbeatArgs heartbeatArgs;
-
- //! configs used in heartbeat(channel), set by user via configureHeartbeatChannel(...)
- std::unordered_map<std::string, RobotHealthHeartbeatArgs> channelHeartbeatConfig;
+ RobotHealthHeartbeatArgs defaultHeartbeatArgs;
};
} // namespace armarx::plugins
diff --git a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
index 93400fe1761849e97c4e4c228eeae3b5e60e0047..21235afefb794e8c78443e840a240f56c6720b14 100644
--- a/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
+++ b/source/RobotAPI/libraries/aron/core/data/variant/container/Dict.cpp
@@ -112,7 +112,14 @@ namespace armarx::aron::data
DictPtr ret(new Dict(p));
for (const auto& [key, val] : getElements())
{
- ret->addElement(key, val->cloneAsVariant());
+ if (val)
+ {
+ ret->addElement(key, val->cloneAsVariant());
+ }
+ else
+ {
+ ret->addElement(key, nullptr);
+ }
}
return ret;
}
@@ -161,6 +168,8 @@ namespace armarx::aron::data
void
Dict::addElement(const std::string& key, const VariantPtr& data)
{
+ // Please note that the data may be null (indicating that a non existing maybetype has been added)
+
if (hasElement(key))
{
ARMARX_TRACE;
@@ -173,6 +182,8 @@ namespace armarx::aron::data
void
Dict::addElementCopy(const std::string& key, const VariantPtr& data)
{
+ // Please note that the data may be null (indicating that a non existing maybetype has been added)
+
if (hasElement(key))
{
ARMARX_TRACE;
@@ -213,11 +224,16 @@ namespace armarx::aron::data
void
Dict::setElement(const std::string& key, const VariantPtr& data)
{
- const auto& p = data->getPath();
- if (not p.hasDirectPrefix(this->getPath()))
+ // Please note that the data may be null (indicating that a non existing maybetype has been added)
+
+ if (data)
{
- ARMARX_WARNING << "An element added to a dict does not have a correct path set. This "
- "may cause errors. Please use setElemetCopy() instead.";
+ const auto& p = data->getPath();
+ if (not p.hasDirectPrefix(this->getPath()))
+ {
+ ARMARX_WARNING << "An element added to a dict does not have a correct path set. This "
+ "may cause errors. Please use setElemetCopy() instead.";
+ }
}
this->childrenNavigators[key] = data;
@@ -234,8 +250,14 @@ namespace armarx::aron::data
void
Dict::setElementCopy(const std::string& key, const VariantPtr& data)
{
- Path newPath = getPath().withElement(key);
- auto copy = data->cloneAsVariant(newPath);
+ // Please note that the data may be null (indicating that a non existing maybetype has been added)
+
+ VariantPtr copy = nullptr;
+ if (data)
+ {
+ Path newPath = getPath().withElement(key);
+ copy = data->cloneAsVariant(newPath);
+ }
setElement(key, copy);
}