[RotationShimController] fix: rotate on short paths (#4716)

Add header data to goal for short paths. Commit d8ae3c1 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 <[email protected]>
ros-navigation · Oct 15, 2024 · fa9aaa9 · fa9aaa9
1 parent 6311978
commit fa9aaa9
Showing 1 changed file with 4 additions and 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
@@ -253,7 +253,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()