Path Planning with nav2_smac_planner/SmacPlannerHybrid Generates Wobbly Paths Instead of Straight Lines

Bug report

Required Info:

Operating System:
- Ubuntu 22.04
ROS2 Version:
- Humble binaries
Version or commit hash:
- ros-humble-navigation2 1.1.15-1jammy.20240530.113754
DDS implementation:
- Fast-RTPS

Steps to reproduce issue

Set up a navigation environment with open spaces and 0-cost areas.
Use nav2_smac_planner/SmacPlannerHybrid to plan a path between two points having the same y coordinate and angle.
Ensure the points are placed in open spaces with a straight line being the shortest path.

Observe the generated path.

planner_server:
ros__parameters:
expected_planner_frequency: 1.0
use_sim_time: True
planner_plugins: ["GridBased"]
GridBased:
  plugin: "nav2_smac_planner/SmacPlannerHybrid"
  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)
  tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found.
  allow_unknown: true # 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 after within tolerances to continue to try to find exact solution
  max_planning_time: 5.0 # max time in s for planner to plan, smooth
  motion_model_for_search: "REEDS_SHEPP" # Hybrid-A* Dubin, Redds-Shepp
  angle_quantization_bins: 72 # Number of angle bins for search
  analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach.
  analytic_expansion_max_length: 1.5 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting
  minimum_turning_radius: 0.40 # minimum turning radius in m of path / vehicle
  reverse_penalty: 1.0 # Penalty to apply if motion is reversing, must be => 1
  change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0
  non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1
  cost_penalty: 3.0 # 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.015
  lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters.
  cache_obstacle_heuristic: false # 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.
  debug_visualizations: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance.
  use_quadratic_cost_penalty: False
  downsample_obstacle_heuristic: True
  allow_primitive_interpolation: False
  smooth_path: True # If true, does a simple and quick smoothing post-processing to the path
  smoother:
    max_iterations: 1000
    w_smooth: 0.3
    w_data: 0.2
    tolerance: 1.0e-10
    do_refinement: true
    refinement_num: 2

Expected behavior

The path generated by nav2_smac_planner/SmacPlannerHybrid should be a straight line between the two points all the time, as they are in open spaces with 0-cost areas in the middle.

Actual behavior

The path generated is often not a straight line and only sometimes follows a straight line, resulting in wobbly motions.

https://github.com/user-attachments/assets/785beb10-9842-4314-8281-1c2f3528d521

Interesting. The downward motion is definitely due to the analytic expansion but the upward motion is from search accounting for cost. I'm not sure off hand why it would be that one direction would use one but not the other. Is this always deterministic or 'sometimes' or 'most of the time'?

I'd be curious if you inverted the situation (do this again, but rotate the robot 180 degrees so that forward is now reversing) what happens. That would be useful data for me to understand where the problem potentially lies. One of the 2 spots is in higher cost than the other to start, so I'm curious if that is related.

What happens if you increase analytic_expansion_max_length: 1.5? That's pretty short so it could be that you're just right off that limitation for analytic expansion to be used in the other direction (though seems awfully deterministic).
Is it the forward or reverse of the robot that has the wiggle?

I'd also be interested if you logged in the analytic expansions to see why it wasn't taking that expansion (assuming analytic_expansion_max_length doesn't work). I wonder if there's another setting or some legit reason why that was not valid to take, so finding the condition where the analytic expansion update on initial planning is rejected would be important root cause to understand.

Maybe even ignore isNodeValid to force its use to visualize the unused expansion - just make sure the robot doesn't actually move or your safety system won't allow it to collide. This would just be good for a visual

This definitely looks like something we should be able to resolve though! It probably won't be too difficult for me once I understand the "why"

ros-navigation / navigation2