Skip to content

Commit

Permalink
feat(obstacle_cruise_planner): use obstacle velocity based obstacle p…
Browse files Browse the repository at this point in the history 
…arameters (autowarefoundation#5510)

* Add extra tag for moving obstacle type

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* add static and moving as parameter postfixes

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* set hysteresis-based obstacle moving classification

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* update config params

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* Use speed norm for object classification

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* changed '_' for '.' to make the code more consistent

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* update documentation

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* move the obstacle moving check to generateSlowDownTrajectory

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* remove unnecessary reference

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* add const to certain variables

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

---------

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
  • Loading branch information
@danielsanchezaran @takayuki5168
danielsanchezaran authored and takayuki5168 committed Nov 22, 2023
1 parent 8eb858a commit 450f832
Show file tree
Hide file tree
Showing 4 changed files with 120 additions and 71 deletions.
32 changes: 19 additions & 13 deletions planning/obstacle_cruise_planner/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -230,19 +230,25 @@ $$

### Slow down planning

| Parameter | Type | Description |
| ----------------------------------------------- | -------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
| `slow_down.labels` | vector(string) | A vector of labels for customizing obstacle-label-based slow down behavior. Each label represents an obstacle type that will be treated differently when applying slow down. The possible labels are ("default" (Mandatory), "unknown","car","truck","bus","trailer","motorcycle","bicycle" or "pedestrian") |
| `slow_down.default.min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels |
| `slow_down.default.max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels |
| `slow_down.default.min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels |
| `slow_down.default.max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels |
| `(optional) slow_down."label".min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` |
| `(optional) slow_down."label".max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` |
| `(optional) slow_down."label".min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` |
| `(optional) slow_down."label".max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` |

The role of the slow down planning is inserting slow down velocity in the trajectory where the trajectory points are close to the obstacles. The parameters can be customized depending on the obstacle type (see `slow_down.labels`), making it possible to adjust the slow down behavior depending if the obstacle is a pedestrian, bicycle, car, etc.
| Parameter | Type | Description |
| ----------------------------------------------------------------- | -------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
| `slow_down.labels` | vector(string) | A vector of labels for customizing obstacle-label-based slow down behavior. Each label represents an obstacle type that will be treated differently when applying slow down. The possible labels are ("default" (Mandatory), "unknown","car","truck","bus","trailer","motorcycle","bicycle" or "pedestrian") |
| `slow_down.default.static.min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be static, or not moving |
| `slow_down.default.static.max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be static, or not moving |
| `slow_down.default.static.min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be static, or not moving |
| `slow_down.default.static.max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be static, or not moving |
| `slow_down.default.moving.min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be moving |
| `slow_down.default.moving.max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be moving |
| `slow_down.default.moving.min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be moving |
| `slow_down.default.moving.max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels and the obstacle is considered to be moving |
| `(optional) slow_down."label".(static & moving).min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels`. Requires a `static` and a `moving` value |
| `(optional) slow_down."label".(static & moving).max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels`. Requires a `static` and a `moving` value |
| `(optional) slow_down."label".(static & moving).min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels`. Requires a `static` and a `moving` value |
| `(optional) slow_down."label".(static & moving).max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels`. Requires a `static` and a `moving` value |

The role of the slow down planning is inserting slow down velocity in the trajectory where the trajectory points are close to the obstacles. The parameters can be customized depending on the obstacle type (see `slow_down.labels`), making it possible to adjust the slow down behavior depending if the obstacle is a pedestrian, bicycle, car, etc. Each obstacle type has a `static` and a `moving` parameter set, so it is possible to customize the slow down response of the ego vehicle according to the obstacle type and if it is moving or not. If an obstacle is determined to be moving, the corresponding `moving` set of parameters will be used to compute the vehicle slow down, otherwise, the `static` parameters will be used. The `static` and `moving` separation is useful for customizing the ego vehicle slow down behavior to, for example, slow down more significantly when passing stopped vehicles that might cause occlusion or that might suddenly open its doors.

An obstacle is classified as `static` if its total speed is less than the `moving_object_speed_threshold` parameter. Furthermore, a hysteresis based approach is used to avoid chattering, it uses the `moving_object_hysteresis_range` parameter range and the obstacle's previous state (`moving` or `static`) to determine if the obstacle is moving or not. In other words, if an obstacle was previously classified as `static`, it will not change its classification to `moving` unless its total speed is greater than `moving_object_speed_threshold` + `moving_object_hysteresis_range`. Likewise, an obstacle previously classified as `moving`, will only change to `static` if its speed is lower than `moving_object_speed_threshold` - `moving_object_hysteresis_range`.

The closest point on the obstacle to the ego's trajectory is calculated.
Then, the slow down velocity is calculated by linear interpolation with the distance between the point and trajectory as follows.
Expand Down
31 changes: 23 additions & 8 deletions planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -183,15 +183,30 @@
- "default"
- "pedestrian"
default:
min_lat_margin: 0.2
max_lat_margin: 1.0
min_ego_velocity: 2.0
max_ego_velocity: 8.0
moving:
min_lat_margin: 0.8
max_lat_margin: 1.3
min_ego_velocity: 0.5
max_ego_velocity: 1.5
static:
min_lat_margin: 0.2
max_lat_margin: 1.0
min_ego_velocity: 2.0
max_ego_velocity: 8.0
pedestrian:
min_lat_margin: 0.5
max_lat_margin: 1.5
min_ego_velocity: 1.0
max_ego_velocity: 2.0
moving:
min_lat_margin: 0.8
max_lat_margin: 1.3
min_ego_velocity: 0.5
max_ego_velocity: 0.8
static:
min_lat_margin: 0.8
max_lat_margin: 1.3
min_ego_velocity: 1.0
max_ego_velocity: 2.0

moving_object_speed_threshold: 0.5 # [m/s] how fast the object needs to move to be considered as "moving"
moving_object_hysteresis_range: 0.1 # [m/s] hysteresis range used to prevent chattering when obstacle moves close to moving_object_speed_threshold

time_margin_on_target_velocity: 1.5 # [s]

Expand Down
Loading

0 comments on commit 450f832

Please sign in to comment.