Skip to content

Commit

Permalink
refactor(avoidance): generate drivable lanes in utils (#5018)
Browse files Browse the repository at this point in the history 
* refactor(avoidance): generate drivable lanes in utils

Signed-off-by: satoshi-ota <[email protected]>

* refactor(avoidance): use std::any_of

Signed-off-by: satoshi-ota <[email protected]>

---------

Signed-off-by: satoshi-ota <[email protected]>
  • Loading branch information
@satoshi-ota
satoshi-ota authored Sep 19, 2023
1 parent 7eabe35 commit 1506a94
Show file tree
Hide file tree
Showing 4 changed files with 165 additions and 158 deletions.
2 changes: 1 addition & 1 deletion ...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -460,7 +460,7 @@ class AvoidanceModule : public SceneModuleInterface
// TODO(murooka) freespace during turning in intersection where there is no neighbor lanes
// NOTE: Assume that there is no situation where there is an object in the middle lane of more
// than two lanes since which way to avoid is not obvious
void generateExtendedDrivableArea(BehaviorModuleOutput & output) const;
void generateExpandDrivableLanes(BehaviorModuleOutput & output) const;

/**
* @brief fill debug markers.
Expand Down
4 changes: 4 additions & 0 deletions planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -168,6 +168,10 @@ std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
const AvoidancePlanningData & data, const std::shared_ptr<AvoidanceParameters> & parameters,
DebugData & debug);

DrivableLanes generateExpandDrivableLanes(
const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters);
} // namespace behavior_path_planner::utils::avoidance

#endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_
162 changes: 5 additions & 157 deletions planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,22 +69,6 @@ using tier4_planning_msgs::msg::AvoidanceDebugFactor;

namespace
{
bool isEndPointsConnected(
const lanelet::ConstLanelet & left_lane, const lanelet::ConstLanelet & right_lane)
{
const auto & left_back_point_2d = right_lane.leftBound2d().back().basicPoint();
const auto & right_back_point_2d = left_lane.rightBound2d().back().basicPoint();

constexpr double epsilon = 1e-5;
return (right_back_point_2d - left_back_point_2d).norm() < epsilon;
}

template <typename T>
void pushUniqueVector(T & base_vector, const T & additional_vector)
{
base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end());
}

bool isDrivingSameLane(
const lanelet::ConstLanelets & previous_lanes, const lanelet::ConstLanelets & current_lanes)
{
Expand Down Expand Up @@ -1912,148 +1896,12 @@ bool AvoidanceModule::isSafePath(
return safe_ || safe_count_ > parameters_->hysteresis_factor_safe_count;
}

void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output) const
void AvoidanceModule::generateExpandDrivableLanes(BehaviorModuleOutput & output) const
{
const auto has_same_lane =
[](const lanelet::ConstLanelets lanes, const lanelet::ConstLanelet & lane) {
if (lanes.empty()) return false;
const auto has_same = [&](const auto & ll) { return ll.id() == lane.id(); };
return std::find_if(lanes.begin(), lanes.end(), has_same) != lanes.end();
};

const auto & route_handler = planner_data_->route_handler;
const auto & current_lanes = avoid_data_.current_lanelets;
const auto & enable_opposite = parameters_->use_opposite_lane;
std::vector<DrivableLanes> drivable_lanes;

for (const auto & current_lane : current_lanes) {
DrivableLanes current_drivable_lanes;
current_drivable_lanes.left_lane = current_lane;
current_drivable_lanes.right_lane = current_lane;

if (!parameters_->use_adjacent_lane) {
drivable_lanes.push_back(current_drivable_lanes);
continue;
}

// 1. get left/right side lanes
const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) {
const auto all_left_lanelets =
route_handler->getAllLeftSharedLinestringLanelets(target_lane, enable_opposite, true);
if (!all_left_lanelets.empty()) {
current_drivable_lanes.left_lane = all_left_lanelets.back(); // leftmost lanelet
pushUniqueVector(
current_drivable_lanes.middle_lanes,
lanelet::ConstLanelets(all_left_lanelets.begin(), all_left_lanelets.end() - 1));
}
};
const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) {
const auto all_right_lanelets =
route_handler->getAllRightSharedLinestringLanelets(target_lane, enable_opposite, true);
if (!all_right_lanelets.empty()) {
current_drivable_lanes.right_lane = all_right_lanelets.back(); // rightmost lanelet
pushUniqueVector(
current_drivable_lanes.middle_lanes,
lanelet::ConstLanelets(all_right_lanelets.begin(), all_right_lanelets.end() - 1));
}
};

update_left_lanelets(current_lane);
update_right_lanelets(current_lane);

// 2.1 when there are multiple lanes whose previous lanelet is the same
const auto get_next_lanes_from_same_previous_lane =
[&route_handler](const lanelet::ConstLanelet & lane) {
// get previous lane, and return false if previous lane does not exist
lanelet::ConstLanelets prev_lanes;
if (!route_handler->getPreviousLaneletsWithinRoute(lane, &prev_lanes)) {
return lanelet::ConstLanelets{};
}

lanelet::ConstLanelets next_lanes;
for (const auto & prev_lane : prev_lanes) {
const auto next_lanes_from_prev = route_handler->getNextLanelets(prev_lane);
pushUniqueVector(next_lanes, next_lanes_from_prev);
}
return next_lanes;
};

const auto next_lanes_for_right =
get_next_lanes_from_same_previous_lane(current_drivable_lanes.right_lane);
const auto next_lanes_for_left =
get_next_lanes_from_same_previous_lane(current_drivable_lanes.left_lane);

// 2.2 look for neighbor lane recursively, where end line of the lane is connected to end line
// of the original lane
const auto update_drivable_lanes =
[&](const lanelet::ConstLanelets & next_lanes, const bool is_left) {
for (const auto & next_lane : next_lanes) {
const auto & edge_lane =
is_left ? current_drivable_lanes.left_lane : current_drivable_lanes.right_lane;
if (next_lane.id() == edge_lane.id()) {
continue;
}

const auto & left_lane = is_left ? next_lane : edge_lane;
const auto & right_lane = is_left ? edge_lane : next_lane;
if (!isEndPointsConnected(left_lane, right_lane)) {
continue;
}

if (is_left) {
current_drivable_lanes.left_lane = next_lane;
} else {
current_drivable_lanes.right_lane = next_lane;
}

if (!has_same_lane(current_drivable_lanes.middle_lanes, edge_lane)) {
if (is_left) {
if (current_drivable_lanes.right_lane.id() != edge_lane.id()) {
current_drivable_lanes.middle_lanes.push_back(edge_lane);
}
} else {
if (current_drivable_lanes.left_lane.id() != edge_lane.id()) {
current_drivable_lanes.middle_lanes.push_back(edge_lane);
}
}
}

return true;
}
return false;
};

const auto expand_drivable_area_recursively =
[&](const lanelet::ConstLanelets & next_lanes, const bool is_left) {
// NOTE: set max search num to avoid infinity loop for drivable area expansion
constexpr size_t max_recursive_search_num = 3;
for (size_t i = 0; i < max_recursive_search_num; ++i) {
const bool is_update_kept = update_drivable_lanes(next_lanes, is_left);
if (!is_update_kept) {
break;
}
if (i == max_recursive_search_num - 1) {
RCLCPP_ERROR(
rclcpp::get_logger("behavior_path_planner").get_child("avoidance"),
"Drivable area expansion reaches max iteration.");
}
}
};
expand_drivable_area_recursively(next_lanes_for_right, false);
expand_drivable_area_recursively(next_lanes_for_left, true);

// 3. update again for new left/right lanes
update_left_lanelets(current_drivable_lanes.left_lane);
update_right_lanelets(current_drivable_lanes.right_lane);

// 4. compensate that current_lane is in either of left_lane, right_lane or middle_lanes.
if (
current_drivable_lanes.left_lane.id() != current_lane.id() &&
current_drivable_lanes.right_lane.id() != current_lane.id()) {
current_drivable_lanes.middle_lanes.push_back(current_lane);
}

drivable_lanes.push_back(current_drivable_lanes);
for (const auto & lanelet : avoid_data_.current_lanelets) {
drivable_lanes.push_back(
utils::avoidance::generateExpandDrivableLanes(lanelet, planner_data_, parameters_));
}

{ // for new architecture
Expand Down Expand Up @@ -2206,7 +2054,7 @@ BehaviorModuleOutput AvoidanceModule::plan()
utils::clipPathLength(*output.path, ego_idx, planner_data_->parameters);

// Drivable area generation.
generateExtendedDrivableArea(output);
generateExpandDrivableLanes(output);
setDrivableLanes(output.drivable_area_info.drivable_lanes);

return output;
Expand Down
155 changes: 155 additions & 0 deletions planning/behavior_path_planner/src/utils/avoidance/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -202,6 +202,22 @@ Polygon2d createOneStepPolygon(

return hull_polygon;
}

bool isEndPointsConnected(
const lanelet::ConstLanelet & left_lane, const lanelet::ConstLanelet & right_lane)
{
const auto & left_back_point_2d = right_lane.leftBound2d().back().basicPoint();
const auto & right_back_point_2d = left_lane.rightBound2d().back().basicPoint();

constexpr double epsilon = 1e-5;
return (right_back_point_2d - left_back_point_2d).norm() < epsilon;
}

template <typename T>
void pushUniqueVector(T & base_vector, const T & additional_vector)
{
base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end());
}
} // namespace

bool isOnRight(const ObjectData & obj)
Expand Down Expand Up @@ -1673,4 +1689,143 @@ std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(

return std::make_pair(target_objects, other_objects);
}

DrivableLanes generateExpandDrivableLanes(
const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & parameters)
{
const auto & route_handler = planner_data->route_handler;

DrivableLanes current_drivable_lanes;
current_drivable_lanes.left_lane = lanelet;
current_drivable_lanes.right_lane = lanelet;

if (!parameters->use_adjacent_lane) {
return current_drivable_lanes;
}

// 1. get left/right side lanes
const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) {
const auto all_left_lanelets = route_handler->getAllLeftSharedLinestringLanelets(
target_lane, parameters->use_opposite_lane, true);
if (!all_left_lanelets.empty()) {
current_drivable_lanes.left_lane = all_left_lanelets.back(); // leftmost lanelet
pushUniqueVector(
current_drivable_lanes.middle_lanes,
lanelet::ConstLanelets(all_left_lanelets.begin(), all_left_lanelets.end() - 1));
}
};
const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) {
const auto all_right_lanelets = route_handler->getAllRightSharedLinestringLanelets(
target_lane, parameters->use_opposite_lane, true);
if (!all_right_lanelets.empty()) {
current_drivable_lanes.right_lane = all_right_lanelets.back(); // rightmost lanelet
pushUniqueVector(
current_drivable_lanes.middle_lanes,
lanelet::ConstLanelets(all_right_lanelets.begin(), all_right_lanelets.end() - 1));
}
};

update_left_lanelets(lanelet);
update_right_lanelets(lanelet);

// 2.1 when there are multiple lanes whose previous lanelet is the same
const auto get_next_lanes_from_same_previous_lane =
[&route_handler](const lanelet::ConstLanelet & lane) {
// get previous lane, and return false if previous lane does not exist
lanelet::ConstLanelets prev_lanes;
if (!route_handler->getPreviousLaneletsWithinRoute(lane, &prev_lanes)) {
return lanelet::ConstLanelets{};
}

lanelet::ConstLanelets next_lanes;
for (const auto & prev_lane : prev_lanes) {
const auto next_lanes_from_prev = route_handler->getNextLanelets(prev_lane);
pushUniqueVector(next_lanes, next_lanes_from_prev);
}
return next_lanes;
};

const auto next_lanes_for_right =
get_next_lanes_from_same_previous_lane(current_drivable_lanes.right_lane);
const auto next_lanes_for_left =
get_next_lanes_from_same_previous_lane(current_drivable_lanes.left_lane);

// 2.2 look for neighbor lane recursively, where end line of the lane is connected to end line
// of the original lane
const auto update_drivable_lanes =
[&](const lanelet::ConstLanelets & next_lanes, const bool is_left) {
for (const auto & next_lane : next_lanes) {
const auto & edge_lane =
is_left ? current_drivable_lanes.left_lane : current_drivable_lanes.right_lane;
if (next_lane.id() == edge_lane.id()) {
continue;
}

const auto & left_lane = is_left ? next_lane : edge_lane;
const auto & right_lane = is_left ? edge_lane : next_lane;
if (!isEndPointsConnected(left_lane, right_lane)) {
continue;
}

if (is_left) {
current_drivable_lanes.left_lane = next_lane;
} else {
current_drivable_lanes.right_lane = next_lane;
}

const auto & middle_lanes = current_drivable_lanes.middle_lanes;
const auto has_same_lane = std::any_of(
middle_lanes.begin(), middle_lanes.end(),
[&edge_lane](const auto & lane) { return lane.id() == edge_lane.id(); });

if (!has_same_lane) {
if (is_left) {
if (current_drivable_lanes.right_lane.id() != edge_lane.id()) {
current_drivable_lanes.middle_lanes.push_back(edge_lane);
}
} else {
if (current_drivable_lanes.left_lane.id() != edge_lane.id()) {
current_drivable_lanes.middle_lanes.push_back(edge_lane);
}
}
}

return true;
}
return false;
};

const auto expand_drivable_area_recursively =
[&](const lanelet::ConstLanelets & next_lanes, const bool is_left) {
// NOTE: set max search num to avoid infinity loop for drivable area expansion
constexpr size_t max_recursive_search_num = 3;
for (size_t i = 0; i < max_recursive_search_num; ++i) {
const bool is_update_kept = update_drivable_lanes(next_lanes, is_left);
if (!is_update_kept) {
break;
}
if (i == max_recursive_search_num - 1) {
RCLCPP_ERROR(
rclcpp::get_logger("behavior_path_planner").get_child("avoidance"),
"Drivable area expansion reaches max iteration.");
}
}
};
expand_drivable_area_recursively(next_lanes_for_right, false);
expand_drivable_area_recursively(next_lanes_for_left, true);

// 3. update again for new left/right lanes
update_left_lanelets(current_drivable_lanes.left_lane);
update_right_lanelets(current_drivable_lanes.right_lane);

// 4. compensate that current_lane is in either of left_lane, right_lane or middle_lanes.
if (
current_drivable_lanes.left_lane.id() != lanelet.id() &&
current_drivable_lanes.right_lane.id() != lanelet.id()) {
current_drivable_lanes.middle_lanes.push_back(lanelet);
}

return current_drivable_lanes;
}
} // namespace behavior_path_planner::utils::avoidance

0 comments on commit 1506a94

Please sign in to comment.