Skip to content

Commit

Permalink
fix(lane_change): skip generating path if lane changing path is too l…
Browse files Browse the repository at this point in the history 
…ong (autowarefoundation#8362)

rework. skip lane changing for insufficeient distance in target lane

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
Signed-off-by: Zulfaqar Azmi <[email protected]>
  • Loading branch information
@zulfaqar-azmi-t4 @saka1-s
zulfaqar-azmi-t4 authored and saka1-s committed Aug 15, 2024
1 parent cc44228 commit 8ae45cf
Show file tree
Hide file tree
Showing 4 changed files with 30 additions and 44 deletions.
1 change: 1 addition & 0 deletions ...ne_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -191,6 +191,7 @@ struct Lanes
{
bool current_lane_in_goal_section{false};
lanelet::ConstLanelets current;
lanelet::ConstLanelets target_neighbor;
lanelet::ConstLanelets target;
std::vector<lanelet::ConstLanelets> preceding_target;
};
Expand Down
4 changes: 0 additions & 4 deletions ...path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,10 +106,6 @@ lanelet::ConstLanelets getTargetNeighborLanes(
const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
const LaneChangeModuleType & type);

lanelet::BasicPolygon2d getTargetNeighborLanesPolygon(
const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
const LaneChangeModuleType & type);

bool isPathInLanelets(
const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes);
Expand Down
49 changes: 27 additions & 22 deletions planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,9 @@ void NormalLaneChange::update_lanes(const bool is_approved)
common_data_ptr_->lanes_ptr->target = target_lanes;

const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr;
common_data_ptr_->lanes_ptr->target_neighbor = utils::lane_change::getTargetNeighborLanes(
*route_handler_ptr, current_lanes, common_data_ptr_->lc_type);

common_data_ptr_->lanes_ptr->current_lane_in_goal_section =
route_handler_ptr->isInGoalRouteSection(current_lanes.back());
common_data_ptr_->lanes_ptr->preceding_target = utils::getPrecedingLanelets(
Expand Down Expand Up @@ -1464,6 +1467,9 @@ bool NormalLaneChange::getLaneChangePaths(
RCLCPP_DEBUG(logger_, " - sampling num for lat_acc: %lu", sample_lat_acc.size());

debug_print("Prepare path satisfy constraints");
const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end(
common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, lane_changing_start_pose);

for (const auto & lateral_acc : sample_lat_acc) {
const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk(
shift_length, lane_change_parameters_->lane_changing_lateral_jerk, lateral_acc);
Expand All @@ -1474,11 +1480,6 @@ bool NormalLaneChange::getLaneChangePaths(
const auto lane_changing_length = utils::lane_change::calcPhaseLength(
initial_lane_changing_velocity, getCommonParam().max_vel,
longitudinal_acc_on_lane_changing, lane_changing_time);
const auto terminal_lane_changing_velocity = std::min(
initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time,
getCommonParam().max_vel);
utils::lane_change::setPrepareVelocity(
prepare_segment, current_velocity, terminal_lane_changing_velocity);

const auto debug_print_lat = [&](const auto & s) {
RCLCPP_DEBUG(
Expand All @@ -1493,26 +1494,30 @@ bool NormalLaneChange::getLaneChangePaths(
continue;
}

if (is_goal_in_route) {
const double s_start =
lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose).length;
const double s_goal =
lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length;
const auto num =
// if multiple lane change is necessary, does the remaining distance is sufficient
const auto remaining_dist_in_target = std::invoke([&]() {
const auto finish_judge_buffer = lane_change_parameters_->lane_change_finish_judge_buffer;
const auto num_to_preferred_lane_from_target_lane =
std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction));
const double backward_buffer =
num == 0 ? 0.0 : lane_change_parameters_->backward_length_buffer_for_end_of_lane;
const double finish_judge_buffer =
lane_change_parameters_->lane_change_finish_judge_buffer;
if (
s_start + lane_changing_length + finish_judge_buffer + backward_buffer +
next_lane_change_buffer >
s_goal) {
debug_print_lat("Reject: length of lane changing path is longer than length to goal!!");
continue;
}
const auto backward_len_buffer =
lane_change_parameters_->backward_length_buffer_for_end_of_lane;
const auto backward_buffer_to_target_lane =
num_to_preferred_lane_from_target_lane == 0 ? 0.0 : backward_len_buffer;
return lane_changing_length + finish_judge_buffer + backward_buffer_to_target_lane +
next_lane_change_buffer;
});

if (remaining_dist_in_target > dist_lc_start_to_end_of_lanes) {
debug_print_lat("Reject: length of lane changing path is longer than length to goal!!");
continue;
}

const auto terminal_lane_changing_velocity = std::min(
initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time,
getCommonParam().max_vel);
utils::lane_change::setPrepareVelocity(
prepare_segment, current_velocity, terminal_lane_changing_velocity);

const auto target_segment = getTargetSegment(
target_lanes, lane_changing_start_pose, target_lane_length, lane_changing_length,
initial_lane_changing_velocity, next_lane_change_buffer);
Expand Down
20 changes: 2 additions & 18 deletions planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -290,21 +290,6 @@ lanelet::ConstLanelets getTargetNeighborLanes(
return neighbor_lanes;
}

lanelet::BasicPolygon2d getTargetNeighborLanesPolygon(
const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
const LaneChangeModuleType & type)
{
const auto target_neighbor_lanelets =
utils::lane_change::getTargetNeighborLanes(route_handler, current_lanes, type);
if (target_neighbor_lanelets.empty()) {
return {};
}
const auto target_neighbor_preferred_lane_poly = lanelet::utils::getPolygonFromArcLength(
target_neighbor_lanelets, 0, std::numeric_limits<double>::max());

return lanelet::utils::to2D(target_neighbor_preferred_lane_poly).basicPolygon();
}

bool isPathInLanelets(
const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes)
Expand Down Expand Up @@ -1227,9 +1212,8 @@ LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr)
lanes_polygon.expanded_target = utils::lane_change::createPolygon(
expanded_target_lanes, 0.0, std::numeric_limits<double>::max());

const auto & route_handler = *common_data_ptr->route_handler_ptr;
lanes_polygon.target_neighbor =
getTargetNeighborLanesPolygon(route_handler, lanes->current, common_data_ptr->lc_type);
lanes_polygon.target_neighbor = *utils::lane_change::createPolygon(
lanes->target_neighbor, 0.0, std::numeric_limits<double>::max());

lanes_polygon.preceding_target.reserve(lanes->preceding_target.size());
for (const auto & preceding_lane : lanes->preceding_target) {
Expand Down

0 comments on commit 8ae45cf

Please sign in to comment.