From 9c02a96c6006d1e5903ae64b73ec7399e4335d76 Mon Sep 17 00:00:00 2001 From: jakubach Date: Wed, 13 Nov 2024 23:45:55 +0100 Subject: [PATCH] Used a condition instead of math conversion of boolean value to int Signed-off-by: jakubach --- .../opennav_docking/src/docking_server.cpp | 26 ++++++++++--------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 65a4060c4a..0dcf7febb6 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -460,24 +460,26 @@ 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); - } + const double backward_projection = 0.25; + const double yaw = tf2::getYaw(target_pose.pose.orientation); // 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. - // (1 - 2 * dock_backwards_) converts `true` to -1 and `false` to 1 - const double backward_projection = 0.25; - const double yaw = tf2::getYaw(target_pose.pose.orientation); - target_pose.pose.position.x += (1 - 2 * dock_backwards_) * cos(yaw) * backward_projection; - target_pose.pose.position.y += (1 - 2 * dock_backwards_) * sin(yaw) * backward_projection; + if (dock_backwards_) { + // 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 + target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( + yaw + M_PI); + target_pose.pose.position.x -= cos(yaw) * backward_projection; + target_pose.pose.position.y -= sin(yaw) * backward_projection; + } else { + target_pose.pose.position.x += cos(yaw) * backward_projection; + target_pose.pose.position.y += sin(yaw) * backward_projection; + } tf2_buffer_->transform(target_pose, target_pose, base_frame_); - + // Compute and publish controls auto command = std::make_unique(); command->header.stamp = now();