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 d60f54c2e641a..e1a2260f3ee6d 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -22,6 +22,7 @@ + diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index 46af1f9b9a1de..fb2c4d73554a3 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -22,8 +22,10 @@ include_directories( # Generate exe file set(MULTI_OBJECT_TRACKER_SRC src/multi_object_tracker_core.cpp - src/debugger.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 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/README.md b/perception/multi_object_tracker/README.md index 311fe7b6da716..ad9e43355aebf 100644 --- a/perception/multi_object_tracker/README.md +++ b/perception/multi_object_tracker/README.md @@ -46,31 +46,38 @@ 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 -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 @@ -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 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..b57f37675d4f1 --- /dev/null +++ b/perception/multi_object_tracker/config/input_channels.param.yaml @@ -0,0 +1,82 @@ +/**: + ros__parameters: + input_channels: + detected_objects: + topic: "/perception/object_recognition/detection/objects" + can_spawn_new_tracker: true + optional: + name: "detected_objects" + short_name: "all" + # LIDAR - rule-based + lidar_clustering: + topic: "/perception/object_recognition/detection/clustering/objects" + can_spawn_new_tracker: true + optional: + name: "clustering" + short_name: "Lcl" + # LIDAR - DNN + lidar_centerpoint: + topic: "/perception/object_recognition/detection/centerpoint/objects" + can_spawn_new_tracker: true + optional: + name: "centerpoint" + short_name: "Lcp" + lidar_centerpoint_validated: + topic: "/perception/object_recognition/detection/centerpoint/validation/objects" + can_spawn_new_tracker: true + optional: + name: "centerpoint" + short_name: "Lcp" + lidar_apollo: + topic: "/perception/object_recognition/detection/apollo/objects" + can_spawn_new_tracker: true + optional: + name: "apollo" + short_name: "Lap" + lidar_apollo_validated: + topic: "/perception/object_recognition/detection/apollo/validation/objects" + can_spawn_new_tracker: true + optional: + name: "apollo" + short_name: "Lap" + # LIDAR-CAMERA - DNN + # cspell:ignore lidar_pointpainitng pointpainting + lidar_pointpainitng: + topic: "/perception/object_recognition/detection/pointpainting/objects" + can_spawn_new_tracker: true + optional: + name: "pointpainting" + 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: true + optional: + name: "camera_lidar_fusion" + short_name: "CLf" + # CAMERA-LIDAR+TRACKER + detection_by_tracker: + topic: "/perception/object_recognition/detection/detection_by_tracker/objects" + can_spawn_new_tracker: false + optional: + name: "detection_by_tracker" + short_name: "dbT" + # RADAR + radar: + topic: "/sensing/radar/detected_objects" + can_spawn_new_tracker: true + optional: + name: "radar" + short_name: "R" + radar_far: + topic: "/perception/object_recognition/detection/radar/far_objects" + can_spawn_new_tracker: true + optional: + name: "radar_far" + short_name: "Rf" 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..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 @@ -17,5 +17,6 @@ # 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] 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..6caa69181dd9d --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp @@ -0,0 +1,102 @@ +// 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 + +#include +#include +#include +#include +#include +#include + +struct ObjectData +{ + rclcpp::Time time; + + // object uuid + boost::uuids::uuid uuid; + int uuid_int; + std::string uuid_str; + + // 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; + + // detection channel id + uint channel_id; +}; + +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_; + + std::vector object_data_list_; + std::list unused_marker_ids_; + 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, + const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects, + const std::unordered_map & direct_assignment, + const std::unordered_map & reverse_assignment); + + void reset(); + void draw( + const std::vector> object_data_groups, + visualization_msgs::msg::MarkerArray & marker_array) const; + void process(); + void getMessage(visualization_msgs::msg::MarkerArray & marker_array) const; +}; + +#endif // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ 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 66% 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..1c632f5bebba0 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/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. @@ -11,11 +11,11 @@ // 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_HPP_ -#define MULTI_OBJECT_TRACKER__DEBUGGER_HPP_ +#ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ +#define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ + +#include "multi_object_tracker/debugger/debug_object.hpp" #include #include @@ -27,7 +27,11 @@ #include #include +#include #include +#include +#include +#include /** * @brief Debugger class for multi object tracker @@ -36,40 +40,68 @@ 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); + explicit TrackerDebugger(rclcpp::Node & node, const std::string & frame_id); - void setupDiagnostics(); - void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat); +private: struct DEBUG_SETTINGS { bool publish_processing_time; bool publish_tentative_objects; + bool publish_debug_markers; 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; + // 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_; rclcpp::Time stamp_process_start_; rclcpp::Time stamp_process_end_; rclcpp::Time stamp_publish_start_; rclcpp::Time stamp_publish_output_; + + // 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); + void startPublishTime(const rclcpp::Time & now); + void endPublishTime(const rclcpp::Time & now, const rclcpp::Time & object_time); + 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, + 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_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 f788dd3895216..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,8 @@ #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" @@ -48,8 +49,16 @@ #include #include #include +#include #include +namespace multi_object_tracker +{ + +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 { public: @@ -57,10 +66,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_; @@ -78,15 +85,24 @@ class MultiObjectTracker : public rclcpp::Node std::unique_ptr data_association_; std::unique_ptr processor_; + // input manager + std::unique_ptr input_manager_; + + std::vector input_channels_{}; + size_t input_channel_size_{}; + // callback functions - void onMeasurement( - const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg); void onTimer(); + void onTrigger(); + void onMessage(const ObjectsList & objects_list); // publish processes + 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; }; +} // namespace multi_object_tracker + #endif // MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_ 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..ad8c4ae6e5e24 --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp @@ -0,0 +1,158 @@ +// 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 +#include +#include + +namespace multi_object_tracker +{ +using DetectedObjects = autoware_auto_perception_msgs::msg::DetectedObjects; +using ObjectsList = std::vector>; + +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 + bool is_spawn_enabled = true; // enable spawn of the object +}; + +class InputStream +{ +public: + 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) + { + func_trigger_ = func_trigger; + } + + 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 initial_count_ > 0; } + uint getIndex() const { return index_; } + void getObjectsOlderThan( + const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, + ObjectsList & objects_list); + void getNames(std::string & long_name, std::string & short_name) + { + 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( + double & latency_mean, double & latency_var, double & interval_mean, + double & interval_var) const + { + latency_mean = latency_mean_; + latency_var = latency_var_; + 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_; } + +private: + rclcpp::Node & node_; + uint index_; + + 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_; + + std::function func_trigger_; + + // bool is_time_initialized_{false}; + int initial_count_{0}; + double expected_interval_; + double latency_mean_; + double latency_var_; + double interval_mean_; + double interval_var_; + + rclcpp::Time latest_measurement_time_; + rclcpp::Time latest_message_time_; +}; + +class InputManager +{ +public: + explicit InputManager(rclcpp::Node & node); + + void init(const std::vector & input_channels); + void setTriggerFunction(std::function func_trigger) { func_trigger_ = func_trigger; } + + void onTrigger(const uint & index) const; + + void getObjectTimeInterval( + const rclcpp::Time & now, rclcpp::Time & object_latest_time, + rclcpp::Time & object_oldest_time) const; + void optimizeTimings(); + bool getObjects(const rclcpp::Time & now, ObjectsList & objects_list); + bool isChannelSpawnEnabled(const uint & index) const + { + return input_streams_[index]->isSpawnEnabled(); + } + +private: + rclcpp::Node & node_; + std::vector::SharedPtr> sub_objects_array_{}; + + bool is_initialized_{false}; + rclcpp::Time latest_object_time_; + + size_t input_size_; + std::vector> input_streams_; + + std::function func_trigger_; + uint target_stream_idx_{0}; + 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 + +#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..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. @@ -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_ @@ -34,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 @@ -42,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 @@ -58,9 +57,12 @@ 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_; + const size_t channel_size_; // parameters float max_elapsed_time_; // [s] @@ -73,7 +75,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..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 @@ -35,33 +35,41 @@ 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 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); - 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 +78,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 +87,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/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/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp new file mode 100644 index 0000000000000..ebbeab43a155b --- /dev/null +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -0,0 +1,389 @@ +// 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 +#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 + frame_id_ = frame_id; + + // initialize markers + markers_.markers.clear(); + current_ids_.clear(); + previous_ids_.clear(); + message_time_ = rclcpp::Time(0, 0); +} + +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 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 = 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; + 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; + } 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; + 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); + } +} + +void TrackerObjectDebugger::process() +{ + if (!is_initialized_) return; + + // update uuid_int + for (auto & object_data : object_data_list_) { + current_ids_.insert(object_data.uuid_int); + } + + // sort by uuid, collect the same uuid object_data as a group, and loop for the groups + object_data_groups_.clear(); + { + // sort by uuid + std::sort( + object_data_list_.begin(), object_data_list_.end(), + [](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; + 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( + const std::vector> object_data_groups, + visualization_msgs::msg::MarkerArray & marker_array) const +{ + // initialize markers + marker_array.markers.clear(); + + constexpr int PALETTE_SIZE = 16; + constexpr std::array, PALETTE_SIZE> color_array = {{ + {{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) { + if (object_data_group.empty()) continue; + 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 = object_data_front.uuid_int; + 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; + marker.color.b = 1.0; // white + marker.lifetime = rclcpp::Duration::from_seconds(0); + + // get marker - existence_probability + visualization_msgs::msg::Marker text_marker; + text_marker = marker; + 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.8; + 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.5; + + // show the last existence probability + // print existence probability with channel name + // probability to text, two digits of percentage + std::string existence_probability_text = ""; + 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 = + 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); + + // loop for each object_data in the group + // boxed to tracker positions + // and link lines to the detected positions + 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; + 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; + + // 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; + + // 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_height_offset; + marker_track_boxes.points.push_back(box_point); + + // set association marker, if exists + if (!object_data.is_associated) continue; + 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 + marker_height_offset + assign_height_offset; + 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; + line_point.x = object_data.tracker_point.x; + line_point.y = object_data.tracker_point.y; + 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 + marker_height_offset + assign_height_offset; + marker_lines.points.push_back(line_point); + } + + // add markers + marker_array.markers.push_back(marker_track_boxes); + 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; +} + +void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & marker_array) const +{ + if (!is_initialized_) return; + + // 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()) { + continue; + } + + visualization_msgs::msg::Marker delete_marker; + delete_marker.header.frame_id = frame_id_; + delete_marker.header.stamp = message_time_; + 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); + + 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; +} diff --git a/perception/multi_object_tracker/src/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp similarity index 79% rename from perception/multi_object_tracker/src/debugger.cpp rename to perception/multi_object_tracker/src/debugger/debugger.cpp index b5632a13e78fc..afb0ecdc22996 100644 --- a/perception/multi_object_tracker/src/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. @@ -11,14 +11,13 @@ // 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.hpp" +#include "multi_object_tracker/debugger/debugger.hpp" #include -TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : diagnostic_updater_(&node), node_(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(); @@ -31,7 +30,12 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : diagnostic_updater_(&nod if (debug_settings_.publish_tentative_objects) { debug_tentative_objects_pub_ = node_.create_publisher( - "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( + "~/debug/objects_markers", rclcpp::QoS{1}); } // initialize timestamps @@ -46,14 +50,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 { @@ -61,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 = @@ -69,11 +66,32 @@ 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; } } +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 +115,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) { @@ -167,3 +177,33 @@ 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) +{ + if (!debug_settings_.publish_debug_markers) return; + object_debugger_.collect( + message_time, list_tracker, channel_index, detected_objects, direct_assignment, + reverse_assignment); +} + +// ObjectDebugger +void TrackerDebugger::publishObjectsMarkers() +{ + if (!debug_settings_.publish_debug_markers) return; + + 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(); +} 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..c3bf7023fb068 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,24 +65,79 @@ 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()), 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"); bool enable_delay_compensation{declare_parameter("enable_delay_compensation")}; + 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 - Input channels + // Get input channels + std::vector input_topic_names; + std::vector input_names_long; + std::vector input_names_short; + std::vector input_is_spawn_enabled; + + if (selected_input_channels.empty()) { + RCLCPP_ERROR(this->get_logger(), "No input topics are specified."); + return; + } + + 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"); + input_topic_names.push_back(input_topic_name); + + // required parameter, but can set a default value + 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; + 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(); + input_channels_.resize(input_channel_size_); + + for (size_t i = 0; i < input_channel_size_; 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].is_spawn_enabled = input_is_spawn_enabled[i]; + } + + // Initialize input manager + input_manager_ = std::make_unique(*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( this->get_node_base_interface(), this->get_node_timers_interface()); @@ -119,7 +172,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 @@ -138,35 +191,96 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) } // Debugger - debugger_ = std::make_unique(*this); + debugger_ = std::make_unique(*this, world_frame_id_); + debugger_->setObjectChannels(input_names_short); published_time_publisher_ = std::make_unique(this); } -void MultiObjectTracker::onMeasurement( - const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg) +void MultiObjectTracker::onTrigger() +{ + const rclcpp::Time current_time = this->now(); + // get objects from the input manager and run process + std::vector> objects_list; + const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list); + if (!is_objects_ready) return; + + 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) { + // 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); + } + } +} + +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) return; + + // get objects from the input manager and run process + ObjectsList objects_list; + const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list); + if (is_objects_ready) { + onMessage(objects_list); + } + + // Publish + checkAndPublish(current_time); +} + +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); + + // process start + debugger_->startMeasurementTime(this->now(), oldest_time); + // run process for each DetectedObjects + for (const auto & objects_data : objects_list) { + runProcess(objects_data.second, objects_data.first); + } + // process end + debugger_->endMeasurementTime(this->now()); +} + +void MultiObjectTracker::runProcess( + const DetectedObjects & input_objects_msg, const uint & channel_index) { // 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()); - /* keep the latest input stamp and check transform*/ - debugger_->startMeasurementTime(this->now(), measurement_time); + // 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 */ - autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; + + // 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; } - ////// Tracker Process - //// Associate and update /* prediction */ processor_->predict(measurement_time); + /* object association */ std::unordered_map direct_assignment, reverse_assignment; { @@ -176,40 +290,22 @@ void MultiObjectTracker::onMeasurement( 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 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); + 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); - - // 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); + /* spawn new tracker */ + if (input_manager_->isChannelSpawnEnabled(channel_index)) { + processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index); } } @@ -234,7 +330,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 +342,14 @@ 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); } + debugger_->publishObjectsMarkers(); } -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(); 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..e29b54e971491 --- /dev/null +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -0,0 +1,347 @@ +// 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" + +#include + +namespace multi_object_tracker +{ +/////////////////////////// +/////// InputStream /////// +/////////////////////////// +InputStream::InputStream(rclcpp::Node & node, uint & index) : node_(node), index_(index) +{ +} + +void InputStream::init(const InputChannel & input_channel) +{ + // Initialize parameters + 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(); + + // Initialize latency statistics + latency_mean_ = 0.2; // [s] (initial value) + latency_var_ = 0.0; + interval_mean_ = 0.0; // [s] (initial value) + 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 (!isTimeInitialized()) { + return false; + } + latest_measurement_time = latest_measurement_time_; + latest_message_time = latest_message_time_; + return true; +} + +void InputStream::onMessage( + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg) +{ + const DetectedObjects objects = *msg; + objects_que_.push_back(objects); + if (objects_que_.size() > que_size_) { + 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) +{ + // 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_ > SKIP_COUNT) { + const double latency = (now - objects_time).seconds(); + 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; + } 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 (initial_count_ > SKIP_COUNT) { + const double interval = (now - latest_message_time_).seconds(); + 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_ < INITIALIZATION_COUNT) { + // 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; + } + } + } + + // Update time + latest_message_time_ = now; + constexpr double delay_threshold = 3.0; // [s] + if (std::abs((latest_measurement_time_ - objects_time).seconds()) > delay_threshold) { + // Reset the latest measurement time if the time difference is too large + 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 + if (initial_count_ < INITIALIZATION_COUNT) { + initial_count_++; + } +} + +void InputStream::getObjectsOlderThan( + const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time, + ObjectsList & objects_list) +{ + assert(object_latest_time.nanoseconds() > object_oldest_time.nanoseconds()); + + for (const auto & object : objects_que_) { + const rclcpp::Time object_time = rclcpp::Time(object.header.stamp); + + // remove objects older than the specified duration + if (object_time < object_oldest_time) { + objects_que_.pop_front(); + continue; + } + + // 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); + objects_list.push_back(object_pair); + // remove the object from the queue + objects_que_.pop_front(); + } + } +} + +//////////////////////////// +/////// InputManager /////// +//////////////////////////// +InputManager::InputManager(rclcpp::Node & node) : node_(node) +{ + latest_object_time_ = node_.now(); +} + +void InputManager::init(const std::vector & input_channels) +{ + // Check input sizes + input_size_ = input_channels.size(); + if (input_size_ == 0) { + RCLCPP_ERROR(node_.get_logger(), "InputManager::init No input streams"); + 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); + 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)); + is_any_spawn_enabled |= input_streams_.at(i)->isSpawnEnabled(); + + // Set subscription + RCLCPP_INFO( + node_.get_logger(), "InputManager::init Initializing %s input stream from %s", + 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_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; +} + +void InputManager::onTrigger(const uint & index) const +{ + // when the target stream triggers, call the trigger function + if (index == target_stream_idx_ && func_trigger_) { + func_trigger_(); + } +} + +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_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 older than the latest measurement time, set it as the latest + // object time + object_latest_time = + object_latest_time < latest_measurement_time ? latest_measurement_time : object_latest_time; + } + + object_oldest_time = object_latest_time - rclcpp::Duration::from_seconds(1.0); + // if the object_oldest_time is older than the latest object time, set it to the latest object + // time + object_oldest_time = + object_oldest_time > latest_object_time_ ? object_oldest_time : latest_object_time_; +} + +void InputManager::optimizeTimings() +{ + double max_latency_mean = 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_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); + } + } + } + + // Set the target stream index, which has the maximum latency + // trigger will be called next time + // 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) +{ + if (!is_initialized_) { + RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Input manager is not initialized"); + return false; + } + + // Clear the objects + objects_list.clear(); + + // Get the time interval for the objects + 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 + for (const auto & input_stream : input_streams_) { + input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects_list); + } + + // Sort objects by timestamp + std::sort( + 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; + }); + + // Update the latest object time + 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_) { + 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_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()); + } + } + + return is_any_object; +} + +} // namespace multi_object_tracker diff --git a/perception/multi_object_tracker/src/processor/processor.cpp b/perception/multi_object_tracker/src/processor/processor.cpp index 0d56e16b431e9..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. @@ -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" @@ -25,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] @@ -50,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; @@ -59,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); } } } @@ -69,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) { @@ -77,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..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,9 +21,18 @@ #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) + const std::vector & classification, + const size_t & channel_size) : classification_(classification), no_measurement_count_(0), total_no_measurement_count_(0), @@ -34,23 +43,81 @@ 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) +{ + // 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; } 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 + { + 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 = -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); + } + + // 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; + + // Update object measure(object, measurement_time, self_transform); + return true; } -bool Tracker::updateWithoutMeasurement() +bool Tracker::updateWithoutMeasurement(const rclcpp::Time & now) { + // Update existence probability ++no_measurement_count_; ++total_no_measurement_count_; + { + // decay existence probability + double const delta_time = (now - last_update_with_measurement_time_).seconds(); + 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); + } + total_existence_probability_ *= std::exp(-decay_rate * delta_time); + } + 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]