feat(autoware_behavior_path_dynamic_obstacle_avoidance): expand driva…

…ble area (autowarefoundation#8295) * add expansion drivable area feature Signed-off-by: beyza <[email protected]> * make expansion optional Signed-off-by: beyza <[email protected]> * style(pre-commit): autofix * clean the code Signed-off-by: beyza <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: beyza <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Signed-off-by: emuemuJP <[email protected]>
arayabrain · Sep 10, 2024 · 829dcbf · 829dcbf
1 parent 35d7043
commit 829dcbf
Show file tree

Hide file tree

Showing 5 changed files with 210 additions and 4 deletions.
diff --git a/...avior_path_dynamic_obstacle_avoidance_module/config/dynamic_obstacle_avoidance.param.yaml b/...avior_path_dynamic_obstacle_avoidance_module/config/dynamic_obstacle_avoidance.param.yaml
@@ -51,6 +51,7 @@
           max_object_vel: 0.5 # [m/s] The object will be determined as stopped if the velocity is smaller than this value.
 
       drivable_area_generation:
+        expand_drivable_area: false
         polygon_generation_method: "ego_path_base" # choose "ego_path_base" and "object_path_base"
         object_path_base:
           min_longitudinal_polygon_margin: 3.0 # [m]

diff --git a/...oidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp b/...oidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp
@@ -95,6 +95,8 @@ struct DynamicAvoidanceParameters
   bool enable_debug_info{true};
   bool use_hatched_road_markings{true};
 
+  std::string use_lane_type{"opposite_direction_lane"};
+
   // obstacle types to avoid
   bool avoid_car{true};
   bool avoid_truck{true};
@@ -130,6 +132,7 @@ struct DynamicAvoidanceParameters
 
   // drivable area generation
   PolygonGenerationMethod polygon_generation_method{};
+  bool expand_drivable_area;
   double min_obj_path_based_lon_polygon_margin{0.0};
   double lat_offset_from_obstacle{0.0};
   double margin_distance_around_pedestrian{0.0};
@@ -169,6 +172,10 @@ 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(
@@ -439,6 +446,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)
   {

diff --git a/...ehavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml b/...ehavior_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>

diff --git a/...ior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp b/...ior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp
@@ -108,6 +108,7 @@ void DynamicObstacleAvoidanceModuleManager::init(rclcpp::Node * node)
 
   {  // drivable_area_generation
     const std::string ns = "dynamic_avoidance.drivable_area_generation.";
+    p.expand_drivable_area = node->declare_parameter<bool>(ns + "expand_drivable_area");
     p.polygon_generation_method = convertToPolygonGenerationMethod(
       node->declare_parameter<std::string>(ns + "polygon_generation_method"));
     p.min_obj_path_based_lon_polygon_margin =
@@ -238,6 +239,7 @@ void DynamicObstacleAvoidanceModuleManager::updateModuleParams(
       p->polygon_generation_method =
         convertToPolygonGenerationMethod(polygon_generation_method_str);
     }
+    updateParam<bool>(parameters, ns + "expand_drivable_area", p->expand_drivable_area);
     updateParam<double>(
       parameters, ns + "object_path_base.min_longitudinal_polygon_margin",
       p->min_obj_path_based_lon_polygon_margin);

diff --git a/...avior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp b/...avior_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)
 {
@@ -384,7 +401,7 @@ void DynamicObstacleAvoidanceModule::updateData()
 
 bool DynamicObstacleAvoidanceModule::canTransitSuccessState()
 {
-  return target_objects_.empty();
+  return planner_data_->dynamic_object->objects.empty();
 }
 
 BehaviorModuleOutput DynamicObstacleAvoidanceModule::plan()
@@ -420,10 +437,19 @@ BehaviorModuleOutput DynamicObstacleAvoidanceModule::plan()
       appendExtractedPolygonMarker(debug_marker_, obstacle_poly.value(), object.pose.position.z);
     }
   }
-
+  // generate drivable lanes
   DrivableAreaInfo current_drivable_area_info;
-  current_drivable_area_info.drivable_lanes =
-    getPreviousModuleOutput().drivable_area_info.drivable_lanes;
+  if (parameters_->expand_drivable_area) {
+    auto current_lanelets =
+      getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_);
+    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_));
+    });
+  } else {
+    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;
@@ -1932,4 +1958,164 @@ 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