Hybrid A* search heavily depends on inflation layer cost_scaling_factor value about navigation2 HOT 8 OPEN

How often does this happen for the choice of cost scaling factor? I.e. is what you show every single time or just on a rare occasion? Are you (reasonably) sure this doesn't also happen for values of 3/8?

This is odd, I agree. When this happens, printing some data would be nice to see what these are returning in terms of cost.

If you uncomment this block (and adjust the resolution/center), what's the heuristic look like? https://github.com/open-navigation/navigation2/blob/main/nav2_smac_planner/src/node_hybrid.cpp#L717-L732

from navigation2.

Happens consistently, 100% reproducible. Yes, I tried a bunch of values:

• 0, 1, 5, 6: KO
• rest integer look fine

as weird as it can get!

I enabled the code, and I only get zeros in either case

from navigation2.

OK, I won't get to it this week, but this has been noted by me as something I'm going to personally investigate. If you have time / interest to look into it yourself more to make progress, that can make this move faster to a resolution.

from navigation2.

OK, I won't get to it this week, but this has been noted by me as something I'm going to personally investigate. If you have time / interest to look into it yourself more to make progress, that can make this move faster to a resolution.

sorry.... I already tried and failed to find the problem...

from navigation2.

I can confirm that I have the exact same issue. I was (in Galactic) used to have the cost_scaling_factor with value 5.

But since upgrading to Humble (patch 8 from February 2024) recently, I wondered why the Humble's SmacPlannerHybrid was barely (/ not at all) able to find any routes when using bigger robot footprints. I tested also with the Rolling version (downloaded/build at 4.4.2024, almost synonym to Jazzy at this point) to see with the debug_visualizations option, how the routes are generated, and got the following results:

With a footprint size of around 4m x 4m, things still work:

But when increasing it to over 5m x 5m, things seem to break:

But when using the 5m x 5m and reducing the cost_scaling_factor from 5 to 3, everything works again nicely:

The (relevant) parameters used (for 4m x 4m, commented for 5m x 5m):

planner_server:
  ros__parameters:
    planner_plugins:
    - GridBased
    use_sim_time: false
    GridBased:
      plugin: nav2_smac_planner::SmacPlannerHybrid
      debug_visualizations: true
      tolerance: 0.01
      downsample_costmap: false
      downsampling_factor: 1
      allow_unknown: false
      max_iterations: 100000
      max_on_approach_iterations: 1000
      max_planning_time: 10.0
      motion_model_for_search: DUBIN
      cost_travel_multiplier: 2.0
      angle_quantization_bins: 64
      analytic_expansion_ratio: 3.5
      analytic_expansion_max_length: 2.0
      minimum_turning_radius: 0.4
      reverse_penalty: 2.1
      change_penalty: 0.0
      non_straight_penalty: 1.2
      cost_penalty: 2.0
      retrospective_penalty: 0.025
      rotation_penalty: 5.0
      lookup_table_size: 20.0
      cache_obstacle_heuristic: true
      allow_reverse_expansion: false
      smooth_path: true
      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 1e-10
        do_refinement: true
global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: false
      rolling_window: false
      resolution: 0.05
      plugins:
      - static_layer
      - inflation_layer
      inflation_layer:
        plugin: nav2_costmap_2d::InflationLayer
        enabled: true
        inflation_radius: 4.16 # 5.46 for 5m x 5m (autoscripted)
        cost_scaling_factor: 5.0
        inflate_unknown: true
        inflate_around_unknown: true
      static_layer:
        plugin: nav2_costmap_2d::StaticLayer
        map_subscribe_transient_local: true
        enabled: true
        subscribe_to_updates: true
        transform_tolerance: 0.1
      always_send_full_costmap: false
      footprint: '[[2.08, 2.08], [-2.08, 2.08], [-2.08, -2.08], [2.08, -2.08]]' # 4m x 4m, works always
     #footprint: '[[2.73, 2.73], [-2.73, 2.73], [-2.73, -2.73], [2.73, -2.73]]' # 5m x 5m, does not work with cost scaling factor 5

This "cost scaling factor 5" behaviour happens both in the Humble version as well as in the one month old Rolling version. In Galactic, there is not such an issue. Seems that there was quite a lot of (heuristical calculation) changes to the Smac Planner in the migration from Galactic to Humble, so likely something there created this "side effect". E.g.

"Replacing the wavefront heuristic with a new, and novel, heuristic dubbed the obstacle heuristic."

sound like a potential source for this, but I haven't done any further/deeper investigation of the actual cause. Luckily the issue is avoidable by just a simple parameter value change (thanks to @corot for finding this affecting parameter), so that will be enough for me now 🙂

from navigation2.

As an extra info, the cost_scaling_factor with value 3 is not again enough when the footprint goes even bigger. Here, a frame size of 13m x 13m will have again the same issues:

      inflation_layer:
        plugin: nav2_costmap_2d::InflationLayer
        enabled: true
        inflation_radius: 13.26 # autoscripted to match footprint size
        cost_scaling_factor: 3.0
        inflate_unknown: true
        inflate_around_unknown: true
      static_layer:
        plugin: nav2_costmap_2d::StaticLayer
        map_subscribe_transient_local: true
        enabled: true
        subscribe_to_updates: true
        transform_tolerance: 0.1
      always_send_full_costmap: false
      footprint: '[[6.63, 6.63], [-6.63, 6.63], [-6.63, -6.63], [6.63, -6.63]]'

In the end, putting the cost_scaling_factor to (its default value) 1 seems to be the best option here, as it does not seem to cause weird behaviours with a wide range of robot footprint sizes.

from navigation2.

That is so frecking strange, thanks @ahopsu for more follow up and detail. This definitely needs to be looked into.

from navigation2.

I was debugging another issue but I think I ran into this as well with SmacPlannerHybrid

I was using the planner playground from @doisyg, here is my branch with the params to reproduce: https://github.com/angsa-robotics/planner_playground/tree/custom-testing-angsa

from navigation2.