[RotationShimController] fix: rotate on short paths (ros-navigation#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]>
Signed-off-by: Tony Najjar <[email protected]>

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()