Ask Your Question

Navfn vs Smac Planner

asked 2021-02-22 03:29:48 -0600

madmax gravatar image

updated 2021-02-22 03:31:23 -0600

Was just trying out the new smac planner in foxy and I saw some unexpected behavior.

It looks like smac planner is ignoring costs when obstacles are too small.
Navfn on the other side doesn't show this behavior.


navfn gif


smac gif

Costmap looks fine, and on the smac planner param side I think they seem reasonable.
But I am not sure if there is a bug or misconfiguration on my side.

    planner_plugins: ["GridBased"]
    use_sim_time: True

    # smac_planner/SmacPlanner is replaced with nav2_navfn_planner/NavfnPlanner, params are from smac, but navfn works better right now
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5                    # tolerance for planning if unable to reach exact pose, in meters, for 2D node
      downsample_costmap: false         # whether or not to downsample the map
      downsampling_factor: 1            # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
      allow_unknown: false              # allow traveling in unknown space
      max_iterations: -1                # maximum total iterations to search for before failing
      max_on_approach_iterations: 1000  # maximum number of iterations to attempt to reach goal once in tolerance, 2D only
      max_planning_time: 2.0            # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning.
      smooth_path: false                # Whether to smooth searched path
      motion_model_for_search: "DUBIN"  # 2D Moore, Von Neumann; SE2 Dubin, Redds-Shepp
      angle_quantization_bins: 72       # For SE2 node: Number of angle bins for search, must be 1 for 2D node (no angle search)
      minimum_turning_radius: 0.20      # For SE2 node & smoother: minimum turning radius in m of path / vehicle
      reverse_penalty: 2.1              # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1
      change_penalty: 0.20              # For SE2 node: penalty to apply if motion is changing directions, must be >= 0
      non_straight_penalty: 1.05        # For SE2 node: penalty to apply if motion is non-straight, must be => 1
      cost_penalty: 1.3                 # For SE2 node: penalty to apply to higher cost zones

          w_curve: 30.0                 # weight to minimize curvature of path
          w_dist: 0.0                   # weight to bind path to original as optional replacement for cost weight
          w_smooth: 30000.0             # weight to maximize smoothness of path
          w_cost: 0.025                 # weight to steer robot away from collision and cost
          cost_scaling_factor: 10.0     # this should match the inflation layer's parameter

        # I do not recommend users mess with this unless they're doing production tuning
          max_time: 0.10                # maximum compute time for smoother
          max_iterations: 500           # max iterations of smoother
          debug_optimizer: false        # print debug info
          gradient_tol: 1.0e-10
          fn_tol: 1.0e-20
          param_tol: 1.0e-15
            min_line_search_step_size: 1.0e-20
            max_num_line_search_step_size_iterations: 50
            line_search_sufficient_function_decrease: 1.0e-20
            max_num_line_search_direction_restarts: 10
            max_line_search_step_expansion: 50
edit retag flag offensive close merge delete


Please file a ticket on Nav2 about this with this info and reproduction examples - I haven't seen this behavior before and we should look into it. It's probably an easy fix, but we just need to understand fully what the issue is first.

stevemacenski gravatar image stevemacenski  ( 2021-02-23 13:22:55 -0600 )edit

I'd be curious to know if you used Nav2 main branch if you saw these issues. This looks awfully similar to a ticket that resulted in a patch that will be in Galactic and newer on the same behavior with NavFn (ABI incompatiable with Foxy, but I'd be open to a PR to make a different fix for Foxy to resolve this behavior) that was the results of current / clearing operations, not the algorithms themselves.

stevemacenski gravatar image stevemacenski  ( 2021-02-23 13:26:23 -0600 )edit

I used foxy debs. Issue is opened here.

madmax gravatar image madmax  ( 2021-02-26 02:54:22 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2021-02-26 18:34:46 -0600

We're going to take this to the issue tracker in Nav2.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2021-02-22 03:29:48 -0600

Seen: 40 times

Last updated: Feb 26