feat: keep stop policy v0.11.2 (reopen) (#1246)

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

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* revert refactoring due to cherry-pick

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
Co-authored-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: saka1-s

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

@@ -192,6 +192,10 @@
         stop_buffer: 1.0                                # [m]
 
       policy:
+        # policy for rtc request. select "shift_line" or "maneuver".
+        # "shift_line": request approval for each shift line.
+        # "maneuver": request approval for avoidance maneuver (avoid + return).
+        approval: "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

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

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

@@ -85,6 +85,11 @@ bool isBestEffort(const std::string & policy)
 {
   return policy == "best_effort";
 }
+
+bool perManeuver(const std::string & policy)
+{
+  return policy == "maneuver";
+}
 }  // namespace
 
 AvoidanceModule::AvoidanceModule(
@@ -2316,9 +2321,16 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat
       break;
     }
 
-    if (!is_ignore_shift(candidate)) {
-      return get_subsequent_shift(i);
+    if (is_ignore_shift(candidate)) {
+      continue;
     }
+
+    if (perManeuver(parameters_->policy_approval)) {
+      return candidates;
+    }
+
+    const auto new_shift_lines = get_subsequent_shift(i);
+    return new_shift_lines;
   }
 
   DEBUG_PRINT("No new shift point exists.");

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

@@ -218,6 +218,7 @@ AvoidanceModuleManager::AvoidanceModuleManager(
   // policy
   {
     std::string ns = "avoidance.policy.";
+    p.policy_approval = getOrDeclareParameter<std::string>(*node, ns + "approval");
     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 =