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

…n#6051)

* fix(bpp): explicitly set the initial state

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* remove isWaitingApproval() from state transition

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

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

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

* explicitly set the initial state

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* remove setInitState in start_planner_module

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

---------

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

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.

planning/behavior_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();

planning/behavior_path_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_;
 

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(

planning/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;
 

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) {

planning/behavior_path_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.

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

planning/behavior_path_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.
    */