[Nav2] Request for Comment -- Route Server

Hi all, your friendly neighborhood navigator here!

I wanted to gather some feedback / requirements / use-cases / needs on a project I intend to start working on early in 2023.

Project: Nav2 Route Server

Description:

This server will complement the Planner Server to generate global routes through the environment. Rather than using free-space planning, this will take in a pre-generated Navigation Graph to plan within to enable reduced-planning times and repeatable navigation behaviors. Some common applications of this are to (1) create directional lanes or robot lanes in spaces so that a robot is routed through only appropriate zones (2) route outdoors or in non-trivially non-planar environments where a occupancy grid isn’t a reasonable representation to plan within globally and accurate global height maps are not available (e.g. most outdoor applications), (3) plan within excessively large environments where free-space planning from the current pose to a goal isn’t reasonable by any current methods within a reasonable timeframe, requiring a reduced-representation to plan within.

Requirements:

  • Enable directional and non-directional edges in the navigation graph
  • Allow the navigation graph to be independent of generation method (e.g. PRM, hand-assembled, from automated algorithm based on visibility, etc)
  • Set weights for graph edges and/or run-time loaded plugin to score edges
  • Standardized navigation graph file format
  • Handle last-mile free-space planning to the goal pose from the navigation graph if the goal is not on a graph node
  • BT plugin and Python3 API access
  • Use optimal heuristic search algorithm and/or optionally Dijkstras / BFS (does anyone really want a plugin interface for different planners or just A*/Dijkstra’s fine?)
  • Visualize navigation graph / edge weights in rviz
  • The usual test coverage and documentation you’ve come to know and love about Nav2 packages

I’m interested to hear from users any additional applications they may have for this server and any additional features or important items I should take into account as I’m designing this server or evaluating existing options for adoption.

Happy routing-sometime-in-2023,

Steve

11 Likes

I think this is a great set of requirements. I have a few more to recommend based on our experiences with RMF:

  • Allow lanes to be manually closed and reopened during runtime
    • Bi-directional lanes can be closed along one or both directions
    • Useful for optimizing traffic flow based on conditions on the ground, e.g. have more one-way lanes going in one direction than another to accommodate unusually heavy traffic flow in one direction
    • Useful for routing traffic away from temporary hazard zones, e.g. maintenance work is happening
  • Provide the route server with occupancy data
    • Automatically close lanes that are occupied by obstacles
    • Alert that a robot needs to be rerouted when a new obstacle appears
    • Use free space planning to move a robot onto the navigation graph from its starting position (in addition to off the graph to its goal, as you already mentioned)
  • Allow speed limits (recommended or expected speed) to be specified per lane
    • Can be factored into the cost calculation, assuming travel time is at least one factor in the cost
    • Yields better predictions of travel/arrival timing
  • Consider how to support interaction events, e.g. open an automated door, use an elevator
    • For large facilities, the navigation problem is inherently intertwined with these interactions
    • We not only need to plan for these interactions, but also control the motion of the robot in sync with commands to these external entities, e.g. command the robot to approach the door, then wait until the robot is close before commanding the door to open, then wait until the door is fully open before commanding the robot to move through the doorway.

On the topic of search algorithms, I’ve found that the easiest way to get good performance for navigating among static obstacles is bidirectional Djikstra where you cache and reuse the forward and reverse search trees. For navigating among moving obstacles, you can use the aforementioned static obstacle search as a heuristic with A* to guide you around the moving obstacles. That being said, I’m investigating the use of Lifelong Planning A* and Safe Interval Path Planning as possible improvements over those techniques, at the cost of being more difficult to implement.

1 Like

Howdy!

I think this is a super interesting topic and I hope for a small contribution at opening the work that Toyota Research Institute, Open Robotics and Ekumen have been doing for years now. The project is called maliput and it aims to provide a reference SDK for road networks and autonomous driving simulation.

Most of the requirements listed in the description above are native features of the runtime API that the framework offers. The framework API offers two distinct frames, the inertial world frame and a lane frame (a non isotropic traffic lane bound volume frame) that allows agents to localize themselves and other agents in a semantic and meaningful space for navigation. There is also support for traffic rules which can be used to model the complexity of each local legislation and phase state transitions. Other features include traffic lights and related traffic rules, an object API, a built in plugin system for different backend implementations, almost 100% python API bindings by means of pybind11 and a ROS2 compatible build system (and deployment).

I am not familiar to the quality level of Nav2, but I can comment on the quality measures used at maliput and family members:

  • seeks 100% public API unit test coverage
  • gcc and clang compatible builds
  • integration tests and evaluation in customized simulator
  • periodically use of static analyzers

maliput has a handful of backends already. One is based on the OpenDRIVE standard, and there is another one (under construction, but not so far from completion) that integrates with lanelets and Open Street Map maps.

There is at the moment a naive routing API implementation on top of the road network that allows to create a path between two given coordinates. Simply, we had no time to prioritize the development of a powerful router that is capable of merging all the features (geometric constraints and logical constraints) based on a dynamic cost evaluation but it is in the roadmap.


About requirements…

This is an interesting requirement for your server. Certain agents may benefit from it while others may simply discard any information they get from it. For example, a parking zone may be or not a free space with no rules at all for navigation (even in different physical levels) and a global routing server could provide no information whatsoever about the area beyond its limits.

Also, I would suggest to also consider a built in coordinate conversion system (probably provided by one of the multiple tools out there). But it can certainly affect your path choice as path lengths become more important due to scale.


I’ve done just a bit of research (scratching the surface only) in my master program of this topic (routing and flow optimization in traffic). When thinking of optimizing flow at the thousands of agents scale and more, it’s important to consider also type of agents and different cost functions for each of them (e.g. from the speed limit point of view: you don’t want to mix a car with a bike on a lane, they are better off in different lanes; from the curvature point of view, a small street may not be suitable for a truck but it could be suitable for a car) and so on.

Another interesting thing that I found is that systems of this sort can be used to optimize policies (traffic rules) and road geometries at the design phase. I wouldn’t be surprised if construction related research groups are using tools with many of your feature requests that already provide these capabilities.

Finally, I would like to mention that in the space of game theory and operations research, auction based servers are super interesting as agents may bid for occupying the space based on their own costs rather than deferring the planning task to a centralized entity. In those cases, the search and optimization tasks are deferred to the agents and servers may be used to hold the state (and also the future state estimation) of the graph and its costs and solve the bids.


I look forward to hearing other references and experiences in building systems of this sort.

Agustin

2 Likes