From 6f6577d98374263d7fcff5bd4e012b23d73b341c Mon Sep 17 00:00:00 2001 From: FallenSigh Date: Wed, 27 May 2026 02:17:35 +0800 Subject: [PATCH] chore: add ros2 udp_teleop package and tools --- .gitignore | 6 +- ros2/src/udp_teleop/README.md | 165 +++++++ ros2/src/udp_teleop/config/params.yaml | 12 + ros2/src/udp_teleop/package.xml | 18 + ros2/src/udp_teleop/resource/udp_teleop | 0 ros2/src/udp_teleop/setup.cfg | 4 + ros2/src/udp_teleop/setup.py | 35 ++ ros2/src/udp_teleop/test/test_copyright.py | 25 ++ ros2/src/udp_teleop/test/test_flake8.py | 25 ++ ros2/src/udp_teleop/test/test_pep257.py | 23 + ros2/src/udp_teleop/udp_teleop/__init__.py | 0 .../udp_teleop/udp_teleop/keyboard_control.py | 411 ++++++++++++++++++ tools/udp_server.py | 22 + 13 files changed, 743 insertions(+), 3 deletions(-) create mode 100644 ros2/src/udp_teleop/README.md create mode 100644 ros2/src/udp_teleop/config/params.yaml create mode 100644 ros2/src/udp_teleop/package.xml create mode 100644 ros2/src/udp_teleop/resource/udp_teleop create mode 100644 ros2/src/udp_teleop/setup.cfg create mode 100644 ros2/src/udp_teleop/setup.py create mode 100644 ros2/src/udp_teleop/test/test_copyright.py create mode 100644 ros2/src/udp_teleop/test/test_flake8.py create mode 100644 ros2/src/udp_teleop/test/test_pep257.py create mode 100644 ros2/src/udp_teleop/udp_teleop/__init__.py create mode 100644 ros2/src/udp_teleop/udp_teleop/keyboard_control.py create mode 100644 tools/udp_server.py diff --git a/.gitignore b/.gitignore index 252f001..1417205 100644 --- a/.gitignore +++ b/.gitignore @@ -11,9 +11,9 @@ # ==================== # ROS 2 # ==================== -ros2/build/ -ros2/install/ -ros2/log/ +ros2/**/build/ +ros2/**/install/ +ros2/**/log/ # ==================== # Python diff --git a/ros2/src/udp_teleop/README.md b/ros2/src/udp_teleop/README.md new file mode 100644 index 0000000..6e9a77c --- /dev/null +++ b/ros2/src/udp_teleop/README.md @@ -0,0 +1,165 @@ +# udp_teleop — ROS 2 底盘 + 机械臂键盘 UDP 遥控 + +通过键盘实时控制底盘（差速驱动）和机械臂（6 电机），指令通过 **单一 UDP socket** 发送到设备端。 + +## 项目结构 + +``` +ros2/ +├── build/ # colcon 构建产物（自动生成） +├── install/ # colcon 安装产物（自动生成） +├── log/ # 构建日志（自动生成） +└── src/ + └── udp_teleop/ # ROS 2 包 + ├── config/ + │ └── params.yaml # 可配置参数 + ├── launch/ # launch 文件（预留） + ├── udp_teleop/ + │ ├── __init__.py + │ └── keyboard_control.py # 键盘遥控节点 + ├── resource/ + ├── test/ + ├── package.xml + ├── setup.cfg + └── setup.py +``` + +## 环境搭建 + +### 1. 安装 Conda + +使用 Miniconda 或 Anaconda。推荐 Miniconda： + +```bash +# 下载并安装 Miniconda +wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh +bash Miniconda3-latest-Linux-x86_64.sh +``` + +### 2. 创建 ROS 2 Humble 环境 + +使用 robostack 频道安装 ROS 2 Humble Desktop 完整版： + +```bash +conda create -n ros2_humble -c robostack-staging -c conda-forge ros-humble-desktop +``` + +### 3. 安装构建工具 + +```bash +conda activate ros2_humble +conda install -c robostack-staging -c conda-forge \ + colcon-common-extensions \ + ros-humble-ament-cmake \ + python3-pip +``` + +### 4. 安装 Python 依赖 + +```bash +pip install pynput +``` + +### 5. 激活环境 + +每次使用前： + +```bash +conda activate ros2_humble +source /path/to/ros2/install/setup.bash # 首次构建后执行 +``` + +## 构建 + +```bash +cd ros2 +colcon build --symlink-install --packages-select udp_teleop +source install/setup.bash +``` + +> `--symlink-install`：修改 Python 源文件后无需重新构建，直接生效。 + +## 运行 + +### 使用参数文件（推荐） + +```bash +ros2 run udp_teleop keyboard_control \ + --ros-args --params-file src/udp_teleop/config/params.yaml +``` + +### 命令行覆盖参数 + +```bash +ros2 run udp_teleop keyboard_control \ + --ros-args -p udp_ip:=192.168.1.100 -p udp_port:=9999 +``` + +## 按键映射 + +### 底盘控制 + +| 按键 | 功能 | +|------|------| +| `W` / `S` | 前进 / 后退 | +| `A` / `D` | 左移 / 右移 | +| `Q` / `E` | 左转 / 右转 | + +### 机械臂控制 + +| 按键 | 功能 | +|------|------| +| `↑` / `↓` | 升降高度（↑ 升高，↓ 降低） | +| `2` ~ `6` | 选择关节 J2 ~ J6 | +| `←` / `→` | 减小 / 增大当前关节角度 | + +> 底盘和机械臂可以**同时操控**。机械臂指令仅在按下机械臂相关按键后发送。 + +### 其他 + +| 按键 | 功能 | +|------|------| +| `Ctrl+C` | 退出并发送底盘停止指令 | + +## UDP 协议 + +### 底盘指令 + +``` +XYW::::XZHY\n +``` + +### 机械臂指令 + +``` +JXB:<高度>::::::0:0:EZHY\n +``` + +- 6 个电机：电机 1 控制高度，电机 2~6 对应关节 J2~J6 +- 末尾补零至 8 个值 + +## 参数配置 + +| 参数 | 默认值 | 说明 | +|------|--------|------| +| `udp_ip` | `127.0.0.1` | UDP 目标 IP 地址 | +| `udp_port` | `8888` | UDP 目标端口 | +| `chassis_linear_speed` | `100` | 底盘线速度 | +| `chassis_angular_speed` | `45` | 底盘角速度 | +| `arm_height_step` | `5` | 高度每步变化量 | +| `arm_joint_step` | `5` | 关节角度每步变化量 | +| `update_rate` | `0.05` | 控制循环周期（秒） | +| `stdin_hold_time` | `0.04` | 按键持续时间（秒），修复箭头键时序问题 | +| `debug_keys` | `false` | 是否在状态行显示当前按键 | +| `keyboard_backend` | `auto` | 键盘后端：`auto` / `stdin` / `pynput` / `win_poll` | + +## 键盘后端 + +| 后端 | 说明 | +|------|------| +| `auto` | 自动选择：Linux/macOS 用 `stdin`，Windows 用 `win_poll` | +| `stdin` | 基于终端原始输入，无需额外依赖，**需要交互终端** | +| `pynput` | 基于 pynput 库，跨平台，需要 `pip install pynput` | +| `win_poll` | Windows 专用，通过 Win32 API 轮询按键状态 | + +> `ros2 launch` 启动的子进程**没有交互终端**，使用 `stdin` 后端会报错。必须通过 `ros2 run` 在终端直接运行。 diff --git a/ros2/src/udp_teleop/config/params.yaml b/ros2/src/udp_teleop/config/params.yaml new file mode 100644 index 0000000..ddd43ec --- /dev/null +++ b/ros2/src/udp_teleop/config/params.yaml @@ -0,0 +1,12 @@ +keyboard_udp_control: + ros__parameters: + udp_ip: "127.0.0.1" + udp_port: 8888 + chassis_linear_speed: 100 + chassis_angular_speed: 45 + arm_height_step: 5 + arm_joint_step: 5 + update_rate: 0.05 + debug_keys: false + keyboard_backend: "auto" + stdin_hold_time: 0.04 diff --git a/ros2/src/udp_teleop/package.xml b/ros2/src/udp_teleop/package.xml new file mode 100644 index 0000000..501df2c --- /dev/null +++ b/ros2/src/udp_teleop/package.xml @@ -0,0 +1,18 @@ + + + + udp_teleop + 0.0.0 + TODO: Package description + fallensigh + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/ros2/src/udp_teleop/resource/udp_teleop b/ros2/src/udp_teleop/resource/udp_teleop new file mode 100644 index 0000000..e69de29 diff --git a/ros2/src/udp_teleop/setup.cfg b/ros2/src/udp_teleop/setup.cfg new file mode 100644 index 0000000..f19d1bc --- /dev/null +++ b/ros2/src/udp_teleop/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/udp_teleop +[install] +install_scripts=$base/lib/udp_teleop diff --git a/ros2/src/udp_teleop/setup.py b/ros2/src/udp_teleop/setup.py new file mode 100644 index 0000000..f2a4c26 --- /dev/null +++ b/ros2/src/udp_teleop/setup.py @@ -0,0 +1,35 @@ +import os +from glob import glob + +from setuptools import find_packages, setup + +package_name = 'udp_teleop' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'config'), + glob('config/*.yaml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='fallensigh', + maintainer_email='fallensigh@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'keyboard_control = udp_teleop.keyboard_control:main' + ], + }, +) diff --git a/ros2/src/udp_teleop/test/test_copyright.py b/ros2/src/udp_teleop/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/ros2/src/udp_teleop/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/ros2/src/udp_teleop/test/test_flake8.py b/ros2/src/udp_teleop/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/ros2/src/udp_teleop/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/ros2/src/udp_teleop/test/test_pep257.py b/ros2/src/udp_teleop/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/ros2/src/udp_teleop/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/ros2/src/udp_teleop/udp_teleop/__init__.py b/ros2/src/udp_teleop/udp_teleop/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ros2/src/udp_teleop/udp_teleop/keyboard_control.py b/ros2/src/udp_teleop/udp_teleop/keyboard_control.py new file mode 100644 index 0000000..832cce2 --- /dev/null +++ b/ros2/src/udp_teleop/udp_teleop/keyboard_control.py @@ -0,0 +1,411 @@ +import os +import socket +import sys +import threading +import time + +import rclpy +from rclpy.node import Node + +try: + from pynput import keyboard +except ImportError: + keyboard = None + +if sys.platform == "win32": + import ctypes + + user32 = ctypes.WinDLL("user32", use_last_error=True) +else: + user32 = None + +if sys.platform != "win32": + import atexit + import select + import termios + import tty +else: + atexit = None + select = None + termios = None + tty = None + + +KEY_UP = "up" +KEY_DOWN = "down" +KEY_LEFT = "left" +KEY_RIGHT = "right" + +WIN_KEY_CODES = { + "w": 0x57, + "a": 0x41, + "s": 0x53, + "d": 0x44, + "q": 0x51, + "e": 0x45, + "2": 0x32, + "3": 0x33, + "4": 0x34, + "5": 0x35, + "6": 0x36, + "up": 0x26, + "down": 0x28, + "left": 0x25, + "right": 0x27, +} + + +class KeyboardUdpControlNode(Node): + """ROS 2 node that sends chassis and arm UDP commands while keys are held.""" + + def __init__(self): + super().__init__("keyboard_udp_control") + + self.declare_parameter("udp_ip", "192.168.233.67") + self.declare_parameter("udp_port", 8888) + self.declare_parameter("chassis_linear_speed", 100) + self.declare_parameter("chassis_angular_speed", 45) + self.declare_parameter("arm_height_step", 5) + self.declare_parameter("arm_joint_step", 5) + self.declare_parameter("update_rate", 0.05) + self.declare_parameter("debug_keys", False) + self.declare_parameter("keyboard_backend", "auto") + self.declare_parameter("stdin_hold_time", 0.04) + + self.udp_ip = self.get_parameter("udp_ip").value + self.udp_port = int(self.get_parameter("udp_port").value) + self.chassis_linear_speed = int( + self.get_parameter("chassis_linear_speed").value + ) + self.chassis_angular_speed = int( + self.get_parameter("chassis_angular_speed").value + ) + self.arm_height_step = int(self.get_parameter("arm_height_step").value) + self.arm_joint_step = int(self.get_parameter("arm_joint_step").value) + self.update_rate = float(self.get_parameter("update_rate").value) + self.debug_keys = bool(self.get_parameter("debug_keys").value) + self.keyboard_backend = str(self.get_parameter("keyboard_backend").value) + self.stdin_hold_time = float(self.get_parameter("stdin_hold_time").value) + self.use_windows_polling = self.should_use_windows_polling() + self.use_stdin_keyboard = self.should_use_stdin_keyboard() + + self.chassis_x = 0 + self.chassis_y = 0 + self.chassis_w = 0 + + self.arm_height = -10 + self.arm_joints = [0] * 5 + self.arm_selected_joint = 0 + self.arm_active = False + + self.pressed_keys = set() + self.stdin_key_times = {} + self.key_lock = threading.Lock() + self.old_terminal_settings = None + + self._stdin_buf = "" + + self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + + self.listener = None + if self.use_windows_polling: + self.get_logger().info("Using Windows keyboard polling backend.") + elif self.use_stdin_keyboard: + self.enable_stdin_keyboard() + self.get_logger().info("Using stdin keyboard backend.") + else: + if keyboard is None: + raise RuntimeError("pynput is required for keyboard_backend:=pynput.") + self.listener = keyboard.Listener( + on_press=self.on_press, + on_release=self.on_release, + ) + self.listener.start() + self.get_logger().info("Using pynput keyboard listener backend.") + + self.timer = self.create_timer(self.update_rate, self.control_tick) + + self.print_help() + self.get_logger().info( + f"UDP target: {self.udp_ip}:{self.udp_port}" + ) + self.get_logger().info( + "Keyboard UDP control node started. Press Ctrl+C to exit." + ) + + def on_press(self, key): + try: + value = key.char.lower() if key.char else key.char + except AttributeError: + value = key + + with self.key_lock: + self.pressed_keys.add(value) + + def on_release(self, key): + try: + value = key.char.lower() if key.char else key.char + except AttributeError: + value = key + + with self.key_lock: + self.pressed_keys.discard(value) + + def should_use_windows_polling(self): + if self.keyboard_backend == "win_poll": + return True + if self.keyboard_backend in ("pynput", "stdin"): + return False + return sys.platform == "win32" and user32 is not None + + def should_use_stdin_keyboard(self): + if self.keyboard_backend == "stdin": + return True + if self.keyboard_backend in ("pynput", "win_poll"): + return False + return sys.platform != "win32" + + def enable_stdin_keyboard(self): + if not sys.stdin.isatty(): + raise RuntimeError("stdin keyboard backend requires an interactive terminal.") + + self.old_terminal_settings = termios.tcgetattr(sys.stdin) + tty.setcbreak(sys.stdin.fileno()) + atexit.register(self.restore_terminal) + + def get_pressed_keys(self): + if self.use_windows_polling: + return self.get_windows_pressed_keys() + if self.use_stdin_keyboard: + return self.get_stdin_pressed_keys() + + with self.key_lock: + return set(self.pressed_keys) + + def get_windows_pressed_keys(self): + keys = set() + for key_name, key_code in WIN_KEY_CODES.items(): + if user32.GetAsyncKeyState(key_code) & 0x8000: + keys.add(key_name) + + return keys + + _ALLOWED_SINGLE_KEYS = frozenset( + {"w", "a", "s", "d", "q", "e", "2", "3", "4", "5", "6"} + ) + _ARROW_MAP = {"A": KEY_UP, "B": KEY_DOWN, "C": KEY_RIGHT, "D": KEY_LEFT} + + def get_stdin_pressed_keys(self): + now = time.monotonic() + fd = sys.stdin.fileno() + + raw = b"" + while select.select([sys.stdin], [], [], 0)[0]: + chunk = os.read(fd, 4096) + if not chunk: + break + raw += chunk + + buf = self._stdin_buf + raw.decode("utf-8", errors="replace") + + i = 0 + n = len(buf) + while i < n: + ch = buf[i] + + if ch == "\x03": + raise KeyboardInterrupt + + if ch == "\x1b": + # Only handle when we have the full 3-byte escape. + if i + 2 < n and buf[i + 1] in ("[", "O"): + arrow = buf[i + 2] + key = self._ARROW_MAP.get(arrow) + if key is not None: + self.stdin_key_times[key] = now + i += 3 + continue + # Incomplete / unknown escape — leave remainder for next tick. + break + + ch_lower = ch.lower() + if ch_lower in self._ALLOWED_SINGLE_KEYS: + self.stdin_key_times[ch_lower] = now + i += 1 + + self._stdin_buf = buf[i:] + + expired_keys = [ + key + for key, last_time in self.stdin_key_times.items() + if now - last_time > self.stdin_hold_time + ] + for key in expired_keys: + del self.stdin_key_times[key] + + return set(self.stdin_key_times.keys()) + + def is_arm_key_pressed(self, keys): + return ( + KEY_UP in keys + or KEY_DOWN in keys + or KEY_LEFT in keys + or KEY_RIGHT in keys + or any(str(i) in keys for i in range(2, 7)) + ) + + def update_state(self, keys): + self.chassis_x = 0 + self.chassis_y = 0 + self.chassis_w = 0 + + if "w" in keys: + self.chassis_y = -self.chassis_linear_speed + if "s" in keys: + self.chassis_y = self.chassis_linear_speed + if "d" in keys: + self.chassis_x = self.chassis_linear_speed + if "a" in keys: + self.chassis_x = -self.chassis_linear_speed + if "e" in keys: + self.chassis_w = self.chassis_angular_speed + if "q" in keys: + self.chassis_w = -self.chassis_angular_speed + + arm_changed = False + + for i in range(2, 7): + if str(i) in keys: + self.arm_selected_joint = i - 2 + arm_changed = True + + if KEY_UP in keys: + self.arm_height = min(self.arm_height + self.arm_height_step, -10) + arm_changed = True + if KEY_DOWN in keys: + self.arm_height = max(self.arm_height - self.arm_height_step, -280) + arm_changed = True + + joint_index = self.arm_selected_joint + if KEY_RIGHT in keys: + self.arm_joints[joint_index] = min( + self.arm_joints[joint_index] + self.arm_joint_step, + 180, + ) + arm_changed = True + if KEY_LEFT in keys: + self.arm_joints[joint_index] = max( + self.arm_joints[joint_index] - self.arm_joint_step, + -180, + ) + arm_changed = True + + if arm_changed: + self.arm_active = True + + def build_chassis_cmd(self): + return f"XYW:{self.chassis_x}:{self.chassis_y}:{self.chassis_w}:XZHY\n".encode() + + def build_arm_cmd(self): + joints = self.arm_joints + return ( + f"JXB:{self.arm_height}:" + f"{joints[0]}:{joints[1]}:{joints[2]}:" + f"{joints[3]}:{joints[4]}:0:0:EZHY\n" + ).encode() + + def control_tick(self): + keys = self.get_pressed_keys() + self.update_state(keys) + + self.sock.sendto( + self.build_chassis_cmd(), + (self.udp_ip, self.udp_port), + ) + + if self.arm_active or self.is_arm_key_pressed(keys): + self.sock.sendto(self.build_arm_cmd(), (self.udp_ip, self.udp_port)) + + self.print_status(keys) + + def print_status(self, keys): + selected_joint = self.arm_selected_joint + 2 + joints = self.arm_joints + key_text = "" + if self.debug_keys: + key_text = " Keys:" + ",".join(self.format_key(key) for key in keys) + + status = ( + f"\rChassis X:{self.chassis_x:>4} Y:{self.chassis_y:>4} " + f"W:{self.chassis_w:>4} | " + f"Arm H:{self.arm_height:>4}mm J{selected_joint}:" + f"[{joints[0]:>4} {joints[1]:>4} {joints[2]:>4} " + f"{joints[3]:>4} {joints[4]:>4}] " + f"{'[ARM]' if self.arm_active else '[STANDBY]'}" + f"{key_text}" + ) + print(status, end="", flush=True) + + def format_key(self, key): + if isinstance(key, str): + return key + return str(key).replace("Key.", "") + + def send_stop(self): + self.sock.sendto( + b"XYW:0:0:0:XZHY\n", + (self.udp_ip, self.udp_port), + ) + + def destroy_node(self): + self.send_stop() + + if self.listener is not None and self.listener.running: + self.listener.stop() + self.restore_terminal() + + self.sock.close() + + print("\nSent chassis stop command.") + super().destroy_node() + + def restore_terminal(self): + if self.old_terminal_settings is not None: + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.old_terminal_settings) + self.old_terminal_settings = None + + def print_help(self): + print("=" * 62) + print(" ROS 2 chassis + arm keyboard UDP control") + print("=" * 62) + print(" Chassis:") + print(" W/S forward / backward") + print(" A/D left / right") + print(" Q/E turn left / turn right") + print(" Arm:") + print(" Up/Down lift up / down") + print(" 2 ~ 6 select joint J2 ~ J6") + print(" Lt/Rt decrease / increase selected joint angle") + print(" Other:") + print(" Ctrl+C exit and send chassis stop command") + print("=" * 62) + print(" Arm commands are sent only after pressing an arm key.") + print(" Make sure the arm pose matches the initial values.") + print("=" * 62) + + +def main(args=None): + rclpy.init(args=args) + node = KeyboardUdpControlNode() + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/tools/udp_server.py b/tools/udp_server.py new file mode 100644 index 0000000..5b367a2 --- /dev/null +++ b/tools/udp_server.py @@ -0,0 +1,22 @@ +import socket + +# 创建 UDP socket +server = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + +# 绑定地址和端口 +HOST = "0.0.0.0" +PORT = 8888 + +server.bind((HOST, PORT)) + +print(f"UDP服务器正在监听 {HOST}:{PORT}") + +while True: + # 接收数据 + data, addr = server.recvfrom(1024) + + print(f"收到来自 {addr} 的消息: {data.decode()}") + + # 回复客户端 + reply = "服务器已收到" + server.sendto(reply.encode(), addr) \ No newline at end of file