Fix dock orientation while moving backwards (#4530)

Signed-off-by: Alberto Tudela <[email protected]>
(cherry picked from commit f73fa24)

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