feat(dynamic_avoidance): ignore oncoming crossing object (autowarefou…

…ndation#4548) * feat(dynamic_avoidance): ignore oncoming crossing object Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * remove debug print Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update config Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
tier4 · Nov 25, 2023 · 791229b · 791229b
1 parent 4f88e1c
commit 791229b
Show file tree

Hide file tree

Showing 4 changed files with 89 additions and 12 deletions.
diff --git a/planning/behavior_path_planner/config/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance.param.yaml
@@ -0,0 +1,58 @@
+/**:
+  ros__parameters:
+    dynamic_avoidance:
+      common:
+        enable_debug_info: true
+
+      # avoidance is performed for the object type with true
+      target_object:
+        car: true
+        truck: true
+        bus: true
+        trailer: true
+        unknown: false
+        bicycle: false
+        motorcycle: true
+        pedestrian: false
+
+        min_obstacle_vel: 0.0 # [m/s]
+
+        successive_num_to_entry_dynamic_avoidance_condition: 5
+        successive_num_to_exit_dynamic_avoidance_condition: 1
+
+        min_obj_lat_offset_to_ego_path: 0.0 # [m]
+        max_obj_lat_offset_to_ego_path: 1.0 # [m]
+
+        cut_in_object:
+          min_time_to_start_cut_in: 1.0 # [s]
+          min_lon_offset_ego_to_object: 0.0 # [m]
+
+        cut_out_object:
+          max_time_from_outside_ego_path: 2.0 # [s]
+          min_object_lat_vel: 0.3 # [m/s]
+
+        crossing_object:
+          min_overtaking_object_vel: 1.0
+          max_overtaking_object_angle: 1.05
+          min_oncoming_object_vel: 0.0
+          max_oncoming_object_angle: 0.523
+
+        front_object:
+          max_object_angle: 0.785
+
+      drivable_area_generation:
+        lat_offset_from_obstacle: 0.8 # [m]
+        max_lat_offset_to_avoid: 0.5 # [m]
+
+        # for same directional object
+        overtaking_object:
+          max_time_to_collision: 40.0 # [s]
+          start_duration_to_avoid: 2.0  # [s]
+          end_duration_to_avoid: 4.0  # [s]
+          duration_to_hold_avoidance: 3.0 # [s]
+
+        # for opposite directional object
+        oncoming_object:
+          max_time_to_collision: 15.0 # [s]
+          start_duration_to_avoid: 12.0  # [s]
+          end_duration_to_avoid: 0.0  # [s]
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
@@ -67,8 +67,10 @@ struct DynamicAvoidanceParameters
   double max_time_from_outside_ego_path_for_cut_out{0.0};
   double min_cut_out_object_lat_vel{0.0};
   double max_front_object_angle{0.0};
-  double min_crossing_object_vel{0.0};
-  double max_crossing_object_angle{0.0};
+  double min_overtaking_crossing_object_vel{0.0};
+  double max_overtaking_crossing_object_angle{0.0};
+  double min_oncoming_crossing_object_vel{0.0};
+  double max_oncoming_crossing_object_angle{0.0};
 
   // drivable area generation
   double lat_offset_from_obstacle{0.0};

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
@@ -374,11 +374,16 @@ void DynamicAvoidanceModule::updateTargetObjects()
 
     // 1.c. check if object is not crossing ego's path
     const double obj_angle = calcDiffAngleAgainstPath(prev_module_path->points, obj_pose);
-    const bool is_obstacle_crossing_path =
-      parameters_->max_crossing_object_angle < std::abs(obj_angle) &&
-      parameters_->max_crossing_object_angle < M_PI - std::abs(obj_angle);
+    const double max_crossing_object_angle = 0.0 <= obj_tangent_vel
+                                               ? parameters_->max_overtaking_crossing_object_angle
+                                               : parameters_->max_oncoming_crossing_object_angle;
+    const bool is_obstacle_crossing_path = max_crossing_object_angle < std::abs(obj_angle) &&
+                                           max_crossing_object_angle < M_PI - std::abs(obj_angle);
+    const double min_crossing_object_vel = 0.0 <= obj_tangent_vel
+                                             ? parameters_->min_overtaking_crossing_object_vel
+                                             : parameters_->min_oncoming_crossing_object_vel;
     const bool is_crossing_object_to_ignore =
-      parameters_->min_crossing_object_vel < std::abs(obj_vel) && is_obstacle_crossing_path;
+      min_crossing_object_vel < std::abs(obj_vel) && is_obstacle_crossing_path;
     if (is_crossing_object_to_ignore) {
       RCLCPP_INFO_EXPRESSION(
         getLogger(), parameters_->enable_debug_info,

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
@@ -68,10 +68,14 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager(
     p.max_front_object_angle =
       node->declare_parameter<double>(ns + "front_object.max_object_angle");
 
-    p.min_crossing_object_vel =
-      node->declare_parameter<double>(ns + "crossing_object.min_object_vel");
-    p.max_crossing_object_angle =
-      node->declare_parameter<double>(ns + "crossing_object.max_object_angle");
+    p.min_overtaking_crossing_object_vel =
+      node->declare_parameter<double>(ns + "crossing_object.min_overtaking_object_vel");
+    p.max_overtaking_crossing_object_angle =
+      node->declare_parameter<double>(ns + "crossing_object.max_overtaking_object_angle");
+    p.min_oncoming_crossing_object_vel =
+      node->declare_parameter<double>(ns + "crossing_object.min_oncoming_object_vel");
+    p.max_oncoming_crossing_object_angle =
+      node->declare_parameter<double>(ns + "crossing_object.max_oncoming_object_angle");
   }
 
   {  // drivable_area_generation
@@ -152,9 +156,17 @@ void DynamicAvoidanceModuleManager::updateModuleParams(
       parameters, ns + "front_object.max_object_angle", p->max_front_object_angle);
 
     updateParam<double>(
-      parameters, ns + "crossing_object.min_object_vel", p->min_crossing_object_vel);
+      parameters, ns + "crossing_object.min_overtaking_object_vel",
+      p->min_overtaking_crossing_object_vel);
     updateParam<double>(
-      parameters, ns + "crossing_object.max_object_angle", p->max_crossing_object_angle);
+      parameters, ns + "crossing_object.max_overtaking_object_angle",
+      p->max_overtaking_crossing_object_angle);
+    updateParam<double>(
+      parameters, ns + "crossing_object.min_oncoming_object_vel",
+      p->min_oncoming_crossing_object_vel);
+    updateParam<double>(
+      parameters, ns + "crossing_object.max_oncoming_object_angle",
+      p->max_oncoming_crossing_object_angle);
   }
 
   {  // drivable_area_generation