Hi all,
Its your friendly neighborhood navigator. Sorry its been so long since you’ve heard from me, but I’ve been heads down trying to get the next major block of Nav2 infrastructure in place for your planning-pleasure.
I’m happy to announce that Nav2 now has a State Lattice Planner within the Smac Planner framework alongside Hybrid-A* and 2D-A*. You can find the code here, documentation about the Smac Planner Framework, and of course State Lattice specific documentation with examples below.
https://navigation.ros.org/configuration/packages/smac/configuring-smac-lattice.html
This is a big move for Nav2 and completes what I consider to be the minimum spanning set of freespace planning algorithms required for Nav2 to work on any robot platform and provide kinematically feasible paths at lightening fast speeds competitive with naive 2D search algorithms. Namely:
- Circular differential / Omnidirectional - NavFn or Smac 2D A*
- Non-circular differential / omnidirectional - Smac State Lattice or Smac Hybrid-A* (dep. on requirements)
- Ackermann (circular or non-circular) - Smac Hybrid-A*
- Legged - Smac Hybrid-A* or Smac State Lattice (dep. on requirements)
- Exotic custom robots - Smac State Lattice, with minimum control set
These planning algorithms make sure that we’re using all the information we have available to us about the robot platforms in the planning space to ensure that paths are kinematically feasible (e.g. drivable in the presence of physical constraints) and use the full robot footprint without any sort of circular approximation such that an irregularly shaped robot can make tight maneuvers in narrow spaces.
Moreover, I’ve painstakingly designed the Smac framework to be highly extensible in the future should other search-based planners be helpful. They share the majority of code making it more bulletproof and only implementing the planner-specifics rather than boilerplate search.
The runtime speeds are also exceptionally fast. I’ve laboriously iterated, designed, and optimized a semi-novel set of heuristic and traversal functions for the SE2 planners (of which they all share) in order to seriously increase speed and path quality, ultimately resulting in planners that are on par, or occasionally faster than NavFn or other 2D counterparts. In fact, within this framework, the SE2 kinematically feasible planners (Hybrid-A* and State Lattice) are appreciably faster than the 2D-A* implementation provided! No 1-3s planning times like ROS 1’s SBPL State Lattice planner, you can expect planning times typically in the range of 50-200ms, in line with NavFn.
The image above shows the State Lattice planner in action, planning a drivable path repelling higher cost zones in under 20ms. It also supports reversing into tight spaces if required, as shown here to achieve a goal with the robot’s back against the wall (to dock, presumably).
It is worth noting that the State Lattice planner is currently in advanced Beta, meaning it is 80% ready for primetime use, missing the following features
- We have been developing a set of minimum control set generator scripts which are not available just yet, but we provide a reasonable default file in the mean-time for testing and evaluation
- We do not yet support control sets that involve rotating in place, but are looking to very shortly
- Optimizing for Omnidirectional robot behaviors (works, but sub-optimally)
You may track our progress in these last few efforts in the following GitHub ticket [Smac State Lattice] Finish Beta to release quality · Issue #2683 · ros-planning/navigation2 · GitHub but you are encouraged to try it out for yourselves right now and let us know if you have any questions, concerns, or feedback!
Happy latticing,
Steve
for all the hard work and sharing this great information.