From dba0690689a93bb376daa6ee72f5476cbfd86845 Mon Sep 17 00:00:00 2001 From: Aldokan Date: Sun, 4 Feb 2024 18:00:25 +0100 Subject: [PATCH 01/18] translated the joystick_interface into ros2 --- mission/joystick_interface_auv/CMakeLists.txt | 41 ++++ .../config/param_joystick_interface_auv.yaml | 8 + .../joystick_interface_auv/__init__.py | 0 .../launch/joystick_interface_auv.launch.py | 20 ++ mission/joystick_interface_auv/package.xml | 26 ++ .../scripts/__init__.py | 0 .../scripts/joystick_interface_auv.py | 226 ++++++++++++++++++ .../joystick_interface_auv/test/__init__.py | 0 .../test/log/COLCON_IGNORE | 0 .../joystick_interface_auv/test/log/latest | 1 + .../test/log/latest_test | 1 + .../test/log/latest_test-result | 1 + .../logger_all.log | 4 + .../log/test_2024-02-04_17-52-25/events.log | 2 + .../test_2024-02-04_17-52-25/logger_all.log | 83 +++++++ .../test/test_joystick_interface_auv.py | 59 +++++ 16 files changed, 472 insertions(+) create mode 100644 mission/joystick_interface_auv/CMakeLists.txt create mode 100644 mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml create mode 100644 mission/joystick_interface_auv/joystick_interface_auv/__init__.py create mode 100644 mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py create mode 100644 mission/joystick_interface_auv/package.xml create mode 100644 mission/joystick_interface_auv/scripts/__init__.py create mode 100644 mission/joystick_interface_auv/scripts/joystick_interface_auv.py create mode 100644 mission/joystick_interface_auv/test/__init__.py create mode 100644 mission/joystick_interface_auv/test/log/COLCON_IGNORE create mode 120000 mission/joystick_interface_auv/test/log/latest create mode 120000 mission/joystick_interface_auv/test/log/latest_test create mode 120000 mission/joystick_interface_auv/test/log/latest_test-result create mode 100644 mission/joystick_interface_auv/test/log/test-result_2024-02-04_17-52-41/logger_all.log create mode 100644 mission/joystick_interface_auv/test/log/test_2024-02-04_17-52-25/events.log create mode 100644 mission/joystick_interface_auv/test/log/test_2024-02-04_17-52-25/logger_all.log create mode 100644 mission/joystick_interface_auv/test/test_joystick_interface_auv.py diff --git a/mission/joystick_interface_auv/CMakeLists.txt b/mission/joystick_interface_auv/CMakeLists.txt new file mode 100644 index 00000000..d6cef301 --- /dev/null +++ b/mission/joystick_interface_auv/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.8) +project(joystick_interface_auv) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +ament_python_install_package(scripts) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS + scripts/joystick_interface_auv.py + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + set(_pytest_tests + test/test_joystick_interface_auv.py + ) + foreach(_test_path ${_pytest_tests}) + get_filename_component(_test_name ${_test_path} NAME_WE) + ament_add_pytest_test(${_test_name} ${_test_path} + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} + TIMEOUT 60 + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + endforeach() +endif() + +ament_package() diff --git a/mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml b/mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml new file mode 100644 index 00000000..44551346 --- /dev/null +++ b/mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml @@ -0,0 +1,8 @@ +joystick_interface: + 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 diff --git a/mission/joystick_interface_auv/joystick_interface_auv/__init__.py b/mission/joystick_interface_auv/joystick_interface_auv/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py b/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py new file mode 100644 index 00000000..72a7c909 --- /dev/null +++ b/mission/joystick_interface_auv/launch/joystick_interface_auv.launch.py @@ -0,0 +1,20 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + + joystick_interface_node = Node( + package='joystick_interface_auv', + 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 + ]) + diff --git a/mission/joystick_interface_auv/package.xml b/mission/joystick_interface_auv/package.xml new file mode 100644 index 00000000..dd220c79 --- /dev/null +++ b/mission/joystick_interface_auv/package.xml @@ -0,0 +1,26 @@ + + + + joystick_interface_auv + 0.0.0 + Joystick interface package + + aleksos + + MIT + + ament_cmake_python + + rclpy + geometry_msgs + sensor_msgs + ros2launch + + ament_lint_auto + ament_lint_common + ament_cmake_pytest + + + ament_cmake + + diff --git a/mission/joystick_interface_auv/scripts/__init__.py b/mission/joystick_interface_auv/scripts/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/mission/joystick_interface_auv/scripts/joystick_interface_auv.py b/mission/joystick_interface_auv/scripts/joystick_interface_auv.py new file mode 100644 index 00000000..2ef126d9 --- /dev/null +++ b/mission/joystick_interface_auv/scripts/joystick_interface_auv.py @@ -0,0 +1,226 @@ +#!/usr/bin/python3 +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Wrench +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.last_button_press_time_ = 0 + self.debounce_duration_ = 0.25 + self.state_ = States.NO_GO + + self.joystick_buttons_map_ = [ + "A", + "B", + "X", + "Y", + "LB", + "RB", + "back", + "start", + "power", + "stick_button_left", + "stick_button_right", + ] + + 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_right_stick", + "RT", #Positive thrust/torque multiplier + "dpad_horizontal", + "dpad_vertical", + ] + + 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) + + #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 + + #Killswitch publisher + self.software_killswitch_signal_publisher_ = self.create_publisher( + Bool, "softWareKillSwitch", 10) + self.software_killswitch_signal_publisher_.publish( + Bool(data=False)) #Killswitch is not active + + #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: + """ + 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. + 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. + yaw (float): The z component of the torque vector. + + Returns: + 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.torque.x = roll + wrench_msg.torque.y = pitch + wrench_msg.torque.z = yaw + return wrench_msg + + def transition_to_xbox_mode(self): + """ + 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): + """ + 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) + 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): + """ + 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 + + 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"] + + 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_ + pitch = -axes[ + "vertical_axis_right_stick"] * self.joystick_pitch_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_: + 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 + + # 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)) + 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)) + # 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) + 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) + + if self.state_ == States.XBOX_MODE: + self.get_logger().info("XBOX mode", throttle_duration_sec=1) + self.wrench_publisher_.publish(wrench_msg) + + if software_control_mode_button: + self.transition_to_autonomous_mode() + + if self.state_ == States.AUTONOMOUS_MODE: + self.get_logger().info("autonomous mode", throttle_duration_sec=1) + + if xbox_control_mode_button: + self.transition_to_xbox_mode() + + return wrench_msg + +def main(): + rclpy.init() + joystick_interface = JoystickInterface() + rclpy.spin(joystick_interface) + joystick_interface.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/mission/joystick_interface_auv/test/__init__.py b/mission/joystick_interface_auv/test/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/mission/joystick_interface_auv/test/log/COLCON_IGNORE b/mission/joystick_interface_auv/test/log/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/mission/joystick_interface_auv/test/log/latest b/mission/joystick_interface_auv/test/log/latest new file mode 120000 index 00000000..86d98e8c --- /dev/null +++ b/mission/joystick_interface_auv/test/log/latest @@ -0,0 +1 @@ +latest_test-result \ No newline at end of file diff --git a/mission/joystick_interface_auv/test/log/latest_test b/mission/joystick_interface_auv/test/log/latest_test new file mode 120000 index 00000000..e737a8ae --- /dev/null +++ b/mission/joystick_interface_auv/test/log/latest_test @@ -0,0 +1 @@ +test_2024-02-04_17-52-25 \ No newline at end of file diff --git a/mission/joystick_interface_auv/test/log/latest_test-result b/mission/joystick_interface_auv/test/log/latest_test-result new file mode 120000 index 00000000..fe6708af --- /dev/null +++ b/mission/joystick_interface_auv/test/log/latest_test-result @@ -0,0 +1 @@ +test-result_2024-02-04_17-52-41 \ No newline at end of file diff --git a/mission/joystick_interface_auv/test/log/test-result_2024-02-04_17-52-41/logger_all.log b/mission/joystick_interface_auv/test/log/test-result_2024-02-04_17-52-41/logger_all.log new file mode 100644 index 00000000..f7e564bc --- /dev/null +++ b/mission/joystick_interface_auv/test/log/test-result_2024-02-04_17-52-41/logger_all.log @@ -0,0 +1,4 @@ +[0.192s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'test-result', '--all'] +[0.192s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='test-result', test_result_base='build', all=True, result_files_only=False, verbose=False, delete=False, delete_yes=False, verb_parser=, verb_extension=, main=>) +[0.194s] Level 1:colcon.colcon_test_result.test_result:get_test_results(ctest) +[0.195s] Level 1:colcon.colcon_test_result.test_result:get_test_results(xunit) diff --git a/mission/joystick_interface_auv/test/log/test_2024-02-04_17-52-25/events.log b/mission/joystick_interface_auv/test/log/test_2024-02-04_17-52-25/events.log new file mode 100644 index 00000000..a74e00e6 --- /dev/null +++ b/mission/joystick_interface_auv/test/log/test_2024-02-04_17-52-25/events.log @@ -0,0 +1,2 @@ +[0.000000] (-) TimerEvent: {} +[0.000546] (-) EventReactorShutdown: {} diff --git a/mission/joystick_interface_auv/test/log/test_2024-02-04_17-52-25/logger_all.log b/mission/joystick_interface_auv/test/log/test_2024-02-04_17-52-25/logger_all.log new file mode 100644 index 00000000..bb87cf52 --- /dev/null +++ b/mission/joystick_interface_auv/test/log/test_2024-02-04_17-52-25/logger_all.log @@ -0,0 +1,83 @@ +[0.292s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'test', '--ctest-args', 'tests'] +[0.292s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='test', build_base='build', install_base='install', merge_install=False, test_result_base=None, retest_until_fail=0, retest_until_pass=0, abort_on_error=False, return_code_on_test_failure=False, executor='parallel', parallel_workers=12, event_handlers=None, ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['.'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], ctest_args=['tests'], python_testing=None, pytest_args=None, pytest_with_coverage=False, unittest_args=None, verb_parser=, verb_extension=, main=>) +[0.311s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters +[0.311s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters +[0.311s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters +[0.311s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters +[0.311s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover +[0.311s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover +[0.311s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/aleksos/Documents/Vortex/ros2_ws/src/vortex-auv/mission/joystick_interface_auv/test' +[0.312s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install'] +[0.312s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore' +[0.312s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install' +[0.312s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg'] +[0.312s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg' +[0.312s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta'] +[0.312s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta' +[0.312s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros'] +[0.312s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros' +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python'] +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake' +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python' +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py'] +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py' +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(__pycache__) by extensions ['ignore', 'ignore_ament_install'] +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(__pycache__) by extension 'ignore' +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(__pycache__) by extension 'ignore_ament_install' +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(__pycache__) by extensions ['colcon_pkg'] +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(__pycache__) by extension 'colcon_pkg' +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(__pycache__) by extensions ['colcon_meta'] +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(__pycache__) by extension 'colcon_meta' +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(__pycache__) by extensions ['ros'] +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(__pycache__) by extension 'ros' +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(__pycache__) by extensions ['cmake', 'python'] +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(__pycache__) by extension 'cmake' +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(__pycache__) by extension 'python' +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(__pycache__) by extensions ['python_setup_py'] +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(__pycache__) by extension 'python_setup_py' +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install'] +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore' +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore_ament_install' +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['colcon_pkg'] +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'colcon_pkg' +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['colcon_meta'] +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'colcon_meta' +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ros'] +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ros' +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['cmake', 'python'] +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'cmake' +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'python' +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['python_setup_py'] +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'python_setup_py' +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install'] +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore' +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore_ament_install' +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['colcon_pkg'] +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'colcon_pkg' +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['colcon_meta'] +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'colcon_meta' +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ros'] +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ros' +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['cmake', 'python'] +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'cmake' +[0.321s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'python' +[0.322s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['python_setup_py'] +[0.322s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'python_setup_py' +[0.322s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install'] +[0.322s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore' +[0.322s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored +[0.322s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults +[0.322s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover +[0.322s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults +[0.322s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover +[0.322s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults +[0.326s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor +[0.328s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete +[0.328s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop +[0.328s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed +[0.328s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '0' +[0.328s] DEBUG:colcon.colcon_core.event_reactor:joining thread +[0.333s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems +[0.333s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems +[0.333s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2' +[0.343s] DEBUG:colcon.colcon_core.event_reactor:joined thread diff --git a/mission/joystick_interface_auv/test/test_joystick_interface_auv.py b/mission/joystick_interface_auv/test/test_joystick_interface_auv.py new file mode 100644 index 00000000..388e7943 --- /dev/null +++ b/mission/joystick_interface_auv/test/test_joystick_interface_auv.py @@ -0,0 +1,59 @@ +from joystick_interface_auv.joystick_interface_auv import JoystickInterface +from joystick_interface_auv.joystick_interface_auv import States +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): + 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 + assert msg.torque.x == 5.0 + assert msg.torque.y == 6.0 + assert msg.torque.z == 7.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] + 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.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] + 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() + + #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] + 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.torque.z == 0.0 + rclpy.shutdown() From cc0fbf9fb411eac45a6506e4e960822e7bfdf0fb Mon Sep 17 00:00:00 2001 From: github-actions Date: Sun, 4 Feb 2024 17:01:17 +0000 Subject: [PATCH 02/18] Automated autoyapf fixes --- .../launch/joystick_interface_auv.launch.py | 13 +-- .../scripts/joystick_interface_auv.py | 104 ++++++++++-------- .../test/test_joystick_interface_auv.py | 23 ++-- 3 files changed, 76 insertions(+), 64 deletions(-) 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 2ef126d9..762b8773 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,30 +155,25 @@ 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"] - 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_ + 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_ 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_: @@ -187,17 +195,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) @@ -214,6 +225,7 @@ def joystick_cb(self, msg : Joy): return wrench_msg + def main(): rclpy.init() joystick_interface = JoystickInterface() @@ -223,4 +235,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 388e7943..38ef91a9 100644 --- a/mission/joystick_interface_auv/test/test_joystick_interface_auv.py +++ b/mission/joystick_interface_auv/test/test_joystick_interface_auv.py @@ -1,4 +1,4 @@ -from joystick_interface_auv.joystick_interface_auv import JoystickInterface +from joystick_interface_auv.joystick_interface_auv import JoystickInterface from joystick_interface_auv.joystick_interface_auv import States import rclpy from sensor_msgs.msg import Joy @@ -9,10 +9,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 +26,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 +40,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 +54,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() From 895598d99caf7731ca2809aa447869d658858f33 Mon Sep 17 00:00:00 2001 From: github-actions Date: Wed, 7 Feb 2024 16:30:26 +0000 Subject: [PATCH 03/18] Automated autoyapf fixes --- .../scripts/joystick_interface_auv.py | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/mission/joystick_interface_auv/scripts/joystick_interface_auv.py b/mission/joystick_interface_auv/scripts/joystick_interface_auv.py index b8cc9733..a6d866e2 100644 --- a/mission/joystick_interface_auv/scripts/joystick_interface_auv.py +++ b/mission/joystick_interface_auv/scripts/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", @@ -170,11 +170,10 @@ def joystick_cb(self, msg: Joy): 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_ + 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_ pitch = -axes[ "vertical_axis_right_stick"] * self.joystick_pitch_scaling_ From eceb4b7439f1e576ef9b3d5af00ab97861e09722 Mon Sep 17 00:00:00 2001 From: Aldokan Date: Wed, 7 Feb 2024 17:57:53 +0100 Subject: [PATCH 04/18] Fixed unit tests --- .../scripts/joystick_interface_auv.py | 14 ++++++++------ .../test/test_joystick_interface_auv.py | 4 ++-- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/mission/joystick_interface_auv/scripts/joystick_interface_auv.py b/mission/joystick_interface_auv/scripts/joystick_interface_auv.py index 2ef126d9..af49ffea 100644 --- a/mission/joystick_interface_auv/scripts/joystick_interface_auv.py +++ b/mission/joystick_interface_auv/scripts/joystick_interface_auv.py @@ -17,8 +17,8 @@ def __init__(self): 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.debounce_duration_ = 0.25 + self.state_ = States.NO_GO self.joystick_buttons_map_ = [ "A", @@ -153,15 +153,17 @@ def joystick_cb(self, msg : Joy): xbox_control_mode_button = buttons["A"] software_killswitch_button = buttons["B"] software_control_mode_button = buttons["X"] + left_trigger = axes["RT"] + right_trigger = axes["LT"] + 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 = -(axes[ - ["RT"] - axes["LT"]])/2 * self.joystick_heave_scaling_ - roll = axes[ - self.buttons["RB"] - self.buttons["LB"]] * self.joystick_roll_scaling_ + heave = -(left_trigger - right_trigger)/2 * 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[ 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 388e7943..d8fe8d4a 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,5 @@ -from joystick_interface_auv.joystick_interface_auv import JoystickInterface -from joystick_interface_auv.joystick_interface_auv import States +from scripts.joystick_interface_auv import JoystickInterface +from scripts.joystick_interface_auv import States import rclpy from sensor_msgs.msg import Joy from sensor_msgs.msg import Joy From 1824f8fa0caf3c25decd37317f83f370b6392be2 Mon Sep 17 00:00:00 2001 From: Aldokan Date: Wed, 7 Feb 2024 18:30:04 +0100 Subject: [PATCH 05/18] Updated auv_setup to ros2 --- auv_setup/CMakeLists.txt | 24 ++++++++++- auv_setup/launch/pc.launch.py | 42 +++++++++++++++++++ auv_setup/package.xml | 15 +++++-- .../scripts/joystick_interface_auv.py | 8 ---- 4 files changed, 76 insertions(+), 13 deletions(-) create mode 100644 auv_setup/launch/pc.launch.py diff --git a/auv_setup/CMakeLists.txt b/auv_setup/CMakeLists.txt index 39183f94..ae1fe2d7 100644 --- a/auv_setup/CMakeLists.txt +++ b/auv_setup/CMakeLists.txt @@ -1,2 +1,24 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(auv_setup) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# Install launch files. +install(DIRECTORY + config + launch + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/auv_setup/launch/pc.launch.py b/auv_setup/launch/pc.launch.py new file mode 100644 index 00000000..bf801d62 --- /dev/null +++ b/auv_setup/launch/pc.launch.py @@ -0,0 +1,42 @@ +import os +from launch import LaunchDescription +from launch.actions import SetEnvironmentVariable, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + # Set environment variable + set_env_var = SetEnvironmentVariable( + name='ROSCONSOLE_FORMAT', + value='[${severity}] [${time}] [${node}]: ${message}' + ) + + # Joystick node + joy_node = Node( + package='joy', + executable='joy_node', + name='joystick_driver', + output='screen', + parameters=[ + {'deadzone': 0.15}, + {'autorepeat_rate': 100.0}, + ], + remappings=[ + ('/joy', '/joystick/joy'), + ], + ) + + # Joystick interface launch + joystick_interface_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory('joystick_interface_auv'), 'launch/joystick_interface_auv.launch.py') + ) + ) + + # Return launch description + return LaunchDescription([ + set_env_var, + joy_node, + joystick_interface_launch + ]) diff --git a/auv_setup/package.xml b/auv_setup/package.xml index e99f74b9..b9099a0c 100644 --- a/auv_setup/package.xml +++ b/auv_setup/package.xml @@ -1,11 +1,18 @@ - + + auv_setup 0.0.0 The auv_setup package - christopher + alekskl01 MIT - catkin + ament_cmake - + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/mission/joystick_interface_auv/scripts/joystick_interface_auv.py b/mission/joystick_interface_auv/scripts/joystick_interface_auv.py index 624fc3b3..b8cc9733 100644 --- a/mission/joystick_interface_auv/scripts/joystick_interface_auv.py +++ b/mission/joystick_interface_auv/scripts/joystick_interface_auv.py @@ -170,20 +170,12 @@ 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_ From c8065701a435c526fe46d9d1e78331bd6f1a5bab Mon Sep 17 00:00:00 2001 From: github-actions Date: Wed, 7 Feb 2024 17:52:55 +0000 Subject: [PATCH 06/18] Automated autoyapf fixes --- auv_setup/launch/pc.launch.py | 24 +++++++++---------- .../scripts/joystick_interface_auv.py | 10 ++++---- .../test/test_joystick_interface_auv.py | 2 +- 3 files changed, 17 insertions(+), 19 deletions(-) diff --git a/auv_setup/launch/pc.launch.py b/auv_setup/launch/pc.launch.py index bf801d62..9367ef83 100644 --- a/auv_setup/launch/pc.launch.py +++ b/auv_setup/launch/pc.launch.py @@ -5,12 +5,12 @@ from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory + def generate_launch_description(): # Set environment variable set_env_var = SetEnvironmentVariable( name='ROSCONSOLE_FORMAT', - value='[${severity}] [${time}] [${node}]: ${message}' - ) + value='[${severity}] [${time}] [${node}]: ${message}') # Joystick node joy_node = Node( @@ -19,8 +19,12 @@ def generate_launch_description(): name='joystick_driver', output='screen', parameters=[ - {'deadzone': 0.15}, - {'autorepeat_rate': 100.0}, + { + 'deadzone': 0.15 + }, + { + 'autorepeat_rate': 100.0 + }, ], remappings=[ ('/joy', '/joystick/joy'), @@ -30,13 +34,9 @@ def generate_launch_description(): # Joystick interface launch joystick_interface_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('joystick_interface_auv'), 'launch/joystick_interface_auv.launch.py') - ) - ) + os.path.join(get_package_share_directory('joystick_interface_auv'), + 'launch/joystick_interface_auv.launch.py'))) # Return launch description - return LaunchDescription([ - set_env_var, - joy_node, - joystick_interface_launch - ]) + return LaunchDescription( + [set_env_var, joy_node, joystick_interface_launch]) diff --git a/mission/joystick_interface_auv/scripts/joystick_interface_auv.py b/mission/joystick_interface_auv/scripts/joystick_interface_auv.py index c46c6e0e..c0a1ee54 100644 --- a/mission/joystick_interface_auv/scripts/joystick_interface_auv.py +++ b/mission/joystick_interface_auv/scripts/joystick_interface_auv.py @@ -94,8 +94,7 @@ 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. @@ -131,8 +130,7 @@ 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 @@ -205,14 +203,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) 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 49203105..ecbf5968 100644 --- a/mission/joystick_interface_auv/test/test_joystick_interface_auv.py +++ b/mission/joystick_interface_auv/test/test_joystick_interface_auv.py @@ -1,4 +1,4 @@ -from scripts.joystick_interface_auv import JoystickInterface +from scripts.joystick_interface_auv import JoystickInterface from scripts.joystick_interface_auv import States import rclpy from sensor_msgs.msg import Joy From f2aab5f74deb09a76aa7bac49d83da9f016bb004 Mon Sep 17 00:00:00 2001 From: Aldokan Date: Wed, 7 Feb 2024 19:51:45 +0100 Subject: [PATCH 07/18] renamed function --- .../scripts/joystick_interface_auv.py | 14 +++++++------- .../test/test_joystick_interface_auv.py | 11 +++-------- 2 files changed, 10 insertions(+), 15 deletions(-) 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 From 4f4a3287a89eff97f1e08577b4958be1a7a4feed Mon Sep 17 00:00:00 2001 From: Aldokan Date: Wed, 7 Feb 2024 21:02:01 +0100 Subject: [PATCH 08/18] Fixed changes --- .gitignore | 1 + .../{pc.launch.py => topside.launch.py} | 0 mission/joystick_interface_auv/CMakeLists.txt | 5 ++- .../config/param_joystick_interface_auv.yaml | 2 +- .../joystick_interface_auv.py | 15 ++++----- .../scripts/__init__.py | 0 .../test/test_joystick_interface_auv.py | 31 ++++++++++++------- 7 files changed, 32 insertions(+), 22 deletions(-) rename auv_setup/launch/{pc.launch.py => topside.launch.py} (100%) rename mission/joystick_interface_auv/{scripts => joystick_interface_auv}/joystick_interface_auv.py (95%) delete mode 100644 mission/joystick_interface_auv/scripts/__init__.py diff --git a/.gitignore b/.gitignore index 13be4f21..18d4200d 100644 --- a/.gitignore +++ b/.gitignore @@ -23,6 +23,7 @@ msg/*Result.msg msg/_*.py build_isolated/ devel_isolated/ +mission/joystick_interface_auv/test/log/ # Generated by dynamic reconfigure *.cfgc diff --git a/auv_setup/launch/pc.launch.py b/auv_setup/launch/topside.launch.py similarity index 100% rename from auv_setup/launch/pc.launch.py rename to auv_setup/launch/topside.launch.py diff --git a/mission/joystick_interface_auv/CMakeLists.txt b/mission/joystick_interface_auv/CMakeLists.txt index d6cef301..284abd55 100644 --- a/mission/joystick_interface_auv/CMakeLists.txt +++ b/mission/joystick_interface_auv/CMakeLists.txt @@ -10,7 +10,7 @@ find_package(rclpy REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) -ament_python_install_package(scripts) +ament_python_install_package(joystick_interface_auv) install(DIRECTORY launch @@ -19,7 +19,7 @@ install(DIRECTORY ) install(PROGRAMS - scripts/joystick_interface_auv.py + joystick_interface_auv/joystick_interface_auv.py DESTINATION lib/${PROJECT_NAME} ) @@ -32,7 +32,6 @@ if(BUILD_TESTING) get_filename_component(_test_name ${_test_path} NAME_WE) ament_add_pytest_test(${_test_name} ${_test_path} APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} - TIMEOUT 60 WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} ) endforeach() 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 44551346..4f77334d 100644 --- a/mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml +++ b/mission/joystick_interface_auv/config/param_joystick_interface_auv.yaml @@ -4,5 +4,5 @@ joystick_interface: sway_scale_factor: 60.0 yaw_scale_factor: 60.0 heave_scale_factor: 35.0 - roll_scale_factor: -30.0 + roll_scale_factor: 30.0 pitch_scale_factor: 20.0 diff --git a/mission/joystick_interface_auv/scripts/joystick_interface_auv.py b/mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv.py similarity index 95% rename from mission/joystick_interface_auv/scripts/joystick_interface_auv.py rename to mission/joystick_interface_auv/joystick_interface_auv/joystick_interface_auv.py index c46c6e0e..2e445008 100644 --- a/mission/joystick_interface_auv/scripts/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", @@ -170,11 +170,12 @@ 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 = (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 = -(left_trigger - right_trigger)/2 * self.joystick_heave_scaling_ + roll = (left_shoulder - right_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_ diff --git a/mission/joystick_interface_auv/scripts/__init__.py b/mission/joystick_interface_auv/scripts/__init__.py deleted file mode 100644 index e69de29b..00000000 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 49203105..52578cde 100644 --- a/mission/joystick_interface_auv/test/test_joystick_interface_auv.py +++ b/mission/joystick_interface_auv/test/test_joystick_interface_auv.py @@ -1,12 +1,12 @@ -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 import rclpy from sensor_msgs.msg import Joy from sensor_msgs.msg import Joy -class TestJoystickInterface: - #test that the wrench msg is created successfully +class TestJoystickInterface(): + #test that the wrench msg is created successfully def test_wrench_msg(self): rclpy.init() msg = JoystickInterface().create_wrench_message( @@ -26,8 +26,11 @@ 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 == -1.0 * JoystickInterface().joystick_surge_scaling_ + assert wrench_msg.force.y == -1.0 * JoystickInterface().joystick_sway_scaling_ + assert wrench_msg.force.z == 0.0 + assert wrench_msg.torque.x == 0.0 + assert wrench_msg.torque.y == 0.0 assert wrench_msg.torque.z == 0.0 rclpy.shutdown() @@ -40,8 +43,11 @@ 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.force.z == 0.0 + assert wrench_msg.torque.x == 0.0 + assert wrench_msg.torque.y == 0.0 assert wrench_msg.torque.z == 0.0 rclpy.shutdown() @@ -54,7 +60,10 @@ 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.torque.z == 0.0 + assert wrench_msg.force.x == -1.0 * JoystickInterface().joystick_surge_scaling_ + assert wrench_msg.force.y == -1.0 * JoystickInterface().joystick_sway_scaling_ + assert wrench_msg.force.z == 0.0 + assert wrench_msg.torque.x == 0.0 + assert wrench_msg.torque.y == 0.0 + assert wrench_msg.torque.z == 0.0 rclpy.shutdown() From e75d24ecbf1a71989c9ba25a746c1e3c8d2b2dc5 Mon Sep 17 00:00:00 2001 From: github-actions Date: Wed, 7 Feb 2024 19:04:28 +0000 Subject: [PATCH 09/18] Automated autoyapf fixes --- .../joystick_interface_auv.py | 13 ++++---- .../test/test_joystick_interface_auv.py | 32 +++++++++++-------- 2 files changed, 24 insertions(+), 21 deletions(-) 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 97d14964..b2d28542 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", @@ -168,11 +168,10 @@ 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_ + 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_ pitch = -axes[ "vertical_axis_right_stick"] * self.joystick_pitch_scaling_ 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 52578cde..52dfd8ba 100644 --- a/mission/joystick_interface_auv/test/test_joystick_interface_auv.py +++ b/mission/joystick_interface_auv/test/test_joystick_interface_auv.py @@ -1,4 +1,4 @@ -from joystick_interface_auv.joystick_interface_auv import JoystickInterface +from joystick_interface_auv.joystick_interface_auv import JoystickInterface from joystick_interface_auv.joystick_interface_auv import States import rclpy from sensor_msgs.msg import Joy @@ -6,7 +6,7 @@ class TestJoystickInterface(): - #test that the wrench msg is created successfully + #test that the wrench msg is created successfully def test_wrench_msg(self): rclpy.init() msg = JoystickInterface().create_wrench_message( @@ -26,9 +26,11 @@ 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 == -1.0 * JoystickInterface().joystick_surge_scaling_ - assert wrench_msg.force.y == -1.0 * JoystickInterface().joystick_sway_scaling_ - assert wrench_msg.force.z == 0.0 + assert wrench_msg.force.x == -1.0 * JoystickInterface( + ).joystick_surge_scaling_ + assert wrench_msg.force.y == -1.0 * JoystickInterface( + ).joystick_sway_scaling_ + assert wrench_msg.force.z == 0.0 assert wrench_msg.torque.x == 0.0 assert wrench_msg.torque.y == 0.0 assert wrench_msg.torque.z == 0.0 @@ -43,9 +45,9 @@ 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.z == 0.0 + assert wrench_msg.force.x == 0.0 + assert wrench_msg.force.y == 0.0 + assert wrench_msg.force.z == 0.0 assert wrench_msg.torque.x == 0.0 assert wrench_msg.torque.y == 0.0 assert wrench_msg.torque.z == 0.0 @@ -60,10 +62,12 @@ 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 == -1.0 * JoystickInterface().joystick_surge_scaling_ - assert wrench_msg.force.y == -1.0 * JoystickInterface().joystick_sway_scaling_ - assert wrench_msg.force.z == 0.0 - assert wrench_msg.torque.x == 0.0 - assert wrench_msg.torque.y == 0.0 - assert wrench_msg.torque.z == 0.0 + assert wrench_msg.force.x == -1.0 * JoystickInterface( + ).joystick_surge_scaling_ + assert wrench_msg.force.y == -1.0 * JoystickInterface( + ).joystick_sway_scaling_ + assert wrench_msg.force.z == 0.0 + assert wrench_msg.torque.x == 0.0 + assert wrench_msg.torque.y == 0.0 + assert wrench_msg.torque.z == 0.0 rclpy.shutdown() From a9d7ade589fe4437272de47c2045337dc482be94 Mon Sep 17 00:00:00 2001 From: github-actions Date: Sun, 11 Feb 2024 11:59:17 +0000 Subject: [PATCH 10/18] Automated autoyapf fixes --- .../joystick_interface_auv.py | 20 ++++++++----------- 1 file changed, 8 insertions(+), 12 deletions(-) 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 b7770cef..dfb4107b 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", @@ -94,8 +94,7 @@ 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. @@ -131,8 +130,7 @@ 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 @@ -170,10 +168,8 @@ 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_ + 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[ @@ -206,14 +202,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) From fc4e5229eb9adf001f0c3814f4bb95f33c090879 Mon Sep 17 00:00:00 2001 From: Alice Date: Sun, 11 Feb 2024 13:52:36 +0100 Subject: [PATCH 11/18] updated documentation for the axes and removed unused functions --- .../joystick_interface_auv.py | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) 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 dfb4107b..76d5f36f 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 @@ -39,13 +39,13 @@ 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_right_stick", - "RT", #Positive thrust/torque multiplier - "dpad_horizontal", + "horizontal_axis_left_stick", #Sway + "vertical_axis_left_stick", #Surge + "LT", #Heave down + "horizontal_axis_right_stick", #Yaw + "vertical_axis_right_stick", #Pitch + "RT", #Heave up + "dpad_horizontal", "dpad_vertical", ] @@ -89,10 +89,6 @@ def __init__(self): # 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_wrench_message(self, surge: float, sway: float, heave: float, roll: float, pitch: float, yaw: float) -> Wrench: """ From e9bf2f58034fa5942f3a384920cb0ae3964326b9 Mon Sep 17 00:00:00 2001 From: github-actions Date: Sun, 11 Feb 2024 12:53:44 +0000 Subject: [PATCH 12/18] Automated autoyapf fixes --- .../joystick_interface_auv/joystick_interface_auv.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 76d5f36f..e698f3ff 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 @@ -43,9 +43,9 @@ def __init__(self): "vertical_axis_left_stick", #Surge "LT", #Heave down "horizontal_axis_right_stick", #Yaw - "vertical_axis_right_stick", #Pitch + "vertical_axis_right_stick", #Pitch "RT", #Heave up - "dpad_horizontal", + "dpad_horizontal", "dpad_vertical", ] From f6ba5d549b334e1ef269ac02a5c9f9ca5793a108 Mon Sep 17 00:00:00 2001 From: github-actions Date: Sun, 11 Feb 2024 12:57:42 +0000 Subject: [PATCH 13/18] Automated autoyapf fixes --- .../joystick_interface_auv.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) 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 be6bb2de..e2a46736 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 @@ -90,7 +90,6 @@ def __init__(self): # Signal that we are in XBOX mode self.operational_mode_signal_publisher_.publish("XBOX mode") - def create_wrench_message(self, surge: float, sway: float, heave: float, roll: float, pitch: float, yaw: float) -> Wrench: """ @@ -187,19 +186,21 @@ def joystick_cb(self, msg: Joy) -> Wrench: if software_killswitch_button: if self.state_ == States.NO_GO: # signal that killswitch is not blocking - self.software_killswitch_signal_publisher_.publish(Bool(data=False)) + self.software_killswitch_signal_publisher_.publish( + Bool(data=False)) self.transition_to_xbox_mode() return - + else: - self.get_logger().info("SW killswitch", throttle_duration_sec=1) + self.get_logger().info("SW killswitch", + throttle_duration_sec=1) # signal that killswitch is blocking self.software_killswitch_signal_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) + 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 From 296cb78e061b9a05abbbfc78cabe1a130efbc53f Mon Sep 17 00:00:00 2001 From: Aldokan Date: Sun, 11 Feb 2024 13:58:59 +0100 Subject: [PATCH 14/18] 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) From 4c53770996fa5d0c702cbf717b46cf2e08b057df Mon Sep 17 00:00:00 2001 From: Aldokan Date: Sun, 11 Feb 2024 14:55:15 +0100 Subject: [PATCH 15/18] Removed lqr_controller publisher and changed operational mode to string, reformatted killswitch logic --- .../joystick_interface_auv.py | 55 +++++++++---------- 1 file changed, 26 insertions(+), 29 deletions(-) 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 b7770cef..79bb8c7a 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 @@ -4,6 +4,7 @@ from geometry_msgs.msg import Wrench from sensor_msgs.msg import Joy from std_msgs.msg import Bool +from std_msgs.msg import String class States: @@ -78,20 +79,17 @@ def __init__(self): #Killswitch publisher self.software_killswitch_signal_publisher_ = self.create_publisher( - Bool, "softWareKillSwitch", 10) + Bool, "softwareKillSwitch", 10) self.software_killswitch_signal_publisher_.publish( - Bool(data=False)) #Killswitch is not active + Bool(data=True)) #Killswitch is active #Operational mode publisher self.operational_mode_signal_publisher_ = self.create_publisher( - Bool, "softWareOperationMode", 10) + String, "softwareOperationMode", 10) - # Signal that we are not in autonomous mode - self.operational_mode_signal_publisher_.publish(Bool(data=True)) + # Signal that we are in XBOX mode + self.operational_mode_signal_publisher_.publish("XBOX mode") - #Controller publisher - self.enable_controller_publisher_ = self.create_publisher( - Bool, "controller/lqr/enable", 10) def create_wrench_message(self, surge: float, sway: float, heave: float, roll: float, pitch: float, @@ -123,8 +121,7 @@ def transition_to_xbox_mode(self): """ 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.operational_mode_signal_publisher_.publish("XBOX mode") self.state_ = States.XBOX_MODE def transition_to_autonomous_mode(self): @@ -134,7 +131,7 @@ def transition_to_autonomous_mode(self): 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.operational_mode_signal_publisher_.publish("Autonomous mode") self.state_ = States.AUTONOMOUS_MODE def joystick_cb(self, msg: Joy) -> Wrench: @@ -191,25 +188,25 @@ def joystick_cb(self, msg: Joy) -> Wrench: self.last_button_press_time_ = current_time # 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)) - 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)) - # 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_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 + if self.state_ == States.NO_GO: + # signal that killswitch is not blocking + self.software_killswitch_signal_publisher_.publish(Bool(data=False)) + self.transition_to_xbox_mode() + return + + else: + self.get_logger().info("SW killswitch", throttle_duration_sec=1) + # signal that killswitch is blocking + self.software_killswitch_signal_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) + 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, From 7abf5fd941d5d8b50a09d60d3407efd4a29e998b Mon Sep 17 00:00:00 2001 From: Aldokan Date: Sun, 11 Feb 2024 15:08:44 +0100 Subject: [PATCH 16/18] Fixed tests and string publisher --- .../joystick_interface_auv.py | 18 +++++++++--------- .../test/test_joystick_interface_auv.py | 4 ++-- 2 files changed, 11 insertions(+), 11 deletions(-) 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 be6bb2de..f2396c22 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 @@ -56,12 +56,12 @@ def __init__(self): "thrust/wrench_input", 1) - 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) + 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', 17.5) + 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( @@ -88,7 +88,7 @@ def __init__(self): String, "softwareOperationMode", 10) # Signal that we are in XBOX mode - self.operational_mode_signal_publisher_.publish("XBOX mode") + self.operational_mode_signal_publisher_.publish(String(data="XBOX mode")) def create_wrench_message(self, surge: float, sway: float, heave: float, @@ -120,7 +120,7 @@ def transition_to_xbox_mode(self): """ Turns off the controller and signals that the operational mode has switched to Xbox mode. """ - self.operational_mode_signal_publisher_.publish("XBOX mode") + self.operational_mode_signal_publisher_.publish(String(data="XBOX mode")) self.state_ = States.XBOX_MODE def transition_to_autonomous_mode(self): @@ -129,7 +129,7 @@ def transition_to_autonomous_mode(self): """ 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("Autonomous mode") + self.operational_mode_signal_publisher_.publish(String(data="Autonomous mode")) self.state_ = States.AUTONOMOUS_MODE def joystick_cb(self, msg: Joy) -> Wrench: 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 52dfd8ba..161e8255 100644 --- a/mission/joystick_interface_auv/test/test_joystick_interface_auv.py +++ b/mission/joystick_interface_auv/test/test_joystick_interface_auv.py @@ -28,7 +28,7 @@ def test_input_from_controller_into_wrench_msg(self): wrench_msg = JoystickInterface().joystick_cb(joy_msg) assert wrench_msg.force.x == -1.0 * JoystickInterface( ).joystick_surge_scaling_ - assert wrench_msg.force.y == -1.0 * JoystickInterface( + assert wrench_msg.force.y == 1.0 * JoystickInterface( ).joystick_sway_scaling_ assert wrench_msg.force.z == 0.0 assert wrench_msg.torque.x == 0.0 @@ -64,7 +64,7 @@ def test_moving_in_of_xbox_mode(self): wrench_msg = joystick.joystick_cb(joy_msg) assert wrench_msg.force.x == -1.0 * JoystickInterface( ).joystick_surge_scaling_ - assert wrench_msg.force.y == -1.0 * JoystickInterface( + assert wrench_msg.force.y == 1.0 * JoystickInterface( ).joystick_sway_scaling_ assert wrench_msg.force.z == 0.0 assert wrench_msg.torque.x == 0.0 From e27486b7c040be6439c2083ca10fd338cd992e40 Mon Sep 17 00:00:00 2001 From: Aldokan Date: Sun, 11 Feb 2024 15:14:00 +0100 Subject: [PATCH 17/18] Fixed false statement in killswitch publisher and added figure in new_auv.yaml, changed XBOX mode to XBOX --- auv_setup/config/robots/new_auv.yaml | 15 +++++++++++++++ .../joystick_interface_auv.py | 10 +++++----- 2 files changed, 20 insertions(+), 5 deletions(-) diff --git a/auv_setup/config/robots/new_auv.yaml b/auv_setup/config/robots/new_auv.yaml index e69de29b..d7c642f5 100644 --- a/auv_setup/config/robots/new_auv.yaml +++ b/auv_setup/config/robots/new_auv.yaml @@ -0,0 +1,15 @@ +# This file defines parameters specific to Beluga +# +# When looking at the AUV from above, the thruster placement is: +# +# front +# |======| +# |=7↗=| |=0↖=| +# | | | | +# | 6• | | 1• | +# | | | | +# | | | | +# | 5• | | 2• | +# | | | | +# |=4↖=|==||==|=3↗=| +# \ No newline at end of file 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 92b79647..a3ef74fa 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 @@ -88,7 +88,7 @@ def __init__(self): String, "softwareOperationMode", 10) # Signal that we are in XBOX mode - self.operational_mode_signal_publisher_.publish(String(data="XBOX mode")) + self.operational_mode_signal_publisher_.publish(String(data="XBOX")) def create_wrench_message(self, surge: float, sway: float, heave: float, roll: float, pitch: float, yaw: float) -> Wrench: @@ -119,7 +119,7 @@ def transition_to_xbox_mode(self): """ Turns off the controller and signals that the operational mode has switched to Xbox mode. """ - self.operational_mode_signal_publisher_.publish(String(data="XBOX mode")) + self.operational_mode_signal_publisher_.publish(String(data="XBOX")) self.state_ = States.XBOX_MODE def transition_to_autonomous_mode(self): @@ -128,7 +128,7 @@ def transition_to_autonomous_mode(self): """ 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(String(data="Autonomous mode")) + self.operational_mode_signal_publisher_.publish(String(data="autonomous mode")) self.state_ = States.AUTONOMOUS_MODE def joystick_cb(self, msg: Joy) -> Wrench: @@ -196,7 +196,7 @@ def joystick_cb(self, msg: Joy) -> Wrench: throttle_duration_sec=1) # signal that killswitch is blocking self.software_killswitch_signal_publisher_.publish( - Bool(data=False)) + Bool(data=True)) # Publish a zero wrench message when sw killing wrench_msg = self.create_wrench_message( @@ -210,7 +210,7 @@ def joystick_cb(self, msg: Joy) -> Wrench: pitch, yaw) if self.state_ == States.XBOX_MODE: - self.get_logger().info("XBOX mode", throttle_duration_sec=1) + self.get_logger().info("XBOX", throttle_duration_sec=1) self.wrench_publisher_.publish(wrench_msg) if software_control_mode_button: From 14f1869335383ddac333ea0a97c58294e21289ef Mon Sep 17 00:00:00 2001 From: github-actions Date: Sun, 11 Feb 2024 14:14:27 +0000 Subject: [PATCH 18/18] Automated autoyapf fixes --- .../joystick_interface_auv/joystick_interface_auv.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 a3ef74fa..a40c39dd 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 @@ -128,7 +128,8 @@ def transition_to_autonomous_mode(self): """ 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(String(data="autonomous mode")) + self.operational_mode_signal_publisher_.publish( + String(data="autonomous mode")) self.state_ = States.AUTONOMOUS_MODE def joystick_cb(self, msg: Joy) -> Wrench: