Added precise_manuevering_mode_

mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv.py

@@ -84,6 +84,8 @@ def __init__(self):
         self.last_button_press_time_ = 0
         self.debounce_duration_ = 0.25
         self.state_ = States.NO_GO
+        self.precise_manuevering_mode_ = False
+        self.scale_factor = 1.0
 
         self.joystick_buttons_map_ = []
 
@@ -218,31 +220,34 @@ def joystick_cb(self, msg: Joy) -> Wrench:
         xbox_control_mode_button = buttons.get("A", 0)
         software_killswitch_button = buttons.get("B", 0)
         software_control_mode_button = buttons.get("X", 0)
+        precise_manuevering_mode_button = buttons.get("Y", 0)
         left_trigger = axes.get("RT", 0.0)
         right_trigger = axes.get("LT", 0.0)
         left_shoulder = buttons.get("LB", 0)
         right_shoulder = buttons.get("RB", 0)
 
         # Extract axis values
         surge = axes.get("vertical_axis_left_stick",
-                         0.0) * self.joystick_surge_scaling_
+                         0.0) * self.joystick_surge_scaling_ * self.scale_factor
         sway = -axes.get("horizontal_axis_left_stick",
-                         0.0) * self.joystick_sway_scaling_
-        heave = (left_trigger - right_trigger) * self.joystick_heave_scaling_
-        roll = (right_shoulder - left_shoulder) * self.joystick_roll_scaling_
+                         0.0) * self.joystick_sway_scaling_ * self.scale_factor
+        heave = (left_trigger - right_trigger) * self.joystick_heave_scaling_ * self.scale_factor
+        roll = (right_shoulder - left_shoulder) * self.joystick_roll_scaling_ * self.scale_factor
         pitch = -axes.get("vertical_axis_right_stick",
-                          0.0) * self.joystick_pitch_scaling_
+                          0.0) * self.joystick_pitch_scaling_ * self.scale_factor
         yaw = -axes.get("horizontal_axis_right_stick",
-                        0.0) * self.joystick_yaw_scaling_
+                        0.0) * self.joystick_yaw_scaling_ * self.scale_factor
+
 
         # Debounce for the buttons
         if current_time - self.last_button_press_time_ < self.debounce_duration_:
             software_control_mode_button = False
             xbox_control_mode_button = False
             software_killswitch_button = False
+            precise_manuevering_mode_button = False
 
         # If any button is pressed, update the last button press time
-        if software_control_mode_button or xbox_control_mode_button or software_killswitch_button:
+        if software_control_mode_button or xbox_control_mode_button or software_killswitch_button or precise_manuevering_mode_button:
             self.last_button_press_time_ = current_time
 
         # Toggle killswitch on and off
@@ -268,6 +273,16 @@ def joystick_cb(self, msg: Joy) -> Wrench:
                 self.state_ = States.NO_GO
                 return wrench_msg
 
+        # Toggle precise maneuvering mode on and off
+        if precise_manuevering_mode_button:
+            self.precise_manuevering_mode_ = not self.precise_manuevering_mode_
+            mode = "enabled" if self.precise_manuevering_mode_ else "disabled"
+            self.get_logger().info(f"Precise maneuvering mode {mode}.")
+
+            self.scale_factor = 0.5 if self.precise_manuevering_mode_ else 1.0
+        else:
+            self.scale_factor = 1.0
+
         # Publish wrench message from joystick_interface to thrust allocation
         wrench_msg = self.create_wrench_message(surge, sway, heave, roll,
                                                 pitch, yaw)