Planning Validator#
The autoware_planning_validator is a module that checks the validity of a trajectory before it is published. The status of the validation can be viewed in the /diagnostics and /validation_status topics. When an invalid trajectory is detected, the autoware_planning_validator will process the trajectory following the selected option: "0. publish the trajectory as it is", "1. stop publishing the trajectory", "2. publish the last validated trajectory".
Supported features#
The following features are supported for trajectory validation and can have thresholds set by parameters:
- Invalid field : e.g. Inf, Nan
- Trajectory points interval : invalid if any of the distance of trajectory points is too large
- Curvature : invalid if the trajectory has too sharp turns that is not feasible for the given vehicle kinematics
- Relative angle : invalid if the yaw angle changes too fast in the sequence of trajectory points
- Lateral acceleration : invalid if the expected lateral acceleration/deceleration is too large
- Longitudinal acceleration/deceleration : invalid if the acceleration/deceleration in the trajectory point is too large
- Steering angle : invalid if the expected steering value is too large estimated from trajectory curvature
- Steering angle rate : invalid if the expected steering rate value is too large
- Velocity deviation : invalid if the planning velocity is too far from the ego velocity
- Distance deviation : invalid if the ego is too far from the trajectory
- Longitudinal distance deviation : invalid if the trajectory is too far from ego in longitudinal direction
- Forward trajectory length : invalid if the trajectory length is not enough to stop within a given deceleration
- Yaw difference : invalid if the difference between the ego yaw and closest trajectory yaw is too large
The following features are to be implemented.
- (TODO) TTC calculation : invalid if the expected time-to-collision is too short on the trajectory
Inputs/Outputs#
Inputs#
The autoware_planning_validator takes in the following inputs:
| Name | Type | Description |
|---|---|---|
~/input/kinematics |
nav_msgs/Odometry | ego pose and twist |
~/input/trajectory |
autoware_planning_msgs/Trajectory | target trajectory to be validated in this node |
Outputs#
It outputs the following:
| Name | Type | Description |
|---|---|---|
~/output/trajectory |
autoware_planning_msgs/Trajectory | validated trajectory |
~/output/validation_status |
planning_validator/PlanningValidatorStatus | validator status to inform the reason why the trajectory is valid/invalid |
/diagnostics |
diagnostic_msgs/DiagnosticStatus | diagnostics to report errors |
Parameters#
The following parameters can be set for the autoware_planning_validator:
System parameters#
| Name | Type | Description | Default value |
|---|---|---|---|
handling_type.noncritical |
int | set the handling type for noncritical invalid trajectory. 0: publish invalid trajectory as it is, 1: stop publishing the trajectory, 2: publish last valid trajectory. |
0 |
handling_type.critical |
int | set the handling type for critical invalid trajectory. 0: publish the trajectory even if it is invalid, 1: stop publishing the trajectory, 2: publish the last validated trajectory. |
0 |
publish_diag |
bool | if true, diagnostics msg is published. | true |
diag_error_count_threshold |
int | Number of consecutive invalid trajectories to set the Diag to ERROR. (Fe.g, threshold = 1 means, even if the trajectory is invalid, the Diag will not be ERROR if the next trajectory is valid.) | 0 |
display_on_terminal |
bool | show error msg on terminal | true |
Algorithm parameters#
Latency Check#
| Name | Type | Description | Default value |
|---|---|---|---|
validity_checks.latency.enable |
bool | flag to enable/disable latency validation check | true |
validity_checks.latency.threshold |
double | max valid value for the age of the trajectory msg [s] | 1.0 |
validity_checks.latency.is_critical |
bool | if true, will use handling type specified for critical checks | false |
Interval Check#
| Name | Type | Description | Default value |
|---|---|---|---|
validity_checks.interval.enable |
bool | flag to enable/disable interval validation check | true |
validity_checks.interval.threshold |
double | max valid distance between two consecutive trajectory points [m] | 100.0 |
validity_checks.interval.is_critical |
bool | if true, will use handling type specified for critical checks | false |
Curvature Check#
| Name | Type | Description | Default value |
|---|---|---|---|
validity_checks.curvature.enable |
bool | flag to enable/disable curvature validation check | true |
validity_checks.curvature.threshold |
double | max valid value for the trajectory curvature [1/m] | 2.0 |
validity_checks.curvature.is_critical |
bool | if true, will use handling type specified for critical checks | false |
Relative Angle Check#
| Name | Type | Description | Default value |
|---|---|---|---|
validity_checks.relative_angle.enable |
bool | flag to enable/disable relative angle validation check | true |
validity_checks.relative_angle.threshold |
double | max valid angle difference between two consecutive trajectory points [rad] | 2.0 |
validity_checks.relative_angle.is_critical |
bool | if true, will use handling type specified for critical checks | false |
Acceleration Check#
| Name | Type | Description | Default value |
|---|---|---|---|
validity_checks.acceleration.enable |
bool | flag to enable/disable acceleration validation check | true |
validity_checks.acceleration.lateral_th |
double | max valid value for the lateral acceleration along the trajectory [m/ss] | 9.8 |
validity_checks.acceleration.longitudinal_max_th |
double | max valid value for the longitudinal acceleration along the trajectory [m/ss] | 9.8 |
validity_checks.acceleration.longitudinal_min_th |
double | min valid value for the longitudinal acceleration along the trajectory [m/ss] | -9.8 |
validity_checks.acceleration.is_critical |
bool | if true, will use handling type specified for critical checks | false |
Steering Check#
| Name | Type | Description | Default value |
|---|---|---|---|
validity_checks.steering.enable |
bool | flag to enable/disable steering validation check | true |
validity_checks.steering.threshold |
double | max valid steering value along the trajectory [rad] | 1.414 |
validity_checks.steering.rate_th |
double | max valid steering rate along the trajectory [rad/s] | 10.0 |
validity_checks.steering.is_critical |
bool | if true, will use handling type specified for critical checks | false |
Deviation Check#
| Name | Type | Description | Default value |
|---|---|---|---|
validity_checks.deviation.enable |
bool | flag to enable/disable deviation validation check | true |
validity_checks.deviation.velocity_th |
double | max valid velocity deviation between ego and nearest trajectory point [m/s] | 100.0 |
validity_checks.deviation.distance_th |
double | max valid euclidean distance between ego and nearest trajectory point [m] | 100.0 |
validity_checks.deviation.lon_distance_th |
double | max valid longitudinal distance between ego and nearest trajectory point [m] | 2.0 |
validity_checks.deviation.yaw_th |
double | max valid yaw deviation between ego and nearest trajectory point [rad] | 1.5708 |
validity_checks.deviation.is_critical |
bool | if true, will use handling type specified for critical checks | false |
Forward Trajectory Length Check#
| Name | Type | Description | Default value |
|---|---|---|---|
validity_checks.forward_trajectory_length.enable |
bool | flag to enable/disable latency validation check | true |
validity_checks.forward_trajectory_length.acceleration |
double | acceleration value used to calculate required trajectory length. [m/ss] | -5.0 |
validity_checks.forward_trajectory_length.margin |
double | margin of the required length not to raise an error when ego slightly exceeds the end point. [m] | 2.0 |
validity_checks.forward_trajectory_length.is_critical |
bool | if true, will use handling type specified for critical checks | false |