From 24ddbc59463325625c3867ebefd977f4b3221029 Mon Sep 17 00:00:00 2001
From: satoshi-ota <satoshi.ota928@gmail.com>
Date: Tue, 23 May 2023 18:12:35 +0900
Subject: [PATCH] fix(avoidance): reset rtc status in yield status

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
 .../avoidance/avoidance_module.hpp            |  1 +
 .../avoidance/avoidance_module.cpp            | 31 ++++++++++++++-----
 2 files changed, 24 insertions(+), 8 deletions(-)

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
index bd65d4a7b285b..fd616ece9812e 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
@@ -199,6 +199,7 @@ class AvoidanceModule : public SceneModuleInterface
 
   // -- for pre-processing --
   void initVariables();
+  void initRTCStatus();
   AvoidancePlanningData calcAvoidancePlanningData(DebugData & debug) const;
 
   ObjectDataArray registered_objects_;
diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
index 0d9b20317c993..4764f67204000 100644
--- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -540,6 +540,10 @@ AvoidanceState AvoidanceModule::updateEgoState(const AvoidancePlanningData & dat
 
 void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, ShiftedPath & path)
 {
+  if (parameters_->disable_path_update) {
+    return;
+  }
+
   switch (data.state) {
     case AvoidanceState::NOT_AVOID: {
       break;
@@ -547,10 +551,9 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif
     case AvoidanceState::YIELD: {
       insertYieldVelocity(path);
       insertWaitPoint(parameters_->use_constraints_for_decel, path);
+      initRTCStatus();
       removeAllRegisteredShiftPoints(path_shifter_);
-      clearWaitingApproval();
       unlockNewModuleLaunch();
-      removeRTCStatus();
       break;
     }
     case AvoidanceState::AVOID_PATH_NOT_READY: {
@@ -2816,13 +2819,14 @@ BehaviorModuleOutput AvoidanceModule::plan()
   }
 
   avoidance_data_.state = updateEgoState(data);
-  if (!parameters_->disable_path_update) {
+
+  // update output data
+  {
     updateEgoBehavior(data, avoidance_path);
+    updateInfoMarker(avoidance_data_);
+    updateDebugMarker(avoidance_data_, path_shifter_, debug_data_);
   }
 
-  updateInfoMarker(avoidance_data_);
-  updateDebugMarker(avoidance_data_, path_shifter_, debug_data_);
-
   output.path = std::make_shared<PathWithLaneId>(avoidance_path.path);
   output.reference_path = getPreviousModuleOutput().reference_path;
   path_reference_ = getPreviousModuleOutput().reference_path;
@@ -3160,12 +3164,14 @@ void AvoidanceModule::updateData()
 void AvoidanceModule::processOnEntry()
 {
   initVariables();
+  initRTCStatus();
   waitApproval();
 }
 
 void AvoidanceModule::processOnExit()
 {
   initVariables();
+  initRTCStatus();
 }
 
 void AvoidanceModule::initVariables()
@@ -3174,8 +3180,6 @@ void AvoidanceModule::initVariables()
   prev_linear_shift_path_ = ShiftedPath();
   prev_reference_ = PathWithLaneId();
   path_shifter_ = PathShifter{};
-  left_shift_array_.clear();
-  right_shift_array_.clear();
 
   debug_data_ = DebugData();
   debug_marker_.markers.clear();
@@ -3187,6 +3191,17 @@ void AvoidanceModule::initVariables()
   is_avoidance_maneuver_starts = false;
 }
 
+void AvoidanceModule::initRTCStatus()
+{
+  removeRTCStatus();
+  clearWaitingApproval();
+  left_shift_array_.clear();
+  right_shift_array_.clear();
+  uuid_map_.at("left") = generateUUID();
+  uuid_map_.at("right") = generateUUID();
+  candidate_uuid_ = generateUUID();
+}
+
 TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) const
 {
   const auto shift_lines = path_shifter_.getShiftLines();