Skip to content

Commit

Permalink
fix(behavior_path_planner): fix check intersection for lane change (a…
Browse files Browse the repository at this point in the history 
…utowarefoundation#1851)

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
  • Loading branch information
@zulfaqar-azmi-t4 @satoshi-ota
zulfaqar-azmi-t4 authored and satoshi-ota committed Sep 14, 2022
1 parent 3bdbb12 commit 3b45bbc
Show file tree
Hide file tree
Showing 4 changed files with 83 additions and 50 deletions.
4 changes: 2 additions & 2 deletions planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -345,7 +345,7 @@ void shiftPose(Pose * pose, double shift_length);
PathWithLaneId getCenterLinePath(
const RouteHandler & route_handler, const lanelet::ConstLanelets & lanelet_sequence,
const Pose & pose, const double backward_path_length, const double forward_path_length,
const BehaviorPathPlannerParameters & parameter, double optional_length = 0.0);
const BehaviorPathPlannerParameters & parameter, const double optional_length = 0.0);

PathWithLaneId setDecelerationVelocity(
const RouteHandler & route_handler, const PathWithLaneId & input,
Expand All @@ -361,7 +361,7 @@ PathWithLaneId setDecelerationVelocity(
bool checkLaneIsInIntersection(
const RouteHandler & route_handler, const PathWithLaneId & ref,
const lanelet::ConstLanelets & lanelet_sequence, const BehaviorPathPlannerParameters & parameters,
double & additional_length_to_add);
const int num_lane_change, double & additional_length_to_add);

PathWithLaneId setDecelerationVelocity(
const PathWithLaneId & input, const double target_velocity, const Pose target_pose,
Expand Down
9 changes: 5 additions & 4 deletions planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -273,20 +273,21 @@ PathWithLaneId LaneChangeModule::getReferencePath() const
common_parameters.forward_path_length, common_parameters);
}

const int num_lane_change =
std::abs(route_handler->getNumLaneToPreferredLane(current_lanes.back()));
double optional_lengths{0.0};
const auto isInIntersection = util::checkLaneIsInIntersection(
*route_handler, reference_path, current_lanes, common_parameters, optional_lengths);
*route_handler, reference_path, current_lanes, common_parameters, num_lane_change,
optional_lengths);
if (isInIntersection) {
reference_path = util::getCenterLinePath(
*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 int num_lane_change =
std::abs(route_handler->getNumLaneToPreferredLane(current_lanes.back()));
const double lane_change_buffer =
num_lane_change * (common_parameters.minimum_lane_change_length + buffer);
num_lane_change * (common_parameters.minimum_lane_change_length + buffer) + optional_lengths;

reference_path = util::setDecelerationVelocity(
*route_handler, reference_path, current_lanes, parameters_->lane_change_prepare_duration,
Expand Down
13 changes: 6 additions & 7 deletions planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,21 +116,20 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const
reference_path, current_seg_idx, p.forward_path_length, p.backward_path_length);

{
const int num_lane_change =
std::abs(route_handler->getNumLaneToPreferredLane(current_lanes.back()));
double optional_lengths{0.0};
const auto isInIntersection = util::checkLaneIsInIntersection(
*route_handler, reference_path, current_lanes, p, optional_lengths);

*route_handler, reference_path, current_lanes, p, num_lane_change, optional_lengths);
if (isInIntersection) {
reference_path = util::getCenterLinePath(
*route_handler, current_lanes, current_pose, p.backward_path_length, p.forward_path_length,
p, optional_lengths);
}

// buffer for min_lane_change_length
const double buffer = p.backward_length_buffer_for_end_of_lane + optional_lengths;
const int num_lane_change =
std::abs(route_handler->getNumLaneToPreferredLane(current_lanes.back()));
const double lane_change_buffer = num_lane_change * (p.minimum_lane_change_length + buffer);
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;

reference_path = util::setDecelerationVelocity(
*route_handler, reference_path, current_lanes, parameters_.lane_change_prepare_duration,
Expand Down
107 changes: 70 additions & 37 deletions planning/behavior_path_planner/src/utilities.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1686,7 +1686,7 @@ void shiftPose(Pose * pose, double shift_length)
PathWithLaneId getCenterLinePath(
const RouteHandler & route_handler, const lanelet::ConstLanelets & lanelet_sequence,
const Pose & pose, const double backward_path_length, const double forward_path_length,
const BehaviorPathPlannerParameters & parameter, double optional_length)
const BehaviorPathPlannerParameters & parameter, const double optional_length)
{
PathWithLaneId reference_path;

Expand All @@ -1699,13 +1699,13 @@ 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 +
optional_length; // buffer for min_lane_change_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 =
num_lane_change * (parameter.minimum_lane_change_length + buffer);
std::fabs(num_lane_change * (parameter.minimum_lane_change_length + buffer) + optional_length);

if (route_handler.isDeadEndLanelet(lanelet_sequence.back())) {
s_forward = std::min(s_forward, lane_length - lane_change_buffer);
Expand All @@ -1723,44 +1723,77 @@ PathWithLaneId getCenterLinePath(
bool checkLaneIsInIntersection(
const RouteHandler & route_handler, const PathWithLaneId & reference_path,
const lanelet::ConstLanelets & lanelet_sequence, const BehaviorPathPlannerParameters & parameter,
double & additional_length_to_add)
const int num_lane_change, double & additional_length_to_add)
{
if (lanelet_sequence.size() < 2 || reference_path.points.empty()) {
if (num_lane_change == 0) {
return false;
}

const auto & path_points = reference_path.points;
auto end_of_route_pose = path_points.back().point.pose;

lanelet::ConstLanelet check_lane;
if (!lanelet::utils::query::getClosestLanelet(lanelet_sequence, end_of_route_pose, &check_lane)) {
if (lanelet_sequence.size() < 2 || reference_path.points.empty()) {
return false;
}

lanelet::ConstLanelets lane_change_prohibited_lanes{check_lane};

const auto checking_rev_iter = std::find_if(
lanelet_sequence.crbegin(), lanelet_sequence.crend(),
[&check_lane](const lanelet::ConstLanelet & lanelet) noexcept {
return lanelet.id() == check_lane.id();
});
if (checking_rev_iter == lanelet_sequence.crend()) {
const auto goal_pose = route_handler.getGoalPose();
lanelet::ConstLanelet goal_lanelet;
if (!route_handler.getGoalLanelet(&goal_lanelet)) {
std::cerr << "fail to get goal lanelet for intersection check.\n";
return false;
}
const auto goal_lanelet_point = lanelet::utils::conversion::toLaneletPoint(goal_pose.position);

const auto prev_lane = std::next(checking_rev_iter);
if (prev_lane == lanelet_sequence.crend()) {
const auto min_lane_change_distance =
num_lane_change *
(parameter.minimum_lane_change_length + parameter.backward_length_buffer_for_end_of_lane);
const double goal_distance_in_goal_lanelet = std::invoke([&goal_lanelet_point, &goal_lanelet]() {
const auto goal_centerline = goal_lanelet.centerline2d();
const auto arc_coordinate =
lanelet::geometry::toArcCoordinates(goal_centerline, goal_lanelet_point.basicPoint2d());
return arc_coordinate.length;
});

if (goal_distance_in_goal_lanelet > min_lane_change_distance) {
return false;
}

const auto lanes = route_handler.getNextLanelets(*prev_lane);
const auto isHaveNeighborWithTurnDirection = [&](const lanelet::ConstLanelets & lanes) noexcept {
return std::any_of(lanes.cbegin(), lanes.cend(), [](const lanelet::ConstLanelet & lane) {
return lane.hasAttribute("turn_direction");
});
};
if (!isHaveNeighborWithTurnDirection(lanes)) {
return false;
const auto & path_points = reference_path.points;
const auto & end_of_route_pose = path_points.back().point.pose;

if (lanelet_sequence.back().id() != goal_lanelet.id()) {
const auto get_signed_distance =
getSignedDistance(end_of_route_pose, goal_pose, lanelet_sequence);

if (get_signed_distance > min_lane_change_distance) {
return false;
}
}
// if you come to here, basically either back lane is goal, or back lane to goal is not enough
// distance
auto prev_lane = lanelet_sequence.crbegin();
auto find_intersection_iter = lanelet_sequence.crbegin();
for (; find_intersection_iter != lanelet_sequence.crend(); ++find_intersection_iter) {
prev_lane = std::next(find_intersection_iter);
if (prev_lane == lanelet_sequence.crend()) {
return false;
}
const auto lanes = route_handler.getNextLanelets(*prev_lane);
const auto is_neighbor_with_turn_direction = std::any_of(
lanes.cbegin(), lanes.cend(),
[](const lanelet::ConstLanelet & lane) { return lane.hasAttribute("turn_direction"); });

if (is_neighbor_with_turn_direction) {
const auto & intersection_back = find_intersection_iter->centerline2d().back();
geometry_msgs::msg::Pose intersection_back_pose;
intersection_back_pose.position.x = intersection_back.x();
intersection_back_pose.position.y = intersection_back.y();
intersection_back_pose.position.z = 0.0;
const auto get_signed_distance =
getSignedDistance(intersection_back_pose, goal_pose, lanelet_sequence);

if (get_signed_distance > min_lane_change_distance) {
return false;
}
break;
}
}

const auto checkAttribute = [](const lanelet::ConstLineString3d & linestring) noexcept {
Expand All @@ -1779,21 +1812,21 @@ bool checkLaneIsInIntersection(
return (checkAttribute(lanelet.rightBound()) || checkAttribute(lanelet.leftBound()));
};

lanelet::ConstLanelets lane_change_prohibited_lanes{*find_intersection_iter};
for (auto prev_ll_itr = prev_lane; prev_ll_itr != lanelet_sequence.crend(); ++prev_ll_itr) {
if (!isLaneChangeAttributeYes(*prev_ll_itr)) {
lane_change_prohibited_lanes.push_back(*prev_ll_itr);
} else {
if (isLaneChangeAttributeYes(*prev_ll_itr)) {
break;
}
lane_change_prohibited_lanes.push_back(*prev_ll_itr);
}

std::reverse(lane_change_prohibited_lanes.begin(), lane_change_prohibited_lanes.end());
const auto prohibited_arc_coordinate =
lanelet::utils::getArcCoordinates(lane_change_prohibited_lanes, end_of_route_pose);
lanelet::utils::getArcCoordinates(lane_change_prohibited_lanes, goal_pose);

additional_length_to_add = prohibited_arc_coordinate.length +
parameter.minimum_lane_change_length +
parameter.backward_length_buffer_for_end_of_lane;
additional_length_to_add =
(num_lane_change > 0 ? 1 : -1) *
(parameter.backward_length_buffer_for_end_of_lane + prohibited_arc_coordinate.length);

return true;
}
Expand All @@ -1812,7 +1845,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 - lane_change_buffer - arclength.length);
std::max(0.0, lane_length - std::fabs(lane_change_buffer) - arclength.length);
point.point.longitudinal_velocity_mps = std::min(
point.point.longitudinal_velocity_mps,
static_cast<float>(distance_to_end / lane_change_prepare_duration));
Expand Down

0 comments on commit 3b45bbc

Please sign in to comment.