From e074c8a90801c97dd0bb9b06dbdd33176ac87f32 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed, 7 Dec 2022 12:03:46 +0900 Subject: [PATCH] feat(behavior_path_planner): cut overlapped path (#2451) * feat(behavior_path_planner): cut overlapped path Signed-off-by: yutaka * clean code Signed-off-by: yutaka Signed-off-by: yutaka --- .../behavior_path_planner/utilities.hpp | 4 + .../avoidance/avoidance_module.cpp | 8 +- .../lane_change/lane_change_module.cpp | 6 +- .../lane_following/lane_following_module.cpp | 4 +- .../scene_module/pull_out/pull_out_module.cpp | 3 +- .../scene_module/pull_out/shift_pull_out.cpp | 3 +- .../pull_over/pull_over_module.cpp | 9 ++- .../side_shift/side_shift_module.cpp | 3 +- .../behavior_path_planner/src/utilities.cpp | 77 +++++++++++++++++++ 9 files changed, 105 insertions(+), 12 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 09ea9630983b4..d244c039b7734 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -289,6 +289,10 @@ std::vector generateDrivableLanes(const lanelet::ConstLanelets & std::vector generateDrivableLanesWithShoulderLanes( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & shoulder_lanes); +size_t getOverlappedLaneletId(const std::vector & lanes); +std::vector cutOverlappedLanes( + PathWithLaneId & path, const std::vector & lanes); + void occupancyGridToImage(const OccupancyGrid & occupancy_grid, cv::Mat * cv_image); void imageToOccupancyGrid(const cv::Mat & cv_image, OccupancyGrid * occupancy_grid); 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 513fc312d0f08..cd248af314fc1 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 @@ -2007,14 +2007,16 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c drivable_lanes.push_back(current_drivable_lanes); } - drivable_lanes = util::expandLanelets( - drivable_lanes, parameters_->drivable_area_left_bound_offset, + const auto shorten_lanes = util::cutOverlappedLanes(shifted_path->path, drivable_lanes); + + const auto extended_lanes = util::expandLanelets( + shorten_lanes, parameters_->drivable_area_left_bound_offset, parameters_->drivable_area_right_bound_offset, {"road_border"}); { const auto & p = planner_data_->parameters; shifted_path->path.drivable_area = util::generateDrivableArea( - shifted_path->path, drivable_lanes, p.drivable_area_resolution, p.vehicle_length, + shifted_path->path, extended_lanes, p.drivable_area_resolution, p.vehicle_length, planner_data_); } } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 17199c68fd895..afb1997d01e74 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -274,8 +274,9 @@ PathWithLaneId LaneChangeModule::getReferencePath() const lane_change_buffer); const auto drivable_lanes = util::generateDrivableLanes(current_lanes); + const auto shorten_lanes = util::cutOverlappedLanes(reference_path, drivable_lanes); const auto expanded_lanes = util::expandLanelets( - drivable_lanes, parameters_->drivable_area_left_bound_offset, + shorten_lanes, parameters_->drivable_area_left_bound_offset, parameters_->drivable_area_right_bound_offset); reference_path.drivable_area = util::generateDrivableArea( @@ -620,8 +621,9 @@ void LaneChangeModule::generateExtendedDrivableArea(PathWithLaneId & path) const auto & route_handler = planner_data_->route_handler; const auto drivable_lanes = lane_change_utils::generateDrivableLanes( *route_handler, status_.current_lanes, status_.lane_change_lanes); + const auto shorten_lanes = util::cutOverlappedLanes(path, drivable_lanes); const auto expanded_lanes = util::expandLanelets( - drivable_lanes, parameters_->drivable_area_left_bound_offset, + shorten_lanes, parameters_->drivable_area_left_bound_offset, parameters_->drivable_area_right_bound_offset); const double & resolution = common_parameters.drivable_area_resolution; diff --git a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp index 3c2c0da5f0634..4c9e3a7894201 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp @@ -137,8 +137,10 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const lane_change_buffer); } + const auto shorten_lanes = util::cutOverlappedLanes(reference_path, drivable_lanes); + const auto expanded_lanes = util::expandLanelets( - drivable_lanes, parameters_.drivable_area_left_bound_offset, + shorten_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); reference_path.drivable_area = util::generateDrivableArea( diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 0dca71c1a895f..532d3173c3155 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -185,8 +185,9 @@ BehaviorModuleOutput PullOutModule::plan() path = status_.backward_path; } + const auto shorten_lanes = util::cutOverlappedLanes(path, status_.lanes); const auto expanded_lanes = util::expandLanelets( - status_.lanes, parameters_.drivable_area_left_bound_offset, + shorten_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); path.drivable_area = util::generateDrivableArea( path, expanded_lanes, planner_data_->parameters.drivable_area_resolution, diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp index 77e6899609711..ab8e77ffe138c 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp @@ -92,8 +92,9 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) // Generate drivable area const double resolution = common_parameters.drivable_area_resolution; + const auto shorten_lanes = util::cutOverlappedLanes(shift_path, drivable_lanes); shift_path.drivable_area = util::generateDrivableArea( - shift_path, drivable_lanes, resolution, common_parameters.vehicle_length, planner_data_); + shift_path, shorten_lanes, resolution, common_parameters.vehicle_length, planner_data_); shift_path.header = planner_data_->route_handler->getRouteHeader(); diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index 55b1a5ebe17c1..faea572f97db6 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -438,8 +438,9 @@ BehaviorModuleOutput PullOverModule::plan() for (size_t i = status_.current_path_idx; i < status_.pull_over_path.partial_paths.size(); ++i) { auto & path = status_.pull_over_path.partial_paths.at(i); const auto p = planner_data_->parameters; + const auto shorten_lanes = util::cutOverlappedLanes(path, status_.lanes); const auto lane = util::expandLanelets( - status_.lanes, parameters_.drivable_area_left_bound_offset, + shorten_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); path.drivable_area = util::generateDrivableArea( path, lane, p.drivable_area_resolution, p.vehicle_length, planner_data_); @@ -610,8 +611,9 @@ PathWithLaneId PullOverModule::getReferencePath() const -calcMinimumShiftPathDistance(), parameters_.deceleration_interval); const auto drivable_lanes = util::generateDrivableLanes(status_.current_lanes); + const auto shorten_lanes = util::cutOverlappedLanes(reference_path, drivable_lanes); const auto lanes = util::expandLanelets( - drivable_lanes, parameters_.drivable_area_left_bound_offset, + shorten_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); reference_path.drivable_area = util::generateDrivableArea( @@ -659,8 +661,9 @@ PathWithLaneId PullOverModule::generateStopPath() const } const auto drivable_lanes = util::generateDrivableLanes(status_.current_lanes); + const auto shorten_lanes = util::cutOverlappedLanes(stop_path, drivable_lanes); const auto lanes = util::expandLanelets( - drivable_lanes, parameters_.drivable_area_left_bound_offset, + shorten_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); stop_path.drivable_area = util::generateDrivableArea( diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index 6ed656acddf05..fb87fadc476b3 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -416,7 +416,8 @@ void SideShiftModule::adjustDrivableArea(ShiftedPath * path) const -parameters_.drivable_area_right_bound_offset); const auto drivable_lanes = util::generateDrivableLanes(current_lanelets_); - const auto expanded_lanes = util::expandLanelets(drivable_lanes, left_offset, right_offset); + const auto shorten_lanes = util::cutOverlappedLanes(path->path, drivable_lanes); + const auto expanded_lanes = util::expandLanelets(shorten_lanes, left_offset, right_offset); { const auto & p = planner_data_->parameters; diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 89933bb020a3d..339965bdea493 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -21,6 +21,8 @@ #include "autoware_auto_perception_msgs/msg/predicted_object.hpp" #include "autoware_auto_perception_msgs/msg/predicted_path.hpp" +#include + #include #include #include @@ -1258,6 +1260,81 @@ std::vector generateDrivableLanesWithShoulderLanes( return drivable_lanes; } +size_t getOverlappedLaneletId(const std::vector & lanes) +{ + auto overlaps = [](const DrivableLanes & lanes, const DrivableLanes & target_lanes) { + const auto lanelets = transformToLanelets(lanes); + const auto target_lanelets = transformToLanelets(target_lanes); + + for (const auto & lanelet : lanelets) { + for (const auto & target_lanelet : target_lanelets) { + if (boost::geometry::overlaps( + lanelet.polygon2d().basicPolygon(), target_lanelet.polygon2d().basicPolygon())) { + return true; + } + } + } + + // No overlapping + return false; + }; + + if (lanes.size() <= 2) { + return lanes.size(); + } + + size_t overlapped_idx = lanes.size(); + for (size_t i = 0; i < lanes.size() - 2; ++i) { + for (size_t j = i + 2; j < lanes.size(); ++j) { + if (overlaps(lanes.at(i), lanes.at(j))) { + overlapped_idx = std::min(overlapped_idx, j); + } + } + } + + return overlapped_idx; +} + +std::vector cutOverlappedLanes( + PathWithLaneId & path, const std::vector & lanes) +{ + const size_t overlapped_lanelet_id = getOverlappedLaneletId(lanes); + if (overlapped_lanelet_id == lanes.size()) { + return lanes; + } + + const std::vector shorten_lanes{ + lanes.begin(), lanes.begin() + overlapped_lanelet_id}; + const std::vector removed_lanes{ + lanes.begin() + overlapped_lanelet_id, lanes.end()}; + + const auto transformed_lanes = util::transformToLanelets(removed_lanes); + + auto isIncluded = [&transformed_lanes](const std::vector & lane_ids) { + if (transformed_lanes.empty() || lane_ids.empty()) return false; + + for (const auto & transformed_lane : transformed_lanes) { + for (const auto & lane_id : lane_ids) { + if (lane_id == transformed_lane.id()) { + return true; + } + } + } + + return false; + }; + + for (size_t i = 0; i < path.points.size(); ++i) { + const auto & lane_ids = path.points.at(i).lane_ids; + if (isIncluded(lane_ids)) { + path.points.erase(path.points.begin() + i, path.points.end()); + break; + } + } + + return shorten_lanes; +} + // input lanes must be in sequence // NOTE: lanes in the path argument is used to calculate the size of the drivable area to cover // designated forward and backward length by getPathScope function.