using debug observer for timing info

source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/AzureKinectPointCloudProvider.cpp

@@ -102,17 +102,7 @@ namespace
         }
     }
 
-    std::function<void(armarx::Duration)>
-    createSwCallback(
-        armarx::ManagedIceObject* obj,
-        const std::string& description)
-    {
-        return [ = ](armarx::Duration duration)
-        {
-            std::string name = "duration " + description + " in [ms]";
-            obj->setMetaInfo(name, new armarx::Variant{duration.toMilliSecondsDouble()});
-        };
-    }
+    
 
 #ifdef INCLUDE_BODY_TRACKING
     void
@@ -139,6 +129,19 @@ namespace
 namespace visionx
 {
 
+    std::function<void(armarx::Duration)>
+    AzureKinectPointCloudProvider::createSwCallback(
+        const std::string& description)
+    {
+        return [ & ](armarx::Duration duration)
+        {
+            std::string name = "duration " + description + " in [ms]";
+            setMetaInfo(name, new armarx::Variant{duration.toMilliSecondsDouble()});
+
+            setDebugObserverDatafield(name, duration.toMilliSecondsDouble());
+        };
+    }
+
     AzureKinectPointCloudProviderPropertyDefinitions::AzureKinectPointCloudProviderPropertyDefinitions(
         std::string prefix) :
         visionx::CapturingPointCloudProviderPropertyDefinitions(std::move(prefix))
@@ -268,6 +271,8 @@ namespace visionx
     {
         ARMARX_DEBUG << "Connecting " << getName();
 
+        setDebugObserverBatchModeEnabled(true);
+
         pointcloudTask = armarx::RunningTask<AzureKinectPointCloudProvider>::pointer_type(
                              new armarx::RunningTask<AzureKinectPointCloudProvider>(
                                  this,
@@ -456,7 +461,7 @@ namespace visionx
         using armarx::core::time::ScopedStopWatch;
         using armarx::core::time::StopWatch;
 
-        ScopedStopWatch sw_total{::createSwCallback(this, "doCapture")};
+        ScopedStopWatch sw_total{createSwCallback("doCapture")};
 
         k4a::capture capture;
         const std::chrono::milliseconds TIMEOUT{1000};
@@ -481,7 +486,7 @@ namespace visionx
 
         if (status)
         {
-            ::createSwCallback(this, "waiting for get_capture")(sw_get_capture.stop());
+            createSwCallback("waiting for get_capture")(sw_get_capture.stop());
 
             setMetaInfo("temperature", new armarx::Variant(capture.get_temperature_c()));
 
@@ -552,14 +557,14 @@ namespace visionx
             else
             {
                 // Convert the k4a image to an IVT image.
-                ScopedStopWatch sw{::createSwCallback(this, "convert k4a image to IVT")};
+                ScopedStopWatch sw{createSwCallback("convert k4a image to IVT")};
                 ::k4aToIvtImage(COLOR_IMAGE, *resultColorImage);
             }
 
 
             // Provide data for pointcloud processing thread and signal to start processing.
             {
-                ScopedStopWatch sw{::createSwCallback(this, "transform depth image to camera")};
+                ScopedStopWatch sw{createSwCallback("transform depth image to camera")};
                 // Acquire lock and write data needed by pointcloud thread (i.e.,
                 // alignedDepthImageScaled and depthImageReady).
                 {
@@ -577,7 +582,7 @@ namespace visionx
 
             // Prepare result depth image.
             {
-                ScopedStopWatch sw{::createSwCallback(this, "prepare result depth image")};
+                ScopedStopWatch sw{createSwCallback("prepare result depth image")};
 
                 const int DW = alignedDepthImage.get_width_pixels();
                 const int DH = alignedDepthImage.get_height_pixels();
@@ -606,11 +611,12 @@ namespace visionx
                         index_2 += 1;
                     }
                 }
+
             }
 
             // Broadcast RGB-D image.
             {
-                ScopedStopWatch sw{::createSwCallback(this, "broadcast RGB-D image")};
+                ScopedStopWatch sw{createSwCallback("broadcast RGB-D image")};
                 CByteImage* images[2] = {resultColorImage.get(), resultDepthImage.get()};
                 provideImages(images, imagesTime);
             }
@@ -624,12 +630,17 @@ namespace visionx
                 ARMARX_DEBUG << "Capturing thread received signal.";
             }
 
+           
+            sendDebugObserverBatch();
+
             return true;
         }
         else
         {
             ARMARX_INFO << deactivateSpam(30) << "Did not get frame until timeout of " << TIMEOUT;
 
+            sendDebugObserverBatch();
+
             return false;
         }
 
@@ -641,9 +652,11 @@ namespace visionx
         armarx::Metronome metronome(armarx::Frequency::Hertz(100));
         while(true)
         {
+
             k4abt::frame body_frame = bodyTracker.pop_result();
             if (body_frame != nullptr)
             {
+                armarx::core::time::ScopedStopWatch sw{createSwCallback("publish body tracking result")};
                 // see https://github.com/microsoft/Azure_Kinect_ROS_Driver/blob/melodic/src/k4a_ros_device.cpp
                 const armarx::DateTime timestamp = timestampToArmarX(body_frame.get_device_timestamp());                
 
@@ -651,6 +664,8 @@ namespace visionx
                 ARMARX_INFO << deactivateSpam(1) << num_bodies << " bodies are detected!";
 
                 std::vector<armarx::armem::human::HumanPose> humanPoses;
+                humanPoses.reserve(num_bodies);
+
                 for (std::uint32_t i = 0; i < num_bodies; i++)
                 {
                     k4abt_body_t body = body_frame.get_body(i);
@@ -658,7 +673,7 @@ namespace visionx
 
                     armarx::armem::human::HumanPose humanPose;
                     humanPose.humanTrackingId = std::to_string(body.id);
-                    humanPose.poseModelId = "K4ABT_BODY_32";
+                    humanPose.poseModelId = armarx::human::pose::model::k4a_bt_body_32::ModelId;
 
                     for (int i = 0; i < static_cast<int>(K4ABT_JOINT_COUNT); i++)
                     {
@@ -681,9 +696,10 @@ namespace visionx
                     humanPoses.push_back(humanPose);
                 }
 
+                setDebugObserverDatafield("bodyTrackingCaptureUntilPublish [ms]", (armarx::Clock::Now() - timestamp).toMilliSeconds());
+
                 humanPoseWriter->commitHumanPosesInCameraFrame(humanPoses, getName(), timestamp);
 
-                debugObserver
 
                 // k4a::image body_index_map = body_frame.get_body_index_map();
                 // if (body_index_map != nullptr)
@@ -966,9 +982,13 @@ namespace visionx
 
         {
             std::lock_guard g{deviceToRealtimeOffsetMtx};
+
+            setDebugObserverDatafield("device_to_realtime_offset [ms]", device_to_realtime_offset_.count() / 1'000'000.f); //  [ns] -> [ms]
+            setDebugObserverDatafield("clock_error", std::abs<float>((device_to_realtime_offset_ - device_to_realtime).count()) / 1'000.f); //  [ns] -> [µs]
+
             // If we're over a second off, just snap into place.
             if (device_to_realtime_offset_.count() == 0 ||
-                std::abs((device_to_realtime_offset_ - device_to_realtime).count()) > 1e7)
+                std::abs((device_to_realtime_offset_ - device_to_realtime).count()) > 1e7) // TODO is 1e7 correct?
             {
                 ARMARX_WARNING << "Initializing or re-initializing the device to realtime offset: " << device_to_realtime.count()
                                                                                                 << " ns";
@@ -978,9 +998,12 @@ namespace visionx
             {
                 // Low-pass filter!
                 constexpr double alpha = 0.10;
-                device_to_realtime_offset_ = device_to_realtime_offset_ +
-                                            std::chrono::nanoseconds(static_cast<int64_t>(
+
+                const std::chrono::nanoseconds timeCorrection(static_cast<int64_t>(
                                                 std::floor(alpha * (device_to_realtime - device_to_realtime_offset_).count())));
+                device_to_realtime_offset_ = device_to_realtime_offset_ + timeCorrection;
+
+                setDebugObserverDatafield("timeCorrection [µs]", timeCorrection.count() / 1'000); // [ns] -> [µs]
             }
         }
 

source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/AzureKinectPointCloudProvider.h

@@ -41,6 +41,7 @@
 #include <ArmarXCore/core/services/tasks/RunningTask.h>
 #include <ArmarXCore/core/time/TimeUtil.h>
 #include <ArmarXCore/interface/observers/ObserverInterface.h>
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
 
 // RobotAPI
 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
@@ -87,7 +88,8 @@ namespace visionx
     class AzureKinectPointCloudProvider :
         virtual public visionx::RGBDPointCloudProviderInterface,
         virtual public visionx::CapturingPointCloudProvider,
-        virtual public visionx::ImageProvider
+        virtual public visionx::ImageProvider,
+        virtual public armarx::DebugObserverComponentPluginUser
 #ifdef INCLUDE_BODY_TRACKING
         ,virtual public armarx::armem::ClientPluginUser 
 #endif
@@ -224,6 +226,9 @@ namespace visionx
             return s.str();
         }
 
+        std::function<void(armarx::Duration)>
+        createSwCallback(const std::string& description);
+
         void updateTimestampOffset(const std::chrono::microseconds& k4a_device_timestamp_us,
                                          const std::chrono::nanoseconds& k4a_system_timestamp_ns);
 

source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/CMakeLists.txt

@@ -5,7 +5,7 @@ find_package(k4abt 1.1 QUIET)
 message(STATUS "k4a found: ${k4a_FOUND} k4a version: ${k4a_VERSION}")
 armarx_build_if(k4a_FOUND "Azure Kinect SDK not available")
 
-set(COMPONENT_LIBS ArmarXCore ArmarXCoreObservers RobotAPIInterfaces VisionXInterfaces VisionXTools VisionXCore
+set(COMPONENT_LIBS ArmarXCore ArmarXCoreObservers RobotAPIInterfaces VisionXInterfaces VisionXTools VisionXCore ArmarXCoreComponentPlugins
     VisionXPointCloud ImageToPointCloud human armem_human
     k4a::k4a
 )