Fix Smac2D final pose to match reached goal

[user247-tai]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(add tickets here #6265)
Primary OS tested on	(Ubuntu)
Robotic platform tested on	(Our robot)
Does this PR contain AI generated software?	(No)

---

Description of contribution in a few bullet points

• Updated SmacPlanner2D to return the requested goal position as the final path pose when the search reaches the goal cell.
• Preserved tolerance fallback behavior by only snapping the final pose when the returned path actually ends in the requested goal cell.
• Made the same-cell start/goal shortcut return the requested goal pose while preserving use_final_approach_orientation behavior.

Description of documentation updates required from your changes

Description of how this change was tested

• Tested manually with the ComputePathToPose action using SmacPlanner2D with this configuration:

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]

    GridBased:
      plugin: "nav2_smac_planner::SmacPlanner2D" # In Iron and older versions, "/" was used instead of "::"
      tolerance: 0.25                        # 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: true                   # allow traveling in unknown space
      max_iterations: -1               # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
      max_on_approach_iterations: 10000      # maximum number of iterations to attempt to reach goal once in tolerance
      max_planning_time: 2.0                # max time in s for planner to plan, smooth
      cost_travel_multiplier: 2.0           # 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*.
      use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true)
      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 1.0e-10

• Verified the existing behavior before the change: when the planner reached the goal cell, the returned path succeeded but the final path pose position could differ from the requested goal pose due to grid-cell coordinate conversion:

• Verified the updated behavior after the change: when the planner reaches the requested goal cell, the final path pose position matches the requested goal position exactly:

• Verified tolerance behavior is preserved after the change: when the exact goal cell cannot be reached and the planner falls back to a valid endpoint within the configured tolerance, the final path pose remains at the tolerance endpoint and is not overwritten with the unreachable requested goal pose:

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[user247-tai]

I believe the reason we have this issue is that SmacPlanner2D converts the requested goal from world coordinates into continuous map coordinates, but the A* search itself operates on discrete costmap cell indices. Once the planner reaches the goal cell, the path is backtraced from integer node indices and then converted back to world coordinates, so the fractional offset of the requested goal inside that cell is lost.

As a result, even though the planner correctly reached the goal cell, the final returned path pose can differ from the requested goal pose by some fraction of the map resolution. This change restores the original requested goal position only when the returned path actually ends in the requested goal cell.

[SteveMacenski]

Please fix the CI reported failures below, otherwise LGTM

[user247-tai]

It looks like a change in the tf2_ros::CreateTimerROS API in ROS 2 Rolling caused the ci/circleci: core_build fail in the nav2_util package.

[SteveMacenski]

I'm in the middle of fixing CI (#6281). Once that's good, I'll ping you back here to rebase/pull in so CI turns over. Otherwise LGTM assuming your PR works at that point!

[mergify]

Tick the box to add this pull request to the merge queue (same as @mergifyio queue).

• Queue this pull request

[SteveMacenski]

Merging that PR imminently, check that its been merged, rebase/pull in main and I can merge if CI passes for your work!

[user247-tai]

@SteveMacenski, It looks like the ci/circleci: algorithm_build was failed because it hit the execution time limit. Do you have a way to handle this ?