diff --git a/asv_setup/launch/pc.launch.py b/asv_setup/launch/pc.launch.py index 1248ae25..98c62f71 100644 --- a/asv_setup/launch/pc.launch.py +++ b/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'), diff --git a/mission/joystick_interface/CMakeLists.txt b/mission/joystick_interface/CMakeLists.txt index e58d5013..cc2e2cb4 100644 --- a/mission/joystick_interface/CMakeLists.txt +++ b/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} ) diff --git a/mission/joystick_interface/README.md b/mission/joystick_interface/README.md index 61f7b0b4..fc84dc37 100644 --- a/mission/joystick_interface/README.md +++ b/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. \ No newline at end of file diff --git a/mission/joystick_interface/package.xml b/mission/joystick_interface/package.xml index 50d62ad2..08e812e9 100644 --- a/mission/joystick_interface/package.xml +++ b/mission/joystick_interface/package.xml @@ -14,6 +14,7 @@ rclpy geometry_msgs sensor_msgs + ros2launch ament_lint_auto ament_lint_common diff --git a/mission/joystick_interface/joystick_interface/__init__.py b/mission/joystick_interface/scripts/__init__.py similarity index 100% rename from mission/joystick_interface/joystick_interface/__init__.py rename to mission/joystick_interface/scripts/__init__.py diff --git a/mission/joystick_interface/joystick_interface/joystick_interface.py b/mission/joystick_interface/scripts/joystick_interface.py similarity index 50% rename from mission/joystick_interface/joystick_interface/joystick_interface.py rename to mission/joystick_interface/scripts/joystick_interface.py index d76f4555..e2123e6f 100755 --- a/mission/joystick_interface/joystick_interface/joystick_interface.py +++ b/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() diff --git a/mission/joystick_interface/test/test_joystick_interface.py b/mission/joystick_interface/test/test_joystick_interface.py index 92f88915..1b492d6a 100644 --- a/mission/joystick_interface/test/test_joystick_interface.py +++ b/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,8 +41,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 == -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() @@ -50,7 +50,7 @@ def test_input_from_controller_into_wrench_msg(self): 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()