|
| 1 | +# Mission Planner |
| 2 | + |
| 3 | +## Purpose |
| 4 | + |
| 5 | +`Mission Planner` calculates a route that navigates from the current ego pose to the goal pose following the given check points. |
| 6 | +The route is made of a sequence of lanes on a static map. |
| 7 | +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. |
| 8 | +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. |
| 9 | + |
| 10 | +The core implementation does not depend on a map format. |
| 11 | +In current Autoware.universe, only Lanelet2 map format is supported. |
| 12 | + |
| 13 | +## Inputs / Outputs |
| 14 | + |
| 15 | +### input |
| 16 | + |
| 17 | +| Name | Type | Description | |
| 18 | +| -------------------- | ------------------------------------ | ------------------------------------------ | |
| 19 | +| `~input/vector_map` | autoware_auto_mapping_msgs/HADMapBin | vector map of Lanelet2 | |
| 20 | +| `~input/goal_pose` | geometry_msgs/PoseStamped | goal pose | |
| 21 | +| `~input/checkpoints` | geometry_msgs/PoseStamped | checkpoint to follow while heading to goal | |
| 22 | + |
| 23 | +### output |
| 24 | + |
| 25 | +| Name | Type | Description | |
| 26 | +| --------------- | --------------------------------------- | --------------------------- | |
| 27 | +| `~output/route` | autoware_auto_planning_msgs/HADMapRoute | route from ego pose to goal | |
| 28 | + |
| 29 | +`autoware_planning_msgs/Route` consists of route sections and goal pose. |
| 30 | + |
| 31 | + |
| 32 | + |
| 33 | +Route section, whose type is `autoware_auto_mapping_msgs/HADMapSegment`, is a "slice" of a road that bundles lane changeable lanes. |
| 34 | +Note that the most atomic unit of route is `autoware_auto_mapping_msgs/MapPrimitive`, which has the unique id of a lane in a vector map and its type. |
| 35 | +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. |
| 36 | + |
| 37 | +The ROS message of route section contains following three elements for each route section. |
| 38 | + |
| 39 | +- `preferred_primitive_id`: Preferred lane to follow towards the goal. |
| 40 | +- `primitives`: All neighbor lanes in the same direction including the preferred lane. |
| 41 | + |
| 42 | +## Implementation |
| 43 | + |
| 44 | +### Mission Planner |
| 45 | + |
| 46 | +Two callbacks (goal and check points) are a trigger for route planning. |
| 47 | +Routing graph, which plans route in Lanelet2, must be created before those callbacks, and this routing graph is created in vector map callback. |
| 48 | + |
| 49 | +`plan route` is explained in detail in the following section. |
| 50 | + |
| 51 | +```plantuml |
| 52 | +@startuml |
| 53 | +title goal callback |
| 54 | +start |
| 55 | +
|
| 56 | +:clear previously memorized check points; |
| 57 | +
|
| 58 | +:memorize ego and goal pose as check points; |
| 59 | +
|
| 60 | +if (routing graph is ready?) then (yes) |
| 61 | +else (no) |
| 62 | + stop |
| 63 | +endif |
| 64 | +
|
| 65 | +:plan route; |
| 66 | +
|
| 67 | +:publish route; |
| 68 | +
|
| 69 | +stop |
| 70 | +@enduml |
| 71 | +``` |
| 72 | + |
| 73 | +Note that during the goal callback, previously memorized check points are removed, and only current ego pose and goal pose are memorized as check points. |
| 74 | + |
| 75 | +```plantuml |
| 76 | +@startuml |
| 77 | +title check point callback |
| 78 | +start |
| 79 | +
|
| 80 | +if (size of check points >= 2?) then (yes) |
| 81 | +else (no) |
| 82 | + stop |
| 83 | +endif |
| 84 | +
|
| 85 | +:memorize check point; |
| 86 | +
|
| 87 | +:plan route; |
| 88 | +
|
| 89 | +:publish route; |
| 90 | +
|
| 91 | +stop |
| 92 | +@enduml |
| 93 | +``` |
| 94 | + |
| 95 | +Note that at least two check points must be already memorized, which are start and goal pose, before the check point callback. |
| 96 | + |
| 97 | +### Route Planner |
| 98 | + |
| 99 | +`plan route` is executed with check points including current ego pose and goal pose. |
| 100 | + |
| 101 | +```plantuml |
| 102 | +@startuml |
| 103 | +title plan route |
| 104 | +start |
| 105 | +
|
| 106 | +if (goal is valid?) then (yes) |
| 107 | +else (no) |
| 108 | + stop |
| 109 | +endif |
| 110 | +
|
| 111 | +:plan path between each check points; |
| 112 | +
|
| 113 | +:initialize route lanelets; |
| 114 | +
|
| 115 | +:get preferred lanelets; |
| 116 | +
|
| 117 | +:create route sections; |
| 118 | +
|
| 119 | +if (planed route is looped?) then (no) |
| 120 | +else (yes) |
| 121 | + :warn that looped route is not supported; |
| 122 | +endif |
| 123 | +
|
| 124 | +:return route; |
| 125 | +
|
| 126 | +stop |
| 127 | +@enduml |
| 128 | +``` |
| 129 | + |
| 130 | +`plan path between each check points` firstly calculates closest lanes to start and goal pose. |
| 131 | +Then routing graph of Lanelet2 plans the shortest path from start and goal pose. |
| 132 | + |
| 133 | +`initialize route lanelets` initializes route handler, and calculates `route_lanelets`. |
| 134 | +`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. |
| 135 | +To calculate `route_lanelets`, |
| 136 | + |
| 137 | +1. All the neighbor (right and left) lanes for the planned path which is lane-changeable is memorized as `route_lanelets`. |
| 138 | +2. All the neighbor (right and left) lanes for the planned path which is not lane-changeable is memorized as `candidate_lanelets`. |
| 139 | +3. If the following and previous lanelets of each `candidate_lanelets` are `route_lanelets`, the `candidate_lanelet` is registered as `route_lanelets` |
| 140 | + - 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` |
| 141 | + |
| 142 | +`get preferred lanelets` extracts `preferred_primitive_id` from `route_lanelets` with the route handler. |
| 143 | + |
| 144 | +`create route sections` extracts `primitives` from `route_lanelets` for each route section with the route handler, and creates route sections. |
| 145 | + |
| 146 | +## Limitations |
| 147 | + |
| 148 | +- 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. |
| 149 | +- Looped route is not supported. |
0 commit comments