From 284548ca7f38b1d83af11f2b9caaac116eb9b09c Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 12 Dec 2022 09:57:19 +0900 Subject: [PATCH] fix(behavior_path_planner): minimum distance for lane change (#2413) Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Azmi --- .../config/behavior_path_planner.param.yaml | 1 + .../behavior_path_planner/parameters.hpp | 2 ++ .../lane_change/lane_change_module_data.hpp | 1 - .../behavior_path_planner/utilities.hpp | 6 ++++++ .../src/behavior_path_planner_node.cpp | 4 +++- .../lane_change/lane_change_module.cpp | 9 +++----- .../src/scene_module/lane_change/util.cpp | 2 +- .../lane_following/lane_following_module.cpp | 3 +-- .../behavior_path_planner/src/utilities.cpp | 21 +++++++++++++++---- 9 files changed, 34 insertions(+), 15 deletions(-) diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index 30dc2eb73489a..68cc14a8299a1 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -6,6 +6,7 @@ backward_length_buffer_for_end_of_pull_over: 5.0 backward_length_buffer_for_end_of_pull_out: 5.0 minimum_lane_change_length: 12.0 + minimum_lane_change_prepare_distance: 2.0 # [m] minimum_pull_over_length: 16.0 drivable_area_resolution: 0.1 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 f599deab7bb24..132e01f5d91d3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -25,6 +25,8 @@ struct BehaviorPathPlannerParameters double backward_length_buffer_for_end_of_pull_over; double backward_length_buffer_for_end_of_pull_out; double minimum_lane_change_length; + double minimum_lane_change_prepare_distance; + double minimum_pull_over_length; double minimum_pull_out_length; double drivable_area_resolution; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp index f9ce032760e45..be14bb120aa42 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp @@ -27,7 +27,6 @@ struct LaneChangeParameters { double lane_change_prepare_duration{2.0}; double lane_changing_duration{4.0}; - double minimum_lane_change_prepare_distance{4.0}; double lane_change_finish_judge_buffer{3.0}; double minimum_lane_change_velocity{5.6}; double prediction_time_resolution{0.5}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index d244c039b7734..2da8d1ab2f156 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -485,6 +485,12 @@ bool isSafeInFreeSpaceCollisionCheck( const double front_decel, const double rear_decel, CollisionCheckDebug & debug); bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold); + +double calcTotalLaneChangeDistanceWithBuffer(const BehaviorPathPlannerParameters & common_param); + +double calcLaneChangeBuffer( + const BehaviorPathPlannerParameters & common_param, const int num_lane_change, + const double length_to_intersection); } // namespace behavior_path_planner::util #endif // BEHAVIOR_PATH_PLANNER__UTILITIES_HPP_ 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 acaa0be7291ef..39089e9b8e0ad 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -205,6 +205,9 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.backward_length_buffer_for_end_of_pull_out = declare_parameter("backward_length_buffer_for_end_of_pull_out", 5.0); p.minimum_lane_change_length = declare_parameter("minimum_lane_change_length", 8.0); + p.minimum_lane_change_prepare_distance = + declare_parameter("minimum_lane_change_prepare_distance", 2.0); + p.minimum_pull_over_length = declare_parameter("minimum_pull_over_length", 15.0); p.drivable_area_resolution = declare_parameter("drivable_area_resolution"); p.drivable_lane_forward_length = declare_parameter("drivable_lane_forward_length"); @@ -359,7 +362,6 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() LaneChangeParameters p{}; p.lane_change_prepare_duration = dp("lane_change_prepare_duration", 2.0); p.lane_changing_duration = dp("lane_changing_duration", 4.0); - p.minimum_lane_change_prepare_distance = dp("minimum_lane_change_prepare_distance", 4.0); p.lane_change_finish_judge_buffer = dp("lane_change_finish_judge_buffer", 3.0); p.minimum_lane_change_velocity = dp("minimum_lane_change_velocity", 5.6); p.prediction_time_resolution = dp("prediction_time_resolution", 0.5); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index afb1997d01e74..5f7e0406967c7 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -264,10 +264,9 @@ PathWithLaneId LaneChangeModule::getReferencePath() const *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, common_parameters.forward_path_length, common_parameters, optional_lengths); } - const double & buffer = - common_parameters.backward_length_buffer_for_end_of_lane; // buffer for min_lane_change_length + const double lane_change_buffer = - num_lane_change * (common_parameters.minimum_lane_change_length + buffer) + optional_lengths; + util::calcLaneChangeBuffer(common_parameters, num_lane_change, optional_lengths); reference_path = util::setDecelerationVelocity( *route_handler, reference_path, current_lanes, parameters_->lane_change_prepare_duration, @@ -416,9 +415,7 @@ bool LaneChangeModule::isValidPath(const PathWithLaneId & path) const bool LaneChangeModule::isNearEndOfLane() const { const auto & current_pose = getEgoPose(); - const auto minimum_lane_change_length = planner_data_->parameters.minimum_lane_change_length; - const auto end_of_lane_buffer = planner_data_->parameters.backward_length_buffer_for_end_of_lane; - const double threshold = end_of_lane_buffer + minimum_lane_change_length; + const double threshold = util::calcTotalLaneChangeDistanceWithBuffer(planner_data_->parameters); return std::max(0.0, util::getDistanceToEndOfLane(current_pose, status_.current_lanes)) < threshold; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 9b3e65ec92a87..e4066f17b20d0 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -177,7 +177,7 @@ LaneChangePaths getLaneChangePaths( const auto & lane_change_prepare_duration = parameter.lane_change_prepare_duration; const auto & lane_changing_duration = parameter.lane_changing_duration; const auto & minimum_lane_change_prepare_distance = - parameter.minimum_lane_change_prepare_distance; + common_parameter.minimum_lane_change_prepare_distance; const auto & minimum_lane_change_length = common_parameter.minimum_lane_change_length; const auto & minimum_lane_change_velocity = parameter.minimum_lane_change_velocity; const auto & maximum_deceleration = parameter.maximum_deceleration; diff --git a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp index 4c9e3a7894201..3ad35afb276a1 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp @@ -128,9 +128,8 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const p, optional_lengths); } - const double buffer = p.backward_length_buffer_for_end_of_lane; const double lane_change_buffer = - num_lane_change * (p.minimum_lane_change_length + buffer) + optional_lengths; + util::calcLaneChangeBuffer(p, num_lane_change, optional_lengths); reference_path = util::setDecelerationVelocity( *route_handler, reference_path, current_lanes, parameters_.lane_change_prepare_duration, diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index aaf3dce50f1fc..6683c987a9c86 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -2022,13 +2022,11 @@ PathWithLaneId getCenterLinePath( const double s_backward = std::max(0., s - backward_path_length); double s_forward = s + forward_path_length; - const double buffer = - parameter.backward_length_buffer_for_end_of_lane; // buffer for min_lane_change_length const int num_lane_change = std::abs(route_handler.getNumLaneToPreferredLane(lanelet_sequence.back())); const double lane_length = lanelet::utils::getLaneletLength2d(lanelet_sequence); const double lane_change_buffer = - std::fabs(num_lane_change * (parameter.minimum_lane_change_length + buffer) + optional_length); + calcLaneChangeBuffer(parameter, std::abs(num_lane_change), optional_length); if (route_handler.isDeadEndLanelet(lanelet_sequence.back())) { s_forward = std::min(s_forward, lane_length - lane_change_buffer); @@ -2168,7 +2166,7 @@ PathWithLaneId setDecelerationVelocity( const double lane_length = lanelet::utils::getLaneletLength2d(lanelet_sequence); const auto arclength = lanelet::utils::getArcCoordinates(lanelet_sequence, point.point.pose); const double distance_to_end = - std::max(0.0, lane_length - std::fabs(lane_change_buffer) - arclength.length); + std::max(0.0, lane_length - std::abs(lane_change_buffer) - arclength.length); point.point.longitudinal_velocity_mps = std::min( point.point.longitudinal_velocity_mps, static_cast(distance_to_end / lane_change_prepare_duration)); @@ -2776,4 +2774,19 @@ bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_thre return true; } +double calcTotalLaneChangeDistanceWithBuffer(const BehaviorPathPlannerParameters & common_param) +{ + const double minimum_lane_change_distance = + common_param.minimum_lane_change_prepare_distance + common_param.minimum_lane_change_length; + const double end_of_lane_buffer = common_param.backward_length_buffer_for_end_of_lane; + return minimum_lane_change_distance + end_of_lane_buffer; +} + +double calcLaneChangeBuffer( + const BehaviorPathPlannerParameters & common_param, const int num_lane_change, + const double length_to_intersection) +{ + return num_lane_change * calcTotalLaneChangeDistanceWithBuffer(common_param) + + length_to_intersection; +} } // namespace behavior_path_planner::util