Autoware Trajectory Modifier#

The autoware_trajectory_modifier package provides a plugin-based architecture for post-processing trajectory points to improve trajectory quality and ensure vehicle safety. It takes candidate trajectories and applies various modification algorithms to enhance their feasibility and safety characteristics.

Features#

Plugin-based architecture for extensible trajectory modifications
Stop point fixing to prevent trajectory issues near stationary conditions
Obstacle detection and stopping to prevent collision
Configurable parameters to adjust modification behavior

Architecture#

The trajectory modifier uses a plugin-based system where different modification algorithms can be implemented as plugins. Each plugin inherits from the TrajectoryModifierPluginBase class and implements the required interface.

Plugin Interface#

All modifier plugins must inherit from TrajectoryModifierPluginBase and implement:

modify_trajectory() - Main method to modify trajectory points
on_initialize() - Initialize plugin members and parameters
update_params() - Handle parameter updates
is_trajectory_modification_required() - Determine if modification is needed

Current Plugins#

Stop Point Fixer#

The Stop Point Fixer plugin addresses trajectory issues when the ego vehicle is stationary or moving at very low speeds. It prevents problematic trajectory points that could cause planning issues by replacing the trajectory with a single stop point when either of two independently configurable conditions is met:

Close stop: the trajectory's last point (stop point) is within a minimum distance threshold from ego
Long stop: the trajectory commands ego to remain stopped for longer than a minimum duration threshold

Both conditions are individually enabled or disabled via parameters, allowing fine-grained control over when the override is applied.

Obstacle Stop#

The Obstacle Stop plugin serves as a deterministic safety shield operating independently of the generative model to:

Enforce Longitudinal Safety: Monitors the gap to dynamic and static obstacles to ensure a safe distance is maintained under all kinematic conditions.
Ensure Definitive Stopping: Guarantees zero-velocity set-points for stationary objects (e.g., traffic lights, stopped vehicles) to prevent "creeping" or oscillating behavior near obstacles.
Provide Predictable Deceleration: Standardizes the vehicle’s stopping profile to ensure consistent, comfortable, and physically guaranteed deceleration regardless of the AI's intended path.

Dependencies#

This package depends on the following packages:

autoware_internal_planning_msgs: For candidate trajectory message types
autoware_planning_msgs: For output trajectory message types
autoware_motion_utils: Motion-related utility functions
autoware_trajectory: Trajectory data structures and utilities
autoware_utils: Common utility functions

Input/Output#

Input: autoware_internal_planning_msgs::msg::CandidateTrajectories
Output: Modified autoware_internal_planning_msgs::msg::CandidateTrajectories and selected autoware_planning_msgs::msg::Trajectory

Parameters#

Name	Type	Description	Default	Range
plugin_names	array	List of trajectory modifier plugins to load	['autoware::trajectory_modifier::plugin::ObstacleStop', 'autoware::trajectory_modifier::plugin::StopPointFixer']	N/A
use_stop_point_fixer	boolean	Enable the stop point fixer modifier plugin	True	N/A
use_obstacle_stop	boolean	Enable the obstacle stop modifier plugin	True	N/A
trajectory_time_step	float	Time step for trajectory modification [s]	0.1	>0.0
stop_point_fixer.force_stop_long_stopped_trajectories	boolean	Force zero velocity trajectory for trajectories with long stops	True	N/A
stop_point_fixer.force_stop_close_stopped_trajectories	boolean	Force zero velocity trajectory for trajectories with stops close to ego	True	N/A
stop_point_fixer.velocity_threshold	float	Velocity threshold below which ego vehicle is considered stationary	0.25	≥0.0
stop_point_fixer.min_distance_threshold	float	Minimum distance threshold to trigger trajectory replacement	1.0	≥0.0
stop_point_fixer.min_stop_duration	float	Minimum duration of stop to consider for trajectory replacement	0.5	≥0.0
obstacle_stop.use_objects	boolean	Enable the use of objects for obstacle stop	True	N/A
obstacle_stop.use_pointcloud	boolean	Enable the use of pointcloud for obstacle stop	True	N/A
obstacle_stop.enable_stop_for_objects	boolean	Enable stopping behavior when object is detected as obstacle	True	N/A
obstacle_stop.enable_stop_for_pointcloud	boolean	Enable stopping behavior when pointcloud is detected as obstacle	False	N/A
obstacle_stop.stop_margin	float	Distance to maintain from an obstacle when stopped [m]	6.0	≥0.0
obstacle_stop.nominal_stopping_decel	float	Nominal deceleration during stopping [m/s^2]	1.0	N/A
obstacle_stop.maximum_stopping_decel	float	Maximum deceleration during stopping [m/s^2]	4.0	N/A
obstacle_stop.stopping_jerk	float	Jerk during stopping [m/s^3]	3.0	N/A
obstacle_stop.lateral_margin	float	Lateral margin on top of ego width from trajectory to consider for obstacle stop [m]	0.5	≥0.0
obstacle_stop.arrived_distance_threshold	float	Threshold to check if the ego vehicle has arrived at the stop point [m]	0.5	≥0.0
obstacle_tracking.on_time_buffer	float	Continuous detection duration to consider object/pointcloud to be persistent [s]	0.5	≥0.0
obstacle_tracking.off_time_buffer	float	Continuous non-detection duration to consider object/pointcloud as not persistent [s]	1.0	≥0.0
obstacle_tracking.object_distance_th	float	Distance threshold for object tracking [m]	1.0	≥0.0
obstacle_tracking.object_yaw_th	float	Yaw threshold for object tracking [rad]	0.1745	≥0.0
obstacle_tracking.pcd_distance_th	float	Distance threshold for pointcloud tracking [m]	0.5	≥0.0
obstacle_tracking.grace_period	float	Grace period for object tracking [s]	0.2	≥0.0
objects.object_types	array	Object types to consider for obstacle stop	['car', 'truck', 'bus', 'trailer', 'motorcycle', 'bicycle', 'pedestrian', 'unknown']	N/A
objects.max_velocity_th	float	Maximum velocity threshold for target object [m/s]	1.0	≥0.0
pointcloud.height_buffer	float	Height buffer to add above ego height [m]	0.5	≥0.0
pointcloud.min_height	float	Minimum height of pointcloud [m]	0.2	≥0.0
voxel_grid_filter.x	float	X dimension of voxel grid filter [m]	0.2	≥0.0
voxel_grid_filter.y	float	Y dimension of voxel grid filter [m]	0.2	≥0.0
voxel_grid_filter.z	float	Z dimension of voxel grid filter [m]	0.2	≥0.0
voxel_grid_filter.min_size	float	Minimum size of voxel grid filter [m]	3	≥0.0
clustering.tolerance	float	Tolerance for clustering [m]	0.5	≥0.0
clustering.min_height	float	Minimum height for clustering [m]	0.5	≥0.0
clustering.min_size	float	Minimum size for clustering [m]	10	≥0.0
clustering.max_size	float	Maximum size for clustering [m]	10000	≥0.0
rss_params.enable	boolean	Enable RSS safety check	True	N/A
object_decel.car	float	Deceleration for car type objects [m/s^2]	1.5	≥0.0
object_decel.truck	float	Deceleration for truck type objects [m/s^2]	1.5	≥0.0
object_decel.bus	float	Deceleration for bus type objects [m/s^2]	1.5	≥0.0
object_decel.trailer	float	Deceleration for trailer type objects [m/s^2]	1.5	≥0.0
object_decel.motorcycle	float	Deceleration for motorcycle type objects [m/s^2]	3.0	≥0.0
object_decel.bicycle	float	Deceleration for bicycle type objects [m/s^2]	5.0	≥0.0
object_decel.pedestrian	float	Deceleration for pedestrian type objects [m/s^2]	5.0	≥0.0
rss_params.reaction_time	float	Reaction time to consider for RSS calculation [s]	0.2	≥0.0
rss_params.safety_margin	float	Safety margin to consider for RSS calculation [m]	2.0	≥0.0
rss_params.min_vel_th	float	Minimum velocity threshold for object to be considered for RSS safety check [m/s]	0.5	≥0.0

Parameters can be set via YAML configuration files in the config/ directory.

Adding New Modifier Plugins#

To add a new modifier plugin:

Create header and source files in trajectory_modifier_plugins/
Inherit from TrajectoryModifierPluginBase
Implement the required virtual methods
Register the plugin in the main node's initialize_modifiers() method
Add plugin-specific parameters to the schema and config files