# ROS2 Teleop over OmniSocket UDP/KCP

`ros-control-py/udp_teleop_bridge` 现在把 teleop 控制流统一接到 OmniSocket peer 传输上。

- `transport:=udp` 表示 OmniSocket UDP，经 `udpserver/udppeer` 的消息协议传输
- `transport:=kcp` 表示 OmniSocket KCP，经 `kcpserver/kcppeer` 的消息协议传输
- 不再使用原来的裸 `socket.sendto()/recvfrom()` UDP 路径

机器人最终接收的话题保持不变:

- topic: `/hric/robot/cmd_vel`
- type: `geometry_msgs/msg/TwistStamped`
- frame_id: `pelvis`

控制负载也保持不变:

- fixed payload: 24-byte little-endian `<6f>`
- order: `lx, ly, lz, ax, ay, az`

## 目录

- `udp_teleop_bridge/udp_teleop_bridge/cmd_vel_udp_sender.py`: 订阅 `TwistStamped`，经 OmniSocket 发送 24 字节控制包
- `udp_teleop_bridge/udp_teleop_bridge/udp_cmd_vel_receiver.py`: 从 OmniSocket 接收控制包，补时间戳并发布到机器人 ROS2 topic
- `udp_teleop_bridge/udp_teleop_bridge/omni_transport.py`: 统一封装 OmniSocket UDP/KCP session
- `udp_teleop_bridge/config/xbox_twist_joy.yaml`: Xbox 手柄映射
- `udp_teleop_bridge/launch/*.launch.py`: Linux 启动入口

## Linux 构建

先安装 ROS 2 官方 teleop 依赖:

```bash
sudo apt install ros-${ROS_DISTRO}-joy ros-${ROS_DISTRO}-teleop-twist-joy ros-${ROS_DISTRO}-teleop-twist-keyboard
```

再构建并安装 OmniSocket Python 扩展:

```bash
make python-ext
make python-install
```

最后构建 ROS 包:

```bash
colcon build --packages-select udp_teleop_bridge
source install/setup.bash
```

如果 `omnisocket` 没有安装到当前 ROS Python 环境，sender/receiver 会直接报错退出。

## 先验证机器人控制语义

在机器人本机先直接低速发布 `/hric/robot/cmd_vel`，确认 `linear.x`、`linear.y`、`angular.z` 的物理方向符合预期:

```bash
ros2 topic pub /hric/robot/cmd_vel geometry_msgs/msg/TwistStamped \
  "{header: {frame_id: pelvis}, twist: {linear: {x: 0.10, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}}" \
  -r 20
```

```bash
ros2 topic pub /hric/robot/cmd_vel geometry_msgs/msg/TwistStamped \
  "{header: {frame_id: pelvis}, twist: {linear: {x: 0.0, y: 0.10, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}}" \
  -r 20
```

```bash
ros2 topic pub /hric/robot/cmd_vel geometry_msgs/msg/TwistStamped \
  "{header: {frame_id: pelvis}, twist: {linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.30}}}" \
  -r 20
```

停止:

```bash
ros2 topic pub --once /hric/robot/cmd_vel geometry_msgs/msg/TwistStamped \
  "{header: {frame_id: pelvis}, twist: {linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}}"
```

## 启动 OmniSocket Hub

OmniSocket UDP:

```bash
./bin/udpserver -listen :9001
```

OmniSocket KCP:

```bash
./bin/kcpserver -listen :9002 -telemetry-peer peer-a-telemetry
```

`server_addr` 不传时，节点会按 `transport` 自动选择默认值:

- `udp` -> `127.0.0.1:9001`
- `kcp` -> `127.0.0.1:9002`

`relay_via` 只在 `transport:=kcp` 时生效。

## 机器人端运行

UDP:

```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:

```bash
ros2 launch udp_teleop_bridge robot_udp_receiver.launch.py \
  transport:=kcp \
  server_addr:=127.0.0.1:9002 \
  peer_id:=ros-bridge-ctrl \
  output_topic:=/hric/robot/cmd_vel \
  frame_id:=pelvis \
  watchdog_timeout:=0.5
```

如果只允许某个 sender 控制，可以加:

```bash
expected_sender:=ros-keyboard-ctrl
```

Local daemon handoff via Unix datagram:

```bash
ros2 launch udp_teleop_bridge robot_udp_receiver.launch.py \
  transport:=unix_dgram \
  local_socket_path:=/tmp/omnisocket-b-side-cmd.sock \
  output_topic:=/hric/robot/cmd_vel \
  frame_id:=pelvis \
  watchdog_timeout:=0.5
```

## 控制端键盘运行

终端 A，启动 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
```

如果走 KCP:

```bash
ros2 launch udp_teleop_bridge keyboard_sender.launch.py \
  transport:=kcp \
  server_addr:=127.0.0.1:9002 \
  peer_id:=ros-keyboard-ctrl \
  target_peer:=ros-bridge-ctrl
```

终端 B，启动官方键盘 teleop:

```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
```

键盘默认键位（`teleop_twist_keyboard`，建议使用 US 键盘布局）:

- `i`: 前进（`linear.x > 0`）
- `,`: 后退（`linear.x < 0`）
- `j`: 左转（`angular.z > 0`）
- `l`: 右转（`angular.z < 0`）
- `Shift + J`: 左平移（`linear.y > 0`）
- `Shift + L`: 右平移（`linear.y < 0`）
- `u` / `o` / `m` / `.`: 组合前进或后退加转向
- `k` 或其他未映射按键: 停止
- `q` / `z`: 整体速度增加 / 降低 10%
- `w` / `x`: 仅线速度增加 / 降低 10%
- `e` / `c`: 仅角速度增加 / 降低 10%
- `Ctrl-C`: 退出键盘 teleop

## 控制端 Xbox 手柄运行

UDP:

```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
```

KCP:

```bash
ros2 launch udp_teleop_bridge xbox_to_udp.launch.py \
  transport:=kcp \
  server_addr:=127.0.0.1:9002 \
  peer_id:=ros-gamepad-ctrl \
  target_peer:=ros-bridge-ctrl \
  joy_dev:=/dev/input/js0 \
  frame_id:=pelvis
```

当前默认手柄映射:

- 左摇杆上下 -> `linear.x`
- 左摇杆左右 -> `linear.y`
- 右摇杆左右 -> `angular.z`
- `RB` 按住才允许运动
- `LB` 为 turbo

手柄实际操控含义（基于 `config/xbox_twist_joy.yaml` 的 Xbox 默认映射）:

- 左摇杆向前 / 向后: 前进 / 后退
- 左摇杆向左 / 向右: 左平移 / 右平移
- 右摇杆向左 / 向右: 左转 / 右转
- 按住 `RB`: 以常速启用运动输出
- 同时按住 `LB` + `RB`: 启用 turbo，更高的线速度和角速度
- 松开 `RB` 或将摇杆回中: 输出回到零速

## 数据流

键盘链路:

```text
teleop_twist_keyboard -> /teleop/cmd_vel (TwistStamped) -> cmd_vel_udp_sender -> OmniSocket UDP/KCP -> udp_cmd_vel_receiver -> /hric/robot/cmd_vel
```

手柄链路:

```text
joy_node -> teleop_twist_joy -> /teleop/cmd_vel (TwistStamped) -> cmd_vel_udp_sender -> OmniSocket UDP/KCP -> udp_cmd_vel_receiver -> /hric/robot/cmd_vel
```

## 安全行为

- sender 默认按 20 Hz 重发最新命令
- sender 输入超时后会改发零速
- sender 退出时会主动发送数个零速控制包
- receiver 超时后会在 ROS 主线程发布零速 stop
- receiver 只接受 `MSG_TYPE_BINARY` 且长度为 24 字节的负载
- 非预期 sender、非 binary 消息、错误长度消息都会被丢弃并记录日志