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,48 @@
+axes:
+  # Each mapping is [axis_index, value, mode]
+  # axis_index is which Joy.axes[] index to control
+  # value is the target value while the key is held (-1.0 to 1.0)
+  # mode is 'hold' or 'sticky'
+
+  # 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']
+
+  # Vertical (axis 7)
+  Key.home: [7, 1.0, 'hold']        # Numpad 7 → D-pad up
+  Key.end:  [7, -1.0, 'hold']       # Numpad 1 → D-pad down
+
+  # Horizontal (axis 6)
+  Key.page_up:   [6, 1.0, 'hold']   # Numpad 9 → D-pad right
+  Key.page_down: [6, -1.0, 'hold']  # Numpad 3 → D-pad left
+
+parameters:
+  axis_update_period: 0.02 # seconds per update
+  publish_period: 0.02 # seconds per publish
+
+  # Motion tuning
+  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_core.py

@@ -0,0 +1,169 @@
+from __future__ import annotations
+
+import threading
+from dataclasses import dataclass
+from enum import Enum
+
+import yaml
+
+
+class AxisMode(Enum):
+    HOLD = "hold"
+    STICKY = "sticky"
+
+
+@dataclass(frozen=True)
+class AxisBinding:
+    axis: int
+    val: float
+    mode: AxisMode
+
+
+@dataclass(frozen=True)
+class JoyState:
+    axes: list[float]
+    buttons: list[int]
+    frame_id: str = "keyboard"
+
+
+class KeyboardJoyCore:
+    def __init__(
+        self,
+        axis_mappings: dict[str, AxisBinding],
+        button_mappings: dict[str, int],
+        *,
+        axis_update_period: float = 0.02,
+        publish_period: float = 0.05,
+        axis_increment_step: float = 0.05,
+        frame_id: str = "keyboard",
+    ):
+        self.axis_mappings = axis_mappings
+        self.button_mappings = button_mappings
+
+        self.axis_update_period = float(axis_update_period)
+        self.publish_period = float(publish_period)
+        self.axis_increment_step = float(axis_increment_step)
+
+        self._frame_id = frame_id
+
+        self._active_axes: dict[int, float] = {}
+        self._sticky_axes: dict[int, float] = {}
+
+        max_axis_index = max((b.axis for b in self.axis_mappings.values()), default=-1)
+        max_button_index = max(self.button_mappings.values(), default=-1)
+
+        self._axes = [0.0] * (max_axis_index + 1)
+        self._buttons = [0] * (max_button_index + 1)
+
+        self._lock = threading.Lock()
+
+    @classmethod
+    def from_yaml_file(cls, config_file: str) -> KeyboardJoyCore:
+        with open(config_file, encoding="utf-8") as f:
+            keymap = yaml.safe_load(f) or {}
+
+        raw_axes = keymap.get("axes", {}) or {}
+        axis_mappings: dict[str, AxisBinding] = {}
+        for key, spec in raw_axes.items():
+            # YAML format: [axis_index, value, "sticky"/"hold"]
+            axis, val, mode = spec
+            axis_mappings[key] = AxisBinding(
+                axis=int(axis),
+                val=float(val),
+                mode=AxisMode(str(mode)),
+            )
+
+        button_mappings = keymap.get("buttons", {}) or {}
+
+        params = keymap.get("parameters", {}) or {}
+        axis_update_period = float(params.get("axis_update_period", 0.02))
+        publish_period = float(params.get("publish_period", 0.05))
+        axis_increment_step = float(params.get("axis_increment_step", 0.05))
+
+        return cls(
+            axis_mappings=axis_mappings,
+            button_mappings=button_mappings,
+            axis_update_period=axis_update_period,
+            publish_period=publish_period,
+            axis_increment_step=axis_increment_step,
+        )
+
+    def press(self, key_str: str) -> None:
+        if not key_str:
+            return
+
+        with self._lock:
+            if key_str in self.axis_mappings:
+                binding = self.axis_mappings[key_str]
+
+                if binding.mode == AxisMode.STICKY:
+                    axis = binding.axis
+                    new_val = (
+                        self._sticky_axes.get(axis, 0.0)
+                        + binding.val * self.axis_increment_step
+                    )
+                    new_val = max(min(new_val, 1.0), -1.0)
+                    self._sticky_axes[axis] = new_val
+                    self._axes[axis] = round(new_val, 3)
+                else:
+                    # HOLD mode: ramp toward binding.val while key is held
+                    self._active_axes[binding.axis] = binding.val
+
+            elif key_str in self.button_mappings:
+                idx = int(self.button_mappings[key_str])
+                if idx >= len(self._buttons):
+                    self._buttons.extend([0] * (idx + 1 - len(self._buttons)))
+                self._buttons[idx] = 1
+
+    def release(self, key_str: str) -> None:
+        if not key_str:
+            return
+
+        with self._lock:
+            if key_str in self.axis_mappings:
+                binding = self.axis_mappings[key_str]
+                self._active_axes.pop(binding.axis, None)
+                if binding.mode != AxisMode.STICKY:
+                    self._axes[binding.axis] = 0.0
+
+            elif key_str in self.button_mappings:
+                idx = int(self.button_mappings[key_str])
+                if idx < len(self._buttons):
+                    self._buttons[idx] = 0
+
+    def update_active_axes(self) -> None:
+        """For HOLD axes: move axis value toward target at axis_increment_step per call."""
+        with self._lock:
+            for axis, target in list(self._active_axes.items()):
+                if axis >= len(self._axes):
+                    self._axes.extend([0.0] * (axis + 1 - len(self._axes)))
+
+                current = self._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._axes[axis] = round(next_val, 3)
+
+    def get_state(self) -> JoyState:
+        with self._lock:
+            return JoyState(
+                axes=list(self._axes),
+                buttons=list(self._buttons),
+                frame_id=self._frame_id,
+            )
+
+    # Helper for tests/debug
+    def set_axis(self, axis: int, value: float) -> None:
+        with self._lock:
+            if axis >= len(self._axes):
+                self._axes.extend([0.0] * (axis + 1 - len(self._axes)))
+            self._axes[axis] = round(max(min(float(value), 1.0), -1.0), 3)

mission/keyboard_joy/keyboard_joy/keyboard_joy_node.py

@@ -0,0 +1,104 @@
+#!/usr/bin/env python3
+import rclpy
+from pynput import keyboard
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from sensor_msgs.msg import Joy
+
+from keyboard_joy.keyboard_joy_core import KeyboardJoyCore
+
+start_message = r"""
+ ██ ▄█▀▓█████▓██   ██▓ ▄▄▄▄    ▒█████   ▄▄▄       ██▀███  ▓█████▄  ▄▄▄██▀▀▀▒█████ ▓██   ██▓
+ ██▄█▒ ▓█   ▀ ▒██  ██▒▓█████▄ ▒██▒  ██▒▒████▄    ▓██ ▒ ██▒▒██▀ ██▌   ▒██  ▒██▒  ██▒▒██  ██▒
+▓███▄░ ▒███    ▒██ ██░▒██▒ ▄██▒██░  ██▒▒██  ▀█▄  ▓██ ░▄█ ▒░██   █▌   ░██  ▒██░  ██▒ ▒██ ██░
+▓██ █▄ ▒▓█  ▄  ░ ▐██▓░▒██░█▀  ▒██   ██░░██▄▄▄▄██ ▒██▀▀█▄  ░▓█▄   ▌▓██▄██▓ ▒██   ██░ ░ ▐██▓░
+▒██▒ █▄░▒████▒ ░ ██▒▓░░▓█  ▀█▓░ ████▓▒░ ▓█   ▓██▒░██▓ ▒██▒░▒████▓  ▓███▒  ░ ████▓▒░ ░ ██▒▓░
+▒ ▒▒ ▓▒░░ ▒░ ░  ██▒▒▒ ░▒▓███▀▒░ ▒░▒░▒░  ▒▒   ▓▒█░░ ▒▓ ░▒▓░ ▒▒▓  ▒  ▒▓▒▒░  ░ ▒░▒░▒░   ██▒▒▒
+░ ░▒ ▒░ ░ ░  ░▓██ ░▒░ ▒░▒   ░   ░ ▒ ▒░   ▒   ▒▒ ░  ░▒ ░ ▒░ ░ ▒  ▒  ▒ ░▒░    ░ ▒ ▒░ ▓██ ░▒░
+░ ░░ ░    ░   ▒ ▒ ░░   ░    ░ ░ ░ ░ ▒    ░   ▒     ░░   ░  ░ ░  ░  ░ ░ ░  ░ ░ ░ ▒  ▒ ▒ ░░
+░  ░      ░  ░░ ░      ░          ░ ░        ░  ░   ░        ░     ░   ░      ░ ░  ░ ░
+              ░ ░           ░                              ░                       ░ ░
+"""
+
+
+class KeyboardJoy(Node):
+    def __init__(self):
+        super().__init__("keyboard_joy")
+
+        self.declare_parameter("config", Parameter.Type.STRING)
+        config_file = self.get_parameter("config").value
+        if not config_file:
+            raise RuntimeError("Parameter 'config' is required (pass it from launch).")
+
+        self.core = KeyboardJoyCore.from_yaml_file(config_file)
+
+        self.declare_parameter("topics.joy", Parameter.Type.STRING)
+        joy_topic = self.get_parameter("topics.joy").value
+        self.joy_pub = self.create_publisher(Joy, joy_topic, 10)
+
+        self.joy_msg = Joy()
+        self.joy_msg.header.frame_id = "keyboard"
+
+        self.listener = keyboard.Listener(
+            on_press=self.on_press,
+            on_release=self.on_release,
+        )
+        self.listener.start()
+
+        self.create_timer(self.core.publish_period, self.publish_joy)
+        self.create_timer(self.core.axis_update_period, self.update_active_axes)
+
+        self.get_logger().info(start_message)
+
+    def on_press(self, key):
+        key_str = self.key_to_string(key)
+        if not key_str:
+            return
+        self.core.press(key_str)
+
+    def on_release(self, key):
+        key_str = self.key_to_string(key)
+        if not key_str:
+            return
+        self.core.release(key_str)
+
+    @staticmethod
+    def key_to_string(key):
+        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):
+        self.core.update_active_axes()
+
+    def publish_joy(self):
+        state = self.core.get_state()
+
+        self.joy_msg.header.stamp = self.get_clock().now().to_msg()
+        self.joy_msg.header.frame_id = state.frame_id
+        self.joy_msg.axes = state.axes
+        self.joy_msg.buttons = state.buttons
+        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,20 @@
+<?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="andreas.svendsrud@vortexntnu.no">kluge7</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>python3-pynput</depend>
+  <depend>python3-yaml</depend>
+
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

mission/keyboard_joy/pytest.ini

@@ -0,0 +1,2 @@
+[pytest]
+testpaths = test

mission/keyboard_joy/setup.cfg

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