Feature/robot health update

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",