Skip to content

Commit

Permalink
refactor(lane_change): refactor min and max lane change length
Browse files Browse the repository at this point in the history 
Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
  • Loading branch information
@zulfaqar-azmi-t4
zulfaqar-azmi-t4 committed Apr 18, 2024
1 parent a336fa8 commit 2f41fe0
Show file tree
Hide file tree
Showing 4 changed files with 89 additions and 82 deletions.
6 changes: 1 addition & 5 deletions planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -273,12 +273,8 @@ double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const
return std::numeric_limits<double>::infinity();
}

// get minimum lane change distance
const auto shift_intervals =
getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_);
return utils::lane_change::calcMinimumLaneChangeLength(
*lane_change_parameters_, shift_intervals,
lane_change_parameters_->backward_length_buffer_for_end_of_lane);
getRouteHandler(), current_lanes.back(), *lane_change_parameters_, direction_);
}

double AvoidanceByLaneChange::calcLateralOffset() const
Expand Down
9 changes: 6 additions & 3 deletions ...behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,8 +58,11 @@ double calcLaneChangeResampleInterval(
const double lane_changing_length, const double lane_changing_velocity);

double calcMinimumLaneChangeLength(
const LaneChangeParameters & lane_change_parameters, const std::vector<double> & shift_intervals,
const double length_to_intersection = 0.0);
const LaneChangeParameters & lane_change_parameters, const std::vector<double> & shift_intervals);

double calcMinimumLaneChangeLength(
const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelet & lane,
const LaneChangeParameters & lane_change_parameters, Direction direction);

double calcMaximumLaneChangeLength(
const double current_velocity, const LaneChangeParameters & lane_change_parameters,
Expand Down Expand Up @@ -139,7 +142,7 @@ std::vector<DrivableLanes> generateDrivableLanes(
double getLateralShift(const LaneChangePath & path);

bool hasEnoughLengthToLaneChangeAfterAbort(
const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelets & current_lanes,
const Pose & curent_pose, const double abort_return_dist,
const LaneChangeParameters & lane_change_parameters, const Direction direction);

Expand Down
61 changes: 26 additions & 35 deletions planning/behavior_path_lane_change_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@
namespace behavior_path_planner
{
using motion_utils::calcSignedArcLength;
using utils::lane_change::calcMinimumLaneChangeLength;
using utils::traffic_light::getDistanceToNextTrafficLight;

NormalLaneChange::NormalLaneChange(
Expand Down Expand Up @@ -274,8 +275,8 @@ void NormalLaneChange::insertStopPoint(
}

const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back());
const double lane_change_buffer =
utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals, 0.0);
const auto lane_change_buffer =
calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals);

const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) {
return utils::getSignedDistance(path.points.front().point.pose, target, lanelets);
Expand Down Expand Up @@ -562,10 +563,8 @@ bool NormalLaneChange::isNearEndOfCurrentLanes(

const auto & route_handler = getRouteHandler();
const auto & current_pose = getEgoPose();
const auto shift_intervals =
route_handler->getLateralIntervalsToPreferredLane(current_lanes.back());
const auto lane_change_buffer =
utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals);
const auto lane_change_buffer = calcMinimumLaneChangeLength(
route_handler, current_lanes.back(), *lane_change_parameters_, Direction::NONE);

const auto distance_to_lane_change_end = std::invoke([&]() {
auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes);
Expand Down Expand Up @@ -922,12 +921,10 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
const lanelet::ConstLanelets & target_backward_lanes) const
{
const auto current_pose = getEgoPose();
const auto & route_handler = *getRouteHandler();
const auto & route_handler = getRouteHandler();
const auto & common_parameters = planner_data_->parameters;
const auto shift_intervals =
route_handler.getLateralIntervalsToPreferredLane(current_lanes.back(), direction_);
const double minimum_lane_change_length =
utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals);
const auto minimum_lane_change_length = calcMinimumLaneChangeLength(
route_handler, current_lanes.back(), *lane_change_parameters_, direction_);

// Guard
if (objects.objects.empty()) {
Expand All @@ -936,9 +933,9 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(

// Get path
const auto path =
route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits<double>::max());
route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits<double>::max());
const auto target_path =
route_handler.getCenterLinePath(target_lanes, 0.0, std::numeric_limits<double>::max());
route_handler->getCenterLinePath(target_lanes, 0.0, std::numeric_limits<double>::max());

const auto current_polygon =
utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits<double>::max());
Expand Down Expand Up @@ -1125,32 +1122,30 @@ bool NormalLaneChange::hasEnoughLength(
const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes, const Direction direction) const
{
if (target_lanes.empty()) {
return false;
}

const auto current_pose = getEgoPose();
const auto & route_handler = *getRouteHandler();
const auto overall_graphs_ptr = route_handler.getOverallGraphPtr();
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::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals);
const auto & route_handler = getRouteHandler();
const auto overall_graphs_ptr = route_handler->getOverallGraphPtr();
const auto minimum_lane_change_length_to_preferred_lane = calcMinimumLaneChangeLength(
route_handler, target_lanes.back(), *lane_change_parameters_, direction);

const double lane_change_length = path.info.length.sum();
if (lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) {
return false;
}

const auto goal_pose = route_handler.getGoalPose();
const auto goal_pose = route_handler->getGoalPose();
if (
route_handler.isInGoalRouteSection(current_lanes.back()) &&
route_handler->isInGoalRouteSection(current_lanes.back()) &&
lane_change_length + minimum_lane_change_length_to_preferred_lane >
utils::getSignedDistance(current_pose, goal_pose, current_lanes)) {
return false;
}

// return if there are no target lanes
if (target_lanes.empty()) {
return true;
}

if (
lane_change_length + minimum_lane_change_length_to_preferred_lane >
utils::getDistanceToEndOfLane(current_pose, target_lanes)) {
Expand Down Expand Up @@ -1787,10 +1782,8 @@ bool NormalLaneChange::calcAbortPath()
const auto ego_nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold;

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::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals);
const auto minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength(
route_handler, selected_path.info.current_lanes.back(), *lane_change_parameters_, direction);

const auto & lane_changing_path = selected_path.path;
const auto lane_changing_end_pose_idx = std::invoke([&]() {
Expand Down Expand Up @@ -1837,7 +1830,7 @@ bool NormalLaneChange::calcAbortPath()
}

if (!utils::lane_change::hasEnoughLengthToLaneChangeAfterAbort(
*route_handler, reference_lanelets, current_pose, abort_return_dist,
route_handler, reference_lanelets, current_pose, abort_return_dist,
*lane_change_parameters_, direction)) {
RCLCPP_ERROR(logger_, "insufficient distance to abort.");
return false;
Expand Down Expand Up @@ -2044,10 +2037,8 @@ bool NormalLaneChange::isVehicleStuck(
route_handler->isInGoalRouteSection(current_lanes.back())
? utils::getSignedDistance(getEgoPose(), route_handler->getGoalPose(), current_lanes)
: utils::getDistanceToEndOfLane(getEgoPose(), current_lanes);
const auto shift_intervals =
route_handler->getLateralIntervalsToPreferredLane(current_lanes.back());
const double lane_change_buffer =
utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals, 0.0);
const auto lane_change_buffer = calcMinimumLaneChangeLength(
route_handler, current_lanes.back(), *lane_change_parameters_, Direction::NONE);
const double stop_point_buffer = lane_change_parameters_->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
95 changes: 56 additions & 39 deletions planning/behavior_path_lane_change_module/src/utils/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,8 @@
#include <algorithm>
#include <iterator>
#include <limits>
#include <memory>
#include <numeric>
#include <optional>
#include <string>
#include <vector>
Expand Down Expand Up @@ -77,29 +79,40 @@ double calcLaneChangeResampleInterval(
}

double calcMinimumLaneChangeLength(
const LaneChangeParameters & lane_change_parameters, const std::vector<double> & shift_intervals,
const double length_to_intersection)
const LaneChangeParameters & lane_change_parameters, const std::vector<double> & shift_intervals)
{
if (shift_intervals.empty()) {
return 0.0;
}

const double & vel = lane_change_parameters.minimum_lane_changing_velocity;
const auto lat_acc = lane_change_parameters.lane_change_lat_acc_map.find(vel);
const double & max_lateral_acc = lat_acc.second;
const double & lateral_jerk = lane_change_parameters.lane_changing_lateral_jerk;
const double & finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer;
const double & backward_buffer = lane_change_parameters.backward_length_buffer_for_end_of_lane;

double accumulated_length = length_to_intersection;
for (const auto & shift_interval : shift_intervals) {
const double t =
PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, max_lateral_acc);
accumulated_length += vel * t + finish_judge_buffer;
const auto min_vel = lane_change_parameters.minimum_lane_changing_velocity;
const auto [min_lat_acc, max_lat_acc] =
lane_change_parameters.lane_change_lat_acc_map.find(min_vel);
const auto lat_jerk = lane_change_parameters.lane_changing_lateral_jerk;
const auto finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer;

const auto calc_sum = [&](double sum, double shift_interval) {
const auto t = PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc);
return sum + (min_vel * t + finish_judge_buffer);
};

const auto total_length =
std::accumulate(shift_intervals.begin(), shift_intervals.end(), 0.0, calc_sum);

const auto backward_buffer = lane_change_parameters.backward_length_buffer_for_end_of_lane;
return total_length + backward_buffer * (static_cast<double>(shift_intervals.size()) - 1.0);
}

double calcMinimumLaneChangeLength(
const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelet & lane,
const LaneChangeParameters & lane_change_parameters, Direction direction)
{
if (!route_handler) {
return std::numeric_limits<double>::max();
}
accumulated_length += backward_buffer * (shift_intervals.size() - 1.0);

return accumulated_length;
const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lane, direction);
return utils::lane_change::calcMinimumLaneChangeLength(lane_change_parameters, shift_intervals);
}

double calcMaximumLaneChangeLength(
Expand All @@ -110,31 +123,34 @@ double calcMaximumLaneChangeLength(
return 0.0;
}

const double & finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer;
const double & lateral_jerk = lane_change_parameters.lane_changing_lateral_jerk;
const double & t_prepare = lane_change_parameters.lane_change_prepare_duration;
const auto finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer;
const auto lat_jerk = lane_change_parameters.lane_changing_lateral_jerk;
const auto t_prepare = lane_change_parameters.lane_change_prepare_duration;

double vel = current_velocity;
double accumulated_length = 0.0;
for (const auto & shift_interval : shift_intervals) {
auto vel = current_velocity;

const auto calc_sum = [&](double sum, double shift_interval) {
// prepare section
const double prepare_length = vel * t_prepare + 0.5 * max_acc * t_prepare * t_prepare;
const auto prepare_length = vel * t_prepare + 0.5 * max_acc * t_prepare * t_prepare;
vel = vel + max_acc * t_prepare;

// lane changing section
const auto lat_acc = lane_change_parameters.lane_change_lat_acc_map.find(vel);
const double t_lane_changing =
PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, lat_acc.second);
const double lane_changing_length =
const auto [min_lat_acc, max_lat_acc] =
lane_change_parameters.lane_change_lat_acc_map.find(vel);
const auto t_lane_changing =
PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc);
const auto lane_changing_length =
vel * t_lane_changing + 0.5 * max_acc * t_lane_changing * t_lane_changing;

accumulated_length += prepare_length + lane_changing_length + finish_judge_buffer;
vel = vel + max_acc * t_lane_changing;
}
accumulated_length +=
lane_change_parameters.backward_length_buffer_for_end_of_lane * (shift_intervals.size() - 1.0);
return sum + (prepare_length + lane_changing_length + finish_judge_buffer);
};

const auto total_length =
std::accumulate(shift_intervals.begin(), shift_intervals.end(), 0.0, calc_sum);

return accumulated_length;
const auto backward_buffer = lane_change_parameters.backward_length_buffer_for_end_of_lane;
return total_length + backward_buffer * (static_cast<double>(shift_intervals.size()) - 1.0);
}

double calcMinimumAcceleration(
Expand Down Expand Up @@ -658,23 +674,24 @@ double getLateralShift(const LaneChangePath & path)
}

bool hasEnoughLengthToLaneChangeAfterAbort(
const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelets & current_lanes,
const Pose & current_pose, const double abort_return_dist,
const LaneChangeParameters & lane_change_parameters, const Direction direction)
{
const auto shift_intervals =
route_handler.getLateralIntervalsToPreferredLane(current_lanes.back(), direction);
const double minimum_lane_change_length =
calcMinimumLaneChangeLength(lane_change_parameters, shift_intervals);
if (current_lanes.empty()) {
return false;
}

const auto minimum_lane_change_length = calcMinimumLaneChangeLength(
route_handler, current_lanes.back(), lane_change_parameters, direction);
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;
}

if (
route_handler.isInGoalRouteSection(current_lanes.back()) &&
abort_plus_lane_change_length >
utils::getSignedDistance(current_pose, route_handler.getGoalPose(), current_lanes)) {
utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)) {
return false;
}

Expand Down

0 comments on commit 2f41fe0

Please sign in to comment.