Path optimizer

Purpose

This package generates a trajectory that is kinematically-feasible to drive and collision-free based on the input path, drivable area. Only position and orientation of trajectory are updated in this module, and velocity is just taken over from the one in the input path.

Feature

This package is able to

• make the trajectory inside the drivable area as much as possible
  • NOTE: Static obstacles to avoid can be removed from the drivable area.
• insert stop point before the planned footprint will be outside the drivable area

Note that the velocity is just taken over from the input path.

Inputs / Outputs

input

output

Flowchart

Flowchart of functions is explained here.

@startuml
title pathCallback
start

:isDataReady;

:createPlannerData;

group generateOptimizedTrajectory
  group optimizeTrajectory
    :check replan;
    if (replanning required?) then (yes)
      :getEBTrajectory;
      :getModelPredictiveTrajectory;
      if (optimization failed?) then (no)
      else (yes)
        :send previous\n trajectory;
      endif
    else (no)
      :send previous\n trajectory;
    endif
  end group

  :applyInputVelocity;
  :insertZeroVelocityOutsideDrivableArea;
  :publishDebugMarkerOfOptimization;
end group


:extendTrajectory;

:setZeroVelocityAfterStopPoint;

:publishDebugData;

stop
@enduml

createPlannerData

The following data for planning is created.

struct PlannerData
{
  // input
  Header header;
  std::vector<TrajectoryPoint> traj_points; // converted from the input path
  std::vector<geometry_msgs::msg::Point> left_bound;
  std::vector<geometry_msgs::msg::Point> right_bound;

  // ego
  geometry_msgs::msg::Pose ego_pose;
  double ego_vel;
};

check replan

When one of the following conditions are met, trajectory optimization will be executed. Otherwise, previously optimized trajectory is used with updating the velocity from the latest input path.

File truncated at 100 lines see the full file

Changelog for package autoware_path_optimizer

0.50.0 (2026-02-14)

• Merge remote-tracking branch 'origin/main' into humble
• fix(path_optimizer): eliminate redundant move (#11874)
• Contributors: Mete Fatih Cırıt, Ryohsuke Mitsudome

0.49.0 (2025-12-30)

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

• docs: fix broken links (#11815)

• feat(autoware_vehicle_info_utils): refactor footprint generation (#11696) Co-authored-by: Mamoru Sobue <<hilo.soblin@gmail.com>>

• feat(path_optimiser): added logging for MPT (#11771)

  • added logging

  * style(pre-commit): autofix ---------Co-authored-by: pre-commit-ci-lite[bot] <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com>

• refactor: fix leftover dependent autoware_utils from updating vehicle_info_utils (#11734)

• Contributors: Arjun Jagdish Ram, Mete Fatih Cırıt, Ryohsuke Mitsudome, Sarun MUKDAPITAK

0.48.0 (2025-11-18)

• Merge remote-tracking branch 'origin/main' into humble
• fix: tf2 uses hpp headers in rolling (and is backported) (#11620)
• fix(path_optimizer): fix out_of_range die (#11211)
• Contributors: Kosuke Takeuchi, Ryohsuke Mitsudome, Tim Clephas

0.47.1 (2025-08-14)

0.47.0 (2025-08-11)

• style(pre-commit): autofix (#10982) Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
• Contributors: Ryohsuke Mitsudome

0.46.0 (2025-06-20)

• Merge remote-tracking branch 'upstream/main' into tmp/TaikiYamada/bump_version_base

• fix(path_optimizer): undefined behavior when optimizer failure (#10726)

  • change AND condition to OR

  * fix condition again Co-authored-by: Arjun Jagdish Ram <<arjun.ram@tier4.jp>>

  • fix build error
  • revert is_optimization_failed_ condition

  * minor refactoring (#1) ---------Co-authored-by: Arjun Jagdish Ram <<arjun.ram@tier4.jp>> Co-authored-by: Takayuki Murooka <<takayuki5168@gmail.com>>

• Contributors: Kento Yabuuchi, TaikiYamada4

0.45.0 (2025-05-22)

0.44.2 (2025-06-10)

0.44.1 (2025-05-01)

0.44.0 (2025-04-18)

• Merge remote-tracking branch 'origin/main' into humble
• feat(autoware_path_optimizer): created the schema file,updated the readme file (#10123)
• build(autoware_path_optimizer): fix missing diagnostic_updater dependency (#10478)
• fix(autoware_path_optimizer): incorrect application of input velocity due to badly mapping output trajectory to input trajectory (#10403)
  • Changes for proper input velocity application

File truncated at 100 lines see the full file