feat(avoidance): use intersection area (#4206)

* feat(avoidance): use intersection areas

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(utils): use intersection areas

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* refactor(behavior_path_planner): clean up

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -20,6 +20,7 @@
       enable_yield_maneuver_during_shifting: false
       disable_path_update: false
       use_hatched_road_markings: false
+      use_intersection_areas: false
 
       # for debug
       publish_debug_marker: false

planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp

@@ -92,6 +92,7 @@ struct DrivableAreaInfo
   std::vector<DrivableLanes> drivable_lanes{};
   std::vector<Obstacle> obstacles{};  // obstacles to extract from the drivable area
   bool enable_expanding_hatched_road_markings{false};
+  bool enable_expanding_intersection_areas{false};
 
   // temporary only for pull over's freespace planning
   double drivable_margin{0.0};

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp

@@ -108,6 +108,9 @@ struct AvoidanceParameters
   // use hatched road markings for avoidance
   bool use_hatched_road_markings{false};
 
+  // use intersection area for avoidance
+  bool use_intersection_areas{false};
+
   // constrains
   bool use_constraints_for_decel{false};
 

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp

@@ -124,8 +124,9 @@ void filterTargetObjects(
 double extendToRoadShoulderDistanceWithPolygon(
   const std::shared_ptr<route_handler::RouteHandler> & rh,
   const lanelet::ConstLineString3d & target_line, const double to_road_shoulder_distance,
-  const geometry_msgs::msg::Point & overhang_pos,
-  const lanelet::BasicPoint3d & overhang_basic_pose);
+  const lanelet::ConstLanelet & overhang_lanelet, const geometry_msgs::msg::Point & overhang_pos,
+  const lanelet::BasicPoint3d & overhang_basic_pose, const bool use_hatched_road_markings,
+  const bool use_intersection_areas);
 
 void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineArray & lines);
 

planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp

@@ -226,7 +226,8 @@ std::vector<DrivableLanes> generateDrivableLanesWithShoulderLanes(
   const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & shoulder_lanes);
 std::vector<geometry_msgs::msg::Point> calcBound(
   const std::shared_ptr<RouteHandler> route_handler,
-  const std::vector<DrivableLanes> & drivable_lanes, const bool enable_expanding_polygon,
+  const std::vector<DrivableLanes> & drivable_lanes,
+  const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas,
   const bool is_left);
 
 boost::optional<size_t> getOverlappedLaneletId(const std::vector<DrivableLanes> & lanes);
@@ -235,8 +236,9 @@ std::vector<DrivableLanes> cutOverlappedLanes(
 
 void generateDrivableArea(
   PathWithLaneId & path, const std::vector<DrivableLanes> & lanes,
-  const bool enable_expanding_polygon, const double vehicle_length,
-  const std::shared_ptr<const PlannerData> planner_data, const bool is_driving_forward = true);
+  const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas,
+  const double vehicle_length, const std::shared_ptr<const PlannerData> planner_data,
+  const bool is_driving_forward = true);
 
 void generateDrivableArea(
   PathWithLaneId & path, const double vehicle_length, const double offset,

planning/behavior_path_planner/src/planner_manager.cpp

@@ -154,7 +154,7 @@ void PlannerManager::generateCombinedDrivableArea(
   } else if (di.is_already_expanded) {
     // for single side shift
     utils::generateDrivableArea(
-      *output.path, di.drivable_lanes, false, data->parameters.vehicle_length, data);
+      *output.path, di.drivable_lanes, false, false, data->parameters.vehicle_length, data);
   } else {
     const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, di.drivable_lanes);
 
@@ -166,7 +166,7 @@ void PlannerManager::generateCombinedDrivableArea(
     // for other modules where multiple modules may be launched
     utils::generateDrivableArea(
       *output.path, expanded_lanes, di.enable_expanding_hatched_road_markings,
-      data->parameters.vehicle_length, data);
+      di.enable_expanding_intersection_areas, data->parameters.vehicle_length, data);
   }
 
   // extract obstacles from drivable area

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

@@ -2365,6 +2365,9 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output
     // expand hatched road markings
     current_drivable_area_info.enable_expanding_hatched_road_markings =
       parameters_->use_hatched_road_markings;
+    // expand intersection areas
+    current_drivable_area_info.enable_expanding_intersection_areas =
+      parameters_->use_intersection_areas;
 
     output.drivable_area_info = utils::combineDrivableAreaInfo(
       current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);

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

@@ -69,6 +69,7 @@ AvoidanceModuleManager::AvoidanceModuleManager(
       get_parameter<bool>(node, ns + "enable_yield_maneuver_during_shifting");
     p.disable_path_update = get_parameter<bool>(node, ns + "disable_path_update");
     p.use_hatched_road_markings = get_parameter<bool>(node, ns + "use_hatched_road_markings");
+    p.use_intersection_areas = get_parameter<bool>(node, ns + "use_intersection_areas");
     p.publish_debug_marker = get_parameter<bool>(node, ns + "publish_debug_marker");
     p.print_debug_info = get_parameter<bool>(node, ns + "print_debug_info");
   }

planning/behavior_path_planner/src/utils/avoidance/utils.cpp

@@ -908,11 +908,11 @@ void filterTargetObjects(
       }
       debug.bounds.push_back(target_line);
 
-      // update to_road_shoulder_distance with expandable polygons
-      if (parameters->use_hatched_road_markings) {
+      {
         o.to_road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon(
-          rh, target_line, o.to_road_shoulder_distance, o.overhang_pose.position,
-          overhang_basic_pose);
+          rh, target_line, o.to_road_shoulder_distance, overhang_lanelet, o.overhang_pose.position,
+          overhang_basic_pose, parameters->use_hatched_road_markings,
+          parameters->use_intersection_areas);
       }
     }
 
@@ -1137,29 +1137,43 @@ void filterTargetObjects(
 double extendToRoadShoulderDistanceWithPolygon(
   const std::shared_ptr<route_handler::RouteHandler> & rh,
   const lanelet::ConstLineString3d & target_line, const double to_road_shoulder_distance,
-  const geometry_msgs::msg::Point & overhang_pos, const lanelet::BasicPoint3d & overhang_basic_pose)
+  const lanelet::ConstLanelet & overhang_lanelet, const geometry_msgs::msg::Point & overhang_pos,
+  const lanelet::BasicPoint3d & overhang_basic_pose, const bool use_hatched_road_markings,
+  const bool use_intersection_areas)
 {
   // get expandable polygons for avoidance (e.g. hatched road markings)
   std::vector<lanelet::Polygon3d> expandable_polygons;
-  for (const auto & point : target_line) {
-    const auto new_polygon_candidate = utils::getPolygonByPoint(rh, point, "hatched_road_markings");
-    if (!new_polygon_candidate) {
-      continue;
-    }
 
-    bool is_new_polygon{true};
-    for (const auto & polygon : expandable_polygons) {
-      if (polygon.id() == new_polygon_candidate->id()) {
-        is_new_polygon = false;
-        break;
+  const auto exist_polygon = [&](const auto & candidate_polygon) {
+    return std::any_of(
+      expandable_polygons.begin(), expandable_polygons.end(),
+      [&](const auto & polygon) { return polygon.id() == candidate_polygon.id(); });
+  };
+
+  if (use_hatched_road_markings) {
+    for (const auto & point : target_line) {
+      const auto new_polygon_candidate =
+        utils::getPolygonByPoint(rh, point, "hatched_road_markings");
+
+      if (!!new_polygon_candidate && !exist_polygon(*new_polygon_candidate)) {
+        expandable_polygons.push_back(*new_polygon_candidate);
       }
     }
+  }
 
-    if (is_new_polygon) {
-      expandable_polygons.push_back(*new_polygon_candidate);
+  if (use_intersection_areas) {
+    const std::string area_id_str = overhang_lanelet.attributeOr("intersection_area", "else");
+
+    if (area_id_str != "else") {
+      expandable_polygons.push_back(
+        rh->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id_str.c_str())));
     }
   }
 
+  if (expandable_polygons.empty()) {
+    return to_road_shoulder_distance;
+  }
+
   // calculate point laterally offset from overhang position to calculate intersection with
   // polygon
   Point lat_offset_overhang_pos;
@@ -1170,7 +1184,7 @@ double extendToRoadShoulderDistanceWithPolygon(
     const auto closest_target_line_point =
       lanelet::geometry::fromArcCoordinates(target_line, arc_coordinates);
 
-    const double ratio = 10.0 / to_road_shoulder_distance;
+    const double ratio = 100.0 / to_road_shoulder_distance;
     lat_offset_overhang_pos.x =
       closest_target_line_point.x() + (closest_target_line_point.x() - overhang_pos.x) * ratio;
     lat_offset_overhang_pos.y =
@@ -1197,12 +1211,8 @@ double extendToRoadShoulderDistanceWithPolygon(
     }
 
     std::sort(intersect_dist_vec.begin(), intersect_dist_vec.end());
-    if (1 < intersect_dist_vec.size()) {
-      if (std::abs(updated_to_road_shoulder_distance - intersect_dist_vec.at(0)) < 1e-3) {
-        updated_to_road_shoulder_distance =
-          std::max(updated_to_road_shoulder_distance, intersect_dist_vec.at(1));
-      }
-    }
+    updated_to_road_shoulder_distance =
+      std::max(updated_to_road_shoulder_distance, intersect_dist_vec.back());
   }
   return updated_to_road_shoulder_distance;
 }