feat: 脚本执行权限

This reverts commit 2f2c2008e7.

.claude/settings.local.json

@@ -11,7 +11,8 @@
       "Bash(git pull:*)",
       "Bash(wc:*)",
       "Bash(python3 -c \"import dis, marshal, types; f = open\\(''''C:/Users/64187/Desktop/Workspace/OmniSocketGo/__pycache__/omnisocket_video_sender.cpython-312.pyc'''',''''rb''''\\); f.read\\(16\\); code=marshal.load\\(f\\); dis.dis\\(code\\)\")",
-      "Bash(gh pr:*)"
+      "Bash(gh pr:*)",
+      "Bash(python3 -c ':*)"
     ]
   }
 }

.gitignore

@@ -17,4 +17,8 @@ c/bin
 /python/build
 /python/omnisocket.egg-info
 
-*.so*
+*.so*
+
+/.venv
+
+**/build/

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到某个远程端执行操作，一定要确保不做删除文件操作，确保留有记录

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到某个远程端执行操作，一定要确保不做删除文件操作，确保留有记录

Makefile

@@ -4,6 +4,10 @@ CPPFLAGS ?= -Iinclude -Ithird_party/cjson -Ithird_party/kcp
 LDFLAGS ?= -pthread
 PYTHON ?= python3
 
+ifeq ($(QUIET_FFMPEG_LOGS),1)
+CFLAGS += -DQUIET_FFMPEG_LOGS
+endif
+
 BIN_DIR := bin
 SRC_DIR := src
 CMD_DIR := cmd
@@ -37,8 +41,8 @@ TARGETS := \
 	$(BIN_DIR)/kcpping
 
 CAMERA_VIDEO_SENDER := $(BIN_DIR)/camera_video_sender
-CAMERA_VIDEO_SENDER_SRCS := \
+FFMPEG_PIPELINE_COMMON_SRCS := \
-	$(CMD_DIR)/v1_camera_pipeline_ifdef.c \
+	$(SRC_DIR)/video_pipeline.c \
 	$(SRC_DIR)/omni_common.c \
 	$(SRC_DIR)/protocol.c \
 	$(SRC_DIR)/latencylog.c \
@@ -50,19 +54,14 @@ CAMERA_VIDEO_SENDER_SRCS := \
 	third_party/cjson/cJSON.c \
 	third_party/kcp/ikcp.c
 
-B_SIDE_VIDEO_SENDER := $(BIN_DIR)/b_side_video_sender
+CAMERA_VIDEO_SENDER_SRCS := \
-B_SIDE_VIDEO_SENDER_SRCS := \
+	$(CMD_DIR)/v1_camera_pipeline_ifdef.c \
-	$(CMD_DIR)/b_side_video_sender.c \
+	$(FFMPEG_PIPELINE_COMMON_SRCS)
-	$(SRC_DIR)/omni_common.c \
+
-	$(SRC_DIR)/protocol.c \
+B_SIDE_OMNID := $(BIN_DIR)/b_side_omnid
-	$(SRC_DIR)/latencylog.c \
+B_SIDE_OMNID_SRCS := \
-	$(SRC_DIR)/kcp_packet_debug.c \
+	$(CMD_DIR)/b_side_omnid.c \
-	$(SRC_DIR)/kcp_session_stats.c \
+	$(FFMPEG_PIPELINE_COMMON_SRCS)
-	$(SRC_DIR)/linux_timestamping.c \
-	$(SRC_DIR)/transport_kcp.c \
-	$(SRC_DIR)/peer_kcp_client.c \
-	third_party/cjson/cJSON.c \
-	third_party/kcp/ikcp.c
 
 all: $(TARGETS)
 
@@ -95,10 +94,10 @@ $(CAMERA_VIDEO_SENDER): $(CAMERA_VIDEO_SENDER_SRCS) | $(BIN_DIR)
 
 camera_video_sender: $(CAMERA_VIDEO_SENDER)
 
-$(B_SIDE_VIDEO_SENDER): $(B_SIDE_VIDEO_SENDER_SRCS) | $(BIN_DIR)
+$(B_SIDE_OMNID): $(B_SIDE_OMNID_SRCS) | $(BIN_DIR)
 	$(CC) $(CFLAGS) $(CPPFLAGS) $$(pkg-config --cflags libavformat libavcodec libavutil libswscale) -o $@ $^ $(LDFLAGS) $$(pkg-config --libs libavformat libavcodec libavutil libswscale)
 
-b_side_video_sender: $(B_SIDE_VIDEO_SENDER)
+b_side_omnid: $(B_SIDE_OMNID)
 
 clean:
 	rm -rf $(BIN_DIR)
@@ -109,4 +108,4 @@ python-ext:
 python-install:
 	cd python && $(PYTHON) -m pip install -e .
 
-.PHONY: all clean python-ext python-install camera_video_sender b_side_video_sender
+.PHONY: all clean python-ext python-install camera_video_sender b_side_omnid

README.md

@@ -27,36 +27,13 @@ make python-ext
 make python-install
 ```
 
-## A-Side OmniDaemon
-
-The A-side daemon is configured from `config/a_side_omnidaemon.yaml` in this repo. The safest way to start it is to pass that file explicitly, because the installed Python package does not bundle the YAML config.
-
-Run from a source checkout:
-
-```bash
-python -m omnisocket_a_side.daemon --config "$(pwd)/config/a_side_omnidaemon.yaml"
-```
-
-Or, if you installed the console script:
-
-```bash
-OMNIDAEMON_CONFIG="$(pwd)/config/a_side_omnidaemon.yaml" \
-omnisocket-a-side-daemon
-```
-
-Optional overrides:
-
-- `OMNIDAEMON_SOCKET=/tmp/omnisocket-a-side.sock` selects the local UDS path.
-- `OMNIDAEMON_CONFIG=/abs/path/to/a_side_omnidaemon.yaml` overrides `--config`.
-
-For `robot-command-center` and the A-side senders, keep the daemon and its clients on the same Linux machine so they can share the Unix-domain socket.
-
 ## Run On Different Machines
 
 Server `D` runs the KCP hub on `0.0.0.0:10909`:
 
 ```bash
 ./bin/kcpserver -listen 0.0.0.0:10909 \
+  -telemetry-peer peer-a-telemetry \
   -kcp-ts-debug-log logs/d-kcp-ts.jsonl \
   -kcp-session-stats-log logs/d-kcp-stats.jsonl
 ```

cmd/b_side_omnid.c

@@ -0,0 +1,428 @@
+#include <errno.h>
+#include <pthread.h>
+#include <signal.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <sys/socket.h>
+#include <sys/un.h>
+#include <unistd.h>
+
+#include "control_protocol.h"
+#include "video_pipeline.h"
+
+#define CONTROL_DEFAULT_PEER_ID "peer-b-ctrl"
+#define CONTROL_DEFAULT_EXPECTED_SENDER "peer-a-ctrl"
+#define CONTROL_DEFAULT_UNIX_SOCKET "/tmp/omnisocket-b-side-cmd.sock"
+
+typedef struct unix_dgram_client {
+    int fd;
+    char bind_path[108];
+    struct sockaddr_un dest_addr;
+    socklen_t dest_len;
+} unix_dgram_client_t;
+
+typedef struct control_bridge_stats {
+    pthread_mutex_t mutex;
+    uint64_t packets_forwarded;
+    uint64_t invalid_packets;
+    uint64_t unix_send_errors;
+    int connected;
+    char last_error[256];
+    kcp_runtime_stats_t transport;
+} control_bridge_stats_t;
+
+typedef struct daemon_state {
+    volatile sig_atomic_t *stop_requested;
+    video_pipeline_config_t video_config;
+    video_pipeline_stats_t video_stats;
+    const char *control_server_addr;
+    const char *control_relay_via;
+    const char *control_bind_ip;
+    const char *control_bind_device;
+    const char *control_peer_id;
+    const char *control_expected_sender;
+    const char *control_unix_socket;
+    unix_dgram_client_t unix_client;
+    control_bridge_stats_t control_stats;
+} daemon_state_t;
+
+static volatile sig_atomic_t g_stop_requested = 0;
+
+static void handle_signal(int signum) {
+    (void) signum;
+    g_stop_requested = 1;
+}
+
+static int install_signal_handler(int signum) {
+    struct sigaction action;
+
+    memset(&action, 0, sizeof(action));
+    action.sa_handler = handle_signal;
+    action.sa_flags = SA_RESTART;
+    if (sigemptyset(&action.sa_mask) != 0) {
+        return -1;
+    }
+    return sigaction(signum, &action, NULL);
+}
+
+static const char *env_or_default(const char *name, const char *fallback) {
+    const char *value = getenv(name);
+    if (value != NULL && value[0] != '\0') {
+        return value;
+    }
+    return fallback;
+}
+
+static const char *env_first_nonempty(const char *first, const char *second, const char *fallback) {
+    const char *value = getenv(first);
+    if (value != NULL && value[0] != '\0') {
+        return value;
+    }
+    value = getenv(second);
+    if (value != NULL && value[0] != '\0') {
+        return value;
+    }
+    return fallback;
+}
+
+static int control_bridge_stats_init(control_bridge_stats_t *stats) {
+    int rc;
+    if (stats == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+    memset(stats, 0, sizeof(*stats));
+    rc = pthread_mutex_init(&stats->mutex, NULL);
+    if (rc != 0) {
+        errno = rc;
+        return -1;
+    }
+    return 0;
+}
+
+static void control_bridge_stats_destroy(control_bridge_stats_t *stats) {
+    if (stats == NULL) {
+        return;
+    }
+    pthread_mutex_destroy(&stats->mutex);
+}
+
+static void control_bridge_set_error(control_bridge_stats_t *stats, const char *message) {
+    if (stats == NULL) {
+        return;
+    }
+    pthread_mutex_lock(&stats->mutex);
+    snprintf(stats->last_error, sizeof(stats->last_error), "%s", message == NULL ? "" : message);
+    pthread_mutex_unlock(&stats->mutex);
+}
+
+static void control_bridge_set_errno_error(control_bridge_stats_t *stats, const char *prefix) {
+    char buffer[256];
+    int saved_errno = errno;
+
+    snprintf(
+        buffer,
+        sizeof(buffer),
+        "%s: %s (errno=%d)",
+        prefix == NULL ? "control bridge error" : prefix,
+        saved_errno != 0 ? strerror(saved_errno) : "unknown error",
+        saved_errno
+    );
+    control_bridge_set_error(stats, buffer);
+}
+
+static void control_bridge_stats_snapshot(control_bridge_stats_t *stats, control_bridge_stats_t *out_stats) {
+    if (stats == NULL || out_stats == NULL) {
+        return;
+    }
+    memset(out_stats, 0, sizeof(*out_stats));
+    pthread_mutex_lock(&stats->mutex);
+    out_stats->packets_forwarded = stats->packets_forwarded;
+    out_stats->invalid_packets = stats->invalid_packets;
+    out_stats->unix_send_errors = stats->unix_send_errors;
+    out_stats->connected = stats->connected;
+    snprintf(out_stats->last_error, sizeof(out_stats->last_error), "%s", stats->last_error);
+    out_stats->transport = stats->transport;
+    pthread_mutex_unlock(&stats->mutex);
+}
+
+static int unix_dgram_client_init(unix_dgram_client_t *client, const char *dest_path) {
+    struct sockaddr_un bind_addr;
+    pid_t pid;
+
+    if (client == NULL || dest_path == NULL || dest_path[0] == '\0') {
+        errno = EINVAL;
+        return -1;
+    }
+
+    memset(client, 0, sizeof(*client));
+    client->fd = socket(AF_UNIX, SOCK_DGRAM, 0);
+    if (client->fd < 0) {
+        return -1;
+    }
+
+    memset(&bind_addr, 0, sizeof(bind_addr));
+    bind_addr.sun_family = AF_UNIX;
+    pid = getpid();
+    snprintf(client->bind_path, sizeof(client->bind_path), "/tmp/omnisocket-b-side-cmd-client-%ld.sock", (long) pid);
+    unlink(client->bind_path);
+    snprintf(bind_addr.sun_path, sizeof(bind_addr.sun_path), "%s", client->bind_path);
+    if (bind(client->fd, (const struct sockaddr *) &bind_addr, sizeof(bind_addr)) != 0) {
+        close(client->fd);
+        unlink(client->bind_path);
+        client->fd = -1;
+        return -1;
+    }
+
+    memset(&client->dest_addr, 0, sizeof(client->dest_addr));
+    client->dest_addr.sun_family = AF_UNIX;
+    snprintf(client->dest_addr.sun_path, sizeof(client->dest_addr.sun_path), "%s", dest_path);
+    client->dest_len = (socklen_t) sizeof(client->dest_addr);
+    return 0;
+}
+
+static int unix_dgram_client_send(unix_dgram_client_t *client, const void *data, size_t len) {
+    ssize_t written;
+    if (client == NULL || client->fd < 0 || (data == NULL && len > 0)) {
+        errno = EINVAL;
+        return -1;
+    }
+    written = sendto(client->fd, data, len, 0, (const struct sockaddr *) &client->dest_addr, client->dest_len);
+    if (written < 0 || (size_t) written != len) {
+        if (written >= 0) {
+            errno = EIO;
+        }
+        return -1;
+    }
+    return 0;
+}
+
+static void unix_dgram_client_close(unix_dgram_client_t *client) {
+    if (client == NULL) {
+        return;
+    }
+    if (client->fd >= 0) {
+        close(client->fd);
+        client->fd = -1;
+    }
+    if (client->bind_path[0] != '\0') {
+        unlink(client->bind_path);
+        client->bind_path[0] = '\0';
+    }
+}
+
+static void *video_thread_main(void *arg) {
+    daemon_state_t *state = (daemon_state_t *) arg;
+
+    while (!*state->stop_requested) {
+        if (video_pipeline_run(&state->video_config, &state->video_stats, state->stop_requested) == 0) {
+            break;
+        }
+        if (!*state->stop_requested) {
+            sleep(1);
+        }
+    }
+    return NULL;
+}
+
+static void *control_thread_main(void *arg) {
+    daemon_state_t *state = (daemon_state_t *) arg;
+
+    while (!*state->stop_requested) {
+        kcp_conn_options_t options;
+        kcp_client_t *client = NULL;
+
+        kcp_conn_options_set_control_defaults(&options);
+        client = kcp_client_dial_with_options(
+            state->control_server_addr,
+            state->control_relay_via,
+            state->control_peer_id,
+            state->control_bind_ip,
+            state->control_bind_device,
+            &options,
+            NULL,
+            NULL,
+            NULL,
+            KCP_DEFAULT_STATS_INTERVAL_MS
+        );
+        if (client == NULL) {
+            control_bridge_set_errno_error(&state->control_stats, "failed to connect control session");
+            sleep(1);
+            continue;
+        }
+
+        pthread_mutex_lock(&state->control_stats.mutex);
+        state->control_stats.connected = 1;
+        state->control_stats.last_error[0] = '\0';
+        pthread_mutex_unlock(&state->control_stats.mutex);
+
+        while (!*state->stop_requested) {
+            message_t msg;
+            int rc;
+
+            protocol_message_init(&msg);
+            rc = kcp_client_receive_timed(client, &msg, 100);
+            if (rc == 1) {
+                protocol_message_clear(&msg);
+                continue;
+            }
+            if (rc != 0) {
+                control_bridge_set_errno_error(&state->control_stats, "control receive loop stopped");
+                protocol_message_clear(&msg);
+                break;
+            }
+
+            if (state->control_expected_sender[0] != '\0' && strcmp(msg.from, state->control_expected_sender) != 0) {
+                pthread_mutex_lock(&state->control_stats.mutex);
+                state->control_stats.invalid_packets += 1;
+                pthread_mutex_unlock(&state->control_stats.mutex);
+                protocol_message_clear(&msg);
+                continue;
+            }
+
+            if (msg.type != MSG_TYPE_BINARY || msg.body_len != OMNI_CONTROL_PACKET_SIZE) {
+                pthread_mutex_lock(&state->control_stats.mutex);
+                state->control_stats.invalid_packets += 1;
+                pthread_mutex_unlock(&state->control_stats.mutex);
+                protocol_message_clear(&msg);
+                continue;
+            }
+
+            if (unix_dgram_client_send(&state->unix_client, msg.body, msg.body_len) != 0) {
+                pthread_mutex_lock(&state->control_stats.mutex);
+                state->control_stats.unix_send_errors += 1;
+                pthread_mutex_unlock(&state->control_stats.mutex);
+                control_bridge_set_errno_error(&state->control_stats, "failed to forward command to unix socket");
+                protocol_message_clear(&msg);
+                continue;
+            }
+
+            pthread_mutex_lock(&state->control_stats.mutex);
+            state->control_stats.packets_forwarded += 1;
+            kcp_client_runtime_stats_snapshot(client, &state->control_stats.transport);
+            pthread_mutex_unlock(&state->control_stats.mutex);
+            protocol_message_clear(&msg);
+        }
+
+        pthread_mutex_lock(&state->control_stats.mutex);
+        state->control_stats.connected = 0;
+        pthread_mutex_unlock(&state->control_stats.mutex);
+        kcp_client_close(client);
+        kcp_client_free(client);
+        if (!*state->stop_requested) {
+            sleep(1);
+        }
+    }
+
+    return NULL;
+}
+
+static void print_stats(daemon_state_t *state) {
+    video_pipeline_stats_t video_stats;
+    control_bridge_stats_t control_stats;
+
+    memset(&video_stats, 0, sizeof(video_stats));
+    memset(&control_stats, 0, sizeof(control_stats));
+    video_pipeline_stats_snapshot(&state->video_stats, &video_stats);
+    control_bridge_stats_snapshot(&state->control_stats, &control_stats);
+
+    fprintf(
+        stderr,
+        "[b_side_omnid] video connected=%d frames=%llu bytes=%llu srtt=%dms | control connected=%d forwarded=%llu invalid=%llu unix_err=%llu srtt=%dms\n",
+        video_stats.connected,
+        (unsigned long long) video_stats.frames_sent,
+        (unsigned long long) video_stats.bytes_sent,
+        video_stats.transport.srtt_ms,
+        control_stats.connected,
+        (unsigned long long) control_stats.packets_forwarded,
+        (unsigned long long) control_stats.invalid_packets,
+        (unsigned long long) control_stats.unix_send_errors,
+        control_stats.transport.srtt_ms
+    );
+}
+
+int main(void) {
+    daemon_state_t state;
+    pthread_t video_thread;
+    pthread_t control_thread;
+
+    memset(&state, 0, sizeof(state));
+    state.stop_requested = &g_stop_requested;
+
+    video_pipeline_config_init(&state.video_config);
+    video_pipeline_config_load_env(&state.video_config);
+    state.control_server_addr = env_first_nonempty("OMNI_CONTROL_SERVER_ADDR", "OMNISOCKET_SERVER_ADDR", "");
+    state.control_relay_via = env_first_nonempty("OMNI_CONTROL_RELAY_VIA", "OMNISOCKET_RELAY_VIA", "");
+    state.control_bind_ip = env_first_nonempty("OMNI_CONTROL_BIND_IP", "OMNISOCKET_BIND_IP", "");
+    state.control_bind_device = env_first_nonempty("OMNI_CONTROL_BIND_DEVICE", "OMNISOCKET_BIND_DEVICE", "");
+    state.control_peer_id = env_or_default("OMNI_CONTROL_PEER_ID", CONTROL_DEFAULT_PEER_ID);
+    state.control_expected_sender = env_or_default("OMNI_CONTROL_EXPECTED_SENDER", CONTROL_DEFAULT_EXPECTED_SENDER);
+    state.control_unix_socket = env_or_default("OMNI_CONTROL_UNIX_SOCKET_PATH", CONTROL_DEFAULT_UNIX_SOCKET);
+
+    if (state.video_config.server_addr == NULL || state.video_config.server_addr[0] == '\0' ||
+        state.control_server_addr == NULL || state.control_server_addr[0] == '\0') {
+        fprintf(stderr, "OMNISOCKET_SERVER_ADDR (or session-specific overrides) is required\n");
+        return 1;
+    }
+
+    if (video_pipeline_stats_init(&state.video_stats) != 0) {
+        perror("video_pipeline_stats_init");
+        return 1;
+    }
+    if (control_bridge_stats_init(&state.control_stats) != 0) {
+        perror("control_bridge_stats_init");
+        video_pipeline_stats_destroy(&state.video_stats);
+        return 1;
+    }
+    if (unix_dgram_client_init(&state.unix_client, state.control_unix_socket) != 0) {
+        perror("unix_dgram_client_init");
+        control_bridge_stats_destroy(&state.control_stats);
+        video_pipeline_stats_destroy(&state.video_stats);
+        return 1;
+    }
+
+    fprintf(
+        stderr,
+        "[b_side_omnid] control forwarding target is unix_dgram://%s\n",
+        state.control_unix_socket
+    );
+
+    if (install_signal_handler(SIGINT) != 0 || install_signal_handler(SIGTERM) != 0) {
+        perror("install_signal_handler");
+        unix_dgram_client_close(&state.unix_client);
+        control_bridge_stats_destroy(&state.control_stats);
+        video_pipeline_stats_destroy(&state.video_stats);
+        return 1;
+    }
+
+    if (pthread_create(&video_thread, NULL, video_thread_main, &state) != 0) {
+        perror("pthread_create(video_thread)");
+        unix_dgram_client_close(&state.unix_client);
+        control_bridge_stats_destroy(&state.control_stats);
+        video_pipeline_stats_destroy(&state.video_stats);
+        return 1;
+    }
+    if (pthread_create(&control_thread, NULL, control_thread_main, &state) != 0) {
+        perror("pthread_create(control_thread)");
+        g_stop_requested = 1;
+        pthread_join(video_thread, NULL);
+        unix_dgram_client_close(&state.unix_client);
+        control_bridge_stats_destroy(&state.control_stats);
+        video_pipeline_stats_destroy(&state.video_stats);
+        return 1;
+    }
+
+    while (!g_stop_requested) {
+        sleep(1);
+        print_stats(&state);
+    }
+
+    pthread_join(video_thread, NULL);
+    pthread_join(control_thread, NULL);
+    unix_dgram_client_close(&state.unix_client);
+    control_bridge_stats_destroy(&state.control_stats);
+    video_pipeline_stats_destroy(&state.video_stats);
+    return 0;
+}

cmd/b_side_video_sender.c

@@ -1,860 +0,0 @@
-#include <errno.h>
-#include <fcntl.h>
-#include <inttypes.h>
-#include <linux/videodev2.h>
-#include <pthread.h>
-#include <signal.h>
-#include <stdbool.h>
-#include <stdint.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <sys/ioctl.h>
-#include <sys/mman.h>
-#include <sys/select.h>
-#include <time.h>
-#include <unistd.h>
-
-#include <libavcodec/avcodec.h>
-#include <libavutil/avutil.h>
-#include <libavutil/imgutils.h>
-#include <libavutil/opt.h>
-#include <libswscale/swscale.h>
-
-#include "cJSON.h"
-#include "peer_kcp_client.h"
-
-#define WORKER_CONTROL_FD 3
-#define WORKER_TELEMETRY_FD 4
-#define NUM_BUFFERS 4
-#define CLEAR(x) memset(&(x), 0, sizeof(x))
-
-#define VIDEO_SERVER_ADDR_ENV "OMNI_VIDEO_SERVER_ADDR"
-#define VIDEO_RELAY_ADDR_ENV "OMNI_VIDEO_RELAY_VIA"
-#define VIDEO_BIND_IP_ENV "OMNI_VIDEO_BIND_IP"
-#define VIDEO_BIND_DEVICE_ENV "OMNI_VIDEO_BIND_DEVICE"
-#define VIDEO_PEER_ID_ENV "OMNI_VIDEO_PEER_ID"
-#define VIDEO_TARGET_PEER_ENV "OMNI_VIDEO_TARGET_PEER"
-#define VIDEO_DEVICE_ENV "OMNI_VIDEO_DEVICE"
-#define VIDEO_CAPTURE_WIDTH_ENV "OMNI_VIDEO_CAPTURE_WIDTH"
-#define VIDEO_CAPTURE_HEIGHT_ENV "OMNI_VIDEO_CAPTURE_HEIGHT"
-#define VIDEO_OUTPUT_WIDTH_ENV "OMNI_VIDEO_OUTPUT_WIDTH"
-#define VIDEO_OUTPUT_HEIGHT_ENV "OMNI_VIDEO_OUTPUT_HEIGHT"
-#define VIDEO_FPS_ENV "OMNI_VIDEO_FPS"
-#define VIDEO_JPEG_QSCALE_ENV "OMNI_VIDEO_JPEG_QSCALE"
-#define VIDEO_MAX_FRAME_BYTES_ENV "OMNI_VIDEO_MAX_FRAME_BYTES"
-#define VIDEO_STATS_INTERVAL_ENV "OMNI_VIDEO_STATS_INTERVAL_MS"
-
-typedef struct {
-    void *start;
-    size_t length;
-} Buffer;
-
-typedef struct {
-    char server_addr[OMNI_MAX_ADDR_TEXT];
-    char relay_addr[OMNI_MAX_ADDR_TEXT];
-    char bind_ip[OMNI_MAX_ADDR_TEXT];
-    char bind_device[128];
-    char peer_id[OMNI_MAX_PEER_ID];
-    char target_peer[OMNI_MAX_PEER_ID];
-    char video_device[256];
-    int capture_width;
-    int capture_height;
-    int output_width;
-    int output_height;
-    int initial_fps;
-    int initial_qscale;
-    int initial_max_frame_bytes;
-    int stats_interval_ms;
-} worker_config_t;
-
-typedef struct {
-    pthread_mutex_t lock;
-    pthread_mutex_t telemetry_lock;
-    FILE *telemetry_stream;
-    int target_fps;
-    int jpeg_quality_qscale;
-    int max_frame_bytes;
-    bool shutdown_requested;
-} runtime_state_t;
-
-typedef struct {
-    kcp_client_t *client;
-    char target_peer[OMNI_MAX_PEER_ID];
-} video_sender_t;
-
-typedef struct {
-    FILE *control_stream;
-    runtime_state_t *runtime;
-} control_thread_args_t;
-
-static volatile sig_atomic_t g_signal_stop = 0;
-
-static double monotonic_ms(void) {
-    struct timespec ts;
-    clock_gettime(CLOCK_MONOTONIC, &ts);
-    return (double) ts.tv_sec * 1000.0 + (double) ts.tv_nsec / 1000000.0;
-}
-
-static int64_t realtime_ms(void) {
-    struct timespec ts;
-    clock_gettime(CLOCK_REALTIME, &ts);
-    return (int64_t) ts.tv_sec * 1000 + (int64_t) ts.tv_nsec / 1000000;
-}
-
-static int env_as_int(const char *name, int fallback) {
-    const char *raw = getenv(name);
-    char *endptr = NULL;
-    long value;
-
-    if (raw == NULL || raw[0] == '\0') {
-        return fallback;
-    }
-    errno = 0;
-    value = strtol(raw, &endptr, 10);
-    if (errno != 0 || endptr == raw || (endptr != NULL && *endptr != '\0')) {
-        return fallback;
-    }
-    return (int) value;
-}
-
-static const char *env_or_default(const char *name, const char *fallback) {
-    const char *value = getenv(name);
-    if (value != NULL && value[0] != '\0') {
-        return value;
-    }
-    return fallback;
-}
-
-static void handle_signal(int signum) {
-    (void) signum;
-    g_signal_stop = 1;
-}
-
-static void runtime_set_shutdown(runtime_state_t *runtime, bool shutdown_requested) {
-    pthread_mutex_lock(&runtime->lock);
-    runtime->shutdown_requested = shutdown_requested;
-    pthread_mutex_unlock(&runtime->lock);
-}
-
-static bool runtime_should_stop(runtime_state_t *runtime) {
-    bool shutdown_requested;
-
-    pthread_mutex_lock(&runtime->lock);
-    shutdown_requested = runtime->shutdown_requested;
-    pthread_mutex_unlock(&runtime->lock);
-    return g_signal_stop != 0 || shutdown_requested;
-}
-
-static void runtime_get_profile(runtime_state_t *runtime, int *fps, int *qscale, int *max_frame_bytes) {
-    pthread_mutex_lock(&runtime->lock);
-    *fps = runtime->target_fps;
-    *qscale = runtime->jpeg_quality_qscale;
-    *max_frame_bytes = runtime->max_frame_bytes;
-    pthread_mutex_unlock(&runtime->lock);
-}
-
-static void runtime_set_profile(runtime_state_t *runtime, int fps, int qscale, int max_frame_bytes) {
-    pthread_mutex_lock(&runtime->lock);
-    if (fps > 0) {
-        runtime->target_fps = fps;
-    }
-    if (qscale > 0) {
-        runtime->jpeg_quality_qscale = qscale;
-    }
-    if (max_frame_bytes > 0) {
-        runtime->max_frame_bytes = max_frame_bytes;
-    }
-    pthread_mutex_unlock(&runtime->lock);
-}
-
-static void telemetry_write_line(runtime_state_t *runtime, const char *line) {
-    pthread_mutex_lock(&runtime->telemetry_lock);
-    if (runtime->telemetry_stream != NULL) {
-        fputs(line, runtime->telemetry_stream);
-        fputc('\n', runtime->telemetry_stream);
-        fflush(runtime->telemetry_stream);
-    }
-    pthread_mutex_unlock(&runtime->telemetry_lock);
-}
-
-static void telemetry_write_frame_stat(
-    runtime_state_t *runtime,
-    int frame_bytes,
-    int encode_us,
-    bool sent,
-    const char *drop_reason
-) {
-    char line[512];
-    int written;
-
-    written = snprintf(
-        line,
-        sizeof(line),
-        "{\"type\":\"frame_stat\",\"frame_bytes\":%d,\"encode_us\":%d,"
-        "\"sent\":%s,\"drop_reason\":\"%s\",\"ts_unix_ms\":%" PRId64 "}",
-        frame_bytes,
-        encode_us,
-        sent ? "true" : "false",
-        drop_reason != NULL ? drop_reason : "",
-        realtime_ms());
-    if (written > 0 && written < (int) sizeof(line)) {
-        telemetry_write_line(runtime, line);
-    }
-}
-
-static void telemetry_write_kcp_metrics(runtime_state_t *runtime, const kcp_conn_metrics_t *metrics) {
-    char line[1024];
-    int written;
-
-    written = snprintf(
-        line,
-        sizeof(line),
-        "{\"type\":\"kcp_metrics\",\"connected\":%d,\"srtt_ms\":%d,\"srttvar_ms\":%d,"
-        "\"rto_ms\":%u,\"bytes_sent\":%" PRIu64 ",\"bytes_received\":%" PRIu64 ","
-        "\"out_pkts\":%" PRIu64 ",\"out_segs\":%" PRIu64 ",\"retrans_segs\":%" PRIu64 ","
-        "\"fast_retrans_segs\":%" PRIu64 ",\"early_retrans_segs\":%" PRIu64 ","
-        "\"lost_segs\":%" PRIu64 ",\"ring_buffer_snd_queue\":%" PRIu64 ","
-        "\"ring_buffer_snd_buffer\":%" PRIu64 ",\"ts_unix_ms\":%" PRId64 "}",
-        metrics->connected,
-        metrics->srtt_ms,
-        metrics->srttvar_ms,
-        metrics->rto_ms,
-        metrics->bytes_sent,
-        metrics->bytes_received,
-        metrics->out_pkts,
-        metrics->out_segs,
-        metrics->retrans_segs,
-        metrics->fast_retrans_segs,
-        metrics->early_retrans_segs,
-        metrics->lost_segs,
-        metrics->ring_buffer_snd_queue,
-        metrics->ring_buffer_snd_buffer,
-        realtime_ms());
-    if (written > 0 && written < (int) sizeof(line)) {
-        telemetry_write_line(runtime, line);
-    }
-}
-
-static int load_worker_config(worker_config_t *cfg) {
-    const char *server_addr = getenv(VIDEO_SERVER_ADDR_ENV);
-
-    if (cfg == NULL) {
-        errno = EINVAL;
-        return -1;
-    }
-    if (server_addr == NULL || server_addr[0] == '\0') {
-        fprintf(stderr, "%s is required\n", VIDEO_SERVER_ADDR_ENV);
-        errno = EINVAL;
-        return -1;
-    }
-
-    CLEAR(*cfg);
-    snprintf(cfg->server_addr, sizeof(cfg->server_addr), "%s", server_addr);
-    snprintf(cfg->relay_addr, sizeof(cfg->relay_addr), "%s", env_or_default(VIDEO_RELAY_ADDR_ENV, ""));
-    snprintf(cfg->bind_ip, sizeof(cfg->bind_ip), "%s", env_or_default(VIDEO_BIND_IP_ENV, ""));
-    snprintf(cfg->bind_device, sizeof(cfg->bind_device), "%s", env_or_default(VIDEO_BIND_DEVICE_ENV, ""));
-    snprintf(cfg->peer_id, sizeof(cfg->peer_id), "%s", env_or_default(VIDEO_PEER_ID_ENV, "peer-b-video"));
-    snprintf(cfg->target_peer, sizeof(cfg->target_peer), "%s", env_or_default(VIDEO_TARGET_PEER_ENV, "peer-a-video"));
-    snprintf(cfg->video_device, sizeof(cfg->video_device), "%s", env_or_default(VIDEO_DEVICE_ENV, "/dev/video0"));
-
-    cfg->capture_width = env_as_int(VIDEO_CAPTURE_WIDTH_ENV, 1280);
-    cfg->capture_height = env_as_int(VIDEO_CAPTURE_HEIGHT_ENV, 720);
-    cfg->output_width = env_as_int(VIDEO_OUTPUT_WIDTH_ENV, 640);
-    cfg->output_height = env_as_int(VIDEO_OUTPUT_HEIGHT_ENV, 360);
-    cfg->initial_fps = env_as_int(VIDEO_FPS_ENV, 10);
-    cfg->initial_qscale = env_as_int(VIDEO_JPEG_QSCALE_ENV, 8);
-    cfg->initial_max_frame_bytes = env_as_int(VIDEO_MAX_FRAME_BYTES_ENV, 40960);
-    cfg->stats_interval_ms = env_as_int(VIDEO_STATS_INTERVAL_ENV, 100);
-    return 0;
-}
-
-static int open_v4l2_device(const char *device) {
-    int fd = open(device, O_RDWR | O_NONBLOCK);
-    if (fd < 0) {
-        perror("open device");
-    }
-    return fd;
-}
-
-static int init_v4l2_device(int fd, const worker_config_t *cfg) {
-    struct v4l2_format fmt;
-
-    CLEAR(fmt);
-    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-    fmt.fmt.pix.width = cfg->capture_width;
-    fmt.fmt.pix.height = cfg->capture_height;
-    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
-    fmt.fmt.pix.field = V4L2_FIELD_NONE;
-
-    if (ioctl(fd, VIDIOC_S_FMT, &fmt) < 0) {
-        perror("VIDIOC_S_FMT");
-        return -1;
-    }
-    return 0;
-}
-
-static int init_mmap(int fd, Buffer **buffers, int *num_buffers) {
-    struct v4l2_requestbuffers req;
-    int index;
-
-    CLEAR(req);
-    req.count = NUM_BUFFERS;
-    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-    req.memory = V4L2_MEMORY_MMAP;
-
-    if (ioctl(fd, VIDIOC_REQBUFS, &req) < 0) {
-        perror("VIDIOC_REQBUFS");
-        return -1;
-    }
-
-    *num_buffers = (int) req.count;
-    *buffers = (Buffer *) calloc(req.count, sizeof(Buffer));
-    if (*buffers == NULL) {
-        return -1;
-    }
-
-    for (index = 0; index < (int) req.count; ++index) {
-        struct v4l2_buffer buf;
-        CLEAR(buf);
-        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-        buf.memory = V4L2_MEMORY_MMAP;
-        buf.index = (unsigned int) index;
-
-        if (ioctl(fd, VIDIOC_QUERYBUF, &buf) < 0) {
-            perror("VIDIOC_QUERYBUF");
-            return -1;
-        }
-
-        (*buffers)[index].length = buf.length;
-        (*buffers)[index].start = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, buf.m.offset);
-        if ((*buffers)[index].start == MAP_FAILED) {
-            perror("mmap");
-            return -1;
-        }
-    }
-
-    return 0;
-}
-
-static int queue_all_buffers(int fd, int num_buffers) {
-    int index;
-    for (index = 0; index < num_buffers; ++index) {
-        struct v4l2_buffer buf;
-        CLEAR(buf);
-        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-        buf.memory = V4L2_MEMORY_MMAP;
-        buf.index = (unsigned int) index;
-        if (ioctl(fd, VIDIOC_QBUF, &buf) < 0) {
-            perror("VIDIOC_QBUF");
-            return -1;
-        }
-    }
-    return 0;
-}
-
-static AVCodecContext *create_mjpeg_decoder(const worker_config_t *cfg) {
-    const AVCodec *decoder = avcodec_find_decoder(AV_CODEC_ID_MJPEG);
-    AVCodecContext *ctx;
-
-    if (decoder == NULL) {
-        fprintf(stderr, "MJPEG decoder not found\n");
-        return NULL;
-    }
-    ctx = avcodec_alloc_context3(decoder);
-    if (ctx == NULL) {
-        return NULL;
-    }
-
-    ctx->width = cfg->capture_width;
-    ctx->height = cfg->capture_height;
-    ctx->pix_fmt = AV_PIX_FMT_YUVJ420P;
-    ctx->color_range = AVCOL_RANGE_JPEG;
-    ctx->thread_count = 1;
-    if (avcodec_open2(ctx, decoder, NULL) < 0) {
-        avcodec_free_context(&ctx);
-        return NULL;
-    }
-    return ctx;
-}
-
-static AVCodecContext *create_mjpeg_encoder(const worker_config_t *cfg) {
-    const AVCodec *encoder = avcodec_find_encoder(AV_CODEC_ID_MJPEG);
-    AVCodecContext *ctx;
-
-    if (encoder == NULL) {
-        fprintf(stderr, "MJPEG encoder not found\n");
-        return NULL;
-    }
-    ctx = avcodec_alloc_context3(encoder);
-    if (ctx == NULL) {
-        return NULL;
-    }
-
-    ctx->width = cfg->output_width;
-    ctx->height = cfg->output_height;
-    ctx->pix_fmt = AV_PIX_FMT_YUVJ420P;
-    ctx->time_base = (AVRational) {1, cfg->initial_fps > 0 ? cfg->initial_fps : 10};
-    ctx->qmin = 1;
-    ctx->qmax = 31;
-    ctx->flags |= AV_CODEC_FLAG_QSCALE;
-    ctx->global_quality = FF_QP2LAMBDA * cfg->initial_qscale;
-    if (avcodec_open2(ctx, encoder, NULL) < 0) {
-        avcodec_free_context(&ctx);
-        return NULL;
-    }
-    return ctx;
-}
-
-static int decode_mjpeg_frame(AVCodecContext *decoder, const uint8_t *data, int size, AVFrame **frame) {
-    AVPacket *pkt;
-    int ret;
-
-    *frame = NULL;
-    pkt = av_packet_alloc();
-    if (pkt == NULL) {
-        return -1;
-    }
-
-    pkt->data = (uint8_t *) data;
-    pkt->size = size;
-    ret = avcodec_send_packet(decoder, pkt);
-    if (ret < 0) {
-        av_packet_free(&pkt);
-        return -1;
-    }
-
-    *frame = av_frame_alloc();
-    if (*frame == NULL) {
-        av_packet_free(&pkt);
-        return -1;
-    }
-    ret = avcodec_receive_frame(decoder, *frame);
-    av_packet_free(&pkt);
-    if (ret < 0) {
-        av_frame_free(frame);
-        return -1;
-    }
-    return 0;
-}
-
-static struct SwsContext *create_scaler(AVFrame *src, const worker_config_t *cfg) {
-    return sws_getContext(
-        src->width,
-        src->height,
-        src->format,
-        cfg->output_width,
-        cfg->output_height,
-        AV_PIX_FMT_YUVJ420P,
-        SWS_BILINEAR,
-        NULL,
-        NULL,
-        NULL);
-}
-
-static int scale_frame(struct SwsContext *sws_ctx, const worker_config_t *cfg, AVFrame *src, AVFrame **dst) {
-    int ret;
-
-    if (sws_ctx == NULL) {
-        return -1;
-    }
-
-    *dst = av_frame_alloc();
-    if (*dst == NULL) {
-        return -1;
-    }
-
-    (*dst)->width = cfg->output_width;
-    (*dst)->height = cfg->output_height;
-    (*dst)->format = AV_PIX_FMT_YUVJ420P;
-    if (av_frame_get_buffer(*dst, 0) < 0) {
-        av_frame_free(dst);
-        return -1;
-    }
-
-    ret = sws_scale(
-        sws_ctx,
-        (const uint8_t *const *) src->data,
-        src->linesize,
-        0,
-        src->height,
-        (*dst)->data,
-        (*dst)->linesize);
-    if (ret < 0) {
-        av_frame_free(dst);
-        return -1;
-    }
-    return 0;
-}
-
-static int encode_frame(AVCodecContext *encoder, AVFrame *frame, int qscale, AVPacket **pkt) {
-    int ret;
-
-    *pkt = av_packet_alloc();
-    if (*pkt == NULL) {
-        return -1;
-    }
-
-    encoder->global_quality = FF_QP2LAMBDA * qscale;
-    frame->quality = encoder->global_quality;
-    ret = avcodec_send_frame(encoder, frame);
-    if (ret < 0) {
-        av_packet_free(pkt);
-        return -1;
-    }
-    ret = avcodec_receive_packet(encoder, *pkt);
-    if (ret < 0) {
-        av_packet_free(pkt);
-        return -1;
-    }
-    return 0;
-}
-
-static int video_sender_init(video_sender_t *sender, const worker_config_t *cfg) {
-    kcp_conn_options_t options;
-
-    if (sender == NULL) {
-        errno = EINVAL;
-        return -1;
-    }
-
-    CLEAR(*sender);
-    snprintf(sender->target_peer, sizeof(sender->target_peer), "%s", cfg->target_peer);
-    kcp_conn_options_set_video_defaults(&options);
-    sender->client = kcp_client_dial_with_options(
-        cfg->server_addr,
-        cfg->relay_addr,
-        cfg->peer_id,
-        cfg->bind_ip,
-        cfg->bind_device,
-        &options,
-        NULL,
-        NULL,
-        NULL,
-        cfg->stats_interval_ms);
-    if (sender->client == NULL) {
-        return -1;
-    }
-
-    fprintf(stderr, "B-side video worker connected as %s -> %s\n", kcp_client_id(sender->client), sender->target_peer);
-    return 0;
-}
-
-static int video_sender_send_packet(video_sender_t *sender, const AVPacket *encoded_pkt) {
-    if (sender == NULL || sender->client == NULL || encoded_pkt == NULL) {
-        errno = EINVAL;
-        return -1;
-    }
-    return kcp_client_send_binary(sender->client, sender->target_peer, encoded_pkt->data, (size_t) encoded_pkt->size);
-}
-
-static void video_sender_close(video_sender_t *sender) {
-    if (sender == NULL || sender->client == NULL) {
-        return;
-    }
-    kcp_client_close(sender->client);
-    kcp_client_free(sender->client);
-    sender->client = NULL;
-}
-
-static void *control_thread_main(void *opaque) {
-    control_thread_args_t *args = (control_thread_args_t *) opaque;
-    char line[512];
-
-    while (fgets(line, sizeof(line), args->control_stream) != NULL) {
-        cJSON *root = cJSON_Parse(line);
-        cJSON *type_item;
-
-        if (root == NULL) {
-            continue;
-        }
-        type_item = cJSON_GetObjectItemCaseSensitive(root, "type");
-        if (!cJSON_IsString(type_item) || type_item->valuestring == NULL) {
-            cJSON_Delete(root);
-            continue;
-        }
-
-        if (strcmp(type_item->valuestring, "shutdown") == 0) {
-            runtime_set_shutdown(args->runtime, true);
-            cJSON_Delete(root);
-            return NULL;
-        }
-        if (strcmp(type_item->valuestring, "set_profile") == 0) {
-            cJSON *fps = cJSON_GetObjectItemCaseSensitive(root, "fps");
-            cJSON *qscale = cJSON_GetObjectItemCaseSensitive(root, "jpeg_quality_qscale");
-            cJSON *max_frame_bytes = cJSON_GetObjectItemCaseSensitive(root, "max_frame_bytes");
-            runtime_set_profile(
-                args->runtime,
-                cJSON_IsNumber(fps) ? fps->valueint : -1,
-                cJSON_IsNumber(qscale) ? qscale->valueint : -1,
-                cJSON_IsNumber(max_frame_bytes) ? max_frame_bytes->valueint : -1);
-        }
-        cJSON_Delete(root);
-    }
-
-    runtime_set_shutdown(args->runtime, true);
-    return NULL;
-}
-
-int main(void) {
-    worker_config_t cfg;
-    runtime_state_t runtime;
-    video_sender_t sender;
-    control_thread_args_t control_args;
-    pthread_t control_thread;
-    FILE *control_stream = NULL;
-    FILE *telemetry_stream = NULL;
-    Buffer *buffers = NULL;
-    int num_buffers = 0;
-    int camera_fd = -1;
-    enum v4l2_buf_type stream_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-    AVCodecContext *decoder = NULL;
-    AVCodecContext *encoder = NULL;
-    struct SwsContext *sws_ctx = NULL;
-    double next_deadline_ms = 0.0;
-    double next_metrics_ms = 0.0;
-    int exit_code = 1;
-    int index;
-    int sws_src_width = 0;
-    int sws_src_height = 0;
-    int sws_src_format = -1;
-    bool control_thread_started = false;
-
-    av_log_set_level(AV_LOG_ERROR);
-    signal(SIGINT, handle_signal);
-    signal(SIGTERM, handle_signal);
-    CLEAR(sender);
-
-    if (load_worker_config(&cfg) != 0) {
-        return 1;
-    }
-
-    CLEAR(runtime);
-    pthread_mutex_init(&runtime.lock, NULL);
-    pthread_mutex_init(&runtime.telemetry_lock, NULL);
-    runtime.target_fps = cfg.initial_fps;
-    runtime.jpeg_quality_qscale = cfg.initial_qscale;
-    runtime.max_frame_bytes = cfg.initial_max_frame_bytes;
-
-    control_stream = fdopen(WORKER_CONTROL_FD, "r");
-    telemetry_stream = fdopen(WORKER_TELEMETRY_FD, "w");
-    if (control_stream == NULL || telemetry_stream == NULL) {
-        perror("fdopen worker control/telemetry");
-        goto cleanup;
-    }
-    setvbuf(telemetry_stream, NULL, _IOLBF, 0);
-    runtime.telemetry_stream = telemetry_stream;
-
-    control_args.control_stream = control_stream;
-    control_args.runtime = &runtime;
-    if (pthread_create(&control_thread, NULL, control_thread_main, &control_args) != 0) {
-        perror("pthread_create");
-        goto cleanup;
-    }
-    control_thread_started = true;
-
-    camera_fd = open_v4l2_device(cfg.video_device);
-    if (camera_fd < 0) {
-        goto cleanup_join_thread;
-    }
-    if (init_v4l2_device(camera_fd, &cfg) != 0) {
-        goto cleanup_join_thread;
-    }
-    if (init_mmap(camera_fd, &buffers, &num_buffers) != 0) {
-        goto cleanup_join_thread;
-    }
-    if (queue_all_buffers(camera_fd, num_buffers) != 0) {
-        goto cleanup_join_thread;
-    }
-    if (ioctl(camera_fd, VIDIOC_STREAMON, &stream_type) < 0) {
-        perror("VIDIOC_STREAMON");
-        goto cleanup_join_thread;
-    }
-
-    decoder = create_mjpeg_decoder(&cfg);
-    encoder = create_mjpeg_encoder(&cfg);
-    if (decoder == NULL || encoder == NULL) {
-        fprintf(stderr, "failed to create codecs\n");
-        goto cleanup_join_thread;
-    }
-    if (video_sender_init(&sender, &cfg) != 0) {
-        perror("video_sender_init");
-        goto cleanup_join_thread;
-    }
-
-    next_deadline_ms = monotonic_ms();
-    next_metrics_ms = next_deadline_ms + 100.0;
-
-    while (!runtime_should_stop(&runtime)) {
-        AVFrame *decoded_frame = NULL;
-        AVFrame *scaled_frame = NULL;
-        AVPacket *encoded_pkt = NULL;
-        struct v4l2_buffer buf;
-        fd_set fds;
-        struct timeval tv;
-        int select_result;
-        int fps;
-        int qscale;
-        int max_frame_bytes;
-        int encode_us = 0;
-        bool sent = false;
-        const char *drop_reason = "";
-        double now_ms = monotonic_ms();
-        double encode_start_ms;
-        double encode_end_ms;
-
-        runtime_get_profile(&runtime, &fps, &qscale, &max_frame_bytes);
-        if (fps < 1) {
-            fps = 1;
-        }
-        if (next_deadline_ms > now_ms) {
-            usleep((useconds_t) ((next_deadline_ms - now_ms) * 1000.0));
-        }
-        next_deadline_ms = monotonic_ms() + (1000.0 / (double) fps);
-
-        FD_ZERO(&fds);
-        FD_SET(camera_fd, &fds);
-        tv.tv_sec = 2;
-        tv.tv_usec = 0;
-        select_result = select(camera_fd + 1, &fds, NULL, NULL, &tv);
-        if (select_result <= 0) {
-            if (select_result < 0 && errno == EINTR) {
-                continue;
-            }
-            fprintf(stderr, "select timeout/error on camera\n");
-            continue;
-        }
-
-        CLEAR(buf);
-        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-        buf.memory = V4L2_MEMORY_MMAP;
-        if (ioctl(camera_fd, VIDIOC_DQBUF, &buf) < 0) {
-            if (errno == EAGAIN) {
-                continue;
-            }
-            perror("VIDIOC_DQBUF");
-            break;
-        }
-
-        if (decode_mjpeg_frame(decoder, (uint8_t *) buffers[buf.index].start, (int) buf.bytesused, &decoded_frame) != 0) {
-            drop_reason = "decode_failed";
-            goto requeue_and_report;
-        }
-
-        if (
-            sws_ctx == NULL
-            || sws_src_width != decoded_frame->width
-            || sws_src_height != decoded_frame->height
-            || sws_src_format != decoded_frame->format
-        ) {
-            sws_freeContext(sws_ctx);
-            sws_ctx = create_scaler(decoded_frame, &cfg);
-            sws_src_width = decoded_frame->width;
-            sws_src_height = decoded_frame->height;
-            sws_src_format = decoded_frame->format;
-        }
-        if (scale_frame(sws_ctx, &cfg, decoded_frame, &scaled_frame) != 0) {
-            drop_reason = "scale_failed";
-            goto requeue_and_report;
-        }
-
-        encode_start_ms = monotonic_ms();
-        if (encode_frame(encoder, scaled_frame, qscale, &encoded_pkt) != 0) {
-            drop_reason = "encode_failed";
-            goto requeue_and_report;
-        }
-        encode_end_ms = monotonic_ms();
-        encode_us = (int) ((encode_end_ms - encode_start_ms) * 1000.0);
-
-        if (encoded_pkt->size > max_frame_bytes) {
-            drop_reason = "frame_too_large";
-            goto requeue_and_report;
-        }
-        if (video_sender_send_packet(&sender, encoded_pkt) != 0) {
-            perror("video_sender_send_packet");
-            drop_reason = "send_failed";
-            goto requeue_and_report;
-        }
-        sent = true;
-
-requeue_and_report:
-        telemetry_write_frame_stat(
-            &runtime,
-            encoded_pkt != NULL ? encoded_pkt->size : 0,
-            encode_us,
-            sent,
-            drop_reason);
-
-        if (ioctl(camera_fd, VIDIOC_QBUF, &buf) < 0) {
-            perror("VIDIOC_QBUF");
-            av_frame_free(&decoded_frame);
-            av_frame_free(&scaled_frame);
-            if (encoded_pkt != NULL) {
-                av_packet_free(&encoded_pkt);
-            }
-            break;
-        }
-
-        av_frame_free(&decoded_frame);
-        av_frame_free(&scaled_frame);
-        if (encoded_pkt != NULL) {
-            av_packet_free(&encoded_pkt);
-        }
-
-        now_ms = monotonic_ms();
-        if (sender.client != NULL && now_ms >= next_metrics_ms) {
-            kcp_conn_metrics_t metrics;
-            if (kcp_client_metrics_snapshot(sender.client, &metrics) == 0) {
-                telemetry_write_kcp_metrics(&runtime, &metrics);
-            }
-            next_metrics_ms = now_ms + 100.0;
-        }
-    }
-
-    exit_code = 0;
-
-cleanup_join_thread:
-    runtime_set_shutdown(&runtime, true);
-    if (control_stream != NULL) {
-        fclose(control_stream);
-        control_stream = NULL;
-    }
-    if (control_thread_started) {
-        pthread_join(control_thread, NULL);
-    }
-
-cleanup:
-    if (control_stream != NULL) {
-        fclose(control_stream);
-        control_stream = NULL;
-    }
-    if (camera_fd >= 0) {
-        ioctl(camera_fd, VIDIOC_STREAMOFF, &stream_type);
-    }
-    if (buffers != NULL) {
-        for (index = 0; index < num_buffers; ++index) {
-            if (buffers[index].start != NULL && buffers[index].start != MAP_FAILED) {
-                munmap(buffers[index].start, buffers[index].length);
-            }
-        }
-        free(buffers);
-    }
-    video_sender_close(&sender);
-    if (encoder != NULL) {
-        avcodec_free_context(&encoder);
-    }
-    if (decoder != NULL) {
-        avcodec_free_context(&decoder);
-    }
-    sws_freeContext(sws_ctx);
-    if (camera_fd >= 0) {
-        close(camera_fd);
-    }
-    if (telemetry_stream != NULL) {
-        fclose(telemetry_stream);
-        telemetry_stream = NULL;
-    }
-    pthread_mutex_destroy(&runtime.lock);
-    pthread_mutex_destroy(&runtime.telemetry_lock);
-    return exit_code;
-}

cmd/kcpserver.c

@@ -6,6 +6,7 @@ static void kcpserver_usage(FILE *out) {
     fprintf(out, "usage: kcpserver [-mode hub|relay] [-listen addr] [-bind-device dev]\n");
     fprintf(out, "                 [-latency-log path] [-kcp-ts-debug-log path]\n");
     fprintf(out, "                 [-kcp-session-stats-log path] [-kcp-session-stats-interval 100ms]\n");
+    fprintf(out, "                 [-telemetry-peer peer-id] [-telemetry-interval 500ms]\n");
     fprintf(out, "                 [-relay-remote addr] [-relay-listen addr] [-relay-peer addr]\n");
 }
 
@@ -17,10 +18,13 @@ int main(int argc, char **argv) {
     const char *packet_log_path = "";
     const char *stats_log_path = "";
     const char *stats_interval_raw = "";
+    const char *telemetry_peer_id = "";
+    const char *telemetry_interval_raw = "";
     const char *relay_listen_alias = "";
     const char *relay_remote_addr = "";
     const char *relay_peer_alias = "";
     int stats_interval_ms = KCP_DEFAULT_STATS_INTERVAL_MS;
+    int telemetry_interval_ms = 500;
     int i;
     int rc = 1;
 
@@ -84,6 +88,20 @@ int main(int argc, char **argv) {
             stats_interval_raw = value;
             continue;
         }
+        if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-telemetry-peer", &value)) < 0) {
+            fprintf(stderr, "kcpserver: flag -telemetry-peer requires a value\n");
+            return 1;
+        } else if (handled) {
+            telemetry_peer_id = value;
+            continue;
+        }
+        if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-telemetry-interval", &value)) < 0) {
+            fprintf(stderr, "kcpserver: flag -telemetry-interval requires a value\n");
+            return 1;
+        } else if (handled) {
+            telemetry_interval_raw = value;
+            continue;
+        }
         if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-relay-listen", &value)) < 0) {
             fprintf(stderr, "kcpserver: flag -relay-listen requires a value\n");
             return 1;
@@ -118,6 +136,10 @@ int main(int argc, char **argv) {
         fprintf(stderr, "kcpserver: invalid -kcp-session-stats-interval value %s\n", stats_interval_raw);
         return 1;
     }
+    if (omni_parse_duration_ms(telemetry_interval_raw, 500, &telemetry_interval_ms) != 0) {
+        fprintf(stderr, "kcpserver: invalid -telemetry-interval value %s\n", telemetry_interval_raw);
+        return 1;
+    }
 
     if (relay_peer_alias[0] != '\0' && relay_remote_addr[0] != '\0' && strcmp(relay_peer_alias, relay_remote_addr) != 0) {
         fprintf(stderr, "kcpserver: flags -relay-remote and -relay-peer must match when both are set\n");
@@ -178,6 +200,10 @@ int main(int argc, char **argv) {
             fprintf(stderr, "kcpserver: create hub failed\n");
             goto cleanup;
         }
+        if (telemetry_peer_id[0] != '\0' && kcp_hub_set_telemetry(hub, telemetry_peer_id, telemetry_interval_ms) != 0) {
+            fprintf(stderr, "kcpserver: configure telemetry peer %s failed\n", telemetry_peer_id);
+            goto cleanup;
+        }
         fprintf(stderr, "kcp hub listening on %s\n", listen_addr);
         if (kcp_hub_serve_listener(hub, listener) != 0) {
             fprintf(stderr, "kcpserver: serve listener failed\n");
@@ -188,6 +214,10 @@ int main(int argc, char **argv) {
     }
 
     if (strcmp(mode, "relay") == 0) {
+        if (telemetry_peer_id[0] != '\0') {
+            fprintf(stderr, "kcpserver: flag -telemetry-peer may only be used in hub mode\n");
+            return 1;
+        }
         if (bind_device[0] != '\0') {
             fprintf(stderr, "kcpserver: flag -bind-device is not supported in relay mode\n");
             return 1;

cmd/v1_camera_pipeline_ifdef.c

@@ -1,652 +1,28 @@
-// camera_pipeline_ifdef_fixed.c
 #include <stdio.h>
 #include <stdlib.h>
-#include <string.h>
-#include <errno.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <sys/ioctl.h>
-#include <sys/mman.h>
-#include <sys/time.h>
-#include <linux/videodev2.h>
-#include <time.h>
 
-// FFmpeg头文件 - 使用纯C包含
+#include "video_pipeline.h"
-#include <libavcodec/avcodec.h>
-#include <libavformat/avformat.h>
-#include <libavutil/avutil.h>
-#include <libavutil/imgutils.h>
-#include <libavutil/opt.h>
-#include <libswscale/swscale.h>
 
-#include "peer_kcp_client.h"
+int main(void) {
+    video_pipeline_config_t config;
+    video_pipeline_stats_t stats;
 
-// ==========================================
+    video_pipeline_config_init(&config);
-// 1. 配置区域：在这里开启或关闭时间打印
+    video_pipeline_config_load_env(&config);
-// ==========================================
+    if (getenv("OMNI_VIDEO_DEBUG_TIMING") == NULL) {
-#define DEBUG_TIMING // 注释掉这一行，所有时间打印和计算都会消失
+        config.enable_timing_logs = 1;
-
-// 定义打印宏
-#ifdef DEBUG_TIMING
-#define PRINT_TIME(fmt, ...) printf(fmt, ##__VA_ARGS__)
-#else
-#define PRINT_TIME(fmt, ...) // 什么都不做，编译器会优化掉
-#endif
-
-#define WIDTH 1280
-#define HEIGHT 720
-#define OUTPUT_WIDTH 640
-#define OUTPUT_HEIGHT 360
-#define NUM_BUFFERS 4
-#define CLEAR(x) memset(&(x), 0, sizeof(x))
-
-#define VIDEO_SERVER_ADDR_ENV "OMNI_VIDEO_SERVER_ADDR"
-#define VIDEO_RELAY_ADDR_ENV "OMNI_VIDEO_RELAY_VIA"
-#define VIDEO_BIND_IP_ENV "OMNI_VIDEO_BIND_IP"
-#define VIDEO_BIND_DEVICE_ENV "OMNI_VIDEO_BIND_DEVICE"
-#define VIDEO_PEER_ID_ENV "OMNI_VIDEO_PEER_ID"
-#define VIDEO_TARGET_PEER_ENV "OMNI_VIDEO_TARGET_PEER"
-#define VIDEO_DEFAULT_PEER_ID "peer-b-video"
-#define VIDEO_DEFAULT_TARGET_PEER "peer-a-video"
-
-typedef struct
-{
-    kcp_client_t *client;
-    char target_peer[OMNI_MAX_PEER_ID];
-} VideoSender;
-
-static int video_sender_init(VideoSender *sender);
-static int video_sender_send_packet(VideoSender *sender, const AVPacket *encoded_pkt);
-static void video_sender_close(VideoSender *sender);
-
-typedef struct
-{
-    void *start;
-    size_t length;
-} Buffer;
-
-double get_time_ms()
-{
-    struct timespec ts;
-    clock_gettime(CLOCK_MONOTONIC, &ts);
-    return ts.tv_sec * 1000.0 + ts.tv_nsec / 1000000.0;
-}
-
-static const char *env_or_default(const char *name, const char *fallback)
-{
-    const char *value = getenv(name);
-
-    if (value != NULL && value[0] != '\0')
-        return value;
-    return fallback;
-}
-
-static int video_sender_init(VideoSender *sender)
-{
-    const char *server_addr = getenv(VIDEO_SERVER_ADDR_ENV);
-    const char *relay_addr = env_or_default(VIDEO_RELAY_ADDR_ENV, "");
-    const char *bind_ip = env_or_default(VIDEO_BIND_IP_ENV, "");
-    const char *bind_device = env_or_default(VIDEO_BIND_DEVICE_ENV, "");
-    const char *peer_id = env_or_default(VIDEO_PEER_ID_ENV, VIDEO_DEFAULT_PEER_ID);
-    const char *target_peer = env_or_default(VIDEO_TARGET_PEER_ENV, VIDEO_DEFAULT_TARGET_PEER);
-    kcp_conn_options_t options;
-
-    if (sender == NULL)
-    {
-        errno = EINVAL;
-        return -1;
     }
-    if (server_addr == NULL || server_addr[0] == '\0')
+    if (video_pipeline_stats_init(&stats) != 0) {
-    {
+        perror("video_pipeline_stats_init");
-        errno = EINVAL;
+        return 1;
-        fprintf(stderr, "%s is required\n", VIDEO_SERVER_ADDR_ENV);
-        return -1;
     }
 
-    memset(sender, 0, sizeof(*sender));
+    if (video_pipeline_run(&config, &stats, NULL) != 0) {
-    snprintf(sender->target_peer, sizeof(sender->target_peer), "%s", target_peer);
+        perror("video_pipeline_run");
-
+        video_pipeline_stats_destroy(&stats);
-    kcp_conn_options_set_video_defaults(&options);
+        return 1;
-    sender->client = kcp_client_dial_with_options(
+    }
-        server_addr,
-        relay_addr,
-        peer_id,
-        bind_ip,
-        bind_device,
-        &options,
-        NULL,
-        NULL,
-        NULL,
-        KCP_DEFAULT_STATS_INTERVAL_MS);
-    if (sender->client == NULL)
-        return -1;
-
-    fprintf(stderr, "Video sender connected as %s -> %s\n",
-            kcp_client_id(sender->client), sender->target_peer);
 
+    video_pipeline_stats_destroy(&stats);
     return 0;
 }
-
-static int video_sender_send_packet(VideoSender *sender, const AVPacket *encoded_pkt)
-{
-    if (sender == NULL || sender->client == NULL || encoded_pkt == NULL)
-    {
-        errno = EINVAL;
-        return -1;
-    }
-    return kcp_client_send_binary(
-        sender->client,
-        sender->target_peer,
-        encoded_pkt->data,
-        (size_t)encoded_pkt->size);
-}
-
-static void video_sender_close(VideoSender *sender)
-{
-    if (sender == NULL || sender->client == NULL)
-        return;
-
-    kcp_client_close(sender->client);
-    kcp_client_free(sender->client);
-    sender->client = NULL;
-}
-
-int open_v4l2_device(const char *device)
-{
-    int fd = open(device, O_RDWR | O_NONBLOCK);
-    if (fd < 0)
-    {
-        perror("open device");
-        return -1;
-    }
-    return fd;
-}
-
-int init_v4l2_device(int fd)
-{
-    struct v4l2_format fmt = {0};
-    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-    fmt.fmt.pix.width = WIDTH;
-    fmt.fmt.pix.height = HEIGHT;
-    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
-    fmt.fmt.pix.field = V4L2_FIELD_NONE;
-
-    if (ioctl(fd, VIDIOC_S_FMT, &fmt) < 0)
-    {
-        perror("VIDIOC_S_FMT");
-        return -1;
-    }
-
-    PRINT_TIME("Set format: %dx%d MJPEG\n", WIDTH, HEIGHT);
-    return 0;
-}
-
-int init_mmap(int fd, Buffer **buffers, int *num_buffers)
-{
-    struct v4l2_requestbuffers req = {0};
-    req.count = NUM_BUFFERS;
-    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-    req.memory = V4L2_MEMORY_MMAP;
-
-    if (ioctl(fd, VIDIOC_REQBUFS, &req) < 0)
-    {
-        perror("VIDIOC_REQBUFS");
-        return -1;
-    }
-
-    *num_buffers = req.count;
-    *buffers = (Buffer *)calloc(req.count, sizeof(Buffer));
-
-    for (int i = 0; i < req.count; i++)
-    {
-        struct v4l2_buffer buf = {0};
-        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-        buf.memory = V4L2_MEMORY_MMAP;
-        buf.index = i;
-
-        if (ioctl(fd, VIDIOC_QUERYBUF, &buf) < 0)
-        {
-            perror("VIDIOC_QUERYBUF");
-            return -1;
-        }
-
-        (*buffers)[i].length = buf.length;
-        (*buffers)[i].start = mmap(NULL, buf.length,
-                                   PROT_READ | PROT_WRITE,
-                                   MAP_SHARED, fd, buf.m.offset);
-
-        if ((*buffers)[i].start == MAP_FAILED)
-        {
-            perror("mmap");
-            return -1;
-        }
-    }
-
-    return 0;
-}
-
-AVCodecContext *create_mjpeg_decoder()
-{
-    const AVCodec *decoder = avcodec_find_decoder(AV_CODEC_ID_MJPEG);
-    if (!decoder)
-    {
-        fprintf(stderr, "MJPEG decoder not found\n");
-        return NULL;
-    }
-
-    AVCodecContext *ctx = avcodec_alloc_context3(decoder);
-    ctx->width = WIDTH;
-    ctx->height = HEIGHT;
-    ctx->pix_fmt = AV_PIX_FMT_YUVJ420P;  // 使用YUVJ420P
-    ctx->color_range = AVCOL_RANGE_JPEG; // JPEG范围
-    ctx->thread_count = 1;
-
-    AVDictionary *opts = NULL;
-    av_dict_set(&opts, "flags2", "+fast", 0);
-
-    if (avcodec_open2(ctx, decoder, &opts) < 0)
-    {
-        avcodec_free_context(&ctx);
-        av_dict_free(&opts);
-        return NULL;
-    }
-
-    av_dict_free(&opts);
-    printf("Decoder created with format: YUVJ420P\n");
-    return ctx;
-}
-
-AVCodecContext *create_mjpeg_encoder()
-{
-    const AVCodec *encoder = avcodec_find_encoder(AV_CODEC_ID_MJPEG);
-    if (!encoder)
-    {
-        fprintf(stderr, "MJPEG encoder not found\n");
-        return NULL;
-    }
-
-    AVCodecContext *ctx = avcodec_alloc_context3(encoder);
-    ctx->width = OUTPUT_WIDTH;
-    ctx->height = OUTPUT_HEIGHT;
-    ctx->pix_fmt = AV_PIX_FMT_YUVJ420P; // 使用YUVJ420P
-    ctx->time_base = (AVRational){1, 30};
-    ctx->qmin = 8;
-    ctx->qmax = 31;
-    ctx->flags |= AV_CODEC_FLAG_QSCALE;
-    ctx->global_quality = FF_QP2LAMBDA * 5;
-
-    AVDictionary *opts = NULL;
-    av_dict_set(&opts, "huffman", "default", 0);
-
-    if (avcodec_open2(ctx, encoder, &opts) < 0)
-    {
-        avcodec_free_context(&ctx);
-        av_dict_free(&opts);
-        return NULL;
-    }
-
-    av_dict_free(&opts);
-    printf("Encoder created with format: YUVJ420P\n");
-    return ctx;
-}
-
-int decode_mjpeg_frame(AVCodecContext *decoder, const uint8_t *data, int size, AVFrame **frame)
-{
-    if (frame == NULL)
-        return -1;
-
-    *frame = NULL;
-    AVPacket *pkt = av_packet_alloc();
-    if (!pkt)
-        return -1;
-
-    pkt->data = (uint8_t *)data;
-    pkt->size = size;
-
-    int ret = avcodec_send_packet(decoder, pkt);
-    if (ret < 0)
-    {
-        av_packet_free(&pkt);
-        return -1;
-    }
-
-    *frame = av_frame_alloc();
-    if (!*frame)
-    {
-        av_packet_free(&pkt);
-        return -1;
-    }
-    ret = avcodec_receive_frame(decoder, *frame);
-
-    av_packet_free(&pkt);
-    if (ret < 0)
-        av_frame_free(frame);
-    return ret;
-}
-
-int scale_frame(AVFrame *src, AVFrame **dst)
-{
-    // 简单缩放，不设置复杂的色彩空间参数
-    struct SwsContext *sws_ctx = sws_getContext(
-        src->width, src->height, src->format,
-        OUTPUT_WIDTH, OUTPUT_HEIGHT, AV_PIX_FMT_YUVJ420P,
-        SWS_BILINEAR, NULL, NULL, NULL);
-
-    if (!sws_ctx)
-    {
-        fprintf(stderr, "Failed to create sws context\n");
-        return -1;
-    }
-
-    *dst = av_frame_alloc();
-    if (!*dst)
-    {
-        sws_freeContext(sws_ctx);
-        return -1;
-    }
-
-    (*dst)->width = OUTPUT_WIDTH;
-    (*dst)->height = OUTPUT_HEIGHT;
-    (*dst)->format = AV_PIX_FMT_YUVJ420P;
-
-    if (av_frame_get_buffer(*dst, 0) < 0)
-    {
-        fprintf(stderr, "Failed to allocate frame buffer\n");
-        av_frame_free(dst);
-        sws_freeContext(sws_ctx);
-        return -1;
-    }
-
-    int ret = sws_scale(sws_ctx, (const uint8_t *const *)src->data, src->linesize,
-                        0, src->height, (*dst)->data, (*dst)->linesize);
-
-    sws_freeContext(sws_ctx);
-
-    if (ret < 0)
-    {
-        fprintf(stderr, "sws_scale failed\n");
-        av_frame_free(dst);
-        return -1;
-    }
-
-    return 0;
-}
-
-int encode_frame(AVCodecContext *encoder, AVFrame *frame, AVPacket **pkt)
-{
-    if (pkt == NULL)
-        return -1;
-
-    *pkt = NULL;
-    *pkt = av_packet_alloc();
-    if (!*pkt)
-        return -1;
-
-    int ret = avcodec_send_frame(encoder, frame);
-    if (ret < 0)
-    {
-        av_packet_free(pkt);
-        return -1;
-    }
-
-    ret = avcodec_receive_packet(encoder, *pkt);
-    if (ret < 0)
-        av_packet_free(pkt);
-    return ret;
-}
-
-int main()
-{
-    VideoSender sender;
-
-    memset(&sender, 0, sizeof(sender));
-
-    PRINT_TIME("=== V4L2 Direct Capture + FFmpeg Processing ===\n");
-
-    // 1. Open V4L2 device
-    int fd = open_v4l2_device("/dev/video0");
-    if (fd < 0)
-    {
-        fprintf(stderr, "Failed to open camera device\n");
-        return 1;
-    }
-
-    // 2. Initialize V4L2
-    if (init_v4l2_device(fd) < 0)
-    {
-        close(fd);
-        return 1;
-    }
-
-    // 3. Setup MMAP buffers
-    Buffer *buffers = NULL;
-    int num_buffers = 0;
-    if (init_mmap(fd, &buffers, &num_buffers) < 0)
-    {
-        close(fd);
-        return 1;
-    }
-
-    // 4. Create FFmpeg codecs
-    AVCodecContext *decoder = create_mjpeg_decoder();
-    AVCodecContext *encoder = create_mjpeg_encoder();
-    if (!decoder || !encoder)
-    {
-        fprintf(stderr, "Failed to create codecs\n");
-        close(fd);
-        return 1;
-    }
-
-    if (video_sender_init(&sender) < 0)
-    {
-        perror("video_sender_init");
-        video_sender_close(&sender);
-        avcodec_free_context(&encoder);
-        avcodec_free_context(&decoder);
-        close(fd);
-        return 1;
-    }
-
-    // 5. Queue buffers
-    for (int i = 0; i < num_buffers; i++)
-    {
-        struct v4l2_buffer buf = {0};
-        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-        buf.memory = V4L2_MEMORY_MMAP;
-        buf.index = i;
-
-        if (ioctl(fd, VIDIOC_QBUF, &buf) < 0)
-        {
-            perror("VIDIOC_QBUF");
-            close(fd);
-            return 1;
-        }
-    }
-
-    // 6. Start streaming
-    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-    if (ioctl(fd, VIDIOC_STREAMON, &type) < 0)
-    {
-        perror("VIDIOC_STREAMON");
-        close(fd);
-        return 1;
-    }
-
-    // 7. Benchmark
-    // 使用宏控制打印表头
-    PRINT_TIME("\nRunning benchmark (100 frames)...\n");
-    PRINT_TIME("Frame | Capture | Decode | Scale | Encode | Total | Size | Marker\n");
-    PRINT_TIME("------|---------|--------|-------|--------|-------|------|--------\n");
-
-    for (int i = 0; i < 100; i++)
-    {
-// 只有在开启 DEBUG_TIMING 时才声明这些时间变量
-#ifdef DEBUG_TIMING
-        double total_start = get_time_ms();
-        double capture_start, capture_end;
-        double decode_start, decode_end;
-        double scale_start, scale_end;
-        double encode_start, encode_end;
-#endif
-
-        // Wait for frame
-        fd_set fds;
-        FD_ZERO(&fds);
-        FD_SET(fd, &fds);
-
-        struct timeval tv = {2, 0};
-        int r = select(fd + 1, &fds, NULL, NULL, &tv);
-        if (r <= 0)
-        {
-            PRINT_TIME("Timeout waiting for frame\n");
-            break;
-        }
-
-#ifdef DEBUG_TIMING
-        capture_start = get_time_ms();
-#endif
-
-        // Dequeue buffer
-        struct v4l2_buffer buf = {0};
-        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-        buf.memory = V4L2_MEMORY_MMAP;
-
-        if (ioctl(fd, VIDIOC_DQBUF, &buf) < 0)
-        {
-            perror("VIDIOC_DQBUF");
-            break;
-        }
-
-#ifdef DEBUG_TIMING
-        capture_end = get_time_ms();
-#endif
-
-// Decode
-#ifdef DEBUG_TIMING
-        decode_start = get_time_ms();
-#endif
-
-        AVFrame *decoded_frame = NULL;
-        int ret = decode_mjpeg_frame(decoder,
-                                     (uint8_t *)buffers[buf.index].start, buf.bytesused, &decoded_frame);
-
-#ifdef DEBUG_TIMING
-        decode_end = get_time_ms();
-#endif
-
-        if (ret < 0 || !decoded_frame)
-        {
-            PRINT_TIME("Frame %d: Decode failed\n", i + 1);
-            ioctl(fd, VIDIOC_QBUF, &buf);
-            continue;
-        }
-
-// Scale
-#ifdef DEBUG_TIMING
-        scale_start = get_time_ms();
-#endif
-
-        AVFrame *scaled_frame = NULL;
-        if (scale_frame(decoded_frame, &scaled_frame) < 0)
-        {
-            PRINT_TIME("Frame %d: Scale failed\n", i + 1);
-            av_frame_free(&decoded_frame);
-            ioctl(fd, VIDIOC_QBUF, &buf);
-            continue;
-        }
-
-#ifdef DEBUG_TIMING
-        scale_end = get_time_ms();
-#endif
-
-// Encode
-#ifdef DEBUG_TIMING
-        encode_start = get_time_ms();
-#endif
-
-        AVPacket *encoded_pkt = NULL;
-        if (encode_frame(encoder, scaled_frame, &encoded_pkt) < 0)
-        {
-            PRINT_TIME("Frame %d: Encode failed\n", i + 1);
-        }
-#ifdef DEBUG_TIMING
-        if (encoded_pkt && i % 50 == 0)
-        {
-            char filename[100];
-            sprintf(filename, "frame_%04d.jpg", i + 1);
-
-            FILE *f = fopen(filename, "wb");
-            if (f)
-            {
-                fwrite(encoded_pkt->data, 1, encoded_pkt->size, f);
-                fclose(f);
-                PRINT_TIME("Saved as %s\n", filename);
-            }
-        }
-#endif
-
-#ifdef DEBUG_TIMING
-        encode_end = get_time_ms();
-        double total_end = get_time_ms();
-#endif
-
-// 打印结果
-#ifdef DEBUG_TIMING
-        PRINT_TIME("%5d | %7.1f | %6.1f | %5.1f | %6.1f | %5.1f | %4d KB  | 0x%02x\n",
-                   i + 1,
-                   capture_end - capture_start,
-                   decode_end - decode_start,
-                   scale_end - scale_start,
-                   encode_end - encode_start,
-                   total_end - total_start,
-                   encoded_pkt ? encoded_pkt->size / 1024 : 0,
-                   encoded_pkt && encoded_pkt->size > 1 ? encoded_pkt->data[1] : 0);
-#else
-        // 如果不开启宏，也打印一些基本信息
-        printf("Frame %d processed\n", i + 1);
-#endif
-
-        if (encoded_pkt && video_sender_send_packet(&sender, encoded_pkt) != 0)
-        {
-            perror("video_sender_send_packet");
-            av_frame_free(&decoded_frame);
-            av_frame_free(&scaled_frame);
-            av_packet_free(&encoded_pkt);
-            ioctl(fd, VIDIOC_QBUF, &buf);
-            break;
-        }
-
-        // Cleanup
-        av_frame_free(&decoded_frame);
-        av_frame_free(&scaled_frame);
-        if (encoded_pkt)
-            av_packet_free(&encoded_pkt);
-
-        // Requeue buffer
-        if (ioctl(fd, VIDIOC_QBUF, &buf) < 0)
-        {
-            perror("VIDIOC_QBUF");
-            break;
-        }
-    }
-
-    // 8. Stop streaming
-    ioctl(fd, VIDIOC_STREAMOFF, &type);
-
-    // 9. Cleanup
-    for (int i = 0; i < num_buffers; i++)
-    {
-        if (buffers[i].start != MAP_FAILED)
-        {
-            munmap(buffers[i].start, buffers[i].length);
-        }
-    }
-    free(buffers);
-    video_sender_close(&sender);
-    avcodec_free_context(&encoder);
-    avcodec_free_context(&decoder);
-    close(fd);
-
-    return 0;
-}
-// gcc -o v1_camera_pipeline_ifdef v1_camera_pipeline_ifdef.c $(pkg-config --cflags --libs libavformat libavcodec libavutil libswscale)

config/a_side_omnidaemon.yaml

@@ -1,51 +0,0 @@
-transport:
-  server_addr: "81.70.156.140:10909"
-  relay_via: "106.55.173.235:10909"
-  bind_ip: ""
-  bind_device: ""
-
-control_sender:
-  peer_id: "peer-a-ctrl"
-  target_peer: "peer-b-ctrl"
-  nodelay: 1
-  interval_ms: 5
-  resend: 2
-  nc: 1
-  sndwnd: 32
-  rcvwnd: 32
-  mtu: 1400
-  stats_interval_ms: 100
-
-video_receiver:
-  peer_id: "peer-a-video"
-  buffer_bytes: 1048576
-  nodelay: 1
-  interval_ms: 10
-  resend: 2
-  nc: 1
-  sndwnd: 256
-  rcvwnd: 256
-  mtu: 1400
-  stats_interval_ms: 100
-
-daemon:
-  socket_path: "/tmp/omnisocket-a-side.sock"
-  reconnect_delay_ms: 2000
-  telemetry_interval_ms: 100
-  analog_send_hz: 100
-  frame_stale_ms: 500
-
-policy:
-  health_window_ms: 2000
-  green_srtt_ms: 35
-  yellow_srtt_ms: 60
-  retrans_red_threshold: 10
-  profile_green:
-    fps: 15
-    max_frame_kb: 60
-  profile_yellow:
-    fps: 10
-    max_frame_kb: 40
-  profile_red:
-    fps: 5
-    max_frame_kb: 20

config/b_side_omnidaemon.yaml

@@ -1,58 +0,0 @@
-transport:
-  server_addr: "81.70.156.140:10909"
-  relay_via: "106.55.173.235:10909"
-  bind_ip: ""
-  bind_device: ""
-
-control_receiver:
-  peer_id: "peer-b-ctrl"
-  nodelay: 1
-  interval_ms: 5
-  resend: 2
-  nc: 1
-  sndwnd: 32
-  rcvwnd: 32
-  mtu: 1400
-  stats_interval_ms: 100
-  queue_capacity: 256
-
-video_sender:
-  enabled: false
-  peer_id: "peer-b-video"
-  target_peer: "peer-a-video"
-  binary_path: "bin/b_side_video_sender"
-  device: "/dev/video0"
-  capture_width: 1280
-  capture_height: 720
-  output_width: 640
-  output_height: 360
-  fps: 10
-  jpeg_quality_qscale: 8
-  max_frame_bytes: 40960
-  stats_interval_ms: 100
-
-daemon:
-  socket_path: "/tmp/omnisocket-b-side.sock"
-  ctrl_socket_path: "/tmp/omnisocket-b-ctrl.sock"
-  reconnect_delay_ms: 2000
-  telemetry_interval_ms: 100
-  worker_restart_delay_ms: 2000
-
-policy:
-  mode: "auto"
-  health_window_ms: 2000
-  green_srtt_ms: 30
-  yellow_srtt_ms: 55
-  retrans_red_threshold: 8
-  profile_green:
-    fps: 10
-    jpeg_quality_qscale: 8
-    max_frame_bytes: 40960
-  profile_yellow:
-    fps: 7
-    jpeg_quality_qscale: 12
-    max_frame_bytes: 28672
-  profile_red:
-    fps: 5
-    jpeg_quality_qscale: 16
-    max_frame_bytes: 20480

include/control_protocol.h

@@ -0,0 +1,7 @@
+#ifndef OMNI_CONTROL_PROTOCOL_H
+#define OMNI_CONTROL_PROTOCOL_H
+
+#define OMNI_CONTROL_PACKET_FLOATS 6
+#define OMNI_CONTROL_PACKET_SIZE (OMNI_CONTROL_PACKET_FLOATS * sizeof(float))
+
+#endif

include/kcp_session_stats.h

@@ -26,6 +26,16 @@ typedef struct kcp_session_stats_record {
     int32_t srtt_ms;
     int has_srttvar_ms;
     int32_t srttvar_ms;
+    int has_snd_wnd;
+    uint32_t snd_wnd;
+    int has_rmt_wnd;
+    uint32_t rmt_wnd;
+    int has_inflight;
+    uint32_t inflight;
+    int has_window_limit;
+    uint32_t window_limit;
+    int has_window_pressure_pct;
+    double window_pressure_pct;
     int has_bytes_sent;
     uint64_t bytes_sent;
     int has_bytes_received;

include/peer_kcp_client.h

@@ -27,7 +27,7 @@ int kcp_client_receive_timed(kcp_client_t *client, message_t *out_msg, int timeo
 int kcp_client_receive(kcp_client_t *client, message_t *out_msg);
 int kcp_client_receive_binary_into(kcp_client_t *client, void *buffer, size_t buffer_len, kcp_client_recv_meta_t *out_meta, int timeout_ms);
 int kcp_client_persist_message(kcp_client_t *client, const message_t *msg, const char *inbox_dir, char *out_path, size_t out_path_len);
-int kcp_client_metrics_snapshot(kcp_client_t *client, kcp_conn_metrics_t *out_metrics);
+void kcp_client_runtime_stats_snapshot(kcp_client_t *client, kcp_runtime_stats_t *out_stats);
 int kcp_client_close(kcp_client_t *client);
 void kcp_client_free(kcp_client_t *client);
 

include/peer_udp_client.h

@@ -8,12 +8,24 @@ extern "C" {
 #endif
 
 typedef struct udp_client udp_client_t;
+typedef struct udp_client_recv_meta {
+    message_type_t type;
+    uint64_t id;
+    char from[OMNI_MAX_PEER_ID];
+    char to[OMNI_MAX_PEER_ID];
+    char file_name[OMNI_MAX_FILE_NAME];
+    size_t body_len;
+} udp_client_recv_meta_t;
 
+udp_client_t *udp_client_dial_with_options(const char *server_addr, const char *peer_id, const char *bind_ip, const char *bind_device, latency_logger_t *logger, tx_timestamp_debug_logger_t *debug_logger, int enable_timestamping);
 udp_client_t *udp_client_dial(const char *server_addr, const char *peer_id, const char *bind_ip, latency_logger_t *logger, tx_timestamp_debug_logger_t *debug_logger, int enable_timestamping);
 const char *udp_client_id(const udp_client_t *client);
 int udp_client_send_text(udp_client_t *client, const char *to, const char *text);
+int udp_client_send_binary(udp_client_t *client, const char *to, const void *data, size_t data_len);
 int udp_client_send_file_path(udp_client_t *client, const char *to, const char *path);
+int udp_client_receive_timed(udp_client_t *client, message_t *out_msg, int timeout_ms);
 int udp_client_receive(udp_client_t *client, message_t *out_msg);
+int udp_client_receive_into(udp_client_t *client, void *buffer, size_t buffer_len, udp_client_recv_meta_t *out_meta, int timeout_ms);
 int udp_client_persist_message(udp_client_t *client, const message_t *msg, const char *inbox_dir, char *out_path, size_t out_path_len);
 int udp_client_close(udp_client_t *client);
 void udp_client_free(udp_client_t *client);

include/server_kcp_hub.h

@@ -14,6 +14,7 @@ int kcp_hub_serve_listener(kcp_hub_t *hub, kcp_listener_t *listener);
 int kcp_hub_serve_session(kcp_hub_t *hub, kcp_conn_t *conn);
 
 int kcp_hub_set_relay(kcp_hub_t *hub, int relay_fd, const struct sockaddr *peer_addr, socklen_t peer_addr_len, int learn_peer);
+int kcp_hub_set_telemetry(kcp_hub_t *hub, const char *peer_id, int interval_ms);
 int kcp_hub_serve_relay(kcp_hub_t *hub);
 
 int kcp_hub_close(kcp_hub_t *hub);

include/transport_kcp.h

@@ -34,6 +34,14 @@ extern "C" {
 #define KCP_VIDEO_RCV_WND 256
 #define KCP_VIDEO_MTU 1400
 
+#define KCP_TELEMETRY_NODELAY 0
+#define KCP_TELEMETRY_INTERVAL_MS 50
+#define KCP_TELEMETRY_RESEND 0
+#define KCP_TELEMETRY_NC 0
+#define KCP_TELEMETRY_SND_WND 64
+#define KCP_TELEMETRY_RCV_WND 64
+#define KCP_TELEMETRY_MTU 1400
+
 #define KCP_NODELAY KCP_DEFAULT_NODELAY
 #define KCP_INTERVAL KCP_DEFAULT_INTERVAL_MS
 #define KCP_RESEND KCP_DEFAULT_RESEND
@@ -43,32 +51,27 @@ extern "C" {
 
 typedef struct kcp_conn kcp_conn_t;
 typedef struct kcp_listener kcp_listener_t;
-typedef struct kcp_conn_metrics {
+typedef struct kcp_runtime_stats {
     int connected;
-    int has_conv;
     uint32_t conv;
-    char local_addr[OMNI_MAX_ADDR_TEXT];
-    char remote_addr[OMNI_MAX_ADDR_TEXT];
     uint32_t rto_ms;
     int32_t srtt_ms;
     int32_t srttvar_ms;
-    uint64_t bytes_sent;
+    uint32_t snd_wnd;
-    uint64_t bytes_received;
+    uint32_t rmt_wnd;
-    uint64_t in_pkts;
+    uint32_t inflight;
-    uint64_t out_pkts;
+    uint32_t window_limit;
-    uint64_t in_segs;
+    double window_pressure_pct;
-    uint64_t out_segs;
+    uint32_t snd_queue;
-    uint64_t retrans_segs;
+    uint32_t rcv_queue;
-    uint64_t fast_retrans_segs;
+    uint32_t snd_buffer;
-    uint64_t early_retrans_segs;
+    uint64_t out_segs_total;
-    uint64_t lost_segs;
+    uint64_t retrans_total;
-    uint64_t repeat_segs;
+    uint64_t fast_retrans_total;
-    uint64_t in_errs;
+    uint64_t lost_total;
-    uint64_t kcp_in_errs;
+    uint64_t repeat_total;
-    uint64_t ring_buffer_snd_queue;
+    uint32_t xmit_total;
-    uint64_t ring_buffer_rcv_queue;
+} kcp_runtime_stats_t;
-    uint64_t ring_buffer_snd_buffer;
-} kcp_conn_metrics_t;
 typedef struct kcp_conn_options {
     int nodelay;
     int interval_ms;
@@ -82,6 +85,7 @@ typedef struct kcp_conn_options {
 void kcp_conn_options_init(kcp_conn_options_t *options);
 void kcp_conn_options_set_control_defaults(kcp_conn_options_t *options);
 void kcp_conn_options_set_video_defaults(kcp_conn_options_t *options);
+void kcp_conn_options_set_telemetry_defaults(kcp_conn_options_t *options);
 
 kcp_conn_t *kcp_conn_dial_with_options(const char *server_addr, const char *bind_ip, const char *bind_device, const kcp_conn_options_t *options, kcp_packet_debug_logger_t *packet_logger, latency_logger_t *logger, const char *node_role, const char *node_id, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms);
 kcp_conn_t *kcp_conn_dial(const char *server_addr, const char *bind_ip, const char *bind_device, kcp_packet_debug_logger_t *packet_logger, latency_logger_t *logger, const char *node_role, const char *node_id, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms);
@@ -94,7 +98,8 @@ int kcp_conn_close(kcp_conn_t *conn);
 void kcp_conn_free(kcp_conn_t *conn);
 uint32_t kcp_conn_conv(const kcp_conn_t *conn);
 int kcp_conn_local_addr(const kcp_conn_t *conn, struct sockaddr_storage *addr, socklen_t *addr_len);
-int kcp_conn_metrics_snapshot(kcp_conn_t *conn, kcp_conn_metrics_t *out_metrics);
+int kcp_conn_remote_addr(const kcp_conn_t *conn, struct sockaddr_storage *addr, socklen_t *addr_len);
+void kcp_conn_runtime_stats_snapshot(kcp_conn_t *conn, kcp_runtime_stats_t *out_stats);
 
 kcp_listener_t *kcp_listener_listen(const char *listen_addr, const char *bind_device, kcp_packet_debug_logger_t *packet_logger, const char *node_role, const char *node_id);
 kcp_conn_t *kcp_listener_accept(kcp_listener_t *listener);

include/video_pipeline.h

@@ -0,0 +1,52 @@
+#ifndef OMNI_VIDEO_PIPELINE_H
+#define OMNI_VIDEO_PIPELINE_H
+
+#include <pthread.h>
+#include <signal.h>
+#include <stdint.h>
+
+#include "peer_kcp_client.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct video_pipeline_config {
+    const char *camera_device;
+    const char *server_addr;
+    const char *relay_via;
+    const char *bind_ip;
+    const char *bind_device;
+    const char *peer_id;
+    const char *target_peer;
+    int capture_width;
+    int capture_height;
+    int output_width;
+    int output_height;
+    int max_frames;
+    int enable_timing_logs;
+} video_pipeline_config_t;
+
+typedef struct video_pipeline_stats {
+    pthread_mutex_t mutex;
+    uint64_t frames_sent;
+    uint64_t bytes_sent;
+    uint64_t send_errors;
+    uint64_t last_frame_bytes;
+    int connected;
+    char last_error[256];
+    kcp_runtime_stats_t transport;
+} video_pipeline_stats_t;
+
+void video_pipeline_config_init(video_pipeline_config_t *config);
+void video_pipeline_config_load_env(video_pipeline_config_t *config);
+int video_pipeline_stats_init(video_pipeline_stats_t *stats);
+void video_pipeline_stats_destroy(video_pipeline_stats_t *stats);
+void video_pipeline_stats_snapshot(video_pipeline_stats_t *stats, video_pipeline_stats_t *out_stats);
+int video_pipeline_run(const video_pipeline_config_t *config, video_pipeline_stats_t *stats, volatile sig_atomic_t *stop_requested);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

python/omnisocket/__init__.py

@@ -6,6 +6,7 @@ try:
         MSG_TYPE_REGISTER,
         MSG_TYPE_TEXT,
         Session,
+        UdpSession,
     )
 except ImportError as exc:
     raise ImportError(
@@ -32,8 +33,19 @@ VIDEO_DEFAULTS = {
     "mtu": 1400,
 }
 
+TELEMETRY_DEFAULTS = {
+    "nodelay": 0,
+    "interval_ms": 50,
+    "resend": 0,
+    "nc": 0,
+    "sndwnd": 64,
+    "rcvwnd": 64,
+    "mtu": 1400,
+}
+
 __all__ = [
     "CONTROL_DEFAULTS",
+    "TELEMETRY_DEFAULTS",
     "VIDEO_DEFAULTS",
     "MSG_TYPE_BINARY",
     "MSG_TYPE_ERROR",
@@ -41,4 +53,5 @@ __all__ = [
     "MSG_TYPE_REGISTER",
     "MSG_TYPE_TEXT",
     "Session",
+    "UdpSession",
 ]

python/omnisocket/_omnisocket.c

@@ -8,6 +8,11 @@ typedef struct PyOmniSession {
     omnisocket_session_t session;
 } PyOmniSession;
 
+typedef struct PyOmniUdpSession {
+    PyObject_HEAD
+    omnisocket_udp_session_t session;
+} PyOmniUdpSession;
+
 PyDoc_STRVAR(
     PyOmniSession_recv_doc,
     "recv(timeout_ms=-1) -> (from_peer, msg_type, payload) | None"
@@ -22,12 +27,114 @@ PyDoc_STRVAR(
     "current frame has already been consumed and is lost."
 );
 
-PyDoc_STRVAR(
+static PyObject *build_recv_result(const message_t *msg) {
-    PyOmniSession_kcp_metrics_doc,
+    PyObject *body = NULL;
-    "kcp_metrics() -> dict\n"
+    PyObject *result = NULL;
-    "\n"
+
-    "Return a snapshot of low-level KCP metrics for the current session."
+    body = PyBytes_FromStringAndSize((const char *) msg->body, (Py_ssize_t) msg->body_len);
-);
+    if (body == NULL) {
+        return NULL;
+    }
+    result = Py_BuildValue("(siO)", msg->from, (int) msg->type, body);
+    Py_DECREF(body);
+    return result;
+}
+
+static PyObject *build_recv_meta_dict(
+    const char *from_peer,
+    const char *to_peer,
+    const char *file_name,
+    int msg_type,
+    unsigned long long message_id,
+    unsigned long long body_len
+) {
+    return Py_BuildValue(
+        "{s:s,s:s,s:s,s:i,s:K,s:K}",
+        "from",
+        from_peer,
+        "to",
+        to_peer,
+        "file_name",
+        file_name,
+        "msg_type",
+        msg_type,
+        "message_id",
+        message_id,
+        "body_len",
+        body_len
+    );
+}
+
+static PyObject *build_stats_dict(const omnisocket_session_stats_t *stats) {
+    return Py_BuildValue(
+        "{s:K,s:K,s:K,s:K,s:K,s:K,s:K,s:i}",
+        "send_calls",
+        (unsigned long long) stats->send_calls,
+        "send_bytes",
+        (unsigned long long) stats->send_bytes,
+        "send_errors",
+        (unsigned long long) stats->send_errors,
+        "recv_calls",
+        (unsigned long long) stats->recv_calls,
+        "recv_bytes",
+        (unsigned long long) stats->recv_bytes,
+        "recv_timeouts",
+        (unsigned long long) stats->recv_timeouts,
+        "recv_errors",
+        (unsigned long long) stats->recv_errors,
+        "connected",
+        stats->connected
+    );
+}
+
+static PyObject *build_kcp_stats_dict(const omnisocket_session_kcp_stats_t *stats) {
+    PyObject *dict = PyDict_New();
+    PyObject *value = NULL;
+
+    if (dict == NULL) {
+        return NULL;
+    }
+
+#define SET_KCP_STAT(key, expr)                      \
+    do {                                            \
+        value = (expr);                             \
+        if (value == NULL) {                        \
+            Py_DECREF(dict);                        \
+            return NULL;                            \
+        }                                           \
+        if (PyDict_SetItemString(dict, (key), value) != 0) { \
+            Py_DECREF(value);                       \
+            Py_DECREF(dict);                        \
+            return NULL;                            \
+        }                                           \
+        Py_DECREF(value);                           \
+        value = NULL;                               \
+    } while (0)
+
+    SET_KCP_STAT("connected", PyLong_FromLong(stats->connected));
+    SET_KCP_STAT("conv", PyLong_FromUnsignedLong(stats->conv));
+    SET_KCP_STAT("rto_ms", PyLong_FromUnsignedLong(stats->rto_ms));
+    SET_KCP_STAT("srtt_ms", PyLong_FromLong(stats->srtt_ms));
+    SET_KCP_STAT("srttvar_ms", PyLong_FromLong(stats->srttvar_ms));
+    SET_KCP_STAT("snd_wnd", PyLong_FromUnsignedLong(stats->snd_wnd));
+    SET_KCP_STAT("rmt_wnd", PyLong_FromUnsignedLong(stats->rmt_wnd));
+    SET_KCP_STAT("inflight", PyLong_FromUnsignedLong(stats->inflight));
+    SET_KCP_STAT("window_limit", PyLong_FromUnsignedLong(stats->window_limit));
+    SET_KCP_STAT("window_pressure_pct", PyFloat_FromDouble(stats->window_pressure_pct));
+    SET_KCP_STAT("snd_queue", PyLong_FromUnsignedLong(stats->snd_queue));
+    SET_KCP_STAT("rcv_queue", PyLong_FromUnsignedLong(stats->rcv_queue));
+    SET_KCP_STAT("snd_buffer", PyLong_FromUnsignedLong(stats->snd_buffer));
+    SET_KCP_STAT("out_segs_total", PyLong_FromUnsignedLongLong((unsigned long long) stats->out_segs_total));
+    SET_KCP_STAT("retrans_total", PyLong_FromUnsignedLongLong((unsigned long long) stats->retrans_total));
+    SET_KCP_STAT("fast_retrans_total", PyLong_FromUnsignedLongLong((unsigned long long) stats->fast_retrans_total));
+    SET_KCP_STAT("lost_total", PyLong_FromUnsignedLongLong((unsigned long long) stats->lost_total));
+    SET_KCP_STAT("repeat_total", PyLong_FromUnsignedLongLong((unsigned long long) stats->repeat_total));
+    SET_KCP_STAT("xmit_total", PyLong_FromUnsignedLong(stats->xmit_total));
+
+#undef SET_KCP_STAT
+
+    return dict;
+}
 
 static PyObject *PyOmniSession_new(PyTypeObject *type, PyObject *args, PyObject *kwargs) {
     PyOmniSession *self;
@@ -172,7 +279,6 @@ static PyObject *PyOmniSession_recv(PyOmniSession *self, PyObject *args, PyObjec
     int timeout_ms = -1;
     int rc;
     message_t msg;
-    PyObject *body = NULL;
     PyObject *result = NULL;
     static char *kwlist[] = {"timeout_ms", NULL};
 
@@ -194,13 +300,7 @@ static PyObject *PyOmniSession_recv(PyOmniSession *self, PyObject *args, PyObjec
         return PyErr_SetFromErrno(PyExc_OSError);
     }
 
-    body = PyBytes_FromStringAndSize((const char *) msg.body, (Py_ssize_t) msg.body_len);
+    result = build_recv_result(&msg);
-    if (body == NULL) {
-        protocol_message_clear(&msg);
-        return NULL;
-    }
-    result = Py_BuildValue("(siO)", msg.from, (int) msg.type, body);
-    Py_DECREF(body);
     protocol_message_clear(&msg);
     return result;
 }
@@ -244,19 +344,12 @@ static PyObject *PyOmniSession_recv_into(PyOmniSession *self, PyObject *args, Py
         return PyErr_SetFromErrno(PyExc_OSError);
     }
 
-    result = Py_BuildValue(
+    result = build_recv_meta_dict(
-        "{s:s,s:s,s:s,s:i,s:K,s:K}",
-        "from",
         meta.from,
-        "to",
         meta.to,
-        "file_name",
         meta.file_name,
-        "msg_type",
         (int) meta.type,
-        "message_id",
         (unsigned long long) meta.id,
-        "body_len",
         (unsigned long long) meta.body_len
     );
     return result;
@@ -267,86 +360,15 @@ static PyObject *PyOmniSession_stats(PyOmniSession *self, PyObject *Py_UNUSED(ig
 
     memset(&stats, 0, sizeof(stats));
     omnisocket_session_stats_snapshot(&self->session, &stats);
-    return Py_BuildValue(
+    return build_stats_dict(&stats);
-        "{s:K,s:K,s:K,s:K,s:K,s:K,s:K,s:i}",
-        "send_calls",
-        (unsigned long long) stats.send_calls,
-        "send_bytes",
-        (unsigned long long) stats.send_bytes,
-        "send_errors",
-        (unsigned long long) stats.send_errors,
-        "recv_calls",
-        (unsigned long long) stats.recv_calls,
-        "recv_bytes",
-        (unsigned long long) stats.recv_bytes,
-        "recv_timeouts",
-        (unsigned long long) stats.recv_timeouts,
-        "recv_errors",
-        (unsigned long long) stats.recv_errors,
-        "connected",
-        stats.connected
-    );
 }
 
-static PyObject *PyOmniSession_kcp_metrics(PyOmniSession *self, PyObject *Py_UNUSED(ignored)) {
+static PyObject *PyOmniSession_kcp_stats(PyOmniSession *self, PyObject *Py_UNUSED(ignored)) {
-    omnisocket_session_kcp_metrics_t metrics;
+    omnisocket_session_kcp_stats_t stats;
 
-    memset(&metrics, 0, sizeof(metrics));
+    memset(&stats, 0, sizeof(stats));
-    if (omnisocket_session_kcp_metrics_snapshot(&self->session, &metrics) != 0) {
+    omnisocket_session_kcp_stats_snapshot(&self->session, &stats);
-        return PyErr_SetFromErrno(PyExc_OSError);
+    return build_kcp_stats_dict(&stats);
-    }
-
-    return Py_BuildValue(
-        "{s:i,s:i,s:I,s:s,s:s,s:I,s:i,s:i,s:K,s:K,s:K,s:K,s:K,s:K,s:K,s:K,s:K,s:K,s:K,s:K,s:K,s:K,s:K}",
-        "connected",
-        metrics.connected,
-        "has_conv",
-        metrics.has_conv,
-        "conv",
-        metrics.conv,
-        "local_addr",
-        metrics.local_addr,
-        "remote_addr",
-        metrics.remote_addr,
-        "rto_ms",
-        metrics.rto_ms,
-        "srtt_ms",
-        metrics.srtt_ms,
-        "srttvar_ms",
-        metrics.srttvar_ms,
-        "bytes_sent",
-        (unsigned long long) metrics.bytes_sent,
-        "bytes_received",
-        (unsigned long long) metrics.bytes_received,
-        "in_pkts",
-        (unsigned long long) metrics.in_pkts,
-        "out_pkts",
-        (unsigned long long) metrics.out_pkts,
-        "in_segs",
-        (unsigned long long) metrics.in_segs,
-        "out_segs",
-        (unsigned long long) metrics.out_segs,
-        "retrans_segs",
-        (unsigned long long) metrics.retrans_segs,
-        "fast_retrans_segs",
-        (unsigned long long) metrics.fast_retrans_segs,
-        "early_retrans_segs",
-        (unsigned long long) metrics.early_retrans_segs,
-        "lost_segs",
-        (unsigned long long) metrics.lost_segs,
-        "repeat_segs",
-        (unsigned long long) metrics.repeat_segs,
-        "in_errs",
-        (unsigned long long) metrics.in_errs,
-        "kcp_in_errs",
-        (unsigned long long) metrics.kcp_in_errs,
-        "ring_buffer_snd_queue",
-        (unsigned long long) metrics.ring_buffer_snd_queue,
-        "ring_buffer_rcv_queue",
-        (unsigned long long) metrics.ring_buffer_rcv_queue,
-        "ring_buffer_snd_buffer",
-        (unsigned long long) metrics.ring_buffer_snd_buffer
-    );
 }
 
 static PyMethodDef PyOmniSession_methods[] = {
@@ -356,7 +378,7 @@ static PyMethodDef PyOmniSession_methods[] = {
     {"recv", (PyCFunction) PyOmniSession_recv, METH_VARARGS | METH_KEYWORDS, PyOmniSession_recv_doc},
     {"recv_into", (PyCFunction) PyOmniSession_recv_into, METH_VARARGS | METH_KEYWORDS, PyOmniSession_recv_into_doc},
     {"stats", (PyCFunction) PyOmniSession_stats, METH_NOARGS, NULL},
-    {"kcp_metrics", (PyCFunction) PyOmniSession_kcp_metrics, METH_NOARGS, PyOmniSession_kcp_metrics_doc},
+    {"kcp_stats", (PyCFunction) PyOmniSession_kcp_stats, METH_NOARGS, NULL},
     {NULL, NULL, 0, NULL}
 };
 
@@ -364,6 +386,211 @@ static PyTypeObject PyOmniSessionType = {
     PyVarObject_HEAD_INIT(NULL, 0)
 };
 
+static PyObject *PyOmniUdpSession_new(PyTypeObject *type, PyObject *args, PyObject *kwargs) {
+    PyOmniUdpSession *self;
+    (void) args;
+    (void) kwargs;
+
+    self = (PyOmniUdpSession *) type->tp_alloc(type, 0);
+    if (self == NULL) {
+        return NULL;
+    }
+    if (omnisocket_udp_session_init(&self->session) != 0) {
+        type->tp_free((PyObject *) self);
+        return PyErr_SetFromErrno(PyExc_OSError);
+    }
+    return (PyObject *) self;
+}
+
+static void PyOmniUdpSession_dealloc(PyOmniUdpSession *self) {
+    omnisocket_udp_session_destroy(&self->session);
+    Py_TYPE(self)->tp_free((PyObject *) self);
+}
+
+static PyObject *PyOmniUdpSession_connect(PyOmniUdpSession *self, PyObject *args, PyObject *kwargs) {
+    const char *server_addr;
+    const char *peer_id;
+    const char *bind_ip = "";
+    const char *bind_device = "";
+    int enable_timestamping = 0;
+    int rc;
+
+    static char *kwlist[] = {
+        "server_addr",
+        "peer_id",
+        "bind_ip",
+        "bind_device",
+        "enable_timestamping",
+        NULL
+    };
+
+    if (!PyArg_ParseTupleAndKeywords(
+            args,
+            kwargs,
+            "ss|ssi",
+            kwlist,
+            &server_addr,
+            &peer_id,
+            &bind_ip,
+            &bind_device,
+            &enable_timestamping)) {
+        return NULL;
+    }
+
+    Py_BEGIN_ALLOW_THREADS
+    rc = omnisocket_udp_session_connect(
+        &self->session,
+        server_addr,
+        peer_id,
+        bind_ip,
+        bind_device,
+        enable_timestamping
+    );
+    Py_END_ALLOW_THREADS
+
+    if (rc != 0) {
+        return PyErr_SetFromErrno(PyExc_OSError);
+    }
+    Py_RETURN_NONE;
+}
+
+static PyObject *PyOmniUdpSession_close(PyOmniUdpSession *self, PyObject *Py_UNUSED(ignored)) {
+    int rc;
+
+    Py_BEGIN_ALLOW_THREADS
+    rc = omnisocket_udp_session_close(&self->session);
+    Py_END_ALLOW_THREADS
+
+    if (rc != 0) {
+        return PyErr_SetFromErrno(PyExc_OSError);
+    }
+    Py_RETURN_NONE;
+}
+
+static PyObject *PyOmniUdpSession_send(PyOmniUdpSession *self, PyObject *args, PyObject *kwargs) {
+    const char *to;
+    Py_buffer payload;
+    int rc;
+    static char *kwlist[] = {"to", "data", NULL};
+
+    memset(&payload, 0, sizeof(payload));
+    if (!PyArg_ParseTupleAndKeywords(args, kwargs, "sy*", kwlist, &to, &payload)) {
+        return NULL;
+    }
+
+    Py_BEGIN_ALLOW_THREADS
+    rc = omnisocket_udp_session_send(&self->session, to, payload.buf, (size_t) payload.len);
+    Py_END_ALLOW_THREADS
+
+    PyBuffer_Release(&payload);
+    if (rc != 0) {
+        return PyErr_SetFromErrno(PyExc_OSError);
+    }
+    Py_RETURN_NONE;
+}
+
+static PyObject *PyOmniUdpSession_recv(PyOmniUdpSession *self, PyObject *args, PyObject *kwargs) {
+    int timeout_ms = -1;
+    int rc;
+    message_t msg;
+    PyObject *result = NULL;
+    static char *kwlist[] = {"timeout_ms", NULL};
+
+    if (!PyArg_ParseTupleAndKeywords(args, kwargs, "|i", kwlist, &timeout_ms)) {
+        return NULL;
+    }
+
+    protocol_message_init(&msg);
+    Py_BEGIN_ALLOW_THREADS
+    rc = omnisocket_udp_session_recv(&self->session, &msg, timeout_ms);
+    Py_END_ALLOW_THREADS
+
+    if (rc == 1) {
+        protocol_message_clear(&msg);
+        Py_RETURN_NONE;
+    }
+    if (rc != 0) {
+        protocol_message_clear(&msg);
+        return PyErr_SetFromErrno(PyExc_OSError);
+    }
+
+    result = build_recv_result(&msg);
+    protocol_message_clear(&msg);
+    return result;
+}
+
+static PyObject *PyOmniUdpSession_recv_into(PyOmniUdpSession *self, PyObject *args, PyObject *kwargs) {
+    PyObject *buffer_obj;
+    Py_buffer view;
+    int timeout_ms = -1;
+    int rc;
+    udp_client_recv_meta_t meta;
+    PyObject *result = NULL;
+    static char *kwlist[] = {"buffer", "timeout_ms", NULL};
+
+    memset(&view, 0, sizeof(view));
+    memset(&meta, 0, sizeof(meta));
+
+    if (!PyArg_ParseTupleAndKeywords(args, kwargs, "O|i", kwlist, &buffer_obj, &timeout_ms)) {
+        return NULL;
+    }
+    if (PyObject_GetBuffer(buffer_obj, &view, PyBUF_WRITABLE) != 0) {
+        return NULL;
+    }
+
+    Py_BEGIN_ALLOW_THREADS
+    rc = omnisocket_udp_session_recv_into(&self->session, view.buf, (size_t) view.len, &meta, timeout_ms);
+    Py_END_ALLOW_THREADS
+
+    PyBuffer_Release(&view);
+    if (rc == 1) {
+        Py_RETURN_NONE;
+    }
+    if (rc == 2) {
+        PyErr_Format(
+            PyExc_BufferError,
+            "buffer too small: need %zu bytes; current frame was already consumed and dropped",
+            meta.body_len
+        );
+        return NULL;
+    }
+    if (rc != 0) {
+        return PyErr_SetFromErrno(PyExc_OSError);
+    }
+
+    result = build_recv_meta_dict(
+        meta.from,
+        meta.to,
+        meta.file_name,
+        (int) meta.type,
+        (unsigned long long) meta.id,
+        (unsigned long long) meta.body_len
+    );
+    return result;
+}
+
+static PyObject *PyOmniUdpSession_stats(PyOmniUdpSession *self, PyObject *Py_UNUSED(ignored)) {
+    omnisocket_session_stats_t stats;
+
+    memset(&stats, 0, sizeof(stats));
+    omnisocket_udp_session_stats_snapshot(&self->session, &stats);
+    return build_stats_dict(&stats);
+}
+
+static PyMethodDef PyOmniUdpSession_methods[] = {
+    {"connect", (PyCFunction) PyOmniUdpSession_connect, METH_VARARGS | METH_KEYWORDS, NULL},
+    {"close", (PyCFunction) PyOmniUdpSession_close, METH_NOARGS, NULL},
+    {"send", (PyCFunction) PyOmniUdpSession_send, METH_VARARGS | METH_KEYWORDS, NULL},
+    {"recv", (PyCFunction) PyOmniUdpSession_recv, METH_VARARGS | METH_KEYWORDS, PyOmniSession_recv_doc},
+    {"recv_into", (PyCFunction) PyOmniUdpSession_recv_into, METH_VARARGS | METH_KEYWORDS, PyOmniSession_recv_into_doc},
+    {"stats", (PyCFunction) PyOmniUdpSession_stats, METH_NOARGS, NULL},
+    {NULL, NULL, 0, NULL}
+};
+
+static PyTypeObject PyOmniUdpSessionType = {
+    PyVarObject_HEAD_INIT(NULL, 0)
+};
+
 static PyModuleDef omnisocket_module = {
     PyModuleDef_HEAD_INIT,
     .m_name = "_omnisocket",
@@ -384,6 +611,17 @@ PyMODINIT_FUNC PyInit__omnisocket(void) {
         return NULL;
     }
 
+    PyOmniUdpSessionType.tp_name = "omnisocket.UdpSession";
+    PyOmniUdpSessionType.tp_basicsize = sizeof(PyOmniUdpSession);
+    PyOmniUdpSessionType.tp_flags = Py_TPFLAGS_DEFAULT;
+    PyOmniUdpSessionType.tp_new = PyOmniUdpSession_new;
+    PyOmniUdpSessionType.tp_dealloc = (destructor) PyOmniUdpSession_dealloc;
+    PyOmniUdpSessionType.tp_methods = PyOmniUdpSession_methods;
+
+    if (PyType_Ready(&PyOmniUdpSessionType) < 0) {
+        return NULL;
+    }
+
     module = PyModule_Create(&omnisocket_module);
     if (module == NULL) {
         return NULL;
@@ -396,6 +634,13 @@ PyMODINIT_FUNC PyInit__omnisocket(void) {
         return NULL;
     }
 
+    Py_INCREF(&PyOmniUdpSessionType);
+    if (PyModule_AddObject(module, "UdpSession", (PyObject *) &PyOmniUdpSessionType) != 0) {
+        Py_DECREF(&PyOmniUdpSessionType);
+        Py_DECREF(module);
+        return NULL;
+    }
+
     if (PyModule_AddIntConstant(module, "MSG_TYPE_TEXT", MSG_TYPE_TEXT) != 0 ||
         PyModule_AddIntConstant(module, "MSG_TYPE_FILE", MSG_TYPE_FILE) != 0 ||
         PyModule_AddIntConstant(module, "MSG_TYPE_REGISTER", MSG_TYPE_REGISTER) != 0 ||

python/omnisocket/omnisocket_client.c

@@ -247,36 +247,195 @@ void omnisocket_session_stats_snapshot(omnisocket_session_t *session, omnisocket
     pthread_mutex_unlock(&session->mutex);
 }
 
-int omnisocket_session_kcp_metrics_snapshot(
+void omnisocket_session_kcp_stats_snapshot(omnisocket_session_t *session, omnisocket_session_kcp_stats_t *out_stats) {
-    omnisocket_session_t *session,
+    kcp_runtime_stats_t runtime_stats;
-    omnisocket_session_kcp_metrics_t *out_metrics
-) {
-    kcp_client_t *client = NULL;
-    kcp_conn_metrics_t metrics;
-    int rc = 0;
 
-    if (session == NULL || out_metrics == NULL) {
+    if (session == NULL || out_stats == NULL) {
+        return;
+    }
+
+    memset(&runtime_stats, 0, sizeof(runtime_stats));
+    pthread_mutex_lock(&session->mutex);
+    if (session->client != NULL) {
+        kcp_client_runtime_stats_snapshot(session->client, &runtime_stats);
+    }
+    pthread_mutex_unlock(&session->mutex);
+
+    memset(out_stats, 0, sizeof(*out_stats));
+    out_stats->connected = runtime_stats.connected;
+    out_stats->conv = runtime_stats.conv;
+    out_stats->rto_ms = runtime_stats.rto_ms;
+    out_stats->srtt_ms = runtime_stats.srtt_ms;
+    out_stats->srttvar_ms = runtime_stats.srttvar_ms;
+    out_stats->snd_wnd = runtime_stats.snd_wnd;
+    out_stats->rmt_wnd = runtime_stats.rmt_wnd;
+    out_stats->inflight = runtime_stats.inflight;
+    out_stats->window_limit = runtime_stats.window_limit;
+    out_stats->window_pressure_pct = runtime_stats.window_pressure_pct;
+    out_stats->snd_queue = runtime_stats.snd_queue;
+    out_stats->rcv_queue = runtime_stats.rcv_queue;
+    out_stats->snd_buffer = runtime_stats.snd_buffer;
+    out_stats->out_segs_total = runtime_stats.out_segs_total;
+    out_stats->retrans_total = runtime_stats.retrans_total;
+    out_stats->fast_retrans_total = runtime_stats.fast_retrans_total;
+    out_stats->lost_total = runtime_stats.lost_total;
+    out_stats->repeat_total = runtime_stats.repeat_total;
+    out_stats->xmit_total = runtime_stats.xmit_total;
+}
+
+int omnisocket_udp_session_init(omnisocket_udp_session_t *session) {
+    int rc;
+
+    if (session == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+    memset(session, 0, sizeof(*session));
+    rc = pthread_mutex_init(&session->mutex, NULL);
+    if (rc != 0) {
+        errno = rc;
+        return -1;
+    }
+    rc = pthread_cond_init(&session->idle_cond, NULL);
+    if (rc != 0) {
+        pthread_mutex_destroy(&session->mutex);
+        errno = rc;
+        return -1;
+    }
+    return 0;
+}
+
+void omnisocket_udp_session_destroy(omnisocket_udp_session_t *session) {
+    if (session == NULL) {
+        return;
+    }
+    (void) omnisocket_udp_session_close(session);
+    pthread_cond_destroy(&session->idle_cond);
+    pthread_mutex_destroy(&session->mutex);
+}
+
+static int omnisocket_udp_session_begin_client_op(omnisocket_udp_session_t *session, udp_client_t **out_client) {
+    if (session == NULL || out_client == NULL) {
         errno = EINVAL;
         return -1;
     }
 
-    memset(out_metrics, 0, sizeof(*out_metrics));
+    pthread_mutex_lock(&session->mutex);
+    if (session->closing) {
+        pthread_mutex_unlock(&session->mutex);
+        errno = ECANCELED;
+        return -1;
+    }
+    if (session->client == NULL) {
+        pthread_mutex_unlock(&session->mutex);
+        errno = ENOTCONN;
+        return -1;
+    }
+    *out_client = session->client;
+    session->active_ops += 1;
+    pthread_mutex_unlock(&session->mutex);
+    return 0;
+}
+
+int omnisocket_udp_session_connect(
+    omnisocket_udp_session_t *session,
+    const char *server_addr,
+    const char *peer_id,
+    const char *bind_ip,
+    const char *bind_device,
+    int enable_timestamping
+) {
+    udp_client_t *client;
+
+    if (session == NULL || server_addr == NULL || peer_id == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
 
     pthread_mutex_lock(&session->mutex);
-    if (session->client != NULL && !session->closing) {
+    while (session->closing) {
-        client = session->client;
+        pthread_cond_wait(&session->idle_cond, &session->mutex);
-        session->active_ops += 1;
     }
+    if (session->client != NULL) {
+        pthread_mutex_unlock(&session->mutex);
+        errno = EISCONN;
+        return -1;
+    }
+    client = udp_client_dial_with_options(
+        server_addr,
+        peer_id,
+        bind_ip,
+        bind_device,
+        NULL,
+        NULL,
+        enable_timestamping
+    );
+    if (client == NULL) {
+        pthread_mutex_unlock(&session->mutex);
+        return -1;
+    }
+    session->client = client;
+    session->stats.connected = 1;
+    pthread_mutex_unlock(&session->mutex);
+    return 0;
+}
+
+int omnisocket_udp_session_close(omnisocket_udp_session_t *session) {
+    udp_client_t *client;
+
+    if (session == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    pthread_mutex_lock(&session->mutex);
+    while (session->closing) {
+        pthread_cond_wait(&session->idle_cond, &session->mutex);
+    }
+    client = session->client;
+    if (client != NULL) {
+        session->closing = 1;
+        session->client = NULL;
+    }
+    session->stats.connected = 0;
     pthread_mutex_unlock(&session->mutex);
 
-    if (client == NULL) {
+    if (client != NULL) {
-        return 0;
+        udp_client_close(client);
+        pthread_mutex_lock(&session->mutex);
+        while (session->active_ops > 0) {
+            pthread_cond_wait(&session->idle_cond, &session->mutex);
+        }
+        pthread_mutex_unlock(&session->mutex);
+        udp_client_free(client);
+        pthread_mutex_lock(&session->mutex);
+        session->closing = 0;
+        pthread_cond_broadcast(&session->idle_cond);
+        pthread_mutex_unlock(&session->mutex);
+    }
+    return 0;
+}
+
+int omnisocket_udp_session_send(omnisocket_udp_session_t *session, const char *to, const void *data, size_t data_len) {
+    udp_client_t *client;
+    int rc;
+
+    if (session == NULL || to == NULL || (data == NULL && data_len > 0)) {
+        errno = EINVAL;
+        return -1;
     }
 
-    memset(&metrics, 0, sizeof(metrics));
+    if (omnisocket_udp_session_begin_client_op(session, &client) != 0) {
-    rc = kcp_client_metrics_snapshot(client, &metrics);
+        return -1;
-
+    }
+    rc = udp_client_send_binary(client, to, data, data_len);
     pthread_mutex_lock(&session->mutex);
+    if (rc == 0) {
+        session->stats.send_calls += 1;
+        session->stats.send_bytes += (uint64_t) data_len;
+    } else {
+        session->stats.send_errors += 1;
+    }
     if (session->active_ops > 0) {
         session->active_ops -= 1;
     }
@@ -284,34 +443,84 @@ int omnisocket_session_kcp_metrics_snapshot(
         pthread_cond_broadcast(&session->idle_cond);
     }
     pthread_mutex_unlock(&session->mutex);
+    return rc;
+}
 
-    if (rc != 0) {
+int omnisocket_udp_session_recv(omnisocket_udp_session_t *session, message_t *out_msg, int timeout_ms) {
-        return rc;
+    udp_client_t *client;
+    int rc;
+
+    if (session == NULL || out_msg == NULL) {
+        errno = EINVAL;
+        return -1;
     }
 
-    out_metrics->connected = metrics.connected;
+    if (omnisocket_udp_session_begin_client_op(session, &client) != 0) {
-    out_metrics->has_conv = metrics.has_conv;
+        return -1;
-    out_metrics->conv = metrics.conv;
+    }
-    snprintf(out_metrics->local_addr, sizeof(out_metrics->local_addr), "%s", metrics.local_addr);
+    rc = udp_client_receive_timed(client, out_msg, timeout_ms);
-    snprintf(out_metrics->remote_addr, sizeof(out_metrics->remote_addr), "%s", metrics.remote_addr);
+    pthread_mutex_lock(&session->mutex);
-    out_metrics->rto_ms = metrics.rto_ms;
+    if (rc == 0) {
-    out_metrics->srtt_ms = metrics.srtt_ms;
+        session->stats.recv_calls += 1;
-    out_metrics->srttvar_ms = metrics.srttvar_ms;
+        session->stats.recv_bytes += (uint64_t) out_msg->body_len;
-    out_metrics->bytes_sent = metrics.bytes_sent;
+    } else if (rc == 1) {
-    out_metrics->bytes_received = metrics.bytes_received;
+        session->stats.recv_timeouts += 1;
-    out_metrics->in_pkts = metrics.in_pkts;
+    } else {
-    out_metrics->out_pkts = metrics.out_pkts;
+        session->stats.recv_errors += 1;
-    out_metrics->in_segs = metrics.in_segs;
+    }
-    out_metrics->out_segs = metrics.out_segs;
+    if (session->active_ops > 0) {
-    out_metrics->retrans_segs = metrics.retrans_segs;
+        session->active_ops -= 1;
-    out_metrics->fast_retrans_segs = metrics.fast_retrans_segs;
+    }
-    out_metrics->early_retrans_segs = metrics.early_retrans_segs;
+    if (session->closing && session->active_ops == 0) {
-    out_metrics->lost_segs = metrics.lost_segs;
+        pthread_cond_broadcast(&session->idle_cond);
-    out_metrics->repeat_segs = metrics.repeat_segs;
+    }
-    out_metrics->in_errs = metrics.in_errs;
+    pthread_mutex_unlock(&session->mutex);
-    out_metrics->kcp_in_errs = metrics.kcp_in_errs;
+    return rc;
-    out_metrics->ring_buffer_snd_queue = metrics.ring_buffer_snd_queue;
+}
-    out_metrics->ring_buffer_rcv_queue = metrics.ring_buffer_rcv_queue;
+
-    out_metrics->ring_buffer_snd_buffer = metrics.ring_buffer_snd_buffer;
+int omnisocket_udp_session_recv_into(
-    return 0;
+    omnisocket_udp_session_t *session,
+    void *buffer,
+    size_t buffer_len,
+    udp_client_recv_meta_t *out_meta,
+    int timeout_ms
+) {
+    udp_client_t *client;
+    int rc;
+
+    if (session == NULL || out_meta == NULL || (buffer == NULL && buffer_len > 0)) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    if (omnisocket_udp_session_begin_client_op(session, &client) != 0) {
+        return -1;
+    }
+    rc = udp_client_receive_into(client, buffer, buffer_len, out_meta, timeout_ms);
+    pthread_mutex_lock(&session->mutex);
+    if (rc == 0) {
+        session->stats.recv_calls += 1;
+        session->stats.recv_bytes += (uint64_t) out_meta->body_len;
+    } else if (rc == 1) {
+        session->stats.recv_timeouts += 1;
+    } else {
+        session->stats.recv_errors += 1;
+    }
+    if (session->active_ops > 0) {
+        session->active_ops -= 1;
+    }
+    if (session->closing && session->active_ops == 0) {
+        pthread_cond_broadcast(&session->idle_cond);
+    }
+    pthread_mutex_unlock(&session->mutex);
+    return rc;
+}
+
+void omnisocket_udp_session_stats_snapshot(omnisocket_udp_session_t *session, omnisocket_session_stats_t *out_stats) {
+    if (session == NULL || out_stats == NULL) {
+        return;
+    }
+    pthread_mutex_lock(&session->mutex);
+    *out_stats = session->stats;
+    pthread_mutex_unlock(&session->mutex);
 }

python/omnisocket/omnisocket_client.h

@@ -2,6 +2,7 @@
 #define OMNISOCKET_PY_CLIENT_H
 
 #include "peer_kcp_client.h"
+#include "peer_udp_client.h"
 
 typedef struct omnisocket_session_stats {
     uint64_t send_calls;
@@ -14,32 +15,27 @@ typedef struct omnisocket_session_stats {
     int connected;
 } omnisocket_session_stats_t;
 
-typedef struct omnisocket_session_kcp_metrics {
+typedef struct omnisocket_session_kcp_stats {
     int connected;
-    int has_conv;
     uint32_t conv;
-    char local_addr[OMNI_MAX_ADDR_TEXT];
-    char remote_addr[OMNI_MAX_ADDR_TEXT];
     uint32_t rto_ms;
     int32_t srtt_ms;
     int32_t srttvar_ms;
-    uint64_t bytes_sent;
+    uint32_t snd_wnd;
-    uint64_t bytes_received;
+    uint32_t rmt_wnd;
-    uint64_t in_pkts;
+    uint32_t inflight;
-    uint64_t out_pkts;
+    uint32_t window_limit;
-    uint64_t in_segs;
+    double window_pressure_pct;
-    uint64_t out_segs;
+    uint32_t snd_queue;
-    uint64_t retrans_segs;
+    uint32_t rcv_queue;
-    uint64_t fast_retrans_segs;
+    uint32_t snd_buffer;
-    uint64_t early_retrans_segs;
+    uint64_t out_segs_total;
-    uint64_t lost_segs;
+    uint64_t retrans_total;
-    uint64_t repeat_segs;
+    uint64_t fast_retrans_total;
-    uint64_t in_errs;
+    uint64_t lost_total;
-    uint64_t kcp_in_errs;
+    uint64_t repeat_total;
-    uint64_t ring_buffer_snd_queue;
+    uint32_t xmit_total;
-    uint64_t ring_buffer_rcv_queue;
+} omnisocket_session_kcp_stats_t;
-    uint64_t ring_buffer_snd_buffer;
-} omnisocket_session_kcp_metrics_t;
 
 typedef struct omnisocket_session {
     pthread_mutex_t mutex;
@@ -50,6 +46,15 @@ typedef struct omnisocket_session {
     omnisocket_session_stats_t stats;
 } omnisocket_session_t;
 
+typedef struct omnisocket_udp_session {
+    pthread_mutex_t mutex;
+    pthread_cond_t idle_cond;
+    udp_client_t *client;
+    size_t active_ops;
+    int closing;
+    omnisocket_session_stats_t stats;
+} omnisocket_udp_session_t;
+
 int omnisocket_session_init(omnisocket_session_t *session);
 void omnisocket_session_destroy(omnisocket_session_t *session);
 
@@ -74,9 +79,29 @@ int omnisocket_session_recv_into(
     int timeout_ms
 );
 void omnisocket_session_stats_snapshot(omnisocket_session_t *session, omnisocket_session_stats_t *out_stats);
-int omnisocket_session_kcp_metrics_snapshot(
+void omnisocket_session_kcp_stats_snapshot(omnisocket_session_t *session, omnisocket_session_kcp_stats_t *out_stats);
-    omnisocket_session_t *session,
+
-    omnisocket_session_kcp_metrics_t *out_metrics
+int omnisocket_udp_session_init(omnisocket_udp_session_t *session);
+void omnisocket_udp_session_destroy(omnisocket_udp_session_t *session);
+
+int omnisocket_udp_session_connect(
+    omnisocket_udp_session_t *session,
+    const char *server_addr,
+    const char *peer_id,
+    const char *bind_ip,
+    const char *bind_device,
+    int enable_timestamping
 );
+int omnisocket_udp_session_close(omnisocket_udp_session_t *session);
+int omnisocket_udp_session_send(omnisocket_udp_session_t *session, const char *to, const void *data, size_t data_len);
+int omnisocket_udp_session_recv(omnisocket_udp_session_t *session, message_t *out_msg, int timeout_ms);
+int omnisocket_udp_session_recv_into(
+    omnisocket_udp_session_t *session,
+    void *buffer,
+    size_t buffer_len,
+    udp_client_recv_meta_t *out_meta,
+    int timeout_ms
+);
+void omnisocket_udp_session_stats_snapshot(omnisocket_udp_session_t *session, omnisocket_session_stats_t *out_stats);
 
 #endif

python/omnisocket_a_side/__init__.py

@@ -1,9 +0,0 @@
-from pathlib import Path
-
-
-PACKAGE_ROOT = Path(__file__).resolve().parent
-PYTHON_ROOT = PACKAGE_ROOT.parent
-REPO_ROOT = PYTHON_ROOT.parent
-DEFAULT_SOCKET_PATH = "/tmp/omnisocket-a-side.sock"
-DEFAULT_CONFIG_PATH = REPO_ROOT / "config" / "a_side_omnidaemon.yaml"
-VERSION = "0.1.0"

python/omnisocket_a_side/__main__.py

@@ -1,5 +0,0 @@
-from .daemon import main
-
-
-if __name__ == "__main__":
-    main()

python/omnisocket_a_side/client.py

@@ -1,149 +0,0 @@
-"""Local Unix-domain HTTP client for the A-side OmniDaemon."""
-
-from __future__ import annotations
-
-import http.client
-import json
-import os
-import socket
-import threading
-from typing import Any
-
-from . import DEFAULT_SOCKET_PATH
-
-
-class OmniDaemonError(RuntimeError):
-    def __init__(self, message: str, status_code: int | None = None) -> None:
-        super().__init__(message)
-        self.status_code = status_code
-
-
-class UnixHTTPConnection(http.client.HTTPConnection):
-    def __init__(self, socket_path: str, timeout: float = 2.0) -> None:
-        super().__init__("localhost", timeout=timeout)
-        self.socket_path = socket_path
-
-    def connect(self) -> None:  # pragma: no cover - runtime depends on Linux socket support
-        if not hasattr(socket, "AF_UNIX"):
-            raise OSError("AF_UNIX sockets are not available on this platform")
-        self.sock = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
-        self.sock.settimeout(self.timeout)
-        self.sock.connect(self.socket_path)
-
-
-class OmniDaemonClient:
-    def __init__(self, socket_path: str | None = None, timeout: float = 2.0) -> None:
-        self.socket_path = socket_path or os.getenv("OMNIDAEMON_SOCKET", DEFAULT_SOCKET_PATH)
-        self.timeout = timeout
-        self._local = threading.local()
-
-    def get_health(self) -> dict[str, Any]:
-        return self._request_json("GET", "/v1/health")
-
-    def get_state(self) -> dict[str, Any]:
-        return self._request_json("GET", "/v1/state")
-
-    def get_video_frame(self) -> bytes:
-        return self._request_bytes("GET", "/v1/video/frame")
-
-    def get_control_status(self) -> dict[str, Any]:
-        return self._request_json("GET", "/v1/control/status")
-
-    def send_control_event(
-        self,
-        *,
-        source: str,
-        event_code: str,
-        drive_value: float = 1.0,
-        client_time_ms: int | None = None,
-    ) -> dict[str, Any]:
-        payload = {
-            "source": source,
-            "event_code": event_code,
-            "drive_value": float(drive_value),
-            "client_time_ms": client_time_ms,
-        }
-        return self._request_json("POST", "/v1/control/event", payload)
-
-    def close(self) -> None:
-        self._reset_connection()
-
-    def _request_json(
-        self,
-        method: str,
-        path: str,
-        payload: dict[str, Any] | None = None,
-    ) -> dict[str, Any]:
-        raw = self._request_bytes(method, path, payload)
-        if not raw:
-            return {}
-        try:
-            return json.loads(raw.decode("utf-8"))
-        except json.JSONDecodeError as error:
-            raise OmniDaemonError(f"invalid daemon JSON response: {error}") from error
-
-    def _request_bytes(
-        self,
-        method: str,
-        path: str,
-        payload: dict[str, Any] | None = None,
-    ) -> bytes:
-        body = b""
-        headers: dict[str, str] = {}
-        if payload is not None:
-            body = json.dumps(payload).encode("utf-8")
-            headers["Content-Type"] = "application/json"
-            headers["Content-Length"] = str(len(body))
-        headers.setdefault("Connection", "keep-alive")
-
-        for attempt in range(2):
-            connection = self._get_connection()
-            try:
-                connection.request(method, path, body=body, headers=headers)
-                response = connection.getresponse()
-                raw = response.read()
-            except FileNotFoundError as error:
-                self._reset_connection()
-                raise OmniDaemonError(
-                    f"daemon socket not found: {self.socket_path}"
-                ) from error
-            except (OSError, http.client.HTTPException) as error:
-                self._reset_connection()
-                if attempt == 0:
-                    continue
-                raise OmniDaemonError(
-                    f"daemon request failed via {self.socket_path}: {error}"
-                ) from error
-
-            if getattr(response, "will_close", False):
-                self._reset_connection()
-
-            if response.status >= 400:
-                message = raw.decode("utf-8", errors="replace").strip() or response.reason
-                try:
-                    parsed = json.loads(message)
-                    if isinstance(parsed, dict) and "error" in parsed:
-                        message = str(parsed["error"])
-                except json.JSONDecodeError:
-                    pass
-                raise OmniDaemonError(message, status_code=response.status)
-            return raw
-
-        raise OmniDaemonError(f"daemon request failed via {self.socket_path}: retry exhausted")
-
-    def _get_connection(self) -> UnixHTTPConnection:
-        connection = getattr(self._local, "connection", None)
-        if connection is None:
-            connection = UnixHTTPConnection(self.socket_path, timeout=self.timeout)
-            self._local.connection = connection
-        return connection
-
-    def _reset_connection(self) -> None:
-        connection = getattr(self._local, "connection", None)
-        if connection is None:
-            return
-        try:
-            connection.close()
-        except OSError:
-            pass
-        self._local.connection = None

python/omnisocket_a_side/control_codec.py

@@ -1,90 +0,0 @@
-"""Binary control packet codec shared by the daemon and local clients."""
-
-from __future__ import annotations
-
-from dataclasses import dataclass
-import struct
-import time
-
-
-CONTROL_PACKET_VERSION = 1
-CONTROL_PACKET_STRUCT = struct.Struct("!BBHIfQ")
-
-EVENT_NAME_TO_ID = {
-    "pose_home": 1,
-    "pose_hold": 2,
-    "mode_stride": 3,
-    "surge_up": 6,
-    "surge_down": 7,
-    "sway_left": 8,
-    "sway_right": 9,
-    "spin_left": 10,
-    "spin_right": 11,
-    "set_surge": 12,
-    "set_sway": 13,
-    "set_spin": 14,
-    "set_lift": 15,
-    "lift_up": 16,
-    "lift_down": 17,
-    "trim_reset": 18,
-    "session_quit": 19,
-}
-EVENT_ID_TO_NAME = {value: key for key, value in EVENT_NAME_TO_ID.items()}
-ANALOG_EVENT_CODES = {"set_surge", "set_sway", "set_spin"}
-
-
-@dataclass(slots=True)
-class ControlPacket:
-    seq_id: int
-    event_id: int
-    drive_value: float = 1.0
-    sent_at_ns: int = 0
-
-    @property
-    def event_name(self) -> str:
-        return EVENT_ID_TO_NAME.get(self.event_id, f"unknown_{self.event_id}")
-
-    def encode(self) -> bytes:
-        sent_at_ns = self.sent_at_ns or time.time_ns()
-        return CONTROL_PACKET_STRUCT.pack(
-            CONTROL_PACKET_VERSION,
-            self.event_id,
-            0,
-            int(self.seq_id),
-            float(self.drive_value),
-            int(sent_at_ns),
-        )
-
-    @classmethod
-    def decode(cls, payload: bytes) -> "ControlPacket":
-        if len(payload) != CONTROL_PACKET_STRUCT.size:
-            raise ValueError(
-                f"invalid control packet length {len(payload)}; "
-                f"want {CONTROL_PACKET_STRUCT.size}"
-            )
-        version, event_id, _reserved, seq_id, drive_value, sent_at_ns = (
-            CONTROL_PACKET_STRUCT.unpack(payload)
-        )
-        if version != CONTROL_PACKET_VERSION:
-            raise ValueError(f"unsupported control packet version {version}")
-        return cls(
-            seq_id=int(seq_id),
-            event_id=int(event_id),
-            drive_value=float(drive_value),
-            sent_at_ns=int(sent_at_ns),
-        )
-
-
-def make_control_packet(
-    seq_id: int,
-    event_name: str,
-    drive_value: float = 1.0,
-    sent_at_ns: int | None = None,
-) -> ControlPacket:
-    event_id = EVENT_NAME_TO_ID[event_name]
-    return ControlPacket(
-        seq_id=seq_id,
-        event_id=event_id,
-        drive_value=drive_value,
-        sent_at_ns=time.time_ns() if sent_at_ns is None else sent_at_ns,
-    )

python/omnisocket_b_side/__init__.py

@@ -1,10 +0,0 @@
-from pathlib import Path
-
-
-PACKAGE_ROOT = Path(__file__).resolve().parent
-PYTHON_ROOT = PACKAGE_ROOT.parent
-REPO_ROOT = PYTHON_ROOT.parent
-DEFAULT_SOCKET_PATH = "/tmp/omnisocket-b-side.sock"
-DEFAULT_CTRL_SOCKET_PATH = "/tmp/omnisocket-b-ctrl.sock"
-DEFAULT_CONFIG_PATH = REPO_ROOT / "config" / "b_side_omnidaemon.yaml"
-VERSION = "0.1.0"

python/omnisocket_b_side/__main__.py

@@ -1,5 +0,0 @@
-from .daemon import main
-
-
-if __name__ == "__main__":
-    main()

python/omnisocket_b_side/client.py

@@ -1,54 +0,0 @@
-"""Local AF_UNIX SOCK_SEQPACKET client for B-side control delivery."""
-
-from __future__ import annotations
-
-import os
-import socket
-
-from omnisocket_a_side.control_codec import CONTROL_PACKET_STRUCT
-
-from . import DEFAULT_CTRL_SOCKET_PATH
-
-
-class BSideControlClient:
-    def __init__(self, socket_path: str | None = None) -> None:
-        self.socket_path = socket_path or os.getenv(
-            "OMNIBDAEMON_CTRL_SOCKET",
-            DEFAULT_CTRL_SOCKET_PATH,
-        )
-        self._sock: socket.socket | None = None
-
-    def connect(self) -> None:
-        if not hasattr(socket, "AF_UNIX"):
-            raise OSError("AF_UNIX sockets are not available on this platform")
-        if not hasattr(socket, "SOCK_SEQPACKET"):
-            raise OSError("SOCK_SEQPACKET is not available on this platform")
-        sock = socket.socket(socket.AF_UNIX, socket.SOCK_SEQPACKET)
-        sock.connect(self.socket_path)
-        self._sock = sock
-
-    def recv_control_packet(self, timeout_ms: int = 100) -> bytes | None:
-        if self._sock is None:
-            raise RuntimeError("B-side control client is not connected")
-
-        self._sock.settimeout(max(0.001, timeout_ms / 1000.0))
-        try:
-            payload = self._sock.recv(CONTROL_PACKET_STRUCT.size)
-        except socket.timeout:
-            return None
-        except BlockingIOError:
-            return None
-
-        if payload == b"":
-            raise ConnectionResetError("daemon control socket closed")
-        if len(payload) != CONTROL_PACKET_STRUCT.size:
-            return None
-        return payload
-
-    def close(self) -> None:
-        if self._sock is None:
-            return
-        try:
-            self._sock.close()
-        finally:
-            self._sock = None

python/setup.py

@@ -35,13 +35,7 @@ COMMON_SOURCES = [
 setup(
     name="omnisocket",
     version="0.1.0",
-    packages=["omnisocket", "omnisocket_a_side", "omnisocket_b_side"],
+    packages=["omnisocket"],
-    entry_points={
-        "console_scripts": [
-            "omnisocket-a-side-daemon=omnisocket_a_side.daemon:main",
-            "omnisocket-b-side-daemon=omnisocket_b_side.daemon:main",
-        ]
-    },
     ext_modules=[
         Extension(
             "omnisocket._omnisocket",

python/tests/test_sessions.py

@@ -0,0 +1,187 @@
+from __future__ import annotations
+
+from contextlib import contextmanager
+from pathlib import Path
+import socket
+import subprocess
+import sys
+import threading
+import time
+
+import pytest
+
+
+pytestmark = pytest.mark.skipif(sys.platform != 'linux', reason='Linux-only OmniSocket extension')
+
+ROOT = Path(__file__).resolve().parents[2]
+PYTHON_ROOT = ROOT / 'python'
+if str(PYTHON_ROOT) not in sys.path:
+    sys.path.insert(0, str(PYTHON_ROOT))
+
+omnisocket = pytest.importorskip('omnisocket')
+
+CONTROL_DEFAULTS = omnisocket.CONTROL_DEFAULTS
+MSG_TYPE_BINARY = omnisocket.MSG_TYPE_BINARY
+Session = omnisocket.Session
+UdpSession = omnisocket.UdpSession
+
+
+def _reserve_port() -> int:
+    with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
+        sock.bind(('127.0.0.1', 0))
+        return int(sock.getsockname()[1])
+
+
+@contextmanager
+def _run_server(binary_name: str, listen_addr: str):
+    binary = ROOT / 'bin' / binary_name
+    if not binary.exists():
+        pytest.skip(f'{binary} is not built')
+
+    process = subprocess.Popen(
+        [str(binary), '-listen', listen_addr],
+        cwd=str(ROOT),
+        stdout=subprocess.DEVNULL,
+        stderr=subprocess.DEVNULL,
+    )
+    try:
+        time.sleep(0.2)
+        yield process
+    finally:
+        process.terminate()
+        try:
+            process.wait(timeout=2.0)
+        except subprocess.TimeoutExpired:
+            process.kill()
+            process.wait(timeout=2.0)
+
+
+def _connect_with_retry(session_cls, *, transport: str, server_addr: str, peer_id: str):
+    deadline = time.monotonic() + 3.0
+    last_error: Exception | None = None
+
+    while time.monotonic() < deadline:
+        session = session_cls()
+        try:
+            kwargs: dict[str, object] = {
+                'server_addr': server_addr,
+                'peer_id': peer_id,
+            }
+            if transport == 'kcp':
+                kwargs.update(CONTROL_DEFAULTS)
+            else:
+                kwargs['enable_timestamping'] = False
+            session.connect(**kwargs)
+            return session
+        except OSError as exc:
+            last_error = exc
+            time.sleep(0.1)
+
+    raise AssertionError(f'failed to connect {peer_id} to {server_addr}: {last_error}')
+
+
+@pytest.mark.parametrize(
+    ('transport', 'binary_name', 'session_cls'),
+    [
+        ('udp', 'udpserver', UdpSession),
+        ('kcp', 'kcpserver', Session),
+    ],
+)
+def test_control_sessions_smoke(transport: str, binary_name: str, session_cls) -> None:
+    port = _reserve_port()
+    listen_addr = f'127.0.0.1:{port}'
+    sender_id = f'pytest-{transport}-sender'
+    receiver_id = f'pytest-{transport}-receiver'
+
+    with _run_server(binary_name, listen_addr):
+        sender = _connect_with_retry(session_cls, transport=transport, server_addr=listen_addr, peer_id=sender_id)
+        receiver = _connect_with_retry(session_cls, transport=transport, server_addr=listen_addr, peer_id=receiver_id)
+
+        try:
+            assert receiver.recv(timeout_ms=20) is None
+
+            payload = b'control-packet-1'
+            sender.send(to=receiver_id, data=payload)
+            from_peer, msg_type, recv_payload = receiver.recv(timeout_ms=1000)
+            assert from_peer == sender_id
+            assert msg_type == MSG_TYPE_BINARY
+            assert recv_payload == payload
+
+            payload2 = b'control-packet-2'
+            sender.send(to=receiver_id, data=payload2)
+            recv_buffer = bytearray(128)
+            meta = receiver.recv_into(buffer=recv_buffer, timeout_ms=1000)
+            assert meta is not None
+            assert meta['from'] == sender_id
+            assert meta['msg_type'] == MSG_TYPE_BINARY
+            assert meta['body_len'] == len(payload2)
+            assert bytes(recv_buffer[: meta['body_len']]) == payload2
+
+            sender_stats = sender.stats()
+            receiver_stats = receiver.stats()
+            assert sender_stats['connected'] == 1
+            assert receiver_stats['connected'] == 1
+            assert sender_stats['send_calls'] >= 2
+            assert receiver_stats['recv_calls'] >= 2
+            if transport == 'kcp':
+                sender_kcp_stats = sender.kcp_stats()
+                receiver_kcp_stats = receiver.kcp_stats()
+                assert sender_kcp_stats['connected'] == 1
+                assert receiver_kcp_stats['connected'] == 1
+                assert 'srtt_ms' in sender_kcp_stats
+                assert 'snd_queue' in receiver_kcp_stats
+        finally:
+            sender.close()
+            receiver.close()
+
+
+def test_udp_session_close_interrupts_blocking_recv() -> None:
+    port = _reserve_port()
+    listen_addr = f'127.0.0.1:{port}'
+    receiver_id = 'pytest-udp-blocking-recv'
+
+    with _run_server('udpserver', listen_addr):
+        receiver = _connect_with_retry(
+            UdpSession,
+            transport='udp',
+            server_addr=listen_addr,
+            peer_id=receiver_id,
+        )
+
+        recv_error: list[BaseException] = []
+        close_error: list[BaseException] = []
+        recv_started = threading.Event()
+        recv_done = threading.Event()
+        close_done = threading.Event()
+
+        def recv_worker() -> None:
+            recv_started.set()
+            try:
+                receiver.recv()
+            except BaseException as exc:  # pragma: no cover - assertion is on thread completion
+                recv_error.append(exc)
+            finally:
+                recv_done.set()
+
+        def close_worker() -> None:
+            try:
+                receiver.close()
+            except BaseException as exc:  # pragma: no cover - assertion is on thread completion
+                close_error.append(exc)
+            finally:
+                close_done.set()
+
+        recv_thread = threading.Thread(target=recv_worker, daemon=True)
+        recv_thread.start()
+        assert recv_started.wait(timeout=1.0)
+        time.sleep(0.05)
+
+        close_thread = threading.Thread(target=close_worker, daemon=True)
+        close_thread.start()
+
+        assert close_done.wait(timeout=1.0), 'UdpSession.close() blocked while recv() was waiting'
+        assert recv_done.wait(timeout=1.0), 'UdpSession.recv() stayed blocked after close()'
+        assert not close_thread.is_alive()
+        assert not recv_thread.is_alive()
+        assert not close_error
+        assert not recv_error or isinstance(recv_error[0], OSError)

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)

ros-control-c/README.md

@@ -0,0 +1,76 @@
+# 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`
+
+Note:
+
+- This README documents the `ros-control-c` path only.
+- `ros-control-py` now uses OmniSocket for both `transport:=udp` and `transport:=kcp`; its `udp` mode is no longer raw socket UDP.
+
+## 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 -telemetry-peer peer-a-telemetry
+```
+
+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
+```

ros-control-c/Robot Remote Control via UDP - Implementation Plan.md

@@ -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

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 */

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;
+    }
+}

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 */

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;
+}

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;
+}

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()

ros-control-py/README.md

@@ -0,0 +1,257 @@
+# 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 -telemetry-peer peer-a-telemetry
+```
+
+`server_addr` 不传时，节点会按 `transport` 自动选择默认值:
+
+- `udp` -> `127.0.0.1:9001`
+- `kcp` -> `127.0.0.1:9002`
+
+`relay_via` 只在 `transport:=kcp` 时生效。
+
+## 机器人端运行
+
+UDP:
+
+```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
+```
+
+Local daemon handoff via Unix datagram:
+
+```bash
+ros2 launch udp_teleop_bridge robot_udp_receiver.launch.py \
+  transport:=unix_dgram \
+  local_socket_path:=/tmp/omnisocket-b-side-cmd.sock \
+  output_topic:=/hric/robot/cmd_vel \
+  frame_id:=pelvis \
+  watchdog_timeout:=0.5
+```
+
+## 控制端键盘运行
+
+终端 A，启动 sender:
+
+```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 消息、错误长度消息都会被丢弃并记录日志

ros-control-py/ROS2 Teleop over UDP.md

@@ -0,0 +1,153 @@
+## 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 -telemetry-peer peer-a-telemetry
+```
+
+### 机器人端 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
+```
+
+如果控制命令来自本机 `b_side_omnid`，可以改为:
+
+```bash
+transport:=unix_dgram local_socket_path:=/tmp/omnisocket-b-side-cmd.sock
+```
+
+只接受指定 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 模式有效

ros-control-py/udp_teleop_bridge/config/xbox_twist_joy.yaml

@@ -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

ros-control-py/udp_teleop_bridge/launch/keyboard_sender.launch.py

@@ -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),
+            }],
+        ),
+    ])

ros-control-py/udp_teleop_bridge/launch/robot_udp_receiver.launch.py

@@ -0,0 +1,38 @@
+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('local_socket_path', default_value='/tmp/omnisocket-b-side-cmd.sock'),
+        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'),
+                'local_socket_path': LaunchConfiguration('local_socket_path'),
+                '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),
+            }],
+        ),
+    ])

ros-control-py/udp_teleop_bridge/launch/xbox_to_udp.launch.py

@@ -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),
+            }],
+        ),
+    ])

ros-control-py/udp_teleop_bridge/package.xml

@@ -0,0 +1,25 @@
+<?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>rosidl_runtime_py</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>

ros-control-py/udp_teleop_bridge/resource/udp_teleop_bridge

@@ -0,0 +1 @@
+udp_teleop_bridge

ros-control-py/udp_teleop_bridge/setup.cfg

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

ros-control-py/udp_teleop_bridge/setup.py

@@ -0,0 +1,35 @@
+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',
+            'topic_status_reader = udp_teleop_bridge.topic_status_reader:main',
+        ],
+    },
+)

ros-control-py/udp_teleop_bridge/test/test_protocol.py

@@ -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')

ros-control-py/udp_teleop_bridge/udp_teleop_bridge/__init__.py

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

ros-control-py/udp_teleop_bridge/udp_teleop_bridge/cmd_vel_udp_sender.py

@@ -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()

ros-control-py/udp_teleop_bridge/udp_teleop_bridge/omni_transport.py

@@ -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',
+]

ros-control-py/udp_teleop_bridge/udp_teleop_bridge/protocol.py

@@ -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))

ros-control-py/udp_teleop_bridge/udp_teleop_bridge/topic_status_reader.py

@@ -0,0 +1,122 @@
+"""Subscribe to a ROS 2 topic with runtime type discovery and print messages."""
+
+from __future__ import annotations
+
+import json
+import time
+from typing import Any
+
+import rclpy
+from rclpy.node import Node
+from rosidl_runtime_py.convert import message_to_ordereddict
+from rosidl_runtime_py.utilities import get_message
+
+
+WAIT_LOG_INTERVAL_SEC = 5.0
+
+
+class TopicStatusReader(Node):
+    """Wait for a topic to appear, subscribe to it, and print each message."""
+
+    def __init__(self) -> None:
+        super().__init__('topic_status_reader')
+
+        self.declare_parameter('topic', '/hric/robot/cmd_vel_status')
+        self.declare_parameter('qos_depth', 10)
+        self.declare_parameter('poll_interval_sec', 0.5)
+
+        self._topic = str(self.get_parameter('topic').value).strip()
+        self._qos_depth = int(self.get_parameter('qos_depth').value)
+        self._poll_interval_sec = float(self.get_parameter('poll_interval_sec').value)
+
+        if not self._topic:
+            raise ValueError('topic must not be empty')
+        if self._qos_depth <= 0:
+            raise ValueError('qos_depth must be > 0')
+        if self._poll_interval_sec <= 0.0:
+            raise ValueError('poll_interval_sec must be > 0')
+
+        self._topic_type: str | None = None
+        self._subscription = None
+        self._message_count = 0
+        self._last_wait_log_monotonic = 0.0
+
+        self._poll_timer = self.create_timer(self._poll_interval_sec, self._ensure_subscription)
+        self._ensure_subscription()
+
+    def _discover_topic_types(self) -> list[str]:
+        for topic_name, topic_types in self.get_topic_names_and_types():
+            if topic_name == self._topic:
+                return list(topic_types)
+        return []
+
+    def _log_waiting(self) -> None:
+        now = time.monotonic()
+        if (now - self._last_wait_log_monotonic) < WAIT_LOG_INTERVAL_SEC:
+            return
+        self._last_wait_log_monotonic = now
+        self.get_logger().info(f'Waiting for topic {self._topic} to appear...')
+
+    def _ensure_subscription(self) -> None:
+        if self._subscription is not None:
+            return
+
+        topic_types = self._discover_topic_types()
+        if not topic_types:
+            self._log_waiting()
+            return
+
+        if len(topic_types) > 1:
+            joined = ', '.join(topic_types)
+            self.get_logger().warning(
+                f'Topic {self._topic} reports multiple types ({joined}); using {topic_types[0]}'
+            )
+
+        self._topic_type = topic_types[0]
+        try:
+            message_type = get_message(self._topic_type)
+        except Exception as exc:
+            self.get_logger().error(
+                f'Failed to import message type {self._topic_type} for {self._topic}: {exc}'
+            )
+            return
+
+        self._subscription = self.create_subscription(
+            message_type,
+            self._topic,
+            self._handle_message,
+            self._qos_depth,
+        )
+        self._poll_timer.cancel()
+        self.get_logger().info(
+            f'Subscribed to {self._topic} with type {self._topic_type} (qos_depth={self._qos_depth})'
+        )
+
+    def _format_message(self, msg: Any) -> str:
+        try:
+            payload = message_to_ordereddict(msg)
+        except Exception:
+            return str(msg)
+        return json.dumps(payload, ensure_ascii=False, indent=2)
+
+    def _handle_message(self, msg: Any) -> None:
+        self._message_count += 1
+        received_at = time.strftime('%Y-%m-%d %H:%M:%S')
+        topic_type = self._topic_type or type(msg).__name__
+        rendered = self._format_message(msg)
+        print(
+            f'[{received_at}] #{self._message_count} {self._topic} ({topic_type})\n{rendered}\n',
+            flush=True,
+        )
+
+
+def main(args: list[str] | None = None) -> None:
+    rclpy.init(args=args)
+    node = TopicStatusReader()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()

ros-control-py/udp_teleop_bridge/udp_teleop_bridge/udp_cmd_vel_receiver.py

@@ -0,0 +1,334 @@
+"""ROS 2 node that receives OmniSocket teleop packets and republishes TwistStamped."""
+
+from __future__ import annotations
+
+import os
+import socket
+import threading
+import time
+from typing import Dict, Optional, Tuple
+
+import rclpy
+from geometry_msgs.msg import TwistStamped
+from rclpy.node import Node
+
+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('local_socket_path', '/tmp/omnisocket-b-side-cmd.sock')
+        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._local_socket_path = str(self.get_parameter('local_socket_path').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._transport_name not in ('udp', 'kcp', 'unix_dgram'):
+            raise ValueError("transport must be one of: udp, kcp, unix_dgram")
+        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 = None
+        self._unix_socket: socket.socket | None = None
+        self._msg_type_binary = 0
+        self._msg_type_error = 0
+        if self._transport_name == 'unix_dgram':
+            self._setup_unix_socket()
+        else:
+            from .omni_transport import MSG_TYPE_BINARY, MSG_TYPE_ERROR, OmniTransport
+
+            self._msg_type_binary = MSG_TYPE_BINARY
+            self._msg_type_error = MSG_TYPE_ERROR
+            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)
+
+        recv_target = self._recv_loop_unix_dgram if self._transport_name == 'unix_dgram' else self._recv_loop
+        self._recv_thread = threading.Thread(target=recv_target, daemon=True)
+        self._recv_thread.start()
+
+        if self._transport_name == 'unix_dgram':
+            self.get_logger().info(
+                'Receiving teleop commands via unix_dgram://%s and publishing TwistStamped to %s '
+                'at %.1f Hz (frame_id=%s, watchdog %.2f s)'
+                % (
+                    self._local_socket_path,
+                    self._output_topic,
+                    self._publish_rate_hz,
+                    self._frame_id,
+                    self._watchdog_timeout,
+                )
+            )
+        else:
+            assert self._transport is not None
+            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 _setup_unix_socket(self) -> None:
+        if not self._local_socket_path:
+            raise ValueError('local_socket_path must not be empty for unix_dgram transport')
+
+        socket_dir = os.path.dirname(self._local_socket_path)
+        if socket_dir:
+            os.makedirs(socket_dir, exist_ok=True)
+        if os.path.exists(self._local_socket_path):
+            self.get_logger().warning(
+                'Removing existing unix datagram socket path before bind: %s'
+                % self._local_socket_path
+            )
+        try:
+            os.unlink(self._local_socket_path)
+        except FileNotFoundError:
+            pass
+
+        self._unix_socket = socket.socket(socket.AF_UNIX, socket.SOCK_DGRAM)
+        self._unix_socket.bind(self._local_socket_path)
+        self._unix_socket.settimeout(0.1)
+
+    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:
+                assert self._transport is not None
+                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 == self._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 != self._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 _recv_loop_unix_dgram(self) -> None:
+        assert self._unix_socket is not None
+
+        while not self._closing.is_set() and rclpy.ok():
+            try:
+                payload = self._unix_socket.recv(DEFAULT_RECV_BUFFER_BYTES)
+            except socket.timeout:
+                continue
+            except OSError as exc:
+                if not self._closing.is_set() and self._should_log('unix_recv_error', 2.0):
+                    self.get_logger().error(f'Unix datagram receive loop stopped: {exc}')
+                return
+
+            if len(payload) != PACKET_SIZE:
+                if self._should_log('unix_packet_size', 2.0):
+                    self.get_logger().warning(
+                        'Dropped unix datagram payload with invalid size %d (expected %d)'
+                        % (len(payload), PACKET_SIZE)
+                    )
+                continue
+
+            try:
+                command = unpack_command(payload)
+            except ValueError as exc:
+                if self._should_log('unix_decode_error', 2.0):
+                    self.get_logger().warning(f'Dropped malformed unix datagram 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 self._unix_socket is not None:
+            try:
+                self._unix_socket.close()
+            except OSError as exc:
+                if self._should_log('unix_close_error', 2.0):
+                    self.get_logger().warning(f'Closing unix socket failed: {exc}')
+            self._unix_socket = None
+            try:
+                os.unlink(self._local_socket_path)
+            except FileNotFoundError:
+                pass
+        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()

scripts/BACDauto_test.sh

@@ -56,6 +56,7 @@ if [ ! -x ./bin/kcpserver ]; then
   exit 1
 fi
 setsid ./bin/kcpserver -listen 0.0.0.0:10909 \
+  -telemetry-peer peer-a-telemetry \
   -kcp-ts-debug-log logs/d-kcp-ts.jsonl \
   -kcp-session-stats-log logs/d-kcp-stats.jsonl > server_console.log 2>&1 </dev/null &
 echo "server D launched (pid=$!)"
@@ -193,9 +194,9 @@ done
 
 # 50 轮发送
 for i in $(seq 1 50); do
-  echo "file peer-a /tmp/test30k.bin" >&3
+  echo "file peer-a /home/boll/test30.bin" >&3
   sleep 1
-  echo "file peer-a /tmp/test5.bin" >&3
+  echo "file peer-a /home/boll/test5.bin" >&3
   sleep 1
 done
 

scripts/BDAanto_test.sh

@@ -56,6 +56,7 @@ if [ ! -x ./bin/kcpserver ]; then
   exit 1
 fi
 setsid ./bin/kcpserver -listen 0.0.0.0:10909 \
+  -telemetry-peer peer-a-telemetry \
   -kcp-ts-debug-log logs/d-kcp-ts.jsonl \
   -kcp-session-stats-log logs/d-kcp-stats.jsonl > server_console.log 2>&1 </dev/null &
 echo "server D launched (pid=$!)"
@@ -155,9 +156,9 @@ done
 
 # 50 轮发送
 for i in $(seq 1 50); do
-  echo "file peer-a /tmp/test30k.bin" >&3
+  echo "file peer-a /home/boll/test30.bin" >&3
   sleep 1
-  echo "file peer-a /tmp/test5.bin" >&3
+  echo "file peer-a /home/boll/test5.bin" >&3
   sleep 1
 done
 

scripts/dev/README.md

@@ -0,0 +1,81 @@
+# Dev Startup Scripts
+
+This directory lives inside the `OmniSocketGo` repo and acts as the main launch entry for the whole local setup.
+
+Default layout:
+
+```text
+~/Documents/
+  OmniSocketGo/
+    scripts/dev/
+  robot-command-center/
+```
+
+The scripts assume:
+
+- `OmniSocketGo` is the current repo
+- `robot-command-center` is a sibling directory next to it
+
+If your `robot-command-center` is elsewhere, set `ROBOT_COMMAND_CENTER_ROOT` in `robot-remote.env.local`.
+
+## Files
+
+- `robot-remote.env`: shared defaults for backend, frontend, ROS, and `b_side_omnid`
+- `robot-remote.env.local`: optional local override file loaded after `robot-remote.env`
+- `load-env.sh`: loads the shared environment into the current shell
+- `start-backend.sh`: starts Django ASGI with `uvicorn`
+- `start-frontend.sh`: starts the Vite dev server
+- `start-ros-receiver.sh`: starts the ROS2 `udp_teleop_bridge` receiver
+- `start-b-side-omnid.sh`: starts `./bin/b_side_omnid` and uses `sudo -E` by default
+- `start-dev-tmux.sh`: optional one-command `tmux` launcher for all four processes
+
+## Usage
+
+Run these from the `OmniSocketGo` repo root:
+
+```bash
+bash scripts/dev/start-backend.sh
+bash scripts/dev/start-frontend.sh
+bash scripts/dev/start-ros-receiver.sh
+bash scripts/dev/start-b-side-omnid.sh
+```
+
+If you prefer one command and use `tmux`:
+
+```bash
+bash scripts/dev/start-dev-tmux.sh
+```
+
+If you only want the shared environment for manual commands:
+
+```bash
+source scripts/dev/load-env.sh
+```
+
+## Customizing
+
+Edit `scripts/dev/robot-remote.env` for shared changes such as:
+
+- `ROBOT_COMMAND_CENTER_ROOT`
+- `CONTROL_SIDE_OMNISOCKET_SERVER_ADDR`
+- `CONTROL_SIDE_OMNISOCKET_RELAY_VIA`
+- `ROBOT_SIDE_OMNISOCKET_SERVER_ADDR`
+- `ROBOT_SIDE_OMNISOCKET_RELAY_VIA`
+- `VITE_API_BASE_URL`
+- `OMNI_CAMERA_DEVICE`
+- `OMNI_VIDEO_PEER_ID`
+- `OMNI_CONTROL_PEER_ID`
+
+Role mapping:
+
+- `start-backend.sh` uses the `CONTROL_SIDE_*` address pair
+- `start-b-side-omnid.sh` uses the `ROBOT_SIDE_*` address pair
+- `start-ros-receiver.sh` defaults to the robot-side address pair, but with `transport=unix_dgram` it usually does not need the server address
+
+Put machine-specific overrides into `scripts/dev/robot-remote.env.local`. Example:
+
+```bash
+ROBOT_COMMAND_CENTER_ROOT="$HOME/Documents/robot-command-center"
+OMNI_CAMERA_DEVICE="/dev/video30"
+B_SIDE_OMNID_USE_SUDO="0"
+```

scripts/dev/load-env.sh

@@ -0,0 +1,78 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+DEFAULT_OMNISOCKETGO_ROOT="$(cd "${SCRIPT_DIR}/../.." && pwd)"
+
+die() {
+  echo "$*" >&2
+  return 1 2>/dev/null || exit 1
+}
+
+is_omnisocketgo_root() {
+  local dir="$1"
+  [[ -f "${dir}/Makefile" && -f "${dir}/cmd/b_side_omnid.c" && -d "${dir}/ros-control-py" ]]
+}
+
+is_robot_command_center_root() {
+  local dir="$1"
+  [[ -f "${dir}/backend/config/asgi.py" && -f "${dir}/frontend/package.json" ]]
+}
+
+export OMNISOCKETGO_ROOT="${OMNISOCKETGO_ROOT:-${DEFAULT_OMNISOCKETGO_ROOT}}"
+
+ENV_FILES=(
+  "${SCRIPT_DIR}/robot-remote.env"
+  "${SCRIPT_DIR}/robot-remote.env.local"
+)
+
+for env_file in "${ENV_FILES[@]}"; do
+  if [[ -f "${env_file}" ]]; then
+    set -a
+    # shellcheck disable=SC1090
+    source "${env_file}"
+    set +a
+  fi
+done
+
+export OMNISOCKETGO_ROOT="${OMNISOCKETGO_ROOT:-${DEFAULT_OMNISOCKETGO_ROOT}}"
+export ROBOT_COMMAND_CENTER_ROOT="${ROBOT_COMMAND_CENTER_ROOT:-$(dirname "${OMNISOCKETGO_ROOT}")/robot-command-center}"
+
+if ! is_omnisocketgo_root "${OMNISOCKETGO_ROOT}"; then
+  die "OMNISOCKETGO_ROOT must point to the OmniSocketGo repo root. Current value: ${OMNISOCKETGO_ROOT}"
+fi
+
+if ! is_robot_command_center_root "${ROBOT_COMMAND_CENTER_ROOT}"; then
+  die "ROBOT_COMMAND_CENTER_ROOT must point to the robot-command-center repo root. Current value: ${ROBOT_COMMAND_CENTER_ROOT}. Set it in ${SCRIPT_DIR}/robot-remote.env.local if needed."
+fi
+
+export BACKEND_DIR="${BACKEND_DIR:-${ROBOT_COMMAND_CENTER_ROOT}/backend}"
+export FRONTEND_DIR="${FRONTEND_DIR:-${ROBOT_COMMAND_CENTER_ROOT}/frontend}"
+export ROS_CONTROL_PY_DIR="${ROS_CONTROL_PY_DIR:-${OMNISOCKETGO_ROOT}/ros-control-py}"
+export PYTHON3_BIN="${PYTHON3_BIN:-python3}"
+export PYTHON_VENV_PATH="${PYTHON_VENV_PATH:-${OMNISOCKETGO_ROOT}/.venv}"
+export BACKEND_HOST="${BACKEND_HOST:-0.0.0.0}"
+export BACKEND_PORT="${BACKEND_PORT:-8001}"
+export FRONTEND_HOST="${FRONTEND_HOST:-0.0.0.0}"
+export FRONTEND_PORT="${FRONTEND_PORT:-5173}"
+export CONTROL_SIDE_OMNISOCKET_SERVER_ADDR="${CONTROL_SIDE_OMNISOCKET_SERVER_ADDR:-}"
+export CONTROL_SIDE_OMNISOCKET_RELAY_VIA="${CONTROL_SIDE_OMNISOCKET_RELAY_VIA:-}"
+export ROBOT_SIDE_OMNISOCKET_SERVER_ADDR="${ROBOT_SIDE_OMNISOCKET_SERVER_ADDR:-}"
+export ROBOT_SIDE_OMNISOCKET_RELAY_VIA="${ROBOT_SIDE_OMNISOCKET_RELAY_VIA:-}"
+export ROS_DISTRO="${ROS_DISTRO:-jazzy}"
+export ROBOT_RECEIVER_TRANSPORT="${ROBOT_RECEIVER_TRANSPORT:-unix_dgram}"
+export ROBOT_RECEIVER_SERVER_ADDR="${ROBOT_RECEIVER_SERVER_ADDR:-${ROBOT_SIDE_OMNISOCKET_SERVER_ADDR:-}}"
+export ROBOT_RECEIVER_RELAY_VIA="${ROBOT_RECEIVER_RELAY_VIA:-${ROBOT_SIDE_OMNISOCKET_RELAY_VIA:-}}"
+export ROBOT_RECEIVER_PEER_ID="${ROBOT_RECEIVER_PEER_ID:-ros-bridge-ctrl}"
+export ROBOT_RECEIVER_EXPECTED_SENDER="${ROBOT_RECEIVER_EXPECTED_SENDER:-}"
+export ROBOT_RECEIVER_LOCAL_SOCKET_PATH="${ROBOT_RECEIVER_LOCAL_SOCKET_PATH:-/tmp/omnisocket-b-side-cmd.sock}"
+export ROBOT_RECEIVER_OUTPUT_TOPIC="${ROBOT_RECEIVER_OUTPUT_TOPIC:-/hric/robot/cmd_vel}"
+export ROBOT_RECEIVER_FRAME_ID="${ROBOT_RECEIVER_FRAME_ID:-pelvis}"
+export ROBOT_RECEIVER_WATCHDOG_TIMEOUT="${ROBOT_RECEIVER_WATCHDOG_TIMEOUT:-0.5}"
+export ROBOT_RECEIVER_PUBLISH_RATE_HZ="${ROBOT_RECEIVER_PUBLISH_RATE_HZ:-100.0}"
+export OMNI_VIDEO_SERVER_ADDR="${OMNI_VIDEO_SERVER_ADDR:-${ROBOT_SIDE_OMNISOCKET_SERVER_ADDR:-}}"
+export OMNI_VIDEO_RELAY_VIA="${OMNI_VIDEO_RELAY_VIA:-${ROBOT_SIDE_OMNISOCKET_RELAY_VIA:-}}"
+export OMNI_CONTROL_SERVER_ADDR="${OMNI_CONTROL_SERVER_ADDR:-${ROBOT_SIDE_OMNISOCKET_SERVER_ADDR:-}}"
+export OMNI_CONTROL_RELAY_VIA="${OMNI_CONTROL_RELAY_VIA:-${ROBOT_SIDE_OMNISOCKET_RELAY_VIA:-}}"
+export OMNI_CONTROL_UNIX_SOCKET_PATH="${OMNI_CONTROL_UNIX_SOCKET_PATH:-${ROBOT_RECEIVER_LOCAL_SOCKET_PATH}}"
+export B_SIDE_OMNID_USE_SUDO="${B_SIDE_OMNID_USE_SUDO:-1}"

scripts/dev/robot-remote.env

@@ -0,0 +1,49 @@
+# Optional absolute path override for the companion repo.
+# By default the scripts assume:
+#   OmniSocketGo -> current repo
+#   robot-command-center -> sibling directory next to OmniSocketGo
+# Example:
+# ROBOT_COMMAND_CENTER_ROOT="$HOME/Documents/robot-command-center"
+
+CONTROL_SIDE_OMNISOCKET_SERVER_ADDR="81.70.156.140:10909"
+CONTROL_SIDE_OMNISOCKET_RELAY_VIA="81.70.156.140:10909"
+
+ROBOT_SIDE_OMNISOCKET_SERVER_ADDR="81.70.156.140:10909"
+ROBOT_SIDE_OMNISOCKET_RELAY_VIA="106.55.173.235:10909"
+
+CONTROL_WS_ALLOWED_ORIGINS="http://127.0.0.1:5173,http://localhost:5173"
+VITE_API_BASE_URL="http://127.0.0.1:8001"
+
+PYTHON3_BIN="python3"
+PYTHON_VENV_PATH="${OMNISOCKETGO_ROOT}/.venv"
+
+BACKEND_HOST="0.0.0.0"
+BACKEND_PORT="8001"
+
+FRONTEND_HOST="0.0.0.0"
+FRONTEND_PORT="5173"
+
+ROS_DISTRO="jazzy"
+ROBOT_RECEIVER_TRANSPORT="unix_dgram"
+ROBOT_RECEIVER_SERVER_ADDR="${ROBOT_SIDE_OMNISOCKET_SERVER_ADDR}"
+ROBOT_RECEIVER_RELAY_VIA="${ROBOT_SIDE_OMNISOCKET_RELAY_VIA}"
+ROBOT_RECEIVER_PEER_ID="ros-bridge-ctrl"
+ROBOT_RECEIVER_EXPECTED_SENDER=""
+ROBOT_RECEIVER_LOCAL_SOCKET_PATH="/tmp/omnisocket-b-side-cmd.sock"
+ROBOT_RECEIVER_OUTPUT_TOPIC="/hric/robot/cmd_vel"
+ROBOT_RECEIVER_FRAME_ID="pelvis"
+ROBOT_RECEIVER_WATCHDOG_TIMEOUT="0.5"
+ROBOT_RECEIVER_PUBLISH_RATE_HZ="100.0"
+
+OMNI_VIDEO_PEER_ID="peer-b-video"
+OMNI_VIDEO_TARGET_PEER="peer-a-video"
+OMNI_CAMERA_DEVICE="/dev/video26"
+OMNI_VIDEO_SERVER_ADDR="${ROBOT_SIDE_OMNISOCKET_SERVER_ADDR}"
+OMNI_VIDEO_RELAY_VIA="${ROBOT_SIDE_OMNISOCKET_RELAY_VIA}"
+OMNI_CONTROL_PEER_ID="peer-b-ctrl"
+OMNI_CONTROL_EXPECTED_SENDER="peer-a-ctrl"
+OMNI_CONTROL_SERVER_ADDR="${ROBOT_SIDE_OMNISOCKET_SERVER_ADDR}"
+OMNI_CONTROL_RELAY_VIA="${ROBOT_SIDE_OMNISOCKET_RELAY_VIA}"
+OMNI_CONTROL_UNIX_SOCKET_PATH="${ROBOT_RECEIVER_LOCAL_SOCKET_PATH}"
+
+B_SIDE_OMNID_USE_SUDO="1"

scripts/dev/start-b-side-omnid.sh

@@ -0,0 +1,25 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+# shellcheck disable=SC1091
+source "${SCRIPT_DIR}/load-env.sh"
+
+cd "${OMNISOCKETGO_ROOT}"
+
+export OMNISOCKET_SERVER_ADDR="${ROBOT_SIDE_OMNISOCKET_SERVER_ADDR}"
+export OMNISOCKET_RELAY_VIA="${ROBOT_SIDE_OMNISOCKET_RELAY_VIA}"
+export OMNI_VIDEO_SERVER_ADDR="${OMNI_VIDEO_SERVER_ADDR}"
+export OMNI_VIDEO_RELAY_VIA="${OMNI_VIDEO_RELAY_VIA}"
+export OMNI_CONTROL_SERVER_ADDR="${OMNI_CONTROL_SERVER_ADDR}"
+export OMNI_CONTROL_RELAY_VIA="${OMNI_CONTROL_RELAY_VIA}"
+
+if [[ ! -x "./bin/b_side_omnid" ]]; then
+  make b_side_omnid
+fi
+
+if [[ "${B_SIDE_OMNID_USE_SUDO}" == "1" && "${EUID}" -ne 0 ]]; then
+  exec sudo -E ./bin/b_side_omnid
+fi
+
+exec ./bin/b_side_omnid

scripts/dev/start-backend.sh

@@ -0,0 +1,18 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+# shellcheck disable=SC1091
+source "${SCRIPT_DIR}/load-env.sh"
+
+if [[ ! -d "${PYTHON_VENV_PATH}" ]]; then
+  "${PYTHON3_BIN}" -m venv "${PYTHON_VENV_PATH}"
+fi
+
+# shellcheck disable=SC1091
+source "${PYTHON_VENV_PATH}/bin/activate"
+
+cd "${BACKEND_DIR}"
+export OMNISOCKET_SERVER_ADDR="${CONTROL_SIDE_OMNISOCKET_SERVER_ADDR}"
+export OMNISOCKET_RELAY_VIA="${CONTROL_SIDE_OMNISOCKET_RELAY_VIA}"
+exec python -m uvicorn config.asgi:application --host "${BACKEND_HOST}" --port "${BACKEND_PORT}"

scripts/dev/start-dev-tmux.sh

@@ -0,0 +1,21 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+SESSION_NAME="${1:-robot-remote}"
+
+if ! command -v tmux >/dev/null 2>&1; then
+  echo "tmux is required for this launcher" >&2
+  exit 1
+fi
+
+if tmux has-session -t "${SESSION_NAME}" 2>/dev/null; then
+  exec tmux attach -t "${SESSION_NAME}"
+fi
+
+tmux new-session -d -s "${SESSION_NAME}" -n backend "bash -lc '${SCRIPT_DIR}/start-backend.sh'"
+tmux new-window -t "${SESSION_NAME}:" -n frontend "bash -lc '${SCRIPT_DIR}/start-frontend.sh'"
+tmux new-window -t "${SESSION_NAME}:" -n ros "bash -lc '${SCRIPT_DIR}/start-ros-receiver.sh'"
+tmux new-window -t "${SESSION_NAME}:" -n b-side "bash -lc '${SCRIPT_DIR}/start-b-side-omnid.sh'"
+
+exec tmux attach -t "${SESSION_NAME}"

scripts/dev/start-frontend.sh

@@ -0,0 +1,9 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+# shellcheck disable=SC1091
+source "${SCRIPT_DIR}/load-env.sh"
+
+cd "${FRONTEND_DIR}"
+exec npm run dev -- --host "${FRONTEND_HOST}" --port "${FRONTEND_PORT}"

scripts/dev/start-ros-receiver.sh

@@ -0,0 +1,24 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+# shellcheck disable=SC1091
+source "${SCRIPT_DIR}/load-env.sh"
+# shellcheck disable=SC1091
+source "/opt/ros/${ROS_DISTRO}/setup.bash"
+
+cd "${ROS_CONTROL_PY_DIR}"
+# shellcheck disable=SC1091
+source "install/setup.bash"
+
+exec ros2 launch udp_teleop_bridge robot_udp_receiver.launch.py \
+  "transport:=${ROBOT_RECEIVER_TRANSPORT}" \
+  "server_addr:=${ROBOT_RECEIVER_SERVER_ADDR}" \
+  "relay_via:=${ROBOT_RECEIVER_RELAY_VIA}" \
+  "peer_id:=${ROBOT_RECEIVER_PEER_ID}" \
+  "expected_sender:=${ROBOT_RECEIVER_EXPECTED_SENDER}" \
+  "local_socket_path:=${ROBOT_RECEIVER_LOCAL_SOCKET_PATH}" \
+  "output_topic:=${ROBOT_RECEIVER_OUTPUT_TOPIC}" \
+  "frame_id:=${ROBOT_RECEIVER_FRAME_ID}" \
+  "watchdog_timeout:=${ROBOT_RECEIVER_WATCHDOG_TIMEOUT}" \
+  "publish_rate_hz:=${ROBOT_RECEIVER_PUBLISH_RATE_HZ}"

scripts/start_b_side.sh

@@ -1,11 +0,0 @@
-#!/usr/bin/env bash
-set -euo pipefail
-
-ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
-CONFIG_PATH="${1:-$ROOT/config/b_side_omnidaemon.yaml}"
-
-export OMNIBDAEMON_CONFIG="$CONFIG_PATH"
-export PYTHONPATH="$ROOT/python${PYTHONPATH:+:$PYTHONPATH}"
-
-cd "$ROOT"
-exec python3 -m omnisocket_b_side.daemon --config "$CONFIG_PATH"

src/kcp_session_stats.c

@@ -160,6 +160,26 @@ int kcp_session_stats_log(kcp_session_stats_logger_t *logger, const kcp_session_
         kcp_session_stats_appendf(&line, &line_len, ",\"srttvar_ms\":%d", record->srttvar_ms) != 0) {
         goto cleanup;
     }
+    if (record->has_snd_wnd &&
+        kcp_session_stats_appendf(&line, &line_len, ",\"snd_wnd\":%u", record->snd_wnd) != 0) {
+        goto cleanup;
+    }
+    if (record->has_rmt_wnd &&
+        kcp_session_stats_appendf(&line, &line_len, ",\"rmt_wnd\":%u", record->rmt_wnd) != 0) {
+        goto cleanup;
+    }
+    if (record->has_inflight &&
+        kcp_session_stats_appendf(&line, &line_len, ",\"inflight\":%u", record->inflight) != 0) {
+        goto cleanup;
+    }
+    if (record->has_window_limit &&
+        kcp_session_stats_appendf(&line, &line_len, ",\"window_limit\":%u", record->window_limit) != 0) {
+        goto cleanup;
+    }
+    if (record->has_window_pressure_pct &&
+        kcp_session_stats_appendf(&line, &line_len, ",\"window_pressure_pct\":%.3f", record->window_pressure_pct) != 0) {
+        goto cleanup;
+    }
     if (record->has_bytes_sent &&
         kcp_session_stats_appendf(&line, &line_len, ",\"bytes_sent\":%" PRIu64, record->bytes_sent) != 0) {
         goto cleanup;

src/peer_kcp_client.c

@@ -294,12 +294,16 @@ int kcp_client_persist_message(kcp_client_t *client, const message_t *msg, const
     return 0;
 }
 
-int kcp_client_metrics_snapshot(kcp_client_t *client, kcp_conn_metrics_t *out_metrics) {
+void kcp_client_runtime_stats_snapshot(kcp_client_t *client, kcp_runtime_stats_t *out_stats) {
-    if (client == NULL || out_metrics == NULL) {
+    if (out_stats == NULL) {
-        errno = EINVAL;
+        return;
-        return -1;
     }
-    return kcp_conn_metrics_snapshot(client->conn, out_metrics);
+
+    memset(out_stats, 0, sizeof(*out_stats));
+    if (client == NULL || client->conn == NULL) {
+        return;
+    }
+    kcp_conn_runtime_stats_snapshot(client->conn, out_stats);
 }
 
 int kcp_client_close(kcp_client_t *client) {

src/peer_udp_client.c

@@ -1,6 +1,8 @@
 #include "peer_udp_client.h"
 
+#include <poll.h>
 #include <pthread.h>
+#include <string.h>
 
 struct udp_client {
     char id[OMNI_MAX_PEER_ID];
@@ -17,6 +19,19 @@ static int client_next_message_id(udp_client_t *client, uint64_t *out_id) {
     return 0;
 }
 
+static void udp_client_fill_recv_meta(udp_client_recv_meta_t *meta, const message_t *msg) {
+    if (meta == NULL || msg == NULL) {
+        return;
+    }
+    memset(meta, 0, sizeof(*meta));
+    meta->type = msg->type;
+    meta->id = msg->id;
+    meta->body_len = msg->body_len;
+    snprintf(meta->from, sizeof(meta->from), "%s", msg->from);
+    snprintf(meta->to, sizeof(meta->to), "%s", msg->to);
+    snprintf(meta->file_name, sizeof(meta->file_name), "%s", msg->file_name);
+}
+
 static int client_persist_message_to_disk(const message_t *msg, const char *inbox_dir, char *out_path, size_t out_path_len) {
     char path[512];
     if (omni_ensure_dir(inbox_dir) != 0) {
@@ -70,7 +85,7 @@ static int client_persist_message_to_disk(const message_t *msg, const char *inbo
     return 0;
 }
 
-udp_client_t *udp_client_dial(const char *server_addr, const char *peer_id, const char *bind_ip, latency_logger_t *logger, tx_timestamp_debug_logger_t *debug_logger, int enable_timestamping) {
+udp_client_t *udp_client_dial_with_options(const char *server_addr, const char *peer_id, const char *bind_ip, const char *bind_device, latency_logger_t *logger, tx_timestamp_debug_logger_t *debug_logger, int enable_timestamping) {
     udp_client_t *client;
     message_t register_msg;
     client = (udp_client_t *) calloc(1, sizeof(*client));
@@ -80,7 +95,7 @@ udp_client_t *udp_client_dial(const char *server_addr, const char *peer_id, cons
     snprintf(client->id, sizeof(client->id), "%s", peer_id);
     pthread_mutex_init(&client->id_mu, NULL);
     client->logger = logger;
-    client->conn = udp_conn_dial(server_addr, bind_ip, NULL, enable_timestamping, logger, OMNI_NODE_ROLE_PEER, peer_id, debug_logger);
+    client->conn = udp_conn_dial(server_addr, bind_ip, bind_device, enable_timestamping, logger, OMNI_NODE_ROLE_PEER, peer_id, debug_logger);
     if (client->conn == NULL) {
         udp_client_free(client);
         return NULL;
@@ -97,6 +112,10 @@ udp_client_t *udp_client_dial(const char *server_addr, const char *peer_id, cons
     return client;
 }
 
+udp_client_t *udp_client_dial(const char *server_addr, const char *peer_id, const char *bind_ip, latency_logger_t *logger, tx_timestamp_debug_logger_t *debug_logger, int enable_timestamping) {
+    return udp_client_dial_with_options(server_addr, peer_id, bind_ip, NULL, logger, debug_logger, enable_timestamping);
+}
+
 const char *udp_client_id(const udp_client_t *client) {
     return client == NULL ? "" : client->id;
 }
@@ -124,6 +143,38 @@ int udp_client_send_text(udp_client_t *client, const char *to, const char *text)
     return 0;
 }
 
+int udp_client_send_binary(udp_client_t *client, const char *to, const void *data, size_t data_len) {
+    message_t msg;
+    uint64_t id;
+
+    if (client == NULL || to == NULL || (data == NULL && data_len > 0)) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    protocol_message_init(&msg);
+    client_next_message_id(client, &id);
+    msg.type = MSG_TYPE_BINARY;
+    msg.id = id;
+    snprintf(msg.from, sizeof(msg.from), "%s", client->id);
+    snprintf(msg.to, sizeof(msg.to), "%s", to);
+    if (data_len > 0) {
+        msg.body = (uint8_t *) malloc(data_len);
+        if (msg.body == NULL) {
+            return -1;
+        }
+        memcpy(msg.body, data, data_len);
+    }
+    msg.body_len = data_len;
+    latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_A_APP_PREP_BEGIN, &msg);
+    if (udp_conn_send(client->conn, &msg) != 0) {
+        protocol_message_clear(&msg);
+        return -1;
+    }
+    protocol_message_clear(&msg);
+    return 0;
+}
+
 int udp_client_send_file_path(udp_client_t *client, const char *to, const char *path) {
     message_t msg;
     uint64_t id;
@@ -151,7 +202,34 @@ int udp_client_send_file_path(udp_client_t *client, const char *to, const char *
     return 0;
 }
 
-int udp_client_receive(udp_client_t *client, message_t *out_msg) {
+int udp_client_receive_timed(udp_client_t *client, message_t *out_msg, int timeout_ms) {
+    if (client == NULL || out_msg == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    if (timeout_ms >= 0) {
+        struct pollfd pfd;
+        int rc;
+
+        memset(&pfd, 0, sizeof(pfd));
+        pfd.fd = udp_conn_fd(client->conn);
+        pfd.events = POLLIN | POLLERR | POLLHUP;
+        do {
+            rc = poll(&pfd, 1, timeout_ms);
+        } while (rc < 0 && errno == EINTR);
+        if (rc == 0) {
+            return 1;
+        }
+        if (rc < 0) {
+            return -1;
+        }
+        if ((pfd.revents & (POLLERR | POLLHUP | POLLNVAL)) != 0 && (pfd.revents & POLLIN) == 0) {
+            errno = ECONNRESET;
+            return -1;
+        }
+    }
+
     if (udp_conn_receive(client->conn, out_msg, NULL, NULL) != 0) {
         return -1;
     }
@@ -159,6 +237,39 @@ int udp_client_receive(udp_client_t *client, message_t *out_msg) {
     return 0;
 }
 
+int udp_client_receive(udp_client_t *client, message_t *out_msg) {
+    return udp_client_receive_timed(client, out_msg, -1);
+}
+
+int udp_client_receive_into(udp_client_t *client, void *buffer, size_t buffer_len, udp_client_recv_meta_t *out_meta, int timeout_ms) {
+    message_t msg;
+    int rc;
+
+    if (client == NULL || (buffer == NULL && buffer_len > 0) || out_meta == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    protocol_message_init(&msg);
+    rc = udp_client_receive_timed(client, &msg, timeout_ms);
+    if (rc != 0) {
+        return rc;
+    }
+
+    udp_client_fill_recv_meta(out_meta, &msg);
+    if (msg.body_len > buffer_len) {
+        protocol_message_clear(&msg);
+        errno = EMSGSIZE;
+        return 2;
+    }
+
+    if (msg.body_len > 0) {
+        memcpy(buffer, msg.body, msg.body_len);
+    }
+    protocol_message_clear(&msg);
+    return 0;
+}
+
 int udp_client_persist_message(udp_client_t *client, const message_t *msg, const char *inbox_dir, char *out_path, size_t out_path_len) {
     if (!latencylog_is_business_message(msg)) {
         errno = EINVAL;

src/server_kcp_hub.c

@@ -1,8 +1,14 @@
 #include "server_kcp_hub.h"
 
+#include "cJSON.h"
+
+#include <stdatomic.h>
 #include <unistd.h>
 
 #define KCP_RELAY_MAX_DATAGRAM_SIZE (60 * 1024)
+#define KCP_HUB_DEFAULT_TELEMETRY_INTERVAL_MS 500
+#define KCP_HUB_TELEMETRY_NODE_ID "hub-telemetry"
+#define KCP_HUB_DEFAULT_NODE_ID "hub"
 
 typedef struct kcp_peer_entry {
     struct kcp_peer_entry *next;
@@ -21,14 +27,29 @@ struct kcp_hub {
     latency_logger_t *logger;
     kcp_session_stats_logger_t *stats_logger;
     int stats_interval_ms;
+    char telemetry_peer_id[OMNI_MAX_PEER_ID];
+    int telemetry_interval_ms;
+    pthread_t telemetry_thread;
+    int telemetry_thread_started;
     int relay_fd;
     int relay_configured;
     int relay_learn_peer;
     struct sockaddr_storage relay_peer_addr;
     socklen_t relay_peer_addr_len;
-    int closed;
+    atomic_int closed;
 };
 
+static int kcp_hub_peer_id_has_suffix(const char *peer_id, const char *suffix);
+static int kcp_hub_deliver_to_local_peer(kcp_hub_t *hub, const message_t *msg);
+
+static int kcp_hub_peer_is_telemetry(const char *peer_id) {
+    return kcp_hub_peer_id_has_suffix(peer_id, "-telemetry");
+}
+
+static const char *kcp_hub_peer_node_id(const char *peer_id) {
+    return kcp_hub_peer_is_telemetry(peer_id) ? KCP_HUB_TELEMETRY_NODE_ID : KCP_HUB_DEFAULT_NODE_ID;
+}
+
 static void kcp_hub_unregister(kcp_hub_t *hub, const char *peer_id, kcp_conn_t *conn) {
     kcp_peer_entry_t *prev = NULL;
     kcp_peer_entry_t *entry;
@@ -90,9 +111,201 @@ static int kcp_hub_configure_peer_transport(kcp_conn_t *conn, const char *peer_i
         kcp_conn_options_set_video_defaults(&options);
         return kcp_conn_apply_options(conn, &options);
     }
+    if (kcp_hub_peer_is_telemetry(peer_id)) {
+        kcp_conn_options_set_telemetry_defaults(&options);
+        return kcp_conn_apply_options(conn, &options);
+    }
     return 0;
 }
 
+static int kcp_hub_add_runtime_stats_json(cJSON *object, const kcp_runtime_stats_t *stats) {
+    if (object == NULL || stats == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+    if (cJSON_AddNumberToObject(object, "connected", stats->connected) == NULL ||
+        cJSON_AddNumberToObject(object, "conv", (double) stats->conv) == NULL ||
+        cJSON_AddNumberToObject(object, "rto_ms", (double) stats->rto_ms) == NULL ||
+        cJSON_AddNumberToObject(object, "srtt_ms", (double) stats->srtt_ms) == NULL ||
+        cJSON_AddNumberToObject(object, "srttvar_ms", (double) stats->srttvar_ms) == NULL ||
+        cJSON_AddNumberToObject(object, "snd_wnd", (double) stats->snd_wnd) == NULL ||
+        cJSON_AddNumberToObject(object, "rmt_wnd", (double) stats->rmt_wnd) == NULL ||
+        cJSON_AddNumberToObject(object, "inflight", (double) stats->inflight) == NULL ||
+        cJSON_AddNumberToObject(object, "window_limit", (double) stats->window_limit) == NULL ||
+        cJSON_AddNumberToObject(object, "window_pressure_pct", stats->window_pressure_pct) == NULL ||
+        cJSON_AddNumberToObject(object, "snd_queue", (double) stats->snd_queue) == NULL ||
+        cJSON_AddNumberToObject(object, "rcv_queue", (double) stats->rcv_queue) == NULL ||
+        cJSON_AddNumberToObject(object, "snd_buffer", (double) stats->snd_buffer) == NULL ||
+        cJSON_AddNumberToObject(object, "out_segs_total", (double) stats->out_segs_total) == NULL ||
+        cJSON_AddNumberToObject(object, "retrans_total", (double) stats->retrans_total) == NULL ||
+        cJSON_AddNumberToObject(object, "fast_retrans_total", (double) stats->fast_retrans_total) == NULL ||
+        cJSON_AddNumberToObject(object, "lost_total", (double) stats->lost_total) == NULL ||
+        cJSON_AddNumberToObject(object, "repeat_total", (double) stats->repeat_total) == NULL ||
+        cJSON_AddNumberToObject(object, "xmit_total", (double) stats->xmit_total) == NULL) {
+        errno = ENOMEM;
+        return -1;
+    }
+    return 0;
+}
+
+static int kcp_hub_build_telemetry_payload_locked(kcp_hub_t *hub, char **out_payload) {
+    cJSON *root = NULL;
+    cJSON *sessions = NULL;
+    char *ts_unix_nano_text = NULL;
+    char *payload = NULL;
+    kcp_peer_entry_t *entry;
+
+    if (hub == NULL || out_payload == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+    *out_payload = NULL;
+
+    root = cJSON_CreateObject();
+    if (root == NULL) {
+        errno = ENOMEM;
+        return -1;
+    }
+    sessions = cJSON_AddArrayToObject(root, "sessions");
+    if (sessions == NULL) {
+        cJSON_Delete(root);
+        errno = ENOMEM;
+        return -1;
+    }
+
+    ts_unix_nano_text = omni_strdup_printf("%" PRId64, omni_now_unix_nano());
+    if (ts_unix_nano_text == NULL) {
+        cJSON_Delete(root);
+        return -1;
+    }
+    if (cJSON_AddStringToObject(root, "type", "hub_kcp_snapshot") == NULL ||
+        cJSON_AddStringToObject(root, "ts_unix_nano", ts_unix_nano_text) == NULL ||
+        cJSON_AddStringToObject(root, "node_id", KCP_HUB_DEFAULT_NODE_ID) == NULL) {
+        free(ts_unix_nano_text);
+        cJSON_Delete(root);
+        errno = ENOMEM;
+        return -1;
+    }
+    free(ts_unix_nano_text);
+
+    for (entry = hub->peers; entry != NULL; entry = entry->next) {
+        cJSON *session = NULL;
+        kcp_runtime_stats_t stats;
+        struct sockaddr_storage local_addr;
+        struct sockaddr_storage remote_addr;
+        socklen_t local_len = sizeof(local_addr);
+        socklen_t remote_len = sizeof(remote_addr);
+        char local_text[OMNI_MAX_ADDR_TEXT] = "";
+        char remote_text[OMNI_MAX_ADDR_TEXT] = "";
+
+        if (entry->conn == NULL || entry->peer_id[0] == '\0' || kcp_hub_peer_is_telemetry(entry->peer_id)) {
+            continue;
+        }
+
+        memset(&stats, 0, sizeof(stats));
+        kcp_conn_runtime_stats_snapshot(entry->conn, &stats);
+        if (kcp_conn_local_addr(entry->conn, &local_addr, &local_len) != 0) {
+            local_len = 0;
+        }
+        if (kcp_conn_remote_addr(entry->conn, &remote_addr, &remote_len) != 0) {
+            remote_len = 0;
+        }
+        if (local_len > 0) {
+            omni_sockaddr_to_string((const struct sockaddr *) &local_addr, local_len, local_text, sizeof(local_text));
+        }
+        if (remote_len > 0) {
+            omni_sockaddr_to_string((const struct sockaddr *) &remote_addr, remote_len, remote_text, sizeof(remote_text));
+        }
+
+        session = cJSON_CreateObject();
+        if (session == NULL) {
+            cJSON_Delete(root);
+            errno = ENOMEM;
+            return -1;
+        }
+        cJSON_AddItemToArray(sessions, session);
+        if (cJSON_AddStringToObject(session, "peer_id", entry->peer_id) == NULL ||
+            cJSON_AddStringToObject(session, "local_addr", local_text) == NULL ||
+            cJSON_AddStringToObject(session, "remote_addr", remote_text) == NULL ||
+            kcp_hub_add_runtime_stats_json(session, &stats) != 0) {
+            cJSON_Delete(root);
+            errno = ENOMEM;
+            return -1;
+        }
+    }
+
+    payload = cJSON_PrintUnformatted(root);
+    cJSON_Delete(root);
+    if (payload == NULL) {
+        errno = ENOMEM;
+        return -1;
+    }
+    *out_payload = payload;
+    return 0;
+}
+
+static int kcp_hub_push_telemetry_snapshot(kcp_hub_t *hub) {
+    message_t msg;
+    char *payload = NULL;
+    char telemetry_peer_id[OMNI_MAX_PEER_ID];
+    int rc;
+
+    if (hub == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    pthread_rwlock_rdlock(&hub->lock);
+    if (hub->telemetry_peer_id[0] == '\0' || kcp_hub_find_peer(hub, hub->telemetry_peer_id) == NULL) {
+        pthread_rwlock_unlock(&hub->lock);
+        return 0;
+    }
+    snprintf(telemetry_peer_id, sizeof(telemetry_peer_id), "%s", hub->telemetry_peer_id);
+    rc = kcp_hub_build_telemetry_payload_locked(hub, &payload);
+    pthread_rwlock_unlock(&hub->lock);
+    if (rc != 0) {
+        return -1;
+    }
+
+    protocol_message_init(&msg);
+    msg.type = MSG_TYPE_TEXT;
+    snprintf(msg.from, sizeof(msg.from), "%s", SERVER_PEER_ID);
+    snprintf(msg.to, sizeof(msg.to), "%s", telemetry_peer_id);
+    msg.body = (uint8_t *) omni_strdup(payload == NULL ? "" : payload);
+    cJSON_free(payload);
+    if (msg.body == NULL) {
+        return -1;
+    }
+    msg.body_len = strlen((const char *) msg.body);
+    rc = kcp_hub_deliver_to_local_peer(hub, &msg);
+    protocol_message_clear(&msg);
+    if (rc != 0 && errno == ENOENT) {
+        return 0;
+    }
+    return rc;
+}
+
+static void *kcp_hub_telemetry_thread_main(void *arg) {
+    kcp_hub_t *hub = (kcp_hub_t *) arg;
+
+    while (!atomic_load(&hub->closed)) {
+        int interval_ms = KCP_HUB_DEFAULT_TELEMETRY_INTERVAL_MS;
+
+        pthread_rwlock_rdlock(&hub->lock);
+        if (hub->telemetry_interval_ms > 0) {
+            interval_ms = hub->telemetry_interval_ms;
+        }
+        pthread_rwlock_unlock(&hub->lock);
+
+        (void) kcp_hub_push_telemetry_snapshot(hub);
+        if (atomic_load(&hub->closed)) {
+            break;
+        }
+        usleep((useconds_t) interval_ms * 1000U);
+    }
+    return NULL;
+}
+
 static int kcp_hub_send_server_error(kcp_conn_t *conn, const char *to, const char *message) {
     message_t msg;
     protocol_message_init(&msg);
@@ -389,11 +602,6 @@ static int kcp_hub_register_conn(kcp_hub_t *hub, kcp_conn_t *conn, char *peer_id
     pthread_rwlock_unlock(&hub->lock);
 
     snprintf(peer_id, peer_id_len, "%s", msg.from);
-    if (kcp_hub_configure_peer_transport(conn, peer_id) != 0) {
-        kcp_hub_unregister(hub, peer_id, conn);
-        protocol_message_clear(&msg);
-        return -1;
-    }
     protocol_message_clear(&msg);
     return 0;
 }
@@ -414,7 +622,9 @@ kcp_hub_t *kcp_hub_new(latency_logger_t *logger, kcp_session_stats_logger_t *sta
     hub->logger = logger;
     hub->stats_logger = stats_logger;
     hub->stats_interval_ms = stats_interval_ms > 0 ? stats_interval_ms : KCP_DEFAULT_STATS_INTERVAL_MS;
+    hub->telemetry_interval_ms = KCP_HUB_DEFAULT_TELEMETRY_INTERVAL_MS;
     hub->relay_fd = -1;
+    atomic_init(&hub->closed, 0);
     return hub;
 }
 
@@ -423,13 +633,13 @@ int kcp_hub_serve_listener(kcp_hub_t *hub, kcp_listener_t *listener) {
         errno = EINVAL;
         return -1;
     }
-    while (!hub->closed) {
+    while (!atomic_load(&hub->closed)) {
         kcp_conn_t *conn = kcp_listener_accept(listener);
         kcp_session_thread_ctx_t *ctx;
         pthread_t thread;
 
         if (conn == NULL) {
-            if (hub->closed) {
+            if (atomic_load(&hub->closed)) {
                 return 0;
             }
             return -1;
@@ -455,6 +665,7 @@ int kcp_hub_serve_listener(kcp_hub_t *hub, kcp_listener_t *listener) {
 
 int kcp_hub_serve_session(kcp_hub_t *hub, kcp_conn_t *conn) {
     char peer_id[OMNI_MAX_PEER_ID];
+    const char *node_id;
     int rc = 0;
 
     if (hub == NULL || conn == NULL) {
@@ -462,12 +673,20 @@ int kcp_hub_serve_session(kcp_hub_t *hub, kcp_conn_t *conn) {
         return -1;
     }
     peer_id[0] = '\0';
-    if (kcp_conn_configure_runtime(conn, hub->logger, OMNI_NODE_ROLE_SERVER, "hub", hub->stats_logger, hub->stats_interval_ms) != 0) {
+    if (kcp_hub_register_conn(hub, conn, peer_id, sizeof(peer_id)) != 0) {
         kcp_conn_close(conn);
         kcp_conn_free(conn);
         return -1;
     }
-    if (kcp_hub_register_conn(hub, conn, peer_id, sizeof(peer_id)) != 0) {
+    if (kcp_hub_configure_peer_transport(conn, peer_id) != 0) {
+        kcp_hub_unregister(hub, peer_id, conn);
+        kcp_conn_close(conn);
+        kcp_conn_free(conn);
+        return -1;
+    }
+    node_id = kcp_hub_peer_node_id(peer_id);
+    if (kcp_conn_configure_runtime(conn, hub->logger, OMNI_NODE_ROLE_SERVER, node_id, hub->stats_logger, hub->stats_interval_ms) != 0) {
+        kcp_hub_unregister(hub, peer_id, conn);
         kcp_conn_close(conn);
         kcp_conn_free(conn);
         return -1;
@@ -512,6 +731,34 @@ int kcp_hub_set_relay(kcp_hub_t *hub, int relay_fd, const struct sockaddr *peer_
     return 0;
 }
 
+int kcp_hub_set_telemetry(kcp_hub_t *hub, const char *peer_id, int interval_ms) {
+    int start_thread = 0;
+
+    if (hub == NULL || peer_id == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+    pthread_rwlock_wrlock(&hub->lock);
+    snprintf(hub->telemetry_peer_id, sizeof(hub->telemetry_peer_id), "%s", peer_id);
+    hub->telemetry_interval_ms = interval_ms > 0 ? interval_ms : KCP_HUB_DEFAULT_TELEMETRY_INTERVAL_MS;
+    if (!hub->telemetry_thread_started && hub->telemetry_peer_id[0] != '\0') {
+        start_thread = 1;
+        hub->telemetry_thread_started = 1;
+    }
+    pthread_rwlock_unlock(&hub->lock);
+
+    if (start_thread) {
+        if (pthread_create(&hub->telemetry_thread, NULL, kcp_hub_telemetry_thread_main, hub) != 0) {
+            pthread_rwlock_wrlock(&hub->lock);
+            hub->telemetry_thread_started = 0;
+            hub->telemetry_peer_id[0] = '\0';
+            pthread_rwlock_unlock(&hub->lock);
+            return -1;
+        }
+    }
+    return 0;
+}
+
 int kcp_hub_serve_relay(kcp_hub_t *hub) {
     uint8_t buffer[KCP_RELAY_MAX_DATAGRAM_SIZE];
 
@@ -519,7 +766,7 @@ int kcp_hub_serve_relay(kcp_hub_t *hub) {
         errno = EINVAL;
         return -1;
     }
-    while (!hub->closed) {
+    while (!atomic_load(&hub->closed)) {
         struct sockaddr_storage source;
         socklen_t source_len = sizeof(source);
         ssize_t n;
@@ -537,7 +784,7 @@ int kcp_hub_serve_relay(kcp_hub_t *hub) {
 
         n = recvfrom(relay_fd, buffer, sizeof(buffer), 0, (struct sockaddr *) &source, &source_len);
         if (n < 0) {
-            if (hub->closed) {
+            if (atomic_load(&hub->closed)) {
                 return 0;
             }
             if (errno == EINTR) {
@@ -568,8 +815,7 @@ int kcp_hub_close(kcp_hub_t *hub) {
     if (hub == NULL) {
         return 0;
     }
-    if (!hub->closed) {
+    if (!atomic_exchange(&hub->closed, 1)) {
-        hub->closed = 1;
         if (hub->relay_fd >= 0) {
             close(hub->relay_fd);
             hub->relay_fd = -1;
@@ -586,6 +832,9 @@ void kcp_hub_free(kcp_hub_t *hub) {
         return;
     }
     kcp_hub_close(hub);
+    if (hub->telemetry_thread_started) {
+        pthread_join(hub->telemetry_thread, NULL);
+    }
     for (entry = hub->peers; entry != NULL; entry = next) {
         next = entry->next;
         if (entry->conn != NULL) {

src/transport_kcp.c

@@ -62,6 +62,7 @@ struct kcp_conn {
     int stats_thread_started;
     kcp_conn_options_t options;
     int update_interval_ms;
+    atomic_uint_fast64_t total_out_segs;
     uint64_t pending_bytes_sent;
     uint64_t pending_bytes_received;
     uint64_t pending_in_pkts;
@@ -129,6 +130,10 @@ struct kcp_process_sampler {
     uint64_t prev_out_segs;
     uint64_t prev_in_errs;
     uint64_t prev_kcp_in_errs;
+    uint64_t prev_retrans_segs;
+    uint64_t prev_fast_retrans_segs;
+    uint64_t prev_lost_segs;
+    uint64_t prev_repeat_segs;
     atomic_uint_fast64_t bytes_sent;
     atomic_uint_fast64_t bytes_received;
     atomic_uint_fast64_t in_pkts;
@@ -185,6 +190,20 @@ void kcp_conn_options_set_video_defaults(kcp_conn_options_t *options) {
     options->mtu = KCP_VIDEO_MTU;
 }
 
+void kcp_conn_options_set_telemetry_defaults(kcp_conn_options_t *options) {
+    if (options == NULL) {
+        return;
+    }
+    memset(options, 0, sizeof(*options));
+    options->nodelay = KCP_TELEMETRY_NODELAY;
+    options->interval_ms = KCP_TELEMETRY_INTERVAL_MS;
+    options->resend = KCP_TELEMETRY_RESEND;
+    options->nc = KCP_TELEMETRY_NC;
+    options->sndwnd = KCP_TELEMETRY_SND_WND;
+    options->rcvwnd = KCP_TELEMETRY_RCV_WND;
+    options->mtu = KCP_TELEMETRY_MTU;
+}
+
 static int kcp_conn_validate_options(const kcp_conn_options_t *options) {
     if (options == NULL) {
         errno = EINVAL;
@@ -328,6 +347,7 @@ static void kcp_conn_record_send(kcp_conn_t *conn, int packet_bytes, size_t segm
     if (conn == NULL) {
         return;
     }
+    atomic_fetch_add_explicit(&conn->total_out_segs, (uint64_t) segments, memory_order_relaxed);
     if (conn->process_sampler != NULL) {
         kcp_process_sampler_record_send(conn->process_sampler, packet_bytes, segments);
         return;
@@ -420,7 +440,14 @@ static void kcp_process_sampler_remove_conn(kcp_process_sampler_t *sampler, kcp_
     }
 }
 
-static void kcp_process_sampler_collect_gauges(kcp_process_sampler_t *sampler, uint64_t *snd_queue, uint64_t *rcv_queue, uint64_t *snd_buffer) {
+static void kcp_process_sampler_collect_gauges(kcp_process_sampler_t *sampler,
+                                               uint64_t *snd_queue,
+                                               uint64_t *rcv_queue,
+                                               uint64_t *snd_buffer,
+                                               uint64_t *retrans_segs,
+                                               uint64_t *fast_retrans_segs,
+                                               uint64_t *lost_segs,
+                                               uint64_t *repeat_segs) {
     kcp_conn_t *conn;
 
     if (snd_queue != NULL) {
@@ -432,6 +459,18 @@ static void kcp_process_sampler_collect_gauges(kcp_process_sampler_t *sampler, u
     if (snd_buffer != NULL) {
         *snd_buffer = 0;
     }
+    if (retrans_segs != NULL) {
+        *retrans_segs = 0;
+    }
+    if (fast_retrans_segs != NULL) {
+        *fast_retrans_segs = 0;
+    }
+    if (lost_segs != NULL) {
+        *lost_segs = 0;
+    }
+    if (repeat_segs != NULL) {
+        *repeat_segs = 0;
+    }
     if (sampler == NULL) {
         return;
     }
@@ -449,6 +488,18 @@ static void kcp_process_sampler_collect_gauges(kcp_process_sampler_t *sampler, u
             if (snd_buffer != NULL) {
                 *snd_buffer += conn->kcp->nsnd_buf;
             }
+            if (lost_segs != NULL) {
+                *lost_segs += conn->kcp->timeout_retrans_total;
+            }
+            if (fast_retrans_segs != NULL) {
+                *fast_retrans_segs += conn->kcp->fast_retrans_total;
+            }
+            if (retrans_segs != NULL) {
+                *retrans_segs += conn->kcp->timeout_retrans_total + conn->kcp->fast_retrans_total;
+            }
+            if (repeat_segs != NULL) {
+                *repeat_segs += conn->kcp->duplicate_recv_total;
+            }
         }
         pthread_mutex_unlock(&conn->kcp_mu);
     }
@@ -468,6 +519,10 @@ static void kcp_process_sampler_log_snapshot(kcp_process_sampler_t *sampler, con
     uint64_t snd_queue = 0;
     uint64_t rcv_queue = 0;
     uint64_t snd_buffer = 0;
+    uint64_t retrans_segs = 0;
+    uint64_t fast_retrans_segs = 0;
+    uint64_t lost_segs = 0;
+    uint64_t repeat_segs = 0;
 
     if (sampler == NULL || sampler->logger == NULL) {
         return;
@@ -481,7 +536,15 @@ static void kcp_process_sampler_log_snapshot(kcp_process_sampler_t *sampler, con
     out_segs = atomic_load_explicit(&sampler->out_segs, memory_order_relaxed);
     in_errs = atomic_load_explicit(&sampler->in_errs, memory_order_relaxed);
     kcp_in_errs = atomic_load_explicit(&sampler->kcp_in_errs, memory_order_relaxed);
-    kcp_process_sampler_collect_gauges(sampler, &snd_queue, &rcv_queue, &snd_buffer);
+    kcp_process_sampler_collect_gauges(
+        sampler,
+        &snd_queue,
+        &rcv_queue,
+        &snd_buffer,
+        &retrans_segs,
+        &fast_retrans_segs,
+        &lost_segs,
+        &repeat_segs);
 
     memset(&record, 0, sizeof(record));
     snprintf(record.record_type, sizeof(record.record_type), "%s", KCP_SESSION_STATS_RECORD_PROCESS_SAMPLE);
@@ -502,6 +565,14 @@ static void kcp_process_sampler_log_snapshot(kcp_process_sampler_t *sampler, con
     record.in_segs = kcp_counter_diff(sampler->prev_in_segs, in_segs);
     record.has_out_segs = 1;
     record.out_segs = kcp_counter_diff(sampler->prev_out_segs, out_segs);
+    record.has_retrans_segs = 1;
+    record.retrans_segs = kcp_counter_diff(sampler->prev_retrans_segs, retrans_segs);
+    record.has_fast_retrans_segs = 1;
+    record.fast_retrans_segs = kcp_counter_diff(sampler->prev_fast_retrans_segs, fast_retrans_segs);
+    record.has_lost_segs = 1;
+    record.lost_segs = kcp_counter_diff(sampler->prev_lost_segs, lost_segs);
+    record.has_repeat_segs = 1;
+    record.repeat_segs = kcp_counter_diff(sampler->prev_repeat_segs, repeat_segs);
     record.has_in_errs = 1;
     record.in_errs = kcp_counter_diff(sampler->prev_in_errs, in_errs);
     record.has_kcp_in_errs = 1;
@@ -521,6 +592,10 @@ static void kcp_process_sampler_log_snapshot(kcp_process_sampler_t *sampler, con
     sampler->prev_out_pkts = out_pkts;
     sampler->prev_in_segs = in_segs;
     sampler->prev_out_segs = out_segs;
+    sampler->prev_retrans_segs = retrans_segs;
+    sampler->prev_fast_retrans_segs = fast_retrans_segs;
+    sampler->prev_lost_segs = lost_segs;
+    sampler->prev_repeat_segs = repeat_segs;
     sampler->prev_in_errs = in_errs;
     sampler->prev_kcp_in_errs = kcp_in_errs;
 
@@ -1006,6 +1081,11 @@ static void kcp_log_session_snapshot(kcp_conn_t *conn, const char *reason) {
     socklen_t local_len = sizeof(local_addr);
     char local_text[OMNI_MAX_ADDR_TEXT];
     char remote_text[OMNI_MAX_ADDR_TEXT];
+    uint32_t inflight = 0;
+    uint32_t window_limit = 0;
+    uint64_t out_segs_total = 0;
+    uint64_t fast_retrans_total = 0;
+    uint64_t lost_total = 0;
     if (conn == NULL || conn->stats_logger == NULL || conn->sock_state == NULL || conn->kcp == NULL) {
         return;
     }
@@ -1029,7 +1109,38 @@ static void kcp_log_session_snapshot(kcp_conn_t *conn, const char *reason) {
     record.srtt_ms = conn->kcp->rx_srtt;
     record.has_srttvar_ms = 1;
     record.srttvar_ms = conn->kcp->rx_rttval;
+    record.has_snd_wnd = 1;
+    record.snd_wnd = conn->kcp->snd_wnd;
+    record.has_rmt_wnd = 1;
+    record.rmt_wnd = conn->kcp->rmt_wnd;
+    inflight = conn->kcp->snd_nxt - conn->kcp->snd_una;
+    window_limit = conn->kcp->snd_wnd < conn->kcp->rmt_wnd ? conn->kcp->snd_wnd : conn->kcp->rmt_wnd;
+    record.has_inflight = 1;
+    record.inflight = inflight;
+    record.has_window_limit = 1;
+    record.window_limit = window_limit;
+    record.has_window_pressure_pct = 1;
+    record.window_pressure_pct = window_limit == 0 ? 0.0 : ((double) inflight * 100.0) / (double) window_limit;
+    record.has_ring_buffer_snd_queue = 1;
+    record.ring_buffer_snd_queue = conn->kcp->nsnd_que;
+    record.has_ring_buffer_rcv_queue = 1;
+    record.ring_buffer_rcv_queue = conn->kcp->nrcv_que;
+    record.has_ring_buffer_snd_buffer = 1;
+    record.ring_buffer_snd_buffer = conn->kcp->nsnd_buf;
+    lost_total = conn->kcp->timeout_retrans_total;
+    fast_retrans_total = conn->kcp->fast_retrans_total;
+    record.has_retrans_segs = 1;
+    record.retrans_segs = lost_total + fast_retrans_total;
+    record.has_fast_retrans_segs = 1;
+    record.fast_retrans_segs = fast_retrans_total;
+    record.has_lost_segs = 1;
+    record.lost_segs = lost_total;
+    record.has_repeat_segs = 1;
+    record.repeat_segs = conn->kcp->duplicate_recv_total;
     pthread_mutex_unlock(&conn->kcp_mu);
+    out_segs_total = atomic_load_explicit(&conn->total_out_segs, memory_order_relaxed);
+    record.has_out_segs = 1;
+    record.out_segs = out_segs_total;
     (void) kcp_session_stats_log(conn->stats_logger, &record);
 }
 
@@ -1788,81 +1899,55 @@ int kcp_conn_local_addr(const kcp_conn_t *conn, struct sockaddr_storage *addr, s
     return 0;
 }
 
-int kcp_conn_metrics_snapshot(kcp_conn_t *conn, kcp_conn_metrics_t *out_metrics) {
+int kcp_conn_remote_addr(const kcp_conn_t *conn, struct sockaddr_storage *addr, socklen_t *addr_len) {
-    struct sockaddr_storage local_addr;
+    if (conn == NULL || addr == NULL || addr_len == NULL) {
-    socklen_t local_len = sizeof(local_addr);
-
-    if (conn == NULL || out_metrics == NULL) {
         errno = EINVAL;
         return -1;
     }
-
+    if (conn->remote_addr_len == 0) {
-    memset(out_metrics, 0, sizeof(*out_metrics));
+        errno = ENOTCONN;
-    out_metrics->connected = !atomic_load(&conn->closed);
+        return -1;
-
-    if (conn->sock_state != NULL && !conn->socket_closed &&
-        getsockname(conn->sock_state->fd, (struct sockaddr *) &local_addr, &local_len) == 0) {
-        omni_sockaddr_to_string(
-            (const struct sockaddr *) &local_addr,
-            local_len,
-            out_metrics->local_addr,
-            sizeof(out_metrics->local_addr)
-        );
     }
-    if (conn->remote_addr_len > 0) {
+    return omni_clone_sockaddr((const struct sockaddr *) &conn->remote_addr, conn->remote_addr_len, addr, addr_len);
-        omni_sockaddr_to_string(
+}
-            (const struct sockaddr *) &conn->remote_addr,
+
-            conn->remote_addr_len,
+void kcp_conn_runtime_stats_snapshot(kcp_conn_t *conn, kcp_runtime_stats_t *out_stats) {
-            out_metrics->remote_addr,
+    if (out_stats == NULL) {
-            sizeof(out_metrics->remote_addr)
+        return;
-        );
     }
 
-    if (conn->process_sampler != NULL) {
+    memset(out_stats, 0, sizeof(*out_stats));
-        out_metrics->bytes_sent = atomic_load_explicit(&conn->process_sampler->bytes_sent, memory_order_relaxed);
+    if (conn == NULL) {
-        out_metrics->bytes_received = atomic_load_explicit(&conn->process_sampler->bytes_received, memory_order_relaxed);
+        return;
-        out_metrics->in_pkts = atomic_load_explicit(&conn->process_sampler->in_pkts, memory_order_relaxed);
-        out_metrics->out_pkts = atomic_load_explicit(&conn->process_sampler->out_pkts, memory_order_relaxed);
-        out_metrics->in_segs = atomic_load_explicit(&conn->process_sampler->in_segs, memory_order_relaxed);
-        out_metrics->out_segs = atomic_load_explicit(&conn->process_sampler->out_segs, memory_order_relaxed);
-        out_metrics->in_errs = atomic_load_explicit(&conn->process_sampler->in_errs, memory_order_relaxed);
-        out_metrics->kcp_in_errs = atomic_load_explicit(&conn->process_sampler->kcp_in_errs, memory_order_relaxed);
-    } else {
-        out_metrics->bytes_sent = conn->pending_bytes_sent;
-        out_metrics->bytes_received = conn->pending_bytes_received;
-        out_metrics->in_pkts = conn->pending_in_pkts;
-        out_metrics->out_pkts = conn->pending_out_pkts;
-        out_metrics->in_segs = conn->pending_in_segs;
-        out_metrics->out_segs = conn->pending_out_segs;
-        out_metrics->in_errs = conn->pending_in_errs;
-        out_metrics->kcp_in_errs = conn->pending_kcp_in_errs;
     }
 
+    out_stats->connected = atomic_load(&conn->closed) ? 0 : 1;
     pthread_mutex_lock(&conn->kcp_mu);
     if (conn->kcp != NULL) {
-        out_metrics->has_conv = 1;
+        out_stats->conv = conn->kcp->conv;
-        out_metrics->conv = conn->kcp->conv;
+        out_stats->rto_ms = conn->kcp->rx_rto;
-        out_metrics->rto_ms = conn->kcp->rx_rto;
+        out_stats->srtt_ms = conn->kcp->rx_srtt;
-        out_metrics->srtt_ms = conn->kcp->rx_srtt;
+        out_stats->srttvar_ms = conn->kcp->rx_rttval;
-        out_metrics->srttvar_ms = conn->kcp->rx_rttval;
+        out_stats->snd_wnd = conn->kcp->snd_wnd;
-        out_metrics->ring_buffer_snd_queue = conn->kcp->nsnd_que;
+        out_stats->rmt_wnd = conn->kcp->rmt_wnd;
-        out_metrics->ring_buffer_rcv_queue = conn->kcp->nrcv_que;
+        out_stats->inflight = conn->kcp->snd_nxt - conn->kcp->snd_una;
-        out_metrics->ring_buffer_snd_buffer = conn->kcp->nsnd_buf;
+        out_stats->window_limit = conn->kcp->snd_wnd < conn->kcp->rmt_wnd ? conn->kcp->snd_wnd : conn->kcp->rmt_wnd;
-        out_metrics->fast_retrans_segs = conn->kcp->fast_retrans_xmit;
+        out_stats->window_pressure_pct = out_stats->window_limit == 0
-        /* This KCP fork does not implement early retransmit, so the counter stays zero. */
+                                             ? 0.0
-        out_metrics->early_retrans_segs = conn->kcp->early_retrans_xmit;
+                                             : ((double) out_stats->inflight * 100.0) / (double) out_stats->window_limit;
-        out_metrics->lost_segs = conn->kcp->lost_xmit;
+        out_stats->snd_queue = conn->kcp->nsnd_que;
-        out_metrics->repeat_segs = conn->kcp->repeat_xmit;
+        out_stats->rcv_queue = conn->kcp->nrcv_que;
-        out_metrics->retrans_segs =
+        out_stats->snd_buffer = conn->kcp->nsnd_buf;
-            out_metrics->fast_retrans_segs +
+        out_stats->out_segs_total = atomic_load_explicit(&conn->total_out_segs, memory_order_relaxed);
-            out_metrics->early_retrans_segs +
+        out_stats->fast_retrans_total = conn->kcp->fast_retrans_total;
-            out_metrics->lost_segs;
+        out_stats->lost_total = conn->kcp->timeout_retrans_total;
+        out_stats->retrans_total = out_stats->lost_total + out_stats->fast_retrans_total;
+        out_stats->repeat_total = conn->kcp->duplicate_recv_total;
+        out_stats->xmit_total = conn->kcp->xmit;
     } else {
-        out_metrics->connected = 0;
+        out_stats->connected = 0;
     }
     pthread_mutex_unlock(&conn->kcp_mu);
-
-    return 0;
 }
 
 int kcp_conn_close(kcp_conn_t *conn) {
@@ -1876,6 +1961,8 @@ int kcp_conn_close(kcp_conn_t *conn) {
         pthread_mutex_lock(&conn->kcp_mu);
         atomic_store(&conn->closed, 1);
         if (conn->owns_socket && !conn->socket_closed) {
+            /* Wake the blocking recv thread before closing the shared UDP socket. */
+            (void) shutdown(conn->fd, SHUT_RDWR);
             close(conn->fd);
             conn->socket_closed = 1;
         }

src/transport_udp.c

@@ -3,6 +3,7 @@
 #include <arpa/inet.h>
 #include <netdb.h>
 #include <pthread.h>
+#include <sys/socket.h>
 #include <unistd.h>
 
 typedef struct udp_pending_tx {
@@ -413,6 +414,13 @@ int udp_conn_receive(udp_conn_t *conn, message_t *out_msg, struct sockaddr_stora
     }
     n = recvmsg(conn->fd, &msg, 0);
     if (n < 0) {
+        if (conn->closed) {
+            errno = ECANCELED;
+        }
+        return -1;
+    }
+    if (n == 0 && conn->closed) {
+        errno = ECANCELED;
         return -1;
     }
     if (conn->timestamping_enabled) {
@@ -454,6 +462,8 @@ int udp_conn_close(udp_conn_t *conn) {
     }
     if (!conn->closed) {
         conn->closed = 1;
+        /* Wake blocking recvmsg()/poll users before tearing down the socket. */
+        (void) shutdown(conn->fd, SHUT_RDWR);
         close(conn->fd);
         if (conn->errqueue_thread_started) {
             pthread_join(conn->errqueue_thread, NULL);

src/video_pipeline.c

@@ -0,0 +1,883 @@
+#include "video_pipeline.h"
+
+#include <errno.h>
+#include <fcntl.h>
+#include <linux/videodev2.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <sys/mman.h>
+#include <sys/select.h>
+#include <sys/time.h>
+#include <time.h>
+#include <unistd.h>
+
+#include <libavcodec/avcodec.h>
+#include <libavformat/avformat.h>
+#include <libavutil/avutil.h>
+#include <libavutil/imgutils.h>
+#include <libavutil/opt.h>
+#include <libswscale/swscale.h>
+
+#define VIDEO_CAPTURE_WIDTH_DEFAULT 1280
+#define VIDEO_CAPTURE_HEIGHT_DEFAULT 720
+#define VIDEO_OUTPUT_WIDTH_DEFAULT 640
+#define VIDEO_OUTPUT_HEIGHT_DEFAULT 360
+#define VIDEO_NUM_BUFFERS 4
+#define VIDEO_DEFAULT_CAMERA_DEVICE "/dev/video0"
+#define VIDEO_DEFAULT_PEER_ID "peer-b-video"
+#define VIDEO_DEFAULT_TARGET_PEER "peer-a-video"
+
+typedef struct video_buffer {
+    void *start;
+    size_t length;
+} video_buffer_t;
+
+typedef struct video_sender {
+    kcp_client_t *client;
+    char target_peer[OMNI_MAX_PEER_ID];
+    uint8_t *send_buffer;
+    size_t send_buffer_cap;
+} video_sender_t;
+
+static int video_pipeline_stop_requested(volatile sig_atomic_t *stop_requested) {
+    return stop_requested != NULL && *stop_requested != 0;
+}
+
+static int env_flag_or_default(const char *name, int fallback) {
+    const char *value = getenv(name);
+
+    if (value == NULL || value[0] == '\0') {
+        return fallback;
+    }
+    if (
+        strcmp(value, "1") == 0 || strcmp(value, "true") == 0 || strcmp(value, "TRUE") == 0
+        || strcmp(value, "yes") == 0 || strcmp(value, "on") == 0
+    ) {
+        return 1;
+    }
+    if (
+        strcmp(value, "0") == 0 || strcmp(value, "false") == 0 || strcmp(value, "FALSE") == 0
+        || strcmp(value, "no") == 0 || strcmp(value, "off") == 0
+    ) {
+        return 0;
+    }
+    return fallback;
+}
+
+static double video_pipeline_now_ms(void) {
+    struct timespec ts;
+
+    clock_gettime(CLOCK_MONOTONIC, &ts);
+    return ts.tv_sec * 1000.0 + ts.tv_nsec / 1000000.0;
+}
+
+static void video_pipeline_print_timing_header(void) {
+    fprintf(stderr, "Frame | Capture | Decode | Scale | Encode | Send | Total | Size | Marker\n");
+    fprintf(stderr, "------|---------|--------|-------|--------|------|-------|------|--------\n");
+}
+
+static void video_pipeline_print_timing_failure(int frame_number, const char *stage) {
+    fprintf(stderr, "Frame %d: %s failed\n", frame_number, stage);
+}
+
+static void video_pipeline_print_timing_row(
+    int frame_number,
+    double capture_ms,
+    double decode_ms,
+    double scale_ms,
+    double encode_ms,
+    double send_ms,
+    double total_ms,
+    const AVPacket *encoded_pkt
+) {
+    size_t size_kb = 0;
+    unsigned int marker = 0;
+
+    if (encoded_pkt != NULL) {
+        size_kb = (size_t) encoded_pkt->size / 1024;
+        if (encoded_pkt->size > 1) {
+            marker = encoded_pkt->data[1];
+        }
+    }
+
+    fprintf(
+        stderr,
+        "%5d | %7.1f | %6.1f | %5.1f | %6.1f | %4.1f | %5.1f | %4zu KB | 0x%02x\n",
+        frame_number,
+        capture_ms,
+        decode_ms,
+        scale_ms,
+        encode_ms,
+        send_ms,
+        total_ms,
+        size_kb,
+        marker
+    );
+}
+
+static const char *env_or_default(const char *name, const char *fallback) {
+    const char *value = getenv(name);
+    if (value != NULL && value[0] != '\0') {
+        return value;
+    }
+    return fallback;
+}
+
+static const char *env_first_nonempty(const char *first, const char *second, const char *fallback) {
+    const char *value = getenv(first);
+    if (value != NULL && value[0] != '\0') {
+        return value;
+    }
+    value = getenv(second);
+    if (value != NULL && value[0] != '\0') {
+        return value;
+    }
+    return fallback;
+}
+
+static void video_pipeline_set_error(video_pipeline_stats_t *stats, const char *message) {
+    if (stats == NULL) {
+        return;
+    }
+    pthread_mutex_lock(&stats->mutex);
+    snprintf(stats->last_error, sizeof(stats->last_error), "%s", message == NULL ? "" : message);
+    pthread_mutex_unlock(&stats->mutex);
+}
+
+static void video_pipeline_set_errno_error(video_pipeline_stats_t *stats, const char *prefix) {
+    char buffer[256];
+    int saved_errno = errno;
+
+    snprintf(
+        buffer,
+        sizeof(buffer),
+        "%s: %s (errno=%d)",
+        prefix == NULL ? "video pipeline error" : prefix,
+        saved_errno != 0 ? strerror(saved_errno) : "unknown error",
+        saved_errno
+    );
+    video_pipeline_set_error(stats, buffer);
+}
+
+void video_pipeline_config_init(video_pipeline_config_t *config) {
+    if (config == NULL) {
+        return;
+    }
+    memset(config, 0, sizeof(*config));
+    config->camera_device = VIDEO_DEFAULT_CAMERA_DEVICE;
+    config->server_addr = "";
+    config->relay_via = "";
+    config->bind_ip = "";
+    config->bind_device = "";
+    config->peer_id = VIDEO_DEFAULT_PEER_ID;
+    config->target_peer = VIDEO_DEFAULT_TARGET_PEER;
+    config->capture_width = VIDEO_CAPTURE_WIDTH_DEFAULT;
+    config->capture_height = VIDEO_CAPTURE_HEIGHT_DEFAULT;
+    config->output_width = VIDEO_OUTPUT_WIDTH_DEFAULT;
+    config->output_height = VIDEO_OUTPUT_HEIGHT_DEFAULT;
+    config->max_frames = 0;
+    config->enable_timing_logs = 0;
+}
+
+void video_pipeline_config_load_env(video_pipeline_config_t *config) {
+    if (config == NULL) {
+        return;
+    }
+    config->camera_device = env_or_default("OMNI_CAMERA_DEVICE", config->camera_device);
+    config->server_addr = env_first_nonempty("OMNI_VIDEO_SERVER_ADDR", "OMNISOCKET_SERVER_ADDR", config->server_addr);
+    config->relay_via = env_first_nonempty("OMNI_VIDEO_RELAY_VIA", "OMNISOCKET_RELAY_VIA", config->relay_via);
+    config->bind_ip = env_first_nonempty("OMNI_VIDEO_BIND_IP", "OMNISOCKET_BIND_IP", config->bind_ip);
+    config->bind_device = env_first_nonempty("OMNI_VIDEO_BIND_DEVICE", "OMNISOCKET_BIND_DEVICE", config->bind_device);
+    config->peer_id = env_or_default("OMNI_VIDEO_PEER_ID", config->peer_id);
+    config->target_peer = env_or_default("OMNI_VIDEO_TARGET_PEER", config->target_peer);
+    if (getenv("OMNI_VIDEO_MAX_FRAMES") != NULL) {
+        config->max_frames = atoi(getenv("OMNI_VIDEO_MAX_FRAMES"));
+    }
+    config->enable_timing_logs = env_flag_or_default("OMNI_VIDEO_DEBUG_TIMING", config->enable_timing_logs);
+}
+
+int video_pipeline_stats_init(video_pipeline_stats_t *stats) {
+    int rc;
+    if (stats == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+    memset(stats, 0, sizeof(*stats));
+    rc = pthread_mutex_init(&stats->mutex, NULL);
+    if (rc != 0) {
+        errno = rc;
+        return -1;
+    }
+    return 0;
+}
+
+void video_pipeline_stats_destroy(video_pipeline_stats_t *stats) {
+    if (stats == NULL) {
+        return;
+    }
+    pthread_mutex_destroy(&stats->mutex);
+}
+
+void video_pipeline_stats_snapshot(video_pipeline_stats_t *stats, video_pipeline_stats_t *out_stats) {
+    if (stats == NULL || out_stats == NULL) {
+        return;
+    }
+    memset(out_stats, 0, sizeof(*out_stats));
+    pthread_mutex_lock(&stats->mutex);
+    out_stats->frames_sent = stats->frames_sent;
+    out_stats->bytes_sent = stats->bytes_sent;
+    out_stats->send_errors = stats->send_errors;
+    out_stats->last_frame_bytes = stats->last_frame_bytes;
+    out_stats->connected = stats->connected;
+    snprintf(out_stats->last_error, sizeof(out_stats->last_error), "%s", stats->last_error);
+    out_stats->transport = stats->transport;
+    pthread_mutex_unlock(&stats->mutex);
+}
+
+static int open_v4l2_device(const char *device) {
+    return open(device, O_RDWR | O_NONBLOCK);
+}
+
+static int init_v4l2_device(int fd, int width, int height) {
+    struct v4l2_format fmt;
+
+    memset(&fmt, 0, sizeof(fmt));
+    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    fmt.fmt.pix.width = width;
+    fmt.fmt.pix.height = height;
+    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
+    fmt.fmt.pix.field = V4L2_FIELD_NONE;
+    return ioctl(fd, VIDIOC_S_FMT, &fmt);
+}
+
+static int init_mmap(int fd, video_buffer_t **buffers, int *num_buffers) {
+    struct v4l2_requestbuffers req;
+    int i;
+
+    memset(&req, 0, sizeof(req));
+    req.count = VIDEO_NUM_BUFFERS;
+    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    req.memory = V4L2_MEMORY_MMAP;
+    if (ioctl(fd, VIDIOC_REQBUFS, &req) < 0) {
+        return -1;
+    }
+
+    *num_buffers = (int) req.count;
+    *buffers = (video_buffer_t *) calloc(req.count, sizeof(video_buffer_t));
+    if (*buffers == NULL) {
+        return -1;
+    }
+
+    for (i = 0; i < (int) req.count; i++) {
+        struct v4l2_buffer buf;
+
+        memset(&buf, 0, sizeof(buf));
+        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        buf.memory = V4L2_MEMORY_MMAP;
+        buf.index = (unsigned int) i;
+        if (ioctl(fd, VIDIOC_QUERYBUF, &buf) < 0) {
+            return -1;
+        }
+
+        (*buffers)[i].length = buf.length;
+        (*buffers)[i].start = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, buf.m.offset);
+        if ((*buffers)[i].start == MAP_FAILED) {
+            return -1;
+        }
+    }
+
+    return 0;
+}
+
+static AVCodecContext *create_mjpeg_decoder(int width, int height) {
+    const AVCodec *decoder = avcodec_find_decoder(AV_CODEC_ID_MJPEG);
+    AVCodecContext *ctx;
+    AVDictionary *opts = NULL;
+
+    if (decoder == NULL) {
+        errno = ENOENT;
+        return NULL;
+    }
+
+    ctx = avcodec_alloc_context3(decoder);
+    if (ctx == NULL) {
+        return NULL;
+    }
+    ctx->width = width;
+    ctx->height = height;
+    ctx->pix_fmt = AV_PIX_FMT_YUVJ420P;
+    ctx->color_range = AVCOL_RANGE_JPEG;
+    ctx->thread_count = 1;
+
+    av_dict_set(&opts, "flags2", "+fast", 0);
+    if (avcodec_open2(ctx, decoder, &opts) < 0) {
+        avcodec_free_context(&ctx);
+        av_dict_free(&opts);
+        errno = EINVAL;
+        return NULL;
+    }
+    av_dict_free(&opts);
+    return ctx;
+}
+
+static AVCodecContext *create_mjpeg_encoder(int width, int height) {
+    const AVCodec *encoder = avcodec_find_encoder(AV_CODEC_ID_MJPEG);
+    AVCodecContext *ctx;
+    AVDictionary *opts = NULL;
+
+    if (encoder == NULL) {
+        errno = ENOENT;
+        return NULL;
+    }
+
+    ctx = avcodec_alloc_context3(encoder);
+    if (ctx == NULL) {
+        return NULL;
+    }
+    ctx->width = width;
+    ctx->height = height;
+    ctx->pix_fmt = AV_PIX_FMT_YUVJ420P;
+    ctx->time_base = (AVRational){1, 30};
+    ctx->qmin = 8;
+    ctx->qmax = 31;
+    ctx->flags |= AV_CODEC_FLAG_QSCALE;
+    ctx->global_quality = FF_QP2LAMBDA * 5;
+
+    av_dict_set(&opts, "huffman", "default", 0);
+    if (avcodec_open2(ctx, encoder, &opts) < 0) {
+        avcodec_free_context(&ctx);
+        av_dict_free(&opts);
+        errno = EINVAL;
+        return NULL;
+    }
+    av_dict_free(&opts);
+    return ctx;
+}
+
+static int decode_mjpeg_frame(AVCodecContext *decoder, const uint8_t *data, int size, AVFrame **frame) {
+    AVPacket *pkt;
+    int ret;
+
+    if (frame == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    *frame = NULL;
+    pkt = av_packet_alloc();
+    if (pkt == NULL) {
+        return -1;
+    }
+    pkt->data = (uint8_t *) data;
+    pkt->size = size;
+
+    ret = avcodec_send_packet(decoder, pkt);
+    if (ret < 0) {
+        av_packet_free(&pkt);
+        errno = EINVAL;
+        return -1;
+    }
+
+    *frame = av_frame_alloc();
+    if (*frame == NULL) {
+        av_packet_free(&pkt);
+        return -1;
+    }
+
+    ret = avcodec_receive_frame(decoder, *frame);
+    av_packet_free(&pkt);
+    if (ret < 0) {
+        av_frame_free(frame);
+        errno = EINVAL;
+        return -1;
+    }
+    return 0;
+}
+
+static int ensure_scale_context(
+    struct SwsContext **sws_ctx,
+    int *cached_src_width,
+    int *cached_src_height,
+    int *cached_src_format,
+    const AVFrame *src,
+    int output_width,
+    int output_height
+) {
+    if (
+        *sws_ctx != NULL
+        && *cached_src_width == src->width
+        && *cached_src_height == src->height
+        && *cached_src_format == src->format
+    ) {
+        return 0;
+    }
+
+    sws_freeContext(*sws_ctx);
+    *sws_ctx = sws_getContext(
+        src->width,
+        src->height,
+        src->format,
+        output_width,
+        output_height,
+        AV_PIX_FMT_YUVJ420P,
+        SWS_BILINEAR,
+        NULL,
+        NULL,
+        NULL
+    );
+    if (*sws_ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    *cached_src_width = src->width;
+    *cached_src_height = src->height;
+    *cached_src_format = src->format;
+    return 0;
+}
+
+static int scale_frame(AVFrame *src, AVFrame **dst, struct SwsContext *sws_ctx, int output_width, int output_height) {
+    int ret;
+
+    if (sws_ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    *dst = av_frame_alloc();
+    if (*dst == NULL) {
+        return -1;
+    }
+    (*dst)->width = output_width;
+    (*dst)->height = output_height;
+    (*dst)->format = AV_PIX_FMT_YUVJ420P;
+    if (av_frame_get_buffer(*dst, 0) < 0) {
+        av_frame_free(dst);
+        errno = ENOMEM;
+        return -1;
+    }
+
+    ret = sws_scale(
+        sws_ctx,
+        (const uint8_t *const *) src->data,
+        src->linesize,
+        0,
+        src->height,
+        (*dst)->data,
+        (*dst)->linesize
+    );
+    if (ret < 0) {
+        av_frame_free(dst);
+        errno = EINVAL;
+        return -1;
+    }
+    return 0;
+}
+
+static int video_sender_ensure_buffer_capacity(video_sender_t *sender, size_t min_capacity) {
+    uint8_t *resized_buffer;
+    size_t next_capacity;
+
+    if (sender == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+    if (sender->send_buffer_cap >= min_capacity) {
+        return 0;
+    }
+
+    next_capacity = sender->send_buffer_cap == 0 ? min_capacity : sender->send_buffer_cap;
+    while (next_capacity < min_capacity) {
+        next_capacity *= 2;
+    }
+
+    resized_buffer = (uint8_t *) realloc(sender->send_buffer, next_capacity);
+    if (resized_buffer == NULL) {
+        return -1;
+    }
+
+    sender->send_buffer = resized_buffer;
+    sender->send_buffer_cap = next_capacity;
+    return 0;
+}
+
+static int encode_frame(AVCodecContext *encoder, AVFrame *frame, AVPacket **pkt) {
+    int ret;
+
+    if (pkt == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    *pkt = av_packet_alloc();
+    if (*pkt == NULL) {
+        return -1;
+    }
+    ret = avcodec_send_frame(encoder, frame);
+    if (ret < 0) {
+        av_packet_free(pkt);
+        errno = EINVAL;
+        return -1;
+    }
+
+    ret = avcodec_receive_packet(encoder, *pkt);
+    if (ret < 0) {
+        av_packet_free(pkt);
+        errno = EINVAL;
+        return -1;
+    }
+    return 0;
+}
+
+static int64_t get_realtime_ms(void) {
+    struct timespec ts;
+    clock_gettime(CLOCK_REALTIME, &ts);
+    return (int64_t) ts.tv_sec * 1000 + ts.tv_nsec / 1000000;
+}
+
+static int video_sender_init(video_sender_t *sender, const video_pipeline_config_t *config) {
+    kcp_conn_options_t options;
+
+    if (sender == NULL || config == NULL || config->server_addr == NULL || config->server_addr[0] == '\0') {
+        errno = EINVAL;
+        return -1;
+    }
+
+    memset(sender, 0, sizeof(*sender));
+    snprintf(sender->target_peer, sizeof(sender->target_peer), "%s", config->target_peer);
+    kcp_conn_options_set_video_defaults(&options);
+    sender->client = kcp_client_dial_with_options(
+        config->server_addr,
+        config->relay_via,
+        config->peer_id,
+        config->bind_ip,
+        config->bind_device,
+        &options,
+        NULL,
+        NULL,
+        NULL,
+        KCP_DEFAULT_STATS_INTERVAL_MS
+    );
+    if (sender->client == NULL) {
+        return -1;
+    }
+    return 0;
+}
+
+static int video_sender_send_packet(video_sender_t *sender, const AVPacket *encoded_pkt, uint64_t timestamp) {
+    uint8_t *payload;
+    size_t payload_len;
+    int rc;
+
+    if (sender == NULL || sender->client == NULL || encoded_pkt == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    payload_len = (size_t) encoded_pkt->size + sizeof(timestamp);
+    if (video_sender_ensure_buffer_capacity(sender, payload_len) != 0) {
+        return -1;
+    }
+    payload = sender->send_buffer;
+
+    memcpy(payload, encoded_pkt->data, (size_t) encoded_pkt->size);
+    memcpy(payload + encoded_pkt->size, &timestamp, sizeof(timestamp));
+    rc = kcp_client_send_binary(sender->client, sender->target_peer, payload, payload_len);
+    return rc;
+}
+
+static void video_sender_close(video_sender_t *sender) {
+    if (sender == NULL) {
+        return;
+    }
+    if (sender->client != NULL) {
+        kcp_client_close(sender->client);
+        kcp_client_free(sender->client);
+        sender->client = NULL;
+    }
+    free(sender->send_buffer);
+    sender->send_buffer = NULL;
+    sender->send_buffer_cap = 0;
+}
+
+static void video_pipeline_cleanup_buffers(video_buffer_t *buffers, int num_buffers) {
+    int i;
+    if (buffers == NULL) {
+        return;
+    }
+    for (i = 0; i < num_buffers; i++) {
+        if (buffers[i].start != NULL && buffers[i].start != MAP_FAILED) {
+            munmap(buffers[i].start, buffers[i].length);
+        }
+    }
+    free(buffers);
+}
+
+int video_pipeline_run(const video_pipeline_config_t *config, video_pipeline_stats_t *stats, volatile sig_atomic_t *stop_requested) {
+    video_pipeline_config_t defaults;
+    video_sender_t sender;
+    video_buffer_t *buffers = NULL;
+    AVCodecContext *decoder = NULL;
+    AVCodecContext *encoder = NULL;
+    struct SwsContext *sws_ctx = NULL;
+    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    int num_buffers = 0;
+    int fd = -1;
+    int frame_index = 0;
+    int rc = -1;
+    int sws_src_width = 0;
+    int sws_src_height = 0;
+    int sws_src_format = -1;
+
+    memset(&sender, 0, sizeof(sender));
+    if (stats == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    video_pipeline_config_init(&defaults);
+    if (config == NULL) {
+        config = &defaults;
+    }
+
+#ifdef QUIET_FFMPEG_LOGS
+    av_log_set_level(AV_LOG_ERROR);
+#endif
+
+    if (config->server_addr == NULL || config->server_addr[0] == '\0') {
+        errno = EINVAL;
+        video_pipeline_set_error(stats, "video server address is required");
+        return -1;
+    }
+
+    fd = open_v4l2_device(config->camera_device);
+    if (fd < 0) {
+        video_pipeline_set_errno_error(stats, "failed to open camera device");
+        goto cleanup;
+    }
+    if (init_v4l2_device(fd, config->capture_width, config->capture_height) < 0) {
+        video_pipeline_set_errno_error(stats, "failed to configure V4L2");
+        goto cleanup;
+    }
+    if (init_mmap(fd, &buffers, &num_buffers) < 0) {
+        video_pipeline_set_errno_error(stats, "failed to initialize V4L2 mmap");
+        goto cleanup;
+    }
+
+    decoder = create_mjpeg_decoder(config->capture_width, config->capture_height);
+    encoder = create_mjpeg_encoder(config->output_width, config->output_height);
+    if (decoder == NULL || encoder == NULL) {
+        video_pipeline_set_errno_error(stats, "failed to initialize codecs");
+        goto cleanup;
+    }
+
+    if (video_sender_init(&sender, config) < 0) {
+        video_pipeline_set_errno_error(stats, "failed to start video sender");
+        goto cleanup;
+    }
+
+    pthread_mutex_lock(&stats->mutex);
+    stats->connected = 1;
+    stats->last_error[0] = '\0';
+    pthread_mutex_unlock(&stats->mutex);
+
+    for (frame_index = 0; frame_index < num_buffers; frame_index++) {
+        struct v4l2_buffer buf;
+
+        memset(&buf, 0, sizeof(buf));
+        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        buf.memory = V4L2_MEMORY_MMAP;
+        buf.index = (unsigned int) frame_index;
+        if (ioctl(fd, VIDIOC_QBUF, &buf) < 0) {
+            video_pipeline_set_errno_error(stats, "failed to queue V4L2 buffer");
+            goto cleanup;
+        }
+    }
+
+    if (ioctl(fd, VIDIOC_STREAMON, &type) < 0) {
+        video_pipeline_set_errno_error(stats, "failed to start V4L2 streaming");
+        goto cleanup;
+    }
+    if (config->enable_timing_logs) {
+        fprintf(stderr, "\nRunning video pipeline timing benchmark...\n");
+        video_pipeline_print_timing_header();
+    }
+
+    frame_index = 0;
+    while (!video_pipeline_stop_requested(stop_requested)) {
+        fd_set fds;
+        struct timeval timeout;
+        struct v4l2_buffer buf;
+        AVFrame *decoded_frame = NULL;
+        AVFrame *scaled_frame = NULL;
+        AVPacket *encoded_pkt = NULL;
+        int select_rc;
+        double total_start_ms = 0.0;
+        double capture_start_ms = 0.0;
+        double capture_end_ms = 0.0;
+        double decode_start_ms = 0.0;
+        double decode_end_ms = 0.0;
+        double scale_start_ms = 0.0;
+        double scale_end_ms = 0.0;
+        double encode_start_ms = 0.0;
+        double encode_end_ms = 0.0;
+        double send_start_ms = 0.0;
+        double send_end_ms = 0.0;
+        int frame_number = frame_index + 1;
+
+        if (config->max_frames > 0 && frame_index >= config->max_frames) {
+            break;
+        }
+        if (config->enable_timing_logs) {
+            total_start_ms = video_pipeline_now_ms();
+        }
+
+        FD_ZERO(&fds);
+        FD_SET(fd, &fds);
+        timeout.tv_sec = 2;
+        timeout.tv_usec = 0;
+        select_rc = select(fd + 1, &fds, NULL, NULL, &timeout);
+        if (select_rc <= 0) {
+            if (select_rc == 0) {
+                errno = ETIMEDOUT;
+            }
+            video_pipeline_set_errno_error(stats, "failed waiting for camera frame");
+            goto cleanup;
+        }
+        if (config->enable_timing_logs) {
+            capture_start_ms = video_pipeline_now_ms();
+        }
+
+        memset(&buf, 0, sizeof(buf));
+        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        buf.memory = V4L2_MEMORY_MMAP;
+        if (ioctl(fd, VIDIOC_DQBUF, &buf) < 0) {
+            video_pipeline_set_errno_error(stats, "failed to dequeue V4L2 buffer");
+            goto cleanup;
+        }
+        if (config->enable_timing_logs) {
+            capture_end_ms = video_pipeline_now_ms();
+            decode_start_ms = capture_end_ms;
+        }
+
+        if (decode_mjpeg_frame(decoder, (const uint8_t *) buffers[buf.index].start, (int) buf.bytesused, &decoded_frame) != 0) {
+            if (config->enable_timing_logs) {
+                video_pipeline_print_timing_failure(frame_number, "decode");
+            }
+            (void) ioctl(fd, VIDIOC_QBUF, &buf);
+            continue;
+        }
+        if (config->enable_timing_logs) {
+            decode_end_ms = video_pipeline_now_ms();
+            scale_start_ms = decode_end_ms;
+        }
+        if (
+            ensure_scale_context(
+                &sws_ctx,
+                &sws_src_width,
+                &sws_src_height,
+                &sws_src_format,
+                decoded_frame,
+                config->output_width,
+                config->output_height
+            ) != 0
+            || scale_frame(decoded_frame, &scaled_frame, sws_ctx, config->output_width, config->output_height) != 0
+        ) {
+            if (config->enable_timing_logs) {
+                video_pipeline_print_timing_failure(frame_number, "scale");
+            }
+            av_frame_free(&decoded_frame);
+            (void) ioctl(fd, VIDIOC_QBUF, &buf);
+            continue;
+        }
+        if (config->enable_timing_logs) {
+            scale_end_ms = video_pipeline_now_ms();
+            encode_start_ms = scale_end_ms;
+        }
+        if (encode_frame(encoder, scaled_frame, &encoded_pkt) != 0) {
+            if (config->enable_timing_logs) {
+                video_pipeline_print_timing_failure(frame_number, "encode");
+            }
+            av_frame_free(&decoded_frame);
+            av_frame_free(&scaled_frame);
+            (void) ioctl(fd, VIDIOC_QBUF, &buf);
+            continue;
+        }
+        if (config->enable_timing_logs) {
+            encode_end_ms = video_pipeline_now_ms();
+            send_start_ms = encode_end_ms;
+        }
+
+        if (video_sender_send_packet(&sender, encoded_pkt, (uint64_t) get_realtime_ms()) != 0) {
+            pthread_mutex_lock(&stats->mutex);
+            stats->send_errors += 1;
+            pthread_mutex_unlock(&stats->mutex);
+            if (config->enable_timing_logs) {
+                video_pipeline_print_timing_failure(frame_number, "send");
+            }
+            video_pipeline_set_errno_error(stats, "failed to send video packet");
+            av_frame_free(&decoded_frame);
+            av_frame_free(&scaled_frame);
+            av_packet_free(&encoded_pkt);
+            (void) ioctl(fd, VIDIOC_QBUF, &buf);
+            goto cleanup;
+        }
+        if (config->enable_timing_logs) {
+            send_end_ms = video_pipeline_now_ms();
+        }
+
+        pthread_mutex_lock(&stats->mutex);
+        stats->frames_sent += 1;
+        stats->bytes_sent += (uint64_t) encoded_pkt->size;
+        stats->last_frame_bytes = (uint64_t) encoded_pkt->size;
+        kcp_client_runtime_stats_snapshot(sender.client, &stats->transport);
+        pthread_mutex_unlock(&stats->mutex);
+        if (config->enable_timing_logs) {
+            video_pipeline_print_timing_row(
+                frame_number,
+                capture_end_ms - capture_start_ms,
+                decode_end_ms - decode_start_ms,
+                scale_end_ms - scale_start_ms,
+                encode_end_ms - encode_start_ms,
+                send_end_ms - send_start_ms,
+                send_end_ms - total_start_ms,
+                encoded_pkt
+            );
+        }
+
+        av_frame_free(&decoded_frame);
+        av_frame_free(&scaled_frame);
+        av_packet_free(&encoded_pkt);
+
+        if (ioctl(fd, VIDIOC_QBUF, &buf) < 0) {
+            video_pipeline_set_errno_error(stats, "failed to requeue V4L2 buffer");
+            goto cleanup;
+        }
+        frame_index += 1;
+    }
+
+    rc = 0;
+
+cleanup:
+    pthread_mutex_lock(&stats->mutex);
+    stats->connected = 0;
+    pthread_mutex_unlock(&stats->mutex);
+    if (fd >= 0) {
+        (void) ioctl(fd, VIDIOC_STREAMOFF, &type);
+    }
+    video_sender_close(&sender);
+    if (encoder != NULL) {
+        avcodec_free_context(&encoder);
+    }
+    if (decoder != NULL) {
+        avcodec_free_context(&decoder);
+    }
+    sws_freeContext(sws_ctx);
+    video_pipeline_cleanup_buffers(buffers, num_buffers);
+    if (fd >= 0) {
+        close(fd);
+    }
+    return rc;
+}

src/video_pipeline_gps.c

@@ -0,0 +1,883 @@
+#include "video_pipeline.h"
+
+#include <errno.h>
+#include <fcntl.h>
+#include <linux/videodev2.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <sys/mman.h>
+#include <sys/select.h>
+#include <sys/time.h>
+#include <time.h>
+#include <unistd.h>
+
+#include <libavcodec/avcodec.h>
+#include <libavformat/avformat.h>
+#include <libavutil/avutil.h>
+#include <libavutil/imgutils.h>
+#include <libavutil/opt.h>
+#include <libswscale/swscale.h>
+
+#define VIDEO_CAPTURE_WIDTH_DEFAULT 1280
+#define VIDEO_CAPTURE_HEIGHT_DEFAULT 720
+#define VIDEO_OUTPUT_WIDTH_DEFAULT 640
+#define VIDEO_OUTPUT_HEIGHT_DEFAULT 360
+#define VIDEO_NUM_BUFFERS 4
+#define VIDEO_DEFAULT_CAMERA_DEVICE "/dev/video0"
+#define VIDEO_DEFAULT_PEER_ID "peer-b-video"
+#define VIDEO_DEFAULT_TARGET_PEER "peer-a-video"
+
+typedef struct video_buffer {
+    void *start;
+    size_t length;
+} video_buffer_t;
+
+typedef struct video_sender {
+    kcp_client_t *client;
+    char target_peer[OMNI_MAX_PEER_ID];
+    uint8_t *send_buffer;
+    size_t send_buffer_cap;
+} video_sender_t;
+
+static int video_pipeline_stop_requested(volatile sig_atomic_t *stop_requested) {
+    return stop_requested != NULL && *stop_requested != 0;
+}
+
+static int env_flag_or_default(const char *name, int fallback) {
+    const char *value = getenv(name);
+
+    if (value == NULL || value[0] == '\0') {
+        return fallback;
+    }
+    if (
+        strcmp(value, "1") == 0 || strcmp(value, "true") == 0 || strcmp(value, "TRUE") == 0
+        || strcmp(value, "yes") == 0 || strcmp(value, "on") == 0
+    ) {
+        return 1;
+    }
+    if (
+        strcmp(value, "0") == 0 || strcmp(value, "false") == 0 || strcmp(value, "FALSE") == 0
+        || strcmp(value, "no") == 0 || strcmp(value, "off") == 0
+    ) {
+        return 0;
+    }
+    return fallback;
+}
+
+static double video_pipeline_now_ms(void) {
+    struct timespec ts;
+
+    clock_gettime(CLOCK_MONOTONIC, &ts);
+    return ts.tv_sec * 1000.0 + ts.tv_nsec / 1000000.0;
+}
+
+static void video_pipeline_print_timing_header(void) {
+    fprintf(stderr, "Frame | Capture | Decode | Scale | Encode | Send | Total | Size | Marker\n");
+    fprintf(stderr, "------|---------|--------|-------|--------|------|-------|------|--------\n");
+}
+
+static void video_pipeline_print_timing_failure(int frame_number, const char *stage) {
+    fprintf(stderr, "Frame %d: %s failed\n", frame_number, stage);
+}
+
+static void video_pipeline_print_timing_row(
+    int frame_number,
+    double capture_ms,
+    double decode_ms,
+    double scale_ms,
+    double encode_ms,
+    double send_ms,
+    double total_ms,
+    const AVPacket *encoded_pkt
+) {
+    size_t size_kb = 0;
+    unsigned int marker = 0;
+
+    if (encoded_pkt != NULL) {
+        size_kb = (size_t) encoded_pkt->size / 1024;
+        if (encoded_pkt->size > 1) {
+            marker = encoded_pkt->data[1];
+        }
+    }
+
+    fprintf(
+        stderr,
+        "%5d | %7.1f | %6.1f | %5.1f | %6.1f | %4.1f | %5.1f | %4zu KB | 0x%02x\n",
+        frame_number,
+        capture_ms,
+        decode_ms,
+        scale_ms,
+        encode_ms,
+        send_ms,
+        total_ms,
+        size_kb,
+        marker
+    );
+}
+
+static const char *env_or_default(const char *name, const char *fallback) {
+    const char *value = getenv(name);
+    if (value != NULL && value[0] != '\0') {
+        return value;
+    }
+    return fallback;
+}
+
+static const char *env_first_nonempty(const char *first, const char *second, const char *fallback) {
+    const char *value = getenv(first);
+    if (value != NULL && value[0] != '\0') {
+        return value;
+    }
+    value = getenv(second);
+    if (value != NULL && value[0] != '\0') {
+        return value;
+    }
+    return fallback;
+}
+
+static void video_pipeline_set_error(video_pipeline_stats_t *stats, const char *message) {
+    if (stats == NULL) {
+        return;
+    }
+    pthread_mutex_lock(&stats->mutex);
+    snprintf(stats->last_error, sizeof(stats->last_error), "%s", message == NULL ? "" : message);
+    pthread_mutex_unlock(&stats->mutex);
+}
+
+static void video_pipeline_set_errno_error(video_pipeline_stats_t *stats, const char *prefix) {
+    char buffer[256];
+    int saved_errno = errno;
+
+    snprintf(
+        buffer,
+        sizeof(buffer),
+        "%s: %s (errno=%d)",
+        prefix == NULL ? "video pipeline error" : prefix,
+        saved_errno != 0 ? strerror(saved_errno) : "unknown error",
+        saved_errno
+    );
+    video_pipeline_set_error(stats, buffer);
+}
+
+void video_pipeline_config_init(video_pipeline_config_t *config) {
+    if (config == NULL) {
+        return;
+    }
+    memset(config, 0, sizeof(*config));
+    config->camera_device = VIDEO_DEFAULT_CAMERA_DEVICE;
+    config->server_addr = "";
+    config->relay_via = "";
+    config->bind_ip = "";
+    config->bind_device = "";
+    config->peer_id = VIDEO_DEFAULT_PEER_ID;
+    config->target_peer = VIDEO_DEFAULT_TARGET_PEER;
+    config->capture_width = VIDEO_CAPTURE_WIDTH_DEFAULT;
+    config->capture_height = VIDEO_CAPTURE_HEIGHT_DEFAULT;
+    config->output_width = VIDEO_OUTPUT_WIDTH_DEFAULT;
+    config->output_height = VIDEO_OUTPUT_HEIGHT_DEFAULT;
+    config->max_frames = 0;
+    config->enable_timing_logs = 0;
+}
+
+void video_pipeline_config_load_env(video_pipeline_config_t *config) {
+    if (config == NULL) {
+        return;
+    }
+    config->camera_device = env_or_default("OMNI_CAMERA_DEVICE", config->camera_device);
+    config->server_addr = env_first_nonempty("OMNI_VIDEO_SERVER_ADDR", "OMNISOCKET_SERVER_ADDR", config->server_addr);
+    config->relay_via = env_first_nonempty("OMNI_VIDEO_RELAY_VIA", "OMNISOCKET_RELAY_VIA", config->relay_via);
+    config->bind_ip = env_first_nonempty("OMNI_VIDEO_BIND_IP", "OMNISOCKET_BIND_IP", config->bind_ip);
+    config->bind_device = env_first_nonempty("OMNI_VIDEO_BIND_DEVICE", "OMNISOCKET_BIND_DEVICE", config->bind_device);
+    config->peer_id = env_or_default("OMNI_VIDEO_PEER_ID", config->peer_id);
+    config->target_peer = env_or_default("OMNI_VIDEO_TARGET_PEER", config->target_peer);
+    if (getenv("OMNI_VIDEO_MAX_FRAMES") != NULL) {
+        config->max_frames = atoi(getenv("OMNI_VIDEO_MAX_FRAMES"));
+    }
+    config->enable_timing_logs = env_flag_or_default("OMNI_VIDEO_DEBUG_TIMING", config->enable_timing_logs);
+}
+
+int video_pipeline_stats_init(video_pipeline_stats_t *stats) {
+    int rc;
+    if (stats == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+    memset(stats, 0, sizeof(*stats));
+    rc = pthread_mutex_init(&stats->mutex, NULL);
+    if (rc != 0) {
+        errno = rc;
+        return -1;
+    }
+    return 0;
+}
+
+void video_pipeline_stats_destroy(video_pipeline_stats_t *stats) {
+    if (stats == NULL) {
+        return;
+    }
+    pthread_mutex_destroy(&stats->mutex);
+}
+
+void video_pipeline_stats_snapshot(video_pipeline_stats_t *stats, video_pipeline_stats_t *out_stats) {
+    if (stats == NULL || out_stats == NULL) {
+        return;
+    }
+    memset(out_stats, 0, sizeof(*out_stats));
+    pthread_mutex_lock(&stats->mutex);
+    out_stats->frames_sent = stats->frames_sent;
+    out_stats->bytes_sent = stats->bytes_sent;
+    out_stats->send_errors = stats->send_errors;
+    out_stats->last_frame_bytes = stats->last_frame_bytes;
+    out_stats->connected = stats->connected;
+    snprintf(out_stats->last_error, sizeof(out_stats->last_error), "%s", stats->last_error);
+    out_stats->transport = stats->transport;
+    pthread_mutex_unlock(&stats->mutex);
+}
+
+static int open_v4l2_device(const char *device) {
+    return open(device, O_RDWR | O_NONBLOCK);
+}
+
+static int init_v4l2_device(int fd, int width, int height) {
+    struct v4l2_format fmt;
+
+    memset(&fmt, 0, sizeof(fmt));
+    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    fmt.fmt.pix.width = width;
+    fmt.fmt.pix.height = height;
+    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
+    fmt.fmt.pix.field = V4L2_FIELD_NONE;
+    return ioctl(fd, VIDIOC_S_FMT, &fmt);
+}
+
+static int init_mmap(int fd, video_buffer_t **buffers, int *num_buffers) {
+    struct v4l2_requestbuffers req;
+    int i;
+
+    memset(&req, 0, sizeof(req));
+    req.count = VIDEO_NUM_BUFFERS;
+    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    req.memory = V4L2_MEMORY_MMAP;
+    if (ioctl(fd, VIDIOC_REQBUFS, &req) < 0) {
+        return -1;
+    }
+
+    *num_buffers = (int) req.count;
+    *buffers = (video_buffer_t *) calloc(req.count, sizeof(video_buffer_t));
+    if (*buffers == NULL) {
+        return -1;
+    }
+
+    for (i = 0; i < (int) req.count; i++) {
+        struct v4l2_buffer buf;
+
+        memset(&buf, 0, sizeof(buf));
+        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        buf.memory = V4L2_MEMORY_MMAP;
+        buf.index = (unsigned int) i;
+        if (ioctl(fd, VIDIOC_QUERYBUF, &buf) < 0) {
+            return -1;
+        }
+
+        (*buffers)[i].length = buf.length;
+        (*buffers)[i].start = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, buf.m.offset);
+        if ((*buffers)[i].start == MAP_FAILED) {
+            return -1;
+        }
+    }
+
+    return 0;
+}
+
+static AVCodecContext *create_mjpeg_decoder(int width, int height) {
+    const AVCodec *decoder = avcodec_find_decoder(AV_CODEC_ID_MJPEG);
+    AVCodecContext *ctx;
+    AVDictionary *opts = NULL;
+
+    if (decoder == NULL) {
+        errno = ENOENT;
+        return NULL;
+    }
+
+    ctx = avcodec_alloc_context3(decoder);
+    if (ctx == NULL) {
+        return NULL;
+    }
+    ctx->width = width;
+    ctx->height = height;
+    ctx->pix_fmt = AV_PIX_FMT_YUVJ420P;
+    ctx->color_range = AVCOL_RANGE_JPEG;
+    ctx->thread_count = 1;
+
+    av_dict_set(&opts, "flags2", "+fast", 0);
+    if (avcodec_open2(ctx, decoder, &opts) < 0) {
+        avcodec_free_context(&ctx);
+        av_dict_free(&opts);
+        errno = EINVAL;
+        return NULL;
+    }
+    av_dict_free(&opts);
+    return ctx;
+}
+
+static AVCodecContext *create_mjpeg_encoder(int width, int height) {
+    const AVCodec *encoder = avcodec_find_encoder(AV_CODEC_ID_MJPEG);
+    AVCodecContext *ctx;
+    AVDictionary *opts = NULL;
+
+    if (encoder == NULL) {
+        errno = ENOENT;
+        return NULL;
+    }
+
+    ctx = avcodec_alloc_context3(encoder);
+    if (ctx == NULL) {
+        return NULL;
+    }
+    ctx->width = width;
+    ctx->height = height;
+    ctx->pix_fmt = AV_PIX_FMT_YUVJ420P;
+    ctx->time_base = (AVRational){1, 30};
+    ctx->qmin = 8;
+    ctx->qmax = 31;
+    ctx->flags |= AV_CODEC_FLAG_QSCALE;
+    ctx->global_quality = FF_QP2LAMBDA * 5;
+
+    av_dict_set(&opts, "huffman", "default", 0);
+    if (avcodec_open2(ctx, encoder, &opts) < 0) {
+        avcodec_free_context(&ctx);
+        av_dict_free(&opts);
+        errno = EINVAL;
+        return NULL;
+    }
+    av_dict_free(&opts);
+    return ctx;
+}
+
+static int decode_mjpeg_frame(AVCodecContext *decoder, const uint8_t *data, int size, AVFrame **frame) {
+    AVPacket *pkt;
+    int ret;
+
+    if (frame == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    *frame = NULL;
+    pkt = av_packet_alloc();
+    if (pkt == NULL) {
+        return -1;
+    }
+    pkt->data = (uint8_t *) data;
+    pkt->size = size;
+
+    ret = avcodec_send_packet(decoder, pkt);
+    if (ret < 0) {
+        av_packet_free(&pkt);
+        errno = EINVAL;
+        return -1;
+    }
+
+    *frame = av_frame_alloc();
+    if (*frame == NULL) {
+        av_packet_free(&pkt);
+        return -1;
+    }
+
+    ret = avcodec_receive_frame(decoder, *frame);
+    av_packet_free(&pkt);
+    if (ret < 0) {
+        av_frame_free(frame);
+        errno = EINVAL;
+        return -1;
+    }
+    return 0;
+}
+
+static int ensure_scale_context(
+    struct SwsContext **sws_ctx,
+    int *cached_src_width,
+    int *cached_src_height,
+    int *cached_src_format,
+    const AVFrame *src,
+    int output_width,
+    int output_height
+) {
+    if (
+        *sws_ctx != NULL
+        && *cached_src_width == src->width
+        && *cached_src_height == src->height
+        && *cached_src_format == src->format
+    ) {
+        return 0;
+    }
+
+    sws_freeContext(*sws_ctx);
+    *sws_ctx = sws_getContext(
+        src->width,
+        src->height,
+        src->format,
+        output_width,
+        output_height,
+        AV_PIX_FMT_YUVJ420P,
+        SWS_BILINEAR,
+        NULL,
+        NULL,
+        NULL
+    );
+    if (*sws_ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    *cached_src_width = src->width;
+    *cached_src_height = src->height;
+    *cached_src_format = src->format;
+    return 0;
+}
+
+static int scale_frame(AVFrame *src, AVFrame **dst, struct SwsContext *sws_ctx, int output_width, int output_height) {
+    int ret;
+
+    if (sws_ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    *dst = av_frame_alloc();
+    if (*dst == NULL) {
+        return -1;
+    }
+    (*dst)->width = output_width;
+    (*dst)->height = output_height;
+    (*dst)->format = AV_PIX_FMT_YUVJ420P;
+    if (av_frame_get_buffer(*dst, 0) < 0) {
+        av_frame_free(dst);
+        errno = ENOMEM;
+        return -1;
+    }
+
+    ret = sws_scale(
+        sws_ctx,
+        (const uint8_t *const *) src->data,
+        src->linesize,
+        0,
+        src->height,
+        (*dst)->data,
+        (*dst)->linesize
+    );
+    if (ret < 0) {
+        av_frame_free(dst);
+        errno = EINVAL;
+        return -1;
+    }
+    return 0;
+}
+
+static int video_sender_ensure_buffer_capacity(video_sender_t *sender, size_t min_capacity) {
+    uint8_t *resized_buffer;
+    size_t next_capacity;
+
+    if (sender == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+    if (sender->send_buffer_cap >= min_capacity) {
+        return 0;
+    }
+
+    next_capacity = sender->send_buffer_cap == 0 ? min_capacity : sender->send_buffer_cap;
+    while (next_capacity < min_capacity) {
+        next_capacity *= 2;
+    }
+
+    resized_buffer = (uint8_t *) realloc(sender->send_buffer, next_capacity);
+    if (resized_buffer == NULL) {
+        return -1;
+    }
+
+    sender->send_buffer = resized_buffer;
+    sender->send_buffer_cap = next_capacity;
+    return 0;
+}
+
+static int encode_frame(AVCodecContext *encoder, AVFrame *frame, AVPacket **pkt) {
+    int ret;
+
+    if (pkt == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    *pkt = av_packet_alloc();
+    if (*pkt == NULL) {
+        return -1;
+    }
+    ret = avcodec_send_frame(encoder, frame);
+    if (ret < 0) {
+        av_packet_free(pkt);
+        errno = EINVAL;
+        return -1;
+    }
+
+    ret = avcodec_receive_packet(encoder, *pkt);
+    if (ret < 0) {
+        av_packet_free(pkt);
+        errno = EINVAL;
+        return -1;
+    }
+    return 0;
+}
+
+static int64_t get_realtime_ms(void) {
+    struct timespec ts;
+    clock_gettime(CLOCK_REALTIME, &ts);
+    return (int64_t) ts.tv_sec * 1000 + ts.tv_nsec / 1000000;
+}
+
+static int video_sender_init(video_sender_t *sender, const video_pipeline_config_t *config) {
+    kcp_conn_options_t options;
+
+    if (sender == NULL || config == NULL || config->server_addr == NULL || config->server_addr[0] == '\0') {
+        errno = EINVAL;
+        return -1;
+    }
+
+    memset(sender, 0, sizeof(*sender));
+    snprintf(sender->target_peer, sizeof(sender->target_peer), "%s", config->target_peer);
+    kcp_conn_options_set_video_defaults(&options);
+    sender->client = kcp_client_dial_with_options(
+        config->server_addr,
+        config->relay_via,
+        config->peer_id,
+        config->bind_ip,
+        config->bind_device,
+        &options,
+        NULL,
+        NULL,
+        NULL,
+        KCP_DEFAULT_STATS_INTERVAL_MS
+    );
+    if (sender->client == NULL) {
+        return -1;
+    }
+    return 0;
+}
+
+static int video_sender_send_packet(video_sender_t *sender, const AVPacket *encoded_pkt, uint64_t timestamp) {
+    uint8_t *payload;
+    size_t payload_len;
+    int rc;
+
+    if (sender == NULL || sender->client == NULL || encoded_pkt == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    payload_len = (size_t) encoded_pkt->size + sizeof(timestamp);
+    if (video_sender_ensure_buffer_capacity(sender, payload_len) != 0) {
+        return -1;
+    }
+    payload = sender->send_buffer;
+
+    memcpy(payload, encoded_pkt->data, (size_t) encoded_pkt->size);
+    memcpy(payload + encoded_pkt->size, &timestamp, sizeof(timestamp));
+    rc = kcp_client_send_binary(sender->client, sender->target_peer, payload, payload_len);
+    return rc;
+}
+
+static void video_sender_close(video_sender_t *sender) {
+    if (sender == NULL) {
+        return;
+    }
+    if (sender->client != NULL) {
+        kcp_client_close(sender->client);
+        kcp_client_free(sender->client);
+        sender->client = NULL;
+    }
+    free(sender->send_buffer);
+    sender->send_buffer = NULL;
+    sender->send_buffer_cap = 0;
+}
+
+static void video_pipeline_cleanup_buffers(video_buffer_t *buffers, int num_buffers) {
+    int i;
+    if (buffers == NULL) {
+        return;
+    }
+    for (i = 0; i < num_buffers; i++) {
+        if (buffers[i].start != NULL && buffers[i].start != MAP_FAILED) {
+            munmap(buffers[i].start, buffers[i].length);
+        }
+    }
+    free(buffers);
+}
+
+int video_pipeline_run(const video_pipeline_config_t *config, video_pipeline_stats_t *stats, volatile sig_atomic_t *stop_requested) {
+    video_pipeline_config_t defaults;
+    video_sender_t sender;
+    video_buffer_t *buffers = NULL;
+    AVCodecContext *decoder = NULL;
+    AVCodecContext *encoder = NULL;
+    struct SwsContext *sws_ctx = NULL;
+    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    int num_buffers = 0;
+    int fd = -1;
+    int frame_index = 0;
+    int rc = -1;
+    int sws_src_width = 0;
+    int sws_src_height = 0;
+    int sws_src_format = -1;
+
+    memset(&sender, 0, sizeof(sender));
+    if (stats == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    video_pipeline_config_init(&defaults);
+    if (config == NULL) {
+        config = &defaults;
+    }
+
+#ifdef QUIET_FFMPEG_LOGS
+    av_log_set_level(AV_LOG_ERROR);
+#endif
+
+    if (config->server_addr == NULL || config->server_addr[0] == '\0') {
+        errno = EINVAL;
+        video_pipeline_set_error(stats, "video server address is required");
+        return -1;
+    }
+
+    fd = open_v4l2_device(config->camera_device);
+    if (fd < 0) {
+        video_pipeline_set_errno_error(stats, "failed to open camera device");
+        goto cleanup;
+    }
+    if (init_v4l2_device(fd, config->capture_width, config->capture_height) < 0) {
+        video_pipeline_set_errno_error(stats, "failed to configure V4L2");
+        goto cleanup;
+    }
+    if (init_mmap(fd, &buffers, &num_buffers) < 0) {
+        video_pipeline_set_errno_error(stats, "failed to initialize V4L2 mmap");
+        goto cleanup;
+    }
+
+    decoder = create_mjpeg_decoder(config->capture_width, config->capture_height);
+    encoder = create_mjpeg_encoder(config->output_width, config->output_height);
+    if (decoder == NULL || encoder == NULL) {
+        video_pipeline_set_errno_error(stats, "failed to initialize codecs");
+        goto cleanup;
+    }
+
+    if (video_sender_init(&sender, config) < 0) {
+        video_pipeline_set_errno_error(stats, "failed to start video sender");
+        goto cleanup;
+    }
+
+    pthread_mutex_lock(&stats->mutex);
+    stats->connected = 1;
+    stats->last_error[0] = '\0';
+    pthread_mutex_unlock(&stats->mutex);
+
+    for (frame_index = 0; frame_index < num_buffers; frame_index++) {
+        struct v4l2_buffer buf;
+
+        memset(&buf, 0, sizeof(buf));
+        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        buf.memory = V4L2_MEMORY_MMAP;
+        buf.index = (unsigned int) frame_index;
+        if (ioctl(fd, VIDIOC_QBUF, &buf) < 0) {
+            video_pipeline_set_errno_error(stats, "failed to queue V4L2 buffer");
+            goto cleanup;
+        }
+    }
+
+    if (ioctl(fd, VIDIOC_STREAMON, &type) < 0) {
+        video_pipeline_set_errno_error(stats, "failed to start V4L2 streaming");
+        goto cleanup;
+    }
+    if (config->enable_timing_logs) {
+        fprintf(stderr, "\nRunning video pipeline timing benchmark...\n");
+        video_pipeline_print_timing_header();
+    }
+
+    frame_index = 0;
+    while (!video_pipeline_stop_requested(stop_requested)) {
+        fd_set fds;
+        struct timeval timeout;
+        struct v4l2_buffer buf;
+        AVFrame *decoded_frame = NULL;
+        AVFrame *scaled_frame = NULL;
+        AVPacket *encoded_pkt = NULL;
+        int select_rc;
+        double total_start_ms = 0.0;
+        double capture_start_ms = 0.0;
+        double capture_end_ms = 0.0;
+        double decode_start_ms = 0.0;
+        double decode_end_ms = 0.0;
+        double scale_start_ms = 0.0;
+        double scale_end_ms = 0.0;
+        double encode_start_ms = 0.0;
+        double encode_end_ms = 0.0;
+        double send_start_ms = 0.0;
+        double send_end_ms = 0.0;
+        int frame_number = frame_index + 1;
+
+        if (config->max_frames > 0 && frame_index >= config->max_frames) {
+            break;
+        }
+        if (config->enable_timing_logs) {
+            total_start_ms = video_pipeline_now_ms();
+        }
+
+        FD_ZERO(&fds);
+        FD_SET(fd, &fds);
+        timeout.tv_sec = 2;
+        timeout.tv_usec = 0;
+        select_rc = select(fd + 1, &fds, NULL, NULL, &timeout);
+        if (select_rc <= 0) {
+            if (select_rc == 0) {
+                errno = ETIMEDOUT;
+            }
+            video_pipeline_set_errno_error(stats, "failed waiting for camera frame");
+            goto cleanup;
+        }
+        if (config->enable_timing_logs) {
+            capture_start_ms = video_pipeline_now_ms();
+        }
+
+        memset(&buf, 0, sizeof(buf));
+        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        buf.memory = V4L2_MEMORY_MMAP;
+        if (ioctl(fd, VIDIOC_DQBUF, &buf) < 0) {
+            video_pipeline_set_errno_error(stats, "failed to dequeue V4L2 buffer");
+            goto cleanup;
+        }
+        if (config->enable_timing_logs) {
+            capture_end_ms = video_pipeline_now_ms();
+            decode_start_ms = capture_end_ms;
+        }
+
+        if (decode_mjpeg_frame(decoder, (const uint8_t *) buffers[buf.index].start, (int) buf.bytesused, &decoded_frame) != 0) {
+            if (config->enable_timing_logs) {
+                video_pipeline_print_timing_failure(frame_number, "decode");
+            }
+            (void) ioctl(fd, VIDIOC_QBUF, &buf);
+            continue;
+        }
+        if (config->enable_timing_logs) {
+            decode_end_ms = video_pipeline_now_ms();
+            scale_start_ms = decode_end_ms;
+        }
+        if (
+            ensure_scale_context(
+                &sws_ctx,
+                &sws_src_width,
+                &sws_src_height,
+                &sws_src_format,
+                decoded_frame,
+                config->output_width,
+                config->output_height
+            ) != 0
+            || scale_frame(decoded_frame, &scaled_frame, sws_ctx, config->output_width, config->output_height) != 0
+        ) {
+            if (config->enable_timing_logs) {
+                video_pipeline_print_timing_failure(frame_number, "scale");
+            }
+            av_frame_free(&decoded_frame);
+            (void) ioctl(fd, VIDIOC_QBUF, &buf);
+            continue;
+        }
+        if (config->enable_timing_logs) {
+            scale_end_ms = video_pipeline_now_ms();
+            encode_start_ms = scale_end_ms;
+        }
+        if (encode_frame(encoder, scaled_frame, &encoded_pkt) != 0) {
+            if (config->enable_timing_logs) {
+                video_pipeline_print_timing_failure(frame_number, "encode");
+            }
+            av_frame_free(&decoded_frame);
+            av_frame_free(&scaled_frame);
+            (void) ioctl(fd, VIDIOC_QBUF, &buf);
+            continue;
+        }
+        if (config->enable_timing_logs) {
+            encode_end_ms = video_pipeline_now_ms();
+            send_start_ms = encode_end_ms;
+        }
+
+        if (video_sender_send_packet(&sender, encoded_pkt, (uint64_t) get_realtime_ms()) != 0) {
+            pthread_mutex_lock(&stats->mutex);
+            stats->send_errors += 1;
+            pthread_mutex_unlock(&stats->mutex);
+            if (config->enable_timing_logs) {
+                video_pipeline_print_timing_failure(frame_number, "send");
+            }
+            video_pipeline_set_errno_error(stats, "failed to send video packet");
+            av_frame_free(&decoded_frame);
+            av_frame_free(&scaled_frame);
+            av_packet_free(&encoded_pkt);
+            (void) ioctl(fd, VIDIOC_QBUF, &buf);
+            goto cleanup;
+        }
+        if (config->enable_timing_logs) {
+            send_end_ms = video_pipeline_now_ms();
+        }
+
+        pthread_mutex_lock(&stats->mutex);
+        stats->frames_sent += 1;
+        stats->bytes_sent += (uint64_t) encoded_pkt->size;
+        stats->last_frame_bytes = (uint64_t) encoded_pkt->size;
+        kcp_client_runtime_stats_snapshot(sender.client, &stats->transport);
+        pthread_mutex_unlock(&stats->mutex);
+        if (config->enable_timing_logs) {
+            video_pipeline_print_timing_row(
+                frame_number,
+                capture_end_ms - capture_start_ms,
+                decode_end_ms - decode_start_ms,
+                scale_end_ms - scale_start_ms,
+                encode_end_ms - encode_start_ms,
+                send_end_ms - send_start_ms,
+                send_end_ms - total_start_ms,
+                encoded_pkt
+            );
+        }
+
+        av_frame_free(&decoded_frame);
+        av_frame_free(&scaled_frame);
+        av_packet_free(&encoded_pkt);
+
+        if (ioctl(fd, VIDIOC_QBUF, &buf) < 0) {
+            video_pipeline_set_errno_error(stats, "failed to requeue V4L2 buffer");
+            goto cleanup;
+        }
+        frame_index += 1;
+    }
+
+    rc = 0;
+
+cleanup:
+    pthread_mutex_lock(&stats->mutex);
+    stats->connected = 0;
+    pthread_mutex_unlock(&stats->mutex);
+    if (fd >= 0) {
+        (void) ioctl(fd, VIDIOC_STREAMOFF, &type);
+    }
+    video_sender_close(&sender);
+    if (encoder != NULL) {
+        avcodec_free_context(&encoder);
+    }
+    if (decoder != NULL) {
+        avcodec_free_context(&decoder);
+    }
+    sws_freeContext(sws_ctx);
+    video_pipeline_cleanup_buffers(buffers, num_buffers);
+    if (fd >= 0) {
+        close(fd);
+    }
+    return rc;
+}

third_party/kcp/ikcp.c

@@ -298,10 +298,9 @@ ikcpcb *ikcp_create(IUINT32 conv, void *user)
     kcp->fastlimit = IKCP_FASTACK_LIMIT;
     kcp->nocwnd = 0;
     kcp->xmit = 0;
-    kcp->fast_retrans_xmit = 0;
+    kcp->timeout_retrans_total = 0;
-    kcp->early_retrans_xmit = 0;
+    kcp->fast_retrans_total = 0;
-    kcp->lost_xmit = 0;
+    kcp->duplicate_recv_total = 0;
-    kcp->repeat_xmit = 0;
     kcp->dead_link = IKCP_DEADLINK;
     kcp->output = NULL;
     kcp->writelog = NULL;
@@ -792,7 +791,7 @@ void ikcp_parse_data(ikcpcb *kcp, IKCPSEG *newseg)
     }
     else
     {
-        kcp->repeat_xmit++;
+        kcp->duplicate_recv_total++;
         ikcp_segment_delete(kcp, newseg);
     }
 
@@ -1197,7 +1196,7 @@ void ikcp_flush(ikcpcb *kcp)
             needsend = 1;
             segment->xmit++;
             kcp->xmit++;
-            kcp->lost_xmit++;
+            kcp->timeout_retrans_total++;
             if (kcp->nodelay == 0)
             {
                 segment->rto += _imax_(segment->rto, (IUINT32)kcp->rx_rto);
@@ -1217,7 +1216,7 @@ void ikcp_flush(ikcpcb *kcp)
             {
                 needsend = 1;
                 segment->xmit++;
-                kcp->fast_retrans_xmit++;
+                kcp->fast_retrans_total++;
                 segment->fastack = 0;
                 segment->resendts = current + segment->rto;
                 change++;

third_party/kcp/ikcp.h

@@ -300,10 +300,6 @@ struct IKCPCB
     IINT32 rx_rttval, rx_srtt, rx_rto, rx_minrto;
     IUINT32 snd_wnd, rcv_wnd, rmt_wnd, cwnd, probe;
     IUINT32 current, interval, ts_flush, xmit;
-    IUINT32 fast_retrans_xmit;
-    IUINT32 early_retrans_xmit;
-    IUINT32 lost_xmit;
-    IUINT32 repeat_xmit;
     IUINT32 nrcv_buf, nsnd_buf;
     IUINT32 nrcv_que, nsnd_que;
     IUINT32 nodelay, updated;
@@ -316,6 +312,9 @@ struct IKCPCB
     IUINT32 *acklist;
     IUINT32 ackcount;
     IUINT32 ackblock;
+    IUINT64 timeout_retrans_total;
+    IUINT64 fast_retrans_total;
+    IUINT64 duplicate_recv_total;
     void *user;
     char *buffer;
     int fastresend;