|
| 1 | +# Stop Line |
| 2 | + |
| 3 | +## Role |
| 4 | + |
| 5 | +This module plans the vehicle's velocity to ensure it stops just before stop lines and can resume movement after stopping. |
| 6 | + |
| 7 | + |
| 8 | + |
| 9 | +## Activation Timing |
| 10 | + |
| 11 | +This module is activated when there is a stop line in a target lane. |
| 12 | + |
| 13 | +## Module Parameters |
| 14 | + |
| 15 | +| Parameter | Type | Description | |
| 16 | +| ------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | |
| 17 | +| `stop_margin` | double | Margin that the vehicle tries to stop in before stop_line | |
| 18 | +| `stop_duration_sec` | double | [s] Time parameter for the ego vehicle to stop before stop line | |
| 19 | +| `hold_stop_margin_distance` | double | [m] Parameter for restart prevention (See Algorithm section). Also, when the ego vehicle is within this distance from a stop line, the ego state becomes STOPPED from APPROACHING | |
| 20 | +| `use_initialization_stop_state` | bool | Flag to determine whether to return to the approaching state when the vehicle moves away from a stop line. | |
| 21 | + |
| 22 | +## Inner-workings / Algorithms |
| 23 | + |
| 24 | +- Gets a stop line from map information. |
| 25 | +- Inserts a stop point on the path from the stop line defined in the map and the ego vehicle length. |
| 26 | +- Sets velocities of the path after the stop point to 0[m/s]. |
| 27 | +- Releases the inserted stop velocity when the vehicle halts at the stop point for `stop_duration_sec` seconds. |
| 28 | + |
| 29 | +### Flowchart |
| 30 | + |
| 31 | +```plantuml |
| 32 | +@startuml |
| 33 | +title modifyPathVelocity |
| 34 | +start |
| 35 | +
|
| 36 | +:find collision between path and stop_line; |
| 37 | +
|
| 38 | +if (collision is found?) then (yes) |
| 39 | +else (no) |
| 40 | + stop |
| 41 | +endif |
| 42 | +
|
| 43 | +:find offset segment; |
| 44 | +
|
| 45 | +:calculate stop pose; |
| 46 | +
|
| 47 | +:calculate distance to stop line; |
| 48 | +
|
| 49 | +if (state is APPROACH) then (yes) |
| 50 | + :set stop velocity; |
| 51 | +
|
| 52 | + if (vehicle is within hold_stop_margin_distance?) then (yes) |
| 53 | + if (vehicle is stopped?) then (yes) |
| 54 | + :change state to STOPPED; |
| 55 | + endif |
| 56 | + endif |
| 57 | +else if (state is STOPPED) then (yes) |
| 58 | + if (stopping time is longer than stop_duration_sec ?) then (yes) |
| 59 | + :change state to START; |
| 60 | + endif |
| 61 | +else if (state is START) then (yes) |
| 62 | + if ([optional] far from stop line?) then (yes) |
| 63 | + :change state to APPROACH; |
| 64 | + endif |
| 65 | +endif |
| 66 | +
|
| 67 | +stop |
| 68 | +@enduml |
| 69 | +``` |
| 70 | + |
| 71 | +This algorithm is based on `segment`. |
| 72 | +`segment` consists of two node points. It's useful for removing boundary conditions because if `segment(i)` exists we can assume `node(i)` and `node(i+1)` exist. |
| 73 | + |
| 74 | + |
| 75 | + |
| 76 | +First, this algorithm finds a collision between reference path and stop line. |
| 77 | +Then, we can get `collision segment` and `collision point`. |
| 78 | + |
| 79 | + |
| 80 | + |
| 81 | +Next, based on `collision point`, it finds `offset segment` by iterating backward points up to a specific offset length. |
| 82 | +The offset length is `stop_margin`(parameter) + `base_link to front`(to adjust head pose to stop line). |
| 83 | +Then, we can get `offset segment` and `offset from segment start`. |
| 84 | + |
| 85 | + |
| 86 | + |
| 87 | +After that, we can calculate an offset point from `offset segment` and `offset`. This will be `stop_pose`. |
| 88 | + |
| 89 | + |
| 90 | + |
| 91 | +### Restart Prevention |
| 92 | + |
| 93 | +If the vehicle requires X meters (e.g. 0.5 meters) to stop once it starts moving due to poor vehicle control performance, it may overshoot the stopping position, which must be strictly observed. This happens when the vehicle begins to move in order to approach a nearby stop point (e.g. 0.3 meters away). |
| 94 | + |
| 95 | +This module includes the parameter `hold_stop_margin_distance` to prevent redundant restarts in such a situation. If the vehicle is stopped within `hold_stop_margin_distance` meters of the stop point (\_front_to_stop_line < hold_stop_margin_distance), the module determines that the vehicle has already stopped at the stop point and will maintain the current stopping position, even if the vehicle has come to a stop due to other factors. |
| 96 | + |
| 97 | +<figure markdown> |
| 98 | + {width=1000} |
| 99 | + <figcaption>parameters</figcaption> |
| 100 | +</figure> |
| 101 | + |
| 102 | +<figure markdown> |
| 103 | + {width=1000} |
| 104 | + <figcaption>outside the hold_stop_margin_distance</figcaption> |
| 105 | +</figure> |
| 106 | + |
| 107 | +<figure markdown> |
| 108 | + {width=1000} |
| 109 | + <figcaption>inside the hold_stop_margin_distance</figcaption> |
| 110 | +</figure> |
0 commit comments