diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/tracking_object_merger/src/utils/utils.cpp index 90de9b6a41cbd..08680380f253c 100644 --- a/perception/tracking_object_merger/src/utils/utils.cpp +++ b/perception/tracking_object_merger/src/utils/utils.cpp @@ -323,6 +323,26 @@ 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, 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_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; // do not merge reverse direction objects if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) {