diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp index 323878923f8e8..8d8afb41730f9 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp @@ -53,10 +53,15 @@ std::optional find_closest_collision_point( auto p = geometry_msgs::msg::Point().set__x(coll_p.x()).set__y(coll_p.y()); const auto dist_ego_to_coll = motion_utils::calcSignedArcLength(ego_data.path.points, ego_pose.position, p); - const auto dist_obj_to_coll = tier4_autoware_utils::calcDistance2d(object_pose.position, p); - const auto tta_cp_npc = abs(dist_obj_to_coll) / object.kinematics.initial_twist_with_covariance.twist.linear.x; - const auto tta_cp_ego = dist_ego_to_coll / ego_data.path.points[path_idx].point.longitudinal_velocity_mps; - if (abs(dist_ego_to_coll) < closest_dist && std::abs(tta_cp_npc - tta_cp_ego) < params.time_horizon) { + const auto dist_obj_to_coll = + tier4_autoware_utils::calcDistance2d(object_pose.position, p); + const auto tta_cp_npc = + abs(dist_obj_to_coll) / object.kinematics.initial_twist_with_covariance.twist.linear.x; + const auto tta_cp_ego = + dist_ego_to_coll / ego_data.path.points[path_idx].point.longitudinal_velocity_mps; + if ( + abs(dist_ego_to_coll) < closest_dist && + std::abs(tta_cp_npc - tta_cp_ego) < params.time_horizon) { closest_dist = dist_ego_to_coll; closest_collision_point = p; }