Merge pull request autowarefoundation#805 from tier4/fix/fixed_goal_v…

…0.11.0

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.
@@ -36,6 +35,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/config/scene_module_manager.param.yaml

@@ -8,6 +8,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: true
+      keep_last: false
       priority: 6
       max_module_size: 1
 
@@ -16,6 +17,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: true
+      keep_last: false
       priority: 6
       max_module_size: 1
 
@@ -24,6 +26,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: true
       enable_simultaneous_execution_as_candidate_module: true
+      keep_last: false
       priority: 5
       max_module_size: 1
 
@@ -32,6 +35,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: true
       enable_simultaneous_execution_as_candidate_module: true
+      keep_last: false
       priority: 5
       max_module_size: 1
 
@@ -40,6 +44,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false
+      keep_last: false
       priority: 0
       max_module_size: 1
 
@@ -48,6 +53,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false
+      keep_last: false
       priority: 2
       max_module_size: 1
 
@@ -56,6 +62,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false
+      keep_last: true
       priority: 1
       max_module_size: 1
 
@@ -64,6 +71,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: true
       enable_simultaneous_execution_as_candidate_module: false
+      keep_last: false
       priority: 4
       max_module_size: 1
 
@@ -72,6 +80,7 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: false
       enable_simultaneous_execution_as_candidate_module: false
+      keep_last: false
       priority: 3
       max_module_size: 1
 
@@ -80,5 +89,6 @@
       enable_rtc: true
       enable_simultaneous_execution_as_approved_module: true
       enable_simultaneous_execution_as_candidate_module: true
+      keep_last: false
       priority: 7
       max_module_size: 1

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**
 
@@ -174,12 +168,21 @@ 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            |
+| 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/parameters.hpp

@@ -28,6 +28,7 @@ struct ModuleConfigParameters
   bool enable_rtc{false};
   bool enable_simultaneous_execution_as_approved_module{false};
   bool enable_simultaneous_execution_as_candidate_module{false};
+  bool keep_last{false};
   uint8_t priority{0};
   uint8_t max_module_size{0};
 };

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

@@ -40,6 +40,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

@@ -55,6 +55,7 @@ class SceneModuleManagerInterface
     enable_simultaneous_execution_as_candidate_module_(
       config.enable_simultaneous_execution_as_candidate_module),
     enable_rtc_(config.enable_rtc),
+    keep_last_(config.keep_last),
     max_module_num_(config.max_module_size),
     priority_(config.priority)
   {
@@ -214,16 +215,37 @@ class SceneModuleManagerInterface
 
   bool canLaunchNewModule() const { return observers_.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_;
   }
 
+  bool isKeepLast() const { return keep_last_; }
+
   void setData(const std::shared_ptr<PlannerData> & planner_data) { planner_data_ = planner_data; }
 
   void reset()
@@ -290,6 +312,8 @@ class SceneModuleManagerInterface
 private:
   bool enable_rtc_;
 
+  bool keep_last_;
+
   size_t max_module_num_;
 
   size_t priority_;

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;
@@ -71,6 +70,7 @@ struct GoalPlannerParameters
   double object_recognition_collision_check_max_extra_stopping_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

@@ -284,6 +284,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
       declare_parameter<bool>(ns + "enable_simultaneous_execution_as_approved_module");
     config.enable_simultaneous_execution_as_candidate_module =
       declare_parameter<bool>(ns + "enable_simultaneous_execution_as_candidate_module");
+    config.keep_last = declare_parameter<bool>(ns + "keep_last");
     config.priority = declare_parameter<int>(ns + "priority");
     config.max_module_size = declare_parameter<int>(ns + "max_module_size");
     return config;