modify drivable area expansion to avoid static objects

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

Author: mkquda

Diff for: planning/behavior_path_planner/autoware_behavior_path_planner/config/drivable_area_expansion.param.yaml

@@ -19,13 +19,15 @@
         extra_wheelbase: 0.0 # [m] extra length to add to the wheelbase
         extra_front_overhang: 0.5 # [m] extra length to add to the front overhang
         extra_width: 1.0 # [m] extra length to add to the width
-      dynamic_objects:
-        avoid: false # if true, the drivable area is not expanded in the predicted path of dynamic objects
+      object_exclusion:
+        exclude_static: false # if true, the drivable area is not expanded over static objects
+        exclude_dynamic: false # if true, the drivable area is not expanded in the predicted path of dynamic objects
+        stopped_object_velocity_threshold: 1.0 # [m/s] velocity threshold for static objects
         extra_footprint_offset:
-          front: 0.5 # [m] extra length to add to the front of the dynamic object footprint
-          rear: 0.5 # [m] extra length to add to the rear of the dynamic object footprint
-          left: 0.5 # [m] extra length to add to the left of the dynamic object footprint
-          right: 0.5 # [m] extra length to add to the rear of the dynamic object footprint
+          front: 0.5 # [m] extra length to add to the front of object footprint
+          rear: 0.5 # [m] extra length to add to the rear of object footprint
+          left: 0.5 # [m] extra length to add to the left of object footprint
+          right: 0.5 # [m] extra length to add to the rear of object footprint
       path_preprocessing:
         max_arc_length: 100.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit)
         resample_interval: 2.0 # [m] fixed interval between resampled path points (0.0 means path points are directly used)

Diff for: planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -804,9 +804,12 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam(
     update_param(
       parameters, DrivableAreaExpansionParameters::ENABLED_PARAM,
       planner_data_->drivable_area_expansion_parameters.enabled);
+    update_param(
+      parameters, DrivableAreaExpansionParameters::AVOID_STA_OBJECTS_PARAM,
+      planner_data_->drivable_area_expansion_parameters.object_exclusion.exclude_static);
     update_param(
       parameters, DrivableAreaExpansionParameters::AVOID_DYN_OBJECTS_PARAM,
-      planner_data_->drivable_area_expansion_parameters.avoid_dynamic_objects);
+      planner_data_->drivable_area_expansion_parameters.object_exclusion.exclude_dynamic);
     update_param(
       parameters, DrivableAreaExpansionParameters::AVOID_LINESTRING_TYPES_PARAM,
       planner_data_->drivable_area_expansion_parameters.avoid_linestring_types);
@@ -823,17 +826,20 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam(
       parameters, DrivableAreaExpansionParameters::EGO_EXTRA_WIDTH,
       planner_data_->drivable_area_expansion_parameters.extra_width);
     update_param(
-      parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_FRONT,
-      planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_front_offset);
+      parameters, DrivableAreaExpansionParameters::OBJECTS_EXTRA_OFFSET_FRONT,
+      planner_data_->drivable_area_expansion_parameters.object_exclusion.front_offset);
+    update_param(
+      parameters, DrivableAreaExpansionParameters::OBJECTS_EXTRA_OFFSET_REAR,
+      planner_data_->drivable_area_expansion_parameters.object_exclusion.rear_offset);
     update_param(
-      parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_REAR,
-      planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_rear_offset);
+      parameters, DrivableAreaExpansionParameters::OBJECTS_EXTRA_OFFSET_LEFT,
+      planner_data_->drivable_area_expansion_parameters.object_exclusion.left_offset);
     update_param(
-      parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_LEFT,
-      planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_left_offset);
+      parameters, DrivableAreaExpansionParameters::OBJECTS_EXTRA_OFFSET_RIGHT,
+      planner_data_->drivable_area_expansion_parameters.object_exclusion.right_offset);
     update_param(
-      parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_RIGHT,
-      planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_right_offset);
+      parameters, DrivableAreaExpansionParameters::STOPPED_OBJ_VEL_THRESH,
+      planner_data_->drivable_area_expansion_parameters.object_exclusion.stopped_obj_vel_th);
     update_param(
       parameters, DrivableAreaExpansionParameters::MAX_EXP_DIST_PARAM,
       planner_data_->drivable_area_expansion_parameters.max_expansion_distance);

Diff for: planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp

@@ -33,22 +33,25 @@ struct DrivableAreaExpansionParameters
   static constexpr auto EGO_EXTRA_FRONT_OVERHANG = "dynamic_expansion.ego.extra_front_overhang";
   static constexpr auto EGO_EXTRA_WHEELBASE = "dynamic_expansion.ego.extra_wheelbase";
   static constexpr auto EGO_EXTRA_WIDTH = "dynamic_expansion.ego.extra_width";
-  static constexpr auto DYN_OBJECTS_EXTRA_OFFSET_FRONT =
-    "dynamic_expansion.dynamic_objects.extra_footprint_offset.front";
-  static constexpr auto DYN_OBJECTS_EXTRA_OFFSET_REAR =
-    "dynamic_expansion.dynamic_objects.extra_footprint_offset.rear";
-  static constexpr auto DYN_OBJECTS_EXTRA_OFFSET_LEFT =
-    "dynamic_expansion.dynamic_objects.extra_footprint_offset.left";
-  static constexpr auto DYN_OBJECTS_EXTRA_OFFSET_RIGHT =
-    "dynamic_expansion.dynamic_objects.extra_footprint_offset.right";
+  static constexpr auto OBJECTS_EXTRA_OFFSET_FRONT =
+    "dynamic_expansion.object_exclusion.extra_footprint_offset.front";
+  static constexpr auto OBJECTS_EXTRA_OFFSET_REAR =
+    "dynamic_expansion.object_exclusion.extra_footprint_offset.rear";
+  static constexpr auto OBJECTS_EXTRA_OFFSET_LEFT =
+    "dynamic_expansion.object_exclusion.extra_footprint_offset.left";
+  static constexpr auto OBJECTS_EXTRA_OFFSET_RIGHT =
+    "dynamic_expansion.object_exclusion.extra_footprint_offset.right";
+  static constexpr auto STOPPED_OBJ_VEL_THRESH =
+    "dynamic_expansion.object_exclusion.stopped_object_velocity_threshold";
   static constexpr auto MAX_EXP_DIST_PARAM = "dynamic_expansion.max_expansion_distance";
   static constexpr auto RESAMPLE_INTERVAL_PARAM =
     "dynamic_expansion.path_preprocessing.resample_interval";
   static constexpr auto MAX_PATH_ARC_LENGTH_PARAM =
     "dynamic_expansion.path_preprocessing.max_arc_length";
   static constexpr auto MAX_REUSE_DEVIATION_PARAM =
     "dynamic_expansion.path_preprocessing.reuse_max_deviation";
-  static constexpr auto AVOID_DYN_OBJECTS_PARAM = "dynamic_expansion.dynamic_objects.avoid";
+  static constexpr auto AVOID_STA_OBJECTS_PARAM = "dynamic_expansion.object_exclusion.exclude_static";
+  static constexpr auto AVOID_DYN_OBJECTS_PARAM = "dynamic_expansion.object_exclusion.exclude_dynamic";
   static constexpr auto AVOID_LINESTRING_TYPES_PARAM = "dynamic_expansion.avoid_linestring.types";
   static constexpr auto AVOID_LINESTRING_DIST_PARAM = "dynamic_expansion.avoid_linestring.distance";
   static constexpr auto SMOOTHING_CURVATURE_WINDOW_PARAM =
@@ -72,18 +75,24 @@ struct DrivableAreaExpansionParameters
   double extra_width{};
   int curvature_average_window{};
   double max_bound_rate{};
-  double dynamic_objects_extra_left_offset{};
-  double dynamic_objects_extra_right_offset{};
-  double dynamic_objects_extra_rear_offset{};
-  double dynamic_objects_extra_front_offset{};
   double max_expansion_distance{};
   double max_path_arc_length{};
   double resample_interval{};
   double arc_length_range{};
   double max_reuse_deviation{};
   double min_bound_interval{};
-  bool avoid_dynamic_objects{};
   bool print_runtime{};
+
+  struct ObjectExclusion{
+    bool exclude_static{};
+    bool exclude_dynamic{};
+    double front_offset{};
+    double rear_offset{};
+    double left_offset{};
+    double right_offset{};
+    double stopped_obj_vel_th{};
+  } object_exclusion;
+
   std::vector<std::string> avoid_linestring_types{};
   autoware::vehicle_info_utils::VehicleInfo vehicle_info;
 
@@ -109,17 +118,15 @@ struct DrivableAreaExpansionParameters
     max_path_arc_length = node.declare_parameter<double>(MAX_PATH_ARC_LENGTH_PARAM);
     resample_interval = node.declare_parameter<double>(RESAMPLE_INTERVAL_PARAM);
     max_reuse_deviation = node.declare_parameter<double>(MAX_REUSE_DEVIATION_PARAM);
-    dynamic_objects_extra_front_offset =
-      node.declare_parameter<double>(DYN_OBJECTS_EXTRA_OFFSET_FRONT);
-    dynamic_objects_extra_rear_offset =
-      node.declare_parameter<double>(DYN_OBJECTS_EXTRA_OFFSET_REAR);
-    dynamic_objects_extra_left_offset =
-      node.declare_parameter<double>(DYN_OBJECTS_EXTRA_OFFSET_LEFT);
-    dynamic_objects_extra_right_offset =
-      node.declare_parameter<double>(DYN_OBJECTS_EXTRA_OFFSET_RIGHT);
+    object_exclusion.exclude_static = node.declare_parameter<bool>(AVOID_STA_OBJECTS_PARAM);
+    object_exclusion.exclude_dynamic = node.declare_parameter<bool>(AVOID_DYN_OBJECTS_PARAM);
+    object_exclusion.stopped_obj_vel_th = node.declare_parameter<double>(STOPPED_OBJ_VEL_THRESH);
+    object_exclusion.front_offset = node.declare_parameter<double>(OBJECTS_EXTRA_OFFSET_FRONT);
+    object_exclusion.rear_offset = node.declare_parameter<double>(OBJECTS_EXTRA_OFFSET_REAR);
+    object_exclusion.left_offset = node.declare_parameter<double>(OBJECTS_EXTRA_OFFSET_LEFT);
+    object_exclusion.right_offset = node.declare_parameter<double>(OBJECTS_EXTRA_OFFSET_RIGHT);
     avoid_linestring_types =
       node.declare_parameter<std::vector<std::string>>(AVOID_LINESTRING_TYPES_PARAM);
-    avoid_dynamic_objects = node.declare_parameter<bool>(AVOID_DYN_OBJECTS_PARAM);
     avoid_linestring_dist = node.declare_parameter<double>(AVOID_LINESTRING_DIST_PARAM);
     min_bound_interval = node.declare_parameter<double>(MIN_BOUND_INTERVAL);
     print_runtime = node.declare_parameter<bool>(PRINT_RUNTIME_PARAM);

Diff for: planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp

@@ -16,6 +16,8 @@
 
 #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp"
 
+#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
+
 #include <autoware_utils/geometry/boost_polygon_utils.hpp>
 #include <autoware_utils/geometry/geometry.hpp>
 
@@ -44,21 +46,39 @@ MultiPolygon2d create_object_footprints(
   const autoware_perception_msgs::msg::PredictedObjects & objects,
   const DrivableAreaExpansionParameters & params)
 {
+  using behavior_path_planner::utils::path_safety_checker::filter::velocity_filter;
+
   MultiPolygon2d footprints;
-  if (params.avoid_dynamic_objects) {
-    for (const auto & object : objects.objects) {
-      const auto front = object.shape.dimensions.x / 2 + params.dynamic_objects_extra_front_offset;
-      const auto rear = -object.shape.dimensions.x / 2 - params.dynamic_objects_extra_rear_offset;
-      const auto left = object.shape.dimensions.y / 2 + params.dynamic_objects_extra_left_offset;
-      const auto right = -object.shape.dimensions.y / 2 - params.dynamic_objects_extra_right_offset;
-      Polygon2d base_footprint;
-      base_footprint.outer() = {
-        Point2d{front, left}, Point2d{front, right}, Point2d{rear, right}, Point2d{rear, left},
-        Point2d{front, left}};
+  if (!params.object_exclusion.exclude_dynamic && !params.object_exclusion.exclude_static) {
+    return footprints;
+  }
+
+  auto get_base_footprint = [&](const auto & object) {
+    const auto front = object.shape.dimensions.x / 2 + params.object_exclusion.front_offset;
+    const auto rear = -object.shape.dimensions.x / 2 - params.object_exclusion.rear_offset;
+    const auto left = object.shape.dimensions.y / 2 + params.object_exclusion.left_offset;
+    const auto right = -object.shape.dimensions.y / 2 - params.object_exclusion.right_offset;
+    Polygon2d footprint;
+    footprint.outer() = {
+      Point2d{front, left}, Point2d{front, right}, Point2d{rear, right}, Point2d{rear, left},
+      Point2d{front, left}};
+    return footprint;
+  };
+
+  for (const auto & object : objects.objects) {
+    const auto base_footprint = get_base_footprint(object);
+    if (params.object_exclusion.exclude_dynamic) {
       for (const auto & path : object.kinematics.predicted_paths)
         for (const auto & pose : path.path)
           footprints.push_back(create_footprint(pose, base_footprint));
+      continue;
     }
+
+    if (params.object_exclusion.exclude_static && velocity_filter(
+        object.kinematics.initial_twist_with_covariance.twist, -std::numeric_limits<double>::epsilon(),
+        params.object_exclusion.stopped_obj_vel_th)) {
+      footprints.push_back(create_footprint(object.kinematics.initial_pose_with_covariance.pose, base_footprint));
+    } 
   }
   return footprints;
 }

Diff for: planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_drivable_area_expansion.cpp

@@ -215,7 +215,7 @@ TEST(DrivableAreaExpansionProjection, expand_drivable_area)
   }
   {  // parameters
     params.enabled = true;
-    params.avoid_dynamic_objects = false;
+    params.object_exclusion.exclude_dynamic = false;
     params.avoid_linestring_dist = 0.0;
     params.avoid_linestring_types = {};
     params.max_expansion_distance = 0.0;  // means no limit

Diff for: planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_footprints.cpp

@@ -104,18 +104,19 @@ TEST(FootprintTest, create_object_footprints)
   objects.objects.push_back(object);
 
   autoware::behavior_path_planner::drivable_area_expansion::DrivableAreaExpansionParameters params;
-  params.avoid_dynamic_objects = false;
-  params.dynamic_objects_extra_front_offset = 0.5;
-  params.dynamic_objects_extra_rear_offset = 0.5;
-  params.dynamic_objects_extra_left_offset = 0.5;
-  params.dynamic_objects_extra_right_offset = 0.5;
+  params.object_exclusion.exclude_static = false;
+  params.object_exclusion.exclude_dynamic = false;
+  params.object_exclusion.front_offset = 0.5;
+  params.object_exclusion.rear_offset = 0.5;
+  params.object_exclusion.left_offset = 0.5;
+  params.object_exclusion.right_offset = 0.5;
 
   // Condition: doesn't avoid dynamic objects
   auto footprints = create_object_footprints(objects, params);
   EXPECT_TRUE(footprints.empty());
 
   // Condition: single object and single point path
-  params.avoid_dynamic_objects = true;
+  params.object_exclusion.exclude_dynamic = true;
   footprints = create_object_footprints(objects, params);
 
   ASSERT_EQ(footprints.size(), 1);