diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index bc3e07cf8dd74..fb1e93185f738 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -414,23 +414,27 @@ bool AdaptiveCruiseController::estimatePointVelocityFromObject( /* get object velocity, and current yaw */ bool get_obj = false; - double obj_vel; - double obj_yaw; + double obj_vel_norm; + double obj_vel_yaw; const Point collision_point_2d = convertPointRosToBoost(nearest_collision_p_ros); for (const auto & obj : object_ptr->objects) { const Polygon obj_poly = getPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape.dimensions, 0.0, param_.object_polygon_length_margin, param_.object_polygon_width_margin); if (boost::geometry::distance(obj_poly, collision_point_2d) <= 0) { - obj_vel = obj.kinematics.initial_twist_with_covariance.twist.linear.x; - obj_yaw = tf2::getYaw(obj.kinematics.initial_pose_with_covariance.pose.orientation); + obj_vel_norm = std::hypot( + obj.kinematics.initial_twist_with_covariance.twist.linear.x, + obj.kinematics.initial_twist_with_covariance.twist.linear.y); + obj_vel_yaw = std::atan2( + obj.kinematics.initial_twist_with_covariance.twist.linear.y, + obj.kinematics.initial_twist_with_covariance.twist.linear.x); get_obj = true; break; } } if (get_obj) { - *velocity = obj_vel * std::cos(obj_yaw - traj_yaw); + *velocity = obj_vel_norm * std::cos(obj_vel_yaw - traj_yaw); debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity; return true; } else { @@ -442,10 +446,15 @@ void AdaptiveCruiseController::calculateProjectedVelocityFromObject( const PredictedObject & object, const double traj_yaw, double * velocity) { /* get object velocity, and current yaw */ - double obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; - double obj_yaw = tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation); - - *velocity = obj_vel * std::cos(tier4_autoware_utils::normalizeRadian(obj_yaw - traj_yaw)); + double obj_vel_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + double obj_vel_yaw = std::atan2( + object.kinematics.initial_twist_with_covariance.twist.linear.y, + object.kinematics.initial_twist_with_covariance.twist.linear.x); + + *velocity = + obj_vel_norm * std::cos(tier4_autoware_utils::normalizeRadian(obj_vel_yaw - traj_yaw)); debug_values_.data.at(DBGVAL::ESTIMATED_VEL_OBJ) = *velocity; }