feat(dynamic_avoidance): avoid oncoming vehicles

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
autowarefoundation · Apr 28, 2023 · 1ad48b0 · 1ad48b0
1 parent 24bce1e
commit 1ad48b0
Show file tree

Hide file tree

Showing 5 changed files with 34 additions and 26 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
@@ -15,6 +15,7 @@
         min_obstacle_vel: 1.0 # [m/s]
 
       drivable_area_generation:
-        lat_offset_from_obstacle: 0.8 # [m]
-        time_to_avoid: 5.0 # [s]
-        max_lat_offset_to_avoid: 1.0 # [m]
+        lat_offset_from_obstacle: 2.0 # [m]
+        time_to_avoid_same_directional_object: 5.0 # [s]
+        time_to_avoid_opposite_directional_object: 6.0 # [s]
+        max_lat_offset_to_avoid: 2.0 # [m]
diff --git a/...ng/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md b/...ng/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design.md
@@ -11,12 +11,13 @@ Then the motion planner, in detail obstacle_avoidance_planner, will generate an
 
 ### Parameters
 
-| Name                                              | Unit  | Type   | Description                             | Default value |
-| :------------------------------------------------ | :---- | :----- | :-------------------------------------- | :------------ |
-| target_object.car                                 | [-]   | bool   | The flag whether to avoid cars or not   | true          |
-| target_object.truck                               | [-]   | bool   | The flag whether to avoid trucks or not | true          |
-| ...                                               | [-]   | bool   | ...                                     | ...           |
-| target_object.min_obstacle_vel                    | [m/s] | double | Minimum obstacle velocity to avoid      | 1.0           |
-| drivable_area_generation.lat_offset_from_obstacle | [m]   | double | Lateral offset to avoid from obstacles  | 0.8           |
-| drivable_area_generation.time_to_avoid            | [s]   | double | Elapsed time for avoiding an obstacle   | 5.0           |
-| drivable_area_generation.max_lat_offset_to_avoid  | [m]   | double | Maximum lateral offset to avoid         | 0.5           |
+| Name                                                               | Unit  | Type   | Description                                                 | Default value |
+| :----------------------------------------------------------------- | :---- | :----- | :---------------------------------------------------------- | :------------ |
+| target_object.car                                                  | [-]   | bool   | The flag whether to avoid cars or not                       | true          |
+| target_object.truck                                                | [-]   | bool   | The flag whether to avoid trucks or not                     | true          |
+| ...                                                                | [-]   | bool   | ...                                                         | ...           |
+| target_object.min_obstacle_vel                                     | [m/s] | double | Minimum obstacle velocity to avoid                          | 1.0           |
+| drivable_area_generation.lat_offset_from_obstacle                  | [m]   | double | Lateral offset to avoid from obstacles                      | 0.8           |
+| drivable_area_generation.time_to_avoid_same_directional_object     | [s]   | double | Elapsed time for avoiding the same directional obstacle     | 5.0           |
+| drivable_area_generation.time_to_avoid_opposite_directional_object | [s]   | double | Elapsed time for avoiding the opposite directional obstacle | 6.0           |
+| drivable_area_generation.max_lat_offset_to_avoid                   | [m]   | double | Maximum lateral offset to avoid                             | 0.5           |
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
@@ -50,7 +50,8 @@ struct DynamicAvoidanceParameters
 
   // drivable area generation
   double lat_offset_from_obstacle{0.0};
-  double time_to_avoid{0.0};
+  double time_to_avoid_same_directional_object{0.0};
+  double time_to_avoid_opposite_directional_object{0.0};
   double max_lat_offset_to_avoid{0.0};
 };
 

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -676,7 +676,10 @@ DynamicAvoidanceParameters BehaviorPathPlannerNode::getDynamicAvoidanceParam()
   {  // drivable_area_generation
     std::string ns = "dynamic_avoidance.drivable_area_generation.";
     p.lat_offset_from_obstacle = declare_parameter<double>(ns + "lat_offset_from_obstacle");
-    p.time_to_avoid = declare_parameter<double>(ns + "time_to_avoid");
+    p.time_to_avoid_same_directional_object =
+      declare_parameter<double>(ns + "time_to_avoid_same_directional_object");
+    p.time_to_avoid_opposite_directional_object =
+      declare_parameter<double>(ns + "time_to_avoid_opposite_directional_object");
     p.max_lat_offset_to_avoid = declare_parameter<double>(ns + "max_lat_offset_to_avoid");
   }
 

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
@@ -307,26 +307,28 @@ std::optional<tier4_autoware_utils::Polygon2d> DynamicAvoidanceModule::calcDynam
   }();
 
   // calculate bound start and end index
-  const double length_to_avoid = object.path_projected_vel * parameters_->time_to_avoid;
-  [[maybe_unused]] const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint(
+  const double length_to_avoid = [&]() {
+    if (0.0 <= object.path_projected_vel) {
+      return object.path_projected_vel * parameters_->time_to_avoid_same_directional_object;
+    }
+    return object.path_projected_vel * parameters_->time_to_avoid_opposite_directional_object;
+  }();
+  const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint(
     obj_seg_idx, min_obj_lon_offset + (length_to_avoid < 0 ? length_to_avoid : 0.0),
     path_for_bound.points);
   const auto lon_bound_end_idx_opt = motion_utils::insertTargetPoint(
-    obj_seg_idx, max_obj_lon_offset + (0 < length_to_avoid ? length_to_avoid : 0.0),
+    obj_seg_idx, max_obj_lon_offset + (0 <= length_to_avoid ? length_to_avoid : 0.0),
     path_for_bound.points);
-  const size_t lon_bound_start_idx = obj_seg_idx;
+  if (!lon_bound_start_idx_opt && !lon_bound_end_idx_opt) {
+    // NOTE: The obstacle is longitudinally out of the ego's trajectory.
+    return std::nullopt;
+  }
+  const size_t lon_bound_start_idx =
+    lon_bound_start_idx_opt ? lon_bound_start_idx_opt.value() : static_cast<size_t>(0);
   const size_t lon_bound_end_idx = lon_bound_end_idx_opt
                                      ? lon_bound_end_idx_opt.value()
                                      : static_cast<size_t>(path_for_bound.points.size() - 1);
 
-  // TODO(murooka) insertTargetPoint does not work well with a negative offset for now.
-  //               When a offset is negative, the bound will be weird.
-  if (
-    lon_bound_start_idx == 0 &&
-    lon_bound_end_idx == static_cast<size_t>(path_for_bound.points.size() - 1)) {
-    return std::nullopt;
-  }
-
   // calculate bound min and max lateral offset
   const double min_bound_lat_offset = [&]() {
     const double raw_min_bound_lat_offset =