feat: 基于Python ROS2的控制程序

2026-04-03 20:00:33 +08:00
parent 6ece408d9f
commit 9ffc36f50d
26 changed files with 2193 additions and 38 deletions
--- a/ros-control-py/ROS2
+++ b/ros-control-py/ROS2
@@ -0,0 +1,147 @@
+## ROS2 Teleop over OmniSocket UDP/KCP
+
+这个文档对应 `ros-control-py/udp_teleop_bridge` 的当前实现。
+
+核心变化:
+
+- `transport:=udp` 现在表示 OmniSocket UDP
+- `transport:=kcp` 表示 OmniSocket KCP
+- 不再使用原来的裸 `socket` UDP 实现
+
+控制接口保持不变:
+
+- topic: `/hric/robot/cmd_vel`
+- type: `geometry_msgs/msg/TwistStamped`
+- frame_id: `pelvis`
+- payload: fixed 24-byte little-endian `<6f>`
+
+负载顺序:
+
+`lx, ly, lz, ax, ay, az`
+
+### 构建顺序
+
+```bash
+make python-ext
+make python-install
+```
+
+```bash
+colcon build --packages-select udp_teleop_bridge
+source install/setup.bash
+```
+
+### 启动 Hub
+
+OmniSocket UDP:
+
+```bash
+./bin/udpserver -listen :9001
+```
+
+OmniSocket KCP:
+
+```bash
+./bin/kcpserver -listen :9002
+```
+
+### 机器人端 Receiver
+
+```bash
+ros2 launch udp_teleop_bridge robot_udp_receiver.launch.py \
+  transport:=udp \
+  server_addr:=127.0.0.1:9001 \
+  peer_id:=ros-bridge-ctrl \
+  output_topic:=/hric/robot/cmd_vel \
+  frame_id:=pelvis \
+  watchdog_timeout:=0.5
+```
+
+KCP 只需把 `transport` 和 `server_addr` 改成:
+
+```bash
+transport:=kcp server_addr:=127.0.0.1:9002
+```
+
+只接受指定 sender:
+
+```bash
+expected_sender:=ros-keyboard-ctrl
+```
+
+### 键盘 Sender
+
+```bash
+ros2 launch udp_teleop_bridge keyboard_sender.launch.py \
+  transport:=udp \
+  server_addr:=127.0.0.1:9001 \
+  peer_id:=ros-keyboard-ctrl \
+  target_peer:=ros-bridge-ctrl
+```
+
+```bash
+ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args \
+  --remap cmd_vel:=/teleop/cmd_vel \
+  -p stamped:=true \
+  -p frame_id:=pelvis \
+  -p speed:=0.20 \
+  -p turn:=0.60
+```
+
+### Xbox Sender
+
+```bash
+ros2 launch udp_teleop_bridge xbox_to_udp.launch.py \
+  transport:=udp \
+  server_addr:=127.0.0.1:9001 \
+  peer_id:=ros-gamepad-ctrl \
+  target_peer:=ros-bridge-ctrl \
+  joy_dev:=/dev/input/js0 \
+  frame_id:=pelvis
+```
+
+### 参数语义
+
+- sender:
+  - `transport`
+  - `server_addr`
+  - `relay_via`
+  - `peer_id`
+  - `target_peer`
+  - `input_topic`
+  - `send_rate_hz`
+  - `input_timeout`
+- receiver:
+  - `transport`
+  - `server_addr`
+  - `relay_via`
+  - `peer_id`
+  - `expected_sender`
+  - `output_topic`
+  - `frame_id`
+  - `watchdog_timeout`
+  - `publish_rate_hz`
+
+`server_addr` 省略时，会按 transport 自动选择:
+
+- `udp` -> `127.0.0.1:9001`
+- `kcp` -> `127.0.0.1:9002`
+
+### 数据流
+
+```text
+teleop_twist_keyboard / teleop_twist_joy
+  -> /teleop/cmd_vel (TwistStamped)
+  -> cmd_vel_udp_sender
+  -> OmniSocket UDP/KCP binary message
+  -> udp_cmd_vel_receiver
+  -> /hric/robot/cmd_vel
+```
+
+### 安全与约束
+
+- sender 默认 20 Hz 重发
+- sender 输入超时后改发零速
+- receiver watchdog 超时后发零速 stop
+- receiver 只接受 24 字节 binary 负载
+- `relay_via` 只在 KCP 模式有效