Removed lqr_controller publisher and changed operational mode to stri…

…ng, reformatted killswitch logic
vortexntnu · Feb 11, 2024 · 96b0f7c · 96b0f7c
2 parents 4c53770 + e9bf2f5
commit 96b0f7c
Showing 1 changed file with 13 additions and 17 deletions.
diff --git a/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv.py b/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv.py
@@ -22,8 +22,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",
@@ -40,12 +40,12 @@ def __init__(self):
         ]
 
         self.joystick_axes_map_ = [
-            "horizontal_axis_left_stick",  #Translation (Left and Right)
-            "vertical_axis_left_stick",  #Translation (Forwards and Backwards)
-            "LT",  #Negative thrust/torque multiplier
-            "horizontal_axis_right_stick",  #Rotation
-            "vertical_axis_right_stick",
-            "RT",  #Positive thrust/torque multiplier
+            "horizontal_axis_left_stick",  #Sway 
+            "vertical_axis_left_stick",  #Surge
+            "LT",  #Heave down
+            "horizontal_axis_right_stick",  #Yaw
+            "vertical_axis_right_stick",  #Pitch
+            "RT",  #Heave up
             "dpad_horizontal",
             "dpad_vertical",
         ]
@@ -92,8 +92,7 @@ def __init__(self):
 
 
     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.
 
@@ -128,8 +127,7 @@ 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("Autonomous mode")
         self.state_ = States.AUTONOMOUS_MODE
@@ -167,10 +165,8 @@ 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_
+        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[
@@ -210,7 +206,7 @@ def joystick_cb(self, msg: Joy) -> Wrench:
 
         #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)