From 01737c11293187ce3d80d811445a282bb12246e7 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 9 Apr 2024 18:35:01 +0900 Subject: [PATCH 1/3] fix(multi_object_tracker): debugger timers checks if the timer is initialized (#6757) fix: debugger timers checks if the timer is initialized Signed-off-by: Taekjin LEE --- .../include/multi_object_tracker/debugger.hpp | 6 ++++ .../multi_object_tracker/src/debugger.cpp | 29 ++++++++++++++++--- .../src/multi_object_tracker_core.cpp | 5 ++++ 3 files changed, 36 insertions(+), 4 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp index ca1b20d26c25d..858d43dcfc5bd 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp @@ -41,7 +41,10 @@ class TrackerDebugger const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const; void startMeasurementTime( const rclcpp::Time & now, const rclcpp::Time & measurement_header_stamp); + void endMeasurementTime(const rclcpp::Time & now); + void startPublishTime(const rclcpp::Time & now); void endPublishTime(const rclcpp::Time & now, const rclcpp::Time & object_time); + void setupDiagnostics(); void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat); struct DEBUG_SETTINGS @@ -56,12 +59,15 @@ class TrackerDebugger private: void loadParameters(); + bool is_initialized_ = false; rclcpp::Node & node_; rclcpp::Publisher::SharedPtr debug_tentative_objects_pub_; std::unique_ptr processing_time_publisher_; rclcpp::Time last_input_stamp_; rclcpp::Time stamp_process_start_; + rclcpp::Time stamp_process_end_; + rclcpp::Time stamp_publish_start_; rclcpp::Time stamp_publish_output_; }; diff --git a/perception/multi_object_tracker/src/debugger.cpp b/perception/multi_object_tracker/src/debugger.cpp index e3fc58dd5d692..4304afbe2e055 100644 --- a/perception/multi_object_tracker/src/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger.cpp @@ -108,16 +108,35 @@ void TrackerDebugger::startMeasurementTime( processing_time_publisher_->publish( "debug/input_latency_ms", input_latency_ms); } + // initialize debug time stamps + if (!is_initialized_) { + stamp_publish_output_ = now; + is_initialized_ = true; + } +} + +void TrackerDebugger::endMeasurementTime(const rclcpp::Time & now) +{ + stamp_process_end_ = now; +} + +void TrackerDebugger::startPublishTime(const rclcpp::Time & now) +{ + stamp_publish_start_ = now; } void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Time & object_time) { const auto current_time = now; - // pipeline latency: time from sensor measurement to publish + // pipeline latency: time from sensor measurement to publish, used for 'checkDelay' pipeline_latency_ms_ = (current_time - last_input_stamp_).seconds() * 1e3; - if (debug_settings_.publish_processing_time) { - // processing time: time between input and publish - double processing_time_ms = (current_time - stamp_process_start_).seconds() * 1e3; + if (debug_settings_.publish_processing_time && is_initialized_) { + // processing latency: time between input and publish + double processing_latency_ms = ((current_time - stamp_process_start_).seconds()) * 1e3; + // processing time: only the time spent in the tracking processes + double processing_time_ms = ((current_time - stamp_publish_start_).seconds() + + (stamp_process_end_ - stamp_process_start_).seconds()) * + 1e3; // cycle time: time between two consecutive publish double cyclic_time_ms = (current_time - stamp_publish_output_).seconds() * 1e3; // measurement to tracked-object time: time from the sensor measurement to the publishing object @@ -131,6 +150,8 @@ void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Tim "debug/cyclic_time_ms", cyclic_time_ms); processing_time_publisher_->publish( "debug/processing_time_ms", processing_time_ms); + processing_time_publisher_->publish( + "debug/processing_latency_ms", processing_latency_ms); processing_time_publisher_->publish( "debug/meas_to_tracked_object_ms", measurement_to_object_ms); } 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 b8e19d4ca2df4..aacd37766e8b0 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -190,6 +190,10 @@ void MultiObjectTracker::onMeasurement( if (tracker) list_tracker_.push_back(tracker); } + // debugger time + debugger_->endMeasurementTime(this->now()); + + // Publish objects if the timer is not enabled if (publish_timer_ == nullptr) { publish(measurement_time); } @@ -337,6 +341,7 @@ inline bool MultiObjectTracker::shouldTrackerPublish( void MultiObjectTracker::publish(const rclcpp::Time & time) const { + debugger_->startPublishTime(this->now()); const auto subscriber_count = tracked_objects_pub_->get_subscription_count() + tracked_objects_pub_->get_intra_process_subscription_count(); if (subscriber_count < 1) { From ef80be4390abb4897de52b8a7a2531256a758be7 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 9 Apr 2024 15:24:21 +0900 Subject: [PATCH 2/3] fix(multi_object_tracker): mot timer bug fix (#6775) * fix: set object timestamp type to follow node time fix: remove comment Signed-off-by: Taekjin LEE * fix: add initialization checkers Signed-off-by: Taekjin LEE * fix: timestamp initialization Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../multi_object_tracker/src/debugger.cpp | 40 ++++++++++++------- .../src/multi_object_tracker_core.cpp | 11 +++-- 2 files changed, 32 insertions(+), 19 deletions(-) diff --git a/perception/multi_object_tracker/src/debugger.cpp b/perception/multi_object_tracker/src/debugger.cpp index 4304afbe2e055..b5632a13e78fc 100644 --- a/perception/multi_object_tracker/src/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger.cpp @@ -18,12 +18,7 @@ #include -TrackerDebugger::TrackerDebugger(rclcpp::Node & node) -: diagnostic_updater_(&node), - node_(node), - last_input_stamp_(node.now()), - stamp_process_start_(node.now()), - stamp_publish_output_(node.now()) +TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : diagnostic_updater_(&node), node_(node) { // declare debug parameters to decide whether to publish debug topics loadParameters(); @@ -39,7 +34,15 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node) "debug/tentative_objects", rclcpp::QoS{1}); } - // initialize stop watch and diagnostics + // initialize timestamps + const rclcpp::Time now = node_.now(); + last_input_stamp_ = now; + stamp_process_start_ = now; + stamp_process_end_ = now; + stamp_publish_start_ = now; + stamp_publish_output_ = now; + + // setup diagnostics setupDiagnostics(); } @@ -73,7 +76,11 @@ void TrackerDebugger::loadParameters() void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat) { - const double delay = pipeline_latency_ms_; // [s] + if (!is_initialized_) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Measurement time is not set."); + return; + } + const double & delay = pipeline_latency_ms_; // [s] if (delay == 0.0) { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Detection delay is not calculated."); @@ -127,18 +134,21 @@ void TrackerDebugger::startPublishTime(const rclcpp::Time & now) void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Time & object_time) { - const auto current_time = now; + // if the measurement time is not set, do not publish debug information + if (!is_initialized_) return; + // pipeline latency: time from sensor measurement to publish, used for 'checkDelay' - pipeline_latency_ms_ = (current_time - last_input_stamp_).seconds() * 1e3; - if (debug_settings_.publish_processing_time && is_initialized_) { + pipeline_latency_ms_ = (now - last_input_stamp_).seconds() * 1e3; + + if (debug_settings_.publish_processing_time) { // processing latency: time between input and publish - double processing_latency_ms = ((current_time - stamp_process_start_).seconds()) * 1e3; + double processing_latency_ms = ((now - stamp_process_start_).seconds()) * 1e3; // processing time: only the time spent in the tracking processes - double processing_time_ms = ((current_time - stamp_publish_start_).seconds() + + double processing_time_ms = ((now - stamp_publish_start_).seconds() + (stamp_process_end_ - stamp_process_start_).seconds()) * 1e3; // cycle time: time between two consecutive publish - double cyclic_time_ms = (current_time - stamp_publish_output_).seconds() * 1e3; + double cyclic_time_ms = (now - stamp_publish_output_).seconds() * 1e3; // measurement to tracked-object time: time from the sensor measurement to the publishing object // time If there is not latency compensation, this value is zero double measurement_to_object_ms = (object_time - last_input_stamp_).seconds() * 1e3; @@ -155,5 +165,5 @@ void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Tim processing_time_publisher_->publish( "debug/meas_to_tracked_object_ms", measurement_to_object_ms); } - stamp_publish_output_ = current_time; + stamp_publish_output_ = now; } 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 aacd37766e8b0..8ff69ffaeaa9a 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -135,10 +135,14 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) void MultiObjectTracker::onMeasurement( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg) { + // Get the time of the measurement + const rclcpp::Time measurement_time = + rclcpp::Time(input_objects_msg->header.stamp, this->now().get_clock_type()); + /* keep the latest input stamp and check transform*/ - debugger_->startMeasurementTime(this->now(), rclcpp::Time(input_objects_msg->header.stamp)); - const auto self_transform = getTransformAnonymous( - tf_buffer_, "base_link", world_frame_id_, input_objects_msg->header.stamp); + debugger_->startMeasurementTime(this->now(), measurement_time); + const auto self_transform = + getTransformAnonymous(tf_buffer_, "base_link", world_frame_id_, measurement_time); if (!self_transform) { return; } @@ -150,7 +154,6 @@ void MultiObjectTracker::onMeasurement( return; } /* tracker prediction */ - const rclcpp::Time measurement_time = input_objects_msg->header.stamp; for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { (*itr)->predict(measurement_time); } From ddc1ec16e2e8416d6c26a9d18a09dbe2b448f009 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 9 Apr 2024 16:15:15 +0900 Subject: [PATCH 3/3] feat(multi_object_tracker): reduce publish delay (#6710) * feat: implement a new publish timer Signed-off-by: Taekjin LEE * chore: add comments for new variables Signed-off-by: Taekjin LEE * fix: variable name was wrong Signed-off-by: Taekjin LEE fix: reduce lower limit of publish interval Signed-off-by: Taekjin LEE fix: enlarge publish range upper limit Signed-off-by: Taekjin LEE fix: set parameter tested and agreed Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../multi_object_tracker_core.hpp | 10 ++-- .../src/multi_object_tracker_core.cpp | 48 +++++++++++++------ 2 files changed, 41 insertions(+), 17 deletions(-) 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 94cc7185bd518..fe41f97cb5a77 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 @@ -58,7 +58,11 @@ class MultiObjectTracker : public rclcpp::Node tracked_objects_pub_; rclcpp::Subscription::SharedPtr detected_object_sub_; - rclcpp::TimerBase::SharedPtr publish_timer_; // publish timer + + // publish timer + rclcpp::TimerBase::SharedPtr publish_timer_; + rclcpp::Time last_published_time_; + double publisher_period_; // debugger class std::unique_ptr debugger_; @@ -79,14 +83,14 @@ class MultiObjectTracker : public rclcpp::Node std::unique_ptr data_association_; void checkTrackerLifeCycle( - std::list> & list_tracker, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform); + std::list> & list_tracker, const rclcpp::Time & time); void sanitizeTracker( std::list> & list_tracker, const rclcpp::Time & time); std::shared_ptr createNewTracker( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) const; + void checkAndPublish(const rclcpp::Time & time); void publish(const rclcpp::Time & time) const; 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 8ff69ffaeaa9a..785038fb513c9 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -68,6 +68,7 @@ boost::optional getTransformAnonymous( MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) : rclcpp::Node("multi_object_tracker", node_options), + last_published_time_(this->now()), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { @@ -83,7 +84,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) create_publisher("output", rclcpp::QoS{1}); // Parameters - double publish_rate = declare_parameter("publish_rate"); + double publish_rate = declare_parameter("publish_rate"); // [hz] world_frame_id_ = declare_parameter("world_frame_id"); bool enable_delay_compensation{declare_parameter("enable_delay_compensation")}; @@ -94,11 +95,15 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) this->get_node_base_interface(), this->get_node_timers_interface()); tf_buffer_.setCreateTimerInterface(cti); - // Create ROS time based timer + // Create ROS time based timer. + // If the delay compensation is enabled, the timer is used to publish the output at the correct + // time. if (enable_delay_compensation) { - const auto period_ns = rclcpp::Rate(publish_rate).period(); + publisher_period_ = 1.0 / publish_rate; // [s] + constexpr double timer_multiplier = 20.0; // 20 times frequent for publish timing check + const auto timer_period = rclcpp::Rate(publish_rate * timer_multiplier).period(); publish_timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&MultiObjectTracker::onTimer, this)); + this, get_clock(), timer_period, std::bind(&MultiObjectTracker::onTimer, this)); } const auto tmp = this->declare_parameter>("can_assign_matrix"); @@ -179,7 +184,7 @@ void MultiObjectTracker::onMeasurement( } /* life cycle check */ - checkTrackerLifeCycle(list_tracker_, measurement_time, *self_transform); + checkTrackerLifeCycle(list_tracker_, measurement_time); /* sanitize trackers */ sanitizeTracker(list_tracker_, measurement_time); @@ -198,7 +203,14 @@ void MultiObjectTracker::onMeasurement( // Publish objects if the timer is not enabled if (publish_timer_ == nullptr) { + // Publish if the delay compensation is disabled publish(measurement_time); + } else { + // Publish if the next publish time is close + const double minimum_publish_interval = publisher_period_ * 0.70; // 70% of the period + if ((this->now() - last_published_time_).seconds() > minimum_publish_interval) { + checkAndPublish(this->now()); + } } } @@ -232,24 +244,32 @@ std::shared_ptr MultiObjectTracker::createNewTracker( void MultiObjectTracker::onTimer() { const rclcpp::Time current_time = this->now(); - const auto self_transform = - getTransformAnonymous(tf_buffer_, world_frame_id_, "base_link", current_time); - if (!self_transform) { - return; + // check the publish period + const auto elapsed_time = (current_time - last_published_time_).seconds(); + // if the elapsed time is over the period, publish objects with prediction + constexpr double maximum_latency_ratio = 1.11; // 11% margin + const double maximum_publish_latency = publisher_period_ * maximum_latency_ratio; + if (elapsed_time > maximum_publish_latency) { + checkAndPublish(current_time); } +} +void MultiObjectTracker::checkAndPublish(const rclcpp::Time & time) +{ /* life cycle check */ - checkTrackerLifeCycle(list_tracker_, current_time, *self_transform); + checkTrackerLifeCycle(list_tracker_, time); /* sanitize trackers */ - sanitizeTracker(list_tracker_, current_time); + sanitizeTracker(list_tracker_, time); // Publish - publish(current_time); + publish(time); + + // Update last published time + last_published_time_ = this->now(); } void MultiObjectTracker::checkTrackerLifeCycle( - std::list> & list_tracker, const rclcpp::Time & time, - [[maybe_unused]] const geometry_msgs::msg::Transform & self_transform) + std::list> & list_tracker, const rclcpp::Time & time) { /* params */ constexpr float max_elapsed_time = 1.0;