Mission Planner

Purpose

Mission Planner calculates a route that navigates from the current ego pose to the goal pose following the given check points. The route is made of a sequence of lanes on a static map. Dynamic objects (e.g. pedestrians and other vehicles) and dynamic map information (e.g. road construction which blocks some lanes) are not considered during route planning. Therefore, the output topic is only published when the goal pose or check points are given and will be latched until the new goal pose or check points are given.

The core implementation does not depend on a map format. Any planning algorithms can be added as plugin modules. In current Autoware Universe, only the plugin for Lanelet2 map format is supported.

This package also manages routes for MRM. The route_selector node duplicates the mission_planner interface and provides it for normal and MRM respectively. It distributes route requests and planning results according to current MRM operation state.

Interfaces

Parameters

Services

Subscriptions

Publications

Route section

Route section, whose type is autoware_planning_msgs/LaneletSegment, is a “slice” of a road that bundles lane changeable lanes. Note that the most atomic unit of route is autoware_planning_msgs/LaneletPrimitive, which has the unique id of a lane in a vector map and its type. Therefore, route message does not contain geometric information about the lane since we did not want to have planning module’s message to have dependency on map data structure.

The ROS message of route section contains following three elements for each route section.

• preferred_primitive: Preferred lane to follow towards the goal.
• primitives: All neighbor lanes in the same direction including the preferred lane.

Goal Validation

The mission planner has control mechanism to validate the given goal pose and create a route. If goal pose angle between goal pose lanelet and goal pose’ yaw is greater than goal_angle_threshold parameter, the goal is rejected. Another control mechanism is the creation of a footprint of the goal pose according to the dimensions of the vehicle and checking whether this footprint is within the lanelets. If goal footprint exceeds lanelets, then the goal is rejected.

At the image below, there are sample goal pose validation cases.

Implementation

Mission Planner

Two callbacks (goal and check points) are a trigger for route planning. Routing graph, which plans route in Lanelet2, must be created before those callbacks, and this routing graph is created in vector map callback.

plan route is explained in detail in the following section.

@startuml
title goal callback
start

:clear previously memorized check points;

:memorize ego and goal pose as check points;

if (routing graph is ready?) then (yes)
else (no)
  stop
endif

:plan route;

:publish route;

stop
@enduml

Route Planner

plan route is executed with check points including current ego pose and goal pose.

@startuml
title plan route
start

if (goal is valid?) then (yes)
else (no)
  stop
endif

:plan path between each check points;

:initialize route lanelets;

:get preferred lanelets;

:create route sections;

if (planed route is looped?) then (no)
else (yes)
  :warn that looped route is not supported;
endif

:return route;

stop
@enduml

plan path between each check points firstly calculates closest lanes to start and goal pose. Then routing graph of Lanelet2 plans the shortest path from start and goal pose.

initialize route lanelets initializes route handler, and calculates route_lanelets. route_lanelets, all of which will be registered in route sections, are lanelets next to the lanelets in the planned path, and used when planning lane change. To calculate route_lanelets,

1. All the neighbor (right and left) lanes for the planned path which is lane-changeable is memorized as route_lanelets.
2. All the neighbor (right and left) lanes for the planned path which is not lane-changeable is memorized as candidate_lanelets.
3. If the following and previous lanelets of each candidate_lanelets are route_lanelets, the candidate_lanelet is registered as route_lanelets
   • This is because even though candidate_lanelet (an adjacent lane) is not lane-changeable, we can pass the candidate_lanelet without lane change if the following and previous lanelets of the candidate_lanelet are route_lanelets

get preferred lanelets extracts preferred_primitive from route_lanelets with the route handler.

create route sections extracts primitives from route_lanelets for each route section with the route handler, and creates route sections.

Rerouting

Reroute here means changing the route while driving. Unlike route setting, it is required to keep a certain distance from vehicle to the point where the route is changed. If the ego vehicle is not on autonomous driving state, the safety checking process will be skipped.

And there are three use cases that require reroute.

• Route change API
• Emergency route
• Goal modification

Route change API

It is used when changing the destination while driving or when driving a divided loop route. When the vehicle is driving on a MRM route, normal rerouting by this interface is not allowed.

Emergency route

The interface for the MRM that pulls over the road shoulder. It has to be stopped as soon as possible, so a reroute is required. The MRM route has priority over the normal route. And if MRM route is cleared, try to return to the normal route also with a rerouting safety check.

Goal modification

This is a goal change to pull over, avoid parked vehicles, and so on by a planning component. If the modified goal is outside the calculated route, a reroute is required. This goal modification is executed by checking the local environment and path safety as the vehicle actually approaches the destination. And this modification is allowed for both normal_route and mrm_route. The new route generated here is sent to the AD API so that it can also be referenced by the application. Note, however, that the specifications here are subject to change in the future.

Rerouting Limitations

• The safety judgment of rerouting is not guaranteed to the level of trajectory or control. Therefore, the distance to the reroute change must be large for the safety.
• The validity of the modified_goal needs to be guaranteed by the behavior_path_planner, e.g., that it is not placed in the wrong lane, that it can be safely rerouted, etc.

Limitations

• Dynamic objects (e.g. pedestrians and other vehicles) and dynamic map information (e.g. road construction which blocks some lanes) are not considered during route planning.
• Looped route is not supported.

Changelog for package autoware_mission_planner_universe

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_mission_planner_universe): add explicit test dependency (#10261)
• Contributors: Hayato Mizushima, Mete Fatih Cırıt, 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)
• feat(mission_planner): tolerate goal footprint being inside the previous lanelets of closest lanelet (#10179)
• feat(autoware_vehicle_info_utils): replace autoware_universe_utils with autoware_utils (#10167)
• Contributors: Fumiya Watanabe, Mamoru Sobue, Ryohsuke Mitsudome, 心刚

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(autoware_mission_planner)!: feat(autoware_mission_planner_universe)!: add _universe suffix to package name (#9941)
• Contributors: Fumiya Watanabe, Ryohsuke Mitsudome

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: autoware_glog_compontnt (#9586) Fixed autoware_glog_compontnt
• fix(cpplint): include what you use - planning (#9570)
• refactor(glog_component): prefix package and namespace with autoware (#9302) Co-authored-by: Takagi, Isamu <<43976882+isamu-takagi@users.noreply.github.com>>
• fix(mission_planner): fix initialization after route set (#9457)
• fix(autoware_mission_planner): fix clang-diagnostic-error (#9432)
• feat(mission_planner): add processing time publisher (#9342)
  • feat(mission_planner): add processing time publisher
  • delete extra line
  • update: mission_planner, route_selector, service_utils.

  * Revert "update: mission_planner, route_selector, service_utils." This reverts commit d460a633c04c166385963c5233c3845c661e595e.

  • Update to show that exceptions are not handled

  * feat(mission_planner,route_selector): add processing time publisher ---------Co-authored-by: Takagi, Isamu <<43976882+isamu-takagi@users.noreply.github.com>> Co-authored-by: Kosuke Takeuchi <<kosuke.tnp@gmail.com>>

• 0.39.0
• update changelog
• fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)
• 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, Kazunori-Nakajima, M. Fatih Cırıt, Ryohsuke Mitsudome, Ryuta Kambe, SakodaShintaro, Takagi, Isamu, Yutaka Kondo

0.39.0 (2024-11-25)

• fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)
• 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
• feat(mission_planner): reroute with current route start pose when triggered by modifed goal (#9136)
  • feat(mission_planner): reroute with current route start pose when triggered by modifed goal
  • check new ego goal is in original preffered lane as much as possible

  * check goal is in goal_lane ---------

• fix(mission_planner): return without change_route if new route is empty (#9101) fix(mission_planner): return if new route is empty without change_route
• chore(mission_planner): fix typo (#9053)
• test(mission_planner): add test of default_planner (#9050)
• test(mission_planner): add unit tests of utility functions (#9011)
• refactor(mission_planner): move anonymous functions to utils and add namespace (#9012) feat(mission_planner): move functions to utils and add namespace
• feat(mission_planner): add option to prevent rerouting in autonomous driving mode (#8757)
• feat(mission_planner): make the "goal inside lanes" function more robuts and add tests (#8760)
• fix(mission_planner): improve condition to check if the goal is within the lane (#8710)
• fix(autoware_mission_planner): fix unusedFunction (#8642) fix:unusedFunction
• fix(autoware_mission_planner): fix noConstructor (#8505) fix:noConstructor
• fix(autoware_mission_planner): fix funcArgNamesDifferent (#8017) fix:funcArgNamesDifferent
• feat(mission_planner): reroute in manual driving (#7842)
  • feat(mission_planner): reroute in manual driving
  • docs(mission_planner): update document

  * feat(mission_planner): fix operation mode state receiving check ---------

• feat: add [autoware_]{.title-ref} prefix to [lanelet2_extension]{.title-ref} (#7640)
• refactor(universe_utils/motion_utils)!: add autoware namespace (#7594)
• 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(route_handler)!: rename to include/autoware/{package_name} (#7530) refactor(route_handler)!: rename to include/autoware/{package_name}
• feat(mission_planner): rename to include/autoware/{package_name} (#7513)
  • feat(mission_planner): rename to include/autoware/{package_name}
  • feat(mission_planner): rename to include/autoware/{package_name}

  * feat(mission_planner): rename to include/autoware/{package_name} ---------

• feat(mission_planner): use polling subscriber (#7447)
• fix(route_handler): route handler overlap removal is too conservative (#7156)
  • add flag to enable/disable loop check in getLaneletSequence functions
  • implement function to get closest route lanelet based on previous closest lanelet
  • refactor DefaultPlanner::plan function
  • modify loop check logic in getLaneletSequenceUpTo function
  • improve logic in isEgoOutOfRoute function
  • fix format
  • check if prev lanelet is a goal lanelet in getLaneletSequenceUpTo function
  • separate function to update current route lanelet in planner manager
  • rename function and add docstring
  • modify functions extendNextLane and extendPrevLane to account for overlap
  • refactor function getClosestRouteLaneletFromLanelet
  • add route handler unit tests for overlapping route case
  • fix function getClosestRouteLaneletFromLanelet
  • format fix

  * move test map to autoware_test_utils ---------

• refactor(route_handler): route handler add autoware prefix (#7341)
  • rename route handler package
  • update packages dependencies
  • update include guards
  • update includes
  • put in autoware namespace
  • fix formats

  * keep header and source file name as before ---------

• refactor(mission_planner)!: add autoware prefix and namespace (#7414)
  • refactor(mission_planner)!: add autoware prefix and namespace

  * fix svg

• Contributors: Fumiya Watanabe, Kosuke Takeuchi, Maxime CLEMENT, Takayuki Murooka, Yutaka Kondo, kobayu858, mkquda

0.26.0 (2024-04-03)