Skip to content

Planning Error Monitor#

Purpose#

planning_error_monitor checks a trajectory that if it has any invalid numerical values in its positions, twist and accel values. In addition, it also checks the distance between any two consecutive points and curvature value at a certain point. This package basically monitors if a trajectory, which is generated by planning module, has any unexpected errors.

Inner-workings / Algorithms#

flow_chart_image

Point Value Checker (onTrajectoryPointValueChecker)#

This function checks position, twist and accel values of all points on a trajectory. If they have Nan or Infinity, this function outputs error status.

Interval Checker (onTrajectoryIntervalChecker)#

This function computes interval distance between two consecutive points, and will output error messages if the distance is over the interval_threshold.

Curvature Checker (onTrajectoryCurvatureChecker)#

This function checks if the curvature at each point on a trajectory has an appropriate value. Calculation details are described in the following picture. First, we choose one point(green point in the picture) that are 1.0[m] behind the current point. Then we get a point(blue point in the picture) 1.0[m] ahead of the current point. Using these three points, we calculate the curvature by this method.

Relative Angle Checker (onTrajectoryRelativeAngleChecker)#

This function checks if the relative angle at point1 generated from point2 and 3 on a trajectory has an appropriate value.

curvature_calculation_diagram

Inputs / Outputs#

Input#

Name Type Description
~/input/trajectory autoware_planning_msgs/Trajectory Planned Trajectory by planning modules

Output#

Name Type Description
/diagnostics diagnostic_msgs/DiagnosticArray diagnostics outputs

Parameters#

Name Type Description Default value
error_interval double Error Interval Distance Threshold [m] 100.0
error_curvature double Error Curvature Threshold 1.0
error_sharp_angle double Error Sharp Angle Threshold \pi/4
ignore_too_close_points double Ignore Too Close Distance Threshold 0.005

Assumptions / Known limits#

  • It cannot compute curvature values at start and end points of the trajectory.
  • If trajectory points are too close, curvature calculation might output incorrect values.

Future extensions / Unimplemented parts#

  • Collision checker with obstacles may be implemented in the future.