diff --git a/perception/multi_object_tracker/config/input_channels.param.yaml b/perception/multi_object_tracker/config/input_channels.param.yaml index 23c567d525508..63de901ef0c11 100644 --- a/perception/multi_object_tracker/config/input_channels.param.yaml +++ b/perception/multi_object_tracker/config/input_channels.param.yaml @@ -3,79 +3,79 @@ input_channels: detected_objects: topic: "/perception/object_recognition/detection/objects" - expected_frequency: 10.0 + expected_interval: 0.1 optional: name: "detected_objects" short_name: "all" # LIDAR - rule-based lidar_clustering: topic: "/perception/object_recognition/detection/clustering/objects" - expected_frequency: 10.0 + expected_interval: 0.1 optional: name: "clustering" short_name: "Lcl" # LIDAR - DNN lidar_centerpoint: topic: "/perception/object_recognition/detection/centerpoint/objects" - expected_frequency: 10.0 + expected_interval: 0.1 optional: name: "centerpoint" short_name: "Lcp" lidar_centerpoint_validated: topic: "/perception/object_recognition/detection/centerpoint/validation/objects" - expected_frequency: 10.0 + expected_interval: 0.1 optional: name: "centerpoint" short_name: "Lcp" lidar_apollo: topic: "/perception/object_recognition/detection/apollo/objects" - expected_frequency: 10.0 + expected_interval: 0.1 optional: name: "apollo" short_name: "Lap" lidar_apollo_validated: topic: "/perception/object_recognition/detection/apollo/validation/objects" - expected_frequency: 10.0 + expected_interval: 0.1 optional: name: "apollo" short_name: "Lap" # LIDAR-CAMERA - DNN lidar_pointpainitng: topic: "/perception/object_recognition/detection/pointpainting/objects" - expected_frequency: 10.0 + expected_interval: 0.1 optional: name: "pointpainting" short_name: "Lpp" lidar_pointpainting_validated: topic: "/perception/object_recognition/detection/pointpainting/validation/objects" - expected_frequency: 10.0 + expected_interval: 0.1 optional: name: "pointpainting" short_name: "Lpp" # CAMERA-LIDAR camera_lidar_fusion: topic: "/perception/object_recognition/detection/clustering/camera_lidar_fusion/objects" - expected_frequency: 10.0 + expected_interval: 0.1 optional: name: "camera_lidar_fusion" short_name: "CLf" # CAMERA-LIDAR+TRACKER detection_by_tracker: topic: "/perception/object_recognition/detection/detection_by_tracker/objects" - expected_frequency: 10.0 + expected_interval: 0.1 optional: name: "detection_by_tracker" short_name: "dbT" # RADAR radar: topic: "/sensing/radar/detected_objects" - expected_frequency: 30.0 + expected_interval: 0.0333 optional: name: "radar" short_name: "R" radar_far: topic: "/perception/object_recognition/detection/radar/far_objects" - expected_frequency: 30.0 + expected_interval: 0.0333 optional: name: "radar_far" short_name: "Rf" 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 cd9181e12d80a..ce2b2075c99a2 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 @@ -36,7 +36,7 @@ struct InputChannel std::string input_topic; // topic name of the detection, e.g. "/detection/lidar" std::string long_name = "Detected Object"; // full name of the detection std::string short_name = "DET"; // abbreviation of the name - double expected_rate = 10.0; // [Hz] + double expected_interval = 0.1; // [s] double expected_latency = 0.2; // [s] }; @@ -77,6 +77,11 @@ class InputStream interval_mean = interval_mean_; interval_var = interval_var_; } + void getLatencyStatistics(double & latency_mean, double & latency_var) const + { + latency_mean = latency_mean_; + latency_var = latency_var_; + } bool getTimestamps( rclcpp::Time & latest_measurement_time, rclcpp::Time & latest_message_time) const; rclcpp::Time getLatestMeasurementTime() const { return latest_measurement_time_; } @@ -95,7 +100,7 @@ class InputStream std::function func_trigger_; bool is_time_initialized_{false}; - double expected_rate_; + double expected_interval_; double latency_mean_; double latency_var_; double interval_mean_; @@ -133,8 +138,12 @@ class InputManager std::function func_trigger_; uint target_stream_idx_{0}; - double target_latency_{0.2}; - double target_latency_band_{0.14}; + double target_stream_latency_{0.2}; // [s] + double target_stream_latency_std_{0.04}; // [s] + double target_stream_interval_{0.1}; // [s] + double target_stream_interval_std_{0.02}; // [s] + double target_latency_{0.2}; // [s] + double target_latency_band_{1.0}; // [s] }; } // namespace multi_object_tracker 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 1de771b354745..8d8a10266fec7 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -92,7 +92,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) std::vector input_topic_names; std::vector input_names_long; std::vector input_names_short; - std::vector input_expected_rates; + std::vector input_expected_intervals; if (selected_input_channels.empty()) { RCLCPP_ERROR(this->get_logger(), "No input topics are specified."); @@ -106,10 +106,10 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) input_topic_names.push_back(input_topic_name); // required parameter, but can set a default value - const double default_expected_rate = 10.0; - const double expected_rate = declare_parameter( - "input_channels." + selected_input_channel + ".expected_rate", default_expected_rate); - input_expected_rates.push_back(expected_rate); + const double default_expected_interval = 0.1; // [s] + const double expected_interval = declare_parameter( + "input_channels." + selected_input_channel + ".expected_interval", default_expected_interval); + input_expected_intervals.push_back(expected_interval); // optional parameters const std::string default_name = selected_input_channel; @@ -130,7 +130,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) input_channels_[i].input_topic = input_topic_names[i]; input_channels_[i].long_name = input_names_long[i]; input_channels_[i].short_name = input_names_short[i]; - input_channels_[i].expected_rate = input_expected_rates[i]; + input_channels_[i].expected_interval = input_expected_intervals[i]; } // Initialize input manager diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index 25e8f46f19033..bf066e23828f8 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -31,15 +31,15 @@ void InputStream::init(const InputChannel & input_channel) input_topic_ = input_channel.input_topic; long_name_ = input_channel.long_name; short_name_ = input_channel.short_name; + expected_interval_ = input_channel.expected_interval; // [s] // Initialize queue objects_que_.clear(); // Initialize latency statistics - expected_rate_ = input_channel.expected_rate; // [Hz] latency_mean_ = input_channel.expected_latency; // [s] latency_var_ = 0.0; - interval_mean_ = 1 / expected_rate_; + interval_mean_ = expected_interval_; // [s] (initial value) interval_var_ = 0.0; latest_measurement_time_ = node_.now(); @@ -84,13 +84,11 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim // Calculate interval, Update interval statistics if (is_time_initialized_) { - bool is_interval_regular = true; const double interval = (now - latest_message_time_).seconds(); - // check if it is outlier - if (interval > 1.5 * interval_mean_ || interval < 0.5 * interval_mean_) { - // no update for the time statistics - is_interval_regular = false; - } + // Check if the interval is regular + // The interval is considered regular if it is within 0.5 and 1.5 times the expected interval + bool is_interval_regular = + interval > 0.5 * expected_interval_ && interval < 1.5 * expected_interval_; if (is_interval_regular) { interval_mean_ = (1.0 - gain) * interval_mean_ + gain * interval; @@ -189,17 +187,33 @@ void InputManager::getObjectTimeInterval( const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time) const { - object_latest_time = now - rclcpp::Duration::from_seconds(target_latency_); - object_oldest_time = now - rclcpp::Duration::from_seconds(target_latency_ + target_latency_band_); - - // try to include the latest message of the target stream + 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 + // 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 = input_streams_.at(target_stream_idx_)->getLatestMeasurementTime(); + // if the object_latest_time is newer than the next expected message time, set it older + // than the next expected message time + rclcpp::Time next_expected_message_time = + latest_measurement_time + + rclcpp::Duration::from_seconds( + target_stream_interval_ - + 1.0 * + target_stream_interval_std_); // next expected message time with 1 sigma safety margin + object_latest_time = object_latest_time > next_expected_message_time + ? next_expected_message_time + : object_latest_time; + + // if the object_latest_time is older than the latest measurement time, set it as the latest + // object time object_latest_time = - object_latest_time > latest_measurement_time ? object_latest_time : latest_measurement_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 = @@ -209,21 +223,24 @@ void InputManager::getObjectTimeInterval( void InputManager::optimizeTimings() { double max_latency_mean = 0.0; - uint stream_selected_idx = 0; - double selected_idx_latency_std = 0.0; - double selected_idx_interval = 0.0; + uint selected_stream_idx = 0; + double selected_stream_latency_std = 0.1; + double selected_stream_interval = 0.1; + double selected_stream_interval_std = 0.01; { // ANALYSIS: Get the streams statistics + // select the stream that has the maximum latency double latency_mean, latency_var, interval_mean, interval_var; for (const auto & input_stream : input_streams_) { if (!input_stream->isTimeInitialized()) continue; input_stream->getTimeStatistics(latency_mean, latency_var, interval_mean, interval_var); if (latency_mean > max_latency_mean) { max_latency_mean = latency_mean; - selected_idx_latency_std = std::sqrt(latency_var); - stream_selected_idx = input_stream->getIndex(); - selected_idx_interval = interval_mean; + selected_stream_idx = input_stream->getIndex(); + selected_stream_latency_std = std::sqrt(latency_var); + selected_stream_interval = interval_mean; + selected_stream_interval_std = std::sqrt(interval_var); } /* DEBUG */ @@ -244,15 +261,12 @@ void InputManager::optimizeTimings() // Set the target stream index, which has the maximum latency // trigger will be called next time - target_stream_idx_ = stream_selected_idx; - target_latency_ = max_latency_mean - selected_idx_latency_std; - target_latency_band_ = 2 * selected_idx_latency_std + selected_idx_interval; - - /* DEBUG */ - RCLCPP_INFO( - node_.get_logger(), "InputManager::getObjects Target stream: %s, target latency: %f, band: %f", - input_streams_.at(target_stream_idx_)->getLongName().c_str(), target_latency_, - target_latency_band_); + // if no stream is initialized, the target stream index will be 0 and wait for the initialization + target_stream_idx_ = selected_stream_idx; + target_stream_latency_ = max_latency_mean; + target_stream_latency_std_ = selected_stream_latency_std; + target_stream_interval_ = selected_stream_interval; + target_stream_interval_std_ = selected_stream_interval_std; } bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_list)