From a959d33341a3e3e8bcccb259d8213d1514d7d262 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 19 Feb 2024 18:54:59 +0900 Subject: [PATCH 1/2] fix(tracking_object_merger): initial fix for flipped mot tracker Signed-off-by: Taekjin LEE --- .../tracking_object_merger/src/utils/utils.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/tracking_object_merger/src/utils/utils.cpp index 90de9b6a41cbd..6040a30f718cc 100644 --- a/perception/tracking_object_merger/src/utils/utils.cpp +++ b/perception/tracking_object_merger/src/utils/utils.cpp @@ -323,6 +323,24 @@ autoware_auto_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMe autoware_auto_perception_msgs::msg::TrackedObjectKinematics output_kinematics; // copy main object at first output_kinematics = main_obj.kinematics; + // if the main object is run opposite direction, flip the main object + if (output_kinematics.twist_with_covariance.twist.linear.x < 0.0) { + // flip velocity + output_kinematics.twist_with_covariance.twist.linear.x *= -1.0; + output_kinematics.twist_with_covariance.twist.linear.y *= -1.0; + // flip orientation, convert quaternion to have the opposite direction + const auto q = tf2::Quaternion( + output_kinematics.pose_with_covariance.pose.orientation.x, + output_kinematics.pose_with_covariance.pose.orientation.y, + output_kinematics.pose_with_covariance.pose.orientation.z, + output_kinematics.pose_with_covariance.pose.orientation.w); + const auto q_flip = q * tf2::Quaternion(0, 0, 1, 0); + output_kinematics.pose_with_covariance.pose.orientation.x = q_flip.x(); + output_kinematics.pose_with_covariance.pose.orientation.y = q_flip.y(); + output_kinematics.pose_with_covariance.pose.orientation.z = q_flip.z(); + output_kinematics.pose_with_covariance.pose.orientation.w = q_flip.w(); + } + auto sub_obj_ = sub_obj; // do not merge reverse direction objects if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) { From 08392db47f825fa50400580e4b96b95dc38babc9 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 19 Feb 2024 19:17:47 +0900 Subject: [PATCH 2/2] fix(tracking_object_merger): fix quaternion implementation Signed-off-by: Taekjin LEE --- .../tracking_object_merger/src/utils/utils.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/tracking_object_merger/src/utils/utils.cpp index 6040a30f718cc..08680380f253c 100644 --- a/perception/tracking_object_merger/src/utils/utils.cpp +++ b/perception/tracking_object_merger/src/utils/utils.cpp @@ -323,22 +323,24 @@ autoware_auto_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMe autoware_auto_perception_msgs::msg::TrackedObjectKinematics output_kinematics; // copy main object at first output_kinematics = main_obj.kinematics; - // if the main object is run opposite direction, flip the main object + + // if the main object is run opposite direction, flip the main object if (output_kinematics.twist_with_covariance.twist.linear.x < 0.0) { // flip velocity output_kinematics.twist_with_covariance.twist.linear.x *= -1.0; output_kinematics.twist_with_covariance.twist.linear.y *= -1.0; - // flip orientation, convert quaternion to have the opposite direction + // flip orientation, rotate 180 deg const auto q = tf2::Quaternion( output_kinematics.pose_with_covariance.pose.orientation.x, output_kinematics.pose_with_covariance.pose.orientation.y, output_kinematics.pose_with_covariance.pose.orientation.z, output_kinematics.pose_with_covariance.pose.orientation.w); - const auto q_flip = q * tf2::Quaternion(0, 0, 1, 0); - output_kinematics.pose_with_covariance.pose.orientation.x = q_flip.x(); - output_kinematics.pose_with_covariance.pose.orientation.y = q_flip.y(); - output_kinematics.pose_with_covariance.pose.orientation.z = q_flip.z(); - output_kinematics.pose_with_covariance.pose.orientation.w = q_flip.w(); + const auto q_rot = tf2::Quaternion(tf2::Vector3(0, 0, 1), M_PI); + const auto q_rotated = q * q_rot; + output_kinematics.pose_with_covariance.pose.orientation.x = q_rotated.x(); + output_kinematics.pose_with_covariance.pose.orientation.y = q_rotated.y(); + output_kinematics.pose_with_covariance.pose.orientation.z = q_rotated.z(); + output_kinematics.pose_with_covariance.pose.orientation.w = q_rotated.w(); } auto sub_obj_ = sub_obj;