Skip to content

Commit

Permalink
fix(behavior_path_planner): additional buffer during lane change for …
Browse files Browse the repository at this point in the history 
…intersection (#532)

* Fix: Add additional buffer in intersection

This is so that if lane change is performed on restricted areas, the final lane
change point will be before the restricted areas.

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

* ci(pre-commit): autofix

* Chore: Change variable names

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

* fix: change lanelet attribute check and intersection check

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

* chore: fix spelling mistake;

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

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@zulfaqar-azmi-t4 @pre-commit-ci
zulfaqar-azmi-t4 and pre-commit-ci[bot] authored Apr 18, 2022
1 parent 155eda4 commit 22e0ef5
Show file tree
Hide file tree
Showing 4 changed files with 117 additions and 16 deletions.
6 changes: 5 additions & 1 deletion planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -315,7 +315,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);
const BehaviorPathPlannerParameters & parameter, double optional_length = 0.0);

PathWithLaneId setDecelerationVelocity(
const RouteHandler & route_handler, const PathWithLaneId & input,
Expand All @@ -328,6 +328,10 @@ PathWithLaneId setDecelerationVelocity(
const double pullover_distance_min, const double distance_before_pull_over,
const double deceleration_interval, Pose goal_pose);

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

// object label
std::uint8_t getHighestProbLabel(const std::vector<ObjectClassification> & classification);

Expand Down
18 changes: 15 additions & 3 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 @@ -237,19 +237,31 @@ PathWithLaneId LaneChangeModule::getReferencePath() const
return reference_path;
}

if (reference_path.points.empty()) {
reference_path = util::getCenterLinePath(
*route_handler, current_lanes, current_pose, common_parameters.backward_path_length,
common_parameters.forward_path_length, common_parameters);
}

double optional_lengths{0.0};
const auto isInIntersection = util::checkLaneIsInIntersection(
*route_handler, reference_path, current_lanes, 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);

reference_path = util::getCenterLinePath(
*route_handler, current_lanes, current_pose, common_parameters.backward_path_length,
common_parameters.forward_path_length, common_parameters);
reference_path = util::setDecelerationVelocity(
*route_handler, reference_path, current_lanes, parameters_.lane_change_prepare_duration,
lane_change_buffer);

reference_path.drivable_area = util::generateDrivableArea(
current_lanes, common_parameters.drivable_area_resolution, common_parameters.vehicle_length,
planner_data_);
Expand Down
13 changes: 12 additions & 1 deletion 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 @@ -97,11 +97,22 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const
*route_handler, current_lanes, current_pose, p.backward_path_length, p.forward_path_length, p);

{
double optional_lengths{0.0};
const auto isInIntersection = util::checkLaneIsInIntersection(
*route_handler, reference_path, current_lanes, 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;
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);

reference_path = util::setDecelerationVelocity(
*route_handler, reference_path, current_lanes, parameters_.lane_change_prepare_duration,
lane_change_buffer);
Expand Down
96 changes: 85 additions & 11 deletions planning/behavior_path_planner/src/utilities.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1470,10 +1470,10 @@ double getDistanceToShoulderBoundary(
}

double getArcLengthToTargetLanelet(
const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelet & target_lane,
const lanelet::ConstLanelets & lanelet_sequence, const lanelet::ConstLanelet & target_lane,
const Pose & pose)
{
const auto arc_pose = lanelet::utils::getArcCoordinates(current_lanes, pose);
const auto arc_pose = lanelet::utils::getArcCoordinates(lanelet_sequence, pose);

const auto target_center_line = target_lane.centerline().basicLineString();

Expand All @@ -1497,8 +1497,8 @@ double getArcLengthToTargetLanelet(
back_pose.orientation = tf2::toMsg(tf_quat);
}

const auto arc_front = lanelet::utils::getArcCoordinates(current_lanes, front_pose);
const auto arc_back = lanelet::utils::getArcCoordinates(current_lanes, back_pose);
const auto arc_front = lanelet::utils::getArcCoordinates(lanelet_sequence, front_pose);
const auto arc_back = lanelet::utils::getArcCoordinates(lanelet_sequence, back_pose);

return std::max(
std::min(arc_front.length - arc_pose.length, arc_back.length - arc_pose.length), 0.0);
Expand Down Expand Up @@ -1601,17 +1601,17 @@ std::shared_ptr<PathWithLaneId> generateCenterLinePath(
return {}; // TODO(Horibe) What should be returned?
}

// For current_lanes with desired length
lanelet::ConstLanelets current_lanes = route_handler->getLaneletSequence(
// For lanelet_sequence with desired length
lanelet::ConstLanelets lanelet_sequence = route_handler->getLaneletSequence(
current_lane, pose->pose, p.backward_path_length, p.forward_path_length);

*centerline_path = getCenterLinePath(
*route_handler, current_lanes, pose->pose, p.backward_path_length, p.forward_path_length, p);
*route_handler, lanelet_sequence, pose->pose, p.backward_path_length, p.forward_path_length, p);

centerline_path->header = route_handler->getRouteHeader();

centerline_path->drivable_area = util::generateDrivableArea(
current_lanes, p.drivable_area_resolution, p.vehicle_length, planner_data);
lanelet_sequence, p.drivable_area_resolution, p.vehicle_length, planner_data);

return centerline_path;
}
Expand Down Expand Up @@ -1673,7 +1673,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)
const BehaviorPathPlannerParameters & parameter, double optional_length)
{
PathWithLaneId reference_path;

Expand All @@ -1686,8 +1686,8 @@ 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; // buffer for min_lane_change_length
const double buffer = parameter.backward_length_buffer_for_end_of_lane +
optional_length; // 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);
Expand All @@ -1707,6 +1707,80 @@ 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, double & additional_length_to_add)
{
if (lanelet_sequence.empty()) {
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)) {
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 prev_lane = std::next(checking_rev_iter);

if (prev_lane != lanelet_sequence.crend()) {
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)) {
// lambdas
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()));
};

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 {
break;
}
}
}
}
}
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);

constexpr double small_earlier_stopping_buffer = 0.2;
additional_length_to_add =
prohibited_arc_coordinate.length +
small_earlier_stopping_buffer; // additional a slight "buffer so that vehicle stop earlier"

return true;
}

PathWithLaneId setDecelerationVelocity(
const RouteHandler & route_handler, const PathWithLaneId & input,
const lanelet::ConstLanelets & lanelet_sequence, const double lane_change_prepare_duration,
Expand Down

0 comments on commit 22e0ef5

Please sign in to comment.