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.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

  * fix

• Contributors: Arjun Jagdish Ram, Esteve Fernandez, Ryohsuke Mitsudome, Vishal Chauhan

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(autoware_path_optimizer): hotfix for wrong logic triggering MRM on start in 3 seconds (#10305) fix

• feat(path_optimizer): additional failure logging and failure mode handling (#10276) MRM when MPT fails

• feat: apply splitting of autoware_utils_geometry (#10270)

  • fix build error

  * merge namespace ---------

• feat: adaption to ROS nodes guidelines about directory structure (#10268)

• fix(path_optimizer): remove unnecesary optional (#10181)

• fix(planning): add missing exec_depend (#10134)

  • fix(planning): add missing exec_depend
  • fix find-pkg-share

  * fix find-pkg-share ---------

• Contributors: Arjun Jagdish Ram, Hayato Mizushima, NorahXiong, Takagi, Isamu, Takayuki Murooka, Yutaka Kondo

File truncated at 100 lines see the full file