From 63a7f35cde07f20b160a6f17f7ba4f30dec83eb8 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Fri, 27 Oct 2023 21:32:31 +0900 Subject: [PATCH 1/3] fix(lane_change): separate backward buffer for blocking object Signed-off-by: Fumiya Watanabe --- .../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 | 38 +++++++++++-------- .../src/utils/lane_change/utils.cpp | 4 +- .../behavior_path_planner/src/utils/utils.cpp | 5 +-- 8 files changed, 34 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 be227f4f7e93e..cfcc1b78b1a26 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 @@ -388,7 +388,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 d3a120b43a073..0cab443c875a8 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,13 @@ 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 - stop_point_buffer - + 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 +461,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); @@ -967,8 +972,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; @@ -1064,9 +1069,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); @@ -1466,8 +1473,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([&]() { @@ -1720,8 +1727,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 ed2979bd1d5a1..6d8e7f6042ee7 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -3065,7 +3065,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; @@ -3083,8 +3083,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; } From 03b5086ab0f548902119e2fd96e64bc177f15b52 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Sat, 28 Oct 2023 02:49:56 +0900 Subject: [PATCH 2/3] Update planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp Co-authored-by: Kosuke Takeuchi --- .../src/scene_module/lane_change/normal.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 0cab443c875a8..e71eef2cc77a1 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 @@ -265,7 +265,7 @@ void NormalLaneChange::insertStopPoint( getCommonParam().backward_length_buffer_for_blocking_object, 0.0); const auto stopping_distance_for_obj = - distance_to_ego_lane_obj - lane_change_buffer_for_blocking_object - stop_point_buffer - + 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 From 1fd95297d652d3766f076d20d059213f17a7da1e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 27 Oct 2023 17:51:42 +0000 Subject: [PATCH 3/3] style(pre-commit): autofix --- .../src/scene_module/lane_change/normal.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 e71eef2cc77a1..e032cf3704849 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 @@ -265,8 +265,9 @@ void NormalLaneChange::insertStopPoint( getCommonParam().backward_length_buffer_for_blocking_object, 0.0); 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; + 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