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.

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

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.

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