From 296cb78e061b9a05abbbfc78cabe1a130efbc53f Mon Sep 17 00:00:00 2001 From: Aldokan Date: Sun, 11 Feb 2024 13:58:59 +0100 Subject: [PATCH] fixed mapping in accordance to NED --- auv_setup/config/robots/new_auv.yaml | 0 mission/joystick_interface_auv/README.md | 3 ++ .../config/param_joystick_interface_auv.yaml | 14 +++---- .../joystick_interface_auv.py | 39 ++++++++++--------- 4 files changed, 31 insertions(+), 25 deletions(-) create mode 100644 auv_setup/config/robots/new_auv.yaml create mode 100644 mission/joystick_interface_auv/README.md diff --git a/auv_setup/config/robots/new_auv.yaml b/auv_setup/config/robots/new_auv.yaml new file mode 100644 index 00000000..e69de29b diff --git a/mission/joystick_interface_auv/README.md b/mission/joystick_interface_auv/README.md new file mode 100644 index 00000000..2c54e8e4 --- /dev/null +++ b/mission/joystick_interface_auv/README.md @@ -0,0 +1,3 @@ +## Joystick interface +A joystick interface for manual control of AUV. A ROS2 node that subscribes on inputs from the XBOX controller and publishes the according wrench message to be used in Thruster Allocation. + diff --git a/mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml b/mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml index 4f77334d..e74be021 100644 --- a/mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml +++ b/mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml @@ -1,8 +1,8 @@ -joystick_interface: +joystick_interface_auv: ros__parameters: - surge_scale_factor: 60.0 - sway_scale_factor: 60.0 - yaw_scale_factor: 60.0 - heave_scale_factor: 35.0 - roll_scale_factor: 30.0 - pitch_scale_factor: 20.0 + surge_scale_factor: 60.0 + sway_scale_factor: 60.0 + yaw_scale_factor: 60.0 + heave_scale_factor: 17.5 + roll_scale_factor: 30.0 + pitch_scale_factor: 20.0 diff --git a/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv.py b/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv.py index b2d28542..b7770cef 100644 --- a/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv.py +++ b/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv.py @@ -21,8 +21,8 @@ def __init__(self): ) self.last_button_press_time_ = 0 - self.debounce_duration_ = 0.25 - self.state_ = States.NO_GO + self.debounce_duration_ = 0.25 + self.state_ = States.NO_GO self.joystick_buttons_map_ = [ "A", @@ -55,12 +55,12 @@ def __init__(self): "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) + self.declare_parameter('surge_scale_factor', 0.0) + self.declare_parameter('sway_scale_factor', 0.0) + self.declare_parameter('yaw_scale_factor', 0.0) + self.declare_parameter('heave_scale_factor', 0.0) + self.declare_parameter('roll_scale_factor', 0.0) + self.declare_parameter('pitch_scale_factor', 0.0) #Gets the scaling factors from the yaml file self.joystick_surge_scaling_ = self.get_parameter( @@ -94,7 +94,8 @@ def __init__(self): Bool, "controller/lqr/enable", 10) def create_wrench_message(self, surge: float, sway: float, heave: float, - roll: float, pitch: float, yaw: float) -> Wrench: + roll: float, pitch: float, + yaw: float) -> Wrench: """ Creates a 2D wrench message with the given x, y, heave, roll, pitch, and yaw values. @@ -130,7 +131,8 @@ 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_wrench_message(0.0, 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 @@ -168,14 +170,15 @@ def joystick_cb(self, msg: Joy) -> Wrench: left_shoulder = buttons["LB"] right_shoulder = buttons["RB"] - 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 = (left_shoulder - right_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 = (left_trigger - right_trigger) * self.joystick_heave_scaling_ + roll = (right_shoulder - left_shoulder) * self.joystick_roll_scaling_ 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_: @@ -203,14 +206,14 @@ def joystick_cb(self, msg: Joy) -> Wrench: self.enable_controller_publisher_.publish(Bool(data=False)) # Publish a zero wrench message when sw killing wrench_msg = self.create_wrench_message(0.0, 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_wrench_message(surge, sway, heave, roll, - pitch, yaw) + pitch, yaw) if self.state_ == States.XBOX_MODE: self.get_logger().info("XBOX mode", throttle_duration_sec=1)