diff --git a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp index d7fc0aef204be..fc383a8d1da00 100644 --- a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp @@ -68,8 +68,10 @@ void box3DToDetectedObject( float vel_x = box3d.vel_x; float vel_y = box3d.vel_y; geometry_msgs::msg::Twist twist; - twist.linear.x = std::sqrt(std::pow(vel_x, 2) + std::pow(vel_y, 2)); - twist.angular.z = 2 * (std::atan2(vel_y, vel_x) - yaw); + twist.linear.x = std::cos(yaw) * vel_x + std::sin(yaw) * vel_y; + twist.linear.y = -std::sin(yaw) * vel_x + std::cos(yaw) * vel_y; + twist.angular.z = 0; // angular velocity is not supported + obj.kinematics.twist_with_covariance.twist = twist; obj.kinematics.has_twist = has_twist; if (has_variance) {