Nav2 Route Server

[SteveMacenski]

Closing #2229 and introducing route navigation graph planning to Nav2

TODOs

• Completed system tests for computation and tracking
• > 90% test coverage
• Completed readme docs
• Cover a few missed easy test coverage gaps -- WOOT 99%
• Test with all plugins
• Test simple navigator new route demos, update for new map
• Get all the linters happy again
• Configuration, Migration Documentation, concepts, plugins, simple commander API Nav2 route server docs (P1) docs.nav2.org#670
• BT nodes config guide Adding BT node docs for route server docs.nav2.org#671
• Merge conflict
• Integrating Leidos PRs: docs & graph saver rviz
• Ideas to document (but not demo) in readme.
• Demo: Route -> Smoother -> Controller for direct route following. First/last Mile to graph if needed.
• Demo: 3 Stage planning BT: Route -> Global planner to node(s) in the future with compute path to pose // compute path through poses -> trajectory planning
• Set ticket with future improvements list (see below) Nav2 Route Server Improvements #5082
• BT XML, Nodes into docs created for demos New Route Server Demo BT Node Docs docs.nav2.org#677
• Contact beta testers to get feedback
• BFS nearest neighbors
• unit testing of BFS to make sure it works properly
• Functional tesing BFS to make sure it works properly

[josephduchesne]

This is really cool, and I'm actually in the middle of trying to implement a similar system for my current workplace. The key difference is that we're doing per-row coverage of a field, and the route is far less dynamic, since the robot needs to load itself for the specific tour based on what the covered edges require (based on the robot's capcity, battery levels etc.) As a result, most of the nominal route planning work has to be done up-front.

I did have one question thought: How would you use this to plan a circuit that started and ended in the same location?

That seems like a fairly common use case for delivery bots, in addition to coverage based bots like the one I'm working on. As far as I can tell it won't do well if I just ask for a tour of N nodes, ending back at the start. Would a delivery bot that has 3 packages to deliver have to seperately plan a traversal home->A, A->B, B->C, C->home? Our use cases seems to be a variant of that, but since we mostly just traverse one edge at a time as a sub-task, we'd only really use this to get to/from a particular end of a row in the field.

Another question: Are the node edges directional, or can they have their directionality locked? In our use case, our robot can't turn around on some edges (too narrow), but it can go forwards or backwards at any time. Right now I'm constraining that by running a custom controller on those edges, and collision aware off the shelf controllers/planners should work (since collision detection should prevent turning as well). We're just being extra cautious since we don't want a planner/controller to get too adventerous in our customer's fields!

I'll be keeping an eye on this and either migrating my work towards re-using the paradigms you've developed, or offering some input/code to expand the functionality towards handling our use case better if it doesn't align with the current implementation, but is otherwise compatible. This plus the Fields2Cover integration are very adjacent to what we're doing, which means we might see some more ROS2+Nav2 farming bots complimenting ours soon!

The fundamentals seem like what we need, but at least for the time being I think I'll end up just generating the tour and using nav-through-poses.

The sementic replanning on failure is super uesful though, and may alone be worth the complexity of migrating ROS versions (Jazzy->Rolling/Kilted), although the timing is quite poor for us. Our product hits the fields in just a few short weeks (and there's no delaying planting!), and while everything is nicely containerized on our end we'll likely be too slammed with troubleshooting to try this out in May.

[DanyResasco]

@josephduchesne in types.hpp , the edges are defined on line 133, so I assume the direction depends on how the graph is designed using the tool.

@SteveMacenski
What I don’t understand are the node operations. They seem disconnected from RouteOperations, and I don’t see how they’re being used. I can see the OperationTrigger, but there’s nothing that clearly shows how to define or call a function related to the operation.

[SteveMacenski]

How would you use this to plan a circuit that started and ended in the same location?

You can just give planning requests for home->A, A->B, B->..., ...->home individually. You cannot (at this moment) describe home->home through these nodes A,B,C, so that wouldn't be possible anyway to promise you hit A,B,C through a home->home request. With that said, I've anticipated this need and filed a ticket #5082 which describes this as a possible improvement to add in a requirement to go through a certain edge / node in a request. That could be implemented at the planner level or even doing something similar to ComputePathThroughPoses where we concatenate planning requests on top of each other automatically as part of the request so that you don't have to call the server N times.

Are the node edges directional, or can they have their directionality locked?

All edges are directional. None are bidirectional unless you setup 2x directional nodes making the transition between the two nodes have both options. I didn't include a bidirectional option internally knowing that many times the cost of traversal one way or the other are not the same, and automatically annotating areas with bidirectionality isn't difficult.

[SteveMacenski]

What I don’t understand are the node operations. They seem disconnected from RouteOperations, and I don’t see how they’re being used. I can see the OperationTrigger, but there’s nothing that clearly shows how to define or call a function related to the operation.

They are related concepts. The operations in the node/edge metadata in the graph are behaviors or semantics that the node / edge themselves are requesting to be performed (think: change max speed, turn on lights, open a door). The operation plugins are the things that take those requests and do the appropriate thing that matches the node/edge metadata. On top of that, operation plugins can also choose nodes/edges that they select to operate on that are not annotated in the node/edge (think: collision checking). That is what the query API is indicating: whether this behavior operation is triggered based on a node, edge, or on general query.

I think this is all pretty well described with examples in the readme, but if you look over that in detail but still have questions or something isn't clear, please let me know and I'll work to make things more clear.

[DanyResasco]

basically, I defined the service name as part of the operations.metadata.service_name , inside the plugin loader (geoJson...) the metadata are saved as a map <string,any> in the Node and triggered based on the enum defined in Operation.

In the case of the open_door, I create a class OpenDoor that inherits RouteOperationClient and listens to the same service_name defined in the Node. I imagine this class should be a plugin or a separate node?
Can you please point out which line of the code this service is called? I don't see where the triggers of the operation metadata are called.

[SteveMacenski]

Maybe these tests will help you understand: https://github.com/ros-navigation/navigation2/blob/nav2_route_server/nav2_route/test/test_operations.cpp#L225-L394

[josephduchesne]

I know it's mentioned in #5082 somewhat, but I do feel like it's worth addressing here as well, since changing this post-release would be a fairly breaking change.

Edges being a line is a major restriction that doesn't really make sense to me in the context of the existing Nav2 system. We have "navigate to pose" and "navigate through poses", but no "navigate along exactly two poses".

Wouldn't it be easier to just aim for the following at the outset?

uint16 edgeid
geometry_msgs/Point[] points

This way the existing code could mostly work as is (edge.points = {start,end};)

It is possible to break the nodegraph up into a bunch of single/dual in/out nodes along a curved edge, but that seems like a poor semantic representation of space. In my prior job, I learned slowly that nothing is truly straight in the built environment, and now that I'm doing agricultural bots straight is an even more fanciful idea.

[SteveMacenski]

That has been noted in the 'future improvements' ticket, actually :-)

#5082

I think there's some effort that's going to go into that, so I think its OK to release it without that. My intention won't be to immediately backport into Jazzy/Humble to give it some 'soaking' time in Rolling before doing so. There's time then before this would be backported that we need to be concerned by API stability. The interfaces are the least of the complexity (i.e. representing them in the files, populating them into files, handling them when generating the paths instead of the straight-lines).

Also, I think I'd probably leave the start/end alone and add an additional field to the message instead that would contain a pre-defined path, if provided. I do wonder though if the Edge message is really needing that path info though. I'm guessing if you want a dense path like that you're using the path field in the action result, not the route field giving the sparse node/edges. Would you prefer to use the sparse route but contain that path info over using the dense path output (assuming both contained that pre-defined path info)?

Any interest in contributing that feature?

[josephduchesne]

The use case I'm working on is driving between corn rows in a field. The robot drives 0.5 - 2km "straight" between the rows with extremely tight clearance (lots of different layers policing that tolerence), then in between traversing corn rows it generally has moderate (wider navigation paths) to loose tolerence (around the base station), and corresponding vastly different controller/planner/monitoring configurations in nav2.

In reality though, nothing is ever straight so there are waypoints that can be anywhere from 100m apart to 0.5m apart depending on how much of a mess the planter + terrain made of it. Sometimes they swerved around a telephone pole in the middle of the field for example, and that caused all manner of corner cases throughout the otherwise orderly grid.

I'm most likely going to create something akin to ExtractRouteNodesAsGoals, but instead ExtractRouteEdgesAsGoals. Your proposal of having an alternate representation that makes nav_msgs::msg::Goals attachabe might work, althought it's making me think that it might be easier to have a metadata attachment system for edges that can attach multiple scalar and spatial representaitons of each edge. For example, how much the traversal of the edge would mow the lawn for a lawnmower, clean the floor for a floor scrubber, tend the crop for a pesticide sprayer etc.

Similarly, I'm going to need to attach a whole host of conditions to edges, since one somewhat clunky approach would be to just traverse each edge as a single actionserver call (NavigateThroughPoses), since that allows for easy configuration of that edge's specific needs. Your future work task list mentions several things that we've either actively worked on, or are in the middle of implementing. I might be able to align some of our proprietary solutions with the new route server and get approval to contribute them back, although timelines on that might be several months out given our busy season is now.

My half-baked solution to transition smoothing the plan 1-and-a-bit edges at a time, getting the Goals arrays for say edge AB and BC, truncating and interpolating backwards from B in both directions, then using a specialzied planner to compute the transition around B with a radius, and appending that transition to the truncated AB. Once the robot has reached the transition point just into BC, it can compute BC to CD in the same way. This does muddle the constraints on transitions, and I may end up treating transitions as their own seperate entities depending on what works best in practice.

[DanyResasco]

Hi @SteveMacenski thanks for the explanation, i found where the operation is called.
I have problem with the controller. I modified the example in simple navigator to navigate between differents nodes in the graph and Im encountering a problem when I need to reach a point that is behind the robot and the closet point is outisde the local map, (line 95 of path_handle.cpp). I know my states are in the map since I'm using the coordinates in graph yaml file. Sems is missing a check if the closest point is in map otherwise find one that is in the map.

I removed that line and I can go to A->B B->A and vicerversa. With that line I can only do A->B and B->A but not back again (pic for reference)

State machine transitioning 'GoTo':'reached'-->'GoToPallet'
[stateMachine]: Executing GoTo State [23.8495, 9.21667391304348, 1.0]
[stateMachine]: ******** ROBOT Pose: 12.366315523155066 3.990893347779546
[stateMachine]: Computing and tracking route...
[route_server]: Computing and tracking route to goal.
[route_server]: Route found with 5 nodes and 4 edges
[stateMachine]: ******** ROBOT Pose: 12.366315523155066 3.990893347779546
[stateMachine]: Executing path...
[controller_server]: Received a goal, begin computing control effort.
[controller_server]:************************* Outisde costmap Points (12.593684 , 1.196261) and in map are (0 , 0)
[controller_server]: Resulting plan has 0 poses in it.

Im this pic im trying to reach nodes 21 but the closest point is outisde the map and the local plan fails.

[SteveMacenski]

In reality though, nothing is ever straight

True, I have another user of this work that has their nodes throughout the field, not just the start/end of the aisles. That way changes over the length can be accounted for. For example:

I'm most likely going to create something akin to ExtractRouteNodesAsGoals, but instead ExtractRouteEdgesAsGoals.

Sounds reasonable! I just don't love duplicating data in the messages too much such that the Path contains the path with the in-line defined edge paths as well as the Route containing the same information. Though, it is broken up into the edge-specific parts of the final path, so that is worth something. Maybe instead it could be the idx of the Path that this edge contains so we don't transmit redundant information over the wire.

it might be easier to have a metadata attachment system for edges

That already exists :-) See the readme on the metadata for nodes and edges in the graph files!

Your future work task list mentions several things that we've either actively worked on, or are in the middle of implementing.

It might be worth having us set up a call to chat about these items and the best path forward to integrate them. It is not currently in my plan to implement any of the future work projects at the moment. I look at these as areas for community contribution to extend the capability, based on some organizations that need those capabilities. It sounds like you're in a good position to fill that gap :-)

Then using a specialzied planner to compute the transition around B with a radius, and appending that transition to the truncated AB.

There's actually work going on to add in a smoother for the edges that the server will publish out automatically in the Path field using splines! @alexanderjyuen can you chime in? The splines are also in a utility function so you could apply that on edges in the BT using the utility in a new BT node rather than using the Path, I suppose (also a good BT node to add into Nav2's library 😉 ).

[alexanderjyuen]

@josephduchesne I am actually working on transition curves sharp corners in a route graph. Right now path_converter has a densify method that linearly interpolates node to node. What I am hoping to do instead is lineally interpolate between two edges when the angle between two edges is sufficiently small (as in they are colinear) and when the angle is large enough, stitch a spline that is jerk continuous at the transitions between the two edges and in the spline itself.

I am still in the preliminary stages of implementing this right now, but we have been able to do this on a relatively large number of nodes on the path. Here is an example image, where Red is the graph edges, and green is the smoothing splines:

right now the spline geometry is controlled by the tolerance to the node that we are deviating from. One limitation I do see in the current implementation is that we are not putting limits on kinematics (ie restricting the geometry based on minimum curvature). This is something I'm presently exploring in my own work presently, but I am open to any suggestions as to what the broader nav2_route user base would like to see

[SteveMacenski]

@alexanderjyuen why not do this on all non-colinear edges (of sufficient length that a transition curve can exist)?

[alexanderjyuen]

@SteveMacenski no reason to not do that, if we have guard rails and warnings informing the user the inability to make a transition curve exist due to very densely packed points, then no reason we can't do this on every edge to edge transition that is not colinear.

[SteveMacenski]

@alexanderjyuen @josephduchesne can we take up this conversation in #5082 or on the Nav2 Slack? I'm planning on merge this today so I want this to be captured somewhere else for 'next steps'

[DanyResasco]

Hi @SteveMacenski, I still think there is a problem with how the prune function is considered.
This is the graph, from 13 I want to reach 20

This is what happened. The edge 13->14 is being pruned because

 [route_server]:  dot_prod > EPSILON: true
 [route_server]:  hypotf(vpx, vpy) = 0.229176
 [route_server]:  [Close enough to edge.](utils::distance(closest_pt_on_edge, start_) <= max_dist_from_edge_) : 1
 [route_server]: Aftr pruned_route.start_node 14

I think we should consider how far the next node is (in fact once I pass this to the local planner, it fails because the first node is outside the map

Points (12.593684 , 1.196261) Outside costmap 
[controller_server]: Resulting plan has 0 poses in it.

).
Or how much progress we made on the edge, in my case, progress. = 0.082875.

  float edge_length = hypotf(vrx, vry);
  float progress = hypotf(closest_pt_on_edge.x - first->coords.x,
                        closest_pt_on_edge.y - first->coords.y) / edge_length;
  [route_server]: Route found with 5 nodes and 4 edges

In this case I get the correct path.

Let me know what you think, maybe I'm using it wrong, or the nodes should be a specific distance.

[SteveMacenski]

The edge 13->14 is being pruned

That is expected since we're along the edge and we wouldn't want to the robot to back up a few centimeters just to go back forward. I built that feature to prune edges that the robot is already navigating along. Sometime that can be a little bit, or alot.

Maybe you want to increase min_prune_dist_from_start so that its larger than 10cm so we don't prune the start if we're still close by to it (like in this example). Maybe increase that to half a meter or so?

[DanyResasco]

Hi, I'm not trying to prevent backing up, i'm trying to make sure the robot has a valid nearby path segment to follow (13→14). especially when it still needs to turn to align itself and start moving along it. In the second image I sent, that is not a valid global plan.

[SteveMacenski]

It is not designed to be a complete global path from the robot's start pose to the goal, its the route segment that the robot should follow in order to navigate to the goal along the route. Please look at that parameter I suggested 😄 You can set that to not prune the start when you're in a set proximity to the start node in the route, which would solve your problem.

In general, I would tune that value to be something like ~3x your goal tolerance value so that if you're only doing on-graph navigation, you never need to connect with freespace planning along requests.

Note that we have examples in the BT package that show that you can easily compute a starting path from the robot's actual start pose to the start of the route both in the behavior tree XML as well as in the C++/python3 API if there's a large discrepancy. This is good when you're navigating on- and off-graph to get back onto the graph when you've left it to obtain some freespace pose.