From 210a908e07efbc8156972037b4b972c234f5dd39 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Tue, 12 Dec 2023 17:13:16 +0900 Subject: [PATCH] feat(multi_object_tracker): enable to output perception topic delay within diagnostics (#5840) * enable debugger in tracker Signed-off-by: yoshiri * fix uninitialized class variable Signed-off-by: yoshiri * feat: separate debugger class for simplification Signed-off-by: yoshiri * enable check topic delay by diagnostics Signed-off-by: yoshiri * fix: update diagnostics periods and threshold Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- .../multi_object_tracker_node.param.yaml | 2 + .../multi_object_tracker_core.hpp | 14 ++++-- .../src/multi_object_tracker_core.cpp | 47 ++++++++++++++++--- 3 files changed, 54 insertions(+), 9 deletions(-) diff --git a/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml b/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml index 2109aa47fcd3d..3d30add05392a 100644 --- a/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml +++ b/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml @@ -17,3 +17,5 @@ # debug parameters publish_processing_time: false publish_tentative_objects: false + diagnostic_warn_delay: 0.5 # [sec] + diagnostic_error_delay: 1.0 # [sec] diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index 6fd1de51adf99..95d33b78ff184 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -40,6 +40,9 @@ #include #endif +#include +#include + #include #include @@ -57,17 +60,22 @@ class TrackerDebugger { public: explicit TrackerDebugger(rclcpp::Node & node); - void publishProcessingTime() const; + void publishProcessingTime(); void publishTentativeObjects( const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const; void startStopWatch(); void startMeasurementTime(const rclcpp::Time & measurement_header_stamp); - + void setupDiagnostics(); + void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat); struct DEBUG_SETTINGS { bool publish_processing_time; bool publish_tentative_objects; + double diagnostics_warn_delay; + double diagnostics_error_delay; } debug_settings_; + double elapsed_time_from_sensor_input_ = 0.0; + diagnostic_updater::Updater diagnostic_updater_; private: void loadParameters(); @@ -116,7 +124,7 @@ class MultiObjectTracker : public rclcpp::Node const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) const; - void publish(const rclcpp::Time & time) const; + void publish(const rclcpp::Time & time); inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; }; diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index 80f270a97fa62..d28833241bd5f 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -65,7 +65,8 @@ boost::optional getTransformAnonymous( } // namespace -TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : node_(node), last_input_stamp_(node.now()) +TrackerDebugger::TrackerDebugger(rclcpp::Node & node) +: diagnostic_updater_(&node), node_(node), last_input_stamp_(node.now()) { // declare debug parameters to decide whether to publish debug topics loadParameters(); @@ -82,8 +83,17 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : node_(node), last_input_ "debug/tentative_objects", rclcpp::QoS{1}); } - // initialize stop watch + // initialize stop watch and diagnostics startStopWatch(); + setupDiagnostics(); +} + +void TrackerDebugger::setupDiagnostics() +{ + diagnostic_updater_.setHardwareID(node_.get_name()); + diagnostic_updater_.add( + "Perception delay check from original header stamp", this, &TrackerDebugger::checkDelay); + diagnostic_updater_.setPeriod(0.1); } void TrackerDebugger::loadParameters() @@ -93,26 +103,51 @@ void TrackerDebugger::loadParameters() node_.declare_parameter("publish_processing_time"); debug_settings_.publish_tentative_objects = node_.declare_parameter("publish_tentative_objects"); + debug_settings_.diagnostics_warn_delay = + node_.declare_parameter("diagnostics_warn_delay"); + debug_settings_.diagnostics_error_delay = + node_.declare_parameter("diagnostics_error_delay"); } catch (const std::exception & e) { RCLCPP_WARN(node_.get_logger(), "Failed to declare parameter: %s", e.what()); debug_settings_.publish_processing_time = false; debug_settings_.publish_tentative_objects = false; + debug_settings_.diagnostics_warn_delay = 0.5; + debug_settings_.diagnostics_error_delay = 1.0; + } +} + +void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + const double delay = elapsed_time_from_sensor_input_; // [s] + + if (delay == 0.0) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Detection delay is not calculated."); + } else if (delay < debug_settings_.diagnostics_warn_delay) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Detection delay is acceptable"); + } else if (delay < debug_settings_.diagnostics_error_delay) { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "Detection delay is over warn threshold."); + } else { + stat.summary( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Detection delay is over error threshold."); } + + stat.add("Detection delay", delay); } -void TrackerDebugger::publishProcessingTime() const +void TrackerDebugger::publishProcessingTime() { const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const auto current_time = node_.now(); - const double elapsed_time_from_sensor_input = (current_time - last_input_stamp_).nanoseconds(); + elapsed_time_from_sensor_input_ = (current_time - last_input_stamp_).seconds(); if (debug_settings_.publish_processing_time) { processing_time_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); processing_time_publisher_->publish( "debug/processing_time_ms", processing_time_ms); processing_time_publisher_->publish( - "debug/elapsed_time_from_sensor_input_ms", elapsed_time_from_sensor_input * 1e-6); + "debug/elapsed_time_from_sensor_input_ms", elapsed_time_from_sensor_input_ * 1e3); } } @@ -401,7 +436,7 @@ inline bool MultiObjectTracker::shouldTrackerPublish( return true; } -void MultiObjectTracker::publish(const rclcpp::Time & time) const +void MultiObjectTracker::publish(const rclcpp::Time & time) { const auto subscriber_count = tracked_objects_pub_->get_subscription_count() + tracked_objects_pub_->get_intra_process_subscription_count();