Enhancement/joystick interface cleanup (#110)

* Cleanup and added docstrings to functions

* Fixed deadzone on joy driver

* Fixed bugs in tests

* Updated Readme

* updated readme again

* removed comment, and type specifiers paranthesis in docstrings

---------

Co-authored-by: Aldokan <[email protected]>

asv_setup/launch/pc.launch.py

@@ -19,8 +19,8 @@ def generate_launch_description():
         name='joystick_driver',
         output='screen',
         parameters=[
-            {'_deadzone': 0.15},
-            {'_autorepeat_rate': 100},
+            {'deadzone': 0.15},
+            {'autorepeat_rate': 100.0},
         ],
         remappings=[
             ('/joy', '/joystick/joy'),

mission/joystick_interface/CMakeLists.txt

@@ -10,16 +10,16 @@ find_package(rclpy REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 
-ament_python_install_package(${PROJECT_NAME})
+ament_python_install_package(scripts)
 
-# Install launch files.
 install(DIRECTORY
   launch
+  config
   DESTINATION share/${PROJECT_NAME}
 )
 
 install(PROGRAMS
-  joystick_interface/joystick_interface.py
+  scripts/joystick_interface.py
   DESTINATION lib/${PROJECT_NAME}
 )
 

mission/joystick_interface/README.md

@@ -1,2 +1,2 @@
-In the launch files we need to make it able to find the yaml files without having an explicit path such as this: "src/vortex-asv/mission/joystick_interface/launch/joystick_interface_launch.yaml" 
-Perhaps change to python launch
+## Joystick interface
+A joystick interface for manual control of ASV Freya. 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/package.xml

@@ -14,6 +14,7 @@
   <exec_depend>rclpy</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>ros2launch</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

mission/joystick_interface/joystick_interface/joystick_interface.py → mission/joystick_interface/scripts/joystick_interface.py

@@ -6,23 +6,23 @@
 from std_msgs.msg import Bool
 
 
-class states:
+class States:
     XBOX_MODE = 1
     AUTONOMOUS_MODE = 2
-    NO_GO = 3  # Do nothing
+    NO_GO = 3
 
 
 class JoystickInterface(Node):
 
     def __init__(self):
         super().__init__('joystick_interface_node')
-        self.get_logger().info("Joystick interface is up and running")
+        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
-        self.state = states.NO_GO
+        self.last_button_press_time_ = 0
+        self.debounce_duration_ = 0.25
+        self.state_ = States.NO_GO
 
-        self.joystick_buttons_map = [
+        self.joystick_buttons_map_ = [
             "A",
             "B",
             "X",
@@ -36,7 +36,7 @@ def __init__(self):
             "stick_button_right",
         ]
 
-        self.joystick_axes_map = [
+        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
@@ -47,9 +47,9 @@ def __init__(self):
             "dpad_vertical",
         ]
 
-        self.joy_subscriber = self.create_subscription(Joy, "joystick/joy",
+        self.joy_subscriber_ = self.create_subscription(Joy, "joystick/joy",
                                                        self.joystick_cb, 1)
-        self.wrench_publisher = self.create_publisher(Wrench,
+        self.wrench_publisher_ = self.create_publisher(Wrench,
                                                       "thrust/wrench_input",
                                                       1)
 
@@ -58,73 +58,120 @@ def __init__(self):
         self.declare_parameter('yaw_scale_factor', 50.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_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
 
         #Killswitch publisher
-        self.software_killswitch_signal_publisher = self.create_publisher(
+        self.software_killswitch_signal_publisher_ = self.create_publisher(
             Bool, "softWareKillSwitch", 10)
-        self.software_killswitch_signal_publisher.publish(
+        self.software_killswitch_signal_publisher_.publish(
             Bool(data=False))  #Killswitch is not active
 
         #Operational mode publisher
-        self.operational_mode_signal_publisher = self.create_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))
+        self.operational_mode_signal_publisher_.publish(Bool(data=True))
 
         #Controller publisher
-        self.enable_controller_publisher = self.create_publisher(
+        self.enable_controller_publisher_ = self.create_publisher(
             Bool, "controller/lqr/enable", 10)
+
+    def right_trigger_linear_converter(self, rt_input: float) -> float:
+        """
+        Does a linear conversion from the right trigger input range of (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):
+        Args:
+            rt_input: The input value of the right trigger, ranging from -1 to 1.
+
+        Returns:
+            float: The output value, ranging from 1 to 2.
+        """
         output_value = (rt_input + 1) * (-0.5) + 2
         return output_value
 
-    #does a linear conversion from trigger input (1 to -1) to (1 to 0.5)
-    def left_trigger_linear_converter(self, lt_input):
+    def left_trigger_linear_converter(self, lt_input: float ) -> float:
+        """
+        Does a linear conversion from the left trigger input range of (1 to -1) to (1 to 0.5)
+
+        Args:
+            lt_input: The input value of the left trigger, ranging from -1 to 1.
+
+        Returns:
+            float: The output value, ranging from 1 to 2.
+        """
         ouput_value = lt_input * 0.25 + 0.75
         return ouput_value
 
-    def create_2d_wrench_message(self, x, y, yaw):
-        wrench_msg = Wrench()
-        wrench_msg.force.x = x
-        wrench_msg.force.y = y
-        wrench_msg.torque.z = yaw
-        return wrench_msg
+    def create_2d_wrench_message(self, x: float, y: float, yaw: float) -> Wrench:
+            """
+            Creates a 2D wrench message with the given x, y, and yaw values.
+
+            Args:
+                x: The x component of the force vector.
+                y: The y component of the force vector.
+                yaw: The z component of the torque vector.
+
+            Returns:
+                Wrench: A 2D wrench message with the given values.
+            """
+            wrench_msg = Wrench()
+            wrench_msg.force.x = x
+            wrench_msg.force.y = y
+            wrench_msg.torque.z = yaw
+            return wrench_msg
 
-    def publish_wrench_message(self, wrench):
-        self.wrench_publisher.publish(wrench)
+    def publish_wrench_message(self, wrench: Wrench):
+            """
+            Publishes a Wrench message to the wrench_publisher_ topic.
+
+            Args:
+                wrench: The Wrench message to be published.
+            """
+            self.wrench_publisher_.publish(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))
-        # signal that we enter xbox mode
-        self.operational_mode_signal_publisher.publish(Bool(data=True))
-        self.state = states.XBOX_MODE
+        """
+        Turns off the controller and signals that the operational mode has switched to Xbox mode.
+        """
+        self.enable_controller_publisher_.publish(Bool(data=False))
+        self.operational_mode_signal_publisher_.publish(Bool(data=True))
+        self.state_ = States.XBOX_MODE
 
     def transition_to_autonomous_mode(self):
-        # We want to publish zero force once when transitioning
+        """
+        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)
         self.publish_wrench_message(wrench_msg)
-        # signal that we are turning on autonomous mode
-        self.operational_mode_signal_publisher.publish(Bool(data=False))
-        self.state = states.AUTONOMOUS_MODE
-
-    def joystick_cb(self, msg):
+        self.operational_mode_signal_publisher_.publish(Bool(data=False))
+        self.state_ = States.AUTONOMOUS_MODE
+
+    def joystick_cb(self, msg : Joy) -> Wrench:
+        """
+        Callback function that receives joy messages and converts them into
+        wrench messages to be sent to the thruster allocation node. 
+        Handles software killswitch and control mode buttons,
+        and transitions between different states of operation.
+
+        Args:
+            msg: A ROS message containing the joy input data.
+
+        Returns:
+            A ROS message containing the wrench data that was sent to the thruster allocation node.
+        """       
         current_time = self.get_clock().now().to_msg()._sec
 
-        #Input from controller to joystick_interface
         buttons = {}
         axes = {}
 
         for i in range(len(msg.buttons)):
-            buttons[self.joystick_buttons_map[i]] = msg.buttons[i]
+            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]
+            axes[self.joystick_axes_map_[i]] = msg.axes[i]
 
         xbox_control_mode_button = buttons["A"]
         software_killswitch_button = buttons["B"]
@@ -135,52 +182,52 @@ def joystick_cb(self, msg):
         left_trigger = self.left_trigger_linear_converter(left_trigger)
 
         surge = axes[
-            "vertical_axis_left_stick"] * self.joystick_surge_scaling * left_trigger * right_trigger
+            "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
+            "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
+            "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:
+        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
 
         # 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
+            self.last_button_press_time_ = current_time
 
         # Toggle ks on and off
-        if self.state == states.NO_GO and software_killswitch_button:
+        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))
+            self.software_killswitch_signal_publisher_.publish(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))
+            self.software_killswitch_signal_publisher_.publish(Bool(data=False))
             # Turn off controller in sw killswitch
-            self.enable_controller_publisher.publish(Bool(data=False))
+            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)
             self.publish_wrench_message(wrench_msg)
-            self.state = states.NO_GO
+            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, yaw)
 
-        if self.state == states.XBOX_MODE:
+        if self.state_ == States.XBOX_MODE:
             self.get_logger().info("XBOX mode", throttle_duration_sec=1)
             self.publish_wrench_message(wrench_msg)
 
             if software_control_mode_button:
                 self.transition_to_autonomous_mode()
 
-        if self.state == states.AUTONOMOUS_MODE:
+        if self.state_ == States.AUTONOMOUS_MODE:
             self.get_logger().info("autonomous mode", throttle_duration_sec=1)
 
             if xbox_control_mode_button:
@@ -191,11 +238,8 @@ def joystick_cb(self, msg):
 
 def main():
     rclpy.init()
-
     joystick_interface = JoystickInterface()
-    print(joystick_interface.joystick_surge_scaling)
     rclpy.spin(joystick_interface)
-
     joystick_interface.destroy_node()
     rclpy.shutdown()
 

mission/joystick_interface/test/test_joystick_interface.py

@@ -1,5 +1,5 @@
 from joystick_interface.joystick_interface import JoystickInterface
-from joystick_interface.joystick_interface import states
+from joystick_interface.joystick_interface import States
 import rclpy
 from sensor_msgs.msg import Joy
 from sensor_msgs.msg import Joy
@@ -41,16 +41,16 @@ 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 == -100.0
-        assert wrench_msg.force.y == -100.0
+        assert wrench_msg.force.x == -50.0
+        assert wrench_msg.force.y == -50.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
+        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]
         joy_msg.buttons = [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0]
@@ -64,12 +64,12 @@ def test_killswitch_button(self):
     def test_moving_in_of_xbox_mode(self):
         rclpy.init()
         joystick = JoystickInterface()
-        joystick.state = states.XBOX_MODE
+        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]
         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
+        assert wrench_msg.force.x == -50.0
+        assert wrench_msg.force.y == -50.0
         assert wrench_msg.torque.z == 0.0
         rclpy.shutdown()