SmacPlannerHybrid does not respect tolerance

[aniket11bh]

Bug report

Required Info:

• Operating System:
  • Ubuntu 22.04
• ROS2 Version:
  • humble
• Version or commit hash:
  • ros-humble-navigation2 1.1.12-1jammy.20231005.224358
• DDS implementation:
  • cycloneDDS

Steps to reproduce issue

• Give a set of poses, and use smac planner hybrid for generating the path.
• In the attached figure, the grid size (resolution) is 1m.

Expected behavior

SmacPlannerHybrid should generate path within tolerance, if it is unable to generate within tolerance it should fail.

Actual behavior

Planner is generating paths, but it not generating paths within given tolerance.

Additional information

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    use_sim_time: True

    GridBased:
      plugin: "nav2_smac_planner/SmacPlannerHybrid"
      tolerance: 0.01                      # tolerance for planning if unable to reach exact pose, in meters
      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: 5.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.
      motion_model_for_search: "DUBIN"    # For Hybrid Dubin, Redds-Shepp

      minimum_turning_radius: 10.0        # For Hybrid/Lattice nodes: minimum turning radius in m of path / vehicle
      analytic_expansion_max_length: 30.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
      analytic_expansion_ratio: 3.5       # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach.
      angle_quantization_bins: 72         # For Hybrid nodes: Number of angle bins for search, must be 1 for 2D node (no angle search)
      cache_obstacle_heuristic: false      # 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.  

      change_penalty: 0.3                 # For Hybrid nodes: penalty to apply if motion is changing directions, must be >= 0
      cost_penalty: 10.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.
      reverse_penalty: 2.0                # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1

      non_straight_penalty: 2.0           # For Hybrid nodes: penalty to apply if motion is non-straight, must be => 1
      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
      lookup_table_size: 20.0             # For Hybrid nodes: Size of the dubin/reeds-sheep distance window to cache, in meters.
      smooth_path: False                   # 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: 1e-10
        do_refinement: true               # Whether to recursively run the smoother 3 times on the results from prior runs to refine the results further