add expansion drivable area feature

Signed-off-by: beyza <[email protected]>

planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/config/dynamic_obstacle_avoidance.param.yaml

@@ -3,6 +3,7 @@
     dynamic_avoidance:
       common:
         enable_debug_info: false
+        use_lane_type: "opposite_direction_lane"
         use_hatched_road_markings: true
 
       # avoidance is performed for the object type with true

planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp

@@ -95,6 +95,9 @@ struct DynamicAvoidanceParameters
   bool enable_debug_info{true};
   bool use_hatched_road_markings{true};
 
+  // drivable lane config
+  std::string use_lane_type{"opposite_direction_lane"};
+
   // obstacle types to avoid
   bool avoid_car{true};
   bool avoid_truck{true};
@@ -169,6 +172,11 @@ struct LatFeasiblePaths
 class DynamicObstacleAvoidanceModule : public SceneModuleInterface
 {
 public:
+  static constexpr const char * logger_namespace =
+    "planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.dynamic_"
+    "obstacle_"
+    "avoidance";
+
   struct DynamicAvoidanceObject
   {
     DynamicAvoidanceObject(
@@ -350,6 +358,7 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface
     parameters_ = std::any_cast<std::shared_ptr<DynamicAvoidanceParameters>>(parameters);
   }
 
+  bool arrived_path_end_{false};
   bool isExecutionRequested() const override;
   bool isExecutionReady() const override;
   // TODO(someone): remove this, and use base class function
@@ -438,6 +447,11 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface
   std::optional<autoware::universe_utils::Polygon2d> calcPredictedPathBasedDynamicObstaclePolygon(
     const DynamicAvoidanceObject & object, const EgoPathReservePoly & ego_path_poly) const;
   EgoPathReservePoly calcEgoPathReservePoly(const PathWithLaneId & ego_path) const;
+  lanelet::ConstLanelets getCurrentLanesFromPath(
+    const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data);
+  DrivableLanes generateExpandedDrivableLanes(
+    const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data,
+    const std::shared_ptr<DynamicAvoidanceParameters> & parameters);
 
   void printIgnoreReason(const std::string & obj_uuid, const std::string & reason)
   {

planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml

@@ -30,6 +30,11 @@
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>signal_processing</depend>
+  <depend>tf2</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tier4_planning_msgs</depend>
   <depend>visualization_msgs</depend>
 
   <test_depend>ament_cmake_ros</test_depend>

planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp

@@ -20,6 +20,7 @@
 #include "object_recognition_utils/predicted_path_utils.hpp"
 #include "signal_processing/lowpass_filter_1d.hpp"
 
+#include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
 
 #include <boost/geometry/algorithms/buffer.hpp>
@@ -99,6 +100,22 @@ void appendExtractedPolygonMarker(
   marker_array.markers.push_back(marker);
 }
 
+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());
+}
+
 template <typename T>
 std::optional<T> getObjectFromUuid(const std::vector<T> & objects, const std::string & target_uuid)
 {
@@ -402,7 +419,21 @@ void DynamicObstacleAvoidanceModule::updateData()
 
 bool DynamicObstacleAvoidanceModule::canTransitSuccessState()
 {
-  return target_objects_.empty();
+  const auto & data = getPreviousModuleOutput();
+  const auto idx = planner_data_->findEgoIndex(data.reference_path.points);
+  if (idx == data.reference_path.points.size() - 1) {
+    arrived_path_end_ = true;
+  }
+  constexpr double THRESHOLD = 2.0;
+  const auto is_further_than_threshold =
+    autoware::universe_utils::calcDistance2d(
+      getEgoPose(), autoware::universe_utils::getPose(data.reference_path.points.back())) >
+    THRESHOLD;
+  if (is_further_than_threshold && arrived_path_end_) {
+    RCLCPP_WARN(getLogger(), "Reach path end point. Exit.");
+    return true;
+  }
+  return planner_data_->dynamic_object->objects.empty();
 }
 
 BehaviorModuleOutput DynamicObstacleAvoidanceModule::plan()
@@ -416,36 +447,11 @@ BehaviorModuleOutput DynamicObstacleAvoidanceModule::plan()
 
   // create obstacles to avoid (= extract from the drivable area)
   std::vector<DrivableAreaInfo::Obstacle> obstacles_for_drivable_area;
-  for (const auto & object : target_objects_) {
-    const auto obstacle_poly = [&]() {
-      if (getObjectType(object.label) == ObjectType::UNREGULATED) {
-        return calcPredictedPathBasedDynamicObstaclePolygon(object, ego_path_reserve_poly);
-      }
-
-      if (parameters_->polygon_generation_method == PolygonGenerationMethod::EGO_PATH_BASE) {
-        return calcEgoPathBasedDynamicObstaclePolygon(object);
-      }
-      if (parameters_->polygon_generation_method == PolygonGenerationMethod::OBJECT_PATH_BASE) {
-        return calcObjectPathBasedDynamicObstaclePolygon(object);
-      }
-      throw std::logic_error("The polygon_generation_method's string is invalid.");
-    }();
-    if (obstacle_poly) {
-      obstacles_for_drivable_area.push_back(
-        {object.pose, obstacle_poly.value(), object.is_collision_left});
-
-      appendObjectMarker(info_marker_, object.pose);
-      appendExtractedPolygonMarker(debug_marker_, obstacle_poly.value(), object.pose.position.z);
-    }
-  }
 
+  // generate drivable lanes
+  auto current_lanelets =
+    getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_);
   DrivableAreaInfo current_drivable_area_info;
-  current_drivable_area_info.drivable_lanes =
-    getPreviousModuleOutput().drivable_area_info.drivable_lanes;
-  current_drivable_area_info.obstacles = obstacles_for_drivable_area;
-  current_drivable_area_info.enable_expanding_hatched_road_markings =
-    parameters_->use_hatched_road_markings;
-
   BehaviorModuleOutput output;
   output.path = input_path;
   output.drivable_area_info = utils::combineDrivableAreaInfo(
@@ -454,6 +460,47 @@ BehaviorModuleOutput DynamicObstacleAvoidanceModule::plan()
   output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
   output.modified_goal = getPreviousModuleOutput().modified_goal;
 
+  {
+    // generate drivable lanes
+    std::for_each(current_lanelets.begin(), current_lanelets.end(), [&](const auto & lanelet) {
+      current_drivable_area_info.drivable_lanes.push_back(
+        generateExpandedDrivableLanes(lanelet, planner_data_, parameters_));
+    });
+    // expand hatched road markings
+    current_drivable_area_info.enable_expanding_hatched_road_markings =
+      parameters_->use_hatched_road_markings;
+    current_drivable_area_info.obstacles.clear();
+
+    // generate obstacle polygons
+    for (const auto & object : target_objects_) {
+      const auto obstacle_poly = [&]() {
+        if (getObjectType(object.label) == ObjectType::UNREGULATED) {
+          return calcPredictedPathBasedDynamicObstaclePolygon(object, ego_path_reserve_poly);
+        }
+
+        if (parameters_->polygon_generation_method == PolygonGenerationMethod::EGO_PATH_BASE) {
+          return calcEgoPathBasedDynamicObstaclePolygon(object);
+        }
+        if (parameters_->polygon_generation_method == PolygonGenerationMethod::OBJECT_PATH_BASE) {
+          return calcObjectPathBasedDynamicObstaclePolygon(object);
+        }
+        throw std::logic_error("The polygon_generation_method's string is invalid.");
+      }();
+      if (obstacle_poly) {
+        obstacles_for_drivable_area.push_back(
+          {object.pose, obstacle_poly.value(), object.is_collision_left});
+
+        appendObjectMarker(info_marker_, object.pose);
+        appendExtractedPolygonMarker(debug_marker_, obstacle_poly.value(), object.pose.position.z);
+      }
+    }
+    current_drivable_area_info.obstacles = obstacles_for_drivable_area;
+
+    output.drivable_area_info = utils::combineDrivableAreaInfo(
+      current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);
+
+    setDrivableLanes(output.drivable_area_info.drivable_lanes);
+  }
   return output;
 }
 
@@ -1950,4 +1997,165 @@ DynamicObstacleAvoidanceModule::calcEgoPathReservePoly(const PathWithLaneId & eg
   return {left_avoid_poly, right_avoid_poly};
 }
 
+lanelet::ConstLanelets DynamicObstacleAvoidanceModule::getCurrentLanesFromPath(
+  const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data)
+{
+  if (path.points.empty()) {
+    throw std::logic_error("empty path.");
+  }
+
+  const auto idx = planner_data->findEgoIndex(path.points);
+
+  if (path.points.at(idx).lane_ids.empty()) {
+    throw std::logic_error("empty lane ids.");
+  }
+
+  const auto start_id = path.points.at(idx).lane_ids.front();
+  const auto start_lane = planner_data->route_handler->getLaneletsFromId(start_id);
+  const auto & p = planner_data->parameters;
+
+  return planner_data->route_handler->getLaneletSequence(
+    start_lane, p.backward_path_length, p.forward_path_length);
+}
+
+DrivableLanes DynamicObstacleAvoidanceModule::generateExpandedDrivableLanes(
+  const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data,
+  const std::shared_ptr<DynamicAvoidanceParameters> & 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_lane_type == "current_lane") {
+    return current_drivable_lanes;
+  }
+
+  const auto use_opposite_lane = parameters->use_lane_type == "opposite_direction_lane";
+
+  // 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, 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, 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_DEBUG(
+            rclcpp::get_logger(logger_namespace), "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 autoware::behavior_path_planner