Route Planner Design Demo 2

Overview

The route planner oversees map level navigation. Rather than generating a single path, it evaluates all nearby lanelets for routing cost: Lanelets close to the destination are given a low cost, while lanelets far from the destination are given a high cost. The route planner may internally keep track of a single route, but this does not need to be provided. Route costs are relative to the current position for easy comparison: a positive-costed lanelet is further away from the current position while a negative-costed lanelet is closer.

Costs are assigned to each "nearby" lanelet that can reach the destination. "Nearby" means within a certain distance horizon, which must exceed the horizon for the path planner. If a nearby lanelet is not included in the output, it is assumed to be unable to reach the destination.

Example

In this example, all lanes near the car have been assigned a route cost based on the start and goal:

• Moving from the current lane to itself (lane 2 to lane 2) does not impact distance from route, so it is assigned a zero
• Moving away from the goal (lane 2 to lane 1) incurs a cost
• Moving from lane 2 to lane 3 makes progress to the goal, which means it has a negative cost.
• Moving from lane 2 to lane 4 also makes progress to the goal, but it is longer, meaning it has a greater cost. If the length of lane 4 were to increase, it could potentially incur positive cost.
• Lane 5 is not within the horizon of the car and does not need to be assigned a cost at this time.

Inputs

• Lanelet Map data: the map for the current location, from the HAD map service
• Goal Pose (geometry_msgs::msg::PoseStamped): destination location. Meant to be set infrequently
• Current Pose (autoware_auto_msgs::msg::VehicleKinematicState): Current location, updated regularly

Outputs

• voltron_msgs::msg::RouteCosts for nearby lanelets, published by default to route_costs topic