docs(behavior_path_planner): update goal planner docs

planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md

@@ -2,50 +2,54 @@
 
 ## Purpose / Role
 
-Search for a space where there are no objects and goal planner there.
+Plan path around the goal.
+
+- Park at the designated goal.
+- Modify the goal to avoid obstacles or to pull over at the side of tha lane.
 
 ## Design
 
+If goal modification is not allowed, park at the designated fixed goal. (`fixed_goal_planner` in the figure below)
+When allowed, park in accordance with the specified policy(e.g pull over on left/right side of the lane). (`rough_goal_planner` in the figure below). Currently rough goal planner only support pull_over feature, but it would be desirable to be able to accommodate various parking policies in the future.
+
 ```plantuml
 @startuml
 package goal_planner{
-    abstract class PullOverPlannerBase {
-    }
-    abstract class GoalSeacherBase {
-    }
 
-    package lane_parking <<Rectangle>>{
-        class ShiftPullOver {
-        }
-        class GeometricPullOver {
+    class GoalPlannerModule {}
+
+    package rough_goal_planner <<Rectangle>>{
+
+        package lane_parking <<Rectangle>>{
+            class ShiftPullOver {}
+            class GeometricPullOver {}
         }
-    }
 
-    package freespace_parking <<Rectangle>>{
-        class FreeSpacePullOver {
+        package freespace_parking <<Rectangle>>{
+            class FreeSpacePullOver {}
         }
-    }
 
-    class GoalSeacher {
-    }
+        class GoalSeacher {}
 
-    class GoalPlannerModule {
-    }
+        struct GoalCandidates {}
+        struct PullOverPath{}
 
+        abstract class PullOverPlannerBase {}
+        abstract class GoalSeacherBase {}
 
-    struct GoalCandidates {
     }
 
-    struct PullOverPath{}
+    package fixed_goal_planner <<Rectangle>>{
+        abstract class FixedGoalPlannerBase {}
+        class DefaultFixedPlanner{}
+    }
 }
 
 
 package utils{
-    class PathShifter {
-    }
+    class PathShifter {}
 
-    class GeometricParallelParking {
-    }
+    class GeometricParallelParking {}
 }
 
 package freespace_planning_algorithms
@@ -59,6 +63,7 @@ ShiftPullOver --|> PullOverPlannerBase
 GeometricPullOver --|> PullOverPlannerBase
 FreeSpacePullOver --|> PullOverPlannerBase
 GoalSeacher --|> GoalSeacherBase
+DefaultFixedPlanner --|> FixedGoalPlannerBase
 
 PathShifter --o ShiftPullOver
 GeometricParallelParking --o GeometricPullOver
@@ -67,18 +72,55 @@ RRTStar --o FreeSpacePullOver
 
 PullOverPlannerBase --o GoalPlannerModule
 GoalSeacherBase --o GoalPlannerModule
+FixedGoalPlannerBase --o GoalPlannerModule
 
 PullOverPath --o PullOverPlannerBase
 GoalCandidates --o GoalSeacherBase
 
 @enduml
 ```
 
+## start condition
+
+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.
+
+<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`.
+- 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.
+- ego-vehicle is in the same lane as the goal.
+
+<img src="https://user-images.githubusercontent.com/39142679/237929950-989ca6c3-d48c-4bb5-81e5-e8d6a38911aa.png" width="600">
+
+#### pull over on shoulder lane
+
+- The distance between the goal and ego-vehicle is shorter than `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">
+
+## finish condition
+
+- The distance to the goal from your vehicle is lower than threshold (default: < `1m`).
+- The ego-vehicle is stopped.
+  - The speed is lower than threshold (default: < `0.01m/s`).
+
 ## 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.                          | 200.0         |
+| 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           |

planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md

@@ -13,15 +13,16 @@ The manager launches and executes scene modules in `behavior_path_planner` depen
 
 Support status:
 
-| Name                     | Simple exclusive execution | Advanced simultaneous execution |
-| :----------------------- | :------------------------: | :-----------------------------: |
-| Avoidance                |     :heavy_check_mark:     |       :heavy_check_mark:        |
-| Avoidance By Lane Change |     :heavy_check_mark:     |    :heavy_multiplication_x:     |
-| Lane Change              |     :heavy_check_mark:     |       :heavy_check_mark:        |
-| External Lane Change     |     :heavy_check_mark:     |    :heavy_multiplication_x:     |
-| Pull Over                |     :heavy_check_mark:     |    :heavy_multiplication_x:     |
-| Pull Out                 |     :heavy_check_mark:     |    :heavy_multiplication_x:     |
-| Side Shift               |     :heavy_check_mark:     |    :heavy_multiplication_x:     |
+| Name                                     | Simple exclusive execution | Advanced simultaneous execution |
+| :--------------------------------------- | :------------------------: | :-----------------------------: |
+| Avoidance                                |     :heavy_check_mark:     |       :heavy_check_mark:        |
+| Avoidance By Lane Change                 |     :heavy_check_mark:     |    :heavy_multiplication_x:     |
+| Lane Change                              |     :heavy_check_mark:     |       :heavy_check_mark:        |
+| External Lane Change                     |     :heavy_check_mark:     |    :heavy_multiplication_x:     |
+| Goal Planner (without goal modification) |     :heavy_check_mark:     |       :heavy_check_mark:        |
+| Goal Planner (with goal modification)    |     :heavy_check_mark:     |    :heavy_multiplication_x:     |
+| Pull Out                                 |     :heavy_check_mark:     |       :heavy_check_mark:        |
+| Side Shift                               |     :heavy_check_mark:     |    :heavy_multiplication_x:     |
 
 Click [here](../README.md) for supported scene modules.