diff --git a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp index 5227bee38e728999d4a0f7fed6809913144573b6..55c65ddae518acc6dfa0c7eb78a32febf03e33b9 100644 --- a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp +++ b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.cpp @@ -24,7 +24,9 @@ #include <ArmarXCore/observers/variant/TimestampVariant.h> - +#include <ArmarXCore/core/time/DateTime.h> +#include <ArmarXCore/core/time/Duration.h> +#include <ArmarXCore/core/time/ice_conversions.h> @@ -52,6 +54,17 @@ namespace armarx ARMARX_INFO << "onConnect GamepadControlUnit"; platformUnitPrx = getProxy<PlatformUnitInterfacePrx>(getProperty<std::string>("PlatformUnitName").getValue()); emergencyStop = getProxy<EmergencyStopMasterInterfacePrx>("EmergencyStopMaster"); + + RobotHealthHeartbeatArgs args; + args.componentName = getDefaultName(); + + auto warningTime = armarx::core::time::Duration::MilliSeconds(1000); + + armarx::core::time::toIce(args.maximumCycleTimeWarning, warningTime); + args.description = "The GamepadControlUnit"; + args.requiredByDefault = true; + args.aliases = {"Gamepad"}; + this->heartbeat->signUp(args); } @@ -175,11 +188,10 @@ namespace armarx if(enableHeartBeat) - { - RobotHealthHeartbeatArgs args; - args.maximumCycleTimeErrorMS = 1000; - robotHealthTopic->heartbeat(getDefaultName(), args); - + { + auto now = armarx::core::time::dto::DateTime(); + armarx::core::time::toIce(now, armarx::core::time::DateTime::Now()); + robotHealthTopic->heartbeat(getDefaultName(), "", now); } //ARMARX_INFO << "sending targets" << data.leftStickX* scaleX << " " << data.leftStickY* scaleY << " " << data.rightStickX* scaleRotation; diff --git a/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h b/source/RobotAPI/components/GamepadControlUnit/GamepadControlUnit.h index da5945c44c7e289081df35385ea7e75184d69c90..ee35989d9ab3c4532e4c2aa920695853d6b8e32f 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> @@ -69,6 +70,7 @@ namespace armarx */ class GamepadControlUnit : virtual public armarx::Component, + virtual public armarx::HeartbeatComponentPluginUser, virtual public GamepadUnitListener { public: @@ -111,8 +113,7 @@ namespace armarx - 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 5364b5de5215fbd9ca8856a2f6ff5e28dfa34b32..e5494c8e15709ee7d7c9efcef3fb1805b6a089e0 100644 --- a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp +++ b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp @@ -22,6 +22,8 @@ #include "RobotHealth.h" +#include <algorithm> + #include <boost/regex.hpp> #include <ArmarXCore/core/time/ice_conversions.h> @@ -36,9 +38,6 @@ namespace armarx monitorUpdateHealthTask = new PeriodicTask<RobotHealth>(this, &RobotHealth::monitorHealthUpdateTaskClb, 10); - monitorAggregatedHealthTask = - new PeriodicTask<RobotHealth>(this, &RobotHealth::monitorAggregatedHealthTaskClb, 10); - defaultMaximumCycleTimeWarn = armarx::core::time::Duration::MilliSeconds(p.maximumCycleTimeWarnMS); defaultMaximumCycleTimeErr = @@ -99,44 +98,47 @@ namespace armarx e.state = HealthWarning; } + else + { + // If everything is OK (again), update health + e.state = HealthOK; + } } - reportDebugObserver(); - } - - void - RobotHealth::monitorAggregatedHealthTaskClb() - { + // update aggregated health RobotHealthState overallHealthState = RobotHealthState::HealthOK; + + // get aggregated status + for (size_t i = 0; i < updateEntries.size(); i++) { - std::shared_lock lock(updateMutex); + ARMARX_TRACE; + UpdateEntry& e = updateEntries.at(i); - // 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); - std::shared_lock elock(e.mutex); + // trim history + while (e.history.size() > 20) + { + e.history.pop_front(); + } - if (e.required) + if (e.required) + { + if (e.state == HealthWarning && overallHealthState != HealthError) { - if (e.state == HealthWarning && overallHealthState != HealthError) - { - overallHealthState = RobotHealthState::HealthWarning; - } - else if (e.state == HealthError) - { - overallHealthState = RobotHealthState::HealthError; - } + overallHealthState = RobotHealthState::HealthWarning; } - else + else if (e.state == HealthError) { - if (e.state != HealthOK && overallHealthState != HealthError) - { - overallHealthState = RobotHealthState::HealthWarning; - } + overallHealthState = RobotHealthState::HealthError; + } + } + else + { + if (e.state != HealthOK && overallHealthState != HealthError) + { + overallHealthState = RobotHealthState::HealthWarning; } } } @@ -149,6 +151,8 @@ namespace armarx } ARMARX_TRACE; p.aggregatedRobotHealthTopicPrx->aggregatedHeartbeat(overallHealthState); + + reportDebugObserver(); } void @@ -185,6 +189,23 @@ namespace armarx return defs; } + std::optional<RobotHealth::UpdateEntry*> + RobotHealth::findUpdateEntry(const std::string& name) + { + ARMARX_TRACE; + std::shared_lock lock(updateMutex); + + for (size_t i = 0; i < updateEntries.size(); i++) + { + if (updateEntries.at(i).name == name) + { + return &updateEntries.at(i); + } + } + + return nullptr; + } + std::pair<bool, RobotHealth::UpdateEntry&> RobotHealth::findOrCreateUpdateEntry(const std::string& name) { @@ -204,7 +225,7 @@ namespace armarx } std::unique_lock lock(updateMutex); - ARMARX_INFO << "registering for updates: '" << name << "'"; + ARMARX_INFO << "registering for heartbeat: '" << name << "'"; updateEntries.emplace_back(name, defaultMaximumCycleTimeWarn, defaultMaximumCycleTimeErr); @@ -212,72 +233,61 @@ namespace armarx } void - RobotHealth::signUp(const std::string& componentName, - const RobotHealthHeartbeatArgs& args, - const Ice::Current& current) + RobotHealth::signUp(const RobotHealthHeartbeatArgs& args, const Ice::Current& current) { ARMARX_TRACE; - if (args->mode == POLLING) - { - // TODO - } - else - { - auto argsCasted = RobotHealthUpdateHeartbeatArgsPtr::dynamicCast(args); + auto e = findOrCreateUpdateEntry(args.componentName); - auto e = findOrCreateUpdateEntry(componentName); - - std::unique_lock lock(e.second.mutex); + 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."; - } + if (e.first) + { + ARMARX_INFO << "Newly registering component " << args.componentName << " for updates."; + } + // else no messsage as components may spam sign up for legacy reasons (when there was no sign up) - armarx::core::time::fromIce(argsCasted->maximumCycleTimeWarning, - e.second.maximumCycleTimeWarn); - armarx::core::time::fromIce(argsCasted->maximumCycleTimeError, - e.second.maximumCycleTimeErr); + armarx::core::time::fromIce(args.maximumCycleTimeWarning, e.second.maximumCycleTimeWarn); + armarx::core::time::fromIce(args.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(); - } + e.second.aliases = args.aliases; + e.second.required = args.requiredByDefault; + 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 std::string& message, const core::time::dto::DateTime& referenceTime, const Ice::Current&) { ARMARX_TRACE; - auto e = findOrCreateUpdateEntry(componentName); - - std::unique_lock lock(e.second.mutex); + auto o = findUpdateEntry(componentName); - if (e.first) // newly created + if (!o) { - 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."; + ARMARX_WARNING << "Attention. Component " << componentName << " with message " + << message + << " was not signed up for heartbeat. Ignoring heartbeat for now..."; + return; } - if (!e.second.enabled) + auto& e = o.value(); + + std::unique_lock lock(e->mutex); + + if (!e->enabled) { - 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; + ARMARX_WARNING << deactivateSpam() << "Attention. Component " << componentName + << " with message " << message + << " was not enabled. The heartbeat is ignored... "; + return; } auto now = armarx::core::time::DateTime::Now(); + e->history.push_back(now); } void @@ -286,7 +296,7 @@ namespace armarx ARMARX_TRACE; bool found = false; { - std::shared_lock lock(pollingMutex); + std::shared_lock lock(updateMutex); for (size_t i = 0; i < updateEntries.size(); i++) { @@ -294,7 +304,9 @@ namespace armarx if (e.name == componentName) { std::unique_lock elock(e.mutex); + e.required = false; e.enabled = false; + found = true; break; } } @@ -311,46 +323,102 @@ namespace armarx } } - RobotHealthInfo - RobotHealth::getSummary(const Ice::Current&) + void + armarx::RobotHealth::require(const std::vector<std::string>& namesOrTags, + const Ice::Current& current) { - ARMARX_TRACE; - auto now = armarx::core::time::DateTime::Now(); + std::shared_lock lock(updateMutex); - std::stringstream ss; - for (size_t i = 0; i < updateEntries.size(); i++) + for (const auto& nameOrAlias : namesOrTags) { - ARMARX_TRACE; - UpdateEntry& e = updateEntries.at(i); - auto delta = now - e.history.back(); - - auto minDelta = delta; - auto maxDelta = delta; - for (size_t i = 1; i < e.history.size(); i++) + for (auto& e : updateEntries) { - auto historicalDelta = e.history.at(i); - - if (minDelta > historicalDelta) + std::unique_lock lock(e.mutex); + if (e.name == nameOrAlias or + (std::find(e.aliases.begin(), e.aliases.end(), nameOrAlias) != e.aliases.end())) { + e.required = true; + if (!e.enabled) + { + ARMARX_WARNING << "You are requiring the disaled component " << e.name + << ". Enabling it..."; + e.enabled = true; + } } + } + } + } + + void + armarx::RobotHealth::unrequire(const std::vector<std::string>& namesOrTags, + const Ice::Current& current) + { + std::shared_lock lock(updateMutex); - if (maxDelta < historicalDelta) + for (const auto& nameOrAlias : namesOrTags) + { + for (auto& e : updateEntries) + { + std::unique_lock lock(e.mutex); + if (e.name == nameOrAlias or + std::find(e.aliases.begin(), e.aliases.end(), nameOrAlias) != e.aliases.end()) { + e.required = false; } } + } + } + + RobotHealthInfo + RobotHealth::getSummary(const Ice::Current&) + { + ARMARX_TRACE; + RobotHealthInfo ret; + + std::stringstream ss; + auto now = armarx::core::time::DateTime::Now(); + + armarx::core::time::Duration minDelta; + armarx::core::time::Duration maxDelta; + + for (size_t j = 0; j < updateEntries.size(); j++) + { + RobotHealthInfoEntry healthEntry; + auto& e = updateEntries[j]; + + ss << e.name << ":\n"; - ss << e.name; if (e.required) { - ss << ", required"; + ss << " - required\n"; } if (!e.required && !e.enabled) { - ss << ": disabled"; + ss << " - disabled\n"; } - else + + ss << " - history:\n"; + + for (size_t i = 0; i < e.history.size(); i++) { + auto later = e.history[i]; + auto pre = e.history[i - 1]; + + auto delta = later - pre; + + if (minDelta > delta) + { + minDelta = delta; + } + + if (maxDelta < delta) + { + maxDelta = delta; + } + + ss << " -- " + << "[" << later << " - " << pre << "]"; if (delta > e.maximumCycleTimeErr) { ss << ": ERROR"; @@ -361,22 +429,30 @@ namespace armarx } else { - ss << ": ok"; + ss << ": OK"; } - ss << " (" << (delta / 1000) << "ms, min: " << (minDelta / 1000) - << "ms, max: " << (maxDelta / 1000) << "ms)"; + ss << "\n"; } - ARMARX_TRACE; - std::unique_lock lock(e.mutex); - if (e.message.size()) - { - ss << " - " << e.message; - } + ss << " (" + << "min: " << (minDelta / 1000) << "ms, max: " << (maxDelta / 1000) << "ms)"; ss << "\n"; + + healthEntry.componentName = e.name; + healthEntry.state = e.state; + healthEntry.message = ss.str(); + + ret[e.name] = healthEntry; } - return ss.str(); + + return ret; + } + + std::string + RobotHealth::getTopicName(const Ice::Current& current) + { + return p.robotHealthTopicName; } void @@ -384,11 +460,19 @@ namespace armarx { 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]; + auto delta = later - pre; + + setDebugObserverDatafield("RobotHealth_" + entry.name + "_lastDelta", + delta.toMilliSeconds()); + setDebugObserverDatafield("RobotHealth_" + entry.name + "_maximumCycleTimeWarn", + entry.maximumCycleTimeWarn.toMilliSeconds()); + setDebugObserverDatafield("RobotHealth_" + entry.name + "_maximumCycleTimeErr", + entry.maximumCycleTimeErr.toMilliSeconds()); + } } sendDebugObserverBatch(); diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.h b/source/RobotAPI/components/RobotHealth/RobotHealth.h index a47b7a7d010553598f4e44107915f2a26112afa2..a0257873265ac3a86753b8942fd8b8579f66b8d1 100644 --- a/source/RobotAPI/components/RobotHealth/RobotHealth.h +++ b/source/RobotAPI/components/RobotHealth/RobotHealth.h @@ -109,11 +109,12 @@ namespace armarx } std::string name; + std::vector<std::string> aliases; RobotHealthState state = HealthOK; std::string description = ""; bool required = false; - bool enabled = true; + bool enabled = false; mutable std::shared_mutex mutex; std::deque<armarx::core::time::DateTime> history; @@ -123,8 +124,8 @@ namespace armarx }; void monitorHealthUpdateTaskClb(); - void monitorAggregatedHealthTaskClb(); + std::optional<UpdateEntry*> findUpdateEntry(const std::string& name); std::pair<bool, UpdateEntry&> findOrCreateUpdateEntry(const std::string& name); void reportDebugObserver(); @@ -132,13 +133,16 @@ namespace armarx // RobotHealthInterface interface public: void unregister(const std::string& componentName, const Ice::Current&) override; - void signUp(const std::string& componentName, - const RobotHealthHeartbeatArgs& args, - const Ice::Current& current) override; + void signUp(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; + void require(const std::vector<std::string>& namesOrTags, + const Ice::Current& current) override; + void unrequire(const std::vector<std::string>& namesOrTags, + const Ice::Current& current) override; + std::string getTopicName(const Ice::Current& current) override; // RobotHealthComponentInterface interface public: @@ -150,7 +154,6 @@ namespace armarx std::deque<UpdateEntry> updateEntries; PeriodicTask<RobotHealth>::pointer_type monitorUpdateHealthTask; - PeriodicTask<RobotHealth>::pointer_type monitorAggregatedHealthTask; armarx::core::time::Duration defaultMaximumCycleTimeWarn; armarx::core::time::Duration defaultMaximumCycleTimeErr; diff --git a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp index 416a43da0472c2c7bd35fb6df183b93cf9bd1a2c..0c80dabee185cefda0c32e6f93d8773eb7fd2ed4 100644 --- a/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp +++ b/source/RobotAPI/components/RobotHealth/RobotHealthDummy.cpp @@ -93,8 +93,9 @@ namespace armarx void RobotHealthDummy::runTask() { - - robotHealthTopicPrx->signUp(getName(), RobotHealthHeartbeatArgs()); + auto args = RobotHealthHeartbeatArgs(); + args.componentName = getName(); + robotHealthTopicPrx->signUp(args); ARMARX_INFO << "starting rinning task"; while (!dummyTask->isStopped()) @@ -104,7 +105,7 @@ namespace armarx armarx::core::time::dto::DateTime now; armarx::core::time::toIce(now, armarx::core::time::DateTime::Now()); - robotHealthTopicPrx->heartbeat(getName(), now); + robotHealthTopicPrx->heartbeat(getName(), "", now); long afterTopicCall = TimeUtil::GetTime().toMicroSeconds(); if (sleepmode == "nanosleep") { diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp index 52af2c8dd81679509f7aaf965caa91e3b49349eb..7332a8cbe302ac112430ae566db6a9669e57e5b1 100644 --- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp +++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp @@ -22,17 +22,19 @@ #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; - -void HokuyoLaserUnit::onInitComponent() +void +HokuyoLaserUnit::onInitComponent() { offeringTopicFromProperty("RobotHealthTopicName"); offeringTopicFromProperty("DebugObserverName"); @@ -72,19 +74,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()); + robotHealthTopic = getTopic<RobotHealthInterfacePrx>( + getProperty<std::string>("RobotHealthTopicName").getValue()); 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 +99,30 @@ 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; } + RobotHealthHeartbeatArgs args; + auto warn = armarx::core::time::Duration::MilliSeconds(500); + auto err = armarx::core::time::Duration::MilliSeconds(800); + armarx::core::time::toIce(args.maximumCycleTimeWarning, warn); + armarx::core::time::toIce(args.maximumCycleTimeError, err); + args.componentName = device.componentName + "_" + device.ip; + + if (robotHealthTopic) + { + robotHealthTopic->signUp(args); + } + else + { + ARMARX_WARNING << "No robot health topic available: IP: " << device.ip << ", Port: " << device.port; + } + LaserScannerInfo info; info.device = device.ip; info.frame = device.frame; @@ -120,18 +142,17 @@ void HokuyoLaserUnit::onConnectComponent() 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 +169,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 +207,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,13 +220,15 @@ 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() { while (!task->isStopped()) { @@ -210,7 +236,7 @@ void HokuyoLaserScanDevice::run() 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 +245,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 +263,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); } @@ -265,7 +293,8 @@ void HokuyoLaserScanDevice::run() } 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 +303,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..57595aff8584f6e845664b9b5f69e52ba715c7e5 100644 --- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h +++ b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h @@ -48,6 +48,8 @@ namespace armarx 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>("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 +68,7 @@ namespace armarx bool reconnect(); + void run(); RunningTask<HokuyoLaserScanDevice>::pointer_type task; @@ -73,6 +76,7 @@ namespace armarx std::string componentName; LaserScannerUnitListenerPrx scanTopic; + RobotHealthPrx RobotHealthInterfacePrx robotHealthTopic; DebugObserverInterfacePrx debugObserver; }; diff --git a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp index ca95dd07836e035a04019bd2fd1d176c4ca01256..f1759a72961cfa07a27a05181cb6d5d25e119dad 100644 --- a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp +++ b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp @@ -22,10 +22,15 @@ #include "GuiHealthClientWidgetController.h" -#include <QLabel> #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 { GuiHealthClientWidgetController::GuiHealthClientWidgetController() @@ -36,9 +41,20 @@ namespace armarx 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(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); updateSummaryTimer = new QTimer(this); updateSummaryTimer->setInterval(100); @@ -50,39 +66,38 @@ namespace armarx //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) @@ -91,15 +106,24 @@ namespace armarx try { //ARMARX_IMPORTANT << "before set text"; - widget.labelHealthState->setText(QString::fromUtf8(robotHealthComponentPrx->getSummary().c_str())); + auto summary = robotHealthComponentPrx->getSummary(); + std::stringstream ss; + for (const auto& [key, entry] : summary) + { + ss << entry.message; + } + + widget.labelHealthState->setText(QString::fromUtf8(ss.str().c_str())); //ARMARX_IMPORTANT << "after set text"; } catch (...) - {} + { + } } } - void GuiHealthClientWidgetController::toggleSendOwnHeartbeats() + void + GuiHealthClientWidgetController::toggleSendOwnHeartbeats() { if (ownHeartbeatsActive) { @@ -117,32 +141,44 @@ namespace armarx widget.labelState->setText("sending heartbeats..."); widget.pushButtonToggleOwnHeartbeats->setText("unregister self"); } - } - - void GuiHealthClientWidgetController::onConnectComponent() + void + GuiHealthClientWidgetController::onConnectComponent() { //ARMARX_IMPORTANT << "onConnectComponent"; robotHealthTopicPrx = getTopic<RobotHealthInterfacePrx>("RobotHealthTopic"); robotHealthComponentPrx = getProxy<RobotHealthComponentInterfacePrx>("RobotHealth"); invokeConnectComponentQt(); + + RobotHealthHeartbeatArgs rhha; + auto warn = armarx::core::time::Duration::MilliSeconds(250); + auto err = armarx::core::time::Duration::MilliSeconds(500); + armarx::core::time::toIce(rhha.maximumCycleTimeWarning, warn); + armarx::core::time::toIce(rhha.maximumCycleTimeError, err); + rhha.componentName = getName(); + + robotHealthComponentPrx->signUp(rhha); } - 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/interface/components/RobotHealthInterface.ice b/source/RobotAPI/interface/components/RobotHealthInterface.ice index 9a0ce888fd65b89ddebbb53306d5e25549f45f77..1b8037b9ae30c6bb4d47b163374fff1865c5e26c 100644 --- a/source/RobotAPI/interface/components/RobotHealthInterface.ice +++ b/source/RobotAPI/interface/components/RobotHealthInterface.ice @@ -27,17 +27,18 @@ module armarx { + enum RobotHealthState { HealthOK, HealthWarning, HealthError - };´ + }; struct RobotHealthHeartbeatArgs { string componentName; - bool required; + bool requiredByDefault; string description; - Ice::StringList tags; + Ice::StringSeq aliases; armarx::core::time::dto::Duration maximumCycleTimeWarning; armarx::core::time::dto::Duration maximumCycleTimeError; @@ -45,13 +46,16 @@ module armarx interface RobotHealthInterface { - idempotent void require(Ice::StringSeq namesOrTags); - idempotent void unrequire(Ice::StringSeq namesOrTags); + void require(Ice::StringSeq namesOrTags); + void unrequire(Ice::StringSeq namesOrTags); + + void signUp(RobotHealthHeartbeatArgs args); + + void heartbeat(string componentName, string message, armarx::core::time::dto::DateTime referenceTime); - idempotent void signUp(RobotHealthHeartbeatArgs args); + void unregister(string componentName); - idempotent void heartbeat(string componentName, string message, armarx::core::time::dto::DateTime referenceTime); - idempotent void unregister(string componentName); + string getTopicName(); }; // Used by the RobotHealth to send an overall status update ehich e.g. the emergency stop listens to. diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp index ceafcbf56a5ecc0938e259f5ab7ada03b7b00db3..6411af0bbd006ba74f08edbe3bd079e76f321fa6 100644 --- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp @@ -2,6 +2,8 @@ #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> @@ -16,20 +18,43 @@ namespace armarx::plugins void HeartbeatComponentPlugin::configureHeartbeatChannel(const std::string& channel, - const std::string& message) + const std::string& description) { - auto args = heartbeatArgs; - args.message = message; + auto args = defaultHeartbeatArgs; + args.description = description; configureHeartbeatChannel(channel, args); } void - HeartbeatComponentPlugin::heartbeat() + HeartbeatComponentPlugin::signUp() + { + RobotHealthHeartbeatArgs args = defaultHeartbeatArgs; + args.componentName = componentName; + p.rhprx->signUp(args); + } + + void + HeartbeatComponentPlugin::signUp(const std::string& channelName) + { + const auto argsIt = channelHeartbeatConfig.find(channelName); + ARMARX_CHECK(argsIt != channelHeartbeatConfig.end()) + << "signUp(channel) called for unknown channel '" << channelName << "'." + << "You must register the config using configureHeartbeatChannel(channel) first!"; + + RobotHealthHeartbeatArgs args = channelHeartbeatConfig[channelName]; + args.componentName = componentName + "_" + channelName; + p.rhprx->signUp(args); + } + + void + HeartbeatComponentPlugin::heartbeat(const std::string& message) { if (robotHealthTopic) { - robotHealthTopic->heartbeat(componentName, heartbeatArgs); + armarx::core::time::dto::DateTime now; + armarx::core::time::toIce(now, armarx::core::time::DateTime::Now()); + robotHealthTopic->heartbeat(componentName, message, now); } else { @@ -38,18 +63,13 @@ namespace armarx::plugins } void - HeartbeatComponentPlugin::heartbeat(const std::string& channel) + HeartbeatComponentPlugin::heartbeat(const std::string& channelName, const std::string& message) { - 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); + armarx::core::time::dto::DateTime now; + armarx::core::time::toIce(now, armarx::core::time::DateTime::Now()); + robotHealthTopic->heartbeat(componentName + "_" + channelName, message, now); } else { @@ -60,6 +80,13 @@ namespace armarx::plugins 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)); @@ -75,31 +102,31 @@ namespace armarx::plugins void HeartbeatComponentPlugin::preOnConnectComponent() { - // robotHealthTopic = parent<Component>().getTopic<RobotHealthInterfacePrx>(topicName); - componentName = parent<Component>().getName(); + topicName = p.rhprx->getTopicName(); + robotHealthTopic = parent<Component>().getTopic<RobotHealthInterfacePrx>(topicName); } 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(p.rhprx, + "RobotHealth", + healthPropertyName, + "Name of the robot health component."); } if (not properties->hasDefinition(makePropertyName(maximumCycleTimeWarningMSPropertyName))) { - properties->required(heartbeatArgs.maximumCycleTimeWarningMS, + properties->required(p.maximumCycleTimeWarningMS, maximumCycleTimeWarningMSPropertyName, "maximum cycle time before warning is emitted"); } if (not properties->hasDefinition(makePropertyName(maximumCycleTimeErrorMSPropertyName))) { - properties->required(heartbeatArgs.maximumCycleTimeErrorMS, + 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 26d4d9537f51f77e124999ddd9864df3fa2734af..4f4e9564f03850443cc8d45263a1664fecb6e50b 100644 --- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h @@ -35,27 +35,20 @@ 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 RobotHealthHeartbeatArgs& args); /** - * @brief Configures a heartbeat subchannel. - * - * @param channel Identifier of the heartbeat channel - * @param args Configuration of this channel's heartbeat properties + * @brief register component with a specific name to heartbeat */ - void configureHeartbeatChannel(const std::string& channel, - const RobotHealthHeartbeatArgs& args); + void signUp(const std::string& channelName, const RobotHealthHeartbeatArgs& args); /** * @brief Sends out a heartbeat using the default config * */ - void heartbeat(); + void heartbeat(const std::string& message = ""); /** * @brief Sends out a heartbeat for a subchannel. @@ -64,7 +57,7 @@ namespace armarx::plugins * * @param channel Identifier of the heartbeat channel */ - void heartbeat(const std::string& channel); + void heartbeat(const std::string& channelName, const std::string& message = ""); protected: void preOnInitComponent() override; @@ -75,22 +68,30 @@ namespace armarx::plugins private: //! heartbeat topic name (outgoing) - std::string topicName{"RobotHealthTopic"}; //! name of this component used as identifier for heartbeats std::string componentName; // - 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; + std::string topicName{"RobotHealthTopic"}; + + struct Properties + { + RobotHealthComponentInterfacePrx rhprx; + long maximumCycleTimeWarningMS = 50; + long maximumCycleTimeErrorMS = 100; + } p; //! default config used in heartbeat(), set via properties - RobotHealthHeartbeatArgs heartbeatArgs; + RobotHealthHeartbeatArgs defaultHeartbeatArgs; //! configs used in heartbeat(channel), set by user via configureHeartbeatChannel(...) std::unordered_map<std::string, RobotHealthHeartbeatArgs> channelHeartbeatConfig;