diff --git a/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/AzureKinectPointCloudProvider.cpp b/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/AzureKinectPointCloudProvider.cpp index 77d890b17a87865e0407b5d9851a83219050de0c..83036b8e9cb7c34b9b1c2ab7a371730cda20594d 100644 --- a/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/AzureKinectPointCloudProvider.cpp +++ b/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] } } diff --git a/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/AzureKinectPointCloudProvider.h b/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/AzureKinectPointCloudProvider.h index bcf1f2d27aee817584b3f872798ff93a10e1cff4..9e76d05a0eb2e1e621f1bff395cfd441c27c8343 100644 --- a/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/AzureKinectPointCloudProvider.h +++ b/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); diff --git a/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/CMakeLists.txt b/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/CMakeLists.txt index 5f3faa4bec2b9cf821e806e48cda30e79425aafd..d67795f295762a5a0f384e852c3c15d7f4ed6b24 100644 --- a/source/VisionX/components/pointcloud_provider/AzureKinectPointCloudProvider/CMakeLists.txt +++ b/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 )