diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
index 0daef1ab73ee13ed22d59090270ffe94a6736c44..0b67edc552c9bd77aa0f8ca78d95c2f2a5659282 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.cpp
@@ -24,6 +24,8 @@
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/time/Clock.h"
+#include "ArmarXCore/core/time/DateTime.h"
+#include "ArmarXCore/core/time/Duration.h"
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
@@ -91,11 +93,11 @@ namespace armarx
continue;
}
- auto delta = now - e.history.back();
+ auto deltaToArrival = now - e.history.back().arrivalTime;
ARMARX_TRACE;
- if (delta > e.maximumCycleTimeErr)
+ if (deltaToArrival > e.maximumCycleTimeErr)
{
ARMARX_TRACE;
@@ -107,7 +109,7 @@ namespace armarx
e.state = HealthError;
}
}
- else if (delta > e.maximumCycleTimeWarn)
+ else if (deltaToArrival > e.maximumCycleTimeWarn)
{
ARMARX_TRACE;
ARMARX_WARNING << deactivateSpam(0.1, e.name) << "Component " << e.name
@@ -163,7 +165,7 @@ namespace armarx
if (overallHealthState == HealthError)
{
ARMARX_TRACE;
- ARMARX_INFO << "Requesting emergency stop";
+ ARMARX_INFO << deactivateSpam(3) << "Requesting emergency stop";
ARMARX_CHECK_NOT_NULL(p.emergencyStopTopicPrx);
p.emergencyStopTopicPrx->reportEmergencyStopState(
EmergencyStopState::eEmergencyStopActive);
@@ -281,12 +283,15 @@ namespace armarx
void
RobotHealth::heartbeat(const std::string& identifier,
- const core::time::dto::DateTime& /*referenceTime*/,
+ const core::time::dto::DateTime& referenceTime,
const Ice::Current& current)
{
ARMARX_TRACE;
ARMARX_VERBOSE << "Finding update entry";
+ const DateTime arrivalTimestamp = Clock::Now();
+
+
// 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);
@@ -294,7 +299,7 @@ namespace armarx
if (entry == nullptr)
{
- ARMARX_WARNING << "Attention. Component `" << identifier
+ ARMARX_WARNING << deactivateSpam() << "Attention. Component `" << identifier
<< "` was not signed up for heartbeat. Ignoring heartbeat for now...";
return;
}
@@ -314,10 +319,9 @@ namespace armarx
// 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);
+ armarx::core::time::DateTime refTime;
+ fromIce(referenceTime, refTime);
+ entry->history.push_back(UpdateEntry::TimeInfo{.referenceTime = refTime, .arrivalTime = arrivalTimestamp});
}
void
@@ -472,7 +476,7 @@ namespace armarx
auto later = e.history[i];
auto pre = e.history[i - 1];
- auto delta = later - pre;
+ auto delta = later.arrivalTime - pre.arrivalTime;
if (minDelta > delta)
{
@@ -485,7 +489,12 @@ namespace armarx
}
}
- const Duration timeSinceLastUpdate = e.history.empty() ? Duration() : Clock::Now() - e.history.back();
+ const Duration timeSinceLastUpdateArrival = e.history.empty() ? Duration() : Clock::Now() - e.history.back().arrivalTime;
+ const Duration timeSinceLastUpdateReference = e.history.empty() ? Duration() : Clock::Now() - e.history.back().referenceTime;
+
+ const DateTime lastReferenceTime = e.history.empty() ? armarx::core::time::DateTime::Invalid() : e.history.back().referenceTime;
+
+ const Duration timeSyncDelayAndIce = e.history.empty() ? armarx::core::time::Duration() : e.history.back().arrivalTime - e.history.back().referenceTime;
healthEntry.identifier = e.name;
healthEntry.state = e.state;
@@ -493,7 +502,10 @@ namespace armarx
healthEntry.required = e.required;
toIce(healthEntry.minDelta, minDelta);
toIce(healthEntry.maxDelta,maxDelta);
- toIce(healthEntry.timeSinceLastUpdate, timeSinceLastUpdate);
+ toIce(healthEntry.lastReferenceTimestamp, lastReferenceTime);
+ toIce(healthEntry.timeSinceLastArrival, timeSinceLastUpdateArrival);
+ toIce(healthEntry.timeSyncDelayAndIce, timeSyncDelayAndIce);
+ toIce(healthEntry.timeSinceLastUpdateReference, timeSinceLastUpdateReference);
toIce(healthEntry.maximumCycleTimeWarning, e.maximumCycleTimeWarn);
toIce(healthEntry.maximumCycleTimeError, e.maximumCycleTimeErr);
healthEntry.tags = e.tags;
@@ -522,14 +534,20 @@ namespace armarx
{
auto later = entry.history[entry.history.size() - 1];
auto pre = entry.history[entry.history.size() - 2];
- const Duration delta = later - pre;
+ const Duration delta = later.arrivalTime - pre.arrivalTime;
- const Duration deltaToNow = Clock::Now() - entry.history.back();
+ const Duration timeSinceLastArrival = Clock::Now() - entry.history.back().arrivalTime;
+ const Duration timeToLastReference = Clock::Now() - entry.history.back().referenceTime;
+ const Duration timeSyncDelay = entry.history.back().arrivalTime - entry.history.back().referenceTime;
setDebugObserverDatafield("RobotHealth_" + entry.name + "_lastDelta",
delta.toMilliSecondsDouble());
- setDebugObserverDatafield("RobotHealth_" + entry.name + "_deltaToNow",
- deltaToNow.toMilliSecondsDouble());
+ setDebugObserverDatafield("RobotHealth_" + entry.name + "_timeSinceLastArrival",
+ timeSinceLastArrival.toMilliSecondsDouble());
+ setDebugObserverDatafield("RobotHealth_" + entry.name + "_timeToLastReference",
+ timeToLastReference.toMilliSecondsDouble());
+ setDebugObserverDatafield("RobotHealth_" + entry.name + "_timeSyncDelayAndIce",
+ timeSyncDelay.toMilliSecondsDouble());
setDebugObserverDatafield("RobotHealth_" + entry.name + "_maximumCycleTimeWarn",
entry.maximumCycleTimeWarn.toMilliSecondsDouble());
setDebugObserverDatafield("RobotHealth_" + entry.name + "_maximumCycleTimeErr",
diff --git a/source/RobotAPI/components/RobotHealth/RobotHealth.h b/source/RobotAPI/components/RobotHealth/RobotHealth.h
index 899c237647f8c381bab41dd4db35d96dbe6499a4..a27f864e8f639983650ece96ac8d101f709b8618 100644
--- a/source/RobotAPI/components/RobotHealth/RobotHealth.h
+++ b/source/RobotAPI/components/RobotHealth/RobotHealth.h
@@ -117,7 +117,17 @@ namespace armarx
bool enabled = false;
mutable std::shared_mutex mutex;
- std::deque<armarx::core::time::DateTime> history;
+
+ struct TimeInfo
+ {
+ //< Timestamp sent by component
+ armarx::core::time::DateTime referenceTime;
+
+ //< Timestamp on this PC, set by this component
+ armarx::core::time::DateTime arrivalTime;
+ };
+
+ std::deque<TimeInfo> history;
armarx::core::time::Duration maximumCycleTimeWarn;
armarx::core::time::Duration maximumCycleTimeErr;
diff --git a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
index aa795eb22ff8c96dfcb82b77f2f4f46f1512471a..d976d77e6742b63152b172b7a55107929405302d 100644
--- a/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/GuiHealthClient/GuiHealthClientWidgetController.cpp
@@ -25,6 +25,7 @@
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <qcolor.h>
#include <qnamespace.h>
+#include <qrgb.h>
#include <qtablewidget.h>
#include <string>
@@ -120,7 +121,7 @@ namespace armarx
{
auto summary = robotHealthComponentPrx->getSummary();
- const std::size_t nCols = 7;
+ const std::size_t nCols = 10;
auto& tableWidget = widget.tableHealthState;
tableWidget->setRowCount(summary.entries.size());
@@ -129,7 +130,7 @@ namespace armarx
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]"
+ "identifier", "required", "keeps frequency", "tags", "time since\nlast arrival [ms]", "time to\nreference [ms]", "time sync \n+ Ice [ms]", "warning\nthreshold [ms]", "error\nthreshold [ms]", "hostname"
});
tableWidget->setColumnWidth(0, 250);
@@ -139,6 +140,7 @@ namespace armarx
tableWidget->setColumnWidth(4, 120);
tableWidget->setColumnWidth(5, 120);
tableWidget->setColumnWidth(6, 120);
+ tableWidget->setColumnWidth(7, 120);
for(std::size_t i = 0; i < summary.entries.size(); i++)
{
@@ -162,9 +164,15 @@ namespace armarx
break;
}
- const std::string timeSinceLastRepr = std::to_string(entry.timeSinceLastUpdate.microSeconds / 1000);
+ const std::string hostname = entry.lastReferenceTimestamp.hostname;
+
+ const std::string timeSinceLastArrivalRepr = std::to_string(entry.timeSinceLastArrival.microSeconds / 1000);
+ const std::string timeToLastReferenceRepr = std::to_string(entry.timeSinceLastUpdateReference.microSeconds / 1000);
const std::string tagsRepr = serializeList(entry.tags);
+ const long syncErrorMilliSeconds = std::abs(entry.timeSinceLastArrival.microSeconds - entry.timeSinceLastUpdateReference.microSeconds) / 1000;
+
+
tableWidget->setItem(i, 0, new QTableWidgetItem(QString::fromStdString(entry.identifier)));
auto* requiredItem = new QTableWidgetItem(QString::fromStdString(entry.required ? "yes" : "no"));
@@ -178,9 +186,26 @@ namespace armarx
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))));
+ tableWidget->setItem(i, 4, new QTableWidgetItem(QString::fromStdString(timeSinceLastArrivalRepr)));
+ tableWidget->setItem(i, 5, new QTableWidgetItem(QString::fromStdString(timeToLastReferenceRepr)));
+
+ tableWidget->setItem(i, 6, new QTableWidgetItem(QString::fromStdString(std::to_string(syncErrorMilliSeconds))));
+
+ if(syncErrorMilliSeconds > 20)
+ {
+ QColor timeSyncColor;
+ timeSyncColor.setRgb(255, 0, 0);
+ tableWidget->item(i, 6)->setBackgroundColor(timeSyncColor);
+ }else {
+ QColor timeSyncColor;
+ timeSyncColor.setRgb(0, 255, 0);
+ tableWidget->item(i, 6)->setBackgroundColor(timeSyncColor);
+ }
+
+
+ tableWidget->setItem(i, 7, new QTableWidgetItem(QString::fromStdString(std::to_string(entry.maximumCycleTimeWarning.microSeconds / 1000))));
+ tableWidget->setItem(i, 8, new QTableWidgetItem(QString::fromStdString(std::to_string(entry.maximumCycleTimeError.microSeconds / 1000))));
+ tableWidget->setItem(i, 9, new QTableWidgetItem(QString::fromStdString(hostname)));
}
std::string tagsText = "Active tags: [";
diff --git a/source/RobotAPI/interface/components/RobotHealthInterface.ice b/source/RobotAPI/interface/components/RobotHealthInterface.ice
index a42072ae33903969d0de84e5bd7de708d50532a0..0832146d934a45cf35dd2d42591a4b4949f8d15d 100644
--- a/source/RobotAPI/interface/components/RobotHealthInterface.ice
+++ b/source/RobotAPI/interface/components/RobotHealthInterface.ice
@@ -72,7 +72,16 @@ module armarx
// string message;
armarx::core::time::dto::Duration minDelta;
armarx::core::time::dto::Duration maxDelta;
- armarx::core::time::dto::Duration timeSinceLastUpdate;
+
+ armarx::core::time::dto::DateTime lastReferenceTimestamp;
+
+ //< Time delta to now() when arrived at heart beat component
+ armarx::core::time::dto::Duration timeSinceLastArrival;
+
+ //< Time delta to reference timestamp sent by component
+ armarx::core::time::dto::Duration timeSinceLastUpdateReference;
+
+ armarx::core::time::dto::Duration timeSyncDelayAndIce;
bool required; //
bool enabled;