Autoware Trajectory Processor

Unified Node and Pipeline

The autoware_trajectory_processor package runs safety modifiers and trajectory optimizers in one ordered plugin pipeline. It takes candidate trajectories as input, applies modifier plugins first and optimizer plugins second by default, and publishes both the processed candidates and the first candidate as the selected trajectory.

Features

• Plugin-based architecture - Modular optimization pipeline where each step is a separate plugin
• Multiple smoothing methods:
  • Elastic Band (EB) smoother for path optimization
  • Akima spline interpolation for smooth path interpolation
  • QP-based smoother with quadratic programming for path smoothing with jerk constraints
• Velocity optimization - Jerk-filtered velocity smoothing from autoware_velocity_smoother
• Temporal MPT optimization - acados MPC (8 s / 0.1 s horizon) on a kinematic bicycle with x, y, yaw, and speed states
• Trajectory validation - Removes invalid points and fixes trajectory orientation
• Backward trajectory extension - Extends trajectory using past ego states
• Dynamic parameter reconfiguration - Runtime parameter updates supported

Architecture

The package uses one TrajectoryProcessor ROS 2 component. Plugins are dynamically loaded at startup, inherit from TrajectoryProcessorPluginBase, and execute in the exact order configured by plugin_names.

Plugin Loading and Execution

Plugins are loaded based on the plugin_names parameter, which defines both which plugins to load and their execution order:

plugin_names:
  - "autoware::trajectory_processor::plugin::StopPointFixer"
  - "autoware::trajectory_processor::plugin::ObstacleStop"
  - "autoware::trajectory_processor::plugin::VelocityModifier"
  - "autoware::trajectory_processor::plugin::TrajectoryPointFixer"
  - "autoware::trajectory_processor::plugin::TrajectoryQPSmoother"
  - "autoware::trajectory_processor::plugin::TrajectoryEBSmootherOptimizer"
  - "autoware::trajectory_processor::plugin::TrajectorySplineSmoother"
  - "autoware::trajectory_processor::plugin::TrajectoryVelocityOptimizer"
  - "autoware::trajectory_processor::plugin::TrajectoryExtender"
  - "autoware::trajectory_processor::plugin::TrajectoryPointFixer"

Available Plugins

1. TrajectoryPointFixer - Removes invalid/repeated points and fixes trajectory direction
2. TrajectoryQPSmoother - QP-based path smoothing with jerk constraints
3. TrajectoryEBSmootherOptimizer - Elastic Band path smoothing
4. TrajectorySplineSmoother - Akima spline interpolation
5. TrajectoryMPTOptimizer - Model predictive trajectory optimization with adaptive corridor bounds. Uses bicycle kinematics model for trajectory refinement. Disabled by default (experimental). See docs/mpt_optimizer.md for details.
6. TrajectoryTemporalMPTOptimizer - Temporal acados MPC path tracking on a time-ordered reference (8.0 s horizon, 0.1 s discretization). Must be listed in plugin_names and enabled via use_temporal_mpt_optimizer. See docs/temporal_mpt_optimizer.md for details.
7. TrajectoryVelocityOptimizer - Velocity profile optimization with lateral acceleration limits
8. TrajectoryExtender - Extends trajectory backward using past ego states
9. TrajectoryKinematicFeasibilityEnforcer - Enforces Ackermann steering and yaw rate constraints

Each plugin can be enabled/disabled at runtime via activation flags (e.g., use_qp_smoother) and manages its own configuration independently.

⚠️ Important: Plugin Ordering Constraints

The order of plugin execution is critical and must be carefully maintained:

• QP Smoother must run before EB/Akima smoothers: The QP solver relies on constant time intervals (Δt) between trajectory points (default: 0.1s). Both Elastic Band and Akima spline smoothers resample trajectories without preserving the time domain structure, which breaks the QP solver's assumptions. Therefore, when using multiple smoothers together, the QP smoother must execute first.

• Trajectory Extender positioning: The trajectory extender has known discontinuity issues when placed early in the pipeline. It negatively affects the QP solver results and introduces artifacts. For this reason, it has been moved to near the end of the pipeline and is disabled by default (use_trajectory_extender: false). Fixing the extender's discontinuity issues is future work.

• Temporal MPT vs spatial MPT: TrajectoryTemporalMPTOptimizer is independent of TrajectoryMPTOptimizer (different solver, no corridor bounds). It can replace the entire post-TrajectoryPointFixer plugin chain (kinematic enforcer, QP/EB/spline smoothers, velocity optimizer, spatial MPT) with a single MPC step; see docs/temporal_mpt_optimizer.md. It overwrites at most the first 81 points (8.0 s at 0.1 s spacing).

Design Specification

For a detailed description of the optimizer's core assumptions, the constant-dt contract, how velocity and acceleration are derived from positions, plugin pipeline specification, and known limitations, see docs/trajectory_optimizer_specification.md.

QP Smoother

The QP smoother uses quadratic programming (OSQP solver) to optimize trajectory paths with advanced features:

• Objective: Minimizes path curvature while maintaining fidelity to the original trajectory
• Decision variables: Path positions (x, y) for each trajectory point
• Constraints: Fixed initial position (optionally fixed last position)
• Velocity-based fidelity: Automatically reduces fidelity weight at low speeds for aggressive smoothing of noise
• Post-processing: Recalculates velocities, accelerations, and orientations from smoothed positions

For detailed documentation, see docs/qp_smoother.md which covers:

• Mathematical formulation
• Velocity-based fidelity weighting (sigmoid function)
• Parameter tuning guidelines
• Usage examples
• Performance characteristics

Dependencies

• acados - Required to build the temporal MPT solver (acados_interface_temporal)
• autoware_motion_utils - Trajectory manipulation utilities
• autoware_osqp_interface - QP solver interface for QP smoother
• autoware_path_smoother - Elastic Band smoother
• autoware_velocity_smoother - Velocity smoothing algorithms
• autoware_utils - Common utilities (geometry, ROS helpers)
• autoware_vehicle_info_utils - Vehicle information

Parameters

Name	Type	Description	Default	Range
plugin_names	array	Ordered list of modifier and optimizer plugin class names	["autoware::trajectory_processor::plugin::StopPointFixer", "autoware::trajectory_processor::plugin::SurroundObstacleStop", "autoware::trajectory_processor::plugin::ObstacleStop", "autoware::trajectory_processor::plugin::TrafficLightStop", "autoware::trajectory_processor::plugin::VelocityModifier", "autoware::trajectory_processor::plugin::TrajectoryPointFixer", "autoware::trajectory_processor::plugin::TrajectoryKinematicFeasibilityEnforcer", "autoware::trajectory_processor::plugin::TrajectoryQPSmoother", "autoware::trajectory_processor::plugin::TrajectoryKinematicFeasibilityEnforcer", "autoware::trajectory_processor::plugin::TrajectoryVelocityOptimizer", "autoware::trajectory_processor::plugin::TrajectoryEBSmootherOptimizer", "autoware::trajectory_processor::plugin::TrajectorySplineSmoother", "autoware::trajectory_processor::plugin::TrajectoryMPTOptimizer", "autoware::trajectory_processor::plugin::TrajectoryExtender"]	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
use_surround_obstacle_stop	boolean	Enable the surround obstacle stop modifier plugin	false	N/A
use_traffic_light_stop	boolean	Enable the traffic light stop modifier plugin	true	N/A
use_velocity_modifier	boolean	Enable the velocity modifier plugin	true	N/A
trajectory_time_step	float	Time step for trajectory modification [s]	0.1	>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
stop_point_fixer.min_distance_threshold	float	Minimum distance threshold to trigger trajectory replacement	1	≥0
stop_point_fixer.min_stop_duration	float	Minimum duration of stop to consider for trajectory replacement	0.5	≥0
stopping_constraints.nominal_deceleration	float	Nominal deceleration during stopping [m/s^2]	1	≥0
stopping_constraints.maximum_deceleration	float	Maximum deceleration during stopping [m/s^2]	4	≥0
stopping_constraints.jerk_limit	float	Jerk limit during stopping [m/s^3]	3	≥0
stopping_constraints.arrived_distance_threshold	float	Threshold to check if the ego vehicle has arrived at the stop point [m]	0.5	≥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
obstacle_stop.minimum_stop_margin	float	Minimum distance to maintain from an obstacle when stopped [m]	3.0	≥0.0
obstacle_stop.lateral_margin	float	Lateral margin on top of ego width from trajectory to consider for obstacle stop [m]	0.5	≥0
obstacle_stop.duplicate_check_threshold	float	Threshold to check if the stop point is already in the trajectory [m]	0.5	≥0
obstacle_stop.obstacle_tracking.on_time_buffer	float	Continuous detection duration to consider object/pointcloud to be persistent [s]	0.5	≥0
obstacle_stop.obstacle_tracking.off_time_buffer	float	Continuous non-detection duration to consider object/pointcloud as not persistent [s]	1	≥0
obstacle_stop.obstacle_tracking.object_distance_th	float	Distance threshold for object tracking [m]	1	≥0
obstacle_stop.obstacle_tracking.object_yaw_th	float	Yaw threshold for object tracking [rad]	0.1745	≥0
obstacle_stop.obstacle_tracking.pcd_distance_th	float	Distance threshold for pointcloud tracking [m]	1	≥0
obstacle_stop.obstacle_tracking.grace_period	float	Grace period for object tracking [s]	0.2	≥0
obstacle_stop.objects.target_objects.bbox	array	Types of bounding box shaped objects to consider for obstacle stop	["car", "truck", "bus", "trailer", "motorcycle", "bicycle", "pedestrian", "hazard", "unknown"]	N/A
obstacle_stop.objects.target_objects.polygon	array	Types of polygon shaped objects to consider for obstacle stop	["car", "truck", "bus", "trailer", "pedestrian", "hazard", "unknown"]	N/A
obstacle_stop.objects.stopped_velocity_th	float	Velocity threshold for target object to be considered as stopped [m/s]	0.25	≥0
obstacle_stop.objects.max_lateral_velocity_th	float	Maximum lateral velocity threshold for target object to be considered for obstacle stop [m/s]	1	≥0
obstacle_stop.objects.safety_buffer	float	Safety buffer to expand object shape [m]	0	≥0
obstacle_stop.pointcloud.height_buffer	float	Height buffer to add above ego height [m]	0.5	≥0
obstacle_stop.pointcloud.min_height	float	Minimum height of pointcloud [m]	0.2	≥0
obstacle_stop.pointcloud.voxel_grid_filter.x	float	X dimension of voxel grid filter [m]	0.2	≥0
obstacle_stop.pointcloud.voxel_grid_filter.y	float	Y dimension of voxel grid filter [m]	0.2	≥0
obstacle_stop.pointcloud.voxel_grid_filter.z	float	Z dimension of voxel grid filter [m]	0.2	≥0
obstacle_stop.pointcloud.voxel_grid_filter.min_size	float	Minimum size of voxel grid filter [m]	3	≥0
obstacle_stop.pointcloud.clustering.tolerance	float	Tolerance for clustering [m]	0.5	≥0
obstacle_stop.pointcloud.clustering.min_height	float	Minimum height for clustering [m]	0.5	≥0
obstacle_stop.pointcloud.clustering.min_size	float	Minimum size for clustering [m]	10	≥0
obstacle_stop.pointcloud.clustering.max_size	float	Maximum size for clustering [m]	10000	≥0
obstacle_stop.rss_params.enable	boolean	Enable RSS safety check	true	N/A
obstacle_stop.rss_params.object_decel.car	float	Deceleration for car type objects [m/s^2]	1.5	≥0
obstacle_stop.rss_params.object_decel.truck	float	Deceleration for truck type objects [m/s^2]	1.5	≥0
obstacle_stop.rss_params.object_decel.bus	float	Deceleration for bus type objects [m/s^2]	1.5	≥0
obstacle_stop.rss_params.object_decel.trailer	float	Deceleration for trailer type objects [m/s^2]	1.5	≥0
obstacle_stop.rss_params.object_decel.motorcycle	float	Deceleration for motorcycle type objects [m/s^2]	3	≥0
obstacle_stop.rss_params.object_decel.bicycle	float	Deceleration for bicycle type objects [m/s^2]	5	≥0
obstacle_stop.rss_params.object_decel.pedestrian	float	Deceleration for pedestrian type objects [m/s^2]	5	≥0
obstacle_stop.rss_params.object_decel.animal	float	Deceleration for animal type objects [m/s^2]	5	≥0
obstacle_stop.rss_params.ego_decel	float	Deceleration for ego vehicle [m/s^2]	4	≥0
obstacle_stop.rss_params.reaction_time	float	Reaction time to consider for RSS calculation [s]	0.2	≥0
obstacle_stop.rss_params.safety_margin	float	Safety margin to consider for RSS calculation [m]	2	≥0
obstacle_stop.rss_params.lookahead_horizon	float	Lookahead horizon for RSS calculation [s]	1.5	≥0
surround_obstacle_stop.use_objects	boolean	Enable the use of objects for surround obstacle stop	true	N/A
surround_obstacle_stop.use_pointcloud	boolean	Enable the use of pointcloud for surround obstacle stop	true	N/A
surround_obstacle_stop.target_objects.bbox	array	Types of bounding box shaped objects to consider for surround obstacle stop	["car", "truck", "bus", "trailer", "motorcycle", "bicycle", "pedestrian", "hazard", "animal", "unknown"]	N/A
surround_obstacle_stop.target_objects.polygon	array	Types of polygon shaped objects to consider for surround obstacle stop	["car", "truck", "bus", "trailer", "pedestrian", "hazard", "animal", "unknown"]	N/A
surround_obstacle_stop.front_distance_th.car	float		0.5	≥0
surround_obstacle_stop.front_distance_th.truck	float		0.5	≥0
surround_obstacle_stop.front_distance_th.bus	float		0.5	≥0
surround_obstacle_stop.front_distance_th.trailer	float		0.5	≥0
surround_obstacle_stop.front_distance_th.motorcycle	float		0.5	≥0
surround_obstacle_stop.front_distance_th.bicycle	float		0.5	≥0
surround_obstacle_stop.front_distance_th.pedestrian	float		0.5	≥0
surround_obstacle_stop.front_distance_th.hazard	float		0.5	≥0
surround_obstacle_stop.front_distance_th.animal	float		0.5	≥0
surround_obstacle_stop.front_distance_th.unknown	float		0.5	≥0
surround_obstacle_stop.front_distance_th.pointcloud	float		0.5	≥0
surround_obstacle_stop.side_distance_th.car	float		0	≥0
surround_obstacle_stop.side_distance_th.truck	float		0	≥0
surround_obstacle_stop.side_distance_th.bus	float		0	≥0
surround_obstacle_stop.side_distance_th.trailer	float		0	≥0
surround_obstacle_stop.side_distance_th.motorcycle	float		1	≥0
surround_obstacle_stop.side_distance_th.bicycle	float		1	≥0
surround_obstacle_stop.side_distance_th.pedestrian	float		1	≥0
surround_obstacle_stop.side_distance_th.hazard	float		0.5	≥0
surround_obstacle_stop.side_distance_th.animal	float		0.5	≥0
surround_obstacle_stop.side_distance_th.unknown	float		0.5	≥0
surround_obstacle_stop.side_distance_th.pointcloud	float		0.5	≥0
surround_obstacle_stop.back_distance_th.car	float		0	≥0
surround_obstacle_stop.back_distance_th.truck	float		0	≥0
surround_obstacle_stop.back_distance_th.bus	float		0	≥0
surround_obstacle_stop.back_distance_th.trailer	float		0	≥0
surround_obstacle_stop.back_distance_th.motorcycle	float		0.5	≥0
surround_obstacle_stop.back_distance_th.bicycle	float		0.5	≥0
surround_obstacle_stop.back_distance_th.pedestrian	float		0.5	≥0
surround_obstacle_stop.back_distance_th.hazard	float		0.5	≥0
surround_obstacle_stop.back_distance_th.animal	float		0.5	≥0
surround_obstacle_stop.back_distance_th.unknown	float		0.5	≥0
surround_obstacle_stop.back_distance_th.pointcloud	float		0.5	≥0
surround_obstacle_stop.hysteresis_distance	float	Distance hysteresis threshold for clearing stop [m]	0.1	≥0
surround_obstacle_stop.hysteresis_time	float	Time hysteresis threshold for clearing stop [s]	0.2	≥0
surround_obstacle_stop.ego_stopped_vel_th	float	Velocity threshold below which ego vehicle is considered stopped [m/s]	0.1	≥0
traffic_light_stop.stop_margin	float	Distance to maintain from a traffic light when stopped [m]	0.75	≥0
traffic_light_stop.stop_for_red_light	boolean	Enable stopping behavior when red light is detected	true	N/A
traffic_light_stop.stop_for_amber_light	boolean	Enable stopping behavior when amber light is detected	true	N/A
traffic_light_stop.treat_amber_light_as_red	boolean	Treat amber light as red light	false	N/A
traffic_light_stop.treat_unknown_light_as_red	boolean	Treat unknown light as red light	false	N/A
traffic_light_stop.overshoot_tolerance	float	Overshoot tolerance [m]	0	≥0
traffic_light_stop.allow_if_cannot_stop_distance	float	allow crossing a stop line within this distance if ego cannot stop before the line plus its margin [m]	0	≥0
traffic_light_stop.th_stable_duration_red	float	Threshold for stable duration of red light [s]	0.2	≥0
traffic_light_stop.th_stable_duration_amber	float	Threshold for stable duration of amber light [s]	0.2	≥0
traffic_light_stop.th_stable_duration_unknown	float	Threshold for stable duration of unknown light [s]	0	≥0
traffic_light_stop.th_amber_rejection_hysteresis	float	Threshold for amber rejection hysteresis [s]	0.5	≥0
traffic_light_stop.crossing_time_limit	float	Crossing time limit [s]	2.75	≥0
traffic_light_stop.delay_response_time	float	Delay response time used to estimate the minimum ego stopping distance [s]	0.5	≥0
traffic_light_stop.ego_stopped_vel_th	float	Velocity below which ego is considered stopped when checking traffic lights [m/s]	0.01	≥0
use_akima_spline_interpolation	boolean	Enable Akima spline interpolation for trajectory smoothing	false	N/A
use_eb_smoother	boolean	Enable Elastic Band smoother for trajectory smoothing	false	N/A
use_qp_smoother	boolean	Enable QP-based trajectory smoothing with explicit jerk constraints	true	N/A
use_trajectory_point_fixer	boolean	Remove repeated or invalid points, or points that go against the general trajectory direction	true	N/A
use_trajectory_extender	boolean	Enable trajectory extender to extend trajectory backward using ego pose history	false	N/A
use_velocity_optimizer	boolean	Enable velocity profile optimization with jerk filtering and constraint enforcement	true	N/A
use_kinematic_feasibility_enforcer	boolean	Enable kinematic feasibility enforcer plugin (Ackermann + yaw rate constraints)	false	N/A
use_mpt_optimizer	boolean	Enable MPT (Model Predictive Trajectory) optimizer plugin for geometry refinement with adaptive bounds	false	N/A
use_temporal_mpt_optimizer	boolean	Enable Temporal MPT optimizer plugin (acados-based kinematic bicycle MPC for combined geometry and velocity optimization)	false	N/A
max_vel	float	Default maximum velocity used when no external velocity limit is available [m/s]	11.1	≥0
limit.max_acc	float	Maximum acceleration limit for velocity smoothing [m/s^2]	1	≥0
limit.min_acc	float	Minimum acceleration limit for velocity smoothing [m/s^2]	-2.5	≤0
limit.max_jerk	float	Maximum jerk limit for velocity smoothing [m/s^3]	1.5	≥0
limit.min_jerk	float	Minimum jerk limit for velocity smoothing [m/s^3]	-1.5	≤0
trajectory_point_fixer.remove_close_points	boolean	Enable removal of close proximity points	true	N/A
trajectory_point_fixer.resample_close_points	boolean	Enable resampling of close proximity points	true	N/A
trajectory_point_fixer.min_dist_to_remove_m	float	Minimum distance threshold for removing close proximity points [m]	0.01	≥0
trajectory_point_fixer.min_dist_to_resample_m	float	Minimum distance threshold for merging close proximity points [m]	0.05	≥0
trajectory_point_fixer.stop_detection_velocity_threshold_mps	float	Velocity threshold used by the stop-detection logic: points whose speed falls below this value while decelerating are treated as stop candidates by both the cluster-based and velocity-profile-based detectors [m/s]	0.3	≥0
trajectory_kinematic_feasibility.max_yaw_rate_rad_s	float	Maximum yaw rate [rad/s]. Default: 0.7 rad/s (~40 deg/s) aligns with MPC controller. Typical range: 0.5-1.0 for autonomous vehicles.	0.7	≥0.1 ≤2
trajectory_kinematic_feasibility.time_step_s	float	Time step for kinematic feasibility enforcement [s]. Default: 0.1 s.	0.1	≥0.01 ≤1
trajectory_mpt_optimizer.corridor_width_m	float	Base corridor width for adaptive bounds generation [m]. Defines the lateral distance from trajectory centerline to left/right bounds.	3.5	≥2 ≤10
trajectory_mpt_optimizer.enable_adaptive_width	boolean	Enable adaptive corridor width based on curvature and velocity. When true, corridor widens in curves and at low speeds.	true	N/A
trajectory_mpt_optimizer.curvature_width_factor	float	Factor for curvature-based corridor widening [m/rad]. Higher values provide more space in curves.	0.5	≥0 ≤2
trajectory_mpt_optimizer.velocity_width_factor	float	Factor for velocity-based corridor widening [m]. Higher values provide more maneuvering room at low speeds.	0.3	≥0 ≤2
trajectory_mpt_optimizer.min_clearance_m	float	Minimum clearance from vehicle edges to corridor bounds [m]. Ensures minimum safe distance.	0.5	≥0 ≤2
trajectory_mpt_optimizer.reset_previous_data_each_iteration	boolean	Reset MPT's previous optimization data each cycle. Should be true for diffusion planner (new trajectories each cycle), false for incremental planners.	true	N/A
trajectory_mpt_optimizer.enable_debug_info	boolean	Enable debug visualization markers for bounds and reference trajectory.	false	N/A
trajectory_mpt_optimizer.output_delta_arc_length_m	float	Spacing between trajectory points in MPT optimization [m]. Smaller values provide finer resolution but increase computation.	1	≥0.1 ≤5
trajectory_mpt_optimizer.output_backward_traj_length_m	float	Length of trajectory extension backward from ego pose [m].	5	≥0 ≤20
trajectory_mpt_optimizer.ego_nearest_dist_threshold_m	float	Distance threshold for finding nearest trajectory point to ego [m].	3	≥0.5 ≤10
trajectory_mpt_optimizer.ego_nearest_yaw_threshold_deg	float	Yaw threshold for finding nearest trajectory point to ego [degrees].	45	≥10 ≤90
trajectory_mpt_optimizer.acceleration_moving_average_window	integer	Moving average window size for acceleration smoothing. Larger values produce smoother acceleration but increase lag. Window of 1 = no smoothing.	5	≥1 ≤20
trajectory_temporal_mpt_optimizer.cg_distance_from_rear_axle_ratio	float	Ratio of wheel base from rear axle to bicycle center of gravity. lr = ratio * wheel_base, lf = wheel_base - lr.	0.8	≥0 ≤1
trajectory_temporal_mpt_optimizer.min_points_for_optimization	integer	Minimum number of trajectory points required to run temporal MPT optimization.	2	≥2 ≤1000
trajectory_temporal_mpt_optimizer.enable_debug_info	boolean	Enable debug logging for temporal MPT solver failures.	false	N/A
trajectory_temporal_mpt_optimizer.publish_debug_topics	boolean	Publish temporal MPT input, output, status, and control debug topics.	false	N/A
trajectory_temporal_mpt_optimizer.write_replay_fixture	boolean	Write replay fixture files when temporal MPT solving fails.	false	N/A
trajectory_temporal_mpt_optimizer.replay_fixture_directory	string	Directory used for temporal MPT replay fixture files.	~/.ros/autoware_temporal_mpt_replay	N/A
trajectory_temporal_mpt_optimizer.log_replay_fixture_to_console	boolean	Log temporal MPT replay fixture contents to console when solving fails.	false	N/A
trajectory_extender.nearest_dist_threshold_m	float	Distance threshold for trajectory matching [m]	1.5	≥0
trajectory_extender.nearest_yaw_threshold_deg	float	Yaw threshold for trajectory matching [deg]	60	≥0
trajectory_extender.backward_trajectory_extension_m	float	Length to extend trajectory backward using ego history [m]	5	≥0
trajectory_spline_smoother.interpolation_resolution_m	float	Interpolation resolution for Akima spline [m]	0.5	>0
trajectory_spline_smoother.max_distance_discrepancy_m	float	Maximum position deviation allowed for orientation copying [m]	5	≥0
trajectory_spline_smoother.preserve_input_trajectory_orientation	boolean	Copy orientations from input trajectory to spline output	true	N/A
trajectory_qp_smoother.weight_smoothness	float	Weight for path curvature minimization (geometric smoothness) [dimensionless]	10	≥0
trajectory_qp_smoother.weight_fidelity	float	Weight for path fidelity (staying close to original path) [dimensionless]	1	≥0
trajectory_qp_smoother.time_step_s	float	Fixed time step for velocity/acceleration calculations [s]	0.1	>0
trajectory_qp_smoother.osqp_eps_abs	float	OSQP absolute convergence tolerance	0.0001	>0
trajectory_qp_smoother.osqp_eps_rel	float	OSQP relative convergence tolerance	0.0001	>0
trajectory_qp_smoother.osqp_max_iter	integer	OSQP maximum solver iterations	100	≥1
trajectory_qp_smoother.osqp_verbose	boolean	Enable OSQP verbose solver output	false	N/A
trajectory_qp_smoother.preserve_input_trajectory_orientation	boolean	Copy orientations from input trajectory to smoothed output	true	N/A
trajectory_qp_smoother.max_distance_for_orientation_m	float	Max distance for nearest neighbor matching when copying orientations [m]	5	≥0
trajectory_qp_smoother.use_velocity_based_fidelity	boolean	Enable velocity-dependent fidelity weighting (lower fidelity at low speeds)	true	N/A
trajectory_qp_smoother.velocity_threshold_mps	float	Velocity threshold at sigmoid midpoint for velocity-based fidelity [m/s]	0.3	≥0
trajectory_qp_smoother.sigmoid_sharpness	float	Sigmoid steepness for velocity-based fidelity transition (higher = sharper)	50	≥1
trajectory_qp_smoother.min_fidelity_weight	float	Minimum fidelity weight at very low speeds [dimensionless]	0.01	≥0
trajectory_qp_smoother.max_fidelity_weight	float	Maximum fidelity weight at high speeds [dimensionless]	1	≥0
trajectory_qp_smoother.num_constrained_points_start	integer	Number of points from the start of the trajectory to constrain as hard constraints (preserves initial state)	3	≥0
trajectory_qp_smoother.num_constrained_points_end	integer	Number of points from the end of the trajectory to constrain as hard constraints	3	≥0
trajectory_velocity_optimizer.nearest_dist_threshold_m	float	Distance threshold for trajectory matching [m]	1.5	≥0
trajectory_velocity_optimizer.nearest_yaw_threshold_deg	float	Yaw threshold for trajectory matching [deg]	60	≥0
trajectory_velocity_optimizer.target_pull_out_speed_mps	float	Target speed during pull-out maneuver [m/s]	1	≥0
trajectory_velocity_optimizer.target_pull_out_acc_mps2	float	Target acceleration during pull-out maneuver [m/s²]	1	≥0
trajectory_velocity_optimizer.max_lateral_accel_mps2	float	Maximum lateral acceleration [m/s²]	1.5	≥0
trajectory_velocity_optimizer.min_limited_speed_mps	float	Minimum speed when applying lateral acceleration limit [m/s]	3	≥0
trajectory_velocity_optimizer.set_engage_speed	boolean	Set minimum speed during engage	false	N/A
trajectory_velocity_optimizer.limit_speed	boolean	Enable speed limiting	true	N/A
trajectory_velocity_optimizer.limit_lateral_acceleration	boolean	Enable lateral acceleration limiting	false	N/A
trajectory_velocity_optimizer.smooth_velocities	boolean	Enable jerk-filtered velocity smoothing	false	N/A
trajectory_velocity_optimizer.continuous_jerk_smoother.jerk_weight	float	Weight for jerk minimization in velocity smoothing	30	≥0
trajectory_velocity_optimizer.continuous_jerk_smoother.over_v_weight	float	Weight for velocity limit violation slack	3000	≥0
trajectory_velocity_optimizer.continuous_jerk_smoother.over_a_weight	float	Weight for acceleration limit violation slack	30	≥0
trajectory_velocity_optimizer.continuous_jerk_smoother.over_j_weight	float	Weight for jerk limit violation slack	10	≥0
trajectory_velocity_optimizer.continuous_jerk_smoother.velocity_tracking_weight	float	Weight for tracking reference velocity	1	≥0
trajectory_velocity_optimizer.continuous_jerk_smoother.accel_tracking_weight	float	Weight for tracking reference acceleration	300	≥0
elastic_band_params.common.output_delta_arc_length	float	Delta arc length for output trajectory [m]	0.5	>0
elastic_band_params.common.output_backward_traj_length	float	Backward length for backward trajectory from base_link [m]	5	≥0
elastic_band_params.elastic_band.option.enable_warm_start	boolean	Enable warm start for optimization	true	N/A
elastic_band_params.elastic_band.option.enable_optimization_validation	boolean	Enable optimization validation	false	N/A
elastic_band_params.elastic_band.common.num_points	integer	Number of points for optimization	100	≥1
elastic_band_params.elastic_band.common.delta_arc_length	float	Delta arc length for optimization [m]	1	>0
elastic_band_params.elastic_band.weight.smooth_weight	float	Weight for smoothness constraint	1	≥0
elastic_band_params.elastic_band.weight.lat_error_weight	float	Weight for lateral error constraint	0.001	≥0
elastic_band_params.ego_nearest_dist_threshold	float	Distance threshold for ego nearest search [m]	3	≥0
elastic_band_params.ego_nearest_yaw_threshold	float	Yaw threshold for ego nearest search [rad]	1.046	≥0

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

Parameter Types

1. Plugin Loading (plugin_names) - Array of plugin class names determining load order and execution sequence
2. Activation Flags - Boolean flags for runtime enable/disable (e.g., use_qp_smoother, use_temporal_mpt_optimizer)
3. Plugin-Specific Parameters - Namespaced parameters for each plugin (e.g., trajectory_qp_smoother.weight_smoothness)

Configuring Plugin Order

To change plugin execution order, modify the plugin_names array in config/trajectory_processor.param.yaml:

# Example: Run spline smoother before velocity optimizer
plugin_names:
  - "autoware::trajectory_processor::plugin::TrajectoryPointFixer"
  - "autoware::trajectory_processor::plugin::TrajectorySplineSmoother"
  - "autoware::trajectory_processor::plugin::TrajectoryVelocityOptimizer"
  - "autoware::trajectory_processor::plugin::TrajectoryPointFixer"

CRITICAL: QP Smoother Ordering Constraint

The TrajectoryQPSmoother plugin MUST run before any plugins that resample or modify trajectory structure:

• TrajectorySplineSmoother (Akima spline - resamples trajectory)
• TrajectoryEBSmootherOptimizer (Elastic Band - resamples trajectory)
• TrajectoryVelocityOptimizer (velocity smoothing with resampling)
• TrajectoryExtender (adds/modifies points at trajectory start)

The QP solver requires constant time intervals (Δt = 0.1s) between points. These plugins modify the time domain structure or add points, breaking the QP solver assumptions. If you need QP smoothing, it must appear first in the pipeline after TrajectoryPointFixer.

Note: Plugin order changes require node restart. Runtime enable/disable is controlled by activation flags.

Modifier Plugins

Modifier plugins run in the same node and ordered pipeline as optimizer plugins. They post-process candidate trajectories to improve safety before the optimizer stage smooths and validates the result.

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

Modifier and optimizer algorithms use the same plugin interface. Each plugin inherits from TrajectoryProcessorPluginBase.

Plugin Interface

All modifier plugins must inherit from TrajectoryProcessorPluginBase and implement:

• process() - Process trajectory points and return ProcessingResult::Modified or ProcessingResult::Unchanged
• on_initialize() - Initialize plugin members and parameters
• update_params() - Handle parameter updates

is_trajectory_modification_required() may be retained as a plugin-local helper, but is not part of the common interface.

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 For Obstacles: Guarantees zero-velocity set-points for stationary objects (e.g., traffic lights, stopped vehicles) to prevent "creeping" or oscillating behavior near obstacles.

Velocity Modifier

The Velocity Modifier plugins is responsible for ensuring the velocity profile is smooth and feasible, and adjusts the velocity profile if an anomaly is detected while respecting deceleration and jerk constraints.

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

See the unified parameter schema above and config/trajectory_processor.param.yaml.

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

Adding New Modifier Plugins

To add a new modifier plugin:

1. Create header and source files in trajectory_modifier_plugins/
2. Inherit from TrajectoryProcessorPluginBase
3. Implement the required virtual methods
4. Export the plugin with TrajectoryProcessorPluginBase and register it in plugins.xml
5. Add plugin-specific parameters to the schema and config files