feat(static_obstacle_avoidance): keep object clipping even after the …

…object becomes non-target (#7591)

Signed-off-by: satoshi-ota <[email protected]>

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml

@@ -5,10 +5,6 @@
       resample_interval_for_planning: 0.3               # [m] FOR DEVELOPER
       resample_interval_for_output: 4.0                 # [m] FOR DEVELOPER
 
-      # avoidance module common setting
-      enable_bound_clipping: false
-      disable_path_update: false
-
       # drivable area setting
       use_adjacent_lane: true
       use_opposite_lane: true
@@ -284,6 +280,19 @@
           max_acceleration: 0.5                         # [m/ss]
           min_velocity_to_limit_max_acceleration: 2.78  # [m/ss]
 
+      # path generation method. select "shift_line_base" or "optimization_base" or "both".
+      # "shift_line_base"  : Create avoidance path based on shift line.
+      #                      User can control avoidance maneuver execution via RTC.
+      #                      However, this method doesn't support complex avoidance scenario (e.g. S-shape maneuver).
+      # "optimization_base": This module selects avoidance target object
+      #                      and bpp module clips drivable area based on avoidance target object polygon shape.
+      #                      But this module doesn't modify the path shape.
+      #                      On the other hand, autoware_path_optimizer module optimizes path shape instead of this module
+      #                      so that the path can be within drivable area. This method is able to deal with complex avoidance scenario.
+      #                      However, user can't control avoidance manuever execution.
+      # "both"             : Use both method.
+      path_generation_method: "shift_line_base"
+
       shift_line_pipeline:
         trim:
           quantize_size: 0.1

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp

@@ -120,9 +120,6 @@ struct AvoidanceParameters
   // enable yield maneuver.
   bool enable_yield_maneuver_during_shifting{false};
 
-  // disable path update
-  bool disable_path_update{false};
-
   // use hatched road markings for avoidance
   bool use_hatched_road_markings{false};
 
@@ -313,6 +310,9 @@ struct AvoidanceParameters
   // policy
   std::string policy_lateral_margin{"best_effort"};
 
+  // path generation method.
+  std::string path_generation_method{"shift_line_base"};
+
   // target velocity matrix
   std::vector<double> velocity_map;
 
@@ -334,9 +334,6 @@ struct AvoidanceParameters
   // rss parameters
   utils::path_safety_checker::RSSparams rss_params{};
 
-  // clip left and right bounds for objects
-  bool enable_bound_clipping{false};
-
   // debug
   bool enable_other_objects_marker{false};
   bool enable_other_objects_info{false};
@@ -438,6 +435,9 @@ struct ObjectData  // avoidance target
   // is ambiguous stopped vehicle.
   bool is_ambiguous{false};
 
+  // is clip targe.
+  bool is_clip_target{false};
+
   // object direction.
   Direction direction{Direction::NONE};
 

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp

@@ -39,8 +39,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
       getOrDeclareParameter<double>(*node, ns + "resample_interval_for_planning");
     p.resample_interval_for_output =
       getOrDeclareParameter<double>(*node, ns + "resample_interval_for_output");
-    p.enable_bound_clipping = getOrDeclareParameter<bool>(*node, ns + "enable_bound_clipping");
-    p.disable_path_update = getOrDeclareParameter<bool>(*node, ns + "disable_path_update");
+    p.path_generation_method =
+      getOrDeclareParameter<std::string>(*node, ns + "path_generation_method");
   }
 
   // drivable area

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp

@@ -436,6 +436,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
 
   UUID candidate_uuid_;
 
+  ObjectDataArray clip_objects_;
+
   // TODO(Satoshi OTA) create detected object manager.
   ObjectDataArray registered_objects_;
 

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp

@@ -121,6 +121,8 @@ void updateRegisteredObject(
   ObjectDataArray & registered_objects, const ObjectDataArray & now_objects,
   const std::shared_ptr<AvoidanceParameters> & parameters);
 
+void updateClipObject(ObjectDataArray & clip_objects, AvoidancePlanningData & data);
+
 void compensateDetectionLost(
   const ObjectDataArray & registered_objects, ObjectDataArray & now_objects,
   ObjectDataArray & other_objects);

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json

@@ -16,15 +16,10 @@
           "description": "Path resample interval for output path. Too short interval increases computational cost for latter modules.",
           "default": "4.0"
         },
-        "enable_bound_clipping": {
-          "type": "boolean",
-          "description": "Enable clipping left and right bound of drivable area when obstacles are in the drivable area.",
-          "default": "false"
-        },
-        "disable_path_update": {
-          "type": "boolean",
-          "description": "Disable path update.",
-          "default": "false"
+        "path_generation_method": {
+          "enum": ["shift_line_base", "optimization_base", "both"],
+          "description": "Path generation method.",
+          "default": "shift_line_base"
         },
         "use_adjacent_lane": {
           "type": "boolean",
@@ -1445,7 +1440,7 @@
       "required": [
         "resample_interval_for_planning",
         "resample_interval_for_output",
-        "enable_bound_clipping",
+        "path_generation_method",
         "use_adjacent_lane",
         "use_opposite_lane",
         "use_hatched_road_markings",

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp

@@ -147,6 +147,7 @@ MarkerArray createObjectInfoMarkerArray(
       string_stream << std::fixed << std::setprecision(2) << std::boolalpha;
       string_stream << "ratio:" << object.shiftable_ratio << " [-]\n"
                     << "lateral:" << object.to_centerline << " [m]\n"
+                    << "clip:" << object.is_clip_target << " [-]\n"
                     << "necessity:" << object.avoid_required << " [-]\n"
                     << "stoppable:" << object.is_stoppable << " [-]\n"
                     << "stop_factor:" << object.to_stop_factor_distance << " [m]\n"

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp

@@ -294,6 +294,7 @@ void StaticObstacleAvoidanceModule::fillFundamentalData(
     registered_objects_, data.target_objects, parameters_);
   utils::static_obstacle_avoidance::compensateDetectionLost(
     registered_objects_, data.target_objects, data.other_objects);
+  utils::static_obstacle_avoidance::updateClipObject(clip_objects_, data);
 
   // sort object order by longitudinal distance
   std::sort(data.target_objects.begin(), data.target_objects.end(), [](auto a, auto b) {
@@ -653,7 +654,7 @@ void StaticObstacleAvoidanceModule::fillDebugData(
 void StaticObstacleAvoidanceModule::updateEgoBehavior(
   const AvoidancePlanningData & data, ShiftedPath & path)
 {
-  if (parameters_->disable_path_update) {
+  if (parameters_->path_generation_method == "optimization_base") {
     return;
   }
 
@@ -998,19 +999,14 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan()
     // expand freespace areas
     current_drivable_area_info.enable_expanding_freespace_areas = parameters_->use_freespace_areas;
     // generate obstacle polygons
-    if (parameters_->enable_bound_clipping) {
-      ObjectDataArray clip_objects;
-      // If avoidance is executed by both behavior and motion, only non-avoidable object will be
-      // extracted from the drivable area.
-      std::for_each(
-        data.target_objects.begin(), data.target_objects.end(), [&](const auto & object) {
-          if (!object.is_avoidable) clip_objects.push_back(object);
-        });
+    current_drivable_area_info.obstacles.clear();
+
+    if (
+      parameters_->path_generation_method == "optimization_base" ||
+      parameters_->path_generation_method == "both") {
       current_drivable_area_info.obstacles =
         utils::static_obstacle_avoidance::generateObstaclePolygonsForDrivableArea(
-          clip_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0);
-    } else {
-      current_drivable_area_info.obstacles.clear();
+          clip_objects_, parameters_, planner_data_->parameters.vehicle_width / 2.0);
     }
 
     output.drivable_area_info = utils::combineDrivableAreaInfo(
@@ -1077,7 +1073,7 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::planWaitingApproval()
 
 void StaticObstacleAvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines)
 {
-  if (parameters_->disable_path_update) {
+  if (parameters_->path_generation_method == "optimization_base") {
     return;
   }
 

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

@@ -1690,6 +1690,59 @@ void compensateDetectionLost(
   }
 }
 
+void updateClipObject(ObjectDataArray & clip_objects, AvoidancePlanningData & data)
+{
+  std::for_each(data.target_objects.begin(), data.target_objects.end(), [](auto & o) {
+    if (o.is_avoidable) {
+      o.is_clip_target = true;
+    }
+  });
+
+  const auto itr =
+    std::remove_if(clip_objects.begin(), clip_objects.end(), [&data](const auto & clip_object) {
+      const auto id = clip_object.object.object_id;
+
+      // update target objects
+      {
+        const auto same_id_obj = std::find_if(
+          data.target_objects.begin(), data.target_objects.end(),
+          [&id](const auto & o) { return o.object.object_id == id; });
+        if (same_id_obj != data.target_objects.end()) {
+          same_id_obj->is_clip_target = true;
+          return false;
+        }
+      }
+
+      // update other objects
+      {
+        const auto same_id_obj = std::find_if(
+          data.other_objects.begin(), data.other_objects.end(),
+          [&id](const auto & o) { return o.object.object_id == id; });
+        if (same_id_obj != data.other_objects.end()) {
+          same_id_obj->is_clip_target = true;
+          return false;
+        }
+      }
+
+      return true;
+    });
+
+  clip_objects.erase(itr, clip_objects.end());
+
+  for (const auto & object : data.target_objects) {
+    const auto id = object.object.object_id;
+    const auto same_id_obj = std::find_if(
+      clip_objects.begin(), clip_objects.end(),
+      [&id](const auto & o) { return o.object.object_id == id; });
+
+    if (same_id_obj != clip_objects.end()) {
+      continue;
+    }
+
+    clip_objects.push_back(object);
+  }
+}
+
 void updateRoadShoulderDistance(
   AvoidancePlanningData & data, const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters)