From 067156abeec1fb111f50048b00b0b0f3373ee076 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 24 Jun 2024 08:52:34 +0900 Subject: [PATCH 1/8] feat(multi_object_tracker): tracker refactoring (#7271) * feat: separate filters Signed-off-by: Taekjin LEE * fix: object validator to modular Signed-off-by: Taekjin LEE * fix: replace xyzrpy covariance index Signed-off-by: Taekjin LEE * chore: refactor tracker class members Signed-off-by: Taekjin LEE * chore: refactor Signed-off-by: Taekjin LEE * chore: refactoring destructor Signed-off-by: Taekjin LEE * chore: refactoring getUpdatingObject Signed-off-by: Taekjin LEE * chore: refactoring object size checker Signed-off-by: Taekjin LEE * fix: debugger delay calculation order is fixed Signed-off-by: Taekjin LEE * fix: bicycle size update bug fix Signed-off-by: Taekjin LEE * fix: renamed message package Signed-off-by: Taekjin LEE * chore: refactoring Signed-off-by: Taekjin LEE * feat: object parameter class Signed-off-by: Taekjin LEE * style(pre-commit): autofix Signed-off-by: Taekjin LEE * chore: refactor includes Signed-off-by: Taekjin LEE * feat: initial impl. of object model Signed-off-by: Taekjin LEE * feat: impl. object model to bicycle, big, normal Signed-off-by: Taekjin LEE * feat: impl. object model to pedestrian Signed-off-by: Taekjin LEE * feat: impl. cont Signed-off-by: Taekjin LEE * style(pre-commit): autofix Signed-off-by: Taekjin LEE * chore: fix missing refactoring Signed-off-by: Taekjin LEE * fix: align unit of angles, yaw rates Signed-off-by: Taekjin LEE * fix: rename tier4_autoware_utils to autoware_universe_utils Signed-off-by: Taekjin LEE * fix: missing unit conversion Signed-off-by: Taekjin LEE * style(pre-commit): autofix Signed-off-by: Taekjin LEE * fix: sources Signed-off-by: Taekjin LEE * chore: Update include and import statements Signed-off-by: Taekjin LEE * fix: unit convert bug Signed-off-by: Taekjin LEE * fix: update to autoware::universe_utils Signed-off-by: Taekjin LEE * fix: mis-implementation of process noise Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Signed-off-by: Taekjin LEE --- .../data_association/data_association.hpp | 13 +- .../debugger/debug_object.hpp | 8 +- .../debugger/debugger.hpp | 8 +- .../multi_object_tracker_core.hpp | 4 +- .../processor/processor.hpp | 4 +- .../tracker/model/bicycle_tracker.hpp | 23 +- .../tracker/model/big_vehicle_tracker.hpp | 25 +- .../model/multiple_vehicle_tracker.hpp | 2 +- .../tracker/model/normal_vehicle_tracker.hpp | 25 +- .../tracker/model/pass_through_tracker.hpp | 4 +- .../model/pedestrian_and_bicycle_tracker.hpp | 3 +- .../tracker/model/pedestrian_tracker.hpp | 23 +- .../tracker/model/tracker_base.hpp | 6 +- .../tracker/model/unknown_tracker.hpp | 7 +- .../motion_model/bicycle_motion_model.hpp | 4 +- .../motion_model/ctrv_motion_model.hpp | 2 +- .../tracker/motion_model/cv_motion_model.hpp | 2 +- .../motion_model/motion_model_base.hpp | 3 +- .../tracker/object_model/object_model.hpp | 305 ++++++++++++++++++ .../multi_object_tracker/utils/utils.hpp | 141 ++------ .../src/debugger/debugger.cpp | 2 +- .../src/multi_object_tracker_core.cpp | 19 +- .../src/processor/processor.cpp | 2 +- .../src/tracker/model/bicycle_tracker.cpp | 188 +++++------ .../src/tracker/model/big_vehicle_tracker.cpp | 213 ++++++------ .../tracker/model/normal_vehicle_tracker.cpp | 214 ++++++------ .../tracker/model/pass_through_tracker.cpp | 46 +-- .../src/tracker/model/pedestrian_tracker.cpp | 204 ++++++------ .../src/tracker/model/unknown_tracker.cpp | 63 ++-- .../motion_model/bicycle_motion_model.cpp | 158 ++++----- .../motion_model/ctrv_motion_model.cpp | 124 +++---- .../tracker/motion_model/cv_motion_model.cpp | 87 ++--- 32 files changed, 1036 insertions(+), 896 deletions(-) create mode 100644 perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp diff --git a/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp b/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp index 56f4c3ac45e52..c30423db02c26 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp @@ -19,19 +19,20 @@ #ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ #define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ -#include -#include -#include -#include - #define EIGEN_MPL2_ONLY + #include "multi_object_tracker/data_association/solver/gnn_solver.hpp" #include "multi_object_tracker/tracker/tracker.hpp" #include #include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" + +#include +#include +#include +#include class DataAssociation { diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp index 7c1b814258bcf..08c9eb975d8e6 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp @@ -15,15 +15,15 @@ #ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ #define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_ +#include "autoware/universe_utils/ros/uuid_helper.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include #include -#include "unique_identifier_msgs/msg/uuid.hpp" -#include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include "autoware_perception_msgs/msg/tracked_objects.hpp" #include +#include #include #include diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp index 3019f16ada7c9..a1c516147b220 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp @@ -15,16 +15,16 @@ #ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ #define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_ +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/ros/published_time_publisher.hpp" #include "multi_object_tracker/debugger/debug_object.hpp" -#include -#include #include #include #include -#include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include "autoware_perception_msgs/msg/tracked_objects.hpp" #include #include 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 4c1ccced1bcf7..aff3cbd00eabe 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 @@ -27,8 +27,8 @@ #include -#include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include "autoware_perception_msgs/msg/tracked_objects.hpp" #include #include 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 b2f5f41c81f0a..6d0dbcd036e53 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 @@ -19,8 +19,8 @@ #include -#include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include "autoware_perception_msgs/msg/tracked_objects.hpp" #include #include 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 36176b8202e72..63ad496b70ed9 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 @@ -19,10 +19,10 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ +#include "kalman_filter/kalman_filter.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" #include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" - -#include +#include "multi_object_tracker/tracker/object_model/object_model.hpp" class BicycleTracker : public Tracker { @@ -30,13 +30,7 @@ class BicycleTracker : public Tracker autoware_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; -private: - struct EkfParams - { - double r_cov_x; - double r_cov_y; - double r_cov_yaw; - } ekf_params_; + object_model::ObjectModel object_model_ = object_model::bicycle; double z_; @@ -48,9 +42,7 @@ class BicycleTracker : public Tracker }; BoundingBox bounding_box_; -private: BicycleMotionModel motion_model_; - const char DIM = motion_model_.DIM; using IDX = BicycleMotionModel::IDX; public: @@ -63,15 +55,16 @@ class BicycleTracker : public Tracker bool measure( const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const override; - virtual ~BicycleTracker() {} + +private: + autoware_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform) const; }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ 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 8eec03c5dc847..7112e6a08ade1 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 @@ -19,10 +19,10 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ +#include "kalman_filter/kalman_filter.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" #include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" - -#include +#include "multi_object_tracker/tracker/object_model/object_model.hpp" class BigVehicleTracker : public Tracker { @@ -30,14 +30,8 @@ class BigVehicleTracker : public Tracker autoware_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; -private: - struct EkfParams - { - double r_cov_x; - double r_cov_y; - double r_cov_yaw; - double r_cov_vel; - } ekf_params_; + object_model::ObjectModel object_model_ = object_model::big_vehicle; + double velocity_deviation_threshold_; double z_; @@ -51,9 +45,7 @@ class BigVehicleTracker : public Tracker BoundingBox bounding_box_; Eigen::Vector2d tracking_offset_; -private: BicycleMotionModel motion_model_; - const char DIM = motion_model_.DIM; using IDX = BicycleMotionModel::IDX; public: @@ -66,15 +58,16 @@ class BigVehicleTracker : public Tracker bool measure( const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const override; - virtual ~BigVehicleTracker() {} + +private: + autoware_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ 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 b3d088da54721..75aec0b06d6b8 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 @@ -19,11 +19,11 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ +#include "kalman_filter/kalman_filter.hpp" #include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" #include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include #include class MultipleVehicleTracker : public Tracker 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 107a3ef194afb..1165cecab258b 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 @@ -19,10 +19,10 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ +#include "kalman_filter/kalman_filter.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" #include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" - -#include +#include "multi_object_tracker/tracker/object_model/object_model.hpp" class NormalVehicleTracker : public Tracker { @@ -30,14 +30,8 @@ class NormalVehicleTracker : public Tracker autoware_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; -private: - struct EkfParams - { - double r_cov_x; - double r_cov_y; - double r_cov_yaw; - double r_cov_vel; - } ekf_params_; + object_model::ObjectModel object_model_ = object_model::normal_vehicle; + double velocity_deviation_threshold_; double z_; @@ -51,9 +45,7 @@ class NormalVehicleTracker : public Tracker BoundingBox bounding_box_; Eigen::Vector2d tracking_offset_; -private: BicycleMotionModel motion_model_; - const char DIM = motion_model_.DIM; using IDX = BicycleMotionModel::IDX; public: @@ -66,15 +58,16 @@ class NormalVehicleTracker : public Tracker bool measure( const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const override; - virtual ~NormalVehicleTracker() {} + +private: + autoware_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ 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 605b3ae462cec..cd661dab52c6e 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 @@ -19,10 +19,9 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ +#include "kalman_filter/kalman_filter.hpp" #include "tracker_base.hpp" -#include - class PassThroughTracker : public Tracker { private: @@ -43,7 +42,6 @@ class PassThroughTracker : public Tracker bool getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const override; - virtual ~PassThroughTracker() {} }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ 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 ad81c504d3719..3c3ac038b085e 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 @@ -19,12 +19,11 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ +#include "kalman_filter/kalman_filter.hpp" #include "multi_object_tracker/tracker/model/bicycle_tracker.hpp" #include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" -#include - class PedestrianAndBicycleTracker : public Tracker { private: 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 e5d718c4b15ee..c5be57f656eb5 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 @@ -19,10 +19,10 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ +#include "kalman_filter/kalman_filter.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" #include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" - -#include +#include "multi_object_tracker/tracker/object_model/object_model.hpp" // cspell: ignore CTRV @@ -32,13 +32,7 @@ class PedestrianTracker : public Tracker autoware_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; -private: - struct EkfParams - { - double r_cov_x; - double r_cov_y; - double r_cov_yaw; - } ekf_params_; + object_model::ObjectModel object_model_ = object_model::pedestrian; double z_; @@ -56,9 +50,7 @@ class PedestrianTracker : public Tracker BoundingBox bounding_box_; Cylinder cylinder_; -private: CTRVMotionModel motion_model_; - const char DIM = motion_model_.DIM; using IDX = CTRVMotionModel::IDX; public: @@ -71,15 +63,16 @@ class PedestrianTracker : public Tracker bool measure( const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const override; - virtual ~PedestrianTracker() {} + +private: + autoware_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform) const; }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ 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 56670e9b54b7e..44b3884c392e6 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 @@ -28,8 +28,8 @@ #include "autoware_perception_msgs/msg/detected_object.hpp" #include "autoware_perception_msgs/msg/tracked_object.hpp" -#include "geometry_msgs/msg/point.hpp" -#include "unique_identifier_msgs/msg/uuid.hpp" +#include +#include #include @@ -54,7 +54,7 @@ class Tracker const rclcpp::Time & time, const std::vector & classification, const size_t & channel_size); - virtual ~Tracker() {} + virtual ~Tracker() = default; void initializeExistenceProbabilities( const uint & channel_index, const float & existence_probability); 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 43a1f2fd14842..ca8ecba160bd8 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 @@ -19,18 +19,16 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ +#include "kalman_filter/kalman_filter.hpp" #include "multi_object_tracker/tracker/model/tracker_base.hpp" #include "multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" -#include - class UnknownTracker : public Tracker { private: autoware_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; -private: struct EkfParams { double r_cov_x; @@ -41,9 +39,7 @@ class UnknownTracker : public Tracker double z_; -private: CVMotionModel motion_model_; - const char DIM = motion_model_.DIM; using IDX = CVMotionModel::IDX; public: @@ -64,7 +60,6 @@ class UnknownTracker : public Tracker bool getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const override; - virtual ~UnknownTracker() {} }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp index 5127d0448835c..f6ce2842388c6 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp @@ -19,10 +19,10 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#include "kalman_filter/kalman_filter.hpp" #include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include -#include #include #ifdef ROS_DISTRO_GALACTIC @@ -47,6 +47,8 @@ class BicycleMotionModel : public MotionModel { double q_stddev_acc_long; double q_stddev_acc_lat; + double q_cov_acc_long; + double q_cov_acc_lat; double q_stddev_yaw_rate_min; double q_stddev_yaw_rate_max; double q_cov_slip_rate_min; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp index 6b071eddec7a9..657048079c46c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp @@ -19,10 +19,10 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +#include "kalman_filter/kalman_filter.hpp" #include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include -#include #include #ifdef ROS_DISTRO_GALACTIC diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp index 59032706b00d6..421e413d7aab7 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp @@ -19,10 +19,10 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#include "kalman_filter/kalman_filter.hpp" #include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include -#include #include #ifdef ROS_DISTRO_GALACTIC diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp index 1aca602ed66a3..e740612e96d4f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp @@ -19,8 +19,9 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +#include "kalman_filter/kalman_filter.hpp" + #include -#include #include #ifdef ROS_DISTRO_GALACTIC diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp new file mode 100644 index 0000000000000..6a771344bb36b --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/object_model/object_model.hpp @@ -0,0 +1,305 @@ +// 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. +// +// +// Author: v1.0 Taekjin Lee +// + +#ifndef MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#define MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ + +#include + +namespace +{ +constexpr double const_g = 9.81; + +template +constexpr T sq(const T x) +{ + return x * x; +} + +template +constexpr T deg2rad(const T deg) +{ + return deg * static_cast(M_PI) / static_cast(180.0); +} + +// cspell: ignore kmph +template +constexpr T kmph2mps(const T kmph) +{ + return kmph * static_cast(1000.0) / static_cast(3600.0); +} + +} // namespace + +namespace object_model +{ + +enum class ObjectModelType { NormalVehicle, BigVehicle, Bicycle, Pedestrian, Unknown }; +struct ObjectSize +{ + double length{0.0}; // [m] + double width{0.0}; // [m] + double height{0.0}; // [m] +}; +struct ObjectSizeLimit +{ + double length_min{0.0}; // [m] + double length_max{0.0}; // [m] + double width_min{0.0}; // [m] + double width_max{0.0}; // [m] + double height_min{0.0}; // [m] + double height_max{0.0}; // [m] +}; +struct MotionProcessNoise +{ + double vel_long{0.0}; // [m/s] uncertain longitudinal velocity + double vel_lat{0.0}; // [m/s] uncertain lateral velocity + double yaw_rate{0.0}; // [rad/s] uncertain yaw rate + double yaw_rate_min{0.0}; // [rad/s] uncertain yaw rate, minimum + double yaw_rate_max{0.0}; // [rad/s] uncertain yaw rate, maximum + double acc_long{0.0}; // [m/s^2] uncertain longitudinal acceleration + double acc_lat{0.0}; // [m/s^2] uncertain lateral acceleration + double acc_turn{0.0}; // [rad/s^2] uncertain rotation acceleration +}; +struct MotionProcessLimit +{ + double acc_long_max{0.0}; // [m/s^2] + double acc_lat_max{0.0}; // [m/s^2] + double vel_long_max{0.0}; // [m/s] + double vel_lat_max{0.0}; // [m/s] + double yaw_rate_max{0.0}; // [rad/s] +}; +struct StateCovariance +{ + double pos_x{0.0}; // [m^2] + double pos_y{0.0}; // [m^2] + double yaw{0.0}; // [rad^2] + double yaw_rate{0.0}; // [rad^2/s^2] + double vel_long{0.0}; // [m^2/s^2] + double vel_lat{0.0}; // [m^2/s^2] + double acc_long{0.0}; // [m^2/s^4] + double acc_lat{0.0}; // [m^2/s^4] +}; +struct BicycleModelState +{ + double init_slip_angle_cov{0.0}; // [rad^2/s^2] initial slip angle covariance + double slip_angle_max{0.0}; // [rad] max slip angle + double slip_rate_stddev_min{0.0}; // [rad/s] uncertain slip angle change rate, minimum + double slip_rate_stddev_max{0.0}; // [rad/s] uncertain slip angle change rate, maximum + double wheel_pos_ratio_front{0.0}; // [-] + double wheel_pos_ratio_rear{0.0}; // [-] + double wheel_pos_front_min{0.0}; // [m] + double wheel_pos_rear_min{0.0}; // [m] +}; + +class ObjectModel +{ +public: + ObjectSize init_size; + ObjectSizeLimit size_limit; + MotionProcessNoise process_noise; + MotionProcessLimit process_limit; + StateCovariance initial_covariance; + StateCovariance measurement_covariance; + BicycleModelState bicycle_state; + + explicit ObjectModel(const ObjectModelType & type) + { + switch (type) { + case ObjectModelType::NormalVehicle: + init_size.length = 3.0; + init_size.width = 2.0; + init_size.height = 1.8; + size_limit.length_min = 1.0; + size_limit.length_max = 20.0; + size_limit.width_min = 1.0; + size_limit.width_max = 5.0; + size_limit.height_min = 1.0; + size_limit.height_max = 5.0; + + process_noise.acc_long = const_g * 0.35; + process_noise.acc_lat = const_g * 0.15; + process_noise.yaw_rate_min = deg2rad(1.5); + process_noise.yaw_rate_max = deg2rad(15.0); + + process_limit.acc_long_max = const_g; + process_limit.acc_lat_max = const_g; + process_limit.vel_long_max = kmph2mps(140.0); + + // initial covariance + initial_covariance.pos_x = sq(1.0); + initial_covariance.pos_y = sq(0.3); + initial_covariance.yaw = sq(deg2rad(25.0)); + initial_covariance.vel_long = sq(kmph2mps(1000.0)); + initial_covariance.vel_lat = sq(0.2); + + // measurement noise model + measurement_covariance.pos_x = sq(0.5); + measurement_covariance.pos_y = sq(0.4); + measurement_covariance.yaw = sq(deg2rad(20.0)); + measurement_covariance.vel_long = sq(1.0); + + // bicycle motion model + bicycle_state.init_slip_angle_cov = sq(deg2rad(5.0)); + bicycle_state.slip_angle_max = deg2rad(30.0); + bicycle_state.slip_rate_stddev_min = deg2rad(0.3); + bicycle_state.slip_rate_stddev_max = deg2rad(10.0); + bicycle_state.wheel_pos_ratio_front = 0.3; + bicycle_state.wheel_pos_ratio_rear = 0.25; + bicycle_state.wheel_pos_front_min = 1.0; + bicycle_state.wheel_pos_rear_min = 1.0; + break; + + case ObjectModelType::BigVehicle: + init_size.length = 6.0; + init_size.width = 2.0; + init_size.height = 2.0; + size_limit.length_min = 1.0; + size_limit.length_max = 35.0; + size_limit.width_min = 1.0; + size_limit.width_max = 10.0; + size_limit.height_min = 1.0; + size_limit.height_max = 10.0; + + process_noise.acc_long = const_g * 0.35; + process_noise.acc_lat = const_g * 0.15; + process_noise.yaw_rate_min = deg2rad(1.5); + process_noise.yaw_rate_max = deg2rad(15.0); + + process_limit.acc_long_max = const_g; + process_limit.acc_lat_max = const_g; + process_limit.vel_long_max = kmph2mps(140.0); + + // initial covariance + initial_covariance.pos_x = sq(1.5); + initial_covariance.pos_y = sq(0.5); + initial_covariance.yaw = sq(deg2rad(25.0)); + initial_covariance.vel_long = sq(kmph2mps(1000.0)); + initial_covariance.vel_lat = sq(0.2); + + // measurement noise model + measurement_covariance.pos_x = sq(0.5); + measurement_covariance.pos_y = sq(0.4); + measurement_covariance.yaw = sq(deg2rad(20.0)); + measurement_covariance.vel_long = sq(kmph2mps(10.0)); + + // bicycle motion model + bicycle_state.init_slip_angle_cov = sq(deg2rad(5.0)); + bicycle_state.slip_angle_max = deg2rad(30.0); + bicycle_state.slip_rate_stddev_min = deg2rad(0.3); + bicycle_state.slip_rate_stddev_max = deg2rad(10.0); + bicycle_state.wheel_pos_ratio_front = 0.3; + bicycle_state.wheel_pos_ratio_rear = 0.25; + bicycle_state.wheel_pos_front_min = 1.5; + bicycle_state.wheel_pos_rear_min = 1.5; + break; + + case ObjectModelType::Bicycle: + init_size.length = 1.0; + init_size.width = 0.5; + init_size.height = 1.7; + size_limit.length_min = 0.5; + size_limit.length_max = 8.0; + size_limit.width_min = 0.5; + size_limit.width_max = 3.0; + size_limit.height_min = 0.5; + size_limit.height_max = 2.5; + + process_noise.acc_long = const_g * 0.35; + process_noise.acc_lat = const_g * 0.15; + process_noise.yaw_rate_min = deg2rad(5.0); + process_noise.yaw_rate_max = deg2rad(15.0); + + process_limit.acc_long_max = const_g; + process_limit.acc_lat_max = const_g; + process_limit.vel_long_max = kmph2mps(120.0); + + // initial covariance + initial_covariance.pos_x = sq(0.8); + initial_covariance.pos_y = sq(0.5); + initial_covariance.yaw = sq(deg2rad(25.0)); + initial_covariance.vel_long = sq(kmph2mps(1000.0)); + initial_covariance.vel_lat = sq(0.2); + + // measurement noise model + measurement_covariance.pos_x = sq(0.5); + measurement_covariance.pos_y = sq(0.4); + measurement_covariance.yaw = sq(deg2rad(30.0)); + measurement_covariance.vel_long = sq(kmph2mps(10.0)); + + // bicycle motion model + bicycle_state.init_slip_angle_cov = sq(deg2rad(5.0)); + bicycle_state.slip_angle_max = deg2rad(30.0); + bicycle_state.slip_rate_stddev_min = deg2rad(1.0); + bicycle_state.slip_rate_stddev_max = deg2rad(10.0); + bicycle_state.wheel_pos_ratio_front = 0.3; + bicycle_state.wheel_pos_ratio_rear = 0.3; + bicycle_state.wheel_pos_front_min = 0.3; + bicycle_state.wheel_pos_rear_min = 0.3; + break; + + case ObjectModelType::Pedestrian: + init_size.length = 0.5; + init_size.width = 0.5; + init_size.height = 1.7; + size_limit.length_min = 0.3; + size_limit.length_max = 2.0; + size_limit.width_min = 0.3; + size_limit.width_max = 1.0; + size_limit.height_min = 0.6; + size_limit.height_max = 2.0; + + process_noise.vel_long = 0.5; + process_noise.vel_lat = 0.5; + process_noise.yaw_rate = deg2rad(20.0); + process_noise.acc_long = const_g * 0.3; + process_noise.acc_turn = deg2rad(30.0); + + process_limit.vel_long_max = kmph2mps(100.0); + process_limit.yaw_rate_max = deg2rad(30.0); + + // initial covariance + initial_covariance.pos_x = sq(2.0); + initial_covariance.pos_y = sq(2.0); + initial_covariance.yaw = sq(deg2rad(1000.0)); + initial_covariance.vel_long = sq(kmph2mps(120.0)); + initial_covariance.yaw_rate = sq(deg2rad(360.0)); + + // measurement noise model + measurement_covariance.pos_x = sq(0.4); + measurement_covariance.pos_y = sq(0.4); + measurement_covariance.yaw = sq(deg2rad(30.0)); + + break; + + default: + break; + } + } + virtual ~ObjectModel() = default; +}; + +// create static objects by using ObjectModel class +static const ObjectModel normal_vehicle(ObjectModelType::NormalVehicle); +static const ObjectModel big_vehicle(ObjectModelType::BigVehicle); +static const ObjectModel bicycle(ObjectModelType::Bicycle); +static const ObjectModel pedestrian(ObjectModelType::Pedestrian); + +} // namespace object_model + +#endif // MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index 259a58342c369..0bda7870ae2b9 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -22,9 +22,9 @@ #include #include -#include -#include -#include +#include "autoware_perception_msgs/msg/detected_object.hpp" +#include "autoware_perception_msgs/msg/shape.hpp" +#include "autoware_perception_msgs/msg/tracked_object.hpp" #include #include #include @@ -38,45 +38,6 @@ namespace utils { -enum MSG_COV_IDX { - X_X = 0, - X_Y = 1, - X_Z = 2, - X_ROLL = 3, - X_PITCH = 4, - X_YAW = 5, - Y_X = 6, - Y_Y = 7, - Y_Z = 8, - Y_ROLL = 9, - Y_PITCH = 10, - Y_YAW = 11, - Z_X = 12, - Z_Y = 13, - Z_Z = 14, - Z_ROLL = 15, - Z_PITCH = 16, - Z_YAW = 17, - ROLL_X = 18, - ROLL_Y = 19, - ROLL_Z = 20, - ROLL_ROLL = 21, - ROLL_PITCH = 22, - ROLL_YAW = 23, - PITCH_X = 24, - PITCH_Y = 25, - PITCH_Z = 26, - PITCH_ROLL = 27, - PITCH_PITCH = 28, - PITCH_YAW = 29, - YAW_X = 30, - YAW_Y = 31, - YAW_Z = 32, - YAW_ROLL = 33, - YAW_PITCH = 34, - YAW_YAW = 35 -}; - enum BBOX_IDX { FRONT_SURFACE = 0, RIGHT_SURFACE = 1, @@ -132,7 +93,8 @@ inline int getNearestCornerOrSurface( // (left) | | (right) // -- // x- (rear) - int xgrid, ygrid; + int xgrid = 0; + int ygrid = 0; const int labels[3][3] = { {BBOX_IDX::FRONT_L_CORNER, BBOX_IDX::FRONT_SURFACE, BBOX_IDX::FRONT_R_CORNER}, {BBOX_IDX::LEFT_SURFACE, BBOX_IDX::INSIDE, BBOX_IDX::RIGHT_SURFACE}, @@ -155,34 +117,6 @@ inline int getNearestCornerOrSurface( return labels[xgrid][ygrid]; // 0 to 7 + 1(null) value } -/** - * @brief Get the Nearest Corner or Surface from detected object - * @param object: input object - * @param yaw: object yaw angle (after solved front and back uncertainty) - * @param self_transform - * @return nearest corner or surface index - */ -inline int getNearestCornerOrSurfaceFromObject( - const autoware_perception_msgs::msg::DetectedObject & object, const double & yaw, - const geometry_msgs::msg::Transform & self_transform) -{ - // only work for BBOX shape - if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - return BBOX_IDX::INVALID; - } - - // extract necessary information from input object - double x, y, width, length; - x = object.kinematics.pose_with_covariance.pose.position.x; - y = object.kinematics.pose_with_covariance.pose.position.y; - // yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); //not use raw yaw - // now - width = object.shape.dimensions.y; - length = object.shape.dimensions.x; - - return getNearestCornerOrSurface(x, y, yaw, width, length, self_transform); -} - /** * @brief Calc bounding box center offset caused by shape change * @param dw: width update [m] = w_new - w_old @@ -225,30 +159,6 @@ inline Eigen::Vector2d calcOffsetVectorFromShapeChange( return offset; // do nothing if indx == INVALID or INSIDE } -/** - * @brief post-processing to recover bounding box center from tracking point and offset vector - * @param x: x of tracking point estimated with ekf - * @param y: y of tracking point estimated with ekf - * @param yaw: yaw of tracking point estimated with ekf - * @param dw: diff of width: w_estimated - w_input - * @param dl: diff of length: l_estimated - l_input - * @param indx: closest corner or surface index - * @param tracking_offset: tracking offset between bounding box center and tracking point - */ -inline Eigen::Vector2d recoverFromTrackingPoint( - const double x, const double y, const double yaw, const double dw, const double dl, - const int indx, const Eigen::Vector2d & tracking_offset) -{ - const Eigen::Vector2d tracking_point{x, y}; - const Eigen::Matrix2d R = Eigen::Rotation2Dd(yaw).toRotationMatrix(); - - const Eigen::Vector2d shape_change_offset = calcOffsetVectorFromShapeChange(dw, dl, indx); - - Eigen::Vector2d output_center = tracking_point - R * tracking_offset - R * shape_change_offset; - - return output_center; -} - /** * @brief Convert input object center to tracking point based on nearest corner information * 1. update anchor offset vector, 2. offset input bbox based on tracking_offset vector and @@ -274,9 +184,8 @@ inline void calcAnchorPointOffset( } // current object width and height - double w_n, l_n; - l_n = input_object.shape.dimensions.x; - w_n = input_object.shape.dimensions.y; + const double w_n = input_object.shape.dimensions.y; + const double l_n = input_object.shape.dimensions.x; // update offset const Eigen::Vector2d offset = calcOffsetVectorFromShapeChange(w_n - w, l_n - l, indx); @@ -304,27 +213,19 @@ inline bool convertConvexHullToBoundingBox( } // look for bounding box boundary - double max_x = 0; - double max_y = 0; - double min_x = 0; - double min_y = 0; - double max_z = 0; - for (size_t i = 0; i < input_object.shape.footprint.points.size(); ++i) { - const double foot_x = input_object.shape.footprint.points.at(i).x; - const double foot_y = input_object.shape.footprint.points.at(i).y; - const double foot_z = input_object.shape.footprint.points.at(i).z; - max_x = std::max(max_x, foot_x); - max_y = std::max(max_y, foot_y); - min_x = std::min(min_x, foot_x); - min_y = std::min(min_y, foot_y); - max_z = std::max(max_z, foot_z); + float max_x = 0; + float max_y = 0; + float min_x = 0; + float min_y = 0; + float max_z = 0; + for (const auto & point : input_object.shape.footprint.points) { + max_x = std::max(max_x, point.x); + max_y = std::max(max_y, point.y); + min_x = std::min(min_x, point.x); + min_y = std::min(min_y, point.y); + max_z = std::max(max_z, point.z); } - // calc bounding box state - const double length = max_x - min_x; - const double width = max_y - min_y; - const double height = max_z; - // calc new center const Eigen::Vector2d center{ input_object.kinematics.pose_with_covariance.pose.position.x, @@ -340,9 +241,9 @@ inline bool convertConvexHullToBoundingBox( output_object.kinematics.pose_with_covariance.pose.position.y = new_center.y(); output_object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; - output_object.shape.dimensions.x = length; - output_object.shape.dimensions.y = width; - output_object.shape.dimensions.z = height; + output_object.shape.dimensions.x = max_x - min_x; + output_object.shape.dimensions.y = max_y - min_y; + output_object.shape.dimensions.z = max_z; return true; } diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp index a3ece17eab112..e692ae76468e4 100644 --- a/perception/multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/multi_object_tracker/src/debugger/debugger.cpp @@ -98,7 +98,7 @@ void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & s stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Measurement time is not set."); return; } - const double & delay = pipeline_latency_ms_; // [s] + const double & delay = pipeline_latency_ms_ / 1e3; // [s] if (delay == 0.0) { stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Detection delay is not calculated."); 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 54255e706e35a..97c3d93d191b2 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -13,6 +13,15 @@ // limitations under the License. // // +#define EIGEN_MPL2_ONLY + +#include "multi_object_tracker/multi_object_tracker_core.hpp" + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include #include @@ -28,14 +37,6 @@ #include #include -#define EIGEN_MPL2_ONLY -#include "multi_object_tracker/multi_object_tracker_core.hpp" -#include "multi_object_tracker/utils/utils.hpp" - -#include -#include -#include - namespace { // Function to get the transform between two frames @@ -207,7 +208,7 @@ void MultiObjectTracker::onTrigger() { const rclcpp::Time current_time = this->now(); // get objects from the input manager and run process - std::vector> objects_list; + ObjectsList objects_list; const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list); if (!is_objects_ready) return; diff --git a/perception/multi_object_tracker/src/processor/processor.cpp b/perception/multi_object_tracker/src/processor/processor.cpp index ff14f7849b161..50d1a021c5838 100644 --- a/perception/multi_object_tracker/src/processor/processor.cpp +++ b/perception/multi_object_tracker/src/processor/processor.cpp @@ -17,7 +17,7 @@ #include "multi_object_tracker/tracker/tracker.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include +#include "autoware_perception_msgs/msg/tracked_objects.hpp" #include 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 05e06107c2247..a438c830596d7 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -15,14 +15,19 @@ // // Author: v1.0 Yukihiro Saito // +#define EIGEN_MPL2_ONLY #include "multi_object_tracker/tracker/model/bicycle_tracker.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include "multi_object_tracker/utils/utils.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" -#include -#include -#include +#include +#include #include #include @@ -34,11 +39,6 @@ #else #include #endif -#include "object_recognition_utils/object_recognition_utils.hpp" - -#define EIGEN_MPL2_ONLY -#include -#include using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -55,45 +55,36 @@ BicycleTracker::BicycleTracker( // 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] - double r_stddev_y = 0.4; // in vehicle coordinate [m] - double r_stddev_yaw = autoware::universe_utils::deg2rad(30); // in map coordinate [rad] - ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); - ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); - ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); - // OBJECT SHAPE MODEL if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; } else { - bounding_box_ = {1.0, 0.5, 1.7}; + bounding_box_ = { + object_model_.init_size.length, object_model_.init_size.width, + object_model_.init_size.height}; // default value } // set maximum and minimum size - constexpr double max_size = 5.0; - constexpr double min_size = 0.3; - bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); - bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); - bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); + bounding_box_.length = std::clamp( + bounding_box_.length, object_model_.size_limit.length_min, object_model_.size_limit.length_max); + bounding_box_.width = std::clamp( + bounding_box_.width, object_model_.size_limit.width_min, object_model_.size_limit.width_max); + bounding_box_.height = std::clamp( + bounding_box_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); // Set motion model parameters { - constexpr double q_stddev_acc_long = - 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - constexpr double q_stddev_yaw_rate_min = 5.0; // [deg/s] uncertain yaw change rate, minimum - constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum - constexpr double q_stddev_slip_rate_min = - 1.0; // [deg/s] uncertain slip angle change rate, minimum - constexpr double q_stddev_slip_rate_max = - 10.0; // [deg/s] uncertain slip angle change rate, maximum - constexpr double q_max_slip_angle = 30; // [deg] max slip angle - constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position - constexpr double lf_min = 0.3; // [m] minimum front wheel position - constexpr double lr_ratio = 0.3; // [-] ratio of rear wheel position - constexpr double lr_min = 0.3; // [m] minimum rear wheel position + 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, @@ -102,59 +93,47 @@ BicycleTracker::BicycleTracker( // Set motion limits { - constexpr double max_vel = autoware::universe_utils::kmph2mps(80); // [m/s] maximum velocity - constexpr double max_slip = 30; // [deg] maximum slip angle + 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::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; const double x = object.kinematics.pose_with_covariance.pose.position.x; const double y = object.kinematics.pose_with_covariance.pose.position.y; const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - auto pose_cov = object.kinematics.pose_with_covariance.covariance; - double vel = 0.0; - double vel_cov; - const double & length = bounding_box_.length; - - if (object.kinematics.has_twist) { - vel = object.kinematics.twist_with_covariance.twist.linear.x; - } + auto pose_cov = object.kinematics.pose_with_covariance.covariance; if (!object.kinematics.has_position_covariance) { // initial state covariance - constexpr double p0_stddev_x = 0.8; // in object coordinate [m] - constexpr double p0_stddev_y = 0.5; // in object coordinate [m] - constexpr double p0_stddev_yaw = - autoware::universe_utils::deg2rad(25); // in map coordinate [rad] - constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; - constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; - constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; + 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[utils::MSG_COV_IDX::X_X] = - p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; - pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; - pose_cov[utils::MSG_COV_IDX::Y_Y] = - p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; - pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; - pose_cov[utils::MSG_COV_IDX::YAW_YAW] = p0_cov_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; } - if (!object.kinematics.has_twist_covariance) { - constexpr double p0_stddev_vel = - autoware::universe_utils::kmph2mps(1000); // in object coordinate [m/s] - vel_cov = std::pow(p0_stddev_vel, 2.0); - } else { - vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + double vel = 0.0; + double vel_cov = object_model_.initial_covariance.vel_long; + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; + } + if (object.kinematics.has_twist_covariance) { + vel_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; } const double slip = 0.0; - const double p0_stddev_slip = - autoware::universe_utils::deg2rad(5); // in object coordinate [rad/s] - const double slip_cov = std::pow(p0_stddev_slip, 2.0); + const double slip_cov = object_model_.bicycle_state.init_slip_angle_cov; + const double & length = bounding_box_.length; // initialize motion model motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); @@ -168,9 +147,9 @@ bool BicycleTracker::predict(const rclcpp::Time & time) autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) + const geometry_msgs::msg::Transform & /*self_transform*/) const { - autoware_perception_msgs::msg::DetectedObject updating_object; + autoware_perception_msgs::msg::DetectedObject updating_object = object; // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape @@ -178,19 +157,42 @@ autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( if (!utils::convertConvexHullToBoundingBox(object, updating_object)) { updating_object = object; } - } else { - updating_object = object; } // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { + // measurement noise covariance + auto r_cov_x = object_model_.measurement_covariance.pos_x; + auto r_cov_y = object_model_.measurement_covariance.pos_y; + + // yaw angle fix + const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const bool is_yaw_available = + object.kinematics.orientation_availability != + autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + // fill covariance matrix + using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; - pose_cov[utils::MSG_COV_IDX::X_X] = ekf_params_.r_cov_x; // x - x - pose_cov[utils::MSG_COV_IDX::X_Y] = 0; // x - y - pose_cov[utils::MSG_COV_IDX::Y_X] = 0; // y - x - pose_cov[utils::MSG_COV_IDX::Y_Y] = ekf_params_.r_cov_y; // y - y - pose_cov[utils::MSG_COV_IDX::YAW_YAW] = ekf_params_.r_cov_yaw; // yaw - yaw + const double cos_yaw = std::cos(pose_yaw); + const double sin_yaw = std::sin(pose_yaw); + const double sin_2yaw = std::sin(2.0 * pose_yaw); + pose_cov[XYZRPY_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[XYZRPY_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x + pose_cov[XYZRPY_COV_IDX::X_YAW] = 0.0; // x - yaw + pose_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0; // y - yaw + pose_cov[XYZRPY_COV_IDX::YAW_X] = 0.0; // yaw - x + pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y + pose_cov[XYZRPY_COV_IDX::YAW_YAW] = object_model_.measurement_covariance.yaw; // yaw - yaw + if (!is_yaw_available) { + pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value + } + auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; + twist_cov[XYZRPY_COV_IDX::X_X] = object_model_.measurement_covariance.vel_long; // vel - vel } return updating_object; @@ -229,8 +231,7 @@ bool BicycleTracker::measureWithPose(const autoware_perception_msgs::msg::Detect bool BicycleTracker::measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object) { - autoware_perception_msgs::msg::DetectedObject bbox_object; - if (!object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + 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; } @@ -239,28 +240,29 @@ bool BicycleTracker::measureWithShape(const autoware_perception_msgs::msg::Detec constexpr double size_max = 30.0; // [m] constexpr double size_min = 0.1; // [m] if ( - bbox_object.shape.dimensions.x > size_max || bbox_object.shape.dimensions.y > size_max || - bbox_object.shape.dimensions.z > size_max) { + object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max || + object.shape.dimensions.z > size_max) { return false; } else if ( - bbox_object.shape.dimensions.x < size_min || bbox_object.shape.dimensions.y < size_min || - bbox_object.shape.dimensions.z < size_min) { + object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min || + object.shape.dimensions.z < size_min) { return false; } // update object size constexpr double gain = 0.1; constexpr double gain_inv = 1.0 - gain; - bounding_box_.length = gain_inv * bounding_box_.length + gain * bbox_object.shape.dimensions.x; - bounding_box_.width = gain_inv * bounding_box_.width + gain * bbox_object.shape.dimensions.y; - bounding_box_.height = gain_inv * bounding_box_.height + gain * bbox_object.shape.dimensions.z; + bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; + bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; + bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; // set maximum and minimum size - constexpr double max_size = 5.0; - constexpr double min_size = 0.3; - bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); - bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); - bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); + bounding_box_.length = std::clamp( + bounding_box_.length, object_model_.size_limit.length_min, object_model_.size_limit.length_max); + bounding_box_.width = std::clamp( + bounding_box_.width, object_model_.size_limit.width_min, object_model_.size_limit.width_max); + bounding_box_.height = std::clamp( + bounding_box_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); // update motion model motion_model_.updateExtendedState(bounding_box_.length); 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 4414d8a21ca01..90d33e65b46bd 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 @@ -15,14 +15,19 @@ // // Author: v1.0 Yukihiro Saito // +#define EIGEN_MPL2_ONLY #include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include "multi_object_tracker/utils/utils.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" -#include -#include -#include +#include +#include #include #include @@ -34,11 +39,6 @@ #else #include #endif -#include "object_recognition_utils/object_recognition_utils.hpp" - -#define EIGEN_MPL2_ONLY -#include -#include using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -56,17 +56,6 @@ BigVehicleTracker::BigVehicleTracker( // 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] - float r_stddev_y = 0.4; // in vehicle coordinate [m] - float r_stddev_yaw = autoware::universe_utils::deg2rad(20); // in map coordinate [rad] - float r_stddev_vel = 1.0; // in object coordinate [m/s] - ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); - ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); - ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); - ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0); - // velocity deviation threshold // if the predicted velocity is close to the observed velocity, // the observed velocity is used as the measurement. @@ -82,7 +71,9 @@ BigVehicleTracker::BigVehicleTracker( RCLCPP_WARN( logger_, "BigVehicleTracker::BigVehicleTracker: Failed to convert convex hull to bounding box."); - bounding_box_ = {6.0, 2.0, 2.0}; // default value + bounding_box_ = { + object_model_.init_size.length, object_model_.init_size.width, + object_model_.init_size.height}; // default value } else { bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, @@ -90,28 +81,26 @@ BigVehicleTracker::BigVehicleTracker( } } // set maximum and minimum size - constexpr double max_size = 30.0; - constexpr double min_size = 1.0; - bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); - bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); - bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); + bounding_box_.length = std::clamp( + bounding_box_.length, object_model_.size_limit.length_min, object_model_.size_limit.length_max); + bounding_box_.width = std::clamp( + bounding_box_.width, object_model_.size_limit.width_min, object_model_.size_limit.width_max); + bounding_box_.height = std::clamp( + bounding_box_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); // Set motion model parameters { - constexpr double q_stddev_acc_long = - 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate, minimum - constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum - constexpr double q_stddev_slip_rate_min = - 0.3; // [deg/s] uncertain slip angle change rate, minimum - constexpr double q_stddev_slip_rate_max = - 10.0; // [deg/s] uncertain slip angle change rate, maximum - constexpr double q_max_slip_angle = 30; // [deg] max slip angle - constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position - constexpr double lf_min = 1.5; // [m] minimum front wheel position - constexpr double lr_ratio = 0.25; // [-] ratio of rear wheel position - constexpr double lr_min = 1.5; // [m] minimum rear wheel position + 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, @@ -120,59 +109,47 @@ BigVehicleTracker::BigVehicleTracker( // Set motion limits { - constexpr double max_vel = autoware::universe_utils::kmph2mps(100); // [m/s] maximum velocity - constexpr double max_slip = 30; // [deg] maximum slip angle + 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::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; const double x = object.kinematics.pose_with_covariance.pose.position.x; const double y = object.kinematics.pose_with_covariance.pose.position.y; const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - auto pose_cov = object.kinematics.pose_with_covariance.covariance; - double vel = 0.0; - double vel_cov; - const double & length = bounding_box_.length; - - if (object.kinematics.has_twist) { - vel = object.kinematics.twist_with_covariance.twist.linear.x; - } + auto pose_cov = object.kinematics.pose_with_covariance.covariance; if (!object.kinematics.has_position_covariance) { // initial state covariance - constexpr double p0_stddev_x = 1.5; // in object coordinate [m] - constexpr double p0_stddev_y = 0.5; // in object coordinate [m] - constexpr double p0_stddev_yaw = - autoware::universe_utils::deg2rad(25); // in map coordinate [rad] - constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; - constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; - constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; + 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[utils::MSG_COV_IDX::X_X] = - p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; - pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; - pose_cov[utils::MSG_COV_IDX::Y_Y] = - p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; - pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; - pose_cov[utils::MSG_COV_IDX::YAW_YAW] = p0_cov_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; } - if (!object.kinematics.has_twist_covariance) { - constexpr double p0_stddev_vel = - autoware::universe_utils::kmph2mps(1000); // in object coordinate [m/s] - vel_cov = std::pow(p0_stddev_vel, 2.0); - } else { - vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + double vel = 0.0; + double vel_cov = object_model_.initial_covariance.vel_long; + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; + } + if (object.kinematics.has_twist_covariance) { + vel_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; } const double slip = 0.0; - const double p0_stddev_slip = - autoware::universe_utils::deg2rad(5); // in object coordinate [rad/s] - const double slip_cov = std::pow(p0_stddev_slip, 2.0); + const double slip_cov = object_model_.bicycle_state.init_slip_angle_cov; + const double & length = bounding_box_.length; // initialize motion model motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); @@ -190,14 +167,9 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje { autoware_perception_msgs::msg::DetectedObject updating_object = object; - // current (predicted) state - const double tracked_x = motion_model_.getStateElement(IDX::X); - const double tracked_y = motion_model_.getStateElement(IDX::Y); - const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); - // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape - autoware_perception_msgs::msg::DetectedObject bbox_object; + autoware_perception_msgs::msg::DetectedObject bbox_object = object; if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { RCLCPP_WARN( @@ -205,10 +177,13 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje "BigVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box."); bbox_object = object; } - } else { - bbox_object = object; } + // current (predicted) state + const double tracked_x = motion_model_.getStateElement(IDX::X); + const double tracked_y = motion_model_.getStateElement(IDX::Y); + const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); + // get offset measurement const int nearest_corner_index = utils::getNearestCornerOrSurface( tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform); @@ -219,50 +194,45 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { // measurement noise covariance - float r_cov_x; - float r_cov_y; - using Label = autoware_perception_msgs::msg::ObjectClassification; + auto r_cov_x = object_model_.measurement_covariance.pos_x; + auto r_cov_y = object_model_.measurement_covariance.pos_y; const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - if (utils::isLargeVehicleLabel(label)) { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; - } else if (label == Label::CAR) { + if (label == autoware_perception_msgs::msg::ObjectClassification::CAR) { // if label is changed, enlarge the measurement noise covariance constexpr float r_stddev_x = 2.0; // [m] constexpr float r_stddev_y = 2.0; // [m] - r_cov_x = std::pow(r_stddev_x, 2.0); - r_cov_y = std::pow(r_stddev_y, 2.0); - } else { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; + r_cov_x = r_stddev_x * r_stddev_x; + r_cov_y = r_stddev_y * r_stddev_y; } // yaw angle fix - double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - bool is_yaw_available = object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const bool is_yaw_available = + object.kinematics.orientation_availability != + autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; // fill covariance matrix + using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; const double cos_yaw = std::cos(pose_yaw); const double sin_yaw = std::sin(pose_yaw); - const double sin_2yaw = std::sin(2.0f * pose_yaw); - pose_cov[utils::MSG_COV_IDX::X_X] = - r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - pose_cov[utils::MSG_COV_IDX::Y_Y] = - r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; // y - x - pose_cov[utils::MSG_COV_IDX::X_YAW] = 0.0; // x - yaw - pose_cov[utils::MSG_COV_IDX::Y_YAW] = 0.0; // y - yaw - pose_cov[utils::MSG_COV_IDX::YAW_X] = 0.0; // yaw - x - pose_cov[utils::MSG_COV_IDX::YAW_Y] = 0.0; // yaw - y - pose_cov[utils::MSG_COV_IDX::YAW_YAW] = ekf_params_.r_cov_yaw; // yaw - yaw + const double sin_2yaw = std::sin(2.0 * pose_yaw); + pose_cov[XYZRPY_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[XYZRPY_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x + pose_cov[XYZRPY_COV_IDX::X_YAW] = 0.0; // x - yaw + pose_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0; // y - yaw + pose_cov[XYZRPY_COV_IDX::YAW_X] = 0.0; // yaw - x + pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y + pose_cov[XYZRPY_COV_IDX::YAW_YAW] = object_model_.measurement_covariance.yaw; // yaw - yaw if (!is_yaw_available) { - pose_cov[utils::MSG_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value + pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value } auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; - twist_cov[utils::MSG_COV_IDX::X_X] = ekf_params_.r_cov_vel; // vel - vel + twist_cov[XYZRPY_COV_IDX::X_X] = object_model_.measurement_covariance.vel_long; // vel - vel } return updating_object; @@ -314,34 +284,35 @@ bool BigVehicleTracker::measureWithPose( bool BigVehicleTracker::measureWithShape( const autoware_perception_msgs::msg::DetectedObject & object) { - if (!object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + 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 = 40.0; // [m] + constexpr double size_max = 35.0; // [m] constexpr double size_min = 1.0; // [m] - if (object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max) { - return false; - } else if (object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min) { + bool is_size_valid = + (object.shape.dimensions.x <= size_max && object.shape.dimensions.y <= size_max && + object.shape.dimensions.x >= size_min && object.shape.dimensions.y >= size_min); + if (!is_size_valid) { return false; } + // update object size constexpr double gain = 0.5; constexpr double gain_inv = 1.0 - gain; - - // update object size bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; // set maximum and minimum size - constexpr double max_size = 30.0; - constexpr double min_size = 1.0; - bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); - bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); - bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); + bounding_box_.length = std::clamp( + bounding_box_.length, object_model_.size_limit.length_min, object_model_.size_limit.length_max); + bounding_box_.width = std::clamp( + bounding_box_.width, object_model_.size_limit.width_min, object_model_.size_limit.width_max); + bounding_box_.height = std::clamp( + bounding_box_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); // update motion model motion_model_.updateExtendedState(bounding_box_.length); 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 1fff1a13add65..67282893083c1 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 @@ -15,14 +15,19 @@ // // Author: v1.0 Yukihiro Saito // +#define EIGEN_MPL2_ONLY #include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include "multi_object_tracker/utils/utils.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" -#include -#include -#include +#include +#include #include #include @@ -34,11 +39,6 @@ #else #include #endif -#include "object_recognition_utils/object_recognition_utils.hpp" - -#define EIGEN_MPL2_ONLY -#include -#include using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -56,17 +56,6 @@ NormalVehicleTracker::NormalVehicleTracker( // 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] - float r_stddev_y = 0.4; // in vehicle coordinate [m] - float r_stddev_yaw = autoware::universe_utils::deg2rad(20); // in map coordinate [rad] - float r_stddev_vel = 1.0; // in object coordinate [m/s] - ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); - ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); - ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); - ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0); - // velocity deviation threshold // if the predicted velocity is close to the observed velocity, // the observed velocity is used as the measurement. @@ -83,7 +72,9 @@ NormalVehicleTracker::NormalVehicleTracker( logger_, "NormalVehicleTracker::NormalVehicleTracker: Failed to convert convex hull to bounding " "box."); - bounding_box_ = {3.0, 2.0, 1.8}; // default value + bounding_box_ = { + object_model_.init_size.length, object_model_.init_size.width, + object_model_.init_size.height}; // default value } else { bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, @@ -91,28 +82,26 @@ NormalVehicleTracker::NormalVehicleTracker( } } // set maximum and minimum size - constexpr double max_size = 20.0; - constexpr double min_size = 1.0; - bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); - bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); - bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); + bounding_box_.length = std::clamp( + bounding_box_.length, object_model_.size_limit.length_min, object_model_.size_limit.length_max); + bounding_box_.width = std::clamp( + bounding_box_.width, object_model_.size_limit.width_min, object_model_.size_limit.width_max); + bounding_box_.height = std::clamp( + bounding_box_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); // Set motion model parameters { - constexpr double q_stddev_acc_long = - 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate, minimum - constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum - constexpr double q_stddev_slip_rate_min = - 0.3; // [deg/s] uncertain slip angle change rate, minimum - constexpr double q_stddev_slip_rate_max = - 10.0; // [deg/s] uncertain slip angle change rate, maximum - constexpr double q_max_slip_angle = 30; // [deg] max slip angle - constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position - constexpr double lf_min = 1.0; // [m] minimum front wheel position - constexpr double lr_ratio = 0.25; // [-] ratio of rear wheel position - constexpr double lr_min = 1.0; // [m] minimum rear wheel position + 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, @@ -121,59 +110,47 @@ NormalVehicleTracker::NormalVehicleTracker( // Set motion limits { - constexpr double max_vel = autoware::universe_utils::kmph2mps(100); // [m/s] maximum velocity - constexpr double max_slip = 30; // [deg] maximum slip angle + 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::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; const double x = object.kinematics.pose_with_covariance.pose.position.x; const double y = object.kinematics.pose_with_covariance.pose.position.y; const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - auto pose_cov = object.kinematics.pose_with_covariance.covariance; - double vel = 0.0; - double vel_cov; - const double & length = bounding_box_.length; - - if (object.kinematics.has_twist) { - vel = object.kinematics.twist_with_covariance.twist.linear.x; - } + auto pose_cov = object.kinematics.pose_with_covariance.covariance; if (!object.kinematics.has_position_covariance) { // initial state covariance - constexpr double p0_stddev_x = 1.0; // in object coordinate [m] - constexpr double p0_stddev_y = 0.3; // in object coordinate [m] - constexpr double p0_stddev_yaw = - autoware::universe_utils::deg2rad(25); // in map coordinate [rad] - constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; - constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; - constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; + 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[utils::MSG_COV_IDX::X_X] = - p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; - pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; - pose_cov[utils::MSG_COV_IDX::Y_Y] = - p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; - pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; - pose_cov[utils::MSG_COV_IDX::YAW_YAW] = p0_cov_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; } - if (!object.kinematics.has_twist_covariance) { - constexpr double p0_stddev_vel = - autoware::universe_utils::kmph2mps(1000); // in object coordinate [m/s] - vel_cov = std::pow(p0_stddev_vel, 2.0); - } else { - vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + double vel = 0.0; + double vel_cov = object_model_.initial_covariance.vel_long; + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; + } + if (object.kinematics.has_twist_covariance) { + vel_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; } const double slip = 0.0; - const double p0_stddev_slip = - autoware::universe_utils::deg2rad(5); // in object coordinate [rad/s] - const double slip_cov = std::pow(p0_stddev_slip, 2.0); + const double slip_cov = object_model_.bicycle_state.init_slip_angle_cov; + const double & length = bounding_box_.length; // initialize motion model motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); @@ -191,14 +168,9 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO { autoware_perception_msgs::msg::DetectedObject updating_object = object; - // current (predicted) state - const double tracked_x = motion_model_.getStateElement(IDX::X); - const double tracked_y = motion_model_.getStateElement(IDX::Y); - const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); - // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape - autoware_perception_msgs::msg::DetectedObject bbox_object; + autoware_perception_msgs::msg::DetectedObject bbox_object = object; if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { RCLCPP_WARN( @@ -206,11 +178,13 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO "NormalVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box."); bbox_object = object; } - - } else { - bbox_object = object; } + // current (predicted) state + const double tracked_x = motion_model_.getStateElement(IDX::X); + const double tracked_y = motion_model_.getStateElement(IDX::Y); + const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); + // get offset measurement const int nearest_corner_index = utils::getNearestCornerOrSurface( tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform); @@ -221,50 +195,45 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { // measurement noise covariance - float r_cov_x; - float r_cov_y; - using Label = autoware_perception_msgs::msg::ObjectClassification; + auto r_cov_x = object_model_.measurement_covariance.pos_x; + auto r_cov_y = object_model_.measurement_covariance.pos_y; const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - if (label == Label::CAR) { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; - } else if (utils::isLargeVehicleLabel(label)) { + if (utils::isLargeVehicleLabel(label)) { // if label is changed, enlarge the measurement noise covariance constexpr float r_stddev_x = 2.0; // [m] constexpr float r_stddev_y = 2.0; // [m] - r_cov_x = std::pow(r_stddev_x, 2.0); - r_cov_y = std::pow(r_stddev_y, 2.0); - } else { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; + r_cov_x = r_stddev_x * r_stddev_x; + r_cov_y = r_stddev_y * r_stddev_y; } // yaw angle fix - double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - bool is_yaw_available = object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const bool is_yaw_available = + object.kinematics.orientation_availability != + autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; // fill covariance matrix + using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; const double cos_yaw = std::cos(pose_yaw); const double sin_yaw = std::sin(pose_yaw); - const double sin_2yaw = std::sin(2.0f * pose_yaw); - pose_cov[utils::MSG_COV_IDX::X_X] = - r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - pose_cov[utils::MSG_COV_IDX::Y_Y] = - r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; // y - x - pose_cov[utils::MSG_COV_IDX::X_YAW] = 0.0; // x - yaw - pose_cov[utils::MSG_COV_IDX::Y_YAW] = 0.0; // y - yaw - pose_cov[utils::MSG_COV_IDX::YAW_X] = 0.0; // yaw - x - pose_cov[utils::MSG_COV_IDX::YAW_Y] = 0.0; // yaw - y - pose_cov[utils::MSG_COV_IDX::YAW_YAW] = ekf_params_.r_cov_yaw; // yaw - yaw + const double sin_2yaw = std::sin(2.0 * pose_yaw); + pose_cov[XYZRPY_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[XYZRPY_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x + pose_cov[XYZRPY_COV_IDX::X_YAW] = 0.0; // x - yaw + pose_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0; // y - yaw + pose_cov[XYZRPY_COV_IDX::YAW_X] = 0.0; // yaw - x + pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y + pose_cov[XYZRPY_COV_IDX::YAW_YAW] = object_model_.measurement_covariance.yaw; // yaw - yaw if (!is_yaw_available) { - pose_cov[utils::MSG_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value + pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value } auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; - twist_cov[utils::MSG_COV_IDX::X_X] = ekf_params_.r_cov_vel; // vel - vel + twist_cov[XYZRPY_COV_IDX::X_X] = object_model_.measurement_covariance.vel_long; // vel - vel } return updating_object; @@ -316,34 +285,35 @@ bool NormalVehicleTracker::measureWithPose( bool NormalVehicleTracker::measureWithShape( const autoware_perception_msgs::msg::DetectedObject & object) { - if (!object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + 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 = 30.0; // [m] + constexpr double size_max = 35.0; // [m] constexpr double size_min = 1.0; // [m] - if (object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max) { - return false; - } else if (object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min) { + bool is_size_valid = + (object.shape.dimensions.x <= size_max && object.shape.dimensions.y <= size_max && + object.shape.dimensions.x >= size_min && object.shape.dimensions.y >= size_min); + if (!is_size_valid) { return false; } + // update object size constexpr double gain = 0.5; constexpr double gain_inv = 1.0 - gain; - - // update object size bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; // set maximum and minimum size - constexpr double max_size = 20.0; - constexpr double min_size = 1.0; - bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); - bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); - bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); + bounding_box_.length = std::clamp( + bounding_box_.length, object_model_.size_limit.length_min, object_model_.size_limit.length_max); + bounding_box_.width = std::clamp( + bounding_box_.width, object_model_.size_limit.width_min, object_model_.size_limit.width_max); + bounding_box_.height = std::clamp( + bounding_box_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); // update motion model motion_model_.updateExtendedState(bounding_box_.length); 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 3e4f23cd440bc..17be415480360 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 @@ -15,6 +15,15 @@ // // Author: v1.0 Yutaka Shimizu // +#define EIGEN_MPL2_ONLY +#include "multi_object_tracker/tracker/model/pass_through_tracker.hpp" + +#include "autoware/universe_utils/ros/msg_covariance.hpp" +#include "multi_object_tracker/utils/utils.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" + +#include +#include #include #include @@ -27,14 +36,6 @@ #include #endif -#define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/pass_through_tracker.hpp" -#include "multi_object_tracker/utils/utils.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" - -#include -#include - PassThroughTracker::PassThroughTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, @@ -86,25 +87,26 @@ bool PassThroughTracker::measure( bool PassThroughTracker::getTrackedObject( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const { + using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; object = object_recognition_utils::toTrackedObject(object_); object.object_id = getUUID(); object.classification = getClassification(); - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] = 0.0; - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y] = 0.0; - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X] = 0.0; - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y] = 0.0; - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Z_Z] = 0.0; - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = 0.0; - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = 0.0; - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW] = 0.0; + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X] = 0.0; + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_Y] = 0.0; + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_X] = 0.0; + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y] = 0.0; + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Z_Z] = 0.0; + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = 0.0; + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = 0.0; + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::YAW_YAW] = 0.0; // twist covariance - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X] = 0.0; - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y] = 0.0; - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Z_Z] = 0.0; - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = 0.0; - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = 0.0; - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW] = 0.0; + object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X] = 0.0; + object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y] = 0.0; + object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::Z_Z] = 0.0; + object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = 0.0; + object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = 0.0; + object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::YAW_YAW] = 0.0; const double dt = (time - last_update_time_).seconds(); if (0.5 /*500msec*/ < dt) { 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 887810066f038..d9412c3b5739c 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -15,14 +15,19 @@ // // Author: v1.0 Yukihiro Saito // +#define EIGEN_MPL2_ONLY #include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include "multi_object_tracker/utils/utils.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" -#include -#include -#include +#include +#include #include #include @@ -34,11 +39,6 @@ #else #include #endif -#include "object_recognition_utils/object_recognition_utils.hpp" - -#define EIGEN_MPL2_ONLY -#include -#include using Label = autoware_perception_msgs::msg::ObjectClassification; @@ -55,18 +55,11 @@ PedestrianTracker::PedestrianTracker( // initialize existence probability initializeExistenceProbabilities(channel_index, object.existence_probability); - // Initialize parameters - // measurement noise covariance - float r_stddev_x = 0.4; // [m] - float r_stddev_y = 0.4; // [m] - float r_stddev_yaw = autoware::universe_utils::deg2rad(30); // [rad] - ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); - ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); - ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); - // OBJECT SHAPE MODEL - bounding_box_ = {0.5, 0.5, 1.7}; - cylinder_ = {0.5, 1.7}; + bounding_box_ = { + object_model_.init_size.length, object_model_.init_size.width, + object_model_.init_size.height}; // default value + cylinder_ = {object_model_.init_size.length, object_model_.init_size.height}; // default value if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; @@ -76,78 +69,70 @@ PedestrianTracker::PedestrianTracker( // do not update polygon shape } // set maximum and minimum size - constexpr double max_size = 5.0; - constexpr double min_size = 0.3; - bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); - bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); - bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); - cylinder_.width = std::min(std::max(cylinder_.width, min_size), max_size); - cylinder_.height = std::min(std::max(cylinder_.height, min_size), max_size); + bounding_box_.length = std::clamp( + bounding_box_.length, object_model_.size_limit.length_min, object_model_.size_limit.length_max); + bounding_box_.width = std::clamp( + bounding_box_.width, object_model_.size_limit.width_min, object_model_.size_limit.width_max); + bounding_box_.height = std::clamp( + bounding_box_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); + cylinder_.width = std::clamp( + cylinder_.width, object_model_.size_limit.length_min, object_model_.size_limit.length_max); + cylinder_.height = std::clamp( + cylinder_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); // Set motion model parameters { - constexpr double q_stddev_x = 0.5; // [m/s] - constexpr double q_stddev_y = 0.5; // [m/s] - constexpr double q_stddev_yaw = autoware::universe_utils::deg2rad(20); // [rad/s] - constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] - constexpr double q_stddev_wz = autoware::universe_utils::deg2rad(30); // [rad/(s*s)] + const double q_stddev_x = object_model_.process_noise.vel_long; + const double q_stddev_y = object_model_.process_noise.vel_lat; + const double q_stddev_yaw = object_model_.process_noise.yaw_rate; + const double q_stddev_vx = object_model_.process_noise.acc_long; + const double q_stddev_wz = object_model_.process_noise.acc_turn; motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vx, q_stddev_wz); } // Set motion limits - motion_model_.setMotionLimits( - autoware::universe_utils::kmph2mps(100), /* [m/s] maximum velocity */ - 30.0 /* [deg/s] maximum turn rate */ - ); + { + const double max_vel = object_model_.process_limit.vel_long_max; + const double max_turn_rate = object_model_.process_limit.yaw_rate_max; + motion_model_.setMotionLimits(max_vel, max_turn_rate); // maximum velocity and slip angle + } // Set initial state { + using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; const double x = object.kinematics.pose_with_covariance.pose.position.x; const double y = object.kinematics.pose_with_covariance.pose.position.y; const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - auto pose_cov = object.kinematics.pose_with_covariance.covariance; - double vel = 0.0; - double wz = 0.0; - double vel_cov; - double wz_cov; - - if (object.kinematics.has_twist) { - vel = object.kinematics.twist_with_covariance.twist.linear.x; - wz = object.kinematics.twist_with_covariance.twist.angular.z; - } + auto pose_cov = object.kinematics.pose_with_covariance.covariance; if (!object.kinematics.has_position_covariance) { // initial state covariance - constexpr double p0_stddev_x = 2.0; // in object coordinate [m] - constexpr double p0_stddev_y = 2.0; // in object coordinate [m] - constexpr double p0_stddev_yaw = - autoware::universe_utils::deg2rad(1000); // in map coordinate [rad] - constexpr double p0_cov_x = p0_stddev_x * p0_stddev_x; - constexpr double p0_cov_y = p0_stddev_y * p0_stddev_y; - constexpr double p0_cov_yaw = p0_stddev_yaw * p0_stddev_yaw; + 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[utils::MSG_COV_IDX::X_X] = - p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; - pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; - pose_cov[utils::MSG_COV_IDX::Y_Y] = - p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; - pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; - pose_cov[utils::MSG_COV_IDX::YAW_YAW] = p0_cov_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 wz = 0.0; + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; + wz = object.kinematics.twist_with_covariance.twist.angular.z; } - if (!object.kinematics.has_twist_covariance) { - constexpr double p0_stddev_vel = - autoware::universe_utils::kmph2mps(120); // in object coordinate [m/s] - constexpr double p0_stddev_wz = - autoware::universe_utils::deg2rad(360); // in object coordinate [rad/s] - vel_cov = std::pow(p0_stddev_vel, 2.0); - wz_cov = std::pow(p0_stddev_wz, 2.0); - } else { - vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - wz_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; + double vel_cov = object_model_.initial_covariance.vel_long; + double wz_cov = object_model_.initial_covariance.yaw_rate; + if (object.kinematics.has_twist_covariance) { + vel_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; + wz_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::YAW_YAW]; } // initialize motion model @@ -162,26 +147,46 @@ bool PedestrianTracker::predict(const rclcpp::Time & time) autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObject( const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) + const geometry_msgs::msg::Transform & /*self_transform*/) const { autoware_perception_msgs::msg::DetectedObject updating_object = object; // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { - const double & r_cov_x = ekf_params_.r_cov_x; - const double & r_cov_y = ekf_params_.r_cov_y; - auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + // measurement noise covariance + auto r_cov_x = object_model_.measurement_covariance.pos_x; + auto r_cov_y = object_model_.measurement_covariance.pos_y; + + // yaw angle fix const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const bool is_yaw_available = + object.kinematics.orientation_availability != + autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + + // fill covariance matrix + using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; const double cos_yaw = std::cos(pose_yaw); const double sin_yaw = std::sin(pose_yaw); - const double sin_2yaw = std::sin(2.0f * pose_yaw); - pose_cov[utils::MSG_COV_IDX::X_X] = - r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - pose_cov[utils::MSG_COV_IDX::Y_Y] = - r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; // y - x + const double sin_2yaw = std::sin(2.0 * pose_yaw); + pose_cov[XYZRPY_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[XYZRPY_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x + pose_cov[XYZRPY_COV_IDX::X_YAW] = 0.0; // x - yaw + pose_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0; // y - yaw + pose_cov[XYZRPY_COV_IDX::YAW_X] = 0.0; // yaw - x + pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y + pose_cov[XYZRPY_COV_IDX::YAW_YAW] = object_model_.measurement_covariance.yaw; // yaw - yaw + if (!is_yaw_available) { + pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value + } + auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; + twist_cov[XYZRPY_COV_IDX::X_X] = object_model_.measurement_covariance.vel_long; // vel - vel } + return updating_object; } @@ -216,43 +221,48 @@ bool PedestrianTracker::measureWithShape( // check bound box size abnormality constexpr double size_max = 30.0; // [m] constexpr double size_min = 0.1; // [m] - if ( - object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max || - object.shape.dimensions.z > size_max) { - return false; - } else if ( - object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min || - object.shape.dimensions.z < size_min) { + bool is_size_valid = + (object.shape.dimensions.x <= size_max && object.shape.dimensions.y <= size_max && + object.shape.dimensions.z <= size_max && object.shape.dimensions.x >= size_min && + object.shape.dimensions.y >= size_min && object.shape.dimensions.z >= size_min); + if (!is_size_valid) { return false; } // update bounding box size bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; + } else if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { // check cylinder size abnormality constexpr double size_max = 30.0; // [m] constexpr double size_min = 0.1; // [m] - if (object.shape.dimensions.x > size_max || object.shape.dimensions.z > size_max) { - return false; - } else if (object.shape.dimensions.x < size_min || object.shape.dimensions.z < size_min) { + bool is_size_valid = + (object.shape.dimensions.x <= size_max && object.shape.dimensions.z <= size_max && + object.shape.dimensions.x >= size_min && object.shape.dimensions.z >= size_min); + if (!is_size_valid) { return false; } + // update cylinder size cylinder_.width = gain_inv * cylinder_.width + gain * object.shape.dimensions.x; cylinder_.height = gain_inv * cylinder_.height + gain * object.shape.dimensions.z; + } else { // do not update polygon shape return false; } // set maximum and minimum size - constexpr double max_size = 5.0; - constexpr double min_size = 0.3; - bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); - bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); - bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); - cylinder_.width = std::min(std::max(cylinder_.width, min_size), max_size); - cylinder_.height = std::min(std::max(cylinder_.height, min_size), max_size); + bounding_box_.length = std::clamp( + bounding_box_.length, object_model_.size_limit.length_min, object_model_.size_limit.length_max); + bounding_box_.width = std::clamp( + bounding_box_.width, object_model_.size_limit.width_min, object_model_.size_limit.width_max); + bounding_box_.height = std::clamp( + bounding_box_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); + cylinder_.width = std::clamp( + cylinder_.width, object_model_.size_limit.length_min, object_model_.size_limit.length_max); + cylinder_.height = std::clamp( + cylinder_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); 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 8e484623ef0aa..6905d7c3b8ced 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -11,14 +11,18 @@ // 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. +#define EIGEN_MPL2_ONLY #include "multi_object_tracker/tracker/model/unknown_tracker.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include "multi_object_tracker/utils/utils.hpp" -#include -#include -#include +#include +#include #include #include @@ -32,10 +36,6 @@ #endif #include "object_recognition_utils/object_recognition_utils.hpp" -#define EIGEN_MPL2_ONLY -#include -#include - UnknownTracker::UnknownTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, @@ -73,6 +73,7 @@ UnknownTracker::UnknownTracker( // Set initial state { + using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; const double x = object.kinematics.pose_with_covariance.pose.position.x; const double y = object.kinematics.pose_with_covariance.pose.position.y; auto pose_cov = object.kinematics.pose_with_covariance.covariance; @@ -98,12 +99,10 @@ UnknownTracker::UnknownTracker( 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[utils::MSG_COV_IDX::X_X] = - p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; - pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; - pose_cov[utils::MSG_COV_IDX::Y_Y] = - p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; - pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; + 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]; } if (!object.kinematics.has_twist_covariance) { @@ -111,24 +110,24 @@ UnknownTracker::UnknownTracker( constexpr double p0_stddev_vy = autoware::universe_utils::kmph2mps(10); // [m/s] const double p0_cov_vx = std::pow(p0_stddev_vx, 2.0); const double p0_cov_vy = std::pow(p0_stddev_vy, 2.0); - twist_cov[utils::MSG_COV_IDX::X_X] = p0_cov_vx; - twist_cov[utils::MSG_COV_IDX::X_Y] = 0.0; - twist_cov[utils::MSG_COV_IDX::Y_X] = 0.0; - twist_cov[utils::MSG_COV_IDX::Y_Y] = p0_cov_vy; + twist_cov[XYZRPY_COV_IDX::X_X] = p0_cov_vx; + twist_cov[XYZRPY_COV_IDX::X_Y] = 0.0; + twist_cov[XYZRPY_COV_IDX::Y_X] = 0.0; + twist_cov[XYZRPY_COV_IDX::Y_Y] = p0_cov_vy; } // rotate twist covariance matrix, since it is in the vehicle coordinate system Eigen::MatrixXd twist_cov_rotate(2, 2); - twist_cov_rotate(0, 0) = twist_cov[utils::MSG_COV_IDX::X_X]; - twist_cov_rotate(0, 1) = twist_cov[utils::MSG_COV_IDX::X_Y]; - twist_cov_rotate(1, 0) = twist_cov[utils::MSG_COV_IDX::Y_X]; - twist_cov_rotate(1, 1) = twist_cov[utils::MSG_COV_IDX::Y_Y]; + twist_cov_rotate(0, 0) = twist_cov[XYZRPY_COV_IDX::X_X]; + twist_cov_rotate(0, 1) = twist_cov[XYZRPY_COV_IDX::X_Y]; + twist_cov_rotate(1, 0) = twist_cov[XYZRPY_COV_IDX::Y_X]; + twist_cov_rotate(1, 1) = twist_cov[XYZRPY_COV_IDX::Y_Y]; Eigen::MatrixXd R_yaw = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); Eigen::MatrixXd twist_cov_rotated = R_yaw * twist_cov_rotate * R_yaw.transpose(); - twist_cov[utils::MSG_COV_IDX::X_X] = twist_cov_rotated(0, 0); - twist_cov[utils::MSG_COV_IDX::X_Y] = twist_cov_rotated(0, 1); - twist_cov[utils::MSG_COV_IDX::Y_X] = twist_cov_rotated(1, 0); - twist_cov[utils::MSG_COV_IDX::Y_Y] = twist_cov_rotated(1, 1); + twist_cov[XYZRPY_COV_IDX::X_X] = twist_cov_rotated(0, 0); + twist_cov[XYZRPY_COV_IDX::X_Y] = twist_cov_rotated(0, 1); + twist_cov[XYZRPY_COV_IDX::Y_X] = twist_cov_rotated(1, 0); + twist_cov[XYZRPY_COV_IDX::Y_Y] = twist_cov_rotated(1, 1); // initialize motion model motion_model_.initialize(time, x, y, pose_cov, vx, vy, twist_cov); @@ -148,6 +147,8 @@ autoware_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject( // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { + // fill covariance matrix + using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; const double & r_cov_x = ekf_params_.r_cov_x; const double & r_cov_y = ekf_params_.r_cov_y; auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; @@ -155,12 +156,12 @@ autoware_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject( const double cos_yaw = std::cos(pose_yaw); const double sin_yaw = std::sin(pose_yaw); const double sin_2yaw = std::sin(2.0f * pose_yaw); - pose_cov[utils::MSG_COV_IDX::X_X] = - r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - pose_cov[utils::MSG_COV_IDX::Y_Y] = - r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; // y - x + pose_cov[XYZRPY_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[XYZRPY_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x } return updating_object; } diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp index f21855c2356ef..466108a2bef0e 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp @@ -15,25 +15,25 @@ // // Author: v1.0 Taekjin Lee // +#define EIGEN_MPL2_ONLY #include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" -#include -#include - -#define EIGEN_MPL2_ONLY #include #include // cspell: ignore CTRV // Bicycle CTRV motion model // CTRV : Constant Turn Rate and constant Velocity +using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; -BicycleMotionModel::BicycleMotionModel() -: MotionModel(), logger_(rclcpp::get_logger("BicycleMotionModel")) +BicycleMotionModel::BicycleMotionModel() : logger_(rclcpp::get_logger("BicycleMotionModel")) { // Initialize motion parameters setDefaultParams(); @@ -44,11 +44,16 @@ void BicycleMotionModel::setDefaultParams() // set default motion parameters constexpr double q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate - constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate - constexpr double q_stddev_slip_rate_min = 0.3; // [deg/s] uncertain slip angle change rate - constexpr double q_stddev_slip_rate_max = 10.0; // [deg/s] uncertain slip angle change rate - constexpr double q_max_slip_angle = 30.0; // [deg] max slip angle + constexpr double q_stddev_yaw_rate_min = + autoware::universe_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate + constexpr double q_stddev_yaw_rate_max = + autoware::universe_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate + constexpr double q_stddev_slip_rate_min = + autoware::universe_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate + constexpr double q_stddev_slip_rate_max = + autoware::universe_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate + constexpr double q_max_slip_angle = + autoware::universe_utils::deg2rad(30.0); // [rad] max slip angle // extended state parameters constexpr double lf_ratio = 0.3; // 30% front from the center constexpr double lf_min = 1.0; // minimum of 1.0m @@ -79,13 +84,13 @@ void BicycleMotionModel::setMotionParams( // set process noise covariance parameters motion_params_.q_stddev_acc_long = q_stddev_acc_long; motion_params_.q_stddev_acc_lat = q_stddev_acc_lat; - motion_params_.q_stddev_yaw_rate_min = autoware::universe_utils::deg2rad(q_stddev_yaw_rate_min); - motion_params_.q_stddev_yaw_rate_max = autoware::universe_utils::deg2rad(q_stddev_yaw_rate_max); - motion_params_.q_cov_slip_rate_min = - std::pow(autoware::universe_utils::deg2rad(q_stddev_slip_rate_min), 2.0); - motion_params_.q_cov_slip_rate_max = - std::pow(autoware::universe_utils::deg2rad(q_stddev_slip_rate_max), 2.0); - motion_params_.q_max_slip_angle = autoware::universe_utils::deg2rad(q_max_slip_angle); + motion_params_.q_cov_acc_long = q_stddev_acc_long * q_stddev_acc_long; + motion_params_.q_cov_acc_lat = q_stddev_acc_lat * q_stddev_acc_lat; + motion_params_.q_stddev_yaw_rate_min = q_stddev_yaw_rate_min; + motion_params_.q_stddev_yaw_rate_max = q_stddev_yaw_rate_max; + motion_params_.q_cov_slip_rate_min = q_stddev_slip_rate_min * q_stddev_slip_rate_min; + motion_params_.q_cov_slip_rate_max = q_stddev_slip_rate_max * q_stddev_slip_rate_max; + motion_params_.q_max_slip_angle = q_max_slip_angle; constexpr double minimum_wheel_pos = 0.01; // minimum of 0.01m if (lf_min < minimum_wheel_pos || lr_min < minimum_wheel_pos) { @@ -103,7 +108,7 @@ void BicycleMotionModel::setMotionLimits(const double & max_vel, const double & { // set motion limitations motion_params_.max_vel = max_vel; - motion_params_.max_slip = autoware::universe_utils::deg2rad(max_slip); + motion_params_.max_slip = max_slip; } bool BicycleMotionModel::initialize( @@ -117,9 +122,9 @@ bool BicycleMotionModel::initialize( // initialize covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); - P(IDX::X, IDX::X) = pose_cov[utils::MSG_COV_IDX::X_X]; - P(IDX::Y, IDX::Y) = pose_cov[utils::MSG_COV_IDX::Y_Y]; - P(IDX::YAW, IDX::YAW) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + P(IDX::X, IDX::X) = pose_cov[XYZRPY_COV_IDX::X_X]; + P(IDX::Y, IDX::Y) = pose_cov[XYZRPY_COV_IDX::Y_Y]; + P(IDX::YAW, IDX::YAW) = pose_cov[XYZRPY_COV_IDX::YAW_YAW]; P(IDX::VEL, IDX::VEL) = vel_cov; P(IDX::SLIP, IDX::SLIP) = slip_cov; @@ -147,10 +152,10 @@ bool BicycleMotionModel::updateStatePose( C(1, IDX::Y) = 1.0; Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); - R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; - R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; - R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_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]; return ekf_.update(Y, C, R); } @@ -186,15 +191,15 @@ bool BicycleMotionModel::updateStatePoseHead( C(2, IDX::YAW) = 1.0; Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); - R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; - R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; - R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; - R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; - R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; - R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; - R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + 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(0, 2) = pose_cov[XYZRPY_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[XYZRPY_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[XYZRPY_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[XYZRPY_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[XYZRPY_COV_IDX::YAW_YAW]; return ekf_.update(Y, C, R); } @@ -232,16 +237,16 @@ bool BicycleMotionModel::updateStatePoseHeadVel( C(3, IDX::VEL) = 1.0; Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); - R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; - R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; - R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; - R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; - R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; - R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; - R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; - R(3, 3) = twist_cov[utils::MSG_COV_IDX::X_X]; + 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(0, 2) = pose_cov[XYZRPY_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[XYZRPY_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[XYZRPY_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[XYZRPY_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[XYZRPY_COV_IDX::YAW_YAW]; + R(3, 3) = twist_cov[XYZRPY_COV_IDX::X_X]; return ekf_.update(Y, C, R); } @@ -354,10 +359,8 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; // Process noise covariance Q - double q_stddev_yaw_rate{0.0}; - if (vel <= 0.01) { - q_stddev_yaw_rate = motion_params_.q_stddev_yaw_rate_min; - } else { + double q_stddev_yaw_rate = motion_params_.q_stddev_yaw_rate_min; + if (vel > 0.01) { /* uncertainty of the yaw rate is limited by the following: * - centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v * - or maximum slip angle slip_max : w = v*sin(slip_max)/l_r @@ -365,8 +368,9 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c q_stddev_yaw_rate = std::min( motion_params_.q_stddev_acc_lat / vel, vel * std::sin(motion_params_.q_max_slip_angle) / lr_); // [rad/s] - q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, motion_params_.q_stddev_yaw_rate_max); - q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, motion_params_.q_stddev_yaw_rate_min); + q_stddev_yaw_rate = std::clamp( + q_stddev_yaw_rate, motion_params_.q_stddev_yaw_rate_min, + motion_params_.q_stddev_yaw_rate_max); } double q_cov_slip_rate{0.0}; if (vel <= 0.01) { @@ -384,11 +388,13 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c q_cov_slip_rate = std::min(q_cov_slip_rate, motion_params_.q_cov_slip_rate_max); q_cov_slip_rate = std::max(q_cov_slip_rate, motion_params_.q_cov_slip_rate_min); } - const double q_cov_x = std::pow(0.5 * motion_params_.q_stddev_acc_long * dt * dt, 2); - const double q_cov_y = std::pow(0.5 * motion_params_.q_stddev_acc_lat * dt * dt, 2); - const double q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); - const double q_cov_vel = std::pow(motion_params_.q_stddev_acc_long * dt, 2); - const double q_cov_slip = q_cov_slip_rate * dt * dt; + const double dt2 = dt * dt; + const double dt4 = dt2 * dt2; + const double q_cov_x = 0.25 * motion_params_.q_cov_acc_long * dt4; + const double q_cov_y = 0.25 * motion_params_.q_cov_acc_lat * dt4; + const double q_cov_yaw = q_stddev_yaw_rate * q_stddev_yaw_rate * dt2; + const double q_cov_vel = motion_params_.q_cov_acc_long * dt2; + const double q_cov_slip = q_cov_slip_rate * dt2; Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(DIM, DIM); // Rotate the covariance matrix according to the vehicle yaw @@ -445,14 +451,14 @@ bool BicycleMotionModel::getPredictedState( constexpr double zz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double rr_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double pp_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_cov[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - pose_cov[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_cov[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_cov[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); - pose_cov[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); - pose_cov[utils::MSG_COV_IDX::Z_Z] = zz_cov; - pose_cov[utils::MSG_COV_IDX::ROLL_ROLL] = rr_cov; - pose_cov[utils::MSG_COV_IDX::PITCH_PITCH] = pp_cov; + pose_cov[XYZRPY_COV_IDX::X_X] = P(IDX::X, IDX::X); + pose_cov[XYZRPY_COV_IDX::X_Y] = P(IDX::X, IDX::Y); + pose_cov[XYZRPY_COV_IDX::Y_X] = P(IDX::Y, IDX::X); + pose_cov[XYZRPY_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); + pose_cov[XYZRPY_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); + pose_cov[XYZRPY_COV_IDX::Z_Z] = zz_cov; + pose_cov[XYZRPY_COV_IDX::ROLL_ROLL] = rr_cov; + pose_cov[XYZRPY_COV_IDX::PITCH_PITCH] = pp_cov; // set twist covariance Eigen::MatrixXd cov_jacob(3, 2); @@ -466,18 +472,18 @@ bool BicycleMotionModel::getPredictedState( constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - twist_cov[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); - twist_cov[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); - twist_cov[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); - twist_cov[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); - twist_cov[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); - twist_cov[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); - twist_cov[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); - twist_cov[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); - twist_cov[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); - twist_cov[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_cov[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_cov[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; + twist_cov[XYZRPY_COV_IDX::X_X] = twist_cov_mat(0, 0); + twist_cov[XYZRPY_COV_IDX::X_Y] = twist_cov_mat(0, 1); + twist_cov[XYZRPY_COV_IDX::X_YAW] = twist_cov_mat(0, 2); + twist_cov[XYZRPY_COV_IDX::Y_X] = twist_cov_mat(1, 0); + twist_cov[XYZRPY_COV_IDX::Y_Y] = twist_cov_mat(1, 1); + twist_cov[XYZRPY_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); + twist_cov[XYZRPY_COV_IDX::YAW_X] = twist_cov_mat(2, 0); + twist_cov[XYZRPY_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); + twist_cov[XYZRPY_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); + twist_cov[XYZRPY_COV_IDX::Z_Z] = vz_cov; + twist_cov[XYZRPY_COV_IDX::ROLL_ROLL] = wx_cov; + twist_cov[XYZRPY_COV_IDX::PITCH_PITCH] = wy_cov; return true; } diff --git a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp index 79ce6e5dbb61c..b19af1d26d162 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp @@ -15,23 +15,24 @@ // // Author: v1.0 Taekjin Lee // +#define EIGEN_MPL2_ONLY #include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" +#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" -#include -#include - -#define EIGEN_MPL2_ONLY #include #include // cspell: ignore CTRV // Constant Turn Rate and constant Velocity (CTRV) motion model +using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; -CTRVMotionModel::CTRVMotionModel() : MotionModel(), logger_(rclcpp::get_logger("CTRVMotionModel")) +CTRVMotionModel::CTRVMotionModel() : logger_(rclcpp::get_logger("CTRVMotionModel")) { // Initialize motion parameters setDefaultParams(); @@ -63,11 +64,11 @@ void CTRVMotionModel::setMotionParams( const double & q_stddev_vel, const double & q_stddev_wz) { // set process noise covariance parameters - motion_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - motion_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - motion_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); - motion_params_.q_cov_vel = std::pow(q_stddev_vel, 2.0); - motion_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); + motion_params_.q_cov_x = q_stddev_x * q_stddev_x; + motion_params_.q_cov_y = q_stddev_y * q_stddev_y; + motion_params_.q_cov_yaw = q_stddev_yaw * q_stddev_yaw; + motion_params_.q_cov_vel = q_stddev_vel * q_stddev_vel; + motion_params_.q_cov_wz = q_stddev_wz * q_stddev_wz; } void CTRVMotionModel::setMotionLimits(const double & max_vel, const double & max_wz) @@ -88,9 +89,9 @@ bool CTRVMotionModel::initialize( // initialize covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); - P(IDX::X, IDX::X) = pose_cov[utils::MSG_COV_IDX::X_X]; - P(IDX::Y, IDX::Y) = pose_cov[utils::MSG_COV_IDX::Y_Y]; - P(IDX::YAW, IDX::YAW) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + P(IDX::X, IDX::X) = pose_cov[XYZRPY_COV_IDX::X_X]; + P(IDX::Y, IDX::Y) = pose_cov[XYZRPY_COV_IDX::Y_Y]; + P(IDX::YAW, IDX::YAW) = pose_cov[XYZRPY_COV_IDX::YAW_YAW]; P(IDX::VEL, IDX::VEL) = vel_cov; P(IDX::WZ, IDX::WZ) = wz_cov; @@ -115,10 +116,10 @@ bool CTRVMotionModel::updateStatePose( C(1, IDX::Y) = 1.0; Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); - R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; - R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; - R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_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]; return ekf_.update(Y, C, R); } @@ -154,15 +155,15 @@ bool CTRVMotionModel::updateStatePoseHead( C(2, IDX::YAW) = 1.0; Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); - R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; - R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; - R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; - R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; - R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; - R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; - R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + 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(0, 2) = pose_cov[XYZRPY_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[XYZRPY_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[XYZRPY_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[XYZRPY_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[XYZRPY_COV_IDX::YAW_YAW]; return ekf_.update(Y, C, R); } @@ -200,16 +201,16 @@ bool CTRVMotionModel::updateStatePoseHeadVel( C(3, IDX::VEL) = 1.0; Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); - R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; - R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; - R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; - R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; - R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; - R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; - R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; - R(3, 3) = twist_cov[utils::MSG_COV_IDX::X_X]; + 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(0, 2) = pose_cov[XYZRPY_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[XYZRPY_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[XYZRPY_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[XYZRPY_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[XYZRPY_COV_IDX::YAW_YAW]; + R(3, 3) = twist_cov[XYZRPY_COV_IDX::X_X]; return ekf_.update(Y, C, R); } @@ -295,11 +296,12 @@ bool CTRVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) cons A(IDX::YAW, IDX::WZ) = dt; // Process noise covariance Q - const double q_cov_x = std::pow(0.5 * motion_params_.q_cov_x * dt, 2); - const double q_cov_y = std::pow(0.5 * motion_params_.q_cov_y * dt, 2); - const double q_cov_yaw = std::pow(motion_params_.q_cov_yaw * dt, 2); - const double q_cov_vel = std::pow(motion_params_.q_cov_vel * dt, 2); - const double q_cov_wz = std::pow(motion_params_.q_cov_wz * dt, 2); + const double dt2 = dt * dt; + const double q_cov_x = motion_params_.q_cov_x * dt2; + const double q_cov_y = motion_params_.q_cov_y * dt2; + const double q_cov_yaw = motion_params_.q_cov_yaw * dt2; + const double q_cov_vel = motion_params_.q_cov_vel * dt2; + const double q_cov_wz = motion_params_.q_cov_wz * dt2; Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(DIM, DIM); // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. @@ -355,32 +357,32 @@ bool CTRVMotionModel::getPredictedState( constexpr double zz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double rr_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double pp_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_cov[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - pose_cov[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_cov[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_cov[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); - pose_cov[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); - pose_cov[utils::MSG_COV_IDX::Z_Z] = zz_cov; - pose_cov[utils::MSG_COV_IDX::ROLL_ROLL] = rr_cov; - pose_cov[utils::MSG_COV_IDX::PITCH_PITCH] = pp_cov; + pose_cov[XYZRPY_COV_IDX::X_X] = P(IDX::X, IDX::X); + pose_cov[XYZRPY_COV_IDX::X_Y] = P(IDX::X, IDX::Y); + pose_cov[XYZRPY_COV_IDX::Y_X] = P(IDX::Y, IDX::X); + pose_cov[XYZRPY_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); + pose_cov[XYZRPY_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); + pose_cov[XYZRPY_COV_IDX::Z_Z] = zz_cov; + pose_cov[XYZRPY_COV_IDX::ROLL_ROLL] = rr_cov; + pose_cov[XYZRPY_COV_IDX::PITCH_PITCH] = pp_cov; // set twist covariance constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - twist_cov[utils::MSG_COV_IDX::X_X] = P(IDX::VEL, IDX::VEL); - twist_cov[utils::MSG_COV_IDX::X_Y] = 0.0; - twist_cov[utils::MSG_COV_IDX::X_YAW] = P(IDX::VEL, IDX::WZ); - twist_cov[utils::MSG_COV_IDX::Y_X] = 0.0; - twist_cov[utils::MSG_COV_IDX::Y_Y] = vy_cov; - twist_cov[utils::MSG_COV_IDX::Y_YAW] = 0.0; - twist_cov[utils::MSG_COV_IDX::YAW_X] = P(IDX::WZ, IDX::VEL); - twist_cov[utils::MSG_COV_IDX::YAW_Y] = 0.0; - twist_cov[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); - twist_cov[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_cov[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_cov[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; + twist_cov[XYZRPY_COV_IDX::X_X] = P(IDX::VEL, IDX::VEL); + twist_cov[XYZRPY_COV_IDX::X_Y] = 0.0; + twist_cov[XYZRPY_COV_IDX::X_YAW] = P(IDX::VEL, IDX::WZ); + twist_cov[XYZRPY_COV_IDX::Y_X] = 0.0; + twist_cov[XYZRPY_COV_IDX::Y_Y] = vy_cov; + twist_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0; + twist_cov[XYZRPY_COV_IDX::YAW_X] = P(IDX::WZ, IDX::VEL); + twist_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; + twist_cov[XYZRPY_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); + twist_cov[XYZRPY_COV_IDX::Z_Z] = vz_cov; + twist_cov[XYZRPY_COV_IDX::ROLL_ROLL] = wx_cov; + twist_cov[XYZRPY_COV_IDX::PITCH_PITCH] = wy_cov; return true; } diff --git a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp index 8b3e9014f52d7..231ba73796ed9 100644 --- a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp +++ b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp @@ -15,25 +15,26 @@ // // Author: v1.0 Taekjin Lee // +#define EIGEN_MPL2_ONLY #include "multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" +#include "autoware/universe_utils/math/normalization.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" #include "multi_object_tracker/utils/utils.hpp" -#include -#include - -#include - -#define EIGEN_MPL2_ONLY #include #include +#include + // cspell: ignore CV // Constant Velocity (CV) motion model +using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; -CVMotionModel::CVMotionModel() : MotionModel(), logger_(rclcpp::get_logger("CVMotionModel")) +CVMotionModel::CVMotionModel() : logger_(rclcpp::get_logger("CVMotionModel")) { // Initialize motion parameters setDefaultParams(); @@ -63,10 +64,10 @@ void CVMotionModel::setMotionParams( const double & q_stddev_vy) { // set process noise covariance parameters - motion_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - motion_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - motion_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - motion_params_.q_cov_vy = std::pow(q_stddev_vy, 2.0); + motion_params_.q_cov_x = q_stddev_x * q_stddev_x; + motion_params_.q_cov_y = q_stddev_y * q_stddev_y; + motion_params_.q_cov_vx = q_stddev_vx * q_stddev_vx; + motion_params_.q_cov_vy = q_stddev_vy * q_stddev_vy; } void CVMotionModel::setMotionLimits(const double & max_vx, const double & max_vy) @@ -87,10 +88,10 @@ bool CVMotionModel::initialize( // initialize covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); - P(IDX::X, IDX::X) = pose_cov[utils::MSG_COV_IDX::X_X]; - P(IDX::Y, IDX::Y) = pose_cov[utils::MSG_COV_IDX::Y_Y]; - P(IDX::VX, IDX::VX) = twist_cov[utils::MSG_COV_IDX::X_X]; - P(IDX::VY, IDX::VY) = twist_cov[utils::MSG_COV_IDX::Y_Y]; + P(IDX::X, IDX::X) = pose_cov[XYZRPY_COV_IDX::X_X]; + P(IDX::Y, IDX::Y) = pose_cov[XYZRPY_COV_IDX::Y_Y]; + P(IDX::VX, IDX::VX) = twist_cov[XYZRPY_COV_IDX::X_X]; + P(IDX::VY, IDX::VY) = twist_cov[XYZRPY_COV_IDX::Y_Y]; return MotionModel::initialize(time, X, P); } @@ -113,10 +114,10 @@ bool CVMotionModel::updateStatePose( C(1, IDX::Y) = 1.0; Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); - R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; - R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; - R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_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]; return ekf_.update(Y, C, R); } @@ -142,14 +143,14 @@ bool CVMotionModel::updateStatePoseVel( C(3, IDX::VY) = 1.0; Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); - R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; - R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; - R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; - R(2, 2) = twist_cov[utils::MSG_COV_IDX::X_X]; - R(2, 3) = twist_cov[utils::MSG_COV_IDX::X_Y]; - R(3, 2) = twist_cov[utils::MSG_COV_IDX::Y_X]; - R(3, 3) = twist_cov[utils::MSG_COV_IDX::Y_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]; + R(2, 3) = twist_cov[XYZRPY_COV_IDX::X_Y]; + R(3, 2) = twist_cov[XYZRPY_COV_IDX::Y_X]; + R(3, 3) = twist_cov[XYZRPY_COV_IDX::Y_Y]; return ekf_.update(Y, C, R); } @@ -272,14 +273,14 @@ bool CVMotionModel::getPredictedState( constexpr double rr_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double pp_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double yaw_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_cov[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - pose_cov[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_cov[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_cov[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); - pose_cov[utils::MSG_COV_IDX::Z_Z] = zz_cov; - pose_cov[utils::MSG_COV_IDX::ROLL_ROLL] = rr_cov; - pose_cov[utils::MSG_COV_IDX::PITCH_PITCH] = pp_cov; - pose_cov[utils::MSG_COV_IDX::YAW_YAW] = yaw_cov; + pose_cov[XYZRPY_COV_IDX::X_X] = P(IDX::X, IDX::X); + pose_cov[XYZRPY_COV_IDX::X_Y] = P(IDX::X, IDX::Y); + pose_cov[XYZRPY_COV_IDX::Y_X] = P(IDX::Y, IDX::X); + pose_cov[XYZRPY_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); + pose_cov[XYZRPY_COV_IDX::Z_Z] = zz_cov; + pose_cov[XYZRPY_COV_IDX::ROLL_ROLL] = rr_cov; + pose_cov[XYZRPY_COV_IDX::PITCH_PITCH] = pp_cov; + pose_cov[XYZRPY_COV_IDX::YAW_YAW] = yaw_cov; // set twist covariance constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative @@ -294,14 +295,14 @@ bool CVMotionModel::getPredictedState( twist_cov_rotate(1, 1) = P(IDX::VY, IDX::VY); Eigen::MatrixXd R_yaw = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); Eigen::MatrixXd twist_cov_rotated = R_yaw * twist_cov_rotate * R_yaw.transpose(); - twist_cov[utils::MSG_COV_IDX::X_X] = twist_cov_rotated(0, 0); - twist_cov[utils::MSG_COV_IDX::X_Y] = twist_cov_rotated(0, 1); - twist_cov[utils::MSG_COV_IDX::Y_X] = twist_cov_rotated(1, 0); - twist_cov[utils::MSG_COV_IDX::Y_Y] = twist_cov_rotated(1, 1); - twist_cov[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_cov[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_cov[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_cov[utils::MSG_COV_IDX::YAW_YAW] = wz_cov; + twist_cov[XYZRPY_COV_IDX::X_X] = twist_cov_rotated(0, 0); + twist_cov[XYZRPY_COV_IDX::X_Y] = twist_cov_rotated(0, 1); + twist_cov[XYZRPY_COV_IDX::Y_X] = twist_cov_rotated(1, 0); + twist_cov[XYZRPY_COV_IDX::Y_Y] = twist_cov_rotated(1, 1); + twist_cov[XYZRPY_COV_IDX::Z_Z] = vz_cov; + twist_cov[XYZRPY_COV_IDX::ROLL_ROLL] = wx_cov; + twist_cov[XYZRPY_COV_IDX::PITCH_PITCH] = wy_cov; + twist_cov[XYZRPY_COV_IDX::YAW_YAW] = wz_cov; return true; } From e497ef7d0b088ccfc4526eaad42333dac506e899 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 24 Jun 2024 18:35:38 +0900 Subject: [PATCH 2/8] chore: refactor file paths in tracking_object_merger module Signed-off-by: Taekjin LEE --- .../tracking_object_merger/CMakeLists.txt | 14 +++++------ .../association}/data_association.hpp | 24 +++++++++---------- .../association}/solver/gnn_solver.hpp | 12 +++++----- .../solver/gnn_solver_interface.hpp | 6 ++--- .../association/solver/mu_ssp.hpp} | 8 +++---- .../association/solver/ssp.hpp} | 8 +++---- .../decorative_tracker_merger.hpp | 18 +++++++------- .../utils/tracker_state.hpp | 14 +++++------ .../tracking_object_merger/utils/utils.hpp | 16 ++++++------- .../decorative_tracker_merger.launch.xml | 2 +- .../data_association.cpp | 6 ++--- .../mu_successive_shortest_path_wrapper.cpp | 2 +- .../ssp}/successive_shortest_path.cpp | 2 +- ...cpp => decorative_tracker_merger_node.cpp} | 14 +++++------ .../src/utils/tracker_state.cpp | 4 ++-- .../src/utils/utils.cpp | 8 +++---- 16 files changed, 79 insertions(+), 79 deletions(-) rename perception/tracking_object_merger/include/{tracking_object_merger/data_association => autoware/tracking_object_merger/association}/data_association.hpp (80%) rename perception/tracking_object_merger/include/{tracking_object_merger/data_association => autoware/tracking_object_merger/association}/solver/gnn_solver.hpp (55%) rename perception/tracking_object_merger/include/{tracking_object_merger/data_association => autoware/tracking_object_merger/association}/solver/gnn_solver_interface.hpp (78%) rename perception/tracking_object_merger/include/{tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp => autoware/tracking_object_merger/association/solver/mu_ssp.hpp} (73%) rename perception/tracking_object_merger/include/{tracking_object_merger/data_association/solver/successive_shortest_path.hpp => autoware/tracking_object_merger/association/solver/ssp.hpp} (73%) rename perception/tracking_object_merger/include/{ => autoware}/tracking_object_merger/decorative_tracker_merger.hpp (88%) rename perception/tracking_object_merger/include/{ => autoware}/tracking_object_merger/utils/tracker_state.hpp (92%) rename perception/tracking_object_merger/include/{ => autoware}/tracking_object_merger/utils/utils.hpp (87%) rename perception/tracking_object_merger/src/{data_association => association}/data_association.cpp (97%) rename perception/tracking_object_merger/src/{data_association/mu_successive_shortest_path => association/mu_ssp}/mu_successive_shortest_path_wrapper.cpp (92%) rename perception/tracking_object_merger/src/{data_association/successive_shortest_path => association/ssp}/successive_shortest_path.cpp (99%) rename perception/tracking_object_merger/src/{decorative_tracker_merger.cpp => decorative_tracker_merger_node.cpp} (98%) diff --git a/perception/tracking_object_merger/CMakeLists.txt b/perception/tracking_object_merger/CMakeLists.txt index 9c8e5a321d9be..db409f136d494 100644 --- a/perception/tracking_object_merger/CMakeLists.txt +++ b/perception/tracking_object_merger/CMakeLists.txt @@ -22,22 +22,22 @@ include_directories( ${EIGEN3_INCLUDE_DIR} ) -ament_auto_add_library(decorative_tracker_merger_node SHARED - src/data_association/data_association.cpp - src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp - src/decorative_tracker_merger.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/association/data_association.cpp + src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp + src/decorative_tracker_merger_node.cpp src/utils/utils.cpp src/utils/tracker_state.cpp ) -target_link_libraries(decorative_tracker_merger_node +target_link_libraries(${PROJECT_NAME} Eigen3::Eigen glog::glog ) -rclcpp_components_register_node(decorative_tracker_merger_node +rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "tracking_object_merger::DecorativeTrackerMergerNode" - EXECUTABLE decorative_tracker_merger + EXECUTABLE decorative_tracker_merger_node ) ament_auto_package(INSTALL_TO_SHARE diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp b/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/data_association.hpp similarity index 80% rename from perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp rename to perception/tracking_object_merger/include/autoware/tracking_object_merger/association/data_association.hpp index 96a474f9dac95..7ef1507160a36 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp +++ b/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/data_association.hpp @@ -16,25 +16,25 @@ // Author: v1.0 Yukihiro Saito // -#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ -#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ - -#include -#include -#include -#include +#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ +#define AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ #define EIGEN_MPL2_ONLY -#include "tracking_object_merger/data_association/solver/gnn_solver.hpp" -#include "tracking_object_merger/utils/tracker_state.hpp" + +#include "autoware/tracking_object_merger/association/solver/gnn_solver.hpp" +#include "autoware/tracking_object_merger/utils/tracker_state.hpp" #include #include -#include -#include +#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include "autoware_perception_msgs/msg/tracked_objects.hpp" +#include +#include #include +#include +#include class DataAssociation { @@ -68,4 +68,4 @@ class DataAssociation virtual ~DataAssociation() {} }; -#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +#endif // AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver.hpp b/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/gnn_solver.hpp similarity index 55% rename from perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver.hpp rename to perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/gnn_solver.hpp index 31240848f1977..64a18d878f7c3 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver.hpp +++ b/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/gnn_solver.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#define AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#include "tracking_object_merger/data_association/solver/gnn_solver_interface.hpp" -#include "tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp" -#include "tracking_object_merger/data_association/solver/successive_shortest_path.hpp" +#include "autoware/tracking_object_merger/association/solver/gnn_solver_interface.hpp" +#include "autoware/tracking_object_merger/association/solver/mu_ssp.hpp" +#include "autoware/tracking_object_merger/association/solver/ssp.hpp" -#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#endif // AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver_interface.hpp b/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/gnn_solver_interface.hpp similarity index 78% rename from perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver_interface.hpp rename to perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/gnn_solver_interface.hpp index e915b5d2a9e7b..a25d235edc1d1 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver_interface.hpp +++ b/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/gnn_solver_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ -#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#define AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ #include #include @@ -32,4 +32,4 @@ class GnnSolverInterface }; } // namespace gnn_solver -#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#endif // AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp b/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/mu_ssp.hpp similarity index 73% rename from perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp rename to perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/mu_ssp.hpp index 5c0ebc04fdde3..682c1a38cae4b 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp +++ b/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/mu_ssp.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ -#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ +#define AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ -#include "tracking_object_merger/data_association/solver/gnn_solver_interface.hpp" +#include "autoware/tracking_object_merger/association/solver/gnn_solver_interface.hpp" #include #include @@ -34,4 +34,4 @@ class MuSSP : public GnnSolverInterface }; } // namespace gnn_solver -#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#endif // AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/successive_shortest_path.hpp b/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/ssp.hpp similarity index 73% rename from perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/successive_shortest_path.hpp rename to perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/ssp.hpp index 47a737cf58298..1da67d29f8718 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/successive_shortest_path.hpp +++ b/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/ssp.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ -#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ +#define AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ -#include "tracking_object_merger/data_association/solver/gnn_solver_interface.hpp" +#include "autoware/tracking_object_merger/association/solver/gnn_solver_interface.hpp" #include #include @@ -34,4 +34,4 @@ class SSP : public GnnSolverInterface }; } // namespace gnn_solver -#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#endif // AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp b/perception/tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger.hpp similarity index 88% rename from perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp rename to perception/tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger.hpp index 3683ba02b38cd..c758e1752d8d8 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp +++ b/perception/tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_ -#define TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_ +#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_ +#define AUTOWARE__TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_ -#include "tracking_object_merger/data_association/data_association.hpp" -#include "tracking_object_merger/utils/tracker_state.hpp" -#include "tracking_object_merger/utils/utils.hpp" +#include "autoware/tracking_object_merger/association/data_association.hpp" +#include "autoware/tracking_object_merger/utils/tracker_state.hpp" +#include "autoware/tracking_object_merger/utils/utils.hpp" +#include "autoware/universe_utils/ros/debug_publisher.hpp" +#include "autoware/universe_utils/ros/published_time_publisher.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" -#include -#include -#include #include #include @@ -133,4 +133,4 @@ class DecorativeTrackerMergerNode : public rclcpp::Node } // namespace tracking_object_merger -#endif // TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_ +#endif // AUTOWARE__TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp b/perception/tracking_object_merger/include/autoware/tracking_object_merger/utils/tracker_state.hpp similarity index 92% rename from perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp rename to perception/tracking_object_merger/include/autoware/tracking_object_merger/utils/tracker_state.hpp index a92cd9194b26a..63bd6cde35e41 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp +++ b/perception/tracking_object_merger/include/autoware/tracking_object_merger/utils/tracker_state.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ -#define TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ +#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ +#define AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ #include #include #include -#include -#include -#include -#include +#include "autoware_perception_msgs/msg/object_classification.hpp" +#include "autoware_perception_msgs/msg/tracked_object.hpp" +#include "autoware_perception_msgs/msg/tracked_object_kinematics.hpp" +#include "autoware_perception_msgs/msg/tracked_objects.hpp" #include #include @@ -145,4 +145,4 @@ class TrackerState TrackedObjects getTrackedObjectsFromTrackerStates( std::vector & tracker_states, const rclcpp::Time & time); -#endif // TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ +#endif // AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp b/perception/tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp similarity index 87% rename from perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp rename to perception/tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp index 03352512d5545..4cfa5d8e6d6bd 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp +++ b/perception/tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp @@ -14,19 +14,19 @@ // // -#ifndef TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ -#define TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ +#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ +#define AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ // #include #include "autoware/universe_utils/geometry/geometry.hpp" #include -#include -#include -#include -#include -#include +#include "autoware_perception_msgs/msg/object_classification.hpp" +#include "autoware_perception_msgs/msg/shape.hpp" +#include "autoware_perception_msgs/msg/tracked_object.hpp" +#include "autoware_perception_msgs/msg/tracked_object_kinematics.hpp" +#include "autoware_perception_msgs/msg/tracked_objects.hpp" #include #include #include @@ -132,4 +132,4 @@ void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & su } // namespace merger_utils -#endif // TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ +#endif // AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ diff --git a/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml b/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml index 52b0841f97e97..2cc2a69e295e6 100644 --- a/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml +++ b/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml @@ -7,7 +7,7 @@ - + diff --git a/perception/tracking_object_merger/src/data_association/data_association.cpp b/perception/tracking_object_merger/src/association/data_association.cpp similarity index 97% rename from perception/tracking_object_merger/src/data_association/data_association.cpp rename to perception/tracking_object_merger/src/association/data_association.cpp index 4d10a415df7da..720f3be34f52a 100644 --- a/perception/tracking_object_merger/src/data_association/data_association.cpp +++ b/perception/tracking_object_merger/src/association/data_association.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tracking_object_merger/data_association/data_association.hpp" +#include "autoware/tracking_object_merger/association/data_association.hpp" +#include "autoware/tracking_object_merger/association/solver/gnn_solver.hpp" +#include "autoware/tracking_object_merger/utils/utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tracking_object_merger/data_association/solver/gnn_solver.hpp" -#include "tracking_object_merger/utils/utils.hpp" #include #include diff --git a/perception/tracking_object_merger/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp b/perception/tracking_object_merger/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp similarity index 92% rename from perception/tracking_object_merger/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp rename to perception/tracking_object_merger/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp index fcc79c0a3cbd7..4e7ed56f92891 100644 --- a/perception/tracking_object_merger/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp +++ b/perception/tracking_object_merger/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp" +#include "autoware/tracking_object_merger/association/solver/mu_ssp.hpp" #include diff --git a/perception/tracking_object_merger/src/data_association/successive_shortest_path/successive_shortest_path.cpp b/perception/tracking_object_merger/src/association/ssp/successive_shortest_path.cpp similarity index 99% rename from perception/tracking_object_merger/src/data_association/successive_shortest_path/successive_shortest_path.cpp rename to perception/tracking_object_merger/src/association/ssp/successive_shortest_path.cpp index 133f0d377373f..2da4332bec020 100644 --- a/perception/tracking_object_merger/src/data_association/successive_shortest_path/successive_shortest_path.cpp +++ b/perception/tracking_object_merger/src/association/ssp/successive_shortest_path.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tracking_object_merger/data_association/solver/successive_shortest_path.hpp" +#include "autoware/tracking_object_merger/association/solver/ssp.hpp" #include #include diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp similarity index 98% rename from perception/tracking_object_merger/src/decorative_tracker_merger.cpp rename to perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp index 99dccab3ff72c..53074734c0efa 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp +++ b/perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp @@ -12,11 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tracking_object_merger/decorative_tracker_merger.hpp" +#define EIGEN_MPL2_ONLY +#include "autoware/tracking_object_merger/association/solver/ssp.hpp" +#include "autoware/tracking_object_merger/decorative_tracker_merger.hpp" +#include "autoware/tracking_object_merger/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tracking_object_merger/data_association/solver/successive_shortest_path.hpp" -#include "tracking_object_merger/utils/utils.hpp" + +#include +#include #include @@ -25,10 +29,6 @@ #include #include -#define EIGEN_MPL2_ONLY -#include -#include - using Label = autoware_perception_msgs::msg::ObjectClassification; namespace tracking_object_merger diff --git a/perception/tracking_object_merger/src/utils/tracker_state.cpp b/perception/tracking_object_merger/src/utils/tracker_state.cpp index 36da060a962c3..6bdf5eb8196fd 100644 --- a/perception/tracking_object_merger/src/utils/tracker_state.cpp +++ b/perception/tracking_object_merger/src/utils/tracker_state.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tracking_object_merger/utils/tracker_state.hpp" +#include "autoware/tracking_object_merger/utils/tracker_state.hpp" -#include "tracking_object_merger/utils/utils.hpp" +#include "autoware/tracking_object_merger/utils/utils.hpp" using autoware_perception_msgs::msg::TrackedObject; using autoware_perception_msgs::msg::TrackedObjects; diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/tracking_object_merger/src/utils/utils.cpp index 90437d3ebd7aa..8da35977ce158 100644 --- a/perception/tracking_object_merger/src/utils/utils.cpp +++ b/perception/tracking_object_merger/src/utils/utils.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tracking_object_merger/utils/utils.hpp" +#include "autoware/tracking_object_merger/utils/utils.hpp" -#include -#include -#include +#include "autoware_perception_msgs/msg/shape.hpp" +#include "autoware_perception_msgs/msg/tracked_object.hpp" +#include "autoware_perception_msgs/msg/tracked_objects.hpp" #include #include From ce17b55ecb0c6bdff2fe4926030e6abc1e4aa6e7 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 24 Jun 2024 19:01:05 +0900 Subject: [PATCH 3/8] chore: refactor file paths in tracking_object_merger module Signed-off-by: Taekjin LEE --- .../tracking_object_merger/CMakeLists.txt | 2 +- .../association/data_association.hpp | 4 ++ .../solver/gnn_solver_interface.hpp | 4 +- .../association/solver/mu_ssp.hpp | 4 +- .../association/solver/ssp.hpp | 4 +- .../decorative_tracker_merger.hpp | 4 +- .../utils/tracker_state.hpp | 4 ++ .../tracking_object_merger/utils/utils.hpp | 47 ++++--------------- .../src/association/data_association.cpp | 4 ++ .../mu_successive_shortest_path_wrapper.cpp | 4 +- .../ssp/successive_shortest_path.cpp | 4 +- .../src/decorative_tracker_merger_node.cpp | 6 +-- .../src/utils/tracker_state.cpp | 5 ++ .../src/utils/utils.cpp | 8 ++-- 14 files changed, 45 insertions(+), 59 deletions(-) diff --git a/perception/tracking_object_merger/CMakeLists.txt b/perception/tracking_object_merger/CMakeLists.txt index db409f136d494..db327b48f0ecd 100644 --- a/perception/tracking_object_merger/CMakeLists.txt +++ b/perception/tracking_object_merger/CMakeLists.txt @@ -36,7 +36,7 @@ target_link_libraries(${PROJECT_NAME} ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "tracking_object_merger::DecorativeTrackerMergerNode" + PLUGIN "autoware::tracking_object_merger::DecorativeTrackerMergerNode" EXECUTABLE decorative_tracker_merger_node ) diff --git a/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/data_association.hpp b/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/data_association.hpp index 7ef1507160a36..af4a65869eca5 100644 --- a/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/data_association.hpp +++ b/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/data_association.hpp @@ -36,6 +36,8 @@ #include #include +namespace autoware::tracking_object_merger +{ class DataAssociation { private: @@ -68,4 +70,6 @@ class DataAssociation virtual ~DataAssociation() {} }; +} // namespace autoware::tracking_object_merger + #endif // AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ diff --git a/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/gnn_solver_interface.hpp b/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/gnn_solver_interface.hpp index a25d235edc1d1..732ed5cd87041 100644 --- a/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/gnn_solver_interface.hpp +++ b/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/gnn_solver_interface.hpp @@ -18,7 +18,7 @@ #include #include -namespace gnn_solver +namespace autoware::tracking_object_merger::gnn_solver { class GnnSolverInterface { @@ -30,6 +30,6 @@ class GnnSolverInterface const std::vector> & cost, std::unordered_map * direct_assignment, std::unordered_map * reverse_assignment) = 0; }; -} // namespace gnn_solver +} // namespace autoware::tracking_object_merger::gnn_solver #endif // AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ diff --git a/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/mu_ssp.hpp b/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/mu_ssp.hpp index 682c1a38cae4b..b219c1ac441eb 100644 --- a/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/mu_ssp.hpp +++ b/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/mu_ssp.hpp @@ -20,7 +20,7 @@ #include #include -namespace gnn_solver +namespace autoware::tracking_object_merger::gnn_solver { class MuSSP : public GnnSolverInterface { @@ -32,6 +32,6 @@ class MuSSP : public GnnSolverInterface const std::vector> & cost, std::unordered_map * direct_assignment, std::unordered_map * reverse_assignment) override; }; -} // namespace gnn_solver +} // namespace autoware::tracking_object_merger::gnn_solver #endif // AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ diff --git a/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/ssp.hpp b/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/ssp.hpp index 1da67d29f8718..03311e4e43694 100644 --- a/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/ssp.hpp +++ b/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/ssp.hpp @@ -20,7 +20,7 @@ #include #include -namespace gnn_solver +namespace autoware::tracking_object_merger::gnn_solver { class SSP : public GnnSolverInterface { @@ -32,6 +32,6 @@ class SSP : public GnnSolverInterface const std::vector> & cost, std::unordered_map * direct_assignment, std::unordered_map * reverse_assignment) override; }; -} // namespace gnn_solver +} // namespace autoware::tracking_object_merger::gnn_solver #endif // AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ diff --git a/perception/tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger.hpp b/perception/tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger.hpp index c758e1752d8d8..027f06ea416cd 100644 --- a/perception/tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger.hpp +++ b/perception/tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger.hpp @@ -43,7 +43,7 @@ #include #include -namespace tracking_object_merger +namespace autoware::tracking_object_merger { class DecorativeTrackerMergerNode : public rclcpp::Node @@ -131,6 +131,6 @@ class DecorativeTrackerMergerNode : public rclcpp::Node } logging_; }; -} // namespace tracking_object_merger +} // namespace autoware::tracking_object_merger #endif // AUTOWARE__TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_ diff --git a/perception/tracking_object_merger/include/autoware/tracking_object_merger/utils/tracker_state.hpp b/perception/tracking_object_merger/include/autoware/tracking_object_merger/utils/tracker_state.hpp index 63bd6cde35e41..5f1ab36f1265e 100644 --- a/perception/tracking_object_merger/include/autoware/tracking_object_merger/utils/tracker_state.hpp +++ b/perception/tracking_object_merger/include/autoware/tracking_object_merger/utils/tracker_state.hpp @@ -32,6 +32,9 @@ #include #include #include + +namespace autoware::tracking_object_merger +{ using autoware_perception_msgs::msg::TrackedObject; using autoware_perception_msgs::msg::TrackedObjects; @@ -144,5 +147,6 @@ class TrackerState TrackedObjects getTrackedObjectsFromTrackerStates( std::vector & tracker_states, const rclcpp::Time & time); +} // namespace autoware::tracking_object_merger #endif // AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ diff --git a/perception/tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp b/perception/tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp index 4cfa5d8e6d6bd..d445c0fd829c3 100644 --- a/perception/tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp +++ b/perception/tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp @@ -42,49 +42,13 @@ #include #include +namespace autoware::tracking_object_merger +{ using autoware_perception_msgs::msg::TrackedObject; using autoware_perception_msgs::msg::TrackedObjects; + namespace utils { -enum MSG_COV_IDX { - X_X = 0, - X_Y = 1, - X_Z = 2, - X_ROLL = 3, - X_PITCH = 4, - X_YAW = 5, - Y_X = 6, - Y_Y = 7, - Y_Z = 8, - Y_ROLL = 9, - Y_PITCH = 10, - Y_YAW = 11, - Z_X = 12, - Z_Y = 13, - Z_Z = 14, - Z_ROLL = 15, - Z_PITCH = 16, - Z_YAW = 17, - ROLL_X = 18, - ROLL_Y = 19, - ROLL_Z = 20, - ROLL_ROLL = 21, - ROLL_PITCH = 22, - ROLL_YAW = 23, - PITCH_X = 24, - PITCH_Y = 25, - PITCH_Z = 26, - PITCH_ROLL = 27, - PITCH_PITCH = 28, - PITCH_YAW = 29, - YAW_X = 30, - YAW_Y = 31, - YAW_Z = 32, - YAW_ROLL = 33, - YAW_PITCH = 34, - YAW_YAW = 35 -}; - // linear interpolation for tracked objects TrackedObject linearInterpolationForTrackedObject( const TrackedObject & obj1, const TrackedObject & obj2); @@ -103,6 +67,9 @@ TrackedObjects interpolateTrackedObjects( namespace merger_utils { +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjects; + // merge policy enum MergePolicy : int { SKIP = 0, OVERWRITE = 1, FUSION = 2 }; @@ -132,4 +99,6 @@ void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & su } // namespace merger_utils +} // namespace autoware::tracking_object_merger + #endif // AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ diff --git a/perception/tracking_object_merger/src/association/data_association.cpp b/perception/tracking_object_merger/src/association/data_association.cpp index 720f3be34f52a..7e9d531b5a413 100644 --- a/perception/tracking_object_merger/src/association/data_association.cpp +++ b/perception/tracking_object_merger/src/association/data_association.cpp @@ -47,6 +47,8 @@ double getFormedYawAngle( } } // namespace +namespace autoware::tracking_object_merger +{ DataAssociation::DataAssociation( std::vector can_assign_vector, std::vector max_dist_vector, std::vector max_rad_vector, std::vector min_iou_vector, @@ -229,3 +231,5 @@ double DataAssociation::calcScoreBetweenObjects( } return score; } + +} // namespace autoware::tracking_object_merger diff --git a/perception/tracking_object_merger/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp b/perception/tracking_object_merger/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp index 4e7ed56f92891..7899d7e1a7898 100644 --- a/perception/tracking_object_merger/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp +++ b/perception/tracking_object_merger/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp @@ -24,7 +24,7 @@ #include #include -namespace gnn_solver +namespace autoware::tracking_object_merger::gnn_solver { void MuSSP::maximizeLinearAssignment( const std::vector> & cost, std::unordered_map * direct_assignment, @@ -38,4 +38,4 @@ void MuSSP::maximizeLinearAssignment( // Solve DA by muSSP solve_muSSP(cost, direct_assignment, reverse_assignment); } -} // namespace gnn_solver +} // namespace autoware::tracking_object_merger::gnn_solver diff --git a/perception/tracking_object_merger/src/association/ssp/successive_shortest_path.cpp b/perception/tracking_object_merger/src/association/ssp/successive_shortest_path.cpp index 2da4332bec020..288e404ef9861 100644 --- a/perception/tracking_object_merger/src/association/ssp/successive_shortest_path.cpp +++ b/perception/tracking_object_merger/src/association/ssp/successive_shortest_path.cpp @@ -22,7 +22,7 @@ #include #include -namespace gnn_solver +namespace autoware::tracking_object_merger::gnn_solver { struct ResidualEdge { @@ -367,4 +367,4 @@ void SSP::maximizeLinearAssignment( } #endif } -} // namespace gnn_solver +} // namespace autoware::tracking_object_merger::gnn_solver diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp index 53074734c0efa..385481c3d55b9 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp +++ b/perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp @@ -31,7 +31,7 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; -namespace tracking_object_merger +namespace autoware::tracking_object_merger { using autoware_perception_msgs::msg::TrackedObject; @@ -419,7 +419,7 @@ TrackerState DecorativeTrackerMergerNode::createNewTracker( return new_tracker; } -} // namespace tracking_object_merger +} // namespace autoware::tracking_object_merger #include -RCLCPP_COMPONENTS_REGISTER_NODE(tracking_object_merger::DecorativeTrackerMergerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::tracking_object_merger::DecorativeTrackerMergerNode) diff --git a/perception/tracking_object_merger/src/utils/tracker_state.cpp b/perception/tracking_object_merger/src/utils/tracker_state.cpp index 6bdf5eb8196fd..eabbd72364129 100644 --- a/perception/tracking_object_merger/src/utils/tracker_state.cpp +++ b/perception/tracking_object_merger/src/utils/tracker_state.cpp @@ -16,6 +16,9 @@ #include "autoware/tracking_object_merger/utils/utils.hpp" +namespace autoware::tracking_object_merger +{ + using autoware_perception_msgs::msg::TrackedObject; using autoware_perception_msgs::msg::TrackedObjects; @@ -325,3 +328,5 @@ TrackedObjects getTrackedObjectsFromTrackerStates( tracked_objects.header.frame_id = "map"; // TODO(yoshiri): get frame_id from input return tracked_objects; } + +} // namespace autoware::tracking_object_merger diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/tracking_object_merger/src/utils/utils.cpp index 8da35977ce158..3a24cc0f8321d 100644 --- a/perception/tracking_object_merger/src/utils/utils.cpp +++ b/perception/tracking_object_merger/src/utils/utils.cpp @@ -25,7 +25,7 @@ using autoware_perception_msgs::msg::TrackedObject; using autoware_perception_msgs::msg::TrackedObjects; -namespace utils +namespace autoware::tracking_object_merger::utils { /** @@ -243,9 +243,9 @@ TrackedObjects interpolateTrackedObjects( return output_objects; } -} // namespace utils +} // namespace autoware::tracking_object_merger::utils -namespace merger_utils +namespace autoware::tracking_object_merger::merger_utils { double mean(const double a, const double b) @@ -513,4 +513,4 @@ void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & su main_obj = sub_obj; } -} // namespace merger_utils +} // namespace autoware::tracking_object_merger::merger_utils From 6513fce7b1f7723f1e4fc60448282ffb1b34e113 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 24 Jun 2024 19:11:26 +0900 Subject: [PATCH 4/8] chore: refactor include statement in decorative_tracker_merger.hpp Signed-off-by: Taekjin LEE --- .../tracking_object_merger/decorative_tracker_merger.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger.hpp b/perception/tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger.hpp index 027f06ea416cd..db4d8528b601a 100644 --- a/perception/tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger.hpp +++ b/perception/tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger.hpp @@ -24,7 +24,7 @@ #include -#include +#include "autoware_perception_msgs/msg/tracked_objects.hpp" #include #ifdef ROS_DISTRO_GALACTIC From c6858299068f48f1838ef17c1e56ac2f323628d1 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 25 Jun 2024 13:20:49 +0900 Subject: [PATCH 5/8] chore: refactor include statement in decorative_tracker_merger.hpp Signed-off-by: Taekjin LEE --- ...racker_merger.hpp => decorative_tracker_merger_node.hpp} | 6 +++--- .../src/decorative_tracker_merger_node.cpp | 3 ++- 2 files changed, 5 insertions(+), 4 deletions(-) rename perception/tracking_object_merger/include/autoware/tracking_object_merger/{decorative_tracker_merger.hpp => decorative_tracker_merger_node.hpp} (99%) diff --git a/perception/tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger.hpp b/perception/tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger_node.hpp similarity index 99% rename from perception/tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger.hpp rename to perception/tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger_node.hpp index db4d8528b601a..060c19f5df81f 100644 --- a/perception/tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger.hpp +++ b/perception/tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_ -#define AUTOWARE__TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_ +#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_NODE_HPP_ +#define AUTOWARE__TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_NODE_HPP_ #include "autoware/tracking_object_merger/association/data_association.hpp" #include "autoware/tracking_object_merger/utils/tracker_state.hpp" @@ -133,4 +133,4 @@ class DecorativeTrackerMergerNode : public rclcpp::Node } // namespace autoware::tracking_object_merger -#endif // AUTOWARE__TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_ +#endif // AUTOWARE__TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_NODE_HPP_ diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp index 385481c3d55b9..187fb6e1d462e 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp +++ b/perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp @@ -14,8 +14,9 @@ #define EIGEN_MPL2_ONLY +#include "autoware/tracking_object_merger/decorative_tracker_merger_node.hpp" + #include "autoware/tracking_object_merger/association/solver/ssp.hpp" -#include "autoware/tracking_object_merger/decorative_tracker_merger.hpp" #include "autoware/tracking_object_merger/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" From 06a35283712cbd08568f16edb82be597ccf6567e Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 25 Jun 2024 17:54:10 +0900 Subject: [PATCH 6/8] chore: refactor association file path in tracking_object_merger module Signed-off-by: Taekjin LEE --- perception/tracking_object_merger/CMakeLists.txt | 2 +- perception/tracking_object_merger/package.xml | 1 - .../{mu_ssp => solver}/mu_successive_shortest_path_wrapper.cpp | 0 .../association/{ssp => solver}/successive_shortest_path.cpp | 0 4 files changed, 1 insertion(+), 2 deletions(-) rename perception/tracking_object_merger/src/association/{mu_ssp => solver}/mu_successive_shortest_path_wrapper.cpp (100%) rename perception/tracking_object_merger/src/association/{ssp => solver}/successive_shortest_path.cpp (100%) diff --git a/perception/tracking_object_merger/CMakeLists.txt b/perception/tracking_object_merger/CMakeLists.txt index db327b48f0ecd..7e7c698365922 100644 --- a/perception/tracking_object_merger/CMakeLists.txt +++ b/perception/tracking_object_merger/CMakeLists.txt @@ -24,7 +24,7 @@ include_directories( ament_auto_add_library(${PROJECT_NAME} SHARED src/association/data_association.cpp - src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp + src/association/solver/mu_successive_shortest_path_wrapper.cpp src/decorative_tracker_merger_node.cpp src/utils/utils.cpp src/utils/tracker_state.cpp diff --git a/perception/tracking_object_merger/package.xml b/perception/tracking_object_merger/package.xml index 370f5beb3e3b3..cc23578773fe2 100644 --- a/perception/tracking_object_merger/package.xml +++ b/perception/tracking_object_merger/package.xml @@ -23,7 +23,6 @@ rclcpp_components tf2 tf2_ros - tier4_perception_msgs ament_lint_auto autoware_lint_common diff --git a/perception/tracking_object_merger/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp b/perception/tracking_object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp similarity index 100% rename from perception/tracking_object_merger/src/association/mu_ssp/mu_successive_shortest_path_wrapper.cpp rename to perception/tracking_object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp diff --git a/perception/tracking_object_merger/src/association/ssp/successive_shortest_path.cpp b/perception/tracking_object_merger/src/association/solver/successive_shortest_path.cpp similarity index 100% rename from perception/tracking_object_merger/src/association/ssp/successive_shortest_path.cpp rename to perception/tracking_object_merger/src/association/solver/successive_shortest_path.cpp From 834a4a2bfc2a35c66626176cff87b982e6a3c384 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 25 Jun 2024 18:30:42 +0900 Subject: [PATCH 7/8] refactor: update include statements in tracking_object_merger module Signed-off-by: Taekjin LEE --- .../association/data_association.hpp | 10 +++++----- .../association/solver/gnn_solver.hpp | 12 ++++++------ .../association/solver/gnn_solver_interface.hpp | 6 +++--- .../association/solver/mu_ssp.hpp | 8 ++++---- .../association/solver/ssp.hpp | 8 ++++---- .../decorative_tracker_merger_node.hpp | 12 ++++++------ .../utils/tracker_state.hpp | 6 +++--- .../utils/utils.hpp | 6 +++--- .../src/association/data_association.cpp | 6 +++--- .../solver/mu_successive_shortest_path_wrapper.cpp | 2 +- .../association/solver/successive_shortest_path.cpp | 2 +- .../src/decorative_tracker_merger_node.cpp | 6 +++--- .../src/utils/tracker_state.cpp | 4 ++-- .../tracking_object_merger/src/utils/utils.cpp | 2 +- 14 files changed, 45 insertions(+), 45 deletions(-) rename perception/tracking_object_merger/include/{autoware/tracking_object_merger => autoware_tracking_object_merger}/association/data_association.hpp (86%) rename perception/tracking_object_merger/include/{autoware/tracking_object_merger => autoware_tracking_object_merger}/association/solver/gnn_solver.hpp (59%) rename perception/tracking_object_merger/include/{autoware/tracking_object_merger => autoware_tracking_object_merger}/association/solver/gnn_solver_interface.hpp (79%) rename perception/tracking_object_merger/include/{autoware/tracking_object_merger => autoware_tracking_object_merger}/association/solver/mu_ssp.hpp (78%) rename perception/tracking_object_merger/include/{autoware/tracking_object_merger => autoware_tracking_object_merger}/association/solver/ssp.hpp (79%) rename perception/tracking_object_merger/include/{autoware/tracking_object_merger => autoware_tracking_object_merger}/decorative_tracker_merger_node.hpp (92%) rename perception/tracking_object_merger/include/{autoware/tracking_object_merger => autoware_tracking_object_merger}/utils/tracker_state.hpp (96%) rename perception/tracking_object_merger/include/{autoware/tracking_object_merger => autoware_tracking_object_merger}/utils/utils.hpp (95%) diff --git a/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/data_association.hpp b/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/data_association.hpp similarity index 86% rename from perception/tracking_object_merger/include/autoware/tracking_object_merger/association/data_association.hpp rename to perception/tracking_object_merger/include/autoware_tracking_object_merger/association/data_association.hpp index af4a65869eca5..a272e42a74e80 100644 --- a/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/data_association.hpp +++ b/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/data_association.hpp @@ -16,13 +16,13 @@ // Author: v1.0 Yukihiro Saito // -#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ -#define AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ +#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ +#define AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ #define EIGEN_MPL2_ONLY -#include "autoware/tracking_object_merger/association/solver/gnn_solver.hpp" -#include "autoware/tracking_object_merger/utils/tracker_state.hpp" +#include "autoware_tracking_object_merger/association/solver/gnn_solver.hpp" +#include "autoware_tracking_object_merger/utils/tracker_state.hpp" #include #include @@ -72,4 +72,4 @@ class DataAssociation } // namespace autoware::tracking_object_merger -#endif // AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ +#endif // AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__DATA_ASSOCIATION_HPP_ diff --git a/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/gnn_solver.hpp b/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/gnn_solver.hpp similarity index 59% rename from perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/gnn_solver.hpp rename to perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/gnn_solver.hpp index 64a18d878f7c3..4cea0e3cbe96f 100644 --- a/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/gnn_solver.hpp +++ b/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/gnn_solver.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#define AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#define AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ -#include "autoware/tracking_object_merger/association/solver/gnn_solver_interface.hpp" -#include "autoware/tracking_object_merger/association/solver/mu_ssp.hpp" -#include "autoware/tracking_object_merger/association/solver/ssp.hpp" +#include "autoware_tracking_object_merger/association/solver/gnn_solver_interface.hpp" +#include "autoware_tracking_object_merger/association/solver/mu_ssp.hpp" +#include "autoware_tracking_object_merger/association/solver/ssp.hpp" -#endif // AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#endif // AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ diff --git a/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/gnn_solver_interface.hpp b/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/gnn_solver_interface.hpp similarity index 79% rename from perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/gnn_solver_interface.hpp rename to perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/gnn_solver_interface.hpp index 732ed5cd87041..d751075773a09 100644 --- a/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/gnn_solver_interface.hpp +++ b/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/gnn_solver_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ -#define AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#define AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ #include #include @@ -32,4 +32,4 @@ class GnnSolverInterface }; } // namespace autoware::tracking_object_merger::gnn_solver -#endif // AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#endif // AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ diff --git a/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/mu_ssp.hpp b/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/mu_ssp.hpp similarity index 78% rename from perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/mu_ssp.hpp rename to perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/mu_ssp.hpp index b219c1ac441eb..47c23bc8eb1b1 100644 --- a/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/mu_ssp.hpp +++ b/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/mu_ssp.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ -#define AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ +#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ +#define AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ -#include "autoware/tracking_object_merger/association/solver/gnn_solver_interface.hpp" +#include "autoware_tracking_object_merger/association/solver/gnn_solver_interface.hpp" #include #include @@ -34,4 +34,4 @@ class MuSSP : public GnnSolverInterface }; } // namespace autoware::tracking_object_merger::gnn_solver -#endif // AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ +#endif // AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__MU_SSP_HPP_ diff --git a/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/ssp.hpp b/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/ssp.hpp similarity index 79% rename from perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/ssp.hpp rename to perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/ssp.hpp index 03311e4e43694..bcfac73f3e43b 100644 --- a/perception/tracking_object_merger/include/autoware/tracking_object_merger/association/solver/ssp.hpp +++ b/perception/tracking_object_merger/include/autoware_tracking_object_merger/association/solver/ssp.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ -#define AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ +#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ +#define AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ -#include "autoware/tracking_object_merger/association/solver/gnn_solver_interface.hpp" +#include "autoware_tracking_object_merger/association/solver/gnn_solver_interface.hpp" #include #include @@ -34,4 +34,4 @@ class SSP : public GnnSolverInterface }; } // namespace autoware::tracking_object_merger::gnn_solver -#endif // AUTOWARE__TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ +#endif // AUTOWARE_TRACKING_OBJECT_MERGER__ASSOCIATION__SOLVER__SSP_HPP_ diff --git a/perception/tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger_node.hpp b/perception/tracking_object_merger/include/autoware_tracking_object_merger/decorative_tracker_merger_node.hpp similarity index 92% rename from perception/tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger_node.hpp rename to perception/tracking_object_merger/include/autoware_tracking_object_merger/decorative_tracker_merger_node.hpp index 060c19f5df81f..8aebad913d53e 100644 --- a/perception/tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger_node.hpp +++ b/perception/tracking_object_merger/include/autoware_tracking_object_merger/decorative_tracker_merger_node.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_NODE_HPP_ -#define AUTOWARE__TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_NODE_HPP_ +#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_NODE_HPP_ +#define AUTOWARE_TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_NODE_HPP_ -#include "autoware/tracking_object_merger/association/data_association.hpp" -#include "autoware/tracking_object_merger/utils/tracker_state.hpp" -#include "autoware/tracking_object_merger/utils/utils.hpp" #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/ros/published_time_publisher.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" +#include "autoware_tracking_object_merger/association/data_association.hpp" +#include "autoware_tracking_object_merger/utils/tracker_state.hpp" +#include "autoware_tracking_object_merger/utils/utils.hpp" #include @@ -133,4 +133,4 @@ class DecorativeTrackerMergerNode : public rclcpp::Node } // namespace autoware::tracking_object_merger -#endif // AUTOWARE__TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_NODE_HPP_ +#endif // AUTOWARE_TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_NODE_HPP_ diff --git a/perception/tracking_object_merger/include/autoware/tracking_object_merger/utils/tracker_state.hpp b/perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/tracker_state.hpp similarity index 96% rename from perception/tracking_object_merger/include/autoware/tracking_object_merger/utils/tracker_state.hpp rename to perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/tracker_state.hpp index 5f1ab36f1265e..ed1f69191b652 100644 --- a/perception/tracking_object_merger/include/autoware/tracking_object_merger/utils/tracker_state.hpp +++ b/perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/tracker_state.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ -#define AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ +#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ +#define AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ #include #include @@ -149,4 +149,4 @@ TrackedObjects getTrackedObjectsFromTrackerStates( std::vector & tracker_states, const rclcpp::Time & time); } // namespace autoware::tracking_object_merger -#endif // AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ +#endif // AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ diff --git a/perception/tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp b/perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/utils.hpp similarity index 95% rename from perception/tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp rename to perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/utils.hpp index d445c0fd829c3..7aeeb4ecc04dd 100644 --- a/perception/tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp +++ b/perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/utils.hpp @@ -14,8 +14,8 @@ // // -#ifndef AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ -#define AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ +#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ +#define AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ // #include #include "autoware/universe_utils/geometry/geometry.hpp" @@ -101,4 +101,4 @@ void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & su } // namespace autoware::tracking_object_merger -#endif // AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ +#endif // AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ diff --git a/perception/tracking_object_merger/src/association/data_association.cpp b/perception/tracking_object_merger/src/association/data_association.cpp index 7e9d531b5a413..e77a71744fe9b 100644 --- a/perception/tracking_object_merger/src/association/data_association.cpp +++ b/perception/tracking_object_merger/src/association/data_association.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/tracking_object_merger/association/data_association.hpp" +#include "autoware_tracking_object_merger/association/data_association.hpp" -#include "autoware/tracking_object_merger/association/solver/gnn_solver.hpp" -#include "autoware/tracking_object_merger/utils/utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_tracking_object_merger/association/solver/gnn_solver.hpp" +#include "autoware_tracking_object_merger/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include diff --git a/perception/tracking_object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp b/perception/tracking_object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp index 7899d7e1a7898..d2db6eba068ec 100644 --- a/perception/tracking_object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp +++ b/perception/tracking_object_merger/src/association/solver/mu_successive_shortest_path_wrapper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/tracking_object_merger/association/solver/mu_ssp.hpp" +#include "autoware_tracking_object_merger/association/solver/mu_ssp.hpp" #include diff --git a/perception/tracking_object_merger/src/association/solver/successive_shortest_path.cpp b/perception/tracking_object_merger/src/association/solver/successive_shortest_path.cpp index 288e404ef9861..7be715b24bd5f 100644 --- a/perception/tracking_object_merger/src/association/solver/successive_shortest_path.cpp +++ b/perception/tracking_object_merger/src/association/solver/successive_shortest_path.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/tracking_object_merger/association/solver/ssp.hpp" +#include "autoware_tracking_object_merger/association/solver/ssp.hpp" #include #include diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp index 187fb6e1d462e..d1f80412afcd2 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp +++ b/perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp @@ -14,10 +14,10 @@ #define EIGEN_MPL2_ONLY -#include "autoware/tracking_object_merger/decorative_tracker_merger_node.hpp" +#include "autoware_tracking_object_merger/decorative_tracker_merger_node.hpp" -#include "autoware/tracking_object_merger/association/solver/ssp.hpp" -#include "autoware/tracking_object_merger/utils/utils.hpp" +#include "autoware_tracking_object_merger/association/solver/ssp.hpp" +#include "autoware_tracking_object_merger/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" #include diff --git a/perception/tracking_object_merger/src/utils/tracker_state.cpp b/perception/tracking_object_merger/src/utils/tracker_state.cpp index eabbd72364129..54b1d73c375a5 100644 --- a/perception/tracking_object_merger/src/utils/tracker_state.cpp +++ b/perception/tracking_object_merger/src/utils/tracker_state.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/tracking_object_merger/utils/tracker_state.hpp" +#include "autoware_tracking_object_merger/utils/tracker_state.hpp" -#include "autoware/tracking_object_merger/utils/utils.hpp" +#include "autoware_tracking_object_merger/utils/utils.hpp" namespace autoware::tracking_object_merger { diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/tracking_object_merger/src/utils/utils.cpp index 3a24cc0f8321d..1ae705aa38f15 100644 --- a/perception/tracking_object_merger/src/utils/utils.cpp +++ b/perception/tracking_object_merger/src/utils/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/tracking_object_merger/utils/utils.hpp" +#include "autoware_tracking_object_merger/utils/utils.hpp" #include "autoware_perception_msgs/msg/shape.hpp" #include "autoware_perception_msgs/msg/tracked_object.hpp" From fb9cc44d462150f07179ffae73fca35c812d116d Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 26 Jun 2024 11:59:45 +0900 Subject: [PATCH 8/8] refactor: move the node header to src Signed-off-by: Taekjin LEE --- .../include/autoware_tracking_object_merger/utils/utils.hpp | 1 - .../src/decorative_tracker_merger_node.cpp | 2 +- .../decorative_tracker_merger_node.hpp | 6 +++--- 3 files changed, 4 insertions(+), 5 deletions(-) rename perception/tracking_object_merger/{include/autoware_tracking_object_merger => src}/decorative_tracker_merger_node.hpp (95%) diff --git a/perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/utils.hpp b/perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/utils.hpp index 7aeeb4ecc04dd..eb288c4e853ad 100644 --- a/perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/utils.hpp +++ b/perception/tracking_object_merger/include/autoware_tracking_object_merger/utils/utils.hpp @@ -17,7 +17,6 @@ #ifndef AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ #define AUTOWARE_TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ -// #include #include "autoware/universe_utils/geometry/geometry.hpp" #include diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp index d1f80412afcd2..6f65941c0a747 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp +++ b/perception/tracking_object_merger/src/decorative_tracker_merger_node.cpp @@ -14,7 +14,7 @@ #define EIGEN_MPL2_ONLY -#include "autoware_tracking_object_merger/decorative_tracker_merger_node.hpp" +#include "decorative_tracker_merger_node.hpp" #include "autoware_tracking_object_merger/association/solver/ssp.hpp" #include "autoware_tracking_object_merger/utils/utils.hpp" diff --git a/perception/tracking_object_merger/include/autoware_tracking_object_merger/decorative_tracker_merger_node.hpp b/perception/tracking_object_merger/src/decorative_tracker_merger_node.hpp similarity index 95% rename from perception/tracking_object_merger/include/autoware_tracking_object_merger/decorative_tracker_merger_node.hpp rename to perception/tracking_object_merger/src/decorative_tracker_merger_node.hpp index 8aebad913d53e..ae19f51876490 100644 --- a/perception/tracking_object_merger/include/autoware_tracking_object_merger/decorative_tracker_merger_node.hpp +++ b/perception/tracking_object_merger/src/decorative_tracker_merger_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_NODE_HPP_ -#define AUTOWARE_TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_NODE_HPP_ +#ifndef DECORATIVE_TRACKER_MERGER_NODE_HPP_ +#define DECORATIVE_TRACKER_MERGER_NODE_HPP_ #include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/ros/published_time_publisher.hpp" @@ -133,4 +133,4 @@ class DecorativeTrackerMergerNode : public rclcpp::Node } // namespace autoware::tracking_object_merger -#endif // AUTOWARE_TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_NODE_HPP_ +#endif // DECORATIVE_TRACKER_MERGER_NODE_HPP_