Merge branch 'development' into Freya_TF_tree

.github/workflows/pep8-formatter.yml

@@ -1,8 +1,6 @@
 name: Format python code
 on: 
-  push:
-    paths:
-    - '**.py'
+  workflow_dispatch:
 jobs:
   autoyapf:
     runs-on: ubuntu-latest
@@ -26,4 +24,4 @@ jobs:
           git config --global user.email '[email protected]'
           git remote set-url origin https://x-access-token:$${{ secrets.GITHUB_TOKEN }}@github.com/${{ github.repository }}
           git commit -am "Automated autoyapf fixes"
-          git push
+          git push

asv_setup/launch/pc.launch.yaml

@@ -1,31 +1,24 @@
 launch:
-  - set_environment_variable:
+  - set_env:
       name: ROSCONSOLE_FORMAT
       value: "[${severity}] [${time}] [${node}]: ${message}"
 
-  - declare_namespace:
-      namespace: joystick
-      with_arguments:
-        - ns_joystick
-
-  - rosparam_load_file:
-      param_file: asv_setup/config/params/gnc.yaml
-
-  - namespace:
-      namespace: ${arg.ns_joystick}
-      prefix: joy
-    launch:
+  - group:
       - node:
-          package: joy
-          executable: joy_node
+          pkg: joy
+          exec: joy_node
           name: joystick_driver
           output: screen
-          parameters:
-            _deadzone: 0.15
-            _autorepeat_rate: 100
+          param:
+            - name: _deadzone
+              value: 0.15
+            - name: _autorepeat_rate
+              value: 100
+          remap:
+          -
+                from: "/joy"
+                to: "/joystick/joy"
+
+  - include:
+      file: "src/vortex-asv/mission/joystick_interface/launch/joystick_interface.launch.yaml" 
 
-  - node:
-      package: joystick_interface
-      executable: joystick_interface.py
-      name: joystick_interface
-      output: screen

mission/joystick_interface/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.8)
+project(joystick_interface)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_python REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+ament_python_install_package(${PROJECT_NAME})
+
+install(PROGRAMS
+  joystick_interface/joystick_interface.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_pytest REQUIRED)
+  set(_pytest_tests
+    joystick_interface/test/test_joystick_interface.py
+  )
+  foreach(_test_path ${_pytest_tests})
+    get_filename_component(_test_name ${_test_path} NAME_WE)
+    ament_add_pytest_test(${_test_name} ${_test_path}
+      APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}
+      TIMEOUT 60
+      WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+    )
+  endforeach()
+endif()
+
+ament_package()

mission/joystick_interface/README.md

@@ -0,0 +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

mission/joystick_interface/config/params.yaml

@@ -0,0 +1,7 @@
+joystick_interface:
+  ros__parameters:
+    surge_scale_factor: 50.0
+    sway_scale_factor: 50.0
+    yaw_scale_factor: 50.0
+
+

mission/joystick_interface/joystick_interface/joystick_interface.py

@@ -0,0 +1,204 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Wrench
+from sensor_msgs.msg import Joy
+from std_msgs.msg import Bool
+
+
+class states:
+    XBOX_MODE = 1
+    AUTONOMOUS_MODE = 2
+    NO_GO = 3  # Do nothing
+
+
+class JoystickInterface(Node):
+
+    def __init__(self):
+        super().__init__('joystick_interface_node')
+        self.get_logger().info("Joystick interface is up and running")
+
+        self.last_button_press_time = 0
+        self.debounce_duration = 0.25
+        self.state = states.NO_GO
+
+        self.joystick_buttons_map = [
+            "A",
+            "B",
+            "X",
+            "Y",
+            "LB",
+            "RB",
+            "back",
+            "start",
+            "power",
+            "stick_button_left",
+            "stick_button_right",
+        ]
+
+        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
+            "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)
+
+        self.declare_parameter('surge_scale_factor', 100.0)
+        self.declare_parameter('sway_scale_factor', 100.0)
+        self.declare_parameter('yaw_scale_factor', 100.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
+
+        #Killswitch publisher
+        self.software_killswitch_signal_publisher = self.create_publisher(
+            Bool, "softWareKillSwitch", 10)
+        self.software_killswitch_signal_publisher.publish(
+            Bool(data=False))  #Killswitch is not active
+
+        #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)
+
+    #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
+
+    #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):
+        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 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
+
+    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))
+        self.state = states.AUTONOMOUS_MODE
+
+    def joystick_cb(self, msg):
+        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]
+
+        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"]
+        left_trigger = axes['LT']
+        right_trigger = axes['RT']
+        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
+
+        # Debounce for the buttons
+        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
+
+        # 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))
+            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))
+            # 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)
+            self.publish_wrench_message(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, yaw)
+
+        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:
+            self.get_logger().info("autonomous mode", throttle_duration_sec=1)
+
+            if xbox_control_mode_button:
+                self.transition_to_xbox_mode()
+
+        return wrench_msg
+
+
+def main():
+    rclpy.init()
+
+    joystick_interface = JoystickInterface()
+    print(joystick_interface.joystick_surge_scaling)
+    rclpy.spin(joystick_interface)
+
+    joystick_interface.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

mission/joystick_interface/joystick_interface/test/test_joystick_interface.py

@@ -0,0 +1,75 @@
+from joystick_interface.joystick_interface import JoystickInterface
+from joystick_interface.joystick_interface import states
+import rclpy
+from sensor_msgs.msg import Joy
+from sensor_msgs.msg import Joy
+
+
+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()
+        assert joystick.right_trigger_linear_converter(1) == 1
+        assert joystick.right_trigger_linear_converter(0) == 1.5
+        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()
+        assert joystick.left_trigger_linear_converter(1) == 1
+        assert joystick.left_trigger_linear_converter(0) == 0.75
+        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)
+        assert msg.force.x == 2.0
+        assert msg.force.y == 3.0
+        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]
+        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]
+        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()
+
+    #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]
+        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.torque.z == 0.0
+        rclpy.shutdown()