Replace Navfn with Hybrid approach

[SteveMacenski]

Something that's on mind and I'm allocating time in 2020 to work on.

navfn is a pretty awesome optimization for circular, differential drive, indoor mobile bases. A smart engineer can easily make a controller to turn to a given heading on a path update to accurately align with a path start and march forward with their controlling approach.

However for the rest of the world (ackermann, skid, non-center drive, ...) (rectangular, concave, etc) this approach leaves some pretty substantive gaps.

You could plan a path to a point on standard regular graph search that is totally bogus because your robot cannot spin in place due to drive constraints, collisions of base type, etc. We need to make planning take into account even the most basic robot constraints like actual footprint, actual dynamic abilities, etc.

This also dramatically helps us on the control front. Right now you have to play a game tuning a controller to both accurately follow a path once on it, as well as get onto a path from a potentially substantial orientation / position differential. Doing either will make the other controller worse and in the case of non-differential non-circular robots, you can't just spin in place so your main controller is tuned as a path follower.

Now with a hybrid approach, I'm thinking about sampling some time t ahead in each march forward of the search. While each cell/branch search is going to be alot heavier, I'm going to place substantial effort into implementing this such that there isn't too much higher of CPU load on planning similar path lengths by not visiting connected neighbors but projecting forward by some amount and checking intermediate states for state validity.

[orduno]

Now with a hybrid approach, I'm thinking about sampling some time t ahead in each march forward of the search. While each cell/branch search is going to be alot heavier, I'm going to place substantial effort into implementing this such that there isn't too much higher of CPU load on planning similar path lengths by not visiting connected neighbors but projecting forward by some amount and checking intermediate states for state validity.

Sounds like you are describing a Hybrid-A* path planner.

I like the idea! We discussed the possibility early on but decided porting navfn was the first step. I'd be really interested in helping out on this, especially if we have interest from teams whose robots are not currently supported by nav2 due to its configuration/design.

We should also think about breaking it down to building blocks so we can later implement other types of planners, currently, the core of navfn is a single class 🤔

[SteveMacenski]

Frankly this isnt a huge amount of work once everything is set up. My plan was to knock this out over a 2 week sprint but lets chat in China on it!

I dont necessarily want to box in with hybrid-A* specifically. By hybrid I mean partially search and partially sampling based. In the sense that in this world I’d likely take some ques from my time in manipulation rather than cell visiting

[SteveMacenski]

Covered in the scope of #1710

Also, nearing completion