limingjie/OmniSocketGo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: 基于Python ROS2的控制程序

Browse Source
This commit is contained in:
Mock
2026-04-03 20:00:33 +08:00
parent 6ece408d9f
commit 9ffc36f50d
26 changed files with 2193 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

246
ros-control-py/README.md Normal file
View 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 消息、错误长度消息都会被丢弃并记录日志

147
ros-control-py/ROS2 Teleop over UDP.md Normal file
View File

@@ -0,0 +1,147 @@
## 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
```
### 机器人端 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
```
只接受指定 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 模式有效

32
ros-control-py/udp_teleop_bridge/config/xbox_twist_joy.yaml Normal file
View File

@@ -0,0 +1,32 @@
/**:
ros__parameters:
require_enable_button: true
enable_button: 5
enable_turbo_button: 4
axis_linear:
x: 1
y: 0
z: -1
scale_linear:
x: -0.30
y: -0.25
z: 0.0
scale_linear_turbo:
x: -0.60
y: -0.45
z: 0.0
axis_angular:
yaw: 3
pitch: -1
roll: -1
scale_angular:
yaw: -0.80
pitch: 0.0
roll: 0.0
scale_angular_turbo:
yaw: -1.20
pitch: 0.0
roll: 0.0
inverted_reverse: false
publish_stamped_twist: true
frame: pelvis

34
ros-control-py/udp_teleop_bridge/launch/keyboard_sender.launch.py Normal file
View File

@@ -0,0 +1,34 @@
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
def generate_launch_description() -> LaunchDescription:
return LaunchDescription([
DeclareLaunchArgument('transport', default_value='udp'),
DeclareLaunchArgument('server_addr', default_value=''),
DeclareLaunchArgument('relay_via', default_value=''),
DeclareLaunchArgument('peer_id', default_value='ros-keyboard-ctrl'),
DeclareLaunchArgument('target_peer', default_value='ros-bridge-ctrl'),
DeclareLaunchArgument('input_topic', default_value='/teleop/cmd_vel'),
DeclareLaunchArgument('send_rate_hz', default_value='20.0'),
DeclareLaunchArgument('input_timeout', default_value='0.75'),
Node(
package='udp_teleop_bridge',
executable='cmd_vel_udp_sender',
name='cmd_vel_udp_sender',
output='screen',
parameters=[{
'transport': LaunchConfiguration('transport'),
'server_addr': LaunchConfiguration('server_addr'),
'relay_via': LaunchConfiguration('relay_via'),
'peer_id': LaunchConfiguration('peer_id'),
'target_peer': LaunchConfiguration('target_peer'),
'input_topic': LaunchConfiguration('input_topic'),
'send_rate_hz': ParameterValue(LaunchConfiguration('send_rate_hz'), value_type=float),
'input_timeout': ParameterValue(LaunchConfiguration('input_timeout'), value_type=float),
}],
),
])

36
ros-control-py/udp_teleop_bridge/launch/robot_udp_receiver.launch.py Normal file
View File

@@ -0,0 +1,36 @@
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
def generate_launch_description() -> LaunchDescription:
return LaunchDescription([
DeclareLaunchArgument('transport', default_value='udp'),
DeclareLaunchArgument('server_addr', default_value=''),
DeclareLaunchArgument('relay_via', default_value=''),
DeclareLaunchArgument('peer_id', default_value='ros-bridge-ctrl'),
DeclareLaunchArgument('expected_sender', default_value=''),
DeclareLaunchArgument('output_topic', default_value='/hric/robot/cmd_vel'),
DeclareLaunchArgument('frame_id', default_value='pelvis'),
DeclareLaunchArgument('watchdog_timeout', default_value='0.5'),
DeclareLaunchArgument('publish_rate_hz', default_value='100.0'),
Node(
package='udp_teleop_bridge',
executable='udp_cmd_vel_receiver',
name='udp_cmd_vel_receiver',
output='screen',
parameters=[{
'transport': LaunchConfiguration('transport'),
'server_addr': LaunchConfiguration('server_addr'),
'relay_via': LaunchConfiguration('relay_via'),
'peer_id': LaunchConfiguration('peer_id'),
'expected_sender': LaunchConfiguration('expected_sender'),
'output_topic': LaunchConfiguration('output_topic'),
'frame_id': LaunchConfiguration('frame_id'),
'watchdog_timeout': ParameterValue(LaunchConfiguration('watchdog_timeout'), value_type=float),
'publish_rate_hz': ParameterValue(LaunchConfiguration('publish_rate_hz'), value_type=float),
}],
),
])

74
ros-control-py/udp_teleop_bridge/launch/xbox_to_udp.launch.py Normal file
View File

@@ -0,0 +1,74 @@
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from launch_ros.substitutions import FindPackageShare
def generate_launch_description() -> LaunchDescription:
teleop_config = PathJoinSubstitution([
FindPackageShare('udp_teleop_bridge'),
'config',
'xbox_twist_joy.yaml',
])
teleop_topic = LaunchConfiguration('teleop_topic')
return LaunchDescription([
DeclareLaunchArgument('transport', default_value='udp'),
DeclareLaunchArgument('server_addr', default_value=''),
DeclareLaunchArgument('relay_via', default_value=''),
DeclareLaunchArgument('peer_id', default_value='ros-gamepad-ctrl'),
DeclareLaunchArgument('target_peer', default_value='ros-bridge-ctrl'),
DeclareLaunchArgument('joy_dev', default_value='/dev/input/js0'),
DeclareLaunchArgument('deadzone', default_value='0.10'),
DeclareLaunchArgument('autorepeat_rate', default_value='20.0'),
DeclareLaunchArgument('frame_id', default_value='pelvis'),
DeclareLaunchArgument('teleop_topic', default_value='/teleop/cmd_vel'),
DeclareLaunchArgument('send_rate_hz', default_value='20.0'),
DeclareLaunchArgument('input_timeout', default_value='0.30'),
Node(
package='joy',
executable='joy_node',
name='joy_node',
output='screen',
parameters=[{
'dev': LaunchConfiguration('joy_dev'),
'deadzone': ParameterValue(LaunchConfiguration('deadzone'), value_type=float),
'autorepeat_rate': ParameterValue(LaunchConfiguration('autorepeat_rate'), value_type=float),
}],
),
Node(
package='teleop_twist_joy',
executable='teleop_node',
name='teleop_twist_joy',
output='screen',
parameters=[
teleop_config,
{
'publish_stamped_twist': True,
'frame': LaunchConfiguration('frame_id'),
},
],
remappings=[
('cmd_vel', teleop_topic),
],
),
Node(
package='udp_teleop_bridge',
executable='cmd_vel_udp_sender',
name='cmd_vel_udp_sender',
output='screen',
parameters=[{
'transport': LaunchConfiguration('transport'),
'server_addr': LaunchConfiguration('server_addr'),
'relay_via': LaunchConfiguration('relay_via'),
'peer_id': LaunchConfiguration('peer_id'),
'target_peer': LaunchConfiguration('target_peer'),
'input_topic': teleop_topic,
'send_rate_hz': ParameterValue(LaunchConfiguration('send_rate_hz'), value_type=float),
'input_timeout': ParameterValue(LaunchConfiguration('input_timeout'), value_type=float),
}],
),
])

24
ros-control-py/udp_teleop_bridge/package.xml Normal file
View File

@@ -0,0 +1,24 @@
<?xml version="1.0"?>
<package format="3">
<name>udp_teleop_bridge</name>
<version>0.1.0</version>
<description>ROS 2 OmniSocket UDP/KCP bridge for teleop TwistStamped commands.</description>
<maintainer email="codex@example.com">Codex</maintainer>
<license>MIT</license>
<buildtool_depend>ament_python</buildtool_depend>
<exec_depend>ament_index_python</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>joy</exec_depend>
<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>teleop_twist_joy</exec_depend>
<exec_depend>teleop_twist_keyboard</exec_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>

1
ros-control-py/udp_teleop_bridge/resource/udp_teleop_bridge Normal file
View File

@@ -0,0 +1 @@
udp_teleop_bridge

5
ros-control-py/udp_teleop_bridge/setup.cfg Normal file
View File

@@ -0,0 +1,5 @@
[develop]
script_dir=$base/lib/udp_teleop_bridge
[install]
install_scripts=$base/lib/udp_teleop_bridge

34
ros-control-py/udp_teleop_bridge/setup.py Normal file
View File

@@ -0,0 +1,34 @@
from setuptools import find_packages, setup
package_name = 'udp_teleop_bridge'
setup(
name=package_name,
version='0.1.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages', [f'resource/{package_name}']),
(f'share/{package_name}', ['package.xml']),
(f'share/{package_name}/launch', [
'launch/keyboard_sender.launch.py',
'launch/robot_udp_receiver.launch.py',
'launch/xbox_to_udp.launch.py',
]),
(f'share/{package_name}/config', ['config/xbox_twist_joy.yaml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='Codex',
maintainer_email='codex@example.com',
description='ROS 2 OmniSocket UDP/KCP bridge for teleop TwistStamped commands.',
license='MIT',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'cmd_vel_udp_sender = udp_teleop_bridge.cmd_vel_udp_sender:main',
'udp_cmd_vel_receiver = udp_teleop_bridge.udp_cmd_vel_receiver:main',
],
},
)

54
ros-control-py/udp_teleop_bridge/test/test_protocol.py Normal file
View File

@@ -0,0 +1,54 @@
from pathlib import Path
import sys
import pytest
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT))
from udp_teleop_bridge.protocol import ( # noqa: E402
PACKET_SIZE,
default_server_addr_for_transport,
normalize_command,
normalize_transport,
pack_command,
unpack_command,
)
def test_pack_unpack_round_trip() -> None:
command = (0.1, -0.2, 0.3, -0.4, 0.5, -0.6)
payload = pack_command(command)
assert len(payload) == PACKET_SIZE
assert unpack_command(payload) == pytest.approx(command)
@pytest.mark.parametrize('value', [float('nan'), float('inf'), float('-inf')])
def test_normalize_command_rejects_non_finite_values(value: float) -> None:
with pytest.raises(ValueError, match='non-finite'):
normalize_command((0.0, 0.0, value, 0.0, 0.0, 0.0))
def test_unpack_command_rejects_wrong_length() -> None:
with pytest.raises(ValueError, match='Expected'):
unpack_command(b'\x00' * (PACKET_SIZE - 1))
@pytest.mark.parametrize(
('transport', 'expected'),
[
('udp', '127.0.0.1:9001'),
('kcp', '127.0.0.1:9002'),
],
)
def test_default_server_addr_for_transport(transport: str, expected: str) -> None:
assert default_server_addr_for_transport(transport) == expected
def test_normalize_transport_rejects_unknown_value() -> None:
with pytest.raises(ValueError, match='Unsupported transport'):
normalize_transport('sctp')

1
ros-control-py/udp_teleop_bridge/udp_teleop_bridge/__init__.py Normal file
View File

@@ -0,0 +1 @@
"""OmniSocket teleop bridge package."""

207
ros-control-py/udp_teleop_bridge/udp_teleop_bridge/cmd_vel_udp_sender.py Normal file
View File

@@ -0,0 +1,207 @@
"""ROS 2 node that forwards TwistStamped teleop commands over OmniSocket."""
from __future__ import annotations
import threading
import time
from typing import Dict, Optional, Tuple
import rclpy
from geometry_msgs.msg import TwistStamped
from rclpy.node import Node
from .omni_transport import MSG_TYPE_ERROR, OmniTransport
from .protocol import (
DEFAULT_EXIT_ZERO_PACKETS,
DEFAULT_INPUT_TIMEOUT,
DEFAULT_INPUT_TOPIC,
DEFAULT_KEYBOARD_PEER_ID,
DEFAULT_QUEUE_DEPTH,
DEFAULT_SEND_RATE_HZ,
DEFAULT_TARGET_PEER,
DEFAULT_TRANSPORT,
ZERO_COMMAND,
pack_command,
)
CommandTuple = Tuple[float, float, float, float, float, float]
class CmdVelUdpSender(Node):
"""Forward TwistStamped messages to a remote OmniSocket peer."""
def __init__(self) -> None:
super().__init__('cmd_vel_udp_sender')
self.declare_parameter('transport', DEFAULT_TRANSPORT)
self.declare_parameter('server_addr', '')
self.declare_parameter('relay_via', '')
self.declare_parameter('peer_id', DEFAULT_KEYBOARD_PEER_ID)
self.declare_parameter('target_peer', DEFAULT_TARGET_PEER)
self.declare_parameter('input_topic', DEFAULT_INPUT_TOPIC)
self.declare_parameter('send_rate_hz', DEFAULT_SEND_RATE_HZ)
self.declare_parameter('input_timeout', DEFAULT_INPUT_TIMEOUT)
self.declare_parameter('queue_depth', DEFAULT_QUEUE_DEPTH)
self.declare_parameter('exit_zero_packets', DEFAULT_EXIT_ZERO_PACKETS)
self._transport_name = str(self.get_parameter('transport').value)
self._server_addr = str(self.get_parameter('server_addr').value)
self._relay_via = str(self.get_parameter('relay_via').value)
self._peer_id = str(self.get_parameter('peer_id').value)
self._target_peer = str(self.get_parameter('target_peer').value).strip()
self._input_topic = str(self.get_parameter('input_topic').value)
self._send_rate_hz = float(self.get_parameter('send_rate_hz').value)
self._input_timeout = float(self.get_parameter('input_timeout').value)
self._queue_depth = int(self.get_parameter('queue_depth').value)
self._exit_zero_packets = int(self.get_parameter('exit_zero_packets').value)
if self._send_rate_hz <= 0.0:
raise ValueError('send_rate_hz must be > 0')
if self._input_timeout < 0.0:
raise ValueError('input_timeout must be >= 0')
if self._queue_depth <= 0:
raise ValueError('queue_depth must be > 0')
if not self._target_peer:
raise ValueError('target_peer must not be empty')
self._transport = OmniTransport(
transport=self._transport_name,
server_addr=self._server_addr,
relay_via=self._relay_via,
peer_id=self._peer_id,
)
self._last_log_times: Dict[str, float] = {}
self._latest_command: CommandTuple = ZERO_COMMAND
self._last_input_monotonic: Optional[float] = None
self._last_sent_command: Optional[CommandTuple] = None
self._closing = threading.Event()
self.create_subscription(
TwistStamped,
self._input_topic,
self._handle_twist,
self._queue_depth,
)
self.create_timer(1.0 / self._send_rate_hz, self._send_latest_command)
self._drain_thread = threading.Thread(target=self._drain_incoming, daemon=True)
self._drain_thread.start()
self.get_logger().info(
'Forwarding TwistStamped from %s via %s://%s as %s -> %s at %.1f Hz '
'(input timeout %.2f s)'
% (
self._input_topic,
self._transport.transport,
self._transport.server_addr,
self._peer_id,
self._target_peer,
self._send_rate_hz,
self._input_timeout,
)
)
def _should_log(self, key: str, throttle_sec: float) -> bool:
now = time.monotonic()
previous = self._last_log_times.get(key)
if previous is None or (now - previous) >= throttle_sec:
self._last_log_times[key] = now
return True
return False
def _handle_twist(self, msg: TwistStamped) -> None:
self._latest_command = (
float(msg.twist.linear.x),
float(msg.twist.linear.y),
float(msg.twist.linear.z),
float(msg.twist.angular.x),
float(msg.twist.angular.y),
float(msg.twist.angular.z),
)
self._last_input_monotonic = time.monotonic()
def _command_for_current_tick(self) -> CommandTuple:
if self._last_input_monotonic is None:
return ZERO_COMMAND
if self._input_timeout == 0.0:
return self._latest_command
age = time.monotonic() - self._last_input_monotonic
if age > self._input_timeout:
return ZERO_COMMAND
return self._latest_command
def _send_command(self, command: CommandTuple) -> None:
payload = pack_command(command)
try:
self._transport.send(to=self._target_peer, data=payload)
self._last_sent_command = command
except OSError as exc:
if self._should_log('send_error', 2.0):
self.get_logger().error(f'OmniSocket send failed: {exc}')
def _send_latest_command(self) -> None:
self._send_command(self._command_for_current_tick())
def _log_inbound_message(self, from_peer: str, msg_type: int, payload: bytes) -> None:
if msg_type == MSG_TYPE_ERROR:
if self._should_log('server_error', 1.0):
text = payload.decode('utf-8', errors='replace')
self.get_logger().error(f'OmniSocket server error from {from_peer}: {text}')
return
if self._should_log('unexpected_inbound', 2.0):
self.get_logger().warning(
'Ignoring unexpected inbound message type %d from %s (%d bytes)'
% (msg_type, from_peer, len(payload))
)
def _drain_incoming(self) -> None:
while not self._closing.is_set() and rclpy.ok():
try:
result = self._transport.recv(timeout_ms=100)
except OSError as exc:
if not self._closing.is_set() and self._should_log('drain_error', 2.0):
self.get_logger().error(f'OmniSocket receive loop stopped: {exc}')
return
if result is None:
continue
from_peer, msg_type, payload = result
self._log_inbound_message(from_peer, msg_type, payload)
def send_zero_burst(self) -> None:
"""Best-effort stop command sent during shutdown."""
for _ in range(max(1, self._exit_zero_packets)):
self._send_command(ZERO_COMMAND)
time.sleep(0.02)
def close(self) -> None:
self._closing.set()
if hasattr(self, '_transport') and self._transport is not None:
try:
self._transport.close()
except OSError as exc:
if self._should_log('close_error', 2.0):
self.get_logger().warning(f'Closing OmniSocket transport failed: {exc}')
self._transport = None
if hasattr(self, '_drain_thread') and self._drain_thread.is_alive():
self._drain_thread.join(timeout=0.5)
def destroy_node(self) -> bool:
self.close()
return super().destroy_node()
def main(args: Optional[list[str]] = None) -> None:
rclpy.init(args=args)
node = CmdVelUdpSender()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.send_zero_burst()
node.destroy_node()
rclpy.shutdown()

95
ros-control-py/udp_teleop_bridge/udp_teleop_bridge/omni_transport.py Normal file
View File

@@ -0,0 +1,95 @@
"""Helpers for working with OmniSocket transport sessions."""
from __future__ import annotations
from .protocol import default_server_addr_for_transport, normalize_transport
try:
from omnisocket import (
CONTROL_DEFAULTS,
MSG_TYPE_BINARY,
MSG_TYPE_ERROR,
Session,
UdpSession,
)
except ImportError as exc: # pragma: no cover - depends on external build/install
raise RuntimeError(
'omnisocket is not installed for this Python environment; run '
'`make python-ext && make python-install` on a Linux host first'
) from exc
def _normalize_optional(value: object) -> str:
return str(value).strip()
class OmniTransport:
"""Small wrapper that normalizes OmniSocket UDP/KCP session setup."""
def __init__(
self,
*,
transport: object,
server_addr: object,
peer_id: object,
relay_via: object = '',
bind_ip: object = '',
bind_device: object = '',
enable_timestamping: bool = False,
) -> None:
self.transport = normalize_transport(transport)
self.server_addr = _normalize_optional(server_addr) or default_server_addr_for_transport(self.transport)
self.peer_id = _normalize_optional(peer_id)
self.relay_via = _normalize_optional(relay_via)
self.bind_ip = _normalize_optional(bind_ip)
self.bind_device = _normalize_optional(bind_device)
if not self.peer_id:
raise ValueError('peer_id must not be empty')
session_cls = Session if self.transport == 'kcp' else UdpSession
self._session = session_cls()
connect_kwargs: dict[str, object] = {
'server_addr': self.server_addr,
'peer_id': self.peer_id,
}
if self.bind_ip:
connect_kwargs['bind_ip'] = self.bind_ip
if self.bind_device:
connect_kwargs['bind_device'] = self.bind_device
if self.transport == 'kcp':
if self.relay_via:
connect_kwargs['relay_via'] = self.relay_via
connect_kwargs.update(CONTROL_DEFAULTS)
else:
connect_kwargs['enable_timestamping'] = bool(enable_timestamping)
self._session.connect(**connect_kwargs)
def send(self, *, to: str, data: bytes) -> None:
self._session.send(to=to, data=data)
def recv(self, *, timeout_ms: int = -1):
return self._session.recv(timeout_ms=timeout_ms)
def recv_into(self, *, buffer, timeout_ms: int = -1):
return self._session.recv_into(buffer=buffer, timeout_ms=timeout_ms)
def close(self) -> None:
self._session.close()
def stats(self) -> dict[str, int]:
return self._session.stats()
__all__ = [
'CONTROL_DEFAULTS',
'MSG_TYPE_BINARY',
'MSG_TYPE_ERROR',
'OmniTransport',
'Session',
'UdpSession',
]

74
ros-control-py/udp_teleop_bridge/udp_teleop_bridge/protocol.py Normal file
View File

@@ -0,0 +1,74 @@
"""Shared teleop protocol helpers and transport defaults."""
from __future__ import annotations
import math
import struct
from typing import Iterable, Tuple
COMMAND_STRUCT = struct.Struct('<6f')
PACKET_SIZE = COMMAND_STRUCT.size
SUPPORTED_TRANSPORTS = ('udp', 'kcp')
DEFAULT_TRANSPORT = 'udp'
DEFAULT_OMNI_UDP_SERVER_ADDR = '127.0.0.1:9001'
DEFAULT_OMNI_KCP_SERVER_ADDR = '127.0.0.1:9002'
DEFAULT_KEYBOARD_PEER_ID = 'ros-keyboard-ctrl'
DEFAULT_GAMEPAD_PEER_ID = 'ros-gamepad-ctrl'
DEFAULT_BRIDGE_PEER_ID = 'ros-bridge-ctrl'
DEFAULT_TARGET_PEER = DEFAULT_BRIDGE_PEER_ID
DEFAULT_FRAME_ID = 'pelvis'
DEFAULT_INPUT_TOPIC = '/teleop/cmd_vel'
DEFAULT_OUTPUT_TOPIC = '/hric/robot/cmd_vel'
DEFAULT_SEND_RATE_HZ = 20.0
DEFAULT_INPUT_TIMEOUT = 0.75
DEFAULT_WATCHDOG_TIMEOUT = 0.5
DEFAULT_PUBLISH_RATE_HZ = 100.0
DEFAULT_QUEUE_DEPTH = 10
DEFAULT_EXIT_ZERO_PACKETS = 3
DEFAULT_RECV_BUFFER_BYTES = 2048
ZERO_COMMAND = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
def normalize_transport(value: object) -> str:
"""Return a supported transport name."""
transport = str(value).strip().lower()
if transport not in SUPPORTED_TRANSPORTS:
supported = ', '.join(SUPPORTED_TRANSPORTS)
raise ValueError(f"Unsupported transport '{transport}', expected one of: {supported}")
return transport
def default_server_addr_for_transport(transport: str) -> str:
"""Return the default OmniSocket server for the chosen transport."""
transport = normalize_transport(transport)
if transport == 'udp':
return DEFAULT_OMNI_UDP_SERVER_ADDR
return DEFAULT_OMNI_KCP_SERVER_ADDR
def normalize_command(values: Iterable[float]) -> Tuple[float, float, float, float, float, float]:
"""Return a finite six-float command tuple."""
command = tuple(float(value) for value in values)
if len(command) != 6:
raise ValueError(f'Expected 6 command values, got {len(command)}')
if any(not math.isfinite(value) for value in command):
raise ValueError('Command contains a non-finite value')
return command
def pack_command(values: Iterable[float]) -> bytes:
"""Pack six floats into the wire format."""
return COMMAND_STRUCT.pack(*normalize_command(values))
def unpack_command(payload: bytes) -> Tuple[float, float, float, float, float, float]:
"""Decode a control packet into a six-float command tuple."""
if len(payload) != PACKET_SIZE:
raise ValueError(f'Expected {PACKET_SIZE} bytes, got {len(payload)}')
return normalize_command(COMMAND_STRUCT.unpack(payload))

238
ros-control-py/udp_teleop_bridge/udp_teleop_bridge/udp_cmd_vel_receiver.py Normal file
View File

@@ -0,0 +1,238 @@
"""ROS 2 node that receives OmniSocket teleop packets and republishes TwistStamped."""
from __future__ import annotations
import threading
import time
from typing import Dict, Optional, Tuple
import rclpy
from geometry_msgs.msg import TwistStamped
from rclpy.node import Node
from .omni_transport import MSG_TYPE_BINARY, MSG_TYPE_ERROR, OmniTransport
from .protocol import (
DEFAULT_BRIDGE_PEER_ID,
DEFAULT_FRAME_ID,
DEFAULT_OUTPUT_TOPIC,
DEFAULT_PUBLISH_RATE_HZ,
DEFAULT_QUEUE_DEPTH,
DEFAULT_RECV_BUFFER_BYTES,
DEFAULT_TRANSPORT,
DEFAULT_WATCHDOG_TIMEOUT,
PACKET_SIZE,
ZERO_COMMAND,
unpack_command,
)
CommandTuple = Tuple[float, float, float, float, float, float]
class UdpCmdVelReceiver(Node):
"""Publish TwistStamped commands from the OmniSocket control wire format."""
def __init__(self) -> None:
super().__init__('udp_cmd_vel_receiver')
self.declare_parameter('transport', DEFAULT_TRANSPORT)
self.declare_parameter('server_addr', '')
self.declare_parameter('relay_via', '')
self.declare_parameter('peer_id', DEFAULT_BRIDGE_PEER_ID)
self.declare_parameter('expected_sender', '')
self.declare_parameter('output_topic', DEFAULT_OUTPUT_TOPIC)
self.declare_parameter('frame_id', DEFAULT_FRAME_ID)
self.declare_parameter('watchdog_timeout', DEFAULT_WATCHDOG_TIMEOUT)
self.declare_parameter('publish_rate_hz', DEFAULT_PUBLISH_RATE_HZ)
self.declare_parameter('queue_depth', DEFAULT_QUEUE_DEPTH)
self._transport_name = str(self.get_parameter('transport').value)
self._server_addr = str(self.get_parameter('server_addr').value)
self._relay_via = str(self.get_parameter('relay_via').value)
self._peer_id = str(self.get_parameter('peer_id').value)
self._expected_sender = str(self.get_parameter('expected_sender').value).strip()
self._output_topic = str(self.get_parameter('output_topic').value)
self._frame_id = str(self.get_parameter('frame_id').value)
self._watchdog_timeout = float(self.get_parameter('watchdog_timeout').value)
self._publish_rate_hz = float(self.get_parameter('publish_rate_hz').value)
self._queue_depth = int(self.get_parameter('queue_depth').value)
if self._watchdog_timeout <= 0.0:
raise ValueError('watchdog_timeout must be > 0')
if self._publish_rate_hz <= 0.0:
raise ValueError('publish_rate_hz must be > 0')
if self._queue_depth <= 0:
raise ValueError('queue_depth must be > 0')
self._publisher = self.create_publisher(TwistStamped, self._output_topic, self._queue_depth)
self._transport = OmniTransport(
transport=self._transport_name,
server_addr=self._server_addr,
relay_via=self._relay_via,
peer_id=self._peer_id,
)
self._lock = threading.Lock()
self._last_log_times: Dict[str, float] = {}
self._latest_command: CommandTuple = ZERO_COMMAND
self._last_packet_monotonic: Optional[float] = None
self._last_published_command: CommandTuple = ZERO_COMMAND
self._closing = threading.Event()
self._recv_buffer = bytearray(DEFAULT_RECV_BUFFER_BYTES)
self.create_timer(1.0 / self._publish_rate_hz, self._publish_tick)
self._recv_thread = threading.Thread(target=self._recv_loop, daemon=True)
self._recv_thread.start()
self.get_logger().info(
'Receiving teleop commands via %s://%s as %s and publishing TwistStamped to %s '
'at %.1f Hz (frame_id=%s, watchdog %.2f s)'
% (
self._transport.transport,
self._transport.server_addr,
self._peer_id,
self._output_topic,
self._publish_rate_hz,
self._frame_id,
self._watchdog_timeout,
)
)
def _should_log(self, key: str, throttle_sec: float) -> bool:
now = time.monotonic()
previous = self._last_log_times.get(key)
if previous is None or (now - previous) >= throttle_sec:
self._last_log_times[key] = now
return True
return False
def _publish_command(self, command: CommandTuple) -> None:
msg = TwistStamped()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = self._frame_id
msg.twist.linear.x = command[0]
msg.twist.linear.y = command[1]
msg.twist.linear.z = command[2]
msg.twist.angular.x = command[3]
msg.twist.angular.y = command[4]
msg.twist.angular.z = command[5]
self._publisher.publish(msg)
self._last_published_command = command
def _handle_error_message(self, from_peer: str, body_len: int) -> None:
if self._should_log('server_error', 1.0):
text = bytes(self._recv_buffer[:body_len]).decode('utf-8', errors='replace')
self.get_logger().error(f'OmniSocket server error from {from_peer}: {text}')
def _recv_loop(self) -> None:
while not self._closing.is_set() and rclpy.ok():
try:
meta = self._transport.recv_into(buffer=self._recv_buffer, timeout_ms=100)
except BufferError as exc:
if self._should_log('buffer_error', 2.0):
self.get_logger().warning(f'Dropped oversized OmniSocket frame: {exc}')
continue
except OSError as exc:
if not self._closing.is_set() and self._should_log('recv_error', 2.0):
self.get_logger().error(f'OmniSocket receive loop stopped: {exc}')
return
if meta is None:
continue
from_peer = str(meta['from'])
msg_type = int(meta['msg_type'])
body_len = int(meta['body_len'])
if msg_type == MSG_TYPE_ERROR:
self._handle_error_message(from_peer, body_len)
continue
if self._expected_sender and from_peer != self._expected_sender:
if self._should_log('unexpected_sender', 2.0):
self.get_logger().warning(
'Ignoring message from unexpected sender %s (expected %s)'
% (from_peer, self._expected_sender)
)
continue
if msg_type != MSG_TYPE_BINARY:
if self._should_log('unexpected_type', 2.0):
self.get_logger().warning(
'Ignoring unexpected message type %d from %s (%d bytes)'
% (msg_type, from_peer, body_len)
)
continue
if body_len != PACKET_SIZE:
if self._should_log('packet_size', 2.0):
self.get_logger().warning(
'Dropped binary payload from %s with invalid size %d (expected %d)'
% (from_peer, body_len, PACKET_SIZE)
)
continue
try:
command = unpack_command(self._recv_buffer[:PACKET_SIZE])
except ValueError as exc:
if self._should_log('decode_error', 2.0):
self.get_logger().warning(f'Dropped malformed command payload: {exc}')
continue
with self._lock:
self._latest_command = command
self._last_packet_monotonic = time.monotonic()
def _command_for_publish_tick(self) -> tuple[CommandTuple, Optional[float], bool]:
with self._lock:
latest_command = self._latest_command
last_packet_monotonic = self._last_packet_monotonic
if last_packet_monotonic is None:
return ZERO_COMMAND, None, False
age = time.monotonic() - last_packet_monotonic
if age > self._watchdog_timeout:
return ZERO_COMMAND, age, True
return latest_command, age, False
def _publish_tick(self) -> None:
publish_command, age, timed_out = self._command_for_publish_tick()
if timed_out and self._last_published_command != ZERO_COMMAND:
if self._should_log('watchdog_stop', 2.0):
self.get_logger().warning(
'Command stream timed out after %.2f s, publishing zero velocity stop'
% age
)
self._publish_command(publish_command)
def close(self) -> None:
self._closing.set()
if hasattr(self, '_transport') and self._transport is not None:
try:
self._transport.close()
except OSError as exc:
if self._should_log('close_error', 2.0):
self.get_logger().warning(f'Closing OmniSocket transport failed: {exc}')
self._transport = None
if hasattr(self, '_recv_thread') and self._recv_thread.is_alive():
self._recv_thread.join(timeout=0.5)
def destroy_node(self) -> bool:
self.close()
return super().destroy_node()
def main(args: Optional[list[str]] = None) -> None:
rclpy.init(args=args)
node = UdpCmdVelReceiver()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()