Velocity Smoother

Purpose

autoware_velocity_smoother outputs a desired velocity profile on a reference trajectory. This module plans a velocity profile within the limitations of the velocity, the acceleration and the jerk to realize both the maximization of velocity and the ride quality. We call this module autoware_velocity_smoother because the limitations of the acceleration and the jerk means the smoothness of the velocity profile.

Inner-workings / Algorithms

Flow chart

Extract trajectory

For the point on the reference trajectory closest to the center of the rear wheel axle of the vehicle, it extracts the reference path between extract_behind_dist behind and extract_ahead_dist ahead.

Apply external velocity limit

It applies the velocity limit input from the external of autoware_velocity_smoother. Remark that the external velocity limit is different from the velocity limit already set on the map and the reference trajectory. The external velocity is applied at the position that it is able to reach the velocity limit with the deceleration and the jerk constraints set as the parameter.

Apply stop approaching velocity

It applies the velocity limit near the stopping point. This function is used to approach near the obstacle or improve the accuracy of stopping.

Apply lateral acceleration limit

It applies the velocity limit to decelerate at the curve. For each point in the trajectory, it will find the maximum velocity, so that the lateral acceleration is under the thresholds defined by lateral_acceleration_limits and velocity_thresholds. If the trajectory speed is larger than the computed max velocity, it will try to decelerate at the curve. The velocity limit is set as not to fall under min_curve_velocity.

Note: velocity limit that requests larger than nominal.jerk is not applied. In other words, even if a sharp curve is planned just in front of the ego, no deceleration is performed.

Apply steering rate limit

It calculates the desired steering angles of trajectory points, and it applies the steering rate limit. For each point in the curve, it will find the maximum velocity that satisfy the steering rate limit defined by steering_angle_rate_limits and velocity_thresholds. If the trajectory speed is larger than the computed max velocity, it will try to decelerate at the curve.

Resample trajectory

It resamples the points on the reference trajectory with designated time interval. Note that the range of the length of the trajectory is set between min_trajectory_length and max_trajectory_length, and the distance between two points is longer than min_trajectory_interval_distance. It samples densely up to the distance traveled between resample_time with the current velocity, then samples sparsely after that. By sampling according to the velocity, both calculation load and accuracy are achieved since it samples finely at low velocity and coarsely at high velocity.

Calculate initial state

Calculate initial values for velocity planning. The initial values are calculated according to the situation as shown in the following table.

Smooth velocity

It plans the velocity. The algorithm of velocity planning is chosen from JerkFiltered, L2 and Linf, and it is set in the launch file. In these algorithms, they use OSQP[1] as the solver of the optimization.

JerkFiltered

It minimizes the sum of the minus of the square of the velocity and the square of the violation of the velocity limit, the acceleration limit and the jerk limit.

L2

It minimizes the sum of the minus of the square of the velocity, the square of the the pseudo-jerk[2] and the square of the violation of the velocity limit and the acceleration limit.

Linf

It minimizes the sum of the minus of the square of the velocity, the maximum absolute value of the the pseudo-jerk[2] and the square of the violation of the velocity limit and the acceleration limit.

Post process

It performs the post-process of the planned velocity.

• Set zero velocity ahead of the stopping point
• Set maximum velocity given in the config named max_velocity
• Set velocity behind the current pose
• Resample trajectory (post resampling)
• Output debug data

After the optimization, a resampling called post resampling is performed before passing the optimized trajectory to the next node. Since the required path interval from optimization may be different from the one for the next module, post resampling helps to fill this gap. Therefore, in post resampling, it is necessary to check the path specification of the following module to determine the parameters. Note that if the computational load of the optimization algorithm is high and the path interval is sparser than the path specification of the following module in the first resampling, post resampling would resample the trajectory densely. On the other hand, if the computational load of the optimization algorithm is small and the path interval is denser than the path specification of the following module in the first resampling, the path is sparsely resampled according to the specification of the following module.

Inputs / Outputs

Input

File truncated at 100 lines see the full file

Changelog for package autoware_velocity_smoother

1.1.0 (2025-05-01)

1.10.0 (2026-09-28)

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

• test(autoware_velocity_smoother): add a new unit test suite for jerk smoother (#1468)

• fix(design): align the autoware_core node designs with the packages they describe (#1416)

  * fix(autoware_velocity_smoother): correct the velocity limit message type in the node design The node publishes current_velocity_limit_mps as autoware_internal_planning_msgs/msg/VelocityLimit (node.hpp), while the design declared the pre-migration tier4_planning_msgs type, failing the connection check against downstream ports.

  • fix(autoware_motion_velocity_planner): correct the velocity limit message types and drop absent publishers in the node design

  * fix(autoware_path_generator): correct the path publisher message type in the node design The node publishes autoware_internal_planning_msgs/msg/PathWithLaneId on ~/output/path (node.hpp:84), not autoware_planning_msgs/msg/Path.

  * fix(autoware_velocity_smoother): declare the kinematic state subscriber in the node design The node polls /localization/kinematic_state for the ego odometry (node.hpp:94-97); the design did not list the input at all.

  * fix(autoware_motion_velocity_planner): name the module-side subscriber topics in the node design The boundary departure prevention module subscribes on absolute topic names, so the default ~/input/<name> remap targets addressed topics the node never opens and the module connections resolved to nothing. Declare the real names and add the steering status input the module also takes.

  * fix(autoware_gnss_poser): declare the map projector info subscriber in the node design The node blocks pose conversion until /map/map_projector_info arrives (gnss_poser_node.cpp:43); the design did not list the input at all.

  * fix(autoware_behavior_velocity_planner): add additional planning factors to publishers in the node design ---------

• fix(autoware_velocity_smoother): use the mode-agnostic ok() so that resampling runs under Agnocast (#1418)

• fix(planning): declare the dependencies these packages use (#1372) Each of these packages uses a package it never declares. Either it includes a header of that package, or it names a symbol of it while the header arrives through another dependency. Both build today only because some declared dependency re-exports the owner, so a change in an unrelated repository can break them without anything here changing. The tag follows where the dependency is used: a use in an installed header or in code compiled into the library takes <depend>, one reached only from test/ takes <test_depend>. System libraries are named by the rosdep key this workspace already prefers. A clean-context review of the pull request found five more direct uses with no manifest entry. Add one entry for each:

  • autoware_path_generator: tf2 (tf2::getYaw in src/utils.cpp)
  • autoware_motion_velocity_planner_common: tf2 (tf2::getYaw in src/planner_data.cpp and src/polygon_utils.cpp)
  • autoware_motion_velocity_obstacle_stop_module: autoware_planning_factor_interface (constructed in src/obstacle_stop_module.cpp)
  • autoware_behavior_velocity_stop_line_module: autoware_planning_factor_interface (used in src/experimental/scene.cpp)
  • autoware_velocity_smoother: rclcpp_components (register_node_macro.hpp in src/node.cpp)

• refactor: migrate node design files from autoware_universe (#1381) Node design files for packages that moved to autoware_core, placed at the in-package convention <package>/design/<Name>.node.yaml. Co-authored-by: Claude Fable 5 <<noreply@anthropic.com>>

• feat(velocity_smoother): apply agnocast_wrapper::Node to autoware_velocity_smoother (#1264)

  • feat(autoware_velocity_smoother): apply agnocast_wrapper::Node and migrate polling to agnocast_wrapper::polling:: API
  • refactor: subscriber
  • style(pre-commit): autofix
  • fix: skip test in cmake
  • refatcor: move smoother constructor definitions back to cpp
  • fix: build

File truncated at 100 lines see the full file