Skip to content

Predicted Path Checker#

Purpose#

The Predicted Path Checker package is designed for autonomous vehicles to check the predicted path generated by control modules. It handles potential collisions that the planning module might not be able to handle and that in the brake distance. In case of collision in brake distance, the package will send a diagnostic message labeled "ERROR" to alert the system to send emergency and in the case of collisions in outside reference trajectory, it sends pause request to pause interface to make the vehicle stop.

general-structure.png

Algorithm#

The package algorithm evaluates the predicted trajectory against the reference trajectory and the predicted objects in the environment. It checks for potential collisions and, if necessary, generates an appropriate response to avoid them ( emergency or pause request).

Inner Algorithm#

FlowChart.png

cutTrajectory() -> It cuts the predicted trajectory with input length. Length is calculated by multiplying the velocity of ego vehicle with "trajectory_check_time" parameter and "min_trajectory_length".

filterObstacles() -> It filters the predicted objects in the environment. It filters the objects which are not in front of the vehicle and far away from predicted trajectory.

checkTrajectoryForCollision() -> It checks the predicted trajectory for collision with the predicted objects. It calculates both polygon of trajectory points and predicted objects and checks intersection of both polygons. If there is an intersection, it calculates the nearest collision point. It returns the nearest collision point of polygon and the predicted object. It also checks predicted objects history which are intersect with the footprint before to avoid unexpected behaviors. Predicted objects history stores the objects if it was detected below the "chattering_threshold" seconds ago.

If the "enable_z_axis_obstacle_filtering" parameter is set to true, it filters the predicted objects in the Z-axis by using "z_axis_filtering_buffer". If the object does not intersect with the Z-axis, it is filtered out.

Z_axis_filtering.png

calculateProjectedVelAndAcc() -> It calculates the projected velocity and acceleration of the predicted object on predicted trajectory's collision point's axes.

isInBrakeDistance() -> It checks if the stop point is in brake distance. It gets relative velocity and acceleration of ego vehicle with respect to the predicted object. It calculates the brake distance, if the point in brake distance, it returns true.

isItDiscretePoint() -> It checks if the stop point on predicted trajectory is discrete point or not. If it is not discrete point, planning should handle the stop.

isThereStopPointOnRefTrajectory() -> It checks if there is a stop point on reference trajectory. If there is a stop point before the stop index, it returns true. Otherwise, it returns false, and node is going to call pause interface to make the vehicle stop.

Inputs#

Name Type Description
~/input/reference_trajectory autoware_planning_msgs::msg::Trajectory Reference trajectory
~/input/predicted_trajectory autoware_planning_msgs::msg::Trajectory Predicted trajectory
~/input/objects autoware_perception_msgs::msg::PredictedObject Dynamic objects in the environment
~/input/odometry nav_msgs::msg::Odometry Odometry message of vehicle to get current velocity
~/input/current_accel geometry_msgs::msg::AccelWithCovarianceStamped Current acceleration
/control/vehicle_cmd_gate/is_paused tier4_control_msgs::msg::IsPaused Current pause state of the vehicle

Outputs#

Name Type Description
~/debug/marker visualization_msgs::msg::MarkerArray Marker for visualization
~/debug/virtual_wall visualization_msgs::msg::MarkerArray Virtual wall marker for visualization
/control/vehicle_cmd_gate/set_pause tier4_control_msgs::srv::SetPause Pause service to make the vehicle stop
/diagnostics diagnostic_msgs::msg::DiagnosticStatus Diagnostic status of vehicle

Parameters#

Node Parameters#

Name Type Description Default value
update_rate double The update rate [Hz] 10.0
delay_time double he time delay considered for the emergency response [s] 0.17
max_deceleration double Max deceleration for ego vehicle to stop [m/s^2] 1.5
resample_interval double Interval for resampling trajectory [m] 0.5
stop_margin double The stopping margin [m] 0.5
ego_nearest_dist_threshold double The nearest distance threshold for ego vehicle [m] 3.0
ego_nearest_yaw_threshold double The nearest yaw threshold for ego vehicle [rad] 1.046
min_trajectory_check_length double The minimum trajectory check length in meters [m] 1.5
trajectory_check_time double The trajectory check time in seconds. [s] 3.0
distinct_point_distance_threshold double The distinct point distance threshold [m] 0.3
distinct_point_yaw_threshold double The distinct point yaw threshold [deg] 5.0
filtering_distance_threshold double It ignores the objects if distance is higher than this [m] 1.5
use_object_prediction bool If true, node predicts current pose of the objects wrt delta time [-] true

Collision Checker Parameters#

Name Type Description Default value
width_margin double The width margin for collision checking [Hz] 0.2
chattering_threshold double The chattering threshold for collision detection [s] 0.2
z_axis_filtering_buffer double The Z-axis filtering buffer [m] 0.3
enable_z_axis_obstacle_filtering bool A boolean flag indicating if Z-axis obstacle filtering is enabled false