From 8ae45cfa6b0470929522188e722072fcaf14c120 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 9 Aug 2024 17:37:36 +0900 Subject: [PATCH] fix(lane_change): skip generating path if lane changing path is too long (#8362) rework. skip lane changing for insufficeient distance in target lane Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Zulfaqar Azmi --- .../utils/data_structs.hpp | 1 + .../utils/utils.hpp | 4 -- .../src/scene.cpp | 49 ++++++++++--------- .../src/utils/utils.cpp | 20 +------- 4 files changed, 30 insertions(+), 44 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 7962c878d7d64..155fbfdb535f9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -191,6 +191,7 @@ struct Lanes { bool current_lane_in_goal_section{false}; lanelet::ConstLanelets current; + lanelet::ConstLanelets target_neighbor; lanelet::ConstLanelets target; std::vector preceding_target; }; 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 d566aceba413b..ec93a3c999152 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 @@ -106,10 +106,6 @@ lanelet::ConstLanelets getTargetNeighborLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const LaneChangeModuleType & type); -lanelet::BasicPolygon2d getTargetNeighborLanesPolygon( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const LaneChangeModuleType & type); - bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); 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 c8bd1f111a2ae..dc4d1022615c6 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 @@ -87,6 +87,9 @@ void NormalLaneChange::update_lanes(const bool is_approved) common_data_ptr_->lanes_ptr->target = target_lanes; const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr; + common_data_ptr_->lanes_ptr->target_neighbor = utils::lane_change::getTargetNeighborLanes( + *route_handler_ptr, current_lanes, common_data_ptr_->lc_type); + common_data_ptr_->lanes_ptr->current_lane_in_goal_section = route_handler_ptr->isInGoalRouteSection(current_lanes.back()); common_data_ptr_->lanes_ptr->preceding_target = utils::getPrecedingLanelets( @@ -1464,6 +1467,9 @@ bool NormalLaneChange::getLaneChangePaths( RCLCPP_DEBUG(logger_, " - sampling num for lat_acc: %lu", sample_lat_acc.size()); debug_print("Prepare path satisfy constraints"); + const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, lane_changing_start_pose); + for (const auto & lateral_acc : sample_lat_acc) { const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( shift_length, lane_change_parameters_->lane_changing_lateral_jerk, lateral_acc); @@ -1474,11 +1480,6 @@ bool NormalLaneChange::getLaneChangePaths( const auto lane_changing_length = utils::lane_change::calcPhaseLength( initial_lane_changing_velocity, getCommonParam().max_vel, longitudinal_acc_on_lane_changing, lane_changing_time); - const auto terminal_lane_changing_velocity = std::min( - initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time, - getCommonParam().max_vel); - utils::lane_change::setPrepareVelocity( - prepare_segment, current_velocity, terminal_lane_changing_velocity); const auto debug_print_lat = [&](const auto & s) { RCLCPP_DEBUG( @@ -1493,26 +1494,30 @@ bool NormalLaneChange::getLaneChangePaths( continue; } - if (is_goal_in_route) { - const double s_start = - lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose).length; - const double s_goal = - lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length; - const auto num = + // if multiple lane change is necessary, does the remaining distance is sufficient + const auto remaining_dist_in_target = std::invoke([&]() { + const auto finish_judge_buffer = lane_change_parameters_->lane_change_finish_judge_buffer; + const auto num_to_preferred_lane_from_target_lane = std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction)); - const double backward_buffer = - num == 0 ? 0.0 : lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const double finish_judge_buffer = - lane_change_parameters_->lane_change_finish_judge_buffer; - if ( - s_start + lane_changing_length + finish_judge_buffer + backward_buffer + - next_lane_change_buffer > - s_goal) { - debug_print_lat("Reject: length of lane changing path is longer than length to goal!!"); - continue; - } + const auto backward_len_buffer = + lane_change_parameters_->backward_length_buffer_for_end_of_lane; + const auto backward_buffer_to_target_lane = + num_to_preferred_lane_from_target_lane == 0 ? 0.0 : backward_len_buffer; + return lane_changing_length + finish_judge_buffer + backward_buffer_to_target_lane + + next_lane_change_buffer; + }); + + if (remaining_dist_in_target > dist_lc_start_to_end_of_lanes) { + debug_print_lat("Reject: length of lane changing path is longer than length to goal!!"); + continue; } + const auto terminal_lane_changing_velocity = std::min( + initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time, + getCommonParam().max_vel); + utils::lane_change::setPrepareVelocity( + prepare_segment, current_velocity, terminal_lane_changing_velocity); + const auto target_segment = getTargetSegment( target_lanes, lane_changing_start_pose, target_lane_length, lane_changing_length, initial_lane_changing_velocity, next_lane_change_buffer); 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 915864c597882..1d8ffaa4b7b18 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 @@ -290,21 +290,6 @@ lanelet::ConstLanelets getTargetNeighborLanes( return neighbor_lanes; } -lanelet::BasicPolygon2d getTargetNeighborLanesPolygon( - const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, - const LaneChangeModuleType & type) -{ - const auto target_neighbor_lanelets = - utils::lane_change::getTargetNeighborLanes(route_handler, current_lanes, type); - if (target_neighbor_lanelets.empty()) { - return {}; - } - const auto target_neighbor_preferred_lane_poly = lanelet::utils::getPolygonFromArcLength( - target_neighbor_lanelets, 0, std::numeric_limits::max()); - - return lanelet::utils::to2D(target_neighbor_preferred_lane_poly).basicPolygon(); -} - bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) @@ -1227,9 +1212,8 @@ LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr) lanes_polygon.expanded_target = utils::lane_change::createPolygon( expanded_target_lanes, 0.0, std::numeric_limits::max()); - const auto & route_handler = *common_data_ptr->route_handler_ptr; - lanes_polygon.target_neighbor = - getTargetNeighborLanesPolygon(route_handler, lanes->current, common_data_ptr->lc_type); + lanes_polygon.target_neighbor = *utils::lane_change::createPolygon( + lanes->target_neighbor, 0.0, std::numeric_limits::max()); lanes_polygon.preceding_target.reserve(lanes->preceding_target.size()); for (const auto & preceding_lane : lanes->preceding_target) {