From 5eafe4e558c7f6b44dd7f7bf5c662590a40f4b8f Mon Sep 17 00:00:00 2001 From: Aldokan Date: Sun, 8 Oct 2023 14:54:20 +0200 Subject: [PATCH] Formatting --- .../joystick_interface/joystick_interface.py | 59 ++++++++++--------- .../test/test_joystick_interface.py | 26 ++++---- 2 files changed, 44 insertions(+), 41 deletions(-) diff --git a/mission/joystick_interface/joystick_interface/joystick_interface.py b/mission/joystick_interface/joystick_interface/joystick_interface.py index 4b8e0809..95e0ffb2 100755 --- a/mission/joystick_interface/joystick_interface/joystick_interface.py +++ b/mission/joystick_interface/joystick_interface/joystick_interface.py @@ -38,11 +38,11 @@ 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", ] @@ -50,8 +50,7 @@ def __init__(self): self.joy_subscriber = self.create_subscription(Joy, "/joystick/joy", self.joystick_cb, 1) self.wrench_publisher = self.create_publisher(Wrench, - "/thrust/wrench_input", - 1) + "/thrust/wrench_input", 1) self.declare_parameter('surge', 100.0) self.declare_parameter('sway', 100.0) @@ -71,22 +70,22 @@ 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)) # Signal that we are not in autonomous mode + Bool(data=True)) #Controller publisher self.enable_controller_publisher = self.create_publisher( Bool, "/controller/lqr/enable", 10) - def right_trigger_linear_converter( - self, rt_input): #triggers have an output from 1 to -1 - output_value = (rt_input + 1) * ( - -0.5) + 2 #does a linear conversion from (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): + output_value = (rt_input + 1) * (-0.5) + 2 return output_value - def left_trigger_linear_converter( - self, lt_input): #triggers have an output from 1 to -1 - ouput_value = lt_input * 0.25 + 0.75 #does a linear conversion from (1 to -1) to (1 to 0.5) + #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): @@ -101,17 +100,20 @@ def publish_wrench_message(self, 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)) + self.enable_controller_publisher.publish( + Bool(data=False)) + # signal that we enter xbox mode self.operational_mode_signal_publisher.publish( - Bool(data=True)) # signal that we enter xbox mode + 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)) # signal that we are turning on autonomous mode + Bool(data=False)) self.state = states.AUTONOMOUS_MODE def joystick_cb(self, msg): @@ -135,12 +137,9 @@ def joystick_cb(self, msg): 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 + 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: @@ -151,19 +150,23 @@ def joystick_cb(self, msg): # 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 - - if self.state == states.NO_GO and software_killswitch_button: # Toggle ks on and off + + # 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)) # signal that killswitch is not blocking + 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)) # signal that killswitch is blocking + Bool(data=False)) + # Turn off controller in sw killswitch self.enable_controller_publisher.publish( - Bool(data=False)) # Turn off controller in sw killswitch + 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) diff --git a/mission/joystick_interface/joystick_interface/test/test_joystick_interface.py b/mission/joystick_interface/joystick_interface/test/test_joystick_interface.py index 5eac192d..38e696b3 100644 --- a/mission/joystick_interface/joystick_interface/test/test_joystick_interface.py +++ b/mission/joystick_interface/joystick_interface/test/test_joystick_interface.py @@ -7,6 +7,7 @@ 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() @@ -15,6 +16,7 @@ def test_right_trigger_linear_converter(self): 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() @@ -23,6 +25,7 @@ def test_left_trigger_linear_converter(self): 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) @@ -31,43 +34,40 @@ def test_2d_wrench_msg(self): 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] #Should have 8 inputs self.joystick_axes_map - joy_msg.buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0] #Should have 11 inputs self.joystick_buttons_map + 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] #Should have 8 inputs self.joystick_axes_map - joy_msg.buttons = [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, - 0] #Should have 11 inputs self.joystick_buttons_map + 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() - def test_moving_in_and_out_of_xbox_mode(self): + #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] #Should have 8 inputs self.joystick_axes_map - joy_msg.buttons = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0] #Should have 11 inputs self.joystick_buttons_map + 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