diff --git a/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py b/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py index 72a7c909..8db781ff 100644 --- a/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py +++ b/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py @@ -1,5 +1,5 @@ import os -from launch import LaunchDescription +from launch import LaunchDescription from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory @@ -11,10 +11,9 @@ def generate_launch_description(): executable='joystick_interface_auv.py', name='joystick_interface_auv', output='screen', - parameters=[os.path.join(get_package_share_directory('joystick_interface_auv'), 'config/param_joystick_interface_auv.yaml')] - ) - - return LaunchDescription([ - joystick_interface_node - ]) + parameters=[ + os.path.join(get_package_share_directory('joystick_interface_auv'), + 'config/param_joystick_interface_auv.yaml') + ]) + return LaunchDescription([joystick_interface_node]) diff --git a/mission/joystick_interface_auv/scripts/joystick_interface_auv.py b/mission/joystick_interface_auv/scripts/joystick_interface_auv.py index af49ffea..624fc3b3 100644 --- a/mission/joystick_interface_auv/scripts/joystick_interface_auv.py +++ b/mission/joystick_interface_auv/scripts/joystick_interface_auv.py @@ -5,16 +5,20 @@ from sensor_msgs.msg import Joy from std_msgs.msg import Bool + class States: XBOX_MODE = 1 AUTONOMOUS_MODE = 2 NO_GO = 3 + class JoystickInterface(Node): def __init__(self): super().__init__('joystick_interface_node') - 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.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 @@ -36,35 +40,41 @@ 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", ] - self.joy_subscriber_ = self.create_subscription(Joy, "joystick/joy", - self.joystick_cb, 1) + 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', 60.0) - self.declare_parameter('sway_scale_factor', 60.0) - self.declare_parameter('yaw_scale_factor', 60.0) - self.declare_parameter('heave_scale_factor', 35.0) - self.declare_parameter('roll_scale_factor', -30.0) - self.declare_parameter('pitch_scale_factor', 20.0) + "thrust/wrench_input", + 1) + + self.declare_parameter('surge_scale_factor', 60.0) + self.declare_parameter('sway_scale_factor', 60.0) + self.declare_parameter('yaw_scale_factor', 60.0) + self.declare_parameter('heave_scale_factor', 35.0) + self.declare_parameter('roll_scale_factor', -30.0) + self.declare_parameter('pitch_scale_factor', 20.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_heave_scaling_ = self.get_parameter('heave_scale_factor').value - self.joystick_roll_scaling_ = self.get_parameter('roll_scale_factor').value - self.joystick_pitch_scaling_ = self.get_parameter('pitch_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 + self.joystick_heave_scaling_ = self.get_parameter( + 'heave_scale_factor').value + self.joystick_roll_scaling_ = self.get_parameter( + 'roll_scale_factor').value + self.joystick_pitch_scaling_ = self.get_parameter( + 'pitch_scale_factor').value #Killswitch publisher self.software_killswitch_signal_publisher_ = self.create_publisher( @@ -75,15 +85,17 @@ 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)) #Controller publisher self.enable_controller_publisher_ = self.create_publisher( Bool, "controller/lqr/enable", 10) - - def create_2d_wrench_message(self, surge: float, sway: float, heave: float, roll: float, pitch: float, yaw: float) -> Wrench: + + def create_2d_wrench_message(self, surge: float, sway: float, heave: float, + roll: float, pitch: float, + yaw: float) -> Wrench: """ Creates a 2D wrench message with the given x, y, heave, roll, pitch, and yaw values. @@ -99,9 +111,9 @@ def create_2d_wrench_message(self, surge: float, sway: float, heave: float, roll Wrench: A 2D wrench message with the given values. """ wrench_msg = Wrench() - wrench_msg.force.x = surge - wrench_msg.force.y = sway - wrench_msg.force.z = heave + wrench_msg.force.x = surge + wrench_msg.force.y = sway + wrench_msg.force.z = heave wrench_msg.torque.x = roll wrench_msg.torque.y = pitch wrench_msg.torque.z = yaw @@ -119,12 +131,13 @@ def transition_to_autonomous_mode(self): """ 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, 0.0, 0.0, 0.0) + wrench_msg = self.create_2d_wrench_message(0.0, 0.0, 0.0, 0.0, 0.0, + 0.0) self.wrench_publisher_.publish(wrench_msg) self.operational_mode_signal_publisher_.publish(Bool(data=False)) self.state_ = States.AUTONOMOUS_MODE - def joystick_cb(self, msg : Joy): + def joystick_cb(self, msg: Joy): """ Callback function that receives joy messages and converts them into wrench messages to be sent to the thruster allocation node. @@ -142,14 +155,13 @@ def joystick_cb(self, msg : Joy): 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"] @@ -158,16 +170,23 @@ def joystick_cb(self, msg : Joy): left_shoulder = buttons["LB"] right_shoulder = buttons["RB"] +<<<<<<< HEAD surge = axes[ "vertical_axis_left_stick"] * self.joystick_surge_scaling_ sway = axes[ "horizontal_axis_left_stick"] * self.joystick_sway_scaling_ heave = -(left_trigger - right_trigger)/2 * self.joystick_heave_scaling_ roll = (right_shoulder - left_shoulder) * self.joystick_roll_scaling_ +======= + surge = axes["vertical_axis_left_stick"] * self.joystick_surge_scaling_ + sway = axes["horizontal_axis_left_stick"] * self.joystick_sway_scaling_ + heave = -(axes[["RT"] - axes["LT"]]) / 2 * self.joystick_heave_scaling_ + roll = axes[self.buttons["RB"] - + self.buttons["LB"]] * self.joystick_roll_scaling_ +>>>>>>> refs/remotes/origin/feature/joystick_interface_in_ros2 pitch = -axes[ "vertical_axis_right_stick"] * self.joystick_pitch_scaling_ - yaw = axes[ - "horizontal_axis_right_stick"] * self.joystick_yaw_scaling_ + yaw = axes["horizontal_axis_right_stick"] * self.joystick_yaw_scaling_ # Debounce for the buttons if current_time - self.last_button_press_time_ < self.debounce_duration_: @@ -189,17 +208,20 @@ def joystick_cb(self, msg : Joy): 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)) # Publish a zero wrench message when sw killing - wrench_msg = self.create_2d_wrench_message(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + wrench_msg = self.create_2d_wrench_message(0.0, 0.0, 0.0, 0.0, 0.0, + 0.0) self.wrench_publisher_.publish(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, heave, roll, pitch, yaw) + wrench_msg = self.create_2d_wrench_message(surge, sway, heave, roll, + pitch, yaw) if self.state_ == States.XBOX_MODE: self.get_logger().info("XBOX mode", throttle_duration_sec=1) @@ -216,6 +238,7 @@ def joystick_cb(self, msg : Joy): return wrench_msg + def main(): rclpy.init() joystick_interface = JoystickInterface() @@ -225,4 +248,4 @@ def main(): if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/mission/joystick_interface_auv/test/test_joystick_interface_auv.py b/mission/joystick_interface_auv/test/test_joystick_interface_auv.py index d8fe8d4a..35900c23 100644 --- a/mission/joystick_interface_auv/test/test_joystick_interface_auv.py +++ b/mission/joystick_interface_auv/test/test_joystick_interface_auv.py @@ -1,5 +1,10 @@ +<<<<<<< HEAD from scripts.joystick_interface_auv import JoystickInterface from scripts.joystick_interface_auv import States +======= +from joystick_interface_auv.joystick_interface_auv import JoystickInterface +from joystick_interface_auv.joystick_interface_auv import States +>>>>>>> refs/remotes/origin/feature/joystick_interface_in_ros2 import rclpy from sensor_msgs.msg import Joy from sensor_msgs.msg import Joy @@ -9,10 +14,11 @@ class TestJoystickInterface: #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, 5.0, 6.0, 7.0) - assert msg.force.x == 2.0 - assert msg.force.y == 3.0 - assert msg.force.z == 4.0 + msg = JoystickInterface().create_2d_wrench_message( + 2.0, 3.0, 4.0, 5.0, 6.0, 7.0) + assert msg.force.x == 2.0 + assert msg.force.y == 3.0 + assert msg.force.z == 4.0 assert msg.torque.x == 5.0 assert msg.torque.y == 6.0 assert msg.torque.z == 7.0 @@ -25,8 +31,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 == -60.0 - assert wrench_msg.force.y == -60.0 + assert wrench_msg.force.x == -60.0 + assert wrench_msg.force.y == -60.0 assert wrench_msg.torque.z == 0.0 rclpy.shutdown() @@ -39,8 +45,8 @@ def test_killswitch_button(self): 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.force.x == 0.0 + assert wrench_msg.force.y == 0.0 assert wrench_msg.torque.z == 0.0 rclpy.shutdown() @@ -53,7 +59,7 @@ def test_moving_in_of_xbox_mode(self): 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 == -60.0 - assert wrench_msg.force.y == -60.0 + assert wrench_msg.force.x == -60.0 + assert wrench_msg.force.y == -60.0 assert wrench_msg.torque.z == 0.0 rclpy.shutdown()