diff --git a/mission/joystick_interface_auv/scripts/joystick_interface_auv.py b/mission/joystick_interface_auv/scripts/joystick_interface_auv.py index b8cc9733..e6b3444a 100644 --- a/mission/joystick_interface_auv/scripts/joystick_interface_auv.py +++ b/mission/joystick_interface_auv/scripts/joystick_interface_auv.py @@ -93,15 +93,15 @@ def __init__(self): self.enable_controller_publisher_ = self.create_publisher( Bool, "controller/lqr/enable", 10) - def create_2d_wrench_message(self, surge: float, sway: float, heave: float, + def create_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. Args: - x (float): The x component of the force vector. - y (float): The y component of the force vector. + surge (float): The x component of the force vector. + sway (float): The y component of the force vector. heave (float): The z component of the force vector. roll (float): The x component of the torque vector. pitch (float): The y component of the torque vector. @@ -131,13 +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, + wrench_msg = self.create_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) -> Wrench: """ Callback function that receives joy messages and converts them into wrench messages to be sent to the thruster allocation node. @@ -205,14 +205,14 @@ def joystick_cb(self, msg: Joy): # 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, + wrench_msg = self.create_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, + wrench_msg = self.create_wrench_message(surge, sway, heave, roll, pitch, yaw) if self.state_ == States.XBOX_MODE: 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 35900c23..49203105 100644 --- a/mission/joystick_interface_auv/test/test_joystick_interface_auv.py +++ b/mission/joystick_interface_auv/test/test_joystick_interface_auv.py @@ -1,20 +1,15 @@ -<<<<<<< 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 class TestJoystickInterface: - #test that the 2d wrench msg is created successfully - def test_2d_wrench_msg(self): + #test that the wrench msg is created successfully + def test_wrench_msg(self): rclpy.init() - msg = JoystickInterface().create_2d_wrench_message( + msg = JoystickInterface().create_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