diff --git a/asv_setup/config/params/gnc.yaml b/asv_setup/config/params/gnc.yaml deleted file mode 100644 index 9a0f9559..00000000 --- a/asv_setup/config/params/gnc.yaml +++ /dev/null @@ -1,5 +0,0 @@ -joystick: - scaling: - surge: 100.0 - sway: 100.0 - yaw: 100.0 diff --git a/asv_setup/launch/pc.yaml b/asv_setup/launch/pc.yaml index a4b13e7b..d65bd83f 100644 --- a/asv_setup/launch/pc.yaml +++ b/asv_setup/launch/pc.yaml @@ -1,31 +1,23 @@ 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" - - node: - package: joystick_interface - executable: joystick_interface.py - name: joystick_interface - output: screen + - include: + file: "src/vortex-asv/mission/joystick_interface/launch/joystick_interface_launch.yaml" diff --git a/mission/joystick_interface/CMakeLists.txt b/mission/joystick_interface/CMakeLists.txt new file mode 100644 index 00000000..0f5262d4 --- /dev/null +++ b/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() diff --git a/mission/joystick_interface/README.md b/mission/joystick_interface/README.md new file mode 100644 index 00000000..61f7b0b4 --- /dev/null +++ b/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 diff --git a/mission/joystick_interface/config/params.yaml b/mission/joystick_interface/config/params.yaml new file mode 100644 index 00000000..45d9bbd6 --- /dev/null +++ b/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 + + diff --git a/mission/joystick_interface/joystick_interface/__init__.py b/mission/joystick_interface/joystick_interface/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/mission/joystick_interface/joystick_interface/joystick_interface.py b/mission/joystick_interface/joystick_interface/joystick_interface.py new file mode 100755 index 00000000..5640b02e --- /dev/null +++ b/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() diff --git a/mission/joystick_interface/joystick_interface/test/__init__.py b/mission/joystick_interface/joystick_interface/test/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/mission/joystick_interface/joystick_interface/test/test_joystick_interface.py b/mission/joystick_interface/joystick_interface/test/test_joystick_interface.py new file mode 100644 index 00000000..92f88915 --- /dev/null +++ b/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() diff --git a/mission/joystick_interface/launch/joystick_interface_launch.yaml b/mission/joystick_interface/launch/joystick_interface_launch.yaml new file mode 100644 index 00000000..61aac95f --- /dev/null +++ b/mission/joystick_interface/launch/joystick_interface_launch.yaml @@ -0,0 +1,13 @@ +launch: +- node: + pkg: joystick_interface + exec: joystick_interface.py + name: joystick_interface + output: screen + param: + - from: src/vortex-asv/mission/joystick_interface/config/params.yaml + + + + + diff --git a/mission/joystick_interface/package.xml b/mission/joystick_interface/package.xml new file mode 100644 index 00000000..50d62ad2 --- /dev/null +++ b/mission/joystick_interface/package.xml @@ -0,0 +1,25 @@ + + + + joystick_interface + 0.0.0 + Joystick interface package + + aleksos + + MIT + + ament_cmake_python + + rclpy + geometry_msgs + sensor_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_pytest + + + ament_cmake + +