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