Used a condition instead of math conversion of boolean value to int

Signed-off-by: jakubach <[email protected]>

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<geometry_msgs::msg::TwistStamped>();
     command->header.stamp = now();