From 664d96721dc043d5b0a718b92101e314d5f4a0b5 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 20 Aug 2023 01:59:26 +0900 Subject: [PATCH] feat(obstacle_stop_planner): consider object velocity direction (#4648) * feat(obstacle_stop_planner): consider object velocity direction Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../src/adaptive_cruise_control.cpp | 27 ++++++++++++------- 1 file changed, 18 insertions(+), 9 deletions(-) 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; }