From f4f47b887c74603ad35924c270b6ee596b3379cd Mon Sep 17 00:00:00 2001
From: satoshi-ota <satoshi.ota928@gmail.com>
Date: Thu, 23 Nov 2023 15:03:51 +0900
Subject: [PATCH] feat(avoidance): keep stopping until all shift lines are
 registered

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
 .../config/avoidance/avoidance.param.yaml     |  4 ++++
 .../utils/avoidance/avoidance_module_data.hpp |  3 +++
 .../src/scene_module/avoidance/manager.cpp    |  1 +
 .../utils/avoidance/shift_line_generator.cpp  | 20 +++++++++++++++----
 4 files changed, 24 insertions(+), 4 deletions(-)

diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
index 27f6a0df63cec..c68c10f2a9489 100644
--- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
+++ b/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.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
index 03747d8dacc94..68c64ff9b7112 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
+++ b/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"};
 
diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp
index d82a105930b99..9b13a9d4d3c51 100644
--- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp
+++ b/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 =
diff --git a/planning/behavior_path_planner/src/utils/avoidance/shift_line_generator.cpp b/planning/behavior_path_planner/src/utils/avoidance/shift_line_generator.cpp
index 7015bb802b927..c7fbaa931737c 100644
--- a/planning/behavior_path_planner/src/utils/avoidance/shift_line_generator.cpp
+++ b/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 {};