Implement Semantic-Map-Based Navigation and Planning
Purpose
This issue is to track the discussion, design, and implementation of a system which uses a Lanelet2-based map to navigate from a provided start point to a provided end point.
Desired Behavior
- User provides the path to a Lanelet2 OSM XML file.
- Lanelet2 Map Loader loads and validates the map.
- Global Planner receives an objective (start point, end point) via a topic.
- Global Planner queries the Lanelet2 Map Loader via service call requesting a complete sub-section of the map containing the start and end points.
- Lanelet2 Map Loader provides the requested data as a binary blob.
- Global Planner publishes a Global Path as a list of Lanelet2 primitive IDs.
- Behavior Planner queries the Lanelet2 Map Loader via a service call requesting the types of the primitives of the IDs provided in the Global Path.
- Lanelet2 Map Loader provides the requested data as a binary blob.
- Behavior Planner uses a Planning Window (distance-based or time-based) to determine how much of the route needs planning.
- Behavior Planner uses the primitive types in the Planning Window to determine which maneuver planners to call to build the Local Path.
- Behavior Planner calls a specific maneuver planner for each subsection of the route in the Planning Window, providing a start point, end point, and list of Lanelet2 primitives that make up that subsection of the route.
- Each maneuver planner calls the Lanelet2 Map Provider to retrieve the necessary primitives to sub-sample that sub-section of the route into a list of waypoints (kinematic states) which it returns to the Behavior Planner.
- The Behavior Planner aggregates the results from all of the maneuver planners called for the section of the route within the Planning Window into the Local Path.
- The Behavior Planner passes the Local Path to the Object Collision Estimator.
- The Object Collision Estimator adjusts the Local Path based on detected collisions with objects provided by the Perception stack and provides the updated Local Path to the Behavior Planner.
- The Behavior Planner publishes the Local Path for the Vehicle Controller to follow.
The above flow and the following diagram was produced based on several meetings of @bdholt1, @smerkli-embotech, @punnu1, @mitsudome-r, Yutaka Shimizu and others from Tier IV and Map.IV (don't know usernames), and @JWhitleyWork.
Definition of Done
-
#452 (closed) Introduce the dependency on Lanelet2 -
#470 (closed) Implement a Lanelet2 Map Loader -
#192 (closed) Implement a Global Planner - Plan a global reference path using the map. The output is a continuous path to get from one end of the parking lot to the other.
-
#176 (closed) Implement a Behavior Planner - Produce a local path which consists of waypoints and associated velocities.
-
#474 (closed) Implement an Object Collision Estimator -
#571 (closed) Implement a Lane Planner - Receive a start point, end point, and list of map primitive IDs and produce a list of kinematic states.
-
#212 (closed) Implement a Parking Planner - Receive a start point, end point, and list of map primitive IDs and produce a list of kinematic states.
Edited by Joshua Whitley