[Smac 2D planner] Path end point has always a 0 orientation

[doisyg]

Bug report

Required Info:

• Operating System:
  • Ubuntu
• ROS2 Version:
  • Galactic bin
• Version or commit hash: last bin for ros2, src sync with main for nav2
• DDS implementation:
  • Cyclone

Steps to reproduce issue

Ask the planner using the Smac 2D plugin to generate a path to a goal with a specific orientation

Expected behavior

Then end point of the path should have the same orientation as the goal sent

Actual behavior

Then end point of the path has a 0 orientation

[SteveMacenski]

Ask the planner using the Smac 2D plugin to generate a path to a goal with a specific orientation

This is a 2D A*, that's not possible the same way its not for any other 2D A* including NavFn or any other planner for that matter. Its very typical as you know to rotate to heading once within tolerance of the goal with a TEB/DWB/RPP/etc. That is common behavior. To get an approach angle, you need a kinematically feasible planner, like Hybrid-A*...

The 0 is the same 0 that all others provide. I think what you're pointing out is that we actually do populate orientations for the other path points in the plan based on the tangent of the path. We should populate the final point as well, which I'd merge a PR to do (its in the smoother). But note that NavFn and anything you used before also provided 0s as well for the path.

It should be easy to just add to the end goal point the final orientation

[doisyg]

Ask the planner using the Smac 2D plugin to generate a path to a goal with a specific orientation

This is a 2D A*, that's not possible the same way its not for any other 2D A* including NavFn or any other planner for that matter. Its very typical as you know to rotate to heading once within tolerance of the goal with a TEB/DWB/RPP/etc. That is common behavior. To get an approach angle, you need a kinematically feasible planner, like Hybrid-A*...

The 0 is the same 0 that all others provide. I think what you're pointing out is that we actually do populate orientations for the other path points in the plan based on the tangent of the path. We should populate the final point as well, which I'd merge a PR to do (its in the smoother). But note that NavFn and anything you used before also provided 0s as well for the path.

It should be easy to just add to the end goal point the final orientation

NavFn and everything I used before populates the end point with the goal orientation (or something close), that's the only way the local planner can have access to the goal orientation.
Here is what I observe:

With SmacPlanner2D
[planner_server]: Found valid path of size 23 to (11.05, 3.50, 1.91)
[planner_server]: Path end pose: (11.03, 3.48, 0.00)

With SmacPlannerHybrid
[planner_server]: Found valid path of size 49 to (11.11, 3.63, 3.13)
[planner_server]: Path end pose: (11.13, 3.63, 3.05)

With NavfnPlanner
[planner_server]: Found valid path of size 35 to (10.80, 3.94, -1.94)
[planner_server]: Path end pose: (10.80, 3.94, -1.94)

SmacPlannerHybrid tries to respect it which is fine to me (up to the tolerance of the smoother).
NavFn does a direct copy independently from the previous points.
SmacPlanner2D puts 0. I believe we should do as NavFn (copy goal orientation on last pose), I will do a PR

[doisyg]

I have a PR ready but it depends on #2475

[SteveMacenski]

That sounds reasonable, but why is it dependent on parameter changes?