feat: 基于Python ROS2的控制程序
This commit is contained in:
246
ros-control-py/README.md
Normal file
246
ros-control-py/README.md
Normal file
@@ -0,0 +1,246 @@
|
||||
# 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
|
||||
```
|
||||
|
||||
`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
|
||||
```
|
||||
|
||||
## 控制端键盘运行
|
||||
|
||||
终端 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 消息、错误长度消息都会被丢弃并记录日志
|
||||
Reference in New Issue
Block a user