limingjie/OmniSocketGo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
c
OmniSocketGo/ros-control-py/README.md

6.8 KiB
Raw Permalink Blame History

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 依赖:

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

再构建并安装 OmniSocket Python 扩展:

make python-ext
make python-install

最后构建 ROS 包:

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 的物理方向符合预期:

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

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

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

停止:

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:

./bin/udpserver -listen :9001

OmniSocket KCP:

./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:

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:

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 控制，可以加:

expected_sender:=ros-keyboard-ctrl

Local daemon handoff via Unix datagram:

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:

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:

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:

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:

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:

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 或将摇杆回中: 输出回到零速

数据流

键盘链路:

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

手柄链路:

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 View Git Blame Copy Permalink