add timings to debug observer

Signed-off-by: ARMAR-6 User <armar6@kit.edu>

source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp

@@ -3,7 +3,6 @@
 #include <filesystem>
 #include <istream>
 
-
 #include "ArmarXCore/core/time/Frequency.h"
 #include "ArmarXCore/core/time/Metronome.h"
 #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
@@ -11,13 +10,11 @@
 #include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
 #include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
 
-
 namespace armarx::armem::server::robot_state::proprioception
 {
 
     RobotUnitReader::RobotUnitReader() = default;
 
-
     void
     RobotUnitReader::connect(armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
                              armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
@@ -48,7 +45,6 @@ namespace armarx::armem::server::robot_state::proprioception
         }
     }
 
-
     void
     RobotUnitReader::run(float pollFrequency, Queue& dataBuffer)
     {
@@ -71,7 +67,6 @@ namespace armarx::armem::server::robot_state::proprioception
         }
     }
 
-
     std::optional<RobotUnitData>
     RobotUnitReader::fetchAndConvertLatestRobotUnitData()
     {
@@ -88,16 +83,16 @@ namespace armarx::armem::server::robot_state::proprioception
 
         RobotUnitData result;
 
-        if(converterProprioception != nullptr)
+        if (converterProprioception != nullptr)
         {
             result.proprioception = converterProprioception->convert(data.value(), description);
         }
 
-        if(converterExteroception != nullptr)
+        if (converterExteroception != nullptr)
         {
             result.exteroception = converterExteroception->convert(data.value(), description);
         }
-        
+
         result.timestamp = Time(Duration::MicroSeconds(data->timestampUSec));
 
         auto stop = std::chrono::high_resolution_clock::now();
@@ -114,7 +109,6 @@ namespace armarx::armem::server::robot_state::proprioception
         return result;
     }
 
-
     std::optional<RobotUnitDataStreaming::TimeStep>
     RobotUnitReader::fetchLatestData()
     {
@@ -122,6 +116,14 @@ namespace armarx::armem::server::robot_state::proprioception
         if (debugObserver)
         {
             debugObserver->setDebugObserverDatafield("RobotUnitReader | Buffer Size", data.size());
+            if (data.size())
+            {
+                debugObserver->setDebugObserverDatafield("RobotUnitReader | Timestamp USec",
+                                                         data.back().timestampUSec);
+                debugObserver->setDebugObserverDatafield(
+                    "RobotUnitReader | Timestamp Since Last Iteration",
+                    data.back().timesSinceLastIterationUSec);
+            }
         }
         if (data.empty())
         {