Route Server anologous to Planner Server

[SteveMacenski]

As the outcome of https://github.com/ros-planning/navigation2/issues/2174, we should have an analog server using the work of #1595 to allow for "route planning" versus "freespace planning" that the planner server enables.

For indoor applications like warehouse and service robot applications, you're in largely planar space. It makes alot of sense to work with an occupancy grid for planning in these situations since you're typically using 2D or 3D SLAM that can be easily formatted in this way. Then you want to do freespace planning from any A to any B. This is often required as you're going to really different places all of the time.

For larger scale outdoor applications, this is very different. A delivery robot in San Francisco doesn't want to work in cartesian coordinates of a map (navigate from 403432, 4302432 to 90655498, 430200), but GPS or a route graph made up of the city streets. "Freespace planning" like what the planner server via grid costmaps and search algorithms aren't the right way to go (even if you could have an occ grid of all of san francisco, it would be memory inefficient to store it, it would constantly need updating, and planning in it would be very slow). Most of the time, its going to be via GPS or VSLAM which don't provide you this occupancy information even if you wanted it. So instead, we plan using routes just like Google Maps provides to accomplish your mission (or via other pre-defined via points / semantic labels).

Instead, we should allow users to use their semantic representation of the environment (GPS, pre-defined route graphs, city route graphs, etc) to plan a route for the controller server to follow. So rather than a plan built off of a searching algorithm on a gridmap, these will be routes built from the semantic navigation graph.

For the controller, this is no different than before, it still seeks to follow a path/route. But now the route is computed via this semantic method versus a freespace map. In this case though, having a dynamic obstacle avoidance local planning system is more important since the route doesn't take into account any of the current information about that area (nor could it with costmaps anyhow from 2 miles away). So that controller should be able to avoid things locally to continue to progress along the route.

This will enable VSLAM, 3D SLAM, and GPS systems to plan in all-terrain and urban environments and use the Nav2 system where it is not possible now. The use of height maps in #1278 would also aid in general out-door navigation for the local planner server, but keep in mind that if you were working on a national-park scale or a city scale environment, you would still not have a global height map of the space the same way you could not have the grid occupancy map from SLAM. Thusly, this is useful for enabling all-terrain outdoor local trajectory planning and control but this route server is required for route and path planning regardless if the height map solution is available currently.

The completion of this ticket and #1278 represents a full solution to indoor, outdoor all-terrain, outdoor urban, and mixed use navigation system design whereas (assuming a valid SLAM/localization solution is available like Visual, 3D and 2D):

• Indoor: costmap local representation + planner server with costmap representation
• Outdoor Urban: costmaps or height maps (dependent on how planar) local representation + route planner
• All terrain: Height maps local representation + route planner
• Mixed use: route planner + planner server running simultaneously for each situation called out in the behavior tree.

[enricosutera]

I saw the discussion in #1595, mainly because I was really interested in multi-floor navigation (via ramps/elevators/etc) and I ended up there. In the end you proposed to chat on slack. Was there any outcome? Anyway, concerning this issue, it seems to me that Indoor indeed requires extension to 3D (or 2.5D as you call it) in two ways:

• in a single floor, e.g. concerning irregularities, hence extension of costmap local representation as you wrote;
• in a multi floor scenario, the extension of both map and planning, from "planar path planning" to "multi-floor route planning", which should involve gateways (between floors or areas), as it's been discussed in #1595. For instance the navigation could involve init pose and goal pose on two different floors which share a ramp (or an elevator or anything else) and the route planning would involve the ramp as waypoint. it don't think this issue could be overcome by improving local costmap representation only (at least not at this stage), do you? I hope this is not out of topic (?), in that case, my apologies!

Edit: to be clearer, I think indoor should have a route and path planning too, like you mentioned for outdoor:

• route planning: take elevator a, navigate till (x,y), take elevator b and so on
• path planning: no need to explain

[SteveMacenski]

Add service to find nearest pt on nav graph to a pose, thusly can create BT nodes for this service so that for a navigation task request, we have a bt that:

• finds nearest to start and goal
• plans from nav graph A->B
• appends B->goal
• prepends start->A
• sends to controller

---

• or parts out those in a subtree step by step

---

Demos:

• plan across warehouse large with graph set at start-end of aisles and important points of interest.

• plan across open hilly / construction multistory env to show how can use for outdoor nav without any freespace planning

Roadmap for this work:

• [ ] Complete semantics work / basic MVP demos with semantics library by itself so that people know how to use it / real-demos of it with practical use / what can be represented by it
• [ ] Create a new route_server which takes in the semantic information and is able to create a nav_msgs/Path based on the navigation graph requested to navigate from A->B or based on some ordered waypoint sequence to travel through. Ordered waypoints is easier, just append with some interpolation between them. For the navigation graph, we'd need to have some navigation-graph-A* to search for the shortest / lowest weight path.
• [ ] Create the action definition for this new route planner with a BT node / XML to utilize it in place of the planner server for semantic navigation
• [ ] Documentation / demos / etc. Most likely outdoors, since that would be the most compelling use-case, or a large networked building like a stupid-large warehouse where freespace planning is impractical.

[SteveMacenski]

Routes could be generated via

• PRM
• manually built with nodes on terminal states of interest, aisle entry ways, etc manually annotated from map
• result of topographical semantic maps automatically generated from grid map (to plan in very large spaces)

[abylikhsanov]

@SteveMacenski Is this still in progress or you need a contributor?

[SteveMacenski]

This is in progress and is on a temporary pause to deal with some more pressing issues. Right now, most of the work required is integration work w.r.t. exposing the server to the BT, Python3 API, testing and tutorial. If that sounds interesting, I could use the help

[abylikhsanov]

I would like to say that this could be very useful even for the indoor environments as many robots are shifting from 2D LiDAR SLAM to Visual based (as they are becoming more robust to dynamic changes and less expensive than 3D LiDARs). So creating 2D occupancy map is more of aligning to how the planners work instead of giving any benefits to the SLAM itself. This is indeed an interesting issue, I will look into it

[asasine]

Chiming in to say this would be immensely useful for our indoor environment. We represent our indoor environments with simplified adjacency graphs, similar to a road network, that has orders of magnitude less nodes and edges than an occupancy grid. We have an intermediate step in our navigation system which searches this graph using Dijkstra's or advanced arc routing / TSP implementations to efficiently identify paths and circuits that traverse certain aisles (an edge in our graph) and minimize deadheading. The ordered endpoints of these edges in the identified circuit/path are fed to the Nav stack in sequence, which performs costmap planning and local control.

Since it didn't exist in move_base (at the time we introduced it) or Nav2, we've ended up reimplementing some of this feature proposal and #1595 with custom solutions. If it were present in the Nav system at the time, we would have chosen it over a bespoke solution.

[asasine]

@SteveMacenski based on your roadmap from above (emphasis mine), I have a question about the downstream effects of the route server on local controllers.

Create a new route_server which takes in the semantic information and is able to create a nav_msgs/Path based on the navigation graph requested to navigate from A->B or based on some ordered waypoint sequence to travel through.

The implementation of nav_msgs/Path is a vector of poses. I wonder if that constrains the benefit of the route server, which would plan in a semantic space as opposed to a costmap/grid, because it forces the planner to discretize its semantic plan into freespace. What do you think of supporting planners and controllers that can communicate with more semantic representations?

One example I can think of is highway/street routing, where the plan is represented as a series of parametrized arcs of curvature and length and an Ackerman controller capable of controlling directly in this semantic space.

[SteveMacenski]

• demo with using this for grid navigation like used in amazon warehouses with QR codes. Drive to one, stop, rotate, go to next vector, etc.

In addition to broader indoor route navigation on key junctions + outdoor demos (with semantics edge costs)