From 965c0bed48a601b588f885e1e5311c28d88c9537 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 5 Aug 2022 11:41:00 +0900 Subject: [PATCH] docs(behavior_velocity_planner, obstacle_stop_planner): add description of redundant restart prevention (#1507) * docs(behavior_velocity_planner): update README Signed-off-by: satoshi-ota * docs(obstacle_stop_planner): update README Signed-off-by: satoshi-ota --- .../detection-area-design.md | 50 +++++++++++++------ .../docs/detection_area/keep_stopping.svg | 3 ++ .../docs/detection_area/restart.svg | 3 ++ .../detection_area/restart_prevention.svg | 3 ++ .../docs/stop_line/keep_stopping.svg | 3 ++ .../docs/stop_line/restart.svg | 3 ++ .../docs/stop_line/restart_prevention.svg | 3 ++ .../virtual_traffic_light/keep_stopping.svg | 3 ++ .../docs/virtual_traffic_light/restart.svg | 3 ++ .../restart_prevention.svg | 3 ++ .../stop-line-design.md | 38 +++++++++++--- .../virtual-traffic-light-design.md | 22 ++++++++ planning/obstacle_stop_planner/README.md | 36 ++++++++++--- .../docs/keep_stopping.svg | 3 ++ .../obstacle_stop_planner/docs/restart.svg | 3 ++ .../docs/restart_prevention.svg | 3 ++ 16 files changed, 153 insertions(+), 29 deletions(-) create mode 100644 planning/behavior_velocity_planner/docs/detection_area/keep_stopping.svg create mode 100644 planning/behavior_velocity_planner/docs/detection_area/restart.svg create mode 100644 planning/behavior_velocity_planner/docs/detection_area/restart_prevention.svg create mode 100644 planning/behavior_velocity_planner/docs/stop_line/keep_stopping.svg create mode 100644 planning/behavior_velocity_planner/docs/stop_line/restart.svg create mode 100644 planning/behavior_velocity_planner/docs/stop_line/restart_prevention.svg create mode 100644 planning/behavior_velocity_planner/docs/virtual_traffic_light/keep_stopping.svg create mode 100644 planning/behavior_velocity_planner/docs/virtual_traffic_light/restart.svg create mode 100644 planning/behavior_velocity_planner/docs/virtual_traffic_light/restart_prevention.svg create mode 100644 planning/obstacle_stop_planner/docs/keep_stopping.svg create mode 100644 planning/obstacle_stop_planner/docs/restart.svg create mode 100644 planning/obstacle_stop_planner/docs/restart_prevention.svg diff --git a/planning/behavior_velocity_planner/detection-area-design.md b/planning/behavior_velocity_planner/detection-area-design.md index 6d36b45509783..20bfcce3204ac 100644 --- a/planning/behavior_velocity_planner/detection-area-design.md +++ b/planning/behavior_velocity_planner/detection-area-design.md @@ -1,16 +1,27 @@ -### Detection Area +## Detection Area -#### Role +### Role If pointcloud is detected in a detection area defined on a map, the stop planning will be executed at the predetermined point. ![brief](./docs/detection_area/detection_area.svg) -#### Activation Timing +### Activation Timing This module is activated when there is a detection area on the target lane. -### Algorithm +### Module Parameters + +| Parameter | Type | Description | +| --------------------------- | ------ | -------------------------------------------------------------------------------------------------- | +| `use_dead_line` | bool | [-] weather to use dead line or not | +| `use_pass_judge_line` | bool | [-] weather to use pass judge line or not | +| `state_clear_time` | double | [s] when the vehicle is stopping for certain time without incoming obstacle, move to STOPPED state | +| `stop_margin` | double | [m] a margin that the vehicle tries to stop before stop_line | +| `dead_line_margin` | double | [m] ignore threshold that vehicle behind is collide with ego vehicle or not | +| `hold_stop_margin_distance` | double | [m] parameter for restart prevention (See Algorithm section) | + +### Inner-workings / Algorithm 1. Gets a detection area and stop line from map information and confirms if there is pointcloud in the detection area 2. Inserts stop point l[m] in front of the stop line @@ -18,16 +29,6 @@ This module is activated when there is a detection area on the target lane. 4. Sets velocity as zero behind the stop line when the ego-vehicle is in front of the pass judge point 5. If the ego vehicle has passed the pass judge point already, it doesn’t stop and pass through. -#### Module Parameters - -| Parameter | Type | Description | -| --------------------- | ------ | -------------------------------------------------------------------------------------------------- | -| `stop_margin` | double | [m] a margin that the vehicle tries to stop before stop_line | -| `use_dead_line` | bool | [-] weather to use dead line or not | -| `dead_line_margin` | double | [m] ignore threshold that vehicle behind is collide with ego vehicle or not | -| `use_pass_judge_line` | bool | [-] weather to use pass judge line or not | -| `state_clear_time` | double | [s] when the vehicle is stopping for certain time without incoming obstacle, move to STOPPED state | - #### Flowchart ```plantuml @@ -87,3 +88,24 @@ endif stop @enduml ``` + +#### Restart prevention + +If it needs X meters (e.g. 0.5 meters) to stop once the vehicle starts moving due to the poor vehicle control performance, the vehicle goes over the stopping position that should be strictly observed when the vehicle starts to moving in order to approach the near stop point (e.g. 0.3 meters away). + +This module has parameter `hold_stop_margin_distance` in order to prevent from these redundant restart. If the vehicle is stopped within `hold_stop_margin_distance` meters from stop point of the module (\_front_to_stop_line < hold_stop_margin_distance), the module judges that the vehicle has already stopped for the module's stop point and plans to keep stopping current position even if the vehicle is stopped due to other factors. + +
+ ![example](docs/detection_area/restart_prevention.svg){width=1000} +
parameters
+
+ +
+ ![example](docs/detection_area/restart.svg){width=1000} +
outside the hold_stop_margin_distance
+
+ +
+ ![example](docs/detection_area/keep_stopping.svg){width=1000} +
inside the hold_stop_margin_distance
+
diff --git a/planning/behavior_velocity_planner/docs/detection_area/keep_stopping.svg b/planning/behavior_velocity_planner/docs/detection_area/keep_stopping.svg new file mode 100644 index 0000000000000..50f344c35d053 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/detection_area/keep_stopping.svg @@ -0,0 +1,3 @@ + + +
Ego
Ego
keep stopping at current position
keep stopping at curr...
target stop position
target stop positionText is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/detection_area/restart.svg b/planning/behavior_velocity_planner/docs/detection_area/restart.svg new file mode 100644 index 0000000000000..c21c6d8ef243b --- /dev/null +++ b/planning/behavior_velocity_planner/docs/detection_area/restart.svg @@ -0,0 +1,3 @@ + + +
Ego
Ego
move toward stop line
move toward stop line
target stop position
target stop positionText is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/detection_area/restart_prevention.svg b/planning/behavior_velocity_planner/docs/detection_area/restart_prevention.svg new file mode 100644 index 0000000000000..cc38ad32399fa --- /dev/null +++ b/planning/behavior_velocity_planner/docs/detection_area/restart_prevention.svg @@ -0,0 +1,3 @@ + + +
Ego
Ego
target stop position
target stop position
hold_stop_margin_distance [m]
hold_stop_margin_distance [m]
front_to_stop_line [m]
front_to_stop_line [...Text is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/stop_line/keep_stopping.svg b/planning/behavior_velocity_planner/docs/stop_line/keep_stopping.svg new file mode 100644 index 0000000000000..50f344c35d053 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/stop_line/keep_stopping.svg @@ -0,0 +1,3 @@ + + +
Ego
Ego
keep stopping at current position
keep stopping at curr...
target stop position
target stop positionText is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/stop_line/restart.svg b/planning/behavior_velocity_planner/docs/stop_line/restart.svg new file mode 100644 index 0000000000000..c21c6d8ef243b --- /dev/null +++ b/planning/behavior_velocity_planner/docs/stop_line/restart.svg @@ -0,0 +1,3 @@ + + +
Ego
Ego
move toward stop line
move toward stop line
target stop position
target stop positionText is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/stop_line/restart_prevention.svg b/planning/behavior_velocity_planner/docs/stop_line/restart_prevention.svg new file mode 100644 index 0000000000000..cc38ad32399fa --- /dev/null +++ b/planning/behavior_velocity_planner/docs/stop_line/restart_prevention.svg @@ -0,0 +1,3 @@ + + +
Ego
Ego
target stop position
target stop position
hold_stop_margin_distance [m]
hold_stop_margin_distance [m]
front_to_stop_line [m]
front_to_stop_line [...Text is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/virtual_traffic_light/keep_stopping.svg b/planning/behavior_velocity_planner/docs/virtual_traffic_light/keep_stopping.svg new file mode 100644 index 0000000000000..50f344c35d053 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/virtual_traffic_light/keep_stopping.svg @@ -0,0 +1,3 @@ + + +
Ego
Ego
keep stopping at current position
keep stopping at curr...
target stop position
target stop positionText is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/virtual_traffic_light/restart.svg b/planning/behavior_velocity_planner/docs/virtual_traffic_light/restart.svg new file mode 100644 index 0000000000000..c21c6d8ef243b --- /dev/null +++ b/planning/behavior_velocity_planner/docs/virtual_traffic_light/restart.svg @@ -0,0 +1,3 @@ + + +
Ego
Ego
move toward stop line
move toward stop line
target stop position
target stop positionText is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/virtual_traffic_light/restart_prevention.svg b/planning/behavior_velocity_planner/docs/virtual_traffic_light/restart_prevention.svg new file mode 100644 index 0000000000000..cc38ad32399fa --- /dev/null +++ b/planning/behavior_velocity_planner/docs/virtual_traffic_light/restart_prevention.svg @@ -0,0 +1,3 @@ + + +
Ego
Ego
target stop position
target stop position
hold_stop_margin_distance [m]
hold_stop_margin_distance [m]
front_to_stop_line [m]
front_to_stop_line [...Text is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/stop-line-design.md b/planning/behavior_velocity_planner/stop-line-design.md index d1980469c7482..e08ea43c07991 100644 --- a/planning/behavior_velocity_planner/stop-line-design.md +++ b/planning/behavior_velocity_planner/stop-line-design.md @@ -10,6 +10,14 @@ This module plans velocity so that the vehicle can stop right before stop lines This module is activated when there is a stop line in a target lane. +### Module Parameters + +| Parameter | Type | Description | +| --------------------------- | ------ | ---------------------------------------------------------------------------------------------- | +| `stop_margin` | double | a margin that the vehicle tries to stop before stop_line | +| `stop_check_dist` | double | when the vehicle is within `stop_check_dist` from stop_line and stopped, move to STOPPED state | +| `hold_stop_margin_distance` | double | [m] parameter for restart prevention (See Algorithm section) | + ### Inner-workings / Algorithms - Gets a stop line from map information. @@ -17,14 +25,7 @@ This module is activated when there is a stop line in a target lane. - Sets velocities of the path after the stop point to 0[m/s]. - Release the inserted stop velocity when the vehicle stops within a radius of 2[m] from the stop point. -### Module Parameters - -| Parameter | Type | Description | -| ----------------- | ------ | ---------------------------------------------------------------------------------------------- | -| `stop_margin` | double | a margin that the vehicle tries to stop before stop_line | -| `stop_check_dist` | double | when the vehicle is within `stop_check_dist` from stop_line and stopped, move to STOPPED state | - -### Flowchart +#### Flowchart ```plantuml @startuml @@ -85,3 +86,24 @@ Then, we can get `offset segment` and `offset from segment start`. After that, we can calculate a offset point from `offset segment` and `offset`. This will be `stop_pose`. ![calculate_stop_pose](./docs/stop_line/calculate_stop_pose.drawio.svg) + +#### Restart prevention + +If it needs X meters (e.g. 0.5 meters) to stop once the vehicle starts moving due to the poor vehicle control performance, the vehicle goes over the stopping position that should be strictly observed when the vehicle starts to moving in order to approach the near stop point (e.g. 0.3 meters away). + +This module has parameter `hold_stop_margin_distance` in order to prevent from these redundant restart. If the vehicle is stopped within `hold_stop_margin_distance` meters from stop point of the module (\_front_to_stop_line < hold_stop_margin_distance), the module judges that the vehicle has already stopped for the module's stop point and plans to keep stopping current position even if the vehicle is stopped due to other factors. + +
+ ![example](docs/stop_line/restart_prevention.svg){width=1000} +
parameters
+
+ +
+ ![example](docs/stop_line/restart.svg){width=1000} +
outside the hold_stop_margin_distance
+
+ +
+ ![example](docs/stop_line/keep_stopping.svg){width=1000} +
inside the hold_stop_margin_distance
+
diff --git a/planning/behavior_velocity_planner/virtual-traffic-light-design.md b/planning/behavior_velocity_planner/virtual-traffic-light-design.md index 9816622433c90..bbcab0e69a514 100644 --- a/planning/behavior_velocity_planner/virtual-traffic-light-design.md +++ b/planning/behavior_velocity_planner/virtual-traffic-light-design.md @@ -211,8 +211,30 @@ remote_controllable_traffic_light -[hidden]down-> warning_light | `max_delay_sec` | double | [s] maximum allowed delay for command | | `near_line_distance` | double | [m] threshold distance to stop line to check ego stop. | | `dead_line_margin` | double | [m] threshold distance that this module continue to insert stop line. | +| `hold_stop_margin_distance` | double | [m] parameter for restart prevention (See following section) | | `check_timeout_after_stop_line` | bool | [-] check timeout to stop when linkage is disconnected | +#### Restart prevention + +If it needs X meters (e.g. 0.5 meters) to stop once the vehicle starts moving due to the poor vehicle control performance, the vehicle goes over the stopping position that should be strictly observed when the vehicle starts to moving in order to approach the near stop point (e.g. 0.3 meters away). + +This module has parameter `hold_stop_margin_distance` in order to prevent from these redundant restart. If the vehicle is stopped within `hold_stop_margin_distance` meters from stop point of the module (\_front_to_stop_line < hold_stop_margin_distance), the module judges that the vehicle has already stopped for the module's stop point and plans to keep stopping current position even if the vehicle is stopped due to other factors. + +
+ ![example](docs/virtual_traffic_light/restart_prevention.svg){width=1000} +
parameters
+
+ +
+ ![example](docs/virtual_traffic_light/restart.svg){width=1000} +
outside the hold_stop_margin_distance
+
+ +
+ ![example](docs/virtual_traffic_light/keep_stopping.svg){width=1000} +
inside the hold_stop_margin_distance
+
+ #### Flowchart ```plantuml diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index 2bbc22acc77be..70d228506adfd 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/planning/obstacle_stop_planner/README.md @@ -62,13 +62,14 @@ When the deceleration section is inserted, the start point of the section is ins `Obstacle Stop Planner` module inserts a stop point in trajectory when there is a static point cloud on the trajectory. This module does not work when `Adaptive Cruise Controller` works. -| Parameter | Type | Description | -| --------------------------------------- | ------ | ----------------------------------------------------------------------------- | -| `stop_planner.stop_margin` | double | stop margin distance from obstacle on the path [m] | -| `stop_planner.min_behavior_stop_margin` | double | stop margin distance when any other stop point is inserted in stop margin [m] | -| `stop_planner.step_length` | double | step length for pointcloud search range [m] | -| `stop_planner.extend_distance` | double | extend trajectory to consider after goal obstacle in the extend_distance [m] | -| `stop_planner.expand_stop_range` | double | margin of vehicle footprint [m] | +| Parameter | Type | Description | +| ---------------------------------------- | ------ | ----------------------------------------------------------------------------- | +| `stop_planner.stop_margin` | double | stop margin distance from obstacle on the path [m] | +| `stop_planner.min_behavior_stop_margin` | double | stop margin distance when any other stop point is inserted in stop margin [m] | +| `stop_planner.step_length` | double | step length for pointcloud search range [m] | +| `stop_planner.extend_distance` | double | extend trajectory to consider after goal obstacle in the extend_distance [m] | +| `stop_planner.expand_stop_range` | double | margin of vehicle footprint [m] | +| `stop_planner.hold_stop_margin_distance` | double | parameter for restart prevention (See following section) [m] | #### Flowchart @@ -113,6 +114,27 @@ The module searches the obstacle pointcloud within detection area. When the poin ![pointcloud](./docs/point_cloud.drawio.svg) +#### Restart prevention + +If it needs X meters (e.g. 0.5 meters) to stop once the vehicle starts moving due to the poor vehicle control performance, the vehicle goes over the stopping position that should be strictly observed when the vehicle starts to moving in order to approach the near stop point (e.g. 0.3 meters away). + +This module has parameter `hold_stop_margin_distance` in order to prevent from these redundant restart. If the vehicle is stopped within `hold_stop_margin_distance` meters from stop point of the module (\_front_to_stop_line < hold_stop_margin_distance), the module judges that the vehicle has already stopped for the module's stop point and plans to keep stopping current position even if the vehicle is stopped due to other factors. + +
+ ![example](./docs/restart_prevention.svg){width=1000} +
parameters
+
+ +
+ ![example](./docs/restart.svg){width=1000} +
outside the hold_stop_margin_distance
+
+ +
+ ![example](./docs/keep_stopping.svg){width=1000} +
inside the hold_stop_margin_distance
+
+ ### Slow Down Planner #### Role diff --git a/planning/obstacle_stop_planner/docs/keep_stopping.svg b/planning/obstacle_stop_planner/docs/keep_stopping.svg new file mode 100644 index 0000000000000..50f344c35d053 --- /dev/null +++ b/planning/obstacle_stop_planner/docs/keep_stopping.svg @@ -0,0 +1,3 @@ + + +
Ego
Ego
keep stopping at current position
keep stopping at curr...
target stop position
target stop positionText is not SVG - cannot display \ No newline at end of file diff --git a/planning/obstacle_stop_planner/docs/restart.svg b/planning/obstacle_stop_planner/docs/restart.svg new file mode 100644 index 0000000000000..c21c6d8ef243b --- /dev/null +++ b/planning/obstacle_stop_planner/docs/restart.svg @@ -0,0 +1,3 @@ + + +
Ego
Ego
move toward stop line
move toward stop line
target stop position
target stop positionText is not SVG - cannot display \ No newline at end of file diff --git a/planning/obstacle_stop_planner/docs/restart_prevention.svg b/planning/obstacle_stop_planner/docs/restart_prevention.svg new file mode 100644 index 0000000000000..cc38ad32399fa --- /dev/null +++ b/planning/obstacle_stop_planner/docs/restart_prevention.svg @@ -0,0 +1,3 @@ + + +
Ego
Ego
target stop position
target stop position
hold_stop_margin_distance [m]
hold_stop_margin_distance [m]
front_to_stop_line [m]
front_to_stop_line [...Text is not SVG - cannot display \ No newline at end of file