From 5412a55041365af139783349692c9e738e5c83ff Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 11 Apr 2024 17:03:16 +0900 Subject: [PATCH 01/67] refactor: frequently used types, namespace Signed-off-by: Taekjin LEE --- .../multi_object_tracker_core.hpp | 18 +++++++++----- .../src/multi_object_tracker_core.cpp | 24 ++++++++++--------- .../src/multi_object_tracker_node.cpp | 2 +- 3 files changed, 26 insertions(+), 18 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 f788dd3895216..483eb38bf2697 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 @@ -50,6 +50,13 @@ #include #include +namespace multi_object_tracker +{ + +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjects; +using autoware_auto_perception_msgs::msg::TrackedObjects; + class MultiObjectTracker : public rclcpp::Node { public: @@ -57,10 +64,8 @@ class MultiObjectTracker : public rclcpp::Node private: // ROS interface - rclcpp::Publisher::SharedPtr - tracked_objects_pub_; - rclcpp::Subscription::SharedPtr - detected_object_sub_; + rclcpp::Publisher::SharedPtr tracked_objects_pub_; + rclcpp::Subscription::SharedPtr detected_object_sub_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -79,8 +84,7 @@ class MultiObjectTracker : public rclcpp::Node std::unique_ptr processor_; // callback functions - void onMeasurement( - const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg); + void onMeasurement(const DetectedObjects::ConstSharedPtr input_objects_msg); void onTimer(); // publish processes @@ -89,4 +93,6 @@ class MultiObjectTracker : public rclcpp::Node inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; }; +} // namespace multi_object_tracker + #endif // MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_ 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 d80a21813b28b..6f343dd5de8fb 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -36,8 +36,6 @@ #include #include -using Label = autoware_auto_perception_msgs::msg::ObjectClassification; - namespace { // Function to get the transform between two frames @@ -67,6 +65,10 @@ boost::optional getTransformAnonymous( } // namespace +namespace multi_object_tracker +{ +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) : rclcpp::Node("multi_object_tracker", node_options), tf_buffer_(this->get_clock()), @@ -74,11 +76,10 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) last_published_time_(this->now()) { // Create publishers and subscribers - detected_object_sub_ = create_subscription( + detected_object_sub_ = create_subscription( "input", rclcpp::QoS{1}, std::bind(&MultiObjectTracker::onMeasurement, this, std::placeholders::_1)); - tracked_objects_pub_ = - create_publisher("output", rclcpp::QoS{1}); + tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); // Get parameters double publish_rate = declare_parameter("publish_rate"); // [hz] @@ -142,8 +143,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) published_time_publisher_ = std::make_unique(this); } -void MultiObjectTracker::onMeasurement( - const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg) +void MultiObjectTracker::onMeasurement(const DetectedObjects::ConstSharedPtr input_objects_msg) { // Get the time of the measurement const rclcpp::Time measurement_time = @@ -157,7 +157,7 @@ void MultiObjectTracker::onMeasurement( return; } /* transform to world coordinate */ - autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; + DetectedObjects transformed_objects; if (!object_recognition_utils::transformObjects( *input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) { return; @@ -234,7 +234,7 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const return; } // Create output msg - autoware_auto_perception_msgs::msg::TrackedObjects output_msg, tentative_objects_msg; + TrackedObjects output_msg, tentative_objects_msg; output_msg.header.frame_id = world_frame_id_; processor_->getTrackedObjects(time, output_msg); @@ -246,11 +246,13 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const debugger_->endPublishTime(this->now(), time); if (debugger_->shouldPublishTentativeObjects()) { - autoware_auto_perception_msgs::msg::TrackedObjects tentative_objects_msg; + TrackedObjects tentative_objects_msg; tentative_objects_msg.header.frame_id = world_frame_id_; processor_->getTentativeObjects(time, tentative_objects_msg); debugger_->publishTentativeObjects(tentative_objects_msg); } } -RCLCPP_COMPONENTS_REGISTER_NODE(MultiObjectTracker) +} // namespace multi_object_tracker + +RCLCPP_COMPONENTS_REGISTER_NODE(multi_object_tracker::MultiObjectTracker) diff --git a/perception/multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp index c6eed297c02c4..273e6ac1a06bd 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp @@ -27,7 +27,7 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); rclcpp::NodeOptions options; - auto multi_object_tracker = std::make_shared(options); + auto multi_object_tracker = std::make_shared(options); rclcpp::executors::SingleThreadedExecutor exec; exec.add_node(multi_object_tracker); exec.spin(); From 4176cec169e2a63d1a933668e8dff9e2c9ae5aa1 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 11 Apr 2024 17:49:48 +0900 Subject: [PATCH 02/67] test: multiple inputs Signed-off-by: Taekjin LEE --- .../multi_object_tracker_node.param.yaml | 2 ++ .../multi_object_tracker_core.hpp | 9 +++++ .../src/multi_object_tracker_core.cpp | 35 +++++++++++++++++-- 3 files changed, 43 insertions(+), 3 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 ca320c7f58442..09b9adee8caa8 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 @@ -19,3 +19,5 @@ publish_tentative_objects: false diagnostics_warn_delay: 0.5 # [sec] diagnostics_error_delay: 1.0 # [sec] + + input_topic_names: ["/perception/object_recognition/detection/objects"] 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 483eb38bf2697..e621479d4e886 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 @@ -83,7 +83,16 @@ class MultiObjectTracker : public rclcpp::Node std::unique_ptr data_association_; std::unique_ptr processor_; + // inputs + rclcpp::Subscription::SharedPtr sub_objects_{}; + std::vector::SharedPtr> sub_objects_array_{}; + + std::vector input_topic_names_{}; + size_t input_topic_size_{}; + std::vector objects_data_{}; + // callback functions + void onData(DetectedObjects::ConstSharedPtr msg, const size_t array_number); void onMeasurement(const DetectedObjects::ConstSharedPtr input_objects_msg); void onTimer(); 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 6f343dd5de8fb..e55b0d0371c55 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -76,15 +76,35 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) last_published_time_(this->now()) { // Create publishers and subscribers - detected_object_sub_ = create_subscription( - "input", rclcpp::QoS{1}, - std::bind(&MultiObjectTracker::onMeasurement, this, std::placeholders::_1)); + // detected_object_sub_ = create_subscription( + // "input", rclcpp::QoS{1}, + // std::bind(&MultiObjectTracker::onMeasurement, this, std::placeholders::_1)); tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); // Get parameters 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")}; + declare_parameter("input_topic_names", std::vector()); + input_topic_names_ = get_parameter("input_topic_names").as_string_array(); + + // Set input topics + if (input_topic_names_.empty()) { + RCLCPP_ERROR(get_logger(), "Need a 'input_topic_names' parameter to be set before continuing"); + return; + } + input_topic_size_ = input_topic_names_.size(); + sub_objects_array_.resize(input_topic_size_); + objects_data_.resize(input_topic_size_); + + for (size_t i = 0; i < input_topic_size_; i++) { + RCLCPP_DEBUG(get_logger(), "Subscribing to %s", input_topic_names_.at(i).c_str()); + + std::function func = + std::bind(&MultiObjectTracker::onData, this, std::placeholders::_1, i); + sub_objects_array_.at(i) = + create_subscription(input_topic_names_.at(i), rclcpp::QoS{1}, func); + } // Create tf timer auto cti = std::make_shared( @@ -143,6 +163,15 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) published_time_publisher_ = std::make_unique(this); } +void MultiObjectTracker::onData(DetectedObjects::ConstSharedPtr msg, const size_t array_number) +{ + objects_data_.at(array_number) = msg; + // debug message + // std::cout << "Received data from topic " << input_topic_names_.at(array_number) << std::endl; + + onMeasurement(msg); +} + void MultiObjectTracker::onMeasurement(const DetectedObjects::ConstSharedPtr input_objects_msg) { // Get the time of the measurement From 96be0ccba7ab71d38377e3c0ce4d8cf40ec8e4f7 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 11 Apr 2024 18:07:29 +0900 Subject: [PATCH 03/67] feat: check latest measurement time Signed-off-by: Taekjin LEE --- .../multi_object_tracker_core.hpp | 1 + .../src/multi_object_tracker_core.cpp | 15 +++++++++++++-- 2 files changed, 14 insertions(+), 2 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 e621479d4e886..0554c2f1720da 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 @@ -86,6 +86,7 @@ class MultiObjectTracker : public rclcpp::Node // inputs rclcpp::Subscription::SharedPtr sub_objects_{}; std::vector::SharedPtr> sub_objects_array_{}; + rclcpp::Time last_measurement_time_; std::vector input_topic_names_{}; size_t input_topic_size_{}; 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 e55b0d0371c55..098b711d9b207 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -89,6 +89,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) input_topic_names_ = get_parameter("input_topic_names").as_string_array(); // Set input topics + last_measurement_time_ = this->now(); if (input_topic_names_.empty()) { RCLCPP_ERROR(get_logger(), "Need a 'input_topic_names' parameter to be set before continuing"); return; @@ -98,7 +99,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) objects_data_.resize(input_topic_size_); for (size_t i = 0; i < input_topic_size_; i++) { - RCLCPP_DEBUG(get_logger(), "Subscribing to %s", input_topic_names_.at(i).c_str()); + RCLCPP_INFO(get_logger(), "Subscribing to %s", input_topic_names_.at(i).c_str()); std::function func = std::bind(&MultiObjectTracker::onData, this, std::placeholders::_1, i); @@ -167,7 +168,17 @@ void MultiObjectTracker::onData(DetectedObjects::ConstSharedPtr msg, const size_ { objects_data_.at(array_number) = msg; // debug message - // std::cout << "Received data from topic " << input_topic_names_.at(array_number) << std::endl; + RCLCPP_INFO( + get_logger(), "Received data from topic %s", input_topic_names_.at(array_number).c_str()); + if (last_measurement_time_ > msg->header.stamp) { + const double delta_time = (last_measurement_time_ - msg->header.stamp).seconds() * 1e3; + RCLCPP_INFO( + get_logger(), "Received data is older than the last measurement time by %f ms", delta_time); + return; + } + if (last_measurement_time_ < msg->header.stamp) { + last_measurement_time_ = msg->header.stamp; + } onMeasurement(msg); } From fd086dcd8bd323c9eaf42bb6201c8ad388a7c465 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 12 Apr 2024 15:32:41 +0900 Subject: [PATCH 04/67] feat: define input manager class Signed-off-by: Taekjin LEE --- .../multi_object_tracker/CMakeLists.txt | 1 + .../multi_object_tracker_core.hpp | 2 +- .../processor/input_manager.hpp | 77 +++++++++++++++++++ .../processor/processor.hpp | 2 - .../src/multi_object_tracker_core.cpp | 17 ++-- .../src/processor/input_manager.cpp | 76 ++++++++++++++++++ .../src/processor/processor.cpp | 2 - 7 files changed, 164 insertions(+), 13 deletions(-) create mode 100644 perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp create mode 100644 perception/multi_object_tracker/src/processor/input_manager.cpp diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index 46af1f9b9a1de..7d779d3bfc093 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -24,6 +24,7 @@ set(MULTI_OBJECT_TRACKER_SRC src/multi_object_tracker_core.cpp src/debugger.cpp src/processor/processor.cpp + src/processor/input_manager.cpp src/data_association/data_association.cpp src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp src/tracker/motion_model/motion_model_base.cpp 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 0554c2f1720da..33450ccb54131 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 @@ -93,7 +93,7 @@ class MultiObjectTracker : public rclcpp::Node std::vector objects_data_{}; // callback functions - void onData(DetectedObjects::ConstSharedPtr msg, const size_t array_number); + void onData(const DetectedObjects::ConstSharedPtr msg, const size_t array_number); void onMeasurement(const DetectedObjects::ConstSharedPtr input_objects_msg); void onTimer(); 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 new file mode 100644 index 0000000000000..c8a9e22e4d616 --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp @@ -0,0 +1,77 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ +#define MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "autoware_auto_perception_msgs/msg/detected_objects.hpp" + +#include +#include +#include +#include + +class InputManager +{ +public: + explicit InputManager(rclcpp::Node & node); + + void init( + const std::string & input_topic, const std::string & long_name, const std::string & short_name); + + void setObjects(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg); + +private: + rclcpp::Node & node_; + + std::string input_topic_; + std::string long_name_; + std::string short_name_; + + rclcpp::Subscription::SharedPtr sub_objects_; + + size_t que_size_{20}; + std::deque objects_que_; + + double expected_rate_; + double latency_mean_; + double latency_var_; + + rclcpp::Time latest_measurement_time_; + rclcpp::Time latest_message_time_; +}; + +class InputManagers +{ +public: + explicit InputManagers(rclcpp::Node & node); + + void init( + const std::vector & input_topics, const std::vector & long_names, + const std::vector & short_names); + + void getObjects( + const rclcpp::Time & now, + std::vector & objects); + +private: + rclcpp::Node & node_; + + size_t input_size_; + std::vector> input_managers_; +}; + +#endif // MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp index 6d6e364d83a41..654c960463933 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp @@ -11,8 +11,6 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -// -// #ifndef MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ #define MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ 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 098b711d9b207..669484b373367 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -75,12 +75,6 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) tf_listener_(tf_buffer_), last_published_time_(this->now()) { - // Create publishers and subscribers - // detected_object_sub_ = create_subscription( - // "input", rclcpp::QoS{1}, - // std::bind(&MultiObjectTracker::onMeasurement, this, std::placeholders::_1)); - tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); - // Get parameters double publish_rate = declare_parameter("publish_rate"); // [hz] world_frame_id_ = declare_parameter("world_frame_id"); @@ -88,8 +82,13 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) declare_parameter("input_topic_names", std::vector()); input_topic_names_ = get_parameter("input_topic_names").as_string_array(); - // Set input topics + // Initialize the last measurement time last_measurement_time_ = this->now(); + + // ROS interface - Publisher + tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); + + // ROS interface - Subscribers if (input_topic_names_.empty()) { RCLCPP_ERROR(get_logger(), "Need a 'input_topic_names' parameter to be set before continuing"); return; @@ -164,9 +163,11 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) published_time_publisher_ = std::make_unique(this); } -void MultiObjectTracker::onData(DetectedObjects::ConstSharedPtr msg, const size_t array_number) +void MultiObjectTracker::onData( + const DetectedObjects::ConstSharedPtr msg, const size_t array_number) { objects_data_.at(array_number) = msg; + // debug message RCLCPP_INFO( get_logger(), "Received data from topic %s", input_topic_names_.at(array_number).c_str()); diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp new file mode 100644 index 0000000000000..f7146c8bc9b29 --- /dev/null +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -0,0 +1,76 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "multi_object_tracker/processor/input_manager.hpp" + +InputManager::InputManager(rclcpp::Node & node) : node_(node) +{ +} + +void InputManager::init( + const std::string & input_topic, const std::string & long_name, const std::string & short_name) +{ + // Initialize parameters + input_topic_ = input_topic; + long_name_ = long_name; + short_name_ = short_name; + + // Initialize subscription + std::function + func = std::bind(&InputManager::setObjects, this, std::placeholders::_1); + sub_objects_ = node_.create_subscription( + input_topic_, rclcpp::QoS{1}, func); + + // Initialize latency statistics + expected_rate_ = 10.0; + latency_mean_ = 1 / expected_rate_; + latency_var_ = 0.0; +} + +void InputManager::setObjects( + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg) +{ + const auto & object = *msg; + objects_que_.push_back(object); + if (objects_que_.size() > que_size_) { + objects_que_.pop_front(); + } + latest_message_time_ = node_.now(); + latest_measurement_time_ = object.header.stamp; + + // Calculate latency + const double latency = (latest_message_time_ - latest_measurement_time_).seconds(); + + // Update latency statistics + constexpr double gain = 0.05; + latency_mean_ = (1.0 - gain) * latency_mean_ + gain * latency; + const double latency_delta = latency - latency_mean_; + latency_var_ = (1.0 - gain) * latency_var_ + gain * latency_delta * latency_delta; +} + +InputManagers::InputManagers(rclcpp::Node & node) : node_(node) +{ +} + +void InputManagers::init( + const std::vector & input_topics, const std::vector & long_names, + const std::vector & short_names) +{ + input_size_ = input_topics.size(); + for (size_t i = 0; i < input_size_; ++i) { + InputManager input_manager(node_); + input_manager.init(input_topics[i], long_names[i], short_names[i]); + input_managers_.push_back(std::make_shared(input_manager)); + } +} diff --git a/perception/multi_object_tracker/src/processor/processor.cpp b/perception/multi_object_tracker/src/processor/processor.cpp index 0d56e16b431e9..8cfb64cab19be 100644 --- a/perception/multi_object_tracker/src/processor/processor.cpp +++ b/perception/multi_object_tracker/src/processor/processor.cpp @@ -11,8 +11,6 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -// -// #include "multi_object_tracker/processor/processor.hpp" From 1462eefe9c2b7eeb7a32e53a574a066e9f9ec229 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 12 Apr 2024 15:37:53 +0900 Subject: [PATCH 05/67] feat: interval measures Signed-off-by: Taekjin LEE --- .../processor/input_manager.hpp | 3 +++ .../src/processor/input_manager.cpp | 22 ++++++++++++++++--- 2 files changed, 22 insertions(+), 3 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 c8a9e22e4d616..9ed3074a8a3ee 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 @@ -46,9 +46,12 @@ class InputManager size_t que_size_{20}; std::deque objects_que_; + bool is_time_initialized_{false}; double expected_rate_; double latency_mean_; double latency_var_; + double interval_mean_; + double interval_var_; rclcpp::Time latest_measurement_time_; rclcpp::Time latest_message_time_; diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index f7146c8bc9b29..5ecd82731edcd 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -34,8 +34,10 @@ void InputManager::init( // Initialize latency statistics expected_rate_ = 10.0; - latency_mean_ = 1 / expected_rate_; + latency_mean_ = 0.10; // [s] latency_var_ = 0.0; + interval_mean_ = 1 / expected_rate_; + interval_var_ = 0.0; } void InputManager::setObjects( @@ -46,14 +48,28 @@ void InputManager::setObjects( if (objects_que_.size() > que_size_) { objects_que_.pop_front(); } - latest_message_time_ = node_.now(); + + // Filter parameters + constexpr double gain = 0.05; + const auto now = node_.now(); + + // Calculate interval, Update interval statistics + if (is_time_initialized_) { + const double interval = (now - latest_message_time_).seconds(); + interval_mean_ = (1.0 - gain) * interval_mean_ + gain * interval; + const double interval_delta = interval - interval_mean_; + interval_var_ = (1.0 - gain) * interval_var_ + gain * interval_delta * interval_delta; + } + + // Update time + latest_message_time_ = now; latest_measurement_time_ = object.header.stamp; + if (!is_time_initialized_) is_time_initialized_ = true; // Calculate latency const double latency = (latest_message_time_ - latest_measurement_time_).seconds(); // Update latency statistics - constexpr double gain = 0.05; latency_mean_ = (1.0 - gain) * latency_mean_ + gain * latency; const double latency_delta = latency - latency_mean_; latency_var_ = (1.0 - gain) * latency_var_ + gain * latency_delta * latency_delta; From 7757212d5dd871026b9d75f1bd7d547df58b65c1 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 12 Apr 2024 17:18:22 +0900 Subject: [PATCH 06/67] feat: store and sort inputs PoC Signed-off-by: Taekjin LEE --- .../multi_object_tracker_node.param.yaml | 2 + .../multi_object_tracker_core.hpp | 4 +- .../src/multi_object_tracker_core.cpp | 100 ++++++++++++------ 3 files changed, 70 insertions(+), 36 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 09b9adee8caa8..494c5f2cd1e28 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 @@ -21,3 +21,5 @@ diagnostics_error_delay: 1.0 # [sec] input_topic_names: ["/perception/object_recognition/detection/objects"] + input_names_long: [merged_objects] + input_names_short: [all] 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 33450ccb54131..75c594e01171b 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 @@ -48,6 +48,7 @@ #include #include #include +#include #include namespace multi_object_tracker @@ -90,7 +91,7 @@ class MultiObjectTracker : public rclcpp::Node std::vector input_topic_names_{}; size_t input_topic_size_{}; - std::vector objects_data_{}; + std::vector> objects_data_{}; // callback functions void onData(const DetectedObjects::ConstSharedPtr msg, const size_t array_number); @@ -98,6 +99,7 @@ class MultiObjectTracker : public rclcpp::Node void onTimer(); // publish processes + void runProcess(const DetectedObjects::ConstSharedPtr input_objects_msg); 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 669484b373367..e9f80c902b664 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -95,7 +95,6 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) } input_topic_size_ = input_topic_names_.size(); sub_objects_array_.resize(input_topic_size_); - objects_data_.resize(input_topic_size_); for (size_t i = 0; i < input_topic_size_; i++) { RCLCPP_INFO(get_logger(), "Subscribing to %s", input_topic_names_.at(i).c_str()); @@ -166,7 +165,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) void MultiObjectTracker::onData( const DetectedObjects::ConstSharedPtr msg, const size_t array_number) { - objects_data_.at(array_number) = msg; + std::pair pair = std::make_pair(msg->header.stamp, *msg); + objects_data_.push_back(pair); // debug message RCLCPP_INFO( @@ -181,7 +181,7 @@ void MultiObjectTracker::onData( last_measurement_time_ = msg->header.stamp; } - onMeasurement(msg); + // onMeasurement(msg); } void MultiObjectTracker::onMeasurement(const DetectedObjects::ConstSharedPtr input_objects_msg) @@ -190,24 +190,79 @@ void MultiObjectTracker::onMeasurement(const DetectedObjects::ConstSharedPtr inp 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(), measurement_time); + runProcess(input_objects_msg); + debugger_->endMeasurementTime(this->now()); + + // 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()); + } + } +} + +void MultiObjectTracker::onTimer() +{ + const rclcpp::Time current_time = this->now(); + + // 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) { + // update objects + if (objects_data_.empty()) { + return; + } + // updated from accumulated objects + // sort by time + sort(objects_data_.begin(), objects_data_.end(), [](const auto & a, const auto & b) { + return a.first.seconds() < b.first.seconds(); + }); + + // run onMeasurement for each queue + while (!objects_data_.empty()) { + const auto & pair = objects_data_.front(); + runProcess(std::make_shared(pair.second)); + objects_data_.erase(objects_data_.begin()); + } + + // Publish + checkAndPublish(current_time); + } +} + +void MultiObjectTracker::runProcess(const 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()); + + // Get the transform of the self frame const auto self_transform = getTransformAnonymous(tf_buffer_, "base_link", world_frame_id_, measurement_time); if (!self_transform) { return; } - /* transform to world coordinate */ + + // Transform the objects to the world frame DetectedObjects transformed_objects; if (!object_recognition_utils::transformObjects( *input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) { return; } - ////// Tracker Process - //// Associate and update /* prediction */ processor_->predict(measurement_time); + /* object association */ std::unordered_map direct_assignment, reverse_assignment; { @@ -218,40 +273,15 @@ void MultiObjectTracker::onMeasurement(const DetectedObjects::ConstSharedPtr inp detected_objects, list_tracker); // row : tracker, col : measurement data_association_->assign(score_matrix, direct_assignment, reverse_assignment); } + /* tracker update */ processor_->update(transformed_objects, *self_transform, direct_assignment); + /* tracker pruning */ processor_->prune(measurement_time); + /* spawn new tracker */ processor_->spawn(transformed_objects, *self_transform, reverse_assignment); - - // debugger time - debugger_->endMeasurementTime(this->now()); - - // 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()); - } - } -} - -void MultiObjectTracker::onTimer() -{ - const rclcpp::Time current_time = this->now(); - // 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) From 2a443e70c5f50a23fe193fab8084aa571735ef02 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 12 Apr 2024 17:33:07 +0900 Subject: [PATCH 07/67] chore: rename classes Signed-off-by: Taekjin LEE --- .../processor/input_manager.hpp | 11 +++++------ .../src/processor/input_manager.cpp | 18 +++++++++--------- 2 files changed, 14 insertions(+), 15 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 9ed3074a8a3ee..b441ebac57def 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 @@ -24,10 +24,10 @@ #include #include -class InputManager +class InputStream { public: - explicit InputManager(rclcpp::Node & node); + explicit InputStream(rclcpp::Node & node); void init( const std::string & input_topic, const std::string & long_name, const std::string & short_name); @@ -57,10 +57,9 @@ class InputManager rclcpp::Time latest_message_time_; }; -class InputManagers -{ +class InputManager{ public: - explicit InputManagers(rclcpp::Node & node); + explicit InputManager(rclcpp::Node & node); void init( const std::vector & input_topics, const std::vector & long_names, @@ -74,7 +73,7 @@ class InputManagers rclcpp::Node & node_; size_t input_size_; - std::vector> input_managers_; + std::vector> input_streams_; }; #endif // MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index 5ecd82731edcd..9e767bcc63cb6 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -14,11 +14,11 @@ #include "multi_object_tracker/processor/input_manager.hpp" -InputManager::InputManager(rclcpp::Node & node) : node_(node) +InputStream::InputStream(rclcpp::Node & node) : node_(node) { } -void InputManager::init( +void InputStream::init( const std::string & input_topic, const std::string & long_name, const std::string & short_name) { // Initialize parameters @@ -28,7 +28,7 @@ void InputManager::init( // Initialize subscription std::function - func = std::bind(&InputManager::setObjects, this, std::placeholders::_1); + func = std::bind(&InputStream::setObjects, this, std::placeholders::_1); sub_objects_ = node_.create_subscription( input_topic_, rclcpp::QoS{1}, func); @@ -40,7 +40,7 @@ void InputManager::init( interval_var_ = 0.0; } -void InputManager::setObjects( +void InputStream::setObjects( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg) { const auto & object = *msg; @@ -75,18 +75,18 @@ void InputManager::setObjects( latency_var_ = (1.0 - gain) * latency_var_ + gain * latency_delta * latency_delta; } -InputManagers::InputManagers(rclcpp::Node & node) : node_(node) +InputManager::InputManager(rclcpp::Node & node) : node_(node) { } -void InputManagers::init( +void InputManager::init( const std::vector & input_topics, const std::vector & long_names, const std::vector & short_names) { input_size_ = input_topics.size(); for (size_t i = 0; i < input_size_; ++i) { - InputManager input_manager(node_); - input_manager.init(input_topics[i], long_names[i], short_names[i]); - input_managers_.push_back(std::make_shared(input_manager)); + InputStream input_stream(node_); + input_stream.init(input_topics[i], long_names[i], short_names[i]); + input_streams_.push_back(std::make_shared(input_stream)); } } From 866b656c67ed1b589c28f61b8f8ab2b464ae32d0 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 12 Apr 2024 18:22:23 +0900 Subject: [PATCH 08/67] feat: object collector Signed-off-by: Taekjin LEE --- .../processor/input_manager.hpp | 6 ++- .../src/multi_object_tracker_core.cpp | 36 +++++++------- .../src/processor/input_manager.cpp | 48 +++++++++++++++++++ 3 files changed, 70 insertions(+), 20 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 b441ebac57def..55a264f92c16b 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 @@ -33,6 +33,9 @@ class InputStream const std::string & input_topic, const std::string & long_name, const std::string & short_name); void setObjects(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg); + void getObjectsOlderThan( + const rclcpp::Time & time, const double duration, + std::vector & objects); private: rclcpp::Node & node_; @@ -57,7 +60,8 @@ class InputStream rclcpp::Time latest_message_time_; }; -class InputManager{ +class InputManager +{ public: explicit InputManager(rclcpp::Node & node); 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 e9f80c902b664..1c3b2aa98129c 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -217,27 +217,25 @@ void MultiObjectTracker::onTimer() 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) { - // update objects - if (objects_data_.empty()) { - return; - } - // updated from accumulated objects - // sort by time - sort(objects_data_.begin(), objects_data_.end(), [](const auto & a, const auto & b) { - return a.first.seconds() < b.first.seconds(); - }); - - // run onMeasurement for each queue - while (!objects_data_.empty()) { - const auto & pair = objects_data_.front(); - runProcess(std::make_shared(pair.second)); - objects_data_.erase(objects_data_.begin()); - } + if (elapsed_time < maximum_publish_latency) return; + + // update trackers from accumulated input objects + if (objects_data_.empty()) return; - // Publish - checkAndPublish(current_time); + // sort by time + sort(objects_data_.begin(), objects_data_.end(), [](const auto & a, const auto & b) { + return a.first.seconds() < b.first.seconds(); + }); + + // run onMeasurement for each queue + while (!objects_data_.empty()) { + const auto & pair = objects_data_.front(); + runProcess(std::make_shared(pair.second)); + objects_data_.erase(objects_data_.begin()); } + + // Publish + checkAndPublish(current_time); } void MultiObjectTracker::runProcess(const DetectedObjects::ConstSharedPtr input_objects_msg) diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index 9e767bcc63cb6..ead42da56487f 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -75,6 +75,25 @@ void InputStream::setObjects( latency_var_ = (1.0 - gain) * latency_var_ + gain * latency_delta * latency_delta; } +void InputStream::getObjectsOlderThan( + const rclcpp::Time & time, const double duration, + std::vector & objects) +{ + objects.clear(); + for (const auto & object : objects_que_) { + // Skip if the object is older than the specified duration + const rclcpp::Time object_time = rclcpp::Time(object.header.stamp); + const double time_diff = (object_time - time).seconds(); + if (time_diff > duration) { + continue; + } + // Add the object if the object is older than the specified time + if (time_diff >= 0.0) { + objects.push_back(object); + } + } +} + InputManager::InputManager(rclcpp::Node & node) : node_(node) { } @@ -90,3 +109,32 @@ void InputManager::init( input_streams_.push_back(std::make_shared(input_stream)); } } + +void InputManager::getObjects( + const rclcpp::Time & now, + std::vector & objects) +{ + objects.clear(); + + // Get proper latency + constexpr double target_latency = 0.15; // [s], measurement to tracking latency, as much as the + // detector latency, the less total latency + constexpr double acceptable_band = 0.15; // [s], acceptable band from the target latency + rclcpp::Time time = now - rclcpp::Duration::from_seconds(target_latency); + + // Get objects from all input streams + for (const auto & input_stream : input_streams_) { + std::vector objects_tmp; + input_stream->getObjectsOlderThan(time, acceptable_band, objects_tmp); + objects.insert(objects.end(), objects_tmp.begin(), objects_tmp.end()); + } + + // Sort objects by timestamp + std::sort( + objects.begin(), objects.end(), + []( + const autoware_auto_perception_msgs::msg::DetectedObjects & a, + const autoware_auto_perception_msgs::msg::DetectedObjects & b) { + return (rclcpp::Time(a.header.stamp) - rclcpp::Time(b.header.stamp)).seconds() < 0; + }); +} From 4b71b57a7430ecd4fd854ee76b0a714938fc4738 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 12 Apr 2024 19:33:38 +0900 Subject: [PATCH 09/67] impl input manager, no subscribe Signed-off-by: Taekjin LEE --- .../multi_object_tracker_node.param.yaml | 4 +- .../multi_object_tracker_core.hpp | 6 +- .../processor/input_manager.hpp | 2 +- .../src/multi_object_tracker_core.cpp | 73 ++++++++++++------- .../src/processor/input_manager.cpp | 14 ++-- 5 files changed, 63 insertions(+), 36 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 494c5f2cd1e28..945cf44113a06 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 @@ -21,5 +21,5 @@ diagnostics_error_delay: 1.0 # [sec] input_topic_names: ["/perception/object_recognition/detection/objects"] - input_names_long: [merged_objects] - input_names_short: [all] + input_names_long: ["merged_objects"] + input_names_short: ["all"] 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 75c594e01171b..e3a2864b1faaf 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 @@ -21,6 +21,7 @@ #include "multi_object_tracker/data_association/data_association.hpp" #include "multi_object_tracker/debugger.hpp" +#include "multi_object_tracker/processor/input_manager.hpp" #include "multi_object_tracker/processor/processor.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" @@ -89,6 +90,9 @@ class MultiObjectTracker : public rclcpp::Node std::vector::SharedPtr> sub_objects_array_{}; rclcpp::Time last_measurement_time_; + // input manager + std::unique_ptr input_manager_; + std::vector input_topic_names_{}; size_t input_topic_size_{}; std::vector> objects_data_{}; @@ -99,7 +103,7 @@ class MultiObjectTracker : public rclcpp::Node void onTimer(); // publish processes - void runProcess(const DetectedObjects::ConstSharedPtr input_objects_msg); + void runProcess(const DetectedObjects & input_objects_msg); 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/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp index 55a264f92c16b..62ddee4a9ec6a 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 @@ -69,7 +69,7 @@ class InputManager const std::vector & input_topics, const std::vector & long_names, const std::vector & short_names); - void getObjects( + bool getObjects( const rclcpp::Time & now, std::vector & objects); 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 1c3b2aa98129c..f0c9a874c7a14 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -79,8 +79,13 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) 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")}; + declare_parameter("input_topic_names", std::vector()); - input_topic_names_ = get_parameter("input_topic_names").as_string_array(); + declare_parameter("input_names_long", std::vector()); + declare_parameter("input_names_short", std::vector()); + std::vector input_topic_names = get_parameter("input_topic_names").as_string_array(); + std::vector input_names_long = get_parameter("input_names_long").as_string_array(); + std::vector input_names_short = get_parameter("input_names_short").as_string_array(); // Initialize the last measurement time last_measurement_time_ = this->now(); @@ -89,21 +94,29 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); // ROS interface - Subscribers - if (input_topic_names_.empty()) { + if (input_topic_names.empty()) { RCLCPP_ERROR(get_logger(), "Need a 'input_topic_names' parameter to be set before continuing"); return; } - input_topic_size_ = input_topic_names_.size(); - sub_objects_array_.resize(input_topic_size_); + input_topic_size_ = input_topic_names.size(); + // print input information for (size_t i = 0; i < input_topic_size_; i++) { - RCLCPP_INFO(get_logger(), "Subscribing to %s", input_topic_names_.at(i).c_str()); - - std::function func = - std::bind(&MultiObjectTracker::onData, this, std::placeholders::_1, i); - sub_objects_array_.at(i) = - create_subscription(input_topic_names_.at(i), rclcpp::QoS{1}, func); + RCLCPP_INFO(get_logger(), "Subscribing to %s %s %s", input_topic_names.at(i).c_str(), input_names_long.at(i).c_str(), input_names_short.at(i).c_str()); } + + input_manager_ = std::make_unique(*this); + input_manager_->init(input_topic_names_, input_names_long, input_names_short); + + // sub_objects_array_.resize(input_topic_size_); + + // for (size_t i = 0; i < input_topic_size_; i++) { + // RCLCPP_INFO(get_logger(), "Subscribing to %s", input_topic_names_.at(i).c_str()); + // std::function func = + // std::bind(&MultiObjectTracker::onData, this, std::placeholders::_1, i); + // sub_objects_array_.at(i) = + // create_subscription(input_topic_names_.at(i), rclcpp::QoS{1}, func); + // } // Create tf timer auto cti = std::make_shared( @@ -191,7 +204,7 @@ void MultiObjectTracker::onMeasurement(const DetectedObjects::ConstSharedPtr inp rclcpp::Time(input_objects_msg->header.stamp, this->now().get_clock_type()); debugger_->startMeasurementTime(this->now(), measurement_time); - runProcess(input_objects_msg); + runProcess(*input_objects_msg); debugger_->endMeasurementTime(this->now()); // Publish objects if the timer is not enabled @@ -219,30 +232,36 @@ void MultiObjectTracker::onTimer() if (elapsed_time < maximum_publish_latency) return; - // update trackers from accumulated input objects - if (objects_data_.empty()) return; - - // sort by time - sort(objects_data_.begin(), objects_data_.end(), [](const auto & a, const auto & b) { - return a.first.seconds() < b.first.seconds(); - }); - - // run onMeasurement for each queue - while (!objects_data_.empty()) { - const auto & pair = objects_data_.front(); - runProcess(std::make_shared(pair.second)); - objects_data_.erase(objects_data_.begin()); + // // update trackers from accumulated input objects + // if (objects_data_.empty()) return; + + // // sort by time + // sort(objects_data_.begin(), objects_data_.end(), [](const auto & a, const auto & b) { + // return a.first.seconds() < b.first.seconds(); + // }); + std::vector objects_data; + if (input_manager_->getObjects(current_time, objects_data)) { + for (const auto & objects : objects_data) { + runProcess(objects); + } } + // // run onMeasurement for each queue + // while (!objects_data_.empty()) { + // const auto & pair = objects_data_.front(); + // runProcess(std::make_shared(pair.second)); + // objects_data_.erase(objects_data_.begin()); + // } + // Publish checkAndPublish(current_time); } -void MultiObjectTracker::runProcess(const DetectedObjects::ConstSharedPtr input_objects_msg) +void MultiObjectTracker::runProcess(const DetectedObjects & 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()); + rclcpp::Time(input_objects_msg.header.stamp, this->now().get_clock_type()); // Get the transform of the self frame const auto self_transform = @@ -254,7 +273,7 @@ void MultiObjectTracker::runProcess(const DetectedObjects::ConstSharedPtr input_ // Transform the objects to the world frame DetectedObjects transformed_objects; if (!object_recognition_utils::transformObjects( - *input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) { + input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) { return; } diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index ead42da56487f..be7b91f848a04 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "multi_object_tracker/processor/input_manager.hpp" +#include InputStream::InputStream(rclcpp::Node & node) : node_(node) { @@ -99,18 +100,19 @@ InputManager::InputManager(rclcpp::Node & node) : node_(node) } void InputManager::init( - const std::vector & input_topics, const std::vector & long_names, - const std::vector & short_names) + const std::vector & input_topics, const std::vector & long_names, const std::vector & short_names) { input_size_ = input_topics.size(); - for (size_t i = 0; i < input_size_; ++i) { + + for (size_t i = 0; i < input_size_; i++) { + InputStream input_stream(node_); - input_stream.init(input_topics[i], long_names[i], short_names[i]); + input_stream.init(input_topics.at(i), long_names.at(i), short_names.at(i)); input_streams_.push_back(std::make_shared(input_stream)); } } -void InputManager::getObjects( +bool InputManager::getObjects( const rclcpp::Time & now, std::vector & objects) { @@ -137,4 +139,6 @@ void InputManager::getObjects( const autoware_auto_perception_msgs::msg::DetectedObjects & b) { return (rclcpp::Time(a.header.stamp) - rclcpp::Time(b.header.stamp)).seconds() < 0; }); + + return !objects.empty(); } From a9fa209b16ca9793f5ecbee6170e4441bec38f7d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 15 Apr 2024 10:14:19 +0900 Subject: [PATCH 10/67] fix: subscribe and trigger callback Signed-off-by: Taekjin LEE --- .../src/multi_object_tracker_core.cpp | 2 +- .../src/processor/input_manager.cpp | 20 ++++++++++++++++--- 2 files changed, 18 insertions(+), 4 deletions(-) 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 f0c9a874c7a14..1b5201d6ae7d3 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -106,7 +106,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) } input_manager_ = std::make_unique(*this); - input_manager_->init(input_topic_names_, input_names_long, input_names_short); + input_manager_->init(input_topic_names, input_names_long, input_names_short); // sub_objects_array_.resize(input_topic_size_); diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index be7b91f848a04..327bb2df7a35a 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -27,12 +27,20 @@ void InputStream::init( long_name_ = long_name; short_name_ = short_name; + // debug message + RCLCPP_INFO( + node_.get_logger(), "Initializing %s input stream from %s", long_name_.c_str(), + input_topic_.c_str()); + // Initialize subscription std::function func = std::bind(&InputStream::setObjects, this, std::placeholders::_1); sub_objects_ = node_.create_subscription( input_topic_, rclcpp::QoS{1}, func); + // Initialize queue + objects_que_.clear(); + // Initialize latency statistics expected_rate_ = 10.0; latency_mean_ = 0.10; // [s] @@ -44,7 +52,11 @@ void InputStream::init( void InputStream::setObjects( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg) { - const auto & object = *msg; + // debug message + RCLCPP_INFO( + node_.get_logger(), "Received %s message from %s", long_name_.c_str(), input_topic_.c_str()); + + const autoware_auto_perception_msgs::msg::DetectedObjects object = *msg; objects_que_.push_back(object); if (objects_que_.size() > que_size_) { objects_que_.pop_front(); @@ -103,9 +115,11 @@ void InputManager::init( const std::vector & input_topics, const std::vector & long_names, const std::vector & short_names) { input_size_ = input_topics.size(); - - for (size_t i = 0; i < input_size_; i++) { + RCLCPP_INFO(node_.get_logger(), "Initializing input manager with %zu input streams", input_size_); + assert(input_size_ == long_names.size()); + assert(input_size_ == short_names.size()); + for (size_t i = 0; i < input_size_; i++) { InputStream input_stream(node_); input_stream.init(input_topics.at(i), long_names.at(i), short_names.at(i)); input_streams_.push_back(std::make_shared(input_stream)); From cf2874d56aee6d98a30f7e87297b0e62c84e8b5b Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 15 Apr 2024 12:06:24 +0900 Subject: [PATCH 11/67] fix: subscriber and callbacks are working Signed-off-by: Taekjin LEE --- .../processor/input_manager.hpp | 7 ++- .../src/processor/input_manager.cpp | 51 ++++++++++++++----- 2 files changed, 42 insertions(+), 16 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 62ddee4a9ec6a..08bdd44269559 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 @@ -33,6 +33,7 @@ class InputStream const std::string & input_topic, const std::string & long_name, const std::string & short_name); void setObjects(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg); + void getObjectsOlderThan( const rclcpp::Time & time, const double duration, std::vector & objects); @@ -44,8 +45,6 @@ class InputStream std::string long_name_; std::string short_name_; - rclcpp::Subscription::SharedPtr sub_objects_; - size_t que_size_{20}; std::deque objects_que_; @@ -75,6 +74,10 @@ class InputManager private: rclcpp::Node & node_; + std::vector::SharedPtr> + sub_objects_array_{}; + + bool is_initialized_{false}; 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 327bb2df7a35a..55d744ef47511 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -13,7 +13,9 @@ // limitations under the License. #include "multi_object_tracker/processor/input_manager.hpp" + #include +#include InputStream::InputStream(rclcpp::Node & node) : node_(node) { @@ -27,17 +29,6 @@ void InputStream::init( long_name_ = long_name; short_name_ = short_name; - // debug message - RCLCPP_INFO( - node_.get_logger(), "Initializing %s input stream from %s", long_name_.c_str(), - input_topic_.c_str()); - - // Initialize subscription - std::function - func = std::bind(&InputStream::setObjects, this, std::placeholders::_1); - sub_objects_ = node_.create_subscription( - input_topic_, rclcpp::QoS{1}, func); - // Initialize queue objects_que_.clear(); @@ -54,7 +45,8 @@ void InputStream::setObjects( { // debug message RCLCPP_INFO( - node_.get_logger(), "Received %s message from %s", long_name_.c_str(), input_topic_.c_str()); + node_.get_logger(), "InputStream::setObjects Received %s message from %s at %d", + long_name_.c_str(), input_topic_.c_str(), msg->header.stamp.sec); const autoware_auto_perception_msgs::msg::DetectedObjects object = *msg; objects_que_.push_back(object); @@ -62,6 +54,9 @@ void InputStream::setObjects( objects_que_.pop_front(); } + // RCLCPP_INFO( + // node_.get_logger(), "InputStream::setObjects Que size: %zu", objects_que_.size()); + // Filter parameters constexpr double gain = 0.05; const auto now = node_.now(); @@ -112,17 +107,39 @@ InputManager::InputManager(rclcpp::Node & node) : node_(node) } void InputManager::init( - const std::vector & input_topics, const std::vector & long_names, const std::vector & short_names) + const std::vector & input_topics, const std::vector & long_names, + const std::vector & short_names) { input_size_ = input_topics.size(); - RCLCPP_INFO(node_.get_logger(), "Initializing input manager with %zu input streams", input_size_); + + // Check input sizes + RCLCPP_INFO( + node_.get_logger(), "InputManager::init Initializing input manager with %zu input streams", + input_size_); assert(input_size_ == long_names.size()); assert(input_size_ == short_names.size()); + sub_objects_array_.resize(input_size_); + for (size_t i = 0; i < input_size_; i++) { InputStream input_stream(node_); input_stream.init(input_topics.at(i), long_names.at(i), short_names.at(i)); input_streams_.push_back(std::make_shared(input_stream)); + + // Set subscription + RCLCPP_INFO( + node_.get_logger(), "InputManager::init Initializing %s input stream from %s", + long_names.at(i).c_str(), input_topics.at(i).c_str()); + std::function + func = std::bind(&InputStream::setObjects, input_stream, std::placeholders::_1); + sub_objects_array_.at(i) = + node_.create_subscription( + input_topics.at(i), rclcpp::QoS{1}, func); + } + + if (input_size_ > 0) { + is_initialized_ = true; } } @@ -130,6 +147,8 @@ bool InputManager::getObjects( const rclcpp::Time & now, std::vector & objects) { + RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Getting objects from input streams"); + objects.clear(); // Get proper latency @@ -154,5 +173,9 @@ bool InputManager::getObjects( return (rclcpp::Time(a.header.stamp) - rclcpp::Time(b.header.stamp)).seconds() < 0; }); + RCLCPP_INFO( + node_.get_logger(), "InputManager::getObjects Got %zu objects from input streams", + objects.size()); + return !objects.empty(); } From 246cb4a4d1df6ce40b6cd2f82fe5752ede63da8f Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 15 Apr 2024 13:10:14 +0900 Subject: [PATCH 12/67] fix: callback object is fixed, tracker is working Signed-off-by: Taekjin LEE --- .../processor/input_manager.hpp | 2 +- .../src/multi_object_tracker_core.cpp | 32 +++------------- .../src/processor/input_manager.cpp | 38 +++++++++++++++---- 3 files changed, 38 insertions(+), 34 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 08bdd44269559..4afcf2a817675 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 @@ -45,7 +45,7 @@ class InputStream std::string long_name_; std::string short_name_; - size_t que_size_{20}; + size_t que_size_{50}; std::deque objects_que_; bool is_time_initialized_{false}; 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 1b5201d6ae7d3..371a6d0996067 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -102,22 +102,15 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) // print input information for (size_t i = 0; i < input_topic_size_; i++) { - RCLCPP_INFO(get_logger(), "Subscribing to %s %s %s", input_topic_names.at(i).c_str(), input_names_long.at(i).c_str(), input_names_short.at(i).c_str()); + RCLCPP_INFO( + get_logger(), "Subscribing to %s %s %s", input_topic_names.at(i).c_str(), + input_names_long.at(i).c_str(), input_names_short.at(i).c_str()); } - + + // Initialize input manager input_manager_ = std::make_unique(*this); input_manager_->init(input_topic_names, input_names_long, input_names_short); - // sub_objects_array_.resize(input_topic_size_); - - // for (size_t i = 0; i < input_topic_size_; i++) { - // RCLCPP_INFO(get_logger(), "Subscribing to %s", input_topic_names_.at(i).c_str()); - // std::function func = - // std::bind(&MultiObjectTracker::onData, this, std::placeholders::_1, i); - // sub_objects_array_.at(i) = - // create_subscription(input_topic_names_.at(i), rclcpp::QoS{1}, func); - // } - // Create tf timer auto cti = std::make_shared( this->get_node_base_interface(), this->get_node_timers_interface()); @@ -232,13 +225,7 @@ void MultiObjectTracker::onTimer() if (elapsed_time < maximum_publish_latency) return; - // // update trackers from accumulated input objects - // if (objects_data_.empty()) return; - - // // sort by time - // sort(objects_data_.begin(), objects_data_.end(), [](const auto & a, const auto & b) { - // return a.first.seconds() < b.first.seconds(); - // }); + // get objects from the input manager and run process std::vector objects_data; if (input_manager_->getObjects(current_time, objects_data)) { for (const auto & objects : objects_data) { @@ -246,13 +233,6 @@ void MultiObjectTracker::onTimer() } } - // // run onMeasurement for each queue - // while (!objects_data_.empty()) { - // const auto & pair = objects_data_.front(); - // runProcess(std::make_shared(pair.second)); - // objects_data_.erase(objects_data_.begin()); - // } - // Publish checkAndPublish(current_time); } diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index 55d744ef47511..928b3874b4b8a 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -54,8 +54,7 @@ void InputStream::setObjects( objects_que_.pop_front(); } - // RCLCPP_INFO( - // node_.get_logger(), "InputStream::setObjects Que size: %zu", objects_que_.size()); + RCLCPP_INFO(node_.get_logger(), "InputStream::setObjects Que size: %zu", objects_que_.size()); // Filter parameters constexpr double gain = 0.05; @@ -88,18 +87,43 @@ void InputStream::getObjectsOlderThan( std::vector & objects) { objects.clear(); + + // remove objects older than the specified duration + const rclcpp::Time old_limit = time - rclcpp::Duration::from_seconds(duration); for (const auto & object : objects_que_) { - // Skip if the object is older than the specified duration const rclcpp::Time object_time = rclcpp::Time(object.header.stamp); - const double time_diff = (object_time - time).seconds(); - if (time_diff > duration) { - continue; + if (object_time < old_limit) { + objects_que_.pop_front(); + } else { + break; } + } + + for (const auto & object : objects_que_) { + // Skip if the object is older than the specified duration + const rclcpp::Time object_time = rclcpp::Time(object.header.stamp); + const double time_diff = (time - object_time).seconds(); // Add the object if the object is older than the specified time if (time_diff >= 0.0) { objects.push_back(object); + // remove the object from the queue + objects_que_.pop_front(); } } + RCLCPP_INFO( + node_.get_logger(), "InputStream::getObjectsOlderThan %s gives %zu objects", long_name_.c_str(), + objects.size()); + + // // message to show the older limit, time, old objects in the queue, latest object in the queue + // if (objects_que_.empty()) { + // RCLCPP_INFO( + // node_.get_logger(), "InputStream::getObjectsOlderThan %s empty queue", long_name_.c_str()); + // return; + // } + // RCLCPP_INFO( + // node_.get_logger(), "InputStream::getObjectsOlderThan %s older limit %f, time %f, %u, %u", + // long_name_.c_str(), old_limit.seconds(), time.seconds(), + // objects_que_.front().header.stamp.sec, objects_que_.back().header.stamp.sec); } InputManager::InputManager(rclcpp::Node & node) : node_(node) @@ -132,7 +156,7 @@ void InputManager::init( long_names.at(i).c_str(), input_topics.at(i).c_str()); std::function - func = std::bind(&InputStream::setObjects, input_stream, std::placeholders::_1); + func = std::bind(&InputStream::setObjects, input_streams_.at(i), std::placeholders::_1); sub_objects_array_.at(i) = node_.create_subscription( input_topics.at(i), rclcpp::QoS{1}, func); From 9ebaf6dee1b3165ff90dc57b44e9dbd9e7aed3b4 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 15 Apr 2024 13:53:13 +0900 Subject: [PATCH 13/67] fix: get object time argument revise Signed-off-by: Taekjin LEE --- .../processor/input_manager.hpp | 16 +++- .../src/processor/input_manager.cpp | 74 ++++++++++--------- 2 files changed, 53 insertions(+), 37 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 4afcf2a817675..6ebaba48a79c8 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 @@ -35,9 +35,23 @@ class InputStream void setObjects(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg); void getObjectsOlderThan( - const rclcpp::Time & time, const double duration, + const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, std::vector & objects); + void getNames(std::string & long_name, std::string & short_name) + { + long_name = long_name_; + short_name = short_name_; + } + void getTimeStatistics( + double & latency_mean, double & latency_var, double & interval_mean, double & interval_var) + { + latency_mean = latency_mean_; + latency_var = latency_var_; + interval_mean = interval_mean_; + interval_var = interval_var_; + } + private: rclcpp::Node & node_; diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index 928b3874b4b8a..dc459bdd344b2 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -83,28 +83,24 @@ void InputStream::setObjects( } void InputStream::getObjectsOlderThan( - const rclcpp::Time & time, const double duration, + const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, std::vector & objects) { objects.clear(); - // remove objects older than the specified duration - const rclcpp::Time old_limit = time - rclcpp::Duration::from_seconds(duration); + assert(object_latest_time.nanoseconds() > object_oldest_time.nanoseconds()); + for (const auto & object : objects_que_) { const rclcpp::Time object_time = rclcpp::Time(object.header.stamp); - if (object_time < old_limit) { + + // remove objects older than the specified duration + if (object_time < object_oldest_time) { objects_que_.pop_front(); - } else { - break; + continue; } - } - for (const auto & object : objects_que_) { - // Skip if the object is older than the specified duration - const rclcpp::Time object_time = rclcpp::Time(object.header.stamp); - const double time_diff = (time - object_time).seconds(); - // Add the object if the object is older than the specified time - if (time_diff >= 0.0) { + // Add the object if the object is older than the specified latest time + if (object_latest_time >= object_time) { objects.push_back(object); // remove the object from the queue objects_que_.pop_front(); @@ -113,17 +109,6 @@ void InputStream::getObjectsOlderThan( RCLCPP_INFO( node_.get_logger(), "InputStream::getObjectsOlderThan %s gives %zu objects", long_name_.c_str(), objects.size()); - - // // message to show the older limit, time, old objects in the queue, latest object in the queue - // if (objects_que_.empty()) { - // RCLCPP_INFO( - // node_.get_logger(), "InputStream::getObjectsOlderThan %s empty queue", long_name_.c_str()); - // return; - // } - // RCLCPP_INFO( - // node_.get_logger(), "InputStream::getObjectsOlderThan %s older limit %f, time %f, %u, %u", - // long_name_.c_str(), old_limit.seconds(), time.seconds(), - // objects_que_.front().header.stamp.sec, objects_que_.back().header.stamp.sec); } InputManager::InputManager(rclcpp::Node & node) : node_(node) @@ -134,12 +119,12 @@ void InputManager::init( const std::vector & input_topics, const std::vector & long_names, const std::vector & short_names) { - input_size_ = input_topics.size(); - // Check input sizes - RCLCPP_INFO( - node_.get_logger(), "InputManager::init Initializing input manager with %zu input streams", - input_size_); + input_size_ = input_topics.size(); + if (input_size_ == 0) { + RCLCPP_ERROR(node_.get_logger(), "InputManager::init No input streams"); + return; + } assert(input_size_ == long_names.size()); assert(input_size_ == short_names.size()); @@ -162,29 +147,46 @@ void InputManager::init( input_topics.at(i), rclcpp::QoS{1}, func); } - if (input_size_ > 0) { - is_initialized_ = true; - } + is_initialized_ = true; } bool InputManager::getObjects( const rclcpp::Time & now, std::vector & objects) { - RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Getting objects from input streams"); + if (!is_initialized_) { + RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Input manager is not initialized"); + return false; + } + RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Getting objects from input streams"); objects.clear(); + // Get the streams statistics + std::string long_name, short_name; + double latency_mean, latency_var, interval_mean, interval_var; + for (const auto & input_stream : input_streams_) { + input_stream->getNames(long_name, short_name); + input_stream->getTimeStatistics(latency_mean, latency_var, interval_mean, interval_var); + RCLCPP_INFO( + node_.get_logger(), + "InputManager::getObjects %s: latency mean: %f, var: %f, interval mean: " + "%f, var: %f", + long_name.c_str(), latency_mean, latency_var, interval_mean, interval_var); + } + // Get proper latency - constexpr double target_latency = 0.15; // [s], measurement to tracking latency, as much as the + constexpr double target_latency = 0.11; // [s], measurement to tracking latency, as much as the // detector latency, the less total latency constexpr double acceptable_band = 0.15; // [s], acceptable band from the target latency - rclcpp::Time time = now - rclcpp::Duration::from_seconds(target_latency); + const rclcpp::Time object_latest_time = now - rclcpp::Duration::from_seconds(target_latency); + const rclcpp::Time object_oldest_time = + object_latest_time - rclcpp::Duration::from_seconds(acceptable_band); // Get objects from all input streams for (const auto & input_stream : input_streams_) { std::vector objects_tmp; - input_stream->getObjectsOlderThan(time, acceptable_band, objects_tmp); + input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects_tmp); objects.insert(objects.end(), objects_tmp.begin(), objects_tmp.end()); } From c496a25ca3e2ae41266e12e4e50e652e3ee1c4f2 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 15 Apr 2024 16:42:59 +0900 Subject: [PATCH 14/67] feat: back to periodic publish, analyze input latency and timings Signed-off-by: Taekjin LEE --- .../multi_object_tracker_core.hpp | 7 -- .../processor/input_manager.hpp | 12 ++- .../src/multi_object_tracker_core.cpp | 66 +++------------ .../src/processor/input_manager.cpp | 81 +++++++++++++++---- 4 files changed, 88 insertions(+), 78 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 e3a2864b1faaf..69c707526eb12 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 @@ -85,11 +85,6 @@ class MultiObjectTracker : public rclcpp::Node std::unique_ptr data_association_; std::unique_ptr processor_; - // inputs - rclcpp::Subscription::SharedPtr sub_objects_{}; - std::vector::SharedPtr> sub_objects_array_{}; - rclcpp::Time last_measurement_time_; - // input manager std::unique_ptr input_manager_; @@ -98,8 +93,6 @@ class MultiObjectTracker : public rclcpp::Node std::vector> objects_data_{}; // callback functions - void onData(const DetectedObjects::ConstSharedPtr msg, const size_t array_number); - void onMeasurement(const DetectedObjects::ConstSharedPtr input_objects_msg); void onTimer(); // publish processes 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 6ebaba48a79c8..5047ffbd21dac 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 @@ -43,8 +43,12 @@ class InputStream long_name = long_name_; short_name = short_name_; } + + size_t getObjectsCount() const { return objects_que_.size(); } + void getTimeStatistics( - double & latency_mean, double & latency_var, double & interval_mean, double & interval_var) + double & latency_mean, double & latency_var, double & interval_mean, + double & interval_var) const { latency_mean = latency_mean_; latency_var = latency_var_; @@ -52,6 +56,9 @@ class InputStream interval_var = interval_var_; } + bool getTimestamps( + rclcpp::Time & latest_measurement_time, rclcpp::Time & latest_message_time) const; + private: rclcpp::Node & node_; @@ -82,6 +89,8 @@ class InputManager const std::vector & input_topics, const std::vector & long_names, const std::vector & short_names); + bool isInputsReady() const; + bool getObjects( const rclcpp::Time & now, std::vector & objects); @@ -92,6 +101,7 @@ class InputManager sub_objects_array_{}; bool is_initialized_{false}; + rclcpp::Time latest_object_time_; size_t input_size_; std::vector> input_streams_; 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 371a6d0996067..59b00f59d884c 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -87,9 +87,6 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) std::vector input_names_long = get_parameter("input_names_long").as_string_array(); std::vector input_names_short = get_parameter("input_names_short").as_string_array(); - // Initialize the last measurement time - last_measurement_time_ = this->now(); - // ROS interface - Publisher tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); @@ -120,8 +117,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) // If the delay compensation is enabled, the timer is used to publish the output at the correct // time. if (enable_delay_compensation) { - publisher_period_ = 1.0 / publish_rate; // [s] - constexpr double timer_multiplier = 20.0; // 20 times frequent for publish timing check + publisher_period_ = 1.0 / publish_rate; // [s] + constexpr double timer_multiplier = 1.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(), timer_period, std::bind(&MultiObjectTracker::onTimer, this)); @@ -168,62 +165,19 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) published_time_publisher_ = std::make_unique(this); } -void MultiObjectTracker::onData( - const DetectedObjects::ConstSharedPtr msg, const size_t array_number) -{ - std::pair pair = std::make_pair(msg->header.stamp, *msg); - objects_data_.push_back(pair); - - // debug message - RCLCPP_INFO( - get_logger(), "Received data from topic %s", input_topic_names_.at(array_number).c_str()); - if (last_measurement_time_ > msg->header.stamp) { - const double delta_time = (last_measurement_time_ - msg->header.stamp).seconds() * 1e3; - RCLCPP_INFO( - get_logger(), "Received data is older than the last measurement time by %f ms", delta_time); - return; - } - if (last_measurement_time_ < msg->header.stamp) { - last_measurement_time_ = msg->header.stamp; - } - - // onMeasurement(msg); -} - -void MultiObjectTracker::onMeasurement(const 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()); - - debugger_->startMeasurementTime(this->now(), measurement_time); - runProcess(*input_objects_msg); - debugger_->endMeasurementTime(this->now()); - - // 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()); - } - } -} - void MultiObjectTracker::onTimer() { + // if (!input_manager_->isInputsReady()) return; + const rclcpp::Time current_time = this->now(); - // 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; + // // 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) return; + // if (elapsed_time < maximum_publish_latency) return; // get objects from the input manager and run process std::vector objects_data; diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index dc459bdd344b2..85a911774d0bc 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -38,15 +38,29 @@ void InputStream::init( latency_var_ = 0.0; interval_mean_ = 1 / expected_rate_; interval_var_ = 0.0; + + latest_measurement_time_ = node_.now(); + latest_message_time_ = node_.now(); +} + +bool InputStream::getTimestamps( + rclcpp::Time & latest_measurement_time, rclcpp::Time & latest_message_time) const +{ + if (!is_time_initialized_) { + return false; + } + latest_measurement_time = latest_measurement_time_; + latest_message_time = latest_message_time_; + return true; } void InputStream::setObjects( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg) { - // debug message - RCLCPP_INFO( - node_.get_logger(), "InputStream::setObjects Received %s message from %s at %d", - long_name_.c_str(), input_topic_.c_str(), msg->header.stamp.sec); + // // debug message + // RCLCPP_INFO( + // node_.get_logger(), "InputStream::setObjects Received %s message from %s at %d.%d", + // long_name_.c_str(), input_topic_.c_str(), msg->header.stamp.sec, msg->header.stamp.nanosec); const autoware_auto_perception_msgs::msg::DetectedObjects object = *msg; objects_que_.push_back(object); @@ -54,7 +68,7 @@ void InputStream::setObjects( objects_que_.pop_front(); } - RCLCPP_INFO(node_.get_logger(), "InputStream::setObjects Que size: %zu", objects_que_.size()); + // RCLCPP_INFO(node_.get_logger(), "InputStream::setObjects Que size: %zu", objects_que_.size()); // Filter parameters constexpr double gain = 0.05; @@ -113,6 +127,7 @@ void InputStream::getObjectsOlderThan( InputManager::InputManager(rclcpp::Node & node) : node_(node) { + latest_object_time_ = node_.now(); } void InputManager::init( @@ -150,6 +165,18 @@ void InputManager::init( is_initialized_ = true; } +bool InputManager::isInputsReady() const +{ + if (!is_initialized_) { + RCLCPP_INFO( + node_.get_logger(), "InputManager::isMajorInputReady Input manager is not initialized"); + return false; + } + + // Check if the major input stream (index of 0) is ready + return input_streams_.at(0)->getObjectsCount() > 0; +} + bool InputManager::getObjects( const rclcpp::Time & now, std::vector & objects) @@ -162,26 +189,38 @@ bool InputManager::getObjects( RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Getting objects from input streams"); objects.clear(); - // Get the streams statistics + // ANALYSIS: Get the streams statistics std::string long_name, short_name; double latency_mean, latency_var, interval_mean, interval_var; + rclcpp::Time latest_measurement_time, latest_message_time; for (const auto & input_stream : input_streams_) { input_stream->getNames(long_name, short_name); input_stream->getTimeStatistics(latency_mean, latency_var, interval_mean, interval_var); + if (!input_stream->getTimestamps(latest_measurement_time, latest_message_time)) { + continue; + } + double latency_message = (now - latest_message_time).seconds(); + double latency_measurement = (now - latest_measurement_time).seconds(); RCLCPP_INFO( node_.get_logger(), - "InputManager::getObjects %s: latency mean: %f, var: %f, interval mean: " - "%f, var: %f", - long_name.c_str(), latency_mean, latency_var, interval_mean, interval_var); + "InputManager::getObjects %s: latency mean: %f, std: %f, interval mean: " + "%f, std: %f, latest measurement latency: %f, latest message latency: %f", + long_name.c_str(), latency_mean, std::sqrt(latency_var), interval_mean, + std::sqrt(interval_var), latency_measurement, latency_message); } // Get proper latency - constexpr double target_latency = 0.11; // [s], measurement to tracking latency, as much as the - // detector latency, the less total latency - constexpr double acceptable_band = 0.15; // [s], acceptable band from the target latency + constexpr double target_latency = 0.15; // [s], measurement to tracking latency, as much as the + // detector latency, the less total latency + constexpr double acceptable_latency = + 0.35; // [s], acceptable band from the target latency, larger than the target latency const rclcpp::Time object_latest_time = now - rclcpp::Duration::from_seconds(target_latency); - const rclcpp::Time object_oldest_time = - object_latest_time - rclcpp::Duration::from_seconds(acceptable_band); + rclcpp::Time object_oldest_time = now - rclcpp::Duration::from_seconds(acceptable_latency); + + // 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_; // Get objects from all input streams for (const auto & input_stream : input_streams_) { @@ -199,9 +238,23 @@ bool InputManager::getObjects( return (rclcpp::Time(a.header.stamp) - rclcpp::Time(b.header.stamp)).seconds() < 0; }); + // ANALYSIS: Obtained object time range + if (!objects.empty()) { + rclcpp::Time oldest_time(objects.front().header.stamp); + rclcpp::Time latest_time(objects.back().header.stamp); + RCLCPP_INFO( + node_.get_logger(), "InputManager::getObjects Object time range: %f - %f", + (now - latest_time).seconds(), (now - oldest_time).seconds()); + } + RCLCPP_INFO( node_.get_logger(), "InputManager::getObjects Got %zu objects from input streams", objects.size()); + // Update the latest object time + if (!objects.empty()) { + latest_object_time_ = rclcpp::Time(objects.back().header.stamp); + } + return !objects.empty(); } From 969483c902dfa6b50dad559f01362418fd40d15a Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 15 Apr 2024 17:27:27 +0900 Subject: [PATCH 15/67] fix: enable timing debugger Signed-off-by: Taekjin LEE --- .../src/multi_object_tracker_core.cpp | 15 +++++++++-- .../src/processor/input_manager.cpp | 26 +++++++++---------- 2 files changed, 26 insertions(+), 15 deletions(-) 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 59b00f59d884c..9714fba08029f 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -181,12 +181,23 @@ void MultiObjectTracker::onTimer() // get objects from the input manager and run process std::vector objects_data; - if (input_manager_->getObjects(current_time, objects_data)) { + const bool is_objects_ready = input_manager_->getObjects(current_time, objects_data); + if (is_objects_ready) { + debugger_->startMeasurementTime(this->now(), rclcpp::Time(objects_data.front().header.stamp)); + for (const auto & objects : objects_data) { runProcess(objects); } - } + // for debug + rclcpp::Time oldest_time(objects_data.front().header.stamp); + rclcpp::Time latest_time(objects_data.back().header.stamp); + RCLCPP_INFO( + this->get_logger(), "MultiObjectTracker::onTimer Objects time range: %f - %f", + (current_time - latest_time).seconds(), (current_time - oldest_time).seconds()); + + debugger_->endMeasurementTime(this->now()); + } // Publish checkAndPublish(current_time); } diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index 85a911774d0bc..d8bcc85e49d52 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -100,7 +100,7 @@ void InputStream::getObjectsOlderThan( const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, std::vector & objects) { - objects.clear(); + // objects.clear(); assert(object_latest_time.nanoseconds() > object_oldest_time.nanoseconds()); @@ -186,7 +186,6 @@ bool InputManager::getObjects( return false; } - RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Getting objects from input streams"); objects.clear(); // ANALYSIS: Get the streams statistics @@ -224,9 +223,10 @@ bool InputManager::getObjects( // Get objects from all input streams for (const auto & input_stream : input_streams_) { - std::vector objects_tmp; - input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects_tmp); - objects.insert(objects.end(), objects_tmp.begin(), objects_tmp.end()); + // std::vector objects_tmp; + // input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects_tmp); + // objects.insert(objects.end(), objects_tmp.begin(), objects_tmp.end()); + input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects); } // Sort objects by timestamp @@ -238,14 +238,14 @@ bool InputManager::getObjects( return (rclcpp::Time(a.header.stamp) - rclcpp::Time(b.header.stamp)).seconds() < 0; }); - // ANALYSIS: Obtained object time range - if (!objects.empty()) { - rclcpp::Time oldest_time(objects.front().header.stamp); - rclcpp::Time latest_time(objects.back().header.stamp); - RCLCPP_INFO( - node_.get_logger(), "InputManager::getObjects Object time range: %f - %f", - (now - latest_time).seconds(), (now - oldest_time).seconds()); - } + // // ANALYSIS: Obtained object time range + // if (!objects.empty()) { + // rclcpp::Time oldest_time(objects.front().header.stamp); + // rclcpp::Time latest_time(objects.back().header.stamp); + // RCLCPP_INFO( + // node_.get_logger(), "InputManager::getObjects Object time range: %f - %f", + // (now - latest_time).seconds(), (now - oldest_time).seconds()); + // } RCLCPP_INFO( node_.get_logger(), "InputManager::getObjects Got %zu objects from input streams", From ceaacc3c176237d47ef71cfa06cdc4823c8d53f4 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 15 Apr 2024 18:28:46 +0900 Subject: [PATCH 16/67] fix: separate object interval function Signed-off-by: Taekjin LEE --- .../processor/input_manager.hpp | 2 + .../src/multi_object_tracker_core.cpp | 3 +- .../src/processor/input_manager.cpp | 90 +++++++++---------- 3 files changed, 48 insertions(+), 47 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 5047ffbd21dac..249b6bcaadb19 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 @@ -90,6 +90,8 @@ class InputManager const std::vector & short_names); bool isInputsReady() const; + void getObjectTimeInterval( + const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time); bool getObjects( const rclcpp::Time & 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 9714fba08029f..9d3a701e26da5 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -118,7 +118,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) // time. if (enable_delay_compensation) { publisher_period_ = 1.0 / publish_rate; // [s] - constexpr double timer_multiplier = 1.0; // 20 times frequent for publish timing check + constexpr double timer_multiplier = 1.0; // 5 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(), timer_period, std::bind(&MultiObjectTracker::onTimer, this)); @@ -167,6 +167,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) void MultiObjectTracker::onTimer() { + // // Check the input manager // if (!input_manager_->isInputsReady()) return; const rclcpp::Time current_time = this->now(); diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index d8bcc85e49d52..b9b5f25ea4f96 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -34,7 +34,7 @@ void InputStream::init( // Initialize latency statistics expected_rate_ = 10.0; - latency_mean_ = 0.10; // [s] + latency_mean_ = 0.18; // [s] latency_var_ = 0.0; interval_mean_ = 1 / expected_rate_; interval_var_ = 0.0; @@ -100,8 +100,6 @@ void InputStream::getObjectsOlderThan( const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, std::vector & objects) { - // objects.clear(); - assert(object_latest_time.nanoseconds() > object_oldest_time.nanoseconds()); for (const auto & object : objects_que_) { @@ -177,55 +175,64 @@ bool InputManager::isInputsReady() const return input_streams_.at(0)->getObjectsCount() > 0; } -bool InputManager::getObjects( - const rclcpp::Time & now, - std::vector & objects) +void InputManager::getObjectTimeInterval( + const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time) { - if (!is_initialized_) { - RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Input manager is not initialized"); - return false; - } - - objects.clear(); - - // ANALYSIS: Get the streams statistics - std::string long_name, short_name; - double latency_mean, latency_var, interval_mean, interval_var; - rclcpp::Time latest_measurement_time, latest_message_time; - for (const auto & input_stream : input_streams_) { - input_stream->getNames(long_name, short_name); - input_stream->getTimeStatistics(latency_mean, latency_var, interval_mean, interval_var); - if (!input_stream->getTimestamps(latest_measurement_time, latest_message_time)) { - continue; + { + // ANALYSIS: Get the streams statistics + std::string long_name, short_name; + double latency_mean, latency_var, interval_mean, interval_var; + rclcpp::Time latest_measurement_time, latest_message_time; + for (const auto & input_stream : input_streams_) { + input_stream->getNames(long_name, short_name); + input_stream->getTimeStatistics(latency_mean, latency_var, interval_mean, interval_var); + if (!input_stream->getTimestamps(latest_measurement_time, latest_message_time)) { + continue; + } + double latency_message = (now - latest_message_time).seconds(); + double latency_measurement = (now - latest_measurement_time).seconds(); + RCLCPP_INFO( + node_.get_logger(), + "InputManager::getObjects %s: latency mean: %f, std: %f, interval mean: " + "%f, std: %f, latest measurement latency: %f, latest message latency: %f", + long_name.c_str(), latency_mean, std::sqrt(latency_var), interval_mean, + std::sqrt(interval_var), latency_measurement, latency_message); } - double latency_message = (now - latest_message_time).seconds(); - double latency_measurement = (now - latest_measurement_time).seconds(); - RCLCPP_INFO( - node_.get_logger(), - "InputManager::getObjects %s: latency mean: %f, std: %f, interval mean: " - "%f, std: %f, latest measurement latency: %f, latest message latency: %f", - long_name.c_str(), latency_mean, std::sqrt(latency_var), interval_mean, - std::sqrt(interval_var), latency_measurement, latency_message); } // Get proper latency - constexpr double target_latency = 0.15; // [s], measurement to tracking latency, as much as the - // detector latency, the less total latency + constexpr double target_latency = 0.15; // [s], measurement to tracking latency + // process latency of a main detection + margin + constexpr double acceptable_latency = 0.35; // [s], acceptable band from the target latency, larger than the target latency - const rclcpp::Time object_latest_time = now - rclcpp::Duration::from_seconds(target_latency); - rclcpp::Time object_oldest_time = now - rclcpp::Duration::from_seconds(acceptable_latency); + object_latest_time = now - rclcpp::Duration::from_seconds(target_latency); + object_oldest_time = now - rclcpp::Duration::from_seconds(acceptable_latency); // 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_; +} + +bool InputManager::getObjects( + const rclcpp::Time & now, + std::vector & objects) +{ + if (!is_initialized_) { + RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Input manager is not initialized"); + return false; + } + + // Clear the objects + objects.clear(); + + // Get the time interval for the objects + rclcpp::Time object_latest_time, object_oldest_time; + getObjectTimeInterval(now, object_latest_time, object_oldest_time); // Get objects from all input streams for (const auto & input_stream : input_streams_) { - // std::vector objects_tmp; - // input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects_tmp); - // objects.insert(objects.end(), objects_tmp.begin(), objects_tmp.end()); input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects); } @@ -238,15 +245,6 @@ bool InputManager::getObjects( return (rclcpp::Time(a.header.stamp) - rclcpp::Time(b.header.stamp)).seconds() < 0; }); - // // ANALYSIS: Obtained object time range - // if (!objects.empty()) { - // rclcpp::Time oldest_time(objects.front().header.stamp); - // rclcpp::Time latest_time(objects.back().header.stamp); - // RCLCPP_INFO( - // node_.get_logger(), "InputManager::getObjects Object time range: %f - %f", - // (now - latest_time).seconds(), (now - oldest_time).seconds()); - // } - RCLCPP_INFO( node_.get_logger(), "InputManager::getObjects Got %zu objects from input streams", objects.size()); From c556460e7ccb3284ac23ec7bc0dd7e4ece03db6a Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 15 Apr 2024 19:24:45 +0900 Subject: [PATCH 17/67] feat: prepare message triggered process Signed-off-by: Taekjin LEE --- .../multi_object_tracker_core.hpp | 1 + .../processor/input_manager.hpp | 2 +- .../src/multi_object_tracker_core.cpp | 23 ++++++++++++++++++- .../src/processor/input_manager.cpp | 8 +++---- 4 files changed, 28 insertions(+), 6 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 69c707526eb12..fd640a073e038 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 @@ -94,6 +94,7 @@ class MultiObjectTracker : public rclcpp::Node // callback functions void onTimer(); + void onMessage(const std::vector & objects_data); // publish processes void runProcess(const DetectedObjects & input_objects_msg); 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 249b6bcaadb19..90148b19c5cf6 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 @@ -32,7 +32,7 @@ class InputStream void init( const std::string & input_topic, const std::string & long_name, const std::string & short_name); - void setObjects(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg); + void onMessage(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg); void getObjectsOlderThan( const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, 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 9d3a701e26da5..01136c15a4f4f 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -165,6 +165,28 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) published_time_publisher_ = std::make_unique(this); } +void MultiObjectTracker::onMessage(const std::vector & objects_data) +{ + const rclcpp::Time current_time = this->now(); + debugger_->startMeasurementTime(this->now(), rclcpp::Time(objects_data.front().header.stamp)); + + // run process for each DetectedObjects + for (const auto & objects : objects_data) { + runProcess(objects); + } + + // for debug + rclcpp::Time oldest_time(objects_data.front().header.stamp); + rclcpp::Time latest_time(objects_data.back().header.stamp); + RCLCPP_INFO( + this->get_logger(), "MultiObjectTracker::onTimer Objects time range: %f - %f", + (current_time - latest_time).seconds(), (current_time - oldest_time).seconds()); + + debugger_->endMeasurementTime(this->now()); + + // Publish +} + void MultiObjectTracker::onTimer() { // // Check the input manager @@ -177,7 +199,6 @@ void MultiObjectTracker::onTimer() // // 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) return; // get objects from the input manager and run process diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index b9b5f25ea4f96..f0c5655b05c6e 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -54,12 +54,12 @@ bool InputStream::getTimestamps( return true; } -void InputStream::setObjects( +void InputStream::onMessage( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg) { // // debug message // RCLCPP_INFO( - // node_.get_logger(), "InputStream::setObjects Received %s message from %s at %d.%d", + // node_.get_logger(), "InputStream::onMessage Received %s message from %s at %d.%d", // long_name_.c_str(), input_topic_.c_str(), msg->header.stamp.sec, msg->header.stamp.nanosec); const autoware_auto_perception_msgs::msg::DetectedObjects object = *msg; @@ -68,7 +68,7 @@ void InputStream::setObjects( objects_que_.pop_front(); } - // RCLCPP_INFO(node_.get_logger(), "InputStream::setObjects Que size: %zu", objects_que_.size()); + // RCLCPP_INFO(node_.get_logger(), "InputStream::onMessage Que size: %zu", objects_que_.size()); // Filter parameters constexpr double gain = 0.05; @@ -154,7 +154,7 @@ void InputManager::init( long_names.at(i).c_str(), input_topics.at(i).c_str()); std::function - func = std::bind(&InputStream::setObjects, input_streams_.at(i), std::placeholders::_1); + func = std::bind(&InputStream::onMessage, input_streams_.at(i), std::placeholders::_1); sub_objects_array_.at(i) = node_.create_subscription( input_topics.at(i), rclcpp::QoS{1}, func); From 4afab3fec1b56a1fcd75292586d914031e0c5a21 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 16 Apr 2024 12:14:11 +0900 Subject: [PATCH 18/67] feat: trigger tracker by main message arrive Signed-off-by: Taekjin LEE --- .../multi_object_tracker_core.hpp | 1 + .../processor/input_manager.hpp | 24 +++-- .../src/multi_object_tracker_core.cpp | 92 ++++++++++--------- .../src/processor/input_manager.cpp | 26 ++++-- 4 files changed, 84 insertions(+), 59 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 fd640a073e038..7ac0cbb303799 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 @@ -94,6 +94,7 @@ class MultiObjectTracker : public rclcpp::Node // callback functions void onTimer(); + void onTrigger(); void onMessage(const std::vector & objects_data); // publish processes 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 90148b19c5cf6..4626a372ce5ea 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 @@ -27,25 +27,28 @@ class InputStream { public: - explicit InputStream(rclcpp::Node & node); + explicit InputStream(rclcpp::Node & node, size_t & index); void init( const std::string & input_topic, const std::string & long_name, const std::string & short_name); + void setQueueSize(size_t que_size) { que_size_ = que_size; } + void setTriggerFunction(std::function func_trigger) + { + func_trigger_ = func_trigger; + } + void onMessage(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg); void getObjectsOlderThan( const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, std::vector & objects); - void getNames(std::string & long_name, std::string & short_name) { long_name = long_name_; short_name = short_name_; } - size_t getObjectsCount() const { return objects_que_.size(); } - void getTimeStatistics( double & latency_mean, double & latency_var, double & interval_mean, double & interval_var) const @@ -55,20 +58,22 @@ class InputStream interval_mean = interval_mean_; interval_var = interval_var_; } - bool getTimestamps( rclcpp::Time & latest_measurement_time, rclcpp::Time & latest_message_time) const; private: rclcpp::Node & node_; + size_t index_; std::string input_topic_; std::string long_name_; std::string short_name_; - size_t que_size_{50}; + size_t que_size_{30}; std::deque objects_que_; + std::function func_trigger_; + bool is_time_initialized_{false}; double expected_rate_; double latency_mean_; @@ -88,11 +93,12 @@ class InputManager void init( const std::vector & input_topics, const std::vector & long_names, const std::vector & short_names); + void setTriggerFunction(std::function func_trigger) { func_trigger_ = func_trigger; } + + void onTrigger(const size_t & index) const; - bool isInputsReady() const; void getObjectTimeInterval( const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time); - bool getObjects( const rclcpp::Time & now, std::vector & objects); @@ -107,6 +113,8 @@ class InputManager size_t input_size_; std::vector> input_streams_; + + std::function func_trigger_; }; #endif // MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ 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 01136c15a4f4f..f4f7a30de7aae 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -107,6 +107,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) // Initialize input manager input_manager_ = std::make_unique(*this); input_manager_->init(input_topic_names, input_names_long, input_names_short); + // Set trigger function + input_manager_->setTriggerFunction(std::bind(&MultiObjectTracker::onTrigger, this)); // Create tf timer auto cti = std::make_shared( @@ -117,8 +119,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) // If the delay compensation is enabled, the timer is used to publish the output at the correct // time. if (enable_delay_compensation) { - publisher_period_ = 1.0 / publish_rate; // [s] - constexpr double timer_multiplier = 1.0; // 5 times frequent for publish timing check + 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(), timer_period, std::bind(&MultiObjectTracker::onTimer, this)); @@ -165,63 +167,71 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) published_time_publisher_ = std::make_unique(this); } -void MultiObjectTracker::onMessage(const std::vector & objects_data) +void MultiObjectTracker::onTrigger() { const rclcpp::Time current_time = this->now(); - debugger_->startMeasurementTime(this->now(), rclcpp::Time(objects_data.front().header.stamp)); - - // run process for each DetectedObjects - for (const auto & objects : objects_data) { - runProcess(objects); + // get objects from the input manager and run process + std::vector objects_data; + const bool is_objects_ready = input_manager_->getObjects(current_time, objects_data); + if (!is_objects_ready) return; + + onMessage(objects_data); + const rclcpp::Time latest_time(objects_data.back().header.stamp); + + // Publish objects if the timer is not enabled + if (publish_timer_ == nullptr) { + // if the delay compensation is disabled, publish the objects in the latest time + publish(latest_time); + } else { + // Publish if the next publish time is close + const double minimum_publish_interval = publisher_period_ * 0.70; // 70% of the period + if ((current_time - last_published_time_).seconds() > minimum_publish_interval) { + checkAndPublish(current_time); + } } - - // for debug - rclcpp::Time oldest_time(objects_data.front().header.stamp); - rclcpp::Time latest_time(objects_data.back().header.stamp); - RCLCPP_INFO( - this->get_logger(), "MultiObjectTracker::onTimer Objects time range: %f - %f", - (current_time - latest_time).seconds(), (current_time - oldest_time).seconds()); - - debugger_->endMeasurementTime(this->now()); - - // Publish } void MultiObjectTracker::onTimer() { - // // Check the input manager - // if (!input_manager_->isInputsReady()) return; - const rclcpp::Time current_time = this->now(); - // // 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) 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) return; // get objects from the input manager and run process std::vector objects_data; const bool is_objects_ready = input_manager_->getObjects(current_time, objects_data); if (is_objects_ready) { - debugger_->startMeasurementTime(this->now(), rclcpp::Time(objects_data.front().header.stamp)); + onMessage(objects_data); + } - for (const auto & objects : objects_data) { - runProcess(objects); - } + // Publish + checkAndPublish(current_time); +} - // for debug - rclcpp::Time oldest_time(objects_data.front().header.stamp); - rclcpp::Time latest_time(objects_data.back().header.stamp); - RCLCPP_INFO( - this->get_logger(), "MultiObjectTracker::onTimer Objects time range: %f - %f", - (current_time - latest_time).seconds(), (current_time - oldest_time).seconds()); +void MultiObjectTracker::onMessage(const std::vector & objects_data) +{ + const rclcpp::Time current_time = this->now(); - debugger_->endMeasurementTime(this->now()); + // process start + debugger_->startMeasurementTime(this->now(), rclcpp::Time(objects_data.front().header.stamp)); + // run process for each DetectedObjects + for (const auto & objects : objects_data) { + runProcess(objects); } - // Publish - checkAndPublish(current_time); + // process end + debugger_->endMeasurementTime(this->now()); + + // for debug + const rclcpp::Time oldest_time(objects_data.front().header.stamp); + const rclcpp::Time latest_time(objects_data.back().header.stamp); + RCLCPP_INFO( + this->get_logger(), "MultiObjectTracker::onTimer Objects time range: %f - %f", + (current_time - latest_time).seconds(), (current_time - oldest_time).seconds()); } void MultiObjectTracker::runProcess(const DetectedObjects & input_objects_msg) diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index f0c5655b05c6e..25dbfd3edc41c 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -17,7 +17,7 @@ #include #include -InputStream::InputStream(rclcpp::Node & node) : node_(node) +InputStream::InputStream(rclcpp::Node & node, size_t & index) : node_(node), index_(index) { } @@ -94,6 +94,11 @@ void InputStream::onMessage( latency_mean_ = (1.0 - gain) * latency_mean_ + gain * latency; const double latency_delta = latency - latency_mean_; latency_var_ = (1.0 - gain) * latency_var_ + gain * latency_delta * latency_delta; + + // trigger the function if it is set + if (func_trigger_) { + func_trigger_(index_); + } } void InputStream::getObjectsOlderThan( @@ -144,8 +149,10 @@ void InputManager::init( sub_objects_array_.resize(input_size_); for (size_t i = 0; i < input_size_; i++) { - InputStream input_stream(node_); + InputStream input_stream(node_, i); input_stream.init(input_topics.at(i), long_names.at(i), short_names.at(i)); + input_stream.setTriggerFunction( + std::bind(&InputManager::onTrigger, this, std::placeholders::_1)); input_streams_.push_back(std::make_shared(input_stream)); // Set subscription @@ -163,16 +170,15 @@ void InputManager::init( is_initialized_ = true; } -bool InputManager::isInputsReady() const +void InputManager::onTrigger(const size_t & index) const { - if (!is_initialized_) { - RCLCPP_INFO( - node_.get_logger(), "InputManager::isMajorInputReady Input manager is not initialized"); - return false; - } + // input stream index of 0 is the target(main) input stream + const size_t target_idx = 0; - // Check if the major input stream (index of 0) is ready - return input_streams_.at(0)->getObjectsCount() > 0; + // when the main stream triggers, call the trigger function + if (index == target_idx && func_trigger_) { + func_trigger_(); + } } void InputManager::getObjectTimeInterval( From 23a9df4e93945b322a3d24941088667968bdd372 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 16 Apr 2024 15:09:34 +0900 Subject: [PATCH 19/67] chore: clean-up, set namespace Signed-off-by: Taekjin LEE --- .../processor/input_manager.hpp | 19 ++++++---- .../src/multi_object_tracker_core.cpp | 4 +- .../src/processor/input_manager.cpp | 38 +++++++++---------- 3 files changed, 32 insertions(+), 29 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 4626a372ce5ea..da19d5628be9c 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 @@ -20,10 +20,16 @@ #include "autoware_auto_perception_msgs/msg/detected_objects.hpp" #include +#include #include #include #include +namespace multi_object_tracker +{ +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjects; + class InputStream { public: @@ -42,7 +48,7 @@ class InputStream void getObjectsOlderThan( const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, - std::vector & objects); + std::vector & objects); void getNames(std::string & long_name, std::string & short_name) { long_name = long_name_; @@ -70,7 +76,7 @@ class InputStream std::string short_name_; size_t que_size_{30}; - std::deque objects_que_; + std::deque objects_que_; std::function func_trigger_; @@ -99,14 +105,11 @@ class InputManager void getObjectTimeInterval( const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time); - bool getObjects( - const rclcpp::Time & now, - std::vector & objects); + bool getObjects(const rclcpp::Time & now, std::vector & objects); private: rclcpp::Node & node_; - std::vector::SharedPtr> - sub_objects_array_{}; + std::vector::SharedPtr> sub_objects_array_{}; bool is_initialized_{false}; rclcpp::Time latest_object_time_; @@ -117,4 +120,6 @@ class InputManager std::function func_trigger_; }; +} // namespace multi_object_tracker + #endif // MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_ 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 f4f7a30de7aae..f3a272acdbad7 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -171,7 +171,7 @@ void MultiObjectTracker::onTrigger() { const rclcpp::Time current_time = this->now(); // get objects from the input manager and run process - std::vector objects_data; + std::vector objects_data; const bool is_objects_ready = input_manager_->getObjects(current_time, objects_data); if (!is_objects_ready) return; @@ -203,7 +203,7 @@ void MultiObjectTracker::onTimer() if (elapsed_time < maximum_publish_latency) return; // get objects from the input manager and run process - std::vector objects_data; + std::vector objects_data; const bool is_objects_ready = input_manager_->getObjects(current_time, objects_data); if (is_objects_ready) { onMessage(objects_data); diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index 25dbfd3edc41c..b9ed035f1e2c6 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -15,7 +15,9 @@ #include "multi_object_tracker/processor/input_manager.hpp" #include -#include + +namespace multi_object_tracker +{ InputStream::InputStream(rclcpp::Node & node, size_t & index) : node_(node), index_(index) { @@ -62,8 +64,8 @@ void InputStream::onMessage( // node_.get_logger(), "InputStream::onMessage Received %s message from %s at %d.%d", // long_name_.c_str(), input_topic_.c_str(), msg->header.stamp.sec, msg->header.stamp.nanosec); - const autoware_auto_perception_msgs::msg::DetectedObjects object = *msg; - objects_que_.push_back(object); + const DetectedObjects objects = *msg; + objects_que_.push_back(objects); if (objects_que_.size() > que_size_) { objects_que_.pop_front(); } @@ -84,7 +86,7 @@ void InputStream::onMessage( // Update time latest_message_time_ = now; - latest_measurement_time_ = object.header.stamp; + latest_measurement_time_ = objects.header.stamp; if (!is_time_initialized_) is_time_initialized_ = true; // Calculate latency @@ -103,7 +105,7 @@ void InputStream::onMessage( void InputStream::getObjectsOlderThan( const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, - std::vector & objects) + std::vector & objects) { assert(object_latest_time.nanoseconds() > object_oldest_time.nanoseconds()); @@ -123,9 +125,9 @@ void InputStream::getObjectsOlderThan( objects_que_.pop_front(); } } - RCLCPP_INFO( - node_.get_logger(), "InputStream::getObjectsOlderThan %s gives %zu objects", long_name_.c_str(), - objects.size()); + // RCLCPP_INFO( + // node_.get_logger(), "InputStream::getObjectsOlderThan %s gives %zu objects", + // long_name_.c_str(), objects.size()); } InputManager::InputManager(rclcpp::Node & node) : node_(node) @@ -159,12 +161,10 @@ void InputManager::init( RCLCPP_INFO( node_.get_logger(), "InputManager::init Initializing %s input stream from %s", long_names.at(i).c_str(), input_topics.at(i).c_str()); - std::function - func = std::bind(&InputStream::onMessage, input_streams_.at(i), std::placeholders::_1); + std::function func = + std::bind(&InputStream::onMessage, input_streams_.at(i), std::placeholders::_1); sub_objects_array_.at(i) = - node_.create_subscription( - input_topics.at(i), rclcpp::QoS{1}, func); + node_.create_subscription(input_topics.at(i), rclcpp::QoS{1}, func); } is_initialized_ = true; @@ -221,9 +221,7 @@ void InputManager::getObjectTimeInterval( object_oldest_time > latest_object_time_ ? object_oldest_time : latest_object_time_; } -bool InputManager::getObjects( - const rclcpp::Time & now, - std::vector & objects) +bool InputManager::getObjects(const rclcpp::Time & now, std::vector & objects) { if (!is_initialized_) { RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Input manager is not initialized"); @@ -238,16 +236,14 @@ bool InputManager::getObjects( getObjectTimeInterval(now, object_latest_time, object_oldest_time); // Get objects from all input streams + // adds-up to the objects vector for efficient processing for (const auto & input_stream : input_streams_) { input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects); } // Sort objects by timestamp std::sort( - objects.begin(), objects.end(), - []( - const autoware_auto_perception_msgs::msg::DetectedObjects & a, - const autoware_auto_perception_msgs::msg::DetectedObjects & b) { + objects.begin(), objects.end(), [](const DetectedObjects & a, const DetectedObjects & b) { return (rclcpp::Time(a.header.stamp) - rclcpp::Time(b.header.stamp)).seconds() < 0; }); @@ -262,3 +258,5 @@ bool InputManager::getObjects( return !objects.empty(); } + +} // namespace multi_object_tracker From 787d48bd343efc948a2cfb5ab821f19d16c3de23 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 16 Apr 2024 17:57:42 +0900 Subject: [PATCH 20/67] feat: object lists with detector index Signed-off-by: Taekjin LEE --- .../multi_object_tracker_core.hpp | 2 +- .../processor/input_manager.hpp | 6 +++-- .../src/multi_object_tracker_core.cpp | 27 ++++++++++--------- .../src/processor/input_manager.cpp | 26 ++++++++++-------- 4 files changed, 34 insertions(+), 27 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 7ac0cbb303799..d917cb90e6fb6 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 @@ -95,7 +95,7 @@ class MultiObjectTracker : public rclcpp::Node // callback functions void onTimer(); void onTrigger(); - void onMessage(const std::vector & objects_data); + void onMessage(const std::vector> & objects_data); // publish processes void runProcess(const DetectedObjects & input_objects_msg); 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 da19d5628be9c..116059426cd23 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 @@ -23,6 +23,7 @@ #include #include #include +#include #include namespace multi_object_tracker @@ -48,7 +49,7 @@ class InputStream void getObjectsOlderThan( const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, - std::vector & objects); + std::vector> & objects); void getNames(std::string & long_name, std::string & short_name) { long_name = long_name_; @@ -105,7 +106,8 @@ class InputManager void getObjectTimeInterval( const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time); - bool getObjects(const rclcpp::Time & now, std::vector & objects); + bool getObjects( + const rclcpp::Time & now, std::vector> & objects_list); private: rclcpp::Node & node_; 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 f3a272acdbad7..88ea287f32266 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -171,12 +171,12 @@ void MultiObjectTracker::onTrigger() { const rclcpp::Time current_time = this->now(); // get objects from the input manager and run process - std::vector objects_data; - const bool is_objects_ready = input_manager_->getObjects(current_time, objects_data); + std::vector> objects_list; + const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list); if (!is_objects_ready) return; - onMessage(objects_data); - const rclcpp::Time latest_time(objects_data.back().header.stamp); + onMessage(objects_list); + const rclcpp::Time latest_time(objects_list.back().second.header.stamp); // Publish objects if the timer is not enabled if (publish_timer_ == nullptr) { @@ -203,32 +203,33 @@ void MultiObjectTracker::onTimer() if (elapsed_time < maximum_publish_latency) return; // get objects from the input manager and run process - std::vector objects_data; - const bool is_objects_ready = input_manager_->getObjects(current_time, objects_data); + std::vector> objects_list; + const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list); if (is_objects_ready) { - onMessage(objects_data); + onMessage(objects_list); } // Publish checkAndPublish(current_time); } -void MultiObjectTracker::onMessage(const std::vector & objects_data) +void MultiObjectTracker::onMessage( + const std::vector> & objects_list) { const rclcpp::Time current_time = this->now(); + const rclcpp::Time oldest_time(objects_list.front().second.header.stamp); // process start - debugger_->startMeasurementTime(this->now(), rclcpp::Time(objects_data.front().header.stamp)); + debugger_->startMeasurementTime(this->now(), oldest_time); // run process for each DetectedObjects - for (const auto & objects : objects_data) { - runProcess(objects); + for (const auto & objects_data : objects_list) { + runProcess(objects_data.second); } // process end debugger_->endMeasurementTime(this->now()); // for debug - const rclcpp::Time oldest_time(objects_data.front().header.stamp); - const rclcpp::Time latest_time(objects_data.back().header.stamp); + const rclcpp::Time latest_time(objects_list.back().second.header.stamp); RCLCPP_INFO( this->get_logger(), "MultiObjectTracker::onTimer Objects time range: %f - %f", (current_time - latest_time).seconds(), (current_time - oldest_time).seconds()); diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index b9ed035f1e2c6..bf2ee2165fb22 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -105,7 +105,7 @@ void InputStream::onMessage( void InputStream::getObjectsOlderThan( const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, - std::vector & objects) + std::vector> & objects_list) { assert(object_latest_time.nanoseconds() > object_oldest_time.nanoseconds()); @@ -120,7 +120,8 @@ void InputStream::getObjectsOlderThan( // Add the object if the object is older than the specified latest time if (object_latest_time >= object_time) { - objects.push_back(object); + std::pair object_pair(index_, object); + objects_list.push_back(object_pair); // remove the object from the queue objects_que_.pop_front(); } @@ -221,7 +222,8 @@ void InputManager::getObjectTimeInterval( object_oldest_time > latest_object_time_ ? object_oldest_time : latest_object_time_; } -bool InputManager::getObjects(const rclcpp::Time & now, std::vector & objects) +bool InputManager::getObjects( + const rclcpp::Time & now, std::vector> & objects_list) { if (!is_initialized_) { RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Input manager is not initialized"); @@ -229,7 +231,7 @@ bool InputManager::getObjects(const rclcpp::Time & now, std::vectorgetObjectsOlderThan(object_latest_time, object_oldest_time, objects); + input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects_list); } // Sort objects by timestamp std::sort( - objects.begin(), objects.end(), [](const DetectedObjects & a, const DetectedObjects & b) { - return (rclcpp::Time(a.header.stamp) - rclcpp::Time(b.header.stamp)).seconds() < 0; + objects_list.begin(), objects_list.end(), + [](const std::pair & a, const std::pair & b) { + return (rclcpp::Time(a.second.header.stamp) - rclcpp::Time(b.second.header.stamp)).seconds() < + 0; }); RCLCPP_INFO( node_.get_logger(), "InputManager::getObjects Got %zu objects from input streams", - objects.size()); + objects_list.size()); // Update the latest object time - if (!objects.empty()) { - latest_object_time_ = rclcpp::Time(objects.back().header.stamp); + if (!objects_list.empty()) { + latest_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp); } - return !objects.empty(); + return !objects_list.empty(); } } // namespace multi_object_tracker From b4993c2a1df2793b69b50a9145e1279427ce9594 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 16 Apr 2024 18:42:51 +0900 Subject: [PATCH 21/67] feat: define input channel struct Signed-off-by: Taekjin LEE --- .../multi_object_tracker_core.hpp | 3 +- .../processor/input_manager.hpp | 18 +++++--- .../src/multi_object_tracker_core.cpp | 43 ++++++++++++++----- .../src/processor/input_manager.cpp | 29 ++++++------- 4 files changed, 57 insertions(+), 36 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 d917cb90e6fb6..45ee40eddc369 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 @@ -88,9 +88,8 @@ class MultiObjectTracker : public rclcpp::Node // input manager std::unique_ptr input_manager_; - std::vector input_topic_names_{}; + std::vector input_channels_{}; size_t input_topic_size_{}; - std::vector> objects_data_{}; // callback functions void onTimer(); 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 116059426cd23..405b98ef1650a 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 @@ -28,16 +28,24 @@ namespace multi_object_tracker { -using autoware_auto_perception_msgs::msg::DetectedObject; using autoware_auto_perception_msgs::msg::DetectedObjects; +struct InputChannel +{ + size_t index; // index of the input channel + 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_latency = 0.2; // [s] +}; + class InputStream { public: explicit InputStream(rclcpp::Node & node, size_t & index); - void init( - const std::string & input_topic, const std::string & long_name, const std::string & short_name); + void init(const InputChannel & input_channel); void setQueueSize(size_t que_size) { que_size_ = que_size; } void setTriggerFunction(std::function func_trigger) @@ -97,9 +105,7 @@ class InputManager public: explicit InputManager(rclcpp::Node & node); - void init( - const std::vector & input_topics, const std::vector & long_names, - const std::vector & short_names); + void init(const std::vector & input_channels); void setTriggerFunction(std::function func_trigger) { func_trigger_ = func_trigger; } void onTrigger(const size_t & index) 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 88ea287f32266..e80cab8022ff9 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -87,6 +87,16 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) std::vector input_names_long = get_parameter("input_names_long").as_string_array(); std::vector input_names_short = get_parameter("input_names_short").as_string_array(); + input_channels_.resize(input_topic_names.size()); + assert(input_topic_names.size() == input_names_long.size()); + assert(input_topic_names.size() == input_names_short.size()); + for (size_t i = 0; i < input_topic_names.size(); i++) { + input_channels_[i].index = i; + 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]; + } + // ROS interface - Publisher tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); @@ -97,18 +107,11 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) } input_topic_size_ = input_topic_names.size(); - // print input information - for (size_t i = 0; i < input_topic_size_; i++) { - RCLCPP_INFO( - get_logger(), "Subscribing to %s %s %s", input_topic_names.at(i).c_str(), - input_names_long.at(i).c_str(), input_names_short.at(i).c_str()); - } - // Initialize input manager input_manager_ = std::make_unique(*this); - input_manager_->init(input_topic_names, input_names_long, input_names_short); - // Set trigger function - input_manager_->setTriggerFunction(std::bind(&MultiObjectTracker::onTrigger, this)); + input_manager_->init(input_channels_); // Initialize input manager, set subscriptions + input_manager_->setTriggerFunction( + std::bind(&MultiObjectTracker::onTrigger, this)); // Set trigger function // Create tf timer auto cti = std::make_shared( @@ -231,8 +234,26 @@ void MultiObjectTracker::onMessage( // for debug const rclcpp::Time latest_time(objects_list.back().second.header.stamp); RCLCPP_INFO( - this->get_logger(), "MultiObjectTracker::onTimer Objects time range: %f - %f", + this->get_logger(), "MultiObjectTracker::onMessage Objects time range: %f - %f", (current_time - latest_time).seconds(), (current_time - oldest_time).seconds()); + + // count objects on each channel + std::vector object_counts; + object_counts.resize(input_topic_size_); + // initialize to zero + for (size_t i = 0; i < input_topic_size_; i++) { + object_counts[i] = 0; + } + for (const auto & objects_data : objects_list) { + object_counts[objects_data.first] += 1; + } + // print result + std::string object_counts_str = "MultiObjectTracker::onMessage Object counts: "; + for (size_t i = 0; i < input_topic_size_; i++) { + object_counts_str += + input_channels_[i].long_name + " " + std::to_string(object_counts[i]) + " "; + } + RCLCPP_INFO(this->get_logger(), object_counts_str.c_str()); } void MultiObjectTracker::runProcess(const DetectedObjects & input_objects_msg) diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index bf2ee2165fb22..605f4b98e9e1b 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -23,20 +23,19 @@ InputStream::InputStream(rclcpp::Node & node, size_t & index) : node_(node), ind { } -void InputStream::init( - const std::string & input_topic, const std::string & long_name, const std::string & short_name) +void InputStream::init(const InputChannel & input_channel) { // Initialize parameters - input_topic_ = input_topic; - long_name_ = long_name; - short_name_ = short_name; + input_topic_ = input_channel.input_topic; + long_name_ = input_channel.long_name; + short_name_ = input_channel.short_name; // Initialize queue objects_que_.clear(); // Initialize latency statistics - expected_rate_ = 10.0; - latency_mean_ = 0.18; // [s] + expected_rate_ = input_channel.expected_rate; // [Hz] + latency_mean_ = input_channel.expected_latency; // [s] latency_var_ = 0.0; interval_mean_ = 1 / expected_rate_; interval_var_ = 0.0; @@ -136,24 +135,20 @@ InputManager::InputManager(rclcpp::Node & node) : node_(node) latest_object_time_ = node_.now(); } -void InputManager::init( - const std::vector & input_topics, const std::vector & long_names, - const std::vector & short_names) +void InputManager::init(const std::vector & input_channels) { // Check input sizes - input_size_ = input_topics.size(); + input_size_ = input_channels.size(); if (input_size_ == 0) { RCLCPP_ERROR(node_.get_logger(), "InputManager::init No input streams"); return; } - assert(input_size_ == long_names.size()); - assert(input_size_ == short_names.size()); sub_objects_array_.resize(input_size_); for (size_t i = 0; i < input_size_; i++) { InputStream input_stream(node_, i); - input_stream.init(input_topics.at(i), long_names.at(i), short_names.at(i)); + input_stream.init(input_channels[i]); input_stream.setTriggerFunction( std::bind(&InputManager::onTrigger, this, std::placeholders::_1)); input_streams_.push_back(std::make_shared(input_stream)); @@ -161,11 +156,11 @@ void InputManager::init( // Set subscription RCLCPP_INFO( node_.get_logger(), "InputManager::init Initializing %s input stream from %s", - long_names.at(i).c_str(), input_topics.at(i).c_str()); + input_channels[i].long_name.c_str(), input_channels[i].input_topic.c_str()); std::function func = std::bind(&InputStream::onMessage, input_streams_.at(i), std::placeholders::_1); - sub_objects_array_.at(i) = - node_.create_subscription(input_topics.at(i), rclcpp::QoS{1}, func); + sub_objects_array_.at(i) = node_.create_subscription( + input_channels[i].input_topic, rclcpp::QoS{1}, func); } is_initialized_ = true; From e495efbbfae2c05ee04481a8e46edb2e50a05106 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 17 Apr 2024 11:37:05 +0900 Subject: [PATCH 22/67] fix: define type for object list Signed-off-by: Taekjin LEE --- .../multi_object_tracker_core.hpp | 8 +++--- .../processor/input_manager.hpp | 18 ++++++------- .../src/multi_object_tracker_core.cpp | 26 +++++++++---------- .../src/processor/input_manager.cpp | 24 ++++++++--------- 4 files changed, 37 insertions(+), 39 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 45ee40eddc369..956cf8406e508 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 @@ -55,9 +55,9 @@ namespace multi_object_tracker { -using autoware_auto_perception_msgs::msg::DetectedObject; -using autoware_auto_perception_msgs::msg::DetectedObjects; -using autoware_auto_perception_msgs::msg::TrackedObjects; +using DetectedObject = autoware_auto_perception_msgs::msg::DetectedObject; +using DetectedObjects = autoware_auto_perception_msgs::msg::DetectedObjects; +using TrackedObjects = autoware_auto_perception_msgs::msg::TrackedObjects; class MultiObjectTracker : public rclcpp::Node { @@ -94,7 +94,7 @@ class MultiObjectTracker : public rclcpp::Node // callback functions void onTimer(); void onTrigger(); - void onMessage(const std::vector> & objects_data); + void onMessage(const ObjectsList & objects_list); // publish processes void runProcess(const DetectedObjects & input_objects_msg); 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 405b98ef1650a..725503c880915 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 @@ -28,7 +28,8 @@ namespace multi_object_tracker { -using autoware_auto_perception_msgs::msg::DetectedObjects; +using DetectedObjects = autoware_auto_perception_msgs::msg::DetectedObjects; +using ObjectsList = std::vector>; struct InputChannel { @@ -43,12 +44,12 @@ struct InputChannel class InputStream { public: - explicit InputStream(rclcpp::Node & node, size_t & index); + explicit InputStream(rclcpp::Node & node, uint & index); void init(const InputChannel & input_channel); void setQueueSize(size_t que_size) { que_size_ = que_size; } - void setTriggerFunction(std::function func_trigger) + void setTriggerFunction(std::function func_trigger) { func_trigger_ = func_trigger; } @@ -57,7 +58,7 @@ class InputStream void getObjectsOlderThan( const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, - std::vector> & objects); + ObjectsList & objects_list); void getNames(std::string & long_name, std::string & short_name) { long_name = long_name_; @@ -78,7 +79,7 @@ class InputStream private: rclcpp::Node & node_; - size_t index_; + uint index_; std::string input_topic_; std::string long_name_; @@ -87,7 +88,7 @@ class InputStream size_t que_size_{30}; std::deque objects_que_; - std::function func_trigger_; + std::function func_trigger_; bool is_time_initialized_{false}; double expected_rate_; @@ -108,12 +109,11 @@ class InputManager void init(const std::vector & input_channels); void setTriggerFunction(std::function func_trigger) { func_trigger_ = func_trigger; } - void onTrigger(const size_t & index) const; + void onTrigger(const uint & index) const; void getObjectTimeInterval( const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time); - bool getObjects( - const rclcpp::Time & now, std::vector> & objects_list); + bool getObjects(const rclcpp::Time & now, ObjectsList & objects_list); private: rclcpp::Node & node_; 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 e80cab8022ff9..c89dd3d82a4f8 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -87,16 +87,6 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) std::vector input_names_long = get_parameter("input_names_long").as_string_array(); std::vector input_names_short = get_parameter("input_names_short").as_string_array(); - input_channels_.resize(input_topic_names.size()); - assert(input_topic_names.size() == input_names_long.size()); - assert(input_topic_names.size() == input_names_short.size()); - for (size_t i = 0; i < input_topic_names.size(); i++) { - input_channels_[i].index = i; - 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]; - } - // ROS interface - Publisher tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); @@ -106,6 +96,15 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) return; } input_topic_size_ = input_topic_names.size(); + input_channels_.resize(input_topic_size_); + assert(input_names_long.size() == input_topic_size_); + assert(input_names_short.size() == input_topic_size_); + for (size_t i = 0; i < input_topic_size_; i++) { + input_channels_[i].index = i; + 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]; + } // Initialize input manager input_manager_ = std::make_unique(*this); @@ -174,7 +173,7 @@ void MultiObjectTracker::onTrigger() { const rclcpp::Time current_time = this->now(); // get objects from the input manager and run process - std::vector> objects_list; + std::vector> objects_list; const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list); if (!is_objects_ready) return; @@ -206,7 +205,7 @@ void MultiObjectTracker::onTimer() if (elapsed_time < maximum_publish_latency) return; // get objects from the input manager and run process - std::vector> objects_list; + ObjectsList objects_list; const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list); if (is_objects_ready) { onMessage(objects_list); @@ -216,8 +215,7 @@ void MultiObjectTracker::onTimer() checkAndPublish(current_time); } -void MultiObjectTracker::onMessage( - const std::vector> & objects_list) +void MultiObjectTracker::onMessage(const ObjectsList & objects_list) { const rclcpp::Time current_time = this->now(); const rclcpp::Time oldest_time(objects_list.front().second.header.stamp); diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index 605f4b98e9e1b..5120f6632ead4 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -19,7 +19,7 @@ namespace multi_object_tracker { -InputStream::InputStream(rclcpp::Node & node, size_t & index) : node_(node), index_(index) +InputStream::InputStream(rclcpp::Node & node, uint & index) : node_(node), index_(index) { } @@ -104,7 +104,7 @@ void InputStream::onMessage( void InputStream::getObjectsOlderThan( const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, - std::vector> & objects_list) + ObjectsList & objects_list) { assert(object_latest_time.nanoseconds() > object_oldest_time.nanoseconds()); @@ -119,7 +119,7 @@ void InputStream::getObjectsOlderThan( // Add the object if the object is older than the specified latest time if (object_latest_time >= object_time) { - std::pair object_pair(index_, object); + std::pair object_pair(index_, object); objects_list.push_back(object_pair); // remove the object from the queue objects_que_.pop_front(); @@ -147,7 +147,8 @@ void InputManager::init(const std::vector & input_channels) sub_objects_array_.resize(input_size_); for (size_t i = 0; i < input_size_; i++) { - InputStream input_stream(node_, i); + uint index(i); + InputStream input_stream(node_, index); input_stream.init(input_channels[i]); input_stream.setTriggerFunction( std::bind(&InputManager::onTrigger, this, std::placeholders::_1)); @@ -166,10 +167,10 @@ void InputManager::init(const std::vector & input_channels) is_initialized_ = true; } -void InputManager::onTrigger(const size_t & index) const +void InputManager::onTrigger(const uint & index) const { // input stream index of 0 is the target(main) input stream - const size_t target_idx = 0; + const uint target_idx = 0; // when the main stream triggers, call the trigger function if (index == target_idx && func_trigger_) { @@ -203,11 +204,11 @@ void InputManager::getObjectTimeInterval( } // Get proper latency - constexpr double target_latency = 0.15; // [s], measurement to tracking latency + constexpr double target_latency = 0.18; // [s], measurement to tracking latency // process latency of a main detection + margin - constexpr double acceptable_latency = - 0.35; // [s], acceptable band from the target latency, larger than the target latency + 0.32; // [s], acceptable band from the target latency, larger than the target latency + object_latest_time = now - rclcpp::Duration::from_seconds(target_latency); object_oldest_time = now - rclcpp::Duration::from_seconds(acceptable_latency); @@ -217,8 +218,7 @@ void InputManager::getObjectTimeInterval( object_oldest_time > latest_object_time_ ? object_oldest_time : latest_object_time_; } -bool InputManager::getObjects( - const rclcpp::Time & now, std::vector> & objects_list) +bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_list) { if (!is_initialized_) { RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Input manager is not initialized"); @@ -241,7 +241,7 @@ bool InputManager::getObjects( // Sort objects by timestamp std::sort( objects_list.begin(), objects_list.end(), - [](const std::pair & a, const std::pair & b) { + [](const std::pair & a, const std::pair & b) { return (rclcpp::Time(a.second.header.stamp) - rclcpp::Time(b.second.header.stamp)).seconds() < 0; }); From 4955207c037ae966da7cd77a8f4ee6cd3e88a78e Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 17 Apr 2024 17:39:55 +0900 Subject: [PATCH 23/67] feat: add channel wise existence probability Signed-off-by: Taekjin LEE --- .../multi_object_tracker_core.hpp | 4 +- .../processor/processor.hpp | 10 +-- .../tracker/model/bicycle_tracker.hpp | 3 +- .../tracker/model/big_vehicle_tracker.hpp | 3 +- .../model/multiple_vehicle_tracker.hpp | 3 +- .../tracker/model/normal_vehicle_tracker.hpp | 3 +- .../tracker/model/pass_through_tracker.hpp | 3 +- .../model/pedestrian_and_bicycle_tracker.hpp | 3 +- .../tracker/model/pedestrian_tracker.hpp | 3 +- .../tracker/model/tracker_base.hpp | 48 ++++++++----- .../tracker/model/unknown_tracker.hpp | 3 +- .../src/multi_object_tracker_core.cpp | 27 ++++---- .../src/processor/processor.cpp | 43 +++++++----- .../src/tracker/model/bicycle_tracker.cpp | 8 ++- .../src/tracker/model/big_vehicle_tracker.cpp | 8 ++- .../model/multiple_vehicle_tracker.cpp | 11 +-- .../tracker/model/normal_vehicle_tracker.cpp | 8 ++- .../tracker/model/pass_through_tracker.cpp | 8 ++- .../model/pedestrian_and_bicycle_tracker.cpp | 11 +-- .../src/tracker/model/pedestrian_tracker.cpp | 8 ++- .../src/tracker/model/tracker_base.cpp | 67 +++++++++++++++++-- .../src/tracker/model/unknown_tracker.cpp | 8 ++- 22 files changed, 206 insertions(+), 87 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 956cf8406e508..1df852cfefb02 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 @@ -89,7 +89,7 @@ class MultiObjectTracker : public rclcpp::Node std::unique_ptr input_manager_; std::vector input_channels_{}; - size_t input_topic_size_{}; + size_t input_channel_size_{}; // callback functions void onTimer(); @@ -97,7 +97,7 @@ class MultiObjectTracker : public rclcpp::Node void onMessage(const ObjectsList & objects_list); // publish processes - void runProcess(const DetectedObjects & input_objects_msg); + void runProcess(const DetectedObjects & input_objects_msg, const uint & channel_index); 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/include/multi_object_tracker/processor/processor.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp index 654c960463933..0b27072c19e4d 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp @@ -32,7 +32,8 @@ class TrackerProcessor { public: - explicit TrackerProcessor(const std::map & tracker_map); + explicit TrackerProcessor( + const std::map & tracker_map, const size_t & channel_size); const std::list> & getListTracker() const { return list_tracker_; } // tracker processes @@ -40,11 +41,11 @@ class TrackerProcessor void update( const autoware_auto_perception_msgs::msg::DetectedObjects & transformed_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & direct_assignment); + const std::unordered_map & direct_assignment, const uint & channel_index); void spawn( const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & reverse_assignment); + const std::unordered_map & reverse_assignment, const uint & channel_index); void prune(const rclcpp::Time & time); // output @@ -59,6 +60,7 @@ class TrackerProcessor private: std::map tracker_map_; std::list> list_tracker_; + const size_t channel_size_; // parameters float max_elapsed_time_; // [s] @@ -71,7 +73,7 @@ class TrackerProcessor void removeOverlappedTracker(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; + const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const; }; #endif // MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 2d69334a2f43f..ead0d8dbf0e5a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -56,7 +56,8 @@ class BicycleTracker : public Tracker public: BicycleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 8d49138b94a24..3b509a3cb47d8 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -61,7 +61,8 @@ class BigVehicleTracker : public Tracker public: BigVehicleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index 5722566b4a633..fef8caaf20d8a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -35,7 +35,8 @@ class MultipleVehicleTracker : public Tracker public: MultipleVehicleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 6ab7e63bce167..b7810d2471416 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -61,7 +61,8 @@ class NormalVehicleTracker : public Tracker public: NormalVehicleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp index 48a8702d182a0..0f892098a373a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -34,7 +34,8 @@ class PassThroughTracker : public Tracker public: PassThroughTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index cf6a82088c7b6..490f6d93c6f6a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -34,7 +34,8 @@ class PedestrianAndBicycleTracker : public Tracker public: PedestrianAndBicycleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index a56250390e34f..72f5a5a002f0f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -64,7 +64,8 @@ class PedestrianTracker : public Tracker public: PedestrianTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp index ab6a747288d4a..b25e1655b5fdb 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp @@ -35,33 +35,36 @@ class Tracker { -protected: - unique_identifier_msgs::msg::UUID getUUID() const { return uuid_; } - void setClassification( - const std::vector & classification) - { - classification_ = classification; - } - void updateClassification( - const std::vector & classification); - private: unique_identifier_msgs::msg::UUID uuid_; + + // classification std::vector classification_; + + // existence states int no_measurement_count_; int total_no_measurement_count_; int total_measurement_count_; rclcpp::Time last_update_with_measurement_time_; + std::vector existence_probabilities_; + float total_existence_probability_; public: Tracker( const rclcpp::Time & time, - const std::vector & classification); + const std::vector & classification, + const size_t & channel_size); virtual ~Tracker() {} + + void initializeExistenceProbabilities( + const uint & channel_index, const float & existence_probability); bool updateWithMeasurement( const autoware_auto_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform); - bool updateWithoutMeasurement(); + const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform, + const uint & channel_index); + bool updateWithoutMeasurement(const rclcpp::Time & now); + + // classification std::vector getClassification() const { return classification_; @@ -70,6 +73,8 @@ class Tracker { return object_recognition_utils::getHighestProbLabel(classification_); } + + // existence states int getNoMeasurementCount() const { return no_measurement_count_; } int getTotalNoMeasurementCount() const { return total_no_measurement_count_; } int getTotalMeasurementCount() const { return total_measurement_count_; } @@ -77,13 +82,22 @@ class Tracker { return (current_time - last_update_with_measurement_time_).seconds(); } + +protected: + unique_identifier_msgs::msg::UUID getUUID() const { return uuid_; } + void setClassification( + const std::vector & classification) + { + classification_ = classification; + } + void updateClassification( + const std::vector & classification); + + // virtual functions +public: virtual geometry_msgs::msg::PoseWithCovariance getPoseWithCovariance( const rclcpp::Time & time) const; - /* - * Pure virtual function - */ - protected: virtual bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp index 73bf7849e13d8..3ef58773b408f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -49,7 +49,8 @@ class UnknownTracker : public Tracker public: UnknownTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index); bool predict(const rclcpp::Time & time) override; bool measure( 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 c89dd3d82a4f8..74f3a24ee0347 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -95,11 +95,11 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) RCLCPP_ERROR(get_logger(), "Need a 'input_topic_names' parameter to be set before continuing"); return; } - input_topic_size_ = input_topic_names.size(); - input_channels_.resize(input_topic_size_); - assert(input_names_long.size() == input_topic_size_); - assert(input_names_short.size() == input_topic_size_); - for (size_t i = 0; i < input_topic_size_; i++) { + input_channel_size_ = input_topic_names.size(); + input_channels_.resize(input_channel_size_); + assert(input_names_long.size() == input_channel_size_); + assert(input_names_short.size() == input_channel_size_); + for (size_t i = 0; i < input_channel_size_; i++) { input_channels_[i].index = i; input_channels_[i].input_topic = input_topic_names[i]; input_channels_[i].long_name = input_names_long[i]; @@ -146,7 +146,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) tracker_map.insert(std::make_pair( Label::MOTORCYCLE, this->declare_parameter("motorcycle_tracker"))); - processor_ = std::make_unique(tracker_map); + processor_ = std::make_unique(tracker_map, input_channel_size_); } // Data association initialization @@ -224,7 +224,7 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list) debugger_->startMeasurementTime(this->now(), oldest_time); // run process for each DetectedObjects for (const auto & objects_data : objects_list) { - runProcess(objects_data.second); + runProcess(objects_data.second, objects_data.first); } // process end debugger_->endMeasurementTime(this->now()); @@ -237,9 +237,9 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list) // count objects on each channel std::vector object_counts; - object_counts.resize(input_topic_size_); + object_counts.resize(input_channel_size_); // initialize to zero - for (size_t i = 0; i < input_topic_size_; i++) { + for (size_t i = 0; i < input_channel_size_; i++) { object_counts[i] = 0; } for (const auto & objects_data : objects_list) { @@ -247,14 +247,15 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list) } // print result std::string object_counts_str = "MultiObjectTracker::onMessage Object counts: "; - for (size_t i = 0; i < input_topic_size_; i++) { + for (size_t i = 0; i < input_channel_size_; i++) { object_counts_str += input_channels_[i].long_name + " " + std::to_string(object_counts[i]) + " "; } RCLCPP_INFO(this->get_logger(), object_counts_str.c_str()); } -void MultiObjectTracker::runProcess(const DetectedObjects & input_objects_msg) +void MultiObjectTracker::runProcess( + const DetectedObjects & input_objects_msg, const uint & channel_index) { // Get the time of the measurement const rclcpp::Time measurement_time = @@ -289,13 +290,13 @@ void MultiObjectTracker::runProcess(const DetectedObjects & input_objects_msg) } /* tracker update */ - processor_->update(transformed_objects, *self_transform, direct_assignment); + processor_->update(transformed_objects, *self_transform, direct_assignment, channel_index); /* tracker pruning */ processor_->prune(measurement_time); /* spawn new tracker */ - processor_->spawn(transformed_objects, *self_transform, reverse_assignment); + processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index); } void MultiObjectTracker::checkAndPublish(const rclcpp::Time & time) diff --git a/perception/multi_object_tracker/src/processor/processor.cpp b/perception/multi_object_tracker/src/processor/processor.cpp index 8cfb64cab19be..a5a1f18a64ca0 100644 --- a/perception/multi_object_tracker/src/processor/processor.cpp +++ b/perception/multi_object_tracker/src/processor/processor.cpp @@ -23,8 +23,9 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; -TrackerProcessor::TrackerProcessor(const std::map & tracker_map) -: tracker_map_(tracker_map) +TrackerProcessor::TrackerProcessor( + const std::map & tracker_map, const size_t & channel_size) +: tracker_map_(tracker_map), channel_size_(channel_size) { // Set tracker lifetime parameters max_elapsed_time_ = 1.0; // [s] @@ -48,7 +49,7 @@ void TrackerProcessor::predict(const rclcpp::Time & time) void TrackerProcessor::update( const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & direct_assignment) + const std::unordered_map & direct_assignment, const uint & channel_index) { int tracker_idx = 0; const auto & time = detected_objects.header.stamp; @@ -57,9 +58,10 @@ void TrackerProcessor::update( if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { // found const auto & associated_object = detected_objects.objects.at(direct_assignment.find(tracker_idx)->second); - (*(tracker_itr))->updateWithMeasurement(associated_object, time, self_transform); + (*(tracker_itr)) + ->updateWithMeasurement(associated_object, time, self_transform, channel_index); } else { // not found - (*(tracker_itr))->updateWithoutMeasurement(); + (*(tracker_itr))->updateWithoutMeasurement(time); } } } @@ -67,7 +69,7 @@ void TrackerProcessor::update( void TrackerProcessor::spawn( const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & reverse_assignment) + const std::unordered_map & reverse_assignment, const uint & channel_index) { const auto & time = detected_objects.header.stamp; for (size_t i = 0; i < detected_objects.objects.size(); ++i) { @@ -75,34 +77,43 @@ void TrackerProcessor::spawn( continue; } const auto & new_object = detected_objects.objects.at(i); - std::shared_ptr tracker = createNewTracker(new_object, time, self_transform); + std::shared_ptr tracker = + createNewTracker(new_object, time, self_transform, channel_index); if (tracker) list_tracker_.push_back(tracker); } } std::shared_ptr TrackerProcessor::createNewTracker( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) const + const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const { const std::uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); if (tracker_map_.count(label) != 0) { const auto tracker = tracker_map_.at(label); if (tracker == "bicycle_tracker") - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); if (tracker == "big_vehicle_tracker") - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); if (tracker == "multi_vehicle_tracker") - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); if (tracker == "normal_vehicle_tracker") - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); if (tracker == "pass_through_tracker") - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); if (tracker == "pedestrian_and_bicycle_tracker") - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); if (tracker == "pedestrian_tracker") - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); } - return std::make_shared(time, object, self_transform); + return std::make_shared( + time, object, self_transform, channel_size_, channel_index); } void TrackerProcessor::prune(const rclcpp::Time & time) diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 12c340516c6c1..0698e4651b6bf 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -44,13 +44,17 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; BicycleTracker::BicycleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) -: Tracker(time, object.classification), + const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("BicycleTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); + // Initialize parameters // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty double r_stddev_x = 0.5; // in vehicle coordinate [m] diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 3c73b9f19cfbb..e8ca8a47bbc78 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -44,14 +44,18 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; BigVehicleTracker::BigVehicleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform) -: Tracker(time, object.classification), + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("BigVehicleTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z), tracking_offset_(Eigen::Vector2d::Zero()) { object_ = object; + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); + // Initialize parameters // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty float r_stddev_x = 0.5; // in vehicle coordinate [m] diff --git a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp index 4f0fb4d7871f2..c925976f65e7e 100644 --- a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp @@ -22,11 +22,14 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; MultipleVehicleTracker::MultipleVehicleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform) -: Tracker(time, object.classification), - normal_vehicle_tracker_(time, object, self_transform), - big_vehicle_tracker_(time, object, self_transform) + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), + normal_vehicle_tracker_(time, object, self_transform, channel_size, channel_index), + big_vehicle_tracker_(time, object, self_transform, channel_size, channel_index) { + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); } bool MultipleVehicleTracker::predict(const rclcpp::Time & time) diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 19fc5a2f71122..54d6e84dd6656 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -44,14 +44,18 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; NormalVehicleTracker::NormalVehicleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform) -: Tracker(time, object.classification), + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("NormalVehicleTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z), tracking_offset_(Eigen::Vector2d::Zero()) { object_ = object; + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); + // Initialize parameters // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty float r_stddev_x = 0.5; // in vehicle coordinate [m] diff --git a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp index 2084ac28e70f0..e1f4383cf11a3 100644 --- a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp @@ -37,13 +37,17 @@ PassThroughTracker::PassThroughTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) -: Tracker(time, object.classification), + const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("PassThroughTracker")), last_update_time_(time) { object_ = object; prev_observed_object_ = object; + + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); } bool PassThroughTracker::predict(const rclcpp::Time & time) diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp index d61a9a02ccd80..e399dade880fa 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -22,11 +22,14 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; PedestrianAndBicycleTracker::PedestrianAndBicycleTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform) -: Tracker(time, object.classification), - pedestrian_tracker_(time, object, self_transform), - bicycle_tracker_(time, object, self_transform) + const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), + pedestrian_tracker_(time, object, self_transform, channel_size, channel_index), + bicycle_tracker_(time, object, self_transform, channel_size, channel_index) { + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); } bool PedestrianAndBicycleTracker::predict(const rclcpp::Time & time) diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index e1c07388836f6..0d4411b5266ad 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -44,13 +44,17 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; PedestrianTracker::PedestrianTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) -: Tracker(time, object.classification), + const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("PedestrianTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); + // Initialize parameters // measurement noise covariance float r_stddev_x = 0.4; // [m] diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp index a3320ff54afcb..4d2bb3301fe38 100644 --- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp @@ -23,7 +23,8 @@ Tracker::Tracker( const rclcpp::Time & time, - const std::vector & classification) + const std::vector & classification, + const size_t & channel_size) : classification_(classification), no_measurement_count_(0), total_no_measurement_count_(0), @@ -34,23 +35,75 @@ Tracker::Tracker( std::mt19937 gen(std::random_device{}()); std::independent_bits_engine bit_eng(gen); std::generate(uuid_.uuid.begin(), uuid_.uuid.end(), bit_eng); + + // Initialize existence probabilities + existence_probabilities_.resize(channel_size, 0.0); +} + +void Tracker::initializeExistenceProbabilities( + const uint & channel_index, const float & existence_probability) +{ + existence_probabilities_[channel_index] = existence_probability; + total_existence_probability_ = existence_probability; } bool Tracker::updateWithMeasurement( const autoware_auto_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform) + const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform, + const uint & channel_index) { - no_measurement_count_ = 0; - ++total_measurement_count_; + // Update existence probability + { + float existence_probability_from_object = object.existence_probability; + no_measurement_count_ = 0; + ++total_measurement_count_; + + double const delta_time = (measurement_time - last_update_with_measurement_time_).seconds(); + double const decay_rate = 5.0 / 10.0; + existence_probabilities_[channel_index] = existence_probability_from_object; + for (size_t i = 0; i < existence_probabilities_.size(); ++i) { + if (i == channel_index) { + continue; + } + existence_probabilities_[i] *= std::exp(-decay_rate * delta_time); + } + + // regularization + total_existence_probability_ = + std::accumulate(existence_probabilities_.begin(), existence_probabilities_.end(), 0.0f); + total_existence_probability_ = std::max(total_existence_probability_, 0.0f); + total_existence_probability_ = std::min(total_existence_probability_, 1.0f); + } + last_update_with_measurement_time_ = measurement_time; + + // Update object measure(object, measurement_time, self_transform); + return true; } -bool Tracker::updateWithoutMeasurement() +bool Tracker::updateWithoutMeasurement(const rclcpp::Time & now) { - ++no_measurement_count_; - ++total_no_measurement_count_; + // Update existence probability + { + ++no_measurement_count_; + ++total_no_measurement_count_; + + // decay + double const delta_time = (now - last_update_with_measurement_time_).seconds(); + double const decay_rate = 5.0 / 10.0; + for (size_t i = 0; i < existence_probabilities_.size(); ++i) { + existence_probabilities_[i] *= std::exp(-decay_rate * delta_time); + } + + // regularization + total_existence_probability_ = + std::accumulate(existence_probabilities_.begin(), existence_probabilities_.end(), 0.0f); + total_existence_probability_ = std::max(total_existence_probability_, 0.0f); + total_existence_probability_ = std::min(total_existence_probability_, 1.0f); + } + return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index 450bb2d94abb6..04638267f7ad8 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -38,13 +38,17 @@ UnknownTracker::UnknownTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) -: Tracker(time, object.classification), + const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, + const uint & channel_index) +: Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("UnknownTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; + // initialize existence probability + initializeExistenceProbabilities(channel_index, object.existence_probability); + // initialize params // measurement noise covariance constexpr double r_stddev_x = 1.0; // [m] From 46c16c7afd75b44228973dbc45ee742d01173f5f Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 18 Apr 2024 11:35:37 +0900 Subject: [PATCH 24/67] fix: relocate debugger Signed-off-by: Taekjin LEE --- .../multi_object_tracker/CMakeLists.txt | 2 +- .../{ => debugger}/debugger.hpp | 44 +++++++++++-------- .../multi_object_tracker_core.hpp | 2 +- .../src/{ => debugger}/debugger.cpp | 40 +++++++++-------- 4 files changed, 50 insertions(+), 38 deletions(-) rename perception/multi_object_tracker/include/multi_object_tracker/{ => debugger}/debugger.hpp (93%) rename perception/multi_object_tracker/src/{ => debugger}/debugger.cpp (96%) diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index 7d779d3bfc093..6adc0606add56 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -22,7 +22,7 @@ include_directories( # Generate exe file set(MULTI_OBJECT_TRACKER_SRC src/multi_object_tracker_core.cpp - src/debugger.cpp + src/debugger/debugger.cpp src/processor/processor.cpp src/processor/input_manager.cpp src/data_association/data_association.cpp diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp similarity index 93% rename from perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp rename to perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp index 90291ae6fec18..2c10fabd2a91e 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp @@ -14,8 +14,8 @@ // // -#ifndef MULTI_OBJECT_TRACKER__DEBUGGER_HPP_ -#define MULTI_OBJECT_TRACKER__DEBUGGER_HPP_ +#ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ +#define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ #include #include @@ -37,16 +37,8 @@ class TrackerDebugger { public: explicit TrackerDebugger(rclcpp::Node & node); - void publishTentativeObjects( - 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); +private: struct DEBUG_SETTINGS { bool publish_processing_time; @@ -54,22 +46,38 @@ class TrackerDebugger double diagnostics_warn_delay; double diagnostics_error_delay; } debug_settings_; - double pipeline_latency_ms_ = 0.0; - diagnostic_updater::Updater diagnostic_updater_; - bool shouldPublishTentativeObjects() const { return debug_settings_.publish_tentative_objects; } -private: - void loadParameters(); - bool is_initialized_ = false; rclcpp::Node & node_; rclcpp::Publisher::SharedPtr debug_tentative_objects_pub_; std::unique_ptr processing_time_publisher_; + + diagnostic_updater::Updater diagnostic_updater_; + + bool is_initialized_ = false; + double pipeline_latency_ms_ = 0.0; 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_; + +public: + bool shouldPublishTentativeObjects() const { return debug_settings_.publish_tentative_objects; } + void setupDiagnostics(); + + void publishTentativeObjects( + 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 checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat); + +private: + void loadParameters(); }; -#endif // MULTI_OBJECT_TRACKER__DEBUGGER_HPP_ +#endif // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ 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 1df852cfefb02..3daeaa2a46322 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 @@ -20,7 +20,7 @@ #define MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_ #include "multi_object_tracker/data_association/data_association.hpp" -#include "multi_object_tracker/debugger.hpp" +#include "multi_object_tracker/debugger/debugger.hpp" #include "multi_object_tracker/processor/input_manager.hpp" #include "multi_object_tracker/processor/processor.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" diff --git a/perception/multi_object_tracker/src/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp similarity index 96% rename from perception/multi_object_tracker/src/debugger.cpp rename to perception/multi_object_tracker/src/debugger/debugger.cpp index b5632a13e78fc..9b4906967d3af 100644 --- a/perception/multi_object_tracker/src/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger/debugger.cpp @@ -14,11 +14,11 @@ // // -#include "multi_object_tracker/debugger.hpp" +#include "multi_object_tracker/debugger/debugger.hpp" #include -TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : diagnostic_updater_(&node), node_(node) +TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : node_(node), diagnostic_updater_(&node) { // declare debug parameters to decide whether to publish debug topics loadParameters(); @@ -46,14 +46,6 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : diagnostic_updater_(&nod 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() { try { @@ -74,6 +66,26 @@ void TrackerDebugger::loadParameters() } } +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); +} + +// object publishing functions + +void TrackerDebugger::publishTentativeObjects( + const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const +{ + if (debug_settings_.publish_tentative_objects) { + debug_tentative_objects_pub_->publish(tentative_objects); + } +} + +// time measurement functions + void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat) { if (!is_initialized_) { @@ -97,14 +109,6 @@ void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & s stat.add("Detection delay", delay); } -void TrackerDebugger::publishTentativeObjects( - const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const -{ - if (debug_settings_.publish_tentative_objects) { - debug_tentative_objects_pub_->publish(tentative_objects); - } -} - void TrackerDebugger::startMeasurementTime( const rclcpp::Time & now, const rclcpp::Time & measurement_header_stamp) { From 99ad038dec2b1c1196996d6a6e92db8bc3607328 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 18 Apr 2024 14:26:25 +0900 Subject: [PATCH 25/67] fix: total existence logic change Signed-off-by: Taekjin LEE --- .../processor/processor.hpp | 2 ++ .../tracker/model/tracker_base.hpp | 5 ++++ .../src/debugger/debugger.cpp | 6 ++--- .../src/tracker/model/tracker_base.cpp | 27 +++++++------------ 4 files changed, 19 insertions(+), 21 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp index 0b27072c19e4d..de0fe0854254a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp @@ -57,6 +57,8 @@ class TrackerProcessor const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const; + void getExistenceProbabilities(std::vector> & existence_vectors) const; + private: std::map tracker_map_; std::list> list_tracker_; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp index b25e1655b5fdb..348ba1f5d7383 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp @@ -58,6 +58,11 @@ class Tracker void initializeExistenceProbabilities( const uint & channel_index, const float & existence_probability); + bool getExistenceProbabilityVector(std::vector & existence_vector) const + { + existence_vector = existence_probabilities_; + return existence_vector.size() > 0; + } bool updateWithMeasurement( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform, diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp index 9b4906967d3af..30838220a566e 100644 --- a/perception/multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger/debugger.cpp @@ -11,8 +11,6 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -// -// #include "multi_object_tracker/debugger/debugger.hpp" @@ -74,7 +72,7 @@ void TrackerDebugger::setupDiagnostics() diagnostic_updater_.setPeriod(0.1); } -// object publishing functions +// Object publishing functions void TrackerDebugger::publishTentativeObjects( const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const @@ -84,7 +82,7 @@ void TrackerDebugger::publishTentativeObjects( } } -// time measurement functions +// Time measurement functions void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat) { diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp index 4d2bb3301fe38..74912655ea0d6 100644 --- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp @@ -58,8 +58,9 @@ bool Tracker::updateWithMeasurement( no_measurement_count_ = 0; ++total_measurement_count_; - double const delta_time = (measurement_time - last_update_with_measurement_time_).seconds(); - double const decay_rate = 5.0 / 10.0; + // existence probability on each channel + const double delta_time = (measurement_time - last_update_with_measurement_time_).seconds(); + const double decay_rate = 5.0 / 10.0; existence_probabilities_[channel_index] = existence_probability_from_object; for (size_t i = 0; i < existence_probabilities_.size(); ++i) { if (i == channel_index) { @@ -68,11 +69,9 @@ bool Tracker::updateWithMeasurement( existence_probabilities_[i] *= std::exp(-decay_rate * delta_time); } - // regularization - total_existence_probability_ = - std::accumulate(existence_probabilities_.begin(), existence_probabilities_.end(), 0.0f); - total_existence_probability_ = std::max(total_existence_probability_, 0.0f); - total_existence_probability_ = std::min(total_existence_probability_, 1.0f); + // total existence probability - object is detected + total_existence_probability_ += + (1 - total_existence_probability_) * existence_probability_from_object; } last_update_with_measurement_time_ = measurement_time; @@ -86,22 +85,16 @@ bool Tracker::updateWithMeasurement( bool Tracker::updateWithoutMeasurement(const rclcpp::Time & now) { // Update existence probability + ++no_measurement_count_; + ++total_no_measurement_count_; { - ++no_measurement_count_; - ++total_no_measurement_count_; - - // decay + // decay existence probability double const delta_time = (now - last_update_with_measurement_time_).seconds(); double const decay_rate = 5.0 / 10.0; for (size_t i = 0; i < existence_probabilities_.size(); ++i) { existence_probabilities_[i] *= std::exp(-decay_rate * delta_time); } - - // regularization - total_existence_probability_ = - std::accumulate(existence_probabilities_.begin(), existence_probabilities_.end(), 0.0f); - total_existence_probability_ = std::max(total_existence_probability_, 0.0f); - total_existence_probability_ = std::min(total_existence_probability_, 1.0f); + total_existence_probability_ *= std::exp(-decay_rate * delta_time); } return true; From 1a761f9ed48a4ec42b1a8917ba0b46b7d23eba7f Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 18 Apr 2024 17:12:06 +0900 Subject: [PATCH 26/67] feat: publishing object debug info, need to fix marker id Signed-off-by: Taekjin LEE --- .../multi_object_tracker/CMakeLists.txt | 1 + .../debugger/debug_object.hpp | 59 ++++++ .../debugger/debugger.hpp | 32 ++- .../src/debugger/debug_object.cpp | 200 ++++++++++++++++++ .../src/debugger/debugger.cpp | 26 ++- .../src/multi_object_tracker_core.cpp | 12 +- 6 files changed, 320 insertions(+), 10 deletions(-) create mode 100644 perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp create mode 100644 perception/multi_object_tracker/src/debugger/debug_object.cpp diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index 6adc0606add56..fb2c4d73554a3 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -23,6 +23,7 @@ include_directories( set(MULTI_OBJECT_TRACKER_SRC src/multi_object_tracker_core.cpp src/debugger/debugger.cpp + src/debugger/debug_object.cpp src/processor/processor.cpp src/processor/input_manager.cpp src/data_association/data_association.cpp diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp new file mode 100644 index 0000000000000..18b8b77739800 --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp @@ -0,0 +1,59 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ +#define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ + +#include "multi_object_tracker/tracker/model/tracker_base.hpp" + +#include +#include + +#include "unique_identifier_msgs/msg/uuid.hpp" +#include +#include + +#include +#include +#include +#include +#include + +class TrackerObjectDebugger +{ +public: + explicit TrackerObjectDebugger(std::string frame_id); + +private: + bool is_initialized_{false}; + std::string frame_id_; + visualization_msgs::msg::MarkerArray markers_; + std::unordered_set current_ids_; + std::unordered_set previous_ids_; + rclcpp::Time message_time_; + +public: + void collect( + const rclcpp::Time & message_time, const std::list> & list_tracker, + const uint & channel_index, + const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, + const std::unordered_map & direct_assignment, + const std::unordered_map & reverse_assignment); + + void reset(); + + void getMessage(visualization_msgs::msg::MarkerArray & message); +}; + +#endif // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp index 2c10fabd2a91e..8a5ebd8a61ff1 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp @@ -11,12 +11,12 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -// -// #ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ #define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ +#include "multi_object_tracker/debugger/debug_object.hpp" + #include #include #include @@ -27,7 +27,10 @@ #include #include +#include #include +#include +#include /** * @brief Debugger class for multi object tracker @@ -36,7 +39,7 @@ class TrackerDebugger { public: - explicit TrackerDebugger(rclcpp::Node & node); + explicit TrackerDebugger(rclcpp::Node & node, const std::string & frame_id); private: struct DEBUG_SETTINGS @@ -47,13 +50,18 @@ class TrackerDebugger double diagnostics_error_delay; } debug_settings_; + // ROS node, publishers rclcpp::Node & node_; rclcpp::Publisher::SharedPtr debug_tentative_objects_pub_; std::unique_ptr processing_time_publisher_; + rclcpp::Publisher::SharedPtr debug_objects_markers_pub_; diagnostic_updater::Updater diagnostic_updater_; + // Object debugger + TrackerObjectDebugger object_debugger_; + // Time measurement bool is_initialized_ = false; double pipeline_latency_ms_ = 0.0; rclcpp::Time last_input_stamp_; @@ -62,13 +70,17 @@ class TrackerDebugger rclcpp::Time stamp_publish_start_; rclcpp::Time stamp_publish_output_; -public: - bool shouldPublishTentativeObjects() const { return debug_settings_.publish_tentative_objects; } + // Configuration void setupDiagnostics(); + void loadParameters(); +public: + // Object publishing + bool shouldPublishTentativeObjects() const { return debug_settings_.publish_tentative_objects; } void publishTentativeObjects( const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const; + // Time measurement void startMeasurementTime( const rclcpp::Time & now, const rclcpp::Time & measurement_header_stamp); void endMeasurementTime(const rclcpp::Time & now); @@ -76,8 +88,14 @@ class TrackerDebugger void endPublishTime(const rclcpp::Time & now, const rclcpp::Time & object_time); void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat); -private: - void loadParameters(); + // Debug object + void collectObjectInfo( + const rclcpp::Time & message_time, const std::list> & list_tracker, + const uint & channel_index, + const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, + const std::unordered_map & direct_assignment, + const std::unordered_map & reverse_assignment); + void publishObjectsMarkers(); }; #endif // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp new file mode 100644 index 0000000000000..7c317c002b754 --- /dev/null +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -0,0 +1,200 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "multi_object_tracker/debugger/debug_object.hpp" + +#include "autoware_auto_perception_msgs/msg/tracked_object.hpp" + +#include + +#include +#include + +namespace +{ +int uuid_to_int(const unique_identifier_msgs::msg::UUID & uuid) +{ + // Convert UUID to string + std::string uuid_str; + for (auto byte : uuid.uuid) { + uuid_str += std::to_string(byte); + } + + // Hash the string to get an int + std::hash hasher; + int hashed_uuid = hasher(uuid_str); + + return hashed_uuid; +} +} // namespace + +TrackerObjectDebugger::TrackerObjectDebugger(std::string frame_id) +{ + // set frame id + frame_id_ = frame_id; + + // initialize markers + markers_.markers.clear(); + current_ids_.clear(); + previous_ids_.clear(); + message_time_ = rclcpp::Time(0, 0); +} + +void TrackerObjectDebugger::reset() +{ + markers_.markers.clear(); + current_ids_.clear(); +} + +void TrackerObjectDebugger::collect( + const rclcpp::Time & message_time, const std::list> & list_tracker, + const uint & channel_index, + const autoware_auto_perception_msgs::msg::DetectedObjects & /*detected_objects*/, + const std::unordered_map & /*direct_assignment*/, + const std::unordered_map & /*reverse_assignment*/) +{ + constexpr int PALETTE_SIZE = 32; + constexpr std::array, PALETTE_SIZE> color_array = {{ + {{1.0, 0.0, 0.0}}, {{0.0, 1.0, 0.0}}, + {{0.0, 0.0, 1.0}}, // Red, Green, Blue + {{1.0, 1.0, 0.0}}, {{0.0, 1.0, 1.0}}, + {{1.0, 0.0, 1.0}}, // Yellow, Cyan, Magenta + {{1.0, 0.64, 0.0}}, {{0.75, 1.0, 0.0}}, + {{0.0, 0.5, 0.5}}, // Orange, Lime, Teal + {{0.5, 0.0, 0.5}}, {{1.0, 0.75, 0.8}}, + {{0.65, 0.17, 0.17}}, // Purple, Pink, Brown + {{0.5, 0.0, 0.0}}, {{0.5, 0.5, 0.0}}, + {{0.0, 0.0, 0.5}}, // Maroon, Olive, Navy + {{0.5, 0.5, 0.5}}, {{1.0, 0.4, 0.4}}, + {{0.4, 1.0, 0.4}}, // Grey, Light Red, Light Green + {{0.4, 0.4, 1.0}}, {{1.0, 1.0, 0.4}}, + {{0.4, 1.0, 1.0}}, // Light Blue, Light Yellow, Light Cyan + {{1.0, 0.4, 1.0}}, {{1.0, 0.7, 0.4}}, + {{0.7, 0.4, 1.0}}, // Light Magenta, Light Orange, Light Purple + {{1.0, 0.6, 0.8}}, {{0.71, 0.4, 0.114}}, + {{0.55, 0.0, 0.0}}, // Light Pink, Light Brown, Dark Red + {{0.0, 0.4, 0.0}}, {{0.0, 0.0, 0.55}}, + {{0.55, 0.55, 0.0}}, // Dark Green, Dark Blue, Dark Yellow + {{0.0, 0.55, 0.55}}, {{0.55, 0.0, 0.55}} // Dark Cyan, Dark Magenta + }}; + + message_time_ = message_time; + if (!is_initialized_) is_initialized_ = true; + + for (const auto & tracker : list_tracker) { + // get object + autoware_auto_perception_msgs::msg::TrackedObject tracked_object; + tracker->getTrackedObject(message_time_, tracked_object); + const unique_identifier_msgs::msg::UUID uuid = tracked_object.object_id; + const int uuid_int = uuid_to_int(uuid); + current_ids_.emplace(uuid_int); + + // get existence probability + + // get color - by channel index + std_msgs::msg::ColorRGBA color; + color.a = 1.0; + color.r = color_array[channel_index % PALETTE_SIZE][0]; + color.g = color_array[channel_index % PALETTE_SIZE][1]; + color.b = color_array[channel_index % PALETTE_SIZE][2]; + + // get marker - box + + visualization_msgs::msg::Marker marker; + marker.header.frame_id = frame_id_; + marker.header.stamp = message_time_; + marker.ns = "boxes"; + marker.id = uuid_int; + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.position.x = tracked_object.kinematics.pose_with_covariance.pose.position.x; + marker.pose.position.y = tracked_object.kinematics.pose_with_covariance.pose.position.y; + marker.pose.position.z = tracked_object.kinematics.pose_with_covariance.pose.position.z; + marker.scale.x = 0.3; + marker.scale.y = 0.3; + marker.scale.z = 0.3; + + marker.color = color; + marker.lifetime = rclcpp::Duration::from_seconds(0); + markers_.markers.push_back(marker); + + // get marker - existence probability text + std::vector existence_vector; + tracker->getExistenceProbabilityVector(existence_vector); + std::string existence_probability_text = "P: "; + for (const auto & existence_probability : existence_vector) { + // probability to text, two digits of percentage + existence_probability_text += + std::to_string(static_cast(existence_probability * 100)) + " "; + } + + visualization_msgs::msg::Marker text_marker; + text_marker.header.frame_id = frame_id_; + text_marker.header.stamp = message_time_; + text_marker.ns = "existence_probability"; + text_marker.id = uuid_int; + text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + text_marker.action = visualization_msgs::msg::Marker::ADD; + text_marker.pose.position.x = tracked_object.kinematics.pose_with_covariance.pose.position.x; + text_marker.pose.position.y = tracked_object.kinematics.pose_with_covariance.pose.position.y; + text_marker.pose.position.z = + tracked_object.kinematics.pose_with_covariance.pose.position.z + 0.8; + text_marker.scale.z = 0.8; + text_marker.color.a = 1.0; + text_marker.color.r = 1.0; + text_marker.color.g = 1.0; + text_marker.color.b = 1.0; + text_marker.text = existence_probability_text; + text_marker.lifetime = rclcpp::Duration::from_seconds(0); + markers_.markers.push_back(text_marker); + + // get marker - association link lines + } +} + +void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & marker_array) +{ + if (!is_initialized_) return; + + // fill in marker array + for (auto & marker : markers_.markers) { + marker_array.markers.push_back(marker); + } + + // remove old markers + for (const auto & previous_id : previous_ids_) { + if (current_ids_.find(previous_id) != current_ids_.end()) { + continue; + } + + visualization_msgs::msg::Marker delete_marker; + delete_marker.header.frame_id = frame_id_; + delete_marker.header.stamp = message_time_; + delete_marker.ns = "boxes"; + delete_marker.id = previous_id; + delete_marker.action = visualization_msgs::msg::Marker::DELETE; + + marker_array.markers.push_back(delete_marker); + + delete_marker.ns = "existence_probability"; + marker_array.markers.push_back(delete_marker); + } + + previous_ids_.clear(); + previous_ids_ = current_ids_; + current_ids_.clear(); + markers_.markers.clear(); + + return; +} diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp index 30838220a566e..bfba05e587c9e 100644 --- a/perception/multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger/debugger.cpp @@ -16,7 +16,8 @@ #include -TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : node_(node), diagnostic_updater_(&node) +TrackerDebugger::TrackerDebugger(rclcpp::Node & node, const std::string & frame_id) +: node_(node), diagnostic_updater_(&node), object_debugger_(frame_id) { // declare debug parameters to decide whether to publish debug topics loadParameters(); @@ -32,6 +33,9 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : node_(node), diagnostic_ "debug/tentative_objects", rclcpp::QoS{1}); } + debug_objects_markers_pub_ = node_.create_publisher( + "multi_object_tracker/debug/objects_markers", rclcpp::QoS{1}); + // initialize timestamps const rclcpp::Time now = node_.now(); last_input_stamp_ = now; @@ -169,3 +173,23 @@ void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Tim } stamp_publish_output_ = now; } + +void TrackerDebugger::collectObjectInfo( + const rclcpp::Time & message_time, const std::list> & list_tracker, + const uint & channel_index, + const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, + const std::unordered_map & direct_assignment, + const std::unordered_map & reverse_assignment) +{ + object_debugger_.collect( + message_time, list_tracker, channel_index, detected_objects, direct_assignment, + reverse_assignment); +} + +// ObjectDebugger +void TrackerDebugger::publishObjectsMarkers() +{ + visualization_msgs::msg::MarkerArray marker_message; + object_debugger_.getMessage(marker_message); + debug_objects_markers_pub_->publish(marker_message); +} 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 74f3a24ee0347..a80f12442f75e 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -165,7 +165,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) } // Debugger - debugger_ = std::make_unique(*this); + debugger_ = std::make_unique(*this, world_frame_id_); published_time_publisher_ = std::make_unique(this); } @@ -229,7 +229,7 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list) // process end debugger_->endMeasurementTime(this->now()); - // for debug + /* DEBUG */ const rclcpp::Time latest_time(objects_list.back().second.header.stamp); RCLCPP_INFO( this->get_logger(), "MultiObjectTracker::onMessage Objects time range: %f - %f", @@ -252,6 +252,7 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list) input_channels_[i].long_name + " " + std::to_string(object_counts[i]) + " "; } RCLCPP_INFO(this->get_logger(), object_counts_str.c_str()); + /* DEBUG END */ } void MultiObjectTracker::runProcess( @@ -297,6 +298,12 @@ void MultiObjectTracker::runProcess( /* spawn new tracker */ processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index); + + // Collect debug information - tracker list, existence probabilities, association result + // TODO(technolojin): add option to enable/disable debug information + debugger_->collectObjectInfo( + measurement_time, processor_->getListTracker(), channel_index, transformed_objects, + direct_assignment, reverse_assignment); } void MultiObjectTracker::checkAndPublish(const rclcpp::Time & time) @@ -337,6 +344,7 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const processor_->getTentativeObjects(time, tentative_objects_msg); debugger_->publishTentativeObjects(tentative_objects_msg); } + debugger_->publishObjectsMarkers(); } } // namespace multi_object_tracker From 15455cd26e74150a64de064b9e3f6053c82d2ab7 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 18 Apr 2024 19:18:59 +0900 Subject: [PATCH 27/67] feat: indexing marker step 1 Signed-off-by: Taekjin LEE --- .../debugger/debug_object.hpp | 90 ++++++++++++++++++- .../src/debugger/debug_object.cpp | 47 ++++++++-- .../src/debugger/debugger.cpp | 1 + 3 files changed, 129 insertions(+), 9 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp index 18b8b77739800..699cb100cb033 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp @@ -22,13 +22,39 @@ #include "unique_identifier_msgs/msg/uuid.hpp" #include +#include +#include #include +#include +#include +#include + #include #include #include #include #include +#include + +struct ObjectData +{ + rclcpp::Time time; + + // object uuid + unique_identifier_msgs::msg::UUID uuid; + + // association link, pair of coordinates + // tracker to detection + geometry_msgs::msg::Point tracker_point; + geometry_msgs::msg::Point detection_point; + + // existence probabilities + std::vector existence_vector; + + // detection channel id + uint channel_id; +}; class TrackerObjectDebugger { @@ -43,6 +69,11 @@ class TrackerObjectDebugger std::unordered_set previous_ids_; rclcpp::Time message_time_; + std::unordered_map object_data_; + std::unordered_map> id_map_; + std::list unused_marker_ids_; + int32_t marker_id_ = 0; + public: void collect( const rclcpp::Time & message_time, const std::list> & list_tracker, @@ -52,8 +83,65 @@ class TrackerObjectDebugger const std::unordered_map & reverse_assignment); void reset(); + void draw(); + void getMessage(visualization_msgs::msg::MarkerArray & message) const; + +private: + std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & u) const + { + std::stringstream ss; + for (auto i = 0; i < 16; ++i) { + ss << std::hex << std::setfill('0') << std::setw(2) << +u.uuid[i]; + } + return ss.str(); + } + + boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg) + { + const std::string uuid_str = uuid_to_string(uuid_msg); + boost::uuids::string_generator gen; + boost::uuids::uuid uuid = gen(uuid_str); + return uuid; + } + + void update_id_map(const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & msg) + { + std::vector new_uuids; + std::vector tracked_uuids; + new_uuids.reserve(msg->objects.size()); + tracked_uuids.reserve(msg->objects.size()); + for (const auto & object : msg->objects) { + const auto uuid = to_boost_uuid(object.object_id); + ((id_map_.find(uuid) != id_map_.end()) ? tracked_uuids : new_uuids).push_back(uuid); + } + + auto itr = id_map_.begin(); + while (itr != id_map_.end()) { + if ( + std::find(tracked_uuids.begin(), tracked_uuids.end(), itr->first) == tracked_uuids.end()) { + unused_marker_ids_.push_back(itr->second); + itr = id_map_.erase(itr); + } else { + ++itr; + } + } + + for (const auto & new_uuid : new_uuids) { + if (unused_marker_ids_.empty()) { + id_map_.emplace(new_uuid, marker_id_); + marker_id_++; + } else { + id_map_.emplace(new_uuid, unused_marker_ids_.front()); + unused_marker_ids_.pop_front(); + } + } + } - void getMessage(visualization_msgs::msg::MarkerArray & message); + int32_t uuid_to_marker_id(const unique_identifier_msgs::msg::UUID & uuid_msg) + { + auto uuid = to_boost_uuid(uuid_msg); + return id_map_.at(uuid); + } }; #endif // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp index 7c317c002b754..1588abad41f09 100644 --- a/perception/multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -37,6 +37,7 @@ int uuid_to_int(const unique_identifier_msgs::msg::UUID & uuid) return hashed_uuid; } + } // namespace TrackerObjectDebugger::TrackerObjectDebugger(std::string frame_id) @@ -53,8 +54,10 @@ TrackerObjectDebugger::TrackerObjectDebugger(std::string frame_id) void TrackerObjectDebugger::reset() { - markers_.markers.clear(); + previous_ids_.clear(); + previous_ids_ = current_ids_; current_ids_.clear(); + markers_.markers.clear(); } void TrackerObjectDebugger::collect( @@ -82,7 +85,7 @@ void TrackerObjectDebugger::collect( {{0.4, 1.0, 1.0}}, // Light Blue, Light Yellow, Light Cyan {{1.0, 0.4, 1.0}}, {{1.0, 0.7, 0.4}}, {{0.7, 0.4, 1.0}}, // Light Magenta, Light Orange, Light Purple - {{1.0, 0.6, 0.8}}, {{0.71, 0.4, 0.114}}, + {{1.0, 0.6, 0.8}}, {{0.71, 0.4, 0.12}}, {{0.55, 0.0, 0.0}}, // Light Pink, Light Brown, Dark Red {{0.0, 0.4, 0.0}}, {{0.0, 0.0, 0.55}}, {{0.55, 0.55, 0.0}}, // Dark Green, Dark Blue, Dark Yellow @@ -163,10 +166,43 @@ void TrackerObjectDebugger::collect( } } -void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & marker_array) +void TrackerObjectDebugger::draw() +{ + // constexpr int PALETTE_SIZE = 32; + // constexpr std::array, PALETTE_SIZE> color_array = {{ + // {{1.0, 0.0, 0.0}}, {{0.0, 1.0, 0.0}}, + // {{0.0, 0.0, 1.0}}, // Red, Green, Blue + // {{1.0, 1.0, 0.0}}, {{0.0, 1.0, 1.0}}, + // {{1.0, 0.0, 1.0}}, // Yellow, Cyan, Magenta + // {{1.0, 0.64, 0.0}}, {{0.75, 1.0, 0.0}}, + // {{0.0, 0.5, 0.5}}, // Orange, Lime, Teal + // {{0.5, 0.0, 0.5}}, {{1.0, 0.75, 0.8}}, + // {{0.65, 0.17, 0.17}}, // Purple, Pink, Brown + // {{0.5, 0.0, 0.0}}, {{0.5, 0.5, 0.0}}, + // {{0.0, 0.0, 0.5}}, // Maroon, Olive, Navy + // {{0.5, 0.5, 0.5}}, {{1.0, 0.4, 0.4}}, + // {{0.4, 1.0, 0.4}}, // Grey, Light Red, Light Green + // {{0.4, 0.4, 1.0}}, {{1.0, 1.0, 0.4}}, + // {{0.4, 1.0, 1.0}}, // Light Blue, Light Yellow, Light Cyan + // {{1.0, 0.4, 1.0}}, {{1.0, 0.7, 0.4}}, + // {{0.7, 0.4, 1.0}}, // Light Magenta, Light Orange, Light Purple + // {{1.0, 0.6, 0.8}}, {{0.71, 0.4, 0.12}}, + // {{0.55, 0.0, 0.0}}, // Light Pink, Light Brown, Dark Red + // {{0.0, 0.4, 0.0}}, {{0.0, 0.0, 0.55}}, + // {{0.55, 0.55, 0.0}}, // Dark Green, Dark Blue, Dark Yellow + // {{0.0, 0.55, 0.55}}, {{0.55, 0.0, 0.55}} // Dark Cyan, Dark Magenta + // }}; + + // do nothing for now + return; +} + +void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & marker_array) const { if (!is_initialized_) return; + // draw markers + // fill in marker array for (auto & marker : markers_.markers) { marker_array.markers.push_back(marker); @@ -191,10 +227,5 @@ void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & ma marker_array.markers.push_back(delete_marker); } - previous_ids_.clear(); - previous_ids_ = current_ids_; - current_ids_.clear(); - markers_.markers.clear(); - return; } diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp index bfba05e587c9e..4d0d747bc2ef1 100644 --- a/perception/multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger/debugger.cpp @@ -192,4 +192,5 @@ void TrackerDebugger::publishObjectsMarkers() visualization_msgs::msg::MarkerArray marker_message; object_debugger_.getMessage(marker_message); debug_objects_markers_pub_->publish(marker_message); + object_debugger_.reset(); } From 212752c1173fa0d100e71990976bc515dab91518 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 19 Apr 2024 11:47:44 +0900 Subject: [PATCH 28/67] fix: uuid management Signed-off-by: Taekjin LEE --- .../debugger/debug_object.hpp | 31 +- .../src/debugger/debug_object.cpp | 276 ++++++++++-------- 2 files changed, 181 insertions(+), 126 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp index 699cb100cb033..707cfd4a475aa 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp @@ -42,12 +42,14 @@ struct ObjectData rclcpp::Time time; // object uuid - unique_identifier_msgs::msg::UUID uuid; + boost::uuids::uuid uuid; + int uuid_int; // association link, pair of coordinates // tracker to detection geometry_msgs::msg::Point tracker_point; geometry_msgs::msg::Point detection_point; + bool is_associated{false}; // existence probabilities std::vector existence_vector; @@ -69,7 +71,7 @@ class TrackerObjectDebugger std::unordered_set previous_ids_; rclcpp::Time message_time_; - std::unordered_map object_data_; + std::vector object_data_list_; std::unordered_map> id_map_; std::list unused_marker_ids_; int32_t marker_id_ = 0; @@ -83,8 +85,10 @@ class TrackerObjectDebugger const std::unordered_map & reverse_assignment); void reset(); - void draw(); - void getMessage(visualization_msgs::msg::MarkerArray & message) const; + void draw( + const std::vector> object_data_groups, + visualization_msgs::msg::MarkerArray & marker_array) const; + void getMessage(visualization_msgs::msg::MarkerArray & marker_array); private: std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & u) const @@ -104,17 +108,20 @@ class TrackerObjectDebugger return uuid; } - void update_id_map(const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & msg) + void update_id_map(const std::vector & object_data_list) { std::vector new_uuids; std::vector tracked_uuids; - new_uuids.reserve(msg->objects.size()); - tracked_uuids.reserve(msg->objects.size()); - for (const auto & object : msg->objects) { - const auto uuid = to_boost_uuid(object.object_id); - ((id_map_.find(uuid) != id_map_.end()) ? tracked_uuids : new_uuids).push_back(uuid); + new_uuids.reserve(object_data_list.size()); + tracked_uuids.reserve(object_data_list.size()); + + // check if the object is already tracked + for (const auto & object_data : object_data_list) { + ((id_map_.find(object_data.uuid) != id_map_.end()) ? tracked_uuids : new_uuids) + .push_back(object_data.uuid); } + // remove untracked objects auto itr = id_map_.begin(); while (itr != id_map_.end()) { if ( @@ -126,6 +133,7 @@ class TrackerObjectDebugger } } + // add new objects for (const auto & new_uuid : new_uuids) { if (unused_marker_ids_.empty()) { id_map_.emplace(new_uuid, marker_id_); @@ -139,9 +147,10 @@ class TrackerObjectDebugger int32_t uuid_to_marker_id(const unique_identifier_msgs::msg::UUID & uuid_msg) { - auto uuid = to_boost_uuid(uuid_msg); + const auto uuid = to_boost_uuid(uuid_msg); return id_map_.at(uuid); } + int32_t uuid_to_marker_id(const boost::uuids::uuid & uuid) { return id_map_.at(uuid); } }; #endif // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp index 1588abad41f09..c6f19235dda11 100644 --- a/perception/multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -21,25 +21,6 @@ #include #include -namespace -{ -int uuid_to_int(const unique_identifier_msgs::msg::UUID & uuid) -{ - // Convert UUID to string - std::string uuid_str; - for (auto byte : uuid.uuid) { - uuid_str += std::to_string(byte); - } - - // Hash the string to get an int - std::hash hasher; - int hashed_uuid = hasher(uuid_str); - - return hashed_uuid; -} - -} // namespace - TrackerObjectDebugger::TrackerObjectDebugger(std::string frame_id) { // set frame id @@ -54,19 +35,100 @@ TrackerObjectDebugger::TrackerObjectDebugger(std::string frame_id) void TrackerObjectDebugger::reset() { + // maintain previous ids previous_ids_.clear(); previous_ids_ = current_ids_; current_ids_.clear(); + + // clear markers, object data list + object_data_list_.clear(); markers_.markers.clear(); } void TrackerObjectDebugger::collect( const rclcpp::Time & message_time, const std::list> & list_tracker, const uint & channel_index, - const autoware_auto_perception_msgs::msg::DetectedObjects & /*detected_objects*/, - const std::unordered_map & /*direct_assignment*/, + const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, + const std::unordered_map & direct_assignment, const std::unordered_map & /*reverse_assignment*/) { + if (!is_initialized_) is_initialized_ = true; + + message_time_ = message_time; + + int tracker_idx = 0; + for (auto tracker_itr = list_tracker.begin(); tracker_itr != list_tracker.end(); + ++tracker_itr, ++tracker_idx) { + ObjectData object_data; + object_data.time = message_time; + object_data.channel_id = channel_index; + + autoware_auto_perception_msgs::msg::TrackedObject tracked_object; + (*(tracker_itr))->getTrackedObject(message_time, tracked_object); + object_data.uuid = to_boost_uuid(tracked_object.object_id); + + // tracker + bool is_associated = false; + geometry_msgs::msg::Point tracker_point, detection_point; + tracker_point.x = tracked_object.kinematics.pose_with_covariance.pose.position.x; + tracker_point.y = tracked_object.kinematics.pose_with_covariance.pose.position.y; + tracker_point.z = tracked_object.kinematics.pose_with_covariance.pose.position.z; + + // detection + if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { + const auto & associated_object = + detected_objects.objects.at(direct_assignment.find(tracker_idx)->second); + detection_point.x = associated_object.kinematics.pose_with_covariance.pose.position.x; + detection_point.y = associated_object.kinematics.pose_with_covariance.pose.position.y; + detection_point.z = associated_object.kinematics.pose_with_covariance.pose.position.z; + is_associated = true; + } + + object_data.tracker_point = tracker_point; + object_data.detection_point = detection_point; + object_data.is_associated = is_associated; + + // existence probabilities + std::vector existence_vector; + (*(tracker_itr))->getExistenceProbabilityVector(existence_vector); + object_data.existence_vector = existence_vector; + + object_data_list_.push_back(object_data); + } + + // constexpr int PALETTE_SIZE = 32; + // constexpr std::array, PALETTE_SIZE> color_array = {{ + // {{1.0, 0.0, 0.0}}, {{0.0, 1.0, 0.0}}, + // {{0.0, 0.0, 1.0}}, // Red, Green, Blue + // {{1.0, 1.0, 0.0}}, {{0.0, 1.0, 1.0}}, + // {{1.0, 0.0, 1.0}}, // Yellow, Cyan, Magenta + // {{1.0, 0.64, 0.0}}, {{0.75, 1.0, 0.0}}, + // {{0.0, 0.5, 0.5}}, // Orange, Lime, Teal + // {{0.5, 0.0, 0.5}}, {{1.0, 0.75, 0.8}}, + // {{0.65, 0.17, 0.17}}, // Purple, Pink, Brown + // {{0.5, 0.0, 0.0}}, {{0.5, 0.5, 0.0}}, + // {{0.0, 0.0, 0.5}}, // Maroon, Olive, Navy + // {{0.5, 0.5, 0.5}}, {{1.0, 0.4, 0.4}}, + // {{0.4, 1.0, 0.4}}, // Grey, Light Red, Light Green + // {{0.4, 0.4, 1.0}}, {{1.0, 1.0, 0.4}}, + // {{0.4, 1.0, 1.0}}, // Light Blue, Light Yellow, Light Cyan + // {{1.0, 0.4, 1.0}}, {{1.0, 0.7, 0.4}}, + // {{0.7, 0.4, 1.0}}, // Light Magenta, Light Orange, Light Purple + // {{1.0, 0.6, 0.8}}, {{0.71, 0.4, 0.12}}, + // {{0.55, 0.0, 0.0}}, // Light Pink, Light Brown, Dark Red + // {{0.0, 0.4, 0.0}}, {{0.0, 0.0, 0.55}}, + // {{0.55, 0.55, 0.0}}, // Dark Green, Dark Blue, Dark Yellow + // {{0.0, 0.55, 0.55}}, {{0.55, 0.0, 0.55}} // Dark Cyan, Dark Magenta + // }}; +} + +void TrackerObjectDebugger::draw( + const std::vector> object_data_groups, + visualization_msgs::msg::MarkerArray & marker_array) const +{ + // initialize markers + marker_array.markers.clear(); + constexpr int PALETTE_SIZE = 32; constexpr std::array, PALETTE_SIZE> color_array = {{ {{1.0, 0.0, 0.0}}, {{0.0, 1.0, 0.0}}, @@ -92,122 +154,106 @@ void TrackerObjectDebugger::collect( {{0.0, 0.55, 0.55}}, {{0.55, 0.0, 0.55}} // Dark Cyan, Dark Magenta }}; - message_time_ = message_time; - if (!is_initialized_) is_initialized_ = true; - - for (const auto & tracker : list_tracker) { - // get object - autoware_auto_perception_msgs::msg::TrackedObject tracked_object; - tracker->getTrackedObject(message_time_, tracked_object); - const unique_identifier_msgs::msg::UUID uuid = tracked_object.object_id; - const int uuid_int = uuid_to_int(uuid); - current_ids_.emplace(uuid_int); - - // get existence probability - - // get color - by channel index - std_msgs::msg::ColorRGBA color; - color.a = 1.0; - color.r = color_array[channel_index % PALETTE_SIZE][0]; - color.g = color_array[channel_index % PALETTE_SIZE][1]; - color.b = color_array[channel_index % PALETTE_SIZE][2]; - - // get marker - box - + for (const auto & object_data_group : object_data_groups) { + if (object_data_group.empty()) continue; + const int & uuid_int = object_data_group.front().uuid_int; + const auto object_data_front = object_data_group.front(); + const auto object_data_back = object_data_group.back(); + // set a reference marker visualization_msgs::msg::Marker marker; marker.header.frame_id = frame_id_; - marker.header.stamp = message_time_; - marker.ns = "boxes"; + marker.header.stamp = object_data_front.time; marker.id = uuid_int; - marker.type = visualization_msgs::msg::Marker::CUBE; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.position.x = tracked_object.kinematics.pose_with_covariance.pose.position.x; - marker.pose.position.y = tracked_object.kinematics.pose_with_covariance.pose.position.y; - marker.pose.position.z = tracked_object.kinematics.pose_with_covariance.pose.position.z; - marker.scale.x = 0.3; - marker.scale.y = 0.3; - marker.scale.z = 0.3; - - marker.color = color; + marker.pose.position.x = object_data_front.tracker_point.x; + marker.pose.position.y = object_data_front.tracker_point.y; + marker.pose.position.z = object_data_front.tracker_point.z; + marker.color.a = 1.0; + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; // white marker.lifetime = rclcpp::Duration::from_seconds(0); - markers_.markers.push_back(marker); - - // get marker - existence probability text - std::vector existence_vector; - tracker->getExistenceProbabilityVector(existence_vector); - std::string existence_probability_text = "P: "; - for (const auto & existence_probability : existence_vector) { - // probability to text, two digits of percentage - existence_probability_text += - std::to_string(static_cast(existence_probability * 100)) + " "; - } + // get marker - existence_probability visualization_msgs::msg::Marker text_marker; - text_marker.header.frame_id = frame_id_; - text_marker.header.stamp = message_time_; + text_marker = marker; text_marker.ns = "existence_probability"; - text_marker.id = uuid_int; text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; text_marker.action = visualization_msgs::msg::Marker::ADD; - text_marker.pose.position.x = tracked_object.kinematics.pose_with_covariance.pose.position.x; - text_marker.pose.position.y = tracked_object.kinematics.pose_with_covariance.pose.position.y; - text_marker.pose.position.z = - tracked_object.kinematics.pose_with_covariance.pose.position.z + 0.8; + text_marker.pose.position.z += 1.5; text_marker.scale.z = 0.8; - text_marker.color.a = 1.0; - text_marker.color.r = 1.0; - text_marker.color.g = 1.0; - text_marker.color.b = 1.0; - text_marker.text = existence_probability_text; - text_marker.lifetime = rclcpp::Duration::from_seconds(0); - markers_.markers.push_back(text_marker); - // get marker - association link lines + // show the last existence probability + std::string existence_probability_text = "P: "; + for (const auto & existence_probability : object_data_front.existence_vector) { + // probability to text, two digits of percentage + existence_probability_text += + std::to_string(static_cast(existence_probability * 100)) + " "; + } + text_marker.text = existence_probability_text; + marker_array.markers.push_back(text_marker); + + // loop for each object_data in the group + // boxed to tracker positions + // and link lines to the detected positions + visualization_msgs::msg::Marker marker_boxes; + marker_boxes = marker; + marker_boxes.ns = "boxes"; + marker_boxes.type = visualization_msgs::msg::Marker::CUBE_LIST; + marker_boxes.action = visualization_msgs::msg::Marker::ADD; + + for (const auto & object_data : object_data_group) { + // get color - by channel index + std_msgs::msg::ColorRGBA color; + color.a = 1.0; + color.r = color_array[object_data.channel_id % PALETTE_SIZE][0]; + color.g = color_array[object_data.channel_id % PALETTE_SIZE][1]; + color.b = color_array[object_data.channel_id % PALETTE_SIZE][2]; + marker_boxes.color = color; + } } -} - -void TrackerObjectDebugger::draw() -{ - // constexpr int PALETTE_SIZE = 32; - // constexpr std::array, PALETTE_SIZE> color_array = {{ - // {{1.0, 0.0, 0.0}}, {{0.0, 1.0, 0.0}}, - // {{0.0, 0.0, 1.0}}, // Red, Green, Blue - // {{1.0, 1.0, 0.0}}, {{0.0, 1.0, 1.0}}, - // {{1.0, 0.0, 1.0}}, // Yellow, Cyan, Magenta - // {{1.0, 0.64, 0.0}}, {{0.75, 1.0, 0.0}}, - // {{0.0, 0.5, 0.5}}, // Orange, Lime, Teal - // {{0.5, 0.0, 0.5}}, {{1.0, 0.75, 0.8}}, - // {{0.65, 0.17, 0.17}}, // Purple, Pink, Brown - // {{0.5, 0.0, 0.0}}, {{0.5, 0.5, 0.0}}, - // {{0.0, 0.0, 0.5}}, // Maroon, Olive, Navy - // {{0.5, 0.5, 0.5}}, {{1.0, 0.4, 0.4}}, - // {{0.4, 1.0, 0.4}}, // Grey, Light Red, Light Green - // {{0.4, 0.4, 1.0}}, {{1.0, 1.0, 0.4}}, - // {{0.4, 1.0, 1.0}}, // Light Blue, Light Yellow, Light Cyan - // {{1.0, 0.4, 1.0}}, {{1.0, 0.7, 0.4}}, - // {{0.7, 0.4, 1.0}}, // Light Magenta, Light Orange, Light Purple - // {{1.0, 0.6, 0.8}}, {{0.71, 0.4, 0.12}}, - // {{0.55, 0.0, 0.0}}, // Light Pink, Light Brown, Dark Red - // {{0.0, 0.4, 0.0}}, {{0.0, 0.0, 0.55}}, - // {{0.55, 0.55, 0.0}}, // Dark Green, Dark Blue, Dark Yellow - // {{0.0, 0.55, 0.55}}, {{0.55, 0.0, 0.55}} // Dark Cyan, Dark Magenta - // }}; - // do nothing for now return; } -void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & marker_array) const +void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & marker_array) { if (!is_initialized_) return; - // draw markers + // check all object data and update marker index + update_id_map(object_data_list_); - // fill in marker array - for (auto & marker : markers_.markers) { - marker_array.markers.push_back(marker); + // update uuid_int + for (auto & object_data : object_data_list_) { + object_data.uuid_int = uuid_to_marker_id(object_data.uuid); } + // sort by uuid, collect the same uuid object_data as a group, and loop for the groups + std::vector> object_data_groups; + { + // sort by uuid_int + std::sort( + object_data_list_.begin(), object_data_list_.end(), + [](const ObjectData & a, const ObjectData & b) { return a.uuid_int < b.uuid_int; }); + + // collect the same uuid object_data as a group + std::vector object_data_group; + boost::uuids::uuid previous_uuid = object_data_list_.front().uuid; + for (const auto & object_data : object_data_list_) { + if (object_data.uuid != previous_uuid) { + object_data_groups.push_back(object_data_group); + object_data_group.clear(); + previous_uuid = object_data.uuid; + } + object_data_group.push_back(object_data); + } + + // fill the vector of groups + object_data_groups.push_back(object_data_group); + } + + // draw markers + draw(object_data_groups, marker_array); + // remove old markers for (const auto & previous_id : previous_ids_) { if (current_ids_.find(previous_id) != current_ids_.end()) { From b6f59a9756cdcf8e1f2f4414422bd2ae97515ba8 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 19 Apr 2024 16:07:09 +0900 Subject: [PATCH 29/67] feat: association line fix Signed-off-by: Taekjin LEE --- .../debugger/debug_object.hpp | 4 +- .../src/debugger/debug_object.cpp | 203 ++++++++++-------- .../src/debugger/debugger.cpp | 7 + 3 files changed, 128 insertions(+), 86 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp index 707cfd4a475aa..4875b6105fa9a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp @@ -75,6 +75,7 @@ class TrackerObjectDebugger std::unordered_map> id_map_; std::list unused_marker_ids_; int32_t marker_id_ = 0; + std::vector> object_data_groups_; public: void collect( @@ -88,7 +89,8 @@ class TrackerObjectDebugger void draw( const std::vector> object_data_groups, visualization_msgs::msg::MarkerArray & marker_array) const; - void getMessage(visualization_msgs::msg::MarkerArray & marker_array); + void process(); + void getMessage(visualization_msgs::msg::MarkerArray & marker_array) const; private: std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & u) const diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp index c6f19235dda11..b7696ed7e7a44 100644 --- a/perception/multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -82,6 +82,10 @@ void TrackerObjectDebugger::collect( detection_point.y = associated_object.kinematics.pose_with_covariance.pose.position.y; detection_point.z = associated_object.kinematics.pose_with_covariance.pose.position.z; is_associated = true; + } else { + detection_point.x = tracker_point.x; + detection_point.y = tracker_point.y; + detection_point.z = tracker_point.z; } object_data.tracker_point = tracker_point; @@ -95,31 +99,47 @@ void TrackerObjectDebugger::collect( object_data_list_.push_back(object_data); } +} + +void TrackerObjectDebugger::process() +{ + if (!is_initialized_) return; + + // check all object data and update marker index + update_id_map(object_data_list_); + + // update uuid_int + for (auto & object_data : object_data_list_) { + object_data.uuid_int = uuid_to_marker_id(object_data.uuid); + current_ids_.insert(object_data.uuid_int); + } + + // sort by uuid, collect the same uuid object_data as a group, and loop for the groups + // std::vector> object_data_groups; + object_data_groups_.clear(); + { + // sort by uuid_int + std::sort( + object_data_list_.begin(), object_data_list_.end(), + [](const ObjectData & a, const ObjectData & b) { return a.uuid_int < b.uuid_int; }); - // constexpr int PALETTE_SIZE = 32; - // constexpr std::array, PALETTE_SIZE> color_array = {{ - // {{1.0, 0.0, 0.0}}, {{0.0, 1.0, 0.0}}, - // {{0.0, 0.0, 1.0}}, // Red, Green, Blue - // {{1.0, 1.0, 0.0}}, {{0.0, 1.0, 1.0}}, - // {{1.0, 0.0, 1.0}}, // Yellow, Cyan, Magenta - // {{1.0, 0.64, 0.0}}, {{0.75, 1.0, 0.0}}, - // {{0.0, 0.5, 0.5}}, // Orange, Lime, Teal - // {{0.5, 0.0, 0.5}}, {{1.0, 0.75, 0.8}}, - // {{0.65, 0.17, 0.17}}, // Purple, Pink, Brown - // {{0.5, 0.0, 0.0}}, {{0.5, 0.5, 0.0}}, - // {{0.0, 0.0, 0.5}}, // Maroon, Olive, Navy - // {{0.5, 0.5, 0.5}}, {{1.0, 0.4, 0.4}}, - // {{0.4, 1.0, 0.4}}, // Grey, Light Red, Light Green - // {{0.4, 0.4, 1.0}}, {{1.0, 1.0, 0.4}}, - // {{0.4, 1.0, 1.0}}, // Light Blue, Light Yellow, Light Cyan - // {{1.0, 0.4, 1.0}}, {{1.0, 0.7, 0.4}}, - // {{0.7, 0.4, 1.0}}, // Light Magenta, Light Orange, Light Purple - // {{1.0, 0.6, 0.8}}, {{0.71, 0.4, 0.12}}, - // {{0.55, 0.0, 0.0}}, // Light Pink, Light Brown, Dark Red - // {{0.0, 0.4, 0.0}}, {{0.0, 0.0, 0.55}}, - // {{0.55, 0.55, 0.0}}, // Dark Green, Dark Blue, Dark Yellow - // {{0.0, 0.55, 0.55}}, {{0.55, 0.0, 0.55}} // Dark Cyan, Dark Magenta - // }}; + // collect the same uuid object_data as a group + std::vector object_data_group; + boost::uuids::uuid previous_uuid = object_data_list_.front().uuid; + for (const auto & object_data : object_data_list_) { + // if the uuid is different, push the group and clear the group + if (object_data.uuid != previous_uuid) { + // push the group + object_data_groups_.push_back(object_data_group); + object_data_group.clear(); + previous_uuid = object_data.uuid; + } + // push the object_data to the group + object_data_group.push_back(object_data); + } + // push the last group + object_data_groups_.push_back(object_data_group); + } } void TrackerObjectDebugger::draw( @@ -129,29 +149,24 @@ void TrackerObjectDebugger::draw( // initialize markers marker_array.markers.clear(); - constexpr int PALETTE_SIZE = 32; + constexpr int PALETTE_SIZE = 16; constexpr std::array, PALETTE_SIZE> color_array = {{ - {{1.0, 0.0, 0.0}}, {{0.0, 1.0, 0.0}}, - {{0.0, 0.0, 1.0}}, // Red, Green, Blue - {{1.0, 1.0, 0.0}}, {{0.0, 1.0, 1.0}}, - {{1.0, 0.0, 1.0}}, // Yellow, Cyan, Magenta - {{1.0, 0.64, 0.0}}, {{0.75, 1.0, 0.0}}, - {{0.0, 0.5, 0.5}}, // Orange, Lime, Teal - {{0.5, 0.0, 0.5}}, {{1.0, 0.75, 0.8}}, - {{0.65, 0.17, 0.17}}, // Purple, Pink, Brown - {{0.5, 0.0, 0.0}}, {{0.5, 0.5, 0.0}}, - {{0.0, 0.0, 0.5}}, // Maroon, Olive, Navy - {{0.5, 0.5, 0.5}}, {{1.0, 0.4, 0.4}}, - {{0.4, 1.0, 0.4}}, // Grey, Light Red, Light Green - {{0.4, 0.4, 1.0}}, {{1.0, 1.0, 0.4}}, - {{0.4, 1.0, 1.0}}, // Light Blue, Light Yellow, Light Cyan - {{1.0, 0.4, 1.0}}, {{1.0, 0.7, 0.4}}, - {{0.7, 0.4, 1.0}}, // Light Magenta, Light Orange, Light Purple - {{1.0, 0.6, 0.8}}, {{0.71, 0.4, 0.12}}, - {{0.55, 0.0, 0.0}}, // Light Pink, Light Brown, Dark Red - {{0.0, 0.4, 0.0}}, {{0.0, 0.0, 0.55}}, - {{0.55, 0.55, 0.0}}, // Dark Green, Dark Blue, Dark Yellow - {{0.0, 0.55, 0.55}}, {{0.55, 0.0, 0.55}} // Dark Cyan, Dark Magenta + {{0.0, 0.0, 1.0}}, // Blue + {{0.0, 1.0, 0.0}}, // Green + {{1.0, 1.0, 0.0}}, // Yellow + {{1.0, 0.0, 0.0}}, // Red + {{0.0, 1.0, 1.0}}, // Cyan + {{1.0, 0.0, 1.0}}, // Magenta + {{1.0, 0.64, 0.0}}, // Orange + {{0.75, 1.0, 0.0}}, // Lime + {{0.0, 0.5, 0.5}}, // Teal + {{0.5, 0.0, 0.5}}, // Purple + {{1.0, 0.75, 0.8}}, // Pink + {{0.65, 0.17, 0.17}}, // Brown + {{0.5, 0.0, 0.0}}, // Maroon + {{0.5, 0.5, 0.0}}, // Olive + {{0.0, 0.0, 0.5}}, // Navy + {{0.5, 0.5, 0.5}} // Grey }}; for (const auto & object_data_group : object_data_groups) { @@ -164,9 +179,9 @@ void TrackerObjectDebugger::draw( marker.header.frame_id = frame_id_; marker.header.stamp = object_data_front.time; marker.id = uuid_int; - marker.pose.position.x = object_data_front.tracker_point.x; - marker.pose.position.y = object_data_front.tracker_point.y; - marker.pose.position.z = object_data_front.tracker_point.z; + marker.pose.position.x = 0; + marker.pose.position.y = 0; + marker.pose.position.z = 0; marker.color.a = 1.0; marker.color.r = 1.0; marker.color.g = 1.0; @@ -181,6 +196,9 @@ void TrackerObjectDebugger::draw( text_marker.action = visualization_msgs::msg::Marker::ADD; text_marker.pose.position.z += 1.5; text_marker.scale.z = 0.8; + text_marker.pose.position.x = object_data_front.tracker_point.x; + text_marker.pose.position.y = object_data_front.tracker_point.y; + text_marker.pose.position.z = object_data_front.tracker_point.z + 1.0; // show the last existence probability std::string existence_probability_text = "P: "; @@ -195,64 +213,77 @@ void TrackerObjectDebugger::draw( // loop for each object_data in the group // boxed to tracker positions // and link lines to the detected positions + const double line_height_offset = 1.0; + visualization_msgs::msg::Marker marker_boxes; marker_boxes = marker; marker_boxes.ns = "boxes"; marker_boxes.type = visualization_msgs::msg::Marker::CUBE_LIST; marker_boxes.action = visualization_msgs::msg::Marker::ADD; + visualization_msgs::msg::Marker marker_lines; + marker_lines = marker; + marker_lines.ns = "association_lines"; + marker_lines.type = visualization_msgs::msg::Marker::LINE_LIST; + marker_lines.action = visualization_msgs::msg::Marker::ADD; + marker_lines.scale.x = 0.2; + marker_lines.points.clear(); + for (const auto & object_data : object_data_group) { + // set box + geometry_msgs::msg::Point box_point; + box_point.x = object_data.tracker_point.x; + box_point.y = object_data.tracker_point.y; + box_point.z = object_data.tracker_point.z; + marker_boxes.points.push_back(box_point); + marker_boxes.scale.x = 0.2; + marker_boxes.scale.y = 0.2; + marker_boxes.scale.z = 0.2; + + // set association marker, if exists + if (!object_data.is_associated) continue; // get color - by channel index std_msgs::msg::ColorRGBA color; color.a = 1.0; color.r = color_array[object_data.channel_id % PALETTE_SIZE][0]; color.g = color_array[object_data.channel_id % PALETTE_SIZE][1]; color.b = color_array[object_data.channel_id % PALETTE_SIZE][2]; + + // association line + geometry_msgs::msg::Point line_point; + marker_lines.color = color; + + line_point.x = object_data.tracker_point.x; + line_point.y = object_data.tracker_point.y; + line_point.z = object_data.tracker_point.z + line_height_offset; + marker_lines.points.push_back(line_point); + line_point.x = object_data.detection_point.x; + line_point.y = object_data.detection_point.y; + line_point.z = object_data.tracker_point.z + line_height_offset + 2; + marker_lines.points.push_back(line_point); + + // associated object box + box_point.x = object_data.detection_point.x; + box_point.y = object_data.detection_point.y; + box_point.z = object_data.detection_point.z; marker_boxes.color = color; + marker_boxes.points.push_back(box_point); } + + // add markers + marker_array.markers.push_back(marker_boxes); + marker_array.markers.push_back(marker_lines); } return; } -void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & marker_array) +void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & marker_array) const { if (!is_initialized_) return; - // check all object data and update marker index - update_id_map(object_data_list_); - - // update uuid_int - for (auto & object_data : object_data_list_) { - object_data.uuid_int = uuid_to_marker_id(object_data.uuid); - } - - // sort by uuid, collect the same uuid object_data as a group, and loop for the groups - std::vector> object_data_groups; - { - // sort by uuid_int - std::sort( - object_data_list_.begin(), object_data_list_.end(), - [](const ObjectData & a, const ObjectData & b) { return a.uuid_int < b.uuid_int; }); - - // collect the same uuid object_data as a group - std::vector object_data_group; - boost::uuids::uuid previous_uuid = object_data_list_.front().uuid; - for (const auto & object_data : object_data_list_) { - if (object_data.uuid != previous_uuid) { - object_data_groups.push_back(object_data_group); - object_data_group.clear(); - previous_uuid = object_data.uuid; - } - object_data_group.push_back(object_data); - } - - // fill the vector of groups - object_data_groups.push_back(object_data_group); - } - // draw markers - draw(object_data_groups, marker_array); + draw(object_data_groups_, marker_array); // remove old markers for (const auto & previous_id : previous_ids_) { @@ -263,13 +294,15 @@ void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & ma visualization_msgs::msg::Marker delete_marker; delete_marker.header.frame_id = frame_id_; delete_marker.header.stamp = message_time_; - delete_marker.ns = "boxes"; + delete_marker.ns = "existence_probability"; delete_marker.id = previous_id; delete_marker.action = visualization_msgs::msg::Marker::DELETE; marker_array.markers.push_back(delete_marker); - delete_marker.ns = "existence_probability"; + delete_marker.ns = "boxes"; + marker_array.markers.push_back(delete_marker); + delete_marker.ns = "association_lines"; marker_array.markers.push_back(delete_marker); } diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp index 4d0d747bc2ef1..04b5baedbb369 100644 --- a/perception/multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger/debugger.cpp @@ -190,7 +190,14 @@ void TrackerDebugger::collectObjectInfo( void TrackerDebugger::publishObjectsMarkers() { visualization_msgs::msg::MarkerArray marker_message; + + // process data + object_debugger_.process(); + + // publish markers object_debugger_.getMessage(marker_message); debug_objects_markers_pub_->publish(marker_message); + + // reset object data object_debugger_.reset(); } From 104dac486a52292f40bb6655cdf654c66ade2528 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 19 Apr 2024 17:07:18 +0900 Subject: [PATCH 30/67] feat: print channel names Signed-off-by: Taekjin LEE --- .../debugger/debug_object.hpp | 6 ++ .../debugger/debugger.hpp | 5 ++ .../src/debugger/debug_object.cpp | 84 ++++++++++++------- .../src/multi_object_tracker_core.cpp | 1 + .../src/tracker/model/tracker_base.cpp | 10 ++- 5 files changed, 73 insertions(+), 33 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp index 4875b6105fa9a..44137c04a3580 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp @@ -77,7 +77,13 @@ class TrackerObjectDebugger int32_t marker_id_ = 0; std::vector> object_data_groups_; + std::vector channel_names_; + public: + void setChannelNames(const std::vector & channel_names) + { + channel_names_ = channel_names; + } void collect( const rclcpp::Time & message_time, const std::list> & list_tracker, const uint & channel_index, diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp index 8a5ebd8a61ff1..32ca7cc9effa7 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp @@ -31,6 +31,7 @@ #include #include #include +#include /** * @brief Debugger class for multi object tracker @@ -89,6 +90,10 @@ class TrackerDebugger void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat); // Debug object + void setObjectChannels(const std::vector & channels) + { + object_debugger_.setChannelNames(channels); + } void collectObjectInfo( const rclcpp::Time & message_time, const std::list> & list_tracker, const uint & channel_index, diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp index b7696ed7e7a44..a74885376c3c5 100644 --- a/perception/multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -19,6 +19,8 @@ #include #include +#include +#include #include TrackerObjectDebugger::TrackerObjectDebugger(std::string frame_id) @@ -194,60 +196,77 @@ void TrackerObjectDebugger::draw( text_marker.ns = "existence_probability"; text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; text_marker.action = visualization_msgs::msg::Marker::ADD; - text_marker.pose.position.z += 1.5; - text_marker.scale.z = 0.8; + text_marker.pose.position.z += 1.8; + text_marker.scale.z = 0.7; text_marker.pose.position.x = object_data_front.tracker_point.x; text_marker.pose.position.y = object_data_front.tracker_point.y; text_marker.pose.position.z = object_data_front.tracker_point.z + 1.0; // show the last existence probability - std::string existence_probability_text = "P: "; - for (const auto & existence_probability : object_data_front.existence_vector) { - // probability to text, two digits of percentage - existence_probability_text += - std::to_string(static_cast(existence_probability * 100)) + " "; + // print existence probability with channel name + // probability to text, two digits of percentage + std::string existence_probability_text = "P:"; + for (size_t i = 0; i < object_data_front.existence_vector.size(); ++i) { + std::stringstream stream; + stream << std::fixed << std::setprecision(0) << object_data_front.existence_vector[i] * 100; + existence_probability_text += " " + channel_names_[i] + ":" + stream.str(); } + text_marker.text = existence_probability_text; marker_array.markers.push_back(text_marker); // loop for each object_data in the group // boxed to tracker positions // and link lines to the detected positions - const double line_height_offset = 1.0; - - visualization_msgs::msg::Marker marker_boxes; - marker_boxes = marker; - marker_boxes.ns = "boxes"; - marker_boxes.type = visualization_msgs::msg::Marker::CUBE_LIST; - marker_boxes.action = visualization_msgs::msg::Marker::ADD; + const double height_offset = 1.0; + + visualization_msgs::msg::Marker marker_track_boxes; + marker_track_boxes = marker; + marker_track_boxes.ns = "track_boxes"; + marker_track_boxes.type = visualization_msgs::msg::Marker::CUBE_LIST; + marker_track_boxes.action = visualization_msgs::msg::Marker::ADD; + marker_track_boxes.scale.x = 0.4; + marker_track_boxes.scale.y = 0.4; + marker_track_boxes.scale.z = 0.4; + + visualization_msgs::msg::Marker marker_detect_boxes; + marker_detect_boxes = marker; + marker_detect_boxes.ns = "detect_boxes"; + marker_detect_boxes.type = visualization_msgs::msg::Marker::CUBE_LIST; + marker_detect_boxes.action = visualization_msgs::msg::Marker::ADD; + marker_detect_boxes.scale.x = 0.2; + marker_detect_boxes.scale.y = 0.2; + marker_detect_boxes.scale.z = 0.2; visualization_msgs::msg::Marker marker_lines; marker_lines = marker; marker_lines.ns = "association_lines"; marker_lines.type = visualization_msgs::msg::Marker::LINE_LIST; marker_lines.action = visualization_msgs::msg::Marker::ADD; - marker_lines.scale.x = 0.2; + marker_lines.scale.x = 0.15; marker_lines.points.clear(); + bool is_line_drawn = false; + for (const auto & object_data : object_data_group) { + int channel_id = object_data.channel_id; + // set box geometry_msgs::msg::Point box_point; box_point.x = object_data.tracker_point.x; box_point.y = object_data.tracker_point.y; - box_point.z = object_data.tracker_point.z; - marker_boxes.points.push_back(box_point); - marker_boxes.scale.x = 0.2; - marker_boxes.scale.y = 0.2; - marker_boxes.scale.z = 0.2; + box_point.z = object_data.tracker_point.z + height_offset; + marker_track_boxes.points.push_back(box_point); // set association marker, if exists if (!object_data.is_associated) continue; + is_line_drawn = true; // get color - by channel index std_msgs::msg::ColorRGBA color; color.a = 1.0; - color.r = color_array[object_data.channel_id % PALETTE_SIZE][0]; - color.g = color_array[object_data.channel_id % PALETTE_SIZE][1]; - color.b = color_array[object_data.channel_id % PALETTE_SIZE][2]; + color.r = color_array[channel_id % PALETTE_SIZE][0]; + color.g = color_array[channel_id % PALETTE_SIZE][1]; + color.b = color_array[channel_id % PALETTE_SIZE][2]; // association line geometry_msgs::msg::Point line_point; @@ -255,24 +274,25 @@ void TrackerObjectDebugger::draw( line_point.x = object_data.tracker_point.x; line_point.y = object_data.tracker_point.y; - line_point.z = object_data.tracker_point.z + line_height_offset; + line_point.z = object_data.tracker_point.z + height_offset; marker_lines.points.push_back(line_point); line_point.x = object_data.detection_point.x; line_point.y = object_data.detection_point.y; - line_point.z = object_data.tracker_point.z + line_height_offset + 2; + line_point.z = object_data.tracker_point.z + height_offset + 1; marker_lines.points.push_back(line_point); // associated object box box_point.x = object_data.detection_point.x; box_point.y = object_data.detection_point.y; - box_point.z = object_data.detection_point.z; - marker_boxes.color = color; - marker_boxes.points.push_back(box_point); + box_point.z = object_data.detection_point.z + height_offset + 1; + marker_detect_boxes.color = color; + marker_detect_boxes.points.push_back(box_point); } // add markers - marker_array.markers.push_back(marker_boxes); - marker_array.markers.push_back(marker_lines); + marker_array.markers.push_back(marker_track_boxes); + marker_array.markers.push_back(marker_detect_boxes); + if (is_line_drawn) marker_array.markers.push_back(marker_lines); } return; @@ -300,7 +320,9 @@ void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & ma marker_array.markers.push_back(delete_marker); - delete_marker.ns = "boxes"; + delete_marker.ns = "track_boxes"; + marker_array.markers.push_back(delete_marker); + delete_marker.ns = "detect_boxes"; marker_array.markers.push_back(delete_marker); delete_marker.ns = "association_lines"; marker_array.markers.push_back(delete_marker); 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 a80f12442f75e..d9da1f78f963b 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -166,6 +166,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) // Debugger debugger_ = std::make_unique(*this, world_frame_id_); + debugger_->setObjectChannels(input_names_short); published_time_publisher_ = std::make_unique(this); } diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp index 74912655ea0d6..7c73ebae131d2 100644 --- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp @@ -43,7 +43,7 @@ Tracker::Tracker( void Tracker::initializeExistenceProbabilities( const uint & channel_index, const float & existence_probability) { - existence_probabilities_[channel_index] = existence_probability; + existence_probabilities_[channel_index] = 0.8 + 0.2 * existence_probability; total_existence_probability_ = existence_probability; } @@ -61,7 +61,13 @@ bool Tracker::updateWithMeasurement( // existence probability on each channel const double delta_time = (measurement_time - last_update_with_measurement_time_).seconds(); const double decay_rate = 5.0 / 10.0; - existence_probabilities_[channel_index] = existence_probability_from_object; + + const float gain = 0.8; + const float probability_detected = 0.8; + // existence_probabilities_[channel_index] = existence_probability_from_object; + existence_probabilities_[channel_index] = + gain * probability_detected + (1 - gain) * existence_probabilities_[channel_index]; + for (size_t i = 0; i < existence_probabilities_.size(); ++i) { if (i == channel_index) { continue; From 8d4408b817f28c2236211723780889f7909d4283 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 19 Apr 2024 17:39:59 +0900 Subject: [PATCH 31/67] feat: association lines are color-coded Signed-off-by: Taekjin LEE --- .../src/debugger/debug_object.cpp | 115 +++++++++++------- 1 file changed, 73 insertions(+), 42 deletions(-) diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp index a74885376c3c5..8593afd77a30d 100644 --- a/perception/multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -229,24 +229,41 @@ void TrackerObjectDebugger::draw( marker_track_boxes.scale.y = 0.4; marker_track_boxes.scale.z = 0.4; - visualization_msgs::msg::Marker marker_detect_boxes; - marker_detect_boxes = marker; - marker_detect_boxes.ns = "detect_boxes"; - marker_detect_boxes.type = visualization_msgs::msg::Marker::CUBE_LIST; - marker_detect_boxes.action = visualization_msgs::msg::Marker::ADD; - marker_detect_boxes.scale.x = 0.2; - marker_detect_boxes.scale.y = 0.2; - marker_detect_boxes.scale.z = 0.2; - - visualization_msgs::msg::Marker marker_lines; - marker_lines = marker; - marker_lines.ns = "association_lines"; - marker_lines.type = visualization_msgs::msg::Marker::LINE_LIST; - marker_lines.action = visualization_msgs::msg::Marker::ADD; - marker_lines.scale.x = 0.15; - marker_lines.points.clear(); - - bool is_line_drawn = false; + // make detected object markers per channel + std::vector marker_detect_boxes_per_channel; + std::vector marker_detect_lines_per_channel; + + for (size_t idx = 0; idx < channel_names_.size(); idx++) { + // get color - by channel index + std_msgs::msg::ColorRGBA color; + color.a = 1.0; + color.r = color_array[idx % PALETTE_SIZE][0]; + color.g = color_array[idx % PALETTE_SIZE][1]; + color.b = color_array[idx % PALETTE_SIZE][2]; + + visualization_msgs::msg::Marker marker_detect_boxes; + marker_detect_boxes = marker; + marker_detect_boxes.ns = "detect_boxes_" + channel_names_[idx]; + marker_detect_boxes.type = visualization_msgs::msg::Marker::CUBE_LIST; + marker_detect_boxes.action = visualization_msgs::msg::Marker::ADD; + marker_detect_boxes.scale.x = 0.2; + marker_detect_boxes.scale.y = 0.2; + marker_detect_boxes.scale.z = 0.2; + marker_detect_boxes.color = color; + marker_detect_boxes_per_channel.push_back(marker_detect_boxes); + + visualization_msgs::msg::Marker marker_lines; + marker_lines = marker; + marker_lines.ns = "association_lines_" + channel_names_[idx]; + marker_lines.type = visualization_msgs::msg::Marker::LINE_LIST; + marker_lines.action = visualization_msgs::msg::Marker::ADD; + marker_lines.scale.x = 0.15; + marker_lines.points.clear(); + marker_lines.color = color; + marker_detect_lines_per_channel.push_back(marker_lines); + } + + bool is_any_associated = false; for (const auto & object_data : object_data_group) { int channel_id = object_data.channel_id; @@ -260,39 +277,51 @@ void TrackerObjectDebugger::draw( // set association marker, if exists if (!object_data.is_associated) continue; - is_line_drawn = true; - // get color - by channel index - std_msgs::msg::ColorRGBA color; - color.a = 1.0; - color.r = color_array[channel_id % PALETTE_SIZE][0]; - color.g = color_array[channel_id % PALETTE_SIZE][1]; - color.b = color_array[channel_id % PALETTE_SIZE][2]; + is_any_associated = true; + + // associated object box + visualization_msgs::msg::Marker & marker_detect_boxes = + marker_detect_boxes_per_channel.at(channel_id); + box_point.x = object_data.detection_point.x; + box_point.y = object_data.detection_point.y; + box_point.z = object_data.detection_point.z + height_offset + 1; + marker_detect_boxes.points.push_back(box_point); // association line + visualization_msgs::msg::Marker & marker_lines = + marker_detect_lines_per_channel.at(channel_id); geometry_msgs::msg::Point line_point; - marker_lines.color = color; - line_point.x = object_data.tracker_point.x; line_point.y = object_data.tracker_point.y; line_point.z = object_data.tracker_point.z + height_offset; marker_lines.points.push_back(line_point); line_point.x = object_data.detection_point.x; line_point.y = object_data.detection_point.y; - line_point.z = object_data.tracker_point.z + height_offset + 1; + line_point.z = object_data.detection_point.z + height_offset + 1; marker_lines.points.push_back(line_point); - - // associated object box - box_point.x = object_data.detection_point.x; - box_point.y = object_data.detection_point.y; - box_point.z = object_data.detection_point.z + height_offset + 1; - marker_detect_boxes.color = color; - marker_detect_boxes.points.push_back(box_point); } // add markers marker_array.markers.push_back(marker_track_boxes); - marker_array.markers.push_back(marker_detect_boxes); - if (is_line_drawn) marker_array.markers.push_back(marker_lines); + if (is_any_associated) { + for (size_t i = 0; i < channel_names_.size(); i++) { + if (marker_detect_boxes_per_channel.at(i).points.empty()) continue; + marker_array.markers.push_back(marker_detect_boxes_per_channel.at(i)); + } + for (size_t i = 0; i < channel_names_.size(); i++) { + if (marker_detect_lines_per_channel.at(i).points.empty()) continue; + marker_array.markers.push_back(marker_detect_lines_per_channel.at(i)); + } + } else { + for (size_t i = 0; i < channel_names_.size(); i++) { + marker_detect_boxes_per_channel.at(i).action = visualization_msgs::msg::Marker::DELETE; + marker_array.markers.push_back(marker_detect_boxes_per_channel.at(i)); + } + for (size_t i = 0; i < channel_names_.size(); i++) { + marker_detect_lines_per_channel.at(i).action = visualization_msgs::msg::Marker::DELETE; + marker_array.markers.push_back(marker_detect_lines_per_channel.at(i)); + } + } } return; @@ -317,15 +346,17 @@ void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & ma delete_marker.ns = "existence_probability"; delete_marker.id = previous_id; delete_marker.action = visualization_msgs::msg::Marker::DELETE; - marker_array.markers.push_back(delete_marker); delete_marker.ns = "track_boxes"; marker_array.markers.push_back(delete_marker); - delete_marker.ns = "detect_boxes"; - marker_array.markers.push_back(delete_marker); - delete_marker.ns = "association_lines"; - marker_array.markers.push_back(delete_marker); + + for (size_t idx = 0; idx < channel_names_.size(); idx++) { + delete_marker.ns = "detect_boxes_" + channel_names_[idx]; + marker_array.markers.push_back(delete_marker); + delete_marker.ns = "association_lines_" + channel_names_[idx]; + marker_array.markers.push_back(delete_marker); + } } return; From 30aed23da45e900041931da01879e36eb3fd0cad Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 22 Apr 2024 12:04:10 +0900 Subject: [PATCH 32/67] fix: association debug marker bugfix Signed-off-by: Taekjin LEE --- .../src/multi_object_tracker_core.cpp | 11 ++++++----- .../src/processor/input_manager.cpp | 10 ---------- 2 files changed, 6 insertions(+), 15 deletions(-) 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 d9da1f78f963b..a6c54e3a2c3c2 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -291,6 +291,12 @@ void MultiObjectTracker::runProcess( data_association_->assign(score_matrix, direct_assignment, reverse_assignment); } + // Collect debug information - tracker list, existence probabilities, association result + // TODO(technolojin): add option to enable/disable debug information + debugger_->collectObjectInfo( + measurement_time, processor_->getListTracker(), channel_index, transformed_objects, + direct_assignment, reverse_assignment); + /* tracker update */ processor_->update(transformed_objects, *self_transform, direct_assignment, channel_index); @@ -300,11 +306,6 @@ void MultiObjectTracker::runProcess( /* spawn new tracker */ processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index); - // Collect debug information - tracker list, existence probabilities, association result - // TODO(technolojin): add option to enable/disable debug information - debugger_->collectObjectInfo( - measurement_time, processor_->getListTracker(), channel_index, transformed_objects, - direct_assignment, reverse_assignment); } void MultiObjectTracker::checkAndPublish(const rclcpp::Time & time) diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index 5120f6632ead4..5ebbacad90ba3 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -58,19 +58,12 @@ bool InputStream::getTimestamps( void InputStream::onMessage( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg) { - // // debug message - // RCLCPP_INFO( - // node_.get_logger(), "InputStream::onMessage Received %s message from %s at %d.%d", - // long_name_.c_str(), input_topic_.c_str(), msg->header.stamp.sec, msg->header.stamp.nanosec); - const DetectedObjects objects = *msg; objects_que_.push_back(objects); if (objects_que_.size() > que_size_) { objects_que_.pop_front(); } - // RCLCPP_INFO(node_.get_logger(), "InputStream::onMessage Que size: %zu", objects_que_.size()); - // Filter parameters constexpr double gain = 0.05; const auto now = node_.now(); @@ -125,9 +118,6 @@ void InputStream::getObjectsOlderThan( objects_que_.pop_front(); } } - // RCLCPP_INFO( - // node_.get_logger(), "InputStream::getObjectsOlderThan %s gives %zu objects", - // long_name_.c_str(), objects.size()); } InputManager::InputManager(rclcpp::Node & node) : node_(node) From 78014b99376075b39ab540827728d87ea0829a04 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 22 Apr 2024 03:06:06 +0000 Subject: [PATCH 33/67] style(pre-commit): autofix Signed-off-by: Taekjin LEE --- .../multi_object_tracker/src/multi_object_tracker_core.cpp | 1 - 1 file changed, 1 deletion(-) 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 a6c54e3a2c3c2..ebc06388d0ad7 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -305,7 +305,6 @@ void MultiObjectTracker::runProcess( /* spawn new tracker */ processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index); - } void MultiObjectTracker::checkAndPublish(const rclcpp::Time & time) From d036f32e459bb231f2b9cd4084feeceecf8d997f Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 22 Apr 2024 18:04:20 +0900 Subject: [PATCH 34/67] feat: add option for debug marker Signed-off-by: Taekjin LEE --- .../multi_object_tracker_node.param.yaml | 3 +- .../debugger/debugger.hpp | 1 + .../src/debugger/debug_object.cpp | 9 ++-- .../src/debugger/debugger.cpp | 12 +++-- .../src/multi_object_tracker_core.cpp | 50 ++++++------------- 5 files changed, 33 insertions(+), 42 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 945cf44113a06..7603d82a75a12 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,9 +17,10 @@ # debug parameters publish_processing_time: false publish_tentative_objects: false + publish_debug_markers: false diagnostics_warn_delay: 0.5 # [sec] diagnostics_error_delay: 1.0 # [sec] input_topic_names: ["/perception/object_recognition/detection/objects"] - input_names_long: ["merged_objects"] + input_names_long: ["detected_objects"] input_names_short: ["all"] diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp index 32ca7cc9effa7..8edfc102b56db 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp @@ -47,6 +47,7 @@ class TrackerDebugger { bool publish_processing_time; bool publish_tentative_objects; + bool publish_debug_markers; double diagnostics_warn_delay; double diagnostics_error_delay; } debug_settings_; diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp index 8593afd77a30d..3f8cb3badc9b1 100644 --- a/perception/multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -197,19 +197,20 @@ void TrackerObjectDebugger::draw( text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; text_marker.action = visualization_msgs::msg::Marker::ADD; text_marker.pose.position.z += 1.8; - text_marker.scale.z = 0.7; + text_marker.scale.z = 0.5; text_marker.pose.position.x = object_data_front.tracker_point.x; text_marker.pose.position.y = object_data_front.tracker_point.y; - text_marker.pose.position.z = object_data_front.tracker_point.z + 1.0; + text_marker.pose.position.z = object_data_front.tracker_point.z + 2.0; // show the last existence probability // print existence probability with channel name // probability to text, two digits of percentage - std::string existence_probability_text = "P:"; + std::string existence_probability_text = ""; + existence_probability_text += std::to_string(uuid_int); for (size_t i = 0; i < object_data_front.existence_vector.size(); ++i) { std::stringstream stream; stream << std::fixed << std::setprecision(0) << object_data_front.existence_vector[i] * 100; - existence_probability_text += " " + channel_names_[i] + ":" + stream.str(); + existence_probability_text += ":" + channel_names_[i] + stream.str(); } text_marker.text = existence_probability_text; diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp index 04b5baedbb369..0334eca58de3a 100644 --- a/perception/multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger/debugger.cpp @@ -33,8 +33,10 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node, const std::string & frame_ "debug/tentative_objects", rclcpp::QoS{1}); } - debug_objects_markers_pub_ = node_.create_publisher( - "multi_object_tracker/debug/objects_markers", rclcpp::QoS{1}); + if (debug_settings_.publish_debug_markers) { + debug_objects_markers_pub_ = node_.create_publisher( + "multi_object_tracker/debug/objects_markers", rclcpp::QoS{1}); + } // initialize timestamps const rclcpp::Time now = node_.now(); @@ -55,6 +57,7 @@ void TrackerDebugger::loadParameters() node_.declare_parameter("publish_processing_time"); debug_settings_.publish_tentative_objects = node_.declare_parameter("publish_tentative_objects"); + debug_settings_.publish_debug_markers = node_.declare_parameter("publish_debug_markers"); debug_settings_.diagnostics_warn_delay = node_.declare_parameter("diagnostics_warn_delay"); debug_settings_.diagnostics_error_delay = @@ -63,6 +66,7 @@ void TrackerDebugger::loadParameters() 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_.publish_debug_markers = false; debug_settings_.diagnostics_warn_delay = 0.5; debug_settings_.diagnostics_error_delay = 1.0; } @@ -181,6 +185,7 @@ void TrackerDebugger::collectObjectInfo( const std::unordered_map & direct_assignment, const std::unordered_map & reverse_assignment) { + if (!debug_settings_.publish_debug_markers) return; object_debugger_.collect( message_time, list_tracker, channel_index, detected_objects, direct_assignment, reverse_assignment); @@ -189,8 +194,9 @@ void TrackerDebugger::collectObjectInfo( // ObjectDebugger void TrackerDebugger::publishObjectsMarkers() { - visualization_msgs::msg::MarkerArray marker_message; + if (!debug_settings_.publish_debug_markers) return; + visualization_msgs::msg::MarkerArray marker_message; // process data object_debugger_.process(); 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 ebc06388d0ad7..9fbad2bf6bb18 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -92,13 +92,21 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) // ROS interface - Subscribers if (input_topic_names.empty()) { - RCLCPP_ERROR(get_logger(), "Need a 'input_topic_names' parameter to be set before continuing"); + RCLCPP_ERROR(this->get_logger(), "No input topics are specified."); return; } + if ( + input_names_long.size() != input_topic_names.size() || + input_names_short.size() != input_topic_names.size()) { + RCLCPP_ERROR( + this->get_logger(), + "The number of input topic names, long names, and short names must be the same."); + return; + } + input_channel_size_ = input_topic_names.size(); input_channels_.resize(input_channel_size_); - assert(input_names_long.size() == input_channel_size_); - assert(input_names_short.size() == input_channel_size_); + for (size_t i = 0; i < input_channel_size_; i++) { input_channels_[i].index = i; input_channels_[i].input_topic = input_topic_names[i]; @@ -229,31 +237,6 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list) } // process end debugger_->endMeasurementTime(this->now()); - - /* DEBUG */ - const rclcpp::Time latest_time(objects_list.back().second.header.stamp); - RCLCPP_INFO( - this->get_logger(), "MultiObjectTracker::onMessage Objects time range: %f - %f", - (current_time - latest_time).seconds(), (current_time - oldest_time).seconds()); - - // count objects on each channel - std::vector object_counts; - object_counts.resize(input_channel_size_); - // initialize to zero - for (size_t i = 0; i < input_channel_size_; i++) { - object_counts[i] = 0; - } - for (const auto & objects_data : objects_list) { - object_counts[objects_data.first] += 1; - } - // print result - std::string object_counts_str = "MultiObjectTracker::onMessage Object counts: "; - for (size_t i = 0; i < input_channel_size_; i++) { - object_counts_str += - input_channels_[i].long_name + " " + std::to_string(object_counts[i]) + " "; - } - RCLCPP_INFO(this->get_logger(), object_counts_str.c_str()); - /* DEBUG END */ } void MultiObjectTracker::runProcess( @@ -289,13 +272,12 @@ void MultiObjectTracker::runProcess( Eigen::MatrixXd score_matrix = data_association_->calcScoreMatrix( detected_objects, list_tracker); // row : tracker, col : measurement data_association_->assign(score_matrix, direct_assignment, reverse_assignment); - } - // Collect debug information - tracker list, existence probabilities, association result - // TODO(technolojin): add option to enable/disable debug information - debugger_->collectObjectInfo( - measurement_time, processor_->getListTracker(), channel_index, transformed_objects, - direct_assignment, reverse_assignment); + // Collect debug information - tracker list, existence probabilities, association results + debugger_->collectObjectInfo( + measurement_time, processor_->getListTracker(), channel_index, transformed_objects, + direct_assignment, reverse_assignment); + } /* tracker update */ processor_->update(transformed_objects, *self_transform, direct_assignment, channel_index); From 8d6009d0566ad80912c01db8136996e8fe786d2b Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 23 Apr 2024 18:17:02 +0900 Subject: [PATCH 35/67] feat: skip time statistics update in case of outlier Signed-off-by: Taekjin LEE --- .../src/multi_object_tracker_core.cpp | 38 +++++++++++++++++++ .../src/processor/input_manager.cpp | 23 ++++++++--- 2 files changed, 56 insertions(+), 5 deletions(-) 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 9fbad2bf6bb18..f423999e803b5 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -237,6 +237,44 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list) } // process end debugger_->endMeasurementTime(this->now()); + + /* DEBUG */ + const rclcpp::Time latest_time(objects_list.back().second.header.stamp); + RCLCPP_INFO( + this->get_logger(), "MultiObjectTracker::onMessage Objects time range: %f - %f", + (current_time - latest_time).seconds(), (current_time - oldest_time).seconds()); + + // count objects on each channel + std::vector object_counts; + object_counts.resize(input_channel_size_); + // initialize to zero + for (size_t i = 0; i < input_channel_size_; i++) { + object_counts[i] = 0; + } + for (const auto & objects_data : objects_list) { + object_counts[objects_data.first] += 1; + } + // print result + std::string object_counts_str = "MultiObjectTracker::onMessage Object counts: "; + for (size_t i = 0; i < input_channel_size_; i++) { + object_counts_str += + input_channels_[i].long_name + " " + std::to_string(object_counts[i]) + " "; + } + RCLCPP_INFO(this->get_logger(), object_counts_str.c_str()); + + std::string object_info_str = ""; + object_info_str += "MultiObjectTracker::onMessage Objects channel and time: \n"; + for (const auto & objects_data : objects_list) { + const auto & objects = objects_data.second; + const auto & channel_index = objects_data.first; + const auto & time = rclcpp::Time(objects.header.stamp); + // print object information + object_info_str += input_channels_[channel_index].long_name + " " + + std::to_string((current_time - time).seconds()) + " \n"; + } + RCLCPP_INFO(this->get_logger(), object_info_str.c_str()); + + /* DEBUG END */ } void MultiObjectTracker::runProcess( diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index 5ebbacad90ba3..67cbcbb71751a 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -70,19 +70,30 @@ void InputStream::onMessage( // Calculate interval, Update interval statistics if (is_time_initialized_) { + bool is_interval_regular = true; const double interval = (now - latest_message_time_).seconds(); - interval_mean_ = (1.0 - gain) * interval_mean_ + gain * interval; - const double interval_delta = interval - interval_mean_; - interval_var_ = (1.0 - gain) * interval_var_ + gain * interval_delta * interval_delta; + // 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; + } + + if (is_interval_regular) { + interval_mean_ = (1.0 - gain) * interval_mean_ + gain * interval; + const double interval_delta = interval - interval_mean_; + interval_var_ = (1.0 - gain) * interval_var_ + gain * interval_delta * interval_delta; + } } // Update time latest_message_time_ = now; - latest_measurement_time_ = objects.header.stamp; + rclcpp::Time objects_time(objects.header.stamp); + latest_measurement_time_ = + latest_measurement_time_ < objects_time ? objects_time : latest_measurement_time_; if (!is_time_initialized_) is_time_initialized_ = true; // Calculate latency - const double latency = (latest_message_time_ - latest_measurement_time_).seconds(); + const double latency = (latest_message_time_ - objects_time).seconds(); // Update latency statistics latency_mean_ = (1.0 - gain) * latency_mean_ + gain * latency; @@ -184,6 +195,8 @@ void InputManager::getObjectTimeInterval( } double latency_message = (now - latest_message_time).seconds(); double latency_measurement = (now - latest_measurement_time).seconds(); + + // print info RCLCPP_INFO( node_.get_logger(), "InputManager::getObjects %s: latency mean: %f, std: %f, interval mean: " From 90d287ce7080dab63b5c0c873d0927ef680e879a Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 24 Apr 2024 19:27:36 +0900 Subject: [PATCH 36/67] feat: auto-tune latency band Signed-off-by: Taekjin LEE --- .../multi_object_tracker_node.param.yaml | 1 + .../processor/input_manager.hpp | 10 +++- .../src/multi_object_tracker_core.cpp | 7 ++- .../src/processor/input_manager.cpp | 51 ++++++++++++------- 4 files changed, 48 insertions(+), 21 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 7603d82a75a12..7e6f9cdbfbe86 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 @@ -24,3 +24,4 @@ input_topic_names: ["/perception/object_recognition/detection/objects"] input_names_long: ["detected_objects"] input_names_short: ["all"] + input_priority: [0] 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 725503c880915..701a01a6dc0c1 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 @@ -33,12 +33,12 @@ using ObjectsList = std::vector>; struct InputChannel { - size_t index; // index of the input channel 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_latency = 0.2; // [s] + int priority = 0; // priority of the input channel, higher is better }; class InputStream @@ -56,6 +56,8 @@ class InputStream void onMessage(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg); + uint getIndex() const { return index_; } + int getPriority() const { return priority_; } void getObjectsOlderThan( const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, ObjectsList & objects_list); @@ -64,6 +66,7 @@ class InputStream long_name = long_name_; short_name = short_name_; } + std::string getLongName() const { return long_name_; } size_t getObjectsCount() const { return objects_que_.size(); } void getTimeStatistics( double & latency_mean, double & latency_var, double & interval_mean, @@ -85,6 +88,8 @@ class InputStream std::string long_name_; std::string short_name_; + int priority_{0}; + size_t que_size_{30}; std::deque objects_que_; @@ -126,6 +131,9 @@ class InputManager std::vector> input_streams_; std::function func_trigger_; + uint target_stream_idx_{0}; + double target_latency_{0.2}; + double target_latency_band_{0.14}; }; } // 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 f423999e803b5..a387a96244794 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -83,9 +83,11 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) declare_parameter("input_topic_names", std::vector()); declare_parameter("input_names_long", std::vector()); declare_parameter("input_names_short", std::vector()); + declare_parameter("input_priority", std::vector()); std::vector input_topic_names = get_parameter("input_topic_names").as_string_array(); std::vector input_names_long = get_parameter("input_names_long").as_string_array(); std::vector input_names_short = get_parameter("input_names_short").as_string_array(); + std::vector input_priority = get_parameter("input_priority").as_integer_array(); // ROS interface - Publisher tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); @@ -97,7 +99,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) } if ( input_names_long.size() != input_topic_names.size() || - input_names_short.size() != input_topic_names.size()) { + input_names_short.size() != input_topic_names.size() || + input_priority.size() != input_topic_names.size()) { RCLCPP_ERROR( this->get_logger(), "The number of input topic names, long names, and short names must be the same."); @@ -108,10 +111,10 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) input_channels_.resize(input_channel_size_); for (size_t i = 0; i < input_channel_size_; i++) { - input_channels_[i].index = i; 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].priority = static_cast(input_priority[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 67cbcbb71751a..39a6f5d253871 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -30,6 +30,8 @@ void InputStream::init(const InputChannel & input_channel) long_name_ = input_channel.long_name; short_name_ = input_channel.short_name; + priority_ = input_channel.priority; + // Initialize queue objects_que_.clear(); @@ -170,11 +172,8 @@ void InputManager::init(const std::vector & input_channels) void InputManager::onTrigger(const uint & index) const { - // input stream index of 0 is the target(main) input stream - const uint target_idx = 0; - - // when the main stream triggers, call the trigger function - if (index == target_idx && func_trigger_) { + // when the target stream triggers, call the trigger function + if (index == target_stream_idx_ && func_trigger_) { func_trigger_(); } } @@ -182,6 +181,20 @@ void InputManager::onTrigger(const uint & index) const void InputManager::getObjectTimeInterval( const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time) { + object_latest_time = now - rclcpp::Duration::from_seconds(target_latency_); + object_oldest_time = now - rclcpp::Duration::from_seconds(target_latency_ + target_latency_band_); + + // 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_; + + // OPTIMIZATION: Get the stream with the maximum latency + double max_latency_mean = 0.0; + uint stream_selected_idx = 0; + double selected_idx_latency_std = 0.0; + double selected_idx_interval = 0.0; + { // ANALYSIS: Get the streams statistics std::string long_name, short_name; @@ -195,6 +208,12 @@ void InputManager::getObjectTimeInterval( } double latency_message = (now - latest_message_time).seconds(); double latency_measurement = (now - latest_measurement_time).seconds(); + if (latency_mean > max_latency_mean && input_stream->getPriority() > 0) { + max_latency_mean = latency_mean; + selected_idx_latency_std = std::sqrt(latency_var); + stream_selected_idx = input_stream->getIndex(); + selected_idx_interval = interval_mean; + } // print info RCLCPP_INFO( @@ -206,19 +225,15 @@ void InputManager::getObjectTimeInterval( } } - // Get proper latency - constexpr double target_latency = 0.18; // [s], measurement to tracking latency - // process latency of a main detection + margin - constexpr double acceptable_latency = - 0.32; // [s], acceptable band from the target latency, larger than the target latency - - object_latest_time = now - rclcpp::Duration::from_seconds(target_latency); - object_oldest_time = now - rclcpp::Duration::from_seconds(acceptable_latency); - - // 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_; + // 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; + 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_); } bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_list) From ca91b5bfbd18c20436ce9bd5642a793c7d817c23 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 26 Apr 2024 11:05:39 +0900 Subject: [PATCH 37/67] feat: pre-defined channels, select on launcher Signed-off-by: Taekjin LEE --- .../config/input_channels.param.yaml | 38 +++++++++++++++ .../multi_object_tracker_node.param.yaml | 5 -- .../launch/multi_object_tracker.launch.xml | 6 ++- .../src/multi_object_tracker_core.cpp | 46 +++++++++++-------- 4 files changed, 69 insertions(+), 26 deletions(-) create mode 100644 perception/multi_object_tracker/config/input_channels.param.yaml diff --git a/perception/multi_object_tracker/config/input_channels.param.yaml b/perception/multi_object_tracker/config/input_channels.param.yaml new file mode 100644 index 0000000000000..09444dd30ec94 --- /dev/null +++ b/perception/multi_object_tracker/config/input_channels.param.yaml @@ -0,0 +1,38 @@ +/**: + ros__parameters: + input_channels: + detected_objects: + topic: "/perception/object_recognition/detection/objects" + name: "detected_objects" + name_short: "all" + expected_frequency: 10.0 + centerpoint: + topic: "/perception/object_recognition/detection/centerpoint/objects" + name: "centerpoint" + name_short: "Lcp" + expected_frequency: 10.0 + centerpoint_validated: + topic: "/perception/object_recognition/detection/centerpoint/validation/objects" + name: "centerpoint" + name_short: "Lcp" + expected_frequency: 10.0 + camera_lidar_fusion: + topic: "/perception/object_recognition/detection/clustering/camera_lidar_fusion/objects" + name: "camera_lidar_fusion" + name_short: "CLf" + expected_frequency: 10.0 + camera_lidar_fusion_filtered: + topic: "/perception/object_recognition/detection/clustering/camera_lidar_fusion/objects" + name: "camera_lidar_fusion" + name_short: "CLf" + expected_frequency: 10.0 + detection_by_tracker: + topic: "/perception/object_recognition/detection/detection_by_tracker/objects" + name: "detection_by_tracker" + name_short: "Ld" + expected_frequency: 10.0 + radar_far: + topic: "/perception/object_recognition/detection/radar/far_objects" + name: "radar_far" + name_short: "Rf" + expected_frequency: 30.0 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 7e6f9cdbfbe86..09621a40c499f 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 @@ -20,8 +20,3 @@ publish_debug_markers: false diagnostics_warn_delay: 0.5 # [sec] diagnostics_error_delay: 1.0 # [sec] - - input_topic_names: ["/perception/object_recognition/detection/objects"] - input_names_long: ["detected_objects"] - input_names_short: ["all"] - input_priority: [0] diff --git a/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml b/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml index dcd080b851c20..526ce23efd33b 100644 --- a/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml +++ b/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml @@ -1,14 +1,16 @@ - + + - + + 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 a387a96244794..7af2922dfd334 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -80,31 +80,38 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) world_frame_id_ = declare_parameter("world_frame_id"); bool enable_delay_compensation{declare_parameter("enable_delay_compensation")}; - declare_parameter("input_topic_names", std::vector()); - declare_parameter("input_names_long", std::vector()); - declare_parameter("input_names_short", std::vector()); - declare_parameter("input_priority", std::vector()); - std::vector input_topic_names = get_parameter("input_topic_names").as_string_array(); - std::vector input_names_long = get_parameter("input_names_long").as_string_array(); - std::vector input_names_short = get_parameter("input_names_short").as_string_array(); - std::vector input_priority = get_parameter("input_priority").as_integer_array(); + declare_parameter("selected_input_channels", std::vector()); + std::vector selected_input_channels = + get_parameter("selected_input_channels").as_string_array(); // ROS interface - Publisher tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); - // ROS interface - Subscribers - if (input_topic_names.empty()) { + // ROS interface - Input channels + // Get input channels + std::vector input_topic_names; + std::vector input_names_long; + std::vector input_names_short; + std::vector input_expected_rates; + + if (selected_input_channels.empty()) { RCLCPP_ERROR(this->get_logger(), "No input topics are specified."); return; } - if ( - input_names_long.size() != input_topic_names.size() || - input_names_short.size() != input_topic_names.size() || - input_priority.size() != input_topic_names.size()) { - RCLCPP_ERROR( - this->get_logger(), - "The number of input topic names, long names, and short names must be the same."); - return; + + for (const auto & selected_input_channel : selected_input_channels) { + const std::string input_topic_name = + declare_parameter("input_channels." + selected_input_channel + ".topic"); + const std::string input_name_long = + declare_parameter("input_channels." + selected_input_channel + ".name"); + const std::string input_name_short = + declare_parameter("input_channels." + selected_input_channel + ".name_short"); + const double input_expected_frequency = + declare_parameter("input_channels." + selected_input_channel + ".expected_frequency"); + input_topic_names.push_back(input_topic_name); + input_names_long.push_back(input_name_long); + input_names_short.push_back(input_name_short); + input_expected_rates.push_back(input_expected_frequency); } input_channel_size_ = input_topic_names.size(); @@ -114,7 +121,8 @@ 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].priority = static_cast(input_priority[i]); + input_channels_[i].priority = 1; + input_channels_[i].expected_rate = input_expected_rates[i]; } // Initialize input manager From 5c621050fbe8e1a047101a9288b7a236a44a60c3 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 26 Apr 2024 12:42:27 +0900 Subject: [PATCH 38/67] feat: add input channels Signed-off-by: Taekjin LEE --- .../config/input_channels.param.yaml | 45 ++++++++++++++++--- 1 file changed, 38 insertions(+), 7 deletions(-) diff --git a/perception/multi_object_tracker/config/input_channels.param.yaml b/perception/multi_object_tracker/config/input_channels.param.yaml index 09444dd30ec94..e13f2a2522544 100644 --- a/perception/multi_object_tracker/config/input_channels.param.yaml +++ b/perception/multi_object_tracker/config/input_channels.param.yaml @@ -6,31 +6,62 @@ name: "detected_objects" name_short: "all" expected_frequency: 10.0 - centerpoint: + # LIDAR - rule-based + lidar-clustering: + topic: "/perception/object_recognition/detection/clustering/objects" + name: "clustering" + name_short: "Lcl" + expected_frequency: 10.0 + # LIDAR - DNN + lidar-centerpoint: topic: "/perception/object_recognition/detection/centerpoint/objects" name: "centerpoint" name_short: "Lcp" expected_frequency: 10.0 - centerpoint_validated: + lidar-centerpoint_validated: topic: "/perception/object_recognition/detection/centerpoint/validation/objects" name: "centerpoint" name_short: "Lcp" expected_frequency: 10.0 - camera_lidar_fusion: - topic: "/perception/object_recognition/detection/clustering/camera_lidar_fusion/objects" - name: "camera_lidar_fusion" - name_short: "CLf" + lidar-apollo: + topic: "/perception/object_recognition/detection/apollo/objects" + name: "apollo" + name_short: "Lap" expected_frequency: 10.0 - camera_lidar_fusion_filtered: + lidar-apollo_validated: + topic: "/perception/object_recognition/detection/apollo/validation/objects" + name: "apollo" + name_short: "Lap" + expected_frequency: 10.0 + # LIDAR-CAMERA - DNN + pointpainitng: + topic: "/perception/object_recognition/detection/pointpainting/objects" + name: "pointpainting" + name_short: "Lpp" + expected_frequency: 10.0 + pointpainting_validated: + topic: "/perception/object_recognition/detection/pointpainting/validation/objects" + name: "pointpainting" + name_short: "Lpp" + expected_frequency: 10.0 + # CAMERA-LIDAR + camera_lidar_fusion: topic: "/perception/object_recognition/detection/clustering/camera_lidar_fusion/objects" name: "camera_lidar_fusion" name_short: "CLf" expected_frequency: 10.0 + # CAMERA-LIDAR+TRACKER detection_by_tracker: topic: "/perception/object_recognition/detection/detection_by_tracker/objects" name: "detection_by_tracker" name_short: "Ld" expected_frequency: 10.0 + # RADAR + radar: + topic: "/sensing/radar/detected_objects" + name: "radar" + name_short: "R" + expected_frequency: 30.0 radar_far: topic: "/perception/object_recognition/detection/radar/far_objects" name: "radar_far" From 0d4b683640a06006627a8d662fc0da9eb41ce810 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 26 Apr 2024 14:28:52 +0900 Subject: [PATCH 39/67] fix: remove marker idx map Signed-off-by: Taekjin LEE --- .../config/input_channels.param.yaml | 10 ++-- .../debugger/debug_object.hpp | 46 +------------------ .../src/debugger/debug_object.cpp | 34 +++++++------- .../src/tracker/model/tracker_base.cpp | 4 +- 4 files changed, 26 insertions(+), 68 deletions(-) diff --git a/perception/multi_object_tracker/config/input_channels.param.yaml b/perception/multi_object_tracker/config/input_channels.param.yaml index e13f2a2522544..7e0a2a9dd8caa 100644 --- a/perception/multi_object_tracker/config/input_channels.param.yaml +++ b/perception/multi_object_tracker/config/input_channels.param.yaml @@ -7,28 +7,28 @@ name_short: "all" expected_frequency: 10.0 # LIDAR - rule-based - lidar-clustering: + lidar_clustering: topic: "/perception/object_recognition/detection/clustering/objects" name: "clustering" name_short: "Lcl" expected_frequency: 10.0 # LIDAR - DNN - lidar-centerpoint: + lidar_centerpoint: topic: "/perception/object_recognition/detection/centerpoint/objects" name: "centerpoint" name_short: "Lcp" expected_frequency: 10.0 - lidar-centerpoint_validated: + lidar_centerpoint_validated: topic: "/perception/object_recognition/detection/centerpoint/validation/objects" name: "centerpoint" name_short: "Lcp" expected_frequency: 10.0 - lidar-apollo: + lidar_apollo: topic: "/perception/object_recognition/detection/apollo/objects" name: "apollo" name_short: "Lap" expected_frequency: 10.0 - lidar-apollo_validated: + lidar_apollo_validated: topic: "/perception/object_recognition/detection/apollo/validation/objects" name: "apollo" name_short: "Lap" diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp index 44137c04a3580..c312edc843259 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp @@ -44,6 +44,7 @@ struct ObjectData // object uuid boost::uuids::uuid uuid; int uuid_int; + std::string uuid_str; // association link, pair of coordinates // tracker to detection @@ -72,7 +73,6 @@ class TrackerObjectDebugger rclcpp::Time message_time_; std::vector object_data_list_; - std::unordered_map> id_map_; std::list unused_marker_ids_; int32_t marker_id_ = 0; std::vector> object_data_groups_; @@ -116,49 +116,7 @@ class TrackerObjectDebugger return uuid; } - void update_id_map(const std::vector & object_data_list) - { - std::vector new_uuids; - std::vector tracked_uuids; - new_uuids.reserve(object_data_list.size()); - tracked_uuids.reserve(object_data_list.size()); - - // check if the object is already tracked - for (const auto & object_data : object_data_list) { - ((id_map_.find(object_data.uuid) != id_map_.end()) ? tracked_uuids : new_uuids) - .push_back(object_data.uuid); - } - - // remove untracked objects - auto itr = id_map_.begin(); - while (itr != id_map_.end()) { - if ( - std::find(tracked_uuids.begin(), tracked_uuids.end(), itr->first) == tracked_uuids.end()) { - unused_marker_ids_.push_back(itr->second); - itr = id_map_.erase(itr); - } else { - ++itr; - } - } - - // add new objects - for (const auto & new_uuid : new_uuids) { - if (unused_marker_ids_.empty()) { - id_map_.emplace(new_uuid, marker_id_); - marker_id_++; - } else { - id_map_.emplace(new_uuid, unused_marker_ids_.front()); - unused_marker_ids_.pop_front(); - } - } - } - - int32_t uuid_to_marker_id(const unique_identifier_msgs::msg::UUID & uuid_msg) - { - const auto uuid = to_boost_uuid(uuid_msg); - return id_map_.at(uuid); - } - int32_t uuid_to_marker_id(const boost::uuids::uuid & uuid) { return id_map_.at(uuid); } + int32_t uuid_to_int(const boost::uuids::uuid & uuid) { return boost::uuids::hash_value(uuid); } }; #endif // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp index 3f8cb3badc9b1..11ecafba19e53 100644 --- a/perception/multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -68,6 +68,8 @@ void TrackerObjectDebugger::collect( autoware_auto_perception_msgs::msg::TrackedObject tracked_object; (*(tracker_itr))->getTrackedObject(message_time, tracked_object); object_data.uuid = to_boost_uuid(tracked_object.object_id); + object_data.uuid_int = uuid_to_int(object_data.uuid); + object_data.uuid_str = uuid_to_string(tracked_object.object_id); // tracker bool is_associated = false; @@ -107,23 +109,18 @@ void TrackerObjectDebugger::process() { if (!is_initialized_) return; - // check all object data and update marker index - update_id_map(object_data_list_); - // update uuid_int for (auto & object_data : object_data_list_) { - object_data.uuid_int = uuid_to_marker_id(object_data.uuid); current_ids_.insert(object_data.uuid_int); } // sort by uuid, collect the same uuid object_data as a group, and loop for the groups - // std::vector> object_data_groups; object_data_groups_.clear(); { - // sort by uuid_int + // sort by uuid std::sort( object_data_list_.begin(), object_data_list_.end(), - [](const ObjectData & a, const ObjectData & b) { return a.uuid_int < b.uuid_int; }); + [](const ObjectData & a, const ObjectData & b) { return a.uuid < b.uuid; }); // collect the same uuid object_data as a group std::vector object_data_group; @@ -173,14 +170,14 @@ void TrackerObjectDebugger::draw( for (const auto & object_data_group : object_data_groups) { if (object_data_group.empty()) continue; - const int & uuid_int = object_data_group.front().uuid_int; const auto object_data_front = object_data_group.front(); const auto object_data_back = object_data_group.back(); + // set a reference marker visualization_msgs::msg::Marker marker; marker.header.frame_id = frame_id_; marker.header.stamp = object_data_front.time; - marker.id = uuid_int; + marker.id = object_data_front.uuid_int; marker.pose.position.x = 0; marker.pose.position.y = 0; marker.pose.position.z = 0; @@ -200,18 +197,20 @@ void TrackerObjectDebugger::draw( text_marker.scale.z = 0.5; text_marker.pose.position.x = object_data_front.tracker_point.x; text_marker.pose.position.y = object_data_front.tracker_point.y; - text_marker.pose.position.z = object_data_front.tracker_point.z + 2.0; + text_marker.pose.position.z = object_data_front.tracker_point.z + 2.5; // show the last existence probability // print existence probability with channel name // probability to text, two digits of percentage std::string existence_probability_text = ""; - existence_probability_text += std::to_string(uuid_int); for (size_t i = 0; i < object_data_front.existence_vector.size(); ++i) { std::stringstream stream; stream << std::fixed << std::setprecision(0) << object_data_front.existence_vector[i] * 100; - existence_probability_text += ":" + channel_names_[i] + stream.str(); + existence_probability_text += channel_names_[i] + stream.str() + ":"; } + existence_probability_text = + existence_probability_text.substr(0, existence_probability_text.size() - 1); + existence_probability_text += "\n" + object_data_front.uuid_str.substr(0, 6); text_marker.text = existence_probability_text; marker_array.markers.push_back(text_marker); @@ -219,7 +218,8 @@ void TrackerObjectDebugger::draw( // loop for each object_data in the group // boxed to tracker positions // and link lines to the detected positions - const double height_offset = 1.0; + const double marker_height_offset = 1.0; + const double assign_height_offset = 0.6; visualization_msgs::msg::Marker marker_track_boxes; marker_track_boxes = marker; @@ -273,7 +273,7 @@ void TrackerObjectDebugger::draw( geometry_msgs::msg::Point box_point; box_point.x = object_data.tracker_point.x; box_point.y = object_data.tracker_point.y; - box_point.z = object_data.tracker_point.z + height_offset; + box_point.z = object_data.tracker_point.z + marker_height_offset; marker_track_boxes.points.push_back(box_point); // set association marker, if exists @@ -285,7 +285,7 @@ void TrackerObjectDebugger::draw( marker_detect_boxes_per_channel.at(channel_id); box_point.x = object_data.detection_point.x; box_point.y = object_data.detection_point.y; - box_point.z = object_data.detection_point.z + height_offset + 1; + box_point.z = object_data.detection_point.z + marker_height_offset + assign_height_offset; marker_detect_boxes.points.push_back(box_point); // association line @@ -294,11 +294,11 @@ void TrackerObjectDebugger::draw( geometry_msgs::msg::Point line_point; line_point.x = object_data.tracker_point.x; line_point.y = object_data.tracker_point.y; - line_point.z = object_data.tracker_point.z + height_offset; + line_point.z = object_data.tracker_point.z + marker_height_offset; marker_lines.points.push_back(line_point); line_point.x = object_data.detection_point.x; line_point.y = object_data.detection_point.y; - line_point.z = object_data.detection_point.z + height_offset + 1; + line_point.z = object_data.detection_point.z + marker_height_offset + assign_height_offset; marker_lines.points.push_back(line_point); } diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp index 7c73ebae131d2..b62cf05449603 100644 --- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp @@ -62,8 +62,8 @@ bool Tracker::updateWithMeasurement( const double delta_time = (measurement_time - last_update_with_measurement_time_).seconds(); const double decay_rate = 5.0 / 10.0; - const float gain = 0.8; - const float probability_detected = 0.8; + const float gain = 0.4; + const float probability_detected = 0.99; // existence_probabilities_[channel_index] = existence_probability_from_object; existence_probabilities_[channel_index] = gain * probability_detected + (1 - gain) * existence_probabilities_[channel_index]; From 9fbf5633ed72dc51e72683031517d66e6e58f7e9 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 26 Apr 2024 15:49:23 +0900 Subject: [PATCH 40/67] fix: to do not miss the latest message of the target stream Signed-off-by: Taekjin LEE --- .../multi_object_tracker/config/input_channels.param.yaml | 6 +++--- .../multi_object_tracker/src/processor/input_manager.cpp | 8 ++++++++ 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/perception/multi_object_tracker/config/input_channels.param.yaml b/perception/multi_object_tracker/config/input_channels.param.yaml index 7e0a2a9dd8caa..e48c2dfa6e281 100644 --- a/perception/multi_object_tracker/config/input_channels.param.yaml +++ b/perception/multi_object_tracker/config/input_channels.param.yaml @@ -34,12 +34,12 @@ name_short: "Lap" expected_frequency: 10.0 # LIDAR-CAMERA - DNN - pointpainitng: + lidar_pointpainitng: topic: "/perception/object_recognition/detection/pointpainting/objects" name: "pointpainting" name_short: "Lpp" expected_frequency: 10.0 - pointpainting_validated: + lidar_pointpainting_validated: topic: "/perception/object_recognition/detection/pointpainting/validation/objects" name: "pointpainting" name_short: "Lpp" @@ -54,7 +54,7 @@ detection_by_tracker: topic: "/perception/object_recognition/detection/detection_by_tracker/objects" name: "detection_by_tracker" - name_short: "Ld" + name_short: "dbT" expected_frequency: 10.0 # RADAR radar: diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index 39a6f5d253871..c4d1f1dfcd94f 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -184,6 +184,14 @@ void InputManager::getObjectTimeInterval( object_latest_time = now - rclcpp::Duration::from_seconds(target_latency_); object_oldest_time = now - rclcpp::Duration::from_seconds(target_latency_ + target_latency_band_); + // get the latest timestamp of the target stream + rclcpp::Time latest_message_time, latest_measurement_time; + if (input_streams_.at(target_stream_idx_) + ->getTimestamps(latest_measurement_time, latest_message_time)) { + object_latest_time = + object_latest_time > latest_message_time ? object_latest_time : latest_message_time; + } + // if the object_oldest_time is older than the latest object time, set it to the latest object // time object_oldest_time = From 736411dbacc7254796d253ae8091e459c460a7d8 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 26 Apr 2024 16:58:28 +0900 Subject: [PATCH 41/67] fix: remove priority, separate timing optimization Signed-off-by: Taekjin LEE --- .../processor/input_manager.hpp | 9 ++-- .../src/multi_object_tracker_core.cpp | 1 - .../src/processor/input_manager.cpp | 50 +++++++++++-------- 3 files changed, 33 insertions(+), 27 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 701a01a6dc0c1..2c5b11bd2bcbf 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 @@ -38,7 +38,6 @@ struct InputChannel std::string short_name = "DET"; // abbreviation of the name double expected_rate = 10.0; // [Hz] double expected_latency = 0.2; // [s] - int priority = 0; // priority of the input channel, higher is better }; class InputStream @@ -56,8 +55,8 @@ class InputStream void onMessage(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg); + bool isTimeInitialized() const { return is_time_initialized_; } uint getIndex() const { return index_; } - int getPriority() const { return priority_; } void getObjectsOlderThan( const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, ObjectsList & objects_list); @@ -88,8 +87,6 @@ class InputStream std::string long_name_; std::string short_name_; - int priority_{0}; - size_t que_size_{30}; std::deque objects_que_; @@ -117,7 +114,9 @@ class InputManager void onTrigger(const uint & index) const; void getObjectTimeInterval( - const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time); + 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); private: 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 7af2922dfd334..8ba66d92d1e21 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -121,7 +121,6 @@ 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].priority = 1; input_channels_[i].expected_rate = input_expected_rates[i]; } diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index c4d1f1dfcd94f..6102c6f1cc854 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -18,7 +18,9 @@ namespace multi_object_tracker { - +/////////////////////////// +/////// InputStream /////// +/////////////////////////// InputStream::InputStream(rclcpp::Node & node, uint & index) : node_(node), index_(index) { } @@ -30,8 +32,6 @@ void InputStream::init(const InputChannel & input_channel) long_name_ = input_channel.long_name; short_name_ = input_channel.short_name; - priority_ = input_channel.priority; - // Initialize queue objects_que_.clear(); @@ -133,6 +133,9 @@ void InputStream::getObjectsOlderThan( } } +//////////////////////////// +/////// InputManager /////// +//////////////////////////// InputManager::InputManager(rclcpp::Node & node) : node_(node) { latest_object_time_ = node_.now(); @@ -179,12 +182,13 @@ void InputManager::onTrigger(const uint & index) const } void InputManager::getObjectTimeInterval( - const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time) + 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_); - // get the latest timestamp of the target stream + // try to include the latest message of the target stream rclcpp::Time latest_message_time, latest_measurement_time; if (input_streams_.at(target_stream_idx_) ->getTimestamps(latest_measurement_time, latest_message_time)) { @@ -196,8 +200,10 @@ void InputManager::getObjectTimeInterval( // time object_oldest_time = object_oldest_time > latest_object_time_ ? object_oldest_time : latest_object_time_; +} - // OPTIMIZATION: Get the stream with the maximum latency +void InputManager::optimizeTimings() +{ double max_latency_mean = 0.0; uint stream_selected_idx = 0; double selected_idx_latency_std = 0.0; @@ -205,25 +211,24 @@ void InputManager::getObjectTimeInterval( { // ANALYSIS: Get the streams statistics - std::string long_name, short_name; double latency_mean, latency_var, interval_mean, interval_var; - rclcpp::Time latest_measurement_time, latest_message_time; for (const auto & input_stream : input_streams_) { - input_stream->getNames(long_name, short_name); + if (!input_stream->isTimeInitialized()) continue; input_stream->getTimeStatistics(latency_mean, latency_var, interval_mean, interval_var); - if (!input_stream->getTimestamps(latest_measurement_time, latest_message_time)) { - continue; - } - double latency_message = (now - latest_message_time).seconds(); - double latency_measurement = (now - latest_measurement_time).seconds(); - if (latency_mean > max_latency_mean && input_stream->getPriority() > 0) { + 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; } - // print info + /* DEBUG */ + std::string long_name, short_name; + rclcpp::Time latest_measurement_time, latest_message_time; + input_stream->getNames(long_name, short_name); + input_stream->getTimestamps(latest_measurement_time, latest_message_time); + double latency_message = (node_.now() - latest_message_time).seconds(); + double latency_measurement = (node_.now() - latest_measurement_time).seconds(); RCLCPP_INFO( node_.get_logger(), "InputManager::getObjects %s: latency mean: %f, std: %f, interval mean: " @@ -238,6 +243,8 @@ void InputManager::getObjectTimeInterval( 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_, @@ -258,8 +265,13 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li rclcpp::Time object_latest_time, object_oldest_time; getObjectTimeInterval(now, object_latest_time, object_oldest_time); + // Optimize the target stream, latency, and its band + // The result will be used for the next time, so the optimization is after getting the time + // interval + optimizeTimings(); + // Get objects from all input streams - // adds-up to the objects vector for efficient processing + // adds up to the objects vector for efficient processing for (const auto & input_stream : input_streams_) { input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects_list); } @@ -272,10 +284,6 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li 0; }); - RCLCPP_INFO( - node_.get_logger(), "InputManager::getObjects Got %zu objects from input streams", - objects_list.size()); - // Update the latest object time if (!objects_list.empty()) { latest_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp); From 5a0757e59429936115cfc2d9097d496a5bd04ef7 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 26 Apr 2024 18:25:24 +0900 Subject: [PATCH 42/67] fix: time interval bug fix Signed-off-by: Taekjin LEE --- .../multi_object_tracker/processor/input_manager.hpp | 1 + .../multi_object_tracker/src/processor/input_manager.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 4 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 2c5b11bd2bcbf..fa5a750ed35db 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 @@ -78,6 +78,7 @@ class InputStream } bool getTimestamps( rclcpp::Time & latest_measurement_time, rclcpp::Time & latest_message_time) const; + rclcpp::Time getLatestMeasurementTime() const { return latest_measurement_time_; } private: rclcpp::Node & node_; diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index 6102c6f1cc854..47ae1b2dc364b 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -189,11 +189,11 @@ void InputManager::getObjectTimeInterval( object_oldest_time = now - rclcpp::Duration::from_seconds(target_latency_ + target_latency_band_); // try to include the latest message of the target stream - rclcpp::Time latest_message_time, latest_measurement_time; - if (input_streams_.at(target_stream_idx_) - ->getTimestamps(latest_measurement_time, latest_message_time)) { + if (input_streams_.at(target_stream_idx_)->isTimeInitialized()) { + rclcpp::Time latest_measurement_time = + input_streams_.at(target_stream_idx_)->getLatestMeasurementTime(); object_latest_time = - object_latest_time > latest_message_time ? object_latest_time : latest_message_time; + object_latest_time > latest_measurement_time ? object_latest_time : latest_measurement_time; } // if the object_oldest_time is older than the latest object time, set it to the latest object From 5437a9e9d3a2d87cfc977509432f6e8d4b51143d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 26 Apr 2024 18:28:47 +0900 Subject: [PATCH 43/67] chore: refactoring timing state update Signed-off-by: Taekjin LEE --- .../processor/input_manager.hpp | 1 + .../src/processor/input_manager.cpp | 24 +++++++++++-------- 2 files changed, 15 insertions(+), 10 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 fa5a750ed35db..cd9181e12d80a 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 @@ -54,6 +54,7 @@ class InputStream } void onMessage(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg); + void updateTimingStatus(const rclcpp::Time & now, const rclcpp::Time & objects_time); bool isTimeInitialized() const { return is_time_initialized_; } uint getIndex() const { return index_; } diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index 47ae1b2dc364b..25e8f46f19033 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -66,9 +66,21 @@ void InputStream::onMessage( objects_que_.pop_front(); } + // update the timing statistics + rclcpp::Time now = node_.now(); + rclcpp::Time objects_time(objects.header.stamp); + updateTimingStatus(now, objects_time); + + // trigger the function if it is set + if (func_trigger_) { + func_trigger_(index_); + } +} + +void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Time & objects_time) +{ // Filter parameters constexpr double gain = 0.05; - const auto now = node_.now(); // Calculate interval, Update interval statistics if (is_time_initialized_) { @@ -89,23 +101,15 @@ void InputStream::onMessage( // Update time latest_message_time_ = now; - rclcpp::Time objects_time(objects.header.stamp); latest_measurement_time_ = latest_measurement_time_ < objects_time ? objects_time : latest_measurement_time_; if (!is_time_initialized_) is_time_initialized_ = true; - // Calculate latency - const double latency = (latest_message_time_ - objects_time).seconds(); - // Update latency statistics + const double latency = (latest_message_time_ - objects_time).seconds(); latency_mean_ = (1.0 - gain) * latency_mean_ + gain * latency; const double latency_delta = latency - latency_mean_; latency_var_ = (1.0 - gain) * latency_var_ + gain * latency_delta * latency_delta; - - // trigger the function if it is set - if (func_trigger_) { - func_trigger_(index_); - } } void InputStream::getObjectsOlderThan( From 6d317e5907c65bc473216b1bc01d5a73a617a738 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 7 May 2024 14:36:18 +0900 Subject: [PATCH 44/67] fix: set parameters optionally Signed-off-by: Taekjin LEE --- .../config/input_channels.param.yaml | 60 +++++++++++-------- .../src/multi_object_tracker_core.cpp | 27 ++++++--- 2 files changed, 54 insertions(+), 33 deletions(-) diff --git a/perception/multi_object_tracker/config/input_channels.param.yaml b/perception/multi_object_tracker/config/input_channels.param.yaml index e48c2dfa6e281..23c567d525508 100644 --- a/perception/multi_object_tracker/config/input_channels.param.yaml +++ b/perception/multi_object_tracker/config/input_channels.param.yaml @@ -3,67 +3,79 @@ input_channels: detected_objects: topic: "/perception/object_recognition/detection/objects" - name: "detected_objects" - name_short: "all" expected_frequency: 10.0 + optional: + name: "detected_objects" + short_name: "all" # LIDAR - rule-based lidar_clustering: topic: "/perception/object_recognition/detection/clustering/objects" - name: "clustering" - name_short: "Lcl" expected_frequency: 10.0 + optional: + name: "clustering" + short_name: "Lcl" # LIDAR - DNN lidar_centerpoint: topic: "/perception/object_recognition/detection/centerpoint/objects" - name: "centerpoint" - name_short: "Lcp" expected_frequency: 10.0 + optional: + name: "centerpoint" + short_name: "Lcp" lidar_centerpoint_validated: topic: "/perception/object_recognition/detection/centerpoint/validation/objects" - name: "centerpoint" - name_short: "Lcp" expected_frequency: 10.0 + optional: + name: "centerpoint" + short_name: "Lcp" lidar_apollo: topic: "/perception/object_recognition/detection/apollo/objects" - name: "apollo" - name_short: "Lap" expected_frequency: 10.0 + optional: + name: "apollo" + short_name: "Lap" lidar_apollo_validated: topic: "/perception/object_recognition/detection/apollo/validation/objects" - name: "apollo" - name_short: "Lap" expected_frequency: 10.0 + optional: + name: "apollo" + short_name: "Lap" # LIDAR-CAMERA - DNN lidar_pointpainitng: topic: "/perception/object_recognition/detection/pointpainting/objects" - name: "pointpainting" - name_short: "Lpp" expected_frequency: 10.0 + optional: + name: "pointpainting" + short_name: "Lpp" lidar_pointpainting_validated: topic: "/perception/object_recognition/detection/pointpainting/validation/objects" - name: "pointpainting" - name_short: "Lpp" expected_frequency: 10.0 + optional: + name: "pointpainting" + short_name: "Lpp" # CAMERA-LIDAR camera_lidar_fusion: topic: "/perception/object_recognition/detection/clustering/camera_lidar_fusion/objects" - name: "camera_lidar_fusion" - name_short: "CLf" expected_frequency: 10.0 + optional: + name: "camera_lidar_fusion" + short_name: "CLf" # CAMERA-LIDAR+TRACKER detection_by_tracker: topic: "/perception/object_recognition/detection/detection_by_tracker/objects" - name: "detection_by_tracker" - name_short: "dbT" expected_frequency: 10.0 + optional: + name: "detection_by_tracker" + short_name: "dbT" # RADAR radar: topic: "/sensing/radar/detected_objects" - name: "radar" - name_short: "R" expected_frequency: 30.0 + optional: + name: "radar" + short_name: "R" radar_far: topic: "/perception/object_recognition/detection/radar/far_objects" - name: "radar_far" - name_short: "Rf" expected_frequency: 30.0 + optional: + name: "radar_far" + short_name: "Rf" 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 8ba66d92d1e21..1de771b354745 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -100,18 +100,27 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) } for (const auto & selected_input_channel : selected_input_channels) { + // required parameters, no default value const std::string input_topic_name = declare_parameter("input_channels." + selected_input_channel + ".topic"); - const std::string input_name_long = - declare_parameter("input_channels." + selected_input_channel + ".name"); - const std::string input_name_short = - declare_parameter("input_channels." + selected_input_channel + ".name_short"); - const double input_expected_frequency = - declare_parameter("input_channels." + selected_input_channel + ".expected_frequency"); input_topic_names.push_back(input_topic_name); - input_names_long.push_back(input_name_long); - input_names_short.push_back(input_name_short); - input_expected_rates.push_back(input_expected_frequency); + + // 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); + + // optional parameters + const std::string default_name = selected_input_channel; + const std::string name_long = declare_parameter( + "input_channels." + selected_input_channel + ".optional.name", default_name); + input_names_long.push_back(name_long); + + const std::string default_name_short = selected_input_channel.substr(0, 3); + const std::string name_short = declare_parameter( + "input_channels." + selected_input_channel + ".optional.short_name", default_name_short); + input_names_short.push_back(name_short); } input_channel_size_ = input_topic_names.size(); From adc84154f4cf0c8f08b3d26a5ddbeb8348437015 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 7 May 2024 19:13:34 +0900 Subject: [PATCH 45/67] feat: revise object time range logic Signed-off-by: Taekjin LEE --- .../config/input_channels.param.yaml | 24 +++---- .../processor/input_manager.hpp | 17 +++-- .../src/multi_object_tracker_core.cpp | 12 ++-- .../src/processor/input_manager.cpp | 70 +++++++++++-------- 4 files changed, 73 insertions(+), 50 deletions(-) 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) From ca5c4ac7c205bfedfb5c3778c0674502fa08d0b5 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 8 May 2024 13:02:43 +0900 Subject: [PATCH 46/67] fix: launcher to set input channels Signed-off-by: Taekjin LEE --- .../launch/object_recognition/tracking/tracking.launch.xml | 2 ++ launch/tier4_perception_launch/launch/perception.launch.xml | 1 + 2 files changed, 3 insertions(+) diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index fed268084792a..cf11c65ac805c 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -17,6 +17,7 @@ + @@ -26,6 +27,7 @@ + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 71fb38f883144..9b9cd6b7f077f 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -22,6 +22,7 @@ + From b2ca6a1607661ef11854e8cf7c987c7d88988e7a Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 10 May 2024 11:08:11 +0900 Subject: [PATCH 47/67] fix: exempt spell check 'pointpainting' Signed-off-by: Taekjin LEE --- perception/multi_object_tracker/config/input_channels.param.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/perception/multi_object_tracker/config/input_channels.param.yaml b/perception/multi_object_tracker/config/input_channels.param.yaml index 63de901ef0c11..9d9b3ae77367f 100644 --- a/perception/multi_object_tracker/config/input_channels.param.yaml +++ b/perception/multi_object_tracker/config/input_channels.param.yaml @@ -40,6 +40,7 @@ name: "apollo" short_name: "Lap" # LIDAR-CAMERA - DNN + # cspell:ignore lidar_pointpainitng pointpainting lidar_pointpainitng: topic: "/perception/object_recognition/detection/pointpainting/objects" expected_interval: 0.1 From df0cd9fd9921d41e4b6fe9433f4624ecafbb7fc1 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 13 May 2024 13:54:43 +0900 Subject: [PATCH 48/67] feat: remove expected interval Signed-off-by: Taekjin LEE --- .../config/input_channels.param.yaml | 23 +++---- .../processor/input_manager.hpp | 8 +-- .../src/multi_object_tracker_core.cpp | 11 ++- .../src/processor/input_manager.cpp | 69 ++++++++++--------- 4 files changed, 56 insertions(+), 55 deletions(-) diff --git a/perception/multi_object_tracker/config/input_channels.param.yaml b/perception/multi_object_tracker/config/input_channels.param.yaml index 9d9b3ae77367f..20d717ee36e1f 100644 --- a/perception/multi_object_tracker/config/input_channels.param.yaml +++ b/perception/multi_object_tracker/config/input_channels.param.yaml @@ -3,39 +3,39 @@ input_channels: detected_objects: topic: "/perception/object_recognition/detection/objects" - expected_interval: 0.1 + can_spawn_new_tracker: true optional: name: "detected_objects" short_name: "all" # LIDAR - rule-based lidar_clustering: topic: "/perception/object_recognition/detection/clustering/objects" - expected_interval: 0.1 + can_spawn_new_tracker: true optional: name: "clustering" short_name: "Lcl" # LIDAR - DNN lidar_centerpoint: topic: "/perception/object_recognition/detection/centerpoint/objects" - expected_interval: 0.1 + can_spawn_new_tracker: true optional: name: "centerpoint" short_name: "Lcp" lidar_centerpoint_validated: topic: "/perception/object_recognition/detection/centerpoint/validation/objects" - expected_interval: 0.1 + can_spawn_new_tracker: true optional: name: "centerpoint" short_name: "Lcp" lidar_apollo: topic: "/perception/object_recognition/detection/apollo/objects" - expected_interval: 0.1 + can_spawn_new_tracker: true optional: name: "apollo" short_name: "Lap" lidar_apollo_validated: topic: "/perception/object_recognition/detection/apollo/validation/objects" - expected_interval: 0.1 + can_spawn_new_tracker: true optional: name: "apollo" short_name: "Lap" @@ -43,40 +43,39 @@ # cspell:ignore lidar_pointpainitng pointpainting lidar_pointpainitng: topic: "/perception/object_recognition/detection/pointpainting/objects" - expected_interval: 0.1 + can_spawn_new_tracker: true optional: name: "pointpainting" short_name: "Lpp" lidar_pointpainting_validated: topic: "/perception/object_recognition/detection/pointpainting/validation/objects" - 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_interval: 0.1 + can_spawn_new_tracker: false optional: name: "camera_lidar_fusion" short_name: "CLf" # CAMERA-LIDAR+TRACKER detection_by_tracker: topic: "/perception/object_recognition/detection/detection_by_tracker/objects" - expected_interval: 0.1 + can_spawn_new_tracker: false optional: name: "detection_by_tracker" short_name: "dbT" # RADAR radar: topic: "/sensing/radar/detected_objects" - expected_interval: 0.0333 + can_spawn_new_tracker: true optional: name: "radar" short_name: "R" radar_far: topic: "/perception/object_recognition/detection/radar/far_objects" - expected_interval: 0.0333 + can_spawn_new_tracker: true 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 ce2b2075c99a2..03465cb56adae 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,8 +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_interval = 0.1; // [s] - double expected_latency = 0.2; // [s] + bool is_spawn_enabled = true; // enable spawn of the object }; class InputStream @@ -56,7 +55,7 @@ class InputStream void onMessage(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg); void updateTimingStatus(const rclcpp::Time & now, const rclcpp::Time & objects_time); - bool isTimeInitialized() const { return is_time_initialized_; } + bool isTimeInitialized() const { return initial_count_ > 0; } uint getIndex() const { return index_; } void getObjectsOlderThan( const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, @@ -99,7 +98,8 @@ class InputStream std::function func_trigger_; - bool is_time_initialized_{false}; + // bool is_time_initialized_{false}; + int initial_count_{0}; double expected_interval_; double latency_mean_; double latency_var_; 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 8d8a10266fec7..85d8911795c72 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_intervals; + std::vector input_is_spawn_enabled; if (selected_input_channels.empty()) { RCLCPP_ERROR(this->get_logger(), "No input topics are specified."); @@ -106,10 +106,9 @@ 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_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); + const bool spawn_enabled = declare_parameter( + "input_channels." + selected_input_channel + ".can_spawn_new_tracker", true); + input_is_spawn_enabled.push_back(spawn_enabled); // optional parameters const std::string default_name = selected_input_channel; @@ -130,7 +129,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_interval = input_expected_intervals[i]; + input_channels_[i].is_spawn_enabled = input_is_spawn_enabled[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 bf066e23828f8..6de48acdc1a84 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -31,15 +31,14 @@ 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 - latency_mean_ = input_channel.expected_latency; // [s] + latency_mean_ = 0.2; // [s] (initial value) latency_var_ = 0.0; - interval_mean_ = expected_interval_; // [s] (initial value) + interval_mean_ = 0.0; // [s] (initial value) interval_var_ = 0.0; latest_measurement_time_ = node_.now(); @@ -49,7 +48,7 @@ void InputStream::init(const InputChannel & input_channel) bool InputStream::getTimestamps( rclcpp::Time & latest_measurement_time, rclcpp::Time & latest_message_time) const { - if (!is_time_initialized_) { + if (!isTimeInitialized()) { return false; } latest_measurement_time = latest_measurement_time_; @@ -79,21 +78,38 @@ void InputStream::onMessage( void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Time & objects_time) { - // Filter parameters - constexpr double gain = 0.05; + // Update latency statistics + // skip initial messages for the latency statistics + if (initial_count_ > 4) { + const double latency = (now - objects_time).seconds(); + if (initial_count_ < 16) { + // set higher gain for the initial messages + constexpr double initial_gain = 0.5; + latency_mean_ = (1.0 - initial_gain) * latency_mean_ + initial_gain * latency; + } else { + constexpr double gain = 0.05; + latency_mean_ = (1.0 - gain) * latency_mean_ + gain * latency; + const double latency_delta = latency - latency_mean_; + latency_var_ = (1.0 - gain) * latency_var_ + gain * latency_delta * latency_delta; + } + } // Calculate interval, Update interval statistics - if (is_time_initialized_) { + if (initial_count_ > 4) { const double interval = (now - latest_message_time_).seconds(); - // 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; - const double interval_delta = interval - interval_mean_; - interval_var_ = (1.0 - gain) * interval_var_ + gain * interval_delta * interval_delta; + if (initial_count_ < 24) { + // Initialization + constexpr double initial_gain = 0.5; + interval_mean_ = (1.0 - initial_gain) * interval_mean_ + initial_gain * interval; + } else { + // The interval is considered regular if it is within 0.5 and 1.5 times the mean interval + bool update_statistics = interval > 0.5 * interval_mean_ && interval < 1.5 * interval_mean_; + if (update_statistics) { + constexpr double gain = 0.05; + interval_mean_ = (1.0 - gain) * interval_mean_ + gain * interval; + const double interval_delta = interval - interval_mean_; + interval_var_ = (1.0 - gain) * interval_var_ + gain * interval_delta * interval_delta; + } } } @@ -101,13 +117,11 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim latest_message_time_ = now; latest_measurement_time_ = latest_measurement_time_ < objects_time ? objects_time : latest_measurement_time_; - if (!is_time_initialized_) is_time_initialized_ = true; - // Update latency statistics - const double latency = (latest_message_time_ - objects_time).seconds(); - latency_mean_ = (1.0 - gain) * latency_mean_ + gain * latency; - const double latency_delta = latency - latency_mean_; - latency_var_ = (1.0 - gain) * latency_var_ + gain * latency_delta * latency_delta; + // Update the initial count, count only first 32 messages + if (initial_count_ < 32) { + initial_count_++; + } } void InputStream::getObjectsOlderThan( @@ -195,17 +209,6 @@ void InputManager::getObjectTimeInterval( 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 From 0c9c2d8b8d78bc7747247b08afbf8408cc1ffd25 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 13 May 2024 14:34:15 +0900 Subject: [PATCH 49/67] feat: implement spawn switch Signed-off-by: Taekjin LEE --- .../multi_object_tracker/processor/input_manager.hpp | 7 +++++++ .../src/multi_object_tracker_core.cpp | 4 +++- .../src/processor/input_manager.cpp | 10 +++++++++- 3 files changed, 19 insertions(+), 2 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 03465cb56adae..d550547ea4278 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 @@ -65,6 +65,8 @@ class InputStream long_name = long_name_; short_name = short_name_; } + bool isSpawnEnabled() const { return is_spawn_enabled_; } + std::string getLongName() const { return long_name_; } size_t getObjectsCount() const { return objects_que_.size(); } void getTimeStatistics( @@ -92,6 +94,7 @@ class InputStream std::string input_topic_; std::string long_name_; std::string short_name_; + bool is_spawn_enabled_; size_t que_size_{30}; std::deque objects_que_; @@ -125,6 +128,10 @@ class InputManager 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(); + } private: rclcpp::Node & node_; 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 85d8911795c72..94512ed0691f4 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -342,7 +342,9 @@ void MultiObjectTracker::runProcess( processor_->prune(measurement_time); /* spawn new tracker */ - processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index); + if (input_manager_->isChannelSpawnEnabled(channel_index)) { + processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index); + } } void MultiObjectTracker::checkAndPublish(const rclcpp::Time & time) diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index 6de48acdc1a84..e3eb9f7c33cd1 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -31,6 +31,7 @@ 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; + is_spawn_enabled_ = input_channel.is_spawn_enabled; // Initialize queue objects_que_.clear(); @@ -166,8 +167,9 @@ void InputManager::init(const std::vector & input_channels) return; } + // Initialize input streams sub_objects_array_.resize(input_size_); - + bool is_any_spawn_enabled = false; for (size_t i = 0; i < input_size_; i++) { uint index(i); InputStream input_stream(node_, index); @@ -175,6 +177,7 @@ void InputManager::init(const std::vector & input_channels) input_stream.setTriggerFunction( std::bind(&InputManager::onTrigger, this, std::placeholders::_1)); input_streams_.push_back(std::make_shared(input_stream)); + is_any_spawn_enabled |= input_streams_.at(i)->isSpawnEnabled(); // Set subscription RCLCPP_INFO( @@ -186,6 +189,11 @@ void InputManager::init(const std::vector & input_channels) input_channels[i].input_topic, rclcpp::QoS{1}, func); } + // Check if any spawn enabled input streams + if (!is_any_spawn_enabled) { + RCLCPP_ERROR(node_.get_logger(), "InputManager::init No spawn enabled input streams"); + return; + } is_initialized_ = true; } From 204595745f9600c0ba6ddcce9a778bd8fccdabce Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 13 May 2024 15:45:34 +0900 Subject: [PATCH 50/67] fix: remove debug messages Signed-off-by: Taekjin LEE --- .../src/multi_object_tracker_core.cpp | 38 ------------------- .../src/processor/input_manager.cpp | 14 ------- 2 files changed, 52 deletions(-) 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 94512ed0691f4..c3bf7023fb068 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -255,44 +255,6 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list) } // process end debugger_->endMeasurementTime(this->now()); - - /* DEBUG */ - const rclcpp::Time latest_time(objects_list.back().second.header.stamp); - RCLCPP_INFO( - this->get_logger(), "MultiObjectTracker::onMessage Objects time range: %f - %f", - (current_time - latest_time).seconds(), (current_time - oldest_time).seconds()); - - // count objects on each channel - std::vector object_counts; - object_counts.resize(input_channel_size_); - // initialize to zero - for (size_t i = 0; i < input_channel_size_; i++) { - object_counts[i] = 0; - } - for (const auto & objects_data : objects_list) { - object_counts[objects_data.first] += 1; - } - // print result - std::string object_counts_str = "MultiObjectTracker::onMessage Object counts: "; - for (size_t i = 0; i < input_channel_size_; i++) { - object_counts_str += - input_channels_[i].long_name + " " + std::to_string(object_counts[i]) + " "; - } - RCLCPP_INFO(this->get_logger(), object_counts_str.c_str()); - - std::string object_info_str = ""; - object_info_str += "MultiObjectTracker::onMessage Objects channel and time: \n"; - for (const auto & objects_data : objects_list) { - const auto & objects = objects_data.second; - const auto & channel_index = objects_data.first; - const auto & time = rclcpp::Time(objects.header.stamp); - // print object information - object_info_str += input_channels_[channel_index].long_name + " " + - std::to_string((current_time - time).seconds()) + " \n"; - } - RCLCPP_INFO(this->get_logger(), object_info_str.c_str()); - - /* DEBUG END */ } void MultiObjectTracker::runProcess( diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index e3eb9f7c33cd1..d6f1619d9dd73 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -253,20 +253,6 @@ void InputManager::optimizeTimings() selected_stream_interval = interval_mean; selected_stream_interval_std = std::sqrt(interval_var); } - - /* DEBUG */ - std::string long_name, short_name; - rclcpp::Time latest_measurement_time, latest_message_time; - input_stream->getNames(long_name, short_name); - input_stream->getTimestamps(latest_measurement_time, latest_message_time); - double latency_message = (node_.now() - latest_message_time).seconds(); - double latency_measurement = (node_.now() - latest_measurement_time).seconds(); - RCLCPP_INFO( - node_.get_logger(), - "InputManager::getObjects %s: latency mean: %f, std: %f, interval mean: " - "%f, std: %f, latest measurement latency: %f, latest message latency: %f", - long_name.c_str(), latency_mean, std::sqrt(latency_var), interval_mean, - std::sqrt(interval_var), latency_measurement, latency_message); } } From eb09e03efaa587bf7dcad4d6a550eda0a332588f Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 13 May 2024 16:39:33 +0900 Subject: [PATCH 51/67] chore: update readme Signed-off-by: Taekjin LEE --- perception/multi_object_tracker/README.md | 43 ++++++++++++----------- 1 file changed, 23 insertions(+), 20 deletions(-) diff --git a/perception/multi_object_tracker/README.md b/perception/multi_object_tracker/README.md index 311fe7b6da716..11302612cd046 100644 --- a/perception/multi_object_tracker/README.md +++ b/perception/multi_object_tracker/README.md @@ -46,27 +46,34 @@ Example: ### Input -| Name | Type | Description | -| --------- | ----------------------------------------------------- | ----------- | -| `~/input` | `autoware_auto_perception_msgs::msg::DetectedObjects` | obstacles | +Multiple inputs are pre-defined in the input channel parameters (described below) and the inputs can be configured + +| Name | Type | Description | +| ------------------------- | -------------------------- | ---------------------- | +| `selected_input_channels` | `std::vector` | array of channel names | + +- default value: `selected_input_channels:="['detected_objects']"`, merged DetectedObject message +- multi-input example: `selected_input_channels:="['lidar_centerpoint','camera_lidar_fusion','detection_by_tracker','radar_far']"` ### Output -| Name | Type | Description | -| ---------- | ---------------------------------------------------- | ------------------ | -| `~/output` | `autoware_auto_perception_msgs::msg::TrackedObjects` | modified obstacles | +| Name | Type | Description | +| ---------- | ---------------------------------------------------- | --------------- | +| `~/output` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracked objects | ## Parameters - +| Name | Type | Description | +| --------------------------------- | ----------------------------------------------------- | ------------------------------------- | +| `` | | the name of channel | +| `.topic` | `autoware_auto_perception_msgs::msg::DetectedObjects` | detected objects | +| `.can_spawn_new_tracker` | `bool` | a switch allow to spawn a new tracker | +| `.optional.name` | `std::string` | channel name for analysis | +| `.optional.short_name` | `std::string` | short name for visualization | ### Core Parameters @@ -80,6 +87,9 @@ Node parameters are defined in [multi_object_tracker.param.yaml](config/multi_ob | `world_frame_id` | double | object kinematics definition frame | | `enable_delay_compensation` | bool | if True, tracker use timers to schedule publishers and use prediction step to extrapolate object state at desired timestamp | | `publish_rate` | double | Timer frequency to output with delay compensation | +| `publish_processing_time` | bool | enable to publish debug message of process time information | +| `publish_tentative_objects` | bool | enable to publish tentative tracked objects, which have lower confidence | +| `publish_debug_markers` | bool | enable to publish debug markers, which indicates association of multi-inputs, existence probability of each detection | #### Association parameters @@ -93,13 +103,6 @@ Node parameters are defined in [multi_object_tracker.param.yaml](config/multi_ob ## Assumptions / Known limits - - See the [model explanations](models.md). ## (Optional) Error detection and handling From dab7cdc88e76278a7a548cc4c2a8dad18dd59942 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 13 May 2024 16:46:27 +0900 Subject: [PATCH 52/67] fix: change tentative object topic Signed-off-by: Taekjin LEE --- perception/multi_object_tracker/src/debugger/debugger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp index 0334eca58de3a..ad4ce4afad498 100644 --- a/perception/multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger/debugger.cpp @@ -30,7 +30,7 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node, const std::string & frame_ if (debug_settings_.publish_tentative_objects) { debug_tentative_objects_pub_ = node_.create_publisher( - "debug/tentative_objects", rclcpp::QoS{1}); + "multi_object_tracker/debug/tentative_objects", rclcpp::QoS{1}); } if (debug_settings_.publish_debug_markers) { From 7a0553e4364444fe88aa47a9a9c08417675245a5 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 13 May 2024 18:17:10 +0900 Subject: [PATCH 53/67] Revert "fix: remove debug messages" This reverts commit 725a49ee6c382f73b54fe50bf9077aca6049e199. Signed-off-by: Taekjin LEE --- .../src/multi_object_tracker_core.cpp | 38 +++++++++++++++++++ .../src/processor/input_manager.cpp | 14 +++++++ 2 files changed, 52 insertions(+) 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 c3bf7023fb068..94512ed0691f4 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -255,6 +255,44 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list) } // process end debugger_->endMeasurementTime(this->now()); + + /* DEBUG */ + const rclcpp::Time latest_time(objects_list.back().second.header.stamp); + RCLCPP_INFO( + this->get_logger(), "MultiObjectTracker::onMessage Objects time range: %f - %f", + (current_time - latest_time).seconds(), (current_time - oldest_time).seconds()); + + // count objects on each channel + std::vector object_counts; + object_counts.resize(input_channel_size_); + // initialize to zero + for (size_t i = 0; i < input_channel_size_; i++) { + object_counts[i] = 0; + } + for (const auto & objects_data : objects_list) { + object_counts[objects_data.first] += 1; + } + // print result + std::string object_counts_str = "MultiObjectTracker::onMessage Object counts: "; + for (size_t i = 0; i < input_channel_size_; i++) { + object_counts_str += + input_channels_[i].long_name + " " + std::to_string(object_counts[i]) + " "; + } + RCLCPP_INFO(this->get_logger(), object_counts_str.c_str()); + + std::string object_info_str = ""; + object_info_str += "MultiObjectTracker::onMessage Objects channel and time: \n"; + for (const auto & objects_data : objects_list) { + const auto & objects = objects_data.second; + const auto & channel_index = objects_data.first; + const auto & time = rclcpp::Time(objects.header.stamp); + // print object information + object_info_str += input_channels_[channel_index].long_name + " " + + std::to_string((current_time - time).seconds()) + " \n"; + } + RCLCPP_INFO(this->get_logger(), object_info_str.c_str()); + + /* DEBUG END */ } void MultiObjectTracker::runProcess( diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index d6f1619d9dd73..e3eb9f7c33cd1 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -253,6 +253,20 @@ void InputManager::optimizeTimings() selected_stream_interval = interval_mean; selected_stream_interval_std = std::sqrt(interval_var); } + + /* DEBUG */ + std::string long_name, short_name; + rclcpp::Time latest_measurement_time, latest_message_time; + input_stream->getNames(long_name, short_name); + input_stream->getTimestamps(latest_measurement_time, latest_message_time); + double latency_message = (node_.now() - latest_message_time).seconds(); + double latency_measurement = (node_.now() - latest_measurement_time).seconds(); + RCLCPP_INFO( + node_.get_logger(), + "InputManager::getObjects %s: latency mean: %f, std: %f, interval mean: " + "%f, std: %f, latest measurement latency: %f, latest message latency: %f", + long_name.c_str(), latency_mean, std::sqrt(latency_var), interval_mean, + std::sqrt(interval_var), latency_measurement, latency_message); } } From 094110f1bc58d652928c4f7e313b232e1ff1e350 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 13 May 2024 18:45:56 +0900 Subject: [PATCH 54/67] fix: reset times when jumps to past Signed-off-by: Taekjin LEE --- .../src/processor/input_manager.cpp | 34 ++++++++++++++++--- 1 file changed, 30 insertions(+), 4 deletions(-) diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index e3eb9f7c33cd1..0bb399a02b0c2 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -116,8 +116,18 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim // Update time latest_message_time_ = now; - latest_measurement_time_ = - latest_measurement_time_ < objects_time ? objects_time : latest_measurement_time_; + if (std::abs((latest_measurement_time_ - objects_time).seconds()) > 3.0) { + // Reset the latest measurement time if the time difference is too large + 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 + latest_measurement_time_ = + latest_measurement_time_ < objects_time ? objects_time : latest_measurement_time_; + } // Update the initial count, count only first 32 messages if (initial_count_ < 32) { @@ -314,11 +324,27 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li }); // Update the latest object time - if (!objects_list.empty()) { + bool is_any_object = !objects_list.empty(); + if (is_any_object) { latest_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp); + } else { + // check time jump + if ((now - latest_object_time_).seconds() < 0.0) { + 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 + } else { + RCLCPP_WARN( + node_.get_logger(), + "InputManager::getObjects No objects in the object list, object time band from %f to %f", + (now - object_oldest_time).seconds(), (now - object_latest_time).seconds()); + } } - return !objects_list.empty(); + return is_any_object; } } // namespace multi_object_tracker From c1a85b8161add9d384b3437559d9ff3289c6e4eb Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 14 May 2024 15:02:56 +0900 Subject: [PATCH 55/67] fix: check if interval is negative Signed-off-by: Taekjin LEE --- .../multi_object_tracker/src/processor/input_manager.cpp | 8 +++++++- 1 file changed, 7 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 0bb399a02b0c2..1e5bee9887063 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -98,7 +98,13 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim // Calculate interval, Update interval statistics if (initial_count_ > 4) { const double interval = (now - latest_message_time_).seconds(); - if (initial_count_ < 24) { + if (interval < 0.0) { + RCLCPP_WARN( + node_.get_logger(), + "InputManager::updateTimingStatus %s: Negative interval detected, now: %f, " + "latest_message_time_: %f", + long_name_.c_str(), now.seconds(), latest_message_time_.seconds()); + } else if (initial_count_ < 24) { // Initialization constexpr double initial_gain = 0.5; interval_mean_ = (1.0 - initial_gain) * interval_mean_ + initial_gain * interval; From c12109a2933cb0c1a5ac00832a38005417e02c9f Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 14 May 2024 15:16:51 +0900 Subject: [PATCH 56/67] fix: missing config, default value Signed-off-by: Taekjin LEE --- .../multi_object_tracker/config/input_channels.param.yaml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/perception/multi_object_tracker/config/input_channels.param.yaml b/perception/multi_object_tracker/config/input_channels.param.yaml index 20d717ee36e1f..b57f37675d4f1 100644 --- a/perception/multi_object_tracker/config/input_channels.param.yaml +++ b/perception/multi_object_tracker/config/input_channels.param.yaml @@ -49,13 +49,14 @@ short_name: "Lpp" lidar_pointpainting_validated: topic: "/perception/object_recognition/detection/pointpainting/validation/objects" + can_spawn_new_tracker: true optional: name: "pointpainting" short_name: "Lpp" # CAMERA-LIDAR camera_lidar_fusion: topic: "/perception/object_recognition/detection/clustering/camera_lidar_fusion/objects" - can_spawn_new_tracker: false + can_spawn_new_tracker: true optional: name: "camera_lidar_fusion" short_name: "CLf" From c589c3a7e424e1202ba0a6f0f39e505ed9bf7274 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 14 May 2024 15:21:01 +0900 Subject: [PATCH 57/67] fix: remove debug messages Signed-off-by: Taekjin LEE --- .../src/multi_object_tracker_core.cpp | 38 ------------------- .../src/processor/input_manager.cpp | 14 ------- 2 files changed, 52 deletions(-) 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 94512ed0691f4..c3bf7023fb068 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -255,44 +255,6 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list) } // process end debugger_->endMeasurementTime(this->now()); - - /* DEBUG */ - const rclcpp::Time latest_time(objects_list.back().second.header.stamp); - RCLCPP_INFO( - this->get_logger(), "MultiObjectTracker::onMessage Objects time range: %f - %f", - (current_time - latest_time).seconds(), (current_time - oldest_time).seconds()); - - // count objects on each channel - std::vector object_counts; - object_counts.resize(input_channel_size_); - // initialize to zero - for (size_t i = 0; i < input_channel_size_; i++) { - object_counts[i] = 0; - } - for (const auto & objects_data : objects_list) { - object_counts[objects_data.first] += 1; - } - // print result - std::string object_counts_str = "MultiObjectTracker::onMessage Object counts: "; - for (size_t i = 0; i < input_channel_size_; i++) { - object_counts_str += - input_channels_[i].long_name + " " + std::to_string(object_counts[i]) + " "; - } - RCLCPP_INFO(this->get_logger(), object_counts_str.c_str()); - - std::string object_info_str = ""; - object_info_str += "MultiObjectTracker::onMessage Objects channel and time: \n"; - for (const auto & objects_data : objects_list) { - const auto & objects = objects_data.second; - const auto & channel_index = objects_data.first; - const auto & time = rclcpp::Time(objects.header.stamp); - // print object information - object_info_str += input_channels_[channel_index].long_name + " " + - std::to_string((current_time - time).seconds()) + " \n"; - } - RCLCPP_INFO(this->get_logger(), object_info_str.c_str()); - - /* DEBUG END */ } void MultiObjectTracker::runProcess( diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index 1e5bee9887063..5d4fefd69b03e 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -269,20 +269,6 @@ void InputManager::optimizeTimings() selected_stream_interval = interval_mean; selected_stream_interval_std = std::sqrt(interval_var); } - - /* DEBUG */ - std::string long_name, short_name; - rclcpp::Time latest_measurement_time, latest_message_time; - input_stream->getNames(long_name, short_name); - input_stream->getTimestamps(latest_measurement_time, latest_message_time); - double latency_message = (node_.now() - latest_message_time).seconds(); - double latency_measurement = (node_.now() - latest_measurement_time).seconds(); - RCLCPP_INFO( - node_.get_logger(), - "InputManager::getObjects %s: latency mean: %f, std: %f, interval mean: " - "%f, std: %f, latest measurement latency: %f, latest message latency: %f", - long_name.c_str(), latency_mean, std::sqrt(latency_var), interval_mean, - std::sqrt(interval_var), latency_measurement, latency_message); } } From 48a8861df8de788a379686a12672ead65c2b623a Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 14 May 2024 15:54:07 +0900 Subject: [PATCH 58/67] fix: change no-object message level Signed-off-by: Taekjin LEE --- .../multi_object_tracker/src/processor/input_manager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index 5d4fefd69b03e..dfaae6252745c 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -321,7 +321,7 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li latest_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp); } else { // check time jump - if ((now - latest_object_time_).seconds() < 0.0) { + if (now < latest_object_time_) { RCLCPP_WARN( node_.get_logger(), "InputManager::getObjects Time jump detected, now: %f, latest_object_time_: %f", @@ -329,7 +329,7 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li latest_object_time_ = now - rclcpp::Duration::from_seconds(3.0); // reset the latest object time to 3 seconds ago } else { - RCLCPP_WARN( + RCLCPP_DEBUG( node_.get_logger(), "InputManager::getObjects No objects in the object list, object time band from %f to %f", (now - object_oldest_time).seconds(), (now - object_latest_time).seconds()); From f873ab8d2efdd12565f783aa2a330fe41d425fb4 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 16 May 2024 16:37:20 +0900 Subject: [PATCH 59/67] Update perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Signed-off-by: Taekjin LEE --- .../include/multi_object_tracker/debugger/debug_object.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp index c312edc843259..8138d388d1901 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 1ba2c79ef839a71afb5844883283eeb8cf8f8568 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 16 May 2024 16:46:47 +0900 Subject: [PATCH 60/67] chore: Update copyright to uppercase Signed-off-by: Taekjin LEE --- .../include/multi_object_tracker/debugger/debugger.hpp | 2 +- .../include/multi_object_tracker/processor/input_manager.hpp | 2 +- .../include/multi_object_tracker/processor/processor.hpp | 2 +- perception/multi_object_tracker/src/debugger/debug_object.cpp | 2 +- perception/multi_object_tracker/src/debugger/debugger.cpp | 2 +- perception/multi_object_tracker/src/processor/input_manager.cpp | 2 +- perception/multi_object_tracker/src/processor/processor.cpp | 2 +- 7 files changed, 7 insertions(+), 7 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp index 8edfc102b56db..1c632f5bebba0 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. 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 d550547ea4278..ad8c4ae6e5e24 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 @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp index de0fe0854254a..310871dfcb07a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp index 11ecafba19e53..65e2068ab04ac 100644 --- a/perception/multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp index ad4ce4afad498..2baa970850a18 100644 --- a/perception/multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger/debugger.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index dfaae6252745c..aada2c0403222 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/perception/multi_object_tracker/src/processor/processor.cpp b/perception/multi_object_tracker/src/processor/processor.cpp index a5a1f18a64ca0..08df50fa66993 100644 --- a/perception/multi_object_tracker/src/processor/processor.cpp +++ b/perception/multi_object_tracker/src/processor/processor.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 9edd2e06913fcb4200ed031b7e91434162adb556 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 16 May 2024 16:53:16 +0900 Subject: [PATCH 61/67] chore: fix readme links to config files Signed-off-by: Taekjin LEE --- perception/multi_object_tracker/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/multi_object_tracker/README.md b/perception/multi_object_tracker/README.md index 11302612cd046..ad9e43355aebf 100644 --- a/perception/multi_object_tracker/README.md +++ b/perception/multi_object_tracker/README.md @@ -77,7 +77,7 @@ Available input channels are defined in [input_channels.param.yaml](config/input ### Core Parameters -Node parameters are defined in [multi_object_tracker.param.yaml](config/multi_object_tracker.param.yaml) and association parameters are defined in [data_association.param.yaml](config/data_association.param.yaml). +Node parameters are defined in [multi_object_tracker_node.param.yaml](config/multi_object_tracker_node.param.yaml) and association parameters are defined in [data_association_matrix.param.yaml](config/data_association_matrix.param.yaml). #### Node parameters From cf6cf3c407249be66c38f69a5a178fd1ad28bd8a Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 17 May 2024 10:21:40 +0900 Subject: [PATCH 62/67] chore: move and rename uuid functions Signed-off-by: Taekjin LEE --- .../debugger/debug_object.hpp | 20 ------------ .../src/debugger/debug_object.cpp | 31 +++++++++++++++++-- 2 files changed, 28 insertions(+), 23 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp index 8138d388d1901..6caa69181dd9d 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp @@ -97,26 +97,6 @@ class TrackerObjectDebugger visualization_msgs::msg::MarkerArray & marker_array) const; void process(); void getMessage(visualization_msgs::msg::MarkerArray & marker_array) const; - -private: - std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & u) const - { - std::stringstream ss; - for (auto i = 0; i < 16; ++i) { - ss << std::hex << std::setfill('0') << std::setw(2) << +u.uuid[i]; - } - return ss.str(); - } - - boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg) - { - const std::string uuid_str = uuid_to_string(uuid_msg); - boost::uuids::string_generator gen; - boost::uuids::uuid uuid = gen(uuid_str); - return uuid; - } - - int32_t uuid_to_int(const boost::uuids::uuid & uuid) { return boost::uuids::hash_value(uuid); } }; #endif // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp index 65e2068ab04ac..ebbeab43a155b 100644 --- a/perception/multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -23,6 +23,31 @@ #include #include +namespace +{ +std::string uuidToString(const unique_identifier_msgs::msg::UUID & uuid_msg) +{ + std::stringstream ss; + for (auto i = 0; i < 16; ++i) { + ss << std::hex << std::setfill('0') << std::setw(2) << +uuid_msg.uuid[i]; + } + return ss.str(); +} + +boost::uuids::uuid uuidToBoostUuid(const unique_identifier_msgs::msg::UUID & uuid_msg) +{ + const std::string uuid_str = uuidToString(uuid_msg); + boost::uuids::string_generator gen; + boost::uuids::uuid uuid = gen(uuid_str); + return uuid; +} + +int32_t uuidToInt(const boost::uuids::uuid & uuid) +{ + return boost::uuids::hash_value(uuid); +} +} // namespace + TrackerObjectDebugger::TrackerObjectDebugger(std::string frame_id) { // set frame id @@ -67,9 +92,9 @@ void TrackerObjectDebugger::collect( autoware_auto_perception_msgs::msg::TrackedObject tracked_object; (*(tracker_itr))->getTrackedObject(message_time, tracked_object); - object_data.uuid = to_boost_uuid(tracked_object.object_id); - object_data.uuid_int = uuid_to_int(object_data.uuid); - object_data.uuid_str = uuid_to_string(tracked_object.object_id); + object_data.uuid = uuidToBoostUuid(tracked_object.object_id); + object_data.uuid_int = uuidToInt(object_data.uuid); + object_data.uuid_str = uuidToString(tracked_object.object_id); // tracker bool is_associated = false; From d5efe1017eec1bb7cfba027d22a8d0e1fdce8f79 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 17 May 2024 10:22:20 +0900 Subject: [PATCH 63/67] chore: fix debug topic to use node name Signed-off-by: Taekjin LEE --- perception/multi_object_tracker/src/debugger/debugger.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp index 2baa970850a18..afb0ecdc22996 100644 --- a/perception/multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger/debugger.cpp @@ -30,12 +30,12 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node, const std::string & frame_ if (debug_settings_.publish_tentative_objects) { debug_tentative_objects_pub_ = node_.create_publisher( - "multi_object_tracker/debug/tentative_objects", rclcpp::QoS{1}); + "~/debug/tentative_objects", rclcpp::QoS{1}); } if (debug_settings_.publish_debug_markers) { debug_objects_markers_pub_ = node_.create_publisher( - "multi_object_tracker/debug/objects_markers", rclcpp::QoS{1}); + "~/debug/objects_markers", rclcpp::QoS{1}); } // initialize timestamps From 4445c45fb9bcd9159683a24afa0cf7d259aa45b2 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 17 May 2024 10:22:46 +0900 Subject: [PATCH 64/67] chore: express meaning of threshold Signed-off-by: Taekjin LEE --- .../multi_object_tracker/src/processor/input_manager.cpp | 3 ++- 1 file changed, 2 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 aada2c0403222..44fa515061d2a 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -122,7 +122,8 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim // Update time latest_message_time_ = now; - if (std::abs((latest_measurement_time_ - objects_time).seconds()) > 3.0) { + 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 latest_measurement_time_ = objects_time; RCLCPP_WARN( From f218f84a943278b28f2ac55c0cf8f142b108e376 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 23 May 2024 09:34:36 +0900 Subject: [PATCH 65/67] feat: revise decay rate, update function Signed-off-by: Taekjin LEE --- .../src/tracker/model/tracker_base.cpp | 45 ++++++++++++------- 1 file changed, 30 insertions(+), 15 deletions(-) diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp index b62cf05449603..4318da0569721 100644 --- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp @@ -21,6 +21,14 @@ #include #include +namespace +{ +float updateProbability(float prior, float true_positive, float false_positive) +{ + return (prior * true_positive) / (prior * true_positive + (1 - prior) * false_positive); +} +} // namespace + Tracker::Tracker( const rclcpp::Time & time, const std::vector & classification, @@ -43,7 +51,13 @@ Tracker::Tracker( void Tracker::initializeExistenceProbabilities( const uint & channel_index, const float & existence_probability) { - existence_probabilities_[channel_index] = 0.8 + 0.2 * existence_probability; + // The initial existence probability is modeled + // since the incoming object's existence probability is not reliable + // existence probability on each channel + constexpr float initial_existence_probability = 0.1; + existence_probabilities_[channel_index] = initial_existence_probability; + + // total existence probability total_existence_probability_ = existence_probability; } @@ -54,30 +68,31 @@ bool Tracker::updateWithMeasurement( { // Update existence probability { - float existence_probability_from_object = object.existence_probability; no_measurement_count_ = 0; ++total_measurement_count_; // existence probability on each channel const double delta_time = (measurement_time - last_update_with_measurement_time_).seconds(); - const double decay_rate = 5.0 / 10.0; - - const float gain = 0.4; - const float probability_detected = 0.99; - // existence_probabilities_[channel_index] = existence_probability_from_object; - existence_probabilities_[channel_index] = - gain * probability_detected + (1 - gain) * existence_probabilities_[channel_index]; - + const double decay_rate = -log(0.5) / 0.3; // 50% decay in 0.3s + constexpr float probability_true_detection = 0.9; + constexpr float probability_false_detection = 0.2; + + // update measured channel probability + existence_probabilities_[channel_index] = updateProbability( + existence_probabilities_[channel_index], probability_true_detection, + probability_false_detection); + // decay other channel probabilities for (size_t i = 0; i < existence_probabilities_.size(); ++i) { if (i == channel_index) { continue; } - existence_probabilities_[i] *= std::exp(-decay_rate * delta_time); + existence_probabilities_[i] *= std::exp(decay_rate * delta_time); } - // total existence probability - object is detected - total_existence_probability_ += - (1 - total_existence_probability_) * existence_probability_from_object; + // update total existence probability + const float & existence_probability_from_object = object.existence_probability; + total_existence_probability_ = updateProbability( + total_existence_probability_, existence_probability_from_object, probability_false_detection); } last_update_with_measurement_time_ = measurement_time; @@ -96,7 +111,7 @@ bool Tracker::updateWithoutMeasurement(const rclcpp::Time & now) { // decay existence probability double const delta_time = (now - last_update_with_measurement_time_).seconds(); - double const decay_rate = 5.0 / 10.0; + const double decay_rate = -log(0.5) / 0.3; // 50% decay in 0.3s for (size_t i = 0; i < existence_probabilities_.size(); ++i) { existence_probabilities_[i] *= std::exp(-decay_rate * delta_time); } From b62399378f67d07e949cd6174a33146616a53bb2 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 23 May 2024 12:14:47 +0900 Subject: [PATCH 66/67] fix: define constants with explanation Signed-off-by: Taekjin LEE --- .../src/processor/input_manager.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index 44fa515061d2a..6fd6325a77b61 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -79,11 +79,15 @@ void InputStream::onMessage( void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Time & objects_time) { + // Define constants + constexpr int SKIP_COUNT = 4; // Skip the initial messages + constexpr int INITIALIZATION_COUNT = 16; // Initialization process count + // Update latency statistics // skip initial messages for the latency statistics - if (initial_count_ > 4) { + if (initial_count_ > SKIP_COUNT) { const double latency = (now - objects_time).seconds(); - if (initial_count_ < 16) { + if (initial_count_ < INITIALIZATION_COUNT) { // set higher gain for the initial messages constexpr double initial_gain = 0.5; latency_mean_ = (1.0 - initial_gain) * latency_mean_ + initial_gain * latency; @@ -96,7 +100,7 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim } // Calculate interval, Update interval statistics - if (initial_count_ > 4) { + if (initial_count_ > SKIP_COUNT) { const double interval = (now - latest_message_time_).seconds(); if (interval < 0.0) { RCLCPP_WARN( @@ -104,7 +108,7 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim "InputManager::updateTimingStatus %s: Negative interval detected, now: %f, " "latest_message_time_: %f", long_name_.c_str(), now.seconds(), latest_message_time_.seconds()); - } else if (initial_count_ < 24) { + } else if (initial_count_ < INITIALIZATION_COUNT) { // Initialization constexpr double initial_gain = 0.5; interval_mean_ = (1.0 - initial_gain) * interval_mean_ + initial_gain * interval; @@ -136,8 +140,8 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim latest_measurement_time_ < objects_time ? objects_time : latest_measurement_time_; } - // Update the initial count, count only first 32 messages - if (initial_count_ < 32) { + // Update the initial count + if (initial_count_ < INITIALIZATION_COUNT) { initial_count_++; } } From 13adc73da8c846d471f6c0e1a0580cfb4f6a6b10 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 23 May 2024 03:16:55 +0000 Subject: [PATCH 67/67] style(pre-commit): autofix --- .../multi_object_tracker/src/processor/input_manager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index 6fd6325a77b61..e29b54e971491 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -80,8 +80,8 @@ void InputStream::onMessage( void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Time & objects_time) { // Define constants - constexpr int SKIP_COUNT = 4; // Skip the initial messages - constexpr int INITIALIZATION_COUNT = 16; // Initialization process count + constexpr int SKIP_COUNT = 4; // Skip the initial messages + constexpr int INITIALIZATION_COUNT = 16; // Initialization process count // Update latency statistics // skip initial messages for the latency statistics