From eac4ad9da44d3acae63e8e5e07f576c7d3e2ae3a Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 12 Mar 2025 19:11:07 +0900 Subject: [PATCH 01/11] refactor(bicycle_motion_model): implement exponential decay for slip angle in state prediction Signed-off-by: Taekjin LEE --- .../lib/tracker/motion_model/bicycle_motion_model.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp index a0b5861acbd0b..ffac0980a742b 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp @@ -326,7 +326,9 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt X_next_t(IDX::VEL) = X_t(IDX::VEL); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) + // Apply exponential decay to slip angle over time, with a half-life of 2 seconds + const double decay_rate = std::exp(-dt * 0.69314718056 / 2.0); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP) * decay_rate; // slip_angle = asin(lr * w / v) // State transition matrix A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(DIM, DIM); From b609123269aa610f0185820b7c03432ff191230a Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 28 Feb 2025 11:32:07 +0900 Subject: [PATCH 02/11] Revert "refactor(multi_object_tracker): simplify input channel configuration by removing trust flags and consolidating parameters" This reverts commit c5155ef2e978b411955ace35f412bbf76c96f354. Signed-off-by: Taekjin LEE --- .../config/input_channels.param.yaml | 78 ++++++++++++++++--- .../object_model/types.hpp | 4 + .../lib/tracker/model/tracker_base.cpp | 3 +- .../src/multi_object_tracker_node.cpp | 18 ++++- .../src/processor/processor.cpp | 8 +- 5 files changed, 95 insertions(+), 16 deletions(-) diff --git a/perception/autoware_multi_object_tracker/config/input_channels.param.yaml b/perception/autoware_multi_object_tracker/config/input_channels.param.yaml index 6f9bfcf6210dd..aaa55e2083b98 100644 --- a/perception/autoware_multi_object_tracker/config/input_channels.param.yaml +++ b/perception/autoware_multi_object_tracker/config/input_channels.param.yaml @@ -3,39 +3,63 @@ 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 + flags: + can_spawn_new_tracker: true + can_trust_existence_probability: false + can_trust_extension: false + can_trust_classification: false + can_trust_orientation: false optional: name: "clustering" short_name: "Lcl" # LIDAR - DNN lidar_centerpoint: topic: "/perception/object_recognition/detection/centerpoint/objects" - can_spawn_new_tracker: true + flags: + can_spawn_new_tracker: true + can_trust_existence_probability: true + can_trust_extension: true + can_trust_classification: true + can_trust_orientation: true optional: name: "centerpoint" short_name: "Lcp" lidar_centerpoint_validated: topic: "/perception/object_recognition/detection/centerpoint/validation/objects" - can_spawn_new_tracker: true + flags: + can_spawn_new_tracker: true + can_trust_existence_probability: true + can_trust_extension: true + can_trust_classification: true + can_trust_orientation: true optional: name: "centerpoint" short_name: "Lcp" lidar_apollo: topic: "/perception/object_recognition/detection/apollo/objects" - can_spawn_new_tracker: true + flags: + can_spawn_new_tracker: true + can_trust_existence_probability: true + can_trust_extension: true + can_trust_classification: true + can_trust_orientation: true optional: name: "apollo" short_name: "Lap" lidar_apollo_validated: topic: "/perception/object_recognition/detection/apollo/validation/objects" - can_spawn_new_tracker: true + flags: + can_spawn_new_tracker: true + can_trust_existence_probability: true + can_trust_extension: true + can_trust_classification: true + can_trust_orientation: true optional: name: "apollo" short_name: "Lap" @@ -43,40 +67,70 @@ # cspell:ignore lidar_pointpainting pointpainting lidar_pointpainting: topic: "/perception/object_recognition/detection/pointpainting/objects" - can_spawn_new_tracker: true + flags: + can_spawn_new_tracker: true + can_trust_existence_probability: true + can_trust_extension: true + can_trust_classification: true + can_trust_orientation: true optional: name: "pointpainting" short_name: "Lpp" lidar_pointpainting_validated: topic: "/perception/object_recognition/detection/pointpainting/validation/objects" - can_spawn_new_tracker: true + flags: + can_spawn_new_tracker: true + can_trust_existence_probability: true + can_trust_extension: true + can_trust_classification: true + can_trust_orientation: 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 + flags: + can_spawn_new_tracker: true + can_trust_existence_probability: false + can_trust_extension: false + can_trust_classification: true + can_trust_orientation: false 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 + flags: + can_spawn_new_tracker: false + can_trust_existence_probability: false + can_trust_extension: false + can_trust_classification: false + can_trust_orientation: false optional: name: "detection_by_tracker" short_name: "dbT" # RADAR radar: topic: "/sensing/radar/detected_objects" - can_spawn_new_tracker: true + flags: + can_spawn_new_tracker: true + can_trust_existence_probability: true + can_trust_extension: true + can_trust_classification: true + can_trust_orientation: true optional: name: "radar" short_name: "R" radar_far: topic: "/perception/object_recognition/detection/radar/far_objects" - can_spawn_new_tracker: true + flags: + can_spawn_new_tracker: true + can_trust_existence_probability: true + can_trust_extension: true + can_trust_classification: true + can_trust_orientation: true optional: name: "radar_far" short_name: "Rf" diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp index 6b930908a79ac..c58db8da3a4a5 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp @@ -50,6 +50,10 @@ struct InputChannel 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 + bool trust_existence_probability = true; // trust object existence probability + bool trust_extension = true; // trust object extension + bool trust_classification = true; // trust object classification + bool trust_orientation = true; // trust object orientation(yaw) }; // object model diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp index a8a52e59cf079..43bb1f2a2a64d 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp @@ -114,8 +114,9 @@ bool Tracker::updateWithMeasurement( // Update classification if ( + channel_info.trust_classification && autoware::object_recognition_utils::getHighestProbLabel(object.classification) != - autoware_perception_msgs::msg::ObjectClassification::UNKNOWN) { + autoware_perception_msgs::msg::ObjectClassification::UNKNOWN) { updateClassification(object.classification); } diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index bf2c2de9b57e3..96a4fa0321828 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -91,7 +91,23 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) // required parameter, but can set a default value input_channel_config.is_spawn_enabled = declare_parameter( - "input_channels." + selected_input_channel + ".can_spawn_new_tracker", true); + "input_channels." + selected_input_channel + ".flags.can_spawn_new_tracker", true); + + // trust object existence probability + input_channel_config.trust_existence_probability = declare_parameter( + "input_channels." + selected_input_channel + ".flags.can_trust_existence_probability", true); + + // trust object extension, size beyond the visible area + input_channel_config.trust_extension = declare_parameter( + "input_channels." + selected_input_channel + ".flags.can_trust_extension", true); + + // trust object classification + input_channel_config.trust_classification = declare_parameter( + "input_channels." + selected_input_channel + ".flags.can_trust_classification", true); + + // trust object orientation(yaw) + input_channel_config.trust_orientation = declare_parameter( + "input_channels." + selected_input_channel + ".flags.can_trust_orientation", true); // optional parameters const std::string default_name = selected_input_channel; diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index bbe09baff150c..092d0aec0eafa 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -105,8 +105,12 @@ void TrackerProcessor::spawn( std::shared_ptr tracker = createNewTracker(new_object, time); // Initialize existence probabilities - tracker->initializeExistenceProbabilities( - new_object.channel_index, new_object.existence_probability); + if (channel_config.trust_existence_probability) { + tracker->initializeExistenceProbabilities( + new_object.channel_index, new_object.existence_probability); + } else { + tracker->initializeExistenceProbabilities(new_object.channel_index, 0.5); + } // Update the tracker with the new object list_tracker_.push_back(tracker); From 343689a4842e88a589bd0ed4ed9bb142bb4d1b0e Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 12 Mar 2025 13:21:45 +0900 Subject: [PATCH 03/11] refactor(multi_object_tracker): update measure function signatures to include InputChannel parameter Signed-off-by: Taekjin LEE --- .../tracker/model/bicycle_tracker.hpp | 4 +++- .../tracker/model/multiple_vehicle_tracker.hpp | 4 +++- .../tracker/model/pass_through_tracker.hpp | 4 +++- .../tracker/model/pedestrian_and_bicycle_tracker.hpp | 4 +++- .../tracker/model/pedestrian_tracker.hpp | 4 +++- .../multi_object_tracker/tracker/model/tracker_base.hpp | 4 +++- .../tracker/model/unknown_tracker.hpp | 4 +++- .../tracker/model/vehicle_tracker.hpp | 4 +++- .../lib/tracker/model/bicycle_tracker.cpp | 8 ++++++-- .../lib/tracker/model/multiple_vehicle_tracker.cpp | 8 +++++--- .../lib/tracker/model/pass_through_tracker.cpp | 4 +++- .../lib/tracker/model/pedestrian_and_bicycle_tracker.cpp | 7 ++++--- .../lib/tracker/model/pedestrian_tracker.cpp | 8 ++++++-- .../lib/tracker/model/tracker_base.cpp | 8 +++++--- .../lib/tracker/model/unknown_tracker.cpp | 4 +++- .../lib/tracker/model/vehicle_tracker.cpp | 8 ++++++-- 16 files changed, 62 insertions(+), 25 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 5bf5ed23da9e9..1e83d49a2fa64 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -43,7 +43,9 @@ class BicycleTracker : public Tracker BicycleTracker(const rclcpp::Time & time, const types::DynamicObject & object); bool predict(const rclcpp::Time & time) override; - bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override; + bool measure( + const types::DynamicObject & object, const rclcpp::Time & time, + const types::InputChannel & channel_info) override; bool measureWithPose(const types::DynamicObject & object); bool measureWithShape(const types::DynamicObject & object); bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index 82122f34101cd..5461adce00d43 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -39,7 +39,9 @@ class MultipleVehicleTracker : public Tracker MultipleVehicleTracker(const rclcpp::Time & time, const types::DynamicObject & object); bool predict(const rclcpp::Time & time) override; - bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override; + bool measure( + const types::DynamicObject & object, const rclcpp::Time & time, + const types::InputChannel & channel_info) override; bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; virtual ~MultipleVehicleTracker() {} }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp index d0dceabfaac25..db4635aca9cfb 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -37,7 +37,9 @@ class PassThroughTracker : public Tracker public: PassThroughTracker(const rclcpp::Time & time, const types::DynamicObject & object); bool predict(const rclcpp::Time & time) override; - bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override; + bool measure( + const types::DynamicObject & object, const rclcpp::Time & time, + const types::InputChannel & channel_info) override; bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index f0e741ddc1993..25abf9074bdee 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -39,7 +39,9 @@ class PedestrianAndBicycleTracker : public Tracker PedestrianAndBicycleTracker(const rclcpp::Time & time, const types::DynamicObject & object); bool predict(const rclcpp::Time & time) override; - bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override; + bool measure( + const types::DynamicObject & object, const rclcpp::Time & time, + const types::InputChannel & channel_info) override; bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; virtual ~PedestrianAndBicycleTracker() {} }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index 65495f8b5e108..b671080e0e3c8 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -44,7 +44,9 @@ class PedestrianTracker : public Tracker PedestrianTracker(const rclcpp::Time & time, const types::DynamicObject & object); bool predict(const rclcpp::Time & time) override; - bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override; + bool measure( + const types::DynamicObject & object, const rclcpp::Time & time, + const types::InputChannel & channel_info) override; bool measureWithPose(const types::DynamicObject & object); bool measureWithShape(const types::DynamicObject & object); bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp index 637969c139cf1..31934791ce435 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp @@ -87,7 +87,9 @@ class Tracker void limitObjectExtension(const object_model::ObjectModel object_model); // virtual functions - virtual bool measure(const types::DynamicObject & object, const rclcpp::Time & time) = 0; + virtual bool measure( + const types::DynamicObject & object, const rclcpp::Time & time, + const types::InputChannel & channel_info) = 0; public: virtual bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const = 0; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp index 5fb0a142ecf34..8573d392bb94b 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -43,7 +43,9 @@ class UnknownTracker : public Tracker UnknownTracker(const rclcpp::Time & time, const types::DynamicObject & object); bool predict(const rclcpp::Time & time) override; - bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override; + bool measure( + const types::DynamicObject & object, const rclcpp::Time & time, + const types::InputChannel & channel_info) override; bool measureWithPose(const types::DynamicObject & object); bool measureWithShape(const types::DynamicObject & object); bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp index 4c682158a4aec..9a9c97ee7f036 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp @@ -48,7 +48,9 @@ class VehicleTracker : public Tracker const types::DynamicObject & object); bool predict(const rclcpp::Time & time) override; - bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override; + bool measure( + const types::DynamicObject & object, const rclcpp::Time & time, + const types::InputChannel & channel_info) override; bool measureWithPose(const types::DynamicObject & object); bool measureWithShape(const types::DynamicObject & object); bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index ad7112360267c..449aecd0e0581 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -194,7 +194,9 @@ bool BicycleTracker::measureWithShape(const types::DynamicObject & object) return true; } -bool BicycleTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time) +bool BicycleTracker::measure( + const types::DynamicObject & object, const rclcpp::Time & time, + const types::InputChannel & channel_info) { // check time gap const double dt = motion_model_.getDeltaTime(time); @@ -208,7 +210,9 @@ bool BicycleTracker::measure(const types::DynamicObject & object, const rclcpp:: // update object measureWithPose(object); - measureWithShape(object); + if (channel_info.trust_extension) { + measureWithShape(object); + } return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp index 2803f65a84d29..bfed071544a30 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp @@ -37,10 +37,12 @@ bool MultipleVehicleTracker::predict(const rclcpp::Time & time) return true; } -bool MultipleVehicleTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time) +bool MultipleVehicleTracker::measure( + const types::DynamicObject & object, const rclcpp::Time & time, + const types::InputChannel & channel_info) { - big_vehicle_tracker_.measure(object, time); - normal_vehicle_tracker_.measure(object, time); + big_vehicle_tracker_.measure(object, time, channel_info); + normal_vehicle_tracker_.measure(object, time, channel_info); return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp index 27500ee942d7f..a77b103e745cf 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp @@ -52,7 +52,9 @@ bool PassThroughTracker::predict(const rclcpp::Time & time) return true; } -bool PassThroughTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time) +bool PassThroughTracker::measure( + const types::DynamicObject & object, const rclcpp::Time & time, + const types::InputChannel & /*channel_info*/) { prev_observed_object_ = object_; object_ = object; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp index 0c28205cc2958..c13f5878f88e6 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -34,10 +34,11 @@ bool PedestrianAndBicycleTracker::predict(const rclcpp::Time & time) } bool PedestrianAndBicycleTracker::measure( - const types::DynamicObject & object, const rclcpp::Time & time) + const types::DynamicObject & object, const rclcpp::Time & time, + const types::InputChannel & channel_info) { - pedestrian_tracker_.measure(object, time); - bicycle_tracker_.measure(object, time); + pedestrian_tracker_.measure(object, time, channel_info); + bicycle_tracker_.measure(object, time, channel_info); return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index 6543591bda22d..986a1c00f897b 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -169,7 +169,9 @@ bool PedestrianTracker::measureWithShape(const types::DynamicObject & object) return true; } -bool PedestrianTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time) +bool PedestrianTracker::measure( + const types::DynamicObject & object, const rclcpp::Time & time, + const types::InputChannel & channel_info) { // check time gap const double dt = motion_model_.getDeltaTime(time); @@ -183,7 +185,9 @@ bool PedestrianTracker::measure(const types::DynamicObject & object, const rclcp // update object measureWithPose(object); - measureWithShape(object); + if (channel_info.trust_extension) { + measureWithShape(object); + } return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp index 43bb1f2a2a64d..6412a73571ddc 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp @@ -29,7 +29,7 @@ float updateProbability( constexpr float min_updated_probability = 0.100; const float probability = (prior * true_positive) / (prior * true_positive + (1 - prior) * false_positive); - return std::max(std::min(probability, max_updated_probability), min_updated_probability); + return std::clamp(probability, min_updated_probability, max_updated_probability); } float decayProbability(const float & prior, const float & delta_time) { @@ -106,8 +106,10 @@ bool Tracker::updateWithMeasurement( } // update total existence probability + const double existence_probability = + channel_info.trust_existence_probability ? object.existence_probability : 0.6; total_existence_probability_ = updateProbability( - total_existence_probability_, object.existence_probability, probability_false_detection); + total_existence_probability_, existence_probability, probability_false_detection); } last_update_with_measurement_time_ = measurement_time; @@ -121,7 +123,7 @@ bool Tracker::updateWithMeasurement( } // Update object - measure(object, measurement_time); + measure(object, measurement_time, channel_info); return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp index f97411dc1dc9c..e313db4721bfc 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp @@ -140,7 +140,9 @@ bool UnknownTracker::measureWithPose(const types::DynamicObject & object) return is_updated; } -bool UnknownTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time) +bool UnknownTracker::measure( + const types::DynamicObject & object, const rclcpp::Time & time, + const types::InputChannel & /*channel_info*/) { // update object shape object_.shape = object.shape; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index 9183ce3168ca5..b253a9a5091a7 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -229,7 +229,9 @@ bool VehicleTracker::measureWithShape(const types::DynamicObject & object) return true; } -bool VehicleTracker::measure(const types::DynamicObject & in_object, const rclcpp::Time & time) +bool VehicleTracker::measure( + const types::DynamicObject & in_object, const rclcpp::Time & time, + const types::InputChannel & channel_info) { // check time gap const double dt = motion_model_.getDeltaTime(time); @@ -245,7 +247,9 @@ bool VehicleTracker::measure(const types::DynamicObject & in_object, const rclcp types::DynamicObject updating_object = in_object; shapes::calcAnchorPointOffset(object_, tracking_offset_, updating_object); measureWithPose(updating_object); - measureWithShape(updating_object); + if (channel_info.trust_extension) { + measureWithShape(updating_object); + } return true; } From ddb9da80794afae48cee6032e7494ed94fb04397 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 12 Mar 2025 15:20:02 +0900 Subject: [PATCH 04/11] refactor(multi_object_tracker): add updateStatePoseVel method to BicycleMotionModel and update measurement logic in VehicleTracker Signed-off-by: Taekjin LEE --- .../motion_model/bicycle_motion_model.hpp | 4 +++ .../lib/tracker/model/vehicle_tracker.cpp | 29 +++++++++++++------ .../motion_model/bicycle_motion_model.cpp | 29 +++++++++++++++++++ 3 files changed, 53 insertions(+), 9 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp index 821e470054f04..58da64da7a115 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp @@ -94,6 +94,10 @@ class BicycleMotionModel : public MotionModel const double & x, const double & y, const double & yaw, const std::array & pose_cov); + bool updateStatePoseVel( + const double & x, const double & y, const std::array & pose_cov, const double & vel, + const std::array & twist_cov); + bool updateStatePoseHeadVel( const double & x, const double & y, const double & yaw, const std::array & pose_cov, const double & vel, const std::array & twist_cov); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index b253a9a5091a7..b9ae672a1272c 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -135,22 +135,20 @@ VehicleTracker::VehicleTracker( bool VehicleTracker::predict(const rclcpp::Time & time) { - bool success = motion_model_.predictState(time); - if (!success) { - return false; - } - return true; + return motion_model_.predictState(time); } bool VehicleTracker::measureWithPose(const types::DynamicObject & object) { - // current (predicted) state - const double tracked_vel = motion_model_.getStateElement(IDX::VEL); + // get measurement yaw angle to update + bool is_yaw_available = + object.kinematics.orientation_availability != types::OrientationAvailability::UNAVAILABLE; // velocity capability is checked only when the object has velocity measurement // and the predicted velocity is close to the observed velocity bool is_velocity_available = false; if (object.kinematics.has_twist) { + const double tracked_vel = motion_model_.getStateElement(IDX::VEL); const double & observed_vel = object.twist.linear.x; if (std::fabs(tracked_vel - observed_vel) < velocity_deviation_threshold_) { // Velocity deviation is small @@ -166,11 +164,21 @@ bool VehicleTracker::measureWithPose(const types::DynamicObject & object) const double yaw = tf2::getYaw(object.pose.orientation); const double vel = object.twist.linear.x; - if (is_velocity_available) { + if (is_yaw_available && is_velocity_available) { + // update with yaw angle and velocity is_updated = motion_model_.updateStatePoseHeadVel( x, y, yaw, object.pose_covariance, vel, object.twist_covariance); - } else { + } else if (is_yaw_available && !is_velocity_available) { + // update with yaw angle, but without velocity is_updated = motion_model_.updateStatePoseHead(x, y, yaw, object.pose_covariance); + } else if (!is_yaw_available && is_velocity_available) { + // update without yaw angle, but with velocity + is_updated = motion_model_.updateStatePoseVel( + x, y, object.pose_covariance, vel, object.twist_covariance); + } else { + // update without yaw angle and velocity + is_updated = motion_model_.updateStatePose( + x, y, object.pose_covariance); // update without yaw angle and velocity } motion_model_.limitStates(); } @@ -206,6 +214,9 @@ bool VehicleTracker::measureWithShape(const types::DynamicObject & object) object_extension.y = gain_inv * object_extension.y + gain * object.shape.dimensions.y; object_extension.z = gain_inv * object_extension.z + gain * object.shape.dimensions.z; + // set shape type, which is bounding box + object_.shape.type = object.shape.type; + // set maximum and minimum size limitObjectExtension(object_model_); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp index ffac0980a742b..0940a5ce7aeed 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp @@ -176,6 +176,35 @@ bool BicycleMotionModel::updateStatePoseHead( return ekf_.update(Y, C, R); } +bool BicycleMotionModel::updateStatePoseVel( + const double & x, const double & y, const std::array & pose_cov, const double & vel, + const std::array & twist_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, with velocity + constexpr int DIM_Y = 4; + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y, vel; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + C(2, IDX::VEL) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[XYZRPY_COV_IDX::X_X]; + R(0, 1) = pose_cov[XYZRPY_COV_IDX::X_Y]; + R(1, 0) = pose_cov[XYZRPY_COV_IDX::Y_X]; + R(1, 1) = pose_cov[XYZRPY_COV_IDX::Y_Y]; + R(2, 2) = twist_cov[XYZRPY_COV_IDX::X_X]; + + return ekf_.update(Y, C, R); +} + bool BicycleMotionModel::updateStatePoseHeadVel( const double & x, const double & y, const double & yaw, const std::array & pose_cov, const double & vel, const std::array & twist_cov) From 3cf1faa0a48a6a5e01a095a8fe0a51983f41e1e8 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 12 Mar 2025 17:22:12 +0900 Subject: [PATCH 05/11] refactor(multi_object_tracker): update measureWithPose method to include InputChannel parameter and adjust related logic Signed-off-by: Taekjin LEE --- .../tracker/model/vehicle_tracker.hpp | 3 ++- .../lib/tracker/model/vehicle_tracker.cpp | 18 +++++++++++------- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp index 9a9c97ee7f036..8b1737efd0b12 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp @@ -51,7 +51,8 @@ class VehicleTracker : public Tracker bool measure( const types::DynamicObject & object, const rclcpp::Time & time, const types::InputChannel & channel_info) override; - bool measureWithPose(const types::DynamicObject & object); + bool measureWithPose( + const types::DynamicObject & object, const types::InputChannel & channel_info); bool measureWithShape(const types::DynamicObject & object); bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; }; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index b9ae672a1272c..b60861d0171cf 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -138,11 +138,13 @@ bool VehicleTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -bool VehicleTracker::measureWithPose(const types::DynamicObject & object) +bool VehicleTracker::measureWithPose( + const types::DynamicObject & object, const types::InputChannel & channel_info) { // get measurement yaw angle to update bool is_yaw_available = - object.kinematics.orientation_availability != types::OrientationAvailability::UNAVAILABLE; + object.kinematics.orientation_availability != types::OrientationAvailability::UNAVAILABLE && + channel_info.trust_orientation; // velocity capability is checked only when the object has velocity measurement // and the predicted velocity is close to the observed velocity @@ -198,11 +200,13 @@ bool VehicleTracker::measureWithShape(const types::DynamicObject & object) } // check object size abnormality - constexpr double size_max = 35.0; // [m] - constexpr double size_min = 1.0; // [m] + constexpr double size_max_multiplier = 1.5; + constexpr double size_min_multiplier = 0.25; if ( - object.shape.dimensions.x > size_max || object.shape.dimensions.x < size_min || - object.shape.dimensions.y > size_max || object.shape.dimensions.y < size_min) { + object.shape.dimensions.x > object_model_.size_limit.length_max * size_max_multiplier || + object.shape.dimensions.x < object_model_.size_limit.length_min * size_min_multiplier || + object.shape.dimensions.y > object_model_.size_limit.width_max * size_max_multiplier || + object.shape.dimensions.y < object_model_.size_limit.width_min * size_min_multiplier) { return false; } @@ -257,7 +261,7 @@ bool VehicleTracker::measure( // update object types::DynamicObject updating_object = in_object; shapes::calcAnchorPointOffset(object_, tracking_offset_, updating_object); - measureWithPose(updating_object); + measureWithPose(updating_object, channel_info); if (channel_info.trust_extension) { measureWithShape(updating_object); } From 0369bca0f2167965627450288d8062cdd85c0801 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 12 Mar 2025 17:57:16 +0900 Subject: [PATCH 06/11] refactor(multi_object_tracker): remove BicycleTracker and update references to use VehicleTracker Signed-off-by: Taekjin LEE --- .../tracker/model/bicycle_tracker.hpp | 3 +- .../lib/tracker/model/bicycle_tracker.cpp | 55 ++++++++++++++----- 2 files changed, 44 insertions(+), 14 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 1e83d49a2fa64..e0ea05a0aa69b 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -32,9 +32,10 @@ namespace autoware::multi_object_tracker class BicycleTracker : public Tracker { private: + object_model::ObjectModel object_model_ = object_model::bicycle; rclcpp::Logger logger_; - object_model::ObjectModel object_model_ = object_model::bicycle; + double velocity_deviation_threshold_; BicycleMotionModel motion_model_; using IDX = BicycleMotionModel::IDX; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index 449aecd0e0581..dd7c13b0ef211 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -43,6 +43,11 @@ namespace autoware::multi_object_tracker BicycleTracker::BicycleTracker(const rclcpp::Time & time, const types::DynamicObject & object) : Tracker(time, object), logger_(rclcpp::get_logger("BicycleTracker")) { + // velocity deviation threshold + // if the predicted velocity is close to the observed velocity, + // the observed velocity is used as the measurement. + velocity_deviation_threshold_ = autoware_utils::kmph2mps(10); // [m/s] + if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { // set default initial size auto & object_extension = object_.shape.dimensions; @@ -131,21 +136,44 @@ bool BicycleTracker::predict(const rclcpp::Time & time) bool BicycleTracker::measureWithPose(const types::DynamicObject & object) { // get measurement yaw angle to update - const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); - double measurement_yaw = 0.0; - bool is_yaw_available = shapes::getMeasurementYaw(object, tracked_yaw, measurement_yaw); + bool is_yaw_available = + object.kinematics.orientation_availability != types::OrientationAvailability::UNAVAILABLE; + + // velocity capability is checked only when the object has velocity measurement + // and the predicted velocity is close to the observed velocity + bool is_velocity_available = false; + if (object.kinematics.has_twist) { + const double tracked_vel = motion_model_.getStateElement(IDX::VEL); + const double & observed_vel = object.twist.linear.x; + if (std::fabs(tracked_vel - observed_vel) < velocity_deviation_threshold_) { + // Velocity deviation is small + is_velocity_available = true; + } + } // update bool is_updated = false; { const double x = object.pose.position.x; const double y = object.pose.position.y; - const double yaw = measurement_yaw; - - if (is_yaw_available) { + const double yaw = tf2::getYaw(object.pose.orientation); + const double vel = object.twist.linear.x; + + if (is_yaw_available && is_velocity_available) { + // update with yaw angle and velocity + is_updated = motion_model_.updateStatePoseHeadVel( + x, y, yaw, object.pose_covariance, vel, object.twist_covariance); + } else if (is_yaw_available && !is_velocity_available) { + // update with yaw angle, but without velocity is_updated = motion_model_.updateStatePoseHead(x, y, yaw, object.pose_covariance); + } else if (!is_yaw_available && is_velocity_available) { + // update without yaw angle, but with velocity + is_updated = motion_model_.updateStatePoseVel( + x, y, object.pose_covariance, vel, object.twist_covariance); } else { - is_updated = motion_model_.updateStatePose(x, y, object.pose_covariance); + // update without yaw angle and velocity + is_updated = motion_model_.updateStatePose( + x, y, object.pose_covariance); // update without yaw angle and velocity } motion_model_.limitStates(); } @@ -164,13 +192,14 @@ bool BicycleTracker::measureWithShape(const types::DynamicObject & object) return false; } - // check bound box size abnormality - constexpr double size_max = 30.0; // [m] - constexpr double size_min = 0.1; // [m] + // check object size abnormality + constexpr double size_max_multiplier = 1.5; + constexpr double size_min_multiplier = 0.25; if ( - object.shape.dimensions.x > size_max || object.shape.dimensions.x < size_min || - object.shape.dimensions.y > size_max || object.shape.dimensions.y < size_min || - object.shape.dimensions.z > size_max || object.shape.dimensions.z < size_min) { + object.shape.dimensions.x > object_model_.size_limit.length_max * size_max_multiplier || + object.shape.dimensions.x < object_model_.size_limit.length_min * size_min_multiplier || + object.shape.dimensions.y > object_model_.size_limit.width_max * size_max_multiplier || + object.shape.dimensions.y < object_model_.size_limit.width_min * size_min_multiplier) { return false; } From 603eef82c9c2ca8de4b57405117d25df10ab2a9a Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 12 Mar 2025 18:23:47 +0900 Subject: [PATCH 07/11] refactor(bicycle_tracker): add tracking_offset to adjust object position based on motion model Signed-off-by: Taekjin LEE --- .../tracker/model/bicycle_tracker.hpp | 2 ++ .../lib/tracker/model/bicycle_tracker.cpp | 25 ++++++++++++++++--- 2 files changed, 23 insertions(+), 4 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp index e0ea05a0aa69b..5b87addefe24b 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -37,6 +37,8 @@ class BicycleTracker : public Tracker double velocity_deviation_threshold_; + Eigen::Vector2d tracking_offset_; + BicycleMotionModel motion_model_; using IDX = BicycleMotionModel::IDX; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index dd7c13b0ef211..4e872e0647e28 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -41,7 +41,8 @@ namespace autoware::multi_object_tracker { BicycleTracker::BicycleTracker(const rclcpp::Time & time, const types::DynamicObject & object) -: Tracker(time, object), logger_(rclcpp::get_logger("BicycleTracker")) +: Tracker(time, object), logger_(rclcpp::get_logger("BicycleTracker")), +tracking_offset_(Eigen::Vector2d::Zero()) { // velocity deviation threshold // if the predicted velocity is close to the observed velocity, @@ -220,11 +221,25 @@ bool BicycleTracker::measureWithShape(const types::DynamicObject & object) // update motion model motion_model_.updateExtendedState(object_extension.x); + // update offset into object position + { + // rotate back the offset vector from object coordinate to global coordinate + const double yaw = motion_model_.getStateElement(IDX::YAW); + const double offset_x_global = + tracking_offset_.x() * std::cos(yaw) - tracking_offset_.y() * std::sin(yaw); + const double offset_y_global = + tracking_offset_.x() * std::sin(yaw) + tracking_offset_.y() * std::cos(yaw); + motion_model_.adjustPosition(-gain * offset_x_global, -gain * offset_y_global); + // update offset (object coordinate) + tracking_offset_.x() = gain_inv * tracking_offset_.x(); + tracking_offset_.y() = gain_inv * tracking_offset_.y(); + } + return true; } bool BicycleTracker::measure( - const types::DynamicObject & object, const rclcpp::Time & time, + const types::DynamicObject & in_object, const rclcpp::Time & time, const types::InputChannel & channel_info) { // check time gap @@ -238,9 +253,11 @@ bool BicycleTracker::measure( } // update object - measureWithPose(object); + types::DynamicObject updating_object = in_object; + shapes::calcAnchorPointOffset(object_, tracking_offset_, updating_object); + measureWithPose(updating_object); if (channel_info.trust_extension) { - measureWithShape(object); + measureWithShape(updating_object); } return true; From 48fea9314bd993f78f58c9c856bd9f6861a154b6 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 12 Mar 2025 19:09:16 +0900 Subject: [PATCH 08/11] refactor(multi_object_tracker): remove BicycleTracker and replace with VehicleTracker in relevant classes Signed-off-by: Taekjin LEE --- .../CMakeLists.txt | 1 - .../tracker/model/bicycle_tracker.hpp | 59 ---- .../model/pedestrian_and_bicycle_tracker.hpp | 4 +- .../multi_object_tracker/tracker/tracker.hpp | 1 - .../lib/tracker/model/bicycle_tracker.cpp | 290 ------------------ .../model/pedestrian_and_bicycle_tracker.cpp | 4 +- .../lib/tracker/model/vehicle_tracker.cpp | 2 +- .../src/processor/processor.cpp | 3 +- 8 files changed, 8 insertions(+), 356 deletions(-) delete mode 100644 perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp delete mode 100644 perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp diff --git a/perception/autoware_multi_object_tracker/CMakeLists.txt b/perception/autoware_multi_object_tracker/CMakeLists.txt index 26895428d4c7f..621d1a6bdc2b4 100644 --- a/perception/autoware_multi_object_tracker/CMakeLists.txt +++ b/perception/autoware_multi_object_tracker/CMakeLists.txt @@ -41,7 +41,6 @@ set(${PROJECT_NAME}_lib lib/tracker/model/tracker_base.cpp lib/tracker/model/vehicle_tracker.cpp lib/tracker/model/multiple_vehicle_tracker.cpp - lib/tracker/model/bicycle_tracker.cpp lib/tracker/model/pedestrian_tracker.cpp lib/tracker/model/pedestrian_and_bicycle_tracker.cpp lib/tracker/model/unknown_tracker.cpp diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp deleted file mode 100644 index 5b87addefe24b..0000000000000 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ /dev/null @@ -1,59 +0,0 @@ -// Copyright 2020 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. -// -// -// Author: v1.0 Yukihiro Saito -// - -#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ -#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ - -#include "autoware/multi_object_tracker/object_model/object_model.hpp" -#include "autoware/multi_object_tracker/object_model/types.hpp" -#include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" -#include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" - -#include - -namespace autoware::multi_object_tracker -{ - -class BicycleTracker : public Tracker -{ -private: - object_model::ObjectModel object_model_ = object_model::bicycle; - rclcpp::Logger logger_; - - double velocity_deviation_threshold_; - - Eigen::Vector2d tracking_offset_; - - BicycleMotionModel motion_model_; - using IDX = BicycleMotionModel::IDX; - -public: - BicycleTracker(const rclcpp::Time & time, const types::DynamicObject & object); - - bool predict(const rclcpp::Time & time) override; - bool measure( - const types::DynamicObject & object, const rclcpp::Time & time, - const types::InputChannel & channel_info) override; - bool measureWithPose(const types::DynamicObject & object); - bool measureWithShape(const types::DynamicObject & object); - bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; -}; - -} // namespace autoware::multi_object_tracker - -#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index 25abf9074bdee..f51f28cbe5e21 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -20,9 +20,9 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ #include "autoware/multi_object_tracker/object_model/types.hpp" -#include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" #include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include "autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp" #include @@ -33,7 +33,7 @@ class PedestrianAndBicycleTracker : public Tracker { private: PedestrianTracker pedestrian_tracker_; - BicycleTracker bicycle_tracker_; + VehicleTracker bicycle_tracker_; public: PedestrianAndBicycleTracker(const rclcpp::Time & time, const types::DynamicObject & object); diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp index ecf727c36936c..27ad3af9e4b13 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp @@ -19,7 +19,6 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ -#include "model/bicycle_tracker.hpp" #include "model/multiple_vehicle_tracker.hpp" #include "model/pass_through_tracker.hpp" #include "model/pedestrian_and_bicycle_tracker.hpp" diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp deleted file mode 100644 index 4e872e0647e28..0000000000000 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ /dev/null @@ -1,290 +0,0 @@ -// Copyright 2020 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. -// -// -// Author: v1.0 Yukihiro Saito -// -#define EIGEN_MPL2_ONLY - -#include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" - -#include "autoware/multi_object_tracker/object_model/shapes.hpp" - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -namespace autoware::multi_object_tracker -{ -BicycleTracker::BicycleTracker(const rclcpp::Time & time, const types::DynamicObject & object) -: Tracker(time, object), logger_(rclcpp::get_logger("BicycleTracker")), -tracking_offset_(Eigen::Vector2d::Zero()) -{ - // velocity deviation threshold - // if the predicted velocity is close to the observed velocity, - // the observed velocity is used as the measurement. - velocity_deviation_threshold_ = autoware_utils::kmph2mps(10); // [m/s] - - if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - // set default initial size - auto & object_extension = object_.shape.dimensions; - object_extension.x = object_model_.init_size.length; - object_extension.y = object_model_.init_size.width; - object_extension.z = object_model_.init_size.height; - } - object_.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; - - // set maximum and minimum size - limitObjectExtension(object_model_); - - // Set motion model parameters - { - const double q_stddev_acc_long = object_model_.process_noise.acc_long; - const double q_stddev_acc_lat = object_model_.process_noise.acc_lat; - const double q_stddev_yaw_rate_min = object_model_.process_noise.yaw_rate_min; - const double q_stddev_yaw_rate_max = object_model_.process_noise.yaw_rate_max; - const double q_stddev_slip_rate_min = object_model_.bicycle_state.slip_rate_stddev_min; - const double q_stddev_slip_rate_max = object_model_.bicycle_state.slip_rate_stddev_max; - const double q_max_slip_angle = object_model_.bicycle_state.slip_angle_max; - const double lf_ratio = object_model_.bicycle_state.wheel_pos_ratio_front; - const double lf_min = object_model_.bicycle_state.wheel_pos_front_min; - const double lr_ratio = object_model_.bicycle_state.wheel_pos_ratio_rear; - const double lr_min = object_model_.bicycle_state.wheel_pos_rear_min; - motion_model_.setMotionParams( - q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, - q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, - lr_min); - } - - // Set motion limits - { - const double max_vel = object_model_.process_limit.vel_long_max; - const double max_slip = object_model_.bicycle_state.slip_angle_max; - motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle - } - - // Set initial state - { - using autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - const double x = object.pose.position.x; - const double y = object.pose.position.y; - const double yaw = tf2::getYaw(object.pose.orientation); - - auto pose_cov = object.pose_covariance; - if (!object.kinematics.has_position_covariance) { - // initial state covariance - const auto & p0_cov_x = object_model_.initial_covariance.pos_x; - const auto & p0_cov_y = object_model_.initial_covariance.pos_y; - const auto & p0_cov_yaw = object_model_.initial_covariance.yaw; - - const double cos_yaw = std::cos(yaw); - const double sin_yaw = std::sin(yaw); - const double sin_2yaw = std::sin(2.0 * yaw); - pose_cov[XYZRPY_COV_IDX::X_X] = p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; - pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; - pose_cov[XYZRPY_COV_IDX::Y_Y] = p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; - pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; - pose_cov[XYZRPY_COV_IDX::YAW_YAW] = p0_cov_yaw; - } - - double vel = 0.0; - double vel_cov = object_model_.initial_covariance.vel_long; - if (object.kinematics.has_twist) { - vel = object.twist.linear.x; - } - if (object.kinematics.has_twist_covariance) { - vel_cov = object.twist_covariance[XYZRPY_COV_IDX::X_X]; - } - - const double slip = 0.0; - const double slip_cov = object_model_.bicycle_state.init_slip_angle_cov; - const double & length = object_.shape.dimensions.x; - - // initialize motion model - motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); - } -} - -bool BicycleTracker::predict(const rclcpp::Time & time) -{ - return motion_model_.predictState(time); -} - -bool BicycleTracker::measureWithPose(const types::DynamicObject & object) -{ - // get measurement yaw angle to update - bool is_yaw_available = - object.kinematics.orientation_availability != types::OrientationAvailability::UNAVAILABLE; - - // velocity capability is checked only when the object has velocity measurement - // and the predicted velocity is close to the observed velocity - bool is_velocity_available = false; - if (object.kinematics.has_twist) { - const double tracked_vel = motion_model_.getStateElement(IDX::VEL); - const double & observed_vel = object.twist.linear.x; - if (std::fabs(tracked_vel - observed_vel) < velocity_deviation_threshold_) { - // Velocity deviation is small - is_velocity_available = true; - } - } - - // update - bool is_updated = false; - { - const double x = object.pose.position.x; - const double y = object.pose.position.y; - const double yaw = tf2::getYaw(object.pose.orientation); - const double vel = object.twist.linear.x; - - if (is_yaw_available && is_velocity_available) { - // update with yaw angle and velocity - is_updated = motion_model_.updateStatePoseHeadVel( - x, y, yaw, object.pose_covariance, vel, object.twist_covariance); - } else if (is_yaw_available && !is_velocity_available) { - // update with yaw angle, but without velocity - is_updated = motion_model_.updateStatePoseHead(x, y, yaw, object.pose_covariance); - } else if (!is_yaw_available && is_velocity_available) { - // update without yaw angle, but with velocity - is_updated = motion_model_.updateStatePoseVel( - x, y, object.pose_covariance, vel, object.twist_covariance); - } else { - // update without yaw angle and velocity - is_updated = motion_model_.updateStatePose( - x, y, object.pose_covariance); // update without yaw angle and velocity - } - motion_model_.limitStates(); - } - - // position z - constexpr double gain = 0.1; - object_.pose.position.z = (1.0 - gain) * object_.pose.position.z + gain * object.pose.position.z; - - return is_updated; -} - -bool BicycleTracker::measureWithShape(const types::DynamicObject & object) -{ - if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - // do not update shape if the input is not a bounding box - return false; - } - - // check object size abnormality - constexpr double size_max_multiplier = 1.5; - constexpr double size_min_multiplier = 0.25; - if ( - object.shape.dimensions.x > object_model_.size_limit.length_max * size_max_multiplier || - object.shape.dimensions.x < object_model_.size_limit.length_min * size_min_multiplier || - object.shape.dimensions.y > object_model_.size_limit.width_max * size_max_multiplier || - object.shape.dimensions.y < object_model_.size_limit.width_min * size_min_multiplier) { - return false; - } - - // update object size - constexpr double gain = 0.1; - constexpr double gain_inv = 1.0 - gain; - auto & object_extension = object_.shape.dimensions; - object_extension.x = gain_inv * object_extension.x + gain * object.shape.dimensions.x; - object_extension.y = gain_inv * object_extension.y + gain * object.shape.dimensions.y; - object_extension.z = gain_inv * object_extension.z + gain * object.shape.dimensions.z; - - // set shape type, which is bounding box - object_.shape.type = object.shape.type; - - // set maximum and minimum size - limitObjectExtension(object_model_); - - // update motion model - motion_model_.updateExtendedState(object_extension.x); - - // update offset into object position - { - // rotate back the offset vector from object coordinate to global coordinate - const double yaw = motion_model_.getStateElement(IDX::YAW); - const double offset_x_global = - tracking_offset_.x() * std::cos(yaw) - tracking_offset_.y() * std::sin(yaw); - const double offset_y_global = - tracking_offset_.x() * std::sin(yaw) + tracking_offset_.y() * std::cos(yaw); - motion_model_.adjustPosition(-gain * offset_x_global, -gain * offset_y_global); - // update offset (object coordinate) - tracking_offset_.x() = gain_inv * tracking_offset_.x(); - tracking_offset_.y() = gain_inv * tracking_offset_.y(); - } - - return true; -} - -bool BicycleTracker::measure( - const types::DynamicObject & in_object, const rclcpp::Time & time, - const types::InputChannel & channel_info) -{ - // check time gap - const double dt = motion_model_.getDeltaTime(time); - if (0.01 /*10msec*/ < dt) { - RCLCPP_WARN( - logger_, - "BicycleTracker::measure There is a large gap between predicted time and measurement time. " - "(%f)", - dt); - } - - // update object - types::DynamicObject updating_object = in_object; - shapes::calcAnchorPointOffset(object_, tracking_offset_, updating_object); - measureWithPose(updating_object); - if (channel_info.trust_extension) { - measureWithShape(updating_object); - } - - return true; -} - -bool BicycleTracker::getTrackedObject( - const rclcpp::Time & time, types::DynamicObject & object) const -{ - object = object_; - - // predict from motion model - auto & pose = object.pose; - auto & pose_cov = object.pose_covariance; - auto & twist = object.twist; - auto & twist_cov = object.twist_covariance; - if (!motion_model_.getPredictedState(time, pose, pose_cov, twist, twist_cov)) { - RCLCPP_WARN(logger_, "BicycleTracker::getTrackedObject: Failed to get predicted state."); - return false; - } - - // set shape - const auto origin_yaw = tf2::getYaw(object_.pose.orientation); - const auto ekf_pose_yaw = tf2::getYaw(pose.orientation); - object.shape.footprint = - autoware_utils::rotate_polygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); - - return true; -} - -} // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp index c13f5878f88e6..8476f7c607e94 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -22,7 +22,9 @@ namespace autoware::multi_object_tracker { PedestrianAndBicycleTracker::PedestrianAndBicycleTracker( const rclcpp::Time & time, const types::DynamicObject & object) -: Tracker(time, object), pedestrian_tracker_(time, object), bicycle_tracker_(time, object) +: Tracker(time, object), + pedestrian_tracker_(time, object), + bicycle_tracker_(object_model::bicycle, time, object) { } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index b60861d0171cf..9f1118de63450 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -211,7 +211,7 @@ bool VehicleTracker::measureWithShape(const types::DynamicObject & object) } // update object size - constexpr double gain = 0.5; + constexpr double gain = 0.4; constexpr double gain_inv = 1.0 - gain; auto & object_extension = object_.shape.dimensions; object_extension.x = gain_inv * object_extension.x + gain * object.shape.dimensions.x; diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index 092d0aec0eafa..920b0b1d15994 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -124,7 +124,8 @@ std::shared_ptr TrackerProcessor::createNewTracker( autoware::object_recognition_utils::getHighestProbLabel(object.classification); if (config_.tracker_map.count(label) != 0) { const auto tracker = config_.tracker_map.at(label); - if (tracker == "bicycle_tracker") return std::make_shared(time, object); + if (tracker == "bicycle_tracker") + return std::make_shared(object_model::bicycle, time, object); if (tracker == "big_vehicle_tracker") return std::make_shared(object_model::big_vehicle, time, object); if (tracker == "multi_vehicle_tracker") From 1a1b9efcdffd511a9247921b67dc019648cc7492 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 17 Mar 2025 17:36:21 +0900 Subject: [PATCH 09/11] refactor(input_channels): disable trust flags for extension and orientation in radar configurations Signed-off-by: Taekjin LEE --- .../config/input_channels.param.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/perception/autoware_multi_object_tracker/config/input_channels.param.yaml b/perception/autoware_multi_object_tracker/config/input_channels.param.yaml index aaa55e2083b98..7dc1c10948537 100644 --- a/perception/autoware_multi_object_tracker/config/input_channels.param.yaml +++ b/perception/autoware_multi_object_tracker/config/input_channels.param.yaml @@ -117,9 +117,9 @@ flags: can_spawn_new_tracker: true can_trust_existence_probability: true - can_trust_extension: true + can_trust_extension: false can_trust_classification: true - can_trust_orientation: true + can_trust_orientation: false optional: name: "radar" short_name: "R" @@ -128,9 +128,9 @@ flags: can_spawn_new_tracker: true can_trust_existence_probability: true - can_trust_extension: true + can_trust_extension: false can_trust_classification: true - can_trust_orientation: true + can_trust_orientation: false optional: name: "radar_far" short_name: "Rf" From c89ffbab8c55d9ec7ca591420cc3cf90751b96f8 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 19 Mar 2025 14:23:41 +0900 Subject: [PATCH 10/11] refactor(input_channels): restructure flags for input channel properties Signed-off-by: Taekjin LEE --- .../schema/input_channels.schema.json | 131 +++++++++++++++--- 1 file changed, 114 insertions(+), 17 deletions(-) diff --git a/perception/autoware_multi_object_tracker/schema/input_channels.schema.json b/perception/autoware_multi_object_tracker/schema/input_channels.schema.json index 6a35ca4f0e3bc..29d54d3370cd2 100644 --- a/perception/autoware_multi_object_tracker/schema/input_channels.schema.json +++ b/perception/autoware_multi_object_tracker/schema/input_channels.schema.json @@ -11,10 +11,35 @@ "description": "The ROS topic name for the input channel.", "default": "/perception/object_recognition/detection/objects" }, - "can_spawn_new_tracker": { - "type": "boolean", - "description": "Indicates if the input channel can spawn new trackers.", - "default": true + "flags": { + "type": "object", + "properties": { + "can_spawn_new_tracker": { + "type": "boolean", + "description": "Indicates if the input channel can spawn new trackers.", + "default": true + }, + "can_trust_existence_probability": { + "type": "boolean", + "description": "Indicates if the input channel can trust the existence probability.", + "default": true + }, + "can_trust_extension": { + "type": "boolean", + "description": "Indicates if the input channel can trust the object size(extension).", + "default": true + }, + "can_trust_classification": { + "type": "boolean", + "description": "Indicates if the input channel can trust the object classification.", + "default": true + }, + "can_trust_orientation": { + "type": "boolean", + "description": "Indicates if the input channel can trust the object orientation.", + "default": true + } + } }, "optional": { "type": "object", @@ -32,7 +57,7 @@ } } }, - "required": ["topic", "can_spawn_new_tracker"] + "required": ["topic", "flags"] } }, "properties": { @@ -49,7 +74,13 @@ "$ref": "#/definitions/input_channel", "default": { "topic": "/perception/object_recognition/detection/objects", - "can_spawn_new_tracker": true, + "flags": { + "can_spawn_new_tracker": true, + "can_trust_existence_probability": true, + "can_trust_extension": true, + "can_trust_classification": true, + "can_trust_orientation": true + }, "optional": { "name": "detected_objects", "short_name": "all" @@ -60,7 +91,13 @@ "$ref": "#/definitions/input_channel", "default": { "topic": "/perception/object_recognition/detection/clustering/objects", - "can_spawn_new_tracker": true, + "flags": { + "can_spawn_new_tracker": true, + "can_trust_existence_probability": false, + "can_trust_extension": false, + "can_trust_classification": false, + "can_trust_orientation": false + }, "optional": { "name": "clustering", "short_name": "Lcl" @@ -71,7 +108,13 @@ "$ref": "#/definitions/input_channel", "default": { "topic": "/perception/object_recognition/detection/centerpoint/objects", - "can_spawn_new_tracker": true, + "flags": { + "can_spawn_new_tracker": true, + "can_trust_existence_probability": true, + "can_trust_extension": true, + "can_trust_classification": true, + "can_trust_orientation": true + }, "optional": { "name": "centerpoint", "short_name": "Lcp" @@ -82,7 +125,13 @@ "$ref": "#/definitions/input_channel", "default": { "topic": "/perception/object_recognition/detection/centerpoint/validation/objects", - "can_spawn_new_tracker": true, + "flags": { + "can_spawn_new_tracker": true, + "can_trust_existence_probability": true, + "can_trust_extension": true, + "can_trust_classification": true, + "can_trust_orientation": true + }, "optional": { "name": "centerpoint", "short_name": "Lcp" @@ -93,7 +142,13 @@ "$ref": "#/definitions/input_channel", "default": { "topic": "/perception/object_recognition/detection/apollo/objects", - "can_spawn_new_tracker": true, + "flags": { + "can_spawn_new_tracker": true, + "can_trust_existence_probability": true, + "can_trust_extension": true, + "can_trust_classification": true, + "can_trust_orientation": true + }, "optional": { "name": "apollo", "short_name": "Lap" @@ -104,7 +159,13 @@ "$ref": "#/definitions/input_channel", "default": { "topic": "/perception/object_recognition/detection/apollo/validation/objects", - "can_spawn_new_tracker": true, + "flags": { + "can_spawn_new_tracker": true, + "can_trust_existence_probability": true, + "can_trust_extension": true, + "can_trust_classification": true, + "can_trust_orientation": true + }, "optional": { "name": "apollo", "short_name": "Lap" @@ -115,7 +176,13 @@ "$ref": "#/definitions/input_channel", "default": { "topic": "/perception/object_recognition/detection/pointpainting/objects", - "can_spawn_new_tracker": true, + "flags": { + "can_spawn_new_tracker": true, + "can_trust_existence_probability": true, + "can_trust_extension": true, + "can_trust_classification": true, + "can_trust_orientation": true + }, "optional": { "name": "pointpainting", "short_name": "Lpp" @@ -126,7 +193,13 @@ "$ref": "#/definitions/input_channel", "default": { "topic": "/perception/object_recognition/detection/pointpainting/validation/objects", - "can_spawn_new_tracker": true, + "flags": { + "can_spawn_new_tracker": true, + "can_trust_existence_probability": true, + "can_trust_extension": true, + "can_trust_classification": true, + "can_trust_orientation": true + }, "optional": { "name": "pointpainting", "short_name": "Lpp" @@ -137,7 +210,13 @@ "$ref": "#/definitions/input_channel", "default": { "topic": "/perception/object_recognition/detection/clustering/camera_lidar_fusion/objects", - "can_spawn_new_tracker": true, + "flags": { + "can_spawn_new_tracker": true, + "can_trust_existence_probability": false, + "can_trust_extension": false, + "can_trust_classification": true, + "can_trust_orientation": false + }, "optional": { "name": "camera_lidar_fusion", "short_name": "CLf" @@ -148,7 +227,13 @@ "$ref": "#/definitions/input_channel", "default": { "topic": "/perception/object_recognition/detection/detection_by_tracker/objects", - "can_spawn_new_tracker": false, + "flags": { + "can_spawn_new_tracker": false, + "can_trust_existence_probability": false, + "can_trust_extension": false, + "can_trust_classification": false, + "can_trust_orientation": false + }, "optional": { "name": "detection_by_tracker", "short_name": "dbT" @@ -159,7 +244,13 @@ "$ref": "#/definitions/input_channel", "default": { "topic": "/sensing/radar/detected_objects", - "can_spawn_new_tracker": true, + "flags": { + "can_spawn_new_tracker": true, + "can_trust_existence_probability": true, + "can_trust_extension": false, + "can_trust_classification": true, + "can_trust_orientation": false + }, "optional": { "name": "radar", "short_name": "R" @@ -170,7 +261,13 @@ "$ref": "#/definitions/input_channel", "default": { "topic": "/perception/object_recognition/detection/radar/far_objects", - "can_spawn_new_tracker": true, + "flags": { + "can_spawn_new_tracker": true, + "can_trust_existence_probability": true, + "can_trust_extension": false, + "can_trust_classification": true, + "can_trust_orientation": false + }, "optional": { "name": "radar_far", "short_name": "Rf" From 430e93636244d6273062c8087affa80616a5487c Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 19 Mar 2025 14:26:37 +0900 Subject: [PATCH 11/11] refactor(input_channels): remove 'flags' from required properties in schema Signed-off-by: Taekjin LEE --- .../schema/input_channels.schema.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/autoware_multi_object_tracker/schema/input_channels.schema.json b/perception/autoware_multi_object_tracker/schema/input_channels.schema.json index 29d54d3370cd2..8db06c5449795 100644 --- a/perception/autoware_multi_object_tracker/schema/input_channels.schema.json +++ b/perception/autoware_multi_object_tracker/schema/input_channels.schema.json @@ -57,7 +57,7 @@ } } }, - "required": ["topic", "flags"] + "required": ["topic"] } }, "properties": {