Hybrid A* search heavily depends on inflation layer cost_scaling_factor value

[corot]

Bug report

Not sure if this is a bug, but the behavior is surely puzzling, so I think it's worth reporting. I found that, for certain values of cost_scaling_factor parameter (I saw with 4.0 and 5.0), the Hybrid A* search becomes way less effective. Indeed looks like the obstacles heuristic somehow stops working, e,g

cost_scaling_factor = 3

cost_scaling_factor = 8

cost_scaling_factor = 5 !!!!

This only happens with a non-circular footprint, so I guess it's related with how we calculate the cost here.
But I cannot imagine how changing this values to an intermediate value can have such a dramatic effect. In the turtlebot 3 little world, it still finds a valid path, but I discover this on a much larger map where it was unable to find a path in several seconds.

Required Info:

• Operating System: ubuntu 22.04
• ROS2 Version: rolling
• Version or commit hash: b6693c6 (current main)
• DDS implementation: Fast-RTPS

Steps to reproduce issue

1. Replace nav2_bringup/params/nav2_params.yaml with the parameter file below. It's similar to the default params except:

• uses hybrid A* as planner
• uses pure pursuit as controller (irrelevant for the issue)
• uses footprint instead of robot radius

2. Replace nav2_bringup/maps/turtlebot3_world.pgm with the attached map (with a wall bisecting the map with a single passage far from the robot)

3. Run TB3 demo. Don not move the robot

ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False

4. Add display for /expansions topic on RViz (planner_server/GridBased/debug_visualizations must be true)
5. Send goals to the other half of the world, as in the pictures above

amcl:
  ros__parameters:
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: "base_footprint"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 100.0
    laser_min_range: -1.0
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 2000
    min_particles: 500
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 1.0
    update_min_a: 0.2
    update_min_d: 0.25
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05
    scan_topic: scan

bt_navigator:
  ros__parameters:
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    wait_for_service_timeout: 1000
    action_server_result_timeout: 900.0
    navigators: ["navigate_to_pose", "navigate_through_poses"]
    navigate_to_pose:
      plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
    navigate_through_poses:
      plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.

    # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings).
    # Built-in plugins are added automatically
    # plugin_lib_names: []

    error_code_names:
      - compute_path_error_code
      - follow_path_error_code

controller_server:
  ros__parameters:
    controller_frequency: 20.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 0.3
    progress_checker_plugins: ["progress_checker"]
    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
    controller_plugins: ["FollowPath"]
    use_realtime_priority: false

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    # Goal checker parameters
    #precise_goal_checker:
    #  plugin: "nav2_controller::SimpleGoalChecker"
    #  xy_goal_tolerance: 0.25
    #  yaw_goal_tolerance: 0.25
    #  stateful: True
    general_goal_checker:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.05
      yaw_goal_tolerance: 0.1
    # RPP parameters
    FollowPath:
      plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
      allow_reversing: true
      desired_linear_vel: 0.5
      lookahead_dist: 0.6
      min_lookahead_dist: 0.3
      max_lookahead_dist: 0.9
      lookahead_time: 1.5
      rotate_to_heading_angular_vel: 1.8
      transform_tolerance: 0.1
      use_velocity_scaled_lookahead_dist: false
      min_approach_linear_velocity: 0.05
      approach_velocity_scaling_dist: 1.0
      use_collision_detection: true
      max_allowed_time_to_collision_up_to_carrot: 1.0
      use_regulated_linear_velocity_scaling: true
      use_cost_regulated_linear_velocity_scaling: false
      regulated_linear_scaling_min_radius: 0.9
      regulated_linear_scaling_min_speed: 0.25
      use_rotate_to_heading: true
      rotate_to_heading_min_angle: 0.785
      max_angular_accel: 3.2
      max_robot_pose_search_dist: 10.0
      use_interpolation: false
      cost_scaling_dist: 0.3
      cost_scaling_gain: 1.0
      inflation_cost_scaling_factor: 3.0

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      footprint: "[[-0.12,-0.12],[-0.12,0.12],[0.12,0.12],[0.12,-0.12]]"
      footprint_padding: 0.01
      plugins: ["voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.7
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      always_send_full_costmap: True

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      footprint: "[[-0.12,-0.12],[-0.12,0.12],[0.12,0.12],[0.12,-0.12]]"
      footprint_padding: 0.01
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 5.0
        inflation_radius: 0.7
      always_send_full_costmap: True

# The yaml_filename does not need to be specified since it going to be set by defaults in launch.
# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py
# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used.
# map_server:
#   ros__parameters:
#     yaml_filename: ""

map_saver:
  ros__parameters:
    save_map_timeout: 5.0
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65
    map_subscribe_transient_local: True

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_smac_planner::SmacPlannerHybrid"
      debug_visualizations: true
      tolerance: 0.1
      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: 1000000             # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
      max_on_approach_iterations: 1000    # maximum number of iterations to attempt to reach goal once in tolerance
      max_planning_time: 3.5              # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning.
      motion_model_for_search: REEDS_SHEPP    # For Hybrid Dubin, REDDS-SHEPP
      cost_travel_multiplier: 2.0         # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*.
      angle_quantization_bins: 64         # For Hybrid nodes: Number of angle bins for search, must be 1 for 2D node (no angle search)
      analytic_expansion_ratio: 3.5       # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach.
      analytic_expansion_max_length: 3.0    # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting (in meters). This should be scaled with minimum turning radius and be no less than 4-5x the minimum radius
      minimum_turning_radius: 0.40        # For Hybrid/Lattice nodes: minimum turning radius in m of path / vehicle
      reverse_penalty: 1.0                # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1
      change_penalty: 0.0                 # For Hybrid nodes: penalty to apply if motion is changing directions, must be >= 0
      non_straight_penalty: 1.20          # For Hybrid nodes: penalty to apply if motion is non-straight, must be => 1
      cost_penalty: 2.0                   # For Hybrid nodes: penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
      retrospective_penalty: 0.025        # For Hybrid/Lattice nodes: penalty to prefer later maneuvers before earlier along the path. Saves search time since earlier nodes are not expanded until it is necessary. Must be >= 0.0 and <= 1.0
      rotation_penalty: 5.0               # For Lattice node: Penalty to apply only to pure rotate in place commands when using minimum control sets containing rotate in place primitives. This should always be set sufficiently high to weight against this action unless strictly necessary for obstacle avoidance or there may be frequent discontinuities in the plan where it requests the robot to rotate in place to short-cut an otherwise smooth path for marginal path distance savings.
      lookup_table_size: 20.0               # For Hybrid nodes: Size of the dubin/reeds-sheep distance window to cache, in meters.
      cache_obstacle_heuristic: True      # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.
      allow_reverse_expansion: False      # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step.
      smooth_path: True                   # For Lattice/Hybrid nodes: Whether or not to smooth the path, always true for 2D nodes.
      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 1.0e-10
        do_refinement: true               # Whether to recursively run the smoother 3 times on the results from prior runs to refine the results further

smoother_server:
  ros__parameters:
    use_sim_time: True
    smoother_plugins: ["simple_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
      tolerance: 1.0e-10
      max_its: 1000
      do_refinement: True

behavior_server:
  ros__parameters:
    local_costmap_topic: local_costmap/costmap_raw
    global_costmap_topic: global_costmap/costmap_raw
    local_footprint_topic: local_costmap/published_footprint
    global_footprint_topic: global_costmap/published_footprint
    cycle_frequency: 10.0
    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
    spin:
      plugin: "nav2_behaviors::Spin"
    backup:
      plugin: "nav2_behaviors::BackUp"
    drive_on_heading:
      plugin: "nav2_behaviors::DriveOnHeading"
    wait:
      plugin: "nav2_behaviors::Wait"
    assisted_teleop:
      plugin: "nav2_behaviors::AssistedTeleop"
    local_frame: odom
    global_frame: map
    robot_base_frame: base_link
    transform_tolerance: 0.1
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

waypoint_follower:
  ros__parameters:
    loop_rate: 20
    stop_on_failure: false
    action_server_result_timeout: 900.0
    waypoint_task_executor_plugin: "wait_at_waypoint"
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      enabled: True
      waypoint_pause_duration: 200

velocity_smoother:
  ros__parameters:
    smoothing_frequency: 20.0
    scale_velocities: False
    feedback: "OPEN_LOOP"
    max_velocity: [1.2, 0.0, 2.0]
    min_velocity: [-1.2, 0.0, -2.0]
    max_accel: [2.5, 0.0, 3.2]
    max_decel: [-2.5, 0.0, -3.2]
    odom_topic: "odom"
    odom_duration: 0.1
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 1.0

collision_monitor:
  ros__parameters:
    base_frame_id: "base_footprint"
    odom_frame_id: "odom"
    cmd_vel_in_topic: "cmd_vel_smoothed"
    cmd_vel_out_topic: "cmd_vel"
    state_topic: "collision_monitor_state"
    transform_tolerance: 0.2
    source_timeout: 1.0
    base_shift_correction: True
    stop_pub_timeout: 2.0
    # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
    # and robot footprint for "approach" action type.
    polygons: ["FootprintApproach"]
    FootprintApproach:
      type: "polygon"
      action_type: "approach"
      footprint_topic: "/local_costmap/published_footprint"
      time_before_collision: 1.2
      simulation_time_step: 0.1
      min_points: 6
      visualize: False
      enabled: True
    observation_sources: ["scan"]
    scan:
      type: "scan"
      topic: "scan"
      min_height: 0.15
      max_height: 2.0
      enabled: True

[SteveMacenski]

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

[corot]

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

[SteveMacenski]

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.

[corot]

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...

[ahopsu]

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 🙂

[ahopsu]

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.

[SteveMacenski]

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

[tonynajjar]

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