# UDP Loopback

独立于原 `rl_control_node.py` 的 localhost UDP 键盘测试目录。

包含：

- `udp_keyboard_sender.py`：从终端读取按键，编码 UDP 报文并发送
- `udp_xbox_sender.py`：订阅 `/xbox_data`，把 Xbox 摇杆/按键转成 UDP 报文
- `udp_loopback_node.py`：接收 UDP 报文，解码事件并计算目标值
- `protocol.py`：自定义协议和状态结构
- `config/udp_loopback.yaml`：本地测试配置

运行方式：

```bash
cd /home/meiqi/tienkung/Deploy_Tienkung
python3 udp_loopback/udp_loopback_node.py
```

另开一个终端：

```bash
cd /home/meiqi/tienkung/Deploy_Tienkung
python3 udp_loopback/udp_keyboard_sender.py
```

接回现有 FSM 的方式：

1. 把 [dex_config.yaml](/home/meiqi/tienkung/Deploy_Tienkung/config/dex_config.yaml) 里的 `control_tool` 改成 `udp_loopback`
2. 启动原控制链，例如：

```bash
cd /home/meiqi/tienkung/Deploy_Tienkung
python3 rl_control_node_sim.py
```

3. 再启动发送端：

```bash
cd /home/meiqi/tienkung/Deploy_Tienkung
python3 udp_loopback/udp_keyboard_sender.py
```

如果改成 Xbox 经 UDP 转发，则启动方式是：

1. 把 [dex_config.yaml](/home/meiqi/tienkung/Deploy_Tienkung/config/dex_config.yaml) 里的 `control_tool` 改成 `udp_loopback`
2. 启动控制节点：

```bash
cd /home/meiqi/tienkung/Deploy_Tienkung
source /opt/ros/jazzy/setup.bash
export ROS_DOMAIN_ID=10
python3 rl_control_node_sim.py
```

3. 启动 MuJoCo：

```bash
cd /home/meiqi/tienkung/xSIM_MUJOCO
source /opt/ros/jazzy/setup.bash
export ROS_DOMAIN_ID=10
python3 scripts/simulator_view_asyn.py -m evt2
```

4. 启动手柄节点：

```bash
source /opt/ros/jazzy/setup.bash
export ROS_DOMAIN_ID=10
ros2 run joy joy_node --ros-args -r joy:=/xbox_data
```

5. 启动 UDP Xbox 转发：

```bash
cd /home/meiqi/tienkung/Deploy_Tienkung
source /opt/ros/jazzy/setup.bash
export ROS_DOMAIN_ID=10
python3 udp_loopback/udp_xbox_sender.py
```

默认按键映射：

- `A -> mode_stride -> gotoWALKAMP`
- `X -> pose_home -> gotoZERO`
- `Y -> pose_hold -> gotoSTOP`
- `START -> trim_reset`
- 左摇杆 Y -> 连续前后速度
- 左摇杆 X -> 连续横移速度
- 右摇杆 X -> 连续转向速度
- 十字键左右 -> 高度增减

此时 UDP 接收结果会在接收侧被映射回现有 FSM 命令：

- `pose_home -> gotoZERO`
- `pose_hold -> gotoSTOP`
- `mode_stride -> gotoWALKAMP`
- `surge/sway/spin -> x/y/yaw speed command`

当前事件码不是原工程里的 `gotoZERO` / `gotoSTOP` / `x_speed_command` 这一套，而是：

- `pose_home`
- `pose_hold`
- `mode_stride`
- `surge_up`
- `surge_down`
- `sway_left`
- `sway_right`
- `spin_left`
- `spin_right`
- `set_surge`
- `set_sway`
- `set_spin`
- `set_lift`
- `lift_up`
- `lift_down`
- `trim_reset`
- `session_quit`