Howdy yal, its Your Friendly Neighborhood Navigator here!
By popular request: the new Nav2 Route Server is now ready for beta testing! I’d love your critical feedback if you have time to review the code (admittedly its a … 18,000 line PR so I’m not expecting full reviews unless you got alot of time on your hands ) and test it on your robots! It has 98% test coverage and I think its ready for use today.
This is a major update to Nav2 enabling the use of pre-defined navigation graphs rather than freespace planning so that you can (1) plan across massive spaces in real-time that might not be possible with any free-space planning algorithm, (2) create plans in spaces without maps or across such vast distances maps aren’t sensible to think about, and/or (3) when you want your robot to navigate very deterministically through certain routes, lanes, or areas without deviating too much.
Here’s the PR:
A couple of tutorials to help you get started with creating the graph are already live!
I also provide you with some example behavior trees in the nav2_bt_navigator package for doing exact-route-following using the route as the plan and general-route-navigation using the route as a general solution with the planner in the loop to create the feasible specific plan in a localized time horizon. There’s also a demo in the nav2_simple_commander package using the servers directly, should you choose.
I’m looking for users to give this a look and try this out on their robots. I already have 2 companies using this in a pre-production application, but I want more feedback from folks:
Does this work for you? Any major bugs to report?
Any features missing or things you wish you had?
How long have you run this continually for without issue?
Videos and/or images to use in potential conference talks on this subject and give your company a shout out!
Let me know what you think and happy routing!
A special shoutout to Joshua Wallace that helped with some key portions of this work when it was started a couple of years ago! Thanks to our sponsors Dexory, Nvidia, Polymath Robotics, Stereolabs for making this development possible!
How about the multi-mapping concept? A robot must traverse from one floor to another via an elevator. The current nav graph file supports one map at a time. What happens if we switch the map via the map server? How do we switch the nav graph for each map?
This work does not require a map at all! You could definitely create a multi-floor navigation capability based on this, though its not one of the demos I’ve put together so far The nodes can be the terminal points for stairs/elevator servicing/etc and use free space planning between them. When you achieve a node, you enact your floor-moving behavior (call elevator, climbs stairs, etc).
I’ve been experimenting a bit with the route server (humble_main branch) to better understand its behavior, and I came across an issue that I found a bit unexpected.
Currently, the first-mile navigation is handled by selecting the nearest graph node, either by Euclidean distance or using the enable_nn_search for traversability. However, in some cases, particularly in multi-lane scenarios, the nearest node isn’t necessarily the most optimal starting point. I’ve included an image to illustrate the situation.
The route server proposes the path marked as “Unacceptable”.
Given this, I’m wondering what would be the most efficient way to address the problem. Would it make sense to add the robot’s starting pose as a temporary node in the graph and let the solver handle the rest? Or would it be better to solve the graph multiple times while selectively disabling nodes to explore several routing options, then choose the cheapest one?
I’d appreciate any insights or suggestions you might have.
I think you should use the Node ID API instead of the poses API (use_poses = false) and specify the start node ID that you would like to use instead. Then you would have a route from the node you selected to use and the first/last mile BT elements will work properly.
I don’t think that would change anything, because how would you define the node’s connectivity to the graph - the nearest nodes? That would be essentially the same thing as using traversibility search and/or using the starting node you know you prefer.
I might utilize the Node ID API, but in such cases, I would need to develop custom logic to determine the startID and endID. This kind of logic is precisely the issue I am currently addressing.
I don’t think that would change anything, because how would you define the node’s connectivity to the graph - the nearest nodes? That would be essentially the same thing as using traversibility search and/or using the starting node you know you prefer.
Regarding your point about connecting the start and goal to the nearest nodes, why would that be considered equivalent to using a traversal search? In the example image I shared, the problem would be resolved as the graph could identify the shortest path.
The issue appears when there are two lanes on a corridor-like infrastructure.
The problem is that if the goal or the start point is very near to a Node of the opposite lane the graph will plan across the whole corridor before arriving to the goal (purple path)
If start and goal would be Nodes of the graph connected with every node in a prescribed radius (let’s say 8 m) the shortest path on graph would already be the yellow path due to the comulative weight of edges. Am I missing something?
Let me add another example:
In this case green is start and blue is goal. Nearest node to start will force the planification to be way longer than it should be even if start is clearly on the right lane to easily reach blue.
If we had start and goal as graph nodes everything would be smooth!
Blockquote
using the starting node you know you prefer.
As for “using the starting node you know you prefer,” I don’t determine beforehand which node I prefer! It needs to be evaluated dynamically before navigation begins.
This is why I think its important to track your previous request nodes so you know where the ‘start’ node should be from the ‘end’ node of the previous request That completely disambiguates which node you want to use knowing what node you are already at according to the behavior. I assume in a situation like this with 2 aisles that you know from a previous request what node you should be at and did not simply spawned out of context in the middle of a couple of aisles in the middle of a facility. In which case, choosing the closest node does seem sensible since there’s no context about which side of the aisle ‘you are really on’. That seems to me to be a trivially easy solution and implementation.
That yellow path volates your directionality constraints.
I don’t think this generically works.
Imagine that green block was instead on the other side of the center node by an equal offset. That would contextually mean that you want to be on the lane going to the left. But, if you did that graph connection logic, then you’d still pick the lower right hand node because it would generate the shortest path, which would violate your directionality constraints.
I don’t mean this to say that we cannot improve the logic for how to select the node to use as the starting point when doing pose-base requests. I’m sure there are and should be improvements made. However, I think there are gaps with your particular suggestion that need to be hashed out and revisited to see how we should best improve it For example, if any dynamically generated edge crosses another edge, it should connect to that edge instead. Probably also something with collision checking if a costmap topic is given.
Asking for a pose-based request when not on or next to a node is asking for some flexibility in how things are handled. We can improve or provide optionality for how that is handled, but the fool-proof solution when you know where you are is to use the node-based requests and that’s why that is implemented. My expectation is that most professional applications would use the node-based request because those nodes can be tied to context (i.e. Shelf-3423 or Loading Bay 7) which the nature of the requested task is tied to.
I like the idea of improving the selection of the node(s) for a pose-based request, but they do need to be general purpose and not violate set constraints on the graph.
) and test it on your robots! It has 98% test coverage and I think its ready for use today.
The nodes can be the terminal points for stairs/elevator servicing/etc and use free space planning between them. When you achieve a node, you enact your floor-moving behavior (call elevator, climbs stairs, etc).
That completely disambiguates which node you want to use knowing what node you are already at according to the behavior. I assume in a situation like this with 2 aisles that you know from a previous request what node you should be at and did not simply spawned out of context in the middle of a couple of aisles in the middle of a facility. In which case, choosing the closest node does seem sensible since there’s no context about which side of the aisle ‘you are really on’. That seems to me to be a trivially easy solution and implementation.
For example, if any dynamically generated edge crosses another edge, it should connect to that edge instead. Probably also something with collision checking if a costmap topic is given.