From 7512dfe20e352ec8d33baafb4751ce634dbfa153 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Tue, 31 Oct 2023 10:56:30 +0900 Subject: [PATCH] fix(lane_change): separate backward buffer for blocking object (#994) * fix(lane_change): separate backward buffer for blocking object Signed-off-by: Fumiya Watanabe * Update planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp Co-authored-by: Kosuke Takeuchi * style(pre-commit): autofix --------- Signed-off-by: Fumiya Watanabe Co-authored-by: Kosuke Takeuchi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../config/lane_change/lane_change.param.yaml | 1 + .../behavior_path_planner/parameters.hpp | 1 + .../behavior_path_planner/utils/utils.hpp | 2 +- .../src/behavior_path_planner_node.cpp | 2 + .../scene_module/lane_change/interface.cpp | 4 +- .../src/scene_module/lane_change/normal.cpp | 39 ++++++++++++------- .../src/utils/lane_change/utils.cpp | 4 +- .../behavior_path_planner/src/utils/utils.cpp | 5 +-- 8 files changed, 35 insertions(+), 23 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index d3c0a22e867f9..f98eac5315006 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -5,6 +5,7 @@ prepare_duration: 4.0 # [s] backward_length_buffer_for_end_of_lane: 3.0 # [m] + backward_length_buffer_for_blocking_object: 3.0 # [m] lane_change_finish_judge_buffer: 2.0 # [m] lane_changing_lateral_jerk: 0.5 # [m/s3] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index e3a3784c5e928..ab0ada409ba8f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -92,6 +92,7 @@ struct BehaviorPathPlannerParameters double backward_path_length; double forward_path_length; double backward_length_buffer_for_end_of_lane; + double backward_length_buffer_for_blocking_object; double backward_length_buffer_for_end_of_pull_over; double backward_length_buffer_for_end_of_pull_out; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index ccf13e01ecc94..cee37956e44aa 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -399,7 +399,7 @@ bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_thre double calcMinimumLaneChangeLength( const BehaviorPathPlannerParameters & common_param, const std::vector & shift_intervals, - const double length_to_intersection = 0.0); + const double backward_buffer, const double length_to_intersection = 0.0); lanelet::ConstLanelets getLaneletsFromPath( const PathWithLaneId & path, const std::shared_ptr & route_handler); diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 647ff641dd806..0fce9c4923856 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -340,6 +340,8 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() // lane change parameters p.backward_length_buffer_for_end_of_lane = declare_parameter("lane_change.backward_length_buffer_for_end_of_lane"); + p.backward_length_buffer_for_blocking_object = + declare_parameter("lane_change.backward_length_buffer_for_blocking_object"); p.lane_changing_lateral_jerk = declare_parameter("lane_change.lane_changing_lateral_jerk"); p.lane_change_prepare_duration = declare_parameter("lane_change.prepare_duration"); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 8d9d4ec23b195..3134c46f12aed 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -467,8 +467,8 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( const auto & common_parameter = module_type_->getCommonParam(); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); - const double next_lane_change_buffer = - utils::calcMinimumLaneChangeLength(common_parameter, shift_intervals); + const double next_lane_change_buffer = utils::calcMinimumLaneChangeLength( + common_parameter, shift_intervals, common_parameter.backward_length_buffer_for_end_of_lane); const double & nearest_dist_threshold = common_parameter.ego_nearest_dist_threshold; const double & nearest_yaw_threshold = common_parameter.ego_nearest_yaw_threshold; const double & base_to_front = common_parameter.base_link2front; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 4e2f973c7fa67..3ab6197b98eb0 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -196,8 +196,9 @@ void NormalLaneChange::insertStopPoint( } const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back()); - const double lane_change_buffer = - utils::calcMinimumLaneChangeLength(getCommonParam(), shift_intervals, 0.0); + const double lane_change_buffer = utils::calcMinimumLaneChangeLength( + getCommonParam(), shift_intervals, getCommonParam().backward_length_buffer_for_end_of_lane, + 0.0); const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { return utils::getSignedDistance(path.points.front().point.pose, target, lanelets); @@ -259,10 +260,14 @@ void NormalLaneChange::insertStopPoint( const auto rss_dist = calcRssDistance( 0.0, planner_data_->parameters.minimum_lane_changing_velocity, lane_change_parameters_->rss_params); + const double lane_change_buffer_for_blocking_object = utils::calcMinimumLaneChangeLength( + getCommonParam(), shift_intervals, + getCommonParam().backward_length_buffer_for_blocking_object, 0.0); - const auto stopping_distance_for_obj = distance_to_ego_lane_obj - lane_change_buffer - - stop_point_buffer - rss_dist - - getCommonParam().base_link2front; + const auto stopping_distance_for_obj = + distance_to_ego_lane_obj - lane_change_buffer_for_blocking_object - + getCommonParam().backward_length_buffer_for_blocking_object - rss_dist - + getCommonParam().base_link2front; // If the target lane in the lane change section is blocked by a stationary obstacle, there // is no reason for stopping with a lane change margin. Instead, stop right behind the @@ -457,8 +462,9 @@ bool NormalLaneChange::isNearEndOfCurrentLanes( const auto & current_pose = getEgoPose(); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); - const auto lane_change_buffer = - utils::calcMinimumLaneChangeLength(planner_data_->parameters, shift_intervals); + const auto lane_change_buffer = utils::calcMinimumLaneChangeLength( + planner_data_->parameters, shift_intervals, + getCommonParam().backward_length_buffer_for_end_of_lane); auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); @@ -985,8 +991,8 @@ bool NormalLaneChange::hasEnoughLength( const double lane_change_length = path.info.length.sum(); const auto shift_intervals = route_handler.getLateralIntervalsToPreferredLane(target_lanes.back(), direction); - double minimum_lane_change_length_to_preferred_lane = - utils::calcMinimumLaneChangeLength(common_parameters, shift_intervals); + double minimum_lane_change_length_to_preferred_lane = utils::calcMinimumLaneChangeLength( + common_parameters, shift_intervals, common_parameters.backward_length_buffer_for_end_of_lane); if (lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { return false; @@ -1082,9 +1088,11 @@ bool NormalLaneChange::getLaneChangePaths( const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); const double lane_change_buffer = utils::calcMinimumLaneChangeLength( - common_parameters, route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); + common_parameters, route_handler.getLateralIntervalsToPreferredLane(current_lanes.back()), + common_parameters.backward_length_buffer_for_end_of_lane); const double next_lane_change_buffer = utils::calcMinimumLaneChangeLength( - common_parameters, route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); + common_parameters, route_handler.getLateralIntervalsToPreferredLane(target_lanes.back()), + common_parameters.backward_length_buffer_for_end_of_lane); const auto dist_to_end_of_current_lanes = utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); @@ -1486,8 +1494,8 @@ bool NormalLaneChange::getAbortPath() const auto direction = getDirection(); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane( selected_path.info.current_lanes.back(), direction); - const double minimum_lane_change_length = - utils::calcMinimumLaneChangeLength(common_param, shift_intervals); + const double minimum_lane_change_length = utils::calcMinimumLaneChangeLength( + common_param, shift_intervals, common_param.backward_length_buffer_for_end_of_lane); const auto & lane_changing_path = selected_path.path; const auto lane_changing_end_pose_idx = std::invoke([&]() { @@ -1740,8 +1748,9 @@ bool NormalLaneChange::isVehicleStuck( : utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); - const double lane_change_buffer = - utils::calcMinimumLaneChangeLength(getCommonParam(), shift_intervals, 0.0); + const double lane_change_buffer = utils::calcMinimumLaneChangeLength( + getCommonParam(), shift_intervals, getCommonParam().backward_length_buffer_for_end_of_lane, + 0.0); const double stop_point_buffer = getCommonParam().backward_length_buffer_for_end_of_lane; const double terminal_judge_buffer = lane_change_buffer + stop_point_buffer + 1.0; if (distance_to_terminal < terminal_judge_buffer) { diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 8337cbee58583..a5f342f9b7017 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -610,8 +610,8 @@ bool hasEnoughLengthToLaneChangeAfterAbort( { const auto shift_intervals = route_handler.getLateralIntervalsToPreferredLane(current_lanes.back(), direction); - const double minimum_lane_change_length = - utils::calcMinimumLaneChangeLength(common_param, shift_intervals); + const double minimum_lane_change_length = utils::calcMinimumLaneChangeLength( + common_param, shift_intervals, common_param.backward_length_buffer_for_end_of_lane); 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; diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 0af3420d0d113..39d31491b961d 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -3223,7 +3223,7 @@ bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_thre double calcMinimumLaneChangeLength( const BehaviorPathPlannerParameters & common_param, const std::vector & shift_intervals, - const double length_to_intersection) + const double backward_buffer, const double length_to_intersection) { if (shift_intervals.empty()) { return 0.0; @@ -3241,8 +3241,7 @@ double calcMinimumLaneChangeLength( PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, max_lateral_acc); accumulated_length += vel * t + finish_judge_buffer; } - accumulated_length += - common_param.backward_length_buffer_for_end_of_lane * (shift_intervals.size() - 1.0); + accumulated_length += backward_buffer * (shift_intervals.size() - 1.0); return accumulated_length; }