Skip to content

Commit

Permalink
fix(lane_change): separate backward buffer for blocking object
Browse files Browse the repository at this point in the history
Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
  • Loading branch information
rej55 committed Oct 27, 2023
1 parent 3e0af70 commit 63a7f35
Show file tree
Hide file tree
Showing 8 changed files with 34 additions and 23 deletions.
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
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
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
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
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
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,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
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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([&]() {
Expand Down Expand Up @@ -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) {
Expand Down
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
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

0 comments on commit 63a7f35

Please sign in to comment.