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..50654591d --- /dev/null +++ b/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 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_core.py b/mission/keyboard_joy/keyboard_joy/keyboard_joy_core.py new file mode 100644 index 000000000..182f148f5 --- /dev/null +++ b/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) 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..b3a500d4f --- /dev/null +++ b/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() 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..19171abdb --- /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 + python3-yaml + + python3-pytest + + + ament_python + + diff --git a/mission/keyboard_joy/pytest.ini b/mission/keyboard_joy/pytest.ini new file mode 100644 index 000000000..226641919 --- /dev/null +++ b/mission/keyboard_joy/pytest.ini @@ -0,0 +1,2 @@ +[pytest] +testpaths = test 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..a045633a5 --- /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', 'pyyaml'], + 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', + ], + }, +) diff --git a/mission/keyboard_joy/test/test_keyboard_joy_core.py b/mission/keyboard_joy/test/test_keyboard_joy_core.py new file mode 100644 index 000000000..210cfcc66 --- /dev/null +++ b/mission/keyboard_joy/test/test_keyboard_joy_core.py @@ -0,0 +1,178 @@ +from keyboard_joy.keyboard_joy_core import AxisBinding, AxisMode, KeyboardJoyCore + + +def test_initializes_axes_and_buttons_to_max_index(): + core = KeyboardJoyCore( + axis_mappings={ + "w": AxisBinding(axis=1, val=1.0, mode=AxisMode.HOLD), + "a": AxisBinding(axis=5, val=-1.0, mode=AxisMode.HOLD), + }, + button_mappings={ + "1": 0, + "q": 7, + }, + ) + + state = core.get_state() + assert len(state.axes) == 6 + assert len(state.buttons) == 8 + assert state.axes == [0.0] * 6 + assert state.buttons == [0] * 8 + + +def test_hold_axis_ramps_toward_target_and_stops_at_target(): + core = KeyboardJoyCore( + axis_mappings={"w": AxisBinding(axis=1, val=1.0, mode=AxisMode.HOLD)}, + button_mappings={}, + axis_increment_step=0.3, + ) + + core.press("w") + core.update_active_axes() + assert core.get_state().axes[1] == 0.3 + + core.update_active_axes() + assert core.get_state().axes[1] == 0.6 + + core.update_active_axes() + assert core.get_state().axes[1] == 0.9 + + core.update_active_axes() + assert core.get_state().axes[1] == 1.0 + + core.update_active_axes() + assert core.get_state().axes[1] == 1.0 + + +def test_hold_axis_negative_ramps_down_and_stops_at_target(): + core = KeyboardJoyCore( + axis_mappings={"s": AxisBinding(axis=0, val=-1.0, mode=AxisMode.HOLD)}, + button_mappings={}, + axis_increment_step=0.4, + ) + + core.press("s") + core.update_active_axes() + assert core.get_state().axes[0] == -0.4 + + core.update_active_axes() + assert core.get_state().axes[0] == -0.8 + + core.update_active_axes() + assert core.get_state().axes[0] == -1.0 + + +def test_hold_axis_release_resets_axis_to_zero(): + core = KeyboardJoyCore( + axis_mappings={"w": AxisBinding(axis=1, val=1.0, mode=AxisMode.HOLD)}, + button_mappings={}, + axis_increment_step=0.5, + ) + + core.press("w") + core.update_active_axes() + assert core.get_state().axes[1] == 0.5 + + core.release("w") + assert core.get_state().axes[1] == 0.0 + + core.update_active_axes() + assert core.get_state().axes[1] == 0.0 + + +def test_sticky_axis_accumulates_in_steps_and_clamps(): + core = KeyboardJoyCore( + axis_mappings={ + "i": AxisBinding(axis=2, val=1.0, mode=AxisMode.STICKY), + "k": AxisBinding(axis=2, val=-1.0, mode=AxisMode.STICKY), + }, + button_mappings={}, + axis_increment_step=0.25, + ) + + core.press("i") + assert core.get_state().axes[2] == 0.25 + core.press("i") + assert core.get_state().axes[2] == 0.5 + + core.press("k") + assert core.get_state().axes[2] == 0.25 + + for _ in range(10): + core.press("i") + assert core.get_state().axes[2] == 1.0 + + for _ in range(20): + core.press("k") + assert core.get_state().axes[2] == -1.0 + + +def test_sticky_axis_release_does_not_reset(): + core = KeyboardJoyCore( + axis_mappings={"i": AxisBinding(axis=0, val=1.0, mode=AxisMode.STICKY)}, + button_mappings={}, + axis_increment_step=0.5, + ) + + core.press("i") + assert core.get_state().axes[0] == 0.5 + + core.release("i") + assert core.get_state().axes[0] == 0.5 + + +def test_button_press_and_release_sets_button_state(): + core = KeyboardJoyCore( + axis_mappings={}, + button_mappings={" ": 0, "q": 4}, + ) + + core.press(" ") + assert core.get_state().buttons[0] == 1 + core.release(" ") + assert core.get_state().buttons[0] == 0 + + core.press("q") + assert core.get_state().buttons[4] == 1 + core.release("q") + assert core.get_state().buttons[4] == 0 + + +def test_button_press_extends_buttons_array_if_needed(): + core = KeyboardJoyCore(axis_mappings={}, button_mappings={"x": 12}) + assert len(core.get_state().buttons) == 13 + + core2 = KeyboardJoyCore(axis_mappings={}, button_mappings={}) + assert len(core2.get_state().buttons) == 0 + core2.button_mappings["x"] = 12 + core2.press("x") + assert len(core2.get_state().buttons) == 13 + assert core2.get_state().buttons[12] == 1 + + +def test_unknown_keys_do_not_change_state(): + core = KeyboardJoyCore( + axis_mappings={"w": AxisBinding(axis=0, val=1.0, mode=AxisMode.HOLD)}, + button_mappings={" ": 0}, + ) + before = core.get_state() + + core.press("does-not-exist") + core.release("does-not-exist") + core.update_active_axes() + + after = core.get_state() + assert after.axes == before.axes + assert after.buttons == before.buttons + + +def test_set_axis_clamps_and_rounds(): + core = KeyboardJoyCore(axis_mappings={}, button_mappings={}) + core.set_axis(3, 1.23456) + assert core.get_state().axes[3] == 1.0 + + core.set_axis(3, -1.23456) + assert core.get_state().axes[3] == -1.0 + + core.set_axis(3, 0.33333) + assert core.get_state().axes[3] == 0.333