Trajectory Follower Nodes

Purpose

Generate control commands to follow a given Trajectory.

Design

This is a node of the functionalities implemented in the controller class derived from autoware_trajectory_follower_base package. It has instances of those functionalities, gives them input data to perform calculations, and publishes control commands.

By default, the controller instance with the Controller class as follows is used.

@startuml
package autoware_trajectory_follower_base {
abstract class LateralControllerBase {
longitudinal_sync_data_

    virtual isReady(InputData)
    virtual run(InputData)
    sync(LongitudinalSyncData)
    reset()

}
abstract class LongitudinalControllerBase {
lateral_sync_data_

    virtual isReady(InputData)
    virtual run(InputData)
    sync(LateralSyncData)
    reset()

}

struct InputData {
trajectory
odometry
steering
accel
}
struct LongitudinalSyncData {
is_steer_converged
}
struct LateralSyncData {
}
}

package autoware_mpc_lateral_controller {
class MPCLateralController {
isReady(InputData) override
run(InputData) override
}
}
package pure_pursuit {
class PurePursuitLateralController {
isReady(InputData) override
run(InputData) override
}
}
package pid_longitudinal_controller {
class PIDLongitudinalController {
isReady(InputData) override
run(InputData) override
}
}

package autoware_trajectory_follower_node {
class Controller {
longitudinal_controller_
lateral_controller_
onTimer()
createInputData(): InputData
}
}

MPCLateralController --|> LateralControllerBase
PurePursuitLateralController --|> LateralControllerBase
PIDLongitudinalController --|> LongitudinalControllerBase

LateralSyncData --> LongitudinalControllerBase
LateralSyncData --> LateralControllerBase
LongitudinalSyncData --> LongitudinalControllerBase
LongitudinalSyncData --> LateralControllerBase
InputData ..> LateralControllerBase
InputData ..> LongitudinalControllerBase

LateralControllerBase --o Controller
LongitudinalControllerBase --o Controller
InputData ..> Controller
@enduml

The process flow of Controller class is as follows.

// 1. create input data
const auto input_data = createInputData(*get_clock());
if (!input_data) {
  return;
}

// 2. check if controllers are ready
const bool is_lat_ready = lateral_controller_->isReady(*input_data);
const bool is_lon_ready = longitudinal_controller_->isReady(*input_data);
if (!is_lat_ready || !is_lon_ready) {
  return;
}

// 3. run controllers
const auto lat_out = lateral_controller_->run(*input_data);
const auto lon_out = longitudinal_controller_->run(*input_data);

// 4. sync with each other controllers
longitudinal_controller_->sync(lat_out.sync_data);
lateral_controller_->sync(lon_out.sync_data);

// 5. publish control command
control_cmd_pub_->publish(out);

Giving the longitudinal controller information about steer convergence allows it to control steer when stopped if following parameters are true

• lateral controller
  • keep_steer_control_until_converged
• longitudinal controller
  • enable_keep_stopped_until_steer_convergence

Inputs / Outputs / API

Inputs

• autoware_planning_msgs/Trajectory : reference trajectory to follow.
• nav_msgs/Odometry: current odometry
• autoware_vehicle_msgs/SteeringReport current steering

Outputs

• autoware_control_msgs/Control: message containing both lateral and longitudinal commands.
• autoware_control_msgs/ControlHorizon: message containing both lateral and longitudinal horizon commands. this is NOT published by default. by using this, the performance of vehicle control may be improved, and by turning the default on, it can be used as an experimental topic.

Parameter

• ctrl_period: control commands publishing period
• timeout_thr_sec: duration in second after which input messages are discarded.
  • Each time the node receives lateral and longitudinal commands from each controller, it publishes an Control if the following two conditions are met.
    1. Both commands have been received.
    2. The last received commands are not older than defined by timeout_thr_sec.
• lateral_controller_mode: mpc or pure_pursuit
  • (currently there is only PID for longitudinal controller)
• enable_control_cmd_horizon_pub: publish ControlHorizon or not (default: false)

Debugging

Debug information are published by the lateral and longitudinal controller using autoware_internal_debug_msgs/Float32MultiArrayStamped messages.

A configuration file for PlotJuggler is provided in the config folder which, when loaded, allow to automatically subscribe and visualize information useful for debugging.

In addition, the predicted MPC trajectory is published on topic output/lateral/predicted_trajectory and can be visualized in Rviz.

Changelog for package autoware_trajectory_follower_node

0.43.0 (2025-03-21)

• Merge remote-tracking branch 'origin/main' into chore/bump-version-0.43
• chore: rename from [autoware.universe]{.title-ref} to [autoware_universe]{.title-ref} (#10306)
• fix(planning, control): reuse stamp of subscribed topic to measure component latency (#10201)
  • fix(behavior_velocity_planner): reuse timestamp of recieved path
  • fix(behavior_path_planner): check timestamp first in timer driven callback
  • fix(trajectory_follower_node): check timestamp first in timer driven callback

  * fix(vehicle_cmd_gate): reuse timestamp of recieved path ---------

• Contributors: Hayato Mizushima, Satoshi OTA, Yutaka Kondo

0.42.0 (2025-03-03)

• Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base
• feat(autoware_utils): replace autoware_universe_utils with autoware_utils (#10191)
• Contributors: Fumiya Watanabe, 心刚

0.41.2 (2025-02-19)

• chore: bump version to 0.41.1 (#10088)
• Contributors: Ryohsuke Mitsudome

0.41.1 (2025-02-10)

0.41.0 (2025-01-29)

• Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base
• feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in fil… (#9853) feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files control/autoware_trajectory_follower_node
• fix: remove unnecessary parameters (#9935)
• chore(trajectory_follower_node): fix typos (#9707)
• feat(pid_longitudinal_controller): update plotjuggler settings (#9703)
• feat(pid_longitudinal_controller): remove trans/rot deviation validation since the control_validator has the same feature (#9675)
  • feat(pid_longitudinal_controller): remove trans/rot deviation validation since the control_validator has the same feature

  * fix test ---------

• Contributors: Fumiya Watanabe, Kosuke Takeuchi, Takayuki Murooka, Vishal Chauhan, Yuki TAKAGI

0.40.0 (2024-12-12)

• Merge branch 'main' into release-0.40.0
• Revert "chore(package.xml): bump version to 0.39.0 (#9587)" This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5.
• fix: fix ticket links in CHANGELOG.rst (#9588)
• chore(package.xml): bump version to 0.39.0 (#9587)
  • chore(package.xml): bump version to 0.39.0
  • fix: fix ticket links in CHANGELOG.rst

  * fix: remove unnecessary diff ---------Co-authored-by: Yutaka Kondo <<yutaka.kondo@youtalk.jp>>

• fix: fix ticket links in CHANGELOG.rst (#9588)
• fix(cpplint): include what you use - control (#9565)
• refactor: correct spelling (#9528)
• 0.39.0
• update changelog
• Merge commit '6a1ddbd08bd' into release-0.39.0
• fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)
• feat(mpc_lateral_controller): suppress rclcpp_warning/error (#9382)
  • feat(mpc_lateral_controller): suppress rclcpp_warning/error
  • fix

  * fix test ---------

• fix(autoware_trajectory_follower_node): fix clang-diagnostic-format-security (#9378)
• refactor(fake_test_node): prefix package and namespace with autoware (#9249)
• feat(trajectory_follower): publsih control horzion (#8977)
  • feat(trajectory_follower): publsih control horzion
  • fix typo
  • rename functions and minor refactor
  • add option to enable horizon pub
  • add tests for horizon
  • update docs

  * rename to ~/debug/control_cmd_horizon ---------

• fix(control): missing dependency in control components (#9073)
• fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)
• chore(package.xml): bump version to 0.38.0 (#9266) (#9284)
  • unify package.xml version to 0.37.0
  • remove system_monitor/CHANGELOG.rst
  • add changelog

  * 0.38.0

• Contributors: Esteve Fernandez, Fumiya Watanabe, Kosuke Takeuchi, M. Fatih Cırıt, Ryohsuke Mitsudome, Ryuta Kambe, Takayuki Murooka, Yutaka Kondo, ぐるぐる

0.39.0 (2024-11-25)

• Merge commit '6a1ddbd08bd' into release-0.39.0
• fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)
• fix(control): missing dependency in control components (#9073)
• fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)
• chore(package.xml): bump version to 0.38.0 (#9266) (#9284)
  • unify package.xml version to 0.37.0
  • remove system_monitor/CHANGELOG.rst
  • add changelog

  * 0.38.0

• Contributors: Esteve Fernandez, Yutaka Kondo, ぐるぐる

0.38.0 (2024-11-08)

• unify package.xml version to 0.37.0
• fix(control): align the parameters with launcher (#8789) align the control parameters
• feat(autoware_mpc_lateral_controller): add predicted trajectory acconts for input delay (#8436)
  • feat: enable delayed initial state for predicted trajectory

  * feat: enable debug publishing of predicted and resampled reference trajectories ---------

• feat(pid_longitudinal_controller)!: add acceleration feedback block (#8325)
• refactor(control/pid_longitudinal_controller): rework parameters (#6707)
  • reset and re-apply refactoring
  • style(pre-commit): autofix
  • .

  * .

  Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]\@users.noreply.github.com>

• feat(pid_longitudinal_controller): re-organize diff limit structure and fix state change condition (#7718) change diff limit structure change stopped condition define a new param
• fix(controller): revival of dry steering (#7903) * Revert "fix(autoware_mpc_lateral_controller): delete the zero speed constraint (#7673)" This reverts commit 69258bd92cb8a0ff8320df9b2302db72975e027f.
  • dry steering
  • add comments

  * add minor fix and modify unit test for dry steering ---------

• ci: disable failing tests undetected due to broken regex filter (#7731)
• fix(autoware_pid_longitudinal_controller, autoware_trajectory_follower_node): unite diagnostic_updater_ in PID and MPC. (#7674)
  • diag_updater_ added in PID
  • correct the pointer form

  * pre-commit ---------

• refactor(universe_utils/motion_utils)!: add autoware namespace (#7594)
• feat(mpc_lateral_controller): signal a MRM when MPC fails. (#7016)
  • mpc fail checker diagnostic added
  • fix some scope issues
  • member attribute added.
  • shared pointer added.
  • member attribute (diag_updater_) added
  • dependency added.
  • implementation of the MpcLateralController corrected!
  • typo in comment corrected!
  • member method argument corrected

  * delete unnecessary reference mark Co-authored-by: Takamasa Horibe <<horibe.takamasa@gmail.com>>

  • rebase
  • correct the include

  * pre-commit ---------Co-authored-by: Takamasa Horibe <<horibe.takamasa@gmail.com>>

• refactor(motion_utils)!: add autoware prefix and include dir (#7539) refactor(motion_utils): add autoware prefix and include dir
• feat(autoware_universe_utils)!: rename from tier4_autoware_utils (#7538) Co-authored-by: kosuke55 <<kosuke.tnp@gmail.com>>
• refactor(control)!: refactor directory structures of the trajectory followers (#7521)
  • control_traj
  • add follower_node

  * fix

• refactor(pure_pursuit): prefix package and namespace with autoware_ (#7301)
  • RT1-6683 add autoware prefix to package and namepace

  * fix precommit ---------

• refactor(trajectory_follower_node): trajectory follower node add autoware prefix (#7344)
  • rename trajectory follower node package
  • update dependencies, launch files, and README files
  • fix formats

  * remove autoware_ prefix from launch arg option ---------

• Contributors: Kosuke Takeuchi, Kyoichi Sugahara, M. Fatih Cırıt, Takayuki Murooka, Yuki TAKAGI, Yutaka Kondo, Zhe Shen, Zulfaqar Azmi, mkquda, oguzkaganozt

0.26.0 (2024-04-03)