diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 52b9ad9d86..8d40b151b5 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -417,16 +417,12 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & // 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 // with the dock to stop the docking procedure. - const double backward_projection = 0.25; + const double backward_projection = dock_backwards_ ? -0.25 : 0.25; const double yaw = tf2::getYaw(target_pose.pose.orientation); target_pose.pose.position.x += cos(yaw) * backward_projection; target_pose.pose.position.y += sin(yaw) * backward_projection;