From 07536f2aeb13543f9429df2581510914e0116099 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 27 May 2024 11:29:06 +0900 Subject: [PATCH 1/3] fix: rename to latest_exported_object_time_ Signed-off-by: Taekjin LEE --- .../processor/input_manager.hpp | 2 +- .../src/processor/input_manager.cpp | 24 ++++++++++--------- 2 files changed, 14 insertions(+), 12 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp index ad8c4ae6e5e24..6782da0c72dee 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp @@ -138,7 +138,7 @@ class InputManager std::vector::SharedPtr> sub_objects_array_{}; bool is_initialized_{false}; - rclcpp::Time latest_object_time_; + rclcpp::Time latest_exported_object_time_; size_t input_size_; std::vector> input_streams_; diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index e29b54e971491..61e6e43aefe3f 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -176,7 +176,7 @@ void InputStream::getObjectsOlderThan( //////////////////////////// InputManager::InputManager(rclcpp::Node & node) : node_(node) { - latest_object_time_ = node_.now(); + latest_exported_object_time_ = node_.now() - rclcpp::Duration::from_seconds(3.0); } void InputManager::init(const std::vector & input_channels) @@ -239,7 +239,7 @@ void InputManager::getObjectTimeInterval( rclcpp::Time latest_measurement_time = input_streams_.at(target_stream_idx_)->getLatestMeasurementTime(); - // if the object_latest_time is older than the latest measurement time, set it as the latest + // if the object_latest_time is older than the latest measurement time, set it to the latest // object time object_latest_time = object_latest_time < latest_measurement_time ? latest_measurement_time : object_latest_time; @@ -248,8 +248,9 @@ void InputManager::getObjectTimeInterval( object_oldest_time = object_latest_time - rclcpp::Duration::from_seconds(1.0); // if the object_oldest_time is older than the latest object time, set it to the latest object // time - object_oldest_time = - object_oldest_time > latest_object_time_ ? object_oldest_time : latest_object_time_; + object_oldest_time = object_oldest_time < latest_exported_object_time_ + ? latest_exported_object_time_ + : object_oldest_time; } void InputManager::optimizeTimings() @@ -320,19 +321,20 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li 0; }); - // Update the latest object time + // Update the latest exported object time bool is_any_object = !objects_list.empty(); if (is_any_object) { - latest_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp); + latest_exported_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp); } else { // check time jump - if (now < latest_object_time_) { + if (now < latest_exported_object_time_) { RCLCPP_WARN( node_.get_logger(), - "InputManager::getObjects Time jump detected, now: %f, latest_object_time_: %f", - now.seconds(), latest_object_time_.seconds()); - latest_object_time_ = - now - rclcpp::Duration::from_seconds(3.0); // reset the latest object time to 3 seconds ago + "InputManager::getObjects Time jump detected, now: %f, latest_exported_object_time_: %f", + now.seconds(), latest_exported_object_time_.seconds()); + latest_exported_object_time_ = + now - rclcpp::Duration::from_seconds( + 3.0); // reset the latest exported object time to 3 seconds ago } else { RCLCPP_DEBUG( node_.get_logger(), From 39ac546378a105188c02925ce30b5a594803ce9d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 27 May 2024 11:29:55 +0900 Subject: [PATCH 2/3] fix: add time range checks Signed-off-by: Taekjin LEE --- .../src/processor/input_manager.cpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index 61e6e43aefe3f..2be8b21717e8f 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -150,7 +150,14 @@ void InputStream::getObjectsOlderThan( const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, ObjectsList & objects_list) { - assert(object_latest_time.nanoseconds() > object_oldest_time.nanoseconds()); + if (object_latest_time < object_oldest_time) { + RCLCPP_WARN( + node_.get_logger(), + "InputManager::getObjectsOlderThan %s: Invalid object time interval, object_latest_time: %f, " + "object_oldest_time: %f", + long_name_.c_str(), object_latest_time.seconds(), object_oldest_time.seconds()); + return; + } for (const auto & object : objects_que_) { const rclcpp::Time object_time = rclcpp::Time(object.header.stamp); @@ -251,6 +258,16 @@ void InputManager::getObjectTimeInterval( object_oldest_time = object_oldest_time < latest_exported_object_time_ ? latest_exported_object_time_ : object_oldest_time; + + // check the object time interval is valid + if (object_oldest_time > object_latest_time) { + RCLCPP_WARN( + node_.get_logger(), + "InputManager::getObjectTimeInterval Invalid object time interval, object_latest_time: %f, " + "object_oldest_time: %f", + (now - object_latest_time).seconds(), (now - object_oldest_time).seconds()); + object_oldest_time = object_latest_time - rclcpp::Duration::from_seconds(1.0); + } } void InputManager::optimizeTimings() From 94bdca4c80df5c2a84babcf4795ba530e7322245 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 27 May 2024 13:16:20 +0900 Subject: [PATCH 3/3] chore: add comments, refactoring Signed-off-by: Taekjin LEE --- .../processor/input_manager.hpp | 14 +++-- .../src/processor/input_manager.cpp | 63 ++++++++++--------- 2 files changed, 42 insertions(+), 35 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp index 6782da0c72dee..f46c088c6d508 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp @@ -117,17 +117,13 @@ class InputManager { public: explicit InputManager(rclcpp::Node & node); - void init(const std::vector & input_channels); - void setTriggerFunction(std::function func_trigger) { func_trigger_ = func_trigger; } + void setTriggerFunction(std::function func_trigger) { func_trigger_ = func_trigger; } void onTrigger(const uint & index) const; - void getObjectTimeInterval( - const rclcpp::Time & now, rclcpp::Time & object_latest_time, - rclcpp::Time & object_oldest_time) const; - void optimizeTimings(); bool getObjects(const rclcpp::Time & now, ObjectsList & objects_list); + bool isChannelSpawnEnabled(const uint & index) const { return input_streams_[index]->isSpawnEnabled(); @@ -151,6 +147,12 @@ class InputManager double target_stream_interval_std_{0.02}; // [s] double target_latency_{0.2}; // [s] double target_latency_band_{1.0}; // [s] + +private: + void getObjectTimeInterval( + const rclcpp::Time & now, rclcpp::Time & object_latest_time, + rclcpp::Time & object_oldest_time) const; + void optimizeTimings(); }; } // namespace multi_object_tracker diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index 2be8b21717e8f..44bdf09046ccd 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -127,15 +127,16 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim // Update time latest_message_time_ = now; constexpr double delay_threshold = 3.0; // [s] - if (std::abs((latest_measurement_time_ - objects_time).seconds()) > delay_threshold) { - // Reset the latest measurement time if the time difference is too large + if (objects_time < latest_measurement_time_ - rclcpp::Duration::from_seconds(delay_threshold)) { + // If the given object time is older than the latest measurement time by more than the + // threshold, the system time may have been reset. Reset the latest measurement time latest_measurement_time_ = objects_time; RCLCPP_WARN( node_.get_logger(), "InputManager::updateTimingStatus %s: Resetting the latest measurement time to %f", long_name_.c_str(), objects_time.seconds()); } else { - // Aware reversed message arrival + // Update only if the object time is newer than the latest measurement time latest_measurement_time_ = latest_measurement_time_ < objects_time ? objects_time : latest_measurement_time_; } @@ -237,36 +238,36 @@ void InputManager::getObjectTimeInterval( const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time) const { + // Set the object time interval + + // 1. object_latest_time + // The object_latest_time is the current time minus the target stream latency object_latest_time = - now - rclcpp::Duration::from_seconds( - target_stream_latency_ - - 0.1 * target_stream_latency_std_); // object_latest_time with 0.1 sigma safety margin + now - rclcpp::Duration::from_seconds(target_stream_latency_ - 0.1 * target_stream_latency_std_); + // check the target stream can be included in the object time interval if (input_streams_.at(target_stream_idx_)->isTimeInitialized()) { - rclcpp::Time latest_measurement_time = + const rclcpp::Time latest_measurement_time = input_streams_.at(target_stream_idx_)->getLatestMeasurementTime(); - // if the object_latest_time is older than the latest measurement time, set it to the latest // object time object_latest_time = object_latest_time < latest_measurement_time ? latest_measurement_time : object_latest_time; } - object_oldest_time = object_latest_time - rclcpp::Duration::from_seconds(1.0); - // if the object_oldest_time is older than the latest object time, set it to the latest object - // time - object_oldest_time = object_oldest_time < latest_exported_object_time_ - ? latest_exported_object_time_ - : object_oldest_time; - - // check the object time interval is valid - if (object_oldest_time > object_latest_time) { - RCLCPP_WARN( - node_.get_logger(), - "InputManager::getObjectTimeInterval Invalid object time interval, object_latest_time: %f, " - "object_oldest_time: %f", - (now - object_latest_time).seconds(), (now - object_oldest_time).seconds()); - object_oldest_time = object_latest_time - rclcpp::Duration::from_seconds(1.0); + // 2. object_oldest_time + // The default object_oldest_time is to have a 1-second time interval + const rclcpp::Time object_oldest_time_default = + object_latest_time - rclcpp::Duration::from_seconds(1.0); + if (latest_exported_object_time_ < object_oldest_time_default) { + // if the latest exported object time is too old, set to the default + object_oldest_time = object_oldest_time_default; + } else if (latest_exported_object_time_ > object_latest_time) { + // if the latest exported object time is newer than the object_latest_time, set to the default + object_oldest_time = object_oldest_time_default; + } else { + // The object_oldest_time is the latest exported object time + object_oldest_time = latest_exported_object_time_; } } @@ -316,7 +317,8 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li objects_list.clear(); // Get the time interval for the objects - rclcpp::Time object_latest_time, object_oldest_time; + rclcpp::Time object_latest_time; + rclcpp::Time object_oldest_time; getObjectTimeInterval(now, object_latest_time, object_oldest_time); // Optimize the target stream, latency, and its band @@ -343,16 +345,19 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li if (is_any_object) { latest_exported_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp); } else { - // check time jump + // check time jump back if (now < latest_exported_object_time_) { RCLCPP_WARN( node_.get_logger(), - "InputManager::getObjects Time jump detected, now: %f, latest_exported_object_time_: %f", + "InputManager::getObjects Detected jump back in time, now: %f, " + "latest_exported_object_time_: %f", now.seconds(), latest_exported_object_time_.seconds()); - latest_exported_object_time_ = - now - rclcpp::Duration::from_seconds( - 3.0); // reset the latest exported object time to 3 seconds ago + // reset the latest exported object time to 3 seconds ago, + const rclcpp::Time latest_exported_object_time_default = + now - rclcpp::Duration::from_seconds(3.0); + latest_exported_object_time_ = latest_exported_object_time_default; } else { + // No objects in the object list, no update for the latest exported object time RCLCPP_DEBUG( node_.get_logger(), "InputManager::getObjects No objects in the object list, object time band from %f to %f",