feat(goal_planner): do not use minimum_request_length for fixed goal …

…planner (#4831)

Signed-off-by: kosuke55 <[email protected]>

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/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**
 
@@ -175,11 +169,21 @@ searched for in certain range of the shoulder lane.
 | 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                                                                                                                                                      | 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/utils/goal_planner/goal_planner_parameters.hpp

@@ -41,7 +41,6 @@ enum class ParkingPolicy {
 struct GoalPlannerParameters
 {
   // general  params
-  double minimum_request_length;
   double th_arrived_distance;
   double th_stopped_velocity;
   double th_stopped_time;
@@ -72,6 +71,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/scene_module/goal_planner/goal_planner_module.cpp

@@ -307,12 +307,19 @@ bool GoalPlannerModule::isExecutionRequested() const
     return false;
   }
 
+  // if goal modification is not allowed
+  // 1) goal_pose is in current_lanes, plan path to the original fixed goal
+  // 2) goal_pose is NOT in current_lanes, do not execute goal_planner
+  if (!goal_planner_utils::isAllowedGoalModification(route_handler)) {
+    return goal_is_in_current_lanes;
+  }
+
   // if goal arc coordinates can be calculated, check if goal is in request_length
   const double self_to_goal_arc_length =
     utils::getSignedDistance(current_pose, goal_pose, current_lanes);
   const double request_length = goal_planner_utils::isAllowedGoalModification(route_handler)
                                   ? calcModuleRequestLength()
-                                  : parameters_->minimum_request_length;
+                                  : parameters_->pull_over_minimum_request_length;
   if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) {
     // if current position is far from goal or behind goal, do not execute goal_planner
     return false;
@@ -365,13 +372,13 @@ double GoalPlannerModule::calcModuleRequestLength() const
   const auto min_stop_distance = calcFeasibleDecelDistance(
     planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0);
   if (!min_stop_distance) {
-    return parameters_->minimum_request_length;
+    return parameters_->pull_over_minimum_request_length;
   }
 
   const double minimum_request_length =
     *min_stop_distance + parameters_->backward_goal_search_length + approximate_pull_over_distance_;
 
-  return std::max(minimum_request_length, parameters_->minimum_request_length);
+  return std::max(minimum_request_length, parameters_->pull_over_minimum_request_length);
 }
 
 Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const

planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp

@@ -35,7 +35,6 @@ GoalPlannerModuleManager::GoalPlannerModuleManager(
   const std::string base_ns = "goal_planner.";
   // general params
   {
-    p.minimum_request_length = node->declare_parameter<double>(base_ns + "minimum_request_length");
     p.th_stopped_velocity = node->declare_parameter<double>(base_ns + "th_stopped_velocity");
     p.th_arrived_distance = node->declare_parameter<double>(base_ns + "th_arrived_distance");
     p.th_stopped_time = node->declare_parameter<double>(base_ns + "th_stopped_time");
@@ -96,6 +95,8 @@ GoalPlannerModuleManager::GoalPlannerModuleManager(
   // pull over general params
   {
     const std::string ns = base_ns + "pull_over.";
+    p.pull_over_minimum_request_length =
+      node->declare_parameter<double>(ns + "minimum_request_length");
     p.pull_over_velocity = node->declare_parameter<double>(ns + "pull_over_velocity");
     p.pull_over_minimum_velocity =
       node->declare_parameter<double>(ns + "pull_over_minimum_velocity");