From 6bff7de5267c2e3d7ede282abdd3c4c0cb62cc3f Mon Sep 17 00:00:00 2001 From: Saitama Date: Tue, 15 Oct 2024 19:55:35 +0200 Subject: [PATCH] [RotationShimController] fix: rotate on short paths (#4716) Add header data to goal for short paths. Commit d8ae3c1f9b8233e86ed54dfbe615b1ba56b51b6d added the possibility to the rotation shim controller to rotate towards the goal when the goal was closer that the `forward_sampling_distance`. This feature was not fully working as the goal was missing proper header data, causing the rotation shim to give back control to the main controller. Co-authored-by: agennart --- .../src/nav2_rotation_shim_controller.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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 4704c0b8eb..6c6efc32b4 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -248,7 +248,10 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt() } } - return current_path_.poses.back(); + auto goal = current_path_.poses.back(); + goal.header.frame_id = current_path_.header.frame_id; + goal.header.stamp = clock_->now(); + return goal; } geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathGoal()