Path Planner

Path Planner Documentation

Note: prior to the adoption of a new design proposal in January 2022, "Path Planner" may refer to the Motion Planner.

Introduction

The role of the Path Planner is to generate a representative set of space-only paths for a large horizon and cost those paths based on safety and routing cost. Let's unpack that statement:

• representative set: There are infinitely many ways the vehicle can move. Representative means that each "option" the vehicle could take is represented by a single path. For instance, driving straight through an intersection would be an "option", and would have one path down the center of the lane rather than many paths representing all the different possibilities of swerving or driving at the edge of the lane.
• space-only paths: Each path describes an "option" on how the vehicle can move through space, and does not care about velocity or orientation. These options do not need to be consistent with the physical capabilities of the car: they may, for instance, demand a lane change is made by immediately jumping from one lane to another without traveling through the intervening space, so long as those lanes are adjacent. It is the Motion Planner's role to make sure the car's trajectory is viable.
• large horizon: Paths represent a high-level view of where the car is going, and so can be extended farther in distance while still using reasonable computational resources than the more resource-intensive Motion Planner. This means that we can use the path planner to weed out the most likely long-term risks.
• cost those paths: related to the reasoning for large horizon, each path is assigned values (costs) that represent its viability, with a high-cost path being undesirable. Costs, especially routing costs, may be negative.
  • safety cost: The presence of obstacles in the road or risky operations like lane changes and left turns.
  • routing cost: Attach a routing cost based on the Global Route Planner's output, where the lower the cost the more the path takes us toward the destination.

Implementation

Implementation for the path planner consists of the node and the logic. The node handles the connections to other parts of the system (subscriptions/publishers), and feeds the appropriate results to the logic. The idea behind the logic can be broadly described as:

• Determine what lanelet(s) we are in
  • Not all lanelets that contain the current position of the vehicle count. Assuming the vehicle travels according to traffic regulations, we should only use the lanelets that are legally reachable from some previous position to avoid illegal behavior.
• Generate all possible lanelet-level ways to traverse the map within horizon. This will result in a set of lanelet sequences.
• Transform lanelet sequences into their corresponding space paths. Attach a routing cost to the path based on the route cost of the lanelet we would end up in.
• Attach a safety cost to the path.
• Pass the set of paths (with costs) back to the node.

Some level of linestring geometry is needed to accomplish these steps. For instance, calculating the horizon requires the length of only the segments of lanes we need to travel, which Lanelet2 currently does not do for us. Splitting linestrings by a nearby point is a suprisingly helpful operation: it lets us find the centerlines of segments of lanelets, which can be turned into their lengths or used when generating paths.