fix(bpp): transition from IDLE to WAITING APPROVAL

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
zulfaqar-azmi-t4 · Jan 11, 2024 · 0c92e1c · 0c92e1c
1 parent 9b736d5
commit 0c92e1c
Show file tree

Hide file tree

Showing 10 changed files with 14 additions and 11 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,7 +78,7 @@ class AvoidanceModule : public SceneModuleInterface
 
   bool canTransitFailureState() override { return false; }
 
-  bool canTransitIdleToRunningState() override { return true; }
+  bool canTransitIdleToWaitingApprovalState() override { return true; }
 
   /**
    * @brief update RTC status for candidate shift line.

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; }
+  bool canTransitIdleToWaitingApprovalState() override { return false; }
 
   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
@@ -330,7 +330,7 @@ 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; }
+  bool canTransitIdleToWaitingApprovalState() 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
@@ -1766,6 +1766,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;
+  bool canTransitIdleToWaitingApprovalState() override;
 
   void setObjectDebugVisualization() const;
 

diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp
@@ -293,7 +293,7 @@ bool LaneChangeInterface::canTransitFailureState()
   return false;
 }
 
-bool LaneChangeInterface::canTransitIdleToRunningState()
+bool LaneChangeInterface::canTransitIdleToWaitingApprovalState()
 {
   setObjectDebugVisualization();
 

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,9 +373,9 @@ class SceneModuleInterface
       RCLCPP_DEBUG(getLogger(), "%s", message.data());
     };
     if (current_state_ == ModuleStatus::IDLE) {
-      if (canTransitIdleToRunningState()) {
+      if (canTransitIdleToWaitingApprovalState()) {
         log_debug_throttled("transiting from IDLE to RUNNING");
-        return ModuleStatus::RUNNING;
+        return ModuleStatus::WAITING_APPROVAL;
       }
 
       log_debug_throttled("transiting from IDLE to IDLE");
@@ -462,7 +462,7 @@ class SceneModuleInterface
   /**
    * @brief State transition condition IDLE -> RUNNING
    */
-  virtual bool canTransitIdleToRunningState() = 0;
+  virtual bool canTransitIdleToWaitingApprovalState() = 0;
 
   /**
    * @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,7 +75,7 @@ class SideShiftModule : public SceneModuleInterface
 
   bool canTransitFailureState() override { return false; }
 
-  bool canTransitIdleToRunningState() override { return true; }
+  bool canTransitIdleToWaitingApprovalState() override { return true; }
 
   void initVariables();
 

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,7 +139,7 @@ class StartPlannerModule : public SceneModuleInterface
 
   bool canTransitFailureState() override { return false; }
 
-  bool canTransitIdleToRunningState() override;
+  bool canTransitIdleToWaitingApprovalState() override;
 
   /**
    * @brief init member variables.

diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
@@ -338,7 +338,7 @@ bool StartPlannerModule::canTransitSuccessState()
   return hasFinishedPullOut();
 }
 
-bool StartPlannerModule::canTransitIdleToRunningState()
+bool StartPlannerModule::canTransitIdleToWaitingApprovalState()
 {
   return isActivated() && !isWaitingApproval();
 }