fix(bpp): transition from IDLE to WAITING APPROVAL (autowarefoundatio…

…n#6051) * fix(bpp): explicitly set the initial state Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> * remove isWaitingApproval() from state transition Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> * Update planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp Co-authored-by: Fumiya Watanabe <[email protected]> * explicitly set the initial state Signed-off-by: Zulfaqar Azmi <[email protected]> * remove setInitState in start_planner_module Signed-off-by: Zulfaqar Azmi <[email protected]> --------- Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> Signed-off-by: Zulfaqar Azmi <[email protected]> Co-authored-by: Fumiya Watanabe <[email protected]>
anhnv3991 · Feb 13, 2024 · 3753b56 · 3753b56
1 parent 3f9c334
commit 3753b56
Show file tree

Hide file tree

Showing 9 changed files with 11 additions and 41 deletions.
diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp
@@ -78,8 +78,6 @@ class AvoidanceModule : public SceneModuleInterface
 
   bool canTransitFailureState() override { return false; }
 
-  bool canTransitIdleToRunningState() override { return true; }
-
   /**
    * @brief update RTC status for candidate shift line.
    * @param candidate path.

diff --git a/...or_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp b/...or_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp
@@ -345,7 +345,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
 
   bool canTransitFailureState() override { return false; }
 
-  bool canTransitIdleToRunningState() override { return false; }
+  ModuleStatus setInitState() const override { return ModuleStatus::IDLE; }
 
   bool isLabelTargetObstacle(const uint8_t label) const;
   void updateTargetObjects();

diff --git a/...ath_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/...ath_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp
@@ -356,7 +356,6 @@ class GoalPlannerModule : public SceneModuleInterface
   // If terminating it, it may switch to lane following and could generate an inappropriate path.
   bool canTransitSuccessState() override { return false; }
   bool canTransitFailureState() override { return false; }
-  bool canTransitIdleToRunningState() override { return true; }
 
   mutable StartGoalPlannerData goal_planner_data_;
 

diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp
@@ -1863,6 +1863,9 @@ void GoalPlannerModule::updateSafetyCheckTargetObjectsData(
 
 std::pair<bool, bool> GoalPlannerModule::isSafePath() const
 {
+  if (!thread_safe_data_.get_pull_over_path()) {
+    return {false, false};
+  }
   const auto pull_over_path = thread_safe_data_.get_pull_over_path()->getCurrentPath();
   const auto & current_pose = planner_data_->self_odometry->pose.pose;
   const double current_velocity = std::hypot(

diff --git a/...g/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp b/...g/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp
@@ -126,7 +126,7 @@ class LaneChangeInterface : public SceneModuleInterface
 
   bool canTransitFailureState() override;
 
-  bool canTransitIdleToRunningState() override;
+  ModuleStatus setInitState() const override { return ModuleStatus::WAITING_APPROVAL; };
 
   void updateDebugMarker() const;
 

diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp
@@ -297,29 +297,6 @@ bool LaneChangeInterface::canTransitFailureState()
   return false;
 }
 
-bool LaneChangeInterface::canTransitIdleToRunningState()
-{
-  updateDebugMarker();
-
-  auto log_debug_throttled = [&](std::string_view message) -> void {
-    RCLCPP_DEBUG(getLogger(), "%s", message.data());
-  };
-
-  log_debug_throttled(__func__);
-
-  if (!isActivated() || isWaitingApproval()) {
-    if (module_type_->specialRequiredCheck()) {
-      return true;
-    }
-    log_debug_throttled("Module is idling.");
-    return false;
-  }
-
-  log_debug_throttled("Can lane change safely. Executing lane change.");
-  module_type_->toNormalState();
-  return true;
-}
-
 void LaneChangeInterface::updateDebugMarker() const
 {
   if (!parameters_->publish_debug_marker) {

diff --git a/..._planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/..._planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp
@@ -373,13 +373,10 @@ class SceneModuleInterface
       RCLCPP_DEBUG(getLogger(), "%s", message.data());
     };
     if (current_state_ == ModuleStatus::IDLE) {
-      if (canTransitIdleToRunningState()) {
-        log_debug_throttled("transiting from IDLE to RUNNING");
-        return ModuleStatus::RUNNING;
-      }
-
-      log_debug_throttled("transiting from IDLE to IDLE");
-      return ModuleStatus::IDLE;
+      auto init_state = setInitState();
+      RCLCPP_DEBUG(
+        getLogger(), "transiting from IDLE to %s", magic_enum::enum_name(init_state).data());
+      return init_state;
     }
 
     if (current_state_ == ModuleStatus::RUNNING) {
@@ -460,9 +457,9 @@ class SceneModuleInterface
   virtual bool canTransitFailureState() = 0;
 
   /**
-   * @brief State transition condition IDLE -> RUNNING
+   * @brief Explicitly set the initial state
    */
-  virtual bool canTransitIdleToRunningState() = 0;
+  virtual ModuleStatus setInitState() const { return ModuleStatus::RUNNING; }
 
   /**
    * @brief Get candidate path. This information is used for external judgement.

diff --git a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp
@@ -75,8 +75,6 @@ class SideShiftModule : public SceneModuleInterface
 
   bool canTransitFailureState() override { return false; }
 
-  bool canTransitIdleToRunningState() override { return true; }
-
   void initVariables();
 
   // non-const methods

diff --git a/..._start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/..._start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp
@@ -139,8 +139,6 @@ class StartPlannerModule : public SceneModuleInterface
 
   bool canTransitFailureState() override { return false; }
 
-  bool canTransitIdleToRunningState() override { return true; }
-
   /**
    * @brief init member variables.
    */
-Original file line number
+Diff line change
@@ Expand Up / @@ -139,8 +139,6 @@ class StartPlannerModule : public SceneModuleInterface @@
       bool canTransitFailureState() override { return false; }
-      bool canTransitIdleToRunningState() override { return true; }
       /**
        * @brief init member variables.
        */
@@ Expand Down @@