From 7f78781640fd8a6274ce302c9eebfbdc539e2953 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 24 Jun 2024 17:54:52 +0900 Subject: [PATCH] refactor: remove MSG_COV_IDX enum in utils.hpp Signed-off-by: Taekjin LEE --- .../radar_object_tracker/utils/utils.hpp | 39 --------- .../constant_turn_rate_motion_tracker.cpp | 56 +++++++------ .../tracker/model/linear_motion_tracker.cpp | 83 ++++++++++--------- 3 files changed, 71 insertions(+), 107 deletions(-) diff --git a/perception/radar_object_tracker/include/autoware/radar_object_tracker/utils/utils.hpp b/perception/radar_object_tracker/include/autoware/radar_object_tracker/utils/utils.hpp index 667cc34226681..b56f42e2e4031 100644 --- a/perception/radar_object_tracker/include/autoware/radar_object_tracker/utils/utils.hpp +++ b/perception/radar_object_tracker/include/autoware/radar_object_tracker/utils/utils.hpp @@ -35,45 +35,6 @@ #include namespace autoware::radar_object_tracker::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 -}; - // matrix concatenate Eigen::MatrixXd stackMatricesVertically(const std::vector & matrices); Eigen::MatrixXd stackMatricesDiagonally(const std::vector & matrices); diff --git a/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp index 3703ba1fb5905..70cbb7e71ed31 100644 --- a/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp +++ b/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp @@ -23,6 +23,7 @@ #include "autoware/radar_object_tracker/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include @@ -45,6 +46,7 @@ namespace autoware::radar_object_tracker { using Label = autoware_perception_msgs::msg::ObjectClassification; +using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; // init static member variables bool ConstantTurnRateMotionTracker::is_initialized_ = false; @@ -116,10 +118,10 @@ ConstantTurnRateMotionTracker::ConstantTurnRateMotionTracker( // Rotate the covariance matrix according to the vehicle yaw // because p0_cov_x and y are in the vehicle coordinate system. if (object.kinematics.has_position_covariance) { - P_xy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + P_xy_local << object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X], + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_Y], + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_X], + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y]; P_xy = R * P_xy_local * R.transpose(); } else { // rotate @@ -130,9 +132,9 @@ ConstantTurnRateMotionTracker::ConstantTurnRateMotionTracker( // covariance often written in object frame if (object.kinematics.has_twist_covariance) { - const auto vx_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + const auto vx_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; // const auto vy_cov = - // object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + // object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y]; P(IDX::VX, IDX::VX) = vx_cov; } else { P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; @@ -373,11 +375,11 @@ bool ConstantTurnRateMotionTracker::measureWithPose( Rxy_local << ekf_params_.r_cov_x, 0, 0, r_cov_y; // xy in base_link coordinate Rxy = RotationBaseLink * Rxy_local * RotationBaseLink.transpose(); } else { - Rxy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X], + Rxy_local << object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X], + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_Y], + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_X], object.kinematics.pose_with_covariance - .covariance[utils::MSG_COV_IDX::Y_Y]; // xy in vehicle coordinate + .covariance[XYZRPY_COV_IDX::Y_Y]; // xy in vehicle coordinate Rxy = RotationYaw * Rxy_local * RotationYaw.transpose(); } R_block_list.push_back(Rxy); @@ -429,7 +431,7 @@ bool ConstantTurnRateMotionTracker::measureWithPose( if (!object.kinematics.has_twist_covariance) { R_vx << ekf_params_.r_cov_vx; } else { - R_vx << object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + R_vx << object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; } R_block_list.push_back(R_vx); } @@ -584,27 +586,27 @@ bool ConstantTurnRateMotionTracker::getTrackedObject( constexpr double z_cov = 1. * 1.; // TODO(yukkysaito) Currently tentative constexpr double yaw_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_xy(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_xy(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_xy(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_xy(IDX::Y, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = yaw_cov; + pose_with_cov.covariance[XYZRPY_COV_IDX::X_X] = P_xy(IDX::X, IDX::X); + pose_with_cov.covariance[XYZRPY_COV_IDX::X_Y] = P_xy(IDX::X, IDX::Y); + pose_with_cov.covariance[XYZRPY_COV_IDX::Y_X] = P_xy(IDX::Y, IDX::X); + pose_with_cov.covariance[XYZRPY_COV_IDX::Y_Y] = P_xy(IDX::Y, IDX::Y); + pose_with_cov.covariance[XYZRPY_COV_IDX::Z_Z] = z_cov; + pose_with_cov.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = no_info_cov; + pose_with_cov.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = no_info_cov; + pose_with_cov.covariance[XYZRPY_COV_IDX::YAW_YAW] = yaw_cov; // twist covariance constexpr double vz_cov = 0.2 * 0.2; // TODO(Yoshi Ri) Currently tentative constexpr double wz_cov = 0.2 * 0.2; // TODO(yukkysaito) Currently tentative - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = 0.0; - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = 0.0; - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = no_info_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = wz_cov; + twist_with_cov.covariance[XYZRPY_COV_IDX::X_X] = P(IDX::VX, IDX::VX); + twist_with_cov.covariance[XYZRPY_COV_IDX::X_Y] = 0.0; + twist_with_cov.covariance[XYZRPY_COV_IDX::Y_X] = 0.0; + twist_with_cov.covariance[XYZRPY_COV_IDX::Y_Y] = no_info_cov; + twist_with_cov.covariance[XYZRPY_COV_IDX::Z_Z] = vz_cov; + twist_with_cov.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = no_info_cov; + twist_with_cov.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = no_info_cov; + twist_with_cov.covariance[XYZRPY_COV_IDX::YAW_YAW] = wz_cov; // set shape if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { diff --git a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp index 8819880943f99..8141c84c01011 100644 --- a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp +++ b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp @@ -21,9 +21,9 @@ #include "autoware/radar_object_tracker/tracker/model/linear_motion_tracker.hpp" #include "autoware/radar_object_tracker/utils/utils.hpp" - -#include -#include +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include @@ -46,6 +46,7 @@ namespace autoware::radar_object_tracker { using Label = autoware_perception_msgs::msg::ObjectClassification; +using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; // initialize static parameter bool LinearMotionTracker::is_initialized_ = false; @@ -122,10 +123,10 @@ LinearMotionTracker::LinearMotionTracker( // Rotate the covariance matrix according to the vehicle yaw // because p0_cov_x and y are in the vehicle coordinate system. if (object.kinematics.has_position_covariance) { - P_xy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + P_xy_local << object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X], + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_Y], + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_X], + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y]; P_xy = R * P_xy_local * R.transpose(); } else { // rotate @@ -133,8 +134,8 @@ LinearMotionTracker::LinearMotionTracker( } // covariance often written in object frame if (object.kinematics.has_twist_covariance) { - const auto vx_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - const auto vy_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + const auto vx_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; + const auto vy_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y]; P_v_xy_local << vx_cov, 0.0, 0.0, vy_cov; P_v_xy = R * P_v_xy_local * R.transpose(); } else { @@ -145,10 +146,10 @@ LinearMotionTracker::LinearMotionTracker( false; // currently message does not have acceleration covariance if (has_acceleration_covariance) { // const auto ax_cov = - // object.kinematics.acceleration_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; // This + // object.kinematics.acceleration_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; // This // is future update // const auto ay_cov = - // object.kinematics.acceleration_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; // This + // object.kinematics.acceleration_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y]; // This // is future update // Eigen::Matrix2d P_a_xy_local; // P_a_xy_local << ax_cov, 0.0, 0.0, ay_cov; @@ -418,11 +419,11 @@ bool LinearMotionTracker::measureWithPose( Rxy_local << ekf_params_.r_cov_x, 0, 0, r_cov_y; // xy in base_link coordinate Rxy = RotationBaseLink * Rxy_local * RotationBaseLink.transpose(); } else { - Rxy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X], + Rxy_local << object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X], + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_Y], + object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_X], object.kinematics.pose_with_covariance - .covariance[utils::MSG_COV_IDX::Y_Y]; // xy in vehicle coordinate + .covariance[XYZRPY_COV_IDX::Y_Y]; // xy in vehicle coordinate Rxy = RotationYaw * Rxy_local * RotationYaw.transpose(); } R_block_list.push_back(Rxy); @@ -450,8 +451,8 @@ bool LinearMotionTracker::measureWithPose( R_v_xy_local << ekf_params_.r_cov_vx, 0, 0, ekf_params_.r_cov_vy; R_v_xy = RotationBaseLink * R_v_xy_local * RotationBaseLink.transpose(); } else { - R_v_xy_local << 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]; + R_v_xy_local << object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X], 0, 0, + object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y]; R_v_xy = RotationYaw * R_v_xy_local * RotationYaw.transpose(); } R_block_list.push_back(R_v_xy); @@ -640,37 +641,37 @@ bool LinearMotionTracker::getTrackedObject( constexpr double z_cov = 1. * 1.; // TODO(yukkysaito) Currently tentative constexpr double yaw_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_xy(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_xy(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_xy(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_xy(IDX::Y, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = yaw_cov; + pose_with_cov.covariance[XYZRPY_COV_IDX::X_X] = P_xy(IDX::X, IDX::X); + pose_with_cov.covariance[XYZRPY_COV_IDX::X_Y] = P_xy(IDX::X, IDX::Y); + pose_with_cov.covariance[XYZRPY_COV_IDX::Y_X] = P_xy(IDX::Y, IDX::X); + pose_with_cov.covariance[XYZRPY_COV_IDX::Y_Y] = P_xy(IDX::Y, IDX::Y); + pose_with_cov.covariance[XYZRPY_COV_IDX::Z_Z] = z_cov; + pose_with_cov.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = no_info_cov; + pose_with_cov.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = no_info_cov; + pose_with_cov.covariance[XYZRPY_COV_IDX::YAW_YAW] = yaw_cov; // twist covariance constexpr double vz_cov = 0.2 * 0.2; // TODO(Yoshi Ri) Currently tentative constexpr double wz_cov = 0.2 * 0.2; // TODO(yukkysaito) Currently tentative - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_v_xy(IDX::X, IDX::X); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_v_xy(IDX::X, IDX::Y); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_v_xy(IDX::Y, IDX::X); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_v_xy(IDX::Y, IDX::Y); - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = wz_cov; + twist_with_cov.covariance[XYZRPY_COV_IDX::X_X] = P_v_xy(IDX::X, IDX::X); + twist_with_cov.covariance[XYZRPY_COV_IDX::X_Y] = P_v_xy(IDX::X, IDX::Y); + twist_with_cov.covariance[XYZRPY_COV_IDX::Y_X] = P_v_xy(IDX::Y, IDX::X); + twist_with_cov.covariance[XYZRPY_COV_IDX::Y_Y] = P_v_xy(IDX::Y, IDX::Y); + twist_with_cov.covariance[XYZRPY_COV_IDX::Z_Z] = vz_cov; + twist_with_cov.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = no_info_cov; + twist_with_cov.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = no_info_cov; + twist_with_cov.covariance[XYZRPY_COV_IDX::YAW_YAW] = wz_cov; // acceleration covariance - acceleration_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_a_xy(IDX::X, IDX::X); - acceleration_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_a_xy(IDX::X, IDX::Y); - acceleration_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_a_xy(IDX::Y, IDX::X); - acceleration_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_a_xy(IDX::Y, IDX::Y); - acceleration_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = no_info_cov; - acceleration_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; - acceleration_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; - acceleration_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = no_info_cov; + acceleration_with_cov.covariance[XYZRPY_COV_IDX::X_X] = P_a_xy(IDX::X, IDX::X); + acceleration_with_cov.covariance[XYZRPY_COV_IDX::X_Y] = P_a_xy(IDX::X, IDX::Y); + acceleration_with_cov.covariance[XYZRPY_COV_IDX::Y_X] = P_a_xy(IDX::Y, IDX::X); + acceleration_with_cov.covariance[XYZRPY_COV_IDX::Y_Y] = P_a_xy(IDX::Y, IDX::Y); + acceleration_with_cov.covariance[XYZRPY_COV_IDX::Z_Z] = no_info_cov; + acceleration_with_cov.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = no_info_cov; + acceleration_with_cov.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = no_info_cov; + acceleration_with_cov.covariance[XYZRPY_COV_IDX::YAW_YAW] = no_info_cov; // set shape if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) {