Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(lane_change): separate backward buffer for blocking object #994

Merged
merged 3 commits into from
Oct 31, 2023
Merged

fix(lane_change): separate backward buffer for blocking object #994

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
63a7f35
fix(lane_change): separate backward buffer for blocking object
rej55 Oct 27, 2023
03b5086
Update planning/behavior_path_planner/src/scene_module/lane_change/no…
rej55 Oct 27, 2023
1fd9529
style(pre-commit): autofix
pre-commit-ci[bot] Oct 27, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions planning/behavior_path_planner/config/lane_change/lane_change.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down
1 change: 1 addition & 0 deletions planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -388,7 +388,7 @@ bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_thre

double calcMinimumLaneChangeLength(
const BehaviorPathPlannerParameters & common_param, const std::vector<double> & 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::RouteHandler> & route_handler);
Expand Down
2 changes: 2 additions & 0 deletions planning/behavior_path_planner/src/behavior_path_planner_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -340,6 +340,8 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
// lane change parameters
p.backward_length_buffer_for_end_of_lane =
declare_parameter<double>("lane_change.backward_length_buffer_for_end_of_lane");
p.backward_length_buffer_for_blocking_object =
declare_parameter<double>("lane_change.backward_length_buffer_for_blocking_object");
p.lane_changing_lateral_jerk =
declare_parameter<double>("lane_change.lane_changing_lateral_jerk");
p.lane_change_prepare_duration = declare_parameter<double>("lane_change.prepare_duration");
Expand Down
4 changes: 2 additions & 2 deletions planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
39 changes: 24 additions & 15 deletions planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -967,8 +973,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;
Expand Down Expand Up @@ -1064,9 +1070,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);
Expand Down Expand Up @@ -1466,8 +1474,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([&]() {
Expand Down Expand Up @@ -1720,8 +1728,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) {
Expand Down
4 changes: 2 additions & 2 deletions planning/behavior_path_planner/src/utils/lane_change/utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
5 changes: 2 additions & 3 deletions planning/behavior_path_planner/src/utils/utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3065,7 +3065,7 @@ bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_thre

double calcMinimumLaneChangeLength(
const BehaviorPathPlannerParameters & common_param, const std::vector<double> & shift_intervals,
const double length_to_intersection)
const double backward_buffer, const double length_to_intersection)
{
if (shift_intervals.empty()) {
return 0.0;
Expand All @@ -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;
}
Expand Down
Loading