feat(avoidance): keep stopping until all shift lines are registered (a…

…utowarefoundation#5658)

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

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -231,6 +231,10 @@
         stop_buffer: 1.0                                # [m]
 
       policy:
+        # policy for rtc request. select "per_shift_line" or "per_avoidance_maneuver".
+        # "per_shift_line": request approval for each shift line.
+        # "per_avoidance_maneuver": request approval for avoidance maneuver (avoid + return).
+        make_approval_request: "per_shift_line"
         # policy for vehicle slow down behavior. select "best_effort" or "reliable".
         # "best_effort": slow down deceleration & jerk are limited by constraints.
         #                but there is a possibility that the vehicle can't stop in front of the vehicle.

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp

@@ -285,6 +285,9 @@ struct AvoidanceParameters
   // policy
   bool use_shorten_margin_immediately{false};
 
+  // policy
+  std::string policy_approval{"per_shift_line"};
+
   // policy
   std::string policy_deceleration{"best_effort"};
 

planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp

@@ -292,6 +292,7 @@ AvoidanceModuleManager::AvoidanceModuleManager(
   // policy
   {
     std::string ns = "avoidance.policy.";
+    p.policy_approval = getOrDeclareParameter<std::string>(*node, ns + "make_approval_request");
     p.policy_deceleration = getOrDeclareParameter<std::string>(*node, ns + "deceleration");
     p.policy_lateral_margin = getOrDeclareParameter<std::string>(*node, ns + "lateral_margin");
     p.use_shorten_margin_immediately =

planning/behavior_path_planner/src/utils/avoidance/shift_line_generator.cpp

@@ -35,6 +35,11 @@ bool isBestEffort(const std::string & policy)
   return policy == "best_effort";
 }
 
+bool perManeuver(const std::string & policy)
+{
+  return policy == "per_avoidance_maneuver";
+}
+
 AvoidLine merge(const AvoidLine & line1, const AvoidLine & line2, const UUID id)
 {
   AvoidLine ret{};
@@ -1213,11 +1218,18 @@ AvoidLineArray ShiftLineGenerator::findNewShiftLine(
       break;
     }
 
-    if (!is_ignore_shift(candidate)) {
-      const auto new_shift_lines = get_subsequent_shift(i);
-      debug.step4_new_shift_line = new_shift_lines;
-      return new_shift_lines;
+    if (is_ignore_shift(candidate)) {
+      continue;
     }
+
+    if (perManeuver(parameters_->policy_approval)) {
+      debug.step4_new_shift_line = shift_lines;
+      return shift_lines;
+    }
+
+    const auto new_shift_lines = get_subsequent_shift(i);
+    debug.step4_new_shift_line = new_shift_lines;
+    return new_shift_lines;
   }
 
   return {};