Waypoint integration at global planner

[sbadamikar-research]

I've worked in a situation where the global planner needs to be fed route based waypoints generated at a fleet manager level. At that point, I refactored move_base_flex to expand the action server for the global planner to accept waypoints as part of the action.

I've been curious about it's absence from move_base, move_base_flex, and nav2. Calling the planner on multiple goals and stitching the plans incurs overhead that is unnecessary while spline generators could generate a spline in one iteration if all waypoints are available at the start itself.

Anyone have insight on why this feature was never added to Nav2 and if there is merit to adding it now?

[SteveMacenski]

Its not absent; check out NavigateThroughPoses 😉

I think what you're communicating is that you want a spline generator for connecting sparse waypoints together as a planner implementation. That could very well be something that is added, however I would ask you how you plan to address potential collisions in this case?

That's a major reason this isn't used - its not going to promise you that this is valid and the planner server is about 'freespace' planning. We have the Route Server now that has more restricted planning on established lanes that's vaguely adjacent to what you bring up, but I say this as an example of where Nav2 does not require freespace planning in order to work well within this framework (unlike movebase flex and others).

How we could do this is to create a new behavior plugin for the behavior server or entire task server (depending on the collision checking question or general complexity of what needs to happen) that would create a plan from a set of waypoints, continuously. A new behavior tree node would call this action to generate the plan and return it for use in the BT logic as if it were from any other system.

The only question in my mind is if we want to enable multiple types of splines/interpolation or there's a few agreed upon approaches that we need to focus on only. That would largely depend if we should pluginize the algorithm, which starts to approach the complexity that this may want to be its own server rather than simply a behavior server plugin.

Two other decent options:

• In the Planner Server have a new interface for ComputePathThroughPoses that rather than calling the planner in a loop to create continuous freespace planning, we call a single new plugin type with the set of via points to return a path. That would be simple enough, though add more complexity to the planner server -- but that may be architecturally warranted and clear for users.
• We also have the Smoother Server which has no eq. in other frameworks, where we give it a plan and it provides back a plan that is smoothed. Very realistically, what you're describing could be thought of as a Path Smoother (if we think of the via points as a sparse path, which in a very real way, it is). If so, then we can implement this spline/interpolation as a smoother plugin and I believe those interfaces give you all the information that you could need to compute that spline + collision check if need be.

So I think there's 3 options:

1. New server for interpolation (I rate as the worst choice for Nav2 architecture, but could make sense [ish] if you were doing this fully internally )
2. Create a new planner server interface for computing path through poses (or a bool as to which to use?) based on a new plugin class that takes all waypoints at once and computes the path -- could be used for splines or other optimization techniques
3. Use the existing infrastructure and treat this as a path smoothing operation. Then just need to implement the spline plugin and call that in your Bt with existing BT nodes and action interface definitions.

What do you think? I think #3 makes alot of sense. Would you be open to contributing this -- that would be a useful capability (and not really much in the way of meaningful application-specific IP [someone could literally vibe code this in 2 hours with testing hah])

[SteveMacenski]

A downside to stitching multiple plans together was the computational overhead of multiple action server calls and integration with BT.

You don't have to do that, ComputePathThroughPoses will do that for you. It even considers your set planning tolerance at each via point in case one is occupied to get close (within acceptable limits) and move to the next, which is nice. Notionally this could be upgraded to set a per-waypoint tolerance to deal with that final goal thing, if you wanted 😉 It may be this is a good solution for you afterall if the issue is networking overhead/latency. This is all done in a single request and combined in the planner server itself.

I am in favor of option 2. I was using a spline as a use case for waypoints. I do mean using it as a optimization input for the inherent path planning algorithm.

Sorry, so there's no spline, you're using a planning solution that takes into account all the waypoints -- which is different from the current combination method of leg-by-leg?

[sbadamikar-research]

You don't have to do that, ComputePathThroughPoses will do that for you.

This was me using mbf so I basically reinvented the wheel and built out a BT node to do ComputePathThroughPoses

if you wanted 😉

😉

Which is different from the current combination method of leg-by-leg?

Yes and no. There is a mechanism that treats the waypoints similar to intermediate goals but the waypoints provide an attractive force to the path similar to the repulsive force of obstacles while during the A* exploration of the lattice. All waypoints suggest a cost to a primitive and the lowest one is added as cost. It prioritizes primitives that approach the waypoints semantically instead of a strict goal achievement.

[sbadamikar-research]

If we go down this path, I'd be happy to eventually expand smac

[SteveMacenski]

Huh, OK. That sounds neat. Something I'd be interested in talking about more.

It sounds like we have the plan then -- do you want to file a ticket on this with the design for the planner server and we can take it from there?

[sbadamikar-research]

I'll put together an implementation plan and put it in a ticket.