Formatting

mission/joystick_interface/joystick_interface/joystick_interface.py

@@ -38,20 +38,19 @@ 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_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
+            "RT",                          #Positive thrust/torque multiplier
             "dpad_horizontal",
             "dpad_vertical",
         ]
 
         self.joy_subscriber = self.create_subscription(Joy, "/joystick/joy",
                                                        self.joystick_cb, 1)
         self.wrench_publisher = self.create_publisher(Wrench,
-                                                      "/thrust/wrench_input",
-                                                      1)
+                                                      "/thrust/wrench_input", 1)
 
         self.declare_parameter('surge', 100.0)
         self.declare_parameter('sway', 100.0)
@@ -71,22 +70,22 @@ def __init__(self):
         #Operational mode publisher
         self.operational_mode_signal_publisher = self.create_publisher(
             Bool, "/softWareOperationMode", 10)
+        # Signal that we are not in autonomous mode
         self.operational_mode_signal_publisher.publish(
-            Bool(data=True))  # Signal that we are not in autonomous mode
+            Bool(data=True)) 
 
         #Controller publisher
         self.enable_controller_publisher = self.create_publisher(
             Bool, "/controller/lqr/enable", 10)
 
-    def right_trigger_linear_converter(
-            self, rt_input):  #triggers have an output from 1 to -1
-        output_value = (rt_input + 1) * (
-            -0.5) + 2  #does a linear conversion from (1 to -1) to (1 to 2)
+    #does a linear conversion from trigger inputs (1 to -1) to (1 to 2)
+    def right_trigger_linear_converter(self, rt_input):  
+        output_value = (rt_input + 1) * (-0.5) + 2  
         return output_value
 
-    def left_trigger_linear_converter(
-            self, lt_input):  #triggers have an output from 1 to -1
-        ouput_value = lt_input * 0.25 + 0.75  #does a linear conversion from (1 to -1) to (1 to 0.5)
+    #does a linear conversion from trigger input (1 to -1) to (1 to 0.5)
+    def left_trigger_linear_converter(self, lt_input):
+        ouput_value = lt_input * 0.25 + 0.75  
         return ouput_value
 
     def create_2d_wrench_message(self, x, y, yaw):
@@ -101,17 +100,20 @@ def publish_wrench_message(self, wrench):
 
     def transition_to_xbox_mode(self):
         # We want to turn off controller when moving to xbox mode
-        self.enable_controller_publisher.publish(Bool(data=False))
+        self.enable_controller_publisher.publish(
+            Bool(data=False))
+        # signal that we enter xbox mode
         self.operational_mode_signal_publisher.publish(
-            Bool(data=True))  # signal that we enter xbox mode
+            Bool(data=True))
         self.state = states.XBOX_MODE
 
     def transition_to_autonomous_mode(self):
         # We want to publish zero force once when transitioning
         wrench_msg = self.create_2d_wrench_message(0.0, 0.0, 0.0)
         self.publish_wrench_message(wrench_msg)
+        # signal that we are turning on autonomous mode
         self.operational_mode_signal_publisher.publish(
-            Bool(data=False))  # signal that we are turning on autonomous mode
+            Bool(data=False))
         self.state = states.AUTONOMOUS_MODE
 
     def joystick_cb(self, msg):
@@ -135,12 +137,9 @@ def joystick_cb(self, msg):
         right_trigger = self.right_trigger_linear_converter(right_trigger)
         left_trigger = self.left_trigger_linear_converter(left_trigger)
 
-        surge = axes[
-            "vertical_axis_left_stick"] * self.joystick_surge_scaling * left_trigger * right_trigger
-        sway = axes[
-            "horizontal_axis_left_stick"] * self.joystick_sway_scaling * left_trigger * right_trigger
-        yaw = axes[
-            "horizontal_axis_right_stick"] * self.joystick_yaw_scaling * left_trigger * right_trigger
+        surge = axes["vertical_axis_left_stick"] * self.joystick_surge_scaling * left_trigger * right_trigger
+        sway = axes["horizontal_axis_left_stick"] * self.joystick_sway_scaling * left_trigger * right_trigger
+        yaw = axes["horizontal_axis_right_stick"] * self.joystick_yaw_scaling * left_trigger * right_trigger
 
         # Debounce for the buttons
         if current_time - self.last_button_press_time < self.debounce_duration:
@@ -151,19 +150,23 @@ def joystick_cb(self, msg):
         # 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:
             self.last_button_press_time = current_time
-
-        if self.state == states.NO_GO and software_killswitch_button:  # Toggle ks on and off
+
+        # Toggle ks on and off
+        if self.state == states.NO_GO and software_killswitch_button:
+            # signal that killswitch is not blocking
             self.software_killswitch_signal_publisher.publish(
-                Bool(data=True))  # signal that killswitch is not blocking
+                Bool(data=True)) 
             self.transition_to_xbox_mode()
             return
 
         if software_killswitch_button:
             self.get_logger().info("SW killswitch", throttle_duration_sec=1)
+            # signal that killswitch is blocking
             self.software_killswitch_signal_publisher.publish(
-                Bool(data=False))  # signal that killswitch is blocking
+                Bool(data=False))
+            # Turn off controller in sw killswitch
             self.enable_controller_publisher.publish(
-                Bool(data=False))  # Turn off controller in sw killswitch
+                Bool(data=False))
             # Publish a zero wrench message when sw killing
             wrench_msg = self.create_2d_wrench_message(0.0, 0.0, 0.0)
             self.publish_wrench_message(wrench_msg)

mission/joystick_interface/joystick_interface/test/test_joystick_interface.py

@@ -7,6 +7,7 @@
 
 class TestJoystickInterface:
 
+    #test that the linear conversion from (1 to -1) to (1 to 2) is working
     def test_right_trigger_linear_converter(self):
         rclpy.init()
         joystick = JoystickInterface()
@@ -15,6 +16,7 @@ def test_right_trigger_linear_converter(self):
         assert joystick.right_trigger_linear_converter(-1) == 2
         rclpy.shutdown()
 
+    #test that the linear conversion from (1 to -1) to (1 to 0.5) is working
     def test_left_trigger_linear_converter(self):
         rclpy.init()
         joystick = JoystickInterface()
@@ -23,6 +25,7 @@ def test_left_trigger_linear_converter(self):
         assert joystick.left_trigger_linear_converter(-1) == 0.5
         rclpy.shutdown()
 
+    #test that the 2d wrench msg is created successfully
     def test_2d_wrench_msg(self):
         rclpy.init()
         msg = JoystickInterface().create_2d_wrench_message(2.0, 3.0, 4.0)
@@ -31,43 +34,40 @@ def test_2d_wrench_msg(self):
         assert msg.torque.z == 4.0
         rclpy.shutdown()
 
+    #Test that the callback function will be able to interpret the joy msg
     def test_input_from_controller_into_wrench_msg(self):
         rclpy.init()
         joy_msg = Joy()
-        joy_msg.axes = [-1.0, -1.0, 1.0, 0.0, 0.0, 1.0, 0.0,
-                        0.0]  #Should have 8 inputs self.joystick_axes_map
-        joy_msg.buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-                           0]  #Should have 11 inputs self.joystick_buttons_map
+        joy_msg.axes = [-1.0, -1.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0]
+        joy_msg.buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
         wrench_msg = JoystickInterface().joystick_cb(joy_msg)
         assert wrench_msg.force.x == -100.0
         assert wrench_msg.force.y == -100.0
         assert wrench_msg.torque.z == 0.0
         rclpy.shutdown()
 
+    #When the killswitch button is activated in the buttons list, it should output a wrench msg with only zeros
     def test_killswitch_button(self):
         rclpy.init()
         joystick = JoystickInterface()
         joystick.state = states.XBOX_MODE
         joy_msg = Joy()
-        joy_msg.axes = [-1.0, -1.0, 1.0, 0.0, 0.0, 1.0, 0.0,
-                        0.0]  #Should have 8 inputs self.joystick_axes_map
-        joy_msg.buttons = [0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
-                           0]  #Should have 11 inputs self.joystick_buttons_map
+        joy_msg.axes = [-1.0, -1.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0]  
+        joy_msg.buttons = [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0] 
         wrench_msg = joystick.joystick_cb(joy_msg)
         assert wrench_msg.force.x == 0.0
         assert wrench_msg.force.y == 0.0
         assert wrench_msg.torque.z == 0.0
         rclpy.shutdown()
 
-    def test_moving_in_and_out_of_xbox_mode(self):
+    #When we move into XBOX mode it should still be able to return this wrench message
+    def test_moving_in_of_xbox_mode(self):
         rclpy.init()
         joystick = JoystickInterface()
         joystick.state = states.XBOX_MODE
         joy_msg = Joy()
-        joy_msg.axes = [-1.0, -1.0, 1.0, 0.0, 0.0, 1.0, 0.0,
-                        0.0]  #Should have 8 inputs self.joystick_axes_map
-        joy_msg.buttons = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-                           0]  #Should have 11 inputs self.joystick_buttons_map
+        joy_msg.axes = [-1.0, -1.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0]
+        joy_msg.buttons = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] 
         wrench_msg = joystick.joystick_cb(joy_msg)
         assert wrench_msg.force.x == -100.0
         assert wrench_msg.force.y == -100.0