feat: C控制程序对接KCP

2026-04-03 12:04:39 +08:00
parent 628583f79d
commit 8a1baa64c0
13 changed files with 1720 additions and 0 deletions
--- a/AGENT.md
+++ b/AGENT.md
@@ -0,0 +1,54 @@
+# 通信场景说明
+
+## 系统拓扑
+- 系统中有 `A/B/C/D` 四个终端。
+- `A` 与 `C` 在同一侧，`B` 与 `D` 在同一侧。
+- 实际运行时不是一条端到端 KCP，而是两条独立 KCP 链路：
+  - `B <-> D`
+  - `D <-> A`
+- `C` 只负责 UDP 端口转发。
+- 整体目标是让 `A` 与 `B` 双向通信：
+  - `A -> B` 发送控制命令
+  - `B -> A` 发送视频和其他反馈数据
+
+## 网络情况
+- 主要瓶颈在 `B` 侧 5G 链路，`A` 侧是有线网络，不是主要约束。
+- `B <-> D` 的时延抖动较大：
+  - 常见约 `15-60 ms`
+  - 最差约 `80-90 ms`
+- 带宽是明显非对称的：
+  - `B -> D` 约 `8 Mbps`，偏低且不稳定
+  - `D -> B` 约 `27-50 Mbps`，更高但也有波动
+- 工程含义：
+  - 控制流必须优先保障
+  - 视频流必须限速并具备自适应能力
+  - 不能默认把所有流合成一个共享传输就一定更优
+
+## 三个项目的角色
+- `OmniSocketGo`
+  - 底层传输项目
+  - 使用 C 编写
+  - 提供 OmniSocket/KCP 传输能力，以及视频发送等原生传输程序
+- `robot-command-center`
+  - A 侧上层应用
+  - 主要负责接收视频帧，并提供监控/展示能力
+  - 当前通过本机 `A-side OmniDaemon` 使用传输能力
+- `tienkung-szu`
+  - 上层控制项目
+  - 同时包含 A 侧控制发送和 B 侧控制执行逻辑
+  - A 侧典型入口是键盘/Xbox sender
+  - B 侧典型入口是 `rl_control_node.py` / `rl_control_node_sim.py`，负责接收控制并驱动机器人行为
+
+## 当前架构方向
+- `A` 侧通过 `A-side OmniDaemon` 统一管理本机的控制发送和视频接收。
+- `B` 侧通过 `B-side OmniDaemon` 统一管理本机的控制接收和视频发送。
+- 核心原则：
+  - 控制流与视频流逻辑分离
+  - 每台主机通过本地 daemon 统一持有传输会话
+  - 后续围绕真实网络瓶颈做 telemetry、调度和速率自适应
+
+## 代码原则
+- 一定要精简，不能写过于复杂和冗余的代码，因为必须确保各个环节可控，在低延迟的要求下，需要细化各个环节的消耗
+
+## SSH原则
+- 如果你需要SSH到某个远程端执行操作，一定要确保不做删除文件操作，确保留有记录
--- a/CLAUDE.md
+++ b/CLAUDE.md
@@ -0,0 +1,54 @@
+# 通信场景说明
+
+## 系统拓扑
+- 系统中有 `A/B/C/D` 四个终端。
+- `A` 与 `C` 在同一侧，`B` 与 `D` 在同一侧。
+- 实际运行时不是一条端到端 KCP，而是两条独立 KCP 链路：
+  - `B <-> D`
+  - `D <-> A`
+- `C` 只负责 UDP 端口转发。
+- 整体目标是让 `A` 与 `B` 双向通信：
+  - `A -> B` 发送控制命令
+  - `B -> A` 发送视频和其他反馈数据
+
+## 网络情况
+- 主要瓶颈在 `B` 侧 5G 链路，`A` 侧是有线网络，不是主要约束。
+- `B <-> D` 的时延抖动较大：
+  - 常见约 `15-60 ms`
+  - 最差约 `80-90 ms`
+- 带宽是明显非对称的：
+  - `B -> D` 约 `8 Mbps`，偏低且不稳定
+  - `D -> B` 约 `27-50 Mbps`，更高但也有波动
+- 工程含义：
+  - 控制流必须优先保障
+  - 视频流必须限速并具备自适应能力
+  - 不能默认把所有流合成一个共享传输就一定更优
+
+## 三个项目的角色
+- `OmniSocketGo`
+  - 底层传输项目
+  - 使用 C 编写
+  - 提供 OmniSocket/KCP 传输能力，以及视频发送等原生传输程序
+- `robot-command-center`
+  - A 侧上层应用
+  - 主要负责接收视频帧，并提供监控/展示能力
+  - 当前通过本机 `A-side OmniDaemon` 使用传输能力
+- `tienkung-szu`
+  - 上层控制项目
+  - 同时包含 A 侧控制发送和 B 侧控制执行逻辑
+  - A 侧典型入口是键盘/Xbox sender
+  - B 侧典型入口是 `rl_control_node.py` / `rl_control_node_sim.py`，负责接收控制并驱动机器人行为
+
+## 当前架构方向
+- `A` 侧通过 `A-side OmniDaemon` 统一管理本机的控制发送和视频接收。
+- `B` 侧通过 `B-side OmniDaemon` 统一管理本机的控制接收和视频发送。
+- 核心原则：
+  - 控制流与视频流逻辑分离
+  - 每台主机通过本地 daemon 统一持有传输会话
+  - 后续围绕真实网络瓶颈做 telemetry、调度和速率自适应
+
+## 代码原则
+- 一定要精简，不能写过于复杂和冗余的代码，因为必须确保各个环节可控，在低延迟的要求下，需要细化各个环节的消耗
+
+## SSH原则
+- 如果你需要SSH到某个远程端执行操作，一定要确保不做删除文件操作，确保留有记录
--- a/ros-control-c/Makefile
+++ b/ros-control-c/Makefile
@@ -0,0 +1,34 @@
+CC      = gcc
+CFLAGS  = -std=c11 -Wall -Wextra -O2 -pthread -D_GNU_SOURCE -I../include -I../third_party/cjson -I../third_party/kcp -I./common
+LDFLAGS = -pthread -lm
+
+OMNI_SRCS = \
+	../src/omni_common.c \
+	../src/protocol.c \
+	../src/latencylog.c \
+	../src/kcp_packet_debug.c \
+	../src/kcp_session_stats.c \
+	../src/linux_timestamping.c \
+	../src/transport_kcp.c \
+	../src/peer_kcp_client.c \
+	../third_party/cjson/cJSON.c \
+	../third_party/kcp/ikcp.c
+
+BUILDDIR = build
+
+TARGETS = $(BUILDDIR)/keyboard_controller $(BUILDDIR)/gamepad_controller
+
+.PHONY: all clean
+
+all: $(TARGETS)
+
+$(BUILDDIR)/keyboard_controller: remote/keyboard_controller.c common/protocol.h common/teleop_transport.h common/teleop_transport.c $(OMNI_SRCS)
+	@mkdir -p $(BUILDDIR)
+	$(CC) $(CFLAGS) -o $@ remote/keyboard_controller.c common/teleop_transport.c $(OMNI_SRCS) $(LDFLAGS)
+
+$(BUILDDIR)/gamepad_controller: remote/gamepad_controller.c common/protocol.h common/teleop_transport.h common/teleop_transport.c $(OMNI_SRCS)
+	@mkdir -p $(BUILDDIR)
+	$(CC) $(CFLAGS) -o $@ remote/gamepad_controller.c common/teleop_transport.c $(OMNI_SRCS) $(LDFLAGS)
+
+clean:
+	rm -rf $(BUILDDIR)
--- a/ros-control-c/README.md
+++ b/ros-control-c/README.md
@@ -0,0 +1,71 @@
+# ros-control-c
+
+`ros-control-c` keeps the original 24-byte `twist_cmd_t` control payload and now supports two runtime transports:
+
+- `udp` (default): unchanged from the original implementation
+- `kcp`: sent through OmniSocket using `MSG_TYPE_BINARY`
+
+## Build
+
+On Linux:
+
+```bash
+make -C ros-control-c
+```
+
+If the robot-side Python bridge will use KCP, build and install the OmniSocket Python extension from the repo root first:
+
+```bash
+make python-ext
+make python-install
+```
+
+## UDP Mode
+
+Sender:
+
+```bash
+./ros-control-c/build/keyboard_controller -i 192.168.1.100 -p 9870
+./ros-control-c/build/gamepad_controller -i 192.168.1.100 -p 9870
+```
+
+Robot bridge:
+
+```bash
+python3 ros-control-c/robot/udp_ros_bridge.py
+```
+
+## KCP Mode
+
+Start the existing OmniSocket KCP hub from the repo root:
+
+```bash
+./bin/kcpserver -listen :9002
+```
+
+Sender:
+
+```bash
+./ros-control-c/build/keyboard_controller -t kcp -s 192.168.1.50:9002 -I ros-keyboard-ctrl -T ros-bridge-ctrl
+./ros-control-c/build/gamepad_controller -t kcp -s 192.168.1.50:9002 -I ros-gamepad-ctrl -T ros-bridge-ctrl
+```
+
+If a relay is needed, add `-r <relay_addr>` to the controller command.
+
+Robot bridge:
+
+```bash
+python3 ros-control-c/robot/udp_ros_bridge.py --ros-args \
+  -p transport:=kcp \
+  -p kcp_server:=192.168.1.50:9002 \
+  -p peer_id:=ros-bridge-ctrl
+```
+
+Optional sender filtering:
+
+```bash
+python3 ros-control-c/robot/udp_ros_bridge.py --ros-args \
+  -p transport:=kcp \
+  -p peer_id:=ros-bridge-ctrl \
+  -p expected_sender:=ros-keyboard-ctrl
+```
--- a/ros-control-c/Robot
+++ b/ros-control-c/Robot
@@ -0,0 +1,221 @@
+Robot Remote Control via UDP — Implementation Plan
+
+ Context
+
+ The robot subscribes to /hric/robot/cmd_vel with geometry_msgs/msg/TwistStamped (frame_id: pelvis). Standard ROS2 teleop tools (teleop_twist_keyboard, teleop_twist_joy) publish   
+ plain Twist, not TwistStamped, so they won't work directly. We build custom keyboard and gamepad controllers in C (zero external dependencies, Linux-only) communicating over UDP  
+ to a robot-side ROS2 bridge.
+
+ How to Make the Robot Move
+
+ Publish TwistStamped to /hric/robot/cmd_vel continuously (~20 Hz):
+
+ ┌─────────────────────┬─────────────────┐
+ │        Field        │     Effect      │
+ ├─────────────────────┼─────────────────┤
+ │ twist.linear.x > 0  │ Walk forward    │
+ ├─────────────────────┼─────────────────┤
+ │ twist.linear.x < 0  │ Walk backward   │
+ ├─────────────────────┼─────────────────┤
+ │ twist.linear.y > 0  │ Strafe left     │
+ ├─────────────────────┼─────────────────┤
+ │ twist.linear.y < 0  │ Strafe right    │
+ ├─────────────────────┼─────────────────┤
+ │ twist.angular.z > 0 │ Turn left (CCW) │
+ ├─────────────────────┼─────────────────┤
+ │ twist.angular.z < 0 │ Turn right (CW) │
+ ├─────────────────────┼─────────────────┤
+ │ All zeros           │ Stop            │
+ └─────────────────────┴─────────────────┘
+
+ Header must have frame_id = "pelvis" and current ROS timestamp.
+
+ ---
+ Architecture
+
+ [PC: Keyboard/Gamepad (C)] --UDP binary struct--> [Robot: Bridge (Python/rclpy)] --> /hric/robot/cmd_vel
+
+ ---
+ Project Structure
+
+ ros-control/
+ ├── topic_example.yaml              # (existing)
+ ├── Makefile                         # Build both C programs
+ ├── common/
+ │   └── protocol.h                   # Shared UDP protocol (binary struct)
+ ├── remote/
+ │   ├── keyboard_controller.c        # Keyboard teleop (C, termios)
+ │   └── gamepad_controller.c         # Gamepad teleop (C, Linux joystick API)
+ └── robot/
+     └── udp_ros_bridge.py            # UDP → ROS2 TwistStamped (Python/rclpy)
+
+ ---
+ UDP Protocol (common/protocol.h)
+
+ Binary packed struct — 24 bytes, no parsing overhead:
+
+ #pragma pack(push, 1)
+ typedef struct {
+     float lx, ly, lz;   // linear velocity (m/s)
+     float ax, ay, az;    // angular velocity (rad/s)
+ } twist_cmd_t;
+ #pragma pack(pop)
+
+ #define DEFAULT_PORT 9870
+ #define DEFAULT_IP   "127.0.0.1"
+
+ On Python side, decode with struct.unpack('<6f', data).
+
+ ---
+ Program 1: Keyboard Controller (remote/keyboard_controller.c)
+
+ Dependencies: None (POSIX + termios only)
+
+ Technical approach:
+ - termios.h: Set terminal to raw mode (~ICANON, ~ECHO, VMIN=0, VTIME=1)
+ - select() with 50ms timeout for non-blocking key detection
+ - Arrow keys: detect ESC sequence (\x1B[A/B/C/D)
+ - UDP send via standard socket() / sendto()
+
+ Key mapping:
+
+ ┌────────┬───────────────────────────────────┐
+ │  Key   │              Action               │
+ ├────────┼───────────────────────────────────┤
+ │ W / ↑  │ Forward (+linear.x)               │
+ ├────────┼───────────────────────────────────┤
+ │ S / ↓  │ Backward (-linear.x)              │
+ ├────────┼───────────────────────────────────┤
+ │ A / ←  │ Turn left (+angular.z)            │
+ ├────────┼───────────────────────────────────┤
+ │ D / →  │ Turn right (-angular.z)           │
+ ├────────┼───────────────────────────────────┤
+ │ Q      │ Strafe left (+linear.y)           │
+ ├────────┼───────────────────────────────────┤
+ │ E      │ Strafe right (-linear.y)          │
+ ├────────┼───────────────────────────────────┤
+ │ Space  │ Emergency stop (all zeros)        │
+ ├────────┼───────────────────────────────────┤
+ │ [ / ]  │ Decrease / increase linear speed  │
+ ├────────┼───────────────────────────────────┤
+ │ - / =  │ Decrease / increase angular speed │
+ ├────────┼───────────────────────────────────┤
+ │ Ctrl+C │ Quit (restore terminal)           │
+ └────────┴───────────────────────────────────┘
+
+ Behavior:
+ - 20 Hz send loop in main thread
+ - On key press: set velocity to ±max_speed
+ - On no key (select timeout): gradually decay velocity to zero OR send zero immediately (configurable)
+ - Print current velocity and speed settings to terminal (refresh in-place with \r)
+ - signal(SIGINT) handler to restore terminal settings before exit
+ - CLI args: -i <ip>, -p <port>, -l <max_linear>, -a <max_angular>
+
+ ---
+ Program 2: Gamepad Controller (remote/gamepad_controller.c)
+
+ Dependencies: None (Linux joystick API only: linux/joystick.h)
+
+ Technical approach:
+ - Open /dev/input/js0 (configurable) with O_RDONLY | O_NONBLOCK
+ - Read struct js_event (8 bytes: __u32 time, __s16 value, __u8 type, __u8 number)
+ - Event types: JS_EVENT_AXIS (0x02), JS_EVENT_BUTTON (0x01)
+ - select() for multiplexing joystick read + periodic UDP send
+
+ Xbox controller axis mapping (xpad driver):
+
+ ┌────────┬───────────────┬───────────────────────────────────┐
+ │ Axis # │   Physical    │              Mapping              │
+ ├────────┼───────────────┼───────────────────────────────────┤
+ │ 0      │ Left stick X  │ linear.y (strafe)                 │
+ ├────────┼───────────────┼───────────────────────────────────┤
+ │ 1      │ Left stick Y  │ linear.x (forward/back, inverted) │
+ ├────────┼───────────────┼───────────────────────────────────┤
+ │ 3      │ Right stick X │ angular.z (turn)                  │
+ └────────┴───────────────┴───────────────────────────────────┘
+
+ Button mapping:
+
+ ┌──────────┬──────────┬────────────────┐
+ │ Button # │ Physical │     Action     │
+ ├──────────┼──────────┼────────────────┤
+ │ 0        │ A        │ Emergency stop │
+ ├──────────┼──────────┼────────────────┤
+ │ 1        │ B        │ Quit           │
+ └──────────┴──────────┴────────────────┘
+
+ Behavior:
+ - Axis values: raw range [-32767, 32767] → normalized to [-1.0, 1.0] → scaled by max_speed
+ - Deadzone: |normalized| < 0.1 → treat as 0 (configurable)
+ - 20 Hz UDP send loop
+ - Print gamepad name (via JSIOCGNAME ioctl), axes, and current velocities
+ - Auto-detect controller disconnect / reconnect
+ - CLI args: -i <ip>, -p <port>, -d <device>, -l <max_linear>, -a <max_angular>, -z <deadzone>
+
+ ---
+ Program 3: UDP-to-ROS2 Bridge (robot/udp_ros_bridge.py)
+
+ Dependencies: rclpy, geometry_msgs (standard ROS2)
+
+ Behavior:
+ - ROS2 node: udp_teleop_bridge
+ - Bind UDP on 0.0.0.0:9870
+ - Receive 24-byte struct → struct.unpack('<6f', data) → build TwistStamped
+ - Set header.stamp = current ROS time, header.frame_id = 'pelvis'
+ - Publish to /hric/robot/cmd_vel at received rate
+ - Watchdog: if no packet for 0.5s, publish zero velocity (safety stop)
+ - UDP recv in separate threading.Thread, ROS2 spin() in main thread
+ - ROS2 parameters: udp_port (int), topic (string), frame_id (string), timeout (float)
+
+ ---
+ Build System (Makefile)
+
+ CC = gcc
+ CFLAGS = -Wall -Wextra -O2 -I./common
+ LDFLAGS = -lm
+
+ all: build/keyboard_controller build/gamepad_controller
+
+ build/keyboard_controller: remote/keyboard_controller.c common/protocol.h
+        @mkdir -p build
+        $(CC) $(CFLAGS) -o $@ $< $(LDFLAGS)
+
+ build/gamepad_controller: remote/gamepad_controller.c common/protocol.h
+        @mkdir -p build
+        $(CC) $(CFLAGS) -o $@ $< $(LDFLAGS)
+
+ clean:
+        rm -rf build
+
+ ---
+ Files to Create (5 total)
+
+ ┌─────┬──────────────────────────────┬────────┬───────────────────────────────────┐
+ │  #  │             File             │  Lang  │              Purpose              │
+ ├─────┼──────────────────────────────┼────────┼───────────────────────────────────┤
+ │ 1   │ common/protocol.h            │ C      │ UDP protocol: struct + constants  │
+ ├─────┼──────────────────────────────┼────────┼───────────────────────────────────┤
+ │ 2   │ remote/keyboard_controller.c │ C      │ Keyboard → UDP (termios, select)  │
+ ├─────┼──────────────────────────────┼────────┼───────────────────────────────────┤
+ │ 3   │ remote/gamepad_controller.c  │ C      │ Gamepad → UDP (linux/joystick.h)  │
+ ├─────┼──────────────────────────────┼────────┼───────────────────────────────────┤
+ │ 4   │ robot/udp_ros_bridge.py      │ Python │ UDP → ROS2 TwistStamped publisher │
+ ├─────┼──────────────────────────────┼────────┼───────────────────────────────────┤
+ │ 5   │ Makefile                     │ Make   │ Build system                      │
+ └─────┴──────────────────────────────┴────────┴───────────────────────────────────┘
+
+ ---
+ Verification
+
+ 1. Build: make — should compile without warnings
+ 2. Keyboard test: Run build/keyboard_controller -i 127.0.0.1, use a simple Python UDP listener to verify packets:
+ import socket, struct
+ s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ s.bind(('0.0.0.0', 9870))
+ while True:
+     data, _ = s.recvfrom(24)
+     print(struct.unpack('<6f', data))
+ 3. Gamepad test: Connect Xbox controller, run build/gamepad_controller, verify stick input produces correct UDP packets
+ 4. Bridge test: Run udp_ros_bridge.py, then ros2 topic echo /hric/robot/cmd_vel to verify TwistStamped messages
+ 5. Safety: Stop controller, confirm bridge sends zero velocity after 0.5s timeout
+ 6. End-to-end: Controller → Bridge → robot moves
--- a/ros-control-c/build/gamepad_controller
+++ b/ros-control-c/build/gamepad_controller
--- a/ros-control-c/build/keyboard_controller
+++ b/ros-control-c/build/keyboard_controller
--- a/ros-control-c/common/protocol.h
+++ b/ros-control-c/common/protocol.h
@@ -0,0 +1,26 @@
+#ifndef PROTOCOL_H
+#define PROTOCOL_H
+
+#include <stdint.h>
+
+#define DEFAULT_PORT    9870
+#define DEFAULT_IP      "127.0.0.1"
+#define SEND_RATE_HZ    20
+#define SEND_INTERVAL_US (1000000 / SEND_RATE_HZ)
+
+#pragma pack(push, 1)
+typedef struct {
+    float lx, ly, lz;  /* linear  velocity (m/s)   */
+    float ax, ay, az;   /* angular velocity (rad/s) */
+} twist_cmd_t;
+#pragma pack(pop)
+
+#define TWIST_CMD_SIZE sizeof(twist_cmd_t)  /* 24 bytes */
+
+static inline void twist_cmd_zero(twist_cmd_t *cmd)
+{
+    cmd->lx = cmd->ly = cmd->lz = 0.0f;
+    cmd->ax = cmd->ay = cmd->az = 0.0f;
+}
+
+#endif /* PROTOCOL_H */
--- a/ros-control-c/common/teleop_transport.c
+++ b/ros-control-c/common/teleop_transport.c
@@ -0,0 +1,300 @@
+#include "teleop_transport.h"
+
+#include <arpa/inet.h>
+#include <errno.h>
+#include <stdio.h>
+#include <string.h>
+#include <sys/socket.h>
+#include <unistd.h>
+
+static void teleop_transport_clear(teleop_transport_t *transport)
+{
+    if (transport == NULL) {
+        return;
+    }
+    memset(transport, 0, sizeof(*transport));
+    transport->mode = TELEOP_TRANSPORT_MODE_UDP;
+    transport->udp_fd = -1;
+}
+
+int teleop_transport_parse_mode(const char *raw, teleop_transport_mode_t *out_mode)
+{
+    if (raw == NULL || out_mode == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+    if (strcmp(raw, "udp") == 0) {
+        *out_mode = TELEOP_TRANSPORT_MODE_UDP;
+        return 0;
+    }
+    if (strcmp(raw, "kcp") == 0) {
+        *out_mode = TELEOP_TRANSPORT_MODE_KCP;
+        return 0;
+    }
+    errno = EINVAL;
+    return -1;
+}
+
+const char *teleop_transport_mode_name(teleop_transport_mode_t mode)
+{
+    return mode == TELEOP_TRANSPORT_MODE_KCP ? "kcp" : "udp";
+}
+
+static void teleop_transport_log_incoming(const message_t *msg)
+{
+    if (msg == NULL) {
+        return;
+    }
+
+    switch (msg->type) {
+    case MSG_TYPE_ERROR:
+        fprintf(stderr,
+                "teleop transport: server error from %s to %s: %.*s\n",
+                msg->from,
+                msg->to,
+                (int)msg->body_len,
+                msg->body == NULL ? "" : (const char *)msg->body);
+        break;
+    case MSG_TYPE_TEXT:
+        fprintf(stderr,
+                "teleop transport: dropped unexpected text from %s to %s: %.*s\n",
+                msg->from,
+                msg->to,
+                (int)msg->body_len,
+                msg->body == NULL ? "" : (const char *)msg->body);
+        break;
+    case MSG_TYPE_BINARY:
+        fprintf(stderr,
+                "teleop transport: dropped unexpected binary payload from %s to %s (%lu bytes)\n",
+                msg->from,
+                msg->to,
+                (unsigned long)msg->body_len);
+        break;
+    case MSG_TYPE_FILE:
+        fprintf(stderr,
+                "teleop transport: dropped unexpected file from %s to %s: %s (%lu bytes)\n",
+                msg->from,
+                msg->to,
+                msg->file_name,
+                (unsigned long)msg->body_len);
+        break;
+    case MSG_TYPE_REGISTER:
+        fprintf(stderr,
+                "teleop transport: dropped unexpected register message from %s to %s\n",
+                msg->from,
+                msg->to);
+        break;
+    default:
+        fprintf(stderr,
+                "teleop transport: dropped unexpected message type %s from %s\n",
+                protocol_message_type_name(msg->type),
+                msg->from);
+        break;
+    }
+}
+
+static void *teleop_transport_kcp_recv_thread_main(void *arg)
+{
+    teleop_transport_t *transport = (teleop_transport_t *)arg;
+
+    for (;;) {
+        message_t msg;
+        int rc;
+
+        if (transport->stop_requested) {
+            return NULL;
+        }
+
+        protocol_message_init(&msg);
+        rc = kcp_client_receive_timed(transport->kcp_client, &msg, 100);
+        if (rc == 1) {
+            protocol_message_clear(&msg);
+            continue;
+        }
+        if (rc != 0) {
+            protocol_message_clear(&msg);
+            if (!transport->stop_requested) {
+                int saved_errno = errno;
+                fprintf(stderr,
+                        "teleop transport: KCP receive loop stopped: %s (errno=%d)\n",
+                        saved_errno != 0 ? strerror(saved_errno) : "unknown error",
+                        saved_errno);
+            }
+            return NULL;
+        }
+
+        teleop_transport_log_incoming(&msg);
+        protocol_message_clear(&msg);
+    }
+}
+
+static int teleop_transport_open_udp(teleop_transport_t *transport, const teleop_transport_config_t *config)
+{
+    int sockfd;
+
+    if (transport == NULL || config == NULL || config->udp_ip == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    sockfd = socket(AF_INET, SOCK_DGRAM, 0);
+    if (sockfd < 0) {
+        perror("socket");
+        return -1;
+    }
+
+    memset(&transport->udp_dest, 0, sizeof(transport->udp_dest));
+    transport->udp_dest.sin_family = AF_INET;
+    transport->udp_dest.sin_port = htons(config->udp_port);
+    if (inet_pton(AF_INET, config->udp_ip, &transport->udp_dest.sin_addr) <= 0) {
+        fprintf(stderr, "Invalid IP: %s\n", config->udp_ip);
+        close(sockfd);
+        errno = EINVAL;
+        return -1;
+    }
+
+    transport->udp_fd = sockfd;
+    return 0;
+}
+
+static int teleop_transport_open_kcp(teleop_transport_t *transport, const teleop_transport_config_t *config)
+{
+    kcp_conn_options_t options;
+    const char *relay_via;
+
+    if (transport == NULL || config == NULL ||
+        config->server_addr == NULL || config->peer_id == NULL || config->target_peer == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    kcp_conn_options_set_control_defaults(&options);
+    relay_via = (config->relay_via != NULL && config->relay_via[0] != '\0') ? config->relay_via : NULL;
+    transport->kcp_client = kcp_client_dial_with_options(
+        config->server_addr,
+        relay_via,
+        config->peer_id,
+        "",
+        "",
+        &options,
+        NULL,
+        NULL,
+        NULL,
+        KCP_DEFAULT_STATS_INTERVAL_MS
+    );
+    if (transport->kcp_client == NULL) {
+        int saved_errno = errno;
+        fprintf(stderr,
+                "teleop transport: failed to open KCP session as %s via %s%s%s: %s (errno=%d)\n",
+                config->peer_id,
+                config->server_addr,
+                relay_via != NULL ? ", relay=" : "",
+                relay_via != NULL ? relay_via : "",
+                saved_errno != 0 ? strerror(saved_errno) : "unknown error",
+                saved_errno);
+        errno = saved_errno;
+        return -1;
+    }
+
+    {
+        int thread_rc = pthread_create(&transport->recv_thread, NULL, teleop_transport_kcp_recv_thread_main, transport);
+        if (thread_rc != 0) {
+            fprintf(stderr,
+                    "teleop transport: failed to start KCP receive thread: %s (errno=%d)\n",
+                    strerror(thread_rc),
+                    thread_rc);
+            kcp_client_close(transport->kcp_client);
+            kcp_client_free(transport->kcp_client);
+            transport->kcp_client = NULL;
+            errno = thread_rc;
+            return -1;
+        }
+    }
+    transport->recv_thread_started = 1;
+    return 0;
+}
+
+int teleop_transport_open(teleop_transport_t *transport, const teleop_transport_config_t *config)
+{
+    if (transport == NULL || config == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    teleop_transport_clear(transport);
+    transport->mode = config->mode;
+    snprintf(transport->server_addr, sizeof(transport->server_addr), "%s",
+             config->server_addr == NULL ? "" : config->server_addr);
+    snprintf(transport->relay_via, sizeof(transport->relay_via), "%s",
+             config->relay_via == NULL ? "" : config->relay_via);
+    snprintf(transport->peer_id, sizeof(transport->peer_id), "%s",
+             config->peer_id == NULL ? "" : config->peer_id);
+    snprintf(transport->target_peer, sizeof(transport->target_peer), "%s",
+             config->target_peer == NULL ? "" : config->target_peer);
+
+    if (config->mode == TELEOP_TRANSPORT_MODE_KCP) {
+        return teleop_transport_open_kcp(transport, config);
+    }
+    return teleop_transport_open_udp(transport, config);
+}
+
+int teleop_transport_send_twist(teleop_transport_t *transport, const twist_cmd_t *cmd)
+{
+    if (transport == NULL || cmd == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    if (transport->mode == TELEOP_TRANSPORT_MODE_KCP) {
+        if (kcp_client_send_binary(transport->kcp_client, transport->target_peer, cmd, TWIST_CMD_SIZE) != 0) {
+            int saved_errno = errno;
+            fprintf(stderr,
+                    "teleop transport: failed to send KCP payload to %s: %s (errno=%d)\n",
+                    transport->target_peer,
+                    saved_errno != 0 ? strerror(saved_errno) : "unknown error",
+                    saved_errno);
+            errno = saved_errno;
+            return -1;
+        }
+        return 0;
+    }
+
+    {
+        ssize_t sent = sendto(transport->udp_fd, cmd, TWIST_CMD_SIZE, 0,
+                              (const struct sockaddr *)&transport->udp_dest, sizeof(transport->udp_dest));
+        if (sent < 0) {
+            perror("sendto");
+            return -1;
+        }
+        if ((size_t)sent != TWIST_CMD_SIZE) {
+            fprintf(stderr, "sendto: short send (%zd/%zu)\n", sent, (size_t)TWIST_CMD_SIZE);
+            errno = EIO;
+            return -1;
+        }
+    }
+    return 0;
+}
+
+void teleop_transport_close(teleop_transport_t *transport)
+{
+    if (transport == NULL) {
+        return;
+    }
+
+    transport->stop_requested = 1;
+    if (transport->kcp_client != NULL) {
+        kcp_client_close(transport->kcp_client);
+    }
+    if (transport->recv_thread_started) {
+        pthread_join(transport->recv_thread, NULL);
+        transport->recv_thread_started = 0;
+    }
+    if (transport->kcp_client != NULL) {
+        kcp_client_free(transport->kcp_client);
+        transport->kcp_client = NULL;
+    }
+    if (transport->udp_fd >= 0) {
+        close(transport->udp_fd);
+        transport->udp_fd = -1;
+    }
+}
--- a/ros-control-c/common/teleop_transport.h
+++ b/ros-control-c/common/teleop_transport.h
@@ -0,0 +1,59 @@
+#ifndef TELEOP_TRANSPORT_H
+#define TELEOP_TRANSPORT_H
+
+#include <netinet/in.h>
+#include <pthread.h>
+
+#include "protocol.h"
+#include "peer_kcp_client.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define DEFAULT_KCP_SERVER_ADDR "127.0.0.1:9002"
+#define DEFAULT_KCP_KEYBOARD_PEER_ID "ros-keyboard-ctrl"
+#define DEFAULT_KCP_GAMEPAD_PEER_ID "ros-gamepad-ctrl"
+#define DEFAULT_KCP_TARGET_PEER_ID "ros-bridge-ctrl"
+
+typedef enum teleop_transport_mode {
+    TELEOP_TRANSPORT_MODE_UDP = 0,
+    TELEOP_TRANSPORT_MODE_KCP = 1
+} teleop_transport_mode_t;
+
+typedef struct teleop_transport_config {
+    teleop_transport_mode_t mode;
+    const char *udp_ip;
+    int udp_port;
+    const char *server_addr;
+    const char *relay_via;
+    const char *peer_id;
+    const char *target_peer;
+} teleop_transport_config_t;
+
+typedef struct teleop_transport {
+    teleop_transport_mode_t mode;
+    int udp_fd;
+    struct sockaddr_in udp_dest;
+    kcp_client_t *kcp_client;
+    pthread_t recv_thread;
+    int recv_thread_started;
+    volatile int stop_requested;
+    char server_addr[OMNI_MAX_ADDR_TEXT];
+    char relay_via[OMNI_MAX_ADDR_TEXT];
+    char peer_id[OMNI_MAX_PEER_ID];
+    char target_peer[OMNI_MAX_PEER_ID];
+} teleop_transport_t;
+
+int teleop_transport_parse_mode(const char *raw, teleop_transport_mode_t *out_mode);
+const char *teleop_transport_mode_name(teleop_transport_mode_t mode);
+
+int teleop_transport_open(teleop_transport_t *transport, const teleop_transport_config_t *config);
+int teleop_transport_send_twist(teleop_transport_t *transport, const twist_cmd_t *cmd);
+void teleop_transport_close(teleop_transport_t *transport);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* TELEOP_TRANSPORT_H */
--- a/ros-control-c/remote/gamepad_controller.c
+++ b/ros-control-c/remote/gamepad_controller.c
@@ -0,0 +1,293 @@
+/*
+ * gamepad_controller.c — Gamepad/joystick teleop over UDP or KCP
+ *
+ * Uses the Linux joystick API (/dev/input/js*).
+ * Zero external dependencies.
+ *
+ * Xbox controller mapping (xpad driver):
+ *   Left stick Y  (axis 1) → linear.x  (forward/back, inverted)
+ *   Left stick X  (axis 0) → linear.y  (strafe)
+ *   Right stick X (axis 3) → angular.z (turn)
+ *   Button A (0) → emergency stop
+ *   Button B (1) → quit
+ *
+ * Build:  gcc -Wall -O2 -I../common -o gamepad_controller gamepad_controller.c -lm
+ * Usage:  ./gamepad_controller [-i IP] [-p PORT] [-d /dev/input/js0]
+ *                              [-l MAX_LIN] [-a MAX_ANG] [-z DEADZONE]
+ *                              [-t udp|kcp] [-s SERVER] [-r RELAY]
+ *                              [-I PEER_ID] [-T TARGET_PEER]
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <signal.h>
+#include <math.h>
+#include <errno.h>
+#include <sys/select.h>
+#include <sys/time.h>
+#include <sys/ioctl.h>
+#include <getopt.h>
+#include <linux/joystick.h>
+
+#include "../common/protocol.h"
+#include "../common/teleop_transport.h"
+
+/* ── config ─────────────────────────────────────────────────────────── */
+#define MAX_AXES    16
+#define MAX_BUTTONS 16
+#define JS_AXIS_MAX 32767.0f
+
+/* Xbox mapping indices */
+#define AXIS_LX  0      /* left stick X  → strafe  */
+#define AXIS_LY  1      /* left stick Y  → fwd/back (inverted) */
+#define AXIS_RX  3      /* right stick X → turn     */
+
+#define BTN_STOP 0      /* A → emergency stop */
+#define BTN_QUIT 1      /* B → quit */
+
+static volatile sig_atomic_t g_running = 1;
+
+static void sigint_handler(int sig) { (void)sig; g_running = 0; }
+
+static int parse_port(const char *text, int *port_out)
+{
+    char *end = NULL;
+    long value = strtol(text, &end, 10);
+
+    if (end == text || *end != '\0' || value < 1 || value > 65535)
+        return -1;
+
+    *port_out = (int)value;
+    return 0;
+}
+
+/* ── apply deadzone ─────────────────────────────────────────────────── */
+static float apply_deadzone(float v, float dz)
+{
+    if (fabsf(v) < dz) return 0.0f;
+    /* rescale so the output starts from 0 just outside the deadzone */
+    float sign = (v > 0) ? 1.0f : -1.0f;
+    return sign * (fabsf(v) - dz) / (1.0f - dz);
+}
+
+/* ── usage ──────────────────────────────────────────────────────────── */
+static void usage(const char *prog)
+{
+    fprintf(stderr,
+        "Usage: %s [options]\n"
+        "  -i IP       target IP       (default %s)\n"
+        "  -p PORT     target port     (default %d)\n"
+        "  -d DEVICE   joystick device (default /dev/input/js0)\n"
+        "  -l SPEED    max linear  m/s (default 0.5)\n"
+        "  -a SPEED    max angular rad/s (default 0.5)\n"
+        "  -z DZ       deadzone 0<=DZ<1 (default 0.1)\n"
+        "  -t MODE     transport mode udp|kcp (default udp)\n"
+        "  -s ADDR     KCP server addr        (default %s)\n"
+        "  -r ADDR     KCP relay addr         (default none)\n"
+        "  -I ID       local KCP peer id      (default %s)\n"
+        "  -T ID       target KCP peer id     (default %s)\n"
+        "  -h          show help\n",
+        prog, DEFAULT_IP, DEFAULT_PORT,
+        DEFAULT_KCP_SERVER_ADDR,
+        DEFAULT_KCP_GAMEPAD_PEER_ID,
+        DEFAULT_KCP_TARGET_PEER_ID);
+}
+
+/* ──────────────────────────────────────────────────────────────────── */
+int main(int argc, char *argv[])
+{
+    char  ip[64]     = DEFAULT_IP;
+    int   port       = DEFAULT_PORT;
+    char  device[128] = "/dev/input/js0";
+    char  kcp_server[OMNI_MAX_ADDR_TEXT] = DEFAULT_KCP_SERVER_ADDR;
+    char  kcp_relay[OMNI_MAX_ADDR_TEXT] = "";
+    char  peer_id[OMNI_MAX_PEER_ID] = DEFAULT_KCP_GAMEPAD_PEER_ID;
+    char  target_peer[OMNI_MAX_PEER_ID] = DEFAULT_KCP_TARGET_PEER_ID;
+    float max_lin    = 0.5f;
+    float max_ang    = 0.5f;
+    float deadzone   = 0.1f;
+    teleop_transport_mode_t transport_mode = TELEOP_TRANSPORT_MODE_UDP;
+    teleop_transport_t transport;
+    teleop_transport_config_t transport_config;
+
+    int opt;
+    while ((opt = getopt(argc, argv, "i:p:d:l:a:z:t:s:r:I:T:h")) != -1) {
+        switch (opt) {
+        case 'i': strncpy(ip, optarg, sizeof(ip)-1); ip[sizeof(ip)-1] = '\0'; break;
+        case 'p':
+            if (parse_port(optarg, &port) != 0) {
+                fprintf(stderr, "Invalid port: %s (expected 1-65535)\n", optarg);
+                return 1;
+            }
+            break;
+        case 'd': strncpy(device, optarg, sizeof(device)-1); device[sizeof(device)-1] = '\0'; break;
+        case 'l': max_lin  = strtof(optarg, NULL);        break;
+        case 'a': max_ang  = strtof(optarg, NULL);        break;
+        case 'z': deadzone = strtof(optarg, NULL);        break;
+        case 't':
+            if (teleop_transport_parse_mode(optarg, &transport_mode) != 0) {
+                fprintf(stderr, "Invalid transport mode: %s (expected udp or kcp)\n", optarg);
+                return 1;
+            }
+            break;
+        case 's': strncpy(kcp_server, optarg, sizeof(kcp_server)-1); kcp_server[sizeof(kcp_server)-1] = '\0'; break;
+        case 'r': strncpy(kcp_relay, optarg, sizeof(kcp_relay)-1); kcp_relay[sizeof(kcp_relay)-1] = '\0'; break;
+        case 'I': strncpy(peer_id, optarg, sizeof(peer_id)-1); peer_id[sizeof(peer_id)-1] = '\0'; break;
+        case 'T': strncpy(target_peer, optarg, sizeof(target_peer)-1); target_peer[sizeof(target_peer)-1] = '\0'; break;
+        default:  usage(argv[0]); return (opt == 'h') ? 0 : 1;
+        }
+    }
+
+    if (deadzone < 0.0f || deadzone >= 1.0f) {
+        fprintf(stderr, "Invalid deadzone %.3f: expected 0 <= dz < 1\n", deadzone);
+        return 1;
+    }
+
+    signal(SIGINT, sigint_handler);
+
+    /* ── open joystick ───────────────────────────────────────────── */
+    int jsfd = open(device, O_RDONLY | O_NONBLOCK);
+    if (jsfd < 0) {
+        fprintf(stderr, "Cannot open %s: %s\n"
+                "  Hint: connect Xbox controller, check 'ls /dev/input/js*'\n",
+                device, strerror(errno));
+        return 1;
+    }
+
+    char js_name[128] = "Unknown";
+    ioctl(jsfd, JSIOCGNAME(sizeof(js_name)), js_name);
+
+    int num_axes = 0, num_buttons = 0;
+    ioctl(jsfd, JSIOCGAXES, &num_axes);
+    ioctl(jsfd, JSIOCGBUTTONS, &num_buttons);
+
+    printf("========================================\n");
+    printf("  Gamepad Teleop Controller\n");
+    printf("========================================\n");
+    printf("  Device : %s\n", device);
+    printf("  Name   : %s\n", js_name);
+    printf("  Axes   : %d   Buttons: %d\n", num_axes, num_buttons);
+    printf("  Transport: %s\n", teleop_transport_mode_name(transport_mode));
+    if (transport_mode == TELEOP_TRANSPORT_MODE_KCP) {
+        printf("  KCP server: %s\n", kcp_server);
+        if (kcp_relay[0] != '\0')
+            printf("  Relay via : %s\n", kcp_relay);
+        printf("  Peer ID   : %s -> %s\n", peer_id, target_peer);
+    } else {
+        printf("  Target : %s:%d\n", ip, port);
+    }
+    printf("  Linear : %.2f m/s   Angular: %.2f rad/s\n", max_lin, max_ang);
+    printf("  Deadzone: %.2f\n", deadzone);
+    printf("----------------------------------------\n");
+    printf("  Left stick  → forward/back + strafe\n");
+    printf("  Right stick → turn\n");
+    printf("  A button    → emergency stop\n");
+    printf("  B button    → quit\n");
+    printf("========================================\n\n");
+
+    memset(&transport_config, 0, sizeof(transport_config));
+    transport_config.mode = transport_mode;
+    transport_config.udp_ip = ip;
+    transport_config.udp_port = port;
+    transport_config.server_addr = kcp_server;
+    transport_config.relay_via = kcp_relay;
+    transport_config.peer_id = peer_id;
+    transport_config.target_peer = target_peer;
+
+    if (teleop_transport_open(&transport, &transport_config) != 0) {
+        close(jsfd);
+        return 1;
+    }
+
+    /* ── state ───────────────────────────────────────────────────── */
+    float axes[MAX_AXES];
+    int   buttons[MAX_BUTTONS];
+    memset(axes, 0, sizeof(axes));
+    memset(buttons, 0, sizeof(buttons));
+
+    twist_cmd_t cmd;
+    twist_cmd_zero(&cmd);
+
+    struct timeval last_send;
+    gettimeofday(&last_send, NULL);
+
+    int e_stop = 0;
+
+    /* ── main loop ───────────────────────────────────────────────── */
+    while (g_running) {
+        /* read all pending joystick events */
+        struct js_event ev;
+        while (read(jsfd, &ev, sizeof(ev)) == sizeof(ev)) {
+            ev.type &= ~JS_EVENT_INIT;   /* strip init flag */
+            if (ev.type == JS_EVENT_AXIS && ev.number < MAX_AXES) {
+                axes[ev.number] = (float)ev.value / JS_AXIS_MAX;
+            } else if (ev.type == JS_EVENT_BUTTON && ev.number < MAX_BUTTONS) {
+                buttons[ev.number] = ev.value;
+                if (ev.number == BTN_QUIT && ev.value) {
+                    g_running = 0;
+                    break;
+                }
+                if (ev.number == BTN_STOP && ev.value) {
+                    e_stop = !e_stop;
+                    if (e_stop)
+                        printf("\r  ** EMERGENCY STOP **                   ");
+                    else
+                        printf("\r  ** E-STOP released **                  ");
+                    fflush(stdout);
+                }
+            }
+        }
+        /* EAGAIN is expected in non-blocking mode */
+        if (errno != EAGAIN && errno != 0) {
+            perror("read joystick");
+            break;
+        }
+        errno = 0;
+
+        /* map axes → twist (skip if e-stopped) */
+        if (e_stop) {
+            twist_cmd_zero(&cmd);
+        } else {
+            float lx_raw = apply_deadzone(-axes[AXIS_LY], deadzone); /* Y inverted */
+            float ly_raw = apply_deadzone(-axes[AXIS_LX], deadzone);
+            float az_raw = apply_deadzone(-axes[AXIS_RX], deadzone);
+
+            cmd.lx = lx_raw * max_lin;
+            cmd.ly = ly_raw * max_lin;
+            cmd.lz = 0.0f;
+            cmd.ax = 0.0f;
+            cmd.ay = 0.0f;
+            cmd.az = az_raw * max_ang;
+        }
+
+        /* rate-limit sending */
+        struct timeval now;
+        gettimeofday(&now, NULL);
+        long elapsed = (now.tv_sec - last_send.tv_sec) * 1000000
+                     + (now.tv_usec - last_send.tv_usec);
+        if (elapsed < SEND_INTERVAL_US) {
+            usleep(5000);   /* 5 ms sleep to avoid busy-spin */
+            continue;
+        }
+        last_send = now;
+
+        teleop_transport_send_twist(&transport, &cmd);
+
+        printf("\r  cmd: lx=%+.2f ly=%+.2f az=%+.2f  |  raw: LY=%+.2f LX=%+.2f RX=%+.2f   ",
+               cmd.lx, cmd.ly, cmd.az,
+               axes[AXIS_LY], axes[AXIS_LX], axes[AXIS_RX]);
+        fflush(stdout);
+    }
+
+    /* send final stop */
+    twist_cmd_zero(&cmd);
+    teleop_transport_send_twist(&transport, &cmd);
+
+    close(jsfd);
+    teleop_transport_close(&transport);
+    printf("\nStopped.\n");
+    return 0;
+}
--- a/ros-control-c/remote/keyboard_controller.c
+++ b/ros-control-c/remote/keyboard_controller.c
@@ -0,0 +1,361 @@
+/*
+ * keyboard_controller.c - Keyboard teleop over UDP or KCP
+ *
+ * Keys:
+ *   W/Up    forward      S/Down  backward
+ *   A/Left  turn left    D/Right turn right
+ *   Q       strafe left  E       strafe right
+ *   Space   stop
+ *   [ / ]   linear speed down/up
+ *   - / =   angular speed down/up
+ *   Ctrl-C  quit
+ *
+ * Build:  gcc -Wall -O2 -I../common -o keyboard_controller keyboard_controller.c
+ * Usage:  ./keyboard_controller [-i IP] [-p PORT] [-l MAX_LIN] [-a MAX_ANG]
+ *                              [-t udp|kcp] [-s SERVER] [-r RELAY]
+ *                              [-I PEER_ID] [-T TARGET_PEER]
+ */
+
+#include <getopt.h>
+#include <signal.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/select.h>
+#include <sys/time.h>
+#include <termios.h>
+#include <unistd.h>
+
+#include "../common/protocol.h"
+#include "../common/teleop_transport.h"
+
+/*
+ * Terminals do not provide key-release events, so keep the last motion command
+ * alive briefly to bridge the initial auto-repeat delay while a key is held.
+ */
+#define KEY_HOLD_TIMEOUT_US 500000L
+
+static struct termios g_orig_termios;
+static volatile sig_atomic_t g_running = 1;
+
+static long elapsed_us(const struct timeval *start, const struct timeval *end)
+{
+    return (end->tv_sec - start->tv_sec) * 1000000L
+         + (end->tv_usec - start->tv_usec);
+}
+
+static int parse_port(const char *text, int *port_out)
+{
+    char *end = NULL;
+    long value = strtol(text, &end, 10);
+
+    if (end == text || *end != '\0' || value < 1 || value > 65535)
+        return -1;
+
+    *port_out = (int)value;
+    return 0;
+}
+
+static void restore_terminal(void)
+{
+    tcsetattr(STDIN_FILENO, TCSANOW, &g_orig_termios);
+    printf("\n\033[?25h");
+    fflush(stdout);
+}
+
+static void sigint_handler(int sig)
+{
+    (void)sig;
+    g_running = 0;
+}
+
+static void set_raw_mode(void)
+{
+    struct termios raw;
+    tcgetattr(STDIN_FILENO, &g_orig_termios);
+    atexit(restore_terminal);
+    raw = g_orig_termios;
+    raw.c_lflag &= ~(ICANON | ECHO | ISIG);
+    raw.c_cc[VMIN] = 0;
+    raw.c_cc[VTIME] = 0;
+    tcsetattr(STDIN_FILENO, TCSANOW, &raw);
+}
+
+static int read_key(long timeout_us)
+{
+    fd_set fds;
+    struct timeval tv;
+    unsigned char c;
+
+    FD_ZERO(&fds);
+    FD_SET(STDIN_FILENO, &fds);
+    tv.tv_sec = timeout_us / 1000000L;
+    tv.tv_usec = timeout_us % 1000000L;
+
+    if (select(STDIN_FILENO + 1, &fds, NULL, NULL, &tv) <= 0)
+        return -1;
+    if (read(STDIN_FILENO, &c, 1) != 1)
+        return -1;
+
+    if (c == 0x1B) {
+        unsigned char seq[2];
+
+        tv.tv_sec = 0;
+        tv.tv_usec = 20000;
+        FD_ZERO(&fds);
+        FD_SET(STDIN_FILENO, &fds);
+        if (select(STDIN_FILENO + 1, &fds, NULL, NULL, &tv) <= 0)
+            return 0x1B;
+        if (read(STDIN_FILENO, &seq[0], 1) != 1)
+            return 0x1B;
+
+        FD_ZERO(&fds);
+        FD_SET(STDIN_FILENO, &fds);
+        tv.tv_sec = 0;
+        tv.tv_usec = 20000;
+        if (select(STDIN_FILENO + 1, &fds, NULL, NULL, &tv) <= 0)
+            return 0x1B;
+        if (read(STDIN_FILENO, &seq[1], 1) != 1)
+            return 0x1B;
+
+        if (seq[0] == '[') {
+            switch (seq[1]) {
+            case 'A': return 'W';
+            case 'B': return 'S';
+            case 'D': return 'A';
+            case 'C': return 'D';
+            default: break;
+            }
+        }
+        return 0x1B;
+    }
+
+    if (c >= 'a' && c <= 'z')
+        c = (unsigned char)(c - ('a' - 'A'));
+    return c;
+}
+
+static void print_banner(void)
+{
+    printf("\033[2J\033[H");
+    printf("========================================\n");
+    printf("  Keyboard Teleop Controller\n");
+    printf("========================================\n");
+    printf("  W/Up    : forward      S/Down : back\n");
+    printf("  A/Left  : turn left    D/Right: turn right\n");
+    printf("  Q       : strafe left  E      : strafe right\n");
+    printf("  Space   : stop\n");
+    printf("  [ / ]   : linear speed  -/+\n");
+    printf("  - / =   : angular speed -/+\n");
+    printf("  Ctrl-C  : quit\n");
+    printf("========================================\n\n");
+}
+
+static void usage(const char *prog)
+{
+    fprintf(stderr,
+            "Usage: %s [options]\n"
+            "  -i IP       target IP   (default %s)\n"
+            "  -p PORT     target port (default %d)\n"
+            "  -l SPEED    max linear  speed m/s   (default 0.5)\n"
+            "  -a SPEED    max angular speed rad/s (default 0.5)\n"
+            "  -t MODE     transport mode udp|kcp  (default udp)\n"
+            "  -s ADDR     KCP server addr         (default %s)\n"
+            "  -r ADDR     KCP relay addr          (default none)\n"
+            "  -I ID       local KCP peer id       (default %s)\n"
+            "  -T ID       target KCP peer id      (default %s)\n"
+            "  -h          show help\n",
+            prog, DEFAULT_IP, DEFAULT_PORT,
+            DEFAULT_KCP_SERVER_ADDR,
+            DEFAULT_KCP_KEYBOARD_PEER_ID,
+            DEFAULT_KCP_TARGET_PEER_ID);
+}
+
+int main(int argc, char *argv[])
+{
+    char ip[64] = DEFAULT_IP;
+    int port = DEFAULT_PORT;
+    char kcp_server[OMNI_MAX_ADDR_TEXT] = DEFAULT_KCP_SERVER_ADDR;
+    char kcp_relay[OMNI_MAX_ADDR_TEXT] = "";
+    char peer_id[OMNI_MAX_PEER_ID] = DEFAULT_KCP_KEYBOARD_PEER_ID;
+    char target_peer[OMNI_MAX_PEER_ID] = DEFAULT_KCP_TARGET_PEER_ID;
+    float max_lin = 0.5f;
+    float max_ang = 0.5f;
+    const float speed_step = 0.1f;
+    teleop_transport_mode_t transport_mode = TELEOP_TRANSPORT_MODE_UDP;
+    teleop_transport_t transport;
+    teleop_transport_config_t transport_config;
+
+    int opt;
+    while ((opt = getopt(argc, argv, "i:p:l:a:t:s:r:I:T:h")) != -1) {
+        switch (opt) {
+        case 'i':
+            strncpy(ip, optarg, sizeof(ip) - 1);
+            ip[sizeof(ip) - 1] = '\0';
+            break;
+        case 'p':
+            if (parse_port(optarg, &port) != 0) {
+                fprintf(stderr, "Invalid port: %s (expected 1-65535)\n", optarg);
+                return 1;
+            }
+            break;
+        case 'l':
+            max_lin = strtof(optarg, NULL);
+            break;
+        case 'a':
+            max_ang = strtof(optarg, NULL);
+            break;
+        case 't':
+            if (teleop_transport_parse_mode(optarg, &transport_mode) != 0) {
+                fprintf(stderr, "Invalid transport mode: %s (expected udp or kcp)\n", optarg);
+                return 1;
+            }
+            break;
+        case 's':
+            strncpy(kcp_server, optarg, sizeof(kcp_server) - 1);
+            kcp_server[sizeof(kcp_server) - 1] = '\0';
+            break;
+        case 'r':
+            strncpy(kcp_relay, optarg, sizeof(kcp_relay) - 1);
+            kcp_relay[sizeof(kcp_relay) - 1] = '\0';
+            break;
+        case 'I':
+            strncpy(peer_id, optarg, sizeof(peer_id) - 1);
+            peer_id[sizeof(peer_id) - 1] = '\0';
+            break;
+        case 'T':
+            strncpy(target_peer, optarg, sizeof(target_peer) - 1);
+            target_peer[sizeof(target_peer) - 1] = '\0';
+            break;
+        default:
+            usage(argv[0]);
+            return (opt == 'h') ? 0 : 1;
+        }
+    }
+
+    memset(&transport_config, 0, sizeof(transport_config));
+    transport_config.mode = transport_mode;
+    transport_config.udp_ip = ip;
+    transport_config.udp_port = port;
+    transport_config.server_addr = kcp_server;
+    transport_config.relay_via = kcp_relay;
+    transport_config.peer_id = peer_id;
+    transport_config.target_peer = target_peer;
+
+    if (teleop_transport_open(&transport, &transport_config) != 0) {
+        return 1;
+    }
+
+    set_raw_mode();
+    signal(SIGINT, sigint_handler);
+    print_banner();
+    printf("  Transport: %s\n", teleop_transport_mode_name(transport_mode));
+    if (transport_mode == TELEOP_TRANSPORT_MODE_KCP) {
+        printf("  KCP server: %s\n", kcp_server);
+        if (kcp_relay[0] != '\0')
+            printf("  Relay via : %s\n", kcp_relay);
+        printf("  Peer ID   : %s -> %s\n", peer_id, target_peer);
+    } else {
+        printf("  Target: %s:%d\n", ip, port);
+    }
+    printf("  Linear: %.2f m/s   Angular: %.2f rad/s\n\n", max_lin, max_ang);
+    printf("\033[?25l");
+
+    twist_cmd_t cmd;
+    twist_cmd_zero(&cmd);
+
+    struct timeval last_send;
+    struct timeval last_motion_key;
+    gettimeofday(&last_send, NULL);
+    last_motion_key = last_send;
+
+    while (g_running) {
+        int key = read_key(SEND_INTERVAL_US);
+
+        if (key >= 0) {
+            twist_cmd_zero(&cmd);
+            switch (key) {
+            case 'W':
+                cmd.lx = max_lin;
+                gettimeofday(&last_motion_key, NULL);
+                break;
+            case 'S':
+                cmd.lx = -max_lin;
+                gettimeofday(&last_motion_key, NULL);
+                break;
+            case 'A':
+                cmd.az = max_ang;
+                gettimeofday(&last_motion_key, NULL);
+                break;
+            case 'D':
+                cmd.az = -max_ang;
+                gettimeofday(&last_motion_key, NULL);
+                break;
+            case 'Q':
+                cmd.ly = max_lin;
+                gettimeofday(&last_motion_key, NULL);
+                break;
+            case 'E':
+                cmd.ly = -max_lin;
+                gettimeofday(&last_motion_key, NULL);
+                break;
+            case ' ':
+                break;
+            case ']':
+                max_lin += speed_step;
+                printf("\r  Linear speed: %.2f m/s       ", max_lin);
+                fflush(stdout);
+                continue;
+            case '[':
+                max_lin = (max_lin > speed_step) ? max_lin - speed_step : speed_step;
+                printf("\r  Linear speed: %.2f m/s       ", max_lin);
+                fflush(stdout);
+                continue;
+            case '=':
+                max_ang += speed_step;
+                printf("\r  Angular speed: %.2f rad/s    ", max_ang);
+                fflush(stdout);
+                continue;
+            case '-':
+                max_ang = (max_ang > speed_step) ? max_ang - speed_step : speed_step;
+                printf("\r  Angular speed: %.2f rad/s    ", max_ang);
+                fflush(stdout);
+                continue;
+            case 0x03:
+                g_running = 0;
+                continue;
+            default:
+                continue;
+            }
+        } else {
+            struct timeval now;
+            gettimeofday(&now, NULL);
+            if (elapsed_us(&last_motion_key, &now) > KEY_HOLD_TIMEOUT_US)
+                twist_cmd_zero(&cmd);
+        }
+
+        {
+            struct timeval now;
+            long elapsed;
+
+            gettimeofday(&now, NULL);
+            elapsed = elapsed_us(&last_send, &now);
+            if (elapsed < SEND_INTERVAL_US)
+                continue;
+            last_send = now;
+        }
+
+        teleop_transport_send_twist(&transport, &cmd);
+
+        printf("\r  cmd: lx=%+.2f ly=%+.2f az=%+.2f  |  lin=%.2f ang=%.2f   ",
+               cmd.lx, cmd.ly, cmd.az, max_lin, max_ang);
+        fflush(stdout);
+    }
+
+    twist_cmd_zero(&cmd);
+    teleop_transport_send_twist(&transport, &cmd);
+
+    teleop_transport_close(&transport);
+    printf("\nStopped.\n");
+    return 0;
+}
--- a/ros-control-c/robot/udp_ros_bridge.py
+++ b/ros-control-c/robot/udp_ros_bridge.py
@@ -0,0 +1,247 @@
+#!/usr/bin/env python3
+"""
+udp_ros_bridge.py — UDP/KCP → ROS2 TwistStamped bridge
+
+Receives 24-byte binary twist commands from keyboard/gamepad controllers
+via UDP or OmniSocket/KCP and publishes geometry_msgs/msg/TwistStamped to
+/hric/robot/cmd_vel.
+
+Usage:
+    ros2 run <your_pkg> udp_ros_bridge      (if installed as a ROS2 package)
+    python3 udp_ros_bridge.py               (standalone)
+
+ROS2 parameters:
+    transport  (string) — udp or kcp           (default udp)
+    udp_port   (int)    — UDP listen port         (default 9870)
+    kcp_server (string) — KCP hub addr            (default 127.0.0.1:9002)
+    kcp_relay_via (string) — optional relay addr  (default "")
+    peer_id    (string) — local KCP peer id       (default ros-bridge-ctrl)
+    expected_sender (string) — optional sender filter (default "")
+    topic      (string) — publish topic            (default /hric/robot/cmd_vel)
+    frame_id   (string) — TwistStamped frame_id    (default pelvis)
+    timeout    (float)  — watchdog timeout seconds  (default 0.5)
+"""
+
+from pathlib import Path
+import struct
+import socket
+import sys
+import threading
+import time
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import TwistStamped
+
+TWIST_CMD_FMT = '<6f'           # 6 little-endian floats, 24 bytes
+TWIST_CMD_SIZE = struct.calcsize(TWIST_CMD_FMT)
+
+
+def _load_omnisocket():
+    try:
+        from omnisocket import CONTROL_DEFAULTS, MSG_TYPE_BINARY, MSG_TYPE_ERROR, Session
+        return CONTROL_DEFAULTS, MSG_TYPE_BINARY, MSG_TYPE_ERROR, Session
+    except ImportError:
+        root = Path(__file__).resolve().parents[2]
+        python_dir = root / 'python'
+        if str(python_dir) not in sys.path:
+            sys.path.insert(0, str(python_dir))
+        from omnisocket import CONTROL_DEFAULTS, MSG_TYPE_BINARY, MSG_TYPE_ERROR, Session
+        return CONTROL_DEFAULTS, MSG_TYPE_BINARY, MSG_TYPE_ERROR, Session
+
+
+class UdpTeleopBridge(Node):
+
+    def __init__(self):
+        super().__init__('udp_teleop_bridge')
+
+        # declare parameters
+        self.declare_parameter('transport', 'udp')
+        self.declare_parameter('udp_port', 9870)
+        self.declare_parameter('kcp_server', '127.0.0.1:9002')
+        self.declare_parameter('kcp_relay_via', '')
+        self.declare_parameter('peer_id', 'ros-bridge-ctrl')
+        self.declare_parameter('expected_sender', '')
+        self.declare_parameter('topic', '/hric/robot/cmd_vel')
+        self.declare_parameter('frame_id', 'pelvis')
+        self.declare_parameter('timeout', 0.5)
+
+        self._transport = str(self.get_parameter('transport').value).strip().lower()
+        self._port     = self.get_parameter('udp_port').value
+        self._kcp_server = str(self.get_parameter('kcp_server').value)
+        self._kcp_relay_via = str(self.get_parameter('kcp_relay_via').value)
+        self._peer_id = str(self.get_parameter('peer_id').value)
+        self._expected_sender = str(self.get_parameter('expected_sender').value)
+        self._topic    = self.get_parameter('topic').value
+        self._frame_id = self.get_parameter('frame_id').value
+        self._timeout  = self.get_parameter('timeout').value
+
+        if self._transport not in ('udp', 'kcp'):
+            raise ValueError(f"Unsupported transport '{self._transport}', expected 'udp' or 'kcp'")
+
+        # publisher
+        self._pub = self.create_publisher(TwistStamped, self._topic, 10)
+
+        # watchdog timer
+        self._last_recv = time.monotonic()
+        self._lock = threading.Lock()
+        self._latest_cmd = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+        self._timer = self.create_timer(1.0 / 20.0, self._timer_cb)
+        self._sock = None
+        self._session = None
+        self._msg_type_binary = None
+        self._msg_type_error = None
+        self._closing = False
+
+        if self._transport == 'kcp':
+            control_defaults, self._msg_type_binary, self._msg_type_error, session_cls = _load_omnisocket()
+            self._session = session_cls()
+            self._session.connect(
+                server_addr=self._kcp_server,
+                peer_id=self._peer_id,
+                relay_via=self._kcp_relay_via,
+                **control_defaults,
+            )
+        else:
+            self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+            self._sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+            self._sock.bind(('0.0.0.0', self._port))
+            self._sock.settimeout(0.1)
+
+        # receive thread
+        recv_target = self._recv_loop_kcp if self._transport == 'kcp' else self._recv_loop_udp
+        self._recv_thread = threading.Thread(target=recv_target, daemon=True)
+        self._recv_thread.start()
+
+        if self._transport == 'kcp':
+            self.get_logger().info(
+                f'Bridge ready — KCP {self._kcp_server} as {self._peer_id} → {self._topic} '
+                f'(frame_id={self._frame_id}, timeout={self._timeout}s)'
+            )
+        else:
+            self.get_logger().info(
+                f'Bridge ready — UDP 0.0.0.0:{self._port} → {self._topic} '
+                f'(frame_id={self._frame_id}, timeout={self._timeout}s)'
+            )
+
+    def _recv_loop_udp(self):
+        """Background thread: receive UDP packets and update latest command."""
+        while rclpy.ok():
+            try:
+                data, addr = self._sock.recvfrom(TWIST_CMD_SIZE + 64)
+            except socket.timeout:
+                continue
+            except OSError:
+                break
+
+            if len(data) != TWIST_CMD_SIZE:
+                self.get_logger().warn(
+                    f'Packet has invalid size {len(data)} bytes from {addr}, '
+                    f'expected {TWIST_CMD_SIZE}'
+                )
+                continue
+
+            values = struct.unpack(TWIST_CMD_FMT, data)
+            with self._lock:
+                self._latest_cmd = values
+                self._last_recv = time.monotonic()
+
+    def _recv_loop_kcp(self):
+        """Background thread: receive KCP packets and update latest command."""
+        while rclpy.ok():
+            try:
+                result = self._session.recv(timeout_ms=100)
+            except OSError as exc:
+                if not self._closing:
+                    self.get_logger().error(f'KCP receive failed: {exc}')
+                break
+
+            if result is None:
+                continue
+
+            from_peer, msg_type, payload = result
+
+            if msg_type == self._msg_type_error:
+                self.get_logger().error(
+                    f'KCP server error from {from_peer}: {payload.decode("utf-8", errors="replace")}'
+                )
+                continue
+
+            if self._expected_sender and from_peer != self._expected_sender:
+                self.get_logger().warn(
+                    f'Ignoring KCP packet from unexpected sender {from_peer}, '
+                    f'expected {self._expected_sender}'
+                )
+                continue
+
+            if msg_type != self._msg_type_binary:
+                self.get_logger().warn(
+                    f'Ignoring non-binary KCP message type {msg_type} from {from_peer}'
+                )
+                continue
+
+            if len(payload) != TWIST_CMD_SIZE:
+                self.get_logger().warn(
+                    f'KCP payload has invalid size {len(payload)} bytes from {from_peer}, '
+                    f'expected {TWIST_CMD_SIZE}'
+                )
+                continue
+
+            values = struct.unpack(TWIST_CMD_FMT, payload)
+            with self._lock:
+                self._latest_cmd = values
+                self._last_recv = time.monotonic()
+
+    def _timer_cb(self):
+        """20 Hz: publish TwistStamped from latest received command."""
+        with self._lock:
+            elapsed = time.monotonic() - self._last_recv
+            if elapsed > self._timeout:
+                lx, ly, lz, ax, ay, az = 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
+            else:
+                lx, ly, lz, ax, ay, az = self._latest_cmd
+
+        msg = TwistStamped()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = self._frame_id
+        msg.twist.linear.x  = float(lx)
+        msg.twist.linear.y  = float(ly)
+        msg.twist.linear.z  = float(lz)
+        msg.twist.angular.x = float(ax)
+        msg.twist.angular.y = float(ay)
+        msg.twist.angular.z = float(az)
+
+        self._pub.publish(msg)
+
+    def destroy_node(self):
+        self._closing = True
+        if self._sock is not None:
+            self._sock.close()
+            self._sock = None
+        if self._session is not None:
+            try:
+                self._session.close()
+            except OSError as exc:
+                self.get_logger().warn(f'Closing KCP session failed: {exc}')
+            self._session = None
+        if hasattr(self, '_recv_thread') and self._recv_thread.is_alive():
+            self._recv_thread.join(timeout=0.2)
+        super().destroy_node()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = None
+    try:
+        node = UdpTeleopBridge()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if node is not None:
+            node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()