Autoware Trajectory Modifier

Unified Node and Pipeline

The autoware_trajectory_modifier 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 TrajectoryModifier ROS 2 component. Plugins are dynamically loaded at startup, inherit from TrajectoryModifierPluginBase, 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_modifier::plugin::StopPointFixer"
  - "autoware::trajectory_modifier::plugin::ObstacleStop"
  - "autoware::trajectory_modifier::plugin::VelocityModifier"
  - "autoware::trajectory_modifier::plugin::TrajectoryPointFixer"
  - "autoware::trajectory_modifier::plugin::TrajectoryQPSmoother"
  - "autoware::trajectory_modifier::plugin::TrajectoryEBSmootherOptimizer"
  - "autoware::trajectory_modifier::plugin::TrajectorySplineSmoother"
  - "autoware::trajectory_modifier::plugin::TrajectoryVelocityOptimizer"
  - "autoware::trajectory_modifier::plugin::TrajectoryExtender"
  - "autoware::trajectory_modifier::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

File truncated at 100 lines see the full file

Changelog for package autoware_trajectory_modifier

0.53.0 (2026-09-29)

• Merge remote-tracking branch 'origin/main' into prepare-0.53.0-changelog

• fix: missing dependencies for tl_expected and libexpected-dev (#13438)

• chore(autoware_trajectory_processor)!: rename to autoware_trajectory_modifier (#13389) rename processor -> modifier

• feat(autoware_trajectory_processor): move modifier and optimizer into new package (#12934)

• feat(trajectory_modifier): support new object classes in trajectory_modifier (#12910)

  • support HAZARD & ANIMAL object classes in modifier obstacle stop

  * add missing object types for surround_obstacle_stop ---------

• feat(trajectory_modifier): add traffic light stop to trajectory modifier (#12875)

  • feat(trajectory_modifier): implement traffic light stop plugin in modifier (#3017)
  • add traffic_light_stop plugin framework
  • refactor traffic_light_compliance_checker, add crossing point and arc length to Violation struct
  • add required inputs to trajectory_modifer
  • add subscribers for lanelet_map, route, and signals
  • add lanelet_map, route, and signal ptrs to trajectory_modifier plugin InputData struct
  • update and refactory trajectory_modifier node to process new input data
  • implement is is_trajectory_modification_required function for traffic_light_stop plugin
  • refactor setting stop point logic and add utility functions to commonize logic
  • implement traffic light stopping logic
  • publish debug string
  • set default param values, update schema

  * check for empty trajectory ---------

  • feat(traffic_light_compliance_checker): add documentation for traffic light compliance checker (#3085)
  • write the readme for traffic_light_compliance_checker package
  • add unit tests for traffic light stop

  * update traffic_light_filter documentation, add docs file for traffic_light_stop module ---------

  * feat(traffic_light_stop): add missing parameter treat_unknown_light_as_red (#3091) add missing parameter treat_unknown_light_as_red

  * fix spelling ---------

• feat(trajectory_modifier): add surround obstacle stop plugin to trajectory modifier (#12894)

  • extract core logic from surround_obstacle_checker to new common package obstacle_proximit_checker
  • add new modifier plugin surround_obstacle_stop which uses common package obstacle_proximity_checker
  • apply pre-commit checks
  • refactor implementation, add integration test for surround_obstacle_stop
  • run proximity checker only once per planning cycle
  • add maintainers for new package
  • update default param values
  • update and use utility function replace_trajectory_with_stop_point
  • add readme for autoware_obstacle_proximity_checker

  * fix unit tests ---------

• Contributors: Maxime CLEMENT, Ryohsuke Mitsudome, mkquda

0.52.0 (2026-06-30)

• Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base

• fix(obstacle_stop): ignore orientation difference for cylinder objects in obstacle tracker (#12862)

File truncated at 100 lines see the full file