Skip to content

Commit

Permalink
fix(lane_following): remove lane change intersection check (autowaref…
Browse files Browse the repository at this point in the history 
…oundation#2826)

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
  • Loading branch information
@zulfaqar-azmi-t4
zulfaqar-azmi-t4 committed Feb 20, 2023
1 parent 8532092 commit 0669eae
Show file tree
Hide file tree
Showing 3 changed files with 1 addition and 126 deletions.
5 changes: 0 additions & 5 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 @@ -434,11 +434,6 @@ PathWithLaneId setDecelerationVelocity(
const lanelet::ConstLanelets & lanelet_sequence, const double lane_change_prepare_duration,
const double lane_change_buffer);

bool checkLaneIsInIntersection(
const RouteHandler & route_handler, const PathWithLaneId & ref,
const lanelet::ConstLanelets & lanelet_sequence, const BehaviorPathPlannerParameters & parameters,
const int num_lane_change, double & additional_length_to_add);

PathWithLaneId setDecelerationVelocity(
const PathWithLaneId & input, const double target_velocity, const Pose target_pose,
const double buffer, const double deceleration_interval);
Expand Down
11 changes: 1 addition & 10 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 @@ -142,17 +142,8 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const
{
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, 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);
}

const double lane_change_buffer =
util::calcLaneChangeBuffer(p, num_lane_change, optional_lengths);
const double lane_change_buffer = util::calcLaneChangeBuffer(p, num_lane_change);

reference_path = util::setDecelerationVelocity(
*route_handler, reference_path, current_lanes, parameters_.lane_change_prepare_duration,
Expand Down
111 changes: 0 additions & 111 deletions planning/behavior_path_planner/src/utilities.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1868,117 +1868,6 @@ PathWithLaneId getCenterLinePath(
return route_handler.getCenterLinePath(lanelet_sequence, s_backward, s_forward, true);
}

bool checkLaneIsInIntersection(
const RouteHandler & route_handler, const PathWithLaneId & reference_path,
const lanelet::ConstLanelets & lanelet_sequence, const BehaviorPathPlannerParameters & parameter,
const int num_lane_change, double & additional_length_to_add)
{
if (num_lane_change == 0) {
return false;
}

if (lanelet_sequence.size() < 2 || reference_path.points.empty()) {
return false;
}

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 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 & 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 {
const auto & attribute_name = lanelet::AttributeNamesString::LaneChange;
if (linestring.hasAttribute(attribute_name)) {
const auto attr = linestring.attribute(attribute_name);
if (attr.value() == std::string("yes")) {
return true;
}
}
return false;
};

const auto isLaneChangeAttributeYes =
[checkAttribute](const lanelet::ConstLanelet & lanelet) noexcept {
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)) {
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, goal_pose);

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

return true;
}

// for lane following
PathWithLaneId setDecelerationVelocity(
const RouteHandler & route_handler, const PathWithLaneId & input,
Expand Down

0 comments on commit 0669eae

Please sign in to comment.