fixed mapping in accordance to NED

mission/joystick_interface_auv/README.md

@@ -0,0 +1,3 @@
+## Joystick interface
+A joystick interface for manual control of AUV. A ROS2 node that subscribes on inputs from the XBOX controller and publishes the according wrench message to be used in Thruster Allocation.
+

mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml

@@ -1,8 +1,8 @@
-joystick_interface:
+joystick_interface_auv:
   ros__parameters:
-    surge_scale_factor:  60.0
-    sway_scale_factor:   60.0
-    yaw_scale_factor:    60.0
-    heave_scale_factor:  35.0
-    roll_scale_factor:   30.0
-    pitch_scale_factor:  20.0
+    surge_scale_factor: 60.0
+    sway_scale_factor: 60.0
+    yaw_scale_factor: 60.0
+    heave_scale_factor: 17.5
+    roll_scale_factor: 30.0
+    pitch_scale_factor: 20.0

mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv.py

@@ -21,8 +21,8 @@ def __init__(self):
         )
 
         self.last_button_press_time_ = 0
-        self.debounce_duration_ = 0.25
-        self.state_ = States.NO_GO
+        self.debounce_duration_   = 0.25
+        self.state_       = States.NO_GO
 
         self.joystick_buttons_map_ = [
             "A",
@@ -55,12 +55,12 @@ def __init__(self):
                                                        "thrust/wrench_input",
                                                        1)
 
-        self.declare_parameter('surge_scale_factor', 60.0)
-        self.declare_parameter('sway_scale_factor', 60.0)
-        self.declare_parameter('yaw_scale_factor', 60.0)
-        self.declare_parameter('heave_scale_factor', 35.0)
-        self.declare_parameter('roll_scale_factor', -30.0)
-        self.declare_parameter('pitch_scale_factor', 20.0)
+        self.declare_parameter('surge_scale_factor', 0.0)
+        self.declare_parameter('sway_scale_factor', 0.0)
+        self.declare_parameter('yaw_scale_factor', 0.0)
+        self.declare_parameter('heave_scale_factor', 0.0)
+        self.declare_parameter('roll_scale_factor', 0.0)
+        self.declare_parameter('pitch_scale_factor', 0.0)
 
         #Gets the scaling factors from the yaml file
         self.joystick_surge_scaling_ = self.get_parameter(
@@ -94,7 +94,8 @@ def __init__(self):
             Bool, "controller/lqr/enable", 10)
 
     def create_wrench_message(self, surge: float, sway: float, heave: float,
-                              roll: float, pitch: float, yaw: float) -> Wrench:
+                                 roll: float, pitch: float,
+                                 yaw: float) -> Wrench:
         """
         Creates a 2D wrench message with the given x, y, heave, roll, pitch, and yaw values.
 
@@ -130,7 +131,8 @@ def transition_to_autonomous_mode(self):
         """
         Publishes a zero force wrench message and signals that the system is turning on autonomous mode.
         """
-        wrench_msg = self.create_wrench_message(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+        wrench_msg = self.create_wrench_message(0.0, 0.0, 0.0, 0.0, 0.0,
+                                                   0.0)
         self.wrench_publisher_.publish(wrench_msg)
         self.operational_mode_signal_publisher_.publish(Bool(data=False))
         self.state_ = States.AUTONOMOUS_MODE
@@ -168,14 +170,15 @@ def joystick_cb(self, msg: Joy) -> Wrench:
         left_shoulder = buttons["LB"]
         right_shoulder = buttons["RB"]
 
-        surge = axes["vertical_axis_left_stick"] * self.joystick_surge_scaling_
-        sway = axes["horizontal_axis_left_stick"] * self.joystick_sway_scaling_
-        heave = -(left_trigger -
-                  right_trigger) / 2 * self.joystick_heave_scaling_
-        roll = (left_shoulder - right_shoulder) * self.joystick_roll_scaling_
+        surge = axes[
+            "vertical_axis_left_stick"] * self.joystick_surge_scaling_
+        sway = - axes[
+            "horizontal_axis_left_stick"] * self.joystick_sway_scaling_
+        heave = (left_trigger - right_trigger) * self.joystick_heave_scaling_
+        roll = (right_shoulder - left_shoulder) * self.joystick_roll_scaling_
         pitch = -axes[
             "vertical_axis_right_stick"] * self.joystick_pitch_scaling_
-        yaw = axes["horizontal_axis_right_stick"] * self.joystick_yaw_scaling_
+        yaw = -axes["horizontal_axis_right_stick"] * self.joystick_yaw_scaling_
 
         # Debounce for the buttons
         if current_time - self.last_button_press_time_ < self.debounce_duration_:
@@ -203,14 +206,14 @@ def joystick_cb(self, msg: Joy) -> Wrench:
             self.enable_controller_publisher_.publish(Bool(data=False))
             # Publish a zero wrench message when sw killing
             wrench_msg = self.create_wrench_message(0.0, 0.0, 0.0, 0.0, 0.0,
-                                                    0.0)
+                                                       0.0)
             self.wrench_publisher_.publish(wrench_msg)
             self.state_ = States.NO_GO
             return wrench_msg
 
         #Msg published from joystick_interface to thrust allocation
         wrench_msg = self.create_wrench_message(surge, sway, heave, roll,
-                                                pitch, yaw)
+                                                   pitch, yaw)
 
         if self.state_ == States.XBOX_MODE:
             self.get_logger().info("XBOX mode", throttle_duration_sec=1)