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

[james-ward]

I have been playing around with this for the last day or so. I have some observations but no reason as to why my fix works.

I believe the root cause is here:

navigation2/nav2_smac_planner/src/node_hybrid.cpp

Line 677 in d1ad640

if (cost >= OCCUPIED) {

If this is changed to INSCRIBED the planner works.

This led me down a rabbit-hole trying to understand why there might have been a different condition for footprint vs radius calculations. So I started looking at the adjustedFootprintCost function.

navigation2/nav2_smac_planner/src/node_hybrid.cpp

Line 551 in d1ad640

float dist_to_obj = (scale_factor * min_radius - log(cost) + log(253.0f)) / scale_factor;

calculates the distance from robot centre to obstacle by reversing the inflation layer cost function.

The inscribed radius is removed a few lines later:

navigation2/nav2_smac_planner/src/node_hybrid.cpp

Line 554 in d1ad640

dist_to_obj -= min_radius;

(Note, this could be achieved on the first line by leaving out the scale_factor * min_radius term)

The distance-from-inscribed-circle-to-obstacle is sent to the inflation layer to have a cost calculated for this distance:

navigation2/nav2_smac_planner/src/node_hybrid.cpp

Line 561 in d1ad640

inflation_layer->computeCost(dist_to_obj / layered_costmap->getCostmap()->getResolution()));

But when this calculation is performed, the inscribed radius is subtracted again:

navigation2/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp

Line 159 in d1ad640

exp(-1.0 * cost_scaling_factor_ * (distance * resolution_ - inscribed_radius_));

If the double inflation radius subtraction is removed, it breaks again. I think this is because if you do that the maths ends up converting from cost->distance->cost, but because costs are quantised to integer values it ends up rounding some of them down which is a problem if the original cost was 253 and it goes to 252 - invalid becomes valid.
Of course, if the double subtraction is removed then there is no point to the adjustedFootprintCost function. I don't understand the intention/implementation of this function well enough to suggest a fix.

tl;dr Changing the rejection condition from OCCUPIED to INSCRIBED makes things work. The adjustedFootprintCost function probably needs a revisit.

[SteveMacenski]

If this is changed to INSCRIBED the planner works.

Graphically, can you show the planning expansion footprints before and after (we have a debug topic for that now)? I want to make sure its not overly pruning all possible footprints that any part of them are in collision with the environment. Obviously if you have confined space and a big robot, parts of the robot's footprint need to be able to enter inscribed space.

why there might have been a different condition for footprint vs radius calculations

Allow me to explain my thinking.

Robot radius checks happens based on the robot's center cost. So the heuristic's use of getCost(x,y) is the same cost as would be seen when computing the traversal costs in getTraversalCost. All good there.

However, robot footprint checks however compute the cost of the footprint edges, not the center cost, and return the single highest value (isNodeValid's collision checker). Thus, to make sure that the heuristic's cost is measuring the same thing as traversal cost, we need to consider what the approximate cost at the edge of a footprint would be at that heuristic expansion point. If currently expanding at cell getCost(x,y) as the center, we need to add back in the radius and check what that cost would be, which is what adjustedFootprintCost aims to do.

Its not exact (since no orientation and not using the footprint), but its approximate and we use the inscribed radius, not the circumscribed radius, so its conservative. The cost used by the heuristic is thus always at least the cost that the robot would actually experience with the traversal cost function. If the robot is non-circular, it could be higher actually, but getting closer to the value (and measuring the same thing between T and H) fixed some problems with long-range planning in confined environments from some users. The better the heuristic is at estimating, the more efficient the search.

I'll note that this feature was added while I was working on a train in the UK, so I'm just as suspicious as you now that maybe in fixing that problem I introduced another in missing a detail - but it did hugely improve the problem I was tackling at the time, so its not all bad!

But when this calculation is performed, the inscribed radius is subtracted again: navigation2/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp

I'm thinking now that this was an oversight on my part. With the context above on the heuristic needing footprint-edge costs instead of center-costs, I'd like to know what you suggest we do here instead? This treatment of the adjusted costs did really improve performance for non-circular robots non-trivially, so I think that's still the right thing to do, even if the implementation of how we accomplish that changes.

I'm hoping you can review my other uses of computeCost, as around the same time, I changed some of the collision checking in MPPI, Smac, and a utils method to use that method and now I feel the need to have an outside opinion validate me or tell me where I messed that up.

Thanks for this dive!

---

(Note, this could be achieved on the first line by leaving out the scale_factor * min_radius term)

Oh, if we keep that in there, please do open a PR for that. I was using the same conversion code from another part of the codebase and then modified it to remove the radius without adjusting the original term.