From a34919342ff5d998a2b55ec75a27d7f15b18a052 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Mon, 22 Jul 2024 22:31:11 +0200 Subject: [PATCH] Fix dock orientation while moving backwards (#4530) Signed-off-by: Alberto Tudela (cherry picked from commit f73fa2499b54bfb346fa3dcdd7a5a200ba349752) --- nav2_docking/opennav_docking/src/docking_server.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 2d533c0871..b5e43f4f4e 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -414,6 +414,13 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & geometry_msgs::msg::PoseStamped target_pose = dock_pose; target_pose.header.stamp = rclcpp::Time(0); + // Make sure that the target pose is pointing at the robot when moving backwards + // This is to ensure that the robot doesn't try to dock from the wrong side + if (dock_backwards_) { + target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( + tf2::getYaw(target_pose.pose.orientation) + M_PI); + } + // The control law can get jittery when close to the end when atan2's can explode. // Thus, we backward project the controller's target pose a little bit after the // dock so that the robot never gets to the end of the spiral before its in contact