Nav2 Route Server

The Route Server is a Nav2 Task server to compliment the Planner Server’s free-space planning capabilities with pre-defined Navigation Route Graph planning, created by Steve Macenski at Open Navigation with assistance from Josh Wallace at Locus Robotics. It can be used to:

• Fully replace free-space planning when following a particular route closely is required (less Controller plugins’ tuning to deviate or track the path closely), or
• Augment the global planner with long-distance routing to a goal and using free-space feasible planning in a more localized fashion for the immediate 10m, 100m, etc future.

This graph has very few rules associated with it and may be generated manually or automatically via AI, geometric, or probabilistic techniques. docs.nav2.org includes tutorials for how to generate such a graph by annotations on a grid map created via SLAM, but can also be procedurally generated. This package then takes a planning request and uses this graph to find a valid route through the environment via an optimal search-based algorithm. It uses plugin-based scoring functions applied each edge based on arbitrary user-defined semantic information and the chosen optimization criteria(s).

The Nav2 Route Server may also live monitor and analyze the route’s process to execute custom behaviors on entering or leaving edges or achieving particular graph nodes. These behaviors are defined as another type of plugin and can leverage the graph’s edges’ and nodes’ arbitrary semantic data.

There are plugin interfaces throughout the server to enable a great deal of application-specific customization:

• Custom search-prioritization behavior with edge scoring plugins (e.g. minimize distance or time, mark blocked routes, enact static or dynamic penalties for danger and application-specific knowledge, prefer main arteries)
• Custom operations to perform during route execution: (a) triggered when entering or leaving an edge, (b) achieving a graph node on the route, (c) performed consistently (e.g. open door, pause at node to wait for clearance, adjust maximum speed, turn on lights, change mode, check for future collisions)
• Parsers of navigation graph files to use any type of format desirable (e.g. geoJSON, OpenStreetMap)

Additionally, the server leverages additional arbitrary metadata specified within the navigation graph files to store information such as speed limits, added costs, or operations to perform. Thus, we do not restrict the data that can be embedded in the navigation route graph for an application and this metadata is communicated to the edge scoring and operations plugins to adjust behavior as demanded by the application. Note that plugins may also use outside information from topics, services, and actions for dynamic behavior or centralized knowledge sharing as well if desired.

A rudimentary demo of the basic features can be seen here using the route_example_launch.py provided in nav2_simple_commander.

Features

• 98% Unit Test Coverage
• Optimized Dikjstra’s planning algorithm modeled off of the Smac Planner A* implementation
• Use of Kd-trees for finding the nearest node(s) to arbitrary start and goal poses in the graph for pose-based planning requests. Optional use of Breadth-First Search to refine those nearest node(s) from simply Euclidean distance to considering traversibility length in the costmap.
• Highly efficient graph representation to maximize caching in a single data structure containing both edges’ and nodes’ objects and relationships with localized information
• All edges are directional allowing for single-direction lanes or planning
• Data in files may be with respect to any frame in the TF tree and are transformed to a centralized frame automatically
• Action interface response returns both a sparse route of nodes and edges for client applications with navigation graph knowledge and nav_msgs/Path dense paths minimicking freespace planning for drop-in behavior replacement of the Planner Server.
• Action interface request can process requests with start / goal node IDs or euclidean poses
• Service interface to change navigation route graphs at run-time
• Edge scoring dynamic plugins return a cost for traversing an edge and may mark an edge as invalid in current conditions from sensor or system state information
• Graph file parsing dynamic plugins allow for use of custom or proprietary formats
• Operation dynamic plugins to perform arbitrary tasks at a given node or when entering or leaving an edge on the route
• Operation may be graph-centric (e.g. graph file identifies operation to perform at a time) or plugin-centric (e.g. plugins self-identify nodes and edges to act upon during execution)
• Operations may trigger rerouting if necessary (e.g. due to new information, blockages, multi-robot data, etc)
• The nodes and edges metadata may be modified or used to communicate information across plugins including different types across different runs
• The Route Tracking action returns regular feedback on important events or state updates (e.g. rerouting requests, passed a node, triggered an operation, etc)
• If rerouting occurs during Route Tracking along the previous current edge, that state will be retained for improved behavior and provide an interpolated nav_msgs/Path from the closest point on the edge to the edge’s end (or rerouting’s starting node) to minimize free-space planning connections where a known edge exists and is being continued.

Practical Architectures for Use

There are several practical architectures and designs for how this Route Serve can be assembled into a robotics solution. Which to use depends on the nature of an application’s needs and behavior. This is not an exhaustive list, but enough to get users started thinking about how to fit this into their system. Architectures (1) and (2) are implemented and tested in nav2_bt_navigator/behavior_trees for immediate use!

• 1. Route Server’s output dense path -> Controller Server for direct route following
     • This is most useful when close or exact route following is required, fully replaces the Planner Server
     • Considering adding in the Smoother Server or spline fitting between the Route Server and Controller Server in a Behavior Tree to smooth out the edge transitions for following
     • Consider ComputingPathToPose for first-mile and last-mile on and off the route if leaving the graph is desirable.
• 1. Route Server’s output sparse route -> Planner Server to plan to the next node(s) -> Controller Server to track global path
     • This is useful when you want to follow the general route, but wish to have the flexibility to leave the route when obstacles are in the way and need the free-space planner to do so
     • This is also useful in conjunction with (1) above as a fallback behavior to follow the route when possible, leave when blocked (after some time, or proactively when it sees blocked in immediate future), and then track the route again. This is great in situations where you want to only leave the route when required but otherwise track the route closely
     • Consider using ComputePathToPose to plan to the next node in the route and change nodes as you approach the next
     • Consider using ComputePathThroughPoses to plan through several nodes in the future to have smooth interchange
• 1. Route Server’s output sparse route -> Waypoint Follower or Navigate Through Poses to navigate with task
     • Similar to (2), this is useful to take the nodes and edges and navigate along the intended route using intelligent navigation
     • This architecturally puts the route planning in the higher-level application-autonomy logic rather than with in the main navigation-planning task logic, which could be useful separation of concerns for some applications.
• 1. Create a behavior tree to NavigateToPose to the first node in the graph, then select (1) or (2) to track the route, finally NavigateToPose to get to the final goal pose off of the graph
     • This is useful when the start and/or goal poses are not on the navigation graph, and thus the robot needs to navigate to its starting node or final goal pose in a ‘first-mile’ and ‘last-mile’ style task
• 1. Route Server’s ComputeAndTrackRoute instead of ComputeRoute and send the dense path in (1) or sparse route in (2) or (3)
     • This is useful to track the progress of the route in the Route Server while following the route as an application sees fit. This process allows for the triggering of spatial, graph, or contextual behaviors while executing the task like adjusting speeds, turning on lights, rerouting due to multi-robot coordination resource constraints, opening doors, etc. This has a wide ranging set of applications.
• 1. Teleoping a robot -> having a script which automatically stores new nodes and/or operator manually triggers a node capture -> saving this to file -> annotating file with operation plugin to do at each waypoint (if any) -> later using the graph to navigate the robot and perform tasks
     • This is one possible way to setup a Teach-and-Repeat behavior using the route server with custom behaviors at each node. There are likely many.
• 1. Multi-floor navigation using graph nodes as terminals for stairs, elevators, etc
     • Then free-space planning can be used between on-floor nodes and graph connections for floor connectors to enact specific behaviors to call elevators, climb stairs, etc.

Design

The Nav2 Route Server is designed as several composed objects to make the system easy to understand and easily unit testable. The breakdown exists between different classes of capabilities like ROS 2 Interfaces (e.g. actions, services, debugging topics), the core search algorithm, scoring factory, route progress tracking, operations factory, file parsing, and action request ‘intent’ extraction. This distinction makes a relatively complex system easier to grasp, as there are a number of moving pieces. Luckily, few of these pieces (tracker is the exception) don’t need to know much about each other so they can be isolated.

The diagram below provides context for how the package is structured from the consititutent files you’ll find in this project.

Each have their own complete unit testing files named similarly in the test/ directory. As the diagram suggests, plugin interfaces exist in the file loader, edge scorer, and operations manager to enable customizable behavior for different applications.

Plugin Interfaces

Several plugin interfaces are provided to enable customizable behavior in the route search, route operation, and route graph file formatting. This allows for a great deal of customization for any number of applications which might want to (1) prioritize time, distance, or other application-specific criteria in routing; (2) perform custom operations or rerouting mechanics at run-time while progressing along the route such as adjusting speed or opening doors; (3) be able to integrate your own custom file format or another format of your interest.

The interface definitions can be found in the include/nav2_route/interfaces directory and are mostly self explanatory via their method names and provided doxygen documentation.

Metrics

A set of 1,000 experiments were run with the route planner with randomly selected start and goal poses, with various sized graphs. These metrics provide some reasonable benchmarking for performance of how the route server will perform in your application with various sized graphs that represent your environment:

File truncated at 100 lines see the full file