Keyboard joy

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.

mission/keyboard_joy/config/key_mappings.yaml

@@ -0,0 +1,32 @@
+axes:
+  # Left stick (surge/sway)
+  w: [1, 1.0, 'normal']
+  s: [1, -1.0, 'normal']
+  a: [0, 1.0, 'normal']
+  d: [0, -1.0, 'normal']
+
+  # Heave
+  Key.space: [2, 1.0, 'normal'] # Up (RT)
+  Key.shift: [2, -1.0, 'normal'] # Down (LT)
+
+  # Rotation (pitch/yaw)
+  Key.up: [4, 1.0, 'normal']
+  Key.down: [4, -1.0, 'normal']
+  Key.left: [3, 1.0, 'normal']
+  Key.right: [3, -1.0, 'normal']
+
+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

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()

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],
+            ),
+        ]
+    )

mission/keyboard_joy/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>keyboard_joy</name>
+  <version>0.0.0</version>
+  <description>Keyboard teleop node that publishes sensor_msgs/Joy messages</description>
+  <maintainer email="[email protected]">kluge7</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>python3-pynput</depend>
+
+  <test_depend>ament_pytest</test_depend>
+  <test_depend>pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

mission/keyboard_joy/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/keyboard_joy
+[install]
+install_scripts=$base/lib/keyboard_joy

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='[email protected]',
+    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',
+        ],
+    },
+)