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

[SteveMacenski]

I'm a little unclear looking at this. You show a red arrow on a goal, but that is clearly not the path's end point since the path continues far after it. So that's not the goal pose to apply with the tolerance.

Please provide more detail.

If you're concatenating paths together: note that your box is on the edges of map cells (e.g. you're on the line segments bordering costmap cells, not in the middle of costmap cells). So, its very likely that this is actually within tolerance or an exact solution, since we obtain a path in that cell -- since that cell is measured from the centers of cells, which that path clearly passes through. There may be misunderstandings of the coordinate system on your side 😄

[aniket11bh]

@SteveMacenski The red arrow is actually one of the goal_poses passed to the navigate through poses navigator.
You can ignore the blue line, as it's just a straight line connecting the subsequent goal points for my own purpose. (Probably shouldn't have shown in the first place)

navigator = BasicNavigator()
path = navigator.getPathThroughPoses(initial_pose, goal_poses)
navigator.goThroughPoses(goal_poses)

I see your point, that the goal point is actually the centre of the closest cell to the input goal point.
Correct me if i am wrong. Though, the subsequent points on the path can be anywhere on the cell (still maintaining the constraint of cost map resolution), so why can't the goal point also can't be anywhere on the cell ?

[SteveMacenski]

Correct me if i am wrong. Though, the subsequent points on the path can be anywhere on the cell (still maintaining the constraint of cost map resolution), so why can't the goal point also can't be anywhere on the cell ?

We convert the world to map coordinates, which are in unsigned int positions on the costmap (https://github.com/ros-planning/navigation2/blob/main/nav2_smac_planner/src/smac_planner_hybrid.cpp#L348-L362). However, you are correct that we maintain continuous coordinates as floats within the node's pose member representing grid space and we could potentially have a different worldToMapContinous function for the feasible planners and provide those as floats as well.

We even convert them to floats for goal_coords (https://github.com/ros-planning/navigation2/blob/e066ed1dee18daad2c6a95a49e5db4fc6407f77e/nav2_smac_planner/src/a_star.cpp#L200C31-L203) once we set them and then set them into the goal's pose at the end of that method.

That seems like a relatively straight forward contribution @aniket11bh if you can do that + open a PR! You'd essentially need to:

• Create a worldToMapContinous to compute the decimals for the world to map conversion.
• Change the setGoal to use floats as inputs and static_cast to unsigned int's for the getIndex method.
• Possibly do the same with setStart as well

I don't see any other complexity of why that would cause problems + improve accuracy! Are you open to doing that and testing to make sure it solves your problem?

[aniket11bh]

@SteveMacenski the solution looks straightforward. I can take this up but mostly it will be after December. As the current tolerance is just enough for now.

[SteveMacenski]

Imminently to be merged