feat(dynamic_avoidance): reintroduce path_generation_method (#5726)

* feat(dynamic_avoidance): reintroduce path_generation_method Signed-off-by: Takayuki Murooka <[email protected]> * fix Signed-off-by: Takayuki Murooka <[email protected]> * pre-commit Signed-off-by: Takayuki Murooka <[email protected]> --------- Signed-off-by: Takayuki Murooka <[email protected]>
autowarefoundation · Dec 5, 2023 · b9fccd7 · b9fccd7
1 parent 60b4030
commit b9fccd7
Show file tree

Hide file tree

Showing 4 changed files with 37 additions and 23 deletions.
diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml
@@ -47,6 +47,7 @@
           max_ego_path_lat_cover_ratio: 0.3  # [-] The object will be ignored if the ratio of the object covering the ego's path is higher than this value.
 
       drivable_area_generation:
+        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/...include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/...include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp
@@ -46,6 +46,11 @@ struct MinMaxValue
   double max_value{0.0};
 };
 
+enum class PolygonGenerationMethod {
+  EGO_PATH_BASE = 0,
+  OBJECT_PATH_BASE,
+};
+
 struct DynamicAvoidanceParameters
 {
   // common
@@ -84,6 +89,7 @@ struct DynamicAvoidanceParameters
   double max_oncoming_crossing_object_angle{0.0};
 
   // drivable area generation
+  PolygonGenerationMethod polygon_generation_method{};
   double min_obj_path_based_lon_polygon_margin{0.0};
   double lat_offset_from_obstacle{0.0};
   double max_lat_offset_to_avoid{0.0};
@@ -103,10 +109,6 @@ struct DynamicAvoidanceParameters
 class DynamicAvoidanceModule : public SceneModuleInterface
 {
 public:
-  enum class PolygonGenerationMethod {
-    EGO_PATH_BASE = 0,
-    OBJECT_PATH_BASE,
-  };
   struct DynamicAvoidanceObject
   {
     DynamicAvoidanceObject(
@@ -141,18 +143,15 @@ class DynamicAvoidanceModule : public SceneModuleInterface
     std::optional<MinMaxValue> lat_offset_to_avoid{std::nullopt};
     bool is_collision_left{false};
     bool should_be_avoided{false};
-    PolygonGenerationMethod polygon_generation_method{PolygonGenerationMethod::OBJECT_PATH_BASE};
 
     void update(
       const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid,
-      const bool arg_is_collision_left, const bool arg_should_be_avoided,
-      const PolygonGenerationMethod & arg_polygon_generation_method)
+      const bool arg_is_collision_left, const bool arg_should_be_avoided)
     {
       lon_offset_to_avoid = arg_lon_offset_to_avoid;
       lat_offset_to_avoid = arg_lat_offset_to_avoid;
       is_collision_left = arg_is_collision_left;
       should_be_avoided = arg_should_be_avoided;
-      polygon_generation_method = arg_polygon_generation_method;
     }
   };
 
@@ -246,12 +245,11 @@ class DynamicAvoidanceModule : public SceneModuleInterface
     void updateObject(
       const std::string & uuid, const MinMaxValue & lon_offset_to_avoid,
       const MinMaxValue & lat_offset_to_avoid, const bool is_collision_left,
-      const bool should_be_avoided, const PolygonGenerationMethod & polygon_generation_method)
+      const bool should_be_avoided)
     {
       if (object_map_.count(uuid) != 0) {
         object_map_.at(uuid).update(
-          lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided,
-          polygon_generation_method);
+          lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided);
       }
     }
 
@@ -311,8 +309,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
   void updateTargetObjects();
   bool willObjectCutIn(
     const std::vector<PathPointWithLaneId> & ego_path, const PredictedPath & predicted_path,
-    const double obj_tangent_vel, const LatLonOffset & lat_lon_offset,
-    PolygonGenerationMethod & polygon_generation_method) const;
+    const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const;
   DecisionWithReason willObjectCutOut(
     const double obj_tangent_vel, const double obj_normal_vel, const bool is_object_left,
     const std::optional<DynamicAvoidanceObject> & prev_object) const;

diff --git a/...ing/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/...ing/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
@@ -319,10 +319,10 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan()
   std::vector<DrivableAreaInfo::Obstacle> obstacles_for_drivable_area;
   for (const auto & object : target_objects_) {
     const auto obstacle_poly = [&]() {
-      if (object.polygon_generation_method == PolygonGenerationMethod::EGO_PATH_BASE) {
+      if (parameters_->polygon_generation_method == PolygonGenerationMethod::EGO_PATH_BASE) {
         return calcEgoPathBasedDynamicObstaclePolygon(object);
       }
-      if (object.polygon_generation_method == PolygonGenerationMethod::OBJECT_PATH_BASE) {
+      if (parameters_->polygon_generation_method == PolygonGenerationMethod::OBJECT_PATH_BASE) {
         return calcObjectPathBasedDynamicObstaclePolygon(object);
       }
       throw std::logic_error("The polygon_generation_method's string is invalid.");
@@ -492,7 +492,6 @@ void DynamicAvoidanceModule::updateTargetObjects()
 
   // 2. Precise filtering of target objects and check if they should be avoided
   for (const auto & object : target_objects_manager_.getValidObjects()) {
-    PolygonGenerationMethod polygon_generation_method{PolygonGenerationMethod::EGO_PATH_BASE};
     const auto obj_uuid = object.uuid;
     const auto prev_object = getObstacleFromUuid(prev_objects, obj_uuid);
     const auto obj_path = *std::max_element(
@@ -519,8 +518,8 @@ void DynamicAvoidanceModule::updateTargetObjects()
       getLateralLongitudinalOffset(input_path->points, object.pose, object.shape);
 
     // 2.c. check if object will not cut in
-    const bool will_object_cut_in = willObjectCutIn(
-      input_path->points, obj_path, object.vel, lat_lon_offset, polygon_generation_method);
+    const bool will_object_cut_in =
+      willObjectCutIn(input_path->points, obj_path, object.vel, lat_lon_offset);
     if (will_object_cut_in) {
       RCLCPP_INFO_EXPRESSION(
         getLogger(), parameters_->enable_debug_info,
@@ -607,8 +606,7 @@ void DynamicAvoidanceModule::updateTargetObjects()
 
     const bool should_be_avoided = true;
     target_objects_manager_.updateObject(
-      obj_uuid, lon_offset_to_avoid, *lat_offset_to_avoid, is_collision_left, should_be_avoided,
-      polygon_generation_method);
+      obj_uuid, lon_offset_to_avoid, *lat_offset_to_avoid, is_collision_left, should_be_avoided);
   }
 
   prev_input_ref_path_points = input_ref_path_points;
@@ -686,8 +684,7 @@ bool DynamicAvoidanceModule::isObjectFarFromPath(
 
 bool DynamicAvoidanceModule::willObjectCutIn(
   const std::vector<PathPointWithLaneId> & ego_path, const PredictedPath & predicted_path,
-  const double obj_tangent_vel, const LatLonOffset & lat_lon_offset,
-  PolygonGenerationMethod & polygon_generation_method) const
+  const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const
 {
   // Ignore oncoming object
   if (obj_tangent_vel < parameters_->min_cut_in_object_vel) {
@@ -721,7 +718,6 @@ bool DynamicAvoidanceModule::willObjectCutIn(
     lon_offset_ego_to_obj < std::max(
                               parameters_->min_lon_offset_ego_to_cut_in_object,
                               relative_velocity * parameters_->min_time_to_start_cut_in)) {
-    polygon_generation_method = PolygonGenerationMethod::EGO_PATH_BASE;
     return false;
   }
 

diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp
@@ -24,6 +24,18 @@
 
 namespace behavior_path_planner
 {
+namespace
+{
+PolygonGenerationMethod convertToPolygonGenerationMethod(const std::string & str)
+{
+  if (str == "ego_path_base") {
+    return PolygonGenerationMethod::EGO_PATH_BASE;
+  } else if (str == "object_path_base") {
+    return PolygonGenerationMethod::OBJECT_PATH_BASE;
+  }
+  throw std::logic_error("The polygon_generation_method's string is invalid.");
+}
+}  // namespace
 
 void DynamicAvoidanceModuleManager::init(rclcpp::Node * node)
 {
@@ -91,6 +103,8 @@ void DynamicAvoidanceModuleManager::init(rclcpp::Node * node)
 
   {  // drivable_area_generation
     const std::string ns = "dynamic_avoidance.drivable_area_generation.";
+    p.polygon_generation_method = convertToPolygonGenerationMethod(
+      node->declare_parameter<std::string>(ns + "polygon_generation_method"));
     p.min_obj_path_based_lon_polygon_margin =
       node->declare_parameter<double>(ns + "object_path_base.min_longitudinal_polygon_margin");
     p.lat_offset_from_obstacle = node->declare_parameter<double>(ns + "lat_offset_from_obstacle");
@@ -197,6 +211,12 @@ void DynamicAvoidanceModuleManager::updateModuleParams(
 
   {  // drivable_area_generation
     const std::string ns = "dynamic_avoidance.drivable_area_generation.";
+    std::string polygon_generation_method_str;
+    if (updateParam<std::string>(
+          parameters, ns + "polygon_generation_method", polygon_generation_method_str)) {
+      p->polygon_generation_method =
+        convertToPolygonGenerationMethod(polygon_generation_method_str);
+    }
     updateParam<double>(
       parameters, ns + "object_path_base.min_longitudinal_polygon_margin",
       p->min_obj_path_based_lon_polygon_margin);