diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index a719ab341b88b..c12f310fda088 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -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. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 1009540062a99..10ce16b00e2ae 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -168,6 +168,10 @@ std::pair separateObjectsByPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, const AvoidancePlanningData & data, const std::shared_ptr & parameters, DebugData & debug); + +DrivableLanes generateExpandDrivableLanes( + const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); } // namespace behavior_path_planner::utils::avoidance #endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index ce5d13d17aedf..eb6a1e125f412 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -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 -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) { @@ -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 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 @@ -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; diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 1c1c155a7b661..331c8912fddf6 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -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 +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) @@ -1673,4 +1689,143 @@ std::pair separateObjectsByPath( return std::make_pair(target_objects, other_objects); } + +DrivableLanes generateExpandDrivableLanes( + const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, + const std::shared_ptr & 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