From b9d569e36ab77e00422587023ac00dd82a001568 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 29 Nov 2024 15:57:53 +0900 Subject: [PATCH] fix(lane_change): cap ego's predicted path velocity (RT1-8505) (#9341) * fix(lane_change): cap ego's predicted path velocity (RT1-8505) Signed-off-by: Zulfaqar Azmi * properly cap based on 0.0 instead of min lc vel Signed-off-by: Zulfaqar Azmi * fix build error Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../utils/utils.hpp | 7 ++- .../src/scene.cpp | 11 +--- .../src/utils/utils.cpp | 51 ++++++++++--------- 3 files changed, 32 insertions(+), 37 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 73bbef1453459..a19508dd11b7f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -136,10 +136,9 @@ std::optional getLaneChangeTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const LaneChangeModuleType type, const Direction & direction); -std::vector convertToPredictedPath( - const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & pose, - const double lane_changing_acceleration, const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters, const double resolution); +std::vector convert_to_predicted_path( + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, + const double lane_changing_acceleration); bool isParkedObject( const PathWithLaneId & path, const RouteHandler & route_handler, diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 39cc4327be07b..28e95580a4bc7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -2171,8 +2171,6 @@ bool NormalLaneChange::has_collision_with_decel_patterns( return false; } - const auto current_pose = common_data_ptr_->get_ego_pose(); - const auto current_twist = common_data_ptr_->get_ego_twist(); const auto bpp_param = *common_data_ptr_->bpp_param_ptr; const auto global_min_acc = bpp_param.min_acc; const auto lane_changing_acc = lane_change_path.info.longitudinal_acceleration.lane_changing; @@ -2189,15 +2187,10 @@ bool NormalLaneChange::has_collision_with_decel_patterns( acceleration_values.begin(), acceleration_values.end(), acceleration_values.begin(), [&](double n) { return lane_changing_acc + n * acc_resolution; }); - const auto time_resolution = lane_change_parameters_->prediction_time_resolution; - const auto all_collided = std::all_of( acceleration_values.begin(), acceleration_values.end(), [&](const auto acceleration) { - const auto ego_predicted_path = utils::lane_change::convertToPredictedPath( - lane_change_path, current_twist, current_pose, acceleration, bpp_param, - *lane_change_parameters_, time_resolution); - const auto debug_predicted_path = - utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); + const auto ego_predicted_path = utils::lane_change::convert_to_predicted_path( + common_data_ptr_, lane_change_path, acceleration); return std::any_of(objects.begin(), objects.end(), [&](const auto & obj) { const auto selected_rss_param = diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 950547c383c68..a063919a1db5a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -661,52 +661,55 @@ std::optional getLaneChangeTargetLane( return route_handler.getLaneChangeTargetExceptPreferredLane(current_lanes, direction); } -std::vector convertToPredictedPath( - const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & vehicle_pose, - const double lane_changing_acceleration, const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters, const double resolution) +std::vector convert_to_predicted_path( + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, + const double lane_changing_acceleration) { if (lane_change_path.path.points.empty()) { return {}; } const auto & path = lane_change_path.path; - const auto prepare_acc = lane_change_path.info.longitudinal_acceleration.prepare; - const auto duration = lane_change_path.info.duration.sum(); - const auto prepare_time = lane_change_path.info.duration.prepare; - const auto & minimum_lane_changing_velocity = - lane_change_parameters.minimum_lane_changing_velocity; - + const auto & vehicle_pose = common_data_ptr->get_ego_pose(); + const auto & bpp_param_ptr = common_data_ptr->bpp_param_ptr; const auto nearest_seg_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, vehicle_pose, common_parameters.ego_nearest_dist_threshold, - common_parameters.ego_nearest_yaw_threshold); + path.points, vehicle_pose, bpp_param_ptr->ego_nearest_dist_threshold, + bpp_param_ptr->ego_nearest_yaw_threshold); - std::vector predicted_path; const auto vehicle_pose_frenet = convertToFrenetPoint(path.points, vehicle_pose.position, nearest_seg_idx); - const double initial_velocity = std::abs(vehicle_twist.linear.x); + + const auto initial_velocity = common_data_ptr->get_ego_speed(); + const auto prepare_acc = lane_change_path.info.longitudinal_acceleration.prepare; + const auto duration = lane_change_path.info.duration.sum(); + const auto prepare_time = lane_change_path.info.duration.prepare; + const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; + const auto resolution = lc_param_ptr->prediction_time_resolution; + std::vector predicted_path; // prepare segment for (double t = 0.0; t < prepare_time; t += resolution) { - const double velocity = - std::max(initial_velocity + prepare_acc * t, minimum_lane_changing_velocity); - const double length = initial_velocity * t + 0.5 * prepare_acc * t * t; + const auto velocity = + std::clamp(initial_velocity + prepare_acc * t, 0.0, lane_change_path.info.velocity.prepare); + const auto length = initial_velocity * t + 0.5 * prepare_acc * t * t; const auto pose = autoware::motion_utils::calcInterpolatedPose( path.points, vehicle_pose_frenet.length + length); predicted_path.emplace_back(t, pose, velocity); } // lane changing segment - const double lane_changing_velocity = - std::max(initial_velocity + prepare_acc * prepare_time, minimum_lane_changing_velocity); - const double offset = + const auto lane_changing_velocity = std::clamp( + initial_velocity + prepare_acc * prepare_time, 0.0, lane_change_path.info.velocity.prepare); + const auto offset = initial_velocity * prepare_time + 0.5 * prepare_acc * prepare_time * prepare_time; for (double t = prepare_time; t < duration; t += resolution) { - const double delta_t = t - prepare_time; - const double velocity = lane_changing_velocity + lane_changing_acceleration * delta_t; - const double length = lane_changing_velocity * delta_t + - 0.5 * lane_changing_acceleration * delta_t * delta_t + offset; + const auto delta_t = t - prepare_time; + const auto velocity = std::clamp( + lane_changing_velocity + lane_changing_acceleration * delta_t, 0.0, + lane_change_path.info.velocity.lane_changing); + const auto length = lane_changing_velocity * delta_t + + 0.5 * lane_changing_acceleration * delta_t * delta_t + offset; const auto pose = autoware::motion_utils::calcInterpolatedPose( path.points, vehicle_pose_frenet.length + length); predicted_path.emplace_back(t, pose, velocity);