Aggregate the Output Trajectories of Several Maneuver Planners into a Local Path (Behavior Planner)
Description
Outputs
The output of the Behavior Planner should be a set of waypoints which include position (x, y, z), orientation (quaternion), and velocity which can be followed by a vehicle control algorithm (MPC, pure pursuit, etc.). This set of waypoints can be referred to as the "Local Path."
Inputs and Behavior
To generate these waypoints, the Behavior Planner should start by receiving a global path from the Global Planner (#192 (closed)). It should then use the current vehicle position to determine a relevant section of the global path to sample (the Planning Window). The Behavior Planner should use data from the Lanelet2 Map Loader to determine which Maneuver Planner (Parking Planner (#212 (closed)) or Lane Planner, at current) to generate the waypoints for each relevant sub-section of the path within the Planning Window. Once the waypoints from all Maneuver Planners have been received, the Behavior Planner should aggregate them into the Local Path. The Behavior Planner should then pass the Local Path to the Object Collision Estimator which will modify the path based on estimated collisions and return the modified Local Path back to the Behavior Planner. The Behavior Planner should then publish the Local Path.
In addition to the above behavior, the Behavior Planner should receive the Ego Vehicle State and calculate a Motion Window along the planned path. Once the end of the Motion Window reaches the end (or beyond) of the Planning Window, the Behavior Planner should re-plan a new Planning Window unless the end of the route exists within the Motion Window.
Definition of Done
-
Create stub architecture for Behavior Planner (including design document) -
Implement actual planning code and tests -
Create documentation on using the Behavior Planner