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

[osama-z-salah]

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

1. Set up a navigation environment with open spaces and 0-cost areas.
2. Use nav2_smac_planner/SmacPlannerHybrid to plan a path between two points having the same y coordinate and angle.
3. Ensure the points are placed in open spaces with a straight line being the shortest path.
4. 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.

global_IcXEJSWo.mp4

[SteveMacenski]

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"

[issouker97]

can you tell me how to launch this on ros2, i am new