Revert "fix(avoidance): fix RTC state transition (autowarefoundation#7142)"

This reverts commit d128e47.

Author: shmpwk

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -501,9 +501,11 @@ using AvoidOutlines = std::vector<AvoidOutline>;
  * avoidance state
  */
 enum class AvoidanceState {
-  RUNNING = 0,
-  CANCEL,
-  SUCCEEDED,
+  NOT_AVOID = 0,
+  AVOID_EXECUTE,
+  YIELD,
+  AVOID_PATH_READY,
+  AVOID_PATH_NOT_READY,
 };
 
 /*
@@ -512,7 +514,7 @@ enum class AvoidanceState {
 struct AvoidancePlanningData
 {
   // ego final state
-  AvoidanceState state{AvoidanceState::RUNNING};
+  AvoidanceState state{AvoidanceState::NOT_AVOID};
 
   // un-shifted pose (for current lane detection)
   Pose reference_pose;
@@ -564,6 +566,8 @@ struct AvoidancePlanningData
 
   bool ready{false};
 
+  bool success{false};
+
   bool comfortable{false};
 
   bool avoid_required{false};

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

@@ -30,7 +30,6 @@
 #include <memory>
 #include <string>
 #include <unordered_map>
-#include <utility>
 #include <vector>
 
 namespace behavior_path_planner
@@ -65,14 +64,7 @@ class AvoidanceModule : public SceneModuleInterface
   std::shared_ptr<AvoidanceDebugMsgArray> get_debug_msg_array() const;
 
 private:
-  /**
-   * @brief return the result whether the module can stop path generation process.
-   * @param avoidance data.
-   * @return it will return AvoidanceState::RUNNING when there are obstacles ego should avoid.
-   *         it will return AvoidanceState::CANCEL when all obstacles have gone.
-   *         it will return AvoidanceState::SUCCEEDED when the ego avoid all obstacles.
-   */
-  AvoidanceState getCurrentModuleState(const AvoidancePlanningData & data) const;
+  bool isSatisfiedSuccessCondition(const AvoidancePlanningData & data) const;
 
   bool canTransitSuccessState() override;
 
@@ -196,6 +188,14 @@ class AvoidanceModule : public SceneModuleInterface
    */
   void updateRTCData();
 
+  // ego state check
+
+  /**
+   * @brief update ego status based on avoidance path and surround condition.
+   * @param ego status. (NOT_AVOID, AVOID, YIELD, AVOID_EXECUTE, AVOID_PATH_NOT_READY)
+   */
+  AvoidanceState updateEgoState(const AvoidancePlanningData & data) const;
+
   // ego behavior update
 
   /**
@@ -359,7 +359,7 @@ class AvoidanceModule : public SceneModuleInterface
    * @brief reset registered shift lines.
    * @details reset only when the base offset is zero. Otherwise, sudden steering will be caused;
    */
-  void removeRegisteredShiftLines(const uint8_t state)
+  void removeRegisteredShiftLines()
   {
     constexpr double threshold = 0.1;
     if (std::abs(path_shifter_.getBaseOffset()) > threshold) {
@@ -370,19 +370,15 @@ class AvoidanceModule : public SceneModuleInterface
     unlockNewModuleLaunch();
 
     for (const auto & left_shift : left_shift_array_) {
-      if (rtc_interface_ptr_map_.at("left")->isRegistered(left_shift.uuid)) {
-        rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
-          left_shift.uuid, true, state, std::numeric_limits<double>::lowest(),
-          std::numeric_limits<double>::lowest(), clock_->now());
-      }
+      rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
+        left_shift.uuid, true, State::FAILED, std::numeric_limits<double>::lowest(),
+        std::numeric_limits<double>::lowest(), clock_->now());
     }
 
     for (const auto & right_shift : right_shift_array_) {
-      if (rtc_interface_ptr_map_.at("right")->isRegistered(right_shift.uuid)) {
-        rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
-          right_shift.uuid, true, state, std::numeric_limits<double>::lowest(),
-          std::numeric_limits<double>::lowest(), clock_->now());
-      }
+      rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
+        right_shift.uuid, true, State::FAILED, std::numeric_limits<double>::lowest(),
+        std::numeric_limits<double>::lowest(), clock_->now());
     }
 
     if (!path_shifter_.getShiftLines().empty()) {

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -115,14 +115,14 @@ bool AvoidanceModule::isExecutionReady() const
   return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid && avoid_data_.ready;
 }
 
-AvoidanceState AvoidanceModule::getCurrentModuleState(const AvoidancePlanningData & data) const
+bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData & data) const
 {
   const bool has_avoidance_target = std::any_of(
     data.target_objects.begin(), data.target_objects.end(),
     [this](const auto & o) { return !helper_->isAbsolutelyNotAvoidable(o); });
 
   if (has_avoidance_target) {
-    return AvoidanceState::RUNNING;
+    return false;
   }
 
   // If the ego is on the shift line, keep RUNNING.
@@ -133,7 +133,7 @@ AvoidanceState AvoidanceModule::getCurrentModuleState(const AvoidancePlanningDat
     };
     for (const auto & shift_line : path_shifter_.getShiftLines()) {
       if (within(shift_line, idx)) {
-        return AvoidanceState::RUNNING;
+        return false;
       }
     }
   }
@@ -142,21 +142,17 @@ AvoidanceState AvoidanceModule::getCurrentModuleState(const AvoidancePlanningDat
   const bool has_base_offset =
     std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_execution_threshold;
 
-  if (has_base_offset) {
-    return AvoidanceState::RUNNING;
-  }
-
   // Nothing to do. -> EXIT.
-  if (!has_shift_point) {
-    return AvoidanceState::SUCCEEDED;
+  if (!has_shift_point && !has_base_offset) {
+    return true;
   }
 
   // Be able to canceling avoidance path. -> EXIT.
   if (!helper_->isShifted() && parameters_->enable_cancel_maneuver) {
-    return AvoidanceState::CANCEL;
+    return true;
   }
 
-  return AvoidanceState::RUNNING;
+  return false;
 }
 
 bool AvoidanceModule::canTransitSuccessState()
@@ -187,7 +183,7 @@ bool AvoidanceModule::canTransitSuccessState()
     }
   }
 
-  return data.state == AvoidanceState::CANCEL || data.state == AvoidanceState::SUCCEEDED;
+  return data.success;
 }
 
 void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugData & debug)
@@ -506,7 +502,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de
 void AvoidanceModule::fillEgoStatus(
   AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const
 {
-  data.state = getCurrentModuleState(data);
+  data.success = isSatisfiedSuccessCondition(data);
 
   /**
    * Find the nearest object that should be avoid. When the ego follows reference path,
@@ -637,6 +633,27 @@ void AvoidanceModule::fillDebugData(
   }
 }
 
+AvoidanceState AvoidanceModule::updateEgoState(const AvoidancePlanningData & data) const
+{
+  if (data.yield_required) {
+    return AvoidanceState::YIELD;
+  }
+
+  if (!data.avoid_required) {
+    return AvoidanceState::NOT_AVOID;
+  }
+
+  if (!data.found_avoidance_path) {
+    return AvoidanceState::AVOID_PATH_NOT_READY;
+  }
+
+  if (isWaitingApproval() && path_shifter_.getShiftLines().empty()) {
+    return AvoidanceState::AVOID_PATH_READY;
+  }
+
+  return AvoidanceState::AVOID_EXECUTE;
+}
+
 void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, ShiftedPath & path)
 {
   if (parameters_->disable_path_update) {
@@ -646,30 +663,29 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif
   insertPrepareVelocity(path);
   insertAvoidanceVelocity(path);
 
-  const auto insert_velocity = [this, &data, &path]() {
-    if (data.yield_required) {
-      insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path);
-      return;
+  switch (data.state) {
+    case AvoidanceState::NOT_AVOID: {
+      break;
     }
-
-    if (!data.avoid_required) {
-      return;
+    case AvoidanceState::YIELD: {
+      insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path);
+      break;
     }
-
-    if (!data.found_avoidance_path) {
+    case AvoidanceState::AVOID_PATH_NOT_READY: {
       insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path);
-      return;
+      break;
     }
-
-    if (isWaitingApproval() && path_shifter_.getShiftLines().empty()) {
+    case AvoidanceState::AVOID_PATH_READY: {
       insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path);
-      return;
+      break;
     }
-
-    insertStopPoint(isBestEffort(parameters_->policy_deceleration), path);
-  };
-
-  insert_velocity();
+    case AvoidanceState::AVOID_EXECUTE: {
+      insertStopPoint(isBestEffort(parameters_->policy_deceleration), path);
+      break;
+    }
+    default:
+      throw std::domain_error("invalid behavior");
+  }
 
   insertReturnDeadLine(isBestEffort(parameters_->policy_deceleration), path);
 
@@ -853,16 +869,12 @@ BehaviorModuleOutput AvoidanceModule::plan()
 
   updatePathShifter(data.safe_shift_line);
 
-  if (data.state == AvoidanceState::SUCCEEDED) {
-    removeRegisteredShiftLines(State::SUCCEEDED);
-  }
-
-  if (data.state == AvoidanceState::CANCEL) {
-    removeRegisteredShiftLines(State::FAILED);
+  if (data.success) {
+    removeRegisteredShiftLines();
   }
 
   if (data.yield_required) {
-    removeRegisteredShiftLines(State::FAILED);
+    removeRegisteredShiftLines();
   }
 
   // generate path with shift points that have been inserted.
@@ -929,6 +941,8 @@ BehaviorModuleOutput AvoidanceModule::plan()
       spline_shift_path.path, parameters_->resample_interval_for_output);
   }
 
+  avoid_data_.state = updateEgoState(data);
+
   // update output data
   {
     updateEgoBehavior(data, spline_shift_path);

planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp

@@ -522,12 +522,9 @@ class SceneModuleInterface
   {
     for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
       if (ptr) {
-        if (ptr->isRegistered(uuid_map_.at(module_name))) {
-          ptr->updateCooperateStatus(
-            uuid_map_.at(module_name), true, State::SUCCEEDED,
-            std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(),
-            clock_->now());
-        }
+        ptr->updateCooperateStatus(
+          uuid_map_.at(module_name), true, State::SUCCEEDED, std::numeric_limits<double>::lowest(),
+          std::numeric_limits<double>::lowest(), clock_->now());
       }
     }
   }