feat(behavior_path_planner): update expansion function for drivable a…

…rea (autowarefoundation#2320)

* fix(behavior_path_planner): delete unnecessary codes

Signed-off-by: yutaka <purewater0901@gmail.com>

* feat(behavior_path_planner): reorganize behavior path

Signed-off-by: yutaka <purewater0901@gmail.com>

* empty commit

Signed-off-by: yutaka <purewater0901@gmail.com>

* feat(behavior_path_planner): reorganize avoidance path

Signed-off-by: yutaka <purewater0901@gmail.com>

* feat(behavior_path_planner): update expand lanelet

Signed-off-by: yutaka <purewater0901@gmail.com>

* fix lane change

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

Signed-off-by: yutaka <purewater0901@gmail.com>
Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp

@@ -59,6 +59,13 @@ struct Approval
   ModuleNameStamped is_force_approved{};
 };
 
+struct DrivableLanes
+{
+  lanelet::ConstLanelet right_lane;
+  lanelet::ConstLanelet left_lane;
+  lanelet::ConstLanelets middle_lanes;
+};
+
 struct PlannerData
 {
   PoseStamped::ConstSharedPtr self_pose{};

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp

@@ -135,6 +135,10 @@ TurnSignalInfo calc_turn_signal_info(
 
 void get_turn_signal_info(
   const LaneChangePath & lane_change_path, TurnSignalInfo * turn_signal_info);
+
+std::vector<DrivableLanes> generateDrivableLanes(
+  const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
+  const lanelet::ConstLanelets & lane_change_lanes);
 }  // namespace behavior_path_planner::lane_change_utils
 
 #endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__UTIL_HPP_

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp

@@ -52,7 +52,7 @@ struct PullOutStatus
   PathWithLaneId backward_path;
   lanelet::ConstLanelets current_lanes;
   lanelet::ConstLanelets pull_out_lanes;
-  lanelet::ConstLanelets lanes;
+  std::vector<DrivableLanes> lanes;
   std::vector<uint64_t> lane_follow_lane_ids;
   std::vector<uint64_t> pull_out_lane_ids;
   bool is_safe = false;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/util.hpp

@@ -40,9 +40,7 @@ PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLa
 PathWithLaneId getBackwardPath(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
   const Pose & current_pose, const Pose & backed_pose, const double velocity);
-lanelet::ConstLanelets getPullOutLanes(
-  const lanelet::ConstLanelets & current_lanes,
-  const std::shared_ptr<const PlannerData> & planner_data);
+lanelet::ConstLanelets getPullOutLanes(const std::shared_ptr<const PlannerData> & planner_data);
 Pose getBackedPose(
   const Pose & current_pose, const double & yaw_shoulder_lane, const double & back_distance);
 }  // namespace behavior_path_planner::pull_out_utils

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp

@@ -59,7 +59,7 @@ struct PUllOverStatus
   std::shared_ptr<PathWithLaneId> prev_stop_path{nullptr};
   lanelet::ConstLanelets current_lanes{};
   lanelet::ConstLanelets pull_over_lanes{};
-  lanelet::ConstLanelets lanes{};  // current + pull_over
+  std::vector<DrivableLanes> lanes{};  // current + pull_over
   bool has_decided_path{false};
   bool is_safe{false};
   bool prev_is_safe{false};

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -276,6 +276,15 @@ PredictedObjects filterObjectsByVelocity(
   const PredictedObjects & objects, double min_v, double max_v);
 
 // drivable area generation
+lanelet::ConstLanelets transformToLanelets(const DrivableLanes & drivable_lanes);
+lanelet::ConstLanelets transformToLanelets(const std::vector<DrivableLanes> & drivable_lanes);
+boost::optional<lanelet::ConstLanelet> getRightLanelet(
+  const lanelet::ConstLanelet & current_lane, const lanelet::ConstLanelets & shoulder_lanes);
+boost::optional<lanelet::ConstLanelet> getLeftLanelet(
+  const lanelet::ConstLanelet & current_lane, const lanelet::ConstLanelets & shoulder_lanes);
+std::vector<DrivableLanes> generateDrivableLanes(const lanelet::ConstLanelets & current_lanes);
+std::vector<DrivableLanes> generateDrivableLanesWithShoulderLanes(
+  const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & shoulder_lanes);
 
 void occupancyGridToImage(const OccupancyGrid & occupancy_grid, cv::Mat * cv_image);
 
@@ -285,22 +294,22 @@ cv::Point toCVPoint(
   const Point & geom_point, const double width_m, const double height_m, const double resolution);
 
 OccupancyGrid generateDrivableArea(
-  const PathWithLaneId & path, const lanelet::ConstLanelets & lanes, const double resolution,
+  const PathWithLaneId & path, const std::vector<DrivableLanes> & lanes, const double resolution,
   const double vehicle_length, const std::shared_ptr<const PlannerData> planner_data);
 
 lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets(
   const std::shared_ptr<const PlannerData> & planner_data);
 
 /**
  * @brief Expand the borders of the given lanelets
- * @param [in] lanelets lanelets to expand
+ * @param [in] drivable_lanes lanelets to expand
  * @param [in] left_bound_offset [m] expansion distance of the left bound
  * @param [in] right_bound_offset [m] expansion distance of the right bound
  * @param [in] types_to_skip linestring types that will not be expanded
  * @return expanded lanelets
  */
-lanelet::ConstLanelets expandLanelets(
-  const lanelet::ConstLanelets & lanelets, const double left_bound_offset,
+std::vector<DrivableLanes> expandLanelets(
+  const std::vector<DrivableLanes> & drivable_lanes, const double left_bound_offset,
   const double right_bound_offset, const std::vector<std::string> & types_to_skip = {});
 
 // goal management

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -1791,24 +1791,52 @@ double AvoidanceModule::getLeftShiftBound() const
 // two lanes since which way to avoid is not obvious
 void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) 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 = avoidance_data_.current_lanelets;
-  lanelet::ConstLanelets extended_lanelets = current_lanes;
+  const auto & enable_opposite = parameters_->enable_avoidance_over_opposite_direction;
+  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_->enable_avoidance_over_same_direction) {
-      break;
+      drivable_lanes.push_back(current_drivable_lanes);
+      continue;
     }
 
-    const auto extend_from_current_lane = std::invoke(
-      [this, &route_handler](const lanelet::ConstLanelet & lane) {
-        const auto enable_opposite = parameters_->enable_avoidance_over_opposite_direction;
-        return route_handler->getAllSharedLineStringLanelets(lane, true, true, enable_opposite);
-      },
-      current_lane);
-    extended_lanelets.reserve(extended_lanelets.size() + extend_from_current_lane.size());
-    extended_lanelets.insert(
-      extended_lanelets.end(), extend_from_current_lane.begin(), extend_from_current_lane.end());
+    // get left side lane
+    const lanelet::ConstLanelets all_left_lanelets =
+      route_handler->getAllLeftSharedLinestringLanelets(current_lane, enable_opposite);
+    if (!all_left_lanelets.empty()) {
+      current_drivable_lanes.left_lane = all_left_lanelets.back();  // leftmost lanelet
+
+      for (int i = all_left_lanelets.size() - 2; i >= 0; --i) {
+        current_drivable_lanes.middle_lanes.push_back(all_left_lanelets.at(i));
+      }
+    }
+
+    // get right side lane
+    const lanelet::ConstLanelets all_right_lanelets =
+      route_handler->getAllRightSharedLinestringLanelets(current_lane, enable_opposite);
+    if (!all_right_lanelets.empty()) {
+      current_drivable_lanes.right_lane = all_right_lanelets.back();  // rightmost lanelet
+      if (current_drivable_lanes.left_lane.id() != current_lane.id()) {
+        current_drivable_lanes.middle_lanes.push_back(current_lane);
+      }
+
+      for (size_t i = 0; i < all_right_lanelets.size() - 1; ++i) {
+        current_drivable_lanes.middle_lanes.push_back(all_right_lanelets.at(i));
+      }
+    }
 
     // 2. when there are multiple turning lanes whose previous lanelet is the same in
     // intersection
@@ -1837,31 +1865,43 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c
 
     // 2.1 look for neighbour lane, where end line of the lane is connected to end line of the
     // original lane
-    std::copy_if(
-      next_lanes_from_intersection.begin(), next_lanes_from_intersection.end(),
-      std::back_inserter(extended_lanelets),
-      [&current_lane](const lanelet::ConstLanelet & neighbor_lane) {
-        const auto & next_left_back_point_2d = neighbor_lane.leftBound2d().back().basicPoint();
-        const auto & next_right_back_point_2d = neighbor_lane.rightBound2d().back().basicPoint();
-
-        const auto & orig_left_back_point_2d = current_lane.leftBound2d().back().basicPoint();
-        const auto & orig_right_back_point_2d = current_lane.rightBound2d().back().basicPoint();
-        constexpr double epsilon = 1e-5;
-        const bool is_neighbour_lane =
-          (next_left_back_point_2d - orig_right_back_point_2d).norm() < epsilon ||
-          (next_right_back_point_2d - orig_left_back_point_2d).norm() < epsilon;
-        return (current_lane.id() != neighbor_lane.id() && is_neighbour_lane);
-      });
-  }
-
-  extended_lanelets = util::expandLanelets(
-    extended_lanelets, parameters_->drivable_area_left_bound_offset,
+    for (const auto & next_lane : next_lanes_from_intersection) {
+      if (current_lane.id() == next_lane.id()) {
+        continue;
+      }
+      constexpr double epsilon = 1e-5;
+      const auto & next_left_back_point_2d = next_lane.leftBound2d().back().basicPoint();
+      const auto & next_right_back_point_2d = next_lane.rightBound2d().back().basicPoint();
+      const auto & orig_left_back_point_2d = current_lane.leftBound2d().back().basicPoint();
+      const auto & orig_right_back_point_2d = current_lane.rightBound2d().back().basicPoint();
+
+      if ((next_right_back_point_2d - orig_left_back_point_2d).norm() < epsilon) {
+        current_drivable_lanes.left_lane = next_lane;
+        if (
+          current_drivable_lanes.right_lane.id() != current_lane.id() &&
+          !has_same_lane(current_drivable_lanes.middle_lanes, current_lane)) {
+          current_drivable_lanes.middle_lanes.push_back(current_lane);
+        }
+      } else if (
+        (next_left_back_point_2d - orig_right_back_point_2d).norm() < epsilon &&
+        !has_same_lane(current_drivable_lanes.middle_lanes, current_lane)) {
+        current_drivable_lanes.right_lane = next_lane;
+        if (current_drivable_lanes.left_lane.id() != current_lane.id()) {
+          current_drivable_lanes.middle_lanes.push_back(current_lane);
+        }
+      }
+    }
+    drivable_lanes.push_back(current_drivable_lanes);
+  }
+
+  drivable_lanes = util::expandLanelets(
+    drivable_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, extended_lanelets, p.drivable_area_resolution, p.vehicle_length,
+      shifted_path->path, drivable_lanes, p.drivable_area_resolution, p.vehicle_length,
       planner_data_);
   }
 }

planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp

@@ -170,17 +170,16 @@ BehaviorModuleOutput LaneChangeModule::plan()
   // Generate drivable area
   {
     const auto & common_parameters = planner_data_->parameters;
-    lanelet::ConstLanelets lanes;
-    lanes.reserve(status_.current_lanes.size() + status_.lane_change_lanes.size());
-    lanes.insert(lanes.end(), status_.current_lanes.begin(), status_.current_lanes.end());
-    lanes.insert(lanes.end(), status_.lane_change_lanes.begin(), status_.lane_change_lanes.end());
-    lanes = util::expandLanelets(
-      lanes, parameters_->drivable_area_left_bound_offset,
+    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 expanded_lanes = util::expandLanelets(
+      drivable_lanes, parameters_->drivable_area_left_bound_offset,
       parameters_->drivable_area_right_bound_offset);
 
     const double & resolution = common_parameters.drivable_area_resolution;
     path.drivable_area = util::generateDrivableArea(
-      path, lanes, resolution, common_parameters.vehicle_length, planner_data_);
+      path, expanded_lanes, resolution, common_parameters.vehicle_length, planner_data_);
   }
 
   if (isAbortConditionSatisfied()) {
@@ -329,12 +328,13 @@ PathWithLaneId LaneChangeModule::getReferencePath() const
     *route_handler, reference_path, current_lanes, parameters_->lane_change_prepare_duration,
     lane_change_buffer);
 
-  const auto current_extended_lanes = util::expandLanelets(
-    current_lanes, parameters_->drivable_area_left_bound_offset,
+  const auto drivable_lanes = util::generateDrivableLanes(current_lanes);
+  const auto expanded_lanes = util::expandLanelets(
+    drivable_lanes, parameters_->drivable_area_left_bound_offset,
     parameters_->drivable_area_right_bound_offset);
 
   reference_path.drivable_area = util::generateDrivableArea(
-    reference_path, current_extended_lanes, common_parameters.drivable_area_resolution,
+    reference_path, expanded_lanes, common_parameters.drivable_area_resolution,
     common_parameters.vehicle_length, planner_data_);
 
   return reference_path;

planning/behavior_path_planner/src/scene_module/lane_change/util.cpp

@@ -627,4 +627,36 @@ void get_turn_signal_info(
   turn_signal_info->desired_end_point = lane_change_path.turn_signal_info.desired_end_point;
 }
 
+std::vector<DrivableLanes> generateDrivableLanes(
+  const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
+  const lanelet::ConstLanelets & lane_change_lanes)
+{
+  std::vector<DrivableLanes> drivable_lanes(current_lanes.size());
+  for (size_t i = 0; i < current_lanes.size(); ++i) {
+    const auto & current_lane = current_lanes.at(i);
+    drivable_lanes.at(i).left_lane = current_lane;
+    drivable_lanes.at(i).right_lane = current_lane;
+
+    const auto left_lane = route_handler.getLeftLanelet(current_lane);
+    const auto right_lane = route_handler.getRightLanelet(current_lane);
+    if (!left_lane && !right_lane) {
+      continue;
+    }
+
+    for (const auto & lc_lane : lane_change_lanes) {
+      if (left_lane && lc_lane.id() == left_lane->id()) {
+        drivable_lanes.at(i).left_lane = lc_lane;
+        break;
+      }
+
+      if (right_lane && lc_lane.id() == right_lane->id()) {
+        drivable_lanes.at(i).right_lane = lc_lane;
+        break;
+      }
+    }
+  }
+
+  return drivable_lanes;
+}
+
 }  // namespace behavior_path_planner::lane_change_utils

planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp

@@ -93,8 +93,9 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const
   }
 
   // For current_lanes with desired length
-  lanelet::ConstLanelets current_lanes = planner_data_->route_handler->getLaneletSequence(
+  const auto current_lanes = planner_data_->route_handler->getLaneletSequence(
     current_lane, current_pose, p.backward_path_length, p.forward_path_length);
+  const auto drivable_lanes = util::generateDrivableLanes(current_lanes);
 
   if (current_lanes.empty()) {
     return reference_path;
@@ -136,12 +137,12 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const
       lane_change_buffer);
   }
 
-  current_lanes = util::expandLanelets(
-    current_lanes, parameters_.drivable_area_left_bound_offset,
+  const auto expanded_lanes = util::expandLanelets(
+    drivable_lanes, parameters_.drivable_area_left_bound_offset,
     parameters_.drivable_area_right_bound_offset);
 
   reference_path.drivable_area = util::generateDrivableArea(
-    reference_path, current_lanes, p.drivable_area_resolution, p.vehicle_length, planner_data_);
+    reference_path, expanded_lanes, p.drivable_area_resolution, p.vehicle_length, planner_data_);
 
   return reference_path;
 }

planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp

@@ -38,7 +38,7 @@ boost::optional<PullOutPath> GeometricPullOut::plan(Pose start_pose, Pose goal_p
 
   // combine road lane and shoulder lane
   const auto road_lanes = util::getExtendedCurrentLanes(planner_data_);
-  const auto shoulder_lanes = getPullOutLanes(road_lanes, planner_data_);
+  const auto shoulder_lanes = getPullOutLanes(planner_data_);
   auto lanes = road_lanes;
   lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end());
 

planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp

@@ -123,7 +123,7 @@ bool PullOutModule::isExecutionRequested() const
 
   // Check if ego is not out of lanes
   const auto current_lanes = util::getExtendedCurrentLanes(planner_data_);
-  const auto pull_out_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_);
+  const auto pull_out_lanes = pull_out_utils::getPullOutLanes(planner_data_);
   auto lanes = current_lanes;
   lanes.insert(lanes.end(), pull_out_lanes.begin(), pull_out_lanes.end());
   if (LaneDepartureChecker::isOutOfLane(lanes, vehicle_footprint)) {
@@ -184,6 +184,7 @@ BehaviorModuleOutput PullOutModule::plan()
   } else {
     path = status_.backward_path;
   }
+
   const auto expanded_lanes = util::expandLanelets(
     status_.lanes, parameters_.drivable_area_left_bound_offset,
     parameters_.drivable_area_right_bound_offset);
@@ -276,17 +277,17 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval()
 
   BehaviorModuleOutput output;
   const auto current_lanes = util::getExtendedCurrentLanes(planner_data_);
-  const auto pull_out_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_);
-  auto lanes = current_lanes;
-  lanes.insert(lanes.end(), pull_out_lanes.begin(), pull_out_lanes.end());
+  const auto pull_out_lanes = pull_out_utils::getPullOutLanes(planner_data_);
+  const auto drivable_lanes =
+    util::generateDrivableLanesWithShoulderLanes(current_lanes, pull_out_lanes);
 
-  lanes = util::expandLanelets(
-    lanes, parameters_.drivable_area_left_bound_offset,
+  const auto expanded_lanes = util::expandLanelets(
+    drivable_lanes, parameters_.drivable_area_left_bound_offset,
     parameters_.drivable_area_right_bound_offset);
 
   auto candidate_path = status_.back_finished ? getCurrentPath() : status_.backward_path;
   candidate_path.drivable_area = util::generateDrivableArea(
-    candidate_path, lanes, planner_data_->parameters.drivable_area_resolution,
+    candidate_path, expanded_lanes, planner_data_->parameters.drivable_area_resolution,
     planner_data_->parameters.vehicle_length, planner_data_);
   auto stop_path = candidate_path;
   for (auto & p : stop_path.points) {
@@ -433,12 +434,11 @@ void PullOutModule::updatePullOutStatus()
   const auto & goal_pose = planner_data_->route_handler->getGoalPose();
 
   status_.current_lanes = util::getExtendedCurrentLanes(planner_data_);
-  status_.pull_out_lanes = pull_out_utils::getPullOutLanes(status_.current_lanes, planner_data_);
+  status_.pull_out_lanes = pull_out_utils::getPullOutLanes(planner_data_);
 
   // combine road and shoulder lanes
-  status_.lanes = status_.current_lanes;
-  status_.lanes.insert(
-    status_.lanes.end(), status_.pull_out_lanes.begin(), status_.pull_out_lanes.end());
+  status_.lanes =
+    util::generateDrivableLanesWithShoulderLanes(status_.current_lanes, status_.pull_out_lanes);
 
   // search pull out start candidates backward
   std::vector<Pose> start_pose_candidates;