feat(goal_planner): always run fixed_goal_planner

planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml

@@ -2,7 +2,6 @@
   ros__parameters:
     goal_planner:
       # general params
-      minimum_request_length: 100.0
       th_arrived_distance: 1.0
       th_stopped_velocity: 0.01
       th_stopped_time: 2.0  # It must be greater than the state_machine's.
@@ -35,6 +34,7 @@
 
       # pull over
       pull_over:
+        minimum_request_length: 100.0
         pull_over_velocity: 3.0
         pull_over_minimum_velocity: 1.38
         decide_path_distance: 10.0

planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md

@@ -86,16 +86,16 @@ Either one is activated when all conditions are met.
 
 ### fixed_goal_planner
 
-- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`.
 - Route is set with `allow_goal_modification=false` by default.
+- ego-vehicle is in the same lane as the goal.
 
 <img src="https://user-images.githubusercontent.com/39142679/237929955-c0adf01b-9e3c-45e3-848d-98cf11e52b65.png" width="600">
 
 ### rough_goal_planner
 
 #### pull over on road lane
 
-- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`.
+- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`.
 - Route is set with `allow_goal_modification=true` .
   - We can set this option with [SetRoute](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/routing/srv/SetRoute.srv#L2) api service.
   - We support `2D Rough Goal Pose` with the key bind `r` in RViz, but in the future there will be a panel of tools to manipulate various Route API from RViz.
@@ -105,7 +105,7 @@ Either one is activated when all conditions are met.
 
 #### pull over on shoulder lane
 
-- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`.
+- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`.
 - Goal is set in the `road_shoulder`.
 
 <img src="https://user-images.githubusercontent.com/39142679/237929941-2ce26ea5-c84d-4d17-8cdc-103f5246db90.png" width="600">
@@ -118,17 +118,11 @@ Either one is activated when all conditions are met.
 
 ## General parameters for goal_planner
 
-| Name                       | Unit   | Type   | Description                                                                                                                             | Default value |
-| :------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
-| minimum_request_length     | [m]    | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, the module is activated.                          | 100.0         |
-| th_arrived_distance        | [m]    | double | distance threshold for arrival of path termination                                                                                      | 1.0           |
-| th_stopped_velocity        | [m/s]  | double | velocity threshold for arrival of path termination                                                                                      | 0.01          |
-| th_stopped_time            | [s]    | double | time threshold for arrival of path termination                                                                                          | 2.0           |
-| pull_over_velocity         | [m/s]  | double | decelerate to this speed by the goal search area                                                                                        | 3.0           |
-| pull_over_minimum_velocity | [m/s]  | double | speed of pull_over after stopping once. this prevents excessive acceleration.                                                           | 1.38          |
-| margin_from_boundary       | [m]    | double | distance margin from edge of the shoulder lane                                                                                          | 0.5           |
-| decide_path_distance       | [m]    | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0          |
-| maximum_deceleration       | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly                                      | 1.0           |
+| Name                | Unit  | Type   | Description                                        | Default value |
+| :------------------ | :---- | :----- | :------------------------------------------------- | :------------ |
+| th_arrived_distance | [m]   | double | distance threshold for arrival of path termination | 1.0           |
+| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01          |
+| th_stopped_time     | [s]   | double | time threshold for arrival of path termination     | 2.0           |
 
 ## **collision check**
 
@@ -173,12 +167,22 @@ searched for in certain range of the shoulder lane.
 | longitudinal_margin             | [m]  | double | margin between ego-vehicle at the goal position and obstacles                                                                                                                                                            | 3.0            |
 | max_lateral_offset              | [m]  | double | maximum offset of goal search in the lateral direction                                                                                                                                                                   | 0.5            |
 | lateral_offset_interval         | [m]  | double | distance interval of goal search in the lateral direction                                                                                                                                                                | 0.25           |
-| ignore_distance_from_lane_start | [m]  | double | distance from start of pull over lanes for ignoring goal candidates                                                                                                                                                      | 15.0           |
+| ignore_distance_from_lane_start | [m]  | double | distance from start of pull over lanes for ignoring goal candidates                                                                                                                                                      | 0.0            |
+| ignore_distance_from_lane_start | [m]  | double | distance from start of pull over lanes for ignoring goal candidates                                                                                                                                                      | 0.0            |
+| margin_from_boundary            | [m]  | double | distance margin from edge of the shoulder lane                                                                                                                                                                           | 0.5            |
 
-## **Path Generation**
+## **Pull Over**
 
 There are three path generation methods.
-The path is generated with a certain margin (default: `0.5 m`) from left boundary of shoulder lane.
+The path is generated with a certain margin (default: `0.5 m`) from the boundary of shoulder lane.
+
+| Name                             | Unit   | Type   | Description                                                                                                                             | Default value |
+| :------------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| pull_over_minimum_request_length | [m]    | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, pull over is activated.                           | 100.0         |
+| pull_over_velocity               | [m/s]  | double | decelerate to this speed by the goal search area                                                                                        | 3.0           |
+| pull_over_minimum_velocity       | [m/s]  | double | speed of pull_over after stopping once. this prevents excessive acceleration.                                                           | 1.38          |
+| decide_path_distance             | [m]    | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0          |
+| maximum_deceleration             | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly                                      | 1.0           |
 
 ### **shift parking**
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp

@@ -41,6 +41,8 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;
 
+  bool isAlwaysExecutableModule() const override;
+
   bool isSimultaneousExecutableAsApprovedModule() const override;
 
   bool isSimultaneousExecutableAsCandidateModule() const override;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp

@@ -201,13 +201,32 @@ class SceneModuleManagerInterface
 
   bool canLaunchNewModule() const { return registered_modules_.size() < max_module_num_; }
 
+  /**
+   * Determine if the module is always executable, regardless of the state of other modules.
+   *
+   * When this returns true:
+   * - The module can be executed even if other modules are not marked as 'simultaneously
+   * executable'.
+   * - Conversely, the presence of this module will not prevent other modules
+   *   from executing, even if they are not marked as 'simultaneously executable'.
+   */
+  virtual bool isAlwaysExecutableModule() const { return false; }
+
   virtual bool isSimultaneousExecutableAsApprovedModule() const
   {
+    if (isAlwaysExecutableModule()) {
+      return true;
+    }
+
     return enable_simultaneous_execution_as_approved_module_;
   }
 
   virtual bool isSimultaneousExecutableAsCandidateModule() const
   {
+    if (isAlwaysExecutableModule()) {
+      return true;
+    }
+
     return enable_simultaneous_execution_as_candidate_module_;
   }
 

planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp

@@ -40,7 +40,6 @@ enum class ParkingPolicy {
 struct GoalPlannerParameters
 {
   // general  params
-  double minimum_request_length;
   double th_arrived_distance;
   double th_stopped_velocity;
   double th_stopped_time;
@@ -70,6 +69,7 @@ struct GoalPlannerParameters
   double object_recognition_collision_check_margin;
 
   // pull over general params
+  double pull_over_minimum_request_length;
   double pull_over_velocity;
   double pull_over_minimum_velocity;
   double decide_path_distance;

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -857,7 +857,6 @@ GoalPlannerParameters BehaviorPathPlannerNode::getGoalPlannerParam()
   // general params
   {
     std::string ns = "goal_planner.";
-    p.minimum_request_length = declare_parameter<double>(ns + "minimum_request_length");
     p.th_stopped_velocity = declare_parameter<double>(ns + "th_stopped_velocity");
     p.th_arrived_distance = declare_parameter<double>(ns + "th_arrived_distance");
     p.th_stopped_time = declare_parameter<double>(ns + "th_stopped_time");
@@ -915,6 +914,7 @@ GoalPlannerParameters BehaviorPathPlannerNode::getGoalPlannerParam()
     std::string ns = "goal_planner.pull_over.";
     p.pull_over_velocity = declare_parameter<double>(ns + "pull_over_velocity");
     p.pull_over_minimum_velocity = declare_parameter<double>(ns + "pull_over_minimum_velocity");
+    p.pull_over_minimum_request_length = declare_parameter<double>(ns + "minimum_request_length");
     p.decide_path_distance = declare_parameter<double>(ns + "decide_path_distance");
     p.maximum_deceleration = declare_parameter<double>(ns + "maximum_deceleration");
     p.maximum_jerk = declare_parameter<double>(ns + "maximum_jerk");

planning/behavior_path_planner/src/planner_manager.cpp

@@ -180,22 +180,6 @@ std::vector<SceneModulePtr> PlannerManager::getRequestModules(
 {
   std::vector<SceneModulePtr> request_modules{};
 
-  /**
-   * check whether it is possible to push back more modules to approved modules.
-   */
-  {
-    const auto find_block_module = [this](const auto & m) {
-      return !getManager(m)->isSimultaneousExecutableAsApprovedModule();
-    };
-
-    const auto itr =
-      std::find_if(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), find_block_module);
-
-    if (itr != approved_module_ptrs_.end()) {
-      return {};
-    }
-  }
-
   const auto toc = [this](const auto & name) {
     processing_time_.at(name) += stop_watch_.toc(name, true);
   };
@@ -204,14 +188,62 @@ std::vector<SceneModulePtr> PlannerManager::getRequestModules(
     stop_watch_.tic(manager_ptr->getModuleName());
 
     /**
-     * don't launch candidate module if approved modules already exist.
+     * determine the execution capability of modules based on existing approved modules.
      */
-    if (!approved_module_ptrs_.empty()) {
-      if (!manager_ptr->isSimultaneousExecutableAsApprovedModule()) {
-        toc(manager_ptr->getModuleName());
+    // Condition 1: always executable module can be added regardless of the existence of other
+    // modules, so skip checking the existence of other modules.
+    // in other cases, need to check the existence of other modules and which module can be added.
+    const bool has_non_always_executable_module = std::any_of(
+      approved_module_ptrs_.begin(), approved_module_ptrs_.end(),
+      [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); });
+    if (!manager_ptr->isAlwaysExecutableModule() && has_non_always_executable_module) {
+      // pairs of find_block_module and is_executable
+      std::vector<std::pair<std::function<bool(const SceneModulePtr &)>, std::function<bool()>>>
+        conditions;
+
+      // Condition 2: do not add modules that are neither always nor simultaneous executable
+      // if there exists at least one approved module that is simultaneous but not always
+      // executable. (only modules that are either always executable or simultaneous executable can
+      // be added)
+      conditions.push_back(
+        {[&](const SceneModulePtr & m) {
+           return !getManager(m)->isAlwaysExecutableModule() &&
+                  getManager(m)->isSimultaneousExecutableAsApprovedModule();
+         },
+         [&]() { return manager_ptr->isSimultaneousExecutableAsApprovedModule(); }});
+
+      // Condition 3: do not add modules that are not always executable if there exists
+      // at least one approved module that is neither always nor simultaneous executable.
+      // (only modules that are always executable can be added)
+      conditions.push_back(
+        {[&](const SceneModulePtr & m) {
+           return !getManager(m)->isAlwaysExecutableModule() &&
+                  !getManager(m)->isSimultaneousExecutableAsApprovedModule();
+         },
+         [&]() { return false; }});
+
+      bool skip_module = false;
+      for (const auto & condition : conditions) {
+        const auto & find_block_module = condition.first;
+        const auto & is_executable = condition.second;
+
+        const auto itr = std::find_if(
+          approved_module_ptrs_.begin(), approved_module_ptrs_.end(), find_block_module);
+
+        if (itr != approved_module_ptrs_.end() && !is_executable()) {
+          toc(manager_ptr->getModuleName());
+          skip_module = true;
+          continue;
+        }
+      }
+      if (skip_module) {
         continue;
       }
     }
+    // else{
+    //   Condition 4: if none of the above conditions are met, any module can be added.
+    //   (when the approved modules are either empty or consist only of always executable modules.)
+    // }
 
     /**
      * launch new candidate module.
@@ -334,20 +366,58 @@ std::pair<SceneModulePtr, BehaviorModuleOutput> PlannerManager::runRequestModule
    * remove non-executable modules.
    */
   for (const auto & module_ptr : sorted_request_modules) {
-    if (!getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule()) {
-      if (executable_modules.empty()) {
-        executable_modules.push_back(module_ptr);
-        break;
-      }
+    // Condition 1: always executable module can be added regardless of the existence of other
+    // modules.
+    if (getManager(module_ptr)->isAlwaysExecutableModule()) {
+      executable_modules.push_back(module_ptr);
+      continue;
     }
 
-    const auto itr =
-      std::find_if(executable_modules.begin(), executable_modules.end(), [this](const auto & m) {
-        return !getManager(m)->isSimultaneousExecutableAsCandidateModule();
-      });
-
-    if (itr == executable_modules.end()) {
+    // Condition 4: If the executable modules are either empty or consist only of always executable
+    // modules, any module can be added.
+    const bool has_non_always_executable_module = std::any_of(
+      executable_modules.begin(), executable_modules.end(),
+      [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); });
+    if (!has_non_always_executable_module) {
       executable_modules.push_back(module_ptr);
+      continue;
+    }
+
+    // pairs of find_block_module and is_executable
+    std::vector<std::pair<std::function<bool(const SceneModulePtr &)>, std::function<bool()>>>
+      conditions;
+
+    // Condition 3: Only modules that are always executable can be added
+    // if there exists at least one executable module that is neither always nor simultaneous
+    // executable.
+    conditions.push_back(
+      {[this](const SceneModulePtr & m) {
+         return !getManager(m)->isAlwaysExecutableModule() &&
+                !getManager(m)->isSimultaneousExecutableAsCandidateModule();
+       },
+       [&]() { return false; }});
+
+    // Condition 2: Only modules that are either always executable or simultaneous executable can be
+    // added if there exists at least one executable module that is simultaneous but not always
+    // executable.
+    conditions.push_back(
+      {[this](const SceneModulePtr & m) {
+         return !getManager(m)->isAlwaysExecutableModule() &&
+                getManager(m)->isSimultaneousExecutableAsCandidateModule();
+       },
+       [&]() { return getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule(); }});
+
+    for (const auto & condition : conditions) {
+      const auto & find_block_module = condition.first;
+      const auto & is_executable = condition.second;
+
+      const auto itr =
+        std::find_if(executable_modules.begin(), executable_modules.end(), find_block_module);
+
+      if (itr != executable_modules.end() && is_executable()) {
+        executable_modules.push_back(module_ptr);
+        break;
+      }
     }
   }