Smac/Hybrid-A* planner #2021
Smac/Hybrid-A* planner #2021
Conversation
0bc8806 to
9bad432
Compare
9bad432 to
a1f2cde
Compare
962a600 to
f7197a8
Compare
|
@SteveMacenski , it looks like your most recent build failure with rosdep is due to a package install issue in ompl:
Looks like a simple fix, as they'll just need to touch an index file to register the package. |
|
That's bazaar. This week has been bazaar :sigh: Thanks for finding that - an issue though is that that requires OMPL to be re-released for us to use, but that's another topic. Is there anything I can do about this in the meantime? |
Yes! You could use this: #2025 . It built your branch locally for me with CircleCI CLI |
* Use OMPL to generate heuristics The calculation is run at every planning cycle. It does not seem to slow down the planner - the calculation time seems to be quick enough that the improvement in graph expansion accounts for it. * Use OMPL to calculate analytic solution when near goal * Make angles multiples of the bin size to stop looping behaviour * Uncrustify * Use faster std::sqrt function * Fix analytic path so that the collision checker has coordinates to check! * Pre-allocate variables in analytic path expansion * Rename typedef to NodeGetter to more accurately describe function * Use distance rather than heuristic to determine when to perform analytic expansion Also force the analytic expansion to run on first iteration in case path is trivial. * Move the check for motion model into the main A* loop * Add copyright notices * Remove comment about relaxing node match tolerances The analytic expansion removes the need for this. * Correctly reset node coordinates when aborting from analytic expansion * Move analytic expansion logic to separate function * Uncrustify * Remove unneeded call to get goal coordinates * Fix the calculation of intervals in the analytic path Reserve the number of candidate nodes we are expecting. Base calculations on intervals rather than points - makes distances between nodes work properly. * Rescale heuristic so that analytic expansions are based on distance * Repeatedly split analytic path in half when checking for collision * Add parameter to control rate of analytic expansion attempts * Uncrustify * Fix incorrect type in templated function * Cpplint * Revert "Repeatedly split analytic path in half when checking for collision" This reverts commit 94d9ee0. There was a marginal speed gain (perhaps!) and the splitting approach made the code harder to understand and maintain. * Uncrustify * Add doxygen comments * Add parameter description for analytic expansion ratio * Set lower limit of 2 on number of iterations between analytic expansions * Reduce expected number of iterations because of analytic completion * Refactor analytic expansion ratio calcs to make logic easier to understand
|
It looks like some of the additional optimizations we made really payed off big time. e.g.
Edit: I made an update and got another ~20-30% speedup on those numbers.
This path was generated in 33ms and is 85 meters long in a map 3.5x the size of the original papers work |
|
|
||
| ### Potential Fields | ||
|
|
||
| Many users and default navigation configuration files I find are really missing the point of the inflation layer. While it's true that you can simply inflate a small radius around the walls, the _true_ value of the inflation layer is creating a consistent potential field around the entire map. |
There was a problem hiding this comment.
A figure including a visual "A vs. B" comparison showcasing what you mean might be helpful. Do you mean expanding the inflation layer radius to be slightly larger than the half the width of hallways, so that there is only a linear trough of costs running parallel to most walls?
There was a problem hiding this comment.
After skimming the paper, this implementation isn't using the inflation layer as a proxy for Voronoi Fields, is it? It seems a true Voronoi Field would assign a non-zero cost for all points on a map, excluding discretized points/edges of free space along Voronoi cell walls. Where as the inflation layer has a finite influence/reach into larger free spaces.
There was a problem hiding this comment.
We use the inflation layer as a potential field and since we know how its computed, we can back out needed gradient information. The use of specifically a voronoi field is largely unnecessary and the paper way overblows the impact.
There was a problem hiding this comment.
But this section has nothing to do with voronoi, this just has to do with people not correctly utilizing potential fields with the inflation layer needed by heuristic driven planners. You won't see anything using a Dijkstras, but anything like Hybrid-A*, A*, SBPL, etc are going to really need a reliable potential field in order to be as smooth and reasonable as possible.
There was a problem hiding this comment.
Ok. That explains why the global plan seems to hug the contour of the inflation layer when traversing sides of open spaces, per the images you included in your above comments.
Hypothetically, for a empty warehouse scenario, with staggered rows of pylons, would we see the planner zigzag when traversing down a row? As opposed to straight down the alley?
o o o o
o o o o
o o o o
Latching to the nearest inflation boundary, rather than a voronoi wall in the center seems like it could effect human intuition of the robots path.
There was a problem hiding this comment.
Ok. That explains why the global plan seems to hug the contour of the inflation layer when traversing sides of open spaces, per the images you included in your above comments.
100%, and the NavFn A* and all others will do the same. Dijkstras gets around that by creating it own potential field, but any heuristic driven planner is going to have some cost-to-travel that isn't dependent on a cost and something that takes into account cost. The result is that it will always steer and ride alongside cost areas if most space is 0. But if you make a nice potential field across the map, which the inflation layer will do, then you see nice and smooth performance using the spaces more effectively. This isn't unique to SMAC or NavFn, its just a feature of any heuristic driven search where we take into account cost.
Hypothetically, for a empty warehouse scenario, with staggered rows of pylons, would we see the planner zigzag when traversing down a row? As opposed to straight down the alley?
It depends. I don't think there's a catch-all answer to that. If the goal is straight across the row, it'll go straight. If its offset, it depends. The penalty functions can also be turned for this kind of behavior as well. You could crank up the non-straight heuristic so that it'll try to go straight when possible and that multiplier on cost would likely make it turn to hit the offset before or after the pylons since the multiplier would be lower in a lower-cost zone before/after since the inflation is lower.
I twouldn't latch to the boundary if you inflated the space properly, which is why I specifically call that out.
Codecov Report
@@ Coverage Diff @@
## main #2021 +/- ##
=======================================
Coverage ? 79.96%
=======================================
Files ? 242
Lines ? 11872
Branches ? 0
=======================================
Hits ? 9494
Misses ? 2378
Partials ? 0
Flags with carried forward coverage won't be shown. Click here to find out more.
Continue to review full report at Codecov.
|
|
85% coverage in unit tests, not bad. Should get > 90% once we throw in an integration test down the road |
There was a problem hiding this comment.
@SteveMacenski I took a quick view over this, not really detailed into the smac additions though themselves.
| @@ -31,18 +31,21 @@ using namespace std::chrono_literals; | |||
| namespace nav2_costmap_2d | |||
| { | |||
|
|
|||
| FootprintCollisionChecker::FootprintCollisionChecker() | |||
| template<typename CostmapT> | |||
| FootprintCollisionChecker<CostmapT>::FootprintCollisionChecker() | |||
There was a problem hiding this comment.
now that these are templated - they should be in a header - not a CPP file - otherwise you can't use this class in a downstream package that wants to define a custom CostmapT
There was a problem hiding this comment.
I don't understand what specific part should be in the header. The bottom of the cpp file has the declaration of the templates as well for all the valid CostmapT types that are used (shared ptr and raw ptr. I was going to make unique_ptr, but never used anywhere). A new declaration of it required for another template could be declared in the headers of the users, I believe.
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. Reserved. | ||
|
|
||
| #ifndef DEPRECATED_UPSAMPLER__UPSAMPLER_COST_FUNCTION_NLLS_HPP_ |
There was a problem hiding this comment.
why would we be including something "deprecated" in a first merge?
There was a problem hiding this comment.
Because I spent a stupid amount of time on it and I'd like to preserve it in case we need it in the future. This may be resurrected in the future. I don't think it will be, but I have an itching feeling Autoware and Nav2 are going to become closer in the future and that application will want this capability to be completed and available.
|
As an aside - seeing that "willow map" in a PR is bringing back some feelings. |
I always like to give a nod to the people that made this all possible 🥇 |
* adding smac_planner to navigation2 metapackage * adding params to metapackage * update config files * adding navfn benchmark testing * updates to costmap_2d for flexility * update planner API for new changes * adding ompl to underlay because ros2 master doesn't contain the rosdep key * patching templated footprint collision checker * fix typo * updating readme config file * Analytic expansion (ros-navigation#43) * Use OMPL to generate heuristics The calculation is run at every planning cycle. It does not seem to slow down the planner - the calculation time seems to be quick enough that the improvement in graph expansion accounts for it. * Use OMPL to calculate analytic solution when near goal * Make angles multiples of the bin size to stop looping behaviour * Uncrustify * Use faster std::sqrt function * Fix analytic path so that the collision checker has coordinates to check! * Pre-allocate variables in analytic path expansion * Rename typedef to NodeGetter to more accurately describe function * Use distance rather than heuristic to determine when to perform analytic expansion Also force the analytic expansion to run on first iteration in case path is trivial. * Move the check for motion model into the main A* loop * Add copyright notices * Remove comment about relaxing node match tolerances The analytic expansion removes the need for this. * Correctly reset node coordinates when aborting from analytic expansion * Move analytic expansion logic to separate function * Uncrustify * Remove unneeded call to get goal coordinates * Fix the calculation of intervals in the analytic path Reserve the number of candidate nodes we are expecting. Base calculations on intervals rather than points - makes distances between nodes work properly. * Rescale heuristic so that analytic expansions are based on distance * Repeatedly split analytic path in half when checking for collision * Add parameter to control rate of analytic expansion attempts * Uncrustify * Fix incorrect type in templated function * Cpplint * Revert "Repeatedly split analytic path in half when checking for collision" This reverts commit 94d9ee0. There was a marginal speed gain (perhaps!) and the splitting approach made the code harder to understand and maintain. * Uncrustify * Add doxygen comments * Add parameter description for analytic expansion ratio * Set lower limit of 2 on number of iterations between analytic expansions * Reduce expected number of iterations because of analytic completion * Refactor analytic expansion ratio calcs to make logic easier to understand * add readme color * fix linting * ceil from floor (and speed up) * a few updates * fix smac tests * fixing smoother test * remove cost check - to be readded at another time * working last test from debug issues * Update README.md * Update README.md * adding getUseRadius API doxygen Co-authored-by: James Ward <james@robomo.co>
* adding smac_planner to navigation2 metapackage * adding params to metapackage * update config files * adding navfn benchmark testing * updates to costmap_2d for flexility * update planner API for new changes * adding ompl to underlay because ros2 master doesn't contain the rosdep key * patching templated footprint collision checker * fix typo * updating readme config file * Analytic expansion (ros-navigation#43) * Use OMPL to generate heuristics The calculation is run at every planning cycle. It does not seem to slow down the planner - the calculation time seems to be quick enough that the improvement in graph expansion accounts for it. * Use OMPL to calculate analytic solution when near goal * Make angles multiples of the bin size to stop looping behaviour * Uncrustify * Use faster std::sqrt function * Fix analytic path so that the collision checker has coordinates to check! * Pre-allocate variables in analytic path expansion * Rename typedef to NodeGetter to more accurately describe function * Use distance rather than heuristic to determine when to perform analytic expansion Also force the analytic expansion to run on first iteration in case path is trivial. * Move the check for motion model into the main A* loop * Add copyright notices * Remove comment about relaxing node match tolerances The analytic expansion removes the need for this. * Correctly reset node coordinates when aborting from analytic expansion * Move analytic expansion logic to separate function * Uncrustify * Remove unneeded call to get goal coordinates * Fix the calculation of intervals in the analytic path Reserve the number of candidate nodes we are expecting. Base calculations on intervals rather than points - makes distances between nodes work properly. * Rescale heuristic so that analytic expansions are based on distance * Repeatedly split analytic path in half when checking for collision * Add parameter to control rate of analytic expansion attempts * Uncrustify * Fix incorrect type in templated function * Cpplint * Revert "Repeatedly split analytic path in half when checking for collision" This reverts commit 94d9ee0. There was a marginal speed gain (perhaps!) and the splitting approach made the code harder to understand and maintain. * Uncrustify * Add doxygen comments * Add parameter description for analytic expansion ratio * Set lower limit of 2 on number of iterations between analytic expansions * Reduce expected number of iterations because of analytic completion * Refactor analytic expansion ratio calcs to make logic easier to understand * add readme color * fix linting * ceil from floor (and speed up) * a few updates * fix smac tests * fixing smoother test * remove cost check - to be readded at another time * working last test from debug issues * Update README.md * Update README.md * adding getUseRadius API doxygen Co-authored-by: James Ward <james@robomo.co>
* Fix memory leak (#1900) * Fix memory leak in nav2_recoveries * Fix recovery server memory leak (better interface) * Fix costmap2d memory leak * Fix nav2_navfn_planner memory leak * Fix planner server and navfn planner memory leak * Make all rclcpp::Node::SharedPtr argument passing const * Fix controller server and DWB plugins memory leak * Minor fixes * Fix formatting errors * Change all plugin interfaces to use weak_ptr intead of shared_ptr to parent rclcpp::Node * Convert all SharedPtr to WeakPtr * Check shared_ptr after lock and before dereferencing * Smac/Hybrid-A* planner (#2021) * adding smac_planner to navigation2 metapackage * adding params to metapackage * update config files * adding navfn benchmark testing * updates to costmap_2d for flexility * update planner API for new changes * adding ompl to underlay because ros2 master doesn't contain the rosdep key * patching templated footprint collision checker * fix typo * updating readme config file * Analytic expansion (#43) * Use OMPL to generate heuristics The calculation is run at every planning cycle. It does not seem to slow down the planner - the calculation time seems to be quick enough that the improvement in graph expansion accounts for it. * Use OMPL to calculate analytic solution when near goal * Make angles multiples of the bin size to stop looping behaviour * Uncrustify * Use faster std::sqrt function * Fix analytic path so that the collision checker has coordinates to check! * Pre-allocate variables in analytic path expansion * Rename typedef to NodeGetter to more accurately describe function * Use distance rather than heuristic to determine when to perform analytic expansion Also force the analytic expansion to run on first iteration in case path is trivial. * Move the check for motion model into the main A* loop * Add copyright notices * Remove comment about relaxing node match tolerances The analytic expansion removes the need for this. * Correctly reset node coordinates when aborting from analytic expansion * Move analytic expansion logic to separate function * Uncrustify * Remove unneeded call to get goal coordinates * Fix the calculation of intervals in the analytic path Reserve the number of candidate nodes we are expecting. Base calculations on intervals rather than points - makes distances between nodes work properly. * Rescale heuristic so that analytic expansions are based on distance * Repeatedly split analytic path in half when checking for collision * Add parameter to control rate of analytic expansion attempts * Uncrustify * Fix incorrect type in templated function * Cpplint * Revert "Repeatedly split analytic path in half when checking for collision" This reverts commit 94d9ee0. There was a marginal speed gain (perhaps!) and the splitting approach made the code harder to understand and maintain. * Uncrustify * Add doxygen comments * Add parameter description for analytic expansion ratio * Set lower limit of 2 on number of iterations between analytic expansions * Reduce expected number of iterations because of analytic completion * Refactor analytic expansion ratio calcs to make logic easier to understand * add readme color * fix linting * ceil from floor (and speed up) * a few updates * fix smac tests * fixing smoother test * remove cost check - to be readded at another time * working last test from debug issues * Update README.md * Update README.md * adding getUseRadius API doxygen Co-authored-by: James Ward <james@robomo.co> * Adding additional SmacPlanner tests (#2036) * adding some more tests to smac_planner * addtl smac tests * remove unused functions * adding additional constants and smoothers tests (#2038) * Revert "Fix memory leak (#1900)" This reverts commit 681ccfa. * Changed WeakPtr to SharedPtr for compatibility with Foxy Co-authored-by: Sarthak Mittal <sarthakmittal2608@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: James Ward <james@robomo.co>
* adding smac_planner to navigation2 metapackage * adding params to metapackage * update config files * adding navfn benchmark testing * updates to costmap_2d for flexility * update planner API for new changes * adding ompl to underlay because ros2 master doesn't contain the rosdep key * patching templated footprint collision checker * fix typo * updating readme config file * Analytic expansion (ros-navigation#43) * Use OMPL to generate heuristics The calculation is run at every planning cycle. It does not seem to slow down the planner - the calculation time seems to be quick enough that the improvement in graph expansion accounts for it. * Use OMPL to calculate analytic solution when near goal * Make angles multiples of the bin size to stop looping behaviour * Uncrustify * Use faster std::sqrt function * Fix analytic path so that the collision checker has coordinates to check! * Pre-allocate variables in analytic path expansion * Rename typedef to NodeGetter to more accurately describe function * Use distance rather than heuristic to determine when to perform analytic expansion Also force the analytic expansion to run on first iteration in case path is trivial. * Move the check for motion model into the main A* loop * Add copyright notices * Remove comment about relaxing node match tolerances The analytic expansion removes the need for this. * Correctly reset node coordinates when aborting from analytic expansion * Move analytic expansion logic to separate function * Uncrustify * Remove unneeded call to get goal coordinates * Fix the calculation of intervals in the analytic path Reserve the number of candidate nodes we are expecting. Base calculations on intervals rather than points - makes distances between nodes work properly. * Rescale heuristic so that analytic expansions are based on distance * Repeatedly split analytic path in half when checking for collision * Add parameter to control rate of analytic expansion attempts * Uncrustify * Fix incorrect type in templated function * Cpplint * Revert "Repeatedly split analytic path in half when checking for collision" This reverts commit 94d9ee0. There was a marginal speed gain (perhaps!) and the splitting approach made the code harder to understand and maintain. * Uncrustify * Add doxygen comments * Add parameter description for analytic expansion ratio * Set lower limit of 2 on number of iterations between analytic expansions * Reduce expected number of iterations because of analytic completion * Refactor analytic expansion ratio calcs to make logic easier to understand * add readme color * fix linting * ceil from floor (and speed up) * a few updates * fix smac tests * fixing smoother test * remove cost check - to be readded at another time * working last test from debug issues * Update README.md * Update README.md * adding getUseRadius API doxygen Co-authored-by: James Ward <james@robomo.co>
* adding smac_planner to navigation2 metapackage * adding params to metapackage * update config files * adding navfn benchmark testing * updates to costmap_2d for flexility * update planner API for new changes * adding ompl to underlay because ros2 master doesn't contain the rosdep key * patching templated footprint collision checker * fix typo * updating readme config file * Analytic expansion (ros-navigation#43) * Use OMPL to generate heuristics The calculation is run at every planning cycle. It does not seem to slow down the planner - the calculation time seems to be quick enough that the improvement in graph expansion accounts for it. * Use OMPL to calculate analytic solution when near goal * Make angles multiples of the bin size to stop looping behaviour * Uncrustify * Use faster std::sqrt function * Fix analytic path so that the collision checker has coordinates to check! * Pre-allocate variables in analytic path expansion * Rename typedef to NodeGetter to more accurately describe function * Use distance rather than heuristic to determine when to perform analytic expansion Also force the analytic expansion to run on first iteration in case path is trivial. * Move the check for motion model into the main A* loop * Add copyright notices * Remove comment about relaxing node match tolerances The analytic expansion removes the need for this. * Correctly reset node coordinates when aborting from analytic expansion * Move analytic expansion logic to separate function * Uncrustify * Remove unneeded call to get goal coordinates * Fix the calculation of intervals in the analytic path Reserve the number of candidate nodes we are expecting. Base calculations on intervals rather than points - makes distances between nodes work properly. * Rescale heuristic so that analytic expansions are based on distance * Repeatedly split analytic path in half when checking for collision * Add parameter to control rate of analytic expansion attempts * Uncrustify * Fix incorrect type in templated function * Cpplint * Revert "Repeatedly split analytic path in half when checking for collision" This reverts commit 94d9ee0. There was a marginal speed gain (perhaps!) and the splitting approach made the code harder to understand and maintain. * Uncrustify * Add doxygen comments * Add parameter description for analytic expansion ratio * Set lower limit of 2 on number of iterations between analytic expansions * Reduce expected number of iterations because of analytic completion * Refactor analytic expansion ratio calcs to make logic easier to understand * add readme color * fix linting * ceil from floor (and speed up) * a few updates * fix smac tests * fixing smoother test * remove cost check - to be readded at another time * working last test from debug issues * Update README.md * Update README.md * adding getUseRadius API doxygen Co-authored-by: James Ward <james@robomo.co>
WIP, the original branch is so old that there's literally 300+ merge conflicts so moving over files to a new branch to avoid that since I don't think there were many changes to non-SMAC items.
ros-navigation/docs.nav2.org#92
TODO
Additional performance optimizations described in #2014