## 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 -telemetry-peer peer-a-telemetry
```

### 机器人端 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
```

如果控制命令来自本机 `b_side_omnid`，可以改为:

```bash
transport:=unix_dgram local_socket_path:=/tmp/omnisocket-b-side-cmd.sock
```

只接受指定 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 模式有效