fix(avoidance): reset rtc status in yield status (autowarefoundation#…

…3798) Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
satoshi-ota · May 25, 2023 · e17a2ff · e17a2ff
1 parent d67dd9e
commit e17a2ff
Show file tree

Hide file tree

Showing 2 changed files with 33 additions and 10 deletions.
diff --git a/...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/...or_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
@@ -532,17 +532,20 @@ 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;
     }
     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: {
@@ -2784,13 +2787,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;
@@ -3141,18 +3145,25 @@ void AvoidanceModule::updateData()
   fillShiftLine(avoidance_data_, debug_data_);
 }
 
-void AvoidanceModule::processOnEntry() { initVariables(); }
+void AvoidanceModule::processOnEntry()
+{
+  initVariables();
+  initRTCStatus();
+  waitApproval();
+}
 
-void AvoidanceModule::processOnExit() { initVariables(); }
+void AvoidanceModule::processOnExit()
+{
+  initVariables();
+  initRTCStatus();
+}
 
 void AvoidanceModule::initVariables()
 {
   prev_output_ = ShiftedPath();
   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();
@@ -3164,6 +3175,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();