updateApproximatePathOrientations method sets orientations of path poses in accordance of 0 rad yaw angle

[CihatAltiparmak]

Bug report

Required Info:

• Operating System:
  • nav2_docker
• Computer:
  • PC
• ROS2 Version:
  • rolling latest
• Version or commit hash:
  • 9cd0f9f
• DDS implementation:
  • fastrtps

Steps to reproduce issue

The decision as to which direction the path segment will go through is made only by zero rad yaw angled pose. Just send the gool poses everwhere you want and then you will see the truth. For differential drive robots which uses nav2_mppi_controller (enabled use_orienation of Path Align Critic), this may be a little troublesome, controller may act just like crazy.

Expected behavior

The orientations of the smoothed path poses should be set in accordance of robot's current pose's orientation at the beginnging (or user should be able to select this option).

Actual behavior

The orientations of the robot sometimes does not set correctly, which may bring about the controller which consider these orientations applying unfeasible velocity commands.

Here is a video of the bug.

Screencast.from.08-07-2025.01.44.44.AM.webm

Reproduction instructions

Additional information

navigation2/nav2_smoother/include/nav2_smoother/smoother_utils.hpp

Line 105 in 9cd0f9f

pt_yaw = tf2::getYaw(path.poses[1].pose.orientation);

this part always will be zero rad yaw if the planner does not set orientations of start pose of the path.

It can be a feature request as well, but IMO, we can fetch the robot's current pose inside smoother and then we can use this robot poses to decide which direction robot should go through. We can make this optional. I am strongly willing to contribute navigation2 project by adding this option if you want. Looking forward to seeing your feedbacks.

[SteveMacenski]

I got to your other tickets more concretely today, but I'm a little lost at this report and not entirely sure how to parse the description.

this part always will be zero rad yaw if the planner does not set orientations of start pose of the path.

What planner doesn't set the start pose of the path's orientation? Maybe its better if we have the planners do it. Smac obviously will, but maybe you're finding that NavFn / Theta* aren't

[CihatAltiparmak]

I got to your other tickets more concretely today, but I'm a little lost at this report and not entirely sure how to parse the description.

Probably you are right, I was using NavFn while reporting this issue. NavFn doesn't arrange orientations. That's the reason of this behaviour. I should add that I remember in smac, I didn't see this issue. Sorry for keeping you busy for this. I was tired yesterday night. But maybe would be nice that planners at least set start and end orientations as you said

[SteveMacenski]

The planners do set the start/end pose orientations, but we're looking at index i=1 which is the second point :-)

I think we just need to use that approximate orientation util in the NavFn/Theta* to populate the path and we should be good to go, no? Those will always be forward, never reverse, so that'll handle it

[CihatAltiparmak]

The planners do set the start/end pose orientations

I said like that beacuse MPPI docs say so too.

navigation2/nav2_mppi_controller/README.md

Line 145 in 1468484

| use_path_orientations | bool | Default false. Whether to consider path's orientations in path alignment, which can be useful when paired with feasible smac planners to incentivize directional changes only where/when the smac planner requests them. If you want the robot to deviate and invert directions where the controller sees fit, keep as false. If your plans do not contain orientation information (e.g. navfn), keep as false. |

approximate orientation util in the NavFn/Theta* to populate the path and we should be good to go,

I agree, planner not planning orientation can be seen reasonable. I write very similar idea in PR. Instead smoothers can solve this, (or add a param to set orientation too, why not if you want?)

[SteveMacenski]

Can we just do that by default? I don't see any downside with providing orientations to the NavFn/Theta* plans in the planners themselves using the approximation method. If we just leave it as 0 deg now, anything's an improvement. Can you submit that PR perhaps?

[SteveMacenski]

@CihatAltiparmak can this be closed from #5423?

[SteveMacenski]

Closing from #5423