diff --git a/.github/workflows/industrial-ci.yml b/.github/workflows/industrial-ci.yml index d2c0e782f..02987c990 100644 --- a/.github/workflows/industrial-ci.yml +++ b/.github/workflows/industrial-ci.yml @@ -11,7 +11,7 @@ on: - cron: '0 1 * * *' # Runs daily to check for dependency issues or flaking tests jobs: call_reusable_workflow: - uses: vortexntnu/vortex-ci/.github/workflows/reusable-industrial-ci.yml@main + uses: vortexntnu/vortex-ci/.github/workflows/reusable-industrial-ci.yml@debs/ament-pytest with: upstream_workspace: './dependencies.repos' before_install_upstream_dependencies: './scripts/ci_install_dependencies.sh' diff --git a/filtering/COLCON_IGNORE b/filtering/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/mission/FSM/COLCON_IGNORE b/mission/FSM/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/mission/keyboard_joy/README.md b/mission/keyboard_joy/README.md new file mode 100644 index 000000000..a74b988f5 --- /dev/null +++ b/mission/keyboard_joy/README.md @@ -0,0 +1,6 @@ +# keyboard_joy + +`keyboard_joy` is a ROS 2 Python node that publishes `sensor_msgs/Joy` messages based on keyboard input, acting as a simple joystick replacement. +Key-to-axis and key-to-button mappings are configurable via YAML and support both hold and sticky axis modes. + +TODO: Currently **only works on Xorg** (not Wayland) due to limitations in global keyboard capture. diff --git a/mission/keyboard_joy/config/key_mappings.yaml b/mission/keyboard_joy/config/key_mappings.yaml new file mode 100644 index 000000000..189caf33d --- /dev/null +++ b/mission/keyboard_joy/config/key_mappings.yaml @@ -0,0 +1,32 @@ +axes: + # Left stick (surge/sway) + w: [1, 1.0, 'hold'] + s: [1, -1.0, 'hold'] + a: [0, 1.0, 'hold'] + d: [0, -1.0, 'hold'] + + # Heave + Key.space: [2, 1.0, 'hold'] # Up (RT) + Key.shift: [2, -1.0, 'hold'] # Down (LT) + + # Rotation (pitch/yaw) + Key.up: [4, 1.0, 'hold'] + Key.down: [4, -1.0, 'hold'] + Key.left: [3, 1.0, 'hold'] + Key.right: [3, -1.0, 'hold'] + +parameters: + axis_increment_rate: 0.02 # update interval (higher = slower response) + axis_increment_step: 0.1 # axis change per update (higher = faster movement) + + +buttons: + # Roll + q: 4 # LB + e: 5 # RB + + # Modes + '1': 0 # A - Xbox mode + '2': 1 # B - Killswitch + '3': 2 # X - Auto + '4': 3 # Y - Reference diff --git a/mission/keyboard_joy/keyboard_joy/__init__.py b/mission/keyboard_joy/keyboard_joy/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/mission/keyboard_joy/keyboard_joy/keyboard_joy_node.py b/mission/keyboard_joy/keyboard_joy/keyboard_joy_node.py new file mode 100755 index 000000000..8ba68265e --- /dev/null +++ b/mission/keyboard_joy/keyboard_joy/keyboard_joy_node.py @@ -0,0 +1,163 @@ +#!/usr/bin/env python3 +import os +import threading + +import rclpy +import yaml +from ament_index_python.packages import get_package_share_directory +from pynput import keyboard +from rclpy.node import Node +from sensor_msgs.msg import Joy + +start_message = r""" + ██ ▄█▀▓█████▓██ ██▓ ▄▄▄▄ ▒█████ ▄▄▄ ██▀███ ▓█████▄ ▄▄▄██▀▀▀▒█████ ▓██ ██▓ + ██▄█▒ ▓█ ▀ ▒██ ██▒▓█████▄ ▒██▒ ██▒▒████▄ ▓██ ▒ ██▒▒██▀ ██▌ ▒██ ▒██▒ ██▒▒██ ██▒ +▓███▄░ ▒███ ▒██ ██░▒██▒ ▄██▒██░ ██▒▒██ ▀█▄ ▓██ ░▄█ ▒░██ █▌ ░██ ▒██░ ██▒ ▒██ ██░ +▓██ █▄ ▒▓█ ▄ ░ ▐██▓░▒██░█▀ ▒██ ██░░██▄▄▄▄██ ▒██▀▀█▄ ░▓█▄ ▌▓██▄██▓ ▒██ ██░ ░ ▐██▓░ +▒██▒ █▄░▒████▒ ░ ██▒▓░░▓█ ▀█▓░ ████▓▒░ ▓█ ▓██▒░██▓ ▒██▒░▒████▓ ▓███▒ ░ ████▓▒░ ░ ██▒▓░ +▒ ▒▒ ▓▒░░ ▒░ ░ ██▒▒▒ ░▒▓███▀▒░ ▒░▒░▒░ ▒▒ ▓▒█░░ ▒▓ ░▒▓░ ▒▒▓ ▒ ▒▓▒▒░ ░ ▒░▒░▒░ ██▒▒▒ +░ ░▒ ▒░ ░ ░ ░▓██ ░▒░ ▒░▒ ░ ░ ▒ ▒░ ▒ ▒▒ ░ ░▒ ░ ▒░ ░ ▒ ▒ ▒ ░▒░ ░ ▒ ▒░ ▓██ ░▒░ +░ ░░ ░ ░ ▒ ▒ ░░ ░ ░ ░ ░ ░ ▒ ░ ▒ ░░ ░ ░ ░ ░ ░ ░ ░ ░ ░ ░ ▒ ▒ ▒ ░░ +░ ░ ░ ░░ ░ ░ ░ ░ ░ ░ ░ ░ ░ ░ ░ ░ ░ ░ + ░ ░ ░ ░ ░ ░ +""" + + +class KeyboardJoy(Node): + def __init__(self): + super().__init__('keyboard_joy') + + self.declare_parameter('config', '') + self.load_key_mappings() + + self.joy_pub = self.create_publisher(Joy, 'joy', 10) + + max_axis_index = max((v[0] for v in self.axis_mappings.values()), default=-1) + max_button_index = max(self.button_mappings.values(), default=-1) + + self.joy_msg = Joy() + self.joy_msg.axes = [0.0] * (max_axis_index + 1) + self.joy_msg.buttons = [0] * (max_button_index + 1) + + self.active_axes = {} + self.sticky_axes = {} + + self.lock = threading.Lock() + + self.listener = keyboard.Listener( + on_press=self.on_press, on_release=self.on_release + ) + self.listener.start() + + self.create_timer(0.05, self.publish_joy) + self.create_timer(self.axis_increment_rate, self.update_active_axes) + + self.get_logger().info(start_message) + + def load_key_mappings(self): + config_file = self.get_parameter( + 'config' + ).get_parameter_value().string_value or os.path.join( + get_package_share_directory('keyboard_joy'), 'config', 'key_mappings.yaml' + ) + + try: + with open(config_file) as f: + keymap = yaml.safe_load(f) or {} + except Exception as e: + self.get_logger().error(f"Failed to load config '{config_file}': {e}") + keymap = {} + + self.axis_mappings = keymap.get('axes', {}) + self.button_mappings = keymap.get('buttons', {}) + params = keymap.get('parameters', {}) + + self.axis_increment_rate = float(params.get('axis_increment_rate', 0.02)) + self.axis_increment_step = float(params.get('axis_increment_step', 0.05)) + + def on_press(self, key): + key_str = self.key_to_string(key) + if not key_str: + return + with self.lock: + if key_str in self.axis_mappings: + axis, val, mode = self.axis_mappings[key_str] + if mode == 'sticky': + self.sticky_axes[axis] = max( + min( + self.sticky_axes.get(axis, 0.0) + + val * self.axis_increment_step, + 1.0, + ), + -1.0, + ) + self.joy_msg.axes[axis] = round(self.sticky_axes[axis], 3) + else: + self.active_axes[axis] = val + elif key_str in self.button_mappings: + self.joy_msg.buttons[self.button_mappings[key_str]] = 1 + + def on_release(self, key): + key_str = self.key_to_string(key) + if not key_str: + return + with self.lock: + if key_str in self.axis_mappings: + axis, _, mode = self.axis_mappings[key_str] + self.active_axes.pop(axis, None) + if mode != 'sticky': + self.joy_msg.axes[axis] = 0.0 + elif key_str in self.button_mappings: + self.joy_msg.buttons[self.button_mappings[key_str]] = 0 + + @staticmethod + def key_to_string(key): + """Convert pynput key event to a normalized string.""" + if hasattr(key, 'char') and key.char: + return key.char.lower() + if hasattr(key, 'name') and key.name: + return f'Key.{key.name}' + return None + + def update_active_axes(self): + """Gradually update active axes towards their target values.""" + with self.lock: + for axis, target in self.active_axes.items(): + current = self.joy_msg.axes[axis] + delta = ( + self.axis_increment_step + if target > 0 + else -self.axis_increment_step + ) + next_val = current + delta + if (delta > 0 and next_val > target) or ( + delta < 0 and next_val < target + ): + next_val = target + self.joy_msg.axes[axis] = round(next_val, 3) + + def publish_joy(self): + with self.lock: + self.joy_msg.header.stamp = self.get_clock().now().to_msg() + self.joy_pub.publish(self.joy_msg) + + def destroy_node(self): + if self.listener: + self.listener.stop() + super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = KeyboardJoy() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/mission/keyboard_joy/launch/keyboard_joy_node.launch.py b/mission/keyboard_joy/launch/keyboard_joy_node.launch.py new file mode 100644 index 000000000..80500f790 --- /dev/null +++ b/mission/keyboard_joy/launch/keyboard_joy_node.launch.py @@ -0,0 +1,35 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + keyboard_config = os.path.join( + get_package_share_directory('keyboard_joy'), 'config', 'key_mappings.yaml' + ) + + orca_params = os.path.join( + get_package_share_directory('auv_setup'), 'config', 'robots', 'orca.yaml' + ) + + return LaunchDescription( + [ + DeclareLaunchArgument( + 'config', + default_value=keyboard_config, + description='Path to key mappings YAML file', + ), + Node( + package='keyboard_joy', + executable='keyboard_joy_node', + name='keyboard_joy', + namespace='orca', + output='screen', + parameters=[{'config': LaunchConfiguration('config')}, orca_params], + ), + ] + ) diff --git a/mission/keyboard_joy/package.xml b/mission/keyboard_joy/package.xml new file mode 100644 index 000000000..3d2a4f17a --- /dev/null +++ b/mission/keyboard_joy/package.xml @@ -0,0 +1,20 @@ + + + + keyboard_joy + 0.0.0 + Keyboard teleop node that publishes sensor_msgs/Joy messages + kluge7 + MIT + + rclpy + sensor_msgs + python3-pynput + + ament_pytest + python3-pytest + + + ament_python + + diff --git a/mission/keyboard_joy/resource/keyboard_joy b/mission/keyboard_joy/resource/keyboard_joy new file mode 100644 index 000000000..e69de29bb diff --git a/mission/keyboard_joy/setup.cfg b/mission/keyboard_joy/setup.cfg new file mode 100644 index 000000000..628e9fc33 --- /dev/null +++ b/mission/keyboard_joy/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/keyboard_joy +[install] +install_scripts=$base/lib/keyboard_joy diff --git a/mission/keyboard_joy/setup.py b/mission/keyboard_joy/setup.py new file mode 100644 index 000000000..8bf70451e --- /dev/null +++ b/mission/keyboard_joy/setup.py @@ -0,0 +1,30 @@ +import os +from glob import glob + +from setuptools import setup + +package_name = 'keyboard_joy' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + ], + install_requires=['setuptools'], + tests_require=['pytest'], + zip_safe=True, + maintainer='kluge7', + maintainer_email='andreas.svendsrud@vortexntnu.no', + description='Keyboard teleop node that publishes sensor_msgs/Joy messages', + license='MIT', + entry_points={ + 'console_scripts': [ + 'keyboard_joy_node = keyboard_joy.keyboard_joy_node:main', + ], + }, +)