[RotationShimController] rotate also on short paths

[gennartan]

When the path is shorter than ´forward_sampling_distance´ the rotatitonShimController would directly give control to the primary controller to navigate to the goal. This would lead to the exact behavior that was tried to be fixed by the rotationShimController: "The result is an awkward, stuttering, or whipping around behavior" [1]. It often resulted in navigation failure.

In this case, the controller should try to rotate towards the last point of the path, so that the primary controller can have a better starting orientation.

[1] https://navigation.ros.org/tuning/index.html#rotate-in-place-behavior

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4281
Primary OS tested on	Ubuntu 20.04 / 22.04
Robotic platform tested on	Gazebo simulation - custom hardware
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

• Let the rotationShimController rotate the robot when the path length is shorter than ´forward_sampling_distance´ (this allow the robot to have the correct orientation when the requested goal is really close)

Description of documentation updates required from your changes

• Need to update the ´forward_sampling_distance´ description of https://navigation.ros.org/tutorials/docs/using_shim_controller.html --> add "if there is no point further than forward_sampling_distance, rotates toward the goal"
• Same gos for https://navigation.ros.org/configuration/packages/configuring-rotation-shim-controller.html

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.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

[SteveMacenski]

Would it be sensible in L179 to consider increasing the minimum size of path to 3 or 4?

[gennartan]

Would it be sensible in L179 to consider increasing the minimum size of path to 3 or 4?

I think it makes sense to keep to 2.
If the path resolution is low, two points can still be far from each other in a path. As an example, my current project was using a path resolution of 0.2m. And we don't want the path resolution to change the behavior of this node.

[SteveMacenski]

Sorry about the toggled CI, I was testing some changes to the CI bots for the org change. Once CI comes back happy I can merge

[SteveMacenski]

@gennartan, a bug was identified by @NikolasE in 94d3a9d

double angular_distance_to_heading = std::abs(goal_yaw - pose_yaw);

---

I think this will fail if goal_yaw is -pi and pose_yaw pi. Robot is already aligned, but I assume this could trigger a full rotation on the spot.

This could help:
https://github.com/ros/angles/blob/54ad72b0ce473a2e1d98da447898bf2fcd36519f/angles/include/angles/angles.h#L104C72-L104C73