Smac/Hybrid-A* planner (ros-navigation#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 (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 <[email protected]>

Author: SteveMacenski

nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml

@@ -161,6 +161,7 @@ local_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
+        inflation_radius: 0.55
       obstacle_layer:
         plugin: "nav2_costmap_2d::ObstacleLayer"
         enabled: True

nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml

@@ -161,6 +161,7 @@ local_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
+        inflation_radius: 0.55
       obstacle_layer:
         plugin: "nav2_costmap_2d::ObstacleLayer"
         enabled: True

nav2_bringup/bringup/params/nav2_params.yaml

@@ -168,6 +168,7 @@ local_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
+        inflation_radius: 0.55
       voxel_layer:
         plugin: "nav2_costmap_2d::VoxelLayer"
         enabled: True
@@ -204,6 +205,7 @@ global_costmap:
       use_sim_time: True
       robot_radius: 0.22
       resolution: 0.05
+      track_unknown_space: true
       plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
       obstacle_layer:
         plugin: "nav2_costmap_2d::ObstacleLayer"
@@ -220,6 +222,8 @@ global_costmap:
         map_subscribe_transient_local: True
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
       always_send_full_costmap: True
   global_costmap_client:
     ros__parameters:

nav2_bringup/bringup/rviz/nav2_default_view.rviz

@@ -249,6 +249,20 @@ Visualization Manager:
             Value: /global_costmap/costmap
           Use Timestamp: false
           Value: true
+        - Alpha: 0.3
+          Class: rviz_default_plugins/Map
+          Color Scheme: costmap
+          Draw Behind: false
+          Enabled: true
+          Name: Downsampled Costmap
+          Topic:
+            Depth: 1
+            Durability Policy: Transient Local
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /downsampled_costmap
+          Use Timestamp: false
+          Value: true
         - Alpha: 1
           Buffer Length: 1
           Class: rviz_default_plugins/Path