Fixed unit tests

mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py

@@ -1,5 +1,5 @@
 import os
-from launch import LaunchDescription 
+from launch import LaunchDescription
 from launch_ros.actions import Node
 from ament_index_python.packages import get_package_share_directory
 
@@ -11,10 +11,9 @@ def generate_launch_description():
         executable='joystick_interface_auv.py',
         name='joystick_interface_auv',
         output='screen',
-        parameters=[os.path.join(get_package_share_directory('joystick_interface_auv'), 'config/param_joystick_interface_auv.yaml')]
-    )
-
-    return LaunchDescription([
-        joystick_interface_node
-    ])
+        parameters=[
+            os.path.join(get_package_share_directory('joystick_interface_auv'),
+                         'config/param_joystick_interface_auv.yaml')
+        ])
 
+    return LaunchDescription([joystick_interface_node])

mission/joystick_interface_auv/scripts/joystick_interface_auv.py

@@ -5,16 +5,20 @@
 from sensor_msgs.msg import Joy
 from std_msgs.msg import Bool
 
+
 class States:
     XBOX_MODE = 1
     AUTONOMOUS_MODE = 2
     NO_GO = 3
 
+
 class JoystickInterface(Node):
 
     def __init__(self):
         super().__init__('joystick_interface_node')
-        self.get_logger().info("Joystick interface is up and running. \n When the XBOX controller is connected, press the killswitch button once to enter XBOX mode.")
+        self.get_logger().info(
+            "Joystick interface is up and running. \n When the XBOX controller is connected, press the killswitch button once to enter XBOX mode."
+        )
 
         self.last_button_press_time_ = 0
         self.debounce_duration_   = 0.25
@@ -36,35 +40,41 @@ 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.joy_subscriber_ = self.create_subscription(
+            Joy, "joystick/joy", self.joystick_cb, 1)
         self.wrench_publisher_ = self.create_publisher(Wrench,
-                                                      "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)
+                                                       "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)
 
         #Gets the scaling factors from the yaml file
-        self.joystick_surge_scaling_ = self.get_parameter('surge_scale_factor').value
-        self.joystick_sway_scaling_  = self.get_parameter('sway_scale_factor').value
-        self.joystick_yaw_scaling_   = self.get_parameter('yaw_scale_factor').value
-        self.joystick_heave_scaling_ = self.get_parameter('heave_scale_factor').value
-        self.joystick_roll_scaling_  = self.get_parameter('roll_scale_factor').value
-        self.joystick_pitch_scaling_ = self.get_parameter('pitch_scale_factor').value
+        self.joystick_surge_scaling_ = self.get_parameter(
+            'surge_scale_factor').value
+        self.joystick_sway_scaling_ = self.get_parameter(
+            'sway_scale_factor').value
+        self.joystick_yaw_scaling_ = self.get_parameter(
+            'yaw_scale_factor').value
+        self.joystick_heave_scaling_ = self.get_parameter(
+            'heave_scale_factor').value
+        self.joystick_roll_scaling_ = self.get_parameter(
+            'roll_scale_factor').value
+        self.joystick_pitch_scaling_ = self.get_parameter(
+            'pitch_scale_factor').value
 
         #Killswitch publisher
         self.software_killswitch_signal_publisher_ = self.create_publisher(
@@ -75,15 +85,17 @@ 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))
 
         #Controller publisher
         self.enable_controller_publisher_ = self.create_publisher(
             Bool, "controller/lqr/enable", 10)
-
-    def create_2d_wrench_message(self, surge: float, sway: float, heave: float, roll: float, pitch: float, yaw: float) -> Wrench:
+
+    def create_2d_wrench_message(self, surge: float, sway: float, heave: float,
+                                 roll: float, pitch: float,
+                                 yaw: float) -> Wrench:
         """
         Creates a 2D wrench message with the given x, y, heave, roll, pitch, and yaw values.
 
@@ -99,9 +111,9 @@ def create_2d_wrench_message(self, surge: float, sway: float, heave: float, roll
             Wrench: A 2D wrench message with the given values.
         """
         wrench_msg = Wrench()
-        wrench_msg.force.x  = surge
-        wrench_msg.force.y  = sway
-        wrench_msg.force.z  = heave
+        wrench_msg.force.x = surge
+        wrench_msg.force.y = sway
+        wrench_msg.force.z = heave
         wrench_msg.torque.x = roll
         wrench_msg.torque.y = pitch
         wrench_msg.torque.z = yaw
@@ -119,12 +131,13 @@ 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_2d_wrench_message(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+        wrench_msg = self.create_2d_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
 
-    def joystick_cb(self, msg : Joy):
+    def joystick_cb(self, msg: Joy):
         """
         Callback function that receives joy messages and converts them into
         wrench messages to be sent to the thruster allocation node. 
@@ -142,14 +155,13 @@ def joystick_cb(self, msg : Joy):
 
         buttons = {}
         axes = {}
-        
+
         for i in range(len(msg.buttons)):
             buttons[self.joystick_buttons_map_[i]] = msg.buttons[i]
 
         for i in range(len(msg.axes)):
             axes[self.joystick_axes_map_[i]] = msg.axes[i]
 
-
         xbox_control_mode_button = buttons["A"]
         software_killswitch_button = buttons["B"]
         software_control_mode_button = buttons["X"]
@@ -158,16 +170,23 @@ def joystick_cb(self, msg : Joy):
         left_shoulder = buttons["LB"]
         right_shoulder = buttons["RB"]
 
+<<<<<<< HEAD
         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 = (right_shoulder - left_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 = -(axes[["RT"] - axes["LT"]]) / 2 * self.joystick_heave_scaling_
+        roll = axes[self.buttons["RB"] -
+                    self.buttons["LB"]] * self.joystick_roll_scaling_
+>>>>>>> refs/remotes/origin/feature/joystick_interface_in_ros2
         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_:
@@ -189,17 +208,20 @@ def joystick_cb(self, msg : Joy):
         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))
+            self.software_killswitch_signal_publisher_.publish(
+                Bool(data=False))
             # Turn off controller in sw killswitch
             self.enable_controller_publisher_.publish(Bool(data=False))
             # Publish a zero wrench message when sw killing
-            wrench_msg = self.create_2d_wrench_message(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+            wrench_msg = self.create_2d_wrench_message(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_2d_wrench_message(surge, sway, heave, roll, pitch, yaw)
+        wrench_msg = self.create_2d_wrench_message(surge, sway, heave, roll,
+                                                   pitch, yaw)
 
         if self.state_ == States.XBOX_MODE:
             self.get_logger().info("XBOX mode", throttle_duration_sec=1)
@@ -216,6 +238,7 @@ def joystick_cb(self, msg : Joy):
 
         return wrench_msg
 
+
 def main():
     rclpy.init()
     joystick_interface = JoystickInterface()
@@ -225,4 +248,4 @@ def main():
 
 
 if __name__ == "__main__":
-    main()
+    main()

mission/joystick_interface_auv/test/test_joystick_interface_auv.py

@@ -1,5 +1,10 @@
+<<<<<<< HEAD
 from scripts.joystick_interface_auv import JoystickInterface 
 from scripts.joystick_interface_auv import States
+=======
+from joystick_interface_auv.joystick_interface_auv import JoystickInterface
+from joystick_interface_auv.joystick_interface_auv import States
+>>>>>>> refs/remotes/origin/feature/joystick_interface_in_ros2
 import rclpy
 from sensor_msgs.msg import Joy
 from sensor_msgs.msg import Joy
@@ -9,10 +14,11 @@ class TestJoystickInterface:
     #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, 5.0, 6.0, 7.0)
-        assert msg.force.x  == 2.0
-        assert msg.force.y  == 3.0
-        assert msg.force.z  == 4.0
+        msg = JoystickInterface().create_2d_wrench_message(
+            2.0, 3.0, 4.0, 5.0, 6.0, 7.0)
+        assert msg.force.x == 2.0
+        assert msg.force.y == 3.0
+        assert msg.force.z == 4.0
         assert msg.torque.x == 5.0
         assert msg.torque.y == 6.0
         assert msg.torque.z == 7.0
@@ -25,8 +31,8 @@ def test_input_from_controller_into_wrench_msg(self):
         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  == -60.0
-        assert wrench_msg.force.y  == -60.0
+        assert wrench_msg.force.x == -60.0
+        assert wrench_msg.force.y == -60.0
         assert wrench_msg.torque.z == 0.0
         rclpy.shutdown()
 
@@ -39,8 +45,8 @@ def test_killswitch_button(self):
         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.force.x == 0.0
+        assert wrench_msg.force.y == 0.0
         assert wrench_msg.torque.z == 0.0
         rclpy.shutdown()
 
@@ -53,7 +59,7 @@ def test_moving_in_of_xbox_mode(self):
         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  == -60.0
-        assert wrench_msg.force.y  == -60.0
+        assert wrench_msg.force.x == -60.0
+        assert wrench_msg.force.y == -60.0
         assert wrench_msg.torque.z == 0.0
         rclpy.shutdown()