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:

This is in comparison with typical run-times of free-space global planners between 50 ms - 400 ms (depending on environment size and structure). Thus, the typical performance of using the route planner - even in truly massive environments to need hundreds of thousands of nodes of interest or importance - is well in excess of freespace planning. This enables Nav2 to operate in much larger spaces and still perform global routing. There’s little reason then to use a heuristic-driven A* when Dikjstra’s Algorithm is plenty fast for even massively connected graphs.

The script used for this analysis can be found in test/performance_benchmarking.cpp.

Parameters

route_server:
  ros__parameters:

    base_frame: "base_link"                       # Robot's base frame
    route_frame: "map"                            # Global reference frame
    path_density: 0.05                            # Density of points for generating the dense nav_msgs/Path from route (m)
    max_iterations: 0                             # Maximum number of search iterations, if 0, uses maximum possible
    max_planning_time: 2.0                        # Maximum planning time (seconds)
    costmap_topic: 'global_costmap/costmap_raw'   # Costmap topic when enable_nn_search is enabled. May also be used by the collision monitor operation and/or the costmap edge scorer if using the same topic to share resources.

    graph_file_loader: "GeoJsonGraphFileLoader"   # Name of default file loader
      plugin: nav2_route::GeoJsonGraphFileLoader  # file loader plugin to use
    graph_filepath: ""                            # file path to graph to use

    edge_cost_functions: ["DistanceScorer", "DynamicEdgesScorer"]  # Edge scoring cost functions to use
    DistanceScorer:
      plugin: "nav2_route::DistanceScorer"
    DynamicEdgesScorer:
      plugin: "nav2_route::DynamicEdgesScorer"

    operations: ["AdjustSpeedLimit", "ReroutingService"] # Route operations plugins to use
    AdjustSpeedLimit:
      plugin: "nav2_route::AdjustSpeedLimit"
    ReroutingService:
      plugin: "nav2_route::ReroutingService"

    tracker_update_rate: 50.0                     # Rate at which to check the status of path tracking
    aggregate_blocked_ids: false                  # Whether to aggregate the blocked IDs reported by route operations over the lifespan of the navigation request or only use the currently blocked IDs.
    boundary_radius_to_achieve_node: 1.0          # Radius (m) near boundary nodes (e.g. start/end) to enable evaluation of achievement metric
    radius_to_achieve_node: 2.0                   # Radius (m) near route nodes as preliminary condition for evaluation of achievement metric

    max_prune_dist_from_edge: 8.0                       # Max distance from an edge to consider pruning it as in-progress (e.g. if we're too far away from the edge, its nonsensical to prune it)
    min_prune_dist_from_goal: 0.15                      # Min distance from goal node away from goal pose to consider goal node pruning as considering it as being passed (in case goal pose is very close to a goal node, but not exact)
    min_prune_dist_from_start: 0.10                     # Min distance from start node away from start pose to consider start node pruning as considering it as being passed (in case start pose is very close to a start node, but not exact)
    prune_goal: true                              # Whether pruning the goal nodes from the route due to being past the goal pose requested is possible (pose requests only)
    enable_nn_search: true                        # Whether to enable breadth first search considering the costmap to find the node closest to the start and edge poses, rather than using the euclidean nearest neighbor alone.
    max_nn_search_iterations: 10000               # Maximum number of iterations for breadth-first search.
    num_nearest_nodes: 5                          # The number of nearest-neighbors to extract for breadth-first search to consider.

CostmapScorer

This edge scoring plugin will score based on the costmap values of the edge (e.g. using either maximum cost on the edge or average cost)

DistanceScorer

This edge scoring plugin will score based on the distance length of the edge, weighted proportionally to percentage-based speed limits (if set in the graph)

DynamicEdgesScorer

This edge scoring plugin will score based on the requested values from a 3rd party application via a service interface. It can set dynamically any cost for any edge and also be used to close and reopen particular edges if they are blocked, in use by other robots locking out its shared use by other robots, higher cost due to overlap with other platforms in service, increased cost due to fleet manager analytics that this space is underperforming throughput, or otherwise temporarily non-traversible. For example, if other robots report an edge to be blocked, all robots can avoid this edge/aisle/etc.

TimeScorer

This edge scoring plugin will score based on the time to traverse the length of the edge. This will use the distance of the edge weighted in proportion to the absolute speed limits of the robot over an edge. If none is set in the graph, a parameterized maximum speed will be used. If an actual, measured time of a previous traversal is in the edge’s metadata, this will be used.

PenaltyScorer

This edge scoring plugin will score based on a statically set penalty in the graph file for a particular edge. This can be based on application known logic to weight preferences of navigation tactics in a space.

SemanticScorer

This edge scoring plugin will score based on semantic information provided in the graph file. It can either check for the edge’s semantic class via a parameterized key’s value or search all key names to match known semantic classes to apply weight (e.g. class: highway or highway: <some other application info>).

StartPoseOrientationScorer

This edge scoring plugin will score an edge starting at the start node (vector from start->goal) based on its angular proximity to the starting pose’s orientation. This will either score a weighted-angular distance or reject traversals that are outside of a set threshold to force the route to go down a particular direction (i.e. direction robot is already facing).

GoalPoseOrientationScorer

This edge scoring plugin will score a an edge with terminus of the goal node (vector from start->goal) based on its angular proximity to the goal pose’s orientation. This will either score a weighted-angular distance or reject traversals that are outside of a set threshold to force the route to go down a particular direction (i.e. direction robot wants to be facing).

AdjustSpeedLimit

This route operation will check the graph at each state change (e.g. node passed) if the new edge entered contains speed limit restrictions. If so, it will publish those to the speed limit topic to be received by the controller server.

CollisionMonitor

This route operation will evaluate a future-looking portion of the route for validity w.r.t. collision in the costmap. If it is blocked, it sets the edge blocked as blocked for rerouting around the blocked edge.

ReroutingService

This route operation will receive service requests from a 3rd party application to cause a rerouting request.

TimeMarker

This route operation will track times taken to traverse particular edges to write times to for later improved navigation time estimation.

TriggerEvent

This route operation will trigger an external service when a graph node or edge contains a route operation of this name. It uses a std_srvs/Trigger interface and is a demonstration of the RouteOperationClient<SrvT> base class which can be used to trigger other events of other types of other names as desired (opening doors, calling elevators, etc).

ROS Interfaces

File Formats

The graphs may be stored in one of the formats the parser plugins can understand or implement your own parser for a particular format of your interest! A parser is provided for GeoJSON formats. The only three required features of the navigation graph is (1) for the nodes and edges to have identifiers from each other to be unique for referencing and (2) for edges to have the IDs of the nodes belonging to the start and end of the edge and (3) nodes contain coordinates. This is strictly required for the Route Server to operate properly in all of its features.

Besides this, we establish some conventions for route graph files to standardize this information to offer consistent and documented behavior with our provided plugins. The unique identifier for each node and edge should be given as id. The edge’s nodes are startid and endid. The coordinates are given in an array called coordinates. While technically optional, it is highly recommended to also provide:

• The node’s frame of reference (frame), if not the global frame and you want it transformed
• The Operation’s trigger (e.g. enter, exit edge, node achieved) and type (e.g. action to perform), as relevant

While optional, it is somewhat recommended to provide, if relevant to your needs:

• The edge’s cost, if it is fixed and known at graph generation time or edge scoring plugins are not used
• Whether the edge’s cost is overridable with edge scoring plugins, if those plugins are provided.

Otherwise, the Node, Edge, and Operations may contain other arbitrary application-specific fields with key-value pairs under the metadata key-name. These can be primitive types (float, int, string, etc), vector types (e.g. a polygon or other vector of information), or even contain information nested under namespaces - whereas a metadata object may exist as a key’s value within metadata.

While GeoJSON is not YAML-based, the following YAML file is provided as a more human-readable example for illustration of the conventions above. Usable real graph file demos can be found in the graphs/ directory. However, the sample_graph.geojson in the graph/ directory exists to show the different API for options for operations, metadata, recursive metadata and vectors.

example_graph.yaml

Node1:                     // <-- If provided by format, stored as name in metadata
  id: 1                    // <-- Required
  coordinates: [0.32, 4.0] // <-- Required
  frame: "map"             // <-- Highly recommended
  metadata:
    class: "living_room"   // <-- Metadata for node (arbitrary)
  operation:
    pause:                 // <-- If provided by format, stored as name in metadata
      type: "stop"         // <-- Required
      trigger: ON_ENTER    // <-- Required
      metadata:
        wait_for: 5.0      // <-- Metadata for operation (arbitrary)

Edge1:                     // <-- If provided by format, stored as name in metadata
  id: 2                    // <-- Required
  startid: 1               // <-- Required
  endid: 3                 // <-- Required
  overridable: False       // <-- Recommended
  cost: 6.0                // <-- Recommended, if relevant
  metadata:
    speed_limit: 85        // <-- Metadata for edge (arbitrary). Use abs_speed_limit if not a percentage
  operations:
    open_door:             // <-- If provided by format, stored as name in metadata
      type: "open_door"    // <-- Required
      trigger: ON_EXIT     // <-- Required
      metadata:
        door_id: 54        // <-- metadata for operation (arbitrary)
        service_name "open-door"  // <-- metadata for operation (Recommended)

Metadata Conventions for Convenience

While other metadata fields are not required nor necessarily needed, there are some useful standards which may make your life easier within in the Route Server framework. These are default fields for the provided plugins, but you’re free to embed this information anyway you like (but may come at the cost of needing to re-implement provided capabilities). A set of conventions are shown in the table below. The details regarding the convention and where they are used within the codebase follows in more detail.