From d8ae3c1f9b8233e86ed54dfbe615b1ba56b51b6d Mon Sep 17 00:00:00 2001 From: Antoine Gennart Date: Sat, 27 Apr 2024 07:35:27 +0200 Subject: [PATCH] [RotationShimController] rotate also on short paths MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 --- .../src/nav2_rotation_shim_controller.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index afd3238145..5097a1fc9c 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/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