[RotationShimController] rotate also on short paths (ros-navigation#4290)

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

Signed-off-by: enricosutera <[email protected]>

Author: gennartan

nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp

@@ -195,10 +195,7 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt()
     }
   }
 
-  throw nav2_core::ControllerException(
-          std::string(
-            "Unable to find a sampling point at least %0.2f from the robot,"
-            "passing off to primary controller plugin.", forward_sampling_distance_));
+  return current_path_.poses.back();
 }
 
 geometry_msgs::msg::Pose