From 940322af527f8284a218b53e63da8337e853f636 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Wed, 10 Jul 2024 00:13:00 +0200 Subject: [PATCH] Fix dock orientation while moving backwards Signed-off-by: Alberto Tudela --- 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 d12d3f526b..54dde07616 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