[Nav2] SmacPlanner (Hybrid-A*, 2D A*) Now Available (Reminder meeting Oct 15 cancelled)!

1

Hi all, friendly neighborhood navigator here.

I’m super happy this week to announce the release of the smac_planner package! This has been an on-going R&D project of mine for the majority of 2020 I’ve been working on between leading community development and working on Nav2 architecture. I think it comes as no surprise that I’m terrible at naming things (come’n STVL & SLAM Toolbox, its a low bar) so I took some creative license to mirror the DLux Global Planner’s naming system :wink: .

image
image1044×983 73.6 KB

The smac_planner package is interesting for a few reasons:

  • Contains a highly optimized, templated, A* search algorithm.
  • This algorithm is then used with custom node templates to create any A*-driven planning behavior required. We provide implementation nodes for 2D A* search (similar to NavFn but without some of the odd artifacts NavFn’s A* has) and more to the point a hybrid-A* implementation built for Ackermann steering, car-like, car and high speed vehicles. Because it does SE2 collision checking, it is also suitable for differential/omnidirectional robots that are large and non-round that require full pose-base collision checking.
  • Includes a Conjugate-Gradient path smoother, costmap downsampler, and a few other bells and whistles.

The original Hybrid-A* paper talks about being able to plan in about 50-300 ms. Our implementation is able to plan in 2-120 ms across maps many multiples larger than those they tested on as part of the DARPA Urban Challenge. If you check out my readme, I also provide some additional analysis, metrics, descriptions, and parameter descriptions. https://github.com/ros-planning/navigation2/tree/main/smac_planner

The full configuration guide is also already available on the Navigation2 documentation and has over 80% unit test coverage (integration tests to follow to push to 90%+): https://navigation.ros.org/configuration/packages/configuring-smac-planner.html.

This marks the first time in ROS Navigation’s history that we have full support for Ackermann steering and cars. Using the Hybrid-A* planner, you will be able to generate 100% kinematically valid paths for ackermann steering vehicles and generate local trajectory plans using the ROS2 TEB local planner. Moreover, additional templated nodes can be added in the future to support new uses like:

  • 3D A* planning
  • Generic State Lattice primitive planners
  • Route planning

Huge thanks to Carlos Luis for helping out with sections of the implementation and @jrw for helping me get this across the finish line and testing on his Ackermann platforms.

As a general announcement, the working group meeting Oct 15 will be cancelled, the next meeting will be Oct 29.

As a general reminder, if you’re interested in mobile robots or navigation, please reach out to us! If you’d like to learn about navigation systems, working on a product, or implementing research, please let us know and we can help you get involved and meet your goals! The navigation working group is a really diverse group of researchers, students, engineers, and executives that are working on our own separate applications but offer community support and working on solutions for everyone in open source. There’s a link to our Slack group in the Navigation2 readme.

Happy planning,

S

22 Likes
2

Congratulations on getting this out! I still remember our discussion about the sbpl_lattice_planner, and I’m happy that you went ahead and created the smac_planner instead. From first impressions, it’s orders of magnitude faster than SBPL (at least in the current state of the SBPL implementation), and also much easier to use and parametrize. Good job!

2 Likes
3

I genuinely hope that SBPL gets back on its feet a bit, but I’m suspect that that is in the cards. I’d love to have fast and well supported options for both.

As an aside, because the A* implementation is templated, we could make an “SBPL” node template into SMAC pretty easily. The only real issue would be how to generate the motion primitives for your arbitrary platform. Once you know what they are, it would take me maybe a day to make a node template to work with them and allow you to generate plans based on your state lattices. I hope to be able to support this, primarily waiting on a user that needs it to instigate the conversation.

This could be a direction we could go if there was an interested party in:

  • Needing this work, so there’s a customer for it (large differential / omnidirectional platforms that can move in other dimensions you want to move in. Dubin/Reeds-shepp is a good approximation but if your omni platform MUST move in the Y direction, we’d need specific primitives)
  • Create / steal / adapt scripts to generate SBPL-like motion primitives for your platform. They don’t strictly have to be tight lattices, but it certainly wouldn’t hurt things and probably would improve speed
  • Create a node template to take in some config file of primitives, load as projections and it should all work from there since it looks no different from the other motion model primitives I generate automatically for users.

Edit: I added that to my optimizations ticket for SMAC https://github.com/ros-planning/navigation2/issues/2014 for posterity. Maybe an interested party will reach out and we can work on that. It wouldn’t take long.

2 Likes
4

Don’t blame me, @tfoote (author of TF) started it. :nerd_face:

4 Likes
5

We’re also looking at using the smac planner framework to implement a state lattice planner which would have all these great speed ups as well. Is this something anyone here might be interested in being involved with? Having a robust, fast, state lattice planner in ROS2 will be useful when your organization eventually has to transition to ROS2 (or just want to learn!).

The benefit of this would be for non-ackermann vehicles (large, non-round, differential/omni drive robots) to make the full use of your drive train with full XYTheta collision checking and the optimized A* search of the generated lattice graph.

It would be remarkably little work because of the templated search algorithm requires no changes for this. Its only 1 Node template and the generation scripts.

If you’re interested, PM me, comment below, or poke me on the Nav2 slack.

1 Like
6

Hi Steve,
I have a large rectangular Omni-drive robot that has to navigate in tight spaces in a warehouse. And I finally end up here :slight_smile:
Can we discuss it?

7

Sure! What’s up?

Also note, there’s a pending PR in Nav2 that SIGNIFICANTLY improves the planners quality, performance, and speed. You’ve come at just the right time to test that all out and give me feedback if you see any issues :wink:

8

Sure, is this PR #2404?
Also is there any progress in PR #2352, or that’s just a blueprint?

9

That contains the state lattice work which is WIP, 2404 is the Hybrid-A* portion of it.

10

Greate, how can I get involved?

11

You could test out and/or review that PR to let me know what you think / if you run into any trouble! Would you be interested in doing some development in Nav2?

12

Absolutely, am interested to get involved in the development process.

13

Join the Nav2 slack group with the following link and introduce yourself on #on-boarding! Please make sure to tell us what you might be interested in getting involved with!

2 Likes
14

Hi, an 6 month later update:

I spent the last few months really hammering away at the SMAC planner to make it faster, better, and stronger. The results of that work can be found in this PR that was just merged.

The tl;dr benefit to end users is as follows:

  • 2-3x speed up in planning times. In nearly all situations, you can get plans in large environments in well under 200 ms. Most times under 100ms. If paired with the new cached heuristic param, you can plan in excess of 100hz (10ms) if you environment is largely unchanging.
  • Significant improvement in path quality, smoothness, and intuitive-ness. The plans are far smoother, far further away from walls due to a newly developed heuristic function, and also improves the speed for full SE2 collision checking (since if the robot is further from walls, the likelihood of samples being the POSSIBLY_INSCRIBED cost away is much smaller, so can approximate). A new path smoothing algorithm is enabled by default and run in less than 2ms, so there’s really no reason not to always use it.
  • Additional parameterizations including caching heuristics, downsampling costmaps, and lookup table sizes to dramatically improve performance. This planner now precomputes everything possible to be precomputed (heuristic values, footprint orientations, primitives, trig, etc).

I’ve replaced the example image in the configuration page with a new one to demonstrate the new improvements to the planner Smac Planner — Navigation 2 1.0.0 documentation. Much more smooth, fast, and intuitive. The plan example that used to be there is now 3x faster and far smoother.

tl;dr of the tl;dr: If you haven’t tried the SMAC planner recently, you haven’t tried the SMAC planner :wink:

Happy planning!

S

3 Likes