From ecbd383a3d33ec5361ba857e9ca020e76d96f171 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 27 Sep 2024 20:40:37 +0900 Subject: [PATCH] fix(lane_change): fix abort distance enough check (#8979) * RT1-7991 fix abort distance enough check Signed-off-by: Zulfaqar Azmi * RT-7991 remove unused function Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../src/goal_planner_module.cpp | 3 +- .../utils/utils.hpp | 11 ----- .../src/scene.cpp | 18 ++++++-- .../src/utils/utils.cpp | 41 ------------------- 4 files changed, 16 insertions(+), 57 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index d8ff9261392b1..92aa83ede9f4e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -1542,8 +1542,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() // return to lane parking if it is possible if ( - is_freespace && - thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE && + is_freespace && thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE && canReturnToLaneParking()) { thread_safe_data_.set_pull_over_path(thread_safe_data_.get_lane_parking_pull_over_path()); } 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 a270900b491c3..717e8d322f05a 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 @@ -141,17 +141,6 @@ std::vector generateDrivableLanes( double getLateralShift(const LaneChangePath & path); -bool hasEnoughLengthToLaneChangeAfterAbort( - const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, - const Pose & curent_pose, const double abort_return_dist, - const LaneChangeParameters & lane_change_parameters, const Direction direction); - -double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0); - -double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer); - -std::string getStrDirection(const std::string & name, const Direction direction); - CandidateOutput assignToCandidate( const LaneChangePath & lane_change_path, const Point & ego_position); std::optional getLaneChangeTargetLane( 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 28efae2549d1e..0beec0909c034 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 @@ -2034,9 +2034,21 @@ bool NormalLaneChange::calcAbortPath() return false; } - if (!utils::lane_change::hasEnoughLengthToLaneChangeAfterAbort( - route_handler, reference_lanelets, current_pose, abort_return_dist, - *lane_change_parameters_, direction)) { + const auto dist_to_terminal_start = std::invoke([&]() { + const auto dist_to_terminal_end = calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr_, get_current_lanes(), common_data_ptr_->get_ego_pose()); + const auto min_lc_length = utils::lane_change::calcMinimumLaneChangeLength( + route_handler, get_current_lanes().back(), *common_data_ptr_->lc_param_ptr, + common_data_ptr_->direction); + return dist_to_terminal_end - min_lc_length; + }); + + const auto enough_abort_dist = + abort_start_dist + abort_return_dist + + calculation::calc_stopping_distance(common_data_ptr_->lc_param_ptr) <= + dist_to_terminal_start; + + if (!enough_abort_dist) { RCLCPP_ERROR(logger_, "insufficient distance to abort."); return false; } 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 71e6b0bf6530e..8bcd6a97d19f5 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 @@ -644,47 +644,6 @@ double getLateralShift(const LaneChangePath & path) return path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx); } -bool hasEnoughLengthToLaneChangeAfterAbort( - const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, - const Pose & current_pose, const double abort_return_dist, - const LaneChangeParameters & lane_change_parameters, const Direction direction) -{ - if (current_lanes.empty()) { - return false; - } - - const auto minimum_lane_change_length = calcMinimumLaneChangeLength( - route_handler, current_lanes.back(), lane_change_parameters, direction); - const auto abort_plus_lane_change_length = abort_return_dist + minimum_lane_change_length; - if (abort_plus_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { - return false; - } - - if ( - abort_plus_lane_change_length > - utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)) { - return false; - } - - return true; -} - -double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer) -{ - return lateral_buffer + 0.5 * vehicle_width; -} - -std::string getStrDirection(const std::string & name, const Direction direction) -{ - if (direction == Direction::LEFT) { - return name + "_left"; - } - if (direction == Direction::RIGHT) { - return name + "_right"; - } - return ""; -} - std::vector> getSortedLaneIds( const RouteHandler & route_handler, const Pose & current_pose, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes)