fix(avoidance): fix RTC state transition (#7142)

* fix(avoidance): fix RTC state transition

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

* fix(rtc_interface): check if the cooperate status is registered

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

* refactor(avoidance): rebuild avoidance module state

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

---------

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

Author: satoshi-ota

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -501,11 +501,9 @@ using AvoidOutlines = std::vector<AvoidOutline>;
  * avoidance state
  */
 enum class AvoidanceState {
-  NOT_AVOID = 0,
-  AVOID_EXECUTE,
-  YIELD,
-  AVOID_PATH_READY,
-  AVOID_PATH_NOT_READY,
+  RUNNING = 0,
+  CANCEL,
+  SUCCEEDED,
 };
 
 /*
@@ -514,7 +512,7 @@ enum class AvoidanceState {
 struct AvoidancePlanningData
 {
   // ego final state
-  AvoidanceState state{AvoidanceState::NOT_AVOID};
+  AvoidanceState state{AvoidanceState::RUNNING};
 
   // un-shifted pose (for current lane detection)
   Pose reference_pose;
@@ -566,8 +564,6 @@ 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,6 +30,7 @@
 #include <memory>
 #include <string>
 #include <unordered_map>
+#include <utility>
 #include <vector>
 
 namespace behavior_path_planner
@@ -64,7 +65,14 @@ class AvoidanceModule : public SceneModuleInterface
   std::shared_ptr<AvoidanceDebugMsgArray> get_debug_msg_array() const;
 
 private:
-  bool isSatisfiedSuccessCondition(const AvoidancePlanningData & data) const;
+  /**
+   * @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 canTransitSuccessState() override;
 
@@ -188,14 +196,6 @@ 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()
+  void removeRegisteredShiftLines(const uint8_t state)
   {
     constexpr double threshold = 0.1;
     if (std::abs(path_shifter_.getBaseOffset()) > threshold) {
@@ -370,15 +370,19 @@ class AvoidanceModule : public SceneModuleInterface
     unlockNewModuleLaunch();
 
     for (const auto & left_shift : left_shift_array_) {
-      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());
+      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());
+      }
     }
 
     for (const auto & right_shift : right_shift_array_) {
-      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 (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());
+      }
     }
 
     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;
 }
 
-bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData & data) const
+AvoidanceState AvoidanceModule::getCurrentModuleState(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 false;
+    return AvoidanceState::RUNNING;
   }
 
   // If the ego is on the shift line, keep RUNNING.
@@ -133,7 +133,7 @@ bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData &
     };
     for (const auto & shift_line : path_shifter_.getShiftLines()) {
       if (within(shift_line, idx)) {
-        return false;
+        return AvoidanceState::RUNNING;
       }
     }
   }
@@ -142,17 +142,21 @@ bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData &
   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 && !has_base_offset) {
-    return true;
+  if (!has_shift_point) {
+    return AvoidanceState::SUCCEEDED;
   }
 
   // Be able to canceling avoidance path. -> EXIT.
   if (!helper_->isShifted() && parameters_->enable_cancel_maneuver) {
-    return true;
+    return AvoidanceState::CANCEL;
   }
 
-  return false;
+  return AvoidanceState::RUNNING;
 }
 
 bool AvoidanceModule::canTransitSuccessState()
@@ -183,7 +187,7 @@ bool AvoidanceModule::canTransitSuccessState()
     }
   }
 
-  return data.success;
+  return data.state == AvoidanceState::CANCEL || data.state == AvoidanceState::SUCCEEDED;
 }
 
 void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugData & debug)
@@ -502,7 +506,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de
 void AvoidanceModule::fillEgoStatus(
   AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const
 {
-  data.success = isSatisfiedSuccessCondition(data);
+  data.state = getCurrentModuleState(data);
 
   /**
    * Find the nearest object that should be avoid. When the ego follows reference path,
@@ -633,27 +637,6 @@ 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) {
@@ -663,29 +646,30 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif
   insertPrepareVelocity(path);
   insertAvoidanceVelocity(path);
 
-  switch (data.state) {
-    case AvoidanceState::NOT_AVOID: {
-      break;
-    }
-    case AvoidanceState::YIELD: {
+  const auto insert_velocity = [this, &data, &path]() {
+    if (data.yield_required) {
       insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path);
-      break;
+      return;
     }
-    case AvoidanceState::AVOID_PATH_NOT_READY: {
-      insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path);
-      break;
+
+    if (!data.avoid_required) {
+      return;
     }
-    case AvoidanceState::AVOID_PATH_READY: {
+
+    if (!data.found_avoidance_path) {
       insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path);
-      break;
+      return;
     }
-    case AvoidanceState::AVOID_EXECUTE: {
-      insertStopPoint(isBestEffort(parameters_->policy_deceleration), path);
-      break;
+
+    if (isWaitingApproval() && path_shifter_.getShiftLines().empty()) {
+      insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path);
+      return;
     }
-    default:
-      throw std::domain_error("invalid behavior");
-  }
+
+    insertStopPoint(isBestEffort(parameters_->policy_deceleration), path);
+  };
+
+  insert_velocity();
 
   insertReturnDeadLine(isBestEffort(parameters_->policy_deceleration), path);
 
@@ -869,12 +853,16 @@ BehaviorModuleOutput AvoidanceModule::plan()
 
   updatePathShifter(data.safe_shift_line);
 
-  if (data.success) {
-    removeRegisteredShiftLines();
+  if (data.state == AvoidanceState::SUCCEEDED) {
+    removeRegisteredShiftLines(State::SUCCEEDED);
+  }
+
+  if (data.state == AvoidanceState::CANCEL) {
+    removeRegisteredShiftLines(State::FAILED);
   }
 
   if (data.yield_required) {
-    removeRegisteredShiftLines();
+    removeRegisteredShiftLines(State::FAILED);
   }
 
   // generate path with shift points that have been inserted.
@@ -941,8 +929,6 @@ 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

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