commit decff19dafa0e8d5ca85f8dcf47328624af445a7 Author: Mike Mi <2873@qq.com> Date: Sun Aug 9 15:56:20 2026 +0800 Organize host and robot streaming releases diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..8f142b4 --- /dev/null +++ b/.gitignore @@ -0,0 +1,32 @@ +# Local configuration and credentials +**/*.env.local +**/.env.local +**/.venv/ + +# Build and runtime output +**/build/ +**/install/ +**/log/ +**/logs/ +**/dist/ +**/node_modules/ +**/__pycache__/ +**/*.pyc +**/bin/ +**/obj/ + +# Source release archives are extracted into this repository; keep the repo +# source-oriented and do not commit duplicate bundles. +**/*.tar.gz +**/*.zip + +# Local editor/OS files +.DS_Store +Thumbs.db +.vscode/ +.idea/ + +# Workspace-agent metadata is not part of the deployable source tree. +**/.codex +**/AGENT.md +**/CLAUDE.md diff --git a/NETWORK_LINK_STARTUP_GUIDE.md b/NETWORK_LINK_STARTUP_GUIDE.md new file mode 100644 index 0000000..ef76245 --- /dev/null +++ b/NETWORK_LINK_STARTUP_GUIDE.md @@ -0,0 +1,407 @@ +# OmniSocketGo 视频链路启动与单中转切换指南 + +本文档记录两种运行方式： + +1. 当前已经验证通过的机器人网线直连主机模式。 +2. 后续计划使用的一台公网 KCP Hub 单中转模式。 + +当前直连模式的主机地址为 `192.168.41.144`，KCP 使用 UDP `10909`。后续公网模式中的 `` 需要替换为实际中转服务器公网 IP。 + +## 1. 两种模式的区别 + +| 项目 | 网线直连 | 单公网中转 | +| --- | --- | --- | +| KCP Hub 位置 | 当前控制主机 | 公网服务器 D | +| 两端连接地址 | `192.168.41.144:10909` | `:10909` | +| 主机 `relay_via` | 空 | 空 | +| 机器人 `relay_via` | 空 | 空 | +| 主机是否启动本地 Hub | 是 | 否 | +| 公网服务器是否启动 Hub | 否 | 是 | +| 视频路径 | 机器人 → 当前主机 | 机器人 → D → 当前主机 | + +单中转模式仍然有 `机器人 ↔ D` 和 `D ↔ 当前主机` 两条 KCP 会话，但只经过一台第三方服务器。 + +## 2. 当前网线直连模式 + +### 2.1 网络拓扑 + +```text +机器人网口，例如 192.168.41.145/24 + │ + │ UDP 10909 + ▼ +当前控制主机 192.168.41.144 + │ + └── 本地 KCP Hub +``` + +机器人与当前主机必须在同一网段。先在机器人上确认： + +```bash +ping -c 3 192.168.41.144 +``` + +### 2.2 直连配置检查 + +当前控制主机配置文件： + +```text +/home/ps/Desktop/OmniSocketGo_add_camera/scripts/dev/robot-remote.env.local +``` + +关键配置应为： + +```bash +CONTROL_SIDE_OMNISOCKET_SERVER_ADDR="192.168.41.144:10909" +CONTROL_SIDE_OMNISOCKET_RELAY_VIA="" +LOCAL_HUB_LISTEN_ADDR="0.0.0.0:10909" +``` + +准备移植到机器人的项目配置文件： + +```text +OmniSocketGo_robot/scripts/dev/robot-remote.env.local +``` + +关键配置应为： + +```bash +ROBOT_SIDE_OMNISOCKET_SERVER_ADDR="192.168.41.144:10909" +ROBOT_SIDE_OMNISOCKET_RELAY_VIA="" + +ROBOT_RECEIVER_SERVER_ADDR="192.168.41.144:10909" +ROBOT_RECEIVER_RELAY_VIA="" + +OMNI_VIDEO_SERVER_ADDR="192.168.41.144:10909" +OMNI_VIDEO_RELAY_VIA="" + +OMNI_CONTROL_SERVER_ADDR="192.168.41.144:10909" +OMNI_CONTROL_RELAY_VIA="" +``` + +### 2.3 当前主机启动命令 + +以下命令分别在三个终端中运行，顺序为：本地 Hub、后端、前端。 + +#### 终端 1：启动本地 KCP Hub + +```bash +cd /home/ps/Desktop/OmniSocketGo_add_camera + +# 第一次运行或可执行文件不存在时构建 +make bin/kcpserver + +# 确认 10909/UDP 没有被另一个 Hub 占用 +ss -lunp | grep ':10909' || true + +bash scripts/dev/start-local-hub.sh +``` + +正常启动会看到类似输出： + +```text +[start-local-hub] listen=0.0.0.0:10909 relay=disabled +kcp hub listening on 0.0.0.0:10909 +``` + +如果已经有正确的 `kcpserver` 在监听 UDP 10909，不要重复启动。 + +#### 终端 2：启动 robot-command-center 后端 + +```bash +cd /home/ps/Desktop/OmniSocketGo_add_camera + +# 先确认 8001/TCP 是否已经存在后端服务 +ss -ltnp | grep ':8001' || true + +bash scripts/dev/start-backend.sh +``` + +如果出现： + +```text +address already in use +``` + +说明 8001 已有服务。先执行： + +```bash +sudo lsof -nP -iTCP:8001 -sTCP:LISTEN +``` + +如果它是已经启动成功的当前后端，直接复用，不要再启动第二份；如果确认是过期进程，再对查到的具体 PID 执行普通 `kill ` 后重新启动。 + +#### 终端 3：启动前端 + +```bash +cd /home/ps/Desktop/OmniSocketGo_add_camera +bash scripts/dev/start-frontend.sh +``` + +浏览器访问： + +```text +http://127.0.0.1:5173 +``` + +### 2.4 机器人端启动命令 + +把 `OmniSocketGo_robot` 项目复制到机器人后，在机器人终端执行： + +```bash +cd /path/to/OmniSocketGo_robot + +# 检查网络、两台相机、FFmpeg 依赖和直连配置 +./check-robot-lan.sh + +# 如果机器人原有开机服务正在占用相机，先停掉对应旧服务 +sudo systemctl stop blitz-watchdog.service blitz-b-side-omnid.service + +# 自动构建并启动机器人视频/控制 daemon +./start-robot-lan.sh +``` + +如果机器人上没有安装上述 systemd 服务，`systemctl stop` 报“unit not found”可以忽略，继续执行 `./start-robot-lan.sh`。 + +双相机正常打开时会看到： + +```text +[video_pipeline] camera head ready on ... +[video_pipeline] camera waist ready on ... +``` + +随后周期日志应满足： + +```text +video registered=1 +frames 持续增加 +``` + +### 2.5 直连模式验证 + +在当前控制主机执行： + +```bash +curl -s http://127.0.0.1:8001/api/video/status/ | python3 -m json.tool +``` + +重点检查： + +```text +connected: true +registered: true +has_recent_frame: true +server_addr: 192.168.41.144:10909 +relay_via: 空 +frames_received: 持续增加 +``` + +机器人端也可以查看运行状态： + +```bash +cd /path/to/OmniSocketGo_robot +python3 -m json.tool logs/runtime/b-side-omnid.status.json +``` + +## 3. 后续切换为单公网 KCP Hub 中转 + +### 3.1 目标拓扑 + +```text +机器人 B ── KCP/UDP ──▶ 公网服务器 D ◀── KCP/UDP ── 当前主机 A + KCP Hub +``` + +视频方向是： + +```text +机器人 B → 公网服务器 D → 当前主机 A +``` + +两端都主动连接 D，因此机器人和当前主机即使都位于 NAT 后面，也不需要在本地暴露 UDP 端口。 + +### 3.2 公网服务器准备计划 + +在公网服务器 D 上部署与两端相同版本的 OmniSocketGo。首次验证建议以前台方式启动，便于直接观察日志： + +```bash +cd /path/to/OmniSocketGo +make bin/kcpserver +mkdir -p logs + +./bin/kcpserver \ + -listen 0.0.0.0:10909 \ + -telemetry-peer peer-a-telemetry \ + -telemetry-interval 1000ms \ + -kcp-session-stats-log logs/d-kcp-stats.jsonl \ + -kcp-session-stats-interval 1000ms +``` + +公网服务器同时需要： + +1. 云安全组允许入站 UDP `10909`。 +2. 操作系统防火墙允许入站 UDP `10909`。 +3. 公网 IP 固定，或者使用解析稳定的域名。 +4. 出站 UDP 不被限制。 + +检查监听状态： + +```bash +ss -lunp | grep ':10909' +``` + +必须启动默认的 KCP Hub 模式，不能使用： + +```text +-mode=relay +``` + +`-mode=relay` 只是原始 UDP 转发器，不能代替按 Peer ID 路由消息的 KCP Hub。 + +### 3.3 当前主机计划修改 + +修改： + +```text +/home/ps/Desktop/OmniSocketGo_add_camera/scripts/dev/robot-remote.env.local +``` + +计划改为： + +```bash +CONTROL_SIDE_OMNISOCKET_SERVER_ADDR=":10909" +CONTROL_SIDE_OMNISOCKET_RELAY_VIA="" +``` + +切换后当前主机不再启动： + +```bash +bash scripts/dev/start-local-hub.sh +``` + +只启动后端和前端： + +```bash +cd /home/ps/Desktop/OmniSocketGo_add_camera +bash scripts/dev/start-backend.sh +``` + +另开终端： + +```bash +cd /home/ps/Desktop/OmniSocketGo_add_camera +bash scripts/dev/start-frontend.sh +``` + +前端仍然访问本机后端，前端代码和 `VITE_API_BASE_URL` 不需要因为 KCP 中转而修改。 + +### 3.4 机器人端计划修改 + +修改机器人项目中的： + +```text +scripts/dev/robot-remote.env.local +``` + +所有网络目标统一改成同一个公网 D，所有 `relay_via` 保持为空： + +```bash +ROBOT_SIDE_OMNISOCKET_SERVER_ADDR=":10909" +ROBOT_SIDE_OMNISOCKET_RELAY_VIA="" + +ROBOT_RECEIVER_SERVER_ADDR=":10909" +ROBOT_RECEIVER_RELAY_VIA="" + +OMNI_VIDEO_SERVER_ADDR=":10909" +OMNI_VIDEO_RELAY_VIA="" +OMNI_VIDEO_PEER_ID="peer-b-video" +OMNI_VIDEO_TARGET_PEER="peer-a-video" + +OMNI_CONTROL_SERVER_ADDR=":10909" +OMNI_CONTROL_RELAY_VIA="" +OMNI_CONTROL_PEER_ID="peer-b-ctrl" +OMNI_CONTROL_EXPECTED_SENDER="peer-a-ctrl" +``` + +Peer ID 不要修改。D 根据 `peer-b-video → peer-a-video` 路由视频，根据控制 Peer ID 路由控制指令和相机切换确认。 + +如果机器人通过 5G 访问 D，还需要让系统明确从 5G 接口访问 D： + +```bash +BLITZ_5G_ROUTE_TARGETS="" +OMNI_5G_LINK_LOG_ENABLED="1" +``` + +具体路由需要在机器人上用以下命令确认： + +```bash +ip route get +``` + +结果应显示预期的 5G 网卡和源地址。 + +### 3.5 单中转启动顺序 + +1. 在公网服务器 D 启动 `kcpserver` Hub。 +2. 在当前主机启动 `start-backend.sh`，让 `peer-a-video`、控制 ACK 和 telemetry Peer 先注册。 +3. 在机器人启动 `start-robot-lan.sh` 或 `start-b-side-omnid.sh`。 +4. 在当前主机启动前端。 +5. 验证视频帧、相机切换、控制命令和 ACK。 + +### 3.6 单中转验证 + +当前主机执行： + +```bash +curl -s http://127.0.0.1:8001/api/video/status/ | python3 -m json.tool +``` + +预期关键字段： + +```text +connected: true +registered: true +has_recent_frame: true +server_addr: :10909 +relay_via: 空 +frames_received: 持续增加 +``` + +机器人周期日志应显示： + +```text +video registered=1 +control registered=1 +frames 持续增加 +``` + +公网 D 应能看到来自机器人和当前主机的 KCP 会话。若两端都显示 `registered=false`，优先检查 UDP 10909 安全组、防火墙和机器人公网路由。 + +## 4. 与原始双服务器模式的关系 + +原项目曾使用两台第三方服务器： + +```text +机器人 B → KCP Hub D → UDP Relay C → 当前主机 A +``` + +其中主机侧配置类似： + +```bash +CONTROL_SIDE_OMNISOCKET_SERVER_ADDR=":10909" +CONTROL_SIDE_OMNISOCKET_RELAY_VIA=":10909" +``` + +机器人侧直接连接 D。`relay_via` 非空时，它会成为实际 UDP 发送目标。本文计划的一次中转不需要 C，所以主机和机器人两侧的 `relay_via` 都必须为空。 + +## 5. 回退到网线直连 + +如果公网链路尚未调通，回退不需要修改代码： + +1. 把当前主机 `CONTROL_SIDE_OMNISOCKET_SERVER_ADDR` 恢复为 `192.168.41.144:10909`。 +2. 把机器人所有 Server 地址恢复为 `192.168.41.144:10909`。 +3. 确认两端所有 `relay_via` 为空。 +4. 当前主机重新启动 `start-local-hub.sh`。 +5. 重启当前主机后端和机器人 `b_side_omnid`。 + +网络模式切换只涉及环境配置和 Hub 所在位置，不需要修改视频编码、双相机切换、robot-command-center 前端或 Peer ID。 diff --git a/README.md b/README.md new file mode 100644 index 0000000..410ff73 --- /dev/null +++ b/README.md @@ -0,0 +1,84 @@ +# OmniSocketGo_streaming + +This repository contains the extracted streaming stack for the control host +and the robot. It keeps two mutually exclusive robot camera implementations: + +| Area | Path | Camera source | Use when | +| --- | --- | --- | --- | +| Control host | `host/` | KCP hub, Django API, Vue UI | Running the operator/control side | +| Robot release A | `robot/v4l2/OmniSocketGo_robot/` | Direct `/dev/video*` (V4L2) | Testing the legacy/direct device path | +| Robot release B | `robot/ros2/OmniSocketGo_robot_ros/` | ROS 2 RGB topics + shared-memory bridge | Keeping `proc_manager`/Orbbec ownership and using the ROS 2 path | + +Do not start both robot releases at the same time on one robot. They use the +same KCP peer identities and their camera ownership requirements are +different. + +## Repository layout + +```text +host/ + OmniSocketGo_add_camera/ C/KCP transport, Python bindings, scripts + robot-command-center/ Django backend and Vue frontend + requirements.txt +robot/ + v4l2/OmniSocketGo_robot/ `/dev/video*` release extracted from 2026-08-08 bundle + ros2/OmniSocketGo_robot_ros/ ROS 2 camera release validated on the robot +release/ Original release notes and SHA256 manifests +NETWORK_LINK_STARTUP_GUIDE.md Direct-LAN and single-public-hub guidance +``` + +The original compressed bundles were intentionally extracted rather than +committed again. Machine-local `*.env.local`, build directories, caches, +frontend dependencies, and generated binaries are ignored. Copy the matching +`*.env` file to a local override on the target machine and edit addresses and +paths there. + +## Quick start + +### Control host + +Read `host/README_PACKAGE.md`, then keep these directories as siblings when +installing on Linux: + +```text +/OmniSocketGo_add_camera +/robot-command-center +``` + +The normal direct-LAN order is local KCP hub, backend, then frontend. The full +commands and the one-public-Hub variation are in +`NETWORK_LINK_STARTUP_GUIDE.md`. + +### Robot: direct V4L2 release + +```bash +cd robot/v4l2/OmniSocketGo_robot +./check-robot-lan.sh +./start-robot-lan.sh +``` + +This release opens the selected V4L2 devices directly. Stop any camera service +that owns the device before starting it. + +### Robot: ROS 2 release + +```bash +cd robot/ros2/OmniSocketGo_robot_ros +make +``` + +Follow `robot/ros2/README.md` and +`robot/ros2/OmniSocketGo_robot_ros/docs/ROS2_CAMERA_FORWARDING.md`. In the +default ROS 2 mode the Orbbec/proc_manager stack owns `/dev/video*`; the ROS 2 +bridge subscribes to RGB topics and exposes the latest frames to the existing +KCP daemon without opening V4L2. + +## Review status + +The original 2026-08-08 host and V4L2 archive hashes are retained under +`release/`. The ROS 2 release is the separately tested source tree that was +developed for ROS 2 camera ownership. The repository is source-only: C/KCP and +ROS 2 builds must be performed on their target Linux/ARM64 environments. + +See `docs/RELEASE_REVIEW.md` for the boundary checks and known machine-local +configuration values. diff --git a/docs/RELEASE_REVIEW.md b/docs/RELEASE_REVIEW.md new file mode 100644 index 0000000..1edfa92 --- /dev/null +++ b/docs/RELEASE_REVIEW.md @@ -0,0 +1,48 @@ +# Release review + +## What was checked + +1. The host archive contains the two expected sibling projects: + `OmniSocketGo_add_camera` and `robot-command-center`. +2. The robot archive is a direct V4L2 implementation. Its source, scripts, + and camera-device discovery remain under `robot/v4l2/`. +3. The ROS 2 variant is kept separate under `robot/ros2/`; it uses the ROS 2 + camera bridge and shared-memory hand-off instead of opening the same V4L2 + node in the default path. +4. The network guide describes both direct-LAN and one-public-KCP-Hub modes. + Both sides must use the same Hub address and leave `relay_via` empty for + the one-Hub topology. +5. Nested release archives, Python caches, frontend dependencies, generated + build output, and `*.env.local` machine overrides were excluded from the + repository. No credential assignment or private-key material was found in + the source scan. + +## Important configuration boundary + +The checked-in `*.env` files are templates and contain example deployment +values. The original guide also contains the LAN address and Linux paths from +the validation machine (`192.168.41.144` and `/home/ps/Desktop`). Treat those +values as examples: copy the templates to ignored `*.env.local` files and +replace the address, install root, camera device, and ROS user for the target +machine. + +## Validation boundary + +The C transport and the ROS 2 package are Linux-targeted. A complete build and +camera smoke test must run on the target Linux host/ARM64 robot. The ROS 2 +variant was previously built and exercised on the robot; this Windows checkout +only performs source/layout, archive/hash, and sensitive-file checks. + +## Runtime choice + +Use exactly one of the robot releases per robot process: + +```text +direct V4L2: robot/v4l2/OmniSocketGo_robot +ROS 2: robot/ros2/OmniSocketGo_robot_ros +``` + +The V4L2 path requires exclusive access to its `/dev/video*` node. The ROS 2 +path requires the Orbbec/proc_manager and bridge path to be running and keeps +those services as the camera owner. Starting both paths creates duplicate +capture/peer processes and is unsupported. diff --git a/host/OmniSocketGo_add_camera/.gitignore b/host/OmniSocketGo_add_camera/.gitignore new file mode 100644 index 0000000..7dab0bd --- /dev/null +++ b/host/OmniSocketGo_add_camera/.gitignore @@ -0,0 +1,38 @@ +bin/* +inbox/* +*.jsonl +*.html +peer-b-latency.* + + +*.bin +.vscode/settings.json +*.log +root@117.78.11.244 + +c/bin + +*__pycache__* + +/python/build +/python/omnisocket.egg-info + +*.so* + +/.venv + +**/build/ + +ros-control-py/install +ros-control-py/log +scripts/boot/modem_network_info.json + +logs/ + +# Machine-specific runtime configuration. +/scripts/dev/robot-remote.env.local +/scripts/boot/robot-boot.env.local + +# Standalone robot transfer bundle generated from this workspace. +/OmniSocketGo_robot/ +/OmniSocketGo_robot.tar.gz diff --git a/host/OmniSocketGo_add_camera/Makefile b/host/OmniSocketGo_add_camera/Makefile new file mode 100644 index 0000000..1fc8978 --- /dev/null +++ b/host/OmniSocketGo_add_camera/Makefile @@ -0,0 +1,112 @@ +CC ?= gcc +CFLAGS ?= -std=c11 -Wall -Wextra -O2 -pthread -D_GNU_SOURCE +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 + +COMMON_SRCS := \ + $(SRC_DIR)/omni_common.c \ + $(SRC_DIR)/protocol.c \ + $(SRC_DIR)/latencylog.c \ + $(SRC_DIR)/tx_timestamp_debug.c \ + $(SRC_DIR)/kcp_packet_debug.c \ + $(SRC_DIR)/kcp_session_stats.c \ + $(SRC_DIR)/linux_timestamping.c \ + $(SRC_DIR)/interactive.c \ + $(SRC_DIR)/transport_udp.c \ + $(SRC_DIR)/transport_kcp.c \ + $(SRC_DIR)/server_udp_relay.c \ + $(SRC_DIR)/server_udp_hub.c \ + $(SRC_DIR)/server_kcp_hub.c \ + $(SRC_DIR)/peer_udp_client.c \ + $(SRC_DIR)/peer_kcp_client.c \ + third_party/cjson/cJSON.c \ + third_party/kcp/ikcp.c + +TARGETS := \ + $(BIN_DIR)/udpserver \ + $(BIN_DIR)/udppeer \ + $(BIN_DIR)/udpping \ + $(BIN_DIR)/udprelay \ + $(BIN_DIR)/kcpserver \ + $(BIN_DIR)/kcppeer \ + $(BIN_DIR)/kcpping + +CAMERA_VIDEO_SENDER := $(BIN_DIR)/camera_video_sender +FFMPEG_PIPELINE_COMMON_SRCS := \ + $(SRC_DIR)/video_pipeline.c \ + $(SRC_DIR)/gps_buffer.c \ + $(SRC_DIR)/omni_common.c \ + $(SRC_DIR)/protocol.c \ + $(SRC_DIR)/latencylog.c \ + $(SRC_DIR)/kcp_packet_debug.c \ + $(SRC_DIR)/kcp_session_stats.c \ + $(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 + +CAMERA_VIDEO_SENDER_SRCS := \ + $(CMD_DIR)/v1_camera_pipeline_ifdef.c \ + $(FFMPEG_PIPELINE_COMMON_SRCS) + +B_SIDE_OMNID := $(BIN_DIR)/b_side_omnid +B_SIDE_OMNID_SRCS := \ + $(CMD_DIR)/b_side_omnid.c \ + $(FFMPEG_PIPELINE_COMMON_SRCS) + +all: $(TARGETS) + +$(BIN_DIR): + mkdir -p $(BIN_DIR) + +$(BIN_DIR)/udpserver: $(CMD_DIR)/udpserver.c $(COMMON_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) -o $@ $^ $(LDFLAGS) + +$(BIN_DIR)/udppeer: $(CMD_DIR)/udppeer.c $(COMMON_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) -o $@ $^ $(LDFLAGS) + +$(BIN_DIR)/udpping: $(CMD_DIR)/udpping.c $(COMMON_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) -o $@ $^ $(LDFLAGS) + +$(BIN_DIR)/udprelay: $(CMD_DIR)/udprelay.c $(COMMON_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) -o $@ $^ $(LDFLAGS) + +$(BIN_DIR)/kcpserver: $(CMD_DIR)/kcpserver.c $(COMMON_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) -o $@ $^ $(LDFLAGS) + +$(BIN_DIR)/kcppeer: $(CMD_DIR)/kcppeer.c $(COMMON_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) -o $@ $^ $(LDFLAGS) + +$(BIN_DIR)/kcpping: $(CMD_DIR)/kcpping.c $(COMMON_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) -o $@ $^ $(LDFLAGS) + +$(CAMERA_VIDEO_SENDER): $(CAMERA_VIDEO_SENDER_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) $$(pkg-config --cflags libavformat libavcodec libavutil libswscale) -o $@ $^ $(LDFLAGS) $$(pkg-config --libs libavformat libavcodec libavutil libswscale) -lm + +camera_video_sender: $(CAMERA_VIDEO_SENDER) + +$(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) -lm + +b_side_omnid: $(B_SIDE_OMNID) + +clean: + rm -rf $(BIN_DIR) + +python-ext: + cd python && $(PYTHON) setup.py build_ext --inplace + +python-install: + cd python && $(PYTHON) -m pip install -e . + +.PHONY: all clean python-ext python-install camera_video_sender b_side_omnid diff --git a/host/OmniSocketGo_add_camera/NETWORK_LINK_STARTUP_GUIDE.md b/host/OmniSocketGo_add_camera/NETWORK_LINK_STARTUP_GUIDE.md new file mode 100644 index 0000000..ef76245 --- /dev/null +++ b/host/OmniSocketGo_add_camera/NETWORK_LINK_STARTUP_GUIDE.md @@ -0,0 +1,407 @@ +# OmniSocketGo 视频链路启动与单中转切换指南 + +本文档记录两种运行方式： + +1. 当前已经验证通过的机器人网线直连主机模式。 +2. 后续计划使用的一台公网 KCP Hub 单中转模式。 + +当前直连模式的主机地址为 `192.168.41.144`，KCP 使用 UDP `10909`。后续公网模式中的 `` 需要替换为实际中转服务器公网 IP。 + +## 1. 两种模式的区别 + +| 项目 | 网线直连 | 单公网中转 | +| --- | --- | --- | +| KCP Hub 位置 | 当前控制主机 | 公网服务器 D | +| 两端连接地址 | `192.168.41.144:10909` | `:10909` | +| 主机 `relay_via` | 空 | 空 | +| 机器人 `relay_via` | 空 | 空 | +| 主机是否启动本地 Hub | 是 | 否 | +| 公网服务器是否启动 Hub | 否 | 是 | +| 视频路径 | 机器人 → 当前主机 | 机器人 → D → 当前主机 | + +单中转模式仍然有 `机器人 ↔ D` 和 `D ↔ 当前主机` 两条 KCP 会话，但只经过一台第三方服务器。 + +## 2. 当前网线直连模式 + +### 2.1 网络拓扑 + +```text +机器人网口，例如 192.168.41.145/24 + │ + │ UDP 10909 + ▼ +当前控制主机 192.168.41.144 + │ + └── 本地 KCP Hub +``` + +机器人与当前主机必须在同一网段。先在机器人上确认： + +```bash +ping -c 3 192.168.41.144 +``` + +### 2.2 直连配置检查 + +当前控制主机配置文件： + +```text +/home/ps/Desktop/OmniSocketGo_add_camera/scripts/dev/robot-remote.env.local +``` + +关键配置应为： + +```bash +CONTROL_SIDE_OMNISOCKET_SERVER_ADDR="192.168.41.144:10909" +CONTROL_SIDE_OMNISOCKET_RELAY_VIA="" +LOCAL_HUB_LISTEN_ADDR="0.0.0.0:10909" +``` + +准备移植到机器人的项目配置文件： + +```text +OmniSocketGo_robot/scripts/dev/robot-remote.env.local +``` + +关键配置应为： + +```bash +ROBOT_SIDE_OMNISOCKET_SERVER_ADDR="192.168.41.144:10909" +ROBOT_SIDE_OMNISOCKET_RELAY_VIA="" + +ROBOT_RECEIVER_SERVER_ADDR="192.168.41.144:10909" +ROBOT_RECEIVER_RELAY_VIA="" + +OMNI_VIDEO_SERVER_ADDR="192.168.41.144:10909" +OMNI_VIDEO_RELAY_VIA="" + +OMNI_CONTROL_SERVER_ADDR="192.168.41.144:10909" +OMNI_CONTROL_RELAY_VIA="" +``` + +### 2.3 当前主机启动命令 + +以下命令分别在三个终端中运行，顺序为：本地 Hub、后端、前端。 + +#### 终端 1：启动本地 KCP Hub + +```bash +cd /home/ps/Desktop/OmniSocketGo_add_camera + +# 第一次运行或可执行文件不存在时构建 +make bin/kcpserver + +# 确认 10909/UDP 没有被另一个 Hub 占用 +ss -lunp | grep ':10909' || true + +bash scripts/dev/start-local-hub.sh +``` + +正常启动会看到类似输出： + +```text +[start-local-hub] listen=0.0.0.0:10909 relay=disabled +kcp hub listening on 0.0.0.0:10909 +``` + +如果已经有正确的 `kcpserver` 在监听 UDP 10909，不要重复启动。 + +#### 终端 2：启动 robot-command-center 后端 + +```bash +cd /home/ps/Desktop/OmniSocketGo_add_camera + +# 先确认 8001/TCP 是否已经存在后端服务 +ss -ltnp | grep ':8001' || true + +bash scripts/dev/start-backend.sh +``` + +如果出现： + +```text +address already in use +``` + +说明 8001 已有服务。先执行： + +```bash +sudo lsof -nP -iTCP:8001 -sTCP:LISTEN +``` + +如果它是已经启动成功的当前后端，直接复用，不要再启动第二份；如果确认是过期进程，再对查到的具体 PID 执行普通 `kill ` 后重新启动。 + +#### 终端 3：启动前端 + +```bash +cd /home/ps/Desktop/OmniSocketGo_add_camera +bash scripts/dev/start-frontend.sh +``` + +浏览器访问： + +```text +http://127.0.0.1:5173 +``` + +### 2.4 机器人端启动命令 + +把 `OmniSocketGo_robot` 项目复制到机器人后，在机器人终端执行： + +```bash +cd /path/to/OmniSocketGo_robot + +# 检查网络、两台相机、FFmpeg 依赖和直连配置 +./check-robot-lan.sh + +# 如果机器人原有开机服务正在占用相机，先停掉对应旧服务 +sudo systemctl stop blitz-watchdog.service blitz-b-side-omnid.service + +# 自动构建并启动机器人视频/控制 daemon +./start-robot-lan.sh +``` + +如果机器人上没有安装上述 systemd 服务，`systemctl stop` 报“unit not found”可以忽略，继续执行 `./start-robot-lan.sh`。 + +双相机正常打开时会看到： + +```text +[video_pipeline] camera head ready on ... +[video_pipeline] camera waist ready on ... +``` + +随后周期日志应满足： + +```text +video registered=1 +frames 持续增加 +``` + +### 2.5 直连模式验证 + +在当前控制主机执行： + +```bash +curl -s http://127.0.0.1:8001/api/video/status/ | python3 -m json.tool +``` + +重点检查： + +```text +connected: true +registered: true +has_recent_frame: true +server_addr: 192.168.41.144:10909 +relay_via: 空 +frames_received: 持续增加 +``` + +机器人端也可以查看运行状态： + +```bash +cd /path/to/OmniSocketGo_robot +python3 -m json.tool logs/runtime/b-side-omnid.status.json +``` + +## 3. 后续切换为单公网 KCP Hub 中转 + +### 3.1 目标拓扑 + +```text +机器人 B ── KCP/UDP ──▶ 公网服务器 D ◀── KCP/UDP ── 当前主机 A + KCP Hub +``` + +视频方向是： + +```text +机器人 B → 公网服务器 D → 当前主机 A +``` + +两端都主动连接 D，因此机器人和当前主机即使都位于 NAT 后面，也不需要在本地暴露 UDP 端口。 + +### 3.2 公网服务器准备计划 + +在公网服务器 D 上部署与两端相同版本的 OmniSocketGo。首次验证建议以前台方式启动，便于直接观察日志： + +```bash +cd /path/to/OmniSocketGo +make bin/kcpserver +mkdir -p logs + +./bin/kcpserver \ + -listen 0.0.0.0:10909 \ + -telemetry-peer peer-a-telemetry \ + -telemetry-interval 1000ms \ + -kcp-session-stats-log logs/d-kcp-stats.jsonl \ + -kcp-session-stats-interval 1000ms +``` + +公网服务器同时需要： + +1. 云安全组允许入站 UDP `10909`。 +2. 操作系统防火墙允许入站 UDP `10909`。 +3. 公网 IP 固定，或者使用解析稳定的域名。 +4. 出站 UDP 不被限制。 + +检查监听状态： + +```bash +ss -lunp | grep ':10909' +``` + +必须启动默认的 KCP Hub 模式，不能使用： + +```text +-mode=relay +``` + +`-mode=relay` 只是原始 UDP 转发器，不能代替按 Peer ID 路由消息的 KCP Hub。 + +### 3.3 当前主机计划修改 + +修改： + +```text +/home/ps/Desktop/OmniSocketGo_add_camera/scripts/dev/robot-remote.env.local +``` + +计划改为： + +```bash +CONTROL_SIDE_OMNISOCKET_SERVER_ADDR=":10909" +CONTROL_SIDE_OMNISOCKET_RELAY_VIA="" +``` + +切换后当前主机不再启动： + +```bash +bash scripts/dev/start-local-hub.sh +``` + +只启动后端和前端： + +```bash +cd /home/ps/Desktop/OmniSocketGo_add_camera +bash scripts/dev/start-backend.sh +``` + +另开终端： + +```bash +cd /home/ps/Desktop/OmniSocketGo_add_camera +bash scripts/dev/start-frontend.sh +``` + +前端仍然访问本机后端，前端代码和 `VITE_API_BASE_URL` 不需要因为 KCP 中转而修改。 + +### 3.4 机器人端计划修改 + +修改机器人项目中的： + +```text +scripts/dev/robot-remote.env.local +``` + +所有网络目标统一改成同一个公网 D，所有 `relay_via` 保持为空： + +```bash +ROBOT_SIDE_OMNISOCKET_SERVER_ADDR=":10909" +ROBOT_SIDE_OMNISOCKET_RELAY_VIA="" + +ROBOT_RECEIVER_SERVER_ADDR=":10909" +ROBOT_RECEIVER_RELAY_VIA="" + +OMNI_VIDEO_SERVER_ADDR=":10909" +OMNI_VIDEO_RELAY_VIA="" +OMNI_VIDEO_PEER_ID="peer-b-video" +OMNI_VIDEO_TARGET_PEER="peer-a-video" + +OMNI_CONTROL_SERVER_ADDR=":10909" +OMNI_CONTROL_RELAY_VIA="" +OMNI_CONTROL_PEER_ID="peer-b-ctrl" +OMNI_CONTROL_EXPECTED_SENDER="peer-a-ctrl" +``` + +Peer ID 不要修改。D 根据 `peer-b-video → peer-a-video` 路由视频，根据控制 Peer ID 路由控制指令和相机切换确认。 + +如果机器人通过 5G 访问 D，还需要让系统明确从 5G 接口访问 D： + +```bash +BLITZ_5G_ROUTE_TARGETS="" +OMNI_5G_LINK_LOG_ENABLED="1" +``` + +具体路由需要在机器人上用以下命令确认： + +```bash +ip route get +``` + +结果应显示预期的 5G 网卡和源地址。 + +### 3.5 单中转启动顺序 + +1. 在公网服务器 D 启动 `kcpserver` Hub。 +2. 在当前主机启动 `start-backend.sh`，让 `peer-a-video`、控制 ACK 和 telemetry Peer 先注册。 +3. 在机器人启动 `start-robot-lan.sh` 或 `start-b-side-omnid.sh`。 +4. 在当前主机启动前端。 +5. 验证视频帧、相机切换、控制命令和 ACK。 + +### 3.6 单中转验证 + +当前主机执行： + +```bash +curl -s http://127.0.0.1:8001/api/video/status/ | python3 -m json.tool +``` + +预期关键字段： + +```text +connected: true +registered: true +has_recent_frame: true +server_addr: :10909 +relay_via: 空 +frames_received: 持续增加 +``` + +机器人周期日志应显示： + +```text +video registered=1 +control registered=1 +frames 持续增加 +``` + +公网 D 应能看到来自机器人和当前主机的 KCP 会话。若两端都显示 `registered=false`，优先检查 UDP 10909 安全组、防火墙和机器人公网路由。 + +## 4. 与原始双服务器模式的关系 + +原项目曾使用两台第三方服务器： + +```text +机器人 B → KCP Hub D → UDP Relay C → 当前主机 A +``` + +其中主机侧配置类似： + +```bash +CONTROL_SIDE_OMNISOCKET_SERVER_ADDR=":10909" +CONTROL_SIDE_OMNISOCKET_RELAY_VIA=":10909" +``` + +机器人侧直接连接 D。`relay_via` 非空时，它会成为实际 UDP 发送目标。本文计划的一次中转不需要 C，所以主机和机器人两侧的 `relay_via` 都必须为空。 + +## 5. 回退到网线直连 + +如果公网链路尚未调通，回退不需要修改代码： + +1. 把当前主机 `CONTROL_SIDE_OMNISOCKET_SERVER_ADDR` 恢复为 `192.168.41.144:10909`。 +2. 把机器人所有 Server 地址恢复为 `192.168.41.144:10909`。 +3. 确认两端所有 `relay_via` 为空。 +4. 当前主机重新启动 `start-local-hub.sh`。 +5. 重启当前主机后端和机器人 `b_side_omnid`。 + +网络模式切换只涉及环境配置和 Hub 所在位置，不需要修改视频编码、双相机切换、robot-command-center 前端或 Peer ID。 diff --git a/host/OmniSocketGo_add_camera/README.md b/host/OmniSocketGo_add_camera/README.md new file mode 100644 index 0000000..046c630 --- /dev/null +++ b/host/OmniSocketGo_add_camera/README.md @@ -0,0 +1,120 @@ +# OmniSocketC + +Linux-only C11 implementation of the UDP/KCP transport stack from `OmniSocketGo`. + +This subtree is intentionally standalone. The Go code stays in place as the behavior reference, while the C implementation builds its own binaries under `c/bin/`. + +## Build + +```bash +make -j$(nproc) +``` + +Build outputs: + +- `./bin/udpserver` +- `./bin/udppeer` +- `./bin/udpping` +- `./bin/udprelay` +- `./bin/kcpserver` +- `./bin/kcppeer` +- `./bin/kcpping` + +Python extension build: + +```bash +make python-ext +make python-install +``` + +## 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 \ + -telemetry-interval 1000ms \ + -kcp-session-stats-log logs/d-kcp-stats.jsonl \ + -kcp-session-stats-interval 1000ms +``` + +For multi-hour runs, keep `-latency-log` and `-kcp-ts-debug-log` off unless you are collecting a short repro trace. + +Relay `C` runs a raw UDP forwarder to `D`: + +```bash +./bin/kcpserver -mode=relay -listen 0.0.0.0:10909 -relay-remote 172.21.32.15:10909 +``` + +Peer `A` dials `D` through relay `C`: + +```bash +./bin/kcppeer -id peer-a -server 172.21.32.15:10909 -relay-via 106.55.173.235:10909 \ + -inbox-dir inbox/a \ + -latency-log logs/a-latency.jsonl \ + -kcp-ts-debug-log logs/a-kcp-ts.jsonl \ + -kcp-session-stats-log logs/a-kcp-stats.jsonl +``` + +Peer `B` dials `D` directly: + +```bash +./bin/kcppeer -id peer-b -server 81.70.156.140:10909 \ + -inbox-dir inbox/b \ + -latency-log logs/b-latency.jsonl \ + -kcp-ts-debug-log logs/b-kcp-ts.jsonl \ + -kcp-session-stats-log logs/b-kcp-stats.jsonl +``` + +Optional ping / echo tools: + +```bash +./bin/kcpping -id peer-a -server 106.55.173.235:10909 -echo +./bin/kcpping -id peer-b -server 81.70.156.140:10909 -to peer-a -count 20 -interval 100ms +./bin/udpserver -listen 0.0.0.0:9001 +./bin/udppeer -id peer-a -server 127.0.0.1:9001 +./bin/udpping -id pinger -server 127.0.0.1:9001 -to peer-a -count 20 +``` + +Python control/video demos use two KCP sessions: + +- `peer-a-ctrl <-> peer-b-ctrl` for small binary control packets +- `peer-b-video -> peer-a-video` for larger binary video frames + +Example demo entry points: + +- `udp_keyboard_sender.py` +- `udp_xbox_sender.py` +- `udp_fsm_controller.py` +- `omnisocket_video_sender.py` +- `omnisocket_video_receiver.py` +- `scripts/kcp_control_benchmark.py` + +Python `recv_into()` note: + +- The writable buffer must be large enough for the full incoming payload. +- If the buffer is too small, `recv_into()` reports the required size but the current frame has already been consumed and is lost. +- For the video demo, keep `video_receiver.buffer_bytes >= video_sender.frame_bytes`. + +## Interactive Commands + +`udppeer` and `kcppeer` support the same interactive shell: + +```text +help +text peer-b hello +text peer-a hi +file peer-a /tmp/test125.bin +quit +``` + +## Notes + +- The C project targets Linux only. +- It preserves the Go wire format for UDP datagrams and KCP stream frames. +- It now supports `binary` payload messages in addition to `text`, `file`, `register`, and `error`. +- Python `Session.recv_into()` is a zero-copy receive helper for already-sized buffers; it does not retain oversized frames for a retry. +- It keeps runtime JSONL logging, UDP TX timestamp debug, KCP packet debug, and KCP session stats. +- Offline `latencysummary` and HTML chart generation are intentionally not migrated. +- No automated C tests are included in this subtree; validation is expected to happen on Linux via `make` and manual smoke tests. diff --git a/host/OmniSocketGo_add_camera/cmd/b_side_omnid.c b/host/OmniSocketGo_add_camera/cmd/b_side_omnid.c new file mode 100644 index 0000000..b67faf2 --- /dev/null +++ b/host/OmniSocketGo_add_camera/cmd/b_side_omnid.c @@ -0,0 +1,1338 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cJSON.h" +#include "control_protocol.h" +#include "latencylog.h" +#include "protocol.h" +#include "video_pipeline.h" + +#define CONTROL_DEFAULT_PEER_ID "peer-b-ctrl" +#define CONTROL_DEFAULT_EXPECTED_SENDER "peer-a-ctrl" +#define CONTROL_ACK_DEFAULT_PEER_ID "peer-b-ctrl-ack" +#define CONTROL_ACK_DEFAULT_TARGET_PEER "peer-a-ctrl-ack" +#define CONTROL_DEFAULT_UNIX_SOCKET "/tmp/omnisocket-b-side-cmd.sock" +#define CONTROL_DEFAULT_SERVER_IDLE_RECONNECT_MS 3000 +#define DEFAULT_RUNTIME_DIR "/run/blitz-robot" +#define DEFAULT_STATUS_FILE_NAME "b-side-omnid.status.json" +#define DEFAULT_VIDEO_THREAD_FAULT_FILE "fault-injection-bside-video-thread-stall" +#define DEFAULT_CONTROL_THREAD_FAULT_FILE "fault-injection-bside-control-thread-stall" +#define DEFAULT_THREAD_HEARTBEAT_TIMEOUT_SEC 15 +#define DEFAULT_KCP_STATS_INTERVAL_MS 1000 +#define DEFAULT_CONTROL_LATENCY_SAMPLE_MOD 100 +#define DEFAULT_CONTROL_ACK_SAMPLE_MOD 10 +#define EXIT_CODE_VIDEO_THREAD_STALLED 101 +#define EXIT_CODE_CONTROL_THREAD_STALLED 102 + +typedef struct unix_dgram_client { + int fd; + char bind_path[108]; + char dest_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; + uint64_t reconnect_count; + uint32_t server_idle_ms; + int ever_connected; + int registered; + char last_error[256]; + char last_reconnect_reason[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; + atomic_int active_camera; + 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_ack_peer_id; + const char *control_ack_target_peer; + const char *control_unix_socket; + int control_server_idle_reconnect_ms; + const char *runtime_dir; + int heartbeat_timeout_sec; + int stats_interval_ms; + uint64_t control_latency_sample_mod; + uint64_t control_ack_sample_mod; + char status_file_path[512]; + char video_thread_fault_file[512]; + char control_thread_fault_file[512]; + atomic_long video_thread_heartbeat_epoch_sec; + atomic_long control_thread_heartbeat_epoch_sec; + atomic_int control_ack_shutdown_requested; + kcp_session_stats_logger_t *stats_logger; + latency_logger_t *control_latency_logger; + video_stage_logger_t *video_stage_logger; + unix_dgram_client_t unix_client; + control_bridge_stats_t control_stats; + pthread_mutex_t control_ack_mutex; + pthread_t control_ack_thread; + kcp_client_t *control_ack_client; + int control_ack_thread_started; + int control_ack_connect_requested; + int control_ack_connect_inflight; +} daemon_state_t; + +static void control_message_body_to_cstr(const message_t *msg, char *buffer, size_t buffer_len); + +static const char *camera_name(int camera) { + return camera == VIDEO_CAMERA_WAIST ? "waist" : "head"; +} + +static int handle_camera_select_message(daemon_state_t *state, kcp_client_t *client, const message_t *msg) { + char body[64]; + char reply[96]; + int selected; + + if (state == NULL || msg == NULL || msg->type != MSG_TYPE_TEXT) { + return 0; + } + control_message_body_to_cstr(msg, body, sizeof(body)); + if (strcmp(body, "camera:head") == 0 || strcmp(body, "camera.select=head") == 0) { + selected = VIDEO_CAMERA_HEAD; + } else if (strcmp(body, "camera:waist") == 0 || strcmp(body, "camera.select=waist") == 0) { + selected = VIDEO_CAMERA_WAIST; + } else { + return 0; + } + atomic_store(&state->active_camera, selected); + fprintf(stderr, "[b_side_omnid] active camera switched to %s\n", camera_name(selected)); + if (client != NULL && msg->from[0] != '\0') { + snprintf( + reply, + sizeof(reply), + "{\"type\":\"camera.selected\",\"camera\":\"%s\"}", + camera_name(selected) + ); + if (kcp_client_send_text(client, msg->from, reply) != 0) { + fprintf(stderr, "[b_side_omnid] failed to acknowledge camera selection: %s\n", strerror(errno)); + } + } + return 1; +} + +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 env_int_or_default(const char *name, int fallback) { + const char *value = getenv(name); + int parsed; + + if (value == NULL || value[0] == '\0') { + return fallback; + } + parsed = atoi(value); + if (parsed <= 0) { + return fallback; + } + return parsed; +} + +static uint64_t env_u64_or_default(const char *name, uint64_t fallback) { + const char *value = getenv(name); + unsigned long long parsed = 0ULL; + char *endptr = NULL; + + if (value == NULL || value[0] == '\0') { + return fallback; + } + parsed = strtoull(value, &endptr, 10); + if (endptr == value || *endptr != '\0' || parsed == 0ULL) { + return fallback; + } + return (uint64_t) parsed; +} + +static int64_t realtime_epoch_ms(void) { + struct timespec ts; + + clock_gettime(CLOCK_REALTIME, &ts); + return (int64_t) ts.tv_sec * 1000 + ts.tv_nsec / 1000000; +} + +static long realtime_epoch_sec(void) { + return (long) time(NULL); +} + +static void update_thread_heartbeat(atomic_long *heartbeat) { + if (heartbeat == NULL) { + return; + } + atomic_store(heartbeat, realtime_epoch_sec()); +} + +static int should_log_control_latency(const daemon_state_t *state, const message_t *msg) { + uint64_t sample_mod; + + if (state == NULL || state->control_latency_logger == NULL || msg == NULL) { + return 0; + } + sample_mod = state->control_latency_sample_mod; + if (sample_mod <= 1U) { + return 1; + } + return msg->id % sample_mod == 0U; +} + +static int should_send_control_ack(const daemon_state_t *state, const message_t *msg) { + uint64_t sample_mod; + + if (state == NULL || msg == NULL) { + return 0; + } + sample_mod = state->control_ack_sample_mod; + if (sample_mod <= 1U) { + return 1; + } + return msg->id % sample_mod == 0U; +} + +static void video_pipeline_heartbeat_progress(void *context) { + update_thread_heartbeat((atomic_long *) context); +} + +static int ensure_runtime_dir(const char *runtime_dir) { + struct stat st; + + if (runtime_dir == NULL || runtime_dir[0] == '\0') { + errno = EINVAL; + return -1; + } + if (stat(runtime_dir, &st) == 0) { + if (S_ISDIR(st.st_mode)) { + return 0; + } + errno = ENOTDIR; + return -1; + } + if (errno != ENOENT) { + return -1; + } + if (mkdir(runtime_dir, 0775) != 0 && errno != EEXIST) { + return -1; + } + return 0; +} + +static int path_exists(const char *path) { + return path != NULL && path[0] != '\0' && access(path, F_OK) == 0; +} + +static int consume_fault_flag(const char *path) { + if (!path_exists(path)) { + return 0; + } + unlink(path); + return 1; +} + +static void maybe_inject_thread_stall(daemon_state_t *state, const char *fault_path, const char *thread_name) { + if (state == NULL || fault_path == NULL || thread_name == NULL) { + return; + } + if (!consume_fault_flag(fault_path)) { + return; + } + fprintf( + stderr, + "[b_side_omnid] fault injection requested for %s thread, sleeping past %d second heartbeat timeout\n", + thread_name, + state->heartbeat_timeout_sec + ); + sleep((unsigned int) state->heartbeat_timeout_sec + 2U); +} + +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 unix_dgram_client_close(unix_dgram_client_t *client); +static void control_bridge_stats_snapshot(control_bridge_stats_t *stats, control_bridge_stats_t *out_stats); +static void close_control_ack_client(kcp_client_t **client_ptr); + +static int control_ack_enabled(const daemon_state_t *state) { + return state != NULL + && state->control_ack_peer_id != NULL + && state->control_ack_peer_id[0] != '\0' + && state->control_ack_target_peer != NULL + && state->control_ack_target_peer[0] != '\0'; +} + +static int control_ack_manager_init(daemon_state_t *state) { + int rc; + + if (state == NULL) { + errno = EINVAL; + return -1; + } + rc = pthread_mutex_init(&state->control_ack_mutex, NULL); + if (rc != 0) { + errno = rc; + return -1; + } + atomic_init(&state->control_ack_shutdown_requested, 0); + state->control_ack_client = NULL; + state->control_ack_thread_started = 0; + state->control_ack_connect_requested = 0; + state->control_ack_connect_inflight = 0; + return 0; +} + +static void control_ack_manager_reset(daemon_state_t *state, int request_connect) { + kcp_client_t *client = NULL; + + if (state == NULL) { + return; + } + pthread_mutex_lock(&state->control_ack_mutex); + client = state->control_ack_client; + state->control_ack_client = NULL; + state->control_ack_connect_requested = request_connect && control_ack_enabled(state) && state->control_ack_thread_started; + pthread_mutex_unlock(&state->control_ack_mutex); + close_control_ack_client(&client); +} + +static void control_ack_manager_destroy(daemon_state_t *state) { + if (state == NULL) { + return; + } + atomic_store(&state->control_ack_shutdown_requested, 1); + if (state->control_ack_thread_started) { + pthread_join(state->control_ack_thread, NULL); + state->control_ack_thread_started = 0; + } + control_ack_manager_reset(state, 0); + pthread_mutex_destroy(&state->control_ack_mutex); +} + +static int write_status_json_atomic(const char *path, cJSON *root) { + char *json; + char temp_path[640]; + FILE *file; + size_t json_len; + + if (path == NULL || root == NULL) { + errno = EINVAL; + return -1; + } + + json = cJSON_PrintUnformatted(root); + if (json == NULL) { + errno = ENOMEM; + return -1; + } + + snprintf(temp_path, sizeof(temp_path), "%s.tmp.%ld", path, (long) getpid()); + file = fopen(temp_path, "wb"); + if (file == NULL) { + cJSON_free(json); + return -1; + } + + json_len = strlen(json); + if (fwrite(json, 1, json_len, file) != json_len || fflush(file) != 0) { + int saved_errno = errno; + + fclose(file); + unlink(temp_path); + cJSON_free(json); + errno = saved_errno; + return -1; + } + if (fclose(file) != 0) { + int saved_errno = errno; + + unlink(temp_path); + cJSON_free(json); + errno = saved_errno; + return -1; + } + if (rename(temp_path, path) != 0) { + int saved_errno = errno; + + unlink(temp_path); + cJSON_free(json); + errno = saved_errno; + return -1; + } + + cJSON_free(json); + return 0; +} + +static int write_daemon_status_file(daemon_state_t *state) { + cJSON *root; + video_pipeline_stats_t video_stats; + control_bridge_stats_t control_stats; + int rc; + + if (state == NULL) { + errno = EINVAL; + return -1; + } + if (ensure_runtime_dir(state->runtime_dir) != 0) { + return -1; + } + + 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); + + root = cJSON_CreateObject(); + if (root == NULL) { + errno = ENOMEM; + return -1; + } + + cJSON_AddNumberToObject(root, "updated_at_epoch_ms", (double) realtime_epoch_ms()); + cJSON_AddNumberToObject(root, "pid", (double) getpid()); + cJSON_AddNumberToObject(root, "video_thread_heartbeat_epoch_ms", (double) atomic_load(&state->video_thread_heartbeat_epoch_sec) * 1000.0); + cJSON_AddNumberToObject(root, "control_thread_heartbeat_epoch_ms", (double) atomic_load(&state->control_thread_heartbeat_epoch_sec) * 1000.0); + cJSON_AddBoolToObject(root, "video_connected", video_stats.connected != 0); + cJSON_AddNumberToObject(root, "video_frames_sent", (double) video_stats.frames_sent); + cJSON_AddNumberToObject(root, "video_send_errors", (double) video_stats.send_errors); + cJSON_AddNumberToObject(root, "video_backlog_resets", (double) video_stats.backlog_resets); + cJSON_AddNumberToObject(root, "video_last_capture_to_send_ms", (double) video_stats.last_capture_to_send_ms); + cJSON_AddNumberToObject(root, "video_avg_capture_to_send_ms", video_stats.avg_capture_to_send_ms); + cJSON_AddStringToObject(root, "video_active_camera", camera_name(atomic_load(&state->active_camera))); + cJSON_AddStringToObject(root, "video_last_error", video_stats.last_error); + cJSON_AddBoolToObject(root, "control_registered", control_stats.registered != 0); + cJSON_AddNumberToObject(root, "control_reconnect_count", (double) control_stats.reconnect_count); + cJSON_AddNumberToObject(root, "control_unix_send_errors", (double) control_stats.unix_send_errors); + cJSON_AddStringToObject(root, "control_last_error", control_stats.last_error); + + rc = write_status_json_atomic(state->status_file_path, root); + cJSON_Delete(root); + return rc; +} + +static int thread_heartbeat_expired(atomic_long *heartbeat, int timeout_sec, long now_sec) { + long heartbeat_sec; + + if (heartbeat == NULL || timeout_sec <= 0) { + return 0; + } + heartbeat_sec = atomic_load(heartbeat); + if (heartbeat_sec <= 0) { + return 0; + } + return now_sec - heartbeat_sec > timeout_sec; +} + +static void exit_if_thread_stalled(daemon_state_t *state) { + long now_sec; + + if (state == NULL || state->heartbeat_timeout_sec <= 0) { + return; + } + now_sec = realtime_epoch_sec(); + if (thread_heartbeat_expired(&state->video_thread_heartbeat_epoch_sec, state->heartbeat_timeout_sec, now_sec)) { + fprintf(stderr, "[b_side_omnid] video thread heartbeat stalled for more than %d seconds\n", state->heartbeat_timeout_sec); + fflush(stderr); + exit(EXIT_CODE_VIDEO_THREAD_STALLED); + } + if (thread_heartbeat_expired(&state->control_thread_heartbeat_epoch_sec, state->heartbeat_timeout_sec, now_sec)) { + fprintf(stderr, "[b_side_omnid] control thread heartbeat stalled for more than %d seconds\n", state->heartbeat_timeout_sec); + fflush(stderr); + exit(EXIT_CODE_CONTROL_THREAD_STALLED); + } +} + +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_reconnect_reason(control_bridge_stats_t *stats, const char *message) { + if (stats == NULL) { + return; + } + pthread_mutex_lock(&stats->mutex); + snprintf(stats->last_reconnect_reason, sizeof(stats->last_reconnect_reason), "%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->reconnect_count = stats->reconnect_count; + out_stats->server_idle_ms = stats->server_idle_ms; + out_stats->registered = stats->registered; + snprintf(out_stats->last_error, sizeof(out_stats->last_error), "%s", stats->last_error); + snprintf(out_stats->last_reconnect_reason, sizeof(out_stats->last_reconnect_reason), "%s", stats->last_reconnect_reason); + out_stats->transport = stats->transport; + pthread_mutex_unlock(&stats->mutex); +} + +static int control_server_error_requires_reconnect(const char *message) { + if (message == NULL || message[0] == '\0') { + return 0; + } + return strstr(message, "not registered") != NULL + || strstr(message, "first message must be register") != NULL + || strstr(message, "peer replaced") != NULL + || strstr(message, "timed out waiting for server_register_ok") != NULL; +} + +static void control_message_body_to_cstr(const message_t *msg, char *buffer, size_t buffer_len) { + size_t copy_len; + + if (buffer == NULL || buffer_len == 0) { + return; + } + buffer[0] = '\0'; + if (msg == NULL || msg->body == NULL || msg->body_len == 0) { + return; + } + copy_len = msg->body_len < (buffer_len - 1U) ? msg->body_len : (buffer_len - 1U); + memcpy(buffer, msg->body, copy_len); + buffer[copy_len] = '\0'; +} + +static kcp_client_t *connect_control_ack_client(const daemon_state_t *state) { + kcp_conn_options_t options; + + if (state == NULL || state->control_ack_peer_id == NULL || state->control_ack_peer_id[0] == '\0') { + errno = EINVAL; + return NULL; + } + kcp_conn_options_set_control_defaults(&options); + return kcp_client_dial_with_options( + state->control_server_addr, + state->control_relay_via, + state->control_ack_peer_id, + state->control_bind_ip, + state->control_bind_device, + &options, + NULL, + NULL, + state->stats_logger, + state->stats_interval_ms + ); +} + +static void close_control_ack_client(kcp_client_t **client_ptr) { + if (client_ptr == NULL || *client_ptr == NULL) { + return; + } + kcp_client_close(*client_ptr); + kcp_client_free(*client_ptr); + *client_ptr = NULL; +} + +static void control_ack_manager_request_connect(daemon_state_t *state) { + if (state == NULL || !control_ack_enabled(state) || !state->control_ack_thread_started) { + return; + } + pthread_mutex_lock(&state->control_ack_mutex); + if (state->control_ack_client == NULL) { + state->control_ack_connect_requested = 1; + } + pthread_mutex_unlock(&state->control_ack_mutex); +} + +static void *control_ack_thread_main(void *arg) { + daemon_state_t *state = (daemon_state_t *) arg; + + while (!atomic_load(&state->control_ack_shutdown_requested) && !*state->stop_requested) { + kcp_client_t *client = NULL; + int connect_failed = 0; + int should_connect = 0; + + pthread_mutex_lock(&state->control_ack_mutex); + if (state->control_ack_connect_requested && state->control_ack_client == NULL && !state->control_ack_connect_inflight) { + state->control_ack_connect_inflight = 1; + should_connect = 1; + } + pthread_mutex_unlock(&state->control_ack_mutex); + + if (!should_connect) { + usleep(200000); + continue; + } + + client = connect_control_ack_client(state); + connect_failed = client == NULL; + + pthread_mutex_lock(&state->control_ack_mutex); + state->control_ack_connect_inflight = 0; + if ( + client != NULL + && state->control_ack_connect_requested + && state->control_ack_client == NULL + && !atomic_load(&state->control_ack_shutdown_requested) + && !*state->stop_requested + ) { + state->control_ack_client = client; + state->control_ack_connect_requested = 0; + client = NULL; + } + pthread_mutex_unlock(&state->control_ack_mutex); + + if (client != NULL) { + close_control_ack_client(&client); + } + if (connect_failed && !atomic_load(&state->control_ack_shutdown_requested) && !*state->stop_requested) { + sleep(1); + } + } + return NULL; +} + +static void maybe_send_control_ack( + daemon_state_t *state, + const message_t *msg, + int64_t recv_unix_nano, + int64_t persist_end_unix_nano, + const char *sample_reason +) { + kcp_client_t *ack_client = NULL; + kcp_client_t *client_to_close = NULL; + char *payload = NULL; + int send_rc = -1; + + if ( + state == NULL || msg == NULL || recv_unix_nano <= 0 || persist_end_unix_nano <= recv_unix_nano + || !control_ack_enabled(state) || !state->control_ack_thread_started + ) { + return; + } + + payload = omni_strdup_printf( + "{\"message_id\":%" PRIu64 ",\"ack_phase\":\"persist_end\",\"b_recv_to_persist_us\":%" PRId64 ",\"unix_send_ok\":true,\"sample_reason\":\"%s\"}", + msg->id, + (persist_end_unix_nano - recv_unix_nano) / 1000, + sample_reason == NULL ? "sample_mod" : sample_reason + ); + if (payload == NULL) { + return; + } + + pthread_mutex_lock(&state->control_ack_mutex); + ack_client = state->control_ack_client; + if (ack_client == NULL) { + state->control_ack_connect_requested = 1; + pthread_mutex_unlock(&state->control_ack_mutex); + free(payload); + return; + } + send_rc = kcp_client_send_text(ack_client, state->control_ack_target_peer, payload); + if (send_rc != 0) { + client_to_close = state->control_ack_client; + state->control_ack_client = NULL; + state->control_ack_connect_requested = 1; + } + pthread_mutex_unlock(&state->control_ack_mutex); + + free(payload); + if (client_to_close != NULL) { + close_control_ack_client(&client_to_close); + } +} + +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_path, sizeof(client->dest_path), "%s", dest_path); + 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 int unix_dgram_client_reopen(unix_dgram_client_t *client) { + char dest_path[sizeof(client->dest_path)]; + + if (client == NULL || client->dest_path[0] == '\0') { + errno = EINVAL; + return -1; + } + snprintf(dest_path, sizeof(dest_path), "%s", client->dest_path); + unix_dgram_client_close(client); + return unix_dgram_client_init(client, dest_path); +} + +static int unix_dgram_client_should_reopen(int error_code) { + return error_code == ENOENT || error_code == ECONNREFUSED || error_code == EBADF || error_code == ENOTCONN; +} + +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) { + update_thread_heartbeat(&state->video_thread_heartbeat_epoch_sec); + maybe_inject_thread_stall(state, state->video_thread_fault_file, "video"); + int video_rc = video_pipeline_run(&state->video_config, &state->video_stats, state->stop_requested); + update_thread_heartbeat(&state->video_thread_heartbeat_epoch_sec); + + if (video_rc == 0) { + break; + } + if (video_rc == VIDEO_PIPELINE_RUN_RETRY_IMMEDIATE) { + continue; + } + 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; + int reconnect_immediately = 0; + + update_thread_heartbeat(&state->control_thread_heartbeat_epoch_sec); + maybe_inject_thread_stall(state, state->control_thread_fault_file, "control"); + 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, + state->stats_logger, + state->stats_interval_ms + ); + if (client == NULL) { + control_bridge_set_errno_error(&state->control_stats, "failed to connect control session"); + sleep(1); + continue; + } + + { + kcp_client_state_t client_state; + + memset(&client_state, 0, sizeof(client_state)); + kcp_client_state_snapshot(client, &client_state); + pthread_mutex_lock(&state->control_stats.mutex); + if (state->control_stats.ever_connected) { + state->control_stats.reconnect_count += 1; + } else { + state->control_stats.ever_connected = 1; + } + state->control_stats.registered = client_state.registered; + state->control_stats.server_idle_ms = client_state.server_idle_ms; + state->control_stats.last_reconnect_reason[0] = '\0'; + snprintf(state->control_stats.last_error, sizeof(state->control_stats.last_error), "%s", client_state.last_server_error); + kcp_client_runtime_stats_snapshot(client, &state->control_stats.transport); + pthread_mutex_unlock(&state->control_stats.mutex); + } + control_ack_manager_request_connect(state); + + while (!*state->stop_requested) { + message_t msg; + int rc; + kcp_client_state_t client_state; + int ack_sampled = 0; + int log_control_latency = 0; + int64_t recv_unix_nano = 0; + int64_t persist_begin_unix_nano = 0; + int64_t persist_end_unix_nano = 0; + + update_thread_heartbeat(&state->control_thread_heartbeat_epoch_sec); + protocol_message_init(&msg); + rc = kcp_client_receive_timed(client, &msg, 100); + update_thread_heartbeat(&state->control_thread_heartbeat_epoch_sec); + if (rc == 1) { + char reconnect_reason[256]; + + protocol_message_clear(&msg); + memset(&client_state, 0, sizeof(client_state)); + kcp_client_state_snapshot(client, &client_state); + pthread_mutex_lock(&state->control_stats.mutex); + state->control_stats.registered = client_state.registered; + state->control_stats.server_idle_ms = client_state.server_idle_ms; + snprintf(state->control_stats.last_error, sizeof(state->control_stats.last_error), "%s", client_state.last_server_error); + kcp_client_runtime_stats_snapshot(client, &state->control_stats.transport); + pthread_mutex_unlock(&state->control_stats.mutex); + if (!client_state.registered) { + snprintf(reconnect_reason, sizeof(reconnect_reason), "control session stale: server reported unregistered"); + } else if ( + state->control_server_idle_reconnect_ms > 0 + && client_state.server_idle_ms >= (uint32_t) state->control_server_idle_reconnect_ms + ) { + snprintf( + reconnect_reason, + sizeof(reconnect_reason), + "control session stale: server idle timeout (%u ms >= %d ms)", + client_state.server_idle_ms, + state->control_server_idle_reconnect_ms + ); + } else if (control_server_error_requires_reconnect(client_state.last_server_error)) { + snprintf( + reconnect_reason, + sizeof(reconnect_reason), + "control session stale: server error %.180s", + client_state.last_server_error + ); + } else { + reconnect_reason[0] = '\0'; + } + if (reconnect_reason[0] != '\0') { + control_bridge_set_error(&state->control_stats, reconnect_reason); + control_bridge_set_reconnect_reason(&state->control_stats, reconnect_reason); + fprintf(stderr, "[b_side_omnid] %s\n", reconnect_reason); + reconnect_immediately = 1; + break; + } + continue; + } + if (rc != 0) { + memset(&client_state, 0, sizeof(client_state)); + kcp_client_state_snapshot(client, &client_state); + pthread_mutex_lock(&state->control_stats.mutex); + state->control_stats.registered = client_state.registered; + state->control_stats.server_idle_ms = client_state.server_idle_ms; + kcp_client_runtime_stats_snapshot(client, &state->control_stats.transport); + pthread_mutex_unlock(&state->control_stats.mutex); + if (client_state.last_server_error[0] != '\0') { + control_bridge_set_error(&state->control_stats, client_state.last_server_error); + if (control_server_error_requires_reconnect(client_state.last_server_error)) { + control_bridge_set_reconnect_reason(&state->control_stats, client_state.last_server_error); + reconnect_immediately = 1; + } + } else { + control_bridge_set_errno_error(&state->control_stats, "control receive loop stopped"); + } + protocol_message_clear(&msg); + break; + } + + if (msg.type == MSG_TYPE_ERROR && strcmp(msg.from, SERVER_PEER_ID) == 0) { + char server_error[256]; + + control_message_body_to_cstr(&msg, server_error, sizeof(server_error)); + control_bridge_set_error(&state->control_stats, server_error); + if (control_server_error_requires_reconnect(server_error)) { + char reconnect_reason[256]; + + snprintf(reconnect_reason, sizeof(reconnect_reason), "control session stale: server error %.180s", server_error); + control_bridge_set_reconnect_reason(&state->control_stats, reconnect_reason); + fprintf(stderr, "[b_side_omnid] %s\n", reconnect_reason); + reconnect_immediately = 1; + protocol_message_clear(&msg); + break; + } + protocol_message_clear(&msg); + continue; + } + 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 (handle_camera_select_message(state, client, &msg)) { + pthread_mutex_lock(&state->control_stats.mutex); + state->control_stats.packets_forwarded += 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; + } + + ack_sampled = should_send_control_ack(state, &msg); + log_control_latency = ack_sampled || should_log_control_latency(state, &msg); + if (log_control_latency) { + recv_unix_nano = omni_now_unix_nano(); + persist_begin_unix_nano = recv_unix_nano; + latencylog_log_message_event_at( + state->control_latency_logger, + OMNI_NODE_ROLE_PEER, + state->control_peer_id, + EVENT_B_APP_RECV, + recv_unix_nano, + &msg + ); + latencylog_log_message_event_at( + state->control_latency_logger, + OMNI_NODE_ROLE_PEER, + state->control_peer_id, + EVENT_B_PERSIST_BEGIN, + persist_begin_unix_nano, + &msg + ); + } + + if (unix_dgram_client_send(&state->unix_client, msg.body, msg.body_len) != 0) { + int send_errno = errno; + int recovered = 0; + + if (unix_dgram_client_should_reopen(send_errno) && unix_dgram_client_reopen(&state->unix_client) == 0) { + recovered = unix_dgram_client_send(&state->unix_client, msg.body, msg.body_len) == 0; + } + if (recovered) { + memset(&client_state, 0, sizeof(client_state)); + kcp_client_state_snapshot(client, &client_state); + pthread_mutex_lock(&state->control_stats.mutex); + state->control_stats.packets_forwarded += 1; + state->control_stats.registered = client_state.registered; + state->control_stats.server_idle_ms = client_state.server_idle_ms; + kcp_client_runtime_stats_snapshot(client, &state->control_stats.transport); + pthread_mutex_unlock(&state->control_stats.mutex); + if (log_control_latency) { + persist_end_unix_nano = omni_now_unix_nano(); + latencylog_log_message_event_at( + state->control_latency_logger, + OMNI_NODE_ROLE_PEER, + state->control_peer_id, + EVENT_B_PERSIST_END, + persist_end_unix_nano, + &msg + ); + } + if (ack_sampled) { + maybe_send_control_ack(state, &msg, recv_unix_nano, persist_end_unix_nano, "sample_mod"); + } + protocol_message_clear(&msg); + continue; + } + errno = send_errno; + 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; + } + + memset(&client_state, 0, sizeof(client_state)); + kcp_client_state_snapshot(client, &client_state); + pthread_mutex_lock(&state->control_stats.mutex); + state->control_stats.packets_forwarded += 1; + state->control_stats.registered = client_state.registered; + state->control_stats.server_idle_ms = client_state.server_idle_ms; + kcp_client_runtime_stats_snapshot(client, &state->control_stats.transport); + pthread_mutex_unlock(&state->control_stats.mutex); + if (log_control_latency) { + persist_end_unix_nano = omni_now_unix_nano(); + latencylog_log_message_event_at( + state->control_latency_logger, + OMNI_NODE_ROLE_PEER, + state->control_peer_id, + EVENT_B_PERSIST_END, + persist_end_unix_nano, + &msg + ); + } + if (ack_sampled) { + maybe_send_control_ack(state, &msg, recv_unix_nano, persist_end_unix_nano, "sample_mod"); + } + protocol_message_clear(&msg); + } + + pthread_mutex_lock(&state->control_stats.mutex); + state->control_stats.registered = 0; + state->control_stats.server_idle_ms = 0; + pthread_mutex_unlock(&state->control_stats.mutex); + control_ack_manager_reset(state, 0); + kcp_client_close(client); + kcp_client_free(client); + if (!*state->stop_requested && !reconnect_immediately) { + 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 registered=%d frames=%llu bytes=%llu drops=%llu resets=%llu backlog=%u cap2send=%ums avg=%.1fms reason=%s srtt=%dms | control registered=%d idle=%ums reconnects=%llu forwarded=%llu invalid=%llu unix_err=%llu srtt=%dms last_reconnect=%s\n", + video_stats.connected, + (unsigned long long) video_stats.frames_sent, + (unsigned long long) video_stats.bytes_sent, + (unsigned long long) video_stats.backpressure_drops, + (unsigned long long) video_stats.backlog_resets, + video_stats.last_backlog_segments, + video_stats.last_capture_to_send_ms, + video_stats.avg_capture_to_send_ms, + video_stats.last_backlog_reason[0] == '\0' ? "-" : video_stats.last_backlog_reason, + video_stats.transport.srtt_ms, + control_stats.registered, + control_stats.server_idle_ms, + (unsigned long long) control_stats.reconnect_count, + (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, + control_stats.last_reconnect_reason[0] == '\0' ? "-" : control_stats.last_reconnect_reason + ); +} + +int main(void) { + daemon_state_t state; + pthread_t video_thread; + pthread_t control_thread; + long initial_heartbeat; + + 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); + atomic_init( + &state.active_camera, + strcmp(env_or_default("OMNI_CAMERA_ACTIVE", "head"), "waist") == 0 + ? VIDEO_CAMERA_WAIST + : VIDEO_CAMERA_HEAD + ); + state.video_config.active_camera = &state.active_camera; + 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_ack_peer_id = env_or_default("OMNI_CONTROL_ACK_PEER_ID", CONTROL_ACK_DEFAULT_PEER_ID); + state.control_ack_target_peer = env_or_default("OMNI_CONTROL_ACK_TARGET_PEER", CONTROL_ACK_DEFAULT_TARGET_PEER); + state.control_unix_socket = env_or_default("OMNI_CONTROL_UNIX_SOCKET_PATH", CONTROL_DEFAULT_UNIX_SOCKET); + state.runtime_dir = env_or_default("BLITZ_RUNTIME_DIR", DEFAULT_RUNTIME_DIR); + state.heartbeat_timeout_sec = env_int_or_default( + "BLITZ_OMNID_THREAD_HEARTBEAT_TIMEOUT_SEC", + DEFAULT_THREAD_HEARTBEAT_TIMEOUT_SEC + ); + state.stats_interval_ms = env_int_or_default("BLITZ_KCP_STATS_INTERVAL_MS", DEFAULT_KCP_STATS_INTERVAL_MS); + state.control_latency_sample_mod = env_u64_or_default("BLITZ_CONTROL_LATENCY_LOG_SAMPLE_MOD", DEFAULT_CONTROL_LATENCY_SAMPLE_MOD); + state.control_ack_sample_mod = env_u64_or_default("BLITZ_CONTROL_ACK_SAMPLE_MOD", DEFAULT_CONTROL_ACK_SAMPLE_MOD); + state.video_config.progress_callback = video_pipeline_heartbeat_progress; + state.video_config.progress_context = &state.video_thread_heartbeat_epoch_sec; + state.video_config.stats_logger = NULL; + state.video_config.stage_logger = NULL; + state.video_config.stats_interval_ms = state.stats_interval_ms; + state.control_server_idle_reconnect_ms = env_int_or_default( + "OMNI_CONTROL_SERVER_IDLE_RECONNECT_MS", + CONTROL_DEFAULT_SERVER_IDLE_RECONNECT_MS + ); + snprintf(state.status_file_path, sizeof(state.status_file_path), "%s/%s", state.runtime_dir, DEFAULT_STATUS_FILE_NAME); + snprintf( + state.video_thread_fault_file, + sizeof(state.video_thread_fault_file), + "%s/%s", + state.runtime_dir, + DEFAULT_VIDEO_THREAD_FAULT_FILE + ); + snprintf( + state.control_thread_fault_file, + sizeof(state.control_thread_fault_file), + "%s/%s", + state.runtime_dir, + DEFAULT_CONTROL_THREAD_FAULT_FILE + ); + initial_heartbeat = realtime_epoch_sec(); + atomic_init(&state.video_thread_heartbeat_epoch_sec, initial_heartbeat); + atomic_init(&state.control_thread_heartbeat_epoch_sec, initial_heartbeat); + + 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 (control_ack_manager_init(&state) != 0) { + perror("control_ack_manager_init"); + control_bridge_stats_destroy(&state.control_stats); + 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_ack_manager_destroy(&state); + 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_ack_manager_destroy(&state); + control_bridge_stats_destroy(&state.control_stats); + video_pipeline_stats_destroy(&state.video_stats); + return 1; + } + + { + const char *stats_log_path = getenv("BLITZ_KCP_STATS_LOG_PATH"); + const char *latency_log_path = getenv("BLITZ_CONTROL_LATENCY_LOG_PATH"); + const char *video_stage_log_path = getenv("BLITZ_VIDEO_STAGE_LOG_PATH"); + int latency_enabled = env_int_or_default("BLITZ_CONTROL_LATENCY_LOG_ENABLED", 1); + int video_stage_log_enabled = env_int_or_default("BLITZ_VIDEO_STAGE_LOG_ENABLED", 1); + uint64_t video_stage_log_sample_mod = env_u64_or_default("BLITZ_VIDEO_STAGE_LOG_SAMPLE_MOD", 10); + + if (stats_log_path != NULL && stats_log_path[0] != '\0') { + state.stats_logger = kcp_session_stats_open_jsonl(stats_log_path); + if (state.stats_logger == NULL) { + fprintf(stderr, "[b_side_omnid] warning: failed to open KCP stats log %s\n", stats_log_path); + } + } + if (latency_enabled && latency_log_path != NULL && latency_log_path[0] != '\0') { + state.control_latency_logger = latencylog_open_jsonl(latency_log_path); + if (state.control_latency_logger == NULL) { + fprintf(stderr, "[b_side_omnid] warning: failed to open control latency log %s\n", latency_log_path); + } + } + if (video_stage_log_enabled && video_stage_log_path != NULL && video_stage_log_path[0] != '\0') { + state.video_stage_logger = video_stage_logger_open_jsonl(video_stage_log_path, video_stage_log_sample_mod); + if (state.video_stage_logger == NULL) { + fprintf(stderr, "[b_side_omnid] warning: failed to open video stage log %s\n", video_stage_log_path); + } + } + state.video_config.stats_logger = state.stats_logger; + state.video_config.stage_logger = state.video_stage_logger; + state.video_config.stats_interval_ms = state.stats_interval_ms; + } + + if (control_ack_enabled(&state)) { + if (pthread_create(&state.control_ack_thread, NULL, control_ack_thread_main, &state) != 0) { + fprintf(stderr, "[b_side_omnid] warning: failed to start async control ACK manager, ACK sampling disabled\n"); + } else { + state.control_ack_thread_started = 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_ack_manager_destroy(&state); + control_bridge_stats_destroy(&state.control_stats); + video_pipeline_stats_destroy(&state.video_stats); + latencylog_close(state.control_latency_logger); + video_stage_logger_close(state.video_stage_logger); + kcp_session_stats_close(state.stats_logger); + 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_ack_manager_destroy(&state); + control_bridge_stats_destroy(&state.control_stats); + video_pipeline_stats_destroy(&state.video_stats); + latencylog_close(state.control_latency_logger); + video_stage_logger_close(state.video_stage_logger); + kcp_session_stats_close(state.stats_logger); + return 1; + } + + while (!g_stop_requested) { + sleep(1); + print_stats(&state); + if (write_daemon_status_file(&state) != 0) { + fprintf(stderr, "[b_side_omnid] failed to write status file %s: %s\n", state.status_file_path, strerror(errno)); + } + exit_if_thread_stalled(&state); + } + + pthread_join(video_thread, NULL); + pthread_join(control_thread, NULL); + unix_dgram_client_close(&state.unix_client); + control_ack_manager_destroy(&state); + control_bridge_stats_destroy(&state.control_stats); + video_pipeline_stats_destroy(&state.video_stats); + latencylog_close(state.control_latency_logger); + video_stage_logger_close(state.video_stage_logger); + kcp_session_stats_close(state.stats_logger); + return 0; +} diff --git a/host/OmniSocketGo_add_camera/cmd/kcppeer.c b/host/OmniSocketGo_add_camera/cmd/kcppeer.c new file mode 100644 index 0000000..e71981c --- /dev/null +++ b/host/OmniSocketGo_add_camera/cmd/kcppeer.c @@ -0,0 +1,352 @@ +#include "cli_parse.h" +#include "interactive.h" +#include "peer_kcp_client.h" + +#include +#include + +typedef struct kcppeer_receive_ctx { + kcp_client_t *client; + const char *inbox_dir; + volatile int stop_requested; + int rc; +} kcppeer_receive_ctx_t; + +static void kcppeer_usage(FILE *out) { + fprintf(out, "usage: kcppeer [-id peer-a] [-server 127.0.0.1:9002] [-relay-via addr]\n"); + fprintf(out, " [-to peer] [-text msg | -file path] [-bind-ip ip] [-bind-device dev]\n"); + fprintf(out, " [-inbox-dir dir] [-latency-log path] [-kcp-ts-debug-log path]\n"); + fprintf(out, " [-kcp-session-stats-log path] [-kcp-session-stats-interval 100ms]\n"); + fprintf(out, " [-interactive[=true|false]]\n"); +} + +static void *kcppeer_receive_thread_main(void *arg) { + kcppeer_receive_ctx_t *ctx = (kcppeer_receive_ctx_t *) arg; + + for (;;) { + message_t msg; + char persisted_path[512]; + + protocol_message_init(&msg); + if (kcp_client_receive(ctx->client, &msg) != 0) { + protocol_message_clear(&msg); + ctx->rc = ctx->stop_requested ? 0 : -1; + return NULL; + } + + switch (msg.type) { + case MSG_TYPE_TEXT: + if (kcp_client_persist_message(ctx->client, &msg, ctx->inbox_dir, persisted_path, sizeof(persisted_path)) != 0) { + fprintf(stderr, "kcppeer: persist text from %s to %s failed\n", msg.from, msg.to); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + fprintf(stderr, "received text from %s to %s and persisted to %s\n", msg.from, msg.to, persisted_path); + break; + case MSG_TYPE_FILE: + if (kcp_client_persist_message(ctx->client, &msg, ctx->inbox_dir, persisted_path, sizeof(persisted_path)) != 0) { + fprintf(stderr, "kcppeer: persist file from %s to %s failed\n", msg.from, msg.to); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + fprintf(stderr, "received file from %s to %s: %s (%lu bytes) -> %s\n", msg.from, msg.to, msg.file_name, (unsigned long) msg.body_len, persisted_path); + break; + case MSG_TYPE_BINARY: + if (kcp_client_persist_message(ctx->client, &msg, ctx->inbox_dir, persisted_path, sizeof(persisted_path)) != 0) { + fprintf(stderr, "kcppeer: persist binary payload from %s to %s failed\n", msg.from, msg.to); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + fprintf(stderr, "received binary payload from %s to %s (%lu bytes) -> %s\n", msg.from, msg.to, (unsigned long) msg.body_len, persisted_path); + break; + case MSG_TYPE_ERROR: + fprintf(stderr, "received error from %s to %s: %.*s\n", msg.from, msg.to, (int) msg.body_len, msg.body == NULL ? "" : (const char *) msg.body); + break; + default: + fprintf(stderr, "received unexpected message type %s from %s\n", protocol_message_type_name(msg.type), msg.from); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + protocol_message_clear(&msg); + } +} + +int main(int argc, char **argv) { + const char *peer_id = "peer-a"; + const char *server_addr = "127.0.0.1:9002"; + const char *relay_via = ""; + const char *actual_dial_target; + const char *target_peer = ""; + const char *text = ""; + const char *file_path = ""; + const char *bind_ip = ""; + const char *bind_device = ""; + const char *inbox_dir = "inbox"; + const char *latency_log_path = ""; + const char *packet_log_path = ""; + const char *stats_log_path = ""; + const char *stats_interval_raw = ""; + int stats_interval_ms = KCP_DEFAULT_STATS_INTERVAL_MS; + int interactive = 1; + latency_logger_t *latency_logger = NULL; + kcp_packet_debug_logger_t *packet_logger = NULL; + kcp_session_stats_logger_t *stats_logger = NULL; + kcp_client_t *client = NULL; + kcppeer_receive_ctx_t receive_ctx; + pthread_t receive_thread; + int receive_thread_started = 0; + int i; + int rc = 1; + + memset(&receive_ctx, 0, sizeof(receive_ctx)); + + for (i = 1; i < argc; ++i) { + const char *value = NULL; + int handled; + + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-id", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -id requires a value\n"); + return 1; + } else if (handled) { + peer_id = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-server", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -server requires a value\n"); + return 1; + } else if (handled) { + server_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-relay-via", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -relay-via requires a value\n"); + return 1; + } else if (handled) { + relay_via = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-to", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -to requires a value\n"); + return 1; + } else if (handled) { + target_peer = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-text", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -text requires a value\n"); + return 1; + } else if (handled) { + text = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-file", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -file requires a value\n"); + return 1; + } else if (handled) { + file_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-bind-ip", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -bind-ip requires a value\n"); + return 1; + } else if (handled) { + bind_ip = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-bind-device", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -bind-device requires a value\n"); + return 1; + } else if (handled) { + bind_device = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-inbox-dir", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -inbox-dir requires a value\n"); + return 1; + } else if (handled) { + inbox_dir = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-latency-log", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -latency-log requires a value\n"); + return 1; + } else if (handled) { + latency_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-kcp-ts-debug-log", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -kcp-ts-debug-log requires a value\n"); + return 1; + } else if (handled) { + packet_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-kcp-session-stats-log", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -kcp-session-stats-log requires a value\n"); + return 1; + } else if (handled) { + stats_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-kcp-session-stats-interval", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -kcp-session-stats-interval requires a value\n"); + return 1; + } else if (handled) { + stats_interval_raw = value; + continue; + } + if ((handled = cli_parse_bool_flag(argv[i], "-interactive", &interactive)) < 0) { + fprintf(stderr, "kcppeer: invalid -interactive value\n"); + return 1; + } else if (handled) { + continue; + } + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + kcppeer_usage(stdout); + return 0; + } + fprintf(stderr, "kcppeer: unknown argument %s\n", argv[i]); + kcppeer_usage(stderr); + return 1; + } + + if (text[0] != '\0' && file_path[0] != '\0') { + fprintf(stderr, "kcppeer: only one of -text or -file may be specified\n"); + return 1; + } + if ((text[0] != '\0' || file_path[0] != '\0') && target_peer[0] == '\0') { + fprintf(stderr, "kcppeer: flag -to is required when sending text or file\n"); + return 1; + } + if (kcp_session_stats_parse_interval_ms(stats_interval_raw, &stats_interval_ms) != 0) { + fprintf(stderr, "kcppeer: invalid -kcp-session-stats-interval value %s\n", stats_interval_raw); + return 1; + } + + if (latency_log_path[0] != '\0') { + latency_logger = latencylog_open_jsonl(latency_log_path); + if (latency_logger == NULL) { + fprintf(stderr, "kcppeer: open latency logger %s failed\n", latency_log_path); + goto cleanup; + } + } + if (packet_log_path[0] != '\0') { + packet_logger = kcp_packet_debug_open_jsonl(packet_log_path); + if (packet_logger == NULL) { + fprintf(stderr, "kcppeer: open kcp packet debug logger %s failed\n", packet_log_path); + goto cleanup; + } + } + if (stats_log_path[0] != '\0') { + stats_logger = kcp_session_stats_open_jsonl(stats_log_path); + if (stats_logger == NULL) { + fprintf(stderr, "kcppeer: open kcp session stats logger %s failed\n", stats_log_path); + goto cleanup; + } + } + + actual_dial_target = relay_via[0] != '\0' ? relay_via : server_addr; + client = kcp_client_dial(server_addr, relay_via, peer_id, bind_ip, bind_device, latency_logger, packet_logger, stats_logger, stats_interval_ms); + if (client == NULL) { + int saved_errno = errno; + const char *reason = saved_errno != 0 ? strerror(saved_errno) : "unknown error"; + if (relay_via[0] != '\0') { + fprintf(stderr, "kcppeer: dial target %s failed (logical server %s): %s (errno=%d)\n", actual_dial_target, server_addr, reason, saved_errno); + } else { + fprintf(stderr, "kcppeer: dial kcp server %s failed: %s (errno=%d)\n", server_addr, reason, saved_errno); + } + goto cleanup; + } + if (relay_via[0] != '\0') { + fprintf(stderr, "opened KCP session as %s; logical server=%s, actual dial target=%s via relay; registration confirmed\n", kcp_client_id(client), server_addr, actual_dial_target); + } else { + fprintf(stderr, "opened KCP session as %s; logical server=%s, actual dial target=%s; registration confirmed\n", kcp_client_id(client), server_addr, actual_dial_target); + } + + receive_ctx.client = client; + receive_ctx.inbox_dir = inbox_dir; + if (pthread_create(&receive_thread, NULL, kcppeer_receive_thread_main, &receive_ctx) != 0) { + fprintf(stderr, "kcppeer: create receive thread failed\n"); + goto cleanup; + } + receive_thread_started = 1; + + if (target_peer[0] != '\0' && text[0] != '\0') { + if (kcp_client_send_text(client, target_peer, text) != 0) { + fprintf(stderr, "kcppeer: send text to %s failed\n", target_peer); + goto cleanup; + } + fprintf(stderr, "sent text to %s\n", target_peer); + } + if (target_peer[0] != '\0' && file_path[0] != '\0') { + if (kcp_client_send_file_path(client, target_peer, file_path) != 0) { + fprintf(stderr, "kcppeer: send file %s to %s failed\n", file_path, target_peer); + goto cleanup; + } + fprintf(stderr, "sent file %s to %s\n", file_path, target_peer); + } + + if (interactive) { + char line[2048]; + char prompt[128]; + + snprintf(prompt, sizeof(prompt), "%s> ", kcp_client_id(client)); + interactive_print_help(stdout, "KCP"); + while (fputs(prompt, stdout) >= 0 && fflush(stdout) == 0 && fgets(line, sizeof(line), stdin) != NULL) { + interactive_command_t command; + char err[128]; + + omni_trim_newline(line); + if (interactive_parse_command(line, &command, err, sizeof(err)) != 0) { + if (strstr(err, "empty command") == NULL) { + fprintf(stderr, "%s\n", err); + } + continue; + } + if (command.type == INTERACTIVE_CMD_HELP) { + interactive_print_help(stdout, "KCP"); + continue; + } + if (command.type == INTERACTIVE_CMD_QUIT) { + break; + } + if (command.type == INTERACTIVE_CMD_TEXT) { + if (kcp_client_send_text(client, command.to, command.value) != 0) { + fprintf(stderr, "kcppeer: send text to %s failed\n", command.to); + continue; + } + fprintf(stderr, "sent text to %s\n", command.to); + continue; + } + if (command.type == INTERACTIVE_CMD_FILE) { + if (kcp_client_send_file_path(client, command.to, command.value) != 0) { + fprintf(stderr, "kcppeer: send file %s to %s failed\n", command.value, command.to); + continue; + } + fprintf(stderr, "sent file %s to %s\n", command.value, command.to); + continue; + } + } + } + + rc = 0; + +cleanup: + receive_ctx.stop_requested = 1; + kcp_client_close(client); + if (receive_thread_started) { + pthread_join(receive_thread, NULL); + if (rc == 0 && receive_ctx.rc != 0) { + rc = 1; + } + } + kcp_client_free(client); + kcp_session_stats_close(stats_logger); + kcp_packet_debug_close(packet_logger); + latencylog_close(latency_logger); + return rc; +} diff --git a/host/OmniSocketGo_add_camera/cmd/kcpping.c b/host/OmniSocketGo_add_camera/cmd/kcpping.c new file mode 100644 index 0000000..42442d8 --- /dev/null +++ b/host/OmniSocketGo_add_camera/cmd/kcpping.c @@ -0,0 +1,788 @@ +#include "cli_parse.h" +#include "peer_kcp_client.h" + +#include "cJSON.h" + +#include +#include + +typedef struct kcp_ping_message_node { + struct kcp_ping_message_node *next; + message_t msg; +} kcp_ping_message_node_t; + +typedef struct kcp_ping_receiver_ctx { + kcp_client_t *client; + pthread_mutex_t mu; + kcp_ping_message_node_t *head; + kcp_ping_message_node_t *tail; + volatile int stop_requested; + int closed; + int rc; +} kcp_ping_receiver_ctx_t; + +typedef struct kcp_pending_ping { + struct kcp_pending_ping *next; + uint64_t seq; + int64_t deadline_ns; +} kcp_pending_ping_t; + +typedef struct kcp_ping_tracker { + kcp_pending_ping_t *pending; + int pending_count; + int sent; + int duplicates; + uint64_t max_seq_sent; + int64_t *samples_ns; + size_t sample_count; + size_t sample_cap; +} kcp_ping_tracker_t; + +static volatile sig_atomic_t g_kcpping_stop = 0; + +static void kcpping_on_signal(int signo) { + (void) signo; + g_kcpping_stop = 1; +} + +static void kcpping_usage(FILE *out) { + fprintf(out, "usage: kcpping [-id pinger] [-server 127.0.0.1:9002] [-to peer] [-echo]\n"); + fprintf(out, " [-count 100] [-interval 100ms] [-size 64] [-timeout 3s]\n"); + fprintf(out, " [-bind-ip ip] [-bind-device dev] [-latency-log path]\n"); +} + +static int kcp_ping_compare_i64(const void *left, const void *right) { + const int64_t *a = (const int64_t *) left; + const int64_t *b = (const int64_t *) right; + if (*a < *b) { + return -1; + } + if (*a > *b) { + return 1; + } + return 0; +} + +static double kcp_ping_sqrt(double value) { + double x = value; + int i; + + if (value <= 0.0) { + return 0.0; + } + if (x < 1.0) { + x = 1.0; + } + for (i = 0; i < 16; ++i) { + x = 0.5 * (x + value / x); + } + return x; +} + +static int kcp_ping_build_payload(uint64_t seq, int64_t ts_ns, int size, char **out_body, size_t *out_len) { + cJSON *root = NULL; + char *json = NULL; + char *pad = NULL; + size_t base_len; + size_t pad_len; + + *out_body = NULL; + *out_len = 0; + + root = cJSON_CreateObject(); + if (root == NULL) { + return -1; + } + cJSON_AddNumberToObject(root, "seq", (double) seq); + cJSON_AddNumberToObject(root, "ts_ns", (double) ts_ns); + cJSON_AddStringToObject(root, "pad", ""); + json = cJSON_PrintUnformatted(root); + cJSON_Delete(root); + if (json == NULL) { + return -1; + } + base_len = strlen(json); + cJSON_free(json); + if ((int) base_len > size) { + errno = EMSGSIZE; + return -1; + } + + pad_len = (size_t) size - base_len; + pad = (char *) malloc(pad_len + 1U); + if (pad == NULL) { + return -1; + } + memset(pad, 'A', pad_len); + pad[pad_len] = '\0'; + + root = cJSON_CreateObject(); + if (root == NULL) { + free(pad); + return -1; + } + cJSON_AddNumberToObject(root, "seq", (double) seq); + cJSON_AddNumberToObject(root, "ts_ns", (double) ts_ns); + cJSON_AddStringToObject(root, "pad", pad); + free(pad); + json = cJSON_PrintUnformatted(root); + cJSON_Delete(root); + if (json == NULL) { + return -1; + } + if ((int) strlen(json) != size) { + cJSON_free(json); + errno = EINVAL; + return -1; + } + *out_body = json; + *out_len = (size_t) size; + return 0; +} + +static int kcp_ping_parse_payload(const uint8_t *body, size_t body_len, uint64_t *seq, int64_t *ts_ns) { + char *text; + cJSON *root; + const cJSON *seq_item; + const cJSON *ts_item; + + if (body == NULL || seq == NULL || ts_ns == NULL) { + errno = EINVAL; + return -1; + } + text = (char *) malloc(body_len + 1U); + if (text == NULL) { + return -1; + } + memcpy(text, body, body_len); + text[body_len] = '\0'; + root = cJSON_Parse(text); + free(text); + if (root == NULL) { + errno = EPROTO; + return -1; + } + seq_item = cJSON_GetObjectItemCaseSensitive(root, "seq"); + ts_item = cJSON_GetObjectItemCaseSensitive(root, "ts_ns"); + if (!cJSON_IsNumber(seq_item) || !cJSON_IsNumber(ts_item) || seq_item->valuedouble <= 0 || ts_item->valuedouble <= 0) { + cJSON_Delete(root); + errno = EPROTO; + return -1; + } + *seq = (uint64_t) seq_item->valuedouble; + *ts_ns = (int64_t) ts_item->valuedouble; + cJSON_Delete(root); + return 0; +} + +static void kcp_ping_receiver_ctx_init(kcp_ping_receiver_ctx_t *ctx, kcp_client_t *client) { + memset(ctx, 0, sizeof(*ctx)); + ctx->client = client; + pthread_mutex_init(&ctx->mu, NULL); +} + +static void kcp_ping_receiver_ctx_destroy(kcp_ping_receiver_ctx_t *ctx) { + kcp_ping_message_node_t *node; + kcp_ping_message_node_t *next; + + if (ctx == NULL) { + return; + } + for (node = ctx->head; node != NULL; node = next) { + next = node->next; + protocol_message_clear(&node->msg); + free(node); + } + pthread_mutex_destroy(&ctx->mu); +} + +static void *kcpping_receive_thread_main(void *arg) { + kcp_ping_receiver_ctx_t *ctx = (kcp_ping_receiver_ctx_t *) arg; + + for (;;) { + message_t msg; + kcp_ping_message_node_t *node; + + protocol_message_init(&msg); + if (kcp_client_receive(ctx->client, &msg) != 0) { + protocol_message_clear(&msg); + pthread_mutex_lock(&ctx->mu); + ctx->rc = ctx->stop_requested ? 0 : -1; + ctx->closed = 1; + pthread_mutex_unlock(&ctx->mu); + return NULL; + } + + node = (kcp_ping_message_node_t *) calloc(1, sizeof(*node)); + if (node == NULL) { + protocol_message_clear(&msg); + pthread_mutex_lock(&ctx->mu); + ctx->rc = -1; + ctx->closed = 1; + pthread_mutex_unlock(&ctx->mu); + return NULL; + } + node->msg = msg; + + pthread_mutex_lock(&ctx->mu); + if (ctx->tail == NULL) { + ctx->head = node; + } else { + ctx->tail->next = node; + } + ctx->tail = node; + pthread_mutex_unlock(&ctx->mu); + } +} + +static int kcp_ping_receiver_pop(kcp_ping_receiver_ctx_t *ctx, message_t *out_msg) { + kcp_ping_message_node_t *node; + + pthread_mutex_lock(&ctx->mu); + node = ctx->head; + if (node != NULL) { + ctx->head = node->next; + if (ctx->head == NULL) { + ctx->tail = NULL; + } + } + pthread_mutex_unlock(&ctx->mu); + + if (node == NULL) { + return 0; + } + *out_msg = node->msg; + free(node); + return 1; +} + +static int kcp_ping_receiver_status(kcp_ping_receiver_ctx_t *ctx, int *closed, int *rc) { + pthread_mutex_lock(&ctx->mu); + *closed = ctx->closed; + *rc = ctx->rc; + pthread_mutex_unlock(&ctx->mu); + return 0; +} + +static void kcp_ping_tracker_init(kcp_ping_tracker_t *tracker) { + memset(tracker, 0, sizeof(*tracker)); +} + +static void kcp_ping_tracker_destroy(kcp_ping_tracker_t *tracker) { + kcp_pending_ping_t *pending; + kcp_pending_ping_t *next; + + for (pending = tracker->pending; pending != NULL; pending = next) { + next = pending->next; + free(pending); + } + free(tracker->samples_ns); +} + +static int kcp_ping_tracker_mark_sent(kcp_ping_tracker_t *tracker, uint64_t seq, int64_t sent_at_ns, int64_t timeout_ns) { + kcp_pending_ping_t *pending = (kcp_pending_ping_t *) calloc(1, sizeof(*pending)); + if (pending == NULL) { + return -1; + } + pending->seq = seq; + pending->deadline_ns = sent_at_ns + timeout_ns; + pending->next = tracker->pending; + tracker->pending = pending; + tracker->pending_count++; + tracker->sent++; + tracker->max_seq_sent = seq; + return 0; +} + +static kcp_pending_ping_t *kcp_ping_tracker_find_pending(kcp_ping_tracker_t *tracker, uint64_t seq, kcp_pending_ping_t **out_prev) { + kcp_pending_ping_t *prev = NULL; + kcp_pending_ping_t *cur; + + for (cur = tracker->pending; cur != NULL; cur = cur->next) { + if (cur->seq == seq) { + if (out_prev != NULL) { + *out_prev = prev; + } + return cur; + } + prev = cur; + } + if (out_prev != NULL) { + *out_prev = NULL; + } + return NULL; +} + +static int kcp_ping_tracker_add_sample(kcp_ping_tracker_t *tracker, int64_t rtt_ns) { + int64_t *next_samples; + size_t next_cap; + + if (tracker->sample_count == tracker->sample_cap) { + next_cap = tracker->sample_cap == 0 ? 16U : tracker->sample_cap * 2U; + next_samples = (int64_t *) realloc(tracker->samples_ns, next_cap * sizeof(*next_samples)); + if (next_samples == NULL) { + return -1; + } + tracker->samples_ns = next_samples; + tracker->sample_cap = next_cap; + } + tracker->samples_ns[tracker->sample_count++] = rtt_ns; + return 0; +} + +static int kcp_ping_tracker_observe_reply(kcp_ping_tracker_t *tracker, uint64_t seq, int64_t sent_ts_ns, int64_t received_ts_ns, int *disposition, int64_t *rtt_ns) { + kcp_pending_ping_t *prev = NULL; + kcp_pending_ping_t *pending; + + if (seq == 0 || seq > tracker->max_seq_sent) { + *disposition = 2; + *rtt_ns = 0; + return 0; + } + pending = kcp_ping_tracker_find_pending(tracker, seq, &prev); + if (pending == NULL) { + tracker->duplicates++; + *disposition = 1; + *rtt_ns = 0; + return 0; + } + if (prev == NULL) { + tracker->pending = pending->next; + } else { + prev->next = pending->next; + } + tracker->pending_count--; + free(pending); + + *rtt_ns = received_ts_ns - sent_ts_ns; + if (*rtt_ns < 0) { + *rtt_ns = 0; + } + if (kcp_ping_tracker_add_sample(tracker, *rtt_ns) != 0) { + return -1; + } + *disposition = 0; + return 0; +} + +static void kcp_ping_tracker_expire(kcp_ping_tracker_t *tracker, int64_t now_ns, FILE *out) { + kcp_pending_ping_t *prev = NULL; + kcp_pending_ping_t *cur = tracker->pending; + + while (cur != NULL) { + if (cur->deadline_ns <= now_ns) { + kcp_pending_ping_t *next = cur->next; + fprintf(out, "seq=%" PRIu64 " timeout\n", cur->seq); + if (prev == NULL) { + tracker->pending = next; + } else { + prev->next = next; + } + free(cur); + tracker->pending_count--; + cur = next; + continue; + } + prev = cur; + cur = cur->next; + } +} + +static int64_t kcp_ping_percentile_ns(const int64_t *sorted, size_t count, double percentile) { + size_t index; + double raw_index; + + if (count == 0) { + return 0; + } + if (percentile <= 0.0) { + return sorted[0]; + } + if (percentile >= 1.0) { + return sorted[count - 1]; + } + raw_index = percentile * (double) count; + index = (size_t) raw_index; + if ((double) index < raw_index) { + index++; + } + if (index > 0) { + index--; + } + if (index >= count) { + index = count - 1; + } + return sorted[index]; +} + +static void kcp_ping_print_summary(FILE *out, const char *target, const kcp_ping_tracker_t *tracker) { + int received = (int) tracker->sample_count; + double loss_pct = tracker->sent == 0 ? 0.0 : ((double) (tracker->sent - received) * 100.0 / (double) tracker->sent); + + fprintf(out, "--- %s kcp ping statistics ---\n", target); + fprintf(out, "%d packets transmitted, %d received, %d duplicates, %.2f%% packet loss\n", tracker->sent, received, tracker->duplicates, loss_pct); + if (tracker->sample_count == 0) { + fprintf(out, "rtt min/avg/max/p50/p95/p99 = n/a/n/a/n/a/n/a/n/a/n/a, stddev=n/a\n"); + return; + } + + { + int64_t *sorted = (int64_t *) malloc(tracker->sample_count * sizeof(*sorted)); + size_t i; + double sum = 0.0; + double variance = 0.0; + double avg; + int64_t min_ns; + int64_t max_ns; + int64_t p50_ns; + int64_t p95_ns; + int64_t p99_ns; + + if (sorted == NULL) { + fprintf(out, "rtt summary unavailable: memory allocation failed\n"); + return; + } + memcpy(sorted, tracker->samples_ns, tracker->sample_count * sizeof(*sorted)); + qsort(sorted, tracker->sample_count, sizeof(*sorted), kcp_ping_compare_i64); + for (i = 0; i < tracker->sample_count; ++i) { + sum += (double) sorted[i]; + } + avg = sum / (double) tracker->sample_count; + for (i = 0; i < tracker->sample_count; ++i) { + double delta = (double) sorted[i] - avg; + variance += delta * delta; + } + variance /= (double) tracker->sample_count; + + min_ns = sorted[0]; + max_ns = sorted[tracker->sample_count - 1]; + p50_ns = kcp_ping_percentile_ns(sorted, tracker->sample_count, 0.50); + p95_ns = kcp_ping_percentile_ns(sorted, tracker->sample_count, 0.95); + p99_ns = kcp_ping_percentile_ns(sorted, tracker->sample_count, 0.99); + + fprintf( + out, + "rtt min/avg/max/p50/p95/p99 = %.2fms/%.2fms/%.2fms/%.2fms/%.2fms/%.2fms, stddev=%.2fms\n", + (double) min_ns / 1000000.0, + avg / 1000000.0, + (double) max_ns / 1000000.0, + (double) p50_ns / 1000000.0, + (double) p95_ns / 1000000.0, + (double) p99_ns / 1000000.0, + kcp_ping_sqrt(variance) / 1000000.0 + ); + free(sorted); + } +} + +static int kcp_ping_expiry_poll_ms(int timeout_ms) { + int interval = timeout_ms / 4; + if (interval < 10) { + return 10; + } + if (interval > 100) { + return 100; + } + return interval; +} + +int main(int argc, char **argv) { + const char *peer_id = "pinger"; + const char *server_addr = "127.0.0.1:9002"; + const char *target_peer = ""; + const char *bind_ip = ""; + const char *bind_device = ""; + const char *latency_log_path = ""; + int echo_mode = 0; + int count = 100; + int interval_ms = 100; + int size = 64; + int timeout_ms = 3000; + latency_logger_t *latency_logger = NULL; + kcp_client_t *client = NULL; + kcp_ping_receiver_ctx_t receiver_ctx; + pthread_t receiver_thread; + int receiver_ctx_initialized = 0; + int receiver_thread_started = 0; + kcp_ping_tracker_t tracker; + int i; + int rc = 1; + + kcp_ping_tracker_init(&tracker); + memset(&receiver_ctx, 0, sizeof(receiver_ctx)); + + for (i = 1; i < argc; ++i) { + const char *value = NULL; + int handled; + + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-id", &value)) < 0) { + fprintf(stderr, "kcpping: flag -id requires a value\n"); + return 1; + } else if (handled) { + peer_id = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-server", &value)) < 0) { + fprintf(stderr, "kcpping: flag -server requires a value\n"); + return 1; + } else if (handled) { + server_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-to", &value)) < 0) { + fprintf(stderr, "kcpping: flag -to requires a value\n"); + return 1; + } else if (handled) { + target_peer = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-count", &value)) < 0) { + fprintf(stderr, "kcpping: flag -count requires a value\n"); + return 1; + } else if (handled) { + count = atoi(value); + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-interval", &value)) < 0) { + fprintf(stderr, "kcpping: flag -interval requires a value\n"); + return 1; + } else if (handled) { + if (omni_parse_duration_ms(value, interval_ms, &interval_ms) != 0) { + fprintf(stderr, "kcpping: invalid -interval value %s\n", value); + return 1; + } + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-size", &value)) < 0) { + fprintf(stderr, "kcpping: flag -size requires a value\n"); + return 1; + } else if (handled) { + size = atoi(value); + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-timeout", &value)) < 0) { + fprintf(stderr, "kcpping: flag -timeout requires a value\n"); + return 1; + } else if (handled) { + if (omni_parse_duration_ms(value, timeout_ms, &timeout_ms) != 0) { + fprintf(stderr, "kcpping: invalid -timeout value %s\n", value); + return 1; + } + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-bind-ip", &value)) < 0) { + fprintf(stderr, "kcpping: flag -bind-ip requires a value\n"); + return 1; + } else if (handled) { + bind_ip = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-bind-device", &value)) < 0) { + fprintf(stderr, "kcpping: flag -bind-device requires a value\n"); + return 1; + } else if (handled) { + bind_device = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-latency-log", &value)) < 0) { + fprintf(stderr, "kcpping: flag -latency-log requires a value\n"); + return 1; + } else if (handled) { + latency_log_path = value; + continue; + } + if ((handled = cli_parse_bool_flag(argv[i], "-echo", &echo_mode)) < 0) { + fprintf(stderr, "kcpping: invalid -echo value\n"); + return 1; + } else if (handled) { + continue; + } + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + kcpping_usage(stdout); + return 0; + } + fprintf(stderr, "kcpping: unknown argument %s\n", argv[i]); + kcpping_usage(stderr); + return 1; + } + + if (peer_id[0] == '\0' || server_addr[0] == '\0') { + fprintf(stderr, "kcpping: flags -id and -server are required\n"); + return 1; + } + if (!echo_mode && target_peer[0] == '\0') { + fprintf(stderr, "kcpping: flag -to is required unless -echo is set\n"); + return 1; + } + if (count < 0 || interval_ms <= 0 || size <= 0 || timeout_ms <= 0) { + fprintf(stderr, "kcpping: invalid numeric flag value\n"); + return 1; + } + + signal(SIGINT, kcpping_on_signal); + + if (latency_log_path[0] != '\0') { + latency_logger = latencylog_open_jsonl(latency_log_path); + if (latency_logger == NULL) { + fprintf(stderr, "kcpping: open latency logger %s failed\n", latency_log_path); + goto cleanup; + } + } + client = kcp_client_dial(server_addr, NULL, peer_id, bind_ip, bind_device, latency_logger, NULL, NULL, KCP_DEFAULT_STATS_INTERVAL_MS); + if (client == NULL) { + fprintf(stderr, "kcpping: dial kcp server %s failed\n", server_addr); + goto cleanup; + } + + if (echo_mode) { + while (!g_kcpping_stop) { + message_t msg; + + protocol_message_init(&msg); + if (kcp_client_receive(client, &msg) != 0) { + protocol_message_clear(&msg); + if (g_kcpping_stop) { + break; + } + fprintf(stderr, "kcpping: receive failed in echo mode\n"); + goto cleanup; + } + if (msg.type == MSG_TYPE_TEXT) { + char *text = (char *) malloc(msg.body_len + 1U); + if (text == NULL) { + protocol_message_clear(&msg); + goto cleanup; + } + memcpy(text, msg.body, msg.body_len); + text[msg.body_len] = '\0'; + if (kcp_client_send_text(client, msg.from, text) != 0) { + free(text); + protocol_message_clear(&msg); + fprintf(stderr, "kcpping: echo send back to %s failed\n", msg.from); + goto cleanup; + } + free(text); + } else if (msg.type == MSG_TYPE_ERROR) { + fprintf(stderr, "server error: %.*s\n", (int) msg.body_len, msg.body == NULL ? "" : (const char *) msg.body); + } else { + fprintf(stderr, "unexpected message type %s from %s ignored\n", protocol_message_type_name(msg.type), msg.from); + } + protocol_message_clear(&msg); + } + rc = 0; + goto cleanup; + } + + fprintf(stdout, "KCP PING %s via %s (payload=%d bytes, KCP)\n", target_peer, server_addr, size); + kcp_ping_receiver_ctx_init(&receiver_ctx, client); + receiver_ctx_initialized = 1; + if (pthread_create(&receiver_thread, NULL, kcpping_receive_thread_main, &receiver_ctx) != 0) { + fprintf(stderr, "kcpping: create receive thread failed\n"); + goto cleanup; + } + receiver_thread_started = 1; + + { + uint64_t next_seq = 1; + int stop_sending = 0; + int64_t next_send_at_ns = omni_now_unix_nano(); + int poll_ms = kcp_ping_expiry_poll_ms(timeout_ms); + int64_t timeout_ns = (int64_t) timeout_ms * 1000000LL; + + while (!g_kcpping_stop || tracker.pending_count > 0 || !stop_sending) { + int64_t now_ns = omni_now_unix_nano(); + message_t msg; + int popped; + int receiver_closed; + int receiver_status_rc; + + if (!stop_sending && now_ns >= next_send_at_ns) { + char *payload = NULL; + size_t payload_len = 0; + + if (count > 0 && tracker.sent >= count) { + stop_sending = 1; + } else { + if (kcp_ping_build_payload(next_seq, now_ns, size, &payload, &payload_len) != 0) { + fprintf(stderr, "kcpping: build payload for seq=%" PRIu64 " failed\n", next_seq); + free(payload); + goto cleanup; + } + if (kcp_client_send_text(client, target_peer, payload) != 0) { + fprintf(stderr, "kcpping: send ping seq=%" PRIu64 " failed\n", next_seq); + free(payload); + goto cleanup; + } + free(payload); + if (kcp_ping_tracker_mark_sent(&tracker, next_seq, now_ns, timeout_ns) != 0) { + goto cleanup; + } + next_seq++; + next_send_at_ns = now_ns + (int64_t) interval_ms * 1000000LL; + if (count > 0 && tracker.sent >= count) { + stop_sending = 1; + } + } + } + + kcp_ping_tracker_expire(&tracker, now_ns, stdout); + + do { + popped = kcp_ping_receiver_pop(&receiver_ctx, &msg); + if (popped == 1) { + if (msg.type == MSG_TYPE_TEXT) { + uint64_t seq; + int64_t sent_ts_ns; + int disposition; + int64_t rtt_ns; + + if (kcp_ping_parse_payload(msg.body, msg.body_len, &seq, &sent_ts_ns) != 0) { + fprintf(stderr, "ignore non-ping text message from %s\n", msg.from); + } else if (kcp_ping_tracker_observe_reply(&tracker, seq, sent_ts_ns, omni_now_unix_nano(), &disposition, &rtt_ns) != 0) { + protocol_message_clear(&msg); + goto cleanup; + } else if (disposition == 0) { + fprintf(stdout, "seq=%" PRIu64 " rtt=%.2fms\n", seq, (double) rtt_ns / 1000000.0); + } else if (disposition == 1) { + fprintf(stderr, "seq=%" PRIu64 " duplicate or late reply ignored\n", seq); + } else { + fprintf(stderr, "seq=%" PRIu64 " unexpected reply ignored\n", seq); + } + } else if (msg.type == MSG_TYPE_ERROR) { + fprintf(stderr, "server error: %.*s\n", (int) msg.body_len, msg.body == NULL ? "" : (const char *) msg.body); + } else { + fprintf(stderr, "unexpected message type %s from %s ignored\n", protocol_message_type_name(msg.type), msg.from); + } + protocol_message_clear(&msg); + } + } while (popped == 1); + + kcp_ping_receiver_status(&receiver_ctx, &receiver_closed, &receiver_status_rc); + if (receiver_closed && receiver_status_rc != 0) { + fprintf(stderr, "kcpping: receive loop failed\n"); + goto cleanup; + } + if ((g_kcpping_stop || stop_sending) && tracker.pending_count == 0) { + break; + } + usleep((useconds_t) poll_ms * 1000U); + } + } + + kcp_ping_print_summary(stdout, target_peer, &tracker); + rc = 0; + +cleanup: + receiver_ctx.stop_requested = 1; + kcp_client_close(client); + if (receiver_thread_started) { + pthread_join(receiver_thread, NULL); + kcp_ping_receiver_ctx_destroy(&receiver_ctx); + } else if (receiver_ctx_initialized) { + kcp_ping_receiver_ctx_destroy(&receiver_ctx); + } + kcp_client_free(client); + latencylog_close(latency_logger); + kcp_ping_tracker_destroy(&tracker); + return rc; +} diff --git a/host/OmniSocketGo_add_camera/cmd/kcpserver.c b/host/OmniSocketGo_add_camera/cmd/kcpserver.c new file mode 100644 index 0000000..afcf8f4 --- /dev/null +++ b/host/OmniSocketGo_add_camera/cmd/kcpserver.c @@ -0,0 +1,253 @@ +#include "cli_parse.h" +#include "server_kcp_hub.h" +#include "server_udp_relay.h" + +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"); +} + +int main(int argc, char **argv) { + const char *mode = "hub"; + const char *listen_addr = ":9002"; + const char *bind_device = ""; + const char *latency_log_path = ""; + 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; + + latency_logger_t *latency_logger = NULL; + kcp_packet_debug_logger_t *packet_logger = NULL; + kcp_session_stats_logger_t *stats_logger = NULL; + kcp_listener_t *listener = NULL; + kcp_hub_t *hub = NULL; + udp_relay_t *relay = NULL; + + for (i = 1; i < argc; ++i) { + const char *value = NULL; + int handled; + + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-mode", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -mode requires a value\n"); + return 1; + } else if (handled) { + mode = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-listen", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -listen requires a value\n"); + return 1; + } else if (handled) { + listen_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-bind-device", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -bind-device requires a value\n"); + return 1; + } else if (handled) { + bind_device = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-latency-log", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -latency-log requires a value\n"); + return 1; + } else if (handled) { + latency_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-kcp-ts-debug-log", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -kcp-ts-debug-log requires a value\n"); + return 1; + } else if (handled) { + packet_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-kcp-session-stats-log", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -kcp-session-stats-log requires a value\n"); + return 1; + } else if (handled) { + stats_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-kcp-session-stats-interval", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -kcp-session-stats-interval requires a value\n"); + return 1; + } else if (handled) { + 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; + } else if (handled) { + relay_listen_alias = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-relay-remote", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -relay-remote requires a value\n"); + return 1; + } else if (handled) { + relay_remote_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-relay-peer", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -relay-peer requires a value\n"); + return 1; + } else if (handled) { + relay_peer_alias = value; + continue; + } + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + kcpserver_usage(stdout); + return 0; + } + fprintf(stderr, "kcpserver: unknown argument %s\n", argv[i]); + kcpserver_usage(stderr); + return 1; + } + + if (kcp_session_stats_parse_interval_ms(stats_interval_raw, &stats_interval_ms) != 0) { + 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"); + return 1; + } + if (relay_remote_addr[0] == '\0' && relay_peer_alias[0] != '\0') { + relay_remote_addr = relay_peer_alias; + } + if (relay_peer_alias[0] != '\0') { + fprintf(stderr, "warning: flag -relay-peer is deprecated; use -relay-remote instead\n"); + } + if (relay_listen_alias[0] != '\0') { + if (strcmp(mode, "relay") != 0) { + fprintf(stderr, "kcpserver: flag -relay-listen may only be used in relay mode\n"); + return 1; + } + if (listen_addr[0] != '\0' && strcmp(listen_addr, ":9002") != 0 && strcmp(listen_addr, relay_listen_alias) != 0) { + fprintf(stderr, "kcpserver: flags -listen and -relay-listen must match when both are set in relay mode\n"); + return 1; + } + listen_addr = relay_listen_alias; + fprintf(stderr, "warning: flag -relay-listen is deprecated; use -listen with -mode=relay instead\n"); + } + + if (strcmp(mode, "hub") == 0) { + if (relay_remote_addr[0] != '\0') { + fprintf(stderr, "kcpserver: flag -relay-remote may only be used in relay mode\n"); + return 1; + } + if (latency_log_path[0] != '\0') { + latency_logger = latencylog_open_jsonl(latency_log_path); + if (latency_logger == NULL) { + fprintf(stderr, "kcpserver: open latency logger %s failed\n", latency_log_path); + goto cleanup; + } + } + if (packet_log_path[0] != '\0') { + packet_logger = kcp_packet_debug_open_jsonl(packet_log_path); + if (packet_logger == NULL) { + fprintf(stderr, "kcpserver: open packet debug logger %s failed\n", packet_log_path); + goto cleanup; + } + } + if (stats_log_path[0] != '\0') { + stats_logger = kcp_session_stats_open_jsonl(stats_log_path); + if (stats_logger == NULL) { + fprintf(stderr, "kcpserver: open session stats logger %s failed\n", stats_log_path); + goto cleanup; + } + } + listener = kcp_listener_listen(listen_addr, bind_device, packet_logger, OMNI_NODE_ROLE_SERVER, "hub"); + if (listener == NULL) { + fprintf(stderr, "kcpserver: listen on %s failed\n", listen_addr); + goto cleanup; + } + hub = kcp_hub_new(latency_logger, stats_logger, stats_interval_ms); + if (hub == NULL) { + 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"); + goto cleanup; + } + rc = 0; + goto cleanup; + } + + 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; + } + if (relay_remote_addr[0] == '\0') { + fprintf(stderr, "kcpserver: flag -relay-remote is required in relay mode\n"); + return 1; + } + relay = udp_relay_open(listen_addr, relay_remote_addr); + if (relay == NULL) { + fprintf(stderr, "kcpserver: open udp relay %s -> %s failed\n", listen_addr, relay_remote_addr); + goto cleanup; + } + fprintf(stderr, "udp relay listening on %s and forwarding to %s\n", listen_addr, relay_remote_addr); + if (udp_relay_serve(relay) != 0) { + fprintf(stderr, "kcpserver: udp relay stopped with error\n"); + goto cleanup; + } + rc = 0; + goto cleanup; + } + + fprintf(stderr, "kcpserver: unsupported -mode=%s; want hub or relay\n", mode); + +cleanup: + udp_relay_free(relay); + kcp_hub_free(hub); + kcp_listener_free(listener); + kcp_session_stats_close(stats_logger); + kcp_packet_debug_close(packet_logger); + latencylog_close(latency_logger); + return rc; +} diff --git a/host/OmniSocketGo_add_camera/cmd/udppeer.c b/host/OmniSocketGo_add_camera/cmd/udppeer.c new file mode 100644 index 0000000..e5656ae --- /dev/null +++ b/host/OmniSocketGo_add_camera/cmd/udppeer.c @@ -0,0 +1,291 @@ +#include "cli_parse.h" +#include "interactive.h" +#include "peer_udp_client.h" + +#include + +typedef struct udppeer_receive_ctx { + udp_client_t *client; + const char *inbox_dir; + volatile int stop_requested; + int rc; +} udppeer_receive_ctx_t; + +static void udppeer_usage(FILE *out) { + fprintf(out, "usage: udppeer [-id peer-a] [-server 127.0.0.1:9001] [-to peer] [-text msg | -file path]\n"); + fprintf(out, " [-bind-ip ip] [-inbox-dir dir] [-latency-log path] [-tx-ts-debug-log path]\n"); + fprintf(out, " [-interactive[=true|false]]\n"); +} + +static void *udppeer_receive_thread_main(void *arg) { + udppeer_receive_ctx_t *ctx = (udppeer_receive_ctx_t *) arg; + + for (;;) { + message_t msg; + char persisted_path[512]; + + protocol_message_init(&msg); + if (udp_client_receive(ctx->client, &msg) != 0) { + protocol_message_clear(&msg); + ctx->rc = ctx->stop_requested ? 0 : -1; + return NULL; + } + + switch (msg.type) { + case MSG_TYPE_TEXT: + if (udp_client_persist_message(ctx->client, &msg, ctx->inbox_dir, persisted_path, sizeof(persisted_path)) != 0) { + fprintf(stderr, "udppeer: persist text from %s to %s failed\n", msg.from, msg.to); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + fprintf(stderr, "received text from %s to %s and persisted to %s\n", msg.from, msg.to, persisted_path); + break; + case MSG_TYPE_FILE: + if (udp_client_persist_message(ctx->client, &msg, ctx->inbox_dir, persisted_path, sizeof(persisted_path)) != 0) { + fprintf(stderr, "udppeer: persist file from %s to %s failed\n", msg.from, msg.to); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + fprintf(stderr, "received file from %s to %s: %s (%lu bytes) -> %s\n", msg.from, msg.to, msg.file_name, (unsigned long) msg.body_len, persisted_path); + break; + case MSG_TYPE_BINARY: + if (udp_client_persist_message(ctx->client, &msg, ctx->inbox_dir, persisted_path, sizeof(persisted_path)) != 0) { + fprintf(stderr, "udppeer: persist binary payload from %s to %s failed\n", msg.from, msg.to); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + fprintf(stderr, "received binary payload from %s to %s (%lu bytes) -> %s\n", msg.from, msg.to, (unsigned long) msg.body_len, persisted_path); + break; + case MSG_TYPE_ERROR: + fprintf(stderr, "received error from %s to %s: %.*s\n", msg.from, msg.to, (int) msg.body_len, msg.body == NULL ? "" : (const char *) msg.body); + break; + default: + fprintf(stderr, "received unexpected message type %s from %s\n", protocol_message_type_name(msg.type), msg.from); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + protocol_message_clear(&msg); + } +} + +int main(int argc, char **argv) { + const char *peer_id = "peer-a"; + const char *server_addr = "127.0.0.1:9001"; + const char *target_peer = ""; + const char *text = ""; + const char *file_path = ""; + const char *bind_ip = ""; + const char *inbox_dir = "inbox"; + const char *latency_log_path = ""; + const char *tx_debug_log_path = ""; + int interactive = 1; + latency_logger_t *latency_logger = NULL; + tx_timestamp_debug_logger_t *debug_logger = NULL; + udp_client_t *client = NULL; + udppeer_receive_ctx_t receive_ctx; + pthread_t receive_thread; + int receive_thread_started = 0; + int i; + int rc = 1; + + memset(&receive_ctx, 0, sizeof(receive_ctx)); + + for (i = 1; i < argc; ++i) { + const char *value = NULL; + int handled; + + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-id", &value)) < 0) { + fprintf(stderr, "udppeer: flag -id requires a value\n"); + return 1; + } else if (handled) { + peer_id = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-server", &value)) < 0) { + fprintf(stderr, "udppeer: flag -server requires a value\n"); + return 1; + } else if (handled) { + server_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-to", &value)) < 0) { + fprintf(stderr, "udppeer: flag -to requires a value\n"); + return 1; + } else if (handled) { + target_peer = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-text", &value)) < 0) { + fprintf(stderr, "udppeer: flag -text requires a value\n"); + return 1; + } else if (handled) { + text = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-file", &value)) < 0) { + fprintf(stderr, "udppeer: flag -file requires a value\n"); + return 1; + } else if (handled) { + file_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-bind-ip", &value)) < 0) { + fprintf(stderr, "udppeer: flag -bind-ip requires a value\n"); + return 1; + } else if (handled) { + bind_ip = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-inbox-dir", &value)) < 0) { + fprintf(stderr, "udppeer: flag -inbox-dir requires a value\n"); + return 1; + } else if (handled) { + inbox_dir = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-latency-log", &value)) < 0) { + fprintf(stderr, "udppeer: flag -latency-log requires a value\n"); + return 1; + } else if (handled) { + latency_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-tx-ts-debug-log", &value)) < 0) { + fprintf(stderr, "udppeer: flag -tx-ts-debug-log requires a value\n"); + return 1; + } else if (handled) { + tx_debug_log_path = value; + continue; + } + if ((handled = cli_parse_bool_flag(argv[i], "-interactive", &interactive)) < 0) { + fprintf(stderr, "udppeer: invalid -interactive value\n"); + return 1; + } else if (handled) { + continue; + } + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + udppeer_usage(stdout); + return 0; + } + fprintf(stderr, "udppeer: unknown argument %s\n", argv[i]); + udppeer_usage(stderr); + return 1; + } + + if (text[0] != '\0' && file_path[0] != '\0') { + fprintf(stderr, "udppeer: only one of -text or -file may be specified\n"); + return 1; + } + if ((text[0] != '\0' || file_path[0] != '\0') && target_peer[0] == '\0') { + fprintf(stderr, "udppeer: flag -to is required when sending text or file\n"); + return 1; + } + + if (latency_log_path[0] != '\0') { + latency_logger = latencylog_open_jsonl(latency_log_path); + if (latency_logger == NULL) { + fprintf(stderr, "udppeer: open latency logger %s failed\n", latency_log_path); + goto cleanup; + } + } + if (tx_debug_log_path[0] != '\0') { + debug_logger = tx_timestamp_debug_open_jsonl(tx_debug_log_path); + if (debug_logger == NULL) { + fprintf(stderr, "udppeer: open tx timestamp debug logger %s failed\n", tx_debug_log_path); + goto cleanup; + } + } + + client = udp_client_dial(server_addr, peer_id, bind_ip, latency_logger, debug_logger, tx_debug_log_path[0] != '\0'); + if (client == NULL) { + fprintf(stderr, "udppeer: dial udp server %s failed\n", server_addr); + goto cleanup; + } + fprintf(stderr, "connected to %s as %s (UDP)\n", server_addr, udp_client_id(client)); + + receive_ctx.client = client; + receive_ctx.inbox_dir = inbox_dir; + if (pthread_create(&receive_thread, NULL, udppeer_receive_thread_main, &receive_ctx) != 0) { + fprintf(stderr, "udppeer: create receive thread failed\n"); + goto cleanup; + } + receive_thread_started = 1; + + if (target_peer[0] != '\0' && text[0] != '\0') { + if (udp_client_send_text(client, target_peer, text) != 0) { + fprintf(stderr, "udppeer: send text to %s failed\n", target_peer); + goto cleanup; + } + fprintf(stderr, "sent text to %s\n", target_peer); + } + if (target_peer[0] != '\0' && file_path[0] != '\0') { + if (udp_client_send_file_path(client, target_peer, file_path) != 0) { + fprintf(stderr, "udppeer: send file %s to %s failed\n", file_path, target_peer); + goto cleanup; + } + fprintf(stderr, "sent file %s to %s\n", file_path, target_peer); + } + + if (interactive) { + char line[2048]; + char prompt[128]; + + snprintf(prompt, sizeof(prompt), "%s> ", udp_client_id(client)); + interactive_print_help(stdout, "UDP"); + while (fputs(prompt, stdout) >= 0 && fflush(stdout) == 0 && fgets(line, sizeof(line), stdin) != NULL) { + interactive_command_t command; + char err[128]; + + omni_trim_newline(line); + if (interactive_parse_command(line, &command, err, sizeof(err)) != 0) { + if (strstr(err, "empty command") == NULL) { + fprintf(stderr, "%s\n", err); + } + continue; + } + if (command.type == INTERACTIVE_CMD_HELP) { + interactive_print_help(stdout, "UDP"); + continue; + } + if (command.type == INTERACTIVE_CMD_QUIT) { + break; + } + if (command.type == INTERACTIVE_CMD_TEXT) { + if (udp_client_send_text(client, command.to, command.value) != 0) { + fprintf(stderr, "udppeer: send text to %s failed\n", command.to); + continue; + } + fprintf(stderr, "sent text to %s\n", command.to); + continue; + } + if (command.type == INTERACTIVE_CMD_FILE) { + if (udp_client_send_file_path(client, command.to, command.value) != 0) { + fprintf(stderr, "udppeer: send file %s to %s failed\n", command.value, command.to); + continue; + } + fprintf(stderr, "sent file %s to %s\n", command.value, command.to); + continue; + } + } + } + + rc = 0; + +cleanup: + receive_ctx.stop_requested = 1; + udp_client_close(client); + if (receive_thread_started) { + pthread_join(receive_thread, NULL); + if (rc == 0 && receive_ctx.rc != 0) { + rc = 1; + } + } + udp_client_free(client); + tx_timestamp_debug_close(debug_logger); + latencylog_close(latency_logger); + return rc; +} diff --git a/host/OmniSocketGo_add_camera/cmd/udpping.c b/host/OmniSocketGo_add_camera/cmd/udpping.c new file mode 100644 index 0000000..1b652da --- /dev/null +++ b/host/OmniSocketGo_add_camera/cmd/udpping.c @@ -0,0 +1,780 @@ +#include "cli_parse.h" +#include "peer_udp_client.h" + +#include "cJSON.h" + +#include +#include + +typedef struct ping_message_node { + struct ping_message_node *next; + message_t msg; +} ping_message_node_t; + +typedef struct ping_receiver_ctx { + udp_client_t *client; + pthread_mutex_t mu; + ping_message_node_t *head; + ping_message_node_t *tail; + volatile int stop_requested; + int closed; + int rc; +} ping_receiver_ctx_t; + +typedef struct pending_ping { + struct pending_ping *next; + uint64_t seq; + int64_t deadline_ns; +} pending_ping_t; + +typedef struct ping_tracker { + pending_ping_t *pending; + int pending_count; + int sent; + int duplicates; + uint64_t max_seq_sent; + int64_t *samples_ns; + size_t sample_count; + size_t sample_cap; +} ping_tracker_t; + +static volatile sig_atomic_t g_udpping_stop = 0; + +static void udpping_on_signal(int signo) { + (void) signo; + g_udpping_stop = 1; +} + +static void udpping_usage(FILE *out) { + fprintf(out, "usage: udpping [-id pinger] [-server 127.0.0.1:9001] [-to peer] [-echo]\n"); + fprintf(out, " [-count 100] [-interval 100ms] [-size 64] [-timeout 3s]\n"); + fprintf(out, " [-bind-ip ip] [-latency-log path]\n"); +} + +static int ping_compare_i64(const void *left, const void *right) { + const int64_t *a = (const int64_t *) left; + const int64_t *b = (const int64_t *) right; + if (*a < *b) { + return -1; + } + if (*a > *b) { + return 1; + } + return 0; +} + +static double ping_sqrt(double value) { + double x = value; + int i; + + if (value <= 0.0) { + return 0.0; + } + if (x < 1.0) { + x = 1.0; + } + for (i = 0; i < 16; ++i) { + x = 0.5 * (x + value / x); + } + return x; +} + +static int ping_build_payload(uint64_t seq, int64_t ts_ns, int size, char **out_body, size_t *out_len) { + cJSON *root = NULL; + char *json = NULL; + char *pad = NULL; + size_t base_len; + size_t pad_len; + + *out_body = NULL; + *out_len = 0; + + root = cJSON_CreateObject(); + if (root == NULL) { + return -1; + } + cJSON_AddNumberToObject(root, "seq", (double) seq); + cJSON_AddNumberToObject(root, "ts_ns", (double) ts_ns); + cJSON_AddStringToObject(root, "pad", ""); + json = cJSON_PrintUnformatted(root); + cJSON_Delete(root); + if (json == NULL) { + return -1; + } + base_len = strlen(json); + cJSON_free(json); + if ((int) base_len > size) { + errno = EMSGSIZE; + return -1; + } + + pad_len = (size_t) size - base_len; + pad = (char *) malloc(pad_len + 1U); + if (pad == NULL) { + return -1; + } + memset(pad, 'A', pad_len); + pad[pad_len] = '\0'; + + root = cJSON_CreateObject(); + if (root == NULL) { + free(pad); + return -1; + } + cJSON_AddNumberToObject(root, "seq", (double) seq); + cJSON_AddNumberToObject(root, "ts_ns", (double) ts_ns); + cJSON_AddStringToObject(root, "pad", pad); + free(pad); + json = cJSON_PrintUnformatted(root); + cJSON_Delete(root); + if (json == NULL) { + return -1; + } + if ((int) strlen(json) != size) { + cJSON_free(json); + errno = EINVAL; + return -1; + } + *out_body = json; + *out_len = (size_t) size; + return 0; +} + +static int ping_parse_payload(const uint8_t *body, size_t body_len, uint64_t *seq, int64_t *ts_ns) { + char *text; + cJSON *root; + const cJSON *seq_item; + const cJSON *ts_item; + + if (body == NULL || seq == NULL || ts_ns == NULL) { + errno = EINVAL; + return -1; + } + text = (char *) malloc(body_len + 1U); + if (text == NULL) { + return -1; + } + memcpy(text, body, body_len); + text[body_len] = '\0'; + root = cJSON_Parse(text); + free(text); + if (root == NULL) { + errno = EPROTO; + return -1; + } + seq_item = cJSON_GetObjectItemCaseSensitive(root, "seq"); + ts_item = cJSON_GetObjectItemCaseSensitive(root, "ts_ns"); + if (!cJSON_IsNumber(seq_item) || !cJSON_IsNumber(ts_item) || seq_item->valuedouble <= 0 || ts_item->valuedouble <= 0) { + cJSON_Delete(root); + errno = EPROTO; + return -1; + } + *seq = (uint64_t) seq_item->valuedouble; + *ts_ns = (int64_t) ts_item->valuedouble; + cJSON_Delete(root); + return 0; +} + +static void ping_receiver_ctx_init(ping_receiver_ctx_t *ctx, udp_client_t *client) { + memset(ctx, 0, sizeof(*ctx)); + ctx->client = client; + pthread_mutex_init(&ctx->mu, NULL); +} + +static void ping_receiver_ctx_destroy(ping_receiver_ctx_t *ctx) { + ping_message_node_t *node; + ping_message_node_t *next; + + if (ctx == NULL) { + return; + } + for (node = ctx->head; node != NULL; node = next) { + next = node->next; + protocol_message_clear(&node->msg); + free(node); + } + pthread_mutex_destroy(&ctx->mu); +} + +static void *udpping_receive_thread_main(void *arg) { + ping_receiver_ctx_t *ctx = (ping_receiver_ctx_t *) arg; + + for (;;) { + message_t msg; + ping_message_node_t *node; + + protocol_message_init(&msg); + if (udp_client_receive(ctx->client, &msg) != 0) { + protocol_message_clear(&msg); + pthread_mutex_lock(&ctx->mu); + ctx->rc = ctx->stop_requested ? 0 : -1; + ctx->closed = 1; + pthread_mutex_unlock(&ctx->mu); + return NULL; + } + + node = (ping_message_node_t *) calloc(1, sizeof(*node)); + if (node == NULL) { + protocol_message_clear(&msg); + pthread_mutex_lock(&ctx->mu); + ctx->rc = -1; + ctx->closed = 1; + pthread_mutex_unlock(&ctx->mu); + return NULL; + } + node->msg = msg; + + pthread_mutex_lock(&ctx->mu); + if (ctx->tail == NULL) { + ctx->head = node; + } else { + ctx->tail->next = node; + } + ctx->tail = node; + pthread_mutex_unlock(&ctx->mu); + } +} + +static int ping_receiver_pop(ping_receiver_ctx_t *ctx, message_t *out_msg) { + ping_message_node_t *node; + + pthread_mutex_lock(&ctx->mu); + node = ctx->head; + if (node != NULL) { + ctx->head = node->next; + if (ctx->head == NULL) { + ctx->tail = NULL; + } + } + pthread_mutex_unlock(&ctx->mu); + + if (node == NULL) { + return 0; + } + *out_msg = node->msg; + free(node); + return 1; +} + +static int ping_receiver_status(ping_receiver_ctx_t *ctx, int *closed, int *rc) { + pthread_mutex_lock(&ctx->mu); + *closed = ctx->closed; + *rc = ctx->rc; + pthread_mutex_unlock(&ctx->mu); + return 0; +} + +static void ping_tracker_init(ping_tracker_t *tracker) { + memset(tracker, 0, sizeof(*tracker)); +} + +static void ping_tracker_destroy(ping_tracker_t *tracker) { + pending_ping_t *pending; + pending_ping_t *next; + + for (pending = tracker->pending; pending != NULL; pending = next) { + next = pending->next; + free(pending); + } + free(tracker->samples_ns); +} + +static int ping_tracker_mark_sent(ping_tracker_t *tracker, uint64_t seq, int64_t sent_at_ns, int64_t timeout_ns) { + pending_ping_t *pending = (pending_ping_t *) calloc(1, sizeof(*pending)); + if (pending == NULL) { + return -1; + } + pending->seq = seq; + pending->deadline_ns = sent_at_ns + timeout_ns; + pending->next = tracker->pending; + tracker->pending = pending; + tracker->pending_count++; + tracker->sent++; + tracker->max_seq_sent = seq; + return 0; +} + +static pending_ping_t *ping_tracker_find_pending(ping_tracker_t *tracker, uint64_t seq, pending_ping_t **out_prev) { + pending_ping_t *prev = NULL; + pending_ping_t *cur; + + for (cur = tracker->pending; cur != NULL; cur = cur->next) { + if (cur->seq == seq) { + if (out_prev != NULL) { + *out_prev = prev; + } + return cur; + } + prev = cur; + } + if (out_prev != NULL) { + *out_prev = NULL; + } + return NULL; +} + +static int ping_tracker_add_sample(ping_tracker_t *tracker, int64_t rtt_ns) { + int64_t *next_samples; + size_t next_cap; + + if (tracker->sample_count == tracker->sample_cap) { + next_cap = tracker->sample_cap == 0 ? 16U : tracker->sample_cap * 2U; + next_samples = (int64_t *) realloc(tracker->samples_ns, next_cap * sizeof(*next_samples)); + if (next_samples == NULL) { + return -1; + } + tracker->samples_ns = next_samples; + tracker->sample_cap = next_cap; + } + tracker->samples_ns[tracker->sample_count++] = rtt_ns; + return 0; +} + +static int ping_tracker_observe_reply(ping_tracker_t *tracker, uint64_t seq, int64_t sent_ts_ns, int64_t received_ts_ns, int *disposition, int64_t *rtt_ns) { + pending_ping_t *prev = NULL; + pending_ping_t *pending; + + if (seq == 0 || seq > tracker->max_seq_sent) { + *disposition = 2; + *rtt_ns = 0; + return 0; + } + pending = ping_tracker_find_pending(tracker, seq, &prev); + if (pending == NULL) { + tracker->duplicates++; + *disposition = 1; + *rtt_ns = 0; + return 0; + } + if (prev == NULL) { + tracker->pending = pending->next; + } else { + prev->next = pending->next; + } + tracker->pending_count--; + free(pending); + + *rtt_ns = received_ts_ns - sent_ts_ns; + if (*rtt_ns < 0) { + *rtt_ns = 0; + } + if (ping_tracker_add_sample(tracker, *rtt_ns) != 0) { + return -1; + } + *disposition = 0; + return 0; +} + +static void ping_tracker_expire(ping_tracker_t *tracker, int64_t now_ns, FILE *out) { + pending_ping_t *prev = NULL; + pending_ping_t *cur = tracker->pending; + + while (cur != NULL) { + if (cur->deadline_ns <= now_ns) { + pending_ping_t *next = cur->next; + fprintf(out, "seq=%" PRIu64 " timeout\n", cur->seq); + if (prev == NULL) { + tracker->pending = next; + } else { + prev->next = next; + } + free(cur); + tracker->pending_count--; + cur = next; + continue; + } + prev = cur; + cur = cur->next; + } +} + +static int64_t ping_percentile_ns(const int64_t *sorted, size_t count, double percentile) { + size_t index; + double raw_index; + + if (count == 0) { + return 0; + } + if (percentile <= 0.0) { + return sorted[0]; + } + if (percentile >= 1.0) { + return sorted[count - 1]; + } + raw_index = percentile * (double) count; + index = (size_t) raw_index; + if ((double) index < raw_index) { + index++; + } + if (index > 0) { + index--; + } + if (index >= count) { + index = count - 1; + } + return sorted[index]; +} + +static void ping_print_summary(FILE *out, const char *target, const ping_tracker_t *tracker) { + int received = (int) tracker->sample_count; + double loss_pct = tracker->sent == 0 ? 0.0 : ((double) (tracker->sent - received) * 100.0 / (double) tracker->sent); + + fprintf(out, "--- %s udp ping statistics ---\n", target); + fprintf(out, "%d packets transmitted, %d received, %d duplicates, %.2f%% packet loss\n", tracker->sent, received, tracker->duplicates, loss_pct); + if (tracker->sample_count == 0) { + fprintf(out, "rtt min/avg/max/p50/p95/p99 = n/a/n/a/n/a/n/a/n/a/n/a, stddev=n/a\n"); + return; + } + + { + int64_t *sorted = (int64_t *) malloc(tracker->sample_count * sizeof(*sorted)); + size_t i; + double sum = 0.0; + double variance = 0.0; + double avg; + int64_t min_ns; + int64_t max_ns; + int64_t p50_ns; + int64_t p95_ns; + int64_t p99_ns; + + if (sorted == NULL) { + fprintf(out, "rtt summary unavailable: memory allocation failed\n"); + return; + } + memcpy(sorted, tracker->samples_ns, tracker->sample_count * sizeof(*sorted)); + qsort(sorted, tracker->sample_count, sizeof(*sorted), ping_compare_i64); + for (i = 0; i < tracker->sample_count; ++i) { + sum += (double) sorted[i]; + } + avg = sum / (double) tracker->sample_count; + for (i = 0; i < tracker->sample_count; ++i) { + double delta = (double) sorted[i] - avg; + variance += delta * delta; + } + variance /= (double) tracker->sample_count; + + min_ns = sorted[0]; + max_ns = sorted[tracker->sample_count - 1]; + p50_ns = ping_percentile_ns(sorted, tracker->sample_count, 0.50); + p95_ns = ping_percentile_ns(sorted, tracker->sample_count, 0.95); + p99_ns = ping_percentile_ns(sorted, tracker->sample_count, 0.99); + + fprintf( + out, + "rtt min/avg/max/p50/p95/p99 = %.2fms/%.2fms/%.2fms/%.2fms/%.2fms/%.2fms, stddev=%.2fms\n", + (double) min_ns / 1000000.0, + avg / 1000000.0, + (double) max_ns / 1000000.0, + (double) p50_ns / 1000000.0, + (double) p95_ns / 1000000.0, + (double) p99_ns / 1000000.0, + ping_sqrt(variance) / 1000000.0 + ); + free(sorted); + } +} + +static int ping_expiry_poll_ms(int timeout_ms) { + int interval = timeout_ms / 4; + if (interval < 10) { + return 10; + } + if (interval > 100) { + return 100; + } + return interval; +} + +int main(int argc, char **argv) { + const char *peer_id = "pinger"; + const char *server_addr = "127.0.0.1:9001"; + const char *target_peer = ""; + const char *bind_ip = ""; + const char *latency_log_path = ""; + int echo_mode = 0; + int count = 100; + int interval_ms = 100; + int size = 64; + int timeout_ms = 3000; + latency_logger_t *latency_logger = NULL; + udp_client_t *client = NULL; + ping_receiver_ctx_t receiver_ctx; + pthread_t receiver_thread; + int receiver_ctx_initialized = 0; + int receiver_thread_started = 0; + ping_tracker_t tracker; + int i; + int rc = 1; + + ping_tracker_init(&tracker); + memset(&receiver_ctx, 0, sizeof(receiver_ctx)); + + for (i = 1; i < argc; ++i) { + const char *value = NULL; + int handled; + + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-id", &value)) < 0) { + fprintf(stderr, "udpping: flag -id requires a value\n"); + return 1; + } else if (handled) { + peer_id = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-server", &value)) < 0) { + fprintf(stderr, "udpping: flag -server requires a value\n"); + return 1; + } else if (handled) { + server_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-to", &value)) < 0) { + fprintf(stderr, "udpping: flag -to requires a value\n"); + return 1; + } else if (handled) { + target_peer = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-count", &value)) < 0) { + fprintf(stderr, "udpping: flag -count requires a value\n"); + return 1; + } else if (handled) { + count = atoi(value); + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-interval", &value)) < 0) { + fprintf(stderr, "udpping: flag -interval requires a value\n"); + return 1; + } else if (handled) { + if (omni_parse_duration_ms(value, interval_ms, &interval_ms) != 0) { + fprintf(stderr, "udpping: invalid -interval value %s\n", value); + return 1; + } + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-size", &value)) < 0) { + fprintf(stderr, "udpping: flag -size requires a value\n"); + return 1; + } else if (handled) { + size = atoi(value); + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-timeout", &value)) < 0) { + fprintf(stderr, "udpping: flag -timeout requires a value\n"); + return 1; + } else if (handled) { + if (omni_parse_duration_ms(value, timeout_ms, &timeout_ms) != 0) { + fprintf(stderr, "udpping: invalid -timeout value %s\n", value); + return 1; + } + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-bind-ip", &value)) < 0) { + fprintf(stderr, "udpping: flag -bind-ip requires a value\n"); + return 1; + } else if (handled) { + bind_ip = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-latency-log", &value)) < 0) { + fprintf(stderr, "udpping: flag -latency-log requires a value\n"); + return 1; + } else if (handled) { + latency_log_path = value; + continue; + } + if ((handled = cli_parse_bool_flag(argv[i], "-echo", &echo_mode)) < 0) { + fprintf(stderr, "udpping: invalid -echo value\n"); + return 1; + } else if (handled) { + continue; + } + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + udpping_usage(stdout); + return 0; + } + fprintf(stderr, "udpping: unknown argument %s\n", argv[i]); + udpping_usage(stderr); + return 1; + } + + if (peer_id[0] == '\0' || server_addr[0] == '\0') { + fprintf(stderr, "udpping: flags -id and -server are required\n"); + return 1; + } + if (!echo_mode && target_peer[0] == '\0') { + fprintf(stderr, "udpping: flag -to is required unless -echo is set\n"); + return 1; + } + if (count < 0 || interval_ms <= 0 || size <= 0 || timeout_ms <= 0) { + fprintf(stderr, "udpping: invalid numeric flag value\n"); + return 1; + } + + signal(SIGINT, udpping_on_signal); + + if (latency_log_path[0] != '\0') { + latency_logger = latencylog_open_jsonl(latency_log_path); + if (latency_logger == NULL) { + fprintf(stderr, "udpping: open latency logger %s failed\n", latency_log_path); + goto cleanup; + } + } + client = udp_client_dial(server_addr, peer_id, bind_ip, latency_logger, NULL, 0); + if (client == NULL) { + fprintf(stderr, "udpping: dial udp server %s failed\n", server_addr); + goto cleanup; + } + + if (echo_mode) { + while (!g_udpping_stop) { + message_t msg; + + protocol_message_init(&msg); + if (udp_client_receive(client, &msg) != 0) { + protocol_message_clear(&msg); + if (g_udpping_stop) { + break; + } + fprintf(stderr, "udpping: receive failed in echo mode\n"); + goto cleanup; + } + if (msg.type == MSG_TYPE_TEXT) { + char *text = (char *) malloc(msg.body_len + 1U); + if (text == NULL) { + protocol_message_clear(&msg); + goto cleanup; + } + memcpy(text, msg.body, msg.body_len); + text[msg.body_len] = '\0'; + if (udp_client_send_text(client, msg.from, text) != 0) { + free(text); + protocol_message_clear(&msg); + fprintf(stderr, "udpping: echo send back to %s failed\n", msg.from); + goto cleanup; + } + free(text); + } else if (msg.type == MSG_TYPE_ERROR) { + fprintf(stderr, "server error: %.*s\n", (int) msg.body_len, msg.body == NULL ? "" : (const char *) msg.body); + } else { + fprintf(stderr, "unexpected message type %s from %s ignored\n", protocol_message_type_name(msg.type), msg.from); + } + protocol_message_clear(&msg); + } + rc = 0; + goto cleanup; + } + + fprintf(stdout, "UDP PING %s via %s (payload=%d bytes, UDP)\n", target_peer, server_addr, size); + ping_receiver_ctx_init(&receiver_ctx, client); + receiver_ctx_initialized = 1; + if (pthread_create(&receiver_thread, NULL, udpping_receive_thread_main, &receiver_ctx) != 0) { + fprintf(stderr, "udpping: create receive thread failed\n"); + goto cleanup; + } + receiver_thread_started = 1; + + { + uint64_t next_seq = 1; + int stop_sending = 0; + int64_t next_send_at_ns = omni_now_unix_nano(); + int poll_ms = ping_expiry_poll_ms(timeout_ms); + int64_t timeout_ns = (int64_t) timeout_ms * 1000000LL; + + while (!g_udpping_stop || tracker.pending_count > 0 || !stop_sending) { + int64_t now_ns = omni_now_unix_nano(); + message_t msg; + int popped; + int receiver_closed; + int receiver_status_rc; + + if (!stop_sending && now_ns >= next_send_at_ns) { + char *payload = NULL; + size_t payload_len = 0; + + if (count > 0 && tracker.sent >= count) { + stop_sending = 1; + } else { + if (ping_build_payload(next_seq, now_ns, size, &payload, &payload_len) != 0) { + fprintf(stderr, "udpping: build payload for seq=%" PRIu64 " failed\n", next_seq); + free(payload); + goto cleanup; + } + if (udp_client_send_text(client, target_peer, payload) != 0) { + fprintf(stderr, "udpping: send ping seq=%" PRIu64 " failed\n", next_seq); + free(payload); + goto cleanup; + } + free(payload); + if (ping_tracker_mark_sent(&tracker, next_seq, now_ns, timeout_ns) != 0) { + goto cleanup; + } + next_seq++; + next_send_at_ns = now_ns + (int64_t) interval_ms * 1000000LL; + if (count > 0 && tracker.sent >= count) { + stop_sending = 1; + } + } + } + + ping_tracker_expire(&tracker, now_ns, stdout); + + do { + popped = ping_receiver_pop(&receiver_ctx, &msg); + if (popped == 1) { + if (msg.type == MSG_TYPE_TEXT) { + uint64_t seq; + int64_t sent_ts_ns; + int disposition; + int64_t rtt_ns; + + if (ping_parse_payload(msg.body, msg.body_len, &seq, &sent_ts_ns) != 0) { + fprintf(stderr, "ignore non-ping text message from %s\n", msg.from); + } else if (ping_tracker_observe_reply(&tracker, seq, sent_ts_ns, omni_now_unix_nano(), &disposition, &rtt_ns) != 0) { + protocol_message_clear(&msg); + goto cleanup; + } else if (disposition == 0) { + fprintf(stdout, "seq=%" PRIu64 " rtt=%.2fms\n", seq, (double) rtt_ns / 1000000.0); + } else if (disposition == 1) { + fprintf(stderr, "seq=%" PRIu64 " duplicate or late reply ignored\n", seq); + } else { + fprintf(stderr, "seq=%" PRIu64 " unexpected reply ignored\n", seq); + } + } else if (msg.type == MSG_TYPE_ERROR) { + fprintf(stderr, "server error: %.*s\n", (int) msg.body_len, msg.body == NULL ? "" : (const char *) msg.body); + } else { + fprintf(stderr, "unexpected message type %s from %s ignored\n", protocol_message_type_name(msg.type), msg.from); + } + protocol_message_clear(&msg); + } + } while (popped == 1); + + ping_receiver_status(&receiver_ctx, &receiver_closed, &receiver_status_rc); + if (receiver_closed && receiver_status_rc != 0) { + fprintf(stderr, "udpping: receive loop failed\n"); + goto cleanup; + } + if ((g_udpping_stop || stop_sending) && tracker.pending_count == 0) { + break; + } + usleep((useconds_t) poll_ms * 1000U); + } + } + + ping_print_summary(stdout, target_peer, &tracker); + rc = 0; + +cleanup: + receiver_ctx.stop_requested = 1; + udp_client_close(client); + if (receiver_thread_started) { + pthread_join(receiver_thread, NULL); + ping_receiver_ctx_destroy(&receiver_ctx); + } else if (receiver_ctx_initialized) { + ping_receiver_ctx_destroy(&receiver_ctx); + } + udp_client_free(client); + latencylog_close(latency_logger); + ping_tracker_destroy(&tracker); + return rc; +} diff --git a/host/OmniSocketGo_add_camera/cmd/udprelay.c b/host/OmniSocketGo_add_camera/cmd/udprelay.c new file mode 100644 index 0000000..57cf5b2 --- /dev/null +++ b/host/OmniSocketGo_add_camera/cmd/udprelay.c @@ -0,0 +1,59 @@ +#include "cli_parse.h" +#include "server_udp_relay.h" + +static void udprelay_usage(FILE *out) { + fprintf(out, "usage: udprelay [-listen addr] [-upstream addr]\n"); +} + +int main(int argc, char **argv) { + const char *listen_addr = ":9003"; + const char *upstream_addr = "127.0.0.1:9002"; + udp_relay_t *relay = NULL; + int i; + int rc = 1; + + for (i = 1; i < argc; ++i) { + const char *value = NULL; + int handled; + + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-listen", &value)) < 0) { + fprintf(stderr, "udprelay: flag -listen requires a value\n"); + return 1; + } else if (handled) { + listen_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-upstream", &value)) < 0) { + fprintf(stderr, "udprelay: flag -upstream requires a value\n"); + return 1; + } else if (handled) { + upstream_addr = value; + continue; + } + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + udprelay_usage(stdout); + return 0; + } + fprintf(stderr, "udprelay: unknown argument %s\n", argv[i]); + udprelay_usage(stderr); + return 1; + } + + relay = udp_relay_open(listen_addr, upstream_addr); + if (relay == NULL) { + fprintf(stderr, "udprelay: open relay %s -> %s failed\n", listen_addr, upstream_addr); + goto cleanup; + } + + fprintf(stderr, "udp relay listening on %s, upstream %s\n", listen_addr, upstream_addr); + if (udp_relay_serve(relay) != 0) { + fprintf(stderr, "udprelay: relay serve failed\n"); + goto cleanup; + } + + rc = 0; + +cleanup: + udp_relay_free(relay); + return rc; +} diff --git a/host/OmniSocketGo_add_camera/cmd/udpserver.c b/host/OmniSocketGo_add_camera/cmd/udpserver.c new file mode 100644 index 0000000..978fd36 --- /dev/null +++ b/host/OmniSocketGo_add_camera/cmd/udpserver.c @@ -0,0 +1,88 @@ +#include "cli_parse.h" +#include "server_udp_hub.h" + +static void udpserver_usage(FILE *out) { + fprintf(out, "usage: udpserver [-listen addr] [-latency-log path] [-tx-ts-debug-log path]\n"); +} + +int main(int argc, char **argv) { + const char *listen_addr = ":9001"; + const char *latency_log_path = ""; + const char *tx_debug_log_path = ""; + latency_logger_t *latency_logger = NULL; + tx_timestamp_debug_logger_t *debug_logger = NULL; + udp_hub_t *hub = NULL; + int enable_timestamping = 0; + int i; + int rc = 1; + + for (i = 1; i < argc; ++i) { + const char *value = NULL; + int handled; + + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-listen", &value)) < 0) { + fprintf(stderr, "udpserver: flag -listen requires a value\n"); + return 1; + } else if (handled) { + listen_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-latency-log", &value)) < 0) { + fprintf(stderr, "udpserver: flag -latency-log requires a value\n"); + return 1; + } else if (handled) { + latency_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-tx-ts-debug-log", &value)) < 0) { + fprintf(stderr, "udpserver: flag -tx-ts-debug-log requires a value\n"); + return 1; + } else if (handled) { + tx_debug_log_path = value; + continue; + } + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + udpserver_usage(stdout); + return 0; + } + fprintf(stderr, "udpserver: unknown argument %s\n", argv[i]); + udpserver_usage(stderr); + return 1; + } + + if (latency_log_path[0] != '\0') { + latency_logger = latencylog_open_jsonl(latency_log_path); + if (latency_logger == NULL) { + fprintf(stderr, "udpserver: open latency logger %s failed\n", latency_log_path); + goto cleanup; + } + } + if (tx_debug_log_path[0] != '\0') { + debug_logger = tx_timestamp_debug_open_jsonl(tx_debug_log_path); + if (debug_logger == NULL) { + fprintf(stderr, "udpserver: open tx timestamp debug logger %s failed\n", tx_debug_log_path); + goto cleanup; + } + enable_timestamping = 1; + } + + hub = udp_hub_open(listen_addr, latency_logger, debug_logger, enable_timestamping); + if (hub == NULL) { + fprintf(stderr, "udpserver: listen on %s failed\n", listen_addr); + goto cleanup; + } + + fprintf(stderr, "udp server listening on %s\n", listen_addr); + if (udp_hub_serve(hub) != 0) { + fprintf(stderr, "udpserver: serve failed\n"); + goto cleanup; + } + + rc = 0; + +cleanup: + udp_hub_free(hub); + tx_timestamp_debug_close(debug_logger); + latencylog_close(latency_logger); + return rc; +} diff --git a/host/OmniSocketGo_add_camera/cmd/v1_camera_pipeline_ifdef.c b/host/OmniSocketGo_add_camera/cmd/v1_camera_pipeline_ifdef.c new file mode 100644 index 0000000..093ee6e --- /dev/null +++ b/host/OmniSocketGo_add_camera/cmd/v1_camera_pipeline_ifdef.c @@ -0,0 +1,35 @@ +#include +#include + +#include "video_pipeline.h" + +int main(void) { + video_pipeline_config_t config; + video_pipeline_stats_t stats; + + video_pipeline_config_init(&config); + video_pipeline_config_load_env(&config); + if (getenv("OMNI_VIDEO_DEBUG_TIMING") == NULL) { + config.enable_timing_logs = 1; + } + if (video_pipeline_stats_init(&stats) != 0) { + perror("video_pipeline_stats_init"); + return 1; + } + + for (;;) { + int rc = video_pipeline_run(&config, &stats, NULL); + + if (rc == 0) { + break; + } + if (rc != VIDEO_PIPELINE_RUN_RETRY_IMMEDIATE) { + perror("video_pipeline_run"); + video_pipeline_stats_destroy(&stats); + return 1; + } + } + + video_pipeline_stats_destroy(&stats); + return 0; +} diff --git a/host/OmniSocketGo_add_camera/config/omnisocket_demo.yaml b/host/OmniSocketGo_add_camera/config/omnisocket_demo.yaml new file mode 100644 index 0000000..80542f0 --- /dev/null +++ b/host/OmniSocketGo_add_camera/config/omnisocket_demo.yaml @@ -0,0 +1,41 @@ +transport: + server_addr: "127.0.0.1:10909" + relay_via: "" + bind_ip: "" + bind_device: "" + +control_sender: + peer_id: "peer-a-ctrl" + target_peer: "peer-b-ctrl" + joy_topic: "/xbox_data" + deadzone: 0.10 + analog_epsilon: 0.01 + dpad_threshold: 0.50 + trigger_pressed_threshold: -0.50 + +control_receiver: + peer_id: "peer-b-ctrl" + +motion: + initial_lift: 0.89 + lift_step: 0.05 + max_surge: 1.0 + max_sway: 0.5 + max_spin: 0.5 + max_lift: 0.90 + min_lift: 0.65 + surge_step: 0.1 + sway_step: 0.1 + spin_step: 0.1 + +video_sender: + peer_id: "peer-b-video" + target_peer: "peer-a-video" + frame_bytes: 30720 + frame_interval_ms: 66 + +video_receiver: + peer_id: "peer-a-video" + # recv_into() requires a buffer large enough for the whole frame. + # If buffer_bytes is smaller than video_sender.frame_bytes, the oversize frame is dropped. + buffer_bytes: 65536 diff --git a/host/OmniSocketGo_add_camera/go/README.md b/host/OmniSocketGo_add_camera/go/README.md new file mode 100644 index 0000000..770f75f --- /dev/null +++ b/host/OmniSocketGo_add_camera/go/README.md @@ -0,0 +1,94 @@ +# OmniSocketGo + +Linux only. Go 1.22. + +如果目标机器只运行 `server`，只需要编译并拷贝 `server` 二进制。 +如果目标机器只运行 `peer`，只需要编译并拷贝 `peer` 二进制。 + +`go build ./cmd/server` 和 `go build ./cmd/peer` 会把各自依赖到的功能一起编译进最终二进制，不需要再单独编译 `cmd/internal/...` 包。 + +- `server` 二进制会包含它依赖到的转发、协议、传输等代码 +- `peer` 二进制会包含它依赖到的注册、交互发送、接收落盘、协议、传输等代码 +- 只有没有被这个可执行程序引用的其他命令，才不在该二进制里，比如 `cmd/latencysummary` + +## Build + +按目标架构分别编译。 + mkdir -p bin + go build -o bin/server ./cmd/server + go build -o bin/peer ./cmd/peer + go build -o bin/latencysummary ./cmd/latencysummary + +### Linux amd64 + +```bash +CGO_ENABLED=0 GOOS=linux GOARCH=amd64 go build -o bin/server-linux-amd64 ./cmd/server +``` + +### Linux arm64 + +```bash +CGO_ENABLED=0 GOOS=linux GOARCH=arm64 go build -o bin/peer-linux-arm64 ./cmd/peer +``` + +### Linux armv7 + +```bash +CGO_ENABLED=0 GOOS=linux GOARCH=arm GOARM=7 go build -o bin/server-linux-armv7 ./cmd/server +CGO_ENABLED=0 GOOS=linux GOARCH=arm GOARM=7 go build -o bin/peer-linux-armv7 ./cmd/peer +``` + + + +## Run On Different Machines + +`server D` 所在机器监听 `0.0.0.0:10909`。 + +```bash +go run cmd/kcpserver/ -listen 0.0.0.0:10909 +-kcp-ts-debug-log logs/d-kcp-ts.jsonl -kcp-session-stats-log logs/d-kcp-stats.jsonl +``` + +`relay server C` 所在机器 + +```bash +go run ./cmd/kcpserver/ -mode=relay -listen 0.0.0.0:10909 -relay-remote 172.21.32.15:10909 + +2>&1 | tee logs/c.stdout.log +``` + +### peer-a (A) + +```bash +go run ./cmd/kcppeer/ -id peer-a -server 172.21.32.15:10909 -relay-via 106.55.173.235:10909 -inbox-dir inbox/a + +-latency-log logs/a-latency.jsonl -kcp-ts-debug-log logs/a-kcp-ts.jsonl -kcp-session-stats-log logs/a-kcp-stats.jsonl + +go run ./cmd/kcpping/ -id peer-a -server 106.55.173.235:10909 -echo +``` + +### peer-b (B) + +```bash +go run ./cmd/kcppeer/ -id peer-b -server 81.70.156.140:10909 -inbox-dir inbox/b + +-latency-log logs/b-latency.jsonl -kcp-ts-debug-log logs/b-kcp-ts.jsonl -kcp-session-stats-log logs/b-kcp-stats.jsonl + +go run ./cmd/kcpping -id peer-b -server 81.70.156.140:10909 -to peer-a -count 20 -interval 100ms +``` + +## Interactive Commands + +`peer` 启动后可以在终端里持续使用同一条长连接发送多次消息。 + +```text +help +text peer-b hello +text peer-a hi +file peer-a /tmp/test125.bin +file peer-a /tmp/test5.bin +quit +``` +### 自动化拉取更新汇总数据 +cd /home/limingjie/LMJ_Work/OmniSocketGo +./scripts/refresh-latency-summary.sh \ No newline at end of file diff --git a/host/OmniSocketGo_add_camera/go/change_to_c.md b/host/OmniSocketGo_add_camera/go/change_to_c.md new file mode 100644 index 0000000..675c8a4 --- /dev/null +++ b/host/OmniSocketGo_add_camera/go/change_to_c.md @@ -0,0 +1,465 @@ +OmniSocketGo -> OmniSocketC 转换计划 + + Context + + 将现有的 Go 语言实现的 UDP/KCP 传输层项目 (OmniSocketGo) 转换为纯 C 语言项目，运行在 Linux 系统上。 + + 原项目架构：A(Jetson) <-> C(relay cloud) <-> D(hub cloud) <-> B(host) + - B <-> D：KCP 链路 + - D <-> C：UDP relay 转发 + - C <-> A：KCP 链路（A 通过 relay C 连接到 hub D） + - 最终目的：B 和 A 之间双向传输数据 + + 转换要求： + - 只保留 UDP 和 KCP，不需要 TCP + - 不需要写测试 + - 完整实现协议层、传输层、日志事件系统 + - Linux only + + 项目位置 + + OmniSocketGo/c/ — 作为当前 Go 项目的子目录 + + 项目结构 + + c/ + ├── Makefile + ├── README.md + ├── include/ + │ ├── protocol.h # 协议消息定义 + 编解码 + │ ├── transport_kcp.h # KCP 连接封装 + │ ├── transport_udp.h # UDP 连接封装（含 Linux timestamping） + │ ├── linux_timestamping.h # Linux SO_TIMESTAMPING 底层实现 + │ ├── kcp_packet_debug.h # KCP packet-level kernel timestamp debug logger + │ ├── kcp_session_stats.h # KCP session stats (RTO/SRTT) logger + │ ├── tx_timestamp_debug.h # TX errqueue timestamp debug logger + │ ├── server_kcp_hub.h # KCP Hub (D 节点) + │ ├── server_udp_relay.h # UDP Relay (C 节点) + │ ├── peer_kcp_client.h # KCP Peer Client (A/B 节点) + │ ├── latencylog.h # 延迟日志事件系统 + │ ├── interactive.h # 交互式命令行 + │ └── cJSON.h # JSON 库 (第三方轻量级) + ├── src/ + │ ├── protocol.c + │ ├── transport_kcp.c + │ ├── transport_udp.c + │ ├── linux_timestamping.c + │ ├── kcp_packet_debug.c + │ ├── kcp_session_stats.c + │ ├── tx_timestamp_debug.c + │ ├── server_kcp_hub.c + │ ├── server_udp_relay.c + │ ├── peer_kcp_client.c + │ ├── latencylog.c + │ ├── interactive.c + │ └── cJSON.c + ├── cmd/ + │ ├── kcpserver.c # 主程序: KCP Hub 或 UDP Relay + │ ├── kcppeer.c # 主程序: KCP Peer (A/B) + │ └── kcpping.c # 主程序: KCP Ping 工具 + └── third_party/ + └── kcp/ + ├── ikcp.h # KCP 协议核心实现 (github.com/skywind3000/kcp) + └── ikcp.c + + 依赖说明 + + - KCP: 使用 skywind3000/kcp 的原始 C 实现 (ikcp.h/ikcp.c)，替代 Go 的 xtaci/kcp-go/v5 + - JSON: 使用 cJSON (DaveGamble/cJSON) 替代 Go 的 encoding/json + - 线程: 使用 pthread 替代 Go goroutine + - 同步: 使用 pthread_mutex/pthread_rwlock 替代 Go sync.Mutex/sync.RWMutex + + 模块实现计划 + + 1. 第三方库集成 + + - 下载 ikcp.h/ikcp.c (skywind3000/kcp) + - 下载 cJSON.h/cJSON.c (DaveGamble/cJSON) + + 2. protocol.h / protocol.c + + 对应 Go: cmd/internal/protocol/message.go + codec.go + + // 消息类型 + typedef enum { + MSG_TYPE_TEXT = 0, + MSG_TYPE_FILE = 1, + MSG_TYPE_REGISTER = 2, + MSG_TYPE_ERROR = 3, + } message_type_t; + + // 消息结构 + typedef struct { + message_type_t type; + uint64_t id; + char from[64]; + char to[64]; + char file_name[256]; + uint8_t *body; + int body_len; + } message_t; + + #define MAX_FRAME_SIZE (8 * 1024 * 1024) + #define SERVER_PEER_ID "server" + + 核心函数： + - int protocol_encode_message(const message_t *msg, uint8_t **out, int *out_len) — 编码消息为 [4B headerLen][header JSON][body] + - int protocol_decode_message(const uint8_t *data, int data_len, message_t *msg) — 解码 + - int protocol_write_frame(int fd, const uint8_t *payload, int payload_len) — 写带长度前缀的帧 (用于 KCP stream) + - int protocol_read_frame(int fd, uint8_t **payload, int *payload_len) — 读帧 + - int protocol_write_message(int fd, const message_t *msg) — 完整编码+写帧 + - int protocol_read_message(int fd, message_t *msg) — 读帧+解码 + - int protocol_validate_message(const message_t *msg) — 校验 + - void message_free(message_t *msg) — 释放 body 内存 + + 注意: KCP session 在 stream 模式下行为类似 TCP，需要 [4B frameLen] 前缀来分帧。 + + 3. latencylog.h / latencylog.c + + 对应 Go: cmd/internal/latencylog/logger.go + + // 事件名常量 + #define EVENT_A_APP_PREP_BEGIN "A_APP_PREP_BEGIN" + #define EVENT_SEND_HANDOFF_BEGIN "send_handoff_begin" + #define EVENT_SEND_HANDOFF_END "send_handoff_end" + #define EVENT_B_APP_RECV "B_APP_RECV" + #define EVENT_B_PERSIST_BEGIN "B_PERSIST_BEGIN" + #define EVENT_B_PERSIST_END "B_PERSIST_END" + // ... 其他事件 + + typedef struct { + int64_t ts_unix_nano; + char node_role[16]; + char node_id[64]; + char event[32]; + message_type_t message_type; + uint64_t message_id; + char from[64]; + char to[64]; + char file_name[256]; + int body_size; + } latency_event_t; + + typedef struct latency_logger latency_logger_t; + + 核心函数： + - latency_logger_t *latencylog_new_jsonl(const char *path) — 创建 JSONL 文件日志器 + - void latencylog_log_event(latency_logger_t *logger, const latency_event_t *event) — 写事件 + - void latencylog_log_message_event(latency_logger_t *logger, const char *node_role, const char *node_id, const char *event_name, const message_t + *msg) — 为业务消息记事件 + - void latencylog_close(latency_logger_t *logger) — 关闭 + - int latencylog_is_business_message(const message_t *msg) — 判断是否业务消息 + + 4. transport_kcp.h / transport_kcp.c + + 对应 Go: cmd/internal/transport/kcp.go + kcp_packet_conn.go + + KCP 连接封装，底层用 raw ikcp + UDP socket： + + typedef struct kcp_conn { + ikcpcb *kcp; + int udp_fd; + struct sockaddr_in remote_addr; + pthread_mutex_t write_mu; + pthread_t recv_thread; // 底层 UDP -> ikcp_input 的线程 + latency_logger_t *logger; + char node_role[16]; + char node_id[64]; + int closed; + } kcp_conn_t; + + 核心函数： + - kcp_conn_t *kcp_conn_dial(const char *server_addr, const char *bind_ip, const char *bind_device) — 客户端拨号 + - kcp_conn_t *kcp_conn_accept(int udp_fd, struct sockaddr_in *remote, uint32_t conv) — 服务端接受 + - int kcp_conn_send(kcp_conn_t *conn, const message_t *msg) — 发送消息 + - int kcp_conn_receive(kcp_conn_t *conn, message_t *msg) — 接收消息 + - void kcp_conn_close(kcp_conn_t *conn) — 关闭 + + KCP 配置参数（与 Go 版一致）： + #define KCP_NODELAY 1 + #define KCP_INTERVAL 10 + #define KCP_RESEND 2 + #define KCP_NC 1 + #define KCP_WND_SIZE 256 + #define KCP_MTU 1400 + + KCP 底层架构说明： + Go 版使用 kcp-go 库，该库内部维护了一个 Listener 来多路复用一个 UDP socket 上的多个 KCP session（通过 conv ID 区分）。在 C 中需要自行实现： + - 服务端：一个 UDP socket 监听，一个接收线程读取所有 UDP 包，根据 conv ID 分发到对应的 ikcpcb + - 客户端：一个 UDP socket，一个 ikcpcb，一个后台线程负责 UDP recv -> ikcp_input + + 5. transport_udp.h / transport_udp.c + + 对应 Go: cmd/internal/transport/udp.go + udp_linux.go + + typedef struct udp_conn { + int fd; + struct sockaddr_in peer_addr; + syscall_rawconn_t raw; // syscall.RawConn 等价 + int linux_timestamping_enabled; + latency_logger_t *logger; + tx_timestamp_debug_logger_t *tx_debug_logger; + uint32_t tx_packet_seq; + // pending TX records for errqueue correlation + struct udp_tx_pending *pending_tx; + char node_role[16]; + char node_id[64]; + pthread_mutex_t write_mu; + } udp_conn_t; + + 完整实现 Linux SO_TIMESTAMPING： + - TX: SOF_TIMESTAMPING_TX_SCHED + SOF_TIMESTAMPING_TX_SOFTWARE + OPT_ID + - RX: SOF_TIMESTAMPING_RX_SOFTWARE + - errqueue 采集: recvmsg(MSG_ERRQUEUE) 读取 SCM_TIMESTAMPING 控制消息 + - TX timestamp debug logger: 记录 send_chunk / errqueue_event 到 JSONL + - 对应 Go 文件: udp_linux.go, tx_timestamp_debug.go + + 同时为 KCP packet conn 实现类似的 timestamping： + - 对应 Go 文件: kcp_packet_conn_linux.go, kcp_packet_debug.go + - KCP 底层 UDP 包的 TX/RX kernel timestamp 记录 + + KCP session stats 完整实现： + - session-level: conv, RTO, SRTT, SRTTVar 周期采样 + - 对应 Go 文件: kcp_session_stats.go + + 6. server_kcp_hub.h / server_kcp_hub.c + + 对应 Go: cmd/internal/server/kcp_hub.go + + typedef struct { + pthread_rwlock_t lock; + // peer_id -> kcp_conn_t* 的哈希表 + struct peer_entry *peers; // 简单链表或哈希表 + int peer_count; + latency_logger_t *logger; + // relay 相关 + int relay_udp_fd; + struct sockaddr_in relay_peer_addr; + int relay_peer_known; + } kcp_hub_t; + + 核心函数： + - kcp_hub_t *kcp_hub_new(latency_logger_t *logger) — 创建 hub + - int kcp_hub_serve_session(kcp_hub_t *hub, kcp_conn_t *conn) — 处理新会话（注册 + 转发循环） + - void kcp_hub_set_relay(kcp_hub_t *hub, int udp_fd, struct sockaddr_in *peer_addr) — 配置 relay + - int kcp_hub_serve_relay(kcp_hub_t *hub) — relay 接收循环 + - void kcp_hub_free(kcp_hub_t *hub) — 释放 + + 服务端 KCP listener 实现： + - 主 UDP socket 监听 + - 收到新 conv ID 时创建新 ikcpcb + - 用 pthread 为每个 session 创建处理线程 + + 7. server_udp_relay.h / server_udp_relay.c + + 对应 Go: cmd/internal/server/udp_relay.go + + typedef struct { + int downstream_fd; // 监听端 + int upstream_fd; // 连接到 hub D 的 UDP + struct sockaddr_in upstream_addr; + struct sockaddr_in client_addr; + int client_known; + pthread_mutex_t lock; + } udp_relay_t; + + 核心函数： + - udp_relay_t *udp_relay_new(int listen_fd, struct sockaddr_in *upstream_addr) — 创建 + - int udp_relay_serve(udp_relay_t *relay) — 双向转发循环（两个线程） + - void udp_relay_close(udp_relay_t *relay) — 关闭 + + 8. peer_kcp_client.h / peer_kcp_client.c + + 对应 Go: cmd/internal/peer/kcp_client.go + persist.go + + typedef struct { + char id[64]; + kcp_conn_t *conn; + latency_logger_t *logger; + uint64_t next_msg_id; // atomic + pthread_mutex_t id_mu; + } kcp_client_t; + + 核心函数： + - kcp_client_t *kcp_client_dial(const char *server_addr, const char *peer_id, ...) — 连接并注册 + - int kcp_client_send_text(kcp_client_t *c, const char *to, const char *text) — 发文本 + - int kcp_client_send_file(kcp_client_t *c, const char *to, const char *path) — 发文件 + - int kcp_client_receive(kcp_client_t *c, message_t *msg) — 接收 + - int kcp_client_persist_message(kcp_client_t *c, const message_t *msg, const char *inbox_dir) — 持久化 + - void kcp_client_close(kcp_client_t *c) — 关闭 + + 9. interactive.h / interactive.c + + 对应 Go: cmd/kcppeer/interactive.go + + 交互式命令行 REPL： + - help / text / file / quit + - int run_interactive_shell(kcp_client_t *client) — 运行交互循环 + + 10. cmd/kcpserver.c + + 对应 Go: cmd/kcpserver/main.go + + 用法: + kcpserver -listen 0.0.0.0:10909 # hub 模式 + kcpserver -mode relay -listen 0.0.0.0:10909 -relay-remote 172.21.32.15:10909 # relay 模式 + + - 解析命令行参数 (getopt) + - hub 模式：创建 KCP listener -> 接受连接 -> kcp_hub_serve_session + - relay 模式：创建 UDP relay -> udp_relay_serve + + 11. cmd/kcppeer.c + + 对应 Go: cmd/kcppeer/main.go + + 用法: + kcppeer -id peer-a -server 172.21.32.15:10909 -relay-via 106.55.173.235:10909 -inbox-dir inbox/a + kcppeer -id peer-b -server 81.70.156.140:10909 -inbox-dir inbox/b + + - 连接到 KCP server + - 启动接收线程 + - 运行交互式 shell 或单次发送 + + 12. cmd/kcpping.c + + 对应 Go: cmd/kcpping/main.go + platform_linux.go + + KCP ping 工具： + - ping 模式: 发 JSON payload, 计算 RTT + - echo 模式: 回弹文本消息 + - 统计: min/avg/max/p50/p95/p99/stddev + + KCP session 多路复用实现（核心难点） + + Go 版的 kcp-go 库在一个 UDP socket 上透明地多路复用多个 KCP session。C 版需要手动实现： + + typedef struct kcp_listener { + int udp_fd; + pthread_t recv_thread; + pthread_mutex_t sessions_lock; + // conv -> kcp_session 的哈希表 + struct kcp_session_entry *sessions; + // 新会话通知队列 + kcp_conn_t **accept_queue; + int accept_queue_head, accept_queue_tail, accept_queue_cap; + pthread_mutex_t accept_lock; + pthread_cond_t accept_cond; + } kcp_listener_t; + + - kcp_listener_t *kcp_listen(const char *addr, const char *bind_device) — 创建 listener + - kcp_conn_t *kcp_accept(kcp_listener_t *listener) — 阻塞等待新会话 + - 内部 recv_thread 循环读 UDP 包，解析前 4 字节 conv ID，分发到对应 ikcpcb + - 未知 conv ID 时创建新 session 并放入 accept_queue + + 编译 + + CC = gcc + CFLAGS = -Wall -Wextra -O2 -pthread -D_GNU_SOURCE + LDFLAGS = -lpthread + + SRCS = src/protocol.c src/transport_kcp.c src/transport_udp.c \ + src/server_kcp_hub.c src/server_udp_relay.c \ + src/peer_kcp_client.c src/latencylog.c src/interactive.c \ + src/cJSON.c third_party/kcp/ikcp.c + + all: kcpserver kcppeer kcpping + + kcpserver: cmd/kcpserver.c $(SRCS) + $(CC) $(CFLAGS) -Iinclude -Ithird_party/kcp -o $@ $^ $(LDFLAGS + + kcppeer: cmd/kcppeer.c $(SRCS) + $(CC) $(CFLAGS) -Iinclude -Ithird_party/kcp -o $@ $^ $(LDFLAGS + + kcpping: cmd/kcpping.c $(SRCS) + $(CC) $(CFLAGS) -Iinclude -Ithird_party/kcp -o $@ $^ $(LDFLAGS + + 验证方法 + + 1. 编译: make all 无错误无警告 + 2. 单机测试: + - 启动 hub: ./kcpserver -listen 0.0.0.0:10909 + - 启动 peer-a: ./kcppeer -id peer-a -server 127.0.0.1:10909 -inbox-dir inbox/a + - 启动 peer-b: ./kcppeer -id peer-b -server 127.0.0.1:10909 -inbox-dir inbox/b + - peer-b shell 中: text peer-a hello + - 验证 peer-a 收到消息并落盘到 inbox/a/ + 3. 跨机器 relay 测试: + - D 机器: ./kcpserver -listen 0.0.0.0:10909 + - C 机器: ./kcpserver -mode relay -listen 0.0.0.0:10909 -relay-remote :10909 + - A 机器: ./kcppeer -id peer-a -server :10909 -relay-via :10909 -inbox-dir inbox/a + - B 机器: ./kcppeer -id peer-b -server :10909 -inbox-dir inbox/b + 4. kcpping 测试: + - echo 端: ./kcpping -id peer-a -server :10909 -echo + - ping 端: ./kcpping -id peer-b -server :10909 -to peer-a -count 20 -interval 100 + + 实现顺序 + + 1. 集成第三方库 (ikcp, cJSON) + 2. protocol 模块 (消息编解码) + 3. latencylog 模块 (日志事件) + 4. transport_kcp 模块 (KCP 连接 + listener 多路复用) + 5. transport_udp 模块 (UDP 连接，简化 timestamping) + 6. server_udp_relay 模块 (C 节点 relay) + 7. server_kcp_hub 模块 (D 节点 hub) + 8. peer_kcp_client 模块 (A/B 节点 peer + persist) + 9. interactive 模块 (交互 shell) + 10. cmd/kcpserver.c 主程序 + 11. cmd/kcppeer.c 主程序 + 12. cmd/kcpping.c 主程序 + 13. Makefile + README + 14. 编译测试 + + 简化决策 + + - 不实现 TCP 传输: 去除 transport/tcp.go, server/hub.go(TCP版), peer/client.go(TCP版) 等 TCP 相关代码 + - 不写测试: 去除所有 _test.go 对应的测试代码 + - 完整实现 Linux timestamping: 完整移植 SO_TIMESTAMPING 的 TX/RX timestamp 采集，包括 errqueue TX sched/software timestamp 和 RX software + timestamp，以及对应的 debug logger (KCPPacketDebugLogger, TXTimestampDebugLogger) + - 完整实现 KCP session stats: 包括 session-level RTO/SRTT 采样和 JSONL 记录 + - 不实现 latency summary/chart: 不实现 latencysummary 工具和 HTML chart 生成（这是离线分析工具，不属于核心传输功能） + - peer 哈希表: 使用简单链表实现，hub 连接数不多时性能足够 + + +# OmniSocketGo -> OmniSocketC 全量 UDP/KCP 迁移计划 + +## Summary +- 在仓库新增 `c/` 子项目，作为 Linux-only、C11、`make` 驱动的独立实现；现有 Go 项目保留不动，作为行为对照。 +- 迁移范围按“全量 Go 对齐，但去掉 TCP 和离线 summary/chart”执行：保留 UDP/KCP 协议、纯 UDP 程序族、KCP 程序族、运行时 JSONL 日志、Linux timestamping、KCP packet debug、KCP session stats、以及 KCP hub-to-hub 内部 relay 能力。 +- 你当前草案需要修正的关键点有 5 个：`protocol_*frame(int fd, ...)` 不适合 KCP；KCP 必须补齐 `ikcp_update/check` 调度与 conv 多路复用；纯 UDP 程序族不能省略；`latencysummary`/HTML chart 本次不迁移；Makefile 需要修正链接目标并统一输出到 `c/bin/`。 + +## Public Interfaces +- 新增二进制：`kcpserver`、`kcppeer`、`kcpping`、`udpserver`、`udppeer`、`udpping`、`udprelay`。 +- `kcpserver` 保留当前 Go 旗标语义：`-mode=hub|relay`、`-listen`、`-bind-device`、`-relay-remote`、deprecated relay aliases、`-latency-log`、`-kcp-ts-debug-log`、`-kcp-session-stats-log`、`-kcp-session-stats-interval`。 +- `kcppeer` 保留当前 Go 旗标语义：`-id`、`-server`、`-relay-via`、`-to`、`-text`、`-file`、`-bind-ip`、`-bind-device`、`-inbox-dir`、`-interactive`、`-latency-log`、`-kcp-ts-debug-log`、`-kcp-session-stats-log`、`-kcp-session-stats-interval`。 +- `kcpping`、`udpserver`、`udppeer`、`udpping`、`udprelay` 的参数与输出行为对齐当前 Go 入口；`udpserver` 默认不开 Linux timestamping，只有设置 `-tx-ts-debug-log` 时才启用。 +- 协议层改为内存接口，不再设计 fd 风格 API：`message_t`、datagram 编解码、stream frame 编解码、增量 frame feed。 +- 运行时日志层保留当前 JSON 字段和事件名；server/hub 继续作为 black-box relay，不新增端到端业务事件。 +- 内部网络 API 包括：`udp_conn_t`、`kcp_conn_t`、`kcp_listener_t`、`udp_hub_t`、`kcp_hub_t`、`udp_relay_t`、`udp_client_t`、`kcp_client_t`；KCP hub-to-hub relay 只做库级能力，不新增额外 CLI。 + +## Implementation Changes +- 目录固定为 `c/include`、`c/src`、`c/cmd`、`c/third_party/{ikcp,cjson}`、`c/bin`、`c/README.md`、`c/Makefile`。 +- 第三方依赖直接 vendoring 到仓库：`ikcp` 用于 KCP 核心，`cJSON` 同时用于协议头、ping payload、运行时日志。 +- 协议规则完全保留：`text/file/register/error`、`ServerPeerID`、`8 MiB` 限制、UTF-8 校验、`file_name` 约束、`register/error` 来源与目标约束。 +- 线上 wire format 完全保留：UDP datagram 为 `[4B headerLen][header JSON][body]`；KCP stream 为 `[4B frameLen][4B headerLen][header JSON][body]`。 +- inbox 持久化完全保留：文本追加写 `messages.log` JSONL；文件落盘为 `--`。 +- UDP 传输层实现 connected/unconnected 两种发送模式，保留 register/forward 消息收发、Linux SO_TIMESTAMPING、TX errqueue 关联、JSONL debug 记录。 +- KCP 客户端连接采用“一连接一 UDP socket + 一 `ikcpcb` + 一接收线程 + 一 update 线程 + 一阻塞接收缓冲区/条件变量”模型。 +- KCP 服务端监听采用“单 listener UDP socket + 单 listener RX 线程 + conv->session 表 + accept 队列”模型；每个 session 拥有自己的 `ikcpcb`、update 线程、接收缓冲区和关闭状态，发送通过 listener 共享 socket 和写锁完成。 +- `kcpserver` 的 relay 模式保持为原始 UDP 端口转发，不解码协议；`udprelay` 同样保持透明字节转发。 +- 纯 UDP hub、KCP hub、双 peer、双 ping 工具、两套 interactive shell 全部对齐现有 Go 行为。 +- KCP hub 保留“先本地投递，再尝试 relay”的策略；未知目标、重复注册、已注册 peer 再发 `register/error`、过大 relay 消息等错误路径全部保留。 +- Linux 观测能力完整迁移：业务事件 JSONL、UDP TX debug、KCP packet debug、KCP session/process stats；不迁移 `latencysummary` 与 HTML chart。 + +## Acceptance +- 在 Linux 上执行 `make` 能无缺失符号地构建 7 个二进制，并输出到 `c/bin/`。 +- 纯 UDP 冒烟通过：`udpserver` + 两个 `udppeer` 可双向收发文本和文件，`udpping` 的 echo/ping 正常。 +- 单 hub KCP 冒烟通过：`kcpserver` + 两个 `kcppeer` 可双向收发文本和文件，`kcpping` 的 echo/ping 正常。 +- README 目标拓扑通过：D 跑 `kcpserver -mode=hub`，C 跑 `kcpserver -mode=relay`，A 用 `-relay-via C` 连 D，B 直连 D，A/B 双向传输正常。 +- 全量 Go 对齐场景通过：两个 KCP hub 通过内部 raw UDP relay API 互通，跨 hub 文本、文件、错误回送行为与当前 Go 一致。 +- 负路径通过：重复注册被拒、未注册 UDP sender 被拒、未知目标返回 `error`、已注册 peer 发送 `register/error` 被拒、oversize relayed message 在实际 `WriteTo` 前被拒、`bind-ip`/`bind-device` 非法值在启动时失败。 +- 打开任一日志旗标后，生成的 JSONL 记录字段名、事件名、时间戳语义与现有运行时日志一致，并在 Linux 支持的情况下出现非零 kernel timestamps。 + +## Assumptions +- 默认编译器为 `gcc`/`clang`，编译参数基线为 `-std=c11 -Wall -Wextra -O2 -pthread -D_GNU_SOURCE`。 +- 本次不迁移任何 Go 测试文件，也不为 C 版编写自动化测试；验证仅靠 Linux 构建和手工场景回归。 +- 本次不迁移 TCP 入口，也不迁移 `latencysummary`/HTML chart。 +- hub-to-hub relay 在 C 版中实现为内部库能力，保持与当前 Go 仓库一致的范围，不额外扩展新的公共命令。 diff --git a/host/OmniSocketGo_add_camera/go/cmd/internal/latencylog/logger.go b/host/OmniSocketGo_add_camera/go/cmd/internal/latencylog/logger.go new file mode 100644 index 0000000..f1f1f31 --- /dev/null +++ b/host/OmniSocketGo_add_camera/go/cmd/internal/latencylog/logger.go @@ -0,0 +1,166 @@ +package latencylog + +import ( + "encoding/json" + "os" + "path/filepath" + "sync" + "time" + + "omnisocketgo/cmd/internal/protocol" +) + +const ( + NodeRolePeer = "peer" //客户端节点 + NodeRoleServer = "server" //云端转发节点 +) + +// 记录的消息事件的类型常量。 +const ( + EventAAppPrepBegin = "A_APP_PREP_BEGIN" // A 端应用开始准备这条消息 + EventATXSched = "A_TX_SCHED" // A 端进入 Linux qdisc 之前 + EventATXSoftware = "A_TX_SOFTWARE" // A 端即将交给网卡驱动 + EventATXHardware = "A_TX_HARDWARE" // A 端网卡真正发出到物理介质 + EventBRXHardware = "B_RX_HARDWARE" // B 端网卡真正从物理介质收到 + EventBRXSoftware = "B_RX_SOFTWARE" // B 端驱动把数据交给 Linux 接收栈 + EventBAppRecv = "B_APP_RECV" // B 端应用真正读到完整消息 + EventBPersistBegin = "B_PERSIST_BEGIN" // B 端开始写盘 + EventBPersistEnd = "B_PERSIST_END" // B 端写盘完成 + + EventSendHandoffBegin = "send_handoff_begin" // 调试事件：应用把消息交给传输层开始 + EventSendHandoffEnd = "send_handoff_end" // 调试事件：应用把消息交给传输层结束 +) + +// Event 是一条时延时间戳日志记录。 +type Event struct { + TsUnixNano int64 `json:"ts_unix_nano"` + NodeRole string `json:"node_role"` + NodeID string `json:"node_id"` + Event string `json:"event"` + MessageType protocol.MessageType `json:"message_type"` + MessageID uint64 `json:"message_id"` + From string `json:"from"` + To string `json:"to"` + FileName string `json:"file_name,omitempty"` + BodySize int `json:"body_size"` +} + +// Logger 负责接收事件并将其写入外部介质。 +type Logger interface { + LogEvent(Event) error +} + +// NoopLogger 是默认的空实现。 +type NoopLogger struct{} + +// LogEvent 对空日志实现始终返回 nil。 +func (NoopLogger) LogEvent(Event) error { + return nil +} + +// JSONLLogger 以 JSONL 形式追加写日志文件。 +type JSONLLogger struct { + mu sync.Mutex + closeOnce sync.Once + closeErr error + file *os.File +} + +// NewJSONLLogger 创建一个线程安全的 JSONL 文件日志器。 +func NewJSONLLogger(path string) (*JSONLLogger, error) { + dir := filepath.Dir(path) + if err := os.MkdirAll(dir, 0o755); err != nil { + return nil, err + } + + file, err := os.OpenFile(path, os.O_CREATE|os.O_WRONLY|os.O_APPEND, 0o644) + if err != nil { + return nil, err + } + + return &JSONLLogger{file: file}, nil +} + +// LogEvent 以单行 JSON 的形式追加一条事件。 +func (l *JSONLLogger) LogEvent(event Event) error { + line, err := json.Marshal(event) + if err != nil { + return err + } + + l.mu.Lock() + defer l.mu.Unlock() + + if _, err := l.file.Write(append(line, '\n')); err != nil { + return err + } + + return nil +} + +// Close 关闭底层文件；重复调用是安全的。 +func (l *JSONLLogger) Close() error { + l.closeOnce.Do(func() { + l.closeErr = l.file.Close() + }) + + return l.closeErr +} + +// IsBusinessMessage 判断消息是否属于要参与 A-C-B 时延分析的业务消息。 +func IsBusinessMessage(msg protocol.Message) bool { + switch msg.Type { + case protocol.MessageTypeText, protocol.MessageTypeFile: + return true + default: + return false + } +} + +// NewMessageEvent 用当前 UTC 时间为一条业务消息构造事件。 +func NewMessageEvent(nodeRole, nodeID, eventName string, msg protocol.Message) Event { + return NewMessageEventAt(time.Now().UTC().UnixNano(), nodeRole, nodeID, eventName, msg) +} + +// NewMessageEventAt 用指定的 UnixNano 时间为一条业务消息构造事件。 +func NewMessageEventAt(tsUnixNano int64, nodeRole, nodeID, eventName string, msg protocol.Message) Event { + return Event{ + TsUnixNano: tsUnixNano, + NodeRole: nodeRole, + NodeID: nodeID, + Event: eventName, + MessageType: msg.Type, + MessageID: msg.ID, + From: msg.From, + To: msg.To, + FileName: msg.FileName, + BodySize: len(msg.Body), + } +} + +// LogBestEffort 写一条事件，失败时静默忽略，避免打断主收发流程。 +func LogBestEffort(logger Logger, event Event) { + if logger == nil { + return + } + + _ = logger.LogEvent(event) +} + +// LogMessageEvent 为业务消息构造并写入一条事件。 +func LogMessageEvent(logger Logger, nodeRole, nodeID, eventName string, msg protocol.Message) { + if !IsBusinessMessage(msg) { + return + } + + LogBestEffort(logger, NewMessageEvent(nodeRole, nodeID, eventName, msg)) +} + +// LogMessageEventAt 为业务消息写入一条指定时间戳的事件。 +func LogMessageEventAt(logger Logger, nodeRole, nodeID, eventName string, tsUnixNano int64, msg protocol.Message) { + if !IsBusinessMessage(msg) { + return + } + + LogBestEffort(logger, NewMessageEventAt(tsUnixNano, nodeRole, nodeID, eventName, msg)) +} diff --git a/host/OmniSocketGo_add_camera/go/cmd/internal/latencylog/logger_test.go b/host/OmniSocketGo_add_camera/go/cmd/internal/latencylog/logger_test.go new file mode 100644 index 0000000..d1850fe --- /dev/null +++ b/host/OmniSocketGo_add_camera/go/cmd/internal/latencylog/logger_test.go @@ -0,0 +1,131 @@ +package latencylog + +import ( + "bufio" + "encoding/json" + "os" + "path/filepath" + "sync" + "testing" + + "omnisocketgo/cmd/internal/protocol" +) + +func TestJSONLLoggerWritesOneEventPerLine(t *testing.T) { + path := filepath.Join(t.TempDir(), "latency.jsonl") + + logger, err := NewJSONLLogger(path) + if err != nil { + t.Fatalf("NewJSONLLogger() error = %v", err) + } + t.Cleanup(func() { + _ = logger.Close() + }) + + event := Event{ + TsUnixNano: 123, + NodeRole: NodeRolePeer, + NodeID: "peer-a", + Event: EventAAppPrepBegin, + MessageType: protocol.MessageTypeText, + MessageID: 1, + From: "peer-a", + To: "peer-b", + BodySize: 5, + } + if err := logger.LogEvent(event); err != nil { + t.Fatalf("LogEvent() error = %v", err) + } + + file, err := os.Open(path) + if err != nil { + t.Fatalf("os.Open() error = %v", err) + } + defer file.Close() + + scanner := bufio.NewScanner(file) + if !scanner.Scan() { + t.Fatal("expected one JSONL line, got none") + } + + var got Event + if err := json.Unmarshal(scanner.Bytes(), &got); err != nil { + t.Fatalf("json.Unmarshal() error = %v", err) + } + if got != event { + t.Fatalf("event mismatch: got %+v want %+v", got, event) + } + if scanner.Scan() { + t.Fatal("expected exactly one JSONL line") + } + if err := scanner.Err(); err != nil { + t.Fatalf("scanner.Err() = %v", err) + } +} + +func TestJSONLLoggerHandlesConcurrentWrites(t *testing.T) { + path := filepath.Join(t.TempDir(), "latency.jsonl") + + logger, err := NewJSONLLogger(path) + if err != nil { + t.Fatalf("NewJSONLLogger() error = %v", err) + } + t.Cleanup(func() { + _ = logger.Close() + }) + + const total = 32 + + var wg sync.WaitGroup + for i := 0; i < total; i++ { + i := i + wg.Add(1) + go func() { + defer wg.Done() + + err := logger.LogEvent(Event{ + TsUnixNano: int64(i + 1), + NodeRole: NodeRoleServer, + NodeID: protocol.ServerPeerID, + Event: EventBAppRecv, + MessageType: protocol.MessageTypeFile, + MessageID: uint64(i + 1), + From: "peer-a", + To: "peer-b", + FileName: "payload.bin", + BodySize: 3, + }) + if err != nil { + t.Errorf("LogEvent() error = %v", err) + } + }() + } + wg.Wait() + + file, err := os.Open(path) + if err != nil { + t.Fatalf("os.Open() error = %v", err) + } + defer file.Close() + + scanner := bufio.NewScanner(file) + var count int + seen := make(map[uint64]bool, total) + for scanner.Scan() { + var event Event + if err := json.Unmarshal(scanner.Bytes(), &event); err != nil { + t.Fatalf("json.Unmarshal() error = %v", err) + } + count++ + seen[event.MessageID] = true + } + if err := scanner.Err(); err != nil { + t.Fatalf("scanner.Err() = %v", err) + } + if count != total { + t.Fatalf("line count = %d, want %d", count, total) + } + if len(seen) != total { + t.Fatalf("unique message count = %d, want %d", len(seen), total) + } +} diff --git a/host/OmniSocketGo_add_camera/go/cmd/internal/latencylog/summary.go b/host/OmniSocketGo_add_camera/go/cmd/internal/latencylog/summary.go new file mode 100644 index 0000000..dd825d1 --- /dev/null +++ b/host/OmniSocketGo_add_camera/go/cmd/internal/latencylog/summary.go @@ -0,0 +1,457 @@ +package latencylog + +import ( + "bufio" + "encoding/json" + "fmt" + "os" + "path/filepath" + "sort" + + "omnisocketgo/cmd/internal/protocol" +) + +// Summary 是针对单条消息的时延的规则列表。 +var requiredTimestampNames = []string{ + EventAAppPrepBegin, // A 端应用开始准备这条消息 + EventATXSched, // A 端进入 Linux qdisc 之前 + EventATXSoftware, // A 端即将交给网卡驱动 + EventBRXSoftware, // B 端网卡驱动把数据交给 Linux 接收栈 + EventBAppRecv, // B 端应用真正读到完整消息 + EventBPersistEnd, // B 端写盘完成 +} + +// Summary 是针对单条消息的时延整理结果。 +type Summary struct { + MessageType protocol.MessageType `json:"message_type"` //消息类型 + MessageID uint64 `json:"message_id"` //消息ID + From string `json:"from"` //发送方 + To string `json:"to"` //接收方 + FileName string `json:"file_name,omitempty"` //文件名（仅文件消息） + BodySize int `json:"body_size"` //消息体大小（字节数） + Timestamps map[string]int64 `json:"timestamps"` //事件时间戳，key 是事件名称，value 是 UnixNano 时间戳 + + AProcessingLatencyNS *int64 `json:"a_processing_latency_ns,omitempty"` // A 处理时延：A_TX_SCHED - A_APP_PREP_BEGIN + AQueueLatencyNS *int64 `json:"a_queue_latency_ns,omitempty"` // A 排队时延：A_TX_SOFTWARE - A_TX_SCHED + ABTransportPropagationNS *int64 `json:"a_b_transport_propagation_ns,omitempty"` // A-B 传输+传播时延近似：B_APP_RECV - A_TX_SOFTWARE + BKernelReceivePathLatencyNS *int64 `json:"b_kernel_receive_path_latency_ns,omitempty"` // B 内核接收路径近似：B_APP_RECV - B_RX_SOFTWARE + BProcessingLatencyNS *int64 `json:"b_processing_latency_ns,omitempty"` // B 处理时延：B_PERSIST_END - B_APP_RECV + EndToEndLatencyNS *int64 `json:"end_to_end_latency_ns,omitempty"` // 端到端时延：B_PERSIST_END - A_APP_PREP_BEGIN + AProcessingBitrateBPS *float64 `json:"a_processing_bitrate_bps,omitempty"` // A 处理阶段近似比特率：(BodySize * 8) / A 处理时延（秒） + ABTransportPropagationBitrateBPS *float64 `json:"a_b_transport_propagation_bitrate_bps,omitempty"` // A-B 传输+传播阶段近似比特率：(BodySize * 8) / A-B 传输+传播时延（秒） + EndToEndBitrateBPS *float64 `json:"end_to_end_bitrate_bps,omitempty"` // 端到端近似比特率：(BodySize * 8) / 端到端时延（秒） + ApproxRTTNS *int64 `json:"approx_rtt_ns,omitempty"` // 近似 RTT：首条反向应答的 B_APP_RECV - 当前请求的 A_TX_SOFTWARE + MissingTimestamps []string `json:"missing_timestamps,omitempty"` // 缺失的时间戳列表，包含 requiredTimestampNames 中但在原始事件中没有的事件名称 +} + +// LoadEventsFromFiles 从JSONL 原始日志文件中加载事件。 +type messageKey struct { + MessageType protocol.MessageType //消息类型 + MessageID uint64 //消息ID + From string //发送方 + To string //接收方 +} + +// LoadEventsFromFiles 从多个 JSONL 原始日志文件中加载事件。 +func LoadEventsFromFiles(paths []string) ([]Event, error) { + var events []Event + for _, path := range paths { + fileEvents, err := LoadEventsFromFile(path) + if err != nil { + return nil, err + } + events = append(events, fileEvents...) + } + + return events, nil +} + +// LoadEventsFromFilesWithSharedMaxOffset 从多个 JSONL 原始日志文件中加载事件， +// 并按每个输入文件的最大 message_id 计算共享截断点。 +func LoadEventsFromFilesWithSharedMaxOffset(paths []string, sharedMaxOffset uint64) ([]Event, *uint64, error) { + eventsByFile := make([][]Event, 0, len(paths)) + var minMaxMessageID uint64 + hasSharedMax := false + + for _, path := range paths { + fileEvents, err := LoadEventsFromFile(path) + if err != nil { + return nil, nil, err + } + + eventsByFile = append(eventsByFile, fileEvents) + + fileMaxMessageID, ok := maxBusinessMessageID(fileEvents) + if !ok { + return nil, nil, nil + } + if !hasSharedMax || fileMaxMessageID < minMaxMessageID { + minMaxMessageID = fileMaxMessageID + hasSharedMax = true + } + } + + if !hasSharedMax { + return nil, nil, nil + } + + cutoff, ok := subtractUint64(minMaxMessageID, sharedMaxOffset) + if !ok { + return []Event{}, nil, nil + } + + var events []Event + for _, fileEvents := range eventsByFile { + events = append(events, filterEventsByMaxMessageID(fileEvents, cutoff)...) + } + + return events, &cutoff, nil +} + +// LoadEventsFromFile 从单个 JSONL 原始日志文件中加载事件。 +func LoadEventsFromFile(path string) ([]Event, error) { + file, err := os.Open(path) + if err != nil { + return nil, fmt.Errorf("latencylog: open raw log %s: %w", path, err) + } + defer file.Close() + + var events []Event + scanner := bufio.NewScanner(file) + for scanner.Scan() { + if len(scanner.Bytes()) == 0 { + continue + } + + var event Event + if err := json.Unmarshal(scanner.Bytes(), &event); err != nil { //解析 JSONL 行失败，返回错误 + return nil, fmt.Errorf("latencylog: decode event from %s: %w", path, err) + } + events = append(events, event) + } + if err := scanner.Err(); err != nil { + return nil, fmt.Errorf("latencylog: scan raw log %s: %w", path, err) + } + + return events, nil +} + +// SummarizeEvents 将原始事件整理成按消息分组的时延结果。 +func SummarizeEvents(events []Event) []Summary { + grouped := make(map[messageKey]*Summary) + + for _, event := range events { + if !IsBusinessEvent(event) { + continue + } + + key := messageKey{ + MessageType: event.MessageType, + MessageID: event.MessageID, + From: event.From, + To: event.To, + } + + summary, ok := grouped[key] + if !ok { + summary = &Summary{ + MessageType: event.MessageType, + MessageID: event.MessageID, + From: event.From, + To: event.To, + FileName: event.FileName, + BodySize: event.BodySize, + Timestamps: make(map[string]int64), + } + grouped[key] = summary + } + + if summary.FileName == "" { + summary.FileName = event.FileName + } + if event.BodySize > 0 { + summary.BodySize = event.BodySize + } + + if existing, exists := summary.Timestamps[event.Event]; !exists || event.TsUnixNano < existing { + summary.Timestamps[event.Event] = event.TsUnixNano + } + } + + summaryPointers := make([]*Summary, 0, len(grouped)) + for _, summary := range grouped { + completeSummary(summary) //补全时延指标和缺失时间戳信息 + summaryPointers = append(summaryPointers, summary) + } + assignApproxRTTs(summaryPointers) + + summaries := make([]Summary, 0, len(summaryPointers)) + for _, summary := range summaryPointers { + summaries = append(summaries, *summary) + } + //对整理结果进行排序，先按发送方、再按接收方、再按消息 ID、最后按消息类型排序，保证输出的稳定性和可读性。 + sort.Slice(summaries, func(i, j int) bool { + if summaries[i].From != summaries[j].From { + return summaries[i].From < summaries[j].From + } + if summaries[i].To != summaries[j].To { + return summaries[i].To < summaries[j].To + } + if summaries[i].MessageID != summaries[j].MessageID { + return summaries[i].MessageID < summaries[j].MessageID + } + return summaries[i].MessageType < summaries[j].MessageType + }) + + return summaries +} + +// WriteSummariesJSONL 将整理结果写成 JSONL 汇总文件。 +func WriteSummariesJSONL(path string, summaries []Summary) error { + if err := os.MkdirAll(filepath.Dir(path), 0o755); err != nil { + return fmt.Errorf("latencylog: create summary dir for %s: %w", path, err) + } + + file, err := os.OpenFile(path, os.O_CREATE|os.O_WRONLY|os.O_TRUNC, 0o644) + if err != nil { + return fmt.Errorf("latencylog: open summary file %s: %w", path, err) + } + defer file.Close() + + writer := bufio.NewWriter(file) + for _, summary := range summaries { //将每条整理结果编码成 JSONL 行并写入文件 + line, err := json.Marshal(summary) + if err != nil { + return fmt.Errorf("latencylog: encode summary for message %d: %w", summary.MessageID, err) + } + if _, err := writer.Write(append(line, '\n')); err != nil { + return fmt.Errorf("latencylog: write summary file %s: %w", path, err) + } + } + + if err := writer.Flush(); err != nil { //将缓冲区内容写入文件 + return fmt.Errorf("latencylog: flush summary file %s: %w", path, err) + } + + return nil +} + +// completeSummary 根据事件时间戳计算时延指标，并找出缺失的时间戳。 +func completeSummary(summary *Summary) { + summary.MissingTimestamps = missingTimestampNames(summary.Timestamps) + + if value := subtractIfPresent(summary.Timestamps, EventATXSched, EventAAppPrepBegin); value != nil { + summary.AProcessingLatencyNS = value + } + if value := subtractIfPresent(summary.Timestamps, EventATXSoftware, EventATXSched); value != nil { + summary.AQueueLatencyNS = value + } + if value := subtractIfPresent(summary.Timestamps, EventBAppRecv, EventATXSoftware); value != nil { + summary.ABTransportPropagationNS = value + } + if value := subtractIfPresent(summary.Timestamps, EventBAppRecv, EventBRXSoftware); value != nil { + summary.BKernelReceivePathLatencyNS = value + } + if value := subtractIfPresent(summary.Timestamps, EventBPersistEnd, EventBAppRecv); value != nil { + summary.BProcessingLatencyNS = value + } + if value := subtractIfPresent(summary.Timestamps, EventBPersistEnd, EventAAppPrepBegin); value != nil { + summary.EndToEndLatencyNS = value + } + + summary.AProcessingBitrateBPS = calculateBitrateBPS(summary.BodySize, summary.AProcessingLatencyNS) + summary.ABTransportPropagationBitrateBPS = calculateBitrateBPS(summary.BodySize, summary.ABTransportPropagationNS) + summary.EndToEndBitrateBPS = calculateBitrateBPS(summary.BodySize, summary.EndToEndLatencyNS) +} + +type routeKey struct { + From string + To string +} + +func assignApproxRTTs(summaries []*Summary) { + grouped := make(map[routeKey][]*Summary) + for _, summary := range summaries { + grouped[routeKey{From: summary.From, To: summary.To}] = append(grouped[routeKey{From: summary.From, To: summary.To}], summary) + } + + for key, requests := range grouped { + replies := grouped[routeKey{From: key.To, To: key.From}] + if len(replies) == 0 { + continue + } + + assignApproxRTTsForRoute( + sortSummariesByTimestamp(requests, EventBAppRecv), + sortSummariesByTimestamp(replies, EventATXSoftware), + ) + } +} + +func assignApproxRTTsForRoute(requests, replies []*Summary) { + replyIndex := 0 + for _, request := range requests { + requestReceivedAtResponder, ok := request.Timestamps[EventBAppRecv] + if !ok { + continue + } + + for replyIndex < len(replies) { + reply := replies[replyIndex] + replySentAtResponder, ok := reply.Timestamps[EventATXSoftware] + if !ok { + replyIndex++ + continue + } + if replySentAtResponder < requestReceivedAtResponder { + replyIndex++ + continue + } + + if value := subtractSummaryTimestamps(reply, EventBAppRecv, request, EventATXSoftware); value != nil { + request.ApproxRTTNS = value + } + replyIndex++ + break + } + } +} + +func sortSummariesByTimestamp(summaries []*Summary, eventName string) []*Summary { + sorted := append([]*Summary(nil), summaries...) + sort.SliceStable(sorted, func(i, j int) bool { + leftTS, leftOK := sorted[i].Timestamps[eventName] + rightTS, rightOK := sorted[j].Timestamps[eventName] + switch { + case leftOK && rightOK: + if leftTS != rightTS { + return leftTS < rightTS + } + case leftOK: + return true + case rightOK: + return false + } + + if sorted[i].MessageID != sorted[j].MessageID { + return sorted[i].MessageID < sorted[j].MessageID + } + if sorted[i].From != sorted[j].From { + return sorted[i].From < sorted[j].From + } + if sorted[i].To != sorted[j].To { + return sorted[i].To < sorted[j].To + } + + return sorted[i].MessageType < sorted[j].MessageType + }) + return sorted +} + +// 返回 requiredTimestampNames 中哪些在给定的 timestamps 中缺失。 +func missingTimestampNames(timestamps map[string]int64) []string { + var missing []string + for _, name := range requiredTimestampNames { + if _, ok := timestamps[name]; !ok { + missing = append(missing, name) + } + } + + return missing +} + +// 如果 timestamps 中同时存在 endName 和 beginName，则返回它们的差值；否则返回 nil。 +func subtractIfPresent(timestamps map[string]int64, endName, beginName string) *int64 { + end, ok := timestamps[endName] + if !ok { + return nil + } + begin, ok := timestamps[beginName] + if !ok { + return nil + } + + value := end - begin + return &value +} + +func subtractSummaryTimestamps(endSummary *Summary, endName string, beginSummary *Summary, beginName string) *int64 { + end, ok := endSummary.Timestamps[endName] + if !ok { + return nil + } + begin, ok := beginSummary.Timestamps[beginName] + if !ok { + return nil + } + + value := end - begin + return &value +} + +// 除法函数，如果 bodySize <= 0 或 latencyNS 不存在或 <= 0，则返回 nil；否则返回 bodySize / latencyNS 的结果。 +func calculateBitrateBPS(bodySize int, latencyNS *int64) *float64 { + if bodySize <= 0 || latencyNS == nil || *latencyNS <= 0 { + return nil + } + + value := float64(bodySize) * 8 * 1_000_000_000 / float64(*latencyNS) + return &value +} + +// 最大 message_id 计算函数 +func maxBusinessMessageID(events []Event) (uint64, bool) { + var maxMessageID uint64 + hasBusinessMessage := false + + for _, event := range events { + if !IsBusinessEvent(event) { + continue + } + if !hasBusinessMessage || event.MessageID > maxMessageID { + maxMessageID = event.MessageID + hasBusinessMessage = true + } + } + + return maxMessageID, hasBusinessMessage +} + +// 根据 message_id 截断事件列表的函数 +func filterEventsByMaxMessageID(events []Event, maxMessageID uint64) []Event { + filtered := make([]Event, 0, len(events)) + for _, event := range events { + if event.MessageID > maxMessageID { + continue + } + filtered = append(filtered, event) + } + + return filtered +} + +func subtractUint64(value, offset uint64) (uint64, bool) { + if offset > value { + return 0, false + } + + return value - offset, true +} + +// 判断事件是否是业务相关的时延事件（其中一项） +func IsBusinessEvent(event Event) bool { + switch event.Event { + case EventAAppPrepBegin, + EventATXSched, + EventATXSoftware, + EventATXHardware, + EventBRXHardware, + EventBRXSoftware, + EventBAppRecv, + EventBPersistBegin, + EventBPersistEnd: + return true + default: + return false + } +} diff --git a/host/OmniSocketGo_add_camera/go/cmd/internal/latencylog/summary_chart.go b/host/OmniSocketGo_add_camera/go/cmd/internal/latencylog/summary_chart.go new file mode 100644 index 0000000..a37a4c2 --- /dev/null +++ b/host/OmniSocketGo_add_camera/go/cmd/internal/latencylog/summary_chart.go @@ -0,0 +1,498 @@ +package latencylog + +import ( + "bufio" + "fmt" + "html/template" + "os" + "path/filepath" + "strings" + + "omnisocketgo/cmd/internal/protocol" +) + +const summaryChartHTMLTemplate = ` + + + + + Latency Summary Chart + + + +
+

Latency Summary

+

A simple per-message end-to-end latency chart generated from summarized JSONL records.

+ +
+
+
Messages
+
{{.TotalMessages}}
+
+
+
With End-To-End
+
{{.MessagesWithEndToEnd}}
+
+
+
Average End-To-End
+
{{.AverageEndToEnd}}
+
+
+
Max End-To-End
+
{{.MaxEndToEnd}}
+
+
+ +
+ {{range .Legend}} + + + {{.Label}} + + {{end}} +
+ + {{if .Rows}} +
+ {{range .Rows}} +
+
+

{{.Title}}

+
{{.EndToEnd}}
+
+
{{.Subtitle}}
+
{{.ApproxRTT}}
+ {{if .RatioMetrics}} +
+ {{range .RatioMetrics}} + {{.Label}} {{.Value}} + {{end}} +
+ {{end}} +
+ {{range .Segments}} +
+ {{end}} +
+ {{if .Segments}} +
+ {{range .Segments}} + + + {{.Label}} {{.Value}} + + {{end}} +
+ {{end}} + {{if .MissingTimestamps}} +
Missing timestamps: {{.MissingTimestamps}}
+ {{end}} +
+ {{end}} +
+ {{else}} +
No summarized messages were available for chart rendering.
+ {{end}} +
+ + +` + +type summaryChartPage struct { + TotalMessages int + MessagesWithEndToEnd int + AverageEndToEnd string + MaxEndToEnd string + Legend []summaryChartLegendItem + Rows []summaryChartRow +} + +type summaryChartLegendItem struct { + Label string + Color string +} + +type summaryChartRow struct { + Title string + Subtitle string + EndToEnd string + ApproxRTT string + MissingTimestamps string + RatioMetrics []summaryChartValue + Segments []summaryChartSegment +} + +type summaryChartSegment struct { + Label string + Value string + Color string + WidthPercent float64 +} + +type summaryChartValue struct { + Label string + Value string +} + +type summaryChartSegmentMetric struct { + label string + value *int64 + color string +} + +// WriteSummariesHTMLChart 将整理结果写成一个可直接在浏览器中打开的简单 HTML 图表。 +func WriteSummariesHTMLChart(path string, summaries []Summary) error { + if err := os.MkdirAll(filepath.Dir(path), 0o755); err != nil { + return fmt.Errorf("latencylog: create chart dir for %s: %w", path, err) + } + + file, err := os.OpenFile(path, os.O_CREATE|os.O_WRONLY|os.O_TRUNC, 0o644) + if err != nil { + return fmt.Errorf("latencylog: open chart file %s: %w", path, err) + } + defer file.Close() + + page := buildSummaryChartPage(summaries) + tmpl, err := template.New("summary-chart").Parse(summaryChartHTMLTemplate) + if err != nil { + return fmt.Errorf("latencylog: parse chart template: %w", err) + } + + writer := bufio.NewWriter(file) + if err := tmpl.Execute(writer, page); err != nil { + return fmt.Errorf("latencylog: render chart %s: %w", path, err) + } + if err := writer.Flush(); err != nil { + return fmt.Errorf("latencylog: flush chart %s: %w", path, err) + } + + return nil +} + +func buildSummaryChartPage(summaries []Summary) summaryChartPage { + page := summaryChartPage{ + TotalMessages: len(summaries), + Legend: []summaryChartLegendItem{ + {Label: "A processing", Color: "var(--a-proc)"}, + {Label: "A queue", Color: "var(--a-queue)"}, + {Label: "A-B transport + propagation", Color: "var(--transport)"}, + {Label: "B processing", Color: "var(--b-proc)"}, + {Label: "Unknown / missing", Color: "var(--unknown)"}, + }, + Rows: make([]summaryChartRow, 0, len(summaries)), + } + + var ( + endToEndValues []int64 + totalEndToEnd int64 + maxEndToEnd int64 + ) + + for _, summary := range summaries { + page.Rows = append(page.Rows, buildSummaryChartRow(summary)) + + if summary.EndToEndLatencyNS == nil { + continue + } + endToEnd := *summary.EndToEndLatencyNS + endToEndValues = append(endToEndValues, endToEnd) + totalEndToEnd += endToEnd + if endToEnd > maxEndToEnd { + maxEndToEnd = endToEnd + } + } + + page.MessagesWithEndToEnd = len(endToEndValues) + page.AverageEndToEnd = "n/a" + page.MaxEndToEnd = "n/a" + if len(endToEndValues) > 0 { + page.AverageEndToEnd = formatLatencyNS(totalEndToEnd / int64(len(endToEndValues))) + page.MaxEndToEnd = formatLatencyNS(maxEndToEnd) + } + + return page +} + +func buildSummaryChartRow(summary Summary) summaryChartRow { + row := summaryChartRow{ + Title: buildSummaryChartTitle(summary), + Subtitle: buildSummaryChartSubtitle(summary), + EndToEnd: "End-to-end: n/a", + ApproxRTT: "Approx RTT: n/a", + MissingTimestamps: strings.Join(summary.MissingTimestamps, ", "), + } + if summary.ApproxRTTNS != nil && *summary.ApproxRTTNS > 0 { + row.ApproxRTT = fmt.Sprintf("Approx RTT: %s", formatLatencyNS(*summary.ApproxRTTNS)) + } + + ratioMetrics := []struct { + label string + value *float64 + }{ + {label: "A processing bitrate", value: summary.AProcessingBitrateBPS}, + {label: "A-B transport + propagation bitrate", value: summary.ABTransportPropagationBitrateBPS}, + {label: "End-to-end bitrate", value: summary.EndToEndBitrateBPS}, + } + for _, metric := range ratioMetrics { + if metric.value == nil || *metric.value <= 0 { + continue + } + row.RatioMetrics = append(row.RatioMetrics, summaryChartValue{ + Label: metric.label, + Value: formatBitrateBPS(*metric.value), + }) + } + + if summary.EndToEndLatencyNS == nil || *summary.EndToEndLatencyNS <= 0 { + return row + } + + total := *summary.EndToEndLatencyNS + row.EndToEnd = fmt.Sprintf("End-to-end: %s", formatLatencyNS(total)) + + metrics := []summaryChartSegmentMetric{ + {label: "A processing", value: summary.AProcessingLatencyNS, color: "var(--a-proc)"}, + {label: "A queue", value: summary.AQueueLatencyNS, color: "var(--a-queue)"}, + {label: "A-B transport + propagation", value: summary.ABTransportPropagationNS, color: "var(--transport)"}, + {label: "B processing", value: summary.BProcessingLatencyNS, color: "var(--b-proc)"}, + } + + var knownTotal int64 + for _, metric := range metrics { + if metric.value == nil || *metric.value <= 0 { + continue + } + knownTotal += *metric.value + } + + scaleTotal := total + if knownTotal > scaleTotal { + scaleTotal = knownTotal + } + if scaleTotal <= 0 { + return row + } + + for _, metric := range metrics { + if metric.value == nil || *metric.value <= 0 { + continue + } + row.Segments = append(row.Segments, summaryChartSegment{ + Label: metric.label, + Value: formatLatencyNS(*metric.value), + Color: metric.color, + WidthPercent: float64(*metric.value) * 100 / float64(scaleTotal), + }) + } + + if remaining := total - knownTotal; remaining > 0 { + row.Segments = append(row.Segments, summaryChartSegment{ + Label: "Unknown / missing", + Value: formatLatencyNS(remaining), + Color: "var(--unknown)", + WidthPercent: float64(remaining) * 100 / float64(scaleTotal), + }) + } + + return row +} + +func buildSummaryChartTitle(summary Summary) string { + if summary.MessageType == protocol.MessageTypeFile && summary.FileName != "" { + return fmt.Sprintf("%s #%d (%s)", summary.MessageType, summary.MessageID, summary.FileName) + } + + return fmt.Sprintf("%s #%d", summary.MessageType, summary.MessageID) +} + +func buildSummaryChartSubtitle(summary Summary) string { + parts := []string{ + fmt.Sprintf("%s -> %s", summary.From, summary.To), + fmt.Sprintf("%d bytes", summary.BodySize), + } + + if summary.MessageType == protocol.MessageTypeFile && summary.FileName != "" { + parts = append(parts, fmt.Sprintf("file: %s", summary.FileName)) + } + + return strings.Join(parts, " | ") +} + +func formatLatencyNS(ns int64) string { + return fmt.Sprintf("%.3f ms", float64(ns)/1_000_000) +} + +func formatBitrateBPS(bitsPerSecond float64) string { + return fmt.Sprintf("%.3f Mb/s", bitsPerSecond/1_000_000) +} diff --git a/host/OmniSocketGo_add_camera/go/cmd/internal/latencylog/summary_chart_test.go b/host/OmniSocketGo_add_camera/go/cmd/internal/latencylog/summary_chart_test.go new file mode 100644 index 0000000..d9f41ec --- /dev/null +++ b/host/OmniSocketGo_add_camera/go/cmd/internal/latencylog/summary_chart_test.go @@ -0,0 +1,79 @@ +package latencylog + +import ( + "os" + "path/filepath" + "strings" + "testing" + + "omnisocketgo/cmd/internal/protocol" +) + +func TestWriteSummariesHTMLChart(t *testing.T) { + aProcessing := int64(20_000_000) + aQueue := int64(10_000_000) + transport := int64(40_000_000) + bProcessing := int64(30_000_000) + endToEnd := int64(100_000_000) + aProcessingBitrate := float64(5) * 8 * 1_000_000_000 / float64(aProcessing) + transportBitrate := float64(5) * 8 * 1_000_000_000 / float64(transport) + endToEndBitrate := float64(5) * 8 * 1_000_000_000 / float64(endToEnd) + + summaries := []Summary{ + { + MessageType: protocol.MessageTypeText, + MessageID: 7, + From: "peer-a", + To: "peer-b", + BodySize: 5, + AProcessingLatencyNS: &aProcessing, + AQueueLatencyNS: &aQueue, + ABTransportPropagationNS: &transport, + BProcessingLatencyNS: &bProcessing, + EndToEndLatencyNS: &endToEnd, + AProcessingBitrateBPS: &aProcessingBitrate, + ABTransportPropagationBitrateBPS: &transportBitrate, + EndToEndBitrateBPS: &endToEndBitrate, + ApproxRTTNS: &endToEnd, + }, + { + MessageType: protocol.MessageTypeFile, + MessageID: 8, + From: "peer-b", + To: "peer-a", + FileName: "payload.bin", + BodySize: 128, + MissingTimestamps: []string{EventBRXSoftware}, + }, + } + + path := filepath.Join(t.TempDir(), "charts", "latency-summary.html") + if err := WriteSummariesHTMLChart(path, summaries); err != nil { + t.Fatalf("WriteSummariesHTMLChart() error = %v", err) + } + + data, err := os.ReadFile(path) + if err != nil { + t.Fatalf("os.ReadFile() error = %v", err) + } + + content := string(data) + for _, want := range []string{ + "Latency Summary", + "text #7", + "peer-a -> peer-b | 5 bytes", + "End-to-end: 100.000 ms", + "Approx RTT: 100.000 ms", + "A processing bitrate 0.002 Mb/s", + "A-B transport + propagation bitrate 0.001 Mb/s", + "End-to-end bitrate 0.000 Mb/s", + "A processing 20.000 ms", + "A-B transport + propagation 40.000 ms", + "file #8 (payload.bin)", + "Missing timestamps: B_RX_SOFTWARE", + } { + if !strings.Contains(content, want) { + t.Fatalf("chart content missing %q\n%s", want, content) + } + } +} diff --git a/host/OmniSocketGo_add_camera/go/cmd/internal/latencylog/summary_test.go b/host/OmniSocketGo_add_camera/go/cmd/internal/latencylog/summary_test.go new file mode 100644 index 0000000..f1bb8da --- /dev/null +++ b/host/OmniSocketGo_add_camera/go/cmd/internal/latencylog/summary_test.go @@ -0,0 +1,399 @@ +package latencylog + +import ( + "bufio" + "encoding/json" + "os" + "path/filepath" + "reflect" + "sort" + "testing" + + "omnisocketgo/cmd/internal/protocol" +) + +func TestSummarizeEventsComputesLatencyMetrics(t *testing.T) { + events := []Event{ + {TsUnixNano: 100, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 120, Event: EventATXSched, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 140, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 180, Event: EventBRXSoftware, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 220, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 230, Event: EventBPersistBegin, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 260, Event: EventBPersistEnd, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + } + + summaries := SummarizeEvents(events) + if len(summaries) != 1 { + t.Fatalf("summary count = %d, want 1", len(summaries)) + } + + summary := summaries[0] + if got := ptrValue(summary.AProcessingLatencyNS); got != 20 { + t.Fatalf("AProcessingLatencyNS = %d, want 20", got) + } + if got := ptrValue(summary.AQueueLatencyNS); got != 20 { + t.Fatalf("AQueueLatencyNS = %d, want 20", got) + } + if got := ptrValue(summary.ABTransportPropagationNS); got != 80 { + t.Fatalf("ABTransportPropagationNS = %d, want 80", got) + } + if got := ptrValue(summary.BKernelReceivePathLatencyNS); got != 40 { + t.Fatalf("BKernelReceivePathLatencyNS = %d, want 40", got) + } + if got := ptrValue(summary.BProcessingLatencyNS); got != 40 { + t.Fatalf("BProcessingLatencyNS = %d, want 40", got) + } + if got := ptrValue(summary.EndToEndLatencyNS); got != 160 { + t.Fatalf("EndToEndLatencyNS = %d, want 160", got) + } + if got := ptrValueFloat(summary.AProcessingBitrateBPS); got != 128_000_000_000 { + t.Fatalf("AProcessingBitrateBPS = %v, want 128000000000", got) + } + if got := ptrValueFloat(summary.ABTransportPropagationBitrateBPS); got != 32_000_000_000 { + t.Fatalf("ABTransportPropagationBitrateBPS = %v, want 32000000000", got) + } + if got := ptrValueFloat(summary.EndToEndBitrateBPS); got != 16_000_000_000 { + t.Fatalf("EndToEndBitrateBPS = %v, want 16000000000", got) + } + if got := summary.Timestamps[EventBRXSoftware]; got != 180 { + t.Fatalf("timestamps[%q] = %d, want 180", EventBRXSoftware, got) + } + if len(summary.MissingTimestamps) != 0 { + t.Fatalf("MissingTimestamps = %v, want empty", summary.MissingTimestamps) + } +} + +func TestSummarizeEventsComputesApproxRTTByPairingReverseMessages(t *testing.T) { + events := []Event{ + {TsUnixNano: 100, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b"}, + {TsUnixNano: 110, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b"}, + {TsUnixNano: 180, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b"}, + {TsUnixNano: 120, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b"}, + {TsUnixNano: 140, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b"}, + {TsUnixNano: 190, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b"}, + {TsUnixNano: 200, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 11, From: "peer-b", To: "peer-a"}, + {TsUnixNano: 210, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 11, From: "peer-b", To: "peer-a"}, + {TsUnixNano: 260, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 11, From: "peer-b", To: "peer-a"}, + {TsUnixNano: 220, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 12, From: "peer-b", To: "peer-a"}, + {TsUnixNano: 230, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 12, From: "peer-b", To: "peer-a"}, + {TsUnixNano: 310, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 12, From: "peer-b", To: "peer-a"}, + } + + summaries := SummarizeEvents(events) + if len(summaries) != 4 { + t.Fatalf("summary count = %d, want 4", len(summaries)) + } + + gotByMessageID := make(map[uint64]Summary, len(summaries)) + for _, summary := range summaries { + gotByMessageID[summary.MessageID] = summary + } + + if got := ptrValue(gotByMessageID[1].ApproxRTTNS); got != 150 { + t.Fatalf("message 1 ApproxRTTNS = %d, want 150", got) + } + if got := ptrValue(gotByMessageID[2].ApproxRTTNS); got != 170 { + t.Fatalf("message 2 ApproxRTTNS = %d, want 170", got) + } + if gotByMessageID[11].ApproxRTTNS != nil { + t.Fatalf("message 11 ApproxRTTNS = %d, want nil", ptrValue(gotByMessageID[11].ApproxRTTNS)) + } + if gotByMessageID[12].ApproxRTTNS != nil { + t.Fatalf("message 12 ApproxRTTNS = %d, want nil", ptrValue(gotByMessageID[12].ApproxRTTNS)) + } +} + +func TestSummarizeEventsReportsMissingTimestamps(t *testing.T) { + events := []Event{ + {TsUnixNano: 100, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b"}, + {TsUnixNano: 240, Event: EventBPersistEnd, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b"}, + } + + summaries := SummarizeEvents(events) + if len(summaries) != 1 { + t.Fatalf("summary count = %d, want 1", len(summaries)) + } + + wantMissing := []string{EventATXSched, EventATXSoftware, EventBRXSoftware, EventBAppRecv} + if !reflect.DeepEqual(summaries[0].MissingTimestamps, wantMissing) { + t.Fatalf("MissingTimestamps = %v, want %v", summaries[0].MissingTimestamps, wantMissing) + } + if summaries[0].AProcessingLatencyNS != nil { + t.Fatalf("AProcessingLatencyNS = %v, want nil", ptrValue(summaries[0].AProcessingLatencyNS)) + } + if summaries[0].EndToEndLatencyNS == nil { + t.Fatal("EndToEndLatencyNS = nil, want non-nil because endpoints are present") + } +} + +func TestLoadAndWriteSummaryFiles(t *testing.T) { + rawPath := filepath.Join(t.TempDir(), "raw.jsonl") + rawLogger, err := NewJSONLLogger(rawPath) + if err != nil { + t.Fatalf("NewJSONLLogger() error = %v", err) + } + t.Cleanup(func() { + _ = rawLogger.Close() + }) + + for _, event := range []Event{ + {TsUnixNano: 100, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 120, Event: EventATXSched, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 140, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 180, Event: EventBRXSoftware, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 220, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 260, Event: EventBPersistEnd, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 320}, + } { + if err := rawLogger.LogEvent(event); err != nil { + t.Fatalf("LogEvent() error = %v", err) + } + } + + events, err := LoadEventsFromFile(rawPath) + if err != nil { + t.Fatalf("LoadEventsFromFile() error = %v", err) + } + + summaryPath := filepath.Join(t.TempDir(), "summary.jsonl") + if err := WriteSummariesJSONL(summaryPath, SummarizeEvents(events)); err != nil { + t.Fatalf("WriteSummariesJSONL() error = %v", err) + } + + file, err := os.Open(summaryPath) + if err != nil { + t.Fatalf("os.Open() error = %v", err) + } + defer file.Close() + + scanner := bufio.NewScanner(file) + if !scanner.Scan() { + t.Fatal("expected one summary line, got none") + } + + var summary Summary + if err := json.Unmarshal(scanner.Bytes(), &summary); err != nil { + t.Fatalf("json.Unmarshal() error = %v", err) + } + if summary.MessageID != 3 { + t.Fatalf("MessageID = %d, want 3", summary.MessageID) + } + if got := ptrValue(summary.BKernelReceivePathLatencyNS); got != 40 { + t.Fatalf("BKernelReceivePathLatencyNS = %d, want 40", got) + } + if got := ptrValue(summary.EndToEndLatencyNS); got != 160 { + t.Fatalf("EndToEndLatencyNS = %d, want 160", got) + } + if got := ptrValueFloat(summary.EndToEndBitrateBPS); got != 16_000_000_000 { + t.Fatalf("EndToEndBitrateBPS = %v, want 16000000000", got) + } +} + +func TestLoadEventsFromFilesWithSharedMaxOffsetFiltersToSharedCutoff(t *testing.T) { + t.Parallel() + + testCases := []struct { + name string + firstMessageIDs []uint64 + secondMessageIDs []uint64 + offset uint64 + wantCutoff *uint64 + wantMessageIDs []uint64 + }{ + { + name: "same max message id rolls back one", + firstMessageIDs: []uint64{1, 2, 3, 4, 5, 6, 7}, + secondMessageIDs: []uint64{1, 2, 3, 4, 5, 6, 7}, + offset: 1, + wantCutoff: uint64Ptr(6), + wantMessageIDs: []uint64{1, 2, 3, 4, 5, 6}, + }, + { + name: "smaller input max wins before rollback", + firstMessageIDs: []uint64{1, 2, 3, 4, 5, 6, 7, 8, 9}, + secondMessageIDs: []uint64{1, 2, 3, 4, 5, 6, 7}, + offset: 1, + wantCutoff: uint64Ptr(6), + wantMessageIDs: []uint64{1, 2, 3, 4, 5, 6}, + }, + { + name: "not enough shared messages yields empty result", + firstMessageIDs: []uint64{1}, + secondMessageIDs: []uint64{1}, + offset: 1, + wantCutoff: uint64Ptr(0), + wantMessageIDs: nil, + }, + } + + for _, tt := range testCases { + tt := tt + t.Run(tt.name, func(t *testing.T) { + t.Parallel() + + tempDir := t.TempDir() + firstPath := filepath.Join(tempDir, "first.jsonl") + secondPath := filepath.Join(tempDir, "second.jsonl") + writeEventsJSONL(t, firstPath, testEventsForMessageIDs(tt.firstMessageIDs, "peer-a", "peer-b")) + writeEventsJSONL(t, secondPath, testEventsForMessageIDs(tt.secondMessageIDs, "peer-b", "peer-a")) + + events, cutoff, err := LoadEventsFromFilesWithSharedMaxOffset([]string{firstPath, secondPath}, tt.offset) + if err != nil { + t.Fatalf("LoadEventsFromFilesWithSharedMaxOffset() error = %v", err) + } + if !reflect.DeepEqual(cutoff, tt.wantCutoff) { + t.Fatalf("cutoff = %v, want %v", cutoff, tt.wantCutoff) + } + + if got := businessMessageIDs(events); !reflect.DeepEqual(got, tt.wantMessageIDs) { + t.Fatalf("message IDs = %v, want %v", got, tt.wantMessageIDs) + } + }) + } +} + +func TestLoadEventsFromFilesWithSharedMaxOffsetPreservesEarlierSummaries(t *testing.T) { + tempDir := t.TempDir() + firstPath := filepath.Join(tempDir, "first.jsonl") + secondPath := filepath.Join(tempDir, "second.jsonl") + + writeEventsJSONL(t, firstPath, []Event{ + {TsUnixNano: 100, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 120, Event: EventATXSched, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 140, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 180, Event: EventBRXSoftware, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 220, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 260, Event: EventBPersistEnd, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 300, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b", BodySize: 160}, + {TsUnixNano: 330, Event: EventATXSched, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b", BodySize: 160}, + {TsUnixNano: 360, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b", BodySize: 160}, + {TsUnixNano: 390, Event: EventBRXSoftware, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b", BodySize: 160}, + {TsUnixNano: 420, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b", BodySize: 160}, + {TsUnixNano: 470, Event: EventBPersistEnd, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b", BodySize: 160}, + {TsUnixNano: 500, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 80}, + {TsUnixNano: 520, Event: EventATXSched, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 80}, + {TsUnixNano: 540, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 80}, + {TsUnixNano: 560, Event: EventBRXSoftware, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 80}, + {TsUnixNano: 580, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 80}, + {TsUnixNano: 600, Event: EventBPersistEnd, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 80}, + {TsUnixNano: 700, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 4, From: "peer-a", To: "peer-b", BodySize: 40}, + }) + writeEventsJSONL(t, secondPath, []Event{ + {TsUnixNano: 90, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 95, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 150, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 290, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 295, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 350, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 490, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 495, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 550, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 690, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 4, From: "peer-b", To: "peer-a", BodySize: 20}, + }) + + events, cutoff, err := LoadEventsFromFilesWithSharedMaxOffset([]string{firstPath, secondPath}, 1) + if err != nil { + t.Fatalf("LoadEventsFromFilesWithSharedMaxOffset() error = %v", err) + } + if !reflect.DeepEqual(cutoff, uint64Ptr(3)) { + t.Fatalf("cutoff = %v, want %v", cutoff, uint64Ptr(3)) + } + + summaries := SummarizeEvents(events) + if got := len(summaries); got != 6 { + t.Fatalf("summary count = %d, want 6", got) + } + + for _, summary := range summaries { + if summary.MessageID == 4 { + t.Fatalf("message 4 should have been truncated from summaries: %+v", summary) + } + } + + var forwardMessageTwo Summary + found := false + for _, summary := range summaries { + if summary.From == "peer-a" && summary.To == "peer-b" && summary.MessageID == 2 { + forwardMessageTwo = summary + found = true + break + } + } + if !found { + t.Fatal("summary for message 2 peer-a -> peer-b not found") + } + if got := ptrValue(forwardMessageTwo.EndToEndLatencyNS); got != 170 { + t.Fatalf("message 2 EndToEndLatencyNS = %d, want 170", got) + } + if got := ptrValue(forwardMessageTwo.ApproxRTTNS); got != 190 { + t.Fatalf("message 2 ApproxRTTNS = %d, want 190", got) + } +} + +func ptrValue(value *int64) int64 { + if value == nil { + return 0 + } + return *value +} + +func ptrValueFloat(value *float64) float64 { + if value == nil { + return 0 + } + return *value +} + +func uint64Ptr(value uint64) *uint64 { + return &value +} + +func businessMessageIDs(events []Event) []uint64 { + seen := make(map[uint64]struct{}) + var ids []uint64 + + for _, event := range events { + if !IsBusinessEvent(event) { + continue + } + if _, ok := seen[event.MessageID]; ok { + continue + } + seen[event.MessageID] = struct{}{} + ids = append(ids, event.MessageID) + } + + sort.Slice(ids, func(i, j int) bool { + return ids[i] < ids[j] + }) + return ids +} + +func testEventsForMessageIDs(messageIDs []uint64, from, to string) []Event { + events := make([]Event, 0, len(messageIDs)*2) + for _, messageID := range messageIDs { + events = append(events, + Event{TsUnixNano: int64(messageID*100 + 10), Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: messageID, From: from, To: to, BodySize: 32}, + Event{TsUnixNano: int64(messageID*100 + 20), Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: messageID, From: from, To: to, BodySize: 32}, + ) + } + + return events +} + +func writeEventsJSONL(t *testing.T, path string, events []Event) { + t.Helper() + + file, err := os.OpenFile(path, os.O_CREATE|os.O_WRONLY|os.O_TRUNC, 0o644) + if err != nil { + t.Fatalf("os.OpenFile(%s) error = %v", path, err) + } + defer file.Close() + + encoder := json.NewEncoder(file) + for _, event := range events { + if err := encoder.Encode(event); err != nil { + t.Fatalf("encoder.Encode(%s) error = %v", path, err) + } + } +} diff --git a/host/OmniSocketGo_add_camera/go/cmd/internal/protocol/codec.go b/host/OmniSocketGo_add_camera/go/cmd/internal/protocol/codec.go new file mode 100644 index 0000000..fef658b --- /dev/null +++ b/host/OmniSocketGo_add_camera/go/cmd/internal/protocol/codec.go @@ -0,0 +1,279 @@ +package protocol + +import ( + "encoding/binary" + "encoding/json" + "errors" + "fmt" + "io" + "unicode/utf8" +) + +// MaxFrameSize 用于限制单个帧的最大长度， +// 避免异常对端通过伪造超大长度值导致接收方无上限分配内存。 +const MaxFrameSize = 8 * 1024 * 1024 // 先临时设置传输的视频帧不超过8MB + +var ( + ErrInvalidFrameLength = errors.New("protocol: invalid frame length") // 表示帧长度非法，例如长度为 0。 + ErrFrameTooLarge = errors.New("protocol: frame too large") // 表示帧长度超过允许的上限。 + ErrInvalidMessageType = errors.New("protocol: invalid message type") // 表示消息类型不是当前协议支持的类型。 + ErrMissingFrom = errors.New("protocol: missing from") // 表示消息缺少发送方标识。 + ErrMissingTo = errors.New("protocol: missing to") // 表示消息缺少接收方标识。 + ErrMissingFileName = errors.New("protocol: missing file name") // 表示 file 消息缺少文件名。 + ErrUnexpectedFileName = errors.New("protocol: unexpected file name") // 表示 text 消息错误地携带了文件名。 + ErrInvalidTextBody = errors.New("protocol: invalid text body") // 表示 text 消息正文不是合法 UTF-8。 + ErrUnexpectedBody = errors.New("protocol: unexpected body") // 表示某些控制消息不允许携带正文。 + ErrInvalidRegisterTarget = errors.New("protocol: invalid register target") // 表示 register 消息没有发往 server。 + ErrInvalidErrorSource = errors.New("protocol: invalid error source") // 表示 error 消息不是由 server 发出。 + ErrInvalidHeaderLength = errors.New("protocol: invalid header length") // 表示 header 长度字段为 0、越界或无法完整切分。 + ErrInvalidHeaderJSON = errors.New("protocol: invalid header json") // 表示 header JSON 无法解析，可能是格式错误或缺少必要字段。 + ErrInvalidContentLength = errors.New("protocol: invalid content length") // 表示头部记录的正文长度与实际正文不一致。 +) + +// 应用层消息：[4字节 frameLength][4字节 headerLen][header JSON（下面自定义的Message头）][body bytes] +// 写了 tag：JSON 字段名是你指定的 type；不写 tag：JSON 字段名默认是 Go 字段名 Type +type messageHeader struct { + Type MessageType `json:"type"` + ID uint64 `json:"id"` + From string `json:"from"` + To string `json:"to"` + FileName string `json:"file_name,omitempty"` + ContentLength int `json:"content_length"` +} + +// EncodeMessage 将逻辑消息编码为帧内字节格式： +// 1. 4 字节大端序 header 长度 +// 2. header JSON +// 3. 原始 body 字节 +func EncodeMessage(msg Message) ([]byte, error) { + if err := validateMessage(msg); err != nil { + return nil, err + } + + header := messageHeader{ + Type: msg.Type, + ID: msg.ID, + From: msg.From, + To: msg.To, + FileName: msg.FileName, + ContentLength: len(msg.Body), + } + + headerPayload, err := json.Marshal(header) + if err != nil { + return nil, fmt.Errorf("protocol: encode header: %w", err) + } + // 创建一个新的字节切片来存储完整的帧内容，避免直接在 headerPayload 上修改导致数据混乱。 + payload := make([]byte, 4+len(headerPayload)+len(msg.Body)) + // 在 payload 前 4 字节写入 header 长度，后续内容依次是 header JSON（第五个字节开始） 和 body。 + binary.BigEndian.PutUint32(payload[:4], uint32(len(headerPayload))) + copy(payload[4:], headerPayload) + copy(payload[4+len(headerPayload):], msg.Body) + + //检查整个帧长度是否合法，避免上层调用者构造的消息过大导致发送失败。 + if len(payload) > MaxFrameSize { + return nil, ErrFrameTooLarge + } + + return payload, nil +} + +// DecodeMessage 将帧内字节格式还原为 Message。 +func DecodeMessage(data []byte) (Message, error) { + if len(data) > MaxFrameSize { + return Message{}, ErrFrameTooLarge + } + if len(data) < 4 { + return Message{}, ErrInvalidHeaderLength + } + + headerLen := int(binary.BigEndian.Uint32(data[:4])) + if headerLen == 0 || headerLen > len(data)-4 { + return Message{}, ErrInvalidHeaderLength + } + + headerPayload := data[4 : 4+headerLen] + body := data[4+headerLen:] + + var header messageHeader + if err := json.Unmarshal(headerPayload, &header); err != nil { + return Message{}, fmt.Errorf("protocol: decode header: %w", errors.Join(ErrInvalidHeaderJSON, err)) + } + + if header.ContentLength < 0 || header.ContentLength != len(body) { + return Message{}, ErrInvalidContentLength + } + + bodyCopy := make([]byte, len(body)) + copy(bodyCopy, body) + + msg := Message{ + Type: header.Type, + ID: header.ID, + From: header.From, + To: header.To, + FileName: header.FileName, + Body: bodyCopy, + } + + if err := validateMessage(msg); err != nil { + return Message{}, err + } + + return msg, nil +} + +// WriteFrame 向流中写入一个带长度前缀的帧。 +// TCP帧格式如下： +// 1. 4 字节大端序长度 +// 2. 后续 payload 内容 +// +// TCP 是字节流协议，没有天然的消息边界。 +// 增加显式长度前缀后，接收方就知道一条完整消息应该读取多少字节， +// 从而解决粘包和拆包问题。 +func WriteFrame(w io.Writer, payload []byte) error { + size := len(payload) + //空帧 + if size == 0 { + return ErrInvalidFrameLength + } + //帧过大 + if size > MaxFrameSize { + return ErrFrameTooLarge + } + + var header [4]byte + binary.BigEndian.PutUint32(header[:], uint32(size)) + + // 先写长度头，接收方才能根据长度一次性读取完整消息体。 + if err := writeFull(w, header[:]); err != nil { + return err + } + + return writeFull(w, payload) +} + +// ReadFrame 从流中读取一个完整的长度前缀帧。 +// 它会先读取固定 4 字节长度头，校验长度是否合法， +// 再使用 io.ReadFull 按长度读取完整消息体， +// 这样即使底层 TCP 发生分段读取，也不会把半条消息暴露给上层。 +func ReadFrame(r io.Reader) ([]byte, error) { + var header [4]byte + if _, err := io.ReadFull(r, header[:]); err != nil { + return nil, err + } + + size := binary.BigEndian.Uint32(header[:]) + // 长度为 0 的帧被认为是非法输入，而不是合法的空消息。 + if size == 0 { + return nil, ErrInvalidFrameLength + } + // 长度超过上限的帧会被拒绝，避免接收方无上限分配内存。 + if size > MaxFrameSize { + return nil, ErrFrameTooLarge + } + + payload := make([]byte, int(size)) + if _, err := io.ReadFull(r, payload); err != nil { + return nil, err + } + + return payload, nil +} + +// WriteMessage 是给上层直接使用的完整发送路径： +// 把一条结构化消息完整编码并发送出去”的总入口。 +// Message -> header+body -> 长度前缀帧 -> io.Writer。 +func WriteMessage(w io.Writer, msg Message) error { + payload, err := EncodeMessage(msg) + if err != nil { + return fmt.Errorf("protocol: encode message: %w", err) + } + + if err := WriteFrame(w, payload); err != nil { + return fmt.Errorf("protocol: write frame: %w", err) + } + + return nil +} + +// ReadMessage 是给上层直接使用的完整接收路径： +// io.Reader -> 长度前缀帧 -> header+body -> Message。 +func ReadMessage(r io.Reader) (Message, error) { + payload, err := ReadFrame(r) + if err != nil { + return Message{}, fmt.Errorf("protocol: read frame: %w", err) + } + + msg, err := DecodeMessage(payload) + if err != nil { + return Message{}, fmt.Errorf("protocol: decode message: %w", err) + } + + return msg, nil +} + +// validateMessage 检查 Message 传输的类型（只接受 text 和 file ）。 +func validateMessage(msg Message) error { + if msg.From == "" { + return ErrMissingFrom + } + if msg.To == "" { + return ErrMissingTo + } + + switch msg.Type { + case MessageTypeText: + if msg.FileName != "" { + return ErrUnexpectedFileName + } + if !utf8.Valid(msg.Body) { + return ErrInvalidTextBody + } + case MessageTypeFile: + if msg.FileName == "" { + return ErrMissingFileName + } + case MessageTypeRegister: + if msg.To != ServerPeerID { + return ErrInvalidRegisterTarget + } + if msg.FileName != "" { + return ErrUnexpectedFileName + } + if len(msg.Body) != 0 { + return ErrUnexpectedBody + } + case MessageTypeError: + if msg.From != ServerPeerID { + return ErrInvalidErrorSource + } + if msg.FileName != "" { + return ErrUnexpectedFileName + } + if !utf8.Valid(msg.Body) { + return ErrInvalidTextBody + } + default: + return ErrInvalidMessageType + } + + return nil +} + +// writeFull 会持续写入，直到所有字节都写完或者底层返回错误。 +// 这样可以避免某些 Writer 发生部分写入时破坏帧格式。 +func writeFull(w io.Writer, data []byte) error { + for len(data) > 0 { + n, err := w.Write(data) + if err != nil { + return err + } + if n == 0 { + return io.ErrShortWrite + } + data = data[n:] + } + + return nil +} diff --git a/host/OmniSocketGo_add_camera/go/cmd/internal/protocol/codec_test.go b/host/OmniSocketGo_add_camera/go/cmd/internal/protocol/codec_test.go new file mode 100644 index 0000000..b229b36 --- /dev/null +++ b/host/OmniSocketGo_add_camera/go/cmd/internal/protocol/codec_test.go @@ -0,0 +1,507 @@ +package protocol + +import ( + "bytes" + "encoding/binary" + "encoding/json" + "errors" + "reflect" + "strings" + "testing" +) + +// TestEncodeDecodeMessageTextASCII 验证 ASCII 文本可以按 text 消息往返编解码。 +func TestEncodeDecodeMessageTextASCII(t *testing.T) { + original := Message{ + Type: MessageTypeText, + ID: 42, + From: "peer-a", + To: "peer-b", + Body: []byte("hello"), + } + + data, err := EncodeMessage(original) + if err != nil { + t.Fatalf("EncodeMessage() error = %v", err) + } + + decoded, err := DecodeMessage(data) + if err != nil { + t.Fatalf("DecodeMessage() error = %v", err) + } + + if !reflect.DeepEqual(decoded, original) { + t.Fatalf("round trip mismatch: got %+v want %+v", decoded, original) + } +} + +// TestEncodeDecodeMessageTextUTF8 验证 text 消息允许合法 UTF-8， +// 从而天然兼容 ASCII 之外的普通文本。 +func TestEncodeDecodeMessageTextUTF8(t *testing.T) { + original := Message{ + Type: MessageTypeText, + ID: 43, + From: "peer-a", + To: "peer-b", + Body: []byte("你好, world"), + } + + data, err := EncodeMessage(original) + if err != nil { + t.Fatalf("EncodeMessage() error = %v", err) + } + + decoded, err := DecodeMessage(data) + if err != nil { + t.Fatalf("DecodeMessage() error = %v", err) + } + + if !reflect.DeepEqual(decoded, original) { + t.Fatalf("round trip mismatch: got %+v want %+v", decoded, original) + } +} + +// TestEncodeDecodeMessageFile 验证 file 消息会保留文件名和原始二进制正文。 +func TestEncodeDecodeMessageFile(t *testing.T) { + original := Message{ + Type: MessageTypeFile, + ID: 44, + From: "peer-a", + To: "peer-b", + FileName: "data.bin", + Body: []byte{0x00, 0xff, 0x10, 0x7f}, + } + + data, err := EncodeMessage(original) + if err != nil { + t.Fatalf("EncodeMessage() error = %v", err) + } + + decoded, err := DecodeMessage(data) + if err != nil { + t.Fatalf("DecodeMessage() error = %v", err) + } + + if !reflect.DeepEqual(decoded, original) { + t.Fatalf("round trip mismatch: got %+v want %+v", decoded, original) + } +} + +// TestEncodeDecodeMessageRegister 验证 register 控制消息也能正常编解码。 +func TestEncodeDecodeMessageRegister(t *testing.T) { + original := Message{ + Type: MessageTypeRegister, + ID: 45, + From: "peer-a", + To: ServerPeerID, + Body: []byte{}, + } + + data, err := EncodeMessage(original) + if err != nil { + t.Fatalf("EncodeMessage() error = %v", err) + } + + decoded, err := DecodeMessage(data) + if err != nil { + t.Fatalf("DecodeMessage() error = %v", err) + } + + if !reflect.DeepEqual(decoded, original) { + t.Fatalf("round trip mismatch: got %+v want %+v", decoded, original) + } +} + +// TestEncodeDecodeMessageError 验证 error 控制消息会保留 UTF-8 错误文本。 +func TestEncodeDecodeMessageError(t *testing.T) { + original := Message{ + Type: MessageTypeError, + ID: 46, + From: ServerPeerID, + To: "peer-a", + Body: []byte("unknown target"), + } + + data, err := EncodeMessage(original) + if err != nil { + t.Fatalf("EncodeMessage() error = %v", err) + } + + decoded, err := DecodeMessage(data) + if err != nil { + t.Fatalf("DecodeMessage() error = %v", err) + } + + if !reflect.DeepEqual(decoded, original) { + t.Fatalf("round trip mismatch: got %+v want %+v", decoded, original) + } +} + +// TestWriteReadFrame 单独验证最底层的长度前缀帧逻辑， +// 不依赖 Message 结构，方便确认 TCP 粘包拆包问题是否被正确处理。 +func TestWriteReadFrame(t *testing.T) { + var buf bytes.Buffer + payload := []byte("header+body") + + if err := WriteFrame(&buf, payload); err != nil { + t.Fatalf("WriteFrame() error = %v", err) + } + + got, err := ReadFrame(&buf) + if err != nil { + t.Fatalf("ReadFrame() error = %v", err) + } + + if !bytes.Equal(got, payload) { + t.Fatalf("payload mismatch: got %q want %q", got, payload) + } +} + +// TestWriteReadMessageAllowsEmptyBody 验证空文本和空文件都可以正常通过协议层， +// 因为外层帧非空的前提下，空正文是合法业务内容。 +func TestWriteReadMessageAllowsEmptyBody(t *testing.T) { + tests := []struct { + name string + message Message + }{ + { + name: "empty text", + message: Message{ + Type: MessageTypeText, + ID: 1, + From: "peer-a", + To: "peer-b", + Body: []byte(""), + }, + }, + { + name: "empty file", + message: Message{ + Type: MessageTypeFile, + ID: 2, + From: "peer-a", + To: "peer-b", + FileName: "empty.txt", + Body: []byte{}, + }, + }, + } + + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + var buf bytes.Buffer + + if err := WriteMessage(&buf, tt.message); err != nil { + t.Fatalf("WriteMessage() error = %v", err) + } + + got, err := ReadMessage(&buf) + if err != nil { + t.Fatalf("ReadMessage() error = %v", err) + } + + if !reflect.DeepEqual(got, tt.message) { + t.Fatalf("round trip mismatch: got %+v want %+v", got, tt.message) + } + }) + } +} + +// TestWriteReadMessageRejectsInvalidMessages 验证协议层会在编码前拦住明显非法的消息。 +func TestWriteReadMessageRejectsInvalidMessages(t *testing.T) { + tests := []struct { + name string + message Message + wantErr error + }{ + { + name: "invalid type", + message: Message{ + Type: MessageType("unknown"), + ID: 1, + From: "peer-a", + To: "peer-b", + }, + wantErr: ErrInvalidMessageType, + }, + { + name: "missing from", + message: Message{ + Type: MessageTypeText, + ID: 2, + To: "peer-b", + }, + wantErr: ErrMissingFrom, + }, + { + name: "missing to", + message: Message{ + Type: MessageTypeText, + ID: 3, + From: "peer-a", + }, + wantErr: ErrMissingTo, + }, + { + name: "text with file name", + message: Message{ + Type: MessageTypeText, + ID: 4, + From: "peer-a", + To: "peer-b", + FileName: "bad.txt", + Body: []byte("hello"), + }, + wantErr: ErrUnexpectedFileName, + }, + { + name: "text with invalid utf8", + message: Message{ + Type: MessageTypeText, + ID: 5, + From: "peer-a", + To: "peer-b", + Body: []byte{0xff, 0xfe}, + }, + wantErr: ErrInvalidTextBody, + }, + { + name: "file without file name", + message: Message{ + Type: MessageTypeFile, + ID: 6, + From: "peer-a", + To: "peer-b", + Body: []byte{0x01}, + }, + wantErr: ErrMissingFileName, + }, + { + name: "register with wrong target", + message: Message{ + Type: MessageTypeRegister, + ID: 7, + From: "peer-a", + To: "peer-b", + }, + wantErr: ErrInvalidRegisterTarget, + }, + { + name: "register with body", + message: Message{ + Type: MessageTypeRegister, + ID: 8, + From: "peer-a", + To: ServerPeerID, + Body: []byte("unexpected"), + }, + wantErr: ErrUnexpectedBody, + }, + { + name: "error with wrong source", + message: Message{ + Type: MessageTypeError, + ID: 9, + From: "peer-a", + To: "peer-b", + Body: []byte("bad"), + }, + wantErr: ErrInvalidErrorSource, + }, + { + name: "error with file name", + message: Message{ + Type: MessageTypeError, + ID: 10, + From: ServerPeerID, + To: "peer-a", + FileName: "bad.txt", + Body: []byte("bad"), + }, + wantErr: ErrUnexpectedFileName, + }, + } + + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + _, err := EncodeMessage(tt.message) + if !errors.Is(err, tt.wantErr) { + t.Fatalf("EncodeMessage() error = %v, want %v", err, tt.wantErr) + } + }) + } +} + +// TestReadFrameRejectsInvalidLength 验证长度为 0 的帧会被当成非法输入， +// 而不是被当成一条合法的空消息。 +func TestReadFrameRejectsInvalidLength(t *testing.T) { + var buf bytes.Buffer + + if err := binary.Write(&buf, binary.BigEndian, uint32(0)); err != nil { + t.Fatalf("binary.Write() error = %v", err) + } + + _, err := ReadFrame(&buf) + if !errors.Is(err, ErrInvalidFrameLength) { + t.Fatalf("ReadFrame() error = %v, want %v", err, ErrInvalidFrameLength) + } +} + +// TestReadFrameRejectsTooLargeFrame 验证超大帧会在分配消息体前被拒绝， +// 从而保证最大长度限制真正生效。 +func TestReadFrameRejectsTooLargeFrame(t *testing.T) { + var buf bytes.Buffer + + if err := binary.Write(&buf, binary.BigEndian, uint32(MaxFrameSize+1)); err != nil { + t.Fatalf("binary.Write() error = %v", err) + } + + _, err := ReadFrame(&buf) + if !errors.Is(err, ErrFrameTooLarge) { + t.Fatalf("ReadFrame() error = %v, want %v", err, ErrFrameTooLarge) + } +} + +// TestWriteFrameRejectsEmptyPayload 验证写入端和读取端的约束保持一致： +// 既然读取端不接受 0 长度帧，写入端也不应该产生这种帧。 +func TestWriteFrameRejectsEmptyPayload(t *testing.T) { + var buf bytes.Buffer + + err := WriteFrame(&buf, nil) + if !errors.Is(err, ErrInvalidFrameLength) { + t.Fatalf("WriteFrame() error = %v, want %v", err, ErrInvalidFrameLength) + } +} + +// TestDecodeMessageRejectsInvalidHeaderLength 验证无法切出完整头部时会被立即拒绝。 +func TestDecodeMessageRejectsInvalidHeaderLength(t *testing.T) { + tests := []struct { + name string + data []byte + }{ + { + name: "too short for header len", + data: []byte{0x00, 0x00, 0x00}, + }, + { + name: "zero header len", + data: []byte{0x00, 0x00, 0x00, 0x00}, + }, + { + name: "header len exceeds payload", + data: []byte{0x00, 0x00, 0x00, 0x10, '{', '}'}, + }, + } + + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + _, err := DecodeMessage(tt.data) + if !errors.Is(err, ErrInvalidHeaderLength) { + t.Fatalf("DecodeMessage() error = %v, want %v", err, ErrInvalidHeaderLength) + } + }) + } +} + +// TestDecodeMessageRejectsInvalidHeaderJSON 验证头部 JSON 非法时能返回明确错误。 +func TestDecodeMessageRejectsInvalidHeaderJSON(t *testing.T) { + data := append([]byte{0x00, 0x00, 0x00, 0x09}, []byte("{invalid}")...) + + _, err := DecodeMessage(data) + if !errors.Is(err, ErrInvalidHeaderJSON) { + t.Fatalf("DecodeMessage() error = %v, want %v", err, ErrInvalidHeaderJSON) + } +} + +// TestDecodeMessageRejectsContentLengthMismatch 验证头部声明长度和实际正文不一致时会失败。 +func TestDecodeMessageRejectsContentLengthMismatch(t *testing.T) { + headerPayload, err := json.Marshal(messageHeader{ + Type: MessageTypeText, + ID: 7, + From: "peer-a", + To: "peer-b", + ContentLength: 10, + }) + if err != nil { + t.Fatalf("json.Marshal() error = %v", err) + } + + var data bytes.Buffer + if err := binary.Write(&data, binary.BigEndian, uint32(len(headerPayload))); err != nil { + t.Fatalf("binary.Write() error = %v", err) + } + if _, err := data.Write(headerPayload); err != nil { + t.Fatalf("data.Write(headerPayload) error = %v", err) + } + if _, err := data.Write([]byte("hello")); err != nil { + t.Fatalf("data.Write(body) error = %v", err) + } + + _, err = DecodeMessage(data.Bytes()) + if !errors.Is(err, ErrInvalidContentLength) { + t.Fatalf("DecodeMessage() error = %v, want %v", err, ErrInvalidContentLength) + } +} + +// TestReadMultipleMessages 模拟同一条流中连续写入 text 和 file， +// 验证读取端每次都能严格停在当前帧边界，不会串包。 +func TestReadMultipleMessages(t *testing.T) { + var buf bytes.Buffer + + first := Message{ + Type: MessageTypeText, + ID: 1, + From: "peer-a", + To: "peer-b", + Body: []byte("hello"), + } + + second := Message{ + Type: MessageTypeFile, + ID: 2, + From: "peer-b", + To: "peer-a", + FileName: "payload.bin", + Body: []byte{0x01, 0x02, 0x03}, + } + + if err := WriteMessage(&buf, first); err != nil { + t.Fatalf("WriteMessage(first) error = %v", err) + } + if err := WriteMessage(&buf, second); err != nil { + t.Fatalf("WriteMessage(second) error = %v", err) + } + + gotFirst, err := ReadMessage(&buf) + if err != nil { + t.Fatalf("ReadMessage(first) error = %v", err) + } + gotSecond, err := ReadMessage(&buf) + if err != nil { + t.Fatalf("ReadMessage(second) error = %v", err) + } + + if !reflect.DeepEqual(gotFirst, first) { + t.Fatalf("first message mismatch: got %+v want %+v", gotFirst, first) + } + if !reflect.DeepEqual(gotSecond, second) { + t.Fatalf("second message mismatch: got %+v want %+v", gotSecond, second) + } +} + +// TestReadMessageWrapsDecodeError 验证 ReadMessage 在返回错误时会保留解码阶段上下文。 +func TestReadMessageWrapsDecodeError(t *testing.T) { + var buf bytes.Buffer + + if err := WriteFrame(&buf, append([]byte{0x00, 0x00, 0x00, 0x09}, []byte("{invalid}")...)); err != nil { + t.Fatalf("WriteFrame() error = %v", err) + } + + _, err := ReadMessage(&buf) + if err == nil { + t.Fatal("ReadMessage() error = nil, want non-nil") + } + if !strings.Contains(err.Error(), "decode message") { + t.Fatalf("ReadMessage() error = %v, want wrapped decode error", err) + } +} diff --git a/host/OmniSocketGo_add_camera/go/cmd/internal/protocol/message.go b/host/OmniSocketGo_add_camera/go/cmd/internal/protocol/message.go new file mode 100644 index 0000000..5f5d28b --- /dev/null +++ b/host/OmniSocketGo_add_camera/go/cmd/internal/protocol/message.go @@ -0,0 +1,33 @@ +package protocol + +// MessageType 表示一条消息的传输类型。 +// v1 只区分普通文本和文件两类负载。 +type MessageType string + +const ( + // MessageTypeText 表示正文按 UTF-8 文本解释，天然兼容 ASCII。 + MessageTypeText MessageType = "text" + // MessageTypeFile 表示正文是原始文件字节。 + MessageTypeFile MessageType = "file" + // MessageTypeRegister 表示 peer 向 server 显式注册自己的身份。 + MessageTypeRegister MessageType = "register" + // MessageTypeError 表示 server 向 peer 返回错误信息。 + MessageTypeError MessageType = "error" +) + +// ServerPeerID 是协议中约定的 server 端固定标识。 +const ServerPeerID = "server" + +// Message 是 peer 和 server 共用的传输消息结构。 +// 头部元信息会被编码为 JSON，Body 则作为原始字节拼接在头部之后。 +type Message struct { + Type MessageType `json:"type"` // 消息类型，只允许 text 或 file。 + ID uint64 `json:"id"` // 由发送方生成，用于追踪消息。 + From string `json:"from"` // 发送方标识。 + To string `json:"to"` // 接收方标识。 + + // FileName 仅在 Type 为 file 时使用。 + FileName string `json:"file_name,omitempty"` + // Body 是真正传输的正文内容，不进入头部 JSON。 + Body []byte `json:"-"` +} diff --git a/host/OmniSocketGo_add_camera/go/cmd/latencysummary/main.go b/host/OmniSocketGo_add_camera/go/cmd/latencysummary/main.go new file mode 100644 index 0000000..1e5eac4 --- /dev/null +++ b/host/OmniSocketGo_add_camera/go/cmd/latencysummary/main.go @@ -0,0 +1,67 @@ +package main + +import ( + "flag" + "log" + "path/filepath" + "strings" + + "omnisocketgo/cmd/internal/latencylog" +) + +type stringListFlag []string + +func (f *stringListFlag) String() string { + return "" +} + +func (f *stringListFlag) Set(value string) error { + *f = append(*f, value) + return nil +} + +func main() { + var inputPaths stringListFlag + outputPath := flag.String("output", "latency-summary.jsonl", "output JSONL file for summarized latency metrics") + // shared-max-offset 是一个可选参数，用于在对齐输入文件的 per-file max message_id 后，排除掉最新的共享 message_id 以外的记录。它指定了要排除的共享 message_id 的数量。 + sharedMaxOffset := flag.Uint64("shared-max-offset", 1, "number of newest shared message IDs to exclude after aligning inputs by per-file max message_id") + flag.Var(&inputPaths, "input", "raw latency JSONL file path; can be provided multiple times") + flag.Parse() + + if len(inputPaths) == 0 { + log.Fatal("at least one -input raw latency log file is required") + } + + events, sharedMaxMessageID, err := latencylog.LoadEventsFromFilesWithSharedMaxOffset(inputPaths, *sharedMaxOffset) + if err != nil { + log.Fatalf("load raw latency logs: %v", err) + } + // sharedMaxMessageID 可能为 nil，表示没有可用的共享 message_id 截止值（例如因为输入文件中没有共享消息）。在这种情况下，我们将继续处理所有事件，但会记录一个警告。 + if sharedMaxMessageID != nil { + log.Printf("using shared message_id cutoff <= %d (shared-max-offset=%d)", *sharedMaxMessageID, *sharedMaxOffset) + } else { + log.Printf("no shared message_id cutoff available after applying shared-max-offset=%d", *sharedMaxOffset) + } + + summaries := latencylog.SummarizeEvents(events) + if err := latencylog.WriteSummariesJSONL(*outputPath, summaries); err != nil { + log.Fatalf("write latency summary: %v", err) + } + + chartPath := replaceFileExt(*outputPath, ".html") + if err := latencylog.WriteSummariesHTMLChart(chartPath, summaries); err != nil { + log.Fatalf("write latency chart: %v", err) + } + + log.Printf("wrote %d summarized message records to %s", len(summaries), *outputPath) + log.Printf("wrote simple latency chart to %s", chartPath) +} + +func replaceFileExt(path, ext string) string { + currentExt := filepath.Ext(path) + if currentExt == "" { + return path + ext + } + + return strings.TrimSuffix(path, currentExt) + ext +} diff --git a/host/OmniSocketGo_add_camera/go/go.mod b/host/OmniSocketGo_add_camera/go/go.mod new file mode 100644 index 0000000..8a2d2c0 --- /dev/null +++ b/host/OmniSocketGo_add_camera/go/go.mod @@ -0,0 +1,16 @@ +module omnisocketgo + +go 1.24.0 + +require github.com/xtaci/kcp-go/v5 v5.6.70 + +require ( + github.com/klauspost/cpuid/v2 v2.2.6 // indirect + github.com/klauspost/reedsolomon v1.12.0 // indirect + github.com/pkg/errors v0.9.1 // indirect + github.com/tjfoc/gmsm v1.4.1 // indirect + golang.org/x/crypto v0.45.0 // indirect + golang.org/x/net v0.47.0 // indirect + golang.org/x/sys v0.38.0 // indirect + golang.org/x/time v0.14.0 // indirect +) diff --git a/host/OmniSocketGo_add_camera/go/go.sum b/host/OmniSocketGo_add_camera/go/go.sum new file mode 100644 index 0000000..1876ec0 --- /dev/null +++ b/host/OmniSocketGo_add_camera/go/go.sum @@ -0,0 +1,98 @@ +cloud.google.com/go v0.26.0/go.mod h1:aQUYkXzVsufM+DwF1aE+0xfcU+56JwCaLick0ClmMTw= +github.com/BurntSushi/toml v0.3.1/go.mod h1:xHWCNGjB5oqiDr8zfno3MHue2Ht5sIBksp03qcyfWMU= +github.com/census-instrumentation/opencensus-proto v0.2.1/go.mod h1:f6KPmirojxKA12rnyqOA5BBL4O983OfeGPqjHWSTneU= +github.com/client9/misspell v0.3.4/go.mod h1:qj6jICC3Q7zFZvVWo7KLAzC3yx5G7kyvSDkc90ppPyw= +github.com/cncf/udpa/go v0.0.0-20191209042840-269d4d468f6f/go.mod h1:M8M6+tZqaGXZJjfX53e64911xZQV5JYwmTeXPW+k8Sc= +github.com/davecgh/go-spew v1.1.0 h1:ZDRjVQ15GmhC3fiQ8ni8+OwkZQO4DARzQgrnXU1Liz8= +github.com/davecgh/go-spew v1.1.0/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38= +github.com/envoyproxy/go-control-plane v0.9.0/go.mod h1:YTl/9mNaCwkRvm6d1a2C3ymFceY/DCBVvsKhRF0iEA4= +github.com/envoyproxy/go-control-plane v0.9.4/go.mod h1:6rpuAdCZL397s3pYoYcLgu1mIlRU8Am5FuJP05cCM98= +github.com/envoyproxy/protoc-gen-validate v0.1.0/go.mod h1:iSmxcyjqTsJpI2R4NaDN7+kN2VEUnK/pcBlmesArF7c= +github.com/golang/glog v0.0.0-20160126235308-23def4e6c14b/go.mod h1:SBH7ygxi8pfUlaOkMMuAQtPIUF8ecWP5IEl/CR7VP2Q= +github.com/golang/mock v1.1.1/go.mod h1:oTYuIxOrZwtPieC+H1uAHpcLFnEyAGVDL/k47Jfbm0A= +github.com/golang/protobuf v1.2.0/go.mod h1:6lQm79b+lXiMfvg/cZm0SGofjICqVBUtrP5yJMmIC1U= +github.com/golang/protobuf v1.3.2/go.mod h1:6lQm79b+lXiMfvg/cZm0SGofjICqVBUtrP5yJMmIC1U= +github.com/golang/protobuf v1.3.3/go.mod h1:vzj43D7+SQXF/4pzW/hwtAqwc6iTitCiVSaWz5lYuqw= +github.com/golang/protobuf v1.4.0-rc.1/go.mod h1:ceaxUfeHdC40wWswd/P6IGgMaK3YpKi5j83Wpe3EHw8= +github.com/golang/protobuf v1.4.0-rc.1.0.20200221234624-67d41d38c208/go.mod h1:xKAWHe0F5eneWXFV3EuXVDTCmh+JuBKY0li0aMyXATA= +github.com/golang/protobuf v1.4.0-rc.2/go.mod h1:LlEzMj4AhA7rCAGe4KMBDvJI+AwstrUpVNzEA03Pprs= +github.com/golang/protobuf v1.4.0-rc.4.0.20200313231945-b860323f09d0/go.mod h1:WU3c8KckQ9AFe+yFwt9sWVRKCVIyN9cPHBJSNnbL67w= +github.com/golang/protobuf v1.4.0/go.mod h1:jodUvKwWbYaEsadDk5Fwe5c77LiNKVO9IDvqG2KuDX0= +github.com/golang/protobuf v1.4.2/go.mod h1:oDoupMAO8OvCJWAcko0GGGIgR6R6ocIYbsSw735rRwI= +github.com/google/go-cmp v0.2.0/go.mod h1:oXzfMopK8JAjlY9xF4vHSVASa0yLyX7SntLO5aqRK0M= +github.com/google/go-cmp v0.3.0/go.mod h1:8QqcDgzrUqlUb/G2PQTWiueGozuR1884gddMywk6iLU= +github.com/google/go-cmp v0.3.1/go.mod h1:8QqcDgzrUqlUb/G2PQTWiueGozuR1884gddMywk6iLU= +github.com/google/go-cmp v0.4.0/go.mod h1:v8dTdLbMG2kIc/vJvl+f65V22dbkXbowE6jgT/gNBxE= +github.com/klauspost/cpuid/v2 v2.2.6 h1:ndNyv040zDGIDh8thGkXYjnFtiN02M1PVVF+JE/48xc= +github.com/klauspost/cpuid/v2 v2.2.6/go.mod h1:Lcz8mBdAVJIBVzewtcLocK12l3Y+JytZYpaMropDUws= +github.com/klauspost/reedsolomon v1.12.0 h1:I5FEp3xSwVCcEh3F5A7dofEfhXdF/bWhQWPH+XwBFno= +github.com/klauspost/reedsolomon v1.12.0/go.mod h1:EPLZJeh4l27pUGC3aXOjheaoh1I9yut7xTURiW3LQ9Y= +github.com/pkg/errors v0.9.1 h1:FEBLx1zS214owpjy7qsBeixbURkuhQAwrK5UwLGTwt4= +github.com/pkg/errors v0.9.1/go.mod h1:bwawxfHBFNV+L2hUp1rHADufV3IMtnDRdf1r5NINEl0= +github.com/pmezard/go-difflib v1.0.0 h1:4DBwDE0NGyQoBHbLQYPwSUPoCMWR5BEzIk/f1lZbAQM= +github.com/pmezard/go-difflib v1.0.0/go.mod h1:iKH77koFhYxTK1pcRnkKkqfTogsbg7gZNVY4sRDYZ/4= +github.com/prometheus/client_model v0.0.0-20190812154241-14fe0d1b01d4/go.mod h1:xMI15A0UPsDsEKsMN9yxemIoYk6Tm2C1GtYGdfGttqA= +github.com/stretchr/testify v1.6.1 h1:hDPOHmpOpP40lSULcqw7IrRb/u7w6RpDC9399XyoNd0= +github.com/stretchr/testify v1.6.1/go.mod h1:6Fq8oRcR53rry900zMqJjRRixrwX3KX962/h/Wwjteg= +github.com/tjfoc/gmsm v1.4.1 h1:aMe1GlZb+0bLjn+cKTPEvvn9oUEBlJitaZiiBwsbgho= +github.com/tjfoc/gmsm v1.4.1/go.mod h1:j4INPkHWMrhJb38G+J6W4Tw0AbuN8Thu3PbdVYhVcTE= +github.com/xtaci/kcp-go/v5 v5.6.70 h1:AYX0QZl6PqmNj2IdYGZGuBfZuDUkUfl+eHYNijCqaO0= +github.com/xtaci/kcp-go/v5 v5.6.70/go.mod h1:9O3D8WR+cyyUjGiTILYfg17vn72otWuXK2AFfqIe6CM= +github.com/xtaci/lossyconn v0.0.0-20190602105132-8df528c0c9ae h1:J0GxkO96kL4WF+AIT3M4mfUVinOCPgf2uUWYFUzN0sM= +github.com/xtaci/lossyconn v0.0.0-20190602105132-8df528c0c9ae/go.mod h1:gXtu8J62kEgmN++bm9BVICuT/e8yiLI2KFobd/TRFsE= +golang.org/x/crypto v0.0.0-20190308221718-c2843e01d9a2/go.mod h1:djNgcEr1/C05ACkg1iLfiJU5Ep61QUkGW8qpdssI0+w= +golang.org/x/crypto v0.0.0-20200622213623-75b288015ac9/go.mod h1:LzIPMQfyMNhhGPhUkYOs5KpL4U8rLKemX1yGLhDgUto= +golang.org/x/crypto v0.0.0-20201012173705-84dcc777aaee/go.mod h1:LzIPMQfyMNhhGPhUkYOs5KpL4U8rLKemX1yGLhDgUto= +golang.org/x/crypto v0.45.0 h1:jMBrvKuj23MTlT0bQEOBcAE0mjg8mK9RXFhRH6nyF3Q= +golang.org/x/crypto v0.45.0/go.mod h1:XTGrrkGJve7CYK7J8PEww4aY7gM3qMCElcJQ8n8JdX4= +golang.org/x/exp v0.0.0-20190121172915-509febef88a4/go.mod h1:CJ0aWSM057203Lf6IL+f9T1iT9GByDxfZKAQTCR3kQA= +golang.org/x/lint v0.0.0-20181026193005-c67002cb31c3/go.mod h1:UVdnD1Gm6xHRNCYTkRU2/jEulfH38KcIWyp/GAMgvoE= +golang.org/x/lint v0.0.0-20190227174305-5b3e6a55c961/go.mod h1:wehouNa3lNwaWXcvxsM5YxQ5yQlVC4a0KAMCusXpPoU= +golang.org/x/lint v0.0.0-20190313153728-d0100b6bd8b3/go.mod h1:6SW0HCj/g11FgYtHlgUYUwCkIfeOF89ocIRzGO/8vkc= +golang.org/x/net v0.0.0-20180724234803-3673e40ba225/go.mod h1:mL1N/T3taQHkDXs73rZJwtUhF3w3ftmwwsq0BUmARs4= +golang.org/x/net v0.0.0-20180826012351-8a410e7b638d/go.mod h1:mL1N/T3taQHkDXs73rZJwtUhF3w3ftmwwsq0BUmARs4= +golang.org/x/net v0.0.0-20190213061140-3a22650c66bd/go.mod h1:mL1N/T3taQHkDXs73rZJwtUhF3w3ftmwwsq0BUmARs4= +golang.org/x/net v0.0.0-20190311183353-d8887717615a/go.mod h1:t9HGtf8HONx5eT2rtn7q6eTqICYqUVnKs3thJo3Qplg= +golang.org/x/net v0.0.0-20190404232315-eb5bcb51f2a3/go.mod h1:t9HGtf8HONx5eT2rtn7q6eTqICYqUVnKs3thJo3Qplg= +golang.org/x/net v0.0.0-20201010224723-4f7140c49acb/go.mod h1:sp8m0HH+o8qH0wwXwYZr8TS3Oi6o0r6Gce1SSxlDquU= +golang.org/x/net v0.47.0 h1:Mx+4dIFzqraBXUugkia1OOvlD6LemFo1ALMHjrXDOhY= +golang.org/x/net v0.47.0/go.mod h1:/jNxtkgq5yWUGYkaZGqo27cfGZ1c5Nen03aYrrKpVRU= +golang.org/x/oauth2 v0.0.0-20180821212333-d2e6202438be/go.mod h1:N/0e6XlmueqKjAGxoOufVs8QHGRruUQn6yWY3a++T0U= +golang.org/x/sync v0.0.0-20180314180146-1d60e4601c6f/go.mod h1:RxMgew5VJxzue5/jJTE5uejpjVlOe/izrB70Jof72aM= +golang.org/x/sync v0.0.0-20181108010431-42b317875d0f/go.mod h1:RxMgew5VJxzue5/jJTE5uejpjVlOe/izrB70Jof72aM= +golang.org/x/sync v0.0.0-20190423024810-112230192c58/go.mod h1:RxMgew5VJxzue5/jJTE5uejpjVlOe/izrB70Jof72aM= +golang.org/x/sys v0.0.0-20180830151530-49385e6e1522/go.mod h1:STP8DvDyc/dI5b8T5hshtkjS+E42TnysNCUPdjciGhY= +golang.org/x/sys v0.0.0-20190215142949-d0b11bdaac8a/go.mod h1:STP8DvDyc/dI5b8T5hshtkjS+E42TnysNCUPdjciGhY= +golang.org/x/sys v0.0.0-20190412213103-97732733099d/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs= +golang.org/x/sys v0.0.0-20200930185726-fdedc70b468f/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs= +golang.org/x/sys v0.5.0/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= +golang.org/x/sys v0.38.0 h1:3yZWxaJjBmCWXqhN1qh02AkOnCQ1poK6oF+a7xWL6Gc= +golang.org/x/sys v0.38.0/go.mod h1:OgkHotnGiDImocRcuBABYBEXf8A9a87e/uXjp9XT3ks= +golang.org/x/text v0.3.0/go.mod h1:NqM8EUOU14njkJ3fqMW+pc6Ldnwhi/IjpwHt7yyuwOQ= +golang.org/x/text v0.3.3/go.mod h1:5Zoc/QRtKVWzQhOtBMvqHzDpF6irO9z98xDceosuGiQ= +golang.org/x/time v0.14.0 h1:MRx4UaLrDotUKUdCIqzPC48t1Y9hANFKIRpNx+Te8PI= +golang.org/x/time v0.14.0/go.mod h1:eL/Oa2bBBK0TkX57Fyni+NgnyQQN4LitPmob2Hjnqw4= +golang.org/x/tools v0.0.0-20180917221912-90fa682c2a6e/go.mod h1:n7NCudcB/nEzxVGmLbDWY5pfWTLqBcC2KZ6jyYvM4mQ= +golang.org/x/tools v0.0.0-20190114222345-bf090417da8b/go.mod h1:n7NCudcB/nEzxVGmLbDWY5pfWTLqBcC2KZ6jyYvM4mQ= +golang.org/x/tools v0.0.0-20190226205152-f727befe758c/go.mod h1:9Yl7xja0Znq3iFh3HoIrodX9oNMXvdceNzlUR8zjMvY= +golang.org/x/tools v0.0.0-20190311212946-11955173bddd/go.mod h1:LCzVGOaR6xXOjkQ3onu1FJEFr0SW1gC7cKk1uF8kGRs= +golang.org/x/tools v0.0.0-20190524140312-2c0ae7006135/go.mod h1:RgjU9mgBXZiqYHBnxXauZ1Gv1EHHAz9KjViQ78xBX0Q= +golang.org/x/xerrors v0.0.0-20191204190536-9bdfabe68543/go.mod h1:I/5z698sn9Ka8TeJc9MKroUUfqBBauWjQqLJ2OPfmY0= +google.golang.org/appengine v1.1.0/go.mod h1:EbEs0AVv82hx2wNQdGPgUI5lhzA/G0D9YwlJXL52JkM= +google.golang.org/appengine v1.4.0/go.mod h1:xpcJRLb0r/rnEns0DIKYYv+WjYCduHsrkT7/EB5XEv4= +google.golang.org/genproto v0.0.0-20180817151627-c66870c02cf8/go.mod h1:JiN7NxoALGmiZfu7CAH4rXhgtRTLTxftemlI0sWmxmc= +google.golang.org/genproto v0.0.0-20190819201941-24fa4b261c55/go.mod h1:DMBHOl98Agz4BDEuKkezgsaosCRResVns1a3J2ZsMNc= +google.golang.org/grpc v1.19.0/go.mod h1:mqu4LbDTu4XGKhr4mRzUsmM4RtVoemTSY81AxZiDr8c= +google.golang.org/grpc v1.23.0/go.mod h1:Y5yQAOtifL1yxbo5wqy6BxZv8vAUGQwXBOALyacEbxg= +google.golang.org/grpc v1.25.1/go.mod h1:c3i+UQWmh7LiEpx4sFZnkU36qjEYZ0imhYfXVyQciAY= +google.golang.org/grpc v1.31.0/go.mod h1:N36X2cJ7JwdamYAgDz+s+rVMFjt3numwzf/HckM8pak= +google.golang.org/protobuf v0.0.0-20200109180630-ec00e32a8dfd/go.mod h1:DFci5gLYBciE7Vtevhsrf46CRTquxDuWsQurQQe4oz8= +google.golang.org/protobuf v0.0.0-20200221191635-4d8936d0db64/go.mod h1:kwYJMbMJ01Woi6D6+Kah6886xMZcty6N08ah7+eCXa0= +google.golang.org/protobuf v0.0.0-20200228230310-ab0ca4ff8a60/go.mod h1:cfTl7dwQJ+fmap5saPgwCLgHXTUD7jkjRqWcaiX5VyM= +google.golang.org/protobuf v1.20.1-0.20200309200217-e05f789c0967/go.mod h1:A+miEFZTKqfCUM6K7xSMQL9OKL/b6hQv+e19PK+JZNE= +google.golang.org/protobuf v1.21.0/go.mod h1:47Nbq4nVaFHyn7ilMalzfO3qCViNmqZ2kzikPIcrTAo= +google.golang.org/protobuf v1.23.0/go.mod h1:EGpADcykh3NcUnDUJcl1+ZksZNG86OlYog2l/sGQquU= +gopkg.in/yaml.v3 v3.0.1 h1:fxVm/GzAzEWqLHuvctI91KS9hhNmmWOoWu0XTYJS7CA= +gopkg.in/yaml.v3 v3.0.1/go.mod h1:K4uyk7z7BCEPqu6E+C64Yfv1cQ7kz7rIZviUmN+EgEM= +honnef.co/go/tools v0.0.0-20190102054323-c2f93a96b099/go.mod h1:rf3lG4BRIbNafJWhAfAdb/ePZxsR/4RtNHQocxwk9r4= +honnef.co/go/tools v0.0.0-20190523083050-ea95bdfd59fc/go.mod h1:rf3lG4BRIbNafJWhAfAdb/ePZxsR/4RtNHQocxwk9r4= diff --git a/host/OmniSocketGo_add_camera/include/cli_parse.h b/host/OmniSocketGo_add_camera/include/cli_parse.h new file mode 100644 index 0000000..11bbf78 --- /dev/null +++ b/host/OmniSocketGo_add_camera/include/cli_parse.h @@ -0,0 +1,57 @@ +#ifndef OMNI_CLI_PARSE_H +#define OMNI_CLI_PARSE_H + +#include "omni_common.h" + +static int cli_parse_bool_text(const char *raw, int *out_value) { + if (raw == NULL || out_value == NULL) { + errno = EINVAL; + return -1; + } + if (strcmp(raw, "1") == 0 || strcmp(raw, "true") == 0 || strcmp(raw, "yes") == 0 || strcmp(raw, "on") == 0) { + *out_value = 1; + return 0; + } + if (strcmp(raw, "0") == 0 || strcmp(raw, "false") == 0 || strcmp(raw, "no") == 0 || strcmp(raw, "off") == 0) { + *out_value = 0; + return 0; + } + errno = EINVAL; + return -1; +} + +static int cli_parse_value_flag(int argc, char **argv, int *index, const char *arg, const char *flag, const char **out_value) { + size_t flag_len = strlen(flag); + + if (strcmp(arg, flag) == 0) { + if (*index + 1 >= argc) { + errno = EINVAL; + return -1; + } + *out_value = argv[++(*index)]; + return 1; + } + if (strncmp(arg, flag, flag_len) == 0 && arg[flag_len] == '=') { + *out_value = arg + flag_len + 1; + return 1; + } + return 0; +} + +static int cli_parse_bool_flag(const char *arg, const char *flag, int *out_value) { + size_t flag_len = strlen(flag); + + if (strcmp(arg, flag) == 0) { + *out_value = 1; + return 1; + } + if (strncmp(arg, flag, flag_len) == 0 && arg[flag_len] == '=') { + if (cli_parse_bool_text(arg + flag_len + 1, out_value) != 0) { + return -1; + } + return 1; + } + return 0; +} + +#endif diff --git a/host/OmniSocketGo_add_camera/include/control_protocol.h b/host/OmniSocketGo_add_camera/include/control_protocol.h new file mode 100644 index 0000000..c589f1e --- /dev/null +++ b/host/OmniSocketGo_add_camera/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 diff --git a/host/OmniSocketGo_add_camera/include/gps_buffer.h b/host/OmniSocketGo_add_camera/include/gps_buffer.h new file mode 100644 index 0000000..f38db88 --- /dev/null +++ b/host/OmniSocketGo_add_camera/include/gps_buffer.h @@ -0,0 +1,16 @@ +#ifndef GPS_BUFFER_H +#define GPS_BUFFER_H + +#include + +typedef struct gps_video_sample { + double latitude; + double longitude; +} gps_video_sample_t; + +gps_video_sample_t get_latest_gps_for_video(void); + + +int gps_buffer_init(const char* host); +void gps_buffer_cleanup(void); +#endif diff --git a/host/OmniSocketGo_add_camera/include/interactive.h b/host/OmniSocketGo_add_camera/include/interactive.h new file mode 100644 index 0000000..775f456 --- /dev/null +++ b/host/OmniSocketGo_add_camera/include/interactive.h @@ -0,0 +1,30 @@ +#ifndef OMNI_INTERACTIVE_H +#define OMNI_INTERACTIVE_H + +#include "omni_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef enum interactive_command_type { + INTERACTIVE_CMD_HELP = 0, + INTERACTIVE_CMD_QUIT = 1, + INTERACTIVE_CMD_TEXT = 2, + INTERACTIVE_CMD_FILE = 3 +} interactive_command_type_t; + +typedef struct interactive_command { + interactive_command_type_t type; + char to[OMNI_MAX_PEER_ID]; + char value[1024]; +} interactive_command_t; + +int interactive_parse_command(const char *line, interactive_command_t *command, char *err, size_t err_len); +void interactive_print_help(FILE *out, const char *transport_name); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/host/OmniSocketGo_add_camera/include/kcp_packet_debug.h b/host/OmniSocketGo_add_camera/include/kcp_packet_debug.h new file mode 100644 index 0000000..45632ee --- /dev/null +++ b/host/OmniSocketGo_add_camera/include/kcp_packet_debug.h @@ -0,0 +1,49 @@ +#ifndef OMNI_KCP_PACKET_DEBUG_H +#define OMNI_KCP_PACKET_DEBUG_H + +#include "omni_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct kcp_packet_debug_segment { + uint8_t cmd; + uint32_t sn; + uint32_t una; + uint8_t frg; + uint16_t wnd; + uint32_t len; +} kcp_packet_debug_segment_t; + +typedef struct kcp_packet_debug_record { + char event[OMNI_MAX_EVENT_NAME]; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + char local_addr[OMNI_MAX_ADDR_TEXT]; + char remote_addr[OMNI_MAX_ADDR_TEXT]; + int packet_bytes; + int has_udp_tx_id; + uint32_t udp_tx_id; + int has_kcp_conv; + uint32_t kcp_conv; + int64_t ts_unix_nano; + kcp_packet_debug_segment_t *segments; + size_t segment_count; +} kcp_packet_debug_record_t; + +typedef struct kcp_packet_debug_logger { + omni_file_logger_t file_logger; + int enabled; +} kcp_packet_debug_logger_t; + +kcp_packet_debug_logger_t *kcp_packet_debug_open_jsonl(const char *path); +void kcp_packet_debug_close(kcp_packet_debug_logger_t *logger); +int kcp_packet_debug_log(kcp_packet_debug_logger_t *logger, const kcp_packet_debug_record_t *record); +void kcp_packet_debug_record_clear(kcp_packet_debug_record_t *record); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/host/OmniSocketGo_add_camera/include/kcp_session_stats.h b/host/OmniSocketGo_add_camera/include/kcp_session_stats.h new file mode 100644 index 0000000..166f237 --- /dev/null +++ b/host/OmniSocketGo_add_camera/include/kcp_session_stats.h @@ -0,0 +1,92 @@ +#ifndef OMNI_KCP_SESSION_STATS_H +#define OMNI_KCP_SESSION_STATS_H + +#include "omni_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define KCP_SESSION_STATS_RECORD_SESSION_SAMPLE "session_sample" +#define KCP_SESSION_STATS_RECORD_PROCESS_SAMPLE "process_snmp_sample" + +typedef struct kcp_session_stats_record { + char record_type[32]; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + char local_addr[OMNI_MAX_ADDR_TEXT]; + char remote_addr[OMNI_MAX_ADDR_TEXT]; + int has_conv; + uint32_t conv; + int64_t ts_unix_nano; + char sample_reason[32]; + int has_rto_ms; + uint32_t rto_ms; + int has_srtt_ms; + int32_t srtt_ms; + int has_min_srtt_ms; + int32_t min_srtt_ms; + int has_srttvar_ms; + int32_t srttvar_ms; + int has_last_feedback_age_ms; + uint32_t last_feedback_age_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; + uint64_t bytes_received; + int has_in_pkts; + uint64_t in_pkts; + int has_out_pkts; + uint64_t out_pkts; + int has_in_segs; + uint64_t in_segs; + int has_out_segs; + uint64_t out_segs; + int has_retrans_segs; + uint64_t retrans_segs; + int has_fast_retrans_segs; + uint64_t fast_retrans_segs; + int has_early_retrans_segs; + uint64_t early_retrans_segs; + int has_lost_segs; + uint64_t lost_segs; + int has_repeat_segs; + uint64_t repeat_segs; + int has_in_errs; + uint64_t in_errs; + int has_kcp_in_errs; + uint64_t kcp_in_errs; + int has_ring_buffer_snd_queue; + uint64_t ring_buffer_snd_queue; + int has_ring_buffer_rcv_queue; + uint64_t ring_buffer_rcv_queue; + int has_ring_buffer_snd_buffer; + uint64_t ring_buffer_snd_buffer; + int has_curr_estab; + uint64_t curr_estab; +} kcp_session_stats_record_t; + +typedef struct kcp_session_stats_logger { + omni_file_logger_t file_logger; + int enabled; +} kcp_session_stats_logger_t; + +kcp_session_stats_logger_t *kcp_session_stats_open_jsonl(const char *path); +void kcp_session_stats_close(kcp_session_stats_logger_t *logger); +int kcp_session_stats_log(kcp_session_stats_logger_t *logger, const kcp_session_stats_record_t *record); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/host/OmniSocketGo_add_camera/include/latencylog.h b/host/OmniSocketGo_add_camera/include/latencylog.h new file mode 100644 index 0000000..809f515 --- /dev/null +++ b/host/OmniSocketGo_add_camera/include/latencylog.h @@ -0,0 +1,51 @@ +#ifndef OMNI_LATENCYLOG_H +#define OMNI_LATENCYLOG_H + +#include "protocol.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define EVENT_A_APP_PREP_BEGIN "A_APP_PREP_BEGIN" +#define EVENT_A_TX_SCHED "A_TX_SCHED" +#define EVENT_A_TX_SOFTWARE "A_TX_SOFTWARE" +#define EVENT_A_TX_HARDWARE "A_TX_HARDWARE" +#define EVENT_B_RX_HARDWARE "B_RX_HARDWARE" +#define EVENT_B_RX_SOFTWARE "B_RX_SOFTWARE" +#define EVENT_B_APP_RECV "B_APP_RECV" +#define EVENT_B_PERSIST_BEGIN "B_PERSIST_BEGIN" +#define EVENT_B_PERSIST_END "B_PERSIST_END" +#define EVENT_SEND_HANDOFF_BEGIN "send_handoff_begin" +#define EVENT_SEND_HANDOFF_END "send_handoff_end" + +typedef struct latency_event { + int64_t ts_unix_nano; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + char event[OMNI_MAX_EVENT_NAME]; + message_type_t message_type; + uint64_t message_id; + char from[OMNI_MAX_PEER_ID]; + char to[OMNI_MAX_PEER_ID]; + char file_name[OMNI_MAX_FILE_NAME]; + int body_size; +} latency_event_t; + +typedef struct latency_logger { + omni_file_logger_t file_logger; + int enabled; +} latency_logger_t; + +latency_logger_t *latencylog_open_jsonl(const char *path); +void latencylog_close(latency_logger_t *logger); +int latencylog_log_event(latency_logger_t *logger, const latency_event_t *event); +int latencylog_is_business_message(const message_t *msg); +void latencylog_log_message_event(latency_logger_t *logger, const char *node_role, const char *node_id, const char *event_name, const message_t *msg); +void latencylog_log_message_event_at(latency_logger_t *logger, const char *node_role, const char *node_id, const char *event_name, int64_t ts_unix_nano, const message_t *msg); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/host/OmniSocketGo_add_camera/include/linux_timestamping.h b/host/OmniSocketGo_add_camera/include/linux_timestamping.h new file mode 100644 index 0000000..0cd2572 --- /dev/null +++ b/host/OmniSocketGo_add_camera/include/linux_timestamping.h @@ -0,0 +1,25 @@ +#ifndef OMNI_LINUX_TIMESTAMPING_H +#define OMNI_LINUX_TIMESTAMPING_H + +#include "omni_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct omni_tx_timestamp_event { + char event_name[OMNI_MAX_EVENT_NAME]; + int64_t ts_unix_nano; + uint32_t ee_info; + uint32_t ee_data; +} omni_tx_timestamp_event_t; + +int linux_timestamping_enable_udp_socket(int fd, int enable_rx); +int64_t linux_timestamping_parse_rx_timestamp(const struct msghdr *msg); +int linux_timestamping_parse_tx_timestamp(const struct msghdr *msg, omni_tx_timestamp_event_t *out_event); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/host/OmniSocketGo_add_camera/include/omni_common.h b/host/OmniSocketGo_add_camera/include/omni_common.h new file mode 100644 index 0000000..09b8432 --- /dev/null +++ b/host/OmniSocketGo_add_camera/include/omni_common.h @@ -0,0 +1,78 @@ +#ifndef OMNI_COMMON_H +#define OMNI_COMMON_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define OMNI_NODE_ROLE_PEER "peer" +#define OMNI_NODE_ROLE_SERVER "server" + +#define OMNI_MAX_PEER_ID 64 +#define OMNI_MAX_NODE_ROLE 16 +#define OMNI_MAX_EVENT_NAME 64 +#define OMNI_MAX_FILE_NAME 256 +#define OMNI_MAX_ADDR_TEXT 128 +#define OMNI_MAX_FRAME_SIZE (8U * 1024U * 1024U) + +#define OMNI_ARRAY_LEN(x) (sizeof(x) / sizeof((x)[0])) + +typedef struct omni_file_logger { + FILE *file; + pthread_mutex_t mutex; + char path[PATH_MAX]; + size_t current_bytes; + size_t buffered_bytes; + size_t flush_bytes; + size_t max_bytes; + int flush_interval_ms; + int max_files; + int immediate_flush; + uint64_t last_flush_monotonic_ms; +} omni_file_logger_t; + +int64_t omni_now_unix_nano(void); +uint32_t omni_now_millis32(void); + +int omni_set_nonblocking(int fd, int enabled); +int omni_parse_sockaddr(const char *raw, int passive, struct sockaddr_storage *addr, socklen_t *addr_len, int *family_out); +int omni_clone_sockaddr(const struct sockaddr *src, socklen_t src_len, struct sockaddr_storage *dst, socklen_t *dst_len); +const char *omni_sockaddr_to_string(const struct sockaddr *addr, socklen_t addr_len, char *buffer, size_t buffer_len); + +int omni_bind_device(int fd, const char *device); +int omni_ensure_dir(const char *path); +int omni_ensure_parent_dir(const char *path); +int omni_read_file(const char *path, uint8_t **out, size_t *out_len); +int omni_write_full_fd(int fd, const uint8_t *data, size_t len); +int omni_append_file(const char *path, const uint8_t *data, size_t len); +int omni_write_file(const char *path, const uint8_t *data, size_t len); +int omni_random_u32(uint32_t *out); + +char *omni_strdup(const char *src); +char *omni_strdup_printf(const char *fmt, ...); +char *omni_json_escape(const char *src); +char *omni_json_escape_bytes(const uint8_t *src, size_t len); +int omni_utf8_valid(const uint8_t *data, size_t len); +void omni_trim_newline(char *line); +int omni_parse_duration_ms(const char *raw, int default_ms, int *out_ms); +double omni_duration_ms_to_ns(double ms); +const char *omni_path_base_name(const char *path); + +void omni_file_logger_init(omni_file_logger_t *logger, FILE *file); +void omni_file_logger_init_path(omni_file_logger_t *logger, FILE *file, const char *path, int immediate_flush); +void omni_file_logger_destroy(omni_file_logger_t *logger); +int omni_file_logger_write_line(omni_file_logger_t *logger, const char *line); + +#endif diff --git a/host/OmniSocketGo_add_camera/include/peer_kcp_client.h b/host/OmniSocketGo_add_camera/include/peer_kcp_client.h new file mode 100644 index 0000000..426ec3d --- /dev/null +++ b/host/OmniSocketGo_add_camera/include/peer_kcp_client.h @@ -0,0 +1,46 @@ +#ifndef OMNI_PEER_KCP_CLIENT_H +#define OMNI_PEER_KCP_CLIENT_H + +#include "transport_kcp.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct kcp_client kcp_client_t; +typedef struct kcp_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; +} kcp_client_recv_meta_t; +typedef struct kcp_client_state { + int connected; + int registered; + uint32_t server_idle_ms; + char last_server_error[256]; +} kcp_client_state_t; + +kcp_client_t *kcp_client_dial_with_options(const char *server_addr, const char *dial_addr, const char *peer_id, const char *bind_ip, const char *bind_device, const kcp_conn_options_t *options, latency_logger_t *logger, kcp_packet_debug_logger_t *packet_logger, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms); +kcp_client_t *kcp_client_dial(const char *server_addr, const char *dial_addr, const char *peer_id, const char *bind_ip, const char *bind_device, latency_logger_t *logger, kcp_packet_debug_logger_t *packet_logger, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms); +const char *kcp_client_id(const kcp_client_t *client); +int kcp_client_send_text(kcp_client_t *client, const char *to, const char *text); +int kcp_client_send_binary(kcp_client_t *client, const char *to, const void *data, size_t data_len); +int kcp_client_send_binary_with_id(kcp_client_t *client, const char *to, const void *data, size_t data_len, uint64_t *out_id); +int kcp_client_send_file_path(kcp_client_t *client, const char *to, const char *path); +int kcp_client_receive_timed(kcp_client_t *client, message_t *out_msg, int timeout_ms); +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); +void kcp_client_state_snapshot(kcp_client_t *client, kcp_client_state_t *out_state); +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); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/host/OmniSocketGo_add_camera/include/peer_udp_client.h b/host/OmniSocketGo_add_camera/include/peer_udp_client.h new file mode 100644 index 0000000..937e49e --- /dev/null +++ b/host/OmniSocketGo_add_camera/include/peer_udp_client.h @@ -0,0 +1,37 @@ +#ifndef OMNI_PEER_UDP_CLIENT_H +#define OMNI_PEER_UDP_CLIENT_H + +#include "transport_udp.h" + +#ifdef __cplusplus +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); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/host/OmniSocketGo_add_camera/include/protocol.h b/host/OmniSocketGo_add_camera/include/protocol.h new file mode 100644 index 0000000..a6c64ad --- /dev/null +++ b/host/OmniSocketGo_add_camera/include/protocol.h @@ -0,0 +1,62 @@ +#ifndef OMNI_PROTOCOL_H +#define OMNI_PROTOCOL_H + +#include "omni_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef enum message_type { + MSG_TYPE_TEXT = 0, + MSG_TYPE_FILE = 1, + MSG_TYPE_REGISTER = 2, + MSG_TYPE_ERROR = 3, + MSG_TYPE_BINARY = 4, + MSG_TYPE_INVALID = 255 +} message_type_t; + +#define SERVER_PEER_ID "server" + +typedef struct message { + 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]; + uint8_t *body; + size_t body_len; +} message_t; + +typedef struct protocol_frame_decoder { + uint8_t *buffer; + size_t len; + size_t cap; +} protocol_frame_decoder_t; + +const char *protocol_message_type_name(message_type_t type); +int protocol_message_type_from_name(const char *raw, message_type_t *out); + +void protocol_message_init(message_t *msg); +void protocol_message_clear(message_t *msg); +int protocol_message_copy(message_t *dst, const message_t *src); + +int protocol_validate_message(const message_t *msg, char *err, size_t err_len); + +int protocol_encode_message_datagram(const message_t *msg, uint8_t **out, size_t *out_len); +int protocol_decode_message_datagram(const uint8_t *data, size_t data_len, message_t *out_msg, char *err, size_t err_len); + +int protocol_encode_message_stream(const message_t *msg, uint8_t **out, size_t *out_len); +int protocol_decode_message_stream_payload(const uint8_t *payload, size_t payload_len, message_t *out_msg, char *err, size_t err_len); + +void protocol_frame_decoder_init(protocol_frame_decoder_t *decoder); +void protocol_frame_decoder_reset(protocol_frame_decoder_t *decoder); +void protocol_frame_decoder_destroy(protocol_frame_decoder_t *decoder); +int protocol_frame_decoder_feed(protocol_frame_decoder_t *decoder, const uint8_t *data, size_t data_len); +int protocol_frame_decoder_next(protocol_frame_decoder_t *decoder, uint8_t **payload, size_t *payload_len); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/host/OmniSocketGo_add_camera/include/server_kcp_hub.h b/host/OmniSocketGo_add_camera/include/server_kcp_hub.h new file mode 100644 index 0000000..37140df --- /dev/null +++ b/host/OmniSocketGo_add_camera/include/server_kcp_hub.h @@ -0,0 +1,27 @@ +#ifndef OMNI_SERVER_KCP_HUB_H +#define OMNI_SERVER_KCP_HUB_H + +#include "transport_kcp.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct kcp_hub kcp_hub_t; + +kcp_hub_t *kcp_hub_new(latency_logger_t *logger, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms); +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); +void kcp_hub_free(kcp_hub_t *hub); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/host/OmniSocketGo_add_camera/include/server_udp_hub.h b/host/OmniSocketGo_add_camera/include/server_udp_hub.h new file mode 100644 index 0000000..7baed3c --- /dev/null +++ b/host/OmniSocketGo_add_camera/include/server_udp_hub.h @@ -0,0 +1,21 @@ +#ifndef OMNI_SERVER_UDP_HUB_H +#define OMNI_SERVER_UDP_HUB_H + +#include "transport_udp.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct udp_hub udp_hub_t; + +udp_hub_t *udp_hub_open(const char *listen_addr, latency_logger_t *logger, tx_timestamp_debug_logger_t *debug_logger, int enable_timestamping); +int udp_hub_serve(udp_hub_t *hub); +int udp_hub_close(udp_hub_t *hub); +void udp_hub_free(udp_hub_t *hub); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/host/OmniSocketGo_add_camera/include/server_udp_relay.h b/host/OmniSocketGo_add_camera/include/server_udp_relay.h new file mode 100644 index 0000000..1c7728e --- /dev/null +++ b/host/OmniSocketGo_add_camera/include/server_udp_relay.h @@ -0,0 +1,21 @@ +#ifndef OMNI_SERVER_UDP_RELAY_H +#define OMNI_SERVER_UDP_RELAY_H + +#include "omni_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct udp_relay udp_relay_t; + +udp_relay_t *udp_relay_open(const char *listen_addr, const char *upstream_addr); +int udp_relay_serve(udp_relay_t *relay); +int udp_relay_close(udp_relay_t *relay); +void udp_relay_free(udp_relay_t *relay); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/host/OmniSocketGo_add_camera/include/transport_kcp.h b/host/OmniSocketGo_add_camera/include/transport_kcp.h new file mode 100644 index 0000000..f9e1bbc --- /dev/null +++ b/host/OmniSocketGo_add_camera/include/transport_kcp.h @@ -0,0 +1,117 @@ +#ifndef OMNI_TRANSPORT_KCP_H +#define OMNI_TRANSPORT_KCP_H + +#include "kcp_packet_debug.h" +#include "kcp_session_stats.h" +#include "latencylog.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define KCP_DEFAULT_NODELAY 1 +#define KCP_DEFAULT_INTERVAL_MS 10 +#define KCP_DEFAULT_RESEND 2 +#define KCP_DEFAULT_NC 1 +#define KCP_DEFAULT_SND_WND 256 +#define KCP_DEFAULT_RCV_WND 256 +#define KCP_DEFAULT_MTU 1400 +#define KCP_DEFAULT_STATS_INTERVAL_MS 100 + +#define KCP_CONTROL_NODELAY 1 +#define KCP_CONTROL_INTERVAL_MS 5 +#define KCP_CONTROL_RESEND 2 +#define KCP_CONTROL_NC 1 +#define KCP_CONTROL_SND_WND 32 +#define KCP_CONTROL_RCV_WND 32 +#define KCP_CONTROL_MTU 1400 + +#define KCP_VIDEO_NODELAY 1 +#define KCP_VIDEO_INTERVAL_MS 10 +#define KCP_VIDEO_RESEND 2 +#define KCP_VIDEO_NC 1 +#define KCP_VIDEO_SND_WND 256 +#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 +#define KCP_NC KCP_DEFAULT_NC +#define KCP_WND_SIZE KCP_DEFAULT_SND_WND +#define KCP_MTU KCP_DEFAULT_MTU + +typedef struct kcp_conn kcp_conn_t; +typedef struct kcp_listener kcp_listener_t; +typedef struct kcp_runtime_stats { + int connected; + uint32_t conv; + uint32_t rto_ms; + int32_t srtt_ms; + int32_t min_srtt_ms; + int32_t srttvar_ms; + uint32_t last_feedback_age_ms; + uint32_t snd_wnd; + uint32_t rmt_wnd; + uint32_t inflight; + uint32_t window_limit; + double window_pressure_pct; + uint32_t snd_queue; + uint32_t rcv_queue; + uint32_t snd_buffer; + uint64_t out_segs_total; + uint64_t retrans_total; + uint64_t fast_retrans_total; + uint64_t lost_total; + uint64_t repeat_total; + uint32_t xmit_total; +} kcp_runtime_stats_t; +typedef struct kcp_conn_options { + int nodelay; + int interval_ms; + int resend; + int nc; + int sndwnd; + int rcvwnd; + int mtu; +} kcp_conn_options_t; + +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); +int kcp_conn_configure_runtime(kcp_conn_t *conn, latency_logger_t *logger, const char *node_role, const char *node_id, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms); +int kcp_conn_apply_options(kcp_conn_t *conn, const kcp_conn_options_t *options); +int kcp_conn_send(kcp_conn_t *conn, const message_t *msg); +int kcp_conn_receive_timed(kcp_conn_t *conn, message_t *out_msg, int timeout_ms); +int kcp_conn_receive(kcp_conn_t *conn, message_t *out_msg); +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_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); +int kcp_listener_close(kcp_listener_t *listener); +void kcp_listener_free(kcp_listener_t *listener); + +int kcp_session_stats_parse_interval_ms(const char *raw, int *out_ms); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/host/OmniSocketGo_add_camera/include/transport_udp.h b/host/OmniSocketGo_add_camera/include/transport_udp.h new file mode 100644 index 0000000..54e7155 --- /dev/null +++ b/host/OmniSocketGo_add_camera/include/transport_udp.h @@ -0,0 +1,30 @@ +#ifndef OMNI_TRANSPORT_UDP_H +#define OMNI_TRANSPORT_UDP_H + +#include "latencylog.h" +#include "linux_timestamping.h" +#include "tx_timestamp_debug.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct udp_conn udp_conn_t; + +udp_conn_t *udp_conn_dial(const char *server_addr, const char *bind_ip, const char *bind_device, int enable_timestamping, latency_logger_t *logger, const char *node_role, const char *node_id, tx_timestamp_debug_logger_t *debug_logger); +udp_conn_t *udp_conn_bind(const char *listen_addr, const char *bind_device, int enable_timestamping, latency_logger_t *logger, const char *node_role, const char *node_id, tx_timestamp_debug_logger_t *debug_logger); + +int udp_conn_send(udp_conn_t *conn, const message_t *msg); +int udp_conn_send_to(udp_conn_t *conn, const message_t *msg, const struct sockaddr *addr, socklen_t addr_len); +int udp_conn_receive(udp_conn_t *conn, message_t *out_msg, struct sockaddr_storage *addr, socklen_t *addr_len); + +int udp_conn_fd(const udp_conn_t *conn); +int udp_conn_local_addr(const udp_conn_t *conn, struct sockaddr_storage *addr, socklen_t *addr_len); +int udp_conn_close(udp_conn_t *conn); +void udp_conn_free(udp_conn_t *conn); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/host/OmniSocketGo_add_camera/include/tx_timestamp_debug.h b/host/OmniSocketGo_add_camera/include/tx_timestamp_debug.h new file mode 100644 index 0000000..c5795ca --- /dev/null +++ b/host/OmniSocketGo_add_camera/include/tx_timestamp_debug.h @@ -0,0 +1,51 @@ +#ifndef OMNI_TX_TIMESTAMP_DEBUG_H +#define OMNI_TX_TIMESTAMP_DEBUG_H + +#include "protocol.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define TX_TIMESTAMP_DEBUG_RECORD_SEND_CHUNK "send_chunk" +#define TX_TIMESTAMP_DEBUG_RECORD_ERRQUEUE_EVENT "errqueue_event" + +typedef struct tx_timestamp_debug_record { + char record_type[32]; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + message_type_t message_type; + uint64_t message_id; + char from[OMNI_MAX_PEER_ID]; + char to[OMNI_MAX_PEER_ID]; + char file_name[OMNI_MAX_FILE_NAME]; + int body_size; + char phase[32]; + int send_call_index; + int frame_offset_start; + int frame_offset_end; + int bytes_written; + uint32_t expected_tx_id; + int read_index; + char event_name[OMNI_MAX_EVENT_NAME]; + int64_t ts_unix_nano; + uint32_t ee_info; + uint32_t ee_data; + int matched_send_call_index; + int selected_for_latency; +} tx_timestamp_debug_record_t; + +typedef struct tx_timestamp_debug_logger { + omni_file_logger_t file_logger; + int enabled; +} tx_timestamp_debug_logger_t; + +tx_timestamp_debug_logger_t *tx_timestamp_debug_open_jsonl(const char *path); +void tx_timestamp_debug_close(tx_timestamp_debug_logger_t *logger); +int tx_timestamp_debug_log(tx_timestamp_debug_logger_t *logger, const tx_timestamp_debug_record_t *record); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/host/OmniSocketGo_add_camera/include/video_pipeline.h b/host/OmniSocketGo_add_camera/include/video_pipeline.h new file mode 100644 index 0000000..b63e9db --- /dev/null +++ b/host/OmniSocketGo_add_camera/include/video_pipeline.h @@ -0,0 +1,105 @@ +#ifndef OMNI_VIDEO_PIPELINE_H +#define OMNI_VIDEO_PIPELINE_H + +#include +#include +#include +#include + +#include "gps_buffer.h" +#include "omni_common.h" +#include "peer_kcp_client.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined(__GNUC__) +typedef struct __attribute__((packed)) video_pipeline_packet_metadata { +#else +typedef struct video_pipeline_packet_metadata { +#endif + uint64_t timestamp_ms; + double latitude; + double longitude; + uint32_t capture_to_send_ms; +} video_pipeline_packet_metadata_t; + +typedef struct video_stage_logger { + omni_file_logger_t file_logger; + int enabled; + uint64_t sample_mod; +} video_stage_logger_t; + +typedef void (*video_pipeline_progress_fn)(void *context); + +#if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L +_Static_assert(sizeof(video_pipeline_packet_metadata_t) == 28, "video trailer metadata must be 28 bytes"); +#endif + +typedef struct video_pipeline_config { + const char *camera_device; + const char *camera_head_device; + const char *camera_waist_device; + atomic_int *active_camera; + 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; + int soft_backpressure_segments; + int hard_backpressure_segments; + int hard_backpressure_hold_ms; + int frame_stall_reconnect_ms; + kcp_session_stats_logger_t *stats_logger; + video_stage_logger_t *stage_logger; + int stats_interval_ms; + video_pipeline_progress_fn progress_callback; + void *progress_context; +} video_pipeline_config_t; + +enum { + VIDEO_CAMERA_HEAD = 0, + VIDEO_CAMERA_WAIST = 1 +}; + +typedef struct video_pipeline_stats { + pthread_mutex_t mutex; + uint64_t frames_sent; + uint64_t bytes_sent; + uint64_t send_errors; + uint64_t backpressure_drops; + uint64_t backlog_resets; + uint64_t last_frame_bytes; + uint32_t last_backlog_segments; + uint32_t last_capture_to_send_ms; + double avg_capture_to_send_ms; + int connected; + char last_error[256]; + char last_backlog_reason[128]; + kcp_runtime_stats_t transport; +} video_pipeline_stats_t; + +#define VIDEO_PIPELINE_RUN_RETRY_IMMEDIATE 2 + +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); +video_stage_logger_t *video_stage_logger_open_jsonl(const char *path, uint64_t sample_mod); +void video_stage_logger_close(video_stage_logger_t *logger); +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 diff --git a/host/OmniSocketGo_add_camera/python/omnisocket/__init__.py b/host/OmniSocketGo_add_camera/python/omnisocket/__init__.py new file mode 100644 index 0000000..2b23277 --- /dev/null +++ b/host/OmniSocketGo_add_camera/python/omnisocket/__init__.py @@ -0,0 +1,57 @@ +try: + from ._omnisocket import ( + MSG_TYPE_BINARY, + MSG_TYPE_ERROR, + MSG_TYPE_FILE, + MSG_TYPE_REGISTER, + MSG_TYPE_TEXT, + Session, + UdpSession, + ) +except ImportError as exc: + raise ImportError( + "omnisocket extension is not built; run `make python-ext` on a Linux host first" + ) from exc + +CONTROL_DEFAULTS = { + "nodelay": 1, + "interval_ms": 5, + "resend": 2, + "nc": 1, + "sndwnd": 32, + "rcvwnd": 32, + "mtu": 1400, +} + +VIDEO_DEFAULTS = { + "nodelay": 1, + "interval_ms": 10, + "resend": 2, + "nc": 1, + "sndwnd": 256, + "rcvwnd": 256, + "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", + "MSG_TYPE_FILE", + "MSG_TYPE_REGISTER", + "MSG_TYPE_TEXT", + "Session", + "UdpSession", +] diff --git a/host/OmniSocketGo_add_camera/python/omnisocket/_omnisocket.c b/host/OmniSocketGo_add_camera/python/omnisocket/_omnisocket.c new file mode 100644 index 0000000..5c59f70 --- /dev/null +++ b/host/OmniSocketGo_add_camera/python/omnisocket/_omnisocket.c @@ -0,0 +1,705 @@ +#define PY_SSIZE_T_CLEAN +#include + +#include "omnisocket_client.h" + +typedef struct PyOmniSession { + PyObject_HEAD + 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" +); + +PyDoc_STRVAR( + PyOmniSession_recv_into_doc, + "recv_into(buffer, timeout_ms=-1) -> dict | None\n" + "\n" + "The writable buffer must be large enough for the full message body.\n" + "If it is too small, BufferError reports the required size but the\n" + "current frame has already been consumed and is lost." +); + +static PyObject *build_recv_result(const message_t *msg) { + PyObject *body = NULL; + PyObject *result = NULL; + + 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,s:i,s:s}", + "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, + "registered", + stats->registered, + "last_server_error", + stats->last_server_error + ); +} + +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("min_srtt_ms", PyLong_FromLong(stats->min_srtt_ms)); + SET_KCP_STAT("srttvar_ms", PyLong_FromLong(stats->srttvar_ms)); + SET_KCP_STAT("last_feedback_age_ms", PyLong_FromUnsignedLong(stats->last_feedback_age_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; + (void) args; + (void) kwargs; + + self = (PyOmniSession *) type->tp_alloc(type, 0); + if (self == NULL) { + return NULL; + } + if (omnisocket_session_init(&self->session) != 0) { + type->tp_free((PyObject *) self); + return PyErr_SetFromErrno(PyExc_OSError); + } + return (PyObject *) self; +} + +static void PyOmniSession_dealloc(PyOmniSession *self) { + omnisocket_session_destroy(&self->session); + Py_TYPE(self)->tp_free((PyObject *) self); +} + +static PyObject *PyOmniSession_connect(PyOmniSession *self, PyObject *args, PyObject *kwargs) { + const char *server_addr; + const char *peer_id; + const char *relay_via = ""; + const char *bind_ip = ""; + const char *bind_device = ""; + int nodelay = KCP_DEFAULT_NODELAY; + int interval_ms = KCP_DEFAULT_INTERVAL_MS; + int resend = KCP_DEFAULT_RESEND; + int nc = KCP_DEFAULT_NC; + int sndwnd = KCP_DEFAULT_SND_WND; + int rcvwnd = KCP_DEFAULT_RCV_WND; + int mtu = KCP_DEFAULT_MTU; + int stats_interval_ms = KCP_DEFAULT_STATS_INTERVAL_MS; + kcp_conn_options_t options; + int rc; + + static char *kwlist[] = { + "server_addr", + "peer_id", + "relay_via", + "bind_ip", + "bind_device", + "nodelay", + "interval_ms", + "resend", + "nc", + "sndwnd", + "rcvwnd", + "mtu", + "stats_interval_ms", + NULL + }; + + if (!PyArg_ParseTupleAndKeywords( + args, + kwargs, + "ss|sssiiiiiiii", + kwlist, + &server_addr, + &peer_id, + &relay_via, + &bind_ip, + &bind_device, + &nodelay, + &interval_ms, + &resend, + &nc, + &sndwnd, + &rcvwnd, + &mtu, + &stats_interval_ms)) { + return NULL; + } + + kcp_conn_options_init(&options); + options.nodelay = nodelay; + options.interval_ms = interval_ms; + options.resend = resend; + options.nc = nc; + options.sndwnd = sndwnd; + options.rcvwnd = rcvwnd; + options.mtu = mtu; + + Py_BEGIN_ALLOW_THREADS + rc = omnisocket_session_connect( + &self->session, + server_addr, + relay_via, + peer_id, + bind_ip, + bind_device, + &options, + stats_interval_ms + ); + Py_END_ALLOW_THREADS + + if (rc != 0) { + return PyErr_SetFromErrno(PyExc_OSError); + } + Py_RETURN_NONE; +} + +static PyObject *PyOmniSession_close(PyOmniSession *self, PyObject *Py_UNUSED(ignored)) { + int rc; + + Py_BEGIN_ALLOW_THREADS + rc = omnisocket_session_close(&self->session); + Py_END_ALLOW_THREADS + + if (rc != 0) { + return PyErr_SetFromErrno(PyExc_OSError); + } + Py_RETURN_NONE; +} + +static PyObject *PyOmniSession_send(PyOmniSession *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_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 *PyOmniSession_send_text(PyOmniSession *self, PyObject *args, PyObject *kwargs) { + const char *to; + const char *text; + int rc; + static char *kwlist[] = {"to", "text", NULL}; + + if (!PyArg_ParseTupleAndKeywords(args, kwargs, "ss", kwlist, &to, &text)) { + return NULL; + } + + Py_BEGIN_ALLOW_THREADS + rc = omnisocket_session_send_text(&self->session, to, text); + Py_END_ALLOW_THREADS + + if (rc != 0) { + return PyErr_SetFromErrno(PyExc_OSError); + } + Py_RETURN_NONE; +} + +static PyObject *PyOmniSession_send_with_id(PyOmniSession *self, PyObject *args, PyObject *kwargs) { + const char *to; + Py_buffer payload; + int rc; + uint64_t message_id = 0; + 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_session_send_with_id(&self->session, to, payload.buf, (size_t) payload.len, &message_id); + Py_END_ALLOW_THREADS + + PyBuffer_Release(&payload); + if (rc != 0) { + return PyErr_SetFromErrno(PyExc_OSError); + } + return PyLong_FromUnsignedLongLong((unsigned long long) message_id); +} + +static PyObject *PyOmniSession_recv(PyOmniSession *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_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 *PyOmniSession_recv_into(PyOmniSession *self, PyObject *args, PyObject *kwargs) { + PyObject *buffer_obj; + Py_buffer view; + int timeout_ms = -1; + int rc; + kcp_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_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 *PyOmniSession_stats(PyOmniSession *self, PyObject *Py_UNUSED(ignored)) { + omnisocket_session_stats_t stats; + + memset(&stats, 0, sizeof(stats)); + omnisocket_session_stats_snapshot(&self->session, &stats); + return build_stats_dict(&stats); +} + +static PyObject *PyOmniSession_kcp_stats(PyOmniSession *self, PyObject *Py_UNUSED(ignored)) { + omnisocket_session_kcp_stats_t stats; + + memset(&stats, 0, sizeof(stats)); + omnisocket_session_kcp_stats_snapshot(&self->session, &stats); + return build_kcp_stats_dict(&stats); +} + +static PyMethodDef PyOmniSession_methods[] = { + {"connect", (PyCFunction) PyOmniSession_connect, METH_VARARGS | METH_KEYWORDS, NULL}, + {"close", (PyCFunction) PyOmniSession_close, METH_NOARGS, NULL}, + {"send", (PyCFunction) PyOmniSession_send, METH_VARARGS | METH_KEYWORDS, NULL}, + {"send_text", (PyCFunction) PyOmniSession_send_text, METH_VARARGS | METH_KEYWORDS, NULL}, + {"send_with_id", (PyCFunction) PyOmniSession_send_with_id, METH_VARARGS | METH_KEYWORDS, NULL}, + {"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_stats", (PyCFunction) PyOmniSession_kcp_stats, METH_NOARGS, NULL}, + {NULL, NULL, 0, NULL} +}; + +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", + .m_size = -1, +}; + +PyMODINIT_FUNC PyInit__omnisocket(void) { + PyObject *module; + + PyOmniSessionType.tp_name = "omnisocket.Session"; + PyOmniSessionType.tp_basicsize = sizeof(PyOmniSession); + PyOmniSessionType.tp_flags = Py_TPFLAGS_DEFAULT; + PyOmniSessionType.tp_new = PyOmniSession_new; + PyOmniSessionType.tp_dealloc = (destructor) PyOmniSession_dealloc; + PyOmniSessionType.tp_methods = PyOmniSession_methods; + + if (PyType_Ready(&PyOmniSessionType) < 0) { + 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; + } + + Py_INCREF(&PyOmniSessionType); + if (PyModule_AddObject(module, "Session", (PyObject *) &PyOmniSessionType) != 0) { + Py_DECREF(&PyOmniSessionType); + Py_DECREF(module); + 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 || + PyModule_AddIntConstant(module, "MSG_TYPE_ERROR", MSG_TYPE_ERROR) != 0 || + PyModule_AddIntConstant(module, "MSG_TYPE_BINARY", MSG_TYPE_BINARY) != 0) { + Py_DECREF(module); + return NULL; + } + + return module; +} diff --git a/host/OmniSocketGo_add_camera/python/omnisocket/omnisocket_client.c b/host/OmniSocketGo_add_camera/python/omnisocket/omnisocket_client.c new file mode 100644 index 0000000..c68bfe2 --- /dev/null +++ b/host/OmniSocketGo_add_camera/python/omnisocket/omnisocket_client.c @@ -0,0 +1,603 @@ +#include "omnisocket_client.h" + +static void omnisocket_session_sync_client_state_locked(omnisocket_session_t *session, kcp_client_t *client) { + kcp_client_state_t client_state; + + if (session == NULL) { + return; + } + memset(&client_state, 0, sizeof(client_state)); + if (client != NULL) { + kcp_client_state_snapshot(client, &client_state); + } + session->stats.connected = client_state.connected; + session->stats.registered = client_state.registered; + snprintf( + session->stats.last_server_error, + sizeof(session->stats.last_server_error), + "%s", + client_state.last_server_error + ); +} + +static void omnisocket_session_mark_disconnected_locked(omnisocket_session_t *session) { + if (session == NULL) { + return; + } + session->stats.connected = 0; + session->stats.registered = 0; +} + +int omnisocket_session_init(omnisocket_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_session_destroy(omnisocket_session_t *session) { + if (session == NULL) { + return; + } + (void) omnisocket_session_close(session); + pthread_cond_destroy(&session->idle_cond); + pthread_mutex_destroy(&session->mutex); +} + +static int omnisocket_session_begin_client_op(omnisocket_session_t *session, kcp_client_t **out_client) { + if (session == NULL || out_client == NULL) { + errno = EINVAL; + return -1; + } + + 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_session_connect( + omnisocket_session_t *session, + const char *server_addr, + const char *relay_via, + const char *peer_id, + const char *bind_ip, + const char *bind_device, + const kcp_conn_options_t *options, + int stats_interval_ms +) { + kcp_client_t *client; + + if (session == NULL || server_addr == NULL || peer_id == NULL) { + errno = EINVAL; + return -1; + } + + pthread_mutex_lock(&session->mutex); + while (session->closing) { + pthread_cond_wait(&session->idle_cond, &session->mutex); + } + if (session->client != NULL) { + pthread_mutex_unlock(&session->mutex); + errno = EISCONN; + return -1; + } + client = kcp_client_dial_with_options( + server_addr, + relay_via, + peer_id, + bind_ip, + bind_device, + options, + NULL, + NULL, + NULL, + stats_interval_ms + ); + if (client == NULL) { + pthread_mutex_unlock(&session->mutex); + return -1; + } + session->client = client; + omnisocket_session_sync_client_state_locked(session, client); + pthread_mutex_unlock(&session->mutex); + return 0; +} + +int omnisocket_session_close(omnisocket_session_t *session) { + kcp_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; + } + omnisocket_session_mark_disconnected_locked(session); + pthread_mutex_unlock(&session->mutex); + + if (client != NULL) { + kcp_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); + kcp_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_session_send(omnisocket_session_t *session, const char *to, const void *data, size_t data_len) { + return omnisocket_session_send_with_id(session, to, data, data_len, NULL); +} + +int omnisocket_session_send_text(omnisocket_session_t *session, const char *to, const char *text) { + kcp_client_t *client; + int rc; + + if (session == NULL || to == NULL || text == NULL) { + errno = EINVAL; + return -1; + } + + if (omnisocket_session_begin_client_op(session, &client) != 0) { + return -1; + } + rc = kcp_client_send_text(client, to, text); + pthread_mutex_lock(&session->mutex); + if (rc == 0) { + session->stats.send_calls += 1; + session->stats.send_bytes += (uint64_t) strlen(text); + } else { + session->stats.send_errors += 1; + } + omnisocket_session_sync_client_state_locked(session, client); + 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; +} + +int omnisocket_session_send_with_id( + omnisocket_session_t *session, + const char *to, + const void *data, + size_t data_len, + uint64_t *out_message_id +) { + kcp_client_t *client; + int rc; + + if (session == NULL || to == NULL || (data == NULL && data_len > 0)) { + errno = EINVAL; + return -1; + } + + if (omnisocket_session_begin_client_op(session, &client) != 0) { + return -1; + } + rc = kcp_client_send_binary_with_id(client, to, data, data_len, out_message_id); + 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; + } + omnisocket_session_sync_client_state_locked(session, client); + 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; +} + +int omnisocket_session_recv(omnisocket_session_t *session, message_t *out_msg, int timeout_ms) { + kcp_client_t *client; + int rc; + + if (session == NULL || out_msg == NULL) { + errno = EINVAL; + return -1; + } + + if (omnisocket_session_begin_client_op(session, &client) != 0) { + return -1; + } + rc = kcp_client_receive_timed(client, out_msg, timeout_ms); + pthread_mutex_lock(&session->mutex); + if (rc == 0) { + session->stats.recv_calls += 1; + session->stats.recv_bytes += (uint64_t) out_msg->body_len; + } else if (rc == 1) { + session->stats.recv_timeouts += 1; + } else { + session->stats.recv_errors += 1; + } + omnisocket_session_sync_client_state_locked(session, client); + 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; +} + +int omnisocket_session_recv_into( + omnisocket_session_t *session, + void *buffer, + size_t buffer_len, + kcp_client_recv_meta_t *out_meta, + int timeout_ms +) { + kcp_client_t *client; + int rc; + + if (session == NULL || out_meta == NULL || (buffer == NULL && buffer_len > 0)) { + errno = EINVAL; + return -1; + } + + if (omnisocket_session_begin_client_op(session, &client) != 0) { + return -1; + } + rc = kcp_client_receive_binary_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; + } + omnisocket_session_sync_client_state_locked(session, client); + 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_session_stats_snapshot(omnisocket_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); +} + +void omnisocket_session_kcp_stats_snapshot(omnisocket_session_t *session, omnisocket_session_kcp_stats_t *out_stats) { + kcp_runtime_stats_t runtime_stats; + + 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->min_srtt_ms = runtime_stats.min_srtt_ms; + out_stats->srttvar_ms = runtime_stats.srttvar_ms; + out_stats->last_feedback_age_ms = runtime_stats.last_feedback_age_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; + } + + 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); + while (session->closing) { + pthread_cond_wait(&session->idle_cond, &session->mutex); + } + 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; + session->stats.registered = 1; + session->stats.last_server_error[0] = '\0'; + 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; + session->stats.registered = 0; + pthread_mutex_unlock(&session->mutex); + + if (client != NULL) { + 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; + } + + if (omnisocket_udp_session_begin_client_op(session, &client) != 0) { + 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; + } + if (session->closing && session->active_ops == 0) { + pthread_cond_broadcast(&session->idle_cond); + } + pthread_mutex_unlock(&session->mutex); + return rc; +} + +int omnisocket_udp_session_recv(omnisocket_udp_session_t *session, message_t *out_msg, int timeout_ms) { + udp_client_t *client; + int rc; + + if (session == NULL || out_msg == NULL) { + errno = EINVAL; + return -1; + } + + if (omnisocket_udp_session_begin_client_op(session, &client) != 0) { + return -1; + } + rc = udp_client_receive_timed(client, out_msg, timeout_ms); + pthread_mutex_lock(&session->mutex); + if (rc == 0) { + session->stats.recv_calls += 1; + session->stats.recv_bytes += (uint64_t) out_msg->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; +} + +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 +) { + 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); +} diff --git a/host/OmniSocketGo_add_camera/python/omnisocket/omnisocket_client.h b/host/OmniSocketGo_add_camera/python/omnisocket/omnisocket_client.h new file mode 100644 index 0000000..7313b7a --- /dev/null +++ b/host/OmniSocketGo_add_camera/python/omnisocket/omnisocket_client.h @@ -0,0 +1,119 @@ +#ifndef OMNISOCKET_PY_CLIENT_H +#define OMNISOCKET_PY_CLIENT_H + +#include "peer_kcp_client.h" +#include "peer_udp_client.h" + +typedef struct omnisocket_session_stats { + uint64_t send_calls; + uint64_t send_bytes; + uint64_t send_errors; + uint64_t recv_calls; + uint64_t recv_bytes; + uint64_t recv_timeouts; + uint64_t recv_errors; + int connected; + int registered; + char last_server_error[256]; +} omnisocket_session_stats_t; + +typedef struct omnisocket_session_kcp_stats { + int connected; + uint32_t conv; + uint32_t rto_ms; + int32_t srtt_ms; + int32_t min_srtt_ms; + int32_t srttvar_ms; + uint32_t last_feedback_age_ms; + uint32_t snd_wnd; + uint32_t rmt_wnd; + uint32_t inflight; + uint32_t window_limit; + double window_pressure_pct; + uint32_t snd_queue; + uint32_t rcv_queue; + uint32_t snd_buffer; + uint64_t out_segs_total; + uint64_t retrans_total; + uint64_t fast_retrans_total; + uint64_t lost_total; + uint64_t repeat_total; + uint32_t xmit_total; +} omnisocket_session_kcp_stats_t; + +typedef struct omnisocket_session { + pthread_mutex_t mutex; + pthread_cond_t idle_cond; + kcp_client_t *client; + size_t active_ops; + int closing; + 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); + +int omnisocket_session_connect( + omnisocket_session_t *session, + const char *server_addr, + const char *relay_via, + const char *peer_id, + const char *bind_ip, + const char *bind_device, + const kcp_conn_options_t *options, + int stats_interval_ms +); +int omnisocket_session_close(omnisocket_session_t *session); +int omnisocket_session_send(omnisocket_session_t *session, const char *to, const void *data, size_t data_len); +int omnisocket_session_send_text(omnisocket_session_t *session, const char *to, const char *text); +int omnisocket_session_send_with_id( + omnisocket_session_t *session, + const char *to, + const void *data, + size_t data_len, + uint64_t *out_message_id +); +int omnisocket_session_recv(omnisocket_session_t *session, message_t *out_msg, int timeout_ms); +int omnisocket_session_recv_into( + omnisocket_session_t *session, + void *buffer, + size_t buffer_len, + kcp_client_recv_meta_t *out_meta, + int timeout_ms +); +void omnisocket_session_stats_snapshot(omnisocket_session_t *session, omnisocket_session_stats_t *out_stats); +void omnisocket_session_kcp_stats_snapshot(omnisocket_session_t *session, omnisocket_session_kcp_stats_t *out_stats); + +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 diff --git a/host/OmniSocketGo_add_camera/python/setup.py b/host/OmniSocketGo_add_camera/python/setup.py new file mode 100644 index 0000000..f302f32 --- /dev/null +++ b/host/OmniSocketGo_add_camera/python/setup.py @@ -0,0 +1,58 @@ +from pathlib import Path +import sys + +from setuptools import Extension, setup + + +ROOT = Path(__file__).resolve().parent.parent +PY_ROOT = Path(__file__).resolve().parent + +if sys.platform != "linux": + raise RuntimeError("omnisocket Python extension can only be built on Linux") + + +COMMON_SOURCES = [ + ROOT / "src" / "omni_common.c", + ROOT / "src" / "protocol.c", + ROOT / "src" / "latencylog.c", + ROOT / "src" / "tx_timestamp_debug.c", + ROOT / "src" / "kcp_packet_debug.c", + ROOT / "src" / "kcp_session_stats.c", + ROOT / "src" / "linux_timestamping.c", + ROOT / "src" / "interactive.c", + ROOT / "src" / "transport_udp.c", + ROOT / "src" / "transport_kcp.c", + ROOT / "src" / "server_udp_relay.c", + ROOT / "src" / "server_udp_hub.c", + ROOT / "src" / "server_kcp_hub.c", + ROOT / "src" / "peer_udp_client.c", + ROOT / "src" / "peer_kcp_client.c", + ROOT / "third_party" / "cjson" / "cJSON.c", + ROOT / "third_party" / "kcp" / "ikcp.c", +] + + +setup( + name="omnisocket", + version="0.1.0", + packages=["omnisocket"], + ext_modules=[ + Extension( + "omnisocket._omnisocket", + sources=[ + str(PY_ROOT / "omnisocket" / "_omnisocket.c"), + str(PY_ROOT / "omnisocket" / "omnisocket_client.c"), + *[str(path) for path in COMMON_SOURCES], + ], + include_dirs=[ + str(ROOT / "include"), + str(ROOT / "third_party" / "cjson"), + str(ROOT / "third_party" / "kcp"), + str(PY_ROOT / "omnisocket"), + ], + define_macros=[("_GNU_SOURCE", None)], + extra_compile_args=["-std=c11", "-O2", "-pthread"], + extra_link_args=["-pthread"], + ) + ], +) diff --git a/host/OmniSocketGo_add_camera/python/tests/test_sessions.py b/host/OmniSocketGo_add_camera/python/tests/test_sessions.py new file mode 100644 index 0000000..bc7db39 --- /dev/null +++ b/host/OmniSocketGo_add_camera/python/tests/test_sessions.py @@ -0,0 +1,325 @@ +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 +MSG_TYPE_TEXT = omnisocket.MSG_TYPE_TEXT +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) + + +@contextmanager +def _run_relay(listen_addr: str, remote_addr: str): + binary = ROOT / 'bin' / 'kcpserver' + if not binary.exists(): + pytest.skip(f'{binary} is not built') + + process = subprocess.Popen( + [str(binary), '-mode', 'relay', '-listen', listen_addr, '-relay-remote', remote_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, relay_via: 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) + if relay_via: + kwargs['relay_via'] = relay_via + 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['registered'] == 1 + assert receiver_stats['registered'] == 1 + assert sender_stats['send_calls'] >= 2 + assert receiver_stats['recv_calls'] >= 2 + if transport == 'kcp': + sender.send_text(to=receiver_id, text='camera:waist') + from_peer, msg_type, recv_payload = receiver.recv(timeout_ms=1000) + assert from_peer == sender_id + assert msg_type == MSG_TYPE_TEXT + assert recv_payload == b'camera:waist' + + 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_kcp_duplicate_peer_new_instance_wins() -> None: + port = _reserve_port() + listen_addr = f'127.0.0.1:{port}' + shared_peer_id = 'pytest-kcp-shared-peer' + sender_id = 'pytest-kcp-unique-sender' + + with _run_server('kcpserver', listen_addr): + original = _connect_with_retry(Session, transport='kcp', server_addr=listen_addr, peer_id=shared_peer_id) + sender = _connect_with_retry(Session, transport='kcp', server_addr=listen_addr, peer_id=sender_id) + replacement = None + + try: + replacement = _connect_with_retry(Session, transport='kcp', server_addr=listen_addr, peer_id=shared_peer_id) + replacement_stats = replacement.stats() + assert replacement_stats['connected'] == 1 + assert replacement_stats['registered'] == 1 + + with pytest.raises(OSError): + original.recv(timeout_ms=1000) + + payload = b'registered-replacement' + sender.send(to=shared_peer_id, data=payload) + from_peer, msg_type, recv_payload = replacement.recv(timeout_ms=1000) + assert from_peer == sender_id + assert msg_type == MSG_TYPE_BINARY + assert recv_payload == payload + finally: + original.close() + sender.close() + if replacement is not None: + replacement.close() + + +def test_kcp_idle_video_peers_survive_without_receive_loop() -> None: + port = _reserve_port() + listen_addr = f'127.0.0.1:{port}' + sender_id = 'peer-b-video' + receiver_id = 'pytest-kcp-video-idle-receiver' + + with _run_server('kcpserver', listen_addr): + sender = _connect_with_retry(Session, transport='kcp', server_addr=listen_addr, peer_id=sender_id) + receiver = _connect_with_retry(Session, transport='kcp', server_addr=listen_addr, peer_id=receiver_id) + + try: + time.sleep(5.0) + + payload = b'idle-video-session-still-alive' + 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 + finally: + sender.close() + receiver.close() + + +def test_kcp_peer_a_video_stale_receiver_is_evicted() -> None: + port = _reserve_port() + listen_addr = f'127.0.0.1:{port}' + receiver_id = 'peer-a-video' + + with _run_server('kcpserver', listen_addr): + receiver = _connect_with_retry(Session, transport='kcp', server_addr=listen_addr, peer_id=receiver_id) + + try: + time.sleep(5.0) + with pytest.raises(OSError): + receiver.recv(timeout_ms=1000) + finally: + receiver.close() + + +def test_kcp_relay_routes_multiple_sessions_by_conv() -> None: + hub_port = _reserve_port() + relay_port = _reserve_port() + hub_addr = f'127.0.0.1:{hub_port}' + relay_addr = f'127.0.0.1:{relay_port}' + + with _run_server('kcpserver', hub_addr): + with _run_relay(relay_addr, hub_addr): + sender = _connect_with_retry(Session, transport='kcp', server_addr=hub_addr, peer_id='pytest-relay-sender', relay_via=relay_addr) + receiver = _connect_with_retry(Session, transport='kcp', server_addr=hub_addr, peer_id='pytest-relay-receiver', relay_via=relay_addr) + chatter = _connect_with_retry(Session, transport='kcp', server_addr=hub_addr, peer_id='pytest-relay-chatter', relay_via=relay_addr) + + try: + chatter.send(to='pytest-relay-sender', data=b'chatter-primes-last-client') + from_peer, msg_type, recv_payload = sender.recv(timeout_ms=1000) + assert from_peer == 'pytest-relay-chatter' + assert msg_type == MSG_TYPE_BINARY + assert recv_payload == b'chatter-primes-last-client' + + sender.send(to='pytest-relay-receiver', data=b'relay-video-frame') + from_peer, msg_type, recv_payload = receiver.recv(timeout_ms=1000) + assert from_peer == 'pytest-relay-sender' + assert msg_type == MSG_TYPE_BINARY + assert recv_payload == b'relay-video-frame' + finally: + sender.close() + receiver.close() + chatter.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) diff --git a/host/OmniSocketGo_add_camera/ros-control-c/Makefile b/host/OmniSocketGo_add_camera/ros-control-c/Makefile new file mode 100644 index 0000000..26b692e --- /dev/null +++ b/host/OmniSocketGo_add_camera/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) diff --git a/host/OmniSocketGo_add_camera/ros-control-c/README.md b/host/OmniSocketGo_add_camera/ros-control-c/README.md new file mode 100644 index 0000000..42603d0 --- /dev/null +++ b/host/OmniSocketGo_add_camera/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 ` 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 +``` diff --git a/host/OmniSocketGo_add_camera/ros-control-c/Robot Remote Control via UDP - Implementation Plan.md b/host/OmniSocketGo_add_camera/ros-control-c/Robot Remote Control via UDP - Implementation Plan.md new file mode 100644 index 0000000..350852b --- /dev/null +++ b/host/OmniSocketGo_add_camera/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 , -p , -l , -a + + --- + 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 , -p , -d , -l , -a , -z + + --- + 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 \ No newline at end of file diff --git a/host/OmniSocketGo_add_camera/ros-control-c/common/protocol.h b/host/OmniSocketGo_add_camera/ros-control-c/common/protocol.h new file mode 100644 index 0000000..91adf5b --- /dev/null +++ b/host/OmniSocketGo_add_camera/ros-control-c/common/protocol.h @@ -0,0 +1,26 @@ +#ifndef PROTOCOL_H +#define PROTOCOL_H + +#include + +#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 */ diff --git a/host/OmniSocketGo_add_camera/ros-control-c/common/teleop_transport.c b/host/OmniSocketGo_add_camera/ros-control-c/common/teleop_transport.c new file mode 100644 index 0000000..c5d875d --- /dev/null +++ b/host/OmniSocketGo_add_camera/ros-control-c/common/teleop_transport.c @@ -0,0 +1,300 @@ +#include "teleop_transport.h" + +#include +#include +#include +#include +#include +#include + +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; + } +} diff --git a/host/OmniSocketGo_add_camera/ros-control-c/common/teleop_transport.h b/host/OmniSocketGo_add_camera/ros-control-c/common/teleop_transport.h new file mode 100644 index 0000000..6061aff --- /dev/null +++ b/host/OmniSocketGo_add_camera/ros-control-c/common/teleop_transport.h @@ -0,0 +1,59 @@ +#ifndef TELEOP_TRANSPORT_H +#define TELEOP_TRANSPORT_H + +#include +#include + +#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 */ diff --git a/host/OmniSocketGo_add_camera/ros-control-c/remote/gamepad_controller.c b/host/OmniSocketGo_add_camera/ros-control-c/remote/gamepad_controller.c new file mode 100644 index 0000000..db96133 --- /dev/null +++ b/host/OmniSocketGo_add_camera/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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; +} diff --git a/host/OmniSocketGo_add_camera/ros-control-c/remote/keyboard_controller.c b/host/OmniSocketGo_add_camera/ros-control-c/remote/keyboard_controller.c new file mode 100644 index 0000000..239dad8 --- /dev/null +++ b/host/OmniSocketGo_add_camera/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 +#include +#include +#include +#include +#include +#include +#include +#include + +#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; +} diff --git a/host/OmniSocketGo_add_camera/ros-control-c/robot/udp_ros_bridge.py b/host/OmniSocketGo_add_camera/ros-control-c/robot/udp_ros_bridge.py new file mode 100644 index 0000000..1b435d8 --- /dev/null +++ b/host/OmniSocketGo_add_camera/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 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() diff --git a/host/OmniSocketGo_add_camera/ros-control-py/README.md b/host/OmniSocketGo_add_camera/ros-control-py/README.md new file mode 100644 index 0000000..be5c392 --- /dev/null +++ b/host/OmniSocketGo_add_camera/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 消息、错误长度消息都会被丢弃并记录日志 diff --git a/host/OmniSocketGo_add_camera/ros-control-py/ROS2 Teleop over UDP.md b/host/OmniSocketGo_add_camera/ros-control-py/ROS2 Teleop over UDP.md new file mode 100644 index 0000000..976d719 --- /dev/null +++ b/host/OmniSocketGo_add_camera/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 模式有效 diff --git a/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/config/xbox_twist_joy.yaml b/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/config/xbox_twist_joy.yaml new file mode 100644 index 0000000..c48735e --- /dev/null +++ b/host/OmniSocketGo_add_camera/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 diff --git a/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/launch/keyboard_sender.launch.py b/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/launch/keyboard_sender.launch.py new file mode 100644 index 0000000..49d7667 --- /dev/null +++ b/host/OmniSocketGo_add_camera/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), + }], + ), + ]) diff --git a/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/launch/robot_udp_receiver.launch.py b/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/launch/robot_udp_receiver.launch.py new file mode 100644 index 0000000..4a9d1e5 --- /dev/null +++ b/host/OmniSocketGo_add_camera/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), + }], + ), + ]) diff --git a/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/launch/xbox_to_udp.launch.py b/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/launch/xbox_to_udp.launch.py new file mode 100644 index 0000000..d9038d0 --- /dev/null +++ b/host/OmniSocketGo_add_camera/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), + }], + ), + ]) diff --git a/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/package.xml b/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/package.xml new file mode 100644 index 0000000..fc70b79 --- /dev/null +++ b/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/package.xml @@ -0,0 +1,25 @@ + + + udp_teleop_bridge + 0.1.0 + ROS 2 OmniSocket UDP/KCP bridge for teleop TwistStamped commands. + + Codex + MIT + + ament_python + + ament_index_python + geometry_msgs + joy + launch + launch_ros + rclpy + rosidl_runtime_py + teleop_twist_joy + teleop_twist_keyboard + + + ament_python + + diff --git a/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/resource/udp_teleop_bridge b/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/resource/udp_teleop_bridge new file mode 100644 index 0000000..9cc185f --- /dev/null +++ b/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/resource/udp_teleop_bridge @@ -0,0 +1 @@ +udp_teleop_bridge diff --git a/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/setup.cfg b/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/setup.cfg new file mode 100644 index 0000000..8f79a94 --- /dev/null +++ b/host/OmniSocketGo_add_camera/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 diff --git a/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/setup.py b/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/setup.py new file mode 100644 index 0000000..ae42c8d --- /dev/null +++ b/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/setup.py @@ -0,0 +1,34 @@ +from setuptools import find_packages, setup + + +package_name = 'udp_teleop_bridge' + + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', [f'resource/{package_name}']), + (f'share/{package_name}', ['package.xml']), + (f'share/{package_name}/launch', [ + 'launch/keyboard_sender.launch.py', + 'launch/robot_udp_receiver.launch.py', + 'launch/xbox_to_udp.launch.py', + ]), + (f'share/{package_name}/config', ['config/xbox_twist_joy.yaml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Codex', + maintainer_email='codex@example.com', + description='ROS 2 OmniSocket UDP/KCP bridge for teleop TwistStamped commands.', + license='MIT', + 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', + ], + }, +) diff --git a/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/test/test_protocol.py b/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/test/test_protocol.py new file mode 100644 index 0000000..87cfbe1 --- /dev/null +++ b/host/OmniSocketGo_add_camera/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') diff --git a/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/__init__.py b/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/__init__.py new file mode 100644 index 0000000..094feaf --- /dev/null +++ b/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/__init__.py @@ -0,0 +1 @@ +"""OmniSocket teleop bridge package.""" diff --git a/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/cmd_vel_udp_sender.py b/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/cmd_vel_udp_sender.py new file mode 100644 index 0000000..34a5ce1 --- /dev/null +++ b/host/OmniSocketGo_add_camera/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() diff --git a/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/omni_transport.py b/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/omni_transport.py new file mode 100644 index 0000000..3978ac3 --- /dev/null +++ b/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/omni_transport.py @@ -0,0 +1,101 @@ +"""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 send_with_id(self, *, to: str, data: bytes) -> int: + if not hasattr(self._session, 'send_with_id'): + self._session.send(to=to, data=data) + raise RuntimeError('send_with_id is not available on this omnisocket build') + return int(self._session.send_with_id(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', +] diff --git a/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/protocol.py b/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/protocol.py new file mode 100644 index 0000000..44f4641 --- /dev/null +++ b/host/OmniSocketGo_add_camera/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)) diff --git a/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/topic_status_reader.py b/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/topic_status_reader.py new file mode 100644 index 0000000..8d0f8d1 --- /dev/null +++ b/host/OmniSocketGo_add_camera/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() diff --git a/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/udp_cmd_vel_receiver.py b/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/udp_cmd_vel_receiver.py new file mode 100644 index 0000000..8eac4fb --- /dev/null +++ b/host/OmniSocketGo_add_camera/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/udp_cmd_vel_receiver.py @@ -0,0 +1,480 @@ +"""ROS 2 node that receives OmniSocket teleop packets and republishes TwistStamped.""" + +from __future__ import annotations + +import json +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 = self._create_transport() + + 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._runtime_dir = os.getenv('BLITZ_RUNTIME_DIR', '/run/blitz-robot').strip() or '/run/blitz-robot' + self._status_path = os.path.join(self._runtime_dir, 'ros-receiver.status.json') + self._transport_reconnect_count = 0 + self._recv_thread_heartbeat_epoch_ms = self._now_epoch_ms() + self._runtime_last_error = '' + + self.create_timer(1.0 / self._publish_rate_hz, self._publish_tick) + self.create_timer(1.0, self._write_status_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 _close_unix_socket(self) -> None: + if self._unix_socket is not None: + try: + self._unix_socket.close() + except OSError: + pass + self._unix_socket = None + + def _create_transport(self): + from .omni_transport import OmniTransport + + return OmniTransport( + transport=self._transport_name, + server_addr=self._server_addr, + relay_via=self._relay_via, + peer_id=self._peer_id, + ) + + def _reconnect_transport(self) -> bool: + while not self._closing.is_set() and rclpy.ok(): + current_transport = self._transport + if current_transport is not None: + try: + current_transport.close() + except OSError: + pass + try: + self._transport = self._create_transport() + self._transport_reconnect_count += 1 + self._set_runtime_last_error('') + if self._should_log('transport_reconnected', 1.0): + self.get_logger().info( + 'Reconnected OmniSocket transport %s://%s as %s' + % (self._transport_name, self._server_addr, self._peer_id) + ) + return True + except OSError as exc: + self._transport = None + self._set_runtime_last_error(str(exc)) + if self._should_log('transport_reconnect_error', 2.0): + self.get_logger().error(f'Failed to reconnect OmniSocket transport: {exc}') + time.sleep(0.5) + return False + + def _rebind_unix_socket(self) -> bool: + while not self._closing.is_set() and rclpy.ok(): + self._close_unix_socket() + try: + self._setup_unix_socket() + self._transport_reconnect_count += 1 + self._set_runtime_last_error('') + if self._should_log('unix_rebound', 1.0): + self.get_logger().info(f'Rebound unix datagram socket at {self._local_socket_path}') + return True + except OSError as exc: + self._set_runtime_last_error(str(exc)) + if self._should_log('unix_rebind_error', 2.0): + self.get_logger().error(f'Failed to rebind unix datagram socket: {exc}') + time.sleep(0.5) + return False + + 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 _now_epoch_ms(self) -> int: + return time.time_ns() // 1_000_000 + + def _update_recv_heartbeat(self) -> None: + with self._lock: + self._recv_thread_heartbeat_epoch_ms = self._now_epoch_ms() + + def _last_packet_age_ms(self) -> int | None: + with self._lock: + last_packet_monotonic = self._last_packet_monotonic + if last_packet_monotonic is None: + return None + return max(0, int((time.monotonic() - last_packet_monotonic) * 1000.0)) + + def _socket_bound(self) -> bool: + if self._transport_name == 'unix_dgram': + return self._unix_socket is not None and os.path.exists(self._local_socket_path) + return self._transport is not None + + def _set_runtime_last_error(self, message: str) -> None: + self._runtime_last_error = message + + def _status_payload(self) -> dict[str, object]: + with self._lock: + recv_thread_heartbeat_epoch_ms = self._recv_thread_heartbeat_epoch_ms + return { + 'updated_at_epoch_ms': self._now_epoch_ms(), + 'pid': os.getpid(), + 'recv_thread_heartbeat_epoch_ms': recv_thread_heartbeat_epoch_ms, + 'transport': self._transport_name, + 'local_socket_path': self._local_socket_path, + 'socket_bound': self._socket_bound(), + 'transport_reconnect_count': self._transport_reconnect_count, + 'last_packet_age_ms': self._last_packet_age_ms(), + 'last_error': self._runtime_last_error, + } + + def _write_status_tick(self) -> None: + payload = self._status_payload() + if self._transport_name == 'unix_dgram': + if self._unix_socket is None: + payload['last_error'] = self._runtime_last_error or 'unix datagram socket is not bound' + else: + if self._transport is None: + payload['last_error'] = self._runtime_last_error or 'OmniSocket transport is not connected' + try: + os.makedirs(self._runtime_dir, exist_ok=True) + temp_path = f'{self._status_path}.tmp.{os.getpid()}' + with open(temp_path, 'w', encoding='utf-8') as handle: + json.dump(payload, handle, ensure_ascii=True, separators=(',', ':')) + os.replace(temp_path, self._status_path) + except OSError as exc: + if self._should_log('status_write_error', 5.0): + self.get_logger().warning(f'Failed to write receiver status file: {exc}') + + 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(): + self._update_recv_heartbeat() + try: + assert self._transport is not None + meta = self._transport.recv_into(buffer=self._recv_buffer, timeout_ms=100) + except BufferError as exc: + self._set_runtime_last_error(str(exc)) + if self._should_log('buffer_error', 2.0): + self.get_logger().warning(f'Dropped oversized OmniSocket frame: {exc}') + continue + except OSError as exc: + self._set_runtime_last_error(str(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}') + if not self._reconnect_transport(): + return + continue + + self._update_recv_heartbeat() + if meta is None: + continue + self._set_runtime_last_error('') + + 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._set_runtime_last_error(f'server error message from {from_peer}') + self._handle_error_message(from_peer, body_len) + continue + + if self._expected_sender and from_peer != self._expected_sender: + self._set_runtime_last_error(f'unexpected sender {from_peer}') + 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: + self._set_runtime_last_error(f'unexpected message type {msg_type}') + 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: + self._set_runtime_last_error(f'invalid payload size {body_len}') + 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: + self._set_runtime_last_error(str(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() + self._set_runtime_last_error('') + + def _recv_loop_unix_dgram(self) -> None: + assert self._unix_socket is not None + + while not self._closing.is_set() and rclpy.ok(): + self._update_recv_heartbeat() + try: + payload = self._unix_socket.recv(DEFAULT_RECV_BUFFER_BYTES) + except socket.timeout: + if not os.path.exists(self._local_socket_path): + self._set_runtime_last_error('unix datagram socket path disappeared') + if self._should_log('unix_socket_missing', 2.0): + self.get_logger().warning( + f'Unix datagram socket path disappeared, rebinding {self._local_socket_path}' + ) + if not self._rebind_unix_socket(): + return + continue + except OSError as exc: + self._set_runtime_last_error(str(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}') + if not self._rebind_unix_socket(): + return + continue + + self._update_recv_heartbeat() + if len(payload) != PACKET_SIZE: + self._set_runtime_last_error(f'invalid unix datagram payload size {len(payload)}') + 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: + self._set_runtime_last_error(str(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() + self._set_runtime_last_error('') + + 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._close_unix_socket() + except OSError as exc: + if self._should_log('unix_close_error', 2.0): + self.get_logger().warning(f'Closing unix socket failed: {exc}') + 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() diff --git a/host/OmniSocketGo_add_camera/scripts/BACDauto_test.sh b/host/OmniSocketGo_add_camera/scripts/BACDauto_test.sh new file mode 100644 index 0000000..70f7300 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/BACDauto_test.sh @@ -0,0 +1,296 @@ +#!/bin/bash + +LOCAL_REPO_DIR="/home/limingjie/LMJ_Work/RobotCompetition/OmniSocketGo" +KCP_PEER_BIN="./bin/kcppeer" +OUTPUT_ROOT="/home/limingjie/LMJ_Work/RobotCompetition/KCPData/BDAClogs" +PEERB_POLL_INTERVAL_SEC=5 +PEERB_MAX_POLLS=180 +PEER_A_EXIT_WAIT_SEC=5 + +require_local_binary() { + if [ ! -x "$1" ]; then + echo "ERROR: 缺少可执行文件 $1" + exit 1 + fi +} + +cleanup_remote_peerb() { + ssh omni-peer bash -s <<'EOF' +pids=$(ps -eo pid=,args= | awk '/[b]in\/kcppeer -id peer-b/ {print $1}') +if [ -n "$pids" ]; then + kill $pids 2>/dev/null || true +fi +pids=$(ps -eo pid=,args= | awk '/\/tmp\/peerb_batch\.sh/ {print $1}') +if [ -n "$pids" ]; then + kill $pids 2>/dev/null || true +fi +rm -f /tmp/peerb_batch_done /tmp/peerb_batch.sh /tmp/peerb_commands +EOF +} + +echo "=== 开始自动化测试 ===" + +cd "$LOCAL_REPO_DIR" +require_local_binary "$KCP_PEER_BIN" + +echo ">>> 0. 清理上次残留进程..." +pkill -f 'bin/kcppeer -id peer-a' 2>/dev/null || true +cleanup_remote_peerb || exit 1 + +rm -rf logs +rm -rf inbox/a +mkdir -p logs inbox/a + +# 1. 清理残留 & 启动 Server D 和 Relay C +echo ">>> 1. 启动 Server D 和 Relay C..." + +ssh bj-txy bash -s <<'EOF' +pkill -f kcpserver 2>/dev/null || true +pkill -f 'bin/kcpserver' 2>/dev/null || true +sleep 1 +cd /home/ubuntu/OmniSocketGo +rm -rf logs +mkdir -p logs +if [ ! -x ./bin/kcpserver ]; then + echo "ERROR: 缺少 ./bin/kcpserver" + 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 || true +pkill -f 'bin/kcpserver' 2>/dev/null || true +sleep 1 +cd /home/ubuntu/OmniSocketGo +rm -rf logs +mkdir -p logs +if [ ! -x ./bin/kcpserver ]; then + echo "ERROR: 缺少 ./bin/kcpserver" + exit 1 +fi +setsid ./bin/kcpserver -mode=relay -listen 0.0.0.0:10909 -relay-remote 172.21.32.15:10909 > relay_console.log 2>&1 /dev/null; then + echo " Server D 就绪 (${i}s)" + break + fi + if [ "$i" -eq 60 ]; then + echo " ERROR: Server D 60s 内未就绪，退出" + exit 1 + fi + sleep 1 +done + +# 等待 relay C 端口就绪 +echo " 等待 Relay C 端口就绪..." +for i in $(seq 1 60); do + if ssh sz-txy "ss -ulnp | grep -q 10909" 2>/dev/null; then + echo " Relay C 就绪 (${i}s)" + break + fi + if [ "$i" -eq 60 ]; then + echo " ERROR: Relay C 60s 内未就绪，退出" + exit 1 + fi + sleep 1 +done + +# 2. 启动本地 Peer-A +echo ">>> 2. 启动本地 Peer-A..." +PEER_A_CMD_FIFO="/tmp/peera_commands_$$" +rm -f "$PEER_A_CMD_FIFO" +mkfifo "$PEER_A_CMD_FIFO" +nohup "$KCP_PEER_BIN" \ + -id peer-a \ + -server 172.21.32.15:10909 \ + -relay-via 106.55.173.235:10909 \ + -inbox-dir inbox/a \ + -latency-log logs/a-latency.jsonl \ + -kcp-ts-debug-log logs/a-kcp-ts.jsonl \ + -kcp-session-stats-log logs/a-kcp-stats.jsonl \ + < "$PEER_A_CMD_FIFO" > logs/peera_console.log 2>&1 & +PEER_A_PID=$! +exec 4>"$PEER_A_CMD_FIFO" + +# 等待 peer-a 注册成功 +echo " 等待 Peer-A 注册..." +for i in $(seq 1 30); do + if grep -Eq "opened KCP session as peer-a|connected to .* as peer-a( \\(KCP\\))?" logs/peera_console.log 2>/dev/null; then + echo " Peer-A 就绪 (${i}s)" + break + fi + if [ "$i" -eq 30 ]; then + echo " WARNING: Peer-A 30s 内未就绪" + fi + sleep 1 +done + +# 3. 在远端后台启动 peer-b 整个发送流程，不依赖长 SSH 连接 +echo ">>> 3. 启动远端 Peer-B 并执行 50 轮打流测试..." +ssh omni-peer "cd /home/boll/LMJWork/OmniSocketGo && rm -rf logs inbox/b && mkdir -p logs inbox/b" + +PEERB_DONE_FLAG="/tmp/peerb_batch_done" +PEERB_BATCH_SCRIPT="/tmp/peerb_batch.sh" + +# 把整个发送脚本写到远端，setsid 后台执行 +ssh omni-peer bash -s <<'DEPLOY_SCRIPT' +DONE_FLAG="/tmp/peerb_batch_done" +BATCH_SCRIPT="/tmp/peerb_batch.sh" +rm -f "$DONE_FLAG" + +cat > "$BATCH_SCRIPT" <<'INNER_EOF' +#!/bin/bash +cd /home/boll/LMJWork/OmniSocketGo + +CMD_FIFO=/tmp/peerb_commands +DONE_FLAG="/tmp/peerb_batch_done" +STATUS="error" +rm -f "$CMD_FIFO" "$DONE_FLAG" +mkfifo "$CMD_FIFO" + +finish() { + local status_to_write="$STATUS" + rm -f "$CMD_FIFO" + printf '%s\n' "$status_to_write" > "$DONE_FLAG" +} + +trap finish EXIT + +if [ ! -x ./bin/kcppeer ]; then + echo "ERROR: 缺少 ./bin/kcppeer" > logs/peerb_console.log + exit 1 +fi + +# 启动 peer-b +./bin/kcppeer \ + -id peer-b \ + -server 81.70.156.140:10909 \ + -inbox-dir inbox/b \ + -latency-log logs/b-latency.jsonl \ + -kcp-ts-debug-log logs/b-kcp-ts.jsonl \ + -kcp-session-stats-log logs/b-kcp-stats.jsonl \ + < "$CMD_FIFO" > logs/peerb_console.log 2>&1 & +PEER_B_PID=$! + +exec 3>"$CMD_FIFO" + +# 等 peer-b 就绪 +for i in $(seq 1 60); do + if grep -Eq "opened KCP session as peer-b|connected to .* as peer-b( \\(KCP\\))?" logs/peerb_console.log 2>/dev/null; then + break + fi + sleep 1 +done + +# 50 轮发送 +for i in $(seq 1 50); do + echo "file peer-a /home/boll/test30.bin" >&3 + sleep 1 + echo "file peer-a /home/boll/test5.bin" >&3 + sleep 1 +done + +sleep 5 +echo "quit" >&3 || true +exec 3>&- + +peer_b_exited=0 +for i in $(seq 1 15); do + if ! kill -0 $PEER_B_PID 2>/dev/null; then + peer_b_exited=1 + break + fi + sleep 1 +done + +if [ "$peer_b_exited" -eq 0 ]; then + kill $PEER_B_PID 2>/dev/null || true + sleep 1 +fi + +if kill -0 $PEER_B_PID 2>/dev/null; then + kill -9 $PEER_B_PID 2>/dev/null || true +fi + +wait $PEER_B_PID 2>/dev/null || true + +# 写完成标记 +STATUS="done" +INNER_EOF + +chmod +x "$BATCH_SCRIPT" +setsid bash "$BATCH_SCRIPT" /dev/null 2>&1 & +echo "peer-b batch launched in background" +DEPLOY_SCRIPT + +# 本地轮询等待远端完成（短 SSH 连接，不怕断开） +echo " 等待 peer-b 发送完成（预计 ~110 秒）..." +for i in $(seq 1 "$PEERB_MAX_POLLS"); do + PEERB_STATUS=$(ssh omni-peer "cat /tmp/peerb_batch_done 2>/dev/null || true") + if [ "$PEERB_STATUS" = "done" ]; then + ELAPSED_SEC=$(( (i - 1) * PEERB_POLL_INTERVAL_SEC )) + echo " peer-b 发送完成（约 ${ELAPSED_SEC}s）" + break + fi + if [ "$PEERB_STATUS" = "error" ]; then + echo " ERROR: peer-b 后台任务启动失败，请检查远端 logs/peerb_console.log" + exit 1 + fi + if [ "$i" -eq "$PEERB_MAX_POLLS" ]; then + echo " ERROR: peer-b $((PEERB_MAX_POLLS * PEERB_POLL_INTERVAL_SEC))s 内未完成" + exit 1 + fi + # 每 5 秒查一次，减少 SSH 连接频率 + sleep "$PEERB_POLL_INTERVAL_SEC" +done + +# 4. 清理 +echo ">>> 4. 清理所有进程..." +sleep 2 +echo "quit" >&4 || true +exec 4>&- +for i in $(seq 1 "$PEER_A_EXIT_WAIT_SEC"); do + if ! kill -0 "$PEER_A_PID" 2>/dev/null; then + break + fi + sleep 1 +done +if kill -0 "$PEER_A_PID" 2>/dev/null; then + kill "$PEER_A_PID" 2>/dev/null || true +fi +wait "$PEER_A_PID" 2>/dev/null || true +rm -f "$PEER_A_CMD_FIFO" +ssh bj-txy "pkill -f kcpserver || true; pkill -f 'bin/kcpserver' || true" +ssh sz-txy "pkill -f kcpserver || true; pkill -f 'bin/kcpserver' || true" + +# 获取当前时间戳，格式为 YYYYMMDD_HHMMSS +TIMESTAMP=$(date +"%Y%m%d_%H%M%S") +OUTPUT_DIR="$OUTPUT_ROOT/$TIMESTAMP" +# 5. 拉取数据 & 生成报告 +echo ">>> 5. 拉取数据并生成汇总报告..." +mkdir -p "$OUTPUT_DIR" +scp -o ServerAliveInterval=15 -P 10022 boll@175.178.116.187:/home/boll/LMJWork/OmniSocketGo/logs/b-latency.jsonl "$LOCAL_REPO_DIR/logs/b-latency.jsonl" || exit 1 + +(cd "$LOCAL_REPO_DIR/go" && go run ./cmd/latencysummary \ + -input /home/limingjie/LMJ_Work/RobotCompetition/OmniSocketGo/logs/a-latency.jsonl \ + -input /home/limingjie/LMJ_Work/RobotCompetition/OmniSocketGo/logs/b-latency.jsonl \ + -output /home/limingjie/LMJ_Work/RobotCompetition/OmniSocketGo/logs/latency-summary.jsonl) || exit 1 +cd "$LOCAL_REPO_DIR/.." || exit 1 +mv "$LOCAL_REPO_DIR/logs/a-latency.jsonl" "$OUTPUT_DIR/a-latency.jsonl" || exit 1 +mv "$LOCAL_REPO_DIR/logs/b-latency.jsonl" "$OUTPUT_DIR/b-latency.jsonl" || exit 1 +mv "$LOCAL_REPO_DIR/logs/latency-summary.jsonl" "$OUTPUT_DIR/latency-summary.jsonl" || exit 1 +if [ -f "$LOCAL_REPO_DIR/logs/latency-summary.html" ]; then + mv "$LOCAL_REPO_DIR/logs/latency-summary.html" "$OUTPUT_DIR/latency-summary.html" || exit 1 +fi + +echo "=== 测试完成！===" diff --git a/host/OmniSocketGo_add_camera/scripts/BDAanto_test.sh b/host/OmniSocketGo_add_camera/scripts/BDAanto_test.sh new file mode 100644 index 0000000..dab1169 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/BDAanto_test.sh @@ -0,0 +1,259 @@ +#!/bin/bash + +LOCAL_REPO_DIR="/home/limingjie/LMJ_Work/RobotCompetition/OmniSocketGo" +KCP_PEER_BIN="./bin/kcppeer" +OUTPUT_ROOT="/home/limingjie/LMJ_Work/RobotCompetition/KCPData/BCAlogs/" +PEERB_POLL_INTERVAL_SEC=5 +PEERB_MAX_POLLS=180 +PEER_A_EXIT_WAIT_SEC=5 + +require_local_binary() { + if [ ! -x "$1" ]; then + echo "ERROR: 缺少可执行文件 $1" + exit 1 + fi +} + +cleanup_remote_peerb() { + ssh omni-peer bash -s <<'EOF' +pids=$(ps -eo pid=,args= | awk '/[b]in\/kcppeer -id peer-b/ {print $1}') +if [ -n "$pids" ]; then + kill $pids 2>/dev/null || true +fi +pids=$(ps -eo pid=,args= | awk '/\/tmp\/peerb_batch\.sh/ {print $1}') +if [ -n "$pids" ]; then + kill $pids 2>/dev/null || true +fi +rm -f /tmp/peerb_batch_done /tmp/peerb_batch.sh /tmp/peerb_commands +EOF +} + +echo "=== 开始自动化测试 ===" + +cd "$LOCAL_REPO_DIR" +require_local_binary "$KCP_PEER_BIN" + +echo ">>> 0. 清理上次残留进程..." +pkill -f 'bin/kcppeer -id peer-a' 2>/dev/null || true +cleanup_remote_peerb || exit 1 + +rm -rf logs +rm -rf inbox/a +mkdir -p logs inbox/a + +# 1. 清理残留 & 启动 Server D +echo ">>> 1. 启动 Server D..." + +ssh bj-txy bash -s <<'EOF' +pkill -f kcpserver 2>/dev/null || true +pkill -f 'bin/kcpserver' 2>/dev/null || true +sleep 1 +cd /home/ubuntu/OmniSocketGo +rm -rf logs +mkdir -p logs +if [ ! -x ./bin/kcpserver ]; then + echo "ERROR: 缺少 ./bin/kcpserver" + 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; then + echo " Server D 就绪 (${i}s)" + break + fi + if [ "$i" -eq 60 ]; then + echo " ERROR: Server D 60s 内未就绪，退出" + exit 1 + fi + sleep 1 +done + +# 2. 启动本地 Peer-A +echo ">>> 2. 启动本地 Peer-A..." +PEER_A_CMD_FIFO="/tmp/peera_commands_$$" +rm -f "$PEER_A_CMD_FIFO" +mkfifo "$PEER_A_CMD_FIFO" +nohup "$KCP_PEER_BIN" \ + -id peer-a \ + -server 81.70.156.140:10909 \ + -inbox-dir inbox/a \ + -latency-log logs/a-latency.jsonl \ + -kcp-ts-debug-log logs/a-kcp-ts.jsonl \ + -kcp-session-stats-log logs/a-kcp-stats.jsonl \ + < "$PEER_A_CMD_FIFO" > logs/peera_console.log 2>&1 & +PEER_A_PID=$! +exec 4>"$PEER_A_CMD_FIFO" + +# 等待 peer-a 注册成功 +echo " 等待 Peer-A 注册..." +for i in $(seq 1 30); do + if grep -Eq "opened KCP session as peer-a|connected to .* as peer-a( \\(KCP\\))?" logs/peera_console.log 2>/dev/null; then + echo " Peer-A 就绪 (${i}s)" + break + fi + if [ "$i" -eq 30 ]; then + echo " WARNING: Peer-A 30s 内未就绪" + fi + sleep 1 +done + +# 3. 在远端后台启动 peer-b 整个发送流程，不依赖长 SSH 连接 +echo ">>> 3. 启动远端 Peer-B 并执行 50 轮打流测试..." +ssh omni-peer "cd /home/boll/LMJWork/OmniSocketGo && rm -rf logs inbox/b && mkdir -p logs inbox/b" + +PEERB_DONE_FLAG="/tmp/peerb_batch_done" +PEERB_BATCH_SCRIPT="/tmp/peerb_batch.sh" + +# 把整个发送脚本写到远端，setsid 后台执行 +ssh omni-peer bash -s <<'DEPLOY_SCRIPT' +DONE_FLAG="/tmp/peerb_batch_done" +BATCH_SCRIPT="/tmp/peerb_batch.sh" +rm -f "$DONE_FLAG" + +cat > "$BATCH_SCRIPT" <<'INNER_EOF' +#!/bin/bash +cd /home/boll/LMJWork/OmniSocketGo + +CMD_FIFO=/tmp/peerb_commands +DONE_FLAG="/tmp/peerb_batch_done" +rm -f "$CMD_FIFO" "$DONE_FLAG" +mkfifo "$CMD_FIFO" + +if [ ! -x ./bin/kcppeer ]; then + echo "ERROR: 缺少 ./bin/kcppeer" > logs/peerb_console.log + echo "error" > "$DONE_FLAG" + exit 1 +fi + +# 启动 peer-b +./bin/kcppeer \ + -id peer-b \ + -server 81.70.156.140:10909 \ + -inbox-dir inbox/b \ + -latency-log logs/b-latency.jsonl \ + -kcp-ts-debug-log logs/b-kcp-ts.jsonl \ + -kcp-session-stats-log logs/b-kcp-stats.jsonl \ + < "$CMD_FIFO" > logs/peerb_console.log 2>&1 & +PEER_B_PID=$! + +exec 3>"$CMD_FIFO" + +# 等 peer-b 就绪 +for i in $(seq 1 60); do + if grep -Eq "opened KCP session as peer-b|connected to .* as peer-b( \\(KCP\\))?" logs/peerb_console.log 2>/dev/null; then + break + fi + sleep 1 +done + +# 50 轮发送 +for i in $(seq 1 50); do + echo "file peer-a /home/boll/test30.bin" >&3 + sleep 1 + echo "file peer-a /home/boll/test5.bin" >&3 + sleep 1 +done + +sleep 5 +echo "quit" >&3 +exec 3>&- +rm -f "$CMD_FIFO" + +peer_b_exited=0 +for i in $(seq 1 15); do + if ! kill -0 $PEER_B_PID 2>/dev/null; then + peer_b_exited=1 + break + fi + sleep 1 +done + +if [ "$peer_b_exited" -eq 0 ]; then + kill $PEER_B_PID 2>/dev/null || true + sleep 1 +fi + +if kill -0 $PEER_B_PID 2>/dev/null; then + kill -9 $PEER_B_PID 2>/dev/null || true +fi + +wait $PEER_B_PID 2>/dev/null || true + +# 写完成标记 +echo "done" > "$DONE_FLAG" +INNER_EOF + +chmod +x "$BATCH_SCRIPT" +setsid bash "$BATCH_SCRIPT" /dev/null 2>&1 & +echo "peer-b batch launched in background" +DEPLOY_SCRIPT + +# 本地轮询等待远端完成（短 SSH 连接，不怕断开） +echo " 等待 peer-b 发送完成（预计 ~110 秒）..." +for i in $(seq 1 "$PEERB_MAX_POLLS"); do + PEERB_STATUS=$(ssh omni-peer "cat /tmp/peerb_batch_done 2>/dev/null || true") + if [ "$PEERB_STATUS" = "done" ]; then + ELAPSED_SEC=$(( (i - 1) * PEERB_POLL_INTERVAL_SEC )) + echo " peer-b 发送完成（约 ${ELAPSED_SEC}s）" + break + fi + if [ "$PEERB_STATUS" = "error" ]; then + echo " ERROR: peer-b 后台任务启动失败，请检查远端 logs/peerb_console.log" + exit 1 + fi + if [ "$i" -eq "$PEERB_MAX_POLLS" ]; then + echo " ERROR: peer-b $((PEERB_MAX_POLLS * PEERB_POLL_INTERVAL_SEC))s 内未完成" + exit 1 + fi + sleep "$PEERB_POLL_INTERVAL_SEC" +done + +# 4. 清理 +echo ">>> 4. 清理所有进程..." +sleep 2 +echo "quit" >&4 || true +exec 4>&- +for i in $(seq 1 "$PEER_A_EXIT_WAIT_SEC"); do + if ! kill -0 "$PEER_A_PID" 2>/dev/null; then + break + fi + sleep 1 +done +if kill -0 "$PEER_A_PID" 2>/dev/null; then + kill "$PEER_A_PID" 2>/dev/null || true +fi +wait "$PEER_A_PID" 2>/dev/null || true +rm -f "$PEER_A_CMD_FIFO" +ssh bj-txy "pkill -f kcpserver || true; pkill -f 'bin/kcpserver' || true" + +# 获取当前时间戳，格式为 YYYYMMDD_HHMMSS +TIMESTAMP=$(date +"%Y%m%d_%H%M%S") +OUTPUT_DIR="$OUTPUT_ROOT/$TIMESTAMP" + +# 5. 拉取数据 & 生成报告 +echo ">>> 5. 拉取数据并生成汇总报告..." +mkdir -p "$OUTPUT_DIR" +scp -o ServerAliveInterval=15 -P 10022 boll@175.178.116.187:/home/boll/LMJWork/OmniSocketGo/logs/b-latency.jsonl "$LOCAL_REPO_DIR/logs/b-latency.jsonl" || exit 1 + +(cd "$LOCAL_REPO_DIR/go" && go run ./cmd/latencysummary \ + -input "$LOCAL_REPO_DIR/logs/a-latency.jsonl" \ + -input "$LOCAL_REPO_DIR/logs/b-latency.jsonl" \ + -output "$LOCAL_REPO_DIR/logs/latency-summary.jsonl") || exit 1 + +cd "$LOCAL_REPO_DIR/.." || exit 1 +mv "$LOCAL_REPO_DIR/logs/a-latency.jsonl" "$OUTPUT_DIR/a-latency.jsonl" || exit 1 +mv "$LOCAL_REPO_DIR/logs/b-latency.jsonl" "$OUTPUT_DIR/b-latency.jsonl" || exit 1 +mv "$LOCAL_REPO_DIR/logs/latency-summary.jsonl" "$OUTPUT_DIR/latency-summary.jsonl" || exit 1 +if [ -f "$LOCAL_REPO_DIR/logs/latency-summary.html" ]; then + mv "$LOCAL_REPO_DIR/logs/latency-summary.html" "$OUTPUT_DIR/latency-summary.html" || exit 1 +fi + +echo "=== 测试完成！===" diff --git a/host/OmniSocketGo_add_camera/scripts/boot/5g-dial.sh b/host/OmniSocketGo_add_camera/scripts/boot/5g-dial.sh new file mode 100644 index 0000000..e2c07c7 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/5g-dial.sh @@ -0,0 +1,180 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="5g-dial" + +append_route_targets() { + local raw_list="$1" + local target + + if [[ -z "${raw_list}" ]]; then + return 0 + fi + + for target in ${raw_list//,/ }; do + if [[ -z "${target}" ]]; then + continue + fi + dial_cmd+=(--route-target "${target}") + done +} + +read_detected_interface() { + local info_json="$1" + + if [[ ! -f "${info_json}" ]]; then + return 1 + fi + + python3 -c 'import json, sys; print((json.load(open(sys.argv[1], encoding="utf-8")).get("interface") or "").strip())' "${info_json}" +} + +disable_interfaces() { + local raw_list="$1" + local iface + local nmcli_available=0 + + if [[ -z "${raw_list}" ]]; then + return 0 + fi + if command -v nmcli >/dev/null 2>&1; then + nmcli_available=1 + fi + + for iface in ${raw_list//,/ }; do + if [[ -z "${iface}" ]]; then + continue + fi + blitz_log "${STEP}" "disable-interface" "start" "iface=${iface}" 0 + if [[ "${nmcli_available}" -eq 1 ]]; then + nmcli device disconnect "${iface}" >/dev/null 2>&1 || true + fi + if ip link show dev "${iface}" >/dev/null 2>&1; then + if ip link set dev "${iface}" down; then + blitz_log "${STEP}" "disable-interface" "success" "iface=${iface}" 0 + else + rc=$? + blitz_log "${STEP}" "disable-interface" "failure" "iface=${iface}" "${rc}" + return "${rc}" + fi + else + blitz_log "${STEP}" "disable-interface" "success" "iface=${iface} not present, skipping" 0 + fi + done +} + +wait_for_serial() { + local serial_port="$1" + local timeout_sec="$2" + local waited=0 + + while (( waited < timeout_sec )); do + if [[ -e "${serial_port}" ]]; then + blitz_log "${STEP}" "wait-serial" "success" "serial_port=${serial_port} waited_sec=${waited}" 0 + return 0 + fi + if (( waited == 0 || waited % 5 == 0 )); then + blitz_log "${STEP}" "wait-serial" "waiting" "serial_port=${serial_port} waited_sec=${waited}" 0 + fi + sleep 1 + waited=$(( waited + 1 )) + done + + blitz_log "${STEP}" "wait-serial" "failure" "serial_port=${serial_port} timeout_sec=${timeout_sec}" 1 + return 1 +} + +wait_for_route() { + local target_ip="$1" + local timeout_sec="$2" + local expected_interface="${3:-}" + local waited=0 + local route_output + + while (( waited < timeout_sec )); do + route_output="$(blitz_route_ready "${target_ip}" "${expected_interface}" || true)" + if [[ -n "${route_output}" ]]; then + blitz_log "${STEP}" "route-check" "success" "target_ip=${target_ip} interface=${expected_interface:-auto} route=${route_output}" 0 + return 0 + fi + if (( waited == 0 || waited % 5 == 0 )); then + blitz_log "${STEP}" "route-check" "waiting" "target_ip=${target_ip} interface=${expected_interface:-auto} waited_sec=${waited}" 0 + fi + sleep 1 + waited=$(( waited + 1 )) + done + + blitz_log "${STEP}" "route-check" "failure" "target_ip=${target_ip} interface=${expected_interface:-auto} timeout_sec=${timeout_sec}" 1 + return 1 +} + +blitz_load_boot_env +blitz_require_root "${STEP}" +blitz_require_command ip "${STEP}" +blitz_require_command python3 "${STEP}" +blitz_require_file "${BLITZ_5G_DIAL_DIR}/rndis_dial.py" "${STEP}" + +if [[ -z "${BLITZ_TIME_SERVER_IP}" ]]; then + blitz_log "${STEP}" "precheck" "failure" "BLITZ_TIME_SERVER_IP is empty and no fallback could be derived" 1 + exit 1 +fi + +disable_interfaces "${BLITZ_5G_DISABLE_INTERFACES:-}" + +if [[ -n "${BLITZ_5G_INTERFACE:-}" ]]; then + route_output="$(blitz_route_ready "${BLITZ_TIME_SERVER_IP}" "${BLITZ_5G_INTERFACE}" || true)" + if [[ -n "${route_output}" ]]; then + blitz_log "${STEP}" "dial" "already_up" "target_ip=${BLITZ_TIME_SERVER_IP} interface=${BLITZ_5G_INTERFACE} route=${route_output}" 0 + exit 0 + fi +else + blitz_log "${STEP}" "route-check" "info" "BLITZ_5G_INTERFACE is empty, skipping pre-dial route shortcut and using auto-detect mode" 0 +fi + +wait_for_serial "${BLITZ_5G_SERIAL_PORT}" "${BLITZ_5G_SERIAL_WAIT_SEC}" + +dial_cmd=( + python3 + rndis_dial.py + --serial-port "${BLITZ_5G_SERIAL_PORT}" + --modem-subnet "${BLITZ_5G_MODEM_SUBNET}" +) +if [[ -n "${BLITZ_5G_INTERFACE:-}" ]]; then + dial_cmd+=(--interface "${BLITZ_5G_INTERFACE}") +fi +case "${BLITZ_5G_SKIP_DHCP:-0}" in + 1|true|TRUE|yes|YES) + dial_cmd+=(--skip-dhcp) + ;; +esac +case "${BLITZ_5G_REMOVE_DEFAULT_ROUTE:-1}" in + 1|true|TRUE|yes|YES) + dial_cmd+=(--remove-default-route --gateway "${BLITZ_5G_GATEWAY}" --route-target "${BLITZ_TIME_SERVER_IP}") + append_route_targets "${BLITZ_5G_ROUTE_TARGETS:-}" + ;; +esac + +pushd "${BLITZ_5G_DIAL_DIR}" >/dev/null +blitz_run "${STEP}" "dial" "${dial_cmd[@]}" +popd >/dev/null + +resolved_interface="${BLITZ_5G_INTERFACE:-}" +if [[ -z "${resolved_interface}" ]]; then + resolved_interface="$(read_detected_interface "${BLITZ_5G_INFO_JSON}" || true)" + if [[ -n "${resolved_interface}" ]]; then + blitz_log "${STEP}" "resolve-interface" "success" "resolved interface from ${BLITZ_5G_INFO_JSON}: ${resolved_interface}" 0 + else + blitz_log "${STEP}" "resolve-interface" "failure" "failed to read detected interface from ${BLITZ_5G_INFO_JSON}" 1 + fi +fi + +if [[ -n "${resolved_interface}" ]]; then + wait_for_route "${BLITZ_TIME_SERVER_IP}" "${BLITZ_5G_ROUTE_WAIT_SEC}" "${resolved_interface}" + blitz_log "${STEP}" "complete" "success" "5G dial completed and route is ready on ${resolved_interface}" 0 +else + blitz_log "${STEP}" "complete" "success" "5G dial completed but route wait was skipped because no interface could be resolved; refer to rndis_dial.py logs" 0 +fi diff --git a/host/OmniSocketGo_add_camera/scripts/boot/README.md b/host/OmniSocketGo_add_camera/scripts/boot/README.md new file mode 100644 index 0000000..ab75d33 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/README.md @@ -0,0 +1,219 @@ +# Robot B-Side Boot Chain + +This directory contains the robot-side boot and recovery scripts. + +Normal usage is: + +```bash +sudo bash scripts/boot/install-systemd.sh +sudo systemctl start blitz-robot.target +``` + +After installation, `blitz-robot.target` is enabled and will start automatically on reboot. + +To stop the chain now and disable boot-time autostart for future reboots: + +```bash +sudo bash scripts/boot/disable-systemd.sh +``` + +## Current Startup Order + +The current cold-start chain is: + +1. `blitz-boot-gate.service` +2. `blitz-5g-dial.service` +3. `blitz-ros-receiver.service` +4. `blitz-b-side-omnid.service` +5. `blitz-watchdog.service` + +There is no longer any automatic time-sync step in the boot chain. + +## What Each Script Does + +- `robot-boot.env`: default boot configuration +- `robot-boot.env.local`: machine-local overrides +- `common.sh`: shared env loading, logging, and helper functions +- `boot-gate.sh`: fixed startup delay gate +- `5g-dial.sh`: brings up the 5G modem path and verifies routing +- `start-ros-receiver-service.sh`: boot wrapper for ROS receiver +- `wait-for-unix-socket.sh`: waits for the ROS receiver unix socket +- `start-b-side-omnid-service.sh`: boot wrapper for `b_side_omnid` +- `blitz-watchdog.sh`: runtime health watchdog and recovery orchestrator +- `blitz-fault-inject.sh`: fault injection entrypoint +- `install-systemd.sh`: installs systemd units into `/etc/systemd/system` +- `disable-systemd.sh`: stops the boot chain and disables autostart + +## Important Configuration + +Most machine-specific overrides should go into: + +```text +scripts/boot/robot-boot.env.local +``` + +Typical settings: + +```bash +BLITZ_BOOT_DELAY_SEC="30" +BLITZ_LOG_FILE="/var/log/blitz-robot/startup.log" +BLITZ_RUNTIME_DIR="/run/blitz-robot" + +BLITZ_5G_DIAL_DIR="${OMNISOCKETGO_ROOT}/scripts/boot" +BLITZ_5G_SERIAL_PORT="/dev/ttyUSB2" +BLITZ_5G_INTERFACE="" +BLITZ_5G_MODEM_SUBNET="192.168.224.0/22" +BLITZ_5G_GATEWAY="192.168.225.1" +BLITZ_5G_REMOVE_DEFAULT_ROUTE="1" +BLITZ_5G_ROUTE_TARGETS="106.55.173.235" +BLITZ_5G_INFO_JSON="${OMNISOCKETGO_ROOT}/scripts/boot/modem_network_info.json" + +BLITZ_TIME_SERVER_IP="81.70.156.140" + +BLITZ_ROS_USER="nvidia" +BLITZ_ROS_SOCKET_WAIT_SEC="20" +BLITZ_WATCHDOG_INTERVAL_SEC="5" +BLITZ_HEALTH_STALE_SEC="15" +BLITZ_OMNID_THREAD_HEARTBEAT_TIMEOUT_SEC="15" +BLITZ_NETWORK_FAIL_THRESHOLD="3" +BLITZ_NETWORK_RECOVERY_COOLDOWN_SEC="30" +BLITZ_GPS_MONITOR_ENABLED="1" +BLITZ_GPS_DEVICE_GLOB="/dev/ttyCH341USB*" +BLITZ_GPS_CHECK_INTERVAL_SEC="10" +BLITZ_GPS_RESTART_UNITS="gpsd.socket gpsd.service" +BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION="0" +``` + +`BLITZ_TIME_SERVER_IP` is still used, but only as the 5G route/ping health-check target. It is no longer used for automatic clock synchronization. + +If `BLITZ_TIME_SERVER_IP` is left empty, the scripts fall back to the host part of `ROBOT_SIDE_OMNISOCKET_SERVER_ADDR`. + +## Install Or Upgrade + +Run: + +```bash +sudo bash scripts/boot/install-systemd.sh +sudo systemctl daemon-reload +sudo systemctl restart blitz-robot.target +``` + +`install-systemd.sh` will also remove any old `blitz-time-sync.service` unit left over from earlier versions. + +## Disable Autostart + +To stop the currently running services and disable autostart for future reboots: + +```bash +sudo bash scripts/boot/disable-systemd.sh +``` + +To re-enable later: + +```bash +sudo bash scripts/boot/install-systemd.sh +sudo systemctl start blitz-robot.target +``` + +## Logs + +All boot-chain and watchdog logs are appended to: + +```text +/var/log/blitz-robot/startup.log +``` + +Follow the log live: + +```bash +sudo tail -f /var/log/blitz-robot/startup.log +``` + +Check service state: + +```bash +sudo systemctl status blitz-robot.target +sudo systemctl status blitz-5g-dial.service +sudo systemctl status blitz-ros-receiver.service +sudo systemctl status blitz-b-side-omnid.service +sudo systemctl status blitz-watchdog.service +``` + +Check systemd journal: + +```bash +sudo journalctl -u blitz-robot.target -u blitz-5g-dial.service \ + -u blitz-ros-receiver.service -u blitz-b-side-omnid.service \ + -u blitz-watchdog.service -f +``` + +## Runtime Status Files + +The runtime status directory is: + +```text +/run/blitz-robot +``` + +Key files: + +- `b-side-omnid.status.json` +- `ros-receiver.status.json` +- `watchdog.status.json` + +`watchdog.status.json` now also records `gps_ok` and `gps_device_present` so you can quickly tell whether the GPS USB serial node is currently visible and whether the last `gpsd` reconnect attempt succeeded. + +Pretty-print them: + +```bash +sudo python3 -m json.tool /run/blitz-robot/watchdog.status.json +sudo python3 -m json.tool /run/blitz-robot/b-side-omnid.status.json +sudo python3 -m json.tool /run/blitz-robot/ros-receiver.status.json +``` + +## Fault Injection + +Available test commands: + +```bash +sudo bash scripts/boot/blitz-fault-inject.sh bside-crash +sudo bash scripts/boot/blitz-fault-inject.sh bside-process-freeze +sudo bash scripts/boot/blitz-fault-inject.sh bside-video-thread-stall +sudo bash scripts/boot/blitz-fault-inject.sh bside-control-thread-stall +sudo bash scripts/boot/blitz-fault-inject.sh ros-crash +sudo bash scripts/boot/blitz-fault-inject.sh ros-freeze +``` + +For synthetic network fault injection, first enable it in `robot-boot.env.local`: + +```bash +BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION="1" +``` + +Then restart watchdog and inject: + +```bash +sudo systemctl restart blitz-watchdog.service +sudo bash scripts/boot/blitz-fault-inject.sh network-down on +sudo bash scripts/boot/blitz-fault-inject.sh network-down off +``` + +## Recovery Behavior Summary + +- If `b_side_omnid` dies or its status file goes stale, watchdog first tries a targeted `b_side` restart. +- If ROS receiver dies, loses its socket, or its heartbeat goes stale, watchdog performs an ordered full restart: + - stop `b_side` + - restart ROS receiver + - wait for unix socket + - start `b_side` +- If network checks fail repeatedly, watchdog stops `b_side`, runs `5g-dial.sh`, waits for route recovery, and then restores services. +- While 5G is healthy, watchdog keeps every host route listed by `BLITZ_TIME_SERVER_IP` and `BLITZ_5G_ROUTE_TARGETS` pinned to the resolved 5G interface. When 5G becomes unhealthy, watchdog deletes those host routes so traffic can fall back to the remaining default network path. If that fallback path is still reachable, watchdog keeps `b_side_omnid` running instead of treating it as a full network outage. +- Whenever watchdog changes or restores those host routes, it logs `route-path` lines for each target so you can see which interface Linux currently chooses for `81.70.156.140`, `106.55.173.235`, and any other configured 5G-pinned target. +- If GPS monitoring is enabled, watchdog checks `BLITZ_GPS_DEVICE_GLOB` every `BLITZ_GPS_CHECK_INTERVAL_SEC` seconds. When the GPS serial device disappears and later reappears, watchdog restarts the units in `BLITZ_GPS_RESTART_UNITS` so `gpsd` can bind to the new device node again. +- Camera disappearance is logged as degraded state. Reappearance triggers a `b_side` restart after the device is stable. + +## Notes + +- `time-sync.sh` and `blitz-time-sync.service` are intentionally removed from the automatic boot path. +- `b_side_omnid` must already be built before boot-time startup. +- `bin/b_side_omnid` missing, ROS env missing, or modem script missing will all show up in `startup.log`. diff --git a/host/OmniSocketGo_add_camera/scripts/boot/blitz-5g-link-logger.sh b/host/OmniSocketGo_add_camera/scripts/boot/blitz-5g-link-logger.sh new file mode 100644 index 0000000..bfcdfcd --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/blitz-5g-link-logger.sh @@ -0,0 +1,137 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="5g-link-logger" + +resolve_target_ip() { + if [[ -n "${BLITZ_TIME_SERVER_IP:-}" ]]; then + printf '%s\n' "${BLITZ_TIME_SERVER_IP}" + return 0 + fi + + for candidate in ${BLITZ_5G_ROUTE_TARGETS//,/ }; do + if [[ -n "${candidate}" ]]; then + printf '%s\n' "${candidate}" + return 0 + fi + done + return 1 +} + +emit_sample_json() { + local interface_name="${1:-}" + local target_ip="${2:-}" + + python3 - "${interface_name}" "${target_ip}" <<'PY' +import json +import subprocess +import sys +import time + +interface_name = sys.argv[1] +target_ip = sys.argv[2] + +payload = { + "ts_unix_ms": time.time_ns() // 1_000_000, + "interface": interface_name, + "target_ip": target_ip, + "link_present": False, + "route_output": "", + "route_ok": False, + "probe_ok": False, + "ping_rtt_ms": None, + "rx_bytes": 0, + "tx_bytes": 0, + "rx_packets": 0, + "tx_packets": 0, + "rx_errors": 0, + "tx_errors": 0, + "rx_drops": 0, + "tx_drops": 0, +} + +if interface_name: + try: + output = subprocess.check_output( + ["ip", "-j", "-s", "link", "show", "dev", interface_name], + text=True, + stderr=subprocess.DEVNULL, + ) + stats = json.loads(output) + if stats: + item = stats[0] + payload["link_present"] = True + rx = item.get("stats64", {}).get("rx", {}) + tx = item.get("stats64", {}).get("tx", {}) + if not rx and not tx: + rx = item.get("stats", {}).get("rx", {}) + tx = item.get("stats", {}).get("tx", {}) + payload["rx_bytes"] = int(rx.get("bytes") or 0) + payload["tx_bytes"] = int(tx.get("bytes") or 0) + payload["rx_packets"] = int(rx.get("packets") or 0) + payload["tx_packets"] = int(tx.get("packets") or 0) + payload["rx_errors"] = int(rx.get("errors") or 0) + payload["tx_errors"] = int(tx.get("errors") or 0) + payload["rx_drops"] = int(rx.get("dropped") or 0) + payload["tx_drops"] = int(tx.get("dropped") or 0) + except Exception: + pass + +if target_ip: + try: + route = subprocess.check_output( + ["ip", "route", "get", target_ip], + text=True, + stderr=subprocess.STDOUT, + ).strip() + payload["route_output"] = route.splitlines()[0] if route else "" + payload["route_ok"] = bool(payload["route_output"]) and ( + not interface_name or f" dev {interface_name}" in payload["route_output"] + ) + except Exception as exc: + payload["route_output"] = str(exc) + + ping_cmd = ["ping", "-c", "1", "-W", "2", target_ip] + if interface_name: + ping_cmd[1:1] = ["-I", interface_name] + ping = subprocess.run(ping_cmd, capture_output=True, text=True) + payload["probe_ok"] = ping.returncode == 0 + output = (ping.stdout or "") + "\n" + (ping.stderr or "") + for token in output.replace("\n", " ").split(): + if token.startswith("time="): + value = token.split("=", 1)[1].rstrip("ms") + try: + payload["ping_rtt_ms"] = float(value) + except ValueError: + pass + break + +print(json.dumps(payload, separators=(",", ":"), ensure_ascii=False)) +PY +} + +if [[ "${OMNI_BOOT_MODE:-0}" == "1" ]]; then + blitz_load_boot_env + blitz_require_run_context +fi + +if [[ -z "${BLITZ_RUN_DIR:-}" && -f "${BLITZ_RUN_CONTEXT_FILE:-}" ]]; then + blitz_load_run_context_env || true +fi +blitz_ensure_instance_id + +export BLITZ_5G_LINK_LOG_PATH="${BLITZ_5G_LINK_LOG_PATH:-${BLITZ_RUN_DIR}/b-5g-link-quality.${BLITZ_INSTANCE_ID}.jsonl}" +target_ip="$(resolve_target_ip || true)" + +blitz_log "${STEP}" "start" "start" "path=${BLITZ_5G_LINK_LOG_PATH} interval_sec=${BLITZ_5G_LINK_LOG_INTERVAL_SEC}" 0 + +while true; do + interface_name="$(blitz_resolve_5g_interface || true)" + line="$(emit_sample_json "${interface_name}" "${target_ip}")" + blitz_jsonl_append_line "${BLITZ_5G_LINK_LOG_PATH}" "${line}" + sleep "${BLITZ_5G_LINK_LOG_INTERVAL_SEC}" +done diff --git a/host/OmniSocketGo_add_camera/scripts/boot/blitz-fault-inject.sh b/host/OmniSocketGo_add_camera/scripts/boot/blitz-fault-inject.sh new file mode 100644 index 0000000..8ec1b2f --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/blitz-fault-inject.sh @@ -0,0 +1,139 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="fault-inject" +B_SIDE_SERVICE="blitz-b-side-omnid.service" +ROS_SERVICE="blitz-ros-receiver.service" + +main_pid_for_service() { + local service_name="$1" + systemctl show --property MainPID --value "${service_name}" +} + +wait_for_service_pid_change() { + local service_name="$1" + local previous_pid="$2" + local timeout_sec="${3:-10}" + local waited=0 + local current_pid="" + + while (( waited < timeout_sec )); do + current_pid="$(main_pid_for_service "${service_name}")" + if [[ -n "${current_pid}" && "${current_pid}" != "0" && "${current_pid}" != "${previous_pid}" ]]; then + printf '%s\n' "${current_pid}" + return 0 + fi + sleep 1 + waited=$(( waited + 1 )) + done + + return 1 +} + +require_running_pid() { + local service_name="$1" + local pid + + pid="$(main_pid_for_service "${service_name}")" + if [[ -z "${pid}" || "${pid}" == "0" ]]; then + blitz_log "${STEP}" "lookup-pid" "failure" "service=${service_name}" 1 + exit 1 + fi + printf '%s\n' "${pid}" +} + +write_fault_flag() { + local flag_name="$1" + local flag_path="${BLITZ_RUNTIME_DIR}/${flag_name}" + printf '%s\n' "$(date +%s)" > "${flag_path}" + blitz_log "${STEP}" "flag-on" "success" "path=${flag_path}" 0 +} + +clear_fault_flag() { + local flag_name="$1" + local flag_path="${BLITZ_RUNTIME_DIR}/${flag_name}" + rm -f "${flag_path}" + blitz_log "${STEP}" "flag-off" "success" "path=${flag_path}" 0 +} + +blitz_load_boot_env +blitz_require_root "${STEP}" +blitz_prepare_runtime_dir + +case "${1:-}" in + bside-crash) + target_pid="$(require_running_pid "${B_SIDE_SERVICE}")" + blitz_log "${STEP}" "bside-crash" "start" "service=${B_SIDE_SERVICE} pid=${target_pid}" 0 + kill -9 "${target_pid}" + if restarted_pid="$(wait_for_service_pid_change "${B_SIDE_SERVICE}" "${target_pid}")"; then + blitz_log "${STEP}" "bside-crash" "success" "old_pid=${target_pid} new_pid=${restarted_pid}" 0 + else + blitz_log "${STEP}" "bside-crash" "failure" "old_pid=${target_pid} restart_not_observed_within=10s" 1 + exit 1 + fi + ;; + bside-process-freeze) + target_pid="$(require_running_pid "${B_SIDE_SERVICE}")" + blitz_log "${STEP}" "bside-process-freeze" "start" "service=${B_SIDE_SERVICE} pid=${target_pid}" 0 + kill -STOP "${target_pid}" + blitz_log "${STEP}" "bside-process-freeze" "success" "service=${B_SIDE_SERVICE} pid=${target_pid}" 0 + ;; + bside-video-thread-stall) + write_fault_flag "fault-injection-bside-video-thread-stall" + ;; + bside-control-thread-stall) + write_fault_flag "fault-injection-bside-control-thread-stall" + ;; + ros-crash) + target_pid="$(require_running_pid "${ROS_SERVICE}")" + blitz_log "${STEP}" "ros-crash" "start" "service=${ROS_SERVICE} pid=${target_pid}" 0 + kill -9 "${target_pid}" + if restarted_pid="$(wait_for_service_pid_change "${ROS_SERVICE}" "${target_pid}")"; then + blitz_log "${STEP}" "ros-crash" "success" "old_pid=${target_pid} new_pid=${restarted_pid}" 0 + else + blitz_log "${STEP}" "ros-crash" "failure" "old_pid=${target_pid} restart_not_observed_within=10s" 1 + exit 1 + fi + ;; + ros-freeze) + target_pid="$(require_running_pid "${ROS_SERVICE}")" + blitz_log "${STEP}" "ros-freeze" "start" "service=${ROS_SERVICE} pid=${target_pid}" 0 + kill -STOP "${target_pid}" + blitz_log "${STEP}" "ros-freeze" "success" "service=${ROS_SERVICE} pid=${target_pid}" 0 + ;; + network-down) + if [[ "${BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION}" != "1" ]]; then + blitz_log "${STEP}" "network-down" "failure" "set BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION=1 first" 1 + exit 1 + fi + case "${2:-}" in + on) + write_fault_flag "fault-injection-network-down" + ;; + off) + clear_fault_flag "fault-injection-network-down" + ;; + *) + echo "usage: $0 network-down on|off" >&2 + exit 2 + ;; + esac + ;; + *) + cat <<'EOF' +usage: + blitz-fault-inject.sh bside-crash + blitz-fault-inject.sh bside-process-freeze + blitz-fault-inject.sh bside-video-thread-stall + blitz-fault-inject.sh bside-control-thread-stall + blitz-fault-inject.sh ros-crash + blitz-fault-inject.sh ros-freeze + blitz-fault-inject.sh network-down on|off +EOF + exit 2 + ;; +esac diff --git a/host/OmniSocketGo_add_camera/scripts/boot/blitz-incident-capture-launch.sh b/host/OmniSocketGo_add_camera/scripts/boot/blitz-incident-capture-launch.sh new file mode 100644 index 0000000..0bd788b --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/blitz-incident-capture-launch.sh @@ -0,0 +1,50 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="incident-launch" +incident_id="" +args=() +timeout_bin="" + +while (($# > 0)); do + case "$1" in + --incident-id) + incident_id="${2:-}" + shift 2 + ;; + *) + args+=("$1") + shift + ;; + esac +done + +blitz_load_boot_env +blitz_require_root "${STEP}" +blitz_require_command systemd-run "${STEP}" +blitz_require_command timeout "${STEP}" +timeout_bin="$(command -v timeout)" + +if [[ -z "${incident_id}" ]]; then + incident_id="$(blitz_new_incident_id)" +fi + +unit_name="blitz-incident-${incident_id//[^A-Za-z0-9_.-]/-}" + +systemd-run \ + --quiet \ + --collect \ + --unit "${unit_name}" \ + --property=Type=oneshot \ + --property="StandardOutput=append:${BLITZ_LOG_FILE}" \ + --property="StandardError=append:${BLITZ_LOG_FILE}" \ + "${timeout_bin}" "${BLITZ_INCIDENT_TOTAL_TIMEOUT_SEC}s" \ + /bin/bash "${SCRIPT_DIR}/blitz-incident-capture.sh" \ + --incident-id "${incident_id}" \ + "${args[@]}" + +printf '%s\n' "${incident_id}" diff --git a/host/OmniSocketGo_add_camera/scripts/boot/blitz-incident-capture.sh b/host/OmniSocketGo_add_camera/scripts/boot/blitz-incident-capture.sh new file mode 100644 index 0000000..c6bcdfd --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/blitz-incident-capture.sh @@ -0,0 +1,131 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="incident-capture" +incident_id="" +incident_source="" +incident_reason="" +incident_unit="" +incident_result="" +incident_exit_status="" + +run_capture() { + local output_path="$1" + shift + + if command -v timeout >/dev/null 2>&1; then + timeout "${BLITZ_INCIDENT_COMMAND_TIMEOUT_SEC}s" "$@" > "${output_path}" 2>&1 || true + else + "$@" > "${output_path}" 2>&1 || true + fi +} + +while (($# > 0)); do + case "$1" in + --incident-id) + incident_id="${2:-}" + shift 2 + ;; + --source) + incident_source="${2:-}" + shift 2 + ;; + --reason) + incident_reason="${2:-}" + shift 2 + ;; + --unit) + incident_unit="${2:-}" + shift 2 + ;; + --result) + incident_result="${2:-}" + shift 2 + ;; + --exit-status) + incident_exit_status="${2:-}" + shift 2 + ;; + *) + blitz_log "${STEP}" "parse-arg" "failure" "unknown argument: $1" 2 + exit 2 + ;; + esac +done + +if [[ -n "${incident_result}" && "${incident_result}" == "success" ]]; then + exit 0 +fi + +blitz_load_boot_env +blitz_load_run_context_env || true +blitz_prepare_runtime_dir +blitz_prepare_run_root + +if [[ -z "${incident_id}" ]]; then + incident_id="$(blitz_new_incident_id)" +fi + +incident_dir="${BLITZ_RUN_ROOT}/incidents/${incident_id}" +mkdir -p "${incident_dir}" + +python3 - "${incident_dir}/incident.json" "${incident_id}" "${BLITZ_RUN_ID:-}" "${incident_source}" "${incident_reason}" "${incident_unit}" "${incident_result}" "${incident_exit_status}" "${BLITZ_RUN_DIR:-}" "${HOSTNAME:-$(hostname)}" <<'PY' +import json +import sys +import time + +path, incident_id, run_id, source, reason, unit, result, exit_status, run_dir, hostname = sys.argv[1:10] +payload = { + "incident_id": incident_id, + "run_id": run_id, + "source": source, + "fault_reason": reason, + "unit": unit, + "service_result": result, + "exit_status": exit_status, + "run_dir": run_dir, + "hostname": hostname, + "captured_at": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()), +} +with open(path, "w", encoding="utf-8") as handle: + json.dump(payload, handle, ensure_ascii=False, indent=2, sort_keys=True) +PY + +for status_file in \ + "${BLITZ_RUNTIME_DIR}/watchdog.status.json" \ + "${BLITZ_RUNTIME_DIR}/b-side-omnid.status.json" \ + "${BLITZ_RUNTIME_DIR}/ros-receiver.status.json" +do + if [[ -f "${status_file}" ]]; then + cp -f "${status_file}" "${incident_dir}/$(basename "${status_file}")" + fi +done + +if [[ -f "${BLITZ_LOG_FILE}" ]]; then + tail -n 400 "${BLITZ_LOG_FILE}" > "${incident_dir}/startup.log.tail" +fi + +run_capture "${incident_dir}/systemctl-status.txt" \ + systemctl status blitz-robot.target blitz-run-context.service blitz-5g-dial.service blitz-5g-link-logger.service blitz-ros-receiver.service blitz-b-side-omnid.service blitz-watchdog.service +run_capture "${incident_dir}/journal.txt" \ + journalctl --no-pager --since "5 minutes ago" -u blitz-run-context.service -u blitz-5g-dial.service -u blitz-5g-link-logger.service -u blitz-ros-receiver.service -u blitz-b-side-omnid.service -u blitz-watchdog.service +run_capture "${incident_dir}/ip-addr.txt" ip addr +run_capture "${incident_dir}/ip-route.txt" ip route +run_capture "${incident_dir}/ss-uapn.txt" ss -uapn +run_capture "${incident_dir}/ss-xlp.txt" ss -xlp + +if [[ -f "${BLITZ_5G_INFO_JSON:-}" ]]; then + cp -f "${BLITZ_5G_INFO_JSON}" "${incident_dir}/$(basename "${BLITZ_5G_INFO_JSON}")" +fi + +if [[ -n "${BLITZ_RUN_DIR:-}" && -d "${BLITZ_RUN_DIR}" ]]; then + while IFS= read -r -d '' jsonl; do + tail -n 200 "${jsonl}" > "${incident_dir}/tail-$(basename "${jsonl}")" + done < <(find "${BLITZ_RUN_DIR}" -maxdepth 1 -type f -name '*.jsonl' -print0 2>/dev/null) +fi + +blitz_log "${STEP}" "complete" "success" "incident_id=${incident_id} path=${incident_dir}" 0 diff --git a/host/OmniSocketGo_add_camera/scripts/boot/blitz-run-context.sh b/host/OmniSocketGo_add_camera/scripts/boot/blitz-run-context.sh new file mode 100644 index 0000000..b159722 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/blitz-run-context.sh @@ -0,0 +1,22 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="run-context" + +on_error() { + local rc="$?" + blitz_log "${STEP}" "error" "failure" "line=${1:-unknown} cmd=${BASH_COMMAND:-unknown}" "${rc}" + exit "${rc}" +} + +trap 'on_error "${LINENO}"' ERR + +blitz_load_boot_env +blitz_require_root "${STEP}" +blitz_require_command python3 "${STEP}" +blitz_init_run_context +blitz_log "${STEP}" "complete" "success" "run_id=${BLITZ_RUN_ID} run_dir=${BLITZ_RUN_DIR}" 0 diff --git a/host/OmniSocketGo_add_camera/scripts/boot/blitz-watchdog.sh b/host/OmniSocketGo_add_camera/scripts/boot/blitz-watchdog.sh new file mode 100644 index 0000000..758d521 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/blitz-watchdog.sh @@ -0,0 +1,971 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="watchdog" +B_SIDE_SERVICE="blitz-b-side-omnid.service" +ROS_SERVICE="blitz-ros-receiver.service" +B_SIDE_STATUS_FILE="" +ROS_STATUS_FILE="" +WATCHDOG_STATUS_FILE="" +NETWORK_FAULT_FILE="" +WATCHDOG_EVENT_LOG="" +WATCHDOG_SAMPLE_LOG="" +WATCHDOG_EVENT_LOG_FAILURE_REPORTED=0 +WATCHDOG_SAMPLE_LOG_FAILURE_REPORTED=0 +CAMERA_MISSING_PREV=0 +CAMERA_RECOVERY_STABLE_COUNT=0 +NETWORK_FAIL_COUNT=0 +NETWORK_COOLDOWN_UNTIL=0 +BACKOFF_UNTIL=0 +LAST_ACTION="none" +LAST_ACTION_EPOCH_MS=0 +FULL_RESTART_WINDOW_START=0 +FULL_RESTART_WINDOW_COUNT=0 +NETWORK_LAST_INTERFACE="" +NETWORK_ROUTE_INTERFACE_LAST_KNOWN="" +NETWORK_PRIMARY_LAST_RETRY_SEC=0 +GPS_LAST_CHECK_SEC=0 +GPS_DEVICE_PRESENT_PREV=-1 +GPS_DEVICE_PRESENT_STATE=1 +GPS_STACK_ACTIVE_STATE=1 +LAST_REPORTED_FAULT_REASON="" +LAST_REPORTED_RECOVERY_STATE="" +declare -A TARGETED_RESTART_WINDOW_START=() +declare -A TARGETED_RESTART_WINDOW_COUNT=() + +now_epoch_sec() { + date +%s +} + +now_epoch_ms() { + date +%s%3N +} + +service_is_active() { + systemctl is-active --quiet "$1" +} + +gps_monitor_enabled() { + [[ "${BLITZ_GPS_MONITOR_ENABLED:-0}" == "1" ]] +} + +gps_stack_active() { + local units=() + local unit + + read -r -a units <<< "${BLITZ_GPS_RESTART_UNITS:-}" + if (( ${#units[@]} == 0 )); then + return 1 + fi + + for unit in "${units[@]}"; do + if service_is_active "${unit}"; then + return 0 + fi + done + return 1 +} + +restart_gps_stack() { + local reason="$1" + local devices="$2" + local units=() + local rc + + read -r -a units <<< "${BLITZ_GPS_RESTART_UNITS:-}" + if (( ${#units[@]} == 0 )); then + GPS_STACK_ACTIVE_STATE=0 + blitz_log "${STEP}" "gps-reconnect" "failure" "reason=${reason} devices=${devices} units=empty" 1 + return 1 + fi + + set_last_action "gps-reconnect" + blitz_log "${STEP}" "gps-reconnect" "start" "reason=${reason} devices=${devices} units=${BLITZ_GPS_RESTART_UNITS}" 0 + if systemctl restart "${units[@]}"; then + GPS_STACK_ACTIVE_STATE=1 + blitz_log "${STEP}" "gps-reconnect" "success" "reason=${reason} devices=${devices} units=${BLITZ_GPS_RESTART_UNITS}" 0 + return 0 + fi + + rc=$? + GPS_STACK_ACTIVE_STATE=0 + blitz_log "${STEP}" "gps-reconnect" "failure" "reason=${reason} devices=${devices} units=${BLITZ_GPS_RESTART_UNITS}" "${rc}" + return "${rc}" +} + +check_gps_health() { + local now_sec="$1" + local check_interval_sec="${BLITZ_GPS_CHECK_INTERVAL_SEC:-10}" + local device_glob="${BLITZ_GPS_DEVICE_GLOB:-}" + local previous_present="${GPS_DEVICE_PRESENT_PREV}" + local recovery_reason="" + local device_summary="" + local -a devices=() + + if ! gps_monitor_enabled; then + GPS_DEVICE_PRESENT_STATE=1 + GPS_STACK_ACTIVE_STATE=1 + return 0 + fi + + if (( check_interval_sec < 1 )); then + check_interval_sec=1 + fi + if (( GPS_LAST_CHECK_SEC != 0 && now_sec - GPS_LAST_CHECK_SEC < check_interval_sec )); then + if (( GPS_DEVICE_PRESENT_STATE == 1 && GPS_STACK_ACTIVE_STATE == 1 )); then + return 0 + fi + return 1 + fi + GPS_LAST_CHECK_SEC="${now_sec}" + + mapfile -t devices < <(compgen -G "${device_glob}" || true) + if (( ${#devices[@]} == 0 )); then + GPS_DEVICE_PRESENT_STATE=0 + GPS_STACK_ACTIVE_STATE=0 + if (( previous_present != 0 )); then + blitz_log "${STEP}" "gps-device-check" "failure" "state=missing glob=${device_glob}" 1 + fi + GPS_DEVICE_PRESENT_PREV=0 + return 1 + fi + + device_summary="$(IFS=,; printf '%s' "${devices[*]}")" + GPS_DEVICE_PRESENT_STATE=1 + GPS_DEVICE_PRESENT_PREV=1 + + if (( previous_present == 0 )); then + blitz_log "${STEP}" "gps-device-check" "success" "state=reappeared devices=${device_summary}" 0 + recovery_reason="device-reappeared" + elif ! gps_stack_active; then + recovery_reason="gpsd-inactive" + fi + + if [[ -n "${recovery_reason}" ]]; then + if restart_gps_stack "${recovery_reason}" "${device_summary}"; then + return 0 + fi + return 1 + fi + + GPS_STACK_ACTIVE_STATE=1 + return 0 +} + +status_file_fresh() { + local path="$1" + local max_age_sec="$2" + local now_sec + local mtime_sec + + if [[ ! -f "${path}" ]]; then + return 1 + fi + now_sec="$(now_epoch_sec)" + mtime_sec="$(stat -c %Y "${path}" 2>/dev/null || echo 0)" + (( now_sec - mtime_sec <= max_age_sec )) +} + +ros_receiver_status_fresh() { + local path="$1" + local max_age_sec="$2" + local now_epoch_ms_value + + now_epoch_ms_value="$(now_epoch_ms)" + python3 - "${path}" "${now_epoch_ms_value}" "${max_age_sec}" <<'PY' +import json +import sys + +path = sys.argv[1] +now_epoch_ms = int(sys.argv[2]) +max_age_ms = int(sys.argv[3]) * 1000 + +try: + with open(path, "r", encoding="utf-8") as handle: + payload = json.load(handle) +except Exception: + raise SystemExit(1) + +heartbeat_ms = int(payload.get("recv_thread_heartbeat_epoch_ms") or 0) +socket_bound = bool(payload.get("socket_bound")) + +if heartbeat_ms <= 0 or not socket_bound: + raise SystemExit(1) + +raise SystemExit(0 if now_epoch_ms - heartbeat_ms <= max_age_ms else 1) +PY +} + +ros_receiver_healthy() { + local max_age_sec="$1" + + service_is_active "${ROS_SERVICE}" \ + && [[ -S "${ROBOT_RECEIVER_LOCAL_SOCKET_PATH}" ]] \ + && status_file_fresh "${ROS_STATUS_FILE}" "${max_age_sec}" \ + && ros_receiver_status_fresh "${ROS_STATUS_FILE}" "${max_age_sec}" +} + +write_watchdog_status() { + local fault_reason="$1" + local recovery_state="$2" + local network_ok="$3" + local camera_ok="$4" + local ros_ok="$5" + local bside_ok="$6" + local gps_ok="$7" + local gps_device_present="$8" + local tmp_file + + tmp_file="${WATCHDOG_STATUS_FILE}.tmp.$$" + cat > "${tmp_file}" <&1)"; then + if (( WATCHDOG_EVENT_LOG_FAILURE_REPORTED == 0 )); then + blitz_log "${STEP}" "watchdog-event-log" "failure" "path=${WATCHDOG_EVENT_LOG} detail=${line}" 0 || true + WATCHDOG_EVENT_LOG_FAILURE_REPORTED=1 + fi + return 0 + fi + if ! blitz_jsonl_append_line "${WATCHDOG_EVENT_LOG}" "${line}"; then + if (( WATCHDOG_EVENT_LOG_FAILURE_REPORTED == 0 )); then + blitz_log "${STEP}" "watchdog-event-log" "failure" "path=${WATCHDOG_EVENT_LOG} detail=append-failed" 0 || true + WATCHDOG_EVENT_LOG_FAILURE_REPORTED=1 + fi + return 0 + fi + WATCHDOG_EVENT_LOG_FAILURE_REPORTED=0 +} + +watchdog_append_sample() { + local line="" + + [[ -n "${WATCHDOG_SAMPLE_LOG}" ]] || return 0 + if ! line="$(watchdog_emit_json "$@" 2>&1)"; then + if (( WATCHDOG_SAMPLE_LOG_FAILURE_REPORTED == 0 )); then + blitz_log "${STEP}" "watchdog-sample-log" "failure" "path=${WATCHDOG_SAMPLE_LOG} detail=${line}" 0 || true + WATCHDOG_SAMPLE_LOG_FAILURE_REPORTED=1 + fi + return 0 + fi + if ! blitz_jsonl_append_line "${WATCHDOG_SAMPLE_LOG}" "${line}"; then + if (( WATCHDOG_SAMPLE_LOG_FAILURE_REPORTED == 0 )); then + blitz_log "${STEP}" "watchdog-sample-log" "failure" "path=${WATCHDOG_SAMPLE_LOG} detail=append-failed" 0 || true + WATCHDOG_SAMPLE_LOG_FAILURE_REPORTED=1 + fi + return 0 + fi + WATCHDOG_SAMPLE_LOG_FAILURE_REPORTED=0 +} + +watchdog_record_state_transition() { + local fault_reason="$1" + local recovery_state="$2" + + if [[ "${fault_reason}" == "${LAST_REPORTED_FAULT_REASON}" && "${recovery_state}" == "${LAST_REPORTED_RECOVERY_STATE}" ]]; then + return 0 + fi + watchdog_append_event "event" "state-transition" "${fault_reason}" "${recovery_state}" "" "" + LAST_REPORTED_FAULT_REASON="${fault_reason}" + LAST_REPORTED_RECOVERY_STATE="${recovery_state}" +} + +watchdog_launch_incident() { + local reason="$1" + local unit_name="$2" + + blitz_launch_incident_capture \ + --source watchdog \ + --reason "${reason}" \ + --unit "${unit_name}" \ + --result failure \ + --exit-status 1 2>/dev/null || true +} + +set_last_action() { + LAST_ACTION="$1" + LAST_ACTION_EPOCH_MS="$(now_epoch_ms)" +} + +targeted_restart_total() { + local total=0 + local key + + for key in "${!TARGETED_RESTART_WINDOW_COUNT[@]}"; do + total=$(( total + TARGETED_RESTART_WINDOW_COUNT["${key}"] )) + done + printf '%s\n' "${total}" +} + +register_targeted_restart() { + local fault_key="$1" + local now_sec + local window_start + local count + + now_sec="$(now_epoch_sec)" + window_start="${TARGETED_RESTART_WINDOW_START["${fault_key}"]:-0}" + count="${TARGETED_RESTART_WINDOW_COUNT["${fault_key}"]:-0}" + if (( window_start == 0 || now_sec - window_start > 60 )); then + window_start="${now_sec}" + count=1 + else + count=$(( count + 1 )) + fi + TARGETED_RESTART_WINDOW_START["${fault_key}"]="${window_start}" + TARGETED_RESTART_WINDOW_COUNT["${fault_key}"]="${count}" + (( count >= 2 )) +} + +record_full_restart() { + local now_sec + + now_sec="$(now_epoch_sec)" + if (( FULL_RESTART_WINDOW_START == 0 || now_sec - FULL_RESTART_WINDOW_START > 600 )); then + FULL_RESTART_WINDOW_START="${now_sec}" + FULL_RESTART_WINDOW_COUNT=1 + else + FULL_RESTART_WINDOW_COUNT=$(( FULL_RESTART_WINDOW_COUNT + 1 )) + fi + if (( FULL_RESTART_WINDOW_COUNT >= 3 )); then + BACKOFF_UNTIL=$(( now_sec + 60 )) + watchdog_append_event "event" "backoff-enter" "backoff" "backoff" "full_restart_count=${FULL_RESTART_WINDOW_COUNT}" "" + fi +} + +restart_bside_targeted() { + local fault_key="$1" + local reason="$2" + local rc + local incident_id="" + + if register_targeted_restart "${fault_key}"; then + blitz_log "${STEP}" "escalate-full-restart" "start" "reason=${reason}" 0 + watchdog_append_event "event" "escalate-full-restart" "${reason}-escalated" "recovering" "fault_key=${fault_key}" "" + full_restart_stack "${reason}-escalated" + return 0 + fi + + incident_id="$(watchdog_launch_incident "${reason}" "${B_SIDE_SERVICE}")" + set_last_action "restart-bside" + RECOVERY_ACTION_TAKEN=1 + blitz_log "${STEP}" "restart-bside" "start" "reason=${reason}" 0 + watchdog_append_event "event" "restart-bside-start" "${reason}" "recovering" "fault_key=${fault_key}" "${incident_id}" + if systemctl restart "${B_SIDE_SERVICE}"; then + blitz_log "${STEP}" "restart-bside" "success" "reason=${reason}" 0 + watchdog_append_event "event" "restart-bside-success" "${reason}" "recovering" "fault_key=${fault_key}" "${incident_id}" + return 0 + fi + + rc=$? + blitz_log "${STEP}" "restart-bside" "failure" "reason=${reason}" "${rc}" + watchdog_append_event "event" "restart-bside-failure" "${reason}" "recovering" "fault_key=${fault_key} rc=${rc}" "${incident_id}" + return "${rc}" +} + +full_restart_stack() { + local reason="$1" + local rc + local incident_id="" + + incident_id="$(watchdog_launch_incident "${reason}" "blitz-robot.target")" + set_last_action "full-restart" + RECOVERY_ACTION_TAKEN=1 + recovery_state="recovering" + fault_reason="${reason}" + + blitz_log "${STEP}" "full-restart-stop-bside" "start" "reason=${reason}" 0 + watchdog_append_event "event" "full-restart-start" "${reason}" "recovering" "" "${incident_id}" + systemctl stop "${B_SIDE_SERVICE}" || true + + if systemctl restart "${ROS_SERVICE}"; then + blitz_log "${STEP}" "full-restart-restart-ros" "success" "reason=${reason}" 0 + else + rc=$? + blitz_log "${STEP}" "full-restart-restart-ros" "failure" "reason=${reason}" "${rc}" + record_full_restart + return "${rc}" + fi + + if bash "${BOOT_SCRIPT_DIR}/wait-for-unix-socket.sh" --step "${STEP}" --timeout "${BLITZ_ROS_SOCKET_WAIT_SEC}"; then + : + else + rc=$? + blitz_log "${STEP}" "full-restart-wait-socket" "failure" "reason=${reason}" "${rc}" + record_full_restart + return "${rc}" + fi + + if systemctl start "${B_SIDE_SERVICE}"; then + blitz_log "${STEP}" "full-restart-start-bside" "success" "reason=${reason}" 0 + else + rc=$? + blitz_log "${STEP}" "full-restart-start-bside" "failure" "reason=${reason}" "${rc}" + watchdog_append_event "event" "full-restart-failure" "${reason}" "recovering" "stage=start-bside rc=${rc}" "${incident_id}" + record_full_restart + return "${rc}" + fi + watchdog_append_event "event" "full-restart-success" "${reason}" "recovering" "" "${incident_id}" + record_full_restart +} + +network_fault_injected() { + [[ "${BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION}" == "1" && -f "${NETWORK_FAULT_FILE}" ]] +} + +resolve_network_interface() { + NETWORK_LAST_INTERFACE="$(blitz_resolve_5g_interface || true)" + if [[ -n "${NETWORK_LAST_INTERFACE}" ]]; then + NETWORK_ROUTE_INTERFACE_LAST_KNOWN="${NETWORK_LAST_INTERFACE}" + return 0 + fi + return 1 +} + +network_route_targets() { + local target + + if [[ -n "${BLITZ_TIME_SERVER_IP:-}" ]]; then + printf '%s\n' "${BLITZ_TIME_SERVER_IP}" + fi + for target in ${BLITZ_5G_ROUTE_TARGETS//,/ }; do + if [[ -n "${target}" && "${target}" != "${BLITZ_TIME_SERVER_IP:-}" ]]; then + printf '%s\n' "${target}" + fi + done +} + +log_target_route_paths() { + local action="$1" + local target + local route_output + + while IFS= read -r target; do + [[ -n "${target}" ]] || continue + route_output="$(ip route get "${target}" 2>&1 | head -n 1 || true)" + if [[ -z "${route_output}" ]]; then + route_output="unresolved" + fi + blitz_log "${STEP}" "route-path" "info" "action=${action} target=${target} route=${route_output}" 0 + done < <(network_route_targets) +} + +route_output_uses_interface() { + local route_output="$1" + local interface_name="$2" + + [[ -n "${interface_name}" ]] || return 1 + [[ "${route_output}" == *" dev ${interface_name} "* || "${route_output}" == *" dev ${interface_name}" ]] +} + +route_output_uses_gateway() { + local route_output="$1" + local gateway="$2" + + [[ -n "${gateway}" ]] || return 1 + [[ "${route_output}" == *"via ${gateway}"* ]] +} + +route_is_desired_target_route() { + local route_output="$1" + local interface_name="$2" + local gateway="$3" + + route_output_uses_interface "${route_output}" "${interface_name}" \ + && route_output_uses_gateway "${route_output}" "${gateway}" +} + +route_is_managed_5g_route() { + local route_output="$1" + local interface_name="${2:-}" + local gateway="${3:-}" + + if route_output_uses_interface "${route_output}" "${interface_name}"; then + return 0 + fi + if route_output_uses_gateway "${route_output}" "${gateway}"; then + return 0 + fi + if route_output_uses_gateway "${route_output}" "${BLITZ_5G_GATEWAY:-}"; then + return 0 + fi + return 1 +} + +resolve_route_cleanup_interface() { + local interface_name="" + local info_json="${BLITZ_5G_INFO_JSON:-}" + + if [[ -n "${NETWORK_LAST_INTERFACE}" ]]; then + printf '%s\n' "${NETWORK_LAST_INTERFACE}" + return 0 + fi + if [[ -n "${NETWORK_ROUTE_INTERFACE_LAST_KNOWN}" ]]; then + printf '%s\n' "${NETWORK_ROUTE_INTERFACE_LAST_KNOWN}" + return 0 + fi + + interface_name="$(blitz_read_5g_info_interface "${info_json}" || true)" + if [[ -n "${interface_name}" ]]; then + printf '%s\n' "${interface_name}" + return 0 + fi + return 1 +} + +resolve_network_gateway() { + local interface_name="$1" + local default_route + local gateway="" + local tokens=() + local index + + default_route="$(ip -o route show default dev "${interface_name}" 2>/dev/null | head -n 1 || true)" + if [[ -n "${default_route}" ]]; then + read -r -a tokens <<< "${default_route}" + for (( index=0; index<${#tokens[@]}-1; index++ )); do + if [[ "${tokens[index]}" == "via" ]]; then + gateway="${tokens[index + 1]}" + break + fi + done + fi + + if [[ -n "${gateway}" ]]; then + printf '%s\n' "${gateway}" + return 0 + fi + if [[ -n "${BLITZ_5G_GATEWAY:-}" ]]; then + printf '%s\n' "${BLITZ_5G_GATEWAY}" + return 0 + fi + return 1 +} + +sync_target_routes_to_5g() { + local interface_name="$1" + local gateway="${2:-}" + local route_output="" + local updated=0 + local target + local rc + + if [[ -z "${interface_name}" ]]; then + return 1 + fi + + if [[ -z "${gateway}" ]]; then + gateway="$(resolve_network_gateway "${interface_name}" || true)" + fi + if [[ -z "${gateway}" ]]; then + blitz_log "${STEP}" "route-sync-gateway" "failure" "interface=${interface_name}" 1 + return 1 + fi + + while IFS= read -r target; do + [[ -n "${target}" ]] || continue + route_output="$(ip route show "${target}/32" 2>/dev/null | head -n 1 || true)" + if [[ -n "${route_output}" ]] && route_is_desired_target_route "${route_output}" "${interface_name}" "${gateway}"; then + continue + fi + if ip route replace "${target}/32" via "${gateway}" dev "${interface_name}"; then + updated=1 + blitz_log "${STEP}" "route-sync-target" "success" "target=${target} interface=${interface_name} gateway=${gateway}" 0 + else + rc=$? + blitz_log "${STEP}" "route-sync-target" "failure" "target=${target} interface=${interface_name} gateway=${gateway}" "${rc}" + return "${rc}" + fi + done < <(network_route_targets) + + if (( updated == 1 )); then + NETWORK_ROUTE_INTERFACE_LAST_KNOWN="${interface_name}" + log_target_route_paths "sync-to-5g" + fi + return 0 +} + +clear_target_routes_from_5g() { + local interface_name="${1:-}" + local gateway="${2:-}" + local route_output="" + local target + local removed_any=0 + local rc + + if [[ -z "${interface_name}" ]]; then + interface_name="$(resolve_route_cleanup_interface || true)" + fi + if [[ -z "${gateway}" && -n "${interface_name}" ]]; then + gateway="$(resolve_network_gateway "${interface_name}" || true)" + fi + if [[ -z "${gateway}" ]]; then + gateway="${BLITZ_5G_GATEWAY:-}" + fi + + while IFS= read -r target; do + [[ -n "${target}" ]] || continue + route_output="$(ip route show "${target}/32" 2>/dev/null | head -n 1 || true)" + if [[ -z "${route_output}" ]] || ! route_is_managed_5g_route "${route_output}" "${interface_name}" "${gateway}"; then + continue + fi + if ip route del "${target}/32"; then + removed_any=1 + blitz_log "${STEP}" "route-clear-target" "success" "target=${target} interface=${interface_name:-unknown} gateway=${gateway:-unknown}" 0 + else + rc=$? + blitz_log "${STEP}" "route-clear-target" "failure" "target=${target} interface=${interface_name:-unknown} gateway=${gateway:-unknown}" "${rc}" + return "${rc}" + fi + done < <(network_route_targets) + + if (( removed_any == 1 )); then + blitz_log "${STEP}" "route-clear" "success" "interface=${interface_name:-unknown} gateway=${gateway:-unknown}" 0 + log_target_route_paths "clear-from-5g" + fi + return 0 +} + +repair_network_routes() { + local interface_name="$1" + local gateway="" + local route_output + + if [[ -z "${interface_name}" ]]; then + return 1 + fi + + gateway="$(resolve_network_gateway "${interface_name}" || true)" + if [[ -z "${gateway}" ]]; then + blitz_log "${STEP}" "route-repair-gateway" "failure" "interface=${interface_name}" 1 + return 1 + fi + + if ! sync_target_routes_to_5g "${interface_name}" "${gateway}"; then + clear_target_routes_from_5g "${interface_name}" "${gateway}" || true + return 1 + fi + + route_output="$(blitz_route_ready "${BLITZ_TIME_SERVER_IP}" "${interface_name}" || true)" + if [[ -z "${route_output}" ]]; then + clear_target_routes_from_5g "${interface_name}" "${gateway}" || true + blitz_log "${STEP}" "route-repair-postcheck" "failure" "interface=${interface_name} gateway=${gateway}" 1 + return 1 + fi + + if ! ping -I "${interface_name}" -c 1 -W 2 "${BLITZ_TIME_SERVER_IP}" >/dev/null 2>&1; then + clear_target_routes_from_5g "${interface_name}" "${gateway}" || true + blitz_log "${STEP}" "route-repair-probe" "failure" "interface=${interface_name} target=${BLITZ_TIME_SERVER_IP}" 1 + return 1 + fi + + blitz_log "${STEP}" "route-repair-postcheck" "success" "interface=${interface_name} gateway=${gateway} route=${route_output}" 0 + return 0 +} + +network_is_healthy() { + local route_output + + NETWORK_LAST_INTERFACE="" + if network_fault_injected; then + return 1 + fi + if ! resolve_network_interface; then + return 1 + fi + route_output="$(blitz_route_ready "${BLITZ_TIME_SERVER_IP}" "${NETWORK_LAST_INTERFACE}" || true)" + if [[ -z "${route_output}" ]]; then + return 1 + fi + ping -I "${NETWORK_LAST_INTERFACE}" -c 1 -W 2 "${BLITZ_TIME_SERVER_IP}" >/dev/null 2>&1 +} + +fallback_network_is_healthy() { + local route_output + + if [[ -z "${BLITZ_TIME_SERVER_IP:-}" ]]; then + return 1 + fi + + route_output="$(blitz_route_ready "${BLITZ_TIME_SERVER_IP}" || true)" + if [[ -z "${route_output}" ]]; then + return 1 + fi + + ping -c 1 -W 2 "${BLITZ_TIME_SERVER_IP}" >/dev/null 2>&1 +} + +wait_for_network_recovery() { + local timeout_sec="$1" + local waited=0 + + while (( waited < timeout_sec )); do + if network_is_healthy; then + blitz_log "${STEP}" "network-postcheck" "success" "interface=${NETWORK_LAST_INTERFACE} waited_sec=${waited}" 0 + return 0 + fi + if (( waited == 0 || waited % 5 == 0 )); then + blitz_log "${STEP}" "network-postcheck" "waiting" "interface=${NETWORK_LAST_INTERFACE:-unresolved} waited_sec=${waited}" 0 + fi + sleep 1 + waited=$(( waited + 1 )) + done + + blitz_log "${STEP}" "network-postcheck" "failure" "interface=${NETWORK_LAST_INTERFACE:-unresolved} timeout_sec=${timeout_sec}" 1 + return 1 +} + +perform_network_recovery() { + local rc=0 + local incident_id="" + + if resolve_network_interface && repair_network_routes "${NETWORK_LAST_INTERFACE}"; then + set_last_action "route-repair" + RECOVERY_ACTION_TAKEN=1 + NETWORK_COOLDOWN_UNTIL=$(( $(now_epoch_sec) + BLITZ_NETWORK_RECOVERY_COOLDOWN_SEC )) + NETWORK_FAIL_COUNT=0 + blitz_log "${STEP}" "network-recovery" "success" "mode=route-repair interface=${NETWORK_LAST_INTERFACE}" 0 + watchdog_append_event "event" "route-repair-success" "network_or_robot_unreachable" "recovering" "interface=${NETWORK_LAST_INTERFACE}" "" + return 0 + fi + + incident_id="$(watchdog_launch_incident "network-recovery" "blitz-5g-dial.service")" + set_last_action "network-recovery" + RECOVERY_ACTION_TAKEN=1 + blitz_log "${STEP}" "network-recovery" "start" "fail_count=${NETWORK_FAIL_COUNT}" 0 + watchdog_append_event "event" "network-recovery-start" "network_or_robot_unreachable" "recovering" "fail_count=${NETWORK_FAIL_COUNT}" "${incident_id}" + systemctl stop "${B_SIDE_SERVICE}" || true + + if bash "${BOOT_SCRIPT_DIR}/5g-dial.sh"; then + : + else + rc=$? + blitz_log "${STEP}" "network-redial" "failure" "fail_count=${NETWORK_FAIL_COUNT} script=${BOOT_SCRIPT_DIR}/5g-dial.sh" "${rc}" + watchdog_append_event "event" "network-recovery-failure" "network_or_robot_unreachable" "recovering" "stage=redial rc=${rc}" "${incident_id}" + return "${rc}" + fi + + if wait_for_network_recovery "${BLITZ_5G_ROUTE_WAIT_SEC}"; then + : + else + rc=$? + blitz_log "${STEP}" "network-recovery" "failure" "fail_count=${NETWORK_FAIL_COUNT} interface=${NETWORK_LAST_INTERFACE:-unresolved}" "${rc}" + watchdog_append_event "event" "network-recovery-failure" "network_or_robot_unreachable" "recovering" "stage=postcheck rc=${rc}" "${incident_id}" + return "${rc}" + fi + + NETWORK_COOLDOWN_UNTIL=$(( $(now_epoch_sec) + BLITZ_NETWORK_RECOVERY_COOLDOWN_SEC )) + NETWORK_FAIL_COUNT=0 + watchdog_append_event "event" "network-recovery-success" "network_or_robot_unreachable" "recovering" "interface=${NETWORK_LAST_INTERFACE:-unresolved}" "${incident_id}" + if ros_receiver_healthy "${BLITZ_HEALTH_STALE_SEC}"; then + restart_bside_targeted "network" "network-recovered" + return 0 + fi + full_restart_stack "network-recovered-ros-unhealthy" + return 0 +} + +blitz_load_boot_env +blitz_require_root "${STEP}" +blitz_require_command systemctl "${STEP}" +blitz_require_command stat "${STEP}" +blitz_require_command ping "${STEP}" +blitz_require_command python3 "${STEP}" +blitz_prepare_runtime_dir +blitz_require_run_context + +B_SIDE_STATUS_FILE="${BLITZ_RUNTIME_DIR}/b-side-omnid.status.json" +ROS_STATUS_FILE="${BLITZ_RUNTIME_DIR}/ros-receiver.status.json" +WATCHDOG_STATUS_FILE="${BLITZ_RUNTIME_DIR}/watchdog.status.json" +NETWORK_FAULT_FILE="${BLITZ_RUNTIME_DIR}/fault-injection-network-down" +WATCHDOG_EVENT_LOG="${BLITZ_RUN_DIR}/watchdog-events.jsonl" +WATCHDOG_SAMPLE_LOG="${BLITZ_RUN_DIR}/watchdog-samples.jsonl" + +while true; do + fault_reason="none" + recovery_state="ok" + network_ok=1 + camera_ok=1 + ros_ok=1 + bside_ok=1 + gps_ok=1 + gps_device_present=1 + RECOVERY_ACTION_TAKEN=0 + now_sec="$(now_epoch_sec)" + + if gps_monitor_enabled; then + gps_device_present="${GPS_DEVICE_PRESENT_STATE}" + if (( GPS_DEVICE_PRESENT_STATE == 0 || GPS_STACK_ACTIVE_STATE == 0 )); then + gps_ok=0 + fi + fi + + if (( BACKOFF_UNTIL > now_sec )); then + fault_reason="backoff" + recovery_state="backoff" + watchdog_record_state_transition "${fault_reason}" "${recovery_state}" + write_watchdog_status "${fault_reason}" "${recovery_state}" 0 0 0 0 "${gps_ok}" "${gps_device_present}" + watchdog_append_sample "sample" "loop" "${fault_reason}" "${recovery_state}" "" "" 0 0 0 0 "${gps_ok}" "${gps_device_present}" + sleep "${BLITZ_WATCHDOG_INTERVAL_SEC}" + continue + fi + + if (( NETWORK_COOLDOWN_UNTIL > now_sec )); then + recovery_state="recovering" + elif ! network_is_healthy; then + clear_target_routes_from_5g || true + if fallback_network_is_healthy; then + NETWORK_FAIL_COUNT=0 + fault_reason="network_fallback_active" + recovery_state="degraded" + blitz_log "${STEP}" "network-check" "fallback" "interface=${NETWORK_LAST_INTERFACE:-unresolved} target=${BLITZ_TIME_SERVER_IP}" 0 + if (( NETWORK_PRIMARY_LAST_RETRY_SEC == 0 || now_sec - NETWORK_PRIMARY_LAST_RETRY_SEC >= 10 )); then + NETWORK_PRIMARY_LAST_RETRY_SEC="${now_sec}" + if resolve_network_interface && repair_network_routes "${NETWORK_LAST_INTERFACE}"; then + NETWORK_PRIMARY_LAST_RETRY_SEC=0 + fault_reason="none" + recovery_state="ok" + blitz_log "${STEP}" "network-check" "primary-restored" "interface=${NETWORK_LAST_INTERFACE} target=${BLITZ_TIME_SERVER_IP}" 0 + log_target_route_paths "primary-restored" + fi + fi + else + network_ok=0 + NETWORK_FAIL_COUNT=$(( NETWORK_FAIL_COUNT + 1 )) + fault_reason="network_or_robot_unreachable" + recovery_state="recovering" + blitz_log "${STEP}" "network-check" "failure" "count=${NETWORK_FAIL_COUNT} interface=${NETWORK_LAST_INTERFACE:-unresolved}" 1 + if (( NETWORK_FAIL_COUNT >= BLITZ_NETWORK_FAIL_THRESHOLD )); then + perform_network_recovery || true + fi + fi + else + NETWORK_PRIMARY_LAST_RETRY_SEC=0 + NETWORK_FAIL_COUNT=0 + sync_target_routes_to_5g "${NETWORK_LAST_INTERFACE}" || true + fi + + if check_gps_health "${now_sec}"; then + gps_ok=1 + else + gps_ok=0 + gps_device_present="${GPS_DEVICE_PRESENT_STATE}" + if [[ "${fault_reason}" == "none" ]]; then + if (( GPS_DEVICE_PRESENT_STATE == 0 )); then + fault_reason="gps_device_missing" + else + fault_reason="gps_reconnect_failed" + fi + recovery_state="degraded" + fi + fi + gps_device_present="${GPS_DEVICE_PRESENT_STATE}" + + if [[ ! -e "${OMNI_CAMERA_DEVICE}" ]]; then + camera_ok=0 + fault_reason="camera_missing" + recovery_state="degraded" + CAMERA_MISSING_PREV=1 + CAMERA_RECOVERY_STABLE_COUNT=0 + elif (( RECOVERY_ACTION_TAKEN == 0 && CAMERA_MISSING_PREV == 1 )); then + CAMERA_RECOVERY_STABLE_COUNT=$(( CAMERA_RECOVERY_STABLE_COUNT + 1 )) + recovery_state="recovering" + fault_reason="camera_recovered" + if (( CAMERA_RECOVERY_STABLE_COUNT >= 2 )); then + restart_bside_targeted "camera" "camera-reappeared" || true + CAMERA_MISSING_PREV=0 + CAMERA_RECOVERY_STABLE_COUNT=0 + fi + else + CAMERA_RECOVERY_STABLE_COUNT=0 + fi + + if (( RECOVERY_ACTION_TAKEN == 0 )) && { ! service_is_active "${B_SIDE_SERVICE}" || ! status_file_fresh "${B_SIDE_STATUS_FILE}" "${BLITZ_HEALTH_STALE_SEC}"; }; then + bside_ok=0 + fault_reason="bside_status_stale" + recovery_state="recovering" + restart_bside_targeted "bside" "bside-unhealthy" || true + fi + + if (( RECOVERY_ACTION_TAKEN == 0 )) && ! ros_receiver_healthy "${BLITZ_HEALTH_STALE_SEC}"; then + ros_ok=0 + fault_reason="ros_receiver_unhealthy" + recovery_state="recovering" + full_restart_stack "ros-unhealthy" || true + fi + + watchdog_record_state_transition "${fault_reason}" "${recovery_state}" + write_watchdog_status "${fault_reason}" "${recovery_state}" "${network_ok}" "${camera_ok}" "${ros_ok}" "${bside_ok}" "${gps_ok}" "${gps_device_present}" + watchdog_append_sample "sample" "loop" "${fault_reason}" "${recovery_state}" "" "" "${network_ok}" "${camera_ok}" "${ros_ok}" "${bside_ok}" "${gps_ok}" "${gps_device_present}" + sleep "${BLITZ_WATCHDOG_INTERVAL_SEC}" +done diff --git a/host/OmniSocketGo_add_camera/scripts/boot/boot-gate.sh b/host/OmniSocketGo_add_camera/scripts/boot/boot-gate.sh new file mode 100644 index 0000000..ef22e0f --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/boot-gate.sh @@ -0,0 +1,15 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="boot-gate" + +blitz_load_boot_env + +blitz_log "${STEP}" "start" "start" "delay_sec=${BLITZ_BOOT_DELAY_SEC}" 0 +blitz_log "${STEP}" "delay" "start" "sleep ${BLITZ_BOOT_DELAY_SEC}s before starting Blitz services" 0 +sleep "${BLITZ_BOOT_DELAY_SEC}" +blitz_log "${STEP}" "delay" "success" "boot gate released after ${BLITZ_BOOT_DELAY_SEC}s" 0 diff --git a/host/OmniSocketGo_add_camera/scripts/boot/common.sh b/host/OmniSocketGo_add_camera/scripts/boot/common.sh new file mode 100644 index 0000000..61a2205 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/common.sh @@ -0,0 +1,661 @@ +#!/usr/bin/env bash +set -euo pipefail + +BOOT_SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +DEV_SCRIPT_DIR="$(cd "${BOOT_SCRIPT_DIR}/../dev" && pwd)" + +source_with_nounset_off() { + set +u + # shellcheck disable=SC1090 + source "$1" + set -u +} + +blitz_host_from_addr() { + local value="${1:-}" + + if [[ -z "${value}" ]]; then + return 1 + fi + if [[ "${value}" == \[*\]:* ]]; then + value="${value#\[}" + printf '%s\n' "${value%%]:*}" + return 0 + fi + printf '%s\n' "${value%%:*}" +} + +blitz_load_boot_env() { + local env_file + local default_time_server + local dev_run_root + local dev_runtime_dir + + if [[ "${BLITZ_BOOT_ENV_LOADED:-0}" == "1" ]]; then + return 0 + fi + + export BLITZ_BOOT_LOADING_ENV="1" + # shellcheck disable=SC1091 + source "${DEV_SCRIPT_DIR}/load-env.sh" + unset BLITZ_BOOT_LOADING_ENV + + for env_file in \ + "${BOOT_SCRIPT_DIR}/robot-boot.env" \ + "${BOOT_SCRIPT_DIR}/robot-boot.env.local" + do + if [[ -f "${env_file}" ]]; then + set -a + # shellcheck disable=SC1090 + source "${env_file}" + set +a + fi + done + + if declare -F normalize_loaded_env_vars >/dev/null 2>&1; then + normalize_loaded_env_vars + fi + + dev_run_root="${OMNISOCKETGO_ROOT}/logs" + dev_runtime_dir="${dev_run_root}/runtime" + + if [[ -z "${BLITZ_RUN_ROOT:-}" || "${BLITZ_RUN_ROOT}" == "${dev_run_root}" ]]; then + export BLITZ_RUN_ROOT="/var/log/blitz-robot" + fi + if [[ -z "${BLITZ_RUNTIME_DIR:-}" || "${BLITZ_RUNTIME_DIR}" == "${dev_runtime_dir}" ]]; then + export BLITZ_RUNTIME_DIR="/run/blitz-robot" + fi + if [[ -z "${BLITZ_RUN_CONTEXT_FILE:-}" || "${BLITZ_RUN_CONTEXT_FILE}" == "${dev_runtime_dir}/run-context.env" ]]; then + export BLITZ_RUN_CONTEXT_FILE="${BLITZ_RUNTIME_DIR}/run-context.env" + fi + if [[ -z "${BLITZ_RUN_ID_FILE:-}" || "${BLITZ_RUN_ID_FILE}" == "${dev_runtime_dir}/run-id" ]]; then + export BLITZ_RUN_ID_FILE="${BLITZ_RUNTIME_DIR}/run-id" + fi + if [[ -z "${BLITZ_CURRENT_RUN_LINK:-}" || "${BLITZ_CURRENT_RUN_LINK}" == "${dev_run_root}/current" ]]; then + export BLITZ_CURRENT_RUN_LINK="${BLITZ_RUN_ROOT}/current" + fi + + default_time_server="$(blitz_host_from_addr "${ROBOT_SIDE_OMNISOCKET_SERVER_ADDR:-}" || true)" + + export BLITZ_BOOT_DELAY_SEC="${BLITZ_BOOT_DELAY_SEC:-30}" + export BLITZ_RUN_ROOT="${BLITZ_RUN_ROOT:-/var/log/blitz-robot}" + export BLITZ_LOG_FILE="${BLITZ_LOG_FILE:-/var/log/blitz-robot/startup.log}" + export BLITZ_RUNTIME_DIR="${BLITZ_RUNTIME_DIR:-/run/blitz-robot}" + export BLITZ_RUN_CONTEXT_FILE="${BLITZ_RUN_CONTEXT_FILE:-${BLITZ_RUNTIME_DIR}/run-context.env}" + export BLITZ_RUN_ID_FILE="${BLITZ_RUN_ID_FILE:-${BLITZ_RUNTIME_DIR}/run-id}" + export BLITZ_CURRENT_RUN_LINK="${BLITZ_CURRENT_RUN_LINK:-${BLITZ_RUN_ROOT}/current}" + export BLITZ_5G_DIAL_DIR="${BLITZ_5G_DIAL_DIR:-${BOOT_SCRIPT_DIR}}" + export BLITZ_5G_SERIAL_PORT="${BLITZ_5G_SERIAL_PORT:-/dev/ttyUSB7}" + export BLITZ_5G_INTERFACE="${BLITZ_5G_INTERFACE:-}" + export BLITZ_5G_MODEM_SUBNET="${BLITZ_5G_MODEM_SUBNET:-192.168.224.0/22}" + export BLITZ_5G_GATEWAY="${BLITZ_5G_GATEWAY:-192.168.225.1}" + export BLITZ_5G_SKIP_DHCP="${BLITZ_5G_SKIP_DHCP:-0}" + export BLITZ_5G_REMOVE_DEFAULT_ROUTE="${BLITZ_5G_REMOVE_DEFAULT_ROUTE:-1}" + export BLITZ_5G_ROUTE_TARGETS="${BLITZ_5G_ROUTE_TARGETS:-106.55.173.235}" + export BLITZ_5G_INFO_JSON="${BLITZ_5G_INFO_JSON:-${BLITZ_5G_DIAL_DIR}/modem_network_info.json}" + export BLITZ_5G_DISABLE_INTERFACES="${BLITZ_5G_DISABLE_INTERFACES:-}" + export BLITZ_5G_SERIAL_WAIT_SEC="${BLITZ_5G_SERIAL_WAIT_SEC:-60}" + export BLITZ_5G_ROUTE_WAIT_SEC="${BLITZ_5G_ROUTE_WAIT_SEC:-30}" + export BLITZ_TIME_SERVER_IP="${BLITZ_TIME_SERVER_IP:-${default_time_server}}" + export BLITZ_ROS_USER="${BLITZ_ROS_USER:-nvidia}" + export BLITZ_ROS_SOCKET_WAIT_SEC="${BLITZ_ROS_SOCKET_WAIT_SEC:-20}" + export BLITZ_WATCHDOG_INTERVAL_SEC="${BLITZ_WATCHDOG_INTERVAL_SEC:-5}" + export BLITZ_HEALTH_STALE_SEC="${BLITZ_HEALTH_STALE_SEC:-15}" + export BLITZ_OMNID_THREAD_HEARTBEAT_TIMEOUT_SEC="${BLITZ_OMNID_THREAD_HEARTBEAT_TIMEOUT_SEC:-15}" + export BLITZ_KCP_STATS_INTERVAL_MS="${BLITZ_KCP_STATS_INTERVAL_MS:-1000}" + export BLITZ_CONTROL_LATENCY_LOG_ENABLED="${BLITZ_CONTROL_LATENCY_LOG_ENABLED:-1}" + export BLITZ_CONTROL_LATENCY_LOG_SAMPLE_MOD="${BLITZ_CONTROL_LATENCY_LOG_SAMPLE_MOD:-100}" + export BLITZ_5G_LINK_LOG_INTERVAL_SEC="${BLITZ_5G_LINK_LOG_INTERVAL_SEC:-5}" + export BLITZ_JSONL_FLUSH_INTERVAL_MS="${BLITZ_JSONL_FLUSH_INTERVAL_MS:-1000}" + export BLITZ_JSONL_FLUSH_BYTES="${BLITZ_JSONL_FLUSH_BYTES:-262144}" + export BLITZ_JSONL_ROTATE_BYTES="${BLITZ_JSONL_ROTATE_BYTES:-134217728}" + export BLITZ_JSONL_ROTATE_FILES="${BLITZ_JSONL_ROTATE_FILES:-8}" + export BLITZ_INCIDENT_COMMAND_TIMEOUT_SEC="${BLITZ_INCIDENT_COMMAND_TIMEOUT_SEC:-5}" + export BLITZ_INCIDENT_TOTAL_TIMEOUT_SEC="${BLITZ_INCIDENT_TOTAL_TIMEOUT_SEC:-30}" + export BLITZ_NETWORK_FAIL_THRESHOLD="${BLITZ_NETWORK_FAIL_THRESHOLD:-3}" + export BLITZ_NETWORK_RECOVERY_COOLDOWN_SEC="${BLITZ_NETWORK_RECOVERY_COOLDOWN_SEC:-30}" + export BLITZ_GPS_MONITOR_ENABLED="${BLITZ_GPS_MONITOR_ENABLED:-1}" + export BLITZ_GPS_DEVICE_GLOB="${BLITZ_GPS_DEVICE_GLOB:-/dev/ttyCH341USB*}" + export BLITZ_GPS_CHECK_INTERVAL_SEC="${BLITZ_GPS_CHECK_INTERVAL_SEC:-10}" + export BLITZ_GPS_RESTART_UNITS="${BLITZ_GPS_RESTART_UNITS:-gpsd.socket gpsd.service}" + export BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION="${BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION:-0}" + export BLITZ_BOOT_ENV_LOADED="1" +} + +blitz_timestamp() { + date '+%Y-%m-%d %H:%M:%S%z' +} + +blitz_sanitize_detail() { + local detail="${1:-}" + + detail="${detail//$'\n'/ ; }" + detail="${detail//$'\r'/ }" + printf '%s' "${detail}" +} + +blitz_log() { + local step="${1:-unknown-step}" + local action="${2:-unknown-action}" + local result="${3:-info}" + local details="${4:-}" + local exit_code="${5:-0}" + + printf '%s | %s | %s | %s | %s | %s\n' \ + "$(blitz_timestamp)" \ + "${step}" \ + "${action}" \ + "${result}" \ + "$(blitz_sanitize_detail "${details}")" \ + "${exit_code}" +} + +blitz_join_cmd() { + local cmd=() + local arg + + for arg in "$@"; do + cmd+=("$(printf '%q' "${arg}")") + done + printf '%s' "${cmd[*]}" +} + +blitz_require_command() { + local command_name="$1" + local step="${2:-precheck}" + + if command -v "${command_name}" >/dev/null 2>&1; then + blitz_log "${step}" "require-command" "success" "command=${command_name}" 0 + return 0 + fi + + blitz_log "${step}" "require-command" "failure" "missing command: ${command_name}" 127 + return 127 +} + +blitz_require_file() { + local path="$1" + local step="${2:-precheck}" + + if [[ -f "${path}" ]]; then + blitz_log "${step}" "require-file" "success" "path=${path}" 0 + return 0 + fi + + blitz_log "${step}" "require-file" "failure" "missing file: ${path}" 1 + return 1 +} + +blitz_require_executable() { + local path="$1" + local step="${2:-precheck}" + + if [[ -x "${path}" ]]; then + blitz_log "${step}" "require-executable" "success" "path=${path}" 0 + return 0 + fi + + blitz_log "${step}" "require-executable" "failure" "missing executable: ${path}" 1 + return 1 +} + +blitz_require_root() { + local step="${1:-precheck}" + + if [[ "${EUID}" -eq 0 ]]; then + blitz_log "${step}" "require-root" "success" "uid=${EUID}" 0 + return 0 + fi + + blitz_log "${step}" "require-root" "failure" "root privileges are required" 1 + return 1 +} + +blitz_run() { + local step="$1" + local action="$2" + local rc + shift 2 + + blitz_log "${step}" "${action}" "start" "$(blitz_join_cmd "$@")" 0 + if "$@"; then + blitz_log "${step}" "${action}" "success" "$(blitz_join_cmd "$@")" 0 + return 0 + else + rc=$? + fi + + blitz_log "${step}" "${action}" "failure" "$(blitz_join_cmd "$@")" "${rc}" + return "${rc}" +} + +blitz_route_ready() { + local target_ip="$1" + local expected_interface="${2:-}" + local route_output + + route_output="$(ip route get "${target_ip}" 2>&1 || true)" + if [[ -z "${route_output}" ]]; then + return 1 + fi + if [[ "${route_output}" == *"unreachable"* || "${route_output}" == *"prohibit"* ]]; then + return 1 + fi + if [[ -n "${expected_interface}" && "${route_output}" != *" dev ${expected_interface} "* && "${route_output}" != *" dev ${expected_interface}" ]]; then + return 1 + fi + + printf '%s\n' "${route_output}" + return 0 +} + +blitz_interface_exists() { + local interface_name="${1:-}" + + if [[ -z "${interface_name}" ]]; then + return 1 + fi + ip link show dev "${interface_name}" >/dev/null 2>&1 +} + +blitz_read_5g_info_interface() { + local info_json="$1" + + if [[ -z "${info_json}" || ! -f "${info_json}" ]]; then + return 1 + fi + + python3 - "${info_json}" <<'PY' +import json +import sys + +path = sys.argv[1] + +try: + with open(path, "r", encoding="utf-8") as handle: + payload = json.load(handle) +except Exception: + raise SystemExit(1) + +interface = str(payload.get("interface") or "").strip() +if not interface: + raise SystemExit(1) + +print(interface) +PY +} + +blitz_detect_5g_interface_from_subnet() { + local modem_subnet="${1:-${BLITZ_5G_MODEM_SUBNET:-}}" + + if [[ -z "${modem_subnet}" ]]; then + return 1 + fi + + python3 - "${modem_subnet}" <<'PY' +import ipaddress +import json +import subprocess +import sys + +subnet = ipaddress.ip_network(sys.argv[1], strict=False) +skip = {"lo", "docker0", "l4tbr0"} + +def priority(name: str) -> tuple[int, str]: + if name.startswith("enx"): + return (0, name) + if name.startswith("wwan"): + return (1, name) + if name.startswith("usb"): + return (2, name) + if name.startswith("eth"): + return (3, name) + return (9, name) + +try: + output = subprocess.check_output(["ip", "-j", "-4", "addr", "show"], text=True) + payload = json.loads(output) +except Exception: + raise SystemExit(1) + +candidates = [] +for item in payload: + ifname = str(item.get("ifname") or "").strip() + if not ifname or ifname in skip: + continue + for addr in item.get("addr_info") or []: + if addr.get("family") != "inet": + continue + local = addr.get("local") + prefixlen = addr.get("prefixlen") + if not local or prefixlen is None: + continue + try: + iface = ipaddress.ip_interface(f"{local}/{prefixlen}") + except ValueError: + continue + if iface.ip in subnet: + candidates.append((priority(ifname), ifname)) + break + +if not candidates: + raise SystemExit(1) + +candidates.sort(key=lambda item: item[0]) +print(candidates[0][1]) +PY +} + +blitz_refresh_5g_info_json() { + local interface_name="$1" + local info_json="${2:-${BLITZ_5G_INFO_JSON:-}}" + + if [[ -z "${interface_name}" || -z "${info_json}" ]]; then + return 1 + fi + + python3 - "${interface_name}" "${info_json}" <<'PY' +import json +import os +import subprocess +import sys + +interface_name = sys.argv[1] +path = sys.argv[2] + +try: + output = subprocess.check_output(["ip", "-j", "addr", "show", "dev", interface_name], text=True) + payload = json.loads(output) +except Exception: + raise SystemExit(1) + +if not payload: + raise SystemExit(1) + +item = payload[0] +ipv4 = [] +ipv6 = [] +for addr in item.get("addr_info") or []: + local = addr.get("local") + prefixlen = addr.get("prefixlen") + family = addr.get("family") + if not local or prefixlen is None: + continue + entry = f"{local}/{prefixlen}" + if family == "inet": + ipv4.append(entry) + elif family == "inet6": + ipv6.append(entry) + +data = { + "interface": interface_name, + "ipv4": ipv4, + "ipv6": ipv6, +} + +parent = os.path.dirname(path) +if parent: + os.makedirs(parent, exist_ok=True) +temp_path = f"{path}.tmp.{os.getpid()}" +with open(temp_path, "w", encoding="utf-8") as handle: + json.dump(data, handle, ensure_ascii=False, indent=2) +os.replace(temp_path, path) +PY +} + +blitz_resolve_5g_interface() { + local explicit_interface="${BLITZ_5G_INTERFACE:-}" + local info_json="${BLITZ_5G_INFO_JSON:-}" + local recorded_interface="" + local detected_interface="" + + if [[ -n "${explicit_interface}" ]]; then + if blitz_interface_exists "${explicit_interface}"; then + printf '%s\n' "${explicit_interface}" + return 0 + fi + return 1 + fi + + recorded_interface="$(blitz_read_5g_info_interface "${info_json}" || true)" + if [[ -n "${recorded_interface}" ]] && blitz_interface_exists "${recorded_interface}"; then + printf '%s\n' "${recorded_interface}" + return 0 + fi + + detected_interface="$(blitz_detect_5g_interface_from_subnet || true)" + if [[ -n "${detected_interface}" ]]; then + if [[ "${detected_interface}" != "${recorded_interface}" ]]; then + blitz_refresh_5g_info_json "${detected_interface}" "${info_json}" >/dev/null 2>&1 || true + fi + printf '%s\n' "${detected_interface}" + return 0 + fi + + return 1 +} + +blitz_prepare_runtime_dir() { + local runtime_dir + + blitz_load_boot_env + runtime_dir="${BLITZ_RUNTIME_DIR}" + + mkdir -p "${runtime_dir}" + if [[ "${EUID}" -eq 0 ]]; then + chown "root:${BLITZ_ROS_USER}" "${runtime_dir}" + chmod 0775 "${runtime_dir}" + else + chmod 0775 "${runtime_dir}" 2>/dev/null || true + fi + blitz_log "runtime-dir" "prepare" "success" "path=${runtime_dir}" 0 +} + +blitz_prepare_run_root() { + local run_root + local run_dir + local incidents_dir + + blitz_load_boot_env + run_root="${BLITZ_RUN_ROOT}" + run_dir="${run_root}/runs" + incidents_dir="${run_root}/incidents" + + mkdir -p "${run_dir}" "${incidents_dir}" + if [[ "${EUID}" -eq 0 ]]; then + chown -R "root:${BLITZ_ROS_USER}" "${run_root}" 2>/dev/null || true + chmod 0775 "${run_root}" "${run_dir}" "${incidents_dir}" 2>/dev/null || true + fi +} + +blitz_load_run_context_env() { + local context_file="${1:-${BLITZ_RUN_CONTEXT_FILE:-}}" + + if [[ -z "${context_file}" || ! -f "${context_file}" ]]; then + return 1 + fi + + set -a + # shellcheck disable=SC1090 + source "${context_file}" + set +a + return 0 +} + +blitz_read_run_id() { + local run_id_file="${BLITZ_RUN_ID_FILE:-}" + + if [[ -z "${run_id_file}" || ! -f "${run_id_file}" ]]; then + return 1 + fi + tr -d '\r\n' < "${run_id_file}" +} + +blitz_utc_compact_timestamp() { + date -u '+%Y%m%dT%H%M%SZ' +} + +blitz_new_run_id() { + printf '%s\n' "$(blitz_utc_compact_timestamp)" +} + +blitz_new_incident_id() { + local prefix="${1:-incident}" + printf '%s-%s-%d\n' "${prefix}" "$(blitz_utc_compact_timestamp)" "$$" +} + +blitz_new_instance_id() { + printf '%s-%d\n' "$(blitz_utc_compact_timestamp)" "$$" +} + +blitz_git_commit() { + git -C "${OMNISOCKETGO_ROOT}" rev-parse HEAD 2>/dev/null || true +} + +blitz_git_dirty_flag() { + if git -C "${OMNISOCKETGO_ROOT}" diff --quiet --ignore-submodules=dirty >/dev/null 2>&1; then + printf '0\n' + return 0 + fi + printf '1\n' +} + +blitz_write_run_context() { + local run_id="$1" + local run_dir="$2" + local boot_id="$3" + local context_file="${BLITZ_RUN_CONTEXT_FILE}" + local id_file="${BLITZ_RUN_ID_FILE}" + local temp_context + local temp_info + local commit_hash + local dirty_flag + local started_at + + commit_hash="$(blitz_git_commit)" + dirty_flag="$(blitz_git_dirty_flag)" + started_at="$(date -u '+%Y-%m-%dT%H:%M:%SZ')" + temp_context="${context_file}.tmp.$$" + temp_info="${run_dir}/run-info.json.tmp.$$" + + mkdir -p "${run_dir}" + printf '%s\n' "${run_id}" > "${id_file}" + + cat > "${temp_context}" </dev/null || echo 0)" + if (( size < max_bytes )); then + return 0 + fi + + for (( index=max_files; index>=1; index-- )); do + if [[ "${index}" -eq "${max_files}" ]]; then + rm -f "${path}.${index}" + fi + if [[ -f "${path}.${index}" ]]; then + mv -f "${path}.${index}" "${path}.$(( index + 1 ))" + fi + done + mv -f "${path}" "${path}.1" +} + +blitz_jsonl_append_line() { + local path="$1" + local line="$2" + + mkdir -p "$(dirname "${path}")" + blitz_jsonl_rotate_if_needed "${path}" + printf '%s\n' "${line}" >> "${path}" +} + +blitz_launch_incident_capture() { + local launch_script="${BOOT_SCRIPT_DIR}/blitz-incident-capture-launch.sh" + + if [[ ! -f "${launch_script}" ]]; then + return 1 + fi + /bin/bash "${launch_script}" "$@" >/dev/null 2>&1 || return 1 +} diff --git a/host/OmniSocketGo_add_camera/scripts/boot/disable-systemd.sh b/host/OmniSocketGo_add_camera/scripts/boot/disable-systemd.sh new file mode 100644 index 0000000..e2f6601 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/disable-systemd.sh @@ -0,0 +1,53 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="disable" +SYSTEMD_DEST_DIR="/etc/systemd/system" +UNITS=( + "blitz-watchdog.service" + "blitz-5g-link-logger.service" + "blitz-b-side-omnid.service" + "blitz-ros-receiver.service" + "blitz-5g-dial.service" + "blitz-run-context.service" + "blitz-boot-gate.service" + "blitz-robot.target" +) + +stop_unit_if_present() { + local unit_name="$1" + local unit_path="${SYSTEMD_DEST_DIR}/${unit_name}" + + if [[ ! -f "${unit_path}" ]]; then + return 0 + fi + blitz_run "${STEP}" "stop-unit" systemctl stop "${unit_name}" || true +} + +disable_unit_if_present() { + local unit_name="$1" + local unit_path="${SYSTEMD_DEST_DIR}/${unit_name}" + + if [[ ! -f "${unit_path}" ]]; then + return 0 + fi + blitz_run "${STEP}" "disable-unit" systemctl disable "${unit_name}" || true +} + +blitz_load_boot_env +blitz_require_root "${STEP}" +blitz_require_command systemctl "${STEP}" + +for unit_name in "${UNITS[@]}"; do + stop_unit_if_present "${unit_name}" +done + +for unit_name in "${UNITS[@]}"; do + disable_unit_if_present "${unit_name}" +done + +blitz_log "${STEP}" "complete" "success" "boot chain stopped and disabled; next reboot will not auto-start blitz services" 0 diff --git a/host/OmniSocketGo_add_camera/scripts/boot/install-systemd.sh b/host/OmniSocketGo_add_camera/scripts/boot/install-systemd.sh new file mode 100644 index 0000000..00145a7 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/install-systemd.sh @@ -0,0 +1,71 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +SYSTEMD_TEMPLATE_DIR="${SCRIPT_DIR}/systemd" +SYSTEMD_DEST_DIR="/etc/systemd/system" + +render_template() { + local template_path="$1" + local output_path="$2" + + sed \ + -e "s|@OMNISOCKETGO_ROOT@|${OMNISOCKETGO_ROOT}|g" \ + -e "s|@BLITZ_LOG_FILE@|${BLITZ_LOG_FILE}|g" \ + -e "s|@BLITZ_ROS_USER@|${BLITZ_ROS_USER}|g" \ + "${template_path}" > "${output_path}" +} + +install_unit() { + local template_name="$1" + local temp_output + + temp_output="$(mktemp)" + render_template "${SYSTEMD_TEMPLATE_DIR}/${template_name}" "${temp_output}" + install -m 0644 "${temp_output}" "${SYSTEMD_DEST_DIR}/${template_name%.in}" + rm -f "${temp_output}" + blitz_log "install" "install-unit" "success" "unit=${SYSTEMD_DEST_DIR}/${template_name%.in}" 0 +} + +remove_unit_if_present() { + local unit_name="$1" + local unit_path="${SYSTEMD_DEST_DIR}/${unit_name}" + + if [[ ! -f "${unit_path}" ]]; then + return 0 + fi + + systemctl disable --now "${unit_name}" >/dev/null 2>&1 || true + rm -f "${unit_path}" + blitz_log "install" "remove-unit" "success" "unit=${unit_path}" 0 +} + +blitz_load_boot_env +blitz_require_root "install" +blitz_require_command install "install" +blitz_require_command systemctl "install" + +mkdir -p "${SYSTEMD_DEST_DIR}" +install -d -m 0755 "$(dirname "${BLITZ_LOG_FILE}")" +touch "${BLITZ_LOG_FILE}" +chmod 0644 "${BLITZ_LOG_FILE}" +blitz_log "install" "prepare-log-file" "success" "log_file=${BLITZ_LOG_FILE}" 0 +blitz_prepare_runtime_dir +blitz_prepare_run_root + +install_unit "blitz-boot-gate.service.in" +install_unit "blitz-run-context.service.in" +install_unit "blitz-5g-dial.service.in" +install_unit "blitz-5g-link-logger.service.in" +install_unit "blitz-ros-receiver.service.in" +install_unit "blitz-b-side-omnid.service.in" +install_unit "blitz-watchdog.service.in" +install_unit "blitz-robot.target.in" +remove_unit_if_present "blitz-time-sync.service" + +blitz_run "install" "daemon-reload" systemctl daemon-reload +blitz_run "install" "enable-target" systemctl enable blitz-robot.target +blitz_log "install" "complete" "success" "run systemctl start blitz-robot.target to launch immediately" 0 diff --git a/host/OmniSocketGo_add_camera/scripts/boot/prepare-runtime-dir.sh b/host/OmniSocketGo_add_camera/scripts/boot/prepare-runtime-dir.sh new file mode 100644 index 0000000..c2b954a --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/prepare-runtime-dir.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="runtime-dir" + +blitz_load_boot_env +blitz_prepare_runtime_dir +blitz_log "${STEP}" "complete" "success" "runtime_dir=${BLITZ_RUNTIME_DIR}" 0 diff --git a/host/OmniSocketGo_add_camera/scripts/boot/rndis_dial.py b/host/OmniSocketGo_add_camera/scripts/boot/rndis_dial.py new file mode 100644 index 0000000..956b871 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/rndis_dial.py @@ -0,0 +1,854 @@ +#!/usr/bin/env python3 +"""RM520N-GL RNDIS 自动拨号脚本。 + +流程: +1. 检测 USB 设备是否存在 +2. 打开 AT 口并检查 SIM 状态 +3. 配置 RNDIS 模式: AT+QCFG="usbnet",3 +4. 重启模块: AT+CFUN=1,1 +5. 等待模块重新枚举并识别 5G 网卡 +6. 如果网卡还没有 IPv4, 自动尝试 DHCP + +用法: + sudo python3 rndis_dial.py + sudo python3 rndis_dial.py --serial-port /dev/ttyUSB7 + sudo python3 rndis_dial.py --interface eth0 #指定网口 +""" + +from __future__ import annotations + +import argparse +import errno +import ipaddress +import json +import os +import select +import shlex +import shutil +import subprocess +import sys +import termios +import time +import tty + +USB_ID = "2c7c:0801" +DEFAULT_SERIAL_PORT = "/dev/ttyUSB7" #串口设备节点 +DEFAULT_BAUD_RATE = 115200 +CHECK_INTERVAL = 2 +SERIAL_READ_TIMEOUT = 0.2 +SERIAL_POLL_INTERVAL = 0.1 +SERIAL_SETTLE_DELAY = 0.3 +AT_SYNC_RETRIES = 3 +AT_SYNC_TIMEOUT = 2.5 +# 示例地址 192.168.225.38/22 所在网段。 +# 拨号成功后会用这个网段来最终确认哪个接口是 5G 模组。 +DEFAULT_MODEM_SUBNET = "192.168.224.0/22" +DEFAULT_MODEM_GATEWAY = "192.168.225.1" +DEFAULT_PUBLIC_TARGETS = ("81.70.156.140", "106.55.173.235") +DEFAULT_INFO_JSON = "modem_network_info.json" +SKIP_INTERFACES = {"lo", "docker0", "l4tbr0"} +BAUD_RATE_MAP = { + 9600: termios.B9600, + 19200: termios.B19200, + 38400: termios.B38400, + 57600: termios.B57600, + 115200: termios.B115200, +} + + +def run_cmd(cmd, timeout=30, check=False): + print(f"[CMD] {format_shell_cmd(cmd)}") + result = subprocess.run( + cmd, + capture_output=True, + text=True, + timeout=timeout, + check=False, + ) + output = (result.stdout or "") + (result.stderr or "") + if check and result.returncode != 0: + raise RuntimeError(f"命令执行失败: {' '.join(cmd)}\n{output.strip()}") + return result.returncode, output.strip() + + +def format_shell_cmd(cmd): + """把命令参数格式化成可直接阅读的 shell 形式。""" + return " ".join(shlex.quote(part) for part in cmd) + + +def parse_ipv4_address(value): + try: + return str(ipaddress.IPv4Address(value)) + except ipaddress.AddressValueError as exc: + raise argparse.ArgumentTypeError(f"无效的 IPv4 地址: {value}") from exc + + +def dedupe_keep_order(values): + seen = set() + result = [] + for value in values: + if value in seen: + continue + seen.add(value) + result.append(value) + return result + + +def require_root(): + if os.geteuid() != 0: + print("[FAIL] 请使用 sudo 运行此脚本") + sys.exit(1) + + +def require_commands(): + missing = [cmd for cmd in ("lsusb", "ip") if shutil.which(cmd) is None] + if missing: + print(f"[FAIL] 缺少系统命令: {', '.join(missing)}") + sys.exit(1) + + +def usb_device_present(): + # 1. 第一次检测 lsusb，确认模块已经被系统识别。 + """通过 lsusb 检查模块是否已经被系统识别。""" + code, output = run_cmd(["lsusb"], timeout=10) + if code != 0: + return False, output + + for line in output.splitlines(): + if USB_ID in line: + return True, line.strip() + return False, output + + +def wait_for_usb_device(expected_present, timeout): + """等待模块 USB 设备下线或重新上线。""" + deadline = time.time() + timeout + last_seen = "" + while time.time() < deadline: + present, detail = usb_device_present() + last_seen = detail + if present == expected_present: + return True, detail + time.sleep(CHECK_INTERVAL) + return False, last_seen + + +def wait_for_path(path, timeout): + """等待串口节点或其他路径重新出现。""" + deadline = time.time() + timeout + while time.time() < deadline: + if os.path.exists(path): + return True + time.sleep(1) + return False + + +def normalize_serial_output(text): + """整理串口原始输出，便于后续匹配关键字。""" + cleaned = text.replace("\r", "\n") + return "\n".join(line for line in cleaned.splitlines() if line.strip()).strip() + + +def serial_response_complete(text): + if not text: + return False + + for line in reversed(text.splitlines()): + stripped = line.strip() + if stripped == "OK": + return True + if "ERROR" in stripped: + return True + return False + + +class RawSerialSession: + """使用 Python 标准库直接控制 Linux 串口，尽量贴近 stty/raw 行为。""" + + def __init__(self, port, baudrate): + if baudrate not in BAUD_RATE_MAP: + raise RuntimeError(f"不支持的波特率: {baudrate}") + + self.port = port + self.fd = None + self._original_attrs = None + + try: + self.fd = os.open(port, os.O_RDWR | os.O_NOCTTY | os.O_NONBLOCK) + self._original_attrs = termios.tcgetattr(self.fd) + tty.setraw(self.fd, when=termios.TCSANOW) + + attrs = termios.tcgetattr(self.fd) + attrs[0] = 0 + attrs[1] = 0 + attrs[2] &= ~(termios.PARENB | termios.CSTOPB | termios.CSIZE) + attrs[2] |= termios.CS8 | termios.CLOCAL | termios.CREAD + attrs[3] = 0 + attrs[4] = BAUD_RATE_MAP[baudrate] + attrs[5] = BAUD_RATE_MAP[baudrate] + attrs[6][termios.VMIN] = 0 + attrs[6][termios.VTIME] = 0 + termios.tcsetattr(self.fd, termios.TCSANOW, attrs) + termios.tcflush(self.fd, termios.TCIOFLUSH) + except OSError as exc: + self.close() + raise RuntimeError(f"无法打开串口 {port}: {exc}") from exc + + @property + def is_open(self): + return self.fd is not None + + def reset_input_buffer(self): + if self.fd is not None: + termios.tcflush(self.fd, termios.TCIFLUSH) + + def reset_output_buffer(self): + if self.fd is not None: + termios.tcflush(self.fd, termios.TCOFLUSH) + + def write(self, data): + if self.fd is None: + raise OSError("串口未打开") + + sent = 0 + while sent < len(data): + try: + written = os.write(self.fd, data[sent:]) + except BlockingIOError: + time.sleep(SERIAL_POLL_INTERVAL) + continue + if written <= 0: + raise OSError("串口写入返回 0 字节") + sent += written + + def flush(self): + if self.fd is not None: + termios.tcdrain(self.fd) + + def read_chunk(self, timeout, size=4096): + if self.fd is None: + return b"" + + ready, _, _ = select.select([self.fd], [], [], timeout) + if not ready: + return b"" + + try: + return os.read(self.fd, size) + except BlockingIOError: + return b"" + + def close(self): + if self.fd is None: + return + + fd = self.fd + self.fd = None + + if self._original_attrs is not None: + try: + termios.tcsetattr(fd, termios.TCSANOW, self._original_attrs) + except termios.error: + pass + os.close(fd) + + +def read_serial_output(session, timeout, allow_disconnect=False): + """在给定时间窗口内读取 AT 响应，直到出现结束标记或超时。""" + deadline = time.time() + timeout + chunks = [] + saw_terminal_line = False + last_data_time = None + + while time.time() < deadline: + try: + chunk = session.read_chunk(timeout=min(SERIAL_READ_TIMEOUT, max(deadline - time.time(), 0))) + except OSError as exc: + if allow_disconnect and exc.errno in (errno.EIO, errno.ENODEV, errno.EBADF): + break + raise RuntimeError(f"读取串口响应失败: {exc}") from exc + + if chunk: + chunks.append(chunk.decode(errors="ignore")) + last_data_time = time.time() + current_text = normalize_serial_output("".join(chunks)) + if serial_response_complete(current_text): + saw_terminal_line = True + continue + + if saw_terminal_line and last_data_time is not None and time.time() - last_data_time >= SERIAL_SETTLE_DELAY: + break + + time.sleep(SERIAL_POLL_INTERVAL) + + return normalize_serial_output("".join(chunks)) + + +def open_serial_session(port): + """打开 AT 串口会话，后续在同一连接里顺序发送多条命令。""" + ser = RawSerialSession(port=port, baudrate=DEFAULT_BAUD_RATE) + time.sleep(0.2) + ser.reset_input_buffer() + ser.reset_output_buffer() + return ser + + +def execute_serial_step(ser, command, expect=None, timeout=3, allow_disconnect=False): + """在当前串口会话里发送一条 AT 命令并校验响应。""" + print(f"[AT] {command}") + try: + ser.reset_input_buffer() + ser.write((command + "\r").encode()) + ser.flush() + except OSError as exc: + raise RuntimeError(f"AT 命令 `{command}` 发送失败: {exc}") from exc + + response = read_serial_output(ser, timeout=timeout, allow_disconnect=allow_disconnect) + + if response: + print(response) + else: + print("(无响应)") + + if "ERROR" in response: + raise RuntimeError(f"AT 命令 `{command}` 执行失败: {response}") + if expect and expect not in response and not allow_disconnect: + raise RuntimeError(f"AT 命令 `{command}` 响应异常: {response or '空响应'}") + return response + + +def synchronize_at_channel(ser): + """某些模组 AT 口在刚打开时需要先用 AT 做一次预热。""" + last_error = None + + for attempt in range(1, AT_SYNC_RETRIES + 1): + try: + print(f"[INFO] 预热 AT 通道，第 {attempt} 次") + response = execute_serial_step(ser, "AT", expect="OK", timeout=AT_SYNC_TIMEOUT) + if "OK" in response: + return + except RuntimeError as exc: + last_error = exc + time.sleep(0.5) + + if last_error is not None: + raise RuntimeError( + "AT 通道预热失败，请确认串口是否是 AT 命令口，例如 /dev/ttyUSB2" + ) from last_error + raise RuntimeError("AT 通道预热失败") + + +def run_serial_steps(port, steps): + """在同一个串口会话里顺序执行多条 AT 命令。""" + ser = None + + try: + ser = open_serial_session(port) + synchronize_at_channel(ser) + for step in steps: + execute_serial_step( + ser, + step["command"], + expect=step.get("expect"), + timeout=step.get("timeout", 3), + allow_disconnect=step.get("allow_disconnect", False), + ) + finally: + if ser is not None and ser.is_open: + ser.close() + +def configure_rndis(port): + # 2. 用 Python 串口库在同一会话里顺序执行拨号相关 AT 命令。 + """切换到 RNDIS 模式并触发模块重启。""" + if not wait_for_path(port, timeout=30): + raise RuntimeError(f"串口不存在: {port}") + + print(f"[OK] 串口已打开: {port}") + run_serial_steps( + port, + [ + {"command": "AT+CPIN?", "expect": "READY", "timeout": 4}, + {"command": 'AT+QCFG="usbnet",3', "expect": "OK", "timeout": 5}, + {"command": "AT+CFUN=1,1", "timeout": 4, "allow_disconnect": True}, + ], + ) + + +def get_interfaces(): + """列出当前系统中的接口，过滤明显无关的本地接口。""" + interfaces = [] + try: + for name in os.listdir("/sys/class/net"): + if name in SKIP_INTERFACES or is_usb_gadget(name): + continue + interfaces.append(name) + except FileNotFoundError: + return [] + return sorted(interfaces) + + +def is_usb_gadget(iface): + """过滤 Jetson 自己暴露出去的 gadget 网卡。""" + sysfs_path = f"/sys/class/net/{iface}" + if not os.path.exists(sysfs_path): + return False + return "/gadget/" in os.path.realpath(sysfs_path) + + +def is_usb_network_interface(iface): + """判断接口是否来自 USB 设备。""" + device_path = f"/sys/class/net/{iface}/device" + if not os.path.exists(device_path): + return False + real_path = os.path.realpath(device_path) + return "/usb" in real_path + + +def get_ipv4_addrs(): + """返回所有接口的 IPv4/CIDR 信息。""" + code, output = run_cmd(["ip", "-o", "-4", "addr", "show"], timeout=10) + if code != 0: + return {} + + ipv4_addrs = {} + for line in output.splitlines(): + parts = line.split() + if len(parts) >= 4: + iface = parts[1] + ipv4_addrs.setdefault(iface, []).append(parts[3]) + return ipv4_addrs + + +def get_ipv6_addrs(): + """返回所有接口的 IPv6/CIDR 信息。""" + code, output = run_cmd(["ip", "-o", "-6", "addr", "show"], timeout=10) + if code != 0: + return {} + + ipv6_addrs = {} + for line in output.splitlines(): + parts = line.split() + if len(parts) >= 4: + iface = parts[1] + ipv6_addrs.setdefault(iface, []).append(parts[3]) + return ipv6_addrs + + +def interface_priority(iface): + if iface.startswith("wwan"): + return 0 + if iface.startswith("enx"): + return 1 + if iface.startswith("usb"): + return 2 + return 10 + + +def list_usb_network_candidates(explicit_iface=None): + """列出拨号前可尝试的 USB 网卡候选项。 + + 这里不靠固定网口名确认 5G 模组，只是在还没有 IP 的时候先缩小范围。 + 真正确认模组接口，会在 DHCP 之后根据 IP 网段判断。 + """ + candidates = [] + + for iface in get_interfaces(): + if explicit_iface and iface != explicit_iface: + continue + if not is_usb_network_interface(iface): + continue + candidates.append((interface_priority(iface), iface)) + + if not candidates: + return [] + + candidates.sort() + return [iface for _, iface in candidates] + + +def ip_in_subnet(ip_cidr, subnet): + """判断接口地址是否落在指定网段内。""" + try: + return ipaddress.ip_interface(ip_cidr).ip in ipaddress.ip_network(subnet, strict=False) + except ValueError: + return False + + +def find_interface_by_subnet(modem_subnet, explicit_iface=None): + """拨号成功后，通过 IP 网段确认 5G 模组网卡。""" + candidates = [] + for iface, addrs in get_ipv4_addrs().items(): + if iface in SKIP_INTERFACES or is_usb_gadget(iface): + continue + if not is_usb_network_interface(iface): + continue + if explicit_iface and iface != explicit_iface: + continue + + matched_addrs = [addr for addr in addrs if ip_in_subnet(addr, modem_subnet)] + if matched_addrs: + candidates.append((interface_priority(iface), iface, matched_addrs)) + + if not candidates: + return None, [] + + candidates.sort() + _, iface, matched_addrs = candidates[0] + return iface, matched_addrs + + +def wait_for_usb_candidates(explicit_iface=None, timeout=90): + """等待模块枚举出 USB 网卡候选项。""" + deadline = time.time() + timeout + while time.time() < deadline: + candidates = list_usb_network_candidates(explicit_iface=explicit_iface) + if candidates: + return candidates + time.sleep(CHECK_INTERVAL) + return [] + + +def bring_interface_up(iface): + code, output = run_cmd(["ip", "link", "set", "dev", iface, "up"], timeout=10) + if code != 0: + raise RuntimeError(f"拉起网卡失败: {iface}\n{output}") + + +def renew_dhcp(iface): + dhclient = shutil.which("dhclient") + udhcpc = shutil.which("udhcpc") + + if dhclient: + print(f"[INFO] 使用 dhclient 为 {iface} 获取 IP") + code, output = run_cmd(["dhclient", "-1", "-v", iface], timeout=45) + return code == 0, output + + if udhcpc: + print(f"[INFO] 使用 udhcpc 为 {iface} 获取 IP") + code, output = run_cmd(["udhcpc", "-n", "-q", "-i", iface], timeout=45) + return code == 0, output + + return False, "系统中未找到 dhclient 或 udhcpc" + + +def get_default_routes(iface): + code, output = run_cmd(["ip", "-o", "route", "show", "default", "dev", iface], timeout=10) + if code != 0: + return [] + return [line.strip() for line in output.splitlines() if line.strip()] + + +def resolve_gateway(iface, fallback_gateway): + for route in get_default_routes(iface): + tokens = route.split() + for index, token in enumerate(tokens[:-1]): + if token == "via": + gateway = tokens[index + 1] + print(f"[INFO] 从默认路由检测到 {iface} 网关: {gateway}") + return gateway + + print(f"[INFO] 未从默认路由检测到 {iface} 网关，回退到 {fallback_gateway}") + return fallback_gateway + + +def delete_default_routes(iface): + removed = 0 + + while True: + routes = get_default_routes(iface) + if not routes: + return removed + + deleted_this_round = False + for route in routes: + cmd = ["ip", "route", "del", *route.split()] + code, output = run_cmd(cmd, timeout=10) + if code != 0: + code, output = run_cmd(["ip", "route", "del", "default", "dev", iface], timeout=10) + if code != 0: + raise RuntimeError(f"删除默认路由失败: {iface}\n{output}") + removed += 1 + deleted_this_round = True + + if not deleted_this_round: + raise RuntimeError(f"未能删除 {iface} 的默认路由") + + +def install_host_routes(iface, gateway, targets): + for target in dedupe_keep_order(targets): + cmd = ["ip", "route", "replace", f"{target}/32", "via", gateway, "dev", iface] + code, output = run_cmd(cmd, timeout=10) + if code != 0: + raise RuntimeError(f"添加主机路由失败: {target} via {gateway} dev {iface}\n{output}") + + print(f"[OK] 已添加主机路由: {target}/32 via {gateway} dev {iface}") + + +def enforce_route_policy(iface, fallback_gateway, route_targets): + gateway = resolve_gateway(iface, fallback_gateway) + removed = delete_default_routes(iface) + print(f"[OK] 已删除 {iface} 上的 {removed} 条默认路由") + + if route_targets: + install_host_routes(iface, gateway, route_targets) + else: + print(f"[WARN] {iface} 未配置任何主机路由目标，5G 将不再承载公网流量") + + +def ensure_ipv4(iface): + """为指定接口申请 IPv4 地址。""" + ipv4_addrs = get_ipv4_addrs().get(iface, []) + if ipv4_addrs: + return ipv4_addrs + + bring_interface_up(iface) + ok, output = renew_dhcp(iface) + if output: + print(output) + if not ok: + return [] + + return get_ipv4_addrs().get(iface, []) + + +def acquire_modem_interface(modem_subnet, explicit_iface=None): + """通过 DHCP + IP 网段识别真正的模组接口。""" + iface, matched_addrs = find_interface_by_subnet( + modem_subnet, + explicit_iface=explicit_iface, + ) + if iface: + return iface, matched_addrs + + candidates = list_usb_network_candidates(explicit_iface=explicit_iface) + if not candidates: + raise RuntimeError("未找到可尝试 DHCP 的 USB 网卡候选项") + + print(f"[INFO] 当前 USB 网卡候选项: {', '.join(candidates)}") + + for iface in candidates: + print(f"[INFO] 尝试为 {iface} 获取 IPv4") + ensure_ipv4(iface) + + matched_iface, matched_addrs = find_interface_by_subnet( + modem_subnet, + explicit_iface=explicit_iface, + ) + if matched_iface: + return matched_iface, matched_addrs + + return None, [] + + +def print_interface_status(iface): + # 3. 拨号成功后，打印 ip/ifconfig，确认模组网口和地址。 + print(f"[OK] 检测到 5G 网卡: {iface}") + + code, output = run_cmd(["ip", "-4", "addr", "show", "dev", iface], timeout=10) + if code == 0 and output: + print(output) + + if shutil.which("ifconfig"): + code, ifconfig_output = run_cmd(["ifconfig", iface], timeout=10) + if code == 0 and ifconfig_output: + print("\n===== ifconfig =====") + print(ifconfig_output) + + +def save_interface_info(iface, output_file=DEFAULT_INFO_JSON): + """把网口名称、IPv4、IPv6 保存到 JSON 文件。""" + data = { + "interface": iface, + "ipv4": get_ipv4_addrs().get(iface, []), + "ipv6": get_ipv6_addrs().get(iface, []), + } + + with open(output_file, "w", encoding="utf-8") as json_file: + json.dump(data, json_file, ensure_ascii=False, indent=2) + + print(f"[OK] 网口信息已保存到 {output_file}") + + +def ping_target(iface, target, count=3, timeout=15): + """通过指定网口 ping 一个目标。""" + code, output = run_cmd( + ["ping", "-I", iface, "-c", str(count), "-W", "3", target], + timeout=timeout, + ) + return code == 0, output + + +def print_ping_summary(output): + """只打印 ping 的关键结果。""" + for line in output.splitlines(): + if "packets transmitted" in line or "rtt " in line or "Destination " in line: + print(line) + + +def verify_connectivity(iface, gateway=DEFAULT_MODEM_GATEWAY, targets=DEFAULT_PUBLIC_TARGETS, retry_interval=3, max_wait=45): + # 4. 最后先 ping 模组网关，再重试公网连通性。 + """先测模组网关，再轮询公网目标地址。""" + ok, output = ping_target(iface, gateway, count=3, timeout=15) + if ok: + print(f"[OK] {iface} 可到达模组网关 {gateway}") + print_ping_summary(output) + else: + print(f"[WARN] {iface} 无法到达模组网关 {gateway}") + if output: + print(output) + return False + + deadline = time.time() + max_wait + attempt = 1 + while True: + for target in targets: + ok, output = ping_target(iface, target, count=3, timeout=15) + if ok: + print(f"[OK] {iface} 可通过 {target}") + print_ping_summary(output) + return True + + print(f"[WARN] 第 {attempt} 次 Ping {target} 失败") + if output: + print_ping_summary(output) + + if time.time() >= deadline: + print(f"[WARN] {iface} 在 {max_wait} 秒内仍无法连通 {', '.join(targets)}") + return False + + attempt += 1 + time.sleep(retry_interval) + + +def ping_via_interface(iface, targets=DEFAULT_PUBLIC_TARGETS): + """保留原调用点，内部走完整连通性检查。""" + return verify_connectivity(iface, targets=targets) + + +def parse_args(): + parser = argparse.ArgumentParser(description="RM520N-GL RNDIS 自动拨号脚本") + parser.add_argument( + "--serial-port", + default=DEFAULT_SERIAL_PORT, + help=f"AT 串口路径，默认 {DEFAULT_SERIAL_PORT}", + ) + parser.add_argument( + "--interface", + help="指定期望的 5G 网卡名，例如 eth0", + ) + parser.add_argument( + "--modem-subnet", + default=DEFAULT_MODEM_SUBNET, + help=f"拨号成功后用于识别模组接口的 IPv4 网段，默认 {DEFAULT_MODEM_SUBNET}", + ) + parser.add_argument( + "--gateway", + type=parse_ipv4_address, + default=DEFAULT_MODEM_GATEWAY, + help=f"5G 模组网关地址，默认 {DEFAULT_MODEM_GATEWAY}", + ) + parser.add_argument( + "--skip-dhcp", + action="store_true", + help="只等待 USB 网卡出现，不主动申请 IPv4", + ) + parser.add_argument( + "--remove-default-route", + action="store_true", + help="拨号成功后删除 5G 接口上的默认路由，只保留显式主机路由", + ) + parser.add_argument( + "--route-target", + action="append", + default=[], + type=parse_ipv4_address, + help="拨号完成后通过 5G 接口保留的 IPv4 主机路由目标，可重复传入", + ) + return parser.parse_args() + + +def main(): + args = parse_args() + require_root() + require_commands() + + print("===== RM520N-GL RNDIS 自动拨号 =====") + print(f"[INFO] 目标模组网段: {args.modem_subnet}") + + #1.检测 lsusb，确认是否识别到模块 + present, detail = usb_device_present() + if not present: + print(f"[FAIL] 未检测到模块 USB 设备 {USB_ID}") + if detail: + print(detail) + sys.exit(1) + + print(f"[OK] 检测到 USB 设备: {detail}") + print(f"[INFO] 使用 AT 口: {args.serial_port}") + + #2.进行 Python 串口拨号 + try: + configure_rndis(args.serial_port) + + print("[INFO] 已发送 AT+CFUN=1,1，等待模块重启") + disappeared, _ = wait_for_usb_device(expected_present=False, timeout=25) + if disappeared: + print("[OK] 模块已下线，继续等待重新枚举") + else: + print("[WARN] 未观察到模块下线，继续等待重新枚举") + + reappeared, detail = wait_for_usb_device(expected_present=True, timeout=90) + if not reappeared: + print(f"[FAIL] 模块重启后未重新枚举: {USB_ID}") + sys.exit(1) + + print(f"[OK] 模块已重新枚举: {detail}") + + candidates = wait_for_usb_candidates(explicit_iface=args.interface, timeout=90) + if not candidates: + print("[FAIL] 未检测到 5G 模组枚举出的 USB 网卡") + sys.exit(1) + + if args.skip_dhcp: + print(f"[INFO] 当前 USB 网卡候选项: {', '.join(candidates)}") + iface, ipv4_addrs = find_interface_by_subnet( + args.modem_subnet, + explicit_iface=args.interface, + ) + if not iface: + print(f"[WARN] 当前还没有接口拿到目标网段 {args.modem_subnet} 的地址") + sys.exit(1) + else: + iface, ipv4_addrs = acquire_modem_interface( + args.modem_subnet, + explicit_iface=args.interface, + ) + if not iface: + print(f"[FAIL] 未找到落在目标网段 {args.modem_subnet} 内的模组接口") + sys.exit(1) + + print_interface_status(iface) + + if ipv4_addrs: + for addr in ipv4_addrs: + print(f"[OK] {iface} 已获取 IPv4: {addr}") + save_interface_info(iface) + route_targets = dedupe_keep_order(args.route_target) + if args.remove_default_route: + enforce_route_policy(iface, args.gateway, route_targets) + + connectivity_targets = route_targets or list(DEFAULT_PUBLIC_TARGETS) + ping_via_interface(iface, targets=connectivity_targets) + print(f"[DONE] RNDIS 拨号完成，可执行: sudo python3 speed_test.py {iface}") + return + + print(f"[WARN] {iface} 已出现，但还没有 IPv4 地址") + print(f"[INFO] 可手动检查: ip addr show {iface}") + sys.exit(1) + except (RuntimeError, subprocess.TimeoutExpired) as exc: + print(f"[FAIL] {exc}") + sys.exit(1) + + +if __name__ == "__main__": + main() diff --git a/host/OmniSocketGo_add_camera/scripts/boot/robot-boot.env b/host/OmniSocketGo_add_camera/scripts/boot/robot-boot.env new file mode 100644 index 0000000..152a737 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/robot-boot.env @@ -0,0 +1,60 @@ +# Boot-time settings for the robot-side autostart chain. +# Override machine-specific values in robot-boot.env.local. + +BLITZ_BOOT_DELAY_SEC="30" +BLITZ_RUN_ROOT="/var/log/blitz-robot" +BLITZ_LOG_FILE="/var/log/blitz-robot/startup.log" +BLITZ_RUNTIME_DIR="/run/blitz-robot" +BLITZ_RUN_CONTEXT_FILE="${BLITZ_RUNTIME_DIR}/run-context.env" +BLITZ_RUN_ID_FILE="${BLITZ_RUNTIME_DIR}/run-id" +BLITZ_CURRENT_RUN_LINK="${BLITZ_RUN_ROOT}/current" + +BLITZ_5G_DIAL_DIR="${OMNISOCKETGO_ROOT}/scripts/boot" +BLITZ_5G_SERIAL_PORT="/dev/ttyUSB2" +BLITZ_5G_INTERFACE="" +BLITZ_5G_MODEM_SUBNET="192.168.224.0/22" +BLITZ_5G_GATEWAY="192.168.225.1" +BLITZ_5G_SKIP_DHCP="0" +BLITZ_5G_REMOVE_DEFAULT_ROUTE="1" +BLITZ_5G_ROUTE_TARGETS="106.55.173.235" +BLITZ_5G_INFO_JSON="${OMNISOCKETGO_ROOT}/scripts/boot/modem_network_info.json" +BLITZ_5G_SERIAL_WAIT_SEC="60" +BLITZ_5G_ROUTE_WAIT_SEC="30" + +# Leave empty to fall back to the host part of ROBOT_SIDE_OMNISOCKET_SERVER_ADDR. +BLITZ_TIME_SERVER_IP="81.70.156.140" + +BLITZ_ROS_USER="nvidia" +BLITZ_ROS_SOCKET_WAIT_SEC="20" +BLITZ_WATCHDOG_INTERVAL_SEC="5" +BLITZ_HEALTH_STALE_SEC="15" +BLITZ_OMNID_THREAD_HEARTBEAT_TIMEOUT_SEC="15" +BLITZ_KCP_STATS_INTERVAL_MS="1000" +BLITZ_CONTROL_LATENCY_LOG_ENABLED="1" +BLITZ_CONTROL_LATENCY_LOG_SAMPLE_MOD="100" +BLITZ_CONTROL_ACK_SAMPLE_MOD="10" +BLITZ_VIDEO_STAGE_LOG_ENABLED="1" +BLITZ_VIDEO_STAGE_LOG_SAMPLE_MOD="10" +BLITZ_5G_LINK_LOG_INTERVAL_SEC="5" +BLITZ_JSONL_FLUSH_INTERVAL_MS="1000" +BLITZ_JSONL_FLUSH_BYTES="262144" +BLITZ_JSONL_ROTATE_BYTES="134217728" +BLITZ_JSONL_ROTATE_FILES="8" +# Log one normal relay packet out of every N packets. Drop events still log immediately. +OMNI_RELAY_PACKET_LOG_SAMPLE_EVERY="200" +BLITZ_INCIDENT_COMMAND_TIMEOUT_SEC="5" +BLITZ_INCIDENT_TOTAL_TIMEOUT_SEC="30" +BLITZ_NETWORK_FAIL_THRESHOLD="3" +BLITZ_NETWORK_RECOVERY_COOLDOWN_SEC="30" +BLITZ_GPS_MONITOR_ENABLED="1" +BLITZ_GPS_DEVICE_GLOB="/dev/ttyCH341USB*" +BLITZ_GPS_CHECK_INTERVAL_SEC="10" +BLITZ_GPS_RESTART_UNITS="gpsd.socket gpsd.service" +BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION="0" + +OMNI_CAMERA_DEVICE="/dev/v4l/by-path/platform-a80aa10000.usb-usb-0:3.2:1.4-video-index0" + +# Boot units run b_side_omnid as root directly, so nested sudo must stay off. +B_SIDE_OMNID_USE_SUDO="0" +OMNI_CONTROL_ACK_PEER_ID="peer-b-ctrl-ack" +OMNI_CONTROL_ACK_TARGET_PEER="peer-a-ctrl-ack" diff --git a/host/OmniSocketGo_add_camera/scripts/boot/start-5g-link-logger-service.sh b/host/OmniSocketGo_add_camera/scripts/boot/start-5g-link-logger-service.sh new file mode 100644 index 0000000..ea2c051 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/start-5g-link-logger-service.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}/common.sh" + +STEP="5g-link-logger-service" + +blitz_load_boot_env +blitz_require_run_context + +export OMNI_BOOT_MODE="1" +export BLITZ_INSTANCE_ID="${BLITZ_INSTANCE_ID:-$(blitz_new_instance_id)}" +export BLITZ_5G_LINK_LOG_PATH="${BLITZ_5G_LINK_LOG_PATH:-${BLITZ_RUN_DIR}/b-5g-link-quality.${BLITZ_INSTANCE_ID}.jsonl}" + +blitz_log "${STEP}" "start" "start" "exec bash ${OMNISOCKETGO_ROOT}/scripts/boot/blitz-5g-link-logger.sh" 0 +exec bash "${OMNISOCKETGO_ROOT}/scripts/boot/blitz-5g-link-logger.sh" diff --git a/host/OmniSocketGo_add_camera/scripts/boot/start-b-side-omnid-service.sh b/host/OmniSocketGo_add_camera/scripts/boot/start-b-side-omnid-service.sh new file mode 100644 index 0000000..53eea06 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/start-b-side-omnid-service.sh @@ -0,0 +1,19 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="b-side-omnid" + +blitz_load_boot_env +blitz_require_run_context + +blitz_require_executable "${OMNISOCKETGO_ROOT}/bin/b_side_omnid" "${STEP}" + +export OMNI_BOOT_MODE="1" +export B_SIDE_OMNID_USE_SUDO="0" + +blitz_log "${STEP}" "start" "start" "exec bash ${OMNISOCKETGO_ROOT}/scripts/dev/start-b-side-omnid.sh" 0 +exec bash "${OMNISOCKETGO_ROOT}/scripts/dev/start-b-side-omnid.sh" diff --git a/host/OmniSocketGo_add_camera/scripts/boot/start-ros-receiver-service.sh b/host/OmniSocketGo_add_camera/scripts/boot/start-ros-receiver-service.sh new file mode 100644 index 0000000..8bea80a --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/start-ros-receiver-service.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}/common.sh" + +STEP="ros-receiver" + +blitz_load_boot_env +blitz_require_run_context + +blitz_require_file "/opt/ros/${ROS_DISTRO}/setup.bash" "${STEP}" +blitz_require_file "${ROS_CONTROL_PY_DIR}/install/setup.bash" "${STEP}" + +export OMNI_BOOT_MODE="1" +blitz_log "${STEP}" "start" "start" "exec bash ${OMNISOCKETGO_ROOT}/scripts/dev/start-ros-receiver.sh" 0 +exec bash "${OMNISOCKETGO_ROOT}/scripts/dev/start-ros-receiver.sh" diff --git a/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-5g-dial.service.in b/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-5g-dial.service.in new file mode 100644 index 0000000..02a5c64 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-5g-dial.service.in @@ -0,0 +1,15 @@ +[Unit] +Description=Blitz robot 5G dial +PartOf=blitz-robot.target +After=blitz-run-context.service +Requires=blitz-run-context.service + +[Service] +Type=oneshot +RemainAfterExit=yes +ExecStart=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/5g-dial.sh +StandardOutput=append:@BLITZ_LOG_FILE@ +StandardError=append:@BLITZ_LOG_FILE@ + +[Install] +WantedBy=blitz-robot.target diff --git a/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-5g-link-logger.service.in b/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-5g-link-logger.service.in new file mode 100644 index 0000000..81b810b --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-5g-link-logger.service.in @@ -0,0 +1,19 @@ +[Unit] +Description=Blitz robot 5G link logger +PartOf=blitz-robot.target +After=blitz-run-context.service blitz-5g-dial.service +Requires=blitz-run-context.service +Wants=blitz-run-context.service blitz-5g-dial.service + +[Service] +Type=simple +EnvironmentFile=-/run/blitz-robot/run-context.env +ExecStartPre=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/prepare-runtime-dir.sh +ExecStart=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/start-5g-link-logger-service.sh +Restart=always +RestartSec=5 +StandardOutput=append:@BLITZ_LOG_FILE@ +StandardError=append:@BLITZ_LOG_FILE@ + +[Install] +WantedBy=blitz-robot.target diff --git a/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-b-side-omnid.service.in b/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-b-side-omnid.service.in new file mode 100644 index 0000000..bce9b11 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-b-side-omnid.service.in @@ -0,0 +1,20 @@ +[Unit] +Description=Blitz robot b-side omnid +PartOf=blitz-robot.target +After=blitz-run-context.service blitz-5g-dial.service blitz-ros-receiver.service +Requires=blitz-run-context.service +Wants=blitz-run-context.service blitz-5g-dial.service blitz-ros-receiver.service + +[Service] +Type=simple +EnvironmentFile=-/run/blitz-robot/run-context.env +ExecStartPre=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/prepare-runtime-dir.sh +ExecStart=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/start-b-side-omnid-service.sh +ExecStopPost=/bin/bash -lc 'if [[ "${SERVICE_RESULT:-success}" != "success" ]]; then exec /bin/bash "@OMNISOCKETGO_ROOT@/scripts/boot/blitz-incident-capture-launch.sh" --source exec-stop-post --unit "%n" --result "${SERVICE_RESULT:-}" --exit-status "${EXIT_STATUS:-}" --reason b-side-service-exit; fi' +Restart=always +RestartSec=2 +StandardOutput=append:@BLITZ_LOG_FILE@ +StandardError=append:@BLITZ_LOG_FILE@ + +[Install] +WantedBy=blitz-robot.target diff --git a/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-boot-gate.service.in b/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-boot-gate.service.in new file mode 100644 index 0000000..5f918ef --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-boot-gate.service.in @@ -0,0 +1,15 @@ +[Unit] +Description=Blitz robot boot gate +PartOf=blitz-robot.target +After=multi-user.target network-online.target +Wants=network-online.target + +[Service] +Type=oneshot +RemainAfterExit=yes +ExecStart=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/boot-gate.sh +StandardOutput=append:@BLITZ_LOG_FILE@ +StandardError=append:@BLITZ_LOG_FILE@ + +[Install] +WantedBy=blitz-robot.target diff --git a/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-robot.target.in b/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-robot.target.in new file mode 100644 index 0000000..7590c67 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-robot.target.in @@ -0,0 +1,13 @@ +[Unit] +Description=Blitz robot boot chain +Wants=blitz-boot-gate.service +Wants=blitz-run-context.service +Wants=blitz-5g-dial.service +Wants=blitz-5g-link-logger.service +Wants=blitz-ros-receiver.service +Wants=blitz-b-side-omnid.service +Wants=blitz-watchdog.service +After=multi-user.target + +[Install] +WantedBy=multi-user.target diff --git a/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-ros-receiver.service.in b/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-ros-receiver.service.in new file mode 100644 index 0000000..634b19b --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-ros-receiver.service.in @@ -0,0 +1,23 @@ +[Unit] +Description=Blitz robot ROS receiver +PartOf=blitz-robot.target +After=blitz-run-context.service blitz-5g-dial.service +Requires=blitz-run-context.service +Wants=blitz-run-context.service blitz-5g-dial.service + +[Service] +Type=simple +User=@BLITZ_ROS_USER@ +PermissionsStartOnly=true +EnvironmentFile=-/run/blitz-robot/run-context.env +ExecStartPre=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/prepare-runtime-dir.sh +ExecStart=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/start-ros-receiver-service.sh +ExecStartPost=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/wait-for-unix-socket.sh --step ros-receiver +ExecStopPost=/bin/bash -lc 'if [[ "${SERVICE_RESULT:-success}" != "success" ]]; then exec /bin/bash "@OMNISOCKETGO_ROOT@/scripts/boot/blitz-incident-capture-launch.sh" --source exec-stop-post --unit "%n" --result "${SERVICE_RESULT:-}" --exit-status "${EXIT_STATUS:-}" --reason ros-service-exit; fi' +Restart=always +RestartSec=2 +StandardOutput=append:@BLITZ_LOG_FILE@ +StandardError=append:@BLITZ_LOG_FILE@ + +[Install] +WantedBy=blitz-robot.target diff --git a/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-run-context.service.in b/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-run-context.service.in new file mode 100644 index 0000000..2ace077 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-run-context.service.in @@ -0,0 +1,15 @@ +[Unit] +Description=Blitz robot run context +PartOf=blitz-robot.target +After=blitz-boot-gate.service +Requires=blitz-boot-gate.service + +[Service] +Type=oneshot +RemainAfterExit=yes +ExecStart=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/blitz-run-context.sh +StandardOutput=append:@BLITZ_LOG_FILE@ +StandardError=append:@BLITZ_LOG_FILE@ + +[Install] +WantedBy=blitz-robot.target diff --git a/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-watchdog.service.in b/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-watchdog.service.in new file mode 100644 index 0000000..882d5b7 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/systemd/blitz-watchdog.service.in @@ -0,0 +1,19 @@ +[Unit] +Description=Blitz robot health watchdog +PartOf=blitz-robot.target +After=blitz-run-context.service blitz-b-side-omnid.service blitz-ros-receiver.service +Requires=blitz-run-context.service +Wants=blitz-run-context.service blitz-b-side-omnid.service blitz-ros-receiver.service + +[Service] +Type=simple +EnvironmentFile=-/run/blitz-robot/run-context.env +ExecStartPre=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/prepare-runtime-dir.sh +ExecStart=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/blitz-watchdog.sh +Restart=always +RestartSec=5 +StandardOutput=append:@BLITZ_LOG_FILE@ +StandardError=append:@BLITZ_LOG_FILE@ + +[Install] +WantedBy=blitz-robot.target diff --git a/host/OmniSocketGo_add_camera/scripts/boot/wait-for-unix-socket.sh b/host/OmniSocketGo_add_camera/scripts/boot/wait-for-unix-socket.sh new file mode 100644 index 0000000..2d4d411 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/boot/wait-for-unix-socket.sh @@ -0,0 +1,49 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="ros-receiver" +SOCKET_PATH="" +TIMEOUT_SEC="" + +while [[ $# -gt 0 ]]; do + case "$1" in + --path) + SOCKET_PATH="$2" + shift 2 + ;; + --timeout) + TIMEOUT_SEC="$2" + shift 2 + ;; + --step) + STEP="$2" + shift 2 + ;; + *) + blitz_log "${STEP}" "wait-socket-arg" "failure" "unknown argument: $1" 2 + exit 2 + ;; + esac +done + +blitz_load_boot_env + +SOCKET_PATH="${SOCKET_PATH:-${ROBOT_RECEIVER_LOCAL_SOCKET_PATH}}" +TIMEOUT_SEC="${TIMEOUT_SEC:-${BLITZ_ROS_SOCKET_WAIT_SEC}}" + +blitz_log "${STEP}" "wait-socket" "start" "path=${SOCKET_PATH} timeout_sec=${TIMEOUT_SEC}" 0 + +for (( waited=0; waited< TIMEOUT_SEC; waited++ )); do + if [[ -S "${SOCKET_PATH}" ]]; then + blitz_log "${STEP}" "wait-socket" "success" "path=${SOCKET_PATH} waited_sec=${waited}" 0 + exit 0 + fi + sleep 1 +done + +blitz_log "${STEP}" "wait-socket" "failure" "path=${SOCKET_PATH} timeout_sec=${TIMEOUT_SEC}" 1 +exit 1 diff --git a/host/OmniSocketGo_add_camera/scripts/dev/README.md b/host/OmniSocketGo_add_camera/scripts/dev/README.md new file mode 100644 index 0000000..b635db4 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/dev/README.md @@ -0,0 +1,190 @@ +# 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`. +`start-backend.sh` and `start-frontend.sh` need that repo; `start-ros-receiver.sh` and `start-b-side-omnid.sh` do not. + +## 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 +- `prepare-camera-device.sh`: reports V4L2 owners and optionally stops one explicitly configured camera service +- `resolve-camera-device.sh`: resolves an RGB capture node by USB serial plus MJPEG resolution support +- `apply-camera-controls.sh`: applies the camera preset before `b_side_omnid` starts +- `start-backend.sh`: starts Django ASGI with `uvicorn` +- `log-network-summary.py`: polls the backend `network/latest` API and appends compact JSONL snapshots +- `start-frontend.sh`: starts the Vite dev server +- `start-ros-receiver.sh`: starts the ROS2 `udp_teleop_bridge` receiver +- `start-b-side-omnid.sh`: applies camera controls, then 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/setup-control-side.sh # first run on the control computer +bash scripts/dev/start-local-hub.sh # direct-LAN local KCP hub +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 +``` + +When you launch via `start-*.sh`, you do not need to manually `export` the variables from +`robot-remote.env` or `robot-remote.env.local`. `load-env.sh` loads those files with `set -a`, +so the variables are exported automatically for the child process. Manual `export` is only needed +if you bypass these scripts and start binaries directly from a clean shell. + +## 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_CAMERA_AUTO_DISCOVER=1` resolves both camera nodes on every start instead of trusting unstable `/dev/video*` numbers +- `OMNI_CAMERA_HEAD_SERIAL` and `OMNI_CAMERA_WAIST_SERIAL` permanently map the physical cameras to the head/waist roles +- `OMNI_CAMERA_HEAD_DEVICE` and `OMNI_CAMERA_WAIST_DEVICE` are fallback nodes when automatic discovery is disabled +- `OMNI_CAMERA_DISCOVERY_WIDTH`, `OMNI_CAMERA_DISCOVERY_HEIGHT`, and `OMNI_CAMERA_DISCOVERY_TIMEOUT_SEC` tune capability matching and retry time +- `OMNI_CAMERA_ACTIVE=head|waist` selects the camera sent at startup + +`b_side_omnid` keeps both configured cameras streaming and only decodes/encodes/sends the selected input. Send a text control message with body `camera:head` or `camera:waist` to `peer-b-ctrl` to switch without reopening either camera. After applying the selection, the robot replies to the sender with `{"type":"camera.selected","camera":"head|waist"}` so callers can confirm the actual state. The normal fixed-size binary robot control packets are unchanged. +- `OMNI_CAMERA_OCCUPANCY_POLICY` +- `OMNI_CAMERA_RELEASE_SERVICE` +- `OMNI_CAMERA_PROFILE` +- `OMNI_CAMERA_BRIGHTNESS` +- `OMNI_CAMERA_CUSTOM_CTRL` +- `OMNI_CAMERA_VERIFY` +- `OMNI_VIDEO_PEER_ID` +- `OMNI_CONTROL_PEER_ID` +- `OMNI_VIDEO_SOFT_BACKPRESSURE_SEGMENTS` +- `OMNI_VIDEO_HARD_BACKPRESSURE_SEGMENTS` +- `OMNI_VIDEO_HARD_BACKPRESSURE_HOLD_MS` +- `OMNI_CONTROL_SERVER_IDLE_RECONNECT_MS` +- `OMNI_VIDEO_MAX_FRAME_AGE_MS` +- `OMNISOCKET_TELEMETRY_PEER_ID` +- `OMNISOCKET_TELEMETRY_INTERVAL_MS` +- `OMNISOCKET_TELEMETRY_STALE_AFTER_MS` +- `OMNI_NETWORK_SUMMARY_LOG_ENABLED` +- `OMNI_NETWORK_SUMMARY_LOG_PATH` +- `OMNI_NETWORK_SUMMARY_LOG_INTERVAL_MS` + +Camera discovery uses `udevadm` and `v4l2-ctl` on the robot side. A candidate must have the configured serial number and advertise MJPEG at the configured capture resolution; depth, IR, Bayer, and metadata nodes are rejected. With `OMNI_CAMERA_OCCUPANCY_POLICY=release-known`, the start script stops the configured head/waist Orbbec services before discovery so libusb-owned interfaces can reattach to `uvcvideo`. + +Role mapping: + +- `start-backend.sh` uses the `CONTROL_SIDE_*` address pair +- `start-b-side-omnid.sh` uses the `ROBOT_SIDE_*` address pair +- `start-b-side-omnid.sh` also applies the `OMNI_CAMERA_*` preset before the daemon opens the camera +- `start-b-side-omnid.sh` runs the camera occupancy preflight before applying camera controls +- `start-ros-receiver.sh` defaults to the robot-side address pair, but with `transport=unix_dgram` it usually does not need the server address + +New repair knobs: + +- `OMNI_VIDEO_SOFT_BACKPRESSURE_SEGMENTS`, `OMNI_VIDEO_HARD_BACKPRESSURE_SEGMENTS`, and `OMNI_VIDEO_HARD_BACKPRESSURE_HOLD_MS` are used by `b_side_omnid` +- `OMNI_CONTROL_SERVER_IDLE_RECONNECT_MS` is used by `b_side_omnid` +- `OMNI_VIDEO_MAX_FRAME_AGE_MS` is used by `start-backend.sh` on the A-side backend, not by `b_side_omnid` +- `OMNISOCKET_TELEMETRY_INTERVAL_MS` and `OMNISOCKET_TELEMETRY_STALE_AFTER_MS` tune the backend's D-side telemetry freshness window +- `OMNI_NETWORK_SUMMARY_LOG_*` controls the A-side JSONL summary logger that polls `GET /api/network/latest/` + +Default long-run network logging: + +- A-side starts a compact JSONL logger by default at `${OMNISOCKETGO_ROOT}/logs/a-network-summary.jsonl` +- The default A-side polling interval is `2000 ms` +- For D-side long runs, prefer: + +```bash +./bin/kcpserver -listen 0.0.0.0:10909 \ + -telemetry-peer peer-a-telemetry \ + -telemetry-interval 1000ms \ + -kcp-session-stats-log logs/d-kcp-stats.jsonl \ + -kcp-session-stats-interval 1000ms +``` + +- Keep `-latency-log` and `-kcp-ts-debug-log` off by default for multi-hour runs +- Do not continuously redirect relay `C` stderr to a file unless you are reproducing a short issue window + +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" +OMNI_NETWORK_SUMMARY_LOG_INTERVAL_MS="5000" +``` + +Camera occupancy handling is deliberately narrow. `check` only reports owners and fails if the +device is busy. `release-known` stops exactly `OMNI_CAMERA_RELEASE_SERVICE`, then checks the device +again. It never kills an arbitrary PID and refuses to stop `proc_manager.service` automatically: + +```bash +OMNI_CAMERA_DEVICE="/dev/video18" +OMNI_CAMERA_OCCUPANCY_POLICY="release-known" +OMNI_CAMERA_RELEASE_SERVICE="orbbec_waist.service" +``` + +Run the preflight without starting the daemon: + +```bash +bash scripts/dev/prepare-camera-device.sh +``` + +If a remaining owner belongs to `proc_manager.service`, inspect it with `ros2 component list` and +unload only the camera component. Stopping the complete process manager can interrupt unrelated +robot functions. + +Default camera behavior is the `night` preset: + +```bash +OMNI_CAMERA_PROFILE="night" +# Optional per-machine tweak: +OMNI_CAMERA_BRIGHTNESS="8" +``` + +To switch to a daytime preset with brightness only: + +```bash +OMNI_CAMERA_PROFILE="day" +OMNI_CAMERA_BRIGHTNESS="8" +``` + +To send the raw `v4l2-ctl --set-ctrl=...` payload yourself: + +```bash +OMNI_CAMERA_PROFILE="custom" +OMNI_CAMERA_CUSTOM_CTRL="brightness=8,auto_exposure=1,exposure_time_absolute=800,gain=64" +OMNI_CAMERA_VERIFY="1" +``` diff --git a/host/OmniSocketGo_add_camera/scripts/dev/aggregate-latency-estimates.py b/host/OmniSocketGo_add_camera/scripts/dev/aggregate-latency-estimates.py new file mode 100644 index 0000000..e490d3c --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/dev/aggregate-latency-estimates.py @@ -0,0 +1,292 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import argparse +from datetime import datetime, timezone +import html +import json +from pathlib import Path +from typing import Any + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="Aggregate run logs into control/video latency estimate outputs.") + parser.add_argument("--run-dir", required=True, help="Run directory containing JSONL logs.") + parser.add_argument("--output-dir", help="Output directory. Defaults to --run-dir.") + return parser.parse_args() + + +def iter_jsonl(path: Path) -> list[dict[str, Any]]: + records: list[dict[str, Any]] = [] + if not path.exists(): + return records + with path.open("r", encoding="utf-8") as handle: + for raw_line in handle: + line = raw_line.strip() + if not line: + continue + try: + payload = json.loads(line) + except json.JSONDecodeError: + continue + if isinstance(payload, dict): + records.append(payload) + return records + + +def load_glob_jsonl(run_dir: Path, pattern: str) -> list[dict[str, Any]]: + records: list[dict[str, Any]] = [] + for path in sorted(run_dir.glob(pattern)): + records.extend(iter_jsonl(path)) + return records + + +def write_jsonl(path: Path, records: list[dict[str, Any]]) -> None: + with path.open("w", encoding="utf-8") as handle: + for record in records: + handle.write(json.dumps(record, ensure_ascii=False, separators=(",", ":"))) + handle.write("\n") + + +def parse_unix_ms(value: Any) -> int | None: + if value is None: + return None + if isinstance(value, (int, float)): + return int(value) + text = str(value).strip() + if not text: + return None + if text.endswith("Z"): + text = f"{text[:-1]}+00:00" + try: + return int(datetime.fromisoformat(text).astimezone(timezone.utc).timestamp() * 1000) + except ValueError: + return None + + +def flatten_net_epoch(samples: list[dict[str, Any]]) -> list[dict[str, Any]]: + flattened: list[dict[str, Any]] = [] + for sample in samples: + links = sample.get("links") or {} + a_to_d = (links.get("a_to_d") or {}).get("sessions") or {} + d_to_b = (links.get("d_to_b") or {}).get("sessions") or {} + a_control = (a_to_d.get("control") or {}).get("kcp") or {} + d_control = (d_to_b.get("control") or {}).get("kcp") or {} + a_video = (a_to_d.get("video") or {}).get("kcp") or {} + d_video = (d_to_b.get("video") or {}).get("kcp") or {} + flattened.append( + { + "updated_at": sample.get("updated_at"), + "a_to_d_control_srtt_ms": a_control.get("srtt_ms"), + "a_to_d_control_min_srtt_ms": a_control.get("min_srtt_ms"), + "d_to_b_control_srtt_ms": d_control.get("srtt_ms"), + "d_to_b_control_min_srtt_ms": d_control.get("min_srtt_ms"), + "a_to_d_video_srtt_ms": a_video.get("srtt_ms"), + "a_to_d_video_min_srtt_ms": a_video.get("min_srtt_ms"), + "d_to_b_video_srtt_ms": d_video.get("srtt_ms"), + "d_to_b_video_min_srtt_ms": d_video.get("min_srtt_ms"), + "a_to_d_control_feedback_age_ms": a_control.get("last_feedback_age_ms"), + "d_to_b_control_feedback_age_ms": d_control.get("last_feedback_age_ms"), + "a_to_d_video_feedback_age_ms": a_video.get("last_feedback_age_ms"), + "d_to_b_video_feedback_age_ms": d_video.get("last_feedback_age_ms"), + "a_to_d_control_retrans_delta": ((a_to_d.get("control") or {}).get("trend") or {}).get("retrans_delta"), + "d_to_b_control_retrans_delta": ((d_to_b.get("control") or {}).get("trend") or {}).get("retrans_delta"), + "a_to_d_video_retrans_delta": ((a_to_d.get("video") or {}).get("trend") or {}).get("retrans_delta"), + "d_to_b_video_retrans_delta": ((d_to_b.get("video") or {}).get("trend") or {}).get("retrans_delta"), + "a_to_d_video_window_pressure_pct": a_video.get("window_pressure_pct"), + "d_to_b_video_window_pressure_pct": d_video.get("window_pressure_pct"), + "robot_health": sample.get("robot_health"), + } + ) + return flattened + + +def aggregate_control_estimates( + network_samples: list[dict[str, Any]], + control_events: list[dict[str, Any]], + control_acks: list[dict[str, Any]], +) -> list[dict[str, Any]]: + if control_acks: + return control_acks + + fallback: list[dict[str, Any]] = [] + for sample in network_samples: + estimate = sample.get("latency_estimate") or {} + fallback.append( + { + "updated_at": sample.get("updated_at"), + "estimate_method": "srtt_fallback", + "control_loop_rtt_ms": estimate.get("control_loop_rtt_ms"), + "control_to_persist_est_ms": estimate.get("control_to_persist_est_ms"), + "control_oneway_srtt_est_ms": estimate.get("control_oneway_srtt_est_ms"), + "control_oneway_bestcase_est_ms": estimate.get("control_oneway_bestcase_est_ms"), + "source_event_count": len(control_events), + } + ) + return fallback + + +def aggregate_video_estimates( + network_samples: list[dict[str, Any]], + frame_recv_records: list[dict[str, Any]], + display_probe_records: list[dict[str, Any]], +) -> list[dict[str, Any]]: + network_timeline = sorted( + ( + (updated_at_ms, sample.get("latency_estimate") or {}) + for sample in network_samples + for updated_at_ms in [parse_unix_ms(sample.get("updated_at"))] + if updated_at_ms is not None + ), + key=lambda item: item[0], + ) + probes_by_seq = { + int(record["frame_seq"]): record + for record in display_probe_records + if record.get("frame_seq") is not None + } + estimates: list[dict[str, Any]] = [] + timeline_index = 0 + + for record in frame_recv_records: + frame_seq = record.get("frame_seq") + if frame_seq is None: + continue + probe = probes_by_seq.get(int(frame_seq)) + backend_received_unix_ns = record.get("backend_received_unix_ns") + backend_received_unix_ms = None + try: + if backend_received_unix_ns is not None: + backend_received_unix_ms = int(int(backend_received_unix_ns) / 1_000_000) + except (TypeError, ValueError): + backend_received_unix_ms = None + + latency_estimate: dict[str, Any] = {} + if backend_received_unix_ms is not None and network_timeline: + while timeline_index + 1 < len(network_timeline) and network_timeline[timeline_index + 1][0] <= backend_received_unix_ms: + timeline_index += 1 + if network_timeline[timeline_index][0] <= backend_received_unix_ms: + latency_estimate = network_timeline[timeline_index][1] + + network_oneway = latency_estimate.get("video_network_oneway_est_ms") + capture_to_send = record.get("b_side_capture_to_send_ms") + partial_est = None + if capture_to_send is not None or network_oneway is not None: + partial_est = round(float(capture_to_send or 0.0) + float(network_oneway or 0.0), 3) + request_to_paint_ms = None + if probe is not None and probe.get("request_to_paint_ms") is not None: + request_to_paint_ms = round(float(probe["request_to_paint_ms"]), 3) + elif probe is not None and probe.get("request_started_unix_ms") is not None and probe.get("paint_unix_ms") is not None: + request_to_paint_ms = round(float(probe["paint_unix_ms"]) - float(probe["request_started_unix_ms"]), 3) + video_e2e_est_ms = round(partial_est + request_to_paint_ms, 3) if partial_est is not None and request_to_paint_ms is not None else None + estimates.append( + { + "frame_seq": frame_seq, + "backend_received_unix_ns": record.get("backend_received_unix_ns"), + "frame_hash": record.get("frame_hash"), + "estimate_method": "capture_to_send+srtt/2+request_to_paint" if video_e2e_est_ms is not None else "capture_to_send+srtt/2", + "video_network_oneway_est_ms": network_oneway, + "b_side_capture_to_send_ms": capture_to_send, + "request_to_paint_ms": request_to_paint_ms, + "response_to_paint_ms": probe.get("response_to_paint_ms") if probe is not None else None, + "backend_to_request_ms": probe.get("backend_to_request_ms") if probe is not None else None, + "backend_to_request_ms_raw": probe.get("backend_to_request_ms_raw") if probe is not None else None, + "backend_to_paint_ms": probe.get("backend_to_paint_ms") if probe is not None else None, + "backend_to_paint_ms_raw": probe.get("backend_to_paint_ms_raw") if probe is not None else None, + "browser_backend_clock_offset_ms": probe.get("browser_backend_clock_offset_ms") if probe is not None else None, + "browser_backend_clock_rtt_ms": probe.get("browser_backend_clock_rtt_ms") if probe is not None else None, + "video_partial_est_ms": partial_est, + "video_e2e_est_ms": video_e2e_est_ms, + "sequence_gap": record.get("sequence_gap"), + "repeat_flag": record.get("repeat_flag"), + "sender_clock_delta_ms_raw": record.get("sender_clock_delta_ms_raw"), + } + ) + return estimates + + +def write_html_summary( + path: Path, + *, + net_epochs: list[dict[str, Any]], + control_estimates: list[dict[str, Any]], + video_estimates: list[dict[str, Any]], +) -> None: + latest_control = control_estimates[-1] if control_estimates else {} + latest_video = video_estimates[-1] if video_estimates else {} + latest_net = net_epochs[-1] if net_epochs else {} + html_text = f""" + + + + Latency Estimates + + + +

Latency Estimates

+
+
+

Control

+

loop RTT: {html.escape(str(latest_control.get("control_loop_rtt_ms")))}

+

to persist: {html.escape(str(latest_control.get("control_to_persist_est_ms")))}

+

method: {html.escape(str(latest_control.get("estimate_method")))}

+

samples: {len(control_estimates)}

+
+
+

Video

+

network one-way: {html.escape(str(latest_video.get("video_network_oneway_est_ms")))}

+

partial: {html.escape(str(latest_video.get("video_partial_est_ms")))}

+

end-to-end: {html.escape(str(latest_video.get("video_e2e_est_ms")))}

+

samples: {len(video_estimates)}

+
+
+

Net Epoch

+

a→d control srtt: {html.escape(str(latest_net.get("a_to_d_control_srtt_ms")))}

+

d→b control srtt: {html.escape(str(latest_net.get("d_to_b_control_srtt_ms")))}

+

a→d video srtt: {html.escape(str(latest_net.get("a_to_d_video_srtt_ms")))}

+

d→b video srtt: {html.escape(str(latest_net.get("d_to_b_video_srtt_ms")))}

+
+
+ + +""" + path.write_text(html_text, encoding="utf-8") + + +def main() -> int: + args = parse_args() + run_dir = Path(args.run_dir).resolve() + output_dir = Path(args.output_dir).resolve() if args.output_dir else run_dir + output_dir.mkdir(parents=True, exist_ok=True) + + network_samples = load_glob_jsonl(run_dir, "a-network-summary.*.jsonl") + control_events = load_glob_jsonl(run_dir, "a-control-events.*.jsonl") + control_acks = load_glob_jsonl(run_dir, "a-control-acks.*.jsonl") + frame_recv_records = load_glob_jsonl(run_dir, "a-video-frame-recv.*.jsonl") + display_probe_records = load_glob_jsonl(run_dir, "a-video-display-probe.*.jsonl") + + net_epochs = flatten_net_epoch(network_samples) + control_estimates = aggregate_control_estimates(network_samples, control_events, control_acks) + video_estimates = aggregate_video_estimates(network_samples, frame_recv_records, display_probe_records) + + write_jsonl(output_dir / "net-epoch-summary.jsonl", net_epochs) + write_jsonl(output_dir / "control-latency-estimates.jsonl", control_estimates) + write_jsonl(output_dir / "video-latency-estimates.jsonl", video_estimates) + write_html_summary( + output_dir / "latency-estimates.html", + net_epochs=net_epochs, + control_estimates=control_estimates, + video_estimates=video_estimates, + ) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/host/OmniSocketGo_add_camera/scripts/dev/apply-camera-controls.sh b/host/OmniSocketGo_add_camera/scripts/dev/apply-camera-controls.sh new file mode 100644 index 0000000..6b32c81 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/dev/apply-camera-controls.sh @@ -0,0 +1,111 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" + +camera_device="${OMNI_CAMERA_DEVICE}" +camera_profile="${OMNI_CAMERA_PROFILE}" +camera_brightness="${OMNI_CAMERA_BRIGHTNESS}" +camera_custom_ctrl="${OMNI_CAMERA_CUSTOM_CTRL}" +camera_verify="${OMNI_CAMERA_VERIFY}" + +is_truthy() { + case "${1:-0}" in + 1|true|TRUE|yes|YES|on|ON) + return 0 + ;; + *) + return 1 + ;; + esac +} + +require_v4l2_ctl() { + if command -v v4l2-ctl >/dev/null 2>&1; then + return 0 + fi + + echo "Missing required command: v4l2-ctl. Install v4l-utils on the robot side before starting b_side_omnid." >&2 + exit 1 +} + +run_v4l2_ctl() { + v4l2-ctl -d "${camera_device}" "$@" +} + +set_ctrl() { + local ctrl="$1" + + echo "[camera-controls] set ${camera_device} ${ctrl}" + run_v4l2_ctl "--set-ctrl=${ctrl}" +} + +verify_ctrl() { + local ctrl="$1" + + echo "[camera-controls] verify ${camera_device} ${ctrl}" + run_v4l2_ctl "--get-ctrl=${ctrl}" +} + +needs_v4l2_ctl=0 + +case "${camera_profile}" in + night) + needs_v4l2_ctl=1 + ;; + day) + if [[ -n "${camera_brightness}" ]]; then + needs_v4l2_ctl=1 + fi + ;; + custom) + if [[ -z "${camera_custom_ctrl}" ]]; then + echo "OMNI_CAMERA_CUSTOM_CTRL must be non-empty when OMNI_CAMERA_PROFILE=custom." >&2 + exit 1 + fi + needs_v4l2_ctl=1 + ;; + *) + echo "Unsupported OMNI_CAMERA_PROFILE: ${camera_profile}. Expected one of: night, day, custom." >&2 + exit 1 + ;; +esac + +if is_truthy "${camera_verify}"; then + needs_v4l2_ctl=1 +fi + +if [[ "${needs_v4l2_ctl}" == "0" ]]; then + echo "[camera-controls] profile=${camera_profile}; no camera controls requested" + exit 0 +fi + +require_v4l2_ctl + +case "${camera_profile}" in + night) + set_ctrl "auto_exposure=1" + set_ctrl "exposure_time_absolute=800" + set_ctrl "gain=64" + if [[ -n "${camera_brightness}" ]]; then + set_ctrl "brightness=${camera_brightness}" + fi + ;; + day) + if [[ -n "${camera_brightness}" ]]; then + set_ctrl "brightness=${camera_brightness}" + fi + ;; + custom) + set_ctrl "${camera_custom_ctrl}" + ;; +esac + +if is_truthy "${camera_verify}"; then + verify_ctrl "auto_exposure" + verify_ctrl "exposure_time_absolute" + verify_ctrl "gain" + verify_ctrl "brightness" +fi diff --git a/host/OmniSocketGo_add_camera/scripts/dev/control-side-requirements.txt b/host/OmniSocketGo_add_camera/scripts/dev/control-side-requirements.txt new file mode 100644 index 0000000..d6cd7bc --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/dev/control-side-requirements.txt @@ -0,0 +1,7 @@ +Django>=5.2,<6.0 +djangorestframework>=3.17,<4 +django-cors-headers>=4,<5 +channels>=4,<5 +uvicorn>=0.52,<1 +PyYAML>=6,<7 +setuptools>=80 diff --git a/host/OmniSocketGo_add_camera/scripts/dev/load-env.sh b/host/OmniSocketGo_add_camera/scripts/dev/load-env.sh new file mode 100644 index 0000000..42b44c0 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/dev/load-env.sh @@ -0,0 +1,335 @@ +#!/usr/bin/env bash +set -euo pipefail + +LOAD_ENV_SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +DEFAULT_OMNISOCKETGO_ROOT="$(cd "${LOAD_ENV_SCRIPT_DIR}/../.." && pwd)" + +die() { + echo "$*" >&2 + return 1 2>/dev/null || exit 1 +} + +normalize_loaded_env_vars() { + local var_name + local value + + for var_name in $(compgen -A variable); do + case "${var_name}" in + BACKEND_*|BLITZ_*|B_SIDE_*|CONTROL_*|FRONTEND_*|OMNI_*|PYTHON3_BIN|PYTHON_VENV_PATH|ROBOT_*|ROS_DISTRO|VITE_*) + value="${!var_name}" + if [[ "${value}" == *$'\r' ]]; then + printf -v "${var_name}" '%s' "${value%$'\r'}" + export "${var_name}" + fi + ;; + esac + done +} + +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" ]] +} + +require_robot_command_center_root() { + 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 ${LOAD_ENV_SCRIPT_DIR}/robot-remote.env.local if needed." + fi +} + +export OMNISOCKETGO_ROOT="${OMNISOCKETGO_ROOT:-${DEFAULT_OMNISOCKETGO_ROOT}}" + +omni_camera_device_was_set=0 +omni_camera_profile_was_set=0 +omni_camera_brightness_was_set=0 +omni_camera_custom_ctrl_was_set=0 +omni_camera_verify_was_set=0 + +if [[ "${OMNI_CAMERA_DEVICE+x}" == "x" ]]; then + omni_camera_device_was_set=1 + preserved_omni_camera_device="${OMNI_CAMERA_DEVICE}" +fi +if [[ "${OMNI_CAMERA_PROFILE+x}" == "x" ]]; then + omni_camera_profile_was_set=1 + preserved_omni_camera_profile="${OMNI_CAMERA_PROFILE}" +fi +if [[ "${OMNI_CAMERA_BRIGHTNESS+x}" == "x" ]]; then + omni_camera_brightness_was_set=1 + preserved_omni_camera_brightness="${OMNI_CAMERA_BRIGHTNESS}" +fi +if [[ "${OMNI_CAMERA_CUSTOM_CTRL+x}" == "x" ]]; then + omni_camera_custom_ctrl_was_set=1 + preserved_omni_camera_custom_ctrl="${OMNI_CAMERA_CUSTOM_CTRL}" +fi +if [[ "${OMNI_CAMERA_VERIFY+x}" == "x" ]]; then + omni_camera_verify_was_set=1 + preserved_omni_camera_verify="${OMNI_CAMERA_VERIFY}" +fi + +ENV_FILES=( + "${LOAD_ENV_SCRIPT_DIR}/robot-remote.env" + "${LOAD_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 + +normalize_loaded_env_vars + +if [[ "${omni_camera_device_was_set}" == "1" ]]; then + export OMNI_CAMERA_DEVICE="${preserved_omni_camera_device}" +fi +if [[ "${omni_camera_profile_was_set}" == "1" ]]; then + export OMNI_CAMERA_PROFILE="${preserved_omni_camera_profile}" +fi +if [[ "${omni_camera_brightness_was_set}" == "1" ]]; then + export OMNI_CAMERA_BRIGHTNESS="${preserved_omni_camera_brightness}" +fi +if [[ "${omni_camera_custom_ctrl_was_set}" == "1" ]]; then + export OMNI_CAMERA_CUSTOM_CTRL="${preserved_omni_camera_custom_ctrl}" +fi +if [[ "${omni_camera_verify_was_set}" == "1" ]]; then + export OMNI_CAMERA_VERIFY="${preserved_omni_camera_verify}" +fi + +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 + +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 OMNISOCKET_TELEMETRY_PEER_ID="${OMNISOCKET_TELEMETRY_PEER_ID:-peer-a-telemetry}" +export OMNISOCKET_TELEMETRY_INTERVAL_MS="${OMNISOCKET_TELEMETRY_INTERVAL_MS:-1000}" +export OMNISOCKET_TELEMETRY_STALE_AFTER_MS="${OMNISOCKET_TELEMETRY_STALE_AFTER_MS:-3000}" +export OMNI_NETWORK_SUMMARY_LOG_ENABLED="${OMNI_NETWORK_SUMMARY_LOG_ENABLED:-1}" +export OMNI_NETWORK_SUMMARY_LOG_PATH="${OMNI_NETWORK_SUMMARY_LOG_PATH:-${OMNISOCKETGO_ROOT}/logs/a-network-summary.jsonl}" +export OMNI_NETWORK_SUMMARY_LOG_INTERVAL_MS="${OMNI_NETWORK_SUMMARY_LOG_INTERVAL_MS:-1000}" +export OMNI_NETWORK_SUMMARY_LOG_REQUEST_TIMEOUT_SEC="${OMNI_NETWORK_SUMMARY_LOG_REQUEST_TIMEOUT_SEC:-3}" +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_CAMERA_DEVICE="${OMNI_CAMERA_DEVICE:-/dev/video0}" +export OMNI_CAMERA_HEAD_DEVICE="${OMNI_CAMERA_HEAD_DEVICE:-/dev/video26}" +export OMNI_CAMERA_WAIST_DEVICE="${OMNI_CAMERA_WAIST_DEVICE:-/dev/video18}" +export OMNI_CAMERA_AUTO_DISCOVER="${OMNI_CAMERA_AUTO_DISCOVER:-0}" +export OMNI_CAMERA_HEAD_SERIAL="${OMNI_CAMERA_HEAD_SERIAL:-}" +export OMNI_CAMERA_WAIST_SERIAL="${OMNI_CAMERA_WAIST_SERIAL:-}" +export OMNI_CAMERA_DISCOVERY_WIDTH="${OMNI_CAMERA_DISCOVERY_WIDTH:-1280}" +export OMNI_CAMERA_DISCOVERY_HEIGHT="${OMNI_CAMERA_DISCOVERY_HEIGHT:-720}" +export OMNI_CAMERA_DISCOVERY_TIMEOUT_SEC="${OMNI_CAMERA_DISCOVERY_TIMEOUT_SEC:-10}" +export OMNI_CAMERA_HEAD_RELEASE_SERVICE="${OMNI_CAMERA_HEAD_RELEASE_SERVICE:-orbbec_head.service}" +export OMNI_CAMERA_WAIST_RELEASE_SERVICE="${OMNI_CAMERA_WAIST_RELEASE_SERVICE:-orbbec_waist.service}" +export OMNI_CAMERA_ACTIVE="${OMNI_CAMERA_ACTIVE:-head}" +export OMNI_CAMERA_PROFILE="${OMNI_CAMERA_PROFILE:-night}" +export OMNI_CAMERA_BRIGHTNESS="${OMNI_CAMERA_BRIGHTNESS:-}" +export OMNI_CAMERA_CUSTOM_CTRL="${OMNI_CAMERA_CUSTOM_CTRL:-}" +export OMNI_CAMERA_VERIFY="${OMNI_CAMERA_VERIFY:-0}" +export OMNI_GPSD_HOST="${OMNI_GPSD_HOST:-127.0.0.1}" +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 OMNI_CONTROL_ACK_PEER_ID="${OMNI_CONTROL_ACK_PEER_ID:-peer-b-ctrl-ack}" +export OMNI_CONTROL_ACK_TARGET_PEER="${OMNI_CONTROL_ACK_TARGET_PEER:-peer-a-ctrl-ack}" +export B_SIDE_OMNID_USE_SUDO="${B_SIDE_OMNID_USE_SUDO:-1}" +export BLITZ_RUNTIME_DIR="${BLITZ_RUNTIME_DIR:-${OMNISOCKETGO_ROOT}/logs/runtime}" +export BLITZ_RUN_ROOT="${BLITZ_RUN_ROOT:-${OMNISOCKETGO_ROOT}/logs}" +export BLITZ_RUN_CONTEXT_FILE="${BLITZ_RUN_CONTEXT_FILE:-${BLITZ_RUNTIME_DIR}/run-context.env}" +export BLITZ_RUN_ID_FILE="${BLITZ_RUN_ID_FILE:-${BLITZ_RUNTIME_DIR}/run-id}" +export BLITZ_CURRENT_RUN_LINK="${BLITZ_CURRENT_RUN_LINK:-${BLITZ_RUN_ROOT}/current}" +export BLITZ_5G_INTERFACE="${BLITZ_5G_INTERFACE:-}" +export BLITZ_5G_MODEM_SUBNET="${BLITZ_5G_MODEM_SUBNET:-192.168.224.0/22}" +export BLITZ_5G_GATEWAY="${BLITZ_5G_GATEWAY:-192.168.225.1}" +export BLITZ_5G_ROUTE_TARGETS="${BLITZ_5G_ROUTE_TARGETS:-106.55.173.235}" +export BLITZ_5G_INFO_JSON="${BLITZ_5G_INFO_JSON:-${OMNISOCKETGO_ROOT}/scripts/boot/modem_network_info.json}" +export BLITZ_TIME_SERVER_IP="${BLITZ_TIME_SERVER_IP:-}" +export BLITZ_KCP_STATS_INTERVAL_MS="${BLITZ_KCP_STATS_INTERVAL_MS:-1000}" +export BLITZ_CONTROL_LATENCY_LOG_ENABLED="${BLITZ_CONTROL_LATENCY_LOG_ENABLED:-1}" +export BLITZ_CONTROL_LATENCY_LOG_SAMPLE_MOD="${BLITZ_CONTROL_LATENCY_LOG_SAMPLE_MOD:-100}" +export BLITZ_CONTROL_ACK_SAMPLE_MOD="${BLITZ_CONTROL_ACK_SAMPLE_MOD:-10}" +export BLITZ_VIDEO_STAGE_LOG_ENABLED="${BLITZ_VIDEO_STAGE_LOG_ENABLED:-1}" +export BLITZ_VIDEO_STAGE_LOG_SAMPLE_MOD="${BLITZ_VIDEO_STAGE_LOG_SAMPLE_MOD:-10}" +export BLITZ_5G_LINK_LOG_INTERVAL_SEC="${BLITZ_5G_LINK_LOG_INTERVAL_SEC:-5}" +export BLITZ_JSONL_FLUSH_INTERVAL_MS="${BLITZ_JSONL_FLUSH_INTERVAL_MS:-1000}" +export BLITZ_JSONL_FLUSH_BYTES="${BLITZ_JSONL_FLUSH_BYTES:-262144}" +export BLITZ_JSONL_ROTATE_BYTES="${BLITZ_JSONL_ROTATE_BYTES:-134217728}" +export BLITZ_JSONL_ROTATE_FILES="${BLITZ_JSONL_ROTATE_FILES:-8}" + +blitz_dev_utc_compact_timestamp() { + date -u '+%Y%m%dT%H%M%SZ' +} + +blitz_dev_git_commit() { + git -C "${OMNISOCKETGO_ROOT}" rev-parse HEAD 2>/dev/null || true +} + +blitz_dev_git_dirty_flag() { + if git -C "${OMNISOCKETGO_ROOT}" diff --quiet --ignore-submodules=dirty >/dev/null 2>&1; then + printf '0\n' + return 0 + fi + printf '1\n' +} + +blitz_dev_prepare_dirs() { + mkdir -p "${BLITZ_RUNTIME_DIR}" "${BLITZ_RUN_ROOT}/runs" "${BLITZ_RUN_ROOT}/incidents" +} + +blitz_dev_write_run_info() { + local run_dir="$1" + local run_id="$2" + local boot_id="$3" + local tmp_info="${run_dir}/run-info.json.tmp.$$" + local started_at + local commit_hash + local dirty_flag + + started_at="$(date -u '+%Y-%m-%dT%H:%M:%SZ')" + commit_hash="$(blitz_dev_git_commit)" + dirty_flag="$(blitz_dev_git_dirty_flag)" + + python3 - "${tmp_info}" "${run_id}" "${run_dir}" "${boot_id}" "${started_at}" "${commit_hash}" "${dirty_flag}" "${HOSTNAME:-$(hostname)}" <<'PY' +import json +import os +import sys + +path, run_id, run_dir, boot_id, started_at, commit_hash, dirty_flag, hostname = sys.argv[1:9] +payload = { + "run_id": run_id, + "run_dir": run_dir, + "boot_id": boot_id, + "started_at": started_at, + "hostname": hostname, + "git_commit": commit_hash, + "git_dirty": dirty_flag == "1", + "env": { + key: os.environ.get(key, "") + for key in sorted(os.environ) + if key.startswith(("BLITZ_", "OMNI_", "ROBOT_RECEIVER_")) + }, +} +with open(path, "w", encoding="utf-8") as handle: + json.dump(payload, handle, ensure_ascii=False, indent=2, sort_keys=True) +PY + mv -f "${tmp_info}" "${run_dir}/run-info.json" +} + +blitz_dev_init_run_context() { + local run_id="${1:-$(blitz_dev_utc_compact_timestamp)}" + local boot_id="dev-$(blitz_dev_utc_compact_timestamp)" + local run_dir="${BLITZ_RUN_ROOT}/runs/${run_id}" + local tmp_context="${BLITZ_RUN_CONTEXT_FILE}.tmp.$$" + + blitz_dev_prepare_dirs + mkdir -p "${run_dir}" + export BLITZ_RUN_ID="${run_id}" + export BLITZ_RUN_DIR="${run_dir}" + export BLITZ_BOOT_ID="${boot_id}" + printf '%s\n' "${run_id}" > "${BLITZ_RUN_ID_FILE}" + cat > "${tmp_context}" < None: + del signum, frame + global STOP_REQUESTED + STOP_REQUESTED = True + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="Poll /api/network/latest/ and append JSONL snapshots.") + parser.add_argument("--url", required=True, help="HTTP endpoint that returns the network summary JSON.") + parser.add_argument("--output", required=True, help="Output JSONL path.") + parser.add_argument( + "--interval-ms", + type=int, + default=2000, + help="Polling interval in milliseconds. Default: 2000.", + ) + parser.add_argument( + "--request-timeout-sec", + type=float, + default=3.0, + help="Single request timeout in seconds. Default: 3.0.", + ) + return parser.parse_args() + + +def sleep_with_stop(seconds: float) -> None: + deadline = time.monotonic() + max(0.0, seconds) + while not STOP_REQUESTED: + remaining = deadline - time.monotonic() + if remaining <= 0.0: + return + time.sleep(min(remaining, 0.2)) + + +def fetch_json(url: str, timeout_sec: float) -> str: + request = urllib.request.Request( + url, + headers={ + "Accept": "application/json", + "Cache-Control": "no-cache", + }, + method="GET", + ) + # This logger always polls the local backend. Ignore HTTP_PROXY/HTTPS_PROXY + # so a developer proxy cannot turn a 127.0.0.1 request into a 502. + with LOCAL_HTTP_OPENER.open(request, timeout=timeout_sec) as response: + charset = response.headers.get_content_charset("utf-8") + payload = response.read().decode(charset) + parsed = json.loads(payload) + return json.dumps(parsed, separators=(",", ":"), ensure_ascii=False) + + +def main() -> int: + args = parse_args() + interval_sec = max(args.interval_ms, 200) / 1000.0 + output_path = Path(args.output) + last_error_log_monotonic = 0.0 + + signal.signal(signal.SIGINT, handle_signal) + signal.signal(signal.SIGTERM, handle_signal) + + output_path.parent.mkdir(parents=True, exist_ok=True) + + with output_path.open("a", encoding="utf-8") as output_file: + while not STOP_REQUESTED: + started = time.monotonic() + try: + line = fetch_json(args.url, args.request_timeout_sec) + except (TimeoutError, urllib.error.URLError, urllib.error.HTTPError, json.JSONDecodeError) as error: + now = time.monotonic() + if now - last_error_log_monotonic >= 10.0: + print(f"[network-summary] poll failed: {error}", file=sys.stderr) + last_error_log_monotonic = now + else: + output_file.write(line) + output_file.write("\n") + output_file.flush() + + elapsed = time.monotonic() - started + sleep_with_stop(max(0.0, interval_sec - elapsed)) + + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/host/OmniSocketGo_add_camera/scripts/dev/prepare-camera-device.sh b/host/OmniSocketGo_add_camera/scripts/dev/prepare-camera-device.sh new file mode 100644 index 0000000..b847c6f --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/dev/prepare-camera-device.sh @@ -0,0 +1,108 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" + +camera_device="${OMNI_CAMERA_DEVICE}" +occupancy_policy="${OMNI_CAMERA_OCCUPANCY_POLICY:-check}" +release_service="${OMNI_CAMERA_RELEASE_SERVICE:-}" + +die() { + echo "[camera-preflight] $*" >&2 + exit 1 +} + +camera_pids() { + fuser "${camera_device}" 2>/dev/null \ + | tr ' ' '\n' \ + | grep -E '^[0-9]+$' \ + | sort -nu \ + || true +} + +pid_service() { + local pid="$1" + local service + + service="$(sed -nE 's#.*[/:]([^/:]+\.service)$#\1#p' "/proc/${pid}/cgroup" 2>/dev/null | head -1)" + printf '%s' "${service:-unknown}" +} + +report_owners() { + local pid + local comm + local service + local found=0 + + while read -r pid; do + [[ -n "${pid}" ]] || continue + found=1 + comm="$(cat "/proc/${pid}/comm" 2>/dev/null || printf 'unknown')" + service="$(pid_service "${pid}")" + echo "[camera-preflight] owner pid=${pid} command=${comm} service=${service}" >&2 + done < <(camera_pids) + + if [[ "${found}" == "1" ]]; then + return 0 + fi + return 1 +} + +has_proc_manager_owner() { + local pid + + while read -r pid; do + [[ -n "${pid}" ]] || continue + if [[ "$(pid_service "${pid}")" == "proc_manager.service" ]]; then + return 0 + fi + done < <(camera_pids) + + return 1 +} + +if [[ ! -e "${camera_device}" ]]; then + die "camera device does not exist: ${camera_device}" +fi + +if ! command -v fuser >/dev/null 2>&1; then + die "missing required command: fuser (install the psmisc package)" +fi + +resolved_device="$(readlink -f "${camera_device}" 2>/dev/null || printf '%s' "${camera_device}")" +echo "[camera-preflight] checking ${camera_device} (${resolved_device}) policy=${occupancy_policy}" >&2 + +if ! report_owners; then + echo "[camera-preflight] ${camera_device} is free" >&2 + exit 0 +fi + +case "${occupancy_policy}" in + check) + die "${camera_device} is busy; no process was stopped" + ;; + release-known) + if [[ -z "${release_service}" ]]; then + die "OMNI_CAMERA_RELEASE_SERVICE is required when policy=release-known" + fi + + echo "[camera-preflight] stopping known camera service ${release_service}" >&2 + systemctl stop "${release_service}" + + if ! report_owners; then + echo "[camera-preflight] ${camera_device} was released by ${release_service}" >&2 + exit 0 + fi + + if has_proc_manager_owner; then + die "${camera_device} is still owned by proc_manager.service; refusing to stop the whole process manager. Unload the owning ROS component explicitly." + fi + + die "${camera_device} remains busy after stopping ${release_service}" + ;; + *) + die "unsupported OMNI_CAMERA_OCCUPANCY_POLICY=${occupancy_policy}; expected check or release-known" + ;; +esac diff --git a/host/OmniSocketGo_add_camera/scripts/dev/reset-run-context.sh b/host/OmniSocketGo_add_camera/scripts/dev/reset-run-context.sh new file mode 100644 index 0000000..4b9233d --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/dev/reset-run-context.sh @@ -0,0 +1,10 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +export BLITZ_SKIP_DEV_RUN_CONTEXT_INIT="1" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" + +blitz_dev_reset_run_context +printf 'run_id=%s\nrun_dir=%s\n' "${BLITZ_RUN_ID}" "${BLITZ_RUN_DIR}" diff --git a/host/OmniSocketGo_add_camera/scripts/dev/resolve-camera-device.sh b/host/OmniSocketGo_add_camera/scripts/dev/resolve-camera-device.sh new file mode 100644 index 0000000..42074c1 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/dev/resolve-camera-device.sh @@ -0,0 +1,59 @@ +#!/usr/bin/env bash +set -euo pipefail + +camera_serial="${1:-}" +camera_label="${2:-camera}" +capture_width="${OMNI_CAMERA_DISCOVERY_WIDTH:-1280}" +capture_height="${OMNI_CAMERA_DISCOVERY_HEIGHT:-720}" +timeout_sec="${OMNI_CAMERA_DISCOVERY_TIMEOUT_SEC:-10}" + +die() { + echo "[camera-discovery] ${camera_label}: $*" >&2 + exit 1 +} + +[[ -n "${camera_serial}" ]] || die "camera serial is required" +[[ "${timeout_sec}" =~ ^[0-9]+$ ]] || die "OMNI_CAMERA_DISCOVERY_TIMEOUT_SEC must be a non-negative integer" + +command -v udevadm >/dev/null 2>&1 || die "missing required command: udevadm" +command -v v4l2-ctl >/dev/null 2>&1 || die "missing required command: v4l2-ctl" + +shopt -s nullglob +deadline=$((SECONDS + timeout_sec)) + +while true; do + matches=() + serial_nodes=() + + for device in /dev/video*; do + [[ -c "${device}" ]] || continue + device_serial="$( + udevadm info --query=property --name="${device}" 2>/dev/null \ + | sed -n 's/^ID_SERIAL_SHORT=//p' \ + | head -1 + )" + [[ "${device_serial}" == "${camera_serial}" ]] || continue + serial_nodes+=("${device}") + + formats="$(v4l2-ctl -d "${device}" --list-formats-ext 2>/dev/null || true)" + grep -q "'MJPG'" <<<"${formats}" || continue + grep -q "Size: Discrete ${capture_width}x${capture_height}" <<<"${formats}" || continue + matches+=("${device}") + done + + if (( ${#matches[@]} == 1 )); then + echo "[camera-discovery] ${camera_label}: serial=${camera_serial} -> ${matches[0]} (MJPG ${capture_width}x${capture_height})" >&2 + printf '%s\n' "${matches[0]}" + exit 0 + fi + if (( ${#matches[@]} > 1 )); then + die "serial=${camera_serial} matched multiple MJPG nodes: ${matches[*]}" + fi + if (( SECONDS >= deadline )); then + if (( ${#serial_nodes[@]} == 0 )); then + die "serial=${camera_serial} was not found under /dev/video*" + fi + die "serial=${camera_serial} has no MJPG ${capture_width}x${capture_height} node; serial nodes: ${serial_nodes[*]}" + fi + sleep 0.2 +done diff --git a/host/OmniSocketGo_add_camera/scripts/dev/robot-remote.env b/host/OmniSocketGo_add_camera/scripts/dev/robot-remote.env new file mode 100644 index 0000000..29524be --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/dev/robot-remote.env @@ -0,0 +1,87 @@ +# 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" # D +CONTROL_SIDE_OMNISOCKET_RELAY_VIA="106.55.173.235:10909" # C + +ROBOT_SIDE_OMNISOCKET_SERVER_ADDR="81.70.156.140:10909" # D +ROBOT_SIDE_OMNISOCKET_RELAY_VIA="81.70.156.140:10909" # 直连 D +# Log one normal relay packet out of every N packets. Drop events still log immediately. +OMNI_RELAY_PACKET_LOG_SAMPLE_EVERY="200" + +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" +OMNISOCKET_TELEMETRY_PEER_ID="peer-a-telemetry" +OMNISOCKET_TELEMETRY_INTERVAL_MS="1000" +OMNISOCKET_TELEMETRY_STALE_AFTER_MS="3000" +OMNI_NETWORK_SUMMARY_LOG_ENABLED="1" +OMNI_NETWORK_SUMMARY_LOG_PATH="${OMNISOCKETGO_ROOT}/logs/a-network-summary.jsonl" +OMNI_NETWORK_SUMMARY_LOG_INTERVAL_MS="1000" +OMNI_NETWORK_SUMMARY_LOG_REQUEST_TIMEOUT_SEC="3" + +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_GPSD_HOST="127.0.0.1" +OMNI_CAMERA_HEAD_DEVICE="/dev/video26" +OMNI_CAMERA_WAIST_DEVICE="/dev/video18" +OMNI_CAMERA_AUTO_DISCOVER="1" +OMNI_CAMERA_HEAD_SERIAL="CP9E163000H3" +OMNI_CAMERA_WAIST_SERIAL="CPCK8530005N" +OMNI_CAMERA_DISCOVERY_WIDTH="1280" +OMNI_CAMERA_DISCOVERY_HEIGHT="720" +OMNI_CAMERA_DISCOVERY_TIMEOUT_SEC="10" +OMNI_CAMERA_HEAD_RELEASE_SERVICE="orbbec_head.service" +OMNI_CAMERA_WAIST_RELEASE_SERVICE="orbbec_waist.service" +OMNI_CAMERA_ACTIVE="head" +OMNI_CAMERA_OCCUPANCY_POLICY="release-known" +OMNI_CAMERA_PROFILE="day" +OMNI_CAMERA_BRIGHTNESS="" +OMNI_CAMERA_CUSTOM_CTRL="" +OMNI_CAMERA_VERIFY="0" +OMNI_VIDEO_SERVER_ADDR="${ROBOT_SIDE_OMNISOCKET_SERVER_ADDR}" +OMNI_VIDEO_RELAY_VIA="${ROBOT_SIDE_OMNISOCKET_RELAY_VIA}" +OMNI_VIDEO_SOFT_BACKPRESSURE_SEGMENTS="256" +OMNI_VIDEO_HARD_BACKPRESSURE_SEGMENTS="1024" +OMNI_VIDEO_HARD_BACKPRESSURE_HOLD_MS="5000" +OMNI_VIDEO_FRAME_STALL_RECONNECT_MS="30000" +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}" +OMNI_CONTROL_ACK_PEER_ID="peer-b-ctrl-ack" +OMNI_CONTROL_ACK_TARGET_PEER="peer-a-ctrl-ack" +BLITZ_CONTROL_ACK_SAMPLE_MOD="10" +BLITZ_VIDEO_STAGE_LOG_ENABLED="1" +BLITZ_VIDEO_STAGE_LOG_SAMPLE_MOD="10" +OMNI_CONTROL_SERVER_IDLE_RECONNECT_MS="30000" + +# A-side backend video freshness guard. Used by scripts/dev/start-backend.sh. +OMNI_VIDEO_MAX_FRAME_AGE_MS="1000" + +B_SIDE_OMNID_USE_SUDO="1" diff --git a/host/OmniSocketGo_add_camera/scripts/dev/setup-control-side.sh b/host/OmniSocketGo_add_camera/scripts/dev/setup-control-side.sh new file mode 100644 index 0000000..3f2110d --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/dev/setup-control-side.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 [[ ! -x "${PYTHON_VENV_PATH}/bin/python" ]]; then + "${PYTHON3_BIN}" -m venv "${PYTHON_VENV_PATH}" +fi + +venv_python="${PYTHON_VENV_PATH}/bin/python" + +"${venv_python}" -m pip install -r "${SCRIPT_DIR}/control-side-requirements.txt" +make -C "${OMNISOCKETGO_ROOT}" python-ext PYTHON="${venv_python}" +"${venv_python}" -m pip install --no-build-isolation -e "${OMNISOCKETGO_ROOT}/python" + +echo "[setup-control-side] ready: ${PYTHON_VENV_PATH}" >&2 diff --git a/host/OmniSocketGo_add_camera/scripts/dev/start-5g-link-logger.sh b/host/OmniSocketGo_add_camera/scripts/dev/start-5g-link-logger.sh new file mode 100644 index 0000000..093928f --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/dev/start-5g-link-logger.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" + +blitz_dev_prepare_5g_logging_env +exec bash "${OMNISOCKETGO_ROOT}/scripts/boot/blitz-5g-link-logger.sh" diff --git a/host/OmniSocketGo_add_camera/scripts/dev/start-b-side-omnid.sh b/host/OmniSocketGo_add_camera/scripts/dev/start-b-side-omnid.sh new file mode 100644 index 0000000..cbf6eb4 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/dev/start-b-side-omnid.sh @@ -0,0 +1,100 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" +blitz_dev_prepare_bside_logging_env + +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}" + +logger_pid="" + +cleanup() { + if [[ -n "${logger_pid}" ]]; then + kill "${logger_pid}" 2>/dev/null || true + wait "${logger_pid}" 2>/dev/null || true + fi +} + +start_5g_link_logger_if_needed() { + if [[ "${OMNI_5G_LINK_LOG_ENABLED:-1}" != "1" ]]; then + echo "[start-b-side-omnid] 5G link logger disabled" >&2 + return 0 + fi + if [[ "${OMNI_BOOT_MODE:-0}" == "1" ]]; then + return 0 + fi + bash "${SCRIPT_DIR}/start-5g-link-logger.sh" & + logger_pid=$! + echo "[start-b-side-omnid] 5G link logger -> ${BLITZ_5G_LINK_LOG_PATH:-unset}" >&2 +} + +release_known_camera_service() { + local service="$1" + + if [[ "${OMNI_CAMERA_OCCUPANCY_POLICY:-check}" != "release-known" || -z "${service}" ]]; then + return 0 + fi + if systemctl is-active --quiet "${service}"; then + echo "[start-b-side-omnid] stopping known camera service ${service} before discovery" >&2 + systemctl stop "${service}" + fi +} + +resolve_camera_devices() { + if [[ "${OMNI_CAMERA_AUTO_DISCOVER:-0}" != "1" ]]; then + return 0 + fi + + OMNI_CAMERA_HEAD_DEVICE="$( + bash "${SCRIPT_DIR}/resolve-camera-device.sh" "${OMNI_CAMERA_HEAD_SERIAL}" head + )" + OMNI_CAMERA_WAIST_DEVICE="$( + bash "${SCRIPT_DIR}/resolve-camera-device.sh" "${OMNI_CAMERA_WAIST_SERIAL}" waist + )" + if [[ "${OMNI_CAMERA_HEAD_DEVICE}" == "${OMNI_CAMERA_WAIST_DEVICE}" ]]; then + echo "[start-b-side-omnid] head and waist resolved to the same device: ${OMNI_CAMERA_HEAD_DEVICE}" >&2 + return 1 + fi + export OMNI_CAMERA_HEAD_DEVICE OMNI_CAMERA_WAIST_DEVICE + echo "[start-b-side-omnid] resolved head=${OMNI_CAMERA_HEAD_DEVICE} waist=${OMNI_CAMERA_WAIST_DEVICE}" >&2 +} + +if [[ ! -x "./bin/b_side_omnid" ]]; then + if [[ "${OMNI_BOOT_MODE:-0}" == "1" ]]; then + echo "Missing ./bin/b_side_omnid in boot mode; build it before enabling the autostart service." >&2 + exit 1 + fi + make b_side_omnid +fi + +launch_b_side_omnid() { + trap cleanup EXIT INT TERM + start_5g_link_logger_if_needed + release_known_camera_service "${OMNI_CAMERA_HEAD_RELEASE_SERVICE:-orbbec_head.service}" + release_known_camera_service "${OMNI_CAMERA_WAIST_RELEASE_SERVICE:-orbbec_waist.service}" + resolve_camera_devices + OMNI_CAMERA_DEVICE="${OMNI_CAMERA_HEAD_DEVICE}" \ + OMNI_CAMERA_RELEASE_SERVICE="${OMNI_CAMERA_HEAD_RELEASE_SERVICE:-orbbec_head.service}" \ + bash "${SCRIPT_DIR}/prepare-camera-device.sh" + OMNI_CAMERA_DEVICE="${OMNI_CAMERA_WAIST_DEVICE}" \ + OMNI_CAMERA_RELEASE_SERVICE="${OMNI_CAMERA_WAIST_RELEASE_SERVICE:-orbbec_waist.service}" \ + bash "${SCRIPT_DIR}/prepare-camera-device.sh" + OMNI_CAMERA_DEVICE="${OMNI_CAMERA_HEAD_DEVICE}" bash "${SCRIPT_DIR}/apply-camera-controls.sh" + OMNI_CAMERA_DEVICE="${OMNI_CAMERA_WAIST_DEVICE}" bash "${SCRIPT_DIR}/apply-camera-controls.sh" + ./bin/b_side_omnid +} + +if [[ "${B_SIDE_OMNID_USE_SUDO}" == "1" && "${EUID}" -ne 0 ]]; then + exec sudo -E bash -lc 'cd "$1" && export B_SIDE_OMNID_USE_SUDO=0 && exec bash "$2"' _ "${OMNISOCKETGO_ROOT}" "${SCRIPT_DIR}/start-b-side-omnid.sh" +fi + +launch_b_side_omnid diff --git a/host/OmniSocketGo_add_camera/scripts/dev/start-backend.sh b/host/OmniSocketGo_add_camera/scripts/dev/start-backend.sh new file mode 100644 index 0000000..bc8e4d4 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/dev/start-backend.sh @@ -0,0 +1,60 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" +require_robot_command_center_root +blitz_dev_prepare_backend_logging_env + +if [[ ! -x "${PYTHON_VENV_PATH}/bin/python" ]]; then + echo "[start-backend] creating or repairing virtualenv at ${PYTHON_VENV_PATH}" >&2 + "${PYTHON3_BIN}" -m venv "${PYTHON_VENV_PATH}" +fi + +if [[ ! -x "${PYTHON_VENV_PATH}/bin/python" ]]; then + echo "[start-backend] virtualenv is incomplete: missing ${PYTHON_VENV_PATH}/bin/python" >&2 + exit 1 +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}" + +logger_pid="" + +cleanup() { + if [[ -n "${logger_pid}" ]]; then + kill "${logger_pid}" 2>/dev/null || true + wait "${logger_pid}" 2>/dev/null || true + fi +} + +start_network_summary_logger() { + local logger_url + local logger_dir + + if [[ "${OMNI_NETWORK_SUMMARY_LOG_ENABLED}" != "1" ]]; then + return + fi + + logger_url="http://127.0.0.1:${BACKEND_PORT}/api/network/latest/" + logger_dir="$(dirname "${OMNI_NETWORK_SUMMARY_LOG_PATH}")" + mkdir -p "${logger_dir}" + + python "${SCRIPT_DIR}/log-network-summary.py" \ + --url "${logger_url}" \ + --output "${OMNI_NETWORK_SUMMARY_LOG_PATH}" \ + --interval-ms "${OMNI_NETWORK_SUMMARY_LOG_INTERVAL_MS}" \ + --request-timeout-sec "${OMNI_NETWORK_SUMMARY_LOG_REQUEST_TIMEOUT_SEC}" & + logger_pid=$! + echo "[start-backend] network summary logger -> ${OMNI_NETWORK_SUMMARY_LOG_PATH} (${OMNI_NETWORK_SUMMARY_LOG_INTERVAL_MS} ms)" >&2 +} + +trap cleanup EXIT INT TERM + +start_network_summary_logger +python -m uvicorn config.asgi:application --host "${BACKEND_HOST}" --port "${BACKEND_PORT}" diff --git a/host/OmniSocketGo_add_camera/scripts/dev/start-dev-tmux.sh b/host/OmniSocketGo_add_camera/scripts/dev/start-dev-tmux.sh new file mode 100644 index 0000000..e9c2fe2 --- /dev/null +++ b/host/OmniSocketGo_add_camera/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}" diff --git a/host/OmniSocketGo_add_camera/scripts/dev/start-frontend.sh b/host/OmniSocketGo_add_camera/scripts/dev/start-frontend.sh new file mode 100644 index 0000000..b33a87a --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/dev/start-frontend.sh @@ -0,0 +1,10 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" +require_robot_command_center_root + +cd "${FRONTEND_DIR}" +exec npm run dev -- --host "${FRONTEND_HOST}" --port "${FRONTEND_PORT}" diff --git a/host/OmniSocketGo_add_camera/scripts/dev/start-local-hub.sh b/host/OmniSocketGo_add_camera/scripts/dev/start-local-hub.sh new file mode 100644 index 0000000..3ca0c0f --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/dev/start-local-hub.sh @@ -0,0 +1,22 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" + +hub_binary="${OMNISOCKETGO_ROOT}/bin/kcpserver" +hub_listen_addr="${LOCAL_HUB_LISTEN_ADDR:-0.0.0.0:10909}" +telemetry_peer_id="${LOCAL_HUB_TELEMETRY_PEER_ID:-peer-a-telemetry}" +telemetry_interval="${LOCAL_HUB_TELEMETRY_INTERVAL:-1000ms}" + +if [[ ! -x "${hub_binary}" ]]; then + echo "[start-local-hub] missing executable ${hub_binary}; run: make bin/kcpserver" >&2 + exit 1 +fi + +echo "[start-local-hub] listen=${hub_listen_addr} relay=disabled telemetry_peer=${telemetry_peer_id}" >&2 +exec "${hub_binary}" \ + -listen "${hub_listen_addr}" \ + -telemetry-peer "${telemetry_peer_id}" \ + -telemetry-interval "${telemetry_interval}" diff --git a/host/OmniSocketGo_add_camera/scripts/dev/start-ros-receiver.sh b/host/OmniSocketGo_add_camera/scripts/dev/start-ros-receiver.sh new file mode 100644 index 0000000..6c90630 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/dev/start-ros-receiver.sh @@ -0,0 +1,50 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" + +source_with_nounset_off() { + set +u + # shellcheck disable=SC1090 + source "$1" + set -u +} + +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" +if [[ ! -f "/opt/ros/${ROS_DISTRO}/setup.bash" ]]; then + echo "Missing ROS distro setup: /opt/ros/${ROS_DISTRO}/setup.bash" >&2 + exit 1 +fi +source_with_nounset_off "/opt/ros/${ROS_DISTRO}/setup.bash" + +cd "${ROS_CONTROL_PY_DIR}" +if [[ ! -f "install/setup.bash" ]]; then + echo "Missing ROS workspace setup: ${ROS_CONTROL_PY_DIR}/install/setup.bash" >&2 + exit 1 +fi +source_with_nounset_off "install/setup.bash" + +launch_args=( + "transport:=${ROBOT_RECEIVER_TRANSPORT}" + "peer_id:=${ROBOT_RECEIVER_PEER_ID}" + "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}" +) + +if [[ -n "${ROBOT_RECEIVER_SERVER_ADDR}" ]]; then + launch_args+=("server_addr:=${ROBOT_RECEIVER_SERVER_ADDR}") +fi + +if [[ -n "${ROBOT_RECEIVER_RELAY_VIA}" ]]; then + launch_args+=("relay_via:=${ROBOT_RECEIVER_RELAY_VIA}") +fi + +if [[ -n "${ROBOT_RECEIVER_EXPECTED_SENDER}" ]]; then + launch_args+=("expected_sender:=${ROBOT_RECEIVER_EXPECTED_SENDER}") +fi + +exec ros2 launch udp_teleop_bridge robot_udp_receiver.launch.py "${launch_args[@]}" diff --git a/host/OmniSocketGo_add_camera/scripts/kcp_control_benchmark.py b/host/OmniSocketGo_add_camera/scripts/kcp_control_benchmark.py new file mode 100644 index 0000000..90c5b23 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/kcp_control_benchmark.py @@ -0,0 +1,76 @@ +"""Send high-rate control packets to benchmark the KCP control session.""" + +from __future__ import annotations + +import argparse +from pathlib import Path +import sys +import time + +ROOT = Path(__file__).resolve().parents[1] +sys.path.insert(0, str(ROOT)) + +import yaml + +from omnisocket_control import make_control_packet + +try: + from omnisocket import CONTROL_DEFAULTS, Session +except ImportError: + sys.path.insert(0, str(ROOT / "python")) + from omnisocket import CONTROL_DEFAULTS, Session + + +def load_config() -> dict: + config_path = ROOT / "config" / "omnisocket_demo.yaml" + if not config_path.exists(): + return {} + with config_path.open("r", encoding="utf-8") as file: + return yaml.safe_load(file) or {} + + +def main() -> None: + parser = argparse.ArgumentParser() + parser.add_argument("--rate", type=float, default=200.0, help="send rate in Hz") + parser.add_argument("--count", type=int, default=1000, help="packets to send") + args = parser.parse_args() + + config = load_config() + transport_cfg = config.get("transport", {}) + sender_cfg = config.get("control_sender", {}) + + session = Session() + session.connect( + server_addr=str(transport_cfg.get("server_addr", "127.0.0.1:10909")), + peer_id=str(sender_cfg.get("peer_id", "peer-a-ctrl")), + relay_via=str(transport_cfg.get("relay_via", "")), + bind_ip=str(transport_cfg.get("bind_ip", "")), + bind_device=str(transport_cfg.get("bind_device", "")), + **CONTROL_DEFAULTS, + ) + + target_peer = str(sender_cfg.get("target_peer", "peer-b-ctrl")) + spacing = 1.0 / args.rate if args.rate > 0 else 0.0 + start = time.perf_counter() + + try: + for seq_id in range(args.count): + packet = make_control_packet(seq_id, "set_surge", drive_value=0.25) + session.send(to=target_peer, data=packet.encode()) + if spacing > 0: + target = start + (seq_id + 1) * spacing + remaining = target - time.perf_counter() + if remaining > 0: + time.sleep(remaining) + finally: + elapsed = time.perf_counter() - start + print( + f"sent {args.count} control packets in {elapsed:.3f}s " + f"({(args.count / elapsed) if elapsed > 0 else 0.0:.1f} pkt/s)" + ) + print(f"stats={session.stats()}") + session.close() + + +if __name__ == "__main__": + main() diff --git a/host/OmniSocketGo_add_camera/scripts/refresh-latency-summary.sh b/host/OmniSocketGo_add_camera/scripts/refresh-latency-summary.sh new file mode 100644 index 0000000..6734e65 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/refresh-latency-summary.sh @@ -0,0 +1,136 @@ +#!/usr/bin/env bash + +set -u +set -o pipefail + +repo_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" +remote_source="boll@175.178.116.187:/home/boll/LMJWork/OmniSocketGo/peer-b-latency.jsonl" +local_peer_a="$repo_dir/peer-a-latency.jsonl" +local_peer_b="$repo_dir/peer-b-latency.jsonl" +summary_output="$repo_dir/latency-summary.jsonl" +chart_output="$repo_dir/latency-summary.html" +latency_binary="$repo_dir/bin/latencysummary" +go_cache_dir="${GOCACHE:-/tmp/omnisocketgo-go-build}" +poll_interval_seconds=1 + +remote_tmp="" +summary_tmp="" +chart_tmp="" + +log() { + printf '[%s] %s\n' "$(date '+%Y-%m-%d %H:%M:%S')" "$*" +} + +cleanup_temp_file() { + local path="$1" + if [[ -n "$path" && -e "$path" ]]; then + rm -f "$path" + fi +} + +cleanup() { + cleanup_temp_file "$remote_tmp" + cleanup_temp_file "$summary_tmp" + cleanup_temp_file "$chart_tmp" +} + +handle_interrupt() { + log "received interrupt signal, stopping refresh loop" + cleanup + exit 130 +} + +handle_terminate() { + log "received terminate signal, stopping refresh loop" + cleanup + exit 143 +} + +trap cleanup EXIT +trap handle_interrupt INT +trap handle_terminate TERM + +cd "$repo_dir" || exit 1 + +mkdir -p "$repo_dir/bin" +mkdir -p "$go_cache_dir" +if ! GOCACHE="$go_cache_dir" go build -o "$latency_binary" ./cmd/latencysummary; then + log "build failed; exiting" + exit 1 +fi + +log "starting 1-second refresh loop" + +while true; do + remote_tmp="$(mktemp "$repo_dir/peer-b-latency.jsonl.tmp.XXXXXX")" || exit 1 + if scp -P 10022 "$remote_source" "$remote_tmp"; then + if mv -f "$remote_tmp" "$local_peer_b"; then + remote_tmp="" + else + status=$? + log "failed to replace $(basename "$local_peer_b") after scp (exit $status)" + cleanup_temp_file "$remote_tmp" + remote_tmp="" + sleep "$poll_interval_seconds" + continue + fi + else + status=$? + log "scp refresh failed (exit $status)" + cleanup_temp_file "$remote_tmp" + remote_tmp="" + sleep "$poll_interval_seconds" + continue + fi + + summary_tmp="$(mktemp "$repo_dir/latency-summary.tmp.XXXXXX.jsonl")" || exit 1 + chart_tmp="${summary_tmp%.jsonl}.html" + if "$latency_binary" \ + -input "$local_peer_a" \ + -input "$local_peer_b" \ + -shared-max-offset 1 \ + -output "$summary_tmp"; then + if [[ ! -f "$summary_tmp" || ! -f "$chart_tmp" ]]; then + log "summary succeeded but temporary outputs are incomplete" + cleanup_temp_file "$summary_tmp" + cleanup_temp_file "$chart_tmp" + summary_tmp="" + chart_tmp="" + sleep "$poll_interval_seconds" + continue + fi + + if ! mv -f "$summary_tmp" "$summary_output"; then + status=$? + log "failed to replace $(basename "$summary_output") (exit $status)" + cleanup_temp_file "$summary_tmp" + cleanup_temp_file "$chart_tmp" + summary_tmp="" + chart_tmp="" + sleep "$poll_interval_seconds" + continue + fi + summary_tmp="" + + if ! mv -f "$chart_tmp" "$chart_output"; then + status=$? + log "failed to replace $(basename "$chart_output") (exit $status)" + cleanup_temp_file "$chart_tmp" + chart_tmp="" + sleep "$poll_interval_seconds" + continue + fi + chart_tmp="" + else + status=$? + log "latency summary refresh failed (exit $status)" + cleanup_temp_file "$summary_tmp" + cleanup_temp_file "$chart_tmp" + summary_tmp="" + chart_tmp="" + sleep "$poll_interval_seconds" + continue + fi + + sleep "$poll_interval_seconds" +done diff --git a/host/OmniSocketGo_add_camera/scripts/run-kcp-batch-test.sh b/host/OmniSocketGo_add_camera/scripts/run-kcp-batch-test.sh new file mode 100644 index 0000000..29c29d7 --- /dev/null +++ b/host/OmniSocketGo_add_camera/scripts/run-kcp-batch-test.sh @@ -0,0 +1,1202 @@ +#!/usr/bin/env bash + +set -euo pipefail + +script_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +repo_dir="$(cd "$script_dir/.." && pwd)" +script_name="$(basename "$0")" + +run_mode="direct" +server_ssh="" +peerb_ssh="" +relay_ssh="" +server_addr="" +relay_addr="" +relay_remote="" +log_prefix="" +listen_addr="0.0.0.0:10909" +relay_listen_addr="0.0.0.0:10909" +server_workdir="$repo_dir" +peerb_workdir="$repo_dir" +relay_workdir="$repo_dir" +local_workdir="$repo_dir" +ready_timeout=60 +send_interval=1 +drain_wait=5 +repeat_count=1 + +declare -a peerb_files=() + +server_started=0 +relay_started=0 +peer_b_started=0 +peer_a_pid="" + +usage() { + printf 'Usage:\n' + printf ' %s --mode --server-ssh --peerb-ssh \\\n' "$script_name" + printf ' --server-addr --log-prefix --file [options]\n' + printf '\n' + printf 'Modes:\n' + printf ' direct peer-a -> hub(server) <- peer-b (default)\n' + printf ' relay peer-a -> relay(C) -> hub(D) <- peer-b\n' + printf '\n' + printf 'Required arguments:\n' + printf ' --server-ssh SSH target for the hub server machine\n' + printf ' --peerb-ssh SSH target for the peer-b machine\n' + printf ' --server-addr Hub server IP (combined with listen port for peers)\n' + printf ' --log-prefix Log directory prefix; logs go under logs/\n' + printf ' --file Existing file path on peer-b; repeat for multiple files\n' + printf '\n' + printf 'Relay mode arguments (required when --mode=relay):\n' + printf ' --relay-ssh SSH target for the relay server machine\n' + printf ' --relay-addr Relay server IP (combined with relay listen port for peer-a)\n' + printf ' --relay-remote Hub address from relay perspective (relay -relay-remote)\n' + printf '\n' + printf 'Options:\n' + printf ' --mode Run mode (default: %s)\n' "$run_mode" + printf ' --listen-addr Hub server listen address (default: %s)\n' "$listen_addr" + printf ' --relay-listen-addr Relay server listen address (default: %s)\n' "$relay_listen_addr" + printf ' --server-workdir Hub server-side workdir (default: %s)\n' "$server_workdir" + printf ' --relay-workdir Relay server-side workdir (default: %s)\n' "$relay_workdir" + printf ' --peerb-workdir Peer-b-side workdir (default: %s)\n' "$peerb_workdir" + printf ' --local-workdir Local peer-a workdir (default: %s)\n' "$local_workdir" + printf ' --ready-timeout Startup wait timeout (default: %s)\n' "$ready_timeout" + printf ' --repeat Repeat the full --file list this many rounds (default: %s)\n' "$repeat_count" + printf ' --send-interval Delay between file commands (default: %s)\n' "$send_interval" + printf ' --drain-wait Wait after the last file before quit (default: %s)\n' "$drain_wait" + printf ' -h, --help Show this help\n' + printf '\n' + printf 'Example (direct mode):\n' + printf ' %s \\\n' "$script_name" + printf ' --mode direct \\\n' + printf ' --server-ssh root@server-host \\\n' + printf ' --peerb-ssh root@peer-b-host \\\n' + printf ' --server-addr 203.0.113.10 \\\n' + printf ' --log-prefix case01- \\\n' + printf ' --repeat 30 \\\n' + printf ' --file /tmp/test125.bin\n' + printf '\n' + printf 'Example (relay mode):\n' + printf ' %s \\\n' "$script_name" + printf ' --mode relay \\\n' + printf ' --server-ssh root@hub-host \\\n' + printf ' --relay-ssh root@relay-host \\\n' + printf ' --peerb-ssh root@peer-b-host \\\n' + printf ' --server-addr 152.136.164.246 \\\n' + printf ' --relay-addr 139.199.57.110 \\\n' + printf ' --relay-remote 172.21.0.13:10909 \\\n' + printf ' --log-prefix case01- \\\n' + printf ' --repeat 30 \\\n' + printf ' --file /tmp/test125.bin\n' +} + +log() { + printf '[%s] %s\n' "$(date '+%Y-%m-%d %H:%M:%S')" "$*" +} + +die() { + printf >&2 '[%s] error: %s\n' "$(date '+%Y-%m-%d %H:%M:%S')" "$*" + exit 1 +} + +join_path() { + local base="${1%/}" + printf '%s/%s' "$base" "$2" +} + +build_quoted_command() { + local out_var="$1" + shift + + local command="" + local part="" + local quoted="" + for part in "$@"; do + printf -v quoted '%q' "$part" + if [[ -n "$command" ]]; then + command+=" " + fi + command+="$quoted" + done + + printf -v "$out_var" '%s' "$command" +} + +run_remote_script() { + local target="$1" + local script="$2" + shift 2 + + local parts=("env") + local assignment="" + for assignment in "$@"; do + parts+=("$assignment") + done + parts+=("bash" "-s" "--") + + local remote_cmd="" + build_quoted_command remote_cmd "${parts[@]}" + ssh -T "$target" "$remote_cmd" <<<"$script" +} + +validate_positive_integer() { + local name="$1" + local value="$2" + + if [[ ! "$value" =~ ^[1-9][0-9]*$ ]]; then + die "$name must be a positive integer, got: $value" + fi +} + +validate_sleep_value() { + local name="$1" + local value="$2" + + if [[ ! "$value" =~ ^([0-9]+([.][0-9]+)?|[.][0-9]+)$ ]]; then + die "$name must be a non-negative number understood by sleep, got: $value" + fi +} + +dump_local_log_head() { + local path="$1" + + if [[ -f "$path" ]]; then + sed -n '1,120p' "$path" >&2 || true + fi +} + +dump_remote_log_head() { + local target="$1" + local log_file="$2" + local label="$3" + local script="" + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ -f "$LOG_FILE" ]]; then + sed -n '1,120p' "$LOG_FILE" +fi +EOF +)" + + log "showing $label log head from $target" + run_remote_script "$target" "$script" "LOG_FILE=$log_file" || true +} + +check_local_dependencies() { + command -v ssh >/dev/null 2>&1 || die "ssh is required" + command -v scp >/dev/null 2>&1 || die "scp is required" + command -v go >/dev/null 2>&1 || die "go is required for local peer-a" +} + +copy_remote_file_to_local() { + local remote_source="$1" + local local_dest="$2" + local local_dir="" + local local_tmp="" + + local_dir="$(dirname "$local_dest")" + mkdir -p "$local_dir" + local_tmp="$(mktemp "$local_dir/.copy.tmp.XXXXXX")" + + if scp "$remote_source" "$local_tmp"; then + mv -f "$local_tmp" "$local_dest" + else + local status=$? + rm -f "$local_tmp" + return "$status" + fi +} + +remove_local_log_dir() { + if [[ -e "$local_log_dir" ]]; then + log "removing local log dir: $local_log_dir" + rm -rf "$local_log_dir" + fi +} + +remove_remote_log_dir() { + local target="$1" + local log_dir="$2" + local label="$3" + local pid_file="${4:-}" + local script="" + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ -n "${PID_FILE:-}" && -f "$PID_FILE" ]]; then + existing_pid="$(<"$PID_FILE")" + if [[ -n "$existing_pid" ]] && kill -0 "$existing_pid" 2>/dev/null; then + printf >&2 'refusing to remove log dir while process %s is still running\n' "$existing_pid" + exit 1 + fi +fi + +rm -rf "$LOG_DIR" +EOF +)" + + log "removing $label log dir on $target: $log_dir" + run_remote_script "$target" "$script" \ + "LOG_DIR=$log_dir" \ + "PID_FILE=$pid_file" +} + +clean_log_directories() { + remove_local_log_dir + remove_remote_log_dir "$server_ssh" "$server_log_dir" "server" "$server_pid_file" + remove_remote_log_dir "$peerb_ssh" "$peerb_log_dir" "peer-b" + if [[ "$run_mode" == "relay" ]]; then + remove_remote_log_dir "$relay_ssh" "$relay_log_dir" "relay" "$relay_pid_file" + fi +} + +truncate_local_file() { + local path="$1" + local dir="" + + dir="$(dirname "$path")" + mkdir -p "$dir" + : > "$path" +} + +truncate_remote_file() { + local target="$1" + local path="$2" + local script="" + + script="$(cat <<'EOF' +set -euo pipefail + +mkdir -p "$(dirname "$FILE_PATH")" +: > "$FILE_PATH" +EOF +)" + + run_remote_script "$target" "$script" "FILE_PATH=$path" +} + +reset_logs_after_probe() { + log "resetting peer logs after connectivity probe" + + rm -f "$local_peer_a_messages_log" + truncate_local_file "$local_peer_a_stdout_log" + truncate_local_file "$local_peer_a_latency_log" + truncate_local_file "$local_peer_a_ts_debug_log" + truncate_local_file "$local_peer_a_session_stats_log" + + truncate_remote_file "$peerb_ssh" "$peerb_stdout_log" + truncate_remote_file "$peerb_ssh" "$peerb_latency_log" + truncate_remote_file "$peerb_ssh" "$peerb_ts_debug_log" + truncate_remote_file "$peerb_ssh" "$peerb_session_stats_log" +} + +fetch_remote_peer_b_logs() { + log "copying peer-b latency log from $peerb_ssh:$peerb_latency_log to $local_peer_b_latency_log" + copy_remote_file_to_local "$peerb_ssh:$peerb_latency_log" "$local_peer_b_latency_log" +} + +run_local_latency_summary() { + [[ -f "$local_peer_a_latency_log" ]] || die "local peer-a latency log not found: $local_peer_a_latency_log" + [[ -f "$local_peer_b_latency_log" ]] || die "local peer-b latency log not found: $local_peer_b_latency_log" + + log "generating local latency summary: $local_kcp_latency_summary_log" + ( + cd "$repo_dir" + exec go run ./cmd/latencysummary \ + -input "$local_peer_a_latency_log" \ + -input "$local_peer_b_latency_log" \ + -output "$local_kcp_latency_summary_log" + ) +} + +check_remote_peerb_files() { + local script="" + local file="" + + script="$(cat <<'EOF' +set -euo pipefail + +cd "$PEERB_WORKDIR" +if [[ ! -f "$FILE_PATH" ]]; then + printf >&2 'peer-b file not found: %s\n' "$FILE_PATH" + exit 1 +fi +EOF +)" + + for file in "${peerb_files[@]}"; do + log "checking peer-b file exists: $file" + run_remote_script "$peerb_ssh" "$script" \ + "PEERB_WORKDIR=$peerb_workdir" \ + "FILE_PATH=$file" + done +} + +start_remote_server() { + local script="" + + script="$(cat <<'EOF' +export PATH="$PATH:/usr/local/go/bin:$HOME/go/bin" +set -euo pipefail + +cd "$SERVER_WORKDIR" +mkdir -p "$LOG_DIR" + +if [[ -f "$PID_FILE" ]]; then + existing_pid="$(<"$PID_FILE")" + if [[ -n "$existing_pid" ]] && kill -0 "$existing_pid" 2>/dev/null; then + printf >&2 'server already running with pid %s\n' "$existing_pid" + exit 1 + fi +fi + +: > "$STDOUT_LOG" +setsid go run ./cmd/kcpserver/ \ + -listen "$LISTEN_ADDR" \ + >>"$STDOUT_LOG" 2>&1 "$PID_FILE" +EOF +)" + + log "starting remote kcpserver (hub) on $server_ssh" + run_remote_script "$server_ssh" "$script" \ + "SERVER_WORKDIR=$server_workdir" \ + "LOG_DIR=$server_log_dir" \ + "PID_FILE=$server_pid_file" \ + "STDOUT_LOG=$server_stdout_log" \ + "LISTEN_ADDR=$listen_addr" + + server_started=1 +} + +wait_for_remote_server_ready() { + local pattern="kcp hub listening" + local script="" + local start_time="$SECONDS" + local status=0 + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ -f "$LOG_FILE" ]] && grep -Fq -- "$READY_PATTERN" "$LOG_FILE"; then + exit 0 +fi + +if [[ -f "$PID_FILE" ]]; then + pid="$(<"$PID_FILE")" + if [[ -n "$pid" ]] && kill -0 "$pid" 2>/dev/null; then + exit 10 + fi +fi + +exit 20 +EOF +)" + + while (( SECONDS - start_time < ready_timeout )); do + status=0 + run_remote_script "$server_ssh" "$script" \ + "LOG_FILE=$server_stdout_log" \ + "READY_PATTERN=$pattern" \ + "PID_FILE=$server_pid_file" || status=$? + + case "$status" in + 0) + log "remote server is ready" + return 0 + ;; + 10) + sleep 1 + ;; + 20) + log "remote server exited before readiness" + dump_remote_log_head "$server_ssh" "$server_stdout_log" "server" + return 1 + ;; + *) + log "remote server readiness check failed with status $status" + dump_remote_log_head "$server_ssh" "$server_stdout_log" "server" + return 1 + ;; + esac + done + + log "timed out waiting for remote server readiness after ${ready_timeout}s" + dump_remote_log_head "$server_ssh" "$server_stdout_log" "server" + return 1 +} + +stop_remote_server() { + local script="" + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ ! -f "$PID_FILE" ]]; then + exit 0 +fi + +pid="$(<"$PID_FILE")" +if [[ -z "$pid" ]]; then + rm -f "$PID_FILE" + exit 0 +fi + +# Kill the entire process group (setsid creates a new group with pid == pgid). +kill -- -"$pid" 2>/dev/null || kill "$pid" 2>/dev/null || true +for _ in 1 2 3 4 5; do + if ! kill -0 "$pid" 2>/dev/null; then + rm -f "$PID_FILE" + exit 0 + fi + sleep 1 +done + +kill -9 -- -"$pid" 2>/dev/null || kill -9 "$pid" 2>/dev/null || true +rm -f "$PID_FILE" +EOF +)" + + run_remote_script "$server_ssh" "$script" "PID_FILE=$server_pid_file" +} + +start_remote_relay() { + local script="" + + script="$(cat <<'EOF' +export PATH="$PATH:/usr/local/go/bin:$HOME/go/bin" +set -euo pipefail + +cd "$RELAY_WORKDIR" +mkdir -p "$LOG_DIR" + +if [[ -f "$PID_FILE" ]]; then + existing_pid="$(<"$PID_FILE")" + if [[ -n "$existing_pid" ]] && kill -0 "$existing_pid" 2>/dev/null; then + printf >&2 'relay already running with pid %s\n' "$existing_pid" + exit 1 + fi +fi + +: > "$STDOUT_LOG" +setsid go run ./cmd/kcpserver/ \ + -mode=relay \ + -listen "$LISTEN_ADDR" \ + -relay-remote "$RELAY_REMOTE" \ + >>"$STDOUT_LOG" 2>&1 "$PID_FILE" +EOF +)" + + log "starting remote relay on $relay_ssh" + run_remote_script "$relay_ssh" "$script" \ + "RELAY_WORKDIR=$relay_workdir" \ + "LOG_DIR=$relay_log_dir" \ + "PID_FILE=$relay_pid_file" \ + "STDOUT_LOG=$relay_stdout_log" \ + "LISTEN_ADDR=$relay_listen_addr" \ + "RELAY_REMOTE=$relay_remote" + + relay_started=1 +} + +wait_for_remote_relay_ready() { + local pattern="udp relay listening" + local script="" + local start_time="$SECONDS" + local status=0 + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ -f "$LOG_FILE" ]] && grep -Fq -- "$READY_PATTERN" "$LOG_FILE"; then + exit 0 +fi + +if [[ -f "$PID_FILE" ]]; then + pid="$(<"$PID_FILE")" + if [[ -n "$pid" ]] && kill -0 "$pid" 2>/dev/null; then + exit 10 + fi +fi + +exit 20 +EOF +)" + + while (( SECONDS - start_time < ready_timeout )); do + status=0 + run_remote_script "$relay_ssh" "$script" \ + "LOG_FILE=$relay_stdout_log" \ + "READY_PATTERN=$pattern" \ + "PID_FILE=$relay_pid_file" || status=$? + + case "$status" in + 0) + log "remote relay is ready" + return 0 + ;; + 10) + sleep 1 + ;; + 20) + log "remote relay exited before readiness" + dump_remote_log_head "$relay_ssh" "$relay_stdout_log" "relay" + return 1 + ;; + *) + log "remote relay readiness check failed with status $status" + dump_remote_log_head "$relay_ssh" "$relay_stdout_log" "relay" + return 1 + ;; + esac + done + + log "timed out waiting for remote relay readiness after ${ready_timeout}s" + dump_remote_log_head "$relay_ssh" "$relay_stdout_log" "relay" + return 1 +} + +stop_remote_relay() { + local script="" + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ ! -f "$PID_FILE" ]]; then + exit 0 +fi + +pid="$(<"$PID_FILE")" +if [[ -z "$pid" ]]; then + rm -f "$PID_FILE" + exit 0 +fi + +kill -- -"$pid" 2>/dev/null || kill "$pid" 2>/dev/null || true +for _ in 1 2 3 4 5; do + if ! kill -0 "$pid" 2>/dev/null; then + rm -f "$PID_FILE" + exit 0 + fi + sleep 1 +done + +kill -9 -- -"$pid" 2>/dev/null || kill -9 "$pid" 2>/dev/null || true +rm -f "$PID_FILE" +EOF +)" + + run_remote_script "$relay_ssh" "$script" "PID_FILE=$relay_pid_file" +} + +start_local_peer_a() { + log "starting local peer-a" + mkdir -p "$local_log_dir" "$local_peer_a_inbox" + : > "$local_peer_a_stdout_log" + + local peer_a_args=( + -id peer-a + -server "$server_connect_addr" + -inbox-dir "$local_peer_a_inbox" + -latency-log "$local_peer_a_latency_log" + -kcp-ts-debug-log "$local_peer_a_ts_debug_log" + -kcp-session-stats-log "$local_peer_a_session_stats_log" + -interactive=false + ) + + if [[ "$run_mode" == "relay" ]]; then + peer_a_args+=(-relay-via "$relay_connect_addr") + fi + + ( + cd "$local_workdir" + exec go run ./cmd/kcppeer "${peer_a_args[@]}" \ + >>"$local_peer_a_stdout_log" 2>&1 + ) & + + peer_a_pid="$!" +} + +wait_for_local_peer_a_ready() { + local pattern="opened KCP session as peer-a" + local start_time="$SECONDS" + + while (( SECONDS - start_time < ready_timeout )); do + if [[ -f "$local_peer_a_stdout_log" ]] && grep -Fq -- "$pattern" "$local_peer_a_stdout_log"; then + log "local peer-a is ready" + return 0 + fi + + if [[ -n "$peer_a_pid" ]] && ! kill -0 "$peer_a_pid" 2>/dev/null; then + log "local peer-a exited before readiness" + dump_local_log_head "$local_peer_a_stdout_log" + return 1 + fi + + sleep 1 + done + + log "timed out waiting for local peer-a readiness after ${ready_timeout}s" + dump_local_log_head "$local_peer_a_stdout_log" + return 1 +} + +stop_local_peer_a() { + if [[ -z "$peer_a_pid" ]]; then + return 0 + fi + + if kill -0 "$peer_a_pid" 2>/dev/null; then + kill "$peer_a_pid" 2>/dev/null || true + wait "$peer_a_pid" 2>/dev/null || true + else + wait "$peer_a_pid" 2>/dev/null || true + fi + + peer_a_pid="" +} + +start_remote_peer_b() { + local script="" + + script="$(cat <<'EOF' +export PATH="$PATH:/usr/local/go/bin:$HOME/go/bin" +set -euo pipefail + +cd "$PEERB_WORKDIR" +mkdir -p "$LOG_DIR" "$INBOX_DIR" + +if [[ -f "$PID_FILE" ]]; then + existing_pid="$(<"$PID_FILE")" + if [[ -n "$existing_pid" ]] && kill -0 "$existing_pid" 2>/dev/null; then + printf >&2 'peer-b already running with pid %s\n' "$existing_pid" + exit 1 + fi +fi + +: > "$STDOUT_LOG" +: > "$COMMAND_FILE" + + peer_b_cmd="$(cat <<'INNER' + tail -n +1 -f "$COMMAND_FILE" | exec go run ./cmd/kcppeer/ \ + -id peer-b \ + -server "$SERVER_ADDR" \ + -inbox-dir "$INBOX_DIR" \ + -latency-log "$LATENCY_LOG" \ + -kcp-ts-debug-log "$TS_DEBUG_LOG" \ + -kcp-session-stats-log "$SESSION_STATS_LOG" +INNER +)" + +nohup setsid bash -lc "$peer_b_cmd" >>"$STDOUT_LOG" 2>&1 "$PID_FILE" +EOF +)" + + log "starting remote peer-b on $peerb_ssh" + run_remote_script "$peerb_ssh" "$script" \ + "PEERB_WORKDIR=$peerb_workdir" \ + "LOG_DIR=$peerb_log_dir" \ + "INBOX_DIR=$peerb_inbox_dir" \ + "STDOUT_LOG=$peerb_stdout_log" \ + "COMMAND_FILE=$peerb_command_file" \ + "PID_FILE=$peerb_pid_file" \ + "SERVER_ADDR=$server_connect_addr" \ + "LATENCY_LOG=$peerb_latency_log" \ + "TS_DEBUG_LOG=$peerb_ts_debug_log" \ + "SESSION_STATS_LOG=$peerb_session_stats_log" + + peer_b_started=1 +} + +wait_for_remote_peer_b_ready() { + local pattern="opened KCP session as peer-b" + local script="" + local start_time="$SECONDS" + local status=0 + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ -f "$LOG_FILE" ]] && grep -Fq -- "$READY_PATTERN" "$LOG_FILE"; then + exit 0 +fi + +if [[ -f "$PID_FILE" ]]; then + pid="$(<"$PID_FILE")" + if [[ -n "$pid" ]] && kill -0 "$pid" 2>/dev/null; then + exit 10 + fi +fi + +exit 20 +EOF +)" + + while (( SECONDS - start_time < ready_timeout )); do + status=0 + run_remote_script "$peerb_ssh" "$script" \ + "LOG_FILE=$peerb_stdout_log" \ + "READY_PATTERN=$pattern" \ + "PID_FILE=$peerb_pid_file" || status=$? + + case "$status" in + 0) + log "remote peer-b is ready" + return 0 + ;; + 10) + sleep 1 + ;; + 20) + log "remote peer-b exited before readiness" + dump_remote_log_head "$peerb_ssh" "$peerb_stdout_log" "peer-b" + return 1 + ;; + *) + log "remote peer-b readiness check failed with status $status" + dump_remote_log_head "$peerb_ssh" "$peerb_stdout_log" "peer-b" + return 1 + ;; + esac + done + + log "timed out waiting for remote peer-b readiness after ${ready_timeout}s" + dump_remote_log_head "$peerb_ssh" "$peerb_stdout_log" "peer-b" + return 1 +} + +probe_peer_b_to_local_peer_a() { + local marker="" + local command_line="" + local quoted_command="" + local script="" + local start_time="$SECONDS" + + marker="probe-$(date +%s)-$$" + printf -v command_line 'text peer-a %s' "$marker" + printf -v quoted_command '%q' "$command_line" + + script="$(cat <> "\$COMMAND_FILE" +EOF +)" + + log "probing peer-b -> peer-a message delivery before batch" + run_remote_script "$peerb_ssh" "$script" "COMMAND_FILE=$peerb_command_file" + + while (( SECONDS - start_time < ready_timeout )); do + if [[ -f "$local_peer_a_messages_log" ]] && grep -Fq -- "$marker" "$local_peer_a_messages_log"; then + log "peer-b -> peer-a probe succeeded" + reset_logs_after_probe + return 0 + fi + + if [[ -n "$peer_a_pid" ]] && ! kill -0 "$peer_a_pid" 2>/dev/null; then + log "local peer-a exited during connectivity probe" + dump_local_log_head "$local_peer_a_stdout_log" + return 1 + fi + + sleep 1 + done + + log "timed out waiting for peer-b -> peer-a probe delivery after ${ready_timeout}s" + dump_local_log_head "$local_peer_a_stdout_log" + dump_remote_log_head "$peerb_ssh" "$peerb_stdout_log" "peer-b" + return 1 +} + +run_remote_peer_b_batch() { + local script="" + local batch_commands="" + local round=0 + local i=0 + local send_index=0 + local total_sends=$(( ${#peerb_files[@]} * repeat_count )) + local file="" + local command_line="" + local quoted_command="" + local quoted_sleep="" + + for (( round = 1; round <= repeat_count; round++ )); do + for (( i = 0; i < ${#peerb_files[@]}; i++ )); do + file="${peerb_files[$i]}" + send_index=$(( send_index + 1 )) + log "queueing peer-b -> peer-a file (round $round/$repeat_count, send $send_index/$total_sends): $file" + printf -v command_line 'file peer-a %s' "$file" + printf -v quoted_command '%q' "$command_line" + batch_commands+="printf '%s\n' ${quoted_command} >> \"\$COMMAND_FILE\""$'\n' + if (( send_index < total_sends )); then + printf -v quoted_sleep '%q' "$send_interval" + batch_commands+="sleep ${quoted_sleep}"$'\n' + fi + done + done + printf -v quoted_sleep '%q' "$drain_wait" + batch_commands+="sleep ${quoted_sleep}"$'\n' + batch_commands+="printf '%s\n' quit >> \"\$COMMAND_FILE\""$'\n' + + script="$(cat <&2 'peer-b pid file not found: %s\n' "\$PID_FILE" + exit 1 +fi + +pid="\$(<"\$PID_FILE")" +if [[ -z "\$pid" ]] || ! kill -0 "\$pid" 2>/dev/null; then + printf >&2 'peer-b is not running\n' + exit 1 +fi + +$batch_commands + +for (( i = 0; i < READY_TIMEOUT; i++ )); do + if ! kill -0 "\$pid" 2>/dev/null; then + rm -f "\$PID_FILE" "\$COMMAND_FILE" + exit 0 + fi + sleep 1 +done + +printf >&2 'peer-b did not exit after quit within %s seconds\n' "\$READY_TIMEOUT" +exit 1 +EOF +)" + + log "sending ${#peerb_files[@]} files across $repeat_count rounds ($total_sends sends total) from peer-b" + run_remote_script "$peerb_ssh" "$script" \ + "PID_FILE=$peerb_pid_file" \ + "COMMAND_FILE=$peerb_command_file" \ + "READY_TIMEOUT=$ready_timeout" + + peer_b_started=0 +} + +stop_remote_peer_b() { + local script="" + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ ! -f "$PID_FILE" ]]; then + rm -f "$COMMAND_FILE" + exit 0 +fi + +pid="$(<"$PID_FILE")" +if [[ -z "$pid" ]]; then + rm -f "$PID_FILE" "$COMMAND_FILE" + exit 0 +fi + +if kill -0 "$pid" 2>/dev/null; then + printf 'quit\n' >> "$COMMAND_FILE" 2>/dev/null || true + for _ in 1 2 3 4 5; do + if ! kill -0 "$pid" 2>/dev/null; then + rm -f "$PID_FILE" "$COMMAND_FILE" + exit 0 + fi + sleep 1 + done + kill -- -"$pid" 2>/dev/null || kill "$pid" 2>/dev/null || true + for _ in 1 2 3 4 5; do + if ! kill -0 "$pid" 2>/dev/null; then + rm -f "$PID_FILE" "$COMMAND_FILE" + exit 0 + fi + sleep 1 + done + kill -9 -- -"$pid" 2>/dev/null || kill -9 "$pid" 2>/dev/null || true +fi + +rm -f "$PID_FILE" "$COMMAND_FILE" +EOF +)" + + run_remote_script "$peerb_ssh" "$script" \ + "PID_FILE=$peerb_pid_file" \ + "COMMAND_FILE=$peerb_command_file" +} + +cleanup() { + local exit_code="$?" + + trap - EXIT INT TERM + + if [[ -n "$peer_a_pid" ]]; then + log "stopping local peer-a" + stop_local_peer_a + fi + + if (( peer_b_started == 1 )); then + log "stopping remote peer-b on $peerb_ssh" + stop_remote_peer_b || true + fi + + if (( relay_started == 1 )); then + log "stopping remote relay on $relay_ssh" + stop_remote_relay || true + fi + + if (( server_started == 1 )); then + log "stopping remote server on $server_ssh" + stop_remote_server || true + fi + + exit "$exit_code" +} + +handle_interrupt() { + log "received interrupt signal" + exit 130 +} + +handle_terminate() { + log "received terminate signal" + exit 143 +} + +while [[ $# -gt 0 ]]; do + case "$1" in + --mode) + [[ $# -ge 2 ]] || die "--mode requires a value" + run_mode="$2" + shift 2 + ;; + --server-ssh) + [[ $# -ge 2 ]] || die "--server-ssh requires a value" + server_ssh="$2" + shift 2 + ;; + --peerb-ssh) + [[ $# -ge 2 ]] || die "--peerb-ssh requires a value" + peerb_ssh="$2" + shift 2 + ;; + --relay-ssh) + [[ $# -ge 2 ]] || die "--relay-ssh requires a value" + relay_ssh="$2" + shift 2 + ;; + --server-addr) + [[ $# -ge 2 ]] || die "--server-addr requires a value" + server_addr="$2" + shift 2 + ;; + --relay-addr) + [[ $# -ge 2 ]] || die "--relay-addr requires a value" + relay_addr="$2" + shift 2 + ;; + --relay-remote) + [[ $# -ge 2 ]] || die "--relay-remote requires a value" + relay_remote="$2" + shift 2 + ;; + --log-prefix) + [[ $# -ge 2 ]] || die "--log-prefix requires a value" + log_prefix="$2" + shift 2 + ;; + --listen-addr) + [[ $# -ge 2 ]] || die "--listen-addr requires a value" + listen_addr="$2" + shift 2 + ;; + --relay-listen-addr) + [[ $# -ge 2 ]] || die "--relay-listen-addr requires a value" + relay_listen_addr="$2" + shift 2 + ;; + --server-workdir) + [[ $# -ge 2 ]] || die "--server-workdir requires a value" + server_workdir="$2" + shift 2 + ;; + --relay-workdir) + [[ $# -ge 2 ]] || die "--relay-workdir requires a value" + relay_workdir="$2" + shift 2 + ;; + --peerb-workdir) + [[ $# -ge 2 ]] || die "--peerb-workdir requires a value" + peerb_workdir="$2" + shift 2 + ;; + --local-workdir) + [[ $# -ge 2 ]] || die "--local-workdir requires a value" + local_workdir="$2" + shift 2 + ;; + --ready-timeout) + [[ $# -ge 2 ]] || die "--ready-timeout requires a value" + ready_timeout="$2" + shift 2 + ;; + --repeat) + [[ $# -ge 2 ]] || die "--repeat requires a value" + repeat_count="$2" + shift 2 + ;; + --send-interval) + [[ $# -ge 2 ]] || die "--send-interval requires a value" + send_interval="$2" + shift 2 + ;; + --drain-wait) + [[ $# -ge 2 ]] || die "--drain-wait requires a value" + drain_wait="$2" + shift 2 + ;; + --file) + [[ $# -ge 2 ]] || die "--file requires a value" + peerb_files+=("$2") + shift 2 + ;; + -h|--help) + usage + exit 0 + ;; + *) + die "unknown argument: $1" + ;; + esac +done + +[[ "$run_mode" == "direct" || "$run_mode" == "relay" ]] || die "--mode must be 'direct' or 'relay', got: $run_mode" +[[ -n "$server_ssh" ]] || die "--server-ssh is required" +[[ -n "$peerb_ssh" ]] || die "--peerb-ssh is required" +[[ -n "$server_addr" ]] || die "--server-addr is required" +[[ -n "$log_prefix" ]] || die "--log-prefix is required" +(( ${#peerb_files[@]} > 0 )) || die "at least one --file is required" + +if [[ "$run_mode" == "relay" ]]; then + [[ -n "$relay_ssh" ]] || die "--relay-ssh is required in relay mode" + [[ -n "$relay_addr" ]] || die "--relay-addr is required in relay mode" + [[ -n "$relay_remote" ]] || die "--relay-remote is required in relay mode" +fi + +validate_positive_integer "--ready-timeout" "$ready_timeout" +validate_positive_integer "--repeat" "$repeat_count" +validate_sleep_value "--send-interval" "$send_interval" +validate_sleep_value "--drain-wait" "$drain_wait" + +check_local_dependencies + +# Extract ports and build peer connection addresses. +server_port="${listen_addr##*:}" +server_connect_addr="${server_addr}:${server_port}" + +relay_connect_addr="" +if [[ "$run_mode" == "relay" ]]; then + relay_port="${relay_listen_addr##*:}" + relay_connect_addr="${relay_addr}:${relay_port}" +fi + +log_dir_name="${log_prefix}logs" +inbox_dir_name="${log_prefix}inbox" + +local_log_dir="$(join_path "$local_workdir" "$log_dir_name")" +local_peer_a_inbox="$(join_path "$local_workdir" "$inbox_dir_name/peer-a")" +local_peer_a_messages_log="$(join_path "$local_peer_a_inbox" "messages.log")" +local_peer_a_stdout_log="$(join_path "$local_log_dir" "peer-a.stdout.log")" +local_peer_a_latency_log="$(join_path "$local_log_dir" "peer-a-kcp-latency.jsonl")" +local_peer_a_ts_debug_log="$(join_path "$local_log_dir" "peer-a-kcp-packet-debug.jsonl")" +local_peer_a_session_stats_log="$(join_path "$local_log_dir" "peer-a-kcp-session-stats.jsonl")" +local_peer_b_stdout_log="$(join_path "$local_log_dir" "peer-b.stdout.log")" +local_peer_b_latency_log="$(join_path "$local_log_dir" "peer-b-kcp-latency.jsonl")" +local_peer_b_ts_debug_log="$(join_path "$local_log_dir" "peer-b-kcp-packet-debug.jsonl")" +local_peer_b_session_stats_log="$(join_path "$local_log_dir" "peer-b-kcp-session-stats.jsonl")" +local_kcp_latency_summary_log="$(join_path "$local_log_dir" "kcp-latency-summary.jsonl")" + +server_log_dir="$(join_path "$server_workdir" "$log_dir_name")" +server_pid_file="$(join_path "$server_log_dir" "server.pid")" +server_stdout_log="$(join_path "$server_log_dir" "server.stdout.log")" + +relay_log_dir="" +relay_pid_file="" +relay_stdout_log="" +if [[ "$run_mode" == "relay" ]]; then + relay_log_dir="$(join_path "$relay_workdir" "$log_dir_name")" + relay_pid_file="$(join_path "$relay_log_dir" "relay.pid")" + relay_stdout_log="$(join_path "$relay_log_dir" "relay.stdout.log")" +fi + +peerb_log_dir="$(join_path "$peerb_workdir" "$log_dir_name")" +peerb_inbox_dir="$(join_path "$peerb_workdir" "$inbox_dir_name/peer-b")" +peerb_stdout_log="$(join_path "$peerb_log_dir" "peer-b.stdout.log")" +peerb_latency_log="$(join_path "$peerb_log_dir" "peer-b-kcp-latency.jsonl")" +peerb_ts_debug_log="$(join_path "$peerb_log_dir" "peer-b-kcp-packet-debug.jsonl")" +peerb_session_stats_log="$(join_path "$peerb_log_dir" "peer-b-kcp-session-stats.jsonl")" +peerb_pid_file="$(join_path "$peerb_log_dir" "peer-b.pid")" +peerb_command_file="$(join_path "$peerb_log_dir" "peer-b.commands")" + +trap cleanup EXIT +trap handle_interrupt INT +trap handle_terminate TERM + +clean_log_directories + +mkdir -p "$local_log_dir" "$local_peer_a_inbox" + +log "run mode: $run_mode" +log "local peer-a logs: $local_log_dir" +log "remote server logs: $server_log_dir" +if [[ "$run_mode" == "relay" ]]; then + log "remote relay logs: $relay_log_dir" +fi +log "remote peer-b logs: $peerb_log_dir" + +check_remote_peerb_files +start_remote_server +wait_for_remote_server_ready + +if [[ "$run_mode" == "relay" ]]; then + start_remote_relay + wait_for_remote_relay_ready +fi + +start_local_peer_a +start_remote_peer_b +wait_for_local_peer_a_ready +wait_for_remote_peer_b_ready +probe_peer_b_to_local_peer_a +run_remote_peer_b_batch + +log "batch send completed" + +if [[ -n "$peer_a_pid" ]]; then + log "stopping local peer-a after batch" + stop_local_peer_a +fi + +if (( relay_started == 1 )); then + log "stopping remote relay on $relay_ssh after batch" + if stop_remote_relay; then + relay_started=0 + else + log "failed to stop remote relay cleanly; cleanup will retry" + fi +fi + +if (( server_started == 1 )); then + log "stopping remote server on $server_ssh after batch" + if stop_remote_server; then + server_started=0 + else + log "failed to stop remote server cleanly; cleanup will retry" + fi +fi + +fetch_remote_peer_b_logs +run_local_latency_summary diff --git a/host/OmniSocketGo_add_camera/src/gps_buffer.c b/host/OmniSocketGo_add_camera/src/gps_buffer.c new file mode 100644 index 0000000..9d65b91 --- /dev/null +++ b/host/OmniSocketGo_add_camera/src/gps_buffer.c @@ -0,0 +1,333 @@ +#include "gps_buffer.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include // 确保包含 errno + +// 全局共享变量 +static gps_video_sample_t g_current_gps_data = {0.0, 0.0}; +static volatile int g_running = 0; +static pthread_t g_gps_thread; +static pthread_mutex_t g_gps_mutex = PTHREAD_MUTEX_INITIALIZER; + +static double normalize_coordinate(double coordinate) { + return round(coordinate * 1000000.0) / 1000000.0; +} + +static void store_gps(double latitude, double longitude) { + pthread_mutex_lock(&g_gps_mutex); + g_current_gps_data.latitude = normalize_coordinate(latitude); + g_current_gps_data.longitude = normalize_coordinate(longitude); + pthread_mutex_unlock(&g_gps_mutex); +} + +static void clear_gps(void) { + pthread_mutex_lock(&g_gps_mutex); + g_current_gps_data.latitude = 0.0; + g_current_gps_data.longitude = 0.0; + pthread_mutex_unlock(&g_gps_mutex); +} + +static gps_video_sample_t load_gps(void) { + gps_video_sample_t sample; + + pthread_mutex_lock(&g_gps_mutex); + sample = g_current_gps_data; + pthread_mutex_unlock(&g_gps_mutex); + return sample; +} + +static void gps_sleep_before_retry(void) { + int retry_ms = 1000; + int step_ms = 100; + int elapsed_ms = 0; + + while (g_running && elapsed_ms < retry_ms) { + usleep((useconds_t) step_ms * 1000U); + elapsed_ms += step_ms; + } +} + +// 将经纬度规范化为 double，保留 6 位小数。 +static int normalize_gps(double latitude, double longitude, gps_video_sample_t* sample) { + if (!isfinite(latitude) || !isfinite(longitude)) { + return -1; + } + // 过滤掉 0,0 这种无效坐标 + if (fabs(latitude) < 1e-6 && fabs(longitude) < 1e-6) { + return -1; + } + + if (sample == NULL) { + return -1; + } + + sample->latitude = normalize_coordinate(latitude); + sample->longitude = normalize_coordinate(longitude); + return 0; +} + +// ================================================================= +// 以下是借鉴 gps_parse.c 实现的底层解析函数 +// ================================================================= + +// 1. 辅助函数：在 JSON 字符串中查找键对应的值的起始位置 +static const char* find_json_value(const char* json, const char* key) { + char pattern[64]; + int written; + const char* position; + + if (json == NULL || key == NULL) return NULL; + + // 构建搜索模式: "key": + written = snprintf(pattern, sizeof(pattern), "\"%s\":", key); + if (written < 0 || (size_t)written >= sizeof(pattern)) { + return NULL; + } + + position = strstr(json, pattern); + if (position == NULL) { + return NULL; + } + + // 跳过 "key": + position += written; + + // 跳过可能存在的空格 + while (*position == ' ' || *position == '\t') { + position++; + } + + return position; +} + +// 2. 解析函数：从 JSON 字符串中提取 Double 类型的值 +static int json_extract_double(const char* json, const char* key, double* value) { + const char* position; + char* endptr = NULL; + double parsed; + + position = find_json_value(json, key); + if (position == NULL) { + return 0; // 键不存在 + } + + // 确保当前位置是数字或负号 + if (*position != '-' && !(*position >= '0' && *position <= '9')) { + return 0; + } + + // 重置 errno 以检测错误 + errno = 0; + parsed = strtod(position, &endptr); + + // 检查转换是否成功 + if (errno != 0 || endptr == position || !isfinite(parsed)) { + return 0; + } + + *value = parsed; + return 1; +} + +// 3. 解析函数：从 JSON 字符串中提取 Int 类型的值 +static int json_extract_int(const char* json, const char* key, int* value) { + double dval; + if (json_extract_double(json, key, &dval)) { + *value = (int)dval; + return 1; + } + return 0; +} + +// 4. 检查是否为 TPV (定位数据) 包 +static int is_tpv_class(const char* json) { + char class_buf[32] = {0}; + const char* pos = find_json_value(json, "class"); + if (pos == NULL || *pos != '"') return 0; + + // 简单提取 class 的值 (TPV/SKY/DEVICES) + sscanf(pos, "\"%31[^\"]\"", class_buf); + return (strcmp(class_buf, "TPV") == 0); +} + +// ================================================================= +// 后台线程函数：负责连接 gpsd 并更新全局变量 +// ================================================================= +void* gps_update_thread(void* arg) { + const char* host = (const char*)arg; + const char* gpsd_host = (host != NULL && host[0] != '\0') ? host : "127.0.0.1"; + + while (g_running) { + int sockfd = -1; + struct addrinfo hints; + struct addrinfo *res = NULL; + struct addrinfo *rp = NULL; + int s; + char buffer[4096]; + size_t offset = 0; + + // 1. 解析地址并连接 gpsd (默认端口 2947) + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_UNSPEC; // 兼容 IPv4/IPv6 + hints.ai_socktype = SOCK_STREAM; + + s = getaddrinfo(gpsd_host, "2947", &hints, &res); + if (s != 0) { + fprintf(stderr, "GPS线程: 解析 gpsd 地址失败 %s:2947: %s\n", gpsd_host, gai_strerror(s)); + gps_sleep_before_retry(); + continue; + } + + // 尝试连接每一个解析出来的地址 + for (rp = res; rp != NULL; rp = rp->ai_next) { + sockfd = socket(rp->ai_family, rp->ai_socktype, rp->ai_protocol); + if (sockfd == -1) { + continue; + } + + if (connect(sockfd, rp->ai_addr, rp->ai_addrlen) != -1) { + break; + } + close(sockfd); + sockfd = -1; + } + + freeaddrinfo(res); + + if (sockfd < 0) { + fprintf(stderr, "GPS线程: 无法连接到 %s:2947，1 秒后重试\n", gpsd_host); + gps_sleep_before_retry(); + continue; + } + + printf("GPS线程: 已连接到 gpsd %s\n", gpsd_host); + + // 2. 发送 WATCH 命令，开启 JSON 流 + { + const char* watch_cmd = "?WATCH={\"enable\":true,\"json\":true};\n"; + + if (send(sockfd, watch_cmd, strlen(watch_cmd), 0) < 0) { + perror("GPS线程: 发送 WATCH 命令失败"); + close(sockfd); + gps_sleep_before_retry(); + continue; + } + } + + // 3. 主循环：读取并解析数据流 + // 注意：gpsd 数据是以 \n 结尾的，不能直接用固定长度 recv + while (g_running) { + ssize_t len = recv(sockfd, buffer + offset, sizeof(buffer) - 1 - offset, 0); + + if (len <= 0) { + break; + } + + offset += (size_t) len; + buffer[offset] = '\0'; // 确保字符串结束 + + // 查找换行符 \n，因为一条完整的 JSON 消息以 \n 结尾 + char* start = buffer; + char* end; + + while ((end = memchr(start, '\n', (buffer + offset) - start)) != NULL) { + *end = '\0'; // 临时截断，形成独立字符串 + + // --- 核心解析逻辑 --- + // 1. 检查是否为 TPV 数据包 + if (is_tpv_class(start)) { + double lat = 0.0; + double lon = 0.0; + int mode = 0; + int has_fix = 0; + + // 2. 提取定位模式 (mode: 1=无定位, 2=2D, 3=3D) + if (json_extract_int(start, "mode", &mode)) { + has_fix = (mode >= 2); + } + + // 3. 如果有定位，提取经纬度 + if (has_fix) { + int got_lat = json_extract_double(start, "lat", &lat); + int got_lon = json_extract_double(start, "lon", &lon); + + if (got_lat && got_lon) { + gps_video_sample_t sample; + + // 4. 更新全局共享变量，使用 double 直接携带经纬度。 + if (normalize_gps(lat, lon, &sample) == 0) { + store_gps(sample.latitude, sample.longitude); + } + // 调试：取消注释可查看实时经纬度 + // printf("更新GPS: lat=%.6f, lon=%.6f\n", lat, lon); + } + } + // 如果无定位，这里不操作，保持上一次的有效值 + } + // --- 解析结束 --- + + // 移动指针到下一条消息 + start = end + 1; + } + + // 处理完所有完整消息后，将剩余未处理的数据移到缓冲区头部 + if (start < buffer + offset) { + size_t remaining = (size_t) ((buffer + offset) - start); + memmove(buffer, start, remaining); + offset = remaining; + } else { + offset = 0; // 缓冲区已清空 + } + } + + close(sockfd); + if (g_running) { + fprintf(stderr, "GPS线程: 连接断开，1 秒后重连...\n"); + gps_sleep_before_retry(); + } + } + + return NULL; +} + +// ================================================================= +// 接口函数实现 +// ================================================================= +gps_video_sample_t get_latest_gps_for_video(void) { + return load_gps(); +} + +int gps_buffer_init(const char* host) { + if (g_running) return 0; + + g_running = 1; + clear_gps(); + pthread_attr_t attr; + pthread_attr_init(&attr); + pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED); + // 创建后台线程 + if (pthread_create(&g_gps_thread, &attr, gps_update_thread, (void*)host) != 0) { + g_running = 0; + pthread_attr_destroy(&attr); // 清理属性 + perror("无法创建 GPS 线程"); + return -1; + } + pthread_attr_destroy(&attr); // 清理属性 + return 0; +} + +void gps_buffer_cleanup(void) { + g_running = 0; + // 等待线程结束 + + usleep(10000); // 等待 100ms 让后台线程有机会处理退出标志 +} + + +//gcc main.c video_pipeline_run.c gps_buffer.c -lpthread -lm -o my_app 请确保在编译命令中链接 pthread 和 m (math) 库 diff --git a/host/OmniSocketGo_add_camera/src/interactive.c b/host/OmniSocketGo_add_camera/src/interactive.c new file mode 100644 index 0000000..14f5a89 --- /dev/null +++ b/host/OmniSocketGo_add_camera/src/interactive.c @@ -0,0 +1,77 @@ +#include "interactive.h" + +#include + +static void interactive_skip_spaces(const char **cursor) { + while (**cursor != '\0' && isspace((unsigned char) **cursor)) { + (*cursor)++; + } +} + +int interactive_parse_command(const char *line, interactive_command_t *command, char *err, size_t err_len) { + const char *cursor = line; + char action[16]; + size_t action_len = 0; + size_t to_len = 0; + size_t value_len; + + if (line == NULL || command == NULL) { + snprintf(err, err_len, "interactive: invalid command"); + return -1; + } + memset(command, 0, sizeof(*command)); + interactive_skip_spaces(&cursor); + while (*cursor != '\0' && !isspace((unsigned char) *cursor) && action_len + 1 < sizeof(action)) { + action[action_len++] = *cursor++; + } + action[action_len] = '\0'; + if (action_len == 0) { + snprintf(err, err_len, "interactive: empty command"); + return -1; + } + if (strcmp(action, "help") == 0) { + command->type = INTERACTIVE_CMD_HELP; + return 0; + } + if (strcmp(action, "quit") == 0) { + command->type = INTERACTIVE_CMD_QUIT; + return 0; + } + + interactive_skip_spaces(&cursor); + while (*cursor != '\0' && !isspace((unsigned char) *cursor) && to_len + 1 < sizeof(command->to)) { + command->to[to_len++] = *cursor++; + } + command->to[to_len] = '\0'; + interactive_skip_spaces(&cursor); + if (command->to[0] == '\0' || *cursor == '\0') { + snprintf(err, err_len, "interactive: missing target or value"); + return -1; + } + + value_len = strlen(cursor); + if (value_len >= sizeof(command->value)) { + snprintf(err, err_len, "interactive: value too long"); + return -1; + } + snprintf(command->value, sizeof(command->value), "%s", cursor); + + if (strcmp(action, "text") == 0) { + command->type = INTERACTIVE_CMD_TEXT; + return 0; + } + if (strcmp(action, "file") == 0) { + command->type = INTERACTIVE_CMD_FILE; + return 0; + } + snprintf(err, err_len, "interactive: unknown command %s", action); + return -1; +} + +void interactive_print_help(FILE *out, const char *transport_name) { + fprintf(out, "interactive mode commands (%s):\n", transport_name); + fprintf(out, " help show this help\n"); + fprintf(out, " text send one text message\n"); + fprintf(out, " file send one file\n"); + fprintf(out, " quit exit this process\n"); +} diff --git a/host/OmniSocketGo_add_camera/src/kcp_packet_debug.c b/host/OmniSocketGo_add_camera/src/kcp_packet_debug.c new file mode 100644 index 0000000..87540f8 --- /dev/null +++ b/host/OmniSocketGo_add_camera/src/kcp_packet_debug.c @@ -0,0 +1,166 @@ +#include "kcp_packet_debug.h" + +kcp_packet_debug_logger_t *kcp_packet_debug_open_jsonl(const char *path) { + kcp_packet_debug_logger_t *logger; + FILE *file; + if (path == NULL || path[0] == '\0') { + return NULL; + } + if (omni_ensure_parent_dir(path) != 0) { + return NULL; + } + file = fopen(path, "ab"); + if (file == NULL) { + return NULL; + } + logger = (kcp_packet_debug_logger_t *) calloc(1, sizeof(*logger)); + if (logger == NULL) { + fclose(file); + return NULL; + } + omni_file_logger_init_path(&logger->file_logger, file, path, 0); + logger->enabled = 1; + return logger; +} + +void kcp_packet_debug_close(kcp_packet_debug_logger_t *logger) { + if (logger == NULL) { + return; + } + if (logger->file_logger.file != NULL) { + fclose(logger->file_logger.file); + } + omni_file_logger_destroy(&logger->file_logger); + free(logger); +} + +void kcp_packet_debug_record_clear(kcp_packet_debug_record_t *record) { + if (record == NULL) { + return; + } + free(record->segments); + memset(record, 0, sizeof(*record)); +} + +int kcp_packet_debug_log(kcp_packet_debug_logger_t *logger, const kcp_packet_debug_record_t *record) { + char *event = NULL; + char *node_role = NULL; + char *node_id = NULL; + char *local_addr = NULL; + char *remote_addr = NULL; + char *segments_json = NULL; + char *tx_id_text = NULL; + char *conv_text = NULL; + char *line = NULL; + size_t i; + size_t cap = 128U; + size_t len = 0U; + + if (logger == NULL || record == NULL || !logger->enabled) { + return 0; + } + + event = omni_json_escape(record->event); + node_role = omni_json_escape(record->node_role); + node_id = omni_json_escape(record->node_id); + local_addr = omni_json_escape(record->local_addr); + remote_addr = omni_json_escape(record->remote_addr); + if (event == NULL || node_role == NULL || node_id == NULL || local_addr == NULL || remote_addr == NULL) { + free(event); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + return -1; + } + + segments_json = (char *) malloc(cap); + if (segments_json == NULL) { + free(event); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + return -1; + } + segments_json[len++] = '['; + for (i = 0; i < record->segment_count; ++i) { + int written; + while (len + 96U > cap) { + char *next = (char *) realloc(segments_json, cap * 2U); + if (next == NULL) { + free(event); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + free(segments_json); + return -1; + } + segments_json = next; + cap *= 2U; + } + written = snprintf( + segments_json + len, + cap - len, + "%s{\"cmd\":%u,\"sn\":%u,\"una\":%u,\"frg\":%u,\"wnd\":%u,\"len\":%u}", + i == 0 ? "" : ",", + record->segments[i].cmd, + record->segments[i].sn, + record->segments[i].una, + record->segments[i].frg, + record->segments[i].wnd, + record->segments[i].len + ); + len += (size_t) written; + } + segments_json[len++] = ']'; + segments_json[len] = '\0'; + + tx_id_text = record->has_udp_tx_id ? omni_strdup_printf("%u", record->udp_tx_id) : omni_strdup("null"); + conv_text = record->has_kcp_conv ? omni_strdup_printf("%u", record->kcp_conv) : omni_strdup("null"); + if (tx_id_text == NULL || conv_text == NULL) { + free(event); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + free(segments_json); + free(tx_id_text); + free(conv_text); + return -1; + } + + line = omni_strdup_printf( + "{\"event\":\"%s\",\"node_role\":\"%s\",\"node_id\":\"%s\",\"local_addr\":\"%s\",\"remote_addr\":\"%s\",\"packet_bytes\":%d,\"udp_tx_id\":%s,\"kcp_conv\":%s,\"segments\":%s,\"ts_unix_nano\":%" PRId64 "}", + event, + node_role, + node_id, + local_addr, + remote_addr, + record->packet_bytes, + tx_id_text, + conv_text, + segments_json, + record->ts_unix_nano + ); + + free(event); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + free(segments_json); + free(tx_id_text); + free(conv_text); + + if (line == NULL) { + return -1; + } + if (omni_file_logger_write_line(&logger->file_logger, line) != 0) { + free(line); + return -1; + } + free(line); + return 0; +} diff --git a/host/OmniSocketGo_add_camera/src/kcp_session_stats.c b/host/OmniSocketGo_add_camera/src/kcp_session_stats.c new file mode 100644 index 0000000..c0b8ef8 --- /dev/null +++ b/host/OmniSocketGo_add_camera/src/kcp_session_stats.c @@ -0,0 +1,286 @@ +#include "kcp_session_stats.h" + +static int kcp_session_stats_append(char **line, size_t *len, const char *suffix) { + size_t suffix_len; + char *next; + + if (line == NULL || len == NULL || suffix == NULL) { + errno = EINVAL; + return -1; + } + suffix_len = strlen(suffix); + next = (char *) realloc(*line, *len + suffix_len + 1U); + if (next == NULL) { + return -1; + } + memcpy(next + *len, suffix, suffix_len + 1U); + *line = next; + *len += suffix_len; + return 0; +} + +static int kcp_session_stats_appendf(char **line, size_t *len, const char *fmt, ...) { + va_list args; + va_list copy; + int needed; + char *buffer; + + if (line == NULL || len == NULL || fmt == NULL) { + errno = EINVAL; + return -1; + } + + va_start(args, fmt); + va_copy(copy, args); + needed = vsnprintf(NULL, 0, fmt, copy); + va_end(copy); + if (needed < 0) { + va_end(args); + return -1; + } + + buffer = (char *) malloc((size_t) needed + 1U); + if (buffer == NULL) { + va_end(args); + return -1; + } + vsnprintf(buffer, (size_t) needed + 1U, fmt, args); + va_end(args); + + if (kcp_session_stats_append(line, len, buffer) != 0) { + free(buffer); + return -1; + } + free(buffer); + return 0; +} + +kcp_session_stats_logger_t *kcp_session_stats_open_jsonl(const char *path) { + kcp_session_stats_logger_t *logger; + FILE *file; + if (path == NULL || path[0] == '\0') { + return NULL; + } + if (omni_ensure_parent_dir(path) != 0) { + return NULL; + } + file = fopen(path, "ab"); + if (file == NULL) { + return NULL; + } + logger = (kcp_session_stats_logger_t *) calloc(1, sizeof(*logger)); + if (logger == NULL) { + fclose(file); + return NULL; + } + omni_file_logger_init_path(&logger->file_logger, file, path, 0); + logger->enabled = 1; + return logger; +} + +void kcp_session_stats_close(kcp_session_stats_logger_t *logger) { + if (logger == NULL) { + return; + } + if (logger->file_logger.file != NULL) { + fclose(logger->file_logger.file); + } + omni_file_logger_destroy(&logger->file_logger); + free(logger); +} + +int kcp_session_stats_log(kcp_session_stats_logger_t *logger, const kcp_session_stats_record_t *record) { + char *record_type = NULL; + char *node_role = NULL; + char *node_id = NULL; + char *local_addr = NULL; + char *remote_addr = NULL; + char *sample_reason = NULL; + char *line = NULL; + size_t line_len = 0; + + if (logger == NULL || record == NULL || !logger->enabled) { + return 0; + } + record_type = omni_json_escape(record->record_type); + node_role = omni_json_escape(record->node_role); + node_id = omni_json_escape(record->node_id); + local_addr = omni_json_escape(record->local_addr); + remote_addr = omni_json_escape(record->remote_addr); + sample_reason = omni_json_escape(record->sample_reason); + if (record_type == NULL || node_role == NULL || node_id == NULL || local_addr == NULL || remote_addr == NULL || sample_reason == NULL) { + free(record_type); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + free(sample_reason); + return -1; + } + line = omni_strdup(""); + if (line == NULL) { + free(record_type); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + free(sample_reason); + return -1; + } + + if (kcp_session_stats_appendf(&line, &line_len, "{\"record_type\":\"%s\",\"node_role\":\"%s\",\"node_id\":\"%s\",\"ts_unix_nano\":%" PRId64 ",\"sample_reason\":\"%s\"", + record_type, + node_role, + node_id, + record->ts_unix_nano, + sample_reason) != 0) { + goto cleanup; + } + if (record->local_addr[0] != '\0' && + kcp_session_stats_appendf(&line, &line_len, ",\"local_addr\":\"%s\"", local_addr) != 0) { + goto cleanup; + } + if (record->remote_addr[0] != '\0' && + kcp_session_stats_appendf(&line, &line_len, ",\"remote_addr\":\"%s\"", remote_addr) != 0) { + goto cleanup; + } + if (record->has_conv && + kcp_session_stats_appendf(&line, &line_len, ",\"conv\":%u", record->conv) != 0) { + goto cleanup; + } + if (record->has_rto_ms && + kcp_session_stats_appendf(&line, &line_len, ",\"rto_ms\":%u", record->rto_ms) != 0) { + goto cleanup; + } + if (record->has_srtt_ms && + kcp_session_stats_appendf(&line, &line_len, ",\"srtt_ms\":%d", record->srtt_ms) != 0) { + goto cleanup; + } + if (record->has_min_srtt_ms && + kcp_session_stats_appendf(&line, &line_len, ",\"min_srtt_ms\":%d", record->min_srtt_ms) != 0) { + goto cleanup; + } + if (record->has_srttvar_ms && + kcp_session_stats_appendf(&line, &line_len, ",\"srttvar_ms\":%d", record->srttvar_ms) != 0) { + goto cleanup; + } + if (record->has_last_feedback_age_ms && + kcp_session_stats_appendf(&line, &line_len, ",\"last_feedback_age_ms\":%u", record->last_feedback_age_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; + } + if (record->has_bytes_received && + kcp_session_stats_appendf(&line, &line_len, ",\"bytes_received\":%" PRIu64, record->bytes_received) != 0) { + goto cleanup; + } + if (record->has_in_pkts && + kcp_session_stats_appendf(&line, &line_len, ",\"in_pkts\":%" PRIu64, record->in_pkts) != 0) { + goto cleanup; + } + if (record->has_out_pkts && + kcp_session_stats_appendf(&line, &line_len, ",\"out_pkts\":%" PRIu64, record->out_pkts) != 0) { + goto cleanup; + } + if (record->has_in_segs && + kcp_session_stats_appendf(&line, &line_len, ",\"in_segs\":%" PRIu64, record->in_segs) != 0) { + goto cleanup; + } + if (record->has_out_segs && + kcp_session_stats_appendf(&line, &line_len, ",\"out_segs\":%" PRIu64, record->out_segs) != 0) { + goto cleanup; + } + if (record->has_retrans_segs && + kcp_session_stats_appendf(&line, &line_len, ",\"retrans_segs\":%" PRIu64, record->retrans_segs) != 0) { + goto cleanup; + } + if (record->has_fast_retrans_segs && + kcp_session_stats_appendf(&line, &line_len, ",\"fast_retrans_segs\":%" PRIu64, record->fast_retrans_segs) != 0) { + goto cleanup; + } + if (record->has_early_retrans_segs && + kcp_session_stats_appendf(&line, &line_len, ",\"early_retrans_segs\":%" PRIu64, record->early_retrans_segs) != 0) { + goto cleanup; + } + if (record->has_lost_segs && + kcp_session_stats_appendf(&line, &line_len, ",\"lost_segs\":%" PRIu64, record->lost_segs) != 0) { + goto cleanup; + } + if (record->has_repeat_segs && + kcp_session_stats_appendf(&line, &line_len, ",\"repeat_segs\":%" PRIu64, record->repeat_segs) != 0) { + goto cleanup; + } + if (record->has_in_errs && + kcp_session_stats_appendf(&line, &line_len, ",\"in_errs\":%" PRIu64, record->in_errs) != 0) { + goto cleanup; + } + if (record->has_kcp_in_errs && + kcp_session_stats_appendf(&line, &line_len, ",\"kcp_in_errs\":%" PRIu64, record->kcp_in_errs) != 0) { + goto cleanup; + } + if (record->has_ring_buffer_snd_queue && + kcp_session_stats_appendf(&line, &line_len, ",\"ring_buffer_snd_queue\":%" PRIu64, record->ring_buffer_snd_queue) != 0) { + goto cleanup; + } + if (record->has_ring_buffer_rcv_queue && + kcp_session_stats_appendf(&line, &line_len, ",\"ring_buffer_rcv_queue\":%" PRIu64, record->ring_buffer_rcv_queue) != 0) { + goto cleanup; + } + if (record->has_ring_buffer_snd_buffer && + kcp_session_stats_appendf(&line, &line_len, ",\"ring_buffer_snd_buffer\":%" PRIu64, record->ring_buffer_snd_buffer) != 0) { + goto cleanup; + } + if (record->has_curr_estab && + kcp_session_stats_appendf(&line, &line_len, ",\"curr_estab\":%" PRIu64, record->curr_estab) != 0) { + goto cleanup; + } + if (kcp_session_stats_append(&line, &line_len, "}") != 0) { + goto cleanup; + } + + free(record_type); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + free(sample_reason); + + if (omni_file_logger_write_line(&logger->file_logger, line) != 0) { + free(line); + return -1; + } + free(line); + return 0; + +cleanup: + free(record_type); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + free(sample_reason); + free(line); + return -1; +} diff --git a/host/OmniSocketGo_add_camera/src/latencylog.c b/host/OmniSocketGo_add_camera/src/latencylog.c new file mode 100644 index 0000000..3a3314e --- /dev/null +++ b/host/OmniSocketGo_add_camera/src/latencylog.c @@ -0,0 +1,130 @@ +#include "latencylog.h" + +static void latencylog_fill_event(latency_event_t *event, const char *node_role, const char *node_id, const char *event_name, int64_t ts_unix_nano, const message_t *msg) { + memset(event, 0, sizeof(*event)); + event->ts_unix_nano = ts_unix_nano; + snprintf(event->node_role, sizeof(event->node_role), "%s", node_role == NULL ? "" : node_role); + snprintf(event->node_id, sizeof(event->node_id), "%s", node_id == NULL ? "" : node_id); + snprintf(event->event, sizeof(event->event), "%s", event_name == NULL ? "" : event_name); + event->message_type = msg->type; + event->message_id = msg->id; + snprintf(event->from, sizeof(event->from), "%s", msg->from); + snprintf(event->to, sizeof(event->to), "%s", msg->to); + snprintf(event->file_name, sizeof(event->file_name), "%s", msg->file_name); + event->body_size = (int) msg->body_len; +} + +latency_logger_t *latencylog_open_jsonl(const char *path) { + latency_logger_t *logger; + FILE *file; + if (path == NULL || path[0] == '\0') { + return NULL; + } + if (omni_ensure_parent_dir(path) != 0) { + return NULL; + } + file = fopen(path, "ab"); + if (file == NULL) { + return NULL; + } + logger = (latency_logger_t *) calloc(1, sizeof(*logger)); + if (logger == NULL) { + fclose(file); + return NULL; + } + omni_file_logger_init_path(&logger->file_logger, file, path, 0); + logger->enabled = 1; + return logger; +} + +void latencylog_close(latency_logger_t *logger) { + if (logger == NULL) { + return; + } + if (logger->file_logger.file != NULL) { + fclose(logger->file_logger.file); + } + omni_file_logger_destroy(&logger->file_logger); + free(logger); +} + +int latencylog_log_event(latency_logger_t *logger, const latency_event_t *event) { + char *node_role = NULL; + char *node_id = NULL; + char *event_name = NULL; + char *from = NULL; + char *to = NULL; + char *file_name = NULL; + char *line = NULL; + + if (logger == NULL || event == NULL || !logger->enabled) { + return 0; + } + + node_role = omni_json_escape(event->node_role); + node_id = omni_json_escape(event->node_id); + event_name = omni_json_escape(event->event); + from = omni_json_escape(event->from); + to = omni_json_escape(event->to); + file_name = omni_json_escape(event->file_name); + if (node_role == NULL || node_id == NULL || event_name == NULL || from == NULL || to == NULL || file_name == NULL) { + free(node_role); + free(node_id); + free(event_name); + free(from); + free(to); + free(file_name); + return -1; + } + + line = omni_strdup_printf( + "{\"ts_unix_nano\":%" PRId64 ",\"node_role\":\"%s\",\"node_id\":\"%s\",\"event\":\"%s\",\"message_type\":\"%s\",\"message_id\":%" PRIu64 ",\"from\":\"%s\",\"to\":\"%s\",\"file_name\":\"%s\",\"body_size\":%d}", + event->ts_unix_nano, + node_role, + node_id, + event_name, + protocol_message_type_name(event->message_type), + event->message_id, + from, + to, + file_name, + event->body_size + ); + + free(node_role); + free(node_id); + free(event_name); + free(from); + free(to); + free(file_name); + + if (line == NULL) { + return -1; + } + if (omni_file_logger_write_line(&logger->file_logger, line) != 0) { + free(line); + return -1; + } + free(line); + return 0; +} + +int latencylog_is_business_message(const message_t *msg) { + if (msg == NULL) { + return 0; + } + return msg->type == MSG_TYPE_TEXT || msg->type == MSG_TYPE_FILE || msg->type == MSG_TYPE_BINARY; +} + +void latencylog_log_message_event(latency_logger_t *logger, const char *node_role, const char *node_id, const char *event_name, const message_t *msg) { + latencylog_log_message_event_at(logger, node_role, node_id, event_name, omni_now_unix_nano(), msg); +} + +void latencylog_log_message_event_at(latency_logger_t *logger, const char *node_role, const char *node_id, const char *event_name, int64_t ts_unix_nano, const message_t *msg) { + latency_event_t event; + if (!latencylog_is_business_message(msg)) { + return; + } + latencylog_fill_event(&event, node_role, node_id, event_name, ts_unix_nano, msg); + (void) latencylog_log_event(logger, &event); +} diff --git a/host/OmniSocketGo_add_camera/src/linux_timestamping.c b/host/OmniSocketGo_add_camera/src/linux_timestamping.c new file mode 100644 index 0000000..0a5c910 --- /dev/null +++ b/host/OmniSocketGo_add_camera/src/linux_timestamping.c @@ -0,0 +1,103 @@ +#include "linux_timestamping.h" +#include "latencylog.h" + +#ifdef __linux__ +#include +#include +#include +#include + +static int64_t linux_timespec_to_ns(const struct timespec *ts) { + if (ts == NULL) { + return 0; + } + return (int64_t) ts->tv_sec * 1000000000LL + ts->tv_nsec; +} + +int linux_timestamping_enable_udp_socket(int fd, int enable_rx) { + int flags = SOF_TIMESTAMPING_SOFTWARE | SOF_TIMESTAMPING_TX_SCHED | SOF_TIMESTAMPING_TX_SOFTWARE | SOF_TIMESTAMPING_OPT_ID | SOF_TIMESTAMPING_OPT_TSONLY; + if (enable_rx) { + flags |= SOF_TIMESTAMPING_RX_SOFTWARE; + } + return setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING, &flags, sizeof(flags)); +} + +int64_t linux_timestamping_parse_rx_timestamp(const struct msghdr *msg) { + struct cmsghdr *cmsg; + const struct scm_timestamping *timestamps; + if (msg == NULL) { + return 0; + } + for (cmsg = CMSG_FIRSTHDR((struct msghdr *) msg); cmsg != NULL; cmsg = CMSG_NXTHDR((struct msghdr *) msg, cmsg)) { + if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_TIMESTAMPING) { + timestamps = (const struct scm_timestamping *) CMSG_DATA(cmsg); + if (timestamps->ts[0].tv_sec != 0 || timestamps->ts[0].tv_nsec != 0) { + return linux_timespec_to_ns(×tamps->ts[0]); + } + } + } + return 0; +} + +int linux_timestamping_parse_tx_timestamp(const struct msghdr *msg, omni_tx_timestamp_event_t *out_event) { + struct cmsghdr *cmsg; + const struct scm_timestamping *timestamps = NULL; + const struct sock_extended_err *sock_err = NULL; + int64_t timestamp_ns = 0; + + if (msg == NULL || out_event == NULL) { + errno = EINVAL; + return -1; + } + memset(out_event, 0, sizeof(*out_event)); + + for (cmsg = CMSG_FIRSTHDR((struct msghdr *) msg); cmsg != NULL; cmsg = CMSG_NXTHDR((struct msghdr *) msg, cmsg)) { + if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_TIMESTAMPING) { + timestamps = (const struct scm_timestamping *) CMSG_DATA(cmsg); + } else if ((cmsg->cmsg_level == SOL_IP && cmsg->cmsg_type == IP_RECVERR) || + (cmsg->cmsg_level == SOL_IPV6 && cmsg->cmsg_type == IPV6_RECVERR)) { + sock_err = (const struct sock_extended_err *) CMSG_DATA(cmsg); + } + } + if (timestamps == NULL || sock_err == NULL) { + errno = EAGAIN; + return -1; + } + if (timestamps->ts[0].tv_sec != 0 || timestamps->ts[0].tv_nsec != 0) { + timestamp_ns = linux_timespec_to_ns(×tamps->ts[0]); + snprintf(out_event->event_name, sizeof(out_event->event_name), "%s", EVENT_A_TX_SOFTWARE); + } else if (timestamps->ts[1].tv_sec != 0 || timestamps->ts[1].tv_nsec != 0) { + timestamp_ns = linux_timespec_to_ns(×tamps->ts[1]); + snprintf(out_event->event_name, sizeof(out_event->event_name), "%s", EVENT_A_TX_SCHED); + } else { + errno = EAGAIN; + return -1; + } + out_event->ts_unix_nano = timestamp_ns; + out_event->ee_info = sock_err->ee_info; + out_event->ee_data = sock_err->ee_data; + return 0; +} + +#else + +int linux_timestamping_enable_udp_socket(int fd, int enable_rx) { + (void) fd; + (void) enable_rx; + errno = ENOTSUP; + return -1; +} + +int64_t linux_timestamping_parse_rx_timestamp(const struct msghdr *msg) { + (void) msg; + return 0; +} + +int linux_timestamping_parse_tx_timestamp(const struct msghdr *msg, omni_tx_timestamp_event_t *out_event) { + (void) msg; + (void) out_event; + errno = ENOTSUP; + return -1; +} + +#endif diff --git a/host/OmniSocketGo_add_camera/src/omni_common.c b/host/OmniSocketGo_add_camera/src/omni_common.c new file mode 100644 index 0000000..fe2b1a8 --- /dev/null +++ b/host/OmniSocketGo_add_camera/src/omni_common.c @@ -0,0 +1,795 @@ +#include "omni_common.h" + +#include +#include +#include +#include +#include + +int64_t omni_now_unix_nano(void) { + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + return (int64_t) ts.tv_sec * 1000000000LL + ts.tv_nsec; +} + +uint32_t omni_now_millis32(void) { + struct timespec ts; + uint64_t ms; + clock_gettime(CLOCK_MONOTONIC, &ts); + ms = (uint64_t) ts.tv_sec * 1000ULL + (uint64_t) (ts.tv_nsec / 1000000L); + return (uint32_t) (ms & 0xffffffffu); +} + +int omni_set_nonblocking(int fd, int enabled) { + int flags = fcntl(fd, F_GETFL, 0); + if (flags < 0) { + return -1; + } + if (enabled) { + flags |= O_NONBLOCK; + } else { + flags &= ~O_NONBLOCK; + } + return fcntl(fd, F_SETFL, flags); +} + +int omni_parse_sockaddr(const char *raw, int passive, struct sockaddr_storage *addr, socklen_t *addr_len, int *family_out) { + struct addrinfo hints; + struct addrinfo *result = NULL; + char host_copy[OMNI_MAX_ADDR_TEXT]; + char port_copy[32]; + const char *host = NULL; + const char *service = NULL; + const char *last_colon; + size_t host_len; + + if (raw == NULL || addr == NULL || addr_len == NULL) { + errno = EINVAL; + return -1; + } + + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_DGRAM; + hints.ai_flags = passive ? AI_PASSIVE : 0; + + last_colon = strrchr(raw, ':'); + if (last_colon == NULL) { + host = passive ? NULL : raw; + service = passive ? raw : "0"; + } else { + host_len = (size_t) (last_colon - raw); + if (host_len >= sizeof(host_copy)) { + errno = ENAMETOOLONG; + return -1; + } + memcpy(host_copy, raw, host_len); + host_copy[host_len] = '\0'; + snprintf(port_copy, sizeof(port_copy), "%s", last_colon + 1); + host = host_len == 0 ? NULL : host_copy; + service = port_copy; + } + + if (getaddrinfo(host, service, &hints, &result) != 0 || result == NULL) { + errno = EINVAL; + return -1; + } + memcpy(addr, result->ai_addr, result->ai_addrlen); + *addr_len = (socklen_t) result->ai_addrlen; + if (family_out != NULL) { + *family_out = result->ai_family; + } + freeaddrinfo(result); + return 0; +} + +int omni_clone_sockaddr(const struct sockaddr *src, socklen_t src_len, struct sockaddr_storage *dst, socklen_t *dst_len) { + if (src == NULL || dst == NULL || dst_len == NULL || src_len > sizeof(*dst)) { + errno = EINVAL; + return -1; + } + memset(dst, 0, sizeof(*dst)); + memcpy(dst, src, src_len); + *dst_len = src_len; + return 0; +} + +const char *omni_sockaddr_to_string(const struct sockaddr *addr, socklen_t addr_len, char *buffer, size_t buffer_len) { + char host[NI_MAXHOST]; + char service[NI_MAXSERV]; + + if (buffer == NULL || buffer_len == 0) { + return ""; + } + if (addr == NULL) { + snprintf(buffer, buffer_len, ""); + return buffer; + } + if (getnameinfo(addr, addr_len, host, sizeof(host), service, sizeof(service), NI_NUMERICHOST | NI_NUMERICSERV) != 0) { + snprintf(buffer, buffer_len, ""); + return buffer; + } + if (addr->sa_family == AF_INET6) { + snprintf(buffer, buffer_len, "[%s]:%s", host, service); + } else { + snprintf(buffer, buffer_len, "%s:%s", host, service); + } + return buffer; +} + +int omni_bind_device(int fd, const char *device) { +#ifdef __linux__ + if (device == NULL || device[0] == '\0') { + return 0; + } + return setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE, device, (socklen_t) strlen(device)); +#else + (void) fd; + (void) device; + errno = ENOTSUP; + return -1; +#endif +} + +static int omni_mkdir_single(const char *path) { + if (mkdir(path, 0755) == 0 || errno == EEXIST) { + return 0; + } + return -1; +} + +int omni_ensure_dir(const char *path) { + char tmp[PATH_MAX]; + size_t i; + + if (path == NULL || path[0] == '\0') { + return 0; + } + if (strlen(path) >= sizeof(tmp)) { + errno = ENAMETOOLONG; + return -1; + } + snprintf(tmp, sizeof(tmp), "%s", path); + for (i = 1; tmp[i] != '\0'; ++i) { + if (tmp[i] == '/') { + tmp[i] = '\0'; + if (tmp[0] != '\0' && omni_mkdir_single(tmp) != 0) { + return -1; + } + tmp[i] = '/'; + } + } + return omni_mkdir_single(tmp); +} + +int omni_ensure_parent_dir(const char *path) { + char tmp[PATH_MAX]; + char *slash; + + if (path == NULL || path[0] == '\0') { + return 0; + } + if (strlen(path) >= sizeof(tmp)) { + errno = ENAMETOOLONG; + return -1; + } + snprintf(tmp, sizeof(tmp), "%s", path); + slash = strrchr(tmp, '/'); + if (slash == NULL) { + return 0; + } + if (slash == tmp) { + return omni_mkdir_single("/"); + } + *slash = '\0'; + return omni_ensure_dir(tmp); +} + +int omni_read_file(const char *path, uint8_t **out, size_t *out_len) { + FILE *file; + long size; + uint8_t *buffer; + if (out == NULL || out_len == NULL) { + errno = EINVAL; + return -1; + } + *out = NULL; + *out_len = 0; + file = fopen(path, "rb"); + if (file == NULL) { + return -1; + } + if (fseek(file, 0, SEEK_END) != 0) { + fclose(file); + return -1; + } + size = ftell(file); + if (size < 0) { + fclose(file); + return -1; + } + if (fseek(file, 0, SEEK_SET) != 0) { + fclose(file); + return -1; + } + buffer = (uint8_t *) malloc((size_t) size); + if (size > 0 && buffer == NULL) { + fclose(file); + errno = ENOMEM; + return -1; + } + if ((size_t) size > 0 && fread(buffer, 1, (size_t) size, file) != (size_t) size) { + free(buffer); + fclose(file); + errno = EIO; + return -1; + } + fclose(file); + *out = buffer; + *out_len = (size_t) size; + return 0; +} + +int omni_write_full_fd(int fd, const uint8_t *data, size_t len) { + ssize_t written; + while (len > 0) { + written = write(fd, data, len); + if (written < 0) { + if (errno == EINTR) { + continue; + } + return -1; + } + if (written == 0) { + errno = EIO; + return -1; + } + data += written; + len -= (size_t) written; + } + return 0; +} + +static int omni_write_file_internal(const char *path, const uint8_t *data, size_t len, const char *mode) { + FILE *file; + if (omni_ensure_parent_dir(path) != 0) { + return -1; + } + file = fopen(path, mode); + if (file == NULL) { + return -1; + } + if (len > 0 && fwrite(data, 1, len, file) != len) { + fclose(file); + errno = EIO; + return -1; + } + if (fclose(file) != 0) { + return -1; + } + return 0; +} + +int omni_append_file(const char *path, const uint8_t *data, size_t len) { + return omni_write_file_internal(path, data, len, "ab"); +} + +int omni_write_file(const char *path, const uint8_t *data, size_t len) { + return omni_write_file_internal(path, data, len, "wb"); +} + +int omni_random_u32(uint32_t *out) { + uint8_t *cursor; + size_t remaining; + int fd; + + if (out == NULL) { + errno = EINVAL; + return -1; + } + + fd = open("/dev/urandom", O_RDONLY); + if (fd < 0) { + return -1; + } + + cursor = (uint8_t *) out; + remaining = sizeof(*out); + while (remaining > 0) { + ssize_t n = read(fd, cursor, remaining); + if (n < 0) { + if (errno == EINTR) { + continue; + } + close(fd); + return -1; + } + if (n == 0) { + close(fd); + errno = EIO; + return -1; + } + cursor += n; + remaining -= (size_t) n; + } + close(fd); + + if (*out == 0) { + *out = 1; + } + return 0; +} + +char *omni_strdup(const char *src) { + size_t len; + char *dst; + if (src == NULL) { + return NULL; + } + len = strlen(src); + dst = (char *) malloc(len + 1U); + if (dst == NULL) { + return NULL; + } + memcpy(dst, src, len + 1U); + return dst; +} + +char *omni_strdup_printf(const char *fmt, ...) { + va_list args; + va_list copy; + int needed; + char *buffer; + va_start(args, fmt); + va_copy(copy, args); + needed = vsnprintf(NULL, 0, fmt, copy); + va_end(copy); + if (needed < 0) { + va_end(args); + return NULL; + } + buffer = (char *) malloc((size_t) needed + 1U); + if (buffer == NULL) { + va_end(args); + return NULL; + } + vsnprintf(buffer, (size_t) needed + 1U, fmt, args); + va_end(args); + return buffer; +} + +char *omni_json_escape_bytes(const uint8_t *src, size_t len) { + size_t i; + size_t out_len = 0; + char *out; + char *cursor; + + if (src == NULL) { + if (len == 0) { + return omni_strdup(""); + } + errno = EINVAL; + return NULL; + } + + for (i = 0; i < len; ++i) { + switch (src[i]) { + case '\\': + case '"': + case '\b': + case '\f': + case '\n': + case '\r': + case '\t': + out_len += 2; + break; + default: + out_len += src[i] < 0x20 ? 6U : 1U; + break; + } + } + out = (char *) malloc(out_len + 1U); + if (out == NULL) { + return NULL; + } + cursor = out; + for (i = 0; i < len; ++i) { + switch (src[i]) { + case '\\': + *cursor++ = '\\'; + *cursor++ = '\\'; + break; + case '"': + *cursor++ = '\\'; + *cursor++ = '"'; + break; + case '\b': + *cursor++ = '\\'; + *cursor++ = 'b'; + break; + case '\f': + *cursor++ = '\\'; + *cursor++ = 'f'; + break; + case '\n': + *cursor++ = '\\'; + *cursor++ = 'n'; + break; + case '\r': + *cursor++ = '\\'; + *cursor++ = 'r'; + break; + case '\t': + *cursor++ = '\\'; + *cursor++ = 't'; + break; + default: + if (src[i] < 0x20) { + snprintf(cursor, 7, "\\u%04x", src[i]); + cursor += 6; + } else { + *cursor++ = (char) src[i]; + } + break; + } + } + *cursor = '\0'; + return out; +} + +char *omni_json_escape(const char *src) { + if (src == NULL) { + return omni_strdup(""); + } + return omni_json_escape_bytes((const uint8_t *) src, strlen(src)); +} + +int omni_utf8_valid(const uint8_t *data, size_t len) { + size_t i = 0; + uint8_t c; + while (i < len) { + c = data[i]; + if (c <= 0x7f) { + i++; + continue; + } + if ((c & 0xe0) == 0xc0) { + if (i + 1 >= len || (data[i + 1] & 0xc0) != 0x80 || c < 0xc2) { + return 0; + } + i += 2; + continue; + } + if ((c & 0xf0) == 0xe0) { + if (i + 2 >= len || (data[i + 1] & 0xc0) != 0x80 || (data[i + 2] & 0xc0) != 0x80) { + return 0; + } + if (c == 0xe0 && data[i + 1] < 0xa0) { + return 0; + } + if (c == 0xed && data[i + 1] >= 0xa0) { + return 0; + } + i += 3; + continue; + } + if ((c & 0xf8) == 0xf0) { + if (i + 3 >= len || (data[i + 1] & 0xc0) != 0x80 || (data[i + 2] & 0xc0) != 0x80 || (data[i + 3] & 0xc0) != 0x80) { + return 0; + } + if (c == 0xf0 && data[i + 1] < 0x90) { + return 0; + } + if (c > 0xf4 || (c == 0xf4 && data[i + 1] >= 0x90)) { + return 0; + } + i += 4; + continue; + } + return 0; + } + return 1; +} + +void omni_trim_newline(char *line) { + size_t len; + if (line == NULL) { + return; + } + len = strlen(line); + while (len > 0 && (line[len - 1] == '\n' || line[len - 1] == '\r')) { + line[--len] = '\0'; + } +} + +int omni_parse_duration_ms(const char *raw, int default_ms, int *out_ms) { + char *endptr; + long value; + if (out_ms == NULL) { + errno = EINVAL; + return -1; + } + if (raw == NULL || raw[0] == '\0') { + *out_ms = default_ms; + return 0; + } + value = strtol(raw, &endptr, 10); + if (endptr == raw || value <= 0) { + errno = EINVAL; + return -1; + } + if (*endptr == '\0' || strcmp(endptr, "ms") == 0) { + *out_ms = (int) value; + return 0; + } + if (strcmp(endptr, "s") == 0) { + *out_ms = (int) (value * 1000L); + return 0; + } + errno = EINVAL; + return -1; +} + +double omni_duration_ms_to_ns(double ms) { + return ms * 1000000.0; +} + +const char *omni_path_base_name(const char *path) { + const char *slash; + + if (path == NULL) { + return ""; + } + slash = strrchr(path, '/'); + return slash == NULL ? path : slash + 1; +} + +static uint64_t omni_now_monotonic_ms64(void) { + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + return (uint64_t) ts.tv_sec * 1000ULL + (uint64_t) (ts.tv_nsec / 1000000L); +} + +static int omni_positive_int_env(const char *name, int default_value) { + const char *raw = getenv(name); + long parsed; + char *endptr = NULL; + + if (raw == NULL || raw[0] == '\0') { + return default_value; + } + parsed = strtol(raw, &endptr, 10); + if (endptr == raw || *endptr != '\0' || parsed <= 0) { + return default_value; + } + return (int) parsed; +} + +static size_t omni_positive_size_env(const char *name, size_t default_value) { + const char *raw = getenv(name); + unsigned long long parsed; + char *endptr = NULL; + + if (raw == NULL || raw[0] == '\0') { + return default_value; + } + parsed = strtoull(raw, &endptr, 10); + if (endptr == raw || *endptr != '\0' || parsed == 0ULL) { + return default_value; + } + return (size_t) parsed; +} + +static int omni_file_logger_flush_locked(omni_file_logger_t *logger, uint64_t now_ms) { + if (logger == NULL || logger->file == NULL) { + errno = EINVAL; + return -1; + } + if (fflush(logger->file) != 0) { + return -1; + } + logger->buffered_bytes = 0U; + logger->last_flush_monotonic_ms = now_ms; + return 0; +} + +static int omni_build_rotated_path(char *buffer, size_t buffer_len, const char *path, int suffix) { + size_t path_len; + int written; + + if (buffer == NULL || buffer_len == 0U || path == NULL || path[0] == '\0') { + errno = EINVAL; + return -1; + } + path_len = strlen(path); + if (path_len + 16U >= buffer_len) { + errno = ENAMETOOLONG; + return -1; + } + memcpy(buffer, path, path_len); + written = snprintf(buffer + path_len, buffer_len - path_len, ".%d", suffix); + if (written < 0 || (size_t) written >= buffer_len - path_len) { + errno = ENAMETOOLONG; + return -1; + } + return 0; +} + +static int omni_file_logger_reopen_append_locked(omni_file_logger_t *logger) { + struct stat st; + FILE *file; + + if (logger == NULL || logger->path[0] == '\0') { + errno = EINVAL; + return -1; + } + + file = fopen(logger->path, "ab"); + if (file == NULL) { + return -1; + } + + logger->file = file; + logger->current_bytes = 0U; + if (stat(logger->path, &st) == 0) { + logger->current_bytes = (size_t) st.st_size; + } + logger->buffered_bytes = 0U; + logger->last_flush_monotonic_ms = omni_now_monotonic_ms64(); + return 0; +} + +static int omni_file_logger_recover_after_rotate_locked(omni_file_logger_t *logger, const char *rotated_current_path) { + int reopen_errno; + + if (omni_file_logger_reopen_append_locked(logger) == 0) { + return 0; + } + + reopen_errno = errno; + if (rotated_current_path != NULL && rotated_current_path[0] != '\0') { + if (rename(rotated_current_path, logger->path) == 0) { + if (omni_file_logger_reopen_append_locked(logger) == 0) { + return 0; + } + } + } + + errno = reopen_errno; + return -1; +} + +static int omni_file_logger_rotate_locked(omni_file_logger_t *logger) { + int index; + int saved_errno = 0; + int should_recover = 0; + char rotated_current_path[PATH_MAX]; + char from_path[PATH_MAX]; + char to_path[PATH_MAX]; + + if (logger == NULL || logger->path[0] == '\0' || logger->max_bytes == 0U || logger->max_files <= 0) { + return 0; + } + rotated_current_path[0] = '\0'; + if (logger->file != NULL) { + if (omni_file_logger_flush_locked(logger, omni_now_monotonic_ms64()) != 0) { + return -1; + } + should_recover = 1; + if (fclose(logger->file) != 0) { + logger->file = NULL; + saved_errno = errno; + goto recover; + } + logger->file = NULL; + } + + if (omni_build_rotated_path(from_path, sizeof(from_path), logger->path, logger->max_files) != 0) { + saved_errno = errno; + goto recover; + } + unlink(from_path); + for (index = logger->max_files - 1; index >= 1; --index) { + if (omni_build_rotated_path(from_path, sizeof(from_path), logger->path, index) != 0 || + omni_build_rotated_path(to_path, sizeof(to_path), logger->path, index + 1) != 0) { + saved_errno = errno; + goto recover; + } + if (rename(from_path, to_path) != 0 && errno != ENOENT) { + saved_errno = errno; + goto recover; + } + } + if (omni_build_rotated_path(to_path, sizeof(to_path), logger->path, 1) != 0) { + saved_errno = errno; + goto recover; + } + if (rename(logger->path, to_path) != 0 && errno != ENOENT) { + saved_errno = errno; + goto recover; + } + snprintf(rotated_current_path, sizeof(rotated_current_path), "%s", to_path); + + if (omni_file_logger_reopen_append_locked(logger) != 0) { + saved_errno = errno; + goto recover; + } + return 0; + +recover: + if (should_recover) { + int recover_errno = saved_errno != 0 ? saved_errno : errno; + if (omni_file_logger_recover_after_rotate_locked(logger, rotated_current_path) == 0) { + errno = recover_errno; + } else if (saved_errno != 0) { + errno = saved_errno; + } + } else if (saved_errno != 0) { + errno = saved_errno; + } + return -1; +} + +void omni_file_logger_init(omni_file_logger_t *logger, FILE *file) { + memset(logger, 0, sizeof(*logger)); + logger->file = file; + pthread_mutex_init(&logger->mutex, NULL); + logger->flush_bytes = 1U; + logger->flush_interval_ms = 0; + logger->immediate_flush = 1; + logger->last_flush_monotonic_ms = omni_now_monotonic_ms64(); +} + +void omni_file_logger_init_path(omni_file_logger_t *logger, FILE *file, const char *path, int immediate_flush) { + struct stat st; + + omni_file_logger_init(logger, file); + if (path != NULL && path[0] != '\0') { + snprintf(logger->path, sizeof(logger->path), "%s", path); + if (stat(path, &st) == 0) { + logger->current_bytes = (size_t) st.st_size; + } + } + logger->flush_bytes = omni_positive_size_env("BLITZ_JSONL_FLUSH_BYTES", 262144U); + logger->flush_interval_ms = omni_positive_int_env("BLITZ_JSONL_FLUSH_INTERVAL_MS", 1000); + logger->max_bytes = omni_positive_size_env("BLITZ_JSONL_ROTATE_BYTES", 134217728U); + logger->max_files = omni_positive_int_env("BLITZ_JSONL_ROTATE_FILES", 8); + logger->immediate_flush = immediate_flush != 0; +} + +void omni_file_logger_destroy(omni_file_logger_t *logger) { + pthread_mutex_destroy(&logger->mutex); +} + +int omni_file_logger_write_line(omni_file_logger_t *logger, const char *line) { + int rc = 0; + size_t line_len; + uint64_t now_ms; + if (logger == NULL || logger->file == NULL || line == NULL) { + errno = EINVAL; + return -1; + } + line_len = strlen(line) + 1U; + now_ms = omni_now_monotonic_ms64(); + pthread_mutex_lock(&logger->mutex); + if (fputs(line, logger->file) == EOF || fputc('\n', logger->file) == EOF) { + rc = -1; + } else { + logger->current_bytes += line_len; + logger->buffered_bytes += line_len; + if (logger->immediate_flush || + logger->buffered_bytes >= logger->flush_bytes || + (logger->flush_interval_ms > 0 && now_ms - logger->last_flush_monotonic_ms >= (uint64_t) logger->flush_interval_ms)) { + if (omni_file_logger_flush_locked(logger, now_ms) != 0) { + rc = -1; + } + } + if (rc == 0 && logger->max_bytes > 0U && logger->current_bytes >= logger->max_bytes) { + if (omni_file_logger_rotate_locked(logger) != 0) { + rc = -1; + } + } + } + pthread_mutex_unlock(&logger->mutex); + return rc; +} diff --git a/host/OmniSocketGo_add_camera/src/peer_kcp_client.c b/host/OmniSocketGo_add_camera/src/peer_kcp_client.c new file mode 100644 index 0000000..733f076 --- /dev/null +++ b/host/OmniSocketGo_add_camera/src/peer_kcp_client.c @@ -0,0 +1,675 @@ +#include "peer_kcp_client.h" + +#include +#include +#include +#include + +#define KCP_CLIENT_REGISTER_TIMEOUT_MS 3000 +#define KCP_CLIENT_CTRL_REGISTER_OK "{\"type\":\"server_register_ok\"}" +#define KCP_CLIENT_CTRL_PEER_REPLACED "{\"type\":\"server_peer_replaced\",\"reason\":\"new_instance_wins\"}" +#define KCP_CLIENT_CTRL_HEARTBEAT "{\"type\":\"server_heartbeat\"}" +#define KCP_CLIENT_CTRL_HEARTBEAT_ACK "{\"type\":\"server_heartbeat_ack\"}" + +struct kcp_client { + char id[OMNI_MAX_PEER_ID]; + char server_addr[OMNI_MAX_ADDR_TEXT]; + kcp_conn_t *conn; + latency_logger_t *logger; + pthread_mutex_t state_mu; + uint64_t next_message_id; + int registered; + uint32_t last_server_activity_ms; + char last_server_error[256]; +}; + +static int kcp_client_next_message_id(kcp_client_t *client, uint64_t *out_id) { + if (client == NULL || out_id == NULL) { + errno = EINVAL; + return -1; + } + pthread_mutex_lock(&client->state_mu); + *out_id = ++client->next_message_id; + pthread_mutex_unlock(&client->state_mu); + return 0; +} + +static void kcp_client_set_registered(kcp_client_t *client, int registered) { + if (client == NULL) { + return; + } + pthread_mutex_lock(&client->state_mu); + client->registered = registered != 0; + pthread_mutex_unlock(&client->state_mu); +} + +static void kcp_client_touch_server_activity(kcp_client_t *client) { + if (client == NULL) { + return; + } + pthread_mutex_lock(&client->state_mu); + client->last_server_activity_ms = omni_now_millis32(); + pthread_mutex_unlock(&client->state_mu); +} + +static void kcp_client_set_last_server_error(kcp_client_t *client, const char *message) { + if (client == NULL) { + return; + } + pthread_mutex_lock(&client->state_mu); + snprintf(client->last_server_error, sizeof(client->last_server_error), "%s", message == NULL ? "" : message); + pthread_mutex_unlock(&client->state_mu); +} + +static void kcp_client_clear_last_server_error(kcp_client_t *client) { + kcp_client_set_last_server_error(client, ""); +} + +static int kcp_client_server_error_invalidates_registration(const char *message) { + if (message == NULL || message[0] == '\0') { + return 0; + } + return strstr(message, "not registered") != NULL + || strstr(message, "first message must be register") != NULL + || strstr(message, "peer replaced") != NULL + || strstr(message, "timed out waiting for server_register_ok") != NULL; +} + +static int kcp_client_is_registered(kcp_client_t *client) { + int registered; + + if (client == NULL) { + return 0; + } + pthread_mutex_lock(&client->state_mu); + registered = client->registered; + pthread_mutex_unlock(&client->state_mu); + return registered; +} + +static int kcp_client_text_body_equals(const message_t *msg, const char *payload) { + size_t expected_len; + + if (msg == NULL || payload == NULL || msg->body == NULL) { + return 0; + } + expected_len = strlen(payload); + return msg->body_len == expected_len && memcmp(msg->body, payload, expected_len) == 0; +} + +static void kcp_client_copy_server_error_body(const message_t *msg, char *buffer, size_t buffer_len) { + size_t copy_len; + + if (buffer == NULL || buffer_len == 0) { + return; + } + buffer[0] = '\0'; + if (msg == NULL || msg->body == NULL || msg->body_len == 0) { + return; + } + copy_len = msg->body_len < (buffer_len - 1U) ? msg->body_len : (buffer_len - 1U); + memcpy(buffer, msg->body, copy_len); + buffer[copy_len] = '\0'; +} + +static int kcp_client_registration_errno_from_message(const char *message) { + if (message == NULL || message[0] == '\0') { + return ECONNREFUSED; + } + if (strstr(message, "duplicate peer id") != NULL) { + return EEXIST; + } + if (strstr(message, "first message must be register") != NULL) { + return EPROTO; + } + return ECONNREFUSED; +} + +static int kcp_client_send_text_internal(kcp_client_t *client, const char *to, const char *text, int log_business_event) { + message_t msg; + uint64_t id; + + if (client == NULL || to == NULL || text == NULL || client->conn == NULL) { + errno = EINVAL; + return -1; + } + + protocol_message_init(&msg); + if (kcp_client_next_message_id(client, &id) != 0) { + return -1; + } + msg.type = MSG_TYPE_TEXT; + msg.id = id; + snprintf(msg.from, sizeof(msg.from), "%s", client->id); + snprintf(msg.to, sizeof(msg.to), "%s", to); + msg.body = (uint8_t *) omni_strdup(text); + if (msg.body == NULL) { + return -1; + } + msg.body_len = strlen((const char *) msg.body); + if (log_business_event) { + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_A_APP_PREP_BEGIN, &msg); + } + if (kcp_conn_send(client->conn, &msg) != 0) { + protocol_message_clear(&msg); + return -1; + } + protocol_message_clear(&msg); + return 0; +} + +static int kcp_client_send_business_preflight(kcp_client_t *client) { + if (client == NULL || client->conn == NULL) { + errno = ENOTCONN; + return -1; + } + if (!kcp_client_is_registered(client)) { + errno = ENOTCONN; + return -1; + } + return 0; +} + +static int kcp_client_handle_reserved_server_message(kcp_client_t *client, const message_t *msg) { + if (client == NULL || msg == NULL) { + errno = EINVAL; + return -1; + } + if (msg->type != MSG_TYPE_TEXT || strcmp(msg->from, SERVER_PEER_ID) != 0) { + return 0; + } + kcp_client_touch_server_activity(client); + if (kcp_client_text_body_equals(msg, KCP_CLIENT_CTRL_REGISTER_OK)) { + kcp_client_set_registered(client, 1); + kcp_client_clear_last_server_error(client); + return 1; + } + if (kcp_client_text_body_equals(msg, KCP_CLIENT_CTRL_HEARTBEAT)) { + if (kcp_client_send_text_internal(client, SERVER_PEER_ID, KCP_CLIENT_CTRL_HEARTBEAT_ACK, 0) != 0) { + kcp_client_set_registered(client, 0); + kcp_client_set_last_server_error(client, "failed to acknowledge server heartbeat"); + (void) kcp_conn_close(client->conn); + return -1; + } + return 1; + } + if (kcp_client_text_body_equals(msg, KCP_CLIENT_CTRL_HEARTBEAT_ACK)) { + return 1; + } + if (kcp_client_text_body_equals(msg, KCP_CLIENT_CTRL_PEER_REPLACED)) { + kcp_client_set_registered(client, 0); + kcp_client_set_last_server_error(client, "server peer replaced this session"); + (void) kcp_conn_close(client->conn); + errno = ECONNRESET; + return -1; + } + return 0; +} + +static int kcp_client_remaining_timeout_ms(int original_timeout_ms, uint32_t start_ms) { + uint32_t elapsed_ms; + + if (original_timeout_ms < 0) { + return -1; + } + elapsed_ms = omni_now_millis32() - start_ms; + if (elapsed_ms >= (uint32_t) original_timeout_ms) { + return 0; + } + return original_timeout_ms - (int) elapsed_ms; +} + +static int kcp_client_wait_for_register_ok(kcp_client_t *client) { + uint32_t start_ms; + + if (client == NULL || client->conn == NULL) { + errno = EINVAL; + return -1; + } + + start_ms = omni_now_millis32(); + for (;;) { + message_t msg; + int rc; + int remaining_timeout_ms = kcp_client_remaining_timeout_ms(KCP_CLIENT_REGISTER_TIMEOUT_MS, start_ms); + + if (remaining_timeout_ms <= 0) { + kcp_client_set_registered(client, 0); + kcp_client_set_last_server_error(client, "timed out waiting for server_register_ok"); + (void) kcp_conn_close(client->conn); + errno = ETIMEDOUT; + return -1; + } + + protocol_message_init(&msg); + rc = kcp_conn_receive_timed(client->conn, &msg, remaining_timeout_ms); + if (rc == 1) { + protocol_message_clear(&msg); + kcp_client_set_registered(client, 0); + kcp_client_set_last_server_error(client, "timed out waiting for server_register_ok"); + (void) kcp_conn_close(client->conn); + errno = ETIMEDOUT; + return -1; + } + if (rc != 0) { + protocol_message_clear(&msg); + kcp_client_set_registered(client, 0); + return -1; + } + if (msg.type == MSG_TYPE_ERROR && strcmp(msg.from, SERVER_PEER_ID) == 0) { + char error_text[256]; + + kcp_client_copy_server_error_body(&msg, error_text, sizeof(error_text)); + kcp_client_touch_server_activity(client); + kcp_client_set_registered(client, 0); + kcp_client_set_last_server_error(client, error_text); + protocol_message_clear(&msg); + (void) kcp_conn_close(client->conn); + errno = kcp_client_registration_errno_from_message(error_text); + return -1; + } + rc = kcp_client_handle_reserved_server_message(client, &msg); + protocol_message_clear(&msg); + if (rc < 0) { + return -1; + } + if (rc > 0 && kcp_client_is_registered(client)) { + return 0; + } + + kcp_client_set_registered(client, 0); + kcp_client_set_last_server_error(client, "unexpected message while waiting for server_register_ok"); + (void) kcp_conn_close(client->conn); + errno = EPROTO; + return -1; + } +} + +static int kcp_client_receive_business_timed(kcp_client_t *client, message_t *out_msg, int timeout_ms) { + uint32_t start_ms; + + if (client == NULL || out_msg == NULL || client->conn == NULL) { + errno = EINVAL; + return -1; + } + + start_ms = omni_now_millis32(); + protocol_message_init(out_msg); + for (;;) { + int rc; + int reserved_rc; + int effective_timeout_ms = timeout_ms < 0 ? -1 : kcp_client_remaining_timeout_ms(timeout_ms, start_ms); + + if (timeout_ms >= 0 && effective_timeout_ms <= 0) { + return 1; + } + protocol_message_clear(out_msg); + rc = kcp_conn_receive_timed(client->conn, out_msg, effective_timeout_ms); + if (rc != 0) { + if (rc != 1) { + kcp_client_set_registered(client, 0); + } + return rc; + } + + if (strcmp(out_msg->from, SERVER_PEER_ID) == 0) { + kcp_client_touch_server_activity(client); + } + reserved_rc = kcp_client_handle_reserved_server_message(client, out_msg); + if (reserved_rc < 0) { + protocol_message_clear(out_msg); + return -1; + } + if (reserved_rc > 0) { + protocol_message_clear(out_msg); + continue; + } + if (out_msg->type == MSG_TYPE_ERROR && strcmp(out_msg->from, SERVER_PEER_ID) == 0) { + char error_text[256]; + + kcp_client_copy_server_error_body(out_msg, error_text, sizeof(error_text)); + kcp_client_set_last_server_error(client, error_text); + if (kcp_client_server_error_invalidates_registration(error_text)) { + kcp_client_set_registered(client, 0); + } + } + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_B_APP_RECV, out_msg); + return 0; + } +} + +static int kcp_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) { + return -1; + } + if (msg->type == MSG_TYPE_TEXT) { + char *body = omni_json_escape_bytes(msg->body, msg->body_len); + char *from = omni_json_escape(msg->from); + char *to = omni_json_escape(msg->to); + char *line; + + if (body == NULL || from == NULL || to == NULL) { + free(body); + free(from); + free(to); + return -1; + } + snprintf(path, sizeof(path), "%s/messages.log", inbox_dir); + line = omni_strdup_printf( + "{\"message_type\":\"%s\",\"message_id\":%" PRIu64 ",\"from\":\"%s\",\"to\":\"%s\",\"body\":\"%s\"}\n", + protocol_message_type_name(msg->type), + msg->id, + from, + to, + body + ); + free(body); + free(from); + free(to); + if (line == NULL) { + return -1; + } + if (omni_append_file(path, (const uint8_t *) line, strlen(line)) != 0) { + free(line); + return -1; + } + free(line); + } else if (msg->type == MSG_TYPE_FILE) { + const char *file_name = omni_path_base_name(msg->file_name); + if (file_name[0] == '\0') { + file_name = "unnamed"; + } + snprintf(path, sizeof(path), "%s/%s-%" PRIu64 "-%s", inbox_dir, msg->from, msg->id, file_name); + if (omni_write_file(path, msg->body, msg->body_len) != 0) { + return -1; + } + } else if (msg->type == MSG_TYPE_BINARY) { + snprintf(path, sizeof(path), "%s/%s-%" PRIu64 ".bin", inbox_dir, msg->from, msg->id); + if (omni_write_file(path, msg->body, msg->body_len) != 0) { + return -1; + } + } else { + errno = EINVAL; + return -1; + } + + if (out_path != NULL && out_path_len > 0) { + snprintf(out_path, out_path_len, "%s", path); + } + return 0; +} + +static void kcp_client_fill_recv_meta(kcp_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); +} + +kcp_client_t *kcp_client_dial_with_options(const char *server_addr, const char *dial_addr, const char *peer_id, const char *bind_ip, const char *bind_device, const kcp_conn_options_t *options, latency_logger_t *logger, kcp_packet_debug_logger_t *packet_logger, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms) { + kcp_client_t *client; + const char *actual_dial_addr = (dial_addr != NULL && dial_addr[0] != '\0') ? dial_addr : server_addr; + message_t register_msg; + int saved_errno = 0; + + client = (kcp_client_t *) calloc(1, sizeof(*client)); + if (client == NULL) { + return NULL; + } + snprintf(client->id, sizeof(client->id), "%s", peer_id); + snprintf(client->server_addr, sizeof(client->server_addr), "%s", server_addr == NULL ? "" : server_addr); + pthread_mutex_init(&client->state_mu, NULL); + client->last_server_activity_ms = omni_now_millis32(); + client->logger = logger; + client->conn = kcp_conn_dial_with_options(actual_dial_addr, bind_ip, bind_device, options, packet_logger, logger, OMNI_NODE_ROLE_PEER, peer_id, stats_logger, stats_interval_ms); + if (client->conn == NULL) { + saved_errno = errno; + kcp_client_free(client); + errno = saved_errno; + return NULL; + } + + protocol_message_init(®ister_msg); + register_msg.type = MSG_TYPE_REGISTER; + register_msg.id = 0; + snprintf(register_msg.from, sizeof(register_msg.from), "%s", peer_id); + snprintf(register_msg.to, sizeof(register_msg.to), "%s", SERVER_PEER_ID); + if (kcp_conn_send(client->conn, ®ister_msg) != 0) { + saved_errno = errno; + kcp_client_free(client); + errno = saved_errno; + return NULL; + } + if (kcp_client_wait_for_register_ok(client) != 0) { + saved_errno = errno; + kcp_client_free(client); + errno = saved_errno; + return NULL; + } + return client; +} + +kcp_client_t *kcp_client_dial(const char *server_addr, const char *dial_addr, const char *peer_id, const char *bind_ip, const char *bind_device, latency_logger_t *logger, kcp_packet_debug_logger_t *packet_logger, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms) { + return kcp_client_dial_with_options(server_addr, dial_addr, peer_id, bind_ip, bind_device, NULL, logger, packet_logger, stats_logger, stats_interval_ms); +} + +const char *kcp_client_id(const kcp_client_t *client) { + return client == NULL ? "" : client->id; +} + +int kcp_client_send_text(kcp_client_t *client, const char *to, const char *text) { + if (client == NULL || to == NULL || text == NULL) { + errno = EINVAL; + return -1; + } + if (kcp_client_send_business_preflight(client) != 0) { + return -1; + } + return kcp_client_send_text_internal(client, to, text, 1); +} + +int kcp_client_send_binary(kcp_client_t *client, const char *to, const void *data, size_t data_len) { + return kcp_client_send_binary_with_id(client, to, data, data_len, NULL); +} + +int kcp_client_send_binary_with_id( + kcp_client_t *client, + const char *to, + const void *data, + size_t data_len, + uint64_t *out_id +) { + message_t msg; + uint64_t id; + + if (client == NULL || to == NULL || (data == NULL && data_len > 0)) { + errno = EINVAL; + return -1; + } + if (kcp_client_send_business_preflight(client) != 0) { + return -1; + } + protocol_message_init(&msg); + if (kcp_client_next_message_id(client, &id) != 0) { + return -1; + } + 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 (kcp_conn_send(client->conn, &msg) != 0) { + protocol_message_clear(&msg); + return -1; + } + if (out_id != NULL) { + *out_id = id; + } + protocol_message_clear(&msg); + return 0; +} + +int kcp_client_send_file_path(kcp_client_t *client, const char *to, const char *path) { + message_t msg; + uint64_t id; + uint8_t *body = NULL; + size_t body_len = 0; + const char *base_name = strrchr(path, '/'); + + if (client == NULL || to == NULL || path == NULL) { + errno = EINVAL; + return -1; + } + if (kcp_client_send_business_preflight(client) != 0) { + return -1; + } + if (omni_read_file(path, &body, &body_len) != 0) { + return -1; + } + protocol_message_init(&msg); + if (kcp_client_next_message_id(client, &id) != 0) { + free(body); + return -1; + } + msg.type = MSG_TYPE_FILE; + msg.id = id; + snprintf(msg.from, sizeof(msg.from), "%s", client->id); + snprintf(msg.to, sizeof(msg.to), "%s", to); + snprintf(msg.file_name, sizeof(msg.file_name), "%s", base_name == NULL ? path : base_name + 1); + msg.body = body; + msg.body_len = body_len; + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_A_APP_PREP_BEGIN, &msg); + if (kcp_conn_send(client->conn, &msg) != 0) { + protocol_message_clear(&msg); + return -1; + } + protocol_message_clear(&msg); + return 0; +} + +int kcp_client_receive_timed(kcp_client_t *client, message_t *out_msg, int timeout_ms) { + if (client == NULL || out_msg == NULL) { + errno = EINVAL; + return -1; + } + return kcp_client_receive_business_timed(client, out_msg, timeout_ms); +} + +int kcp_client_receive(kcp_client_t *client, message_t *out_msg) { + if (kcp_client_receive_timed(client, out_msg, -1) != 0) { + return -1; + } + return 0; +} + +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) { + 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 = kcp_client_receive_timed(client, &msg, timeout_ms); + if (rc != 0) { + protocol_message_clear(&msg); + return rc; + } + + kcp_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 kcp_client_persist_message(kcp_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; + return -1; + } + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_B_PERSIST_BEGIN, msg); + if (kcp_client_persist_message_to_disk(msg, inbox_dir, out_path, out_path_len) != 0) { + return -1; + } + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_B_PERSIST_END, msg); + return 0; +} + +void kcp_client_state_snapshot(kcp_client_t *client, kcp_client_state_t *out_state) { + kcp_runtime_stats_t runtime_stats; + + if (out_state == NULL) { + return; + } + memset(out_state, 0, sizeof(*out_state)); + if (client == NULL) { + return; + } + memset(&runtime_stats, 0, sizeof(runtime_stats)); + if (client->conn != NULL) { + kcp_conn_runtime_stats_snapshot(client->conn, &runtime_stats); + out_state->connected = runtime_stats.connected; + } + pthread_mutex_lock(&client->state_mu); + out_state->registered = client->registered; + out_state->server_idle_ms = client->last_server_activity_ms == 0 + ? 0 + : (omni_now_millis32() - client->last_server_activity_ms); + snprintf(out_state->last_server_error, sizeof(out_state->last_server_error), "%s", client->last_server_error); + pthread_mutex_unlock(&client->state_mu); +} + +void kcp_client_runtime_stats_snapshot(kcp_client_t *client, kcp_runtime_stats_t *out_stats) { + if (out_stats == NULL) { + return; + } + + 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) { + if (client == NULL) { + return 0; + } + kcp_client_set_registered(client, 0); + return kcp_conn_close(client->conn); +} + +void kcp_client_free(kcp_client_t *client) { + if (client == NULL) { + return; + } + kcp_conn_free(client->conn); + pthread_mutex_destroy(&client->state_mu); + free(client); +} diff --git a/host/OmniSocketGo_add_camera/src/peer_udp_client.c b/host/OmniSocketGo_add_camera/src/peer_udp_client.c new file mode 100644 index 0000000..9e617ab --- /dev/null +++ b/host/OmniSocketGo_add_camera/src/peer_udp_client.c @@ -0,0 +1,297 @@ +#include "peer_udp_client.h" + +#include +#include +#include + +struct udp_client { + char id[OMNI_MAX_PEER_ID]; + udp_conn_t *conn; + latency_logger_t *logger; + pthread_mutex_t id_mu; + uint64_t next_message_id; +}; + +static int client_next_message_id(udp_client_t *client, uint64_t *out_id) { + pthread_mutex_lock(&client->id_mu); + *out_id = ++client->next_message_id; + pthread_mutex_unlock(&client->id_mu); + 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) { + return -1; + } + if (msg->type == MSG_TYPE_TEXT) { + char *body = omni_json_escape_bytes(msg->body, msg->body_len); + char *from = omni_json_escape(msg->from); + char *to = omni_json_escape(msg->to); + char *line; + if (body == NULL || from == NULL || to == NULL) { + free(body); + free(from); + free(to); + return -1; + } + snprintf(path, sizeof(path), "%s/messages.log", inbox_dir); + line = omni_strdup_printf("{\"message_type\":\"%s\",\"message_id\":%" PRIu64 ",\"from\":\"%s\",\"to\":\"%s\",\"body\":\"%s\"}\n", protocol_message_type_name(msg->type), msg->id, from, to, body); + free(body); + free(from); + free(to); + if (line == NULL) { + return -1; + } + if (omni_append_file(path, (const uint8_t *) line, strlen(line)) != 0) { + free(line); + return -1; + } + free(line); + } else if (msg->type == MSG_TYPE_FILE) { + const char *file_name = omni_path_base_name(msg->file_name); + if (file_name[0] == '\0') { + file_name = "unnamed"; + } + snprintf(path, sizeof(path), "%s/%s-%" PRIu64 "-%s", inbox_dir, msg->from, msg->id, file_name); + if (omni_write_file(path, msg->body, msg->body_len) != 0) { + return -1; + } + } else if (msg->type == MSG_TYPE_BINARY) { + snprintf(path, sizeof(path), "%s/%s-%" PRIu64 ".bin", inbox_dir, msg->from, msg->id); + if (omni_write_file(path, msg->body, msg->body_len) != 0) { + return -1; + } + } else { + errno = EINVAL; + return -1; + } + if (out_path != NULL && out_path_len > 0) { + snprintf(out_path, out_path_len, "%s", path); + } + return 0; +} + +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)); + if (client == NULL) { + return NULL; + } + 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, bind_device, enable_timestamping, logger, OMNI_NODE_ROLE_PEER, peer_id, debug_logger); + if (client->conn == NULL) { + udp_client_free(client); + return NULL; + } + protocol_message_init(®ister_msg); + register_msg.type = MSG_TYPE_REGISTER; + register_msg.id = 0; + snprintf(register_msg.from, sizeof(register_msg.from), "%s", peer_id); + snprintf(register_msg.to, sizeof(register_msg.to), "%s", SERVER_PEER_ID); + if (udp_conn_send(client->conn, ®ister_msg) != 0) { + udp_client_free(client); + return NULL; + } + 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; +} + +int udp_client_send_text(udp_client_t *client, const char *to, const char *text) { + message_t msg; + uint64_t id; + protocol_message_init(&msg); + client_next_message_id(client, &id); + msg.type = MSG_TYPE_TEXT; + msg.id = id; + snprintf(msg.from, sizeof(msg.from), "%s", client->id); + snprintf(msg.to, sizeof(msg.to), "%s", to); + msg.body = (uint8_t *) omni_strdup(text); + if (msg.body == NULL) { + return -1; + } + msg.body_len = strlen((const char *) msg.body); + 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_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; + uint8_t *body = NULL; + size_t body_len = 0; + const char *base_name = strrchr(path, '/'); + if (omni_read_file(path, &body, &body_len) != 0) { + return -1; + } + protocol_message_init(&msg); + client_next_message_id(client, &id); + msg.type = MSG_TYPE_FILE; + msg.id = id; + snprintf(msg.from, sizeof(msg.from), "%s", client->id); + snprintf(msg.to, sizeof(msg.to), "%s", to); + snprintf(msg.file_name, sizeof(msg.file_name), "%s", base_name == NULL ? path : base_name + 1); + msg.body = body; + msg.body_len = body_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_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; + } + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_B_APP_RECV, 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; + return -1; + } + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_B_PERSIST_BEGIN, msg); + if (client_persist_message_to_disk(msg, inbox_dir, out_path, out_path_len) != 0) { + return -1; + } + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_B_PERSIST_END, msg); + return 0; +} + +int udp_client_close(udp_client_t *client) { + return client == NULL ? 0 : udp_conn_close(client->conn); +} + +void udp_client_free(udp_client_t *client) { + if (client == NULL) { + return; + } + udp_conn_free(client->conn); + pthread_mutex_destroy(&client->id_mu); + free(client); +} diff --git a/host/OmniSocketGo_add_camera/src/protocol.c b/host/OmniSocketGo_add_camera/src/protocol.c new file mode 100644 index 0000000..d82d045 --- /dev/null +++ b/host/OmniSocketGo_add_camera/src/protocol.c @@ -0,0 +1,415 @@ +#include "protocol.h" + +#include "cJSON.h" + +#include + +static const char *protocol_message_type_table[] = { + "text", + "file", + "register", + "error", + "binary" +}; + +const char *protocol_message_type_name(message_type_t type) { + if ((int) type < 0 || (size_t) type >= OMNI_ARRAY_LEN(protocol_message_type_table)) { + return "invalid"; + } + return protocol_message_type_table[type]; +} + +int protocol_message_type_from_name(const char *raw, message_type_t *out) { + size_t i; + if (raw == NULL || out == NULL) { + return -1; + } + for (i = 0; i < OMNI_ARRAY_LEN(protocol_message_type_table); ++i) { + if (strcmp(raw, protocol_message_type_table[i]) == 0) { + *out = (message_type_t) i; + return 0; + } + } + return -1; +} + +void protocol_message_init(message_t *msg) { + if (msg == NULL) { + return; + } + memset(msg, 0, sizeof(*msg)); + msg->type = MSG_TYPE_INVALID; +} + +void protocol_message_clear(message_t *msg) { + if (msg == NULL) { + return; + } + free(msg->body); + protocol_message_init(msg); +} + +int protocol_message_copy(message_t *dst, const message_t *src) { + if (dst == NULL || src == NULL) { + errno = EINVAL; + return -1; + } + protocol_message_clear(dst); + memcpy(dst, src, sizeof(*dst)); + dst->body = NULL; + if (src->body_len > 0) { + dst->body = (uint8_t *) malloc(src->body_len); + if (dst->body == NULL) { + protocol_message_init(dst); + errno = ENOMEM; + return -1; + } + memcpy(dst->body, src->body, src->body_len); + } + return 0; +} + +static int protocol_set_err(char *err, size_t err_len, const char *fmt, ...) { + va_list args; + if (err != NULL && err_len > 0) { + va_start(args, fmt); + vsnprintf(err, err_len, fmt, args); + va_end(args); + } + return -1; +} + +int protocol_validate_message(const message_t *msg, char *err, size_t err_len) { + if (msg == NULL) { + return protocol_set_err(err, err_len, "protocol: nil message"); + } + if (msg->from[0] == '\0') { + return protocol_set_err(err, err_len, "protocol: missing from"); + } + if (msg->to[0] == '\0') { + return protocol_set_err(err, err_len, "protocol: missing to"); + } + switch (msg->type) { + case MSG_TYPE_TEXT: + if (msg->file_name[0] != '\0') { + return protocol_set_err(err, err_len, "protocol: unexpected file name"); + } + if (!omni_utf8_valid(msg->body, msg->body_len)) { + return protocol_set_err(err, err_len, "protocol: invalid text body"); + } + break; + case MSG_TYPE_FILE: + if (msg->file_name[0] == '\0') { + return protocol_set_err(err, err_len, "protocol: missing file name"); + } + break; + case MSG_TYPE_BINARY: + if (msg->file_name[0] != '\0') { + return protocol_set_err(err, err_len, "protocol: unexpected file name"); + } + break; + case MSG_TYPE_REGISTER: + if (strcmp(msg->to, SERVER_PEER_ID) != 0) { + return protocol_set_err(err, err_len, "protocol: invalid register target"); + } + if (msg->file_name[0] != '\0') { + return protocol_set_err(err, err_len, "protocol: unexpected file name"); + } + if (msg->body_len != 0) { + return protocol_set_err(err, err_len, "protocol: unexpected body"); + } + break; + case MSG_TYPE_ERROR: + if (strcmp(msg->from, SERVER_PEER_ID) != 0) { + return protocol_set_err(err, err_len, "protocol: invalid error source"); + } + if (msg->file_name[0] != '\0') { + return protocol_set_err(err, err_len, "protocol: unexpected file name"); + } + if (!omni_utf8_valid(msg->body, msg->body_len)) { + return protocol_set_err(err, err_len, "protocol: invalid text body"); + } + break; + default: + return protocol_set_err(err, err_len, "protocol: invalid message type"); + } + return 0; +} + +static int protocol_build_header_json(const message_t *msg, char **out_json, size_t *out_len) { + cJSON *root; + char *json; + + root = cJSON_CreateObject(); + if (root == NULL) { + errno = ENOMEM; + return -1; + } + cJSON_AddStringToObject(root, "type", protocol_message_type_name(msg->type)); + cJSON_AddNumberToObject(root, "id", (double) msg->id); + cJSON_AddStringToObject(root, "from", msg->from); + cJSON_AddStringToObject(root, "to", msg->to); + if (msg->file_name[0] != '\0') { + cJSON_AddStringToObject(root, "file_name", msg->file_name); + } + cJSON_AddNumberToObject(root, "content_length", (double) msg->body_len); + json = cJSON_PrintUnformatted(root); + cJSON_Delete(root); + if (json == NULL) { + errno = ENOMEM; + return -1; + } + *out_len = strlen(json); + *out_json = json; + return 0; +} + +int protocol_encode_message_datagram(const message_t *msg, uint8_t **out, size_t *out_len) { + uint8_t *buffer; + char *header_json; + size_t header_len; + uint32_t net_header_len; + char err[128]; + + if (out == NULL || out_len == NULL) { + errno = EINVAL; + return -1; + } + *out = NULL; + *out_len = 0; + if (protocol_validate_message(msg, err, sizeof(err)) != 0) { + errno = EINVAL; + return -1; + } + if (protocol_build_header_json(msg, &header_json, &header_len) != 0) { + return -1; + } + if (4U + header_len + msg->body_len > OMNI_MAX_FRAME_SIZE) { + cJSON_free(header_json); + errno = EMSGSIZE; + return -1; + } + buffer = (uint8_t *) malloc(4U + header_len + msg->body_len); + if (buffer == NULL) { + cJSON_free(header_json); + errno = ENOMEM; + return -1; + } + net_header_len = htonl((uint32_t) header_len); + memcpy(buffer, &net_header_len, 4); + memcpy(buffer + 4, header_json, header_len); + if (msg->body_len > 0) { + memcpy(buffer + 4 + header_len, msg->body, msg->body_len); + } + cJSON_free(header_json); + *out = buffer; + *out_len = 4U + header_len + msg->body_len; + return 0; +} + +static int protocol_copy_string_field(char *dst, size_t dst_len, const cJSON *object, const char *field, int required, char *err, size_t err_len) { + const cJSON *item = cJSON_GetObjectItemCaseSensitive(object, field); + if (item == NULL) { + if (required) { + return protocol_set_err(err, err_len, "protocol: missing %s", field); + } + dst[0] = '\0'; + return 0; + } + if (!cJSON_IsString(item) || item->valuestring == NULL) { + return protocol_set_err(err, err_len, "protocol: invalid %s", field); + } + snprintf(dst, dst_len, "%s", item->valuestring); + return 0; +} + +static int protocol_copy_u64_field(uint64_t *dst, const cJSON *object, const char *field, int required, char *err, size_t err_len) { + const cJSON *item = cJSON_GetObjectItemCaseSensitive(object, field); + if (item == NULL) { + if (required) { + return protocol_set_err(err, err_len, "protocol: missing %s", field); + } + *dst = 0; + return 0; + } + if (!cJSON_IsNumber(item)) { + return protocol_set_err(err, err_len, "protocol: invalid %s", field); + } + *dst = (uint64_t) item->valuedouble; + return 0; +} + +int protocol_decode_message_datagram(const uint8_t *data, size_t data_len, message_t *out_msg, char *err, size_t err_len) { + uint32_t net_header_len; + uint32_t header_len; + char *header_text = NULL; + cJSON *header = NULL; + const cJSON *type_item; + uint64_t content_length = 0; + + if (data == NULL || out_msg == NULL || data_len < 4U) { + return protocol_set_err(err, err_len, "protocol: invalid datagram"); + } + if (data_len > OMNI_MAX_FRAME_SIZE) { + return protocol_set_err(err, err_len, "protocol: frame too large"); + } + + protocol_message_clear(out_msg); + + memcpy(&net_header_len, data, 4); + header_len = ntohl(net_header_len); + if (header_len == 0 || (size_t) header_len > data_len - 4U) { + return protocol_set_err(err, err_len, "protocol: invalid header length"); + } + header_text = (char *) malloc((size_t) header_len + 1U); + if (header_text == NULL) { + errno = ENOMEM; + return -1; + } + memcpy(header_text, data + 4, header_len); + header_text[header_len] = '\0'; + header = cJSON_Parse(header_text); + free(header_text); + if (header == NULL || !cJSON_IsObject(header)) { + if (header != NULL) { + cJSON_Delete(header); + } + return protocol_set_err(err, err_len, "protocol: invalid header json"); + } + type_item = cJSON_GetObjectItemCaseSensitive(header, "type"); + if (type_item == NULL || !cJSON_IsString(type_item) || protocol_message_type_from_name(type_item->valuestring, &out_msg->type) != 0) { + cJSON_Delete(header); + return protocol_set_err(err, err_len, "protocol: invalid message type"); + } + if (protocol_copy_u64_field(&out_msg->id, header, "id", 1, err, err_len) != 0 || + protocol_copy_string_field(out_msg->from, sizeof(out_msg->from), header, "from", 1, err, err_len) != 0 || + protocol_copy_string_field(out_msg->to, sizeof(out_msg->to), header, "to", 1, err, err_len) != 0 || + protocol_copy_string_field(out_msg->file_name, sizeof(out_msg->file_name), header, "file_name", 0, err, err_len) != 0 || + protocol_copy_u64_field(&content_length, header, "content_length", 1, err, err_len) != 0) { + cJSON_Delete(header); + protocol_message_clear(out_msg); + return -1; + } + cJSON_Delete(header); + if ((size_t) content_length != data_len - 4U - (size_t) header_len) { + protocol_message_clear(out_msg); + return protocol_set_err(err, err_len, "protocol: invalid content length"); + } + out_msg->body_len = (size_t) content_length; + if (out_msg->body_len > 0) { + out_msg->body = (uint8_t *) malloc(out_msg->body_len); + if (out_msg->body == NULL) { + protocol_message_clear(out_msg); + errno = ENOMEM; + return -1; + } + memcpy(out_msg->body, data + 4U + header_len, out_msg->body_len); + } + if (protocol_validate_message(out_msg, err, err_len) != 0) { + protocol_message_clear(out_msg); + return -1; + } + return 0; +} + +int protocol_encode_message_stream(const message_t *msg, uint8_t **out, size_t *out_len) { + uint8_t *payload; + uint8_t *buffer; + size_t payload_len; + uint32_t net_len; + + if (protocol_encode_message_datagram(msg, &payload, &payload_len) != 0) { + return -1; + } + buffer = (uint8_t *) malloc(payload_len + 4U); + if (buffer == NULL) { + free(payload); + errno = ENOMEM; + return -1; + } + net_len = htonl((uint32_t) payload_len); + memcpy(buffer, &net_len, 4); + memcpy(buffer + 4, payload, payload_len); + free(payload); + *out = buffer; + *out_len = payload_len + 4U; + return 0; +} + +int protocol_decode_message_stream_payload(const uint8_t *payload, size_t payload_len, message_t *out_msg, char *err, size_t err_len) { + return protocol_decode_message_datagram(payload, payload_len, out_msg, err, err_len); +} + +void protocol_frame_decoder_init(protocol_frame_decoder_t *decoder) { + memset(decoder, 0, sizeof(*decoder)); +} + +void protocol_frame_decoder_reset(protocol_frame_decoder_t *decoder) { + decoder->len = 0; +} + +void protocol_frame_decoder_destroy(protocol_frame_decoder_t *decoder) { + free(decoder->buffer); + memset(decoder, 0, sizeof(*decoder)); +} + +int protocol_frame_decoder_feed(protocol_frame_decoder_t *decoder, const uint8_t *data, size_t data_len) { + uint8_t *next_buffer; + size_t next_cap; + if (decoder->len + data_len > OMNI_MAX_FRAME_SIZE * 2U) { + errno = EMSGSIZE; + return -1; + } + if (decoder->len + data_len > decoder->cap) { + next_cap = decoder->cap == 0 ? 4096U : decoder->cap; + while (next_cap < decoder->len + data_len) { + next_cap *= 2U; + } + next_buffer = (uint8_t *) realloc(decoder->buffer, next_cap); + if (next_buffer == NULL) { + errno = ENOMEM; + return -1; + } + decoder->buffer = next_buffer; + decoder->cap = next_cap; + } + memcpy(decoder->buffer + decoder->len, data, data_len); + decoder->len += data_len; + return 0; +} + +int protocol_frame_decoder_next(protocol_frame_decoder_t *decoder, uint8_t **payload, size_t *payload_len) { + uint32_t net_len; + uint32_t frame_len; + uint8_t *frame; + + if (payload == NULL || payload_len == NULL) { + errno = EINVAL; + return -1; + } + *payload = NULL; + *payload_len = 0; + if (decoder->len < 4U) { + return 0; + } + memcpy(&net_len, decoder->buffer, 4); + frame_len = ntohl(net_len); + if (frame_len == 0 || frame_len > OMNI_MAX_FRAME_SIZE) { + errno = EMSGSIZE; + return -1; + } + if (decoder->len < 4U + frame_len) { + return 0; + } + frame = (uint8_t *) malloc(frame_len); + if (frame == NULL) { + errno = ENOMEM; + return -1; + } + memcpy(frame, decoder->buffer + 4, frame_len); + memmove(decoder->buffer, decoder->buffer + 4U + frame_len, decoder->len - 4U - frame_len); + decoder->len -= 4U + frame_len; + *payload = frame; + *payload_len = frame_len; + return 1; +} diff --git a/host/OmniSocketGo_add_camera/src/server_kcp_hub.c b/host/OmniSocketGo_add_camera/src/server_kcp_hub.c new file mode 100644 index 0000000..cdcf2f3 --- /dev/null +++ b/host/OmniSocketGo_add_camera/src/server_kcp_hub.c @@ -0,0 +1,1136 @@ +#include "server_kcp_hub.h" + +#include "cJSON.h" + +#include +#include +#include +#include +#include + +#define KCP_RELAY_MAX_DATAGRAM_SIZE (60 * 1024) +#define KCP_HUB_MAINTENANCE_INTERVAL_MS 250 +#define KCP_HUB_DEFAULT_TELEMETRY_INTERVAL_MS 500 +#define KCP_HUB_DEFAULT_HEARTBEAT_INTERVAL_MS 1000 +#define KCP_HUB_DEFAULT_LEASE_TIMEOUT_MS 4000 +#define KCP_HUB_TELEMETRY_NODE_ID "hub-telemetry" +#define KCP_HUB_DEFAULT_NODE_ID "hub" +#define KCP_HUB_CTRL_REGISTER_OK "{\"type\":\"server_register_ok\"}" +#define KCP_HUB_CTRL_PEER_REPLACED "{\"type\":\"server_peer_replaced\",\"reason\":\"new_instance_wins\"}" +#define KCP_HUB_CTRL_HEARTBEAT "{\"type\":\"server_heartbeat\"}" +#define KCP_HUB_CTRL_HEARTBEAT_ACK "{\"type\":\"server_heartbeat_ack\"}" + +typedef struct kcp_peer_entry { + struct kcp_peer_entry *next; + char peer_id[OMNI_MAX_PEER_ID]; + kcp_conn_t *conn; + uint32_t last_seen_ms; + uint32_t last_heartbeat_sent_ms; +} kcp_peer_entry_t; + +typedef struct kcp_session_thread_ctx { + kcp_hub_t *hub; + kcp_conn_t *conn; +} kcp_session_thread_ctx_t; + +typedef struct kcp_hub_pending_action { + struct kcp_hub_pending_action *next; + char peer_id[OMNI_MAX_PEER_ID]; + kcp_conn_t *conn; +} kcp_hub_pending_action_t; + +struct kcp_hub { + pthread_rwlock_t lock; + kcp_peer_entry_t *peers; + 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; + int heartbeat_interval_ms; + int lease_timeout_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; + 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_send_server_text(kcp_conn_t *conn, const char *to, const char *payload); +static void kcp_hub_touch_peer(kcp_hub_t *hub, const char *peer_id, kcp_conn_t *conn); +static void kcp_hub_run_maintenance(kcp_hub_t *hub); + +static uint32_t kcp_hub_now_ms(void) { + return omni_now_millis32(); +} + +static uint32_t kcp_hub_elapsed_ms(uint32_t now_ms, uint32_t then_ms) { + return now_ms - then_ms; +} + +static int kcp_hub_text_body_equals(const message_t *msg, const char *payload) { + size_t expected_len; + + if (msg == NULL || payload == NULL) { + return 0; + } + expected_len = strlen(payload); + return msg->body_len == expected_len && msg->body != NULL && memcmp(msg->body, payload, expected_len) == 0; +} + +static int kcp_hub_append_pending_action(kcp_hub_pending_action_t **head, const char *peer_id, kcp_conn_t *conn) { + kcp_hub_pending_action_t *action; + + if (head == NULL || peer_id == NULL || conn == NULL) { + errno = EINVAL; + return -1; + } + action = (kcp_hub_pending_action_t *) calloc(1, sizeof(*action)); + if (action == NULL) { + return -1; + } + snprintf(action->peer_id, sizeof(action->peer_id), "%s", peer_id); + action->conn = conn; + action->next = *head; + *head = action; + return 0; +} + +static void kcp_hub_free_pending_actions(kcp_hub_pending_action_t *head) { + while (head != NULL) { + kcp_hub_pending_action_t *next = head->next; + free(head); + head = next; + } +} + +static int kcp_hub_peer_is_telemetry(const char *peer_id) { + return kcp_hub_peer_id_has_suffix(peer_id, "-telemetry"); +} + +static int kcp_hub_peer_is_video_receiver(const char *peer_id) { + return peer_id != NULL && strcmp(peer_id, "peer-a-video") == 0; +} + +static int kcp_hub_peer_uses_server_lease(const char *peer_id) { + if (peer_id == NULL || peer_id[0] == '\0') { + return 0; + } + return kcp_hub_peer_id_has_suffix(peer_id, "-ctrl") + || kcp_hub_peer_is_telemetry(peer_id) + || kcp_hub_peer_is_video_receiver(peer_id); +} + +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; + + if (hub == NULL || peer_id == NULL || peer_id[0] == '\0') { + return; + } + + pthread_rwlock_wrlock(&hub->lock); + for (entry = hub->peers; entry != NULL; entry = entry->next) { + if (strcmp(entry->peer_id, peer_id) == 0 && entry->conn == conn) { + if (prev == NULL) { + hub->peers = entry->next; + } else { + prev->next = entry->next; + } + free(entry); + break; + } + prev = entry; + } + pthread_rwlock_unlock(&hub->lock); +} + +static kcp_peer_entry_t *kcp_hub_find_peer(kcp_hub_t *hub, const char *peer_id) { + kcp_peer_entry_t *entry; + for (entry = hub->peers; entry != NULL; entry = entry->next) { + if (strcmp(entry->peer_id, peer_id) == 0) { + return entry; + } + } + return NULL; +} + +static int kcp_hub_peer_id_has_suffix(const char *peer_id, const char *suffix) { + size_t peer_len; + size_t suffix_len; + + if (peer_id == NULL || suffix == NULL) { + return 0; + } + peer_len = strlen(peer_id); + suffix_len = strlen(suffix); + return peer_len >= suffix_len && strcmp(peer_id + peer_len - suffix_len, suffix) == 0; +} + +static int kcp_hub_configure_peer_transport(kcp_conn_t *conn, const char *peer_id) { + kcp_conn_options_t options; + + if (conn == NULL || peer_id == NULL) { + errno = EINVAL; + return -1; + } + if (kcp_hub_peer_id_has_suffix(peer_id, "-ctrl")) { + kcp_conn_options_set_control_defaults(&options); + return kcp_conn_apply_options(conn, &options); + } + if (kcp_hub_peer_id_has_suffix(peer_id, "-video")) { + 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 void kcp_hub_touch_peer_locked(kcp_hub_t *hub, const char *peer_id, kcp_conn_t *conn) { + kcp_peer_entry_t *entry; + + if (hub == NULL || peer_id == NULL || peer_id[0] == '\0') { + return; + } + entry = kcp_hub_find_peer(hub, peer_id); + if (entry != NULL && (conn == NULL || entry->conn == conn)) { + entry->last_seen_ms = kcp_hub_now_ms(); + } +} + +static void kcp_hub_touch_peer(kcp_hub_t *hub, const char *peer_id, kcp_conn_t *conn) { + if (hub == NULL || peer_id == NULL || peer_id[0] == '\0') { + return; + } + pthread_rwlock_wrlock(&hub->lock); + kcp_hub_touch_peer_locked(hub, peer_id, conn); + pthread_rwlock_unlock(&hub->lock); +} + +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, "min_srtt_ms", (double) stats->min_srtt_ms) == NULL || + cJSON_AddNumberToObject(object, "srttvar_ms", (double) stats->srttvar_ms) == NULL || + cJSON_AddNumberToObject(object, "last_feedback_age_ms", (double) stats->last_feedback_age_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; + uint32_t last_telemetry_push_ms = 0; + + while (!atomic_load(&hub->closed)) { + int interval_ms = KCP_HUB_DEFAULT_TELEMETRY_INTERVAL_MS; + uint32_t now_ms = kcp_hub_now_ms(); + int telemetry_enabled = 0; + + pthread_rwlock_rdlock(&hub->lock); + telemetry_enabled = hub->telemetry_peer_id[0] != '\0'; + if (telemetry_enabled && hub->telemetry_interval_ms > 0) { + interval_ms = hub->telemetry_interval_ms; + } + pthread_rwlock_unlock(&hub->lock); + + if (telemetry_enabled && (last_telemetry_push_ms == 0 || kcp_hub_elapsed_ms(now_ms, last_telemetry_push_ms) >= (uint32_t) interval_ms)) { + (void) kcp_hub_push_telemetry_snapshot(hub); + last_telemetry_push_ms = now_ms; + } + kcp_hub_run_maintenance(hub); + if (atomic_load(&hub->closed)) { + break; + } + usleep((useconds_t) KCP_HUB_MAINTENANCE_INTERVAL_MS * 1000U); + } + return NULL; +} + +static int kcp_hub_send_server_text(kcp_conn_t *conn, const char *to, const char *payload) { + message_t msg; + + 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", (to == NULL || to[0] == '\0') ? "unknown" : to); + msg.body = (uint8_t *) omni_strdup(payload == NULL ? "" : payload); + if (msg.body == NULL) { + return -1; + } + msg.body_len = strlen((const char *) msg.body); + if (kcp_conn_send(conn, &msg) != 0) { + protocol_message_clear(&msg); + return -1; + } + protocol_message_clear(&msg); + return 0; +} + +static int kcp_hub_send_server_error(kcp_conn_t *conn, const char *to, const char *message) { + message_t msg; + protocol_message_init(&msg); + msg.type = MSG_TYPE_ERROR; + snprintf(msg.from, sizeof(msg.from), "%s", SERVER_PEER_ID); + snprintf(msg.to, sizeof(msg.to), "%s", (to == NULL || to[0] == '\0') ? "unknown" : to); + msg.body = (uint8_t *) omni_strdup(message == NULL ? "" : message); + if (msg.body == NULL) { + return -1; + } + msg.body_len = strlen((const char *) msg.body); + if (kcp_conn_send(conn, &msg) != 0) { + protocol_message_clear(&msg); + return -1; + } + protocol_message_clear(&msg); + return 0; +} + +static int kcp_hub_sockaddr_equal(const struct sockaddr *left, socklen_t left_len, const struct sockaddr *right, socklen_t right_len) { + char left_text[OMNI_MAX_ADDR_TEXT]; + char right_text[OMNI_MAX_ADDR_TEXT]; + + if (left == NULL || right == NULL) { + return left == right; + } + return strcmp( + omni_sockaddr_to_string(left, left_len, left_text, sizeof(left_text)), + omni_sockaddr_to_string(right, right_len, right_text, sizeof(right_text)) + ) == 0; +} + +static int kcp_hub_accept_relay_peer(kcp_hub_t *hub, const struct sockaddr *addr, socklen_t addr_len) { + int accepted = 0; + + pthread_rwlock_wrlock(&hub->lock); + if (hub->relay_peer_addr_len == 0 && hub->relay_learn_peer) { + omni_clone_sockaddr(addr, addr_len, &hub->relay_peer_addr, &hub->relay_peer_addr_len); + accepted = 1; + } else if (hub->relay_peer_addr_len == 0) { + accepted = 1; + } else { + accepted = kcp_hub_sockaddr_equal((const struct sockaddr *) &hub->relay_peer_addr, hub->relay_peer_addr_len, addr, addr_len); + } + pthread_rwlock_unlock(&hub->lock); + return accepted; +} + +static int kcp_hub_forward_to_relay(kcp_hub_t *hub, const message_t *msg, int *relay_status) { + uint8_t *payload = NULL; + size_t payload_len = 0; + struct sockaddr_storage relay_addr; + socklen_t relay_addr_len = 0; + int relay_fd = -1; + int relay_configured = 0; + + if (relay_status != NULL) { + *relay_status = 0; + } + if (protocol_encode_message_datagram(msg, &payload, &payload_len) != 0) { + return -1; + } + if (payload_len > KCP_RELAY_MAX_DATAGRAM_SIZE) { + free(payload); + errno = EMSGSIZE; + if (relay_status != NULL) { + *relay_status = 3; + } + return -1; + } + + pthread_rwlock_rdlock(&hub->lock); + relay_fd = hub->relay_fd; + relay_configured = hub->relay_configured; + if (hub->relay_peer_addr_len > 0) { + omni_clone_sockaddr((const struct sockaddr *) &hub->relay_peer_addr, hub->relay_peer_addr_len, &relay_addr, &relay_addr_len); + } + pthread_rwlock_unlock(&hub->lock); + + if (!relay_configured || relay_fd < 0) { + free(payload); + errno = ENOTCONN; + if (relay_status != NULL) { + *relay_status = 1; + } + return -1; + } + if (relay_addr_len == 0) { + free(payload); + errno = EDESTADDRREQ; + if (relay_status != NULL) { + *relay_status = 2; + } + return -1; + } + if (sendto(relay_fd, payload, payload_len, 0, (struct sockaddr *) &relay_addr, relay_addr_len) < 0) { + free(payload); + return -1; + } + free(payload); + return 0; +} + +static int kcp_hub_forward_relay_server_error(kcp_hub_t *hub, const char *to, const char *message) { + message_t msg; + int rc; + + protocol_message_init(&msg); + msg.type = MSG_TYPE_ERROR; + snprintf(msg.from, sizeof(msg.from), "%s", SERVER_PEER_ID); + snprintf(msg.to, sizeof(msg.to), "%s", (to == NULL || to[0] == '\0') ? "unknown" : to); + msg.body = (uint8_t *) omni_strdup(message == NULL ? "" : message); + if (msg.body == NULL) { + return -1; + } + msg.body_len = strlen((const char *) msg.body); + rc = kcp_hub_forward_to_relay(hub, &msg, NULL); + protocol_message_clear(&msg); + return rc; +} + +static int kcp_hub_deliver_to_local_peer(kcp_hub_t *hub, const message_t *msg) { + kcp_conn_t *target_conn = NULL; + int rc; + + pthread_rwlock_rdlock(&hub->lock); + { + kcp_peer_entry_t *entry = kcp_hub_find_peer(hub, msg->to); + if (entry != NULL) { + target_conn = entry->conn; + } + } + pthread_rwlock_unlock(&hub->lock); + + if (target_conn == NULL) { + errno = ENOENT; + return -1; + } + rc = kcp_conn_send(target_conn, msg); + if (rc != 0) { + kcp_hub_unregister(hub, msg->to, target_conn); + kcp_conn_close(target_conn); + return -1; + } + return 0; +} + +static int kcp_hub_deliver_relayed_message(kcp_hub_t *hub, const message_t *msg) { + char *error_text; + + if (kcp_hub_deliver_to_local_peer(hub, msg) == 0) { + return 0; + } + if (errno != ENOENT) { + if (msg->type == MSG_TYPE_ERROR) { + return 0; + } + error_text = omni_strdup_printf("failed to forward to %s", msg->to); + if (error_text == NULL) { + return -1; + } + if (kcp_hub_forward_relay_server_error(hub, msg->from, error_text) != 0) { + free(error_text); + return -1; + } + free(error_text); + return 0; + } + + if (msg->type == MSG_TYPE_ERROR) { + return 0; + } + + error_text = omni_strdup_printf("unknown target: %s", msg->to); + if (error_text == NULL) { + return -1; + } + if (kcp_hub_forward_relay_server_error(hub, msg->from, error_text) != 0) { + free(error_text); + return -1; + } + free(error_text); + return 0; +} + +static int kcp_hub_handle_peer_message(kcp_hub_t *hub, const char *peer_id, kcp_conn_t *conn, message_t *msg) { + char *error_text = NULL; + int relay_status = 0; + + kcp_hub_touch_peer(hub, peer_id, conn); + switch (msg->type) { + case MSG_TYPE_TEXT: + if (strcmp(msg->to, SERVER_PEER_ID) == 0) { + if (kcp_hub_text_body_equals(msg, KCP_HUB_CTRL_HEARTBEAT_ACK)) { + return 0; + } + if (kcp_hub_send_server_error(conn, peer_id, "unsupported server control message") != 0) { + return -1; + } + errno = EPROTO; + return -1; + } + snprintf(msg->from, sizeof(msg->from), "%s", peer_id); + if (kcp_hub_deliver_to_local_peer(hub, msg) == 0) { + return 0; + } + if (errno != ENOENT) { + error_text = omni_strdup_printf("failed to forward to %s", msg->to); + if (error_text == NULL) { + return -1; + } + if (kcp_hub_send_server_error(conn, peer_id, error_text) != 0) { + free(error_text); + return -1; + } + free(error_text); + return 0; + } + if (kcp_hub_forward_to_relay(hub, msg, &relay_status) == 0) { + return 0; + } + if (relay_status == 1) { + error_text = omni_strdup_printf("unknown target: %s", msg->to); + } else if (relay_status == 2) { + error_text = omni_strdup("failed to relay to remote peer"); + } else if (relay_status == 3) { + error_text = omni_strdup("message too large for relay udp"); + } else { + error_text = omni_strdup("failed to relay to remote peer"); + } + if (error_text == NULL) { + return -1; + } + if (kcp_hub_send_server_error(conn, peer_id, error_text) != 0) { + free(error_text); + return -1; + } + free(error_text); + return 0; + case MSG_TYPE_FILE: + case MSG_TYPE_BINARY: + snprintf(msg->from, sizeof(msg->from), "%s", peer_id); + if (kcp_hub_deliver_to_local_peer(hub, msg) == 0) { + return 0; + } + if (errno != ENOENT) { + error_text = omni_strdup_printf("failed to forward to %s", msg->to); + if (error_text == NULL) { + return -1; + } + if (kcp_hub_send_server_error(conn, peer_id, error_text) != 0) { + free(error_text); + return -1; + } + free(error_text); + return 0; + } + if (kcp_hub_forward_to_relay(hub, msg, &relay_status) == 0) { + return 0; + } + if (relay_status == 1) { + error_text = omni_strdup_printf("unknown target: %s", msg->to); + } else if (relay_status == 2) { + error_text = omni_strdup("failed to relay to remote peer"); + } else if (relay_status == 3) { + error_text = omni_strdup("message too large for relay udp"); + } else { + error_text = omni_strdup("failed to relay to remote peer"); + } + if (error_text == NULL) { + return -1; + } + if (kcp_hub_send_server_error(conn, peer_id, error_text) != 0) { + free(error_text); + return -1; + } + free(error_text); + return 0; + case MSG_TYPE_REGISTER: + case MSG_TYPE_ERROR: + if (kcp_hub_send_server_error(conn, peer_id, "registered peers can only send text, file, or binary messages") != 0) { + return -1; + } + errno = EPROTO; + return -1; + default: + error_text = omni_strdup_printf("unsupported message type: %s", protocol_message_type_name(msg->type)); + if (error_text == NULL) { + return -1; + } + if (kcp_hub_send_server_error(conn, peer_id, error_text) != 0) { + free(error_text); + return -1; + } + free(error_text); + errno = EPROTO; + return -1; + } +} + +static int kcp_hub_commit_registered_conn( + kcp_hub_t *hub, + const char *peer_id, + kcp_conn_t *conn, + uint32_t now_ms, + kcp_conn_t **out_old_conn +) { + kcp_peer_entry_t *entry; + + if (hub == NULL || peer_id == NULL || peer_id[0] == '\0' || conn == NULL) { + errno = EINVAL; + return -1; + } + if (out_old_conn != NULL) { + *out_old_conn = NULL; + } + + pthread_rwlock_wrlock(&hub->lock); + entry = kcp_hub_find_peer(hub, peer_id); + if (entry != NULL) { + if (out_old_conn != NULL) { + *out_old_conn = entry->conn; + } + entry->conn = conn; + entry->last_seen_ms = now_ms; + entry->last_heartbeat_sent_ms = 0; + pthread_rwlock_unlock(&hub->lock); + return 0; + } + + entry = (kcp_peer_entry_t *) calloc(1, sizeof(*entry)); + if (entry == NULL) { + pthread_rwlock_unlock(&hub->lock); + return -1; + } + snprintf(entry->peer_id, sizeof(entry->peer_id), "%s", peer_id); + entry->conn = conn; + entry->last_seen_ms = now_ms; + entry->last_heartbeat_sent_ms = 0; + entry->next = hub->peers; + hub->peers = entry; + pthread_rwlock_unlock(&hub->lock); + return 0; +} + +static int kcp_hub_register_conn(kcp_hub_t *hub, kcp_conn_t *conn, char *peer_id, size_t peer_id_len) { + message_t msg; + kcp_conn_t *old_conn = NULL; + uint32_t now_ms; + + protocol_message_init(&msg); + if (kcp_conn_receive(conn, &msg) != 0) { + protocol_message_clear(&msg); + return -1; + } + if (msg.type != MSG_TYPE_REGISTER) { + kcp_hub_send_server_error(conn, msg.from, "first message must be register"); + protocol_message_clear(&msg); + errno = EPROTO; + return -1; + } + + snprintf(peer_id, peer_id_len, "%s", msg.from); + if (kcp_hub_send_server_text(conn, msg.from, KCP_HUB_CTRL_REGISTER_OK) != 0) { + protocol_message_clear(&msg); + return -1; + } + + now_ms = kcp_hub_now_ms(); + if (kcp_hub_commit_registered_conn(hub, msg.from, conn, now_ms, &old_conn) != 0) { + protocol_message_clear(&msg); + return -1; + } + + if (old_conn != NULL && old_conn != conn) { + (void) kcp_hub_send_server_text(old_conn, msg.from, KCP_HUB_CTRL_PEER_REPLACED); + kcp_conn_close(old_conn); + } + protocol_message_clear(&msg); + return 0; +} + +static void *kcp_hub_session_thread_main(void *arg) { + kcp_session_thread_ctx_t *ctx = (kcp_session_thread_ctx_t *) arg; + kcp_hub_serve_session(ctx->hub, ctx->conn); + free(ctx); + return NULL; +} + +kcp_hub_t *kcp_hub_new(latency_logger_t *logger, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms) { + kcp_hub_t *hub = (kcp_hub_t *) calloc(1, sizeof(*hub)); + if (hub == NULL) { + return NULL; + } + pthread_rwlock_init(&hub->lock, NULL); + 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->heartbeat_interval_ms = KCP_HUB_DEFAULT_HEARTBEAT_INTERVAL_MS; + hub->lease_timeout_ms = KCP_HUB_DEFAULT_LEASE_TIMEOUT_MS; + hub->relay_fd = -1; + atomic_init(&hub->closed, 0); + if (pthread_create(&hub->telemetry_thread, NULL, kcp_hub_telemetry_thread_main, hub) != 0) { + pthread_rwlock_destroy(&hub->lock); + free(hub); + return NULL; + } + hub->telemetry_thread_started = 1; + return hub; +} + +int kcp_hub_serve_listener(kcp_hub_t *hub, kcp_listener_t *listener) { + if (hub == NULL || listener == NULL) { + errno = EINVAL; + return -1; + } + 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 (atomic_load(&hub->closed)) { + return 0; + } + return -1; + } + ctx = (kcp_session_thread_ctx_t *) calloc(1, sizeof(*ctx)); + if (ctx == NULL) { + kcp_conn_close(conn); + kcp_conn_free(conn); + return -1; + } + ctx->hub = hub; + ctx->conn = conn; + if (pthread_create(&thread, NULL, kcp_hub_session_thread_main, ctx) != 0) { + free(ctx); + kcp_conn_close(conn); + kcp_conn_free(conn); + return -1; + } + pthread_detach(thread); + } + return 0; +} + +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) { + errno = EINVAL; + return -1; + } + peer_id[0] = '\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_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; + } + + for (;;) { + message_t msg; + protocol_message_init(&msg); + if (kcp_conn_receive(conn, &msg) != 0) { + protocol_message_clear(&msg); + rc = -1; + break; + } + if (kcp_hub_handle_peer_message(hub, peer_id, conn, &msg) != 0) { + protocol_message_clear(&msg); + rc = -1; + break; + } + protocol_message_clear(&msg); + } + + kcp_hub_unregister(hub, peer_id, conn); + kcp_conn_close(conn); + kcp_conn_free(conn); + return rc; +} + +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) { + if (hub == NULL || relay_fd < 0) { + errno = EINVAL; + return -1; + } + pthread_rwlock_wrlock(&hub->lock); + hub->relay_fd = relay_fd; + hub->relay_configured = 1; + hub->relay_learn_peer = learn_peer; + hub->relay_peer_addr_len = 0; + if (peer_addr != NULL && peer_addr_len > 0) { + omni_clone_sockaddr(peer_addr, peer_addr_len, &hub->relay_peer_addr, &hub->relay_peer_addr_len); + } + pthread_rwlock_unlock(&hub->lock); + return 0; +} + +int kcp_hub_set_telemetry(kcp_hub_t *hub, const char *peer_id, int interval_ms) { + 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; + pthread_rwlock_unlock(&hub->lock); + return 0; +} + +static void kcp_hub_run_maintenance(kcp_hub_t *hub) { + kcp_hub_pending_action_t *heartbeat_actions = NULL; + kcp_hub_pending_action_t *close_actions = NULL; + uint32_t now_ms; + int heartbeat_interval_ms; + int lease_timeout_ms; + + if (hub == NULL) { + return; + } + + now_ms = kcp_hub_now_ms(); + heartbeat_interval_ms = KCP_HUB_DEFAULT_HEARTBEAT_INTERVAL_MS; + lease_timeout_ms = KCP_HUB_DEFAULT_LEASE_TIMEOUT_MS; + + pthread_rwlock_wrlock(&hub->lock); + if (hub->heartbeat_interval_ms > 0) { + heartbeat_interval_ms = hub->heartbeat_interval_ms; + } + if (hub->lease_timeout_ms > 0) { + lease_timeout_ms = hub->lease_timeout_ms; + } + { + kcp_peer_entry_t *prev = NULL; + kcp_peer_entry_t *entry = hub->peers; + + while (entry != NULL) { + kcp_peer_entry_t *next = entry->next; + uint32_t idle_ms = kcp_hub_elapsed_ms(now_ms, entry->last_seen_ms); + int uses_server_lease = kcp_hub_peer_uses_server_lease(entry->peer_id); + + if (entry->conn == NULL || entry->peer_id[0] == '\0') { + prev = entry; + entry = next; + continue; + } + if (uses_server_lease && lease_timeout_ms > 0 && idle_ms >= (uint32_t) lease_timeout_ms) { + if (prev == NULL) { + hub->peers = next; + } else { + prev->next = next; + } + (void) kcp_hub_append_pending_action(&close_actions, entry->peer_id, entry->conn); + free(entry); + entry = next; + continue; + } + if ( + uses_server_lease + && + heartbeat_interval_ms > 0 + && idle_ms >= (uint32_t) heartbeat_interval_ms + && (entry->last_heartbeat_sent_ms == 0 || kcp_hub_elapsed_ms(now_ms, entry->last_heartbeat_sent_ms) >= (uint32_t) heartbeat_interval_ms) + ) { + entry->last_heartbeat_sent_ms = now_ms; + (void) kcp_hub_append_pending_action(&heartbeat_actions, entry->peer_id, entry->conn); + } + prev = entry; + entry = next; + } + } + pthread_rwlock_unlock(&hub->lock); + + while (heartbeat_actions != NULL) { + kcp_hub_pending_action_t *next = heartbeat_actions->next; + if (kcp_hub_send_server_text(heartbeat_actions->conn, heartbeat_actions->peer_id, KCP_HUB_CTRL_HEARTBEAT) != 0) { + kcp_hub_unregister(hub, heartbeat_actions->peer_id, heartbeat_actions->conn); + kcp_conn_close(heartbeat_actions->conn); + } + free(heartbeat_actions); + heartbeat_actions = next; + } + + while (close_actions != NULL) { + kcp_hub_pending_action_t *next = close_actions->next; + kcp_conn_close(close_actions->conn); + free(close_actions); + close_actions = next; + } + + kcp_hub_free_pending_actions(heartbeat_actions); + kcp_hub_free_pending_actions(close_actions); +} + +int kcp_hub_serve_relay(kcp_hub_t *hub) { + uint8_t buffer[KCP_RELAY_MAX_DATAGRAM_SIZE]; + + if (hub == NULL) { + errno = EINVAL; + return -1; + } + while (!atomic_load(&hub->closed)) { + struct sockaddr_storage source; + socklen_t source_len = sizeof(source); + ssize_t n; + message_t msg; + char err[128]; + int relay_fd; + + pthread_rwlock_rdlock(&hub->lock); + relay_fd = hub->relay_fd; + pthread_rwlock_unlock(&hub->lock); + if (relay_fd < 0) { + errno = ENOTCONN; + return -1; + } + + n = recvfrom(relay_fd, buffer, sizeof(buffer), 0, (struct sockaddr *) &source, &source_len); + if (n < 0) { + if (atomic_load(&hub->closed)) { + return 0; + } + if (errno == EINTR) { + continue; + } + return -1; + } + if (!kcp_hub_accept_relay_peer(hub, (struct sockaddr *) &source, source_len)) { + continue; + } + + protocol_message_init(&msg); + if (protocol_decode_message_datagram(buffer, (size_t) n, &msg, err, sizeof(err)) != 0) { + protocol_message_clear(&msg); + continue; + } + if (msg.type != MSG_TYPE_TEXT && msg.type != MSG_TYPE_FILE && msg.type != MSG_TYPE_BINARY && msg.type != MSG_TYPE_ERROR) { + protocol_message_clear(&msg); + continue; + } + (void) kcp_hub_deliver_relayed_message(hub, &msg); + protocol_message_clear(&msg); + } + return 0; +} + +int kcp_hub_close(kcp_hub_t *hub) { + if (hub == NULL) { + return 0; + } + if (!atomic_exchange(&hub->closed, 1)) { + if (hub->relay_fd >= 0) { + close(hub->relay_fd); + hub->relay_fd = -1; + } + } + return 0; +} + +void kcp_hub_free(kcp_hub_t *hub) { + kcp_peer_entry_t *entry; + kcp_peer_entry_t *next; + + if (hub == NULL) { + 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) { + kcp_conn_close(entry->conn); + } + free(entry); + } + pthread_rwlock_destroy(&hub->lock); + free(hub); +} diff --git a/host/OmniSocketGo_add_camera/src/server_udp_hub.c b/host/OmniSocketGo_add_camera/src/server_udp_hub.c new file mode 100644 index 0000000..faf67ed --- /dev/null +++ b/host/OmniSocketGo_add_camera/src/server_udp_hub.c @@ -0,0 +1,181 @@ +#include "server_udp_hub.h" + +#include + +typedef struct udp_peer_entry { + struct udp_peer_entry *next; + char peer_id[OMNI_MAX_PEER_ID]; + struct sockaddr_storage addr; + socklen_t addr_len; +} udp_peer_entry_t; + +struct udp_hub { + udp_conn_t *conn; + pthread_rwlock_t lock; + udp_peer_entry_t *peers; +}; + +static int udp_addr_equal(const struct sockaddr_storage *a, socklen_t a_len, const struct sockaddr_storage *b, socklen_t b_len) { + return a_len == b_len && memcmp(a, b, a_len) == 0; +} + +static udp_peer_entry_t *udp_hub_find_by_id(udp_hub_t *hub, const char *peer_id) { + udp_peer_entry_t *entry; + for (entry = hub->peers; entry != NULL; entry = entry->next) { + if (strcmp(entry->peer_id, peer_id) == 0) { + return entry; + } + } + return NULL; +} + +static udp_peer_entry_t *udp_hub_find_by_addr(udp_hub_t *hub, const struct sockaddr_storage *addr, socklen_t addr_len) { + udp_peer_entry_t *entry; + for (entry = hub->peers; entry != NULL; entry = entry->next) { + if (udp_addr_equal(&entry->addr, entry->addr_len, addr, addr_len)) { + return entry; + } + } + return NULL; +} + +static int udp_hub_send_error(udp_hub_t *hub, const struct sockaddr_storage *addr, socklen_t addr_len, const char *to, const char *message) { + message_t msg; + protocol_message_init(&msg); + msg.type = MSG_TYPE_ERROR; + msg.id = 0; + snprintf(msg.from, sizeof(msg.from), "%s", SERVER_PEER_ID); + snprintf(msg.to, sizeof(msg.to), "%s", to == NULL || to[0] == '\0' ? "unknown" : to); + msg.body = (uint8_t *) omni_strdup(message); + msg.body_len = msg.body == NULL ? 0 : strlen((const char *) msg.body); + if (msg.body == NULL) { + return -1; + } + if (udp_conn_send_to(hub->conn, &msg, (const struct sockaddr *) addr, addr_len) != 0) { + protocol_message_clear(&msg); + return -1; + } + protocol_message_clear(&msg); + return 0; +} + +udp_hub_t *udp_hub_open(const char *listen_addr, latency_logger_t *logger, tx_timestamp_debug_logger_t *debug_logger, int enable_timestamping) { + udp_hub_t *hub = (udp_hub_t *) calloc(1, sizeof(*hub)); + if (hub == NULL) { + return NULL; + } + hub->conn = udp_conn_bind(listen_addr, NULL, enable_timestamping, logger, OMNI_NODE_ROLE_SERVER, "hub", debug_logger); + if (hub->conn == NULL) { + free(hub); + return NULL; + } + pthread_rwlock_init(&hub->lock, NULL); + return hub; +} + +int udp_hub_serve(udp_hub_t *hub) { + message_t msg; + struct sockaddr_storage addr; + socklen_t addr_len; + udp_peer_entry_t *sender; + udp_peer_entry_t *target; + udp_peer_entry_t *entry; + + if (hub == NULL) { + errno = EINVAL; + return -1; + } + + protocol_message_init(&msg); + for (;;) { + protocol_message_clear(&msg); + if (udp_conn_receive(hub->conn, &msg, &addr, &addr_len) != 0) { + return -1; + } + + if (msg.type == MSG_TYPE_REGISTER) { + pthread_rwlock_wrlock(&hub->lock); + entry = udp_hub_find_by_id(hub, msg.from); + if (entry == NULL) { + entry = (udp_peer_entry_t *) calloc(1, sizeof(*entry)); + if (entry == NULL) { + pthread_rwlock_unlock(&hub->lock); + protocol_message_clear(&msg); + return -1; + } + snprintf(entry->peer_id, sizeof(entry->peer_id), "%s", msg.from); + entry->next = hub->peers; + hub->peers = entry; + } + memcpy(&entry->addr, &addr, sizeof(addr)); + entry->addr_len = addr_len; + pthread_rwlock_unlock(&hub->lock); + continue; + } + if (msg.type != MSG_TYPE_TEXT && msg.type != MSG_TYPE_FILE && msg.type != MSG_TYPE_BINARY) { + if (msg.type == MSG_TYPE_ERROR) { + udp_hub_send_error(hub, &addr, addr_len, msg.from, "peers cannot send error messages"); + } else { + char *error_text = omni_strdup_printf("unsupported message type: %s", protocol_message_type_name(msg.type)); + if (error_text != NULL) { + udp_hub_send_error(hub, &addr, addr_len, msg.from, error_text); + free(error_text); + } + } + continue; + } + + pthread_rwlock_rdlock(&hub->lock); + sender = udp_hub_find_by_addr(hub, &addr, addr_len); + if (sender == NULL) { + pthread_rwlock_unlock(&hub->lock); + udp_hub_send_error(hub, &addr, addr_len, msg.from, "not registered; send register first"); + continue; + } + snprintf(msg.from, sizeof(msg.from), "%s", sender->peer_id); + target = udp_hub_find_by_id(hub, msg.to); + if (target == NULL) { + char *error_text; + pthread_rwlock_unlock(&hub->lock); + error_text = omni_strdup_printf("unknown target: %s", msg.to); + if (error_text != NULL) { + udp_hub_send_error(hub, &addr, addr_len, sender->peer_id, error_text); + free(error_text); + } + continue; + } + if (udp_conn_send_to(hub->conn, &msg, (const struct sockaddr *) &target->addr, target->addr_len) != 0) { + char *error_text; + pthread_rwlock_unlock(&hub->lock); + error_text = omni_strdup_printf("failed to forward to %s", msg.to); + if (error_text != NULL) { + udp_hub_send_error(hub, &addr, addr_len, sender->peer_id, error_text); + free(error_text); + } + continue; + } + pthread_rwlock_unlock(&hub->lock); + } +} + +int udp_hub_close(udp_hub_t *hub) { + if (hub == NULL) { + return 0; + } + return udp_conn_close(hub->conn); +} + +void udp_hub_free(udp_hub_t *hub) { + udp_peer_entry_t *entry; + udp_peer_entry_t *next; + if (hub == NULL) { + return; + } + udp_conn_free(hub->conn); + for (entry = hub->peers; entry != NULL; entry = next) { + next = entry->next; + free(entry); + } + pthread_rwlock_destroy(&hub->lock); + free(hub); +} diff --git a/host/OmniSocketGo_add_camera/src/server_udp_relay.c b/host/OmniSocketGo_add_camera/src/server_udp_relay.c new file mode 100644 index 0000000..c562df1 --- /dev/null +++ b/host/OmniSocketGo_add_camera/src/server_udp_relay.c @@ -0,0 +1,613 @@ +#include "server_udp_relay.h" + +#include +#include +#include +#include +#include + +#define UDP_RELAY_BUF_SIZE (64U * 1024U) +#define UDP_RELAY_ROUTE_TIMEOUT_MS 30000U +#define UDP_RELAY_DEFAULT_PACKET_LOG_SAMPLE_EVERY 200U + +struct udp_relay { + int downstream_fd; + int upstream_fd; + struct sockaddr_storage upstream_addr; + socklen_t upstream_addr_len; + char downstream_local_addr[OMNI_MAX_ADDR_TEXT]; + char upstream_local_addr[OMNI_MAX_ADDR_TEXT]; + struct sockaddr_storage client_addr; + socklen_t client_addr_len; + int has_client; + uint32_t client_last_seen_ms; + struct udp_relay_route *routes; + pthread_mutex_t lock; + pthread_mutex_t log_mu; + unsigned int packet_log_sample_every; + atomic_ullong packet_log_counter; + pthread_mutex_t state_mu; + pthread_cond_t state_cond; + pthread_t downstream_thread; + int downstream_thread_started; + pthread_t upstream_thread; + int upstream_thread_started; + int worker_done; + int worker_rc; + int worker_errno; + int closed; +}; + +typedef struct udp_relay_route { + struct udp_relay_route *next; + uint32_t conv; + struct sockaddr_storage client_addr; + socklen_t client_addr_len; + uint32_t last_seen_ms; +} udp_relay_route_t; + +static uint32_t udp_relay_now_ms(void) { + return omni_now_millis32(); +} + +static uint32_t udp_relay_elapsed_ms(uint32_t now_ms, uint32_t then_ms) { + return now_ms - then_ms; +} + +static unsigned int udp_relay_packet_log_sample_every(void) { + const char *raw = getenv("OMNI_RELAY_PACKET_LOG_SAMPLE_EVERY"); + unsigned long parsed; + char *endptr = NULL; + + if (raw == NULL || raw[0] == '\0') { + return UDP_RELAY_DEFAULT_PACKET_LOG_SAMPLE_EVERY; + } + parsed = strtoul(raw, &endptr, 10); + if (endptr == raw || *endptr != '\0') { + return UDP_RELAY_DEFAULT_PACKET_LOG_SAMPLE_EVERY; + } + return (unsigned int) parsed; +} + +static int udp_relay_event_should_always_log(const char *event_name) { + return event_name != NULL && strstr(event_name, "_drop_") != NULL; +} + +static int udp_relay_should_log_packet(udp_relay_t *relay, const char *event_name) { + unsigned long long seq; + + if (relay == NULL) { + return 0; + } + if (udp_relay_event_should_always_log(event_name)) { + return 1; + } + if (relay->packet_log_sample_every == 0U) { + return 0; + } + if (relay->packet_log_sample_every == 1U) { + return 1; + } + seq = atomic_fetch_add_explicit(&relay->packet_log_counter, 1U, memory_order_relaxed) + 1U; + return (seq % (unsigned long long) relay->packet_log_sample_every) == 0U; +} + +static void udp_relay_parse_kcp_summary(const uint8_t *packet, size_t len, int *has_conv, uint32_t *conv, size_t *segment_count) { + size_t offset = 0; + size_t count = 0; + + if (has_conv != NULL) { + *has_conv = 0; + } + if (conv != NULL) { + *conv = 0; + } + if (segment_count != NULL) { + *segment_count = 0; + } + if (packet == NULL || len < 4U) { + return; + } + if (has_conv != NULL) { + *has_conv = 1; + } + if (conv != NULL) { + *conv = (uint32_t) ((unsigned char) packet[0] | + ((unsigned char) packet[1] << 8) | + ((unsigned char) packet[2] << 16) | + ((unsigned char) packet[3] << 24)); + } + while (offset + 24U <= len) { + uint32_t seg_len = (uint32_t) ((unsigned char) packet[offset + 20] | + ((unsigned char) packet[offset + 21] << 8) | + ((unsigned char) packet[offset + 22] << 16) | + ((unsigned char) packet[offset + 23] << 24)); + if (offset + 24U + seg_len > len) { + return; + } + count++; + offset += 24U + seg_len; + } + if (segment_count != NULL) { + *segment_count = count; + } +} + +static void udp_relay_print_packet(udp_relay_t *relay, const char *event_name, const char *local_addr, const struct sockaddr_storage *remote_addr, socklen_t remote_addr_len, const uint8_t *packet, size_t packet_len) { + char remote_addr_text[OMNI_MAX_ADDR_TEXT]; + int64_t ts_unix_nano; + int has_conv = 0; + uint32_t conv = 0; + size_t segment_count = 0; + + if (relay == NULL) { + return; + } + if (!udp_relay_should_log_packet(relay, event_name)) { + return; + } + + if (remote_addr != NULL && remote_addr_len > 0) { + omni_sockaddr_to_string((const struct sockaddr *) remote_addr, remote_addr_len, remote_addr_text, sizeof(remote_addr_text)); + } else { + remote_addr_text[0] = '\0'; + } + ts_unix_nano = omni_now_unix_nano(); + udp_relay_parse_kcp_summary(packet, packet_len, &has_conv, &conv, &segment_count); + + pthread_mutex_lock(&relay->log_mu); + if (has_conv) { + fprintf(stderr, "[relay] ts=%" PRId64 " event=%s local=%s remote=%s bytes=%zu conv=%" PRIu32 " segs=%zu\n", + ts_unix_nano, + event_name == NULL ? "" : event_name, + local_addr == NULL ? "" : local_addr, + remote_addr_text, + packet_len, + conv, + segment_count); + } else { + fprintf(stderr, "[relay] ts=%" PRId64 " event=%s local=%s remote=%s bytes=%zu\n", + ts_unix_nano, + event_name == NULL ? "" : event_name, + local_addr == NULL ? "" : local_addr, + remote_addr_text, + packet_len); + } + fflush(stderr); + pthread_mutex_unlock(&relay->log_mu); +} + +static int udp_relay_is_closed(udp_relay_t *relay) { + int closed; + + pthread_mutex_lock(&relay->state_mu); + closed = relay->closed; + pthread_mutex_unlock(&relay->state_mu); + return closed; +} + +static void udp_relay_note_result(udp_relay_t *relay, int rc, int errnum) { + pthread_mutex_lock(&relay->state_mu); + if (!relay->worker_done) { + relay->worker_done = 1; + relay->worker_rc = rc; + relay->worker_errno = errnum; + pthread_cond_signal(&relay->state_cond); + } + pthread_mutex_unlock(&relay->state_mu); +} + +static void udp_relay_record_client(udp_relay_t *relay, const struct sockaddr_storage *addr, socklen_t addr_len) { + pthread_mutex_lock(&relay->lock); + memcpy(&relay->client_addr, addr, sizeof(*addr)); + relay->client_addr_len = addr_len; + relay->has_client = 1; + relay->client_last_seen_ms = udp_relay_now_ms(); + pthread_mutex_unlock(&relay->lock); +} + +static void udp_relay_prune_routes_locked(udp_relay_t *relay, uint32_t now_ms) { + udp_relay_route_t *prev = NULL; + udp_relay_route_t *route; + + if (relay == NULL) { + return; + } + + route = relay->routes; + while (route != NULL) { + udp_relay_route_t *next = route->next; + + if (udp_relay_elapsed_ms(now_ms, route->last_seen_ms) >= UDP_RELAY_ROUTE_TIMEOUT_MS) { + if (prev == NULL) { + relay->routes = next; + } else { + prev->next = next; + } + free(route); + route = next; + continue; + } + + prev = route; + route = next; + } + + if (relay->has_client && udp_relay_elapsed_ms(now_ms, relay->client_last_seen_ms) >= UDP_RELAY_ROUTE_TIMEOUT_MS) { + relay->has_client = 0; + relay->client_addr_len = 0; + memset(&relay->client_addr, 0, sizeof(relay->client_addr)); + } +} + +static int udp_relay_record_route(udp_relay_t *relay, uint32_t conv, const struct sockaddr_storage *addr, socklen_t addr_len) { + udp_relay_route_t *route; + uint32_t now_ms; + + if (relay == NULL || addr == NULL || addr_len == 0) { + errno = EINVAL; + return -1; + } + + now_ms = udp_relay_now_ms(); + pthread_mutex_lock(&relay->lock); + udp_relay_prune_routes_locked(relay, now_ms); + for (route = relay->routes; route != NULL; route = route->next) { + if (route->conv == conv) { + memcpy(&route->client_addr, addr, sizeof(*addr)); + route->client_addr_len = addr_len; + route->last_seen_ms = now_ms; + pthread_mutex_unlock(&relay->lock); + return 0; + } + } + + route = (udp_relay_route_t *) calloc(1, sizeof(*route)); + if (route == NULL) { + pthread_mutex_unlock(&relay->lock); + return -1; + } + route->conv = conv; + memcpy(&route->client_addr, addr, sizeof(*addr)); + route->client_addr_len = addr_len; + route->last_seen_ms = now_ms; + route->next = relay->routes; + relay->routes = route; + pthread_mutex_unlock(&relay->lock); + return 0; +} + +static int udp_relay_copy_client(udp_relay_t *relay, struct sockaddr_storage *addr, socklen_t *addr_len) { + int has_client; + uint32_t now_ms; + + now_ms = udp_relay_now_ms(); + pthread_mutex_lock(&relay->lock); + udp_relay_prune_routes_locked(relay, now_ms); + has_client = relay->has_client; + if (has_client) { + memcpy(addr, &relay->client_addr, sizeof(*addr)); + *addr_len = relay->client_addr_len; + } + pthread_mutex_unlock(&relay->lock); + return has_client; +} + +static int udp_relay_copy_route(udp_relay_t *relay, uint32_t conv, struct sockaddr_storage *addr, socklen_t *addr_len) { + udp_relay_route_t *route; + uint32_t now_ms; + + now_ms = udp_relay_now_ms(); + pthread_mutex_lock(&relay->lock); + udp_relay_prune_routes_locked(relay, now_ms); + for (route = relay->routes; route != NULL; route = route->next) { + if (route->conv == conv) { + memcpy(addr, &route->client_addr, sizeof(*addr)); + *addr_len = route->client_addr_len; + pthread_mutex_unlock(&relay->lock); + return 1; + } + } + pthread_mutex_unlock(&relay->lock); + return 0; +} + +static void udp_relay_clear_routes(udp_relay_t *relay) { + udp_relay_route_t *route; + udp_relay_route_t *next; + + if (relay == NULL) { + return; + } + + pthread_mutex_lock(&relay->lock); + route = relay->routes; + relay->routes = NULL; + pthread_mutex_unlock(&relay->lock); + + while (route != NULL) { + next = route->next; + free(route); + route = next; + } +} + +static void *udp_relay_forward_downstream_to_upstream(void *arg) { + udp_relay_t *relay = (udp_relay_t *) arg; + uint8_t buffer[UDP_RELAY_BUF_SIZE]; + + for (;;) { + struct sockaddr_storage source; + socklen_t source_len = sizeof(source); + ssize_t n = recvfrom(relay->downstream_fd, buffer, sizeof(buffer), 0, (struct sockaddr *) &source, &source_len); + int has_conv = 0; + uint32_t conv = 0; + + if (n < 0) { + int errnum = errno; + if (errnum == EINTR) { + continue; + } + if (udp_relay_is_closed(relay)) { + udp_relay_note_result(relay, 0, 0); + } else { + udp_relay_note_result(relay, -1, errnum); + } + return NULL; + } + + udp_relay_record_client(relay, &source, source_len); + udp_relay_parse_kcp_summary(buffer, (size_t) n, &has_conv, &conv, NULL); + if (has_conv) { + (void) udp_relay_record_route(relay, conv, &source, source_len); + } + udp_relay_print_packet(relay, "relay_downstream_rx", relay->downstream_local_addr, &source, source_len, buffer, (size_t) n); + for (;;) { + if (send(relay->upstream_fd, buffer, (size_t) n, 0) >= 0) { + udp_relay_print_packet(relay, "relay_upstream_tx", relay->upstream_local_addr, &relay->upstream_addr, relay->upstream_addr_len, buffer, (size_t) n); + break; + } + { + int errnum = errno; + if (errnum == EINTR) { + continue; + } + if (udp_relay_is_closed(relay)) { + udp_relay_note_result(relay, 0, 0); + } else { + udp_relay_note_result(relay, -1, errnum); + } + return NULL; + } + } + } +} + +static void *udp_relay_forward_upstream_to_downstream(void *arg) { + udp_relay_t *relay = (udp_relay_t *) arg; + uint8_t buffer[UDP_RELAY_BUF_SIZE]; + + for (;;) { + struct sockaddr_storage client_addr; + socklen_t client_addr_len = 0; + ssize_t n = recv(relay->upstream_fd, buffer, sizeof(buffer), 0); + int has_conv = 0; + uint32_t conv = 0; + + if (n < 0) { + int errnum = errno; + if (errnum == EINTR) { + continue; + } + if (udp_relay_is_closed(relay)) { + udp_relay_note_result(relay, 0, 0); + } else { + udp_relay_note_result(relay, -1, errnum); + } + return NULL; + } + + udp_relay_print_packet(relay, "relay_upstream_rx", relay->upstream_local_addr, &relay->upstream_addr, relay->upstream_addr_len, buffer, (size_t) n); + udp_relay_parse_kcp_summary(buffer, (size_t) n, &has_conv, &conv, NULL); + if (has_conv && !udp_relay_copy_route(relay, conv, &client_addr, &client_addr_len)) { + udp_relay_print_packet(relay, "relay_upstream_drop_unknown_conv", relay->upstream_local_addr, &relay->upstream_addr, relay->upstream_addr_len, buffer, (size_t) n); + continue; + } + if (!has_conv && !udp_relay_copy_client(relay, &client_addr, &client_addr_len)) { + udp_relay_print_packet(relay, "relay_upstream_drop_no_client", relay->upstream_local_addr, &relay->upstream_addr, relay->upstream_addr_len, buffer, (size_t) n); + continue; + } + + for (;;) { + if (sendto(relay->downstream_fd, buffer, (size_t) n, 0, (struct sockaddr *) &client_addr, client_addr_len) >= 0) { + udp_relay_print_packet(relay, "relay_downstream_tx", relay->downstream_local_addr, &client_addr, client_addr_len, buffer, (size_t) n); + break; + } + { + int errnum = errno; + if (errnum == EINTR) { + continue; + } + if (udp_relay_is_closed(relay)) { + udp_relay_note_result(relay, 0, 0); + } else { + udp_relay_note_result(relay, -1, errnum); + } + return NULL; + } + } + } +} + +static void udp_relay_join_threads(udp_relay_t *relay) { + if (relay->downstream_thread_started) { + pthread_join(relay->downstream_thread, NULL); + relay->downstream_thread_started = 0; + } + if (relay->upstream_thread_started) { + pthread_join(relay->upstream_thread, NULL); + relay->upstream_thread_started = 0; + } +} + +udp_relay_t *udp_relay_open(const char *listen_addr, const char *upstream_addr) { + struct sockaddr_storage listen_ss; + struct sockaddr_storage upstream_ss; + struct sockaddr_storage downstream_local_ss; + struct sockaddr_storage upstream_local_ss; + socklen_t listen_len; + socklen_t upstream_len; + socklen_t downstream_local_len = sizeof(downstream_local_ss); + socklen_t upstream_local_len = sizeof(upstream_local_ss); + int family; + int fd_listen = -1; + int fd_upstream = -1; + udp_relay_t *relay = NULL; + + if (omni_parse_sockaddr(listen_addr, 1, &listen_ss, &listen_len, &family) != 0 || + omni_parse_sockaddr(upstream_addr, 0, &upstream_ss, &upstream_len, &family) != 0) { + return NULL; + } + fd_listen = socket(listen_ss.ss_family, SOCK_DGRAM, 0); + if (fd_listen < 0) { + return NULL; + } + if (bind(fd_listen, (struct sockaddr *) &listen_ss, listen_len) != 0) { + close(fd_listen); + return NULL; + } + fd_upstream = socket(upstream_ss.ss_family, SOCK_DGRAM, 0); + if (fd_upstream < 0) { + close(fd_listen); + return NULL; + } + if (connect(fd_upstream, (struct sockaddr *) &upstream_ss, upstream_len) != 0) { + close(fd_upstream); + close(fd_listen); + return NULL; + } + relay = (udp_relay_t *) calloc(1, sizeof(*relay)); + if (relay == NULL) { + close(fd_upstream); + close(fd_listen); + return NULL; + } + relay->downstream_fd = fd_listen; + relay->upstream_fd = fd_upstream; + memcpy(&relay->upstream_addr, &upstream_ss, sizeof(upstream_ss)); + relay->upstream_addr_len = upstream_len; + if (getsockname(fd_listen, (struct sockaddr *) &downstream_local_ss, &downstream_local_len) == 0) { + omni_sockaddr_to_string((const struct sockaddr *) &downstream_local_ss, downstream_local_len, relay->downstream_local_addr, sizeof(relay->downstream_local_addr)); + } else { + snprintf(relay->downstream_local_addr, sizeof(relay->downstream_local_addr), "%s", listen_addr == NULL ? "" : listen_addr); + } + if (getsockname(fd_upstream, (struct sockaddr *) &upstream_local_ss, &upstream_local_len) == 0) { + omni_sockaddr_to_string((const struct sockaddr *) &upstream_local_ss, upstream_local_len, relay->upstream_local_addr, sizeof(relay->upstream_local_addr)); + } else { + snprintf(relay->upstream_local_addr, sizeof(relay->upstream_local_addr), "%s", listen_addr == NULL ? "" : listen_addr); + } + pthread_mutex_init(&relay->lock, NULL); + pthread_mutex_init(&relay->log_mu, NULL); + relay->packet_log_sample_every = udp_relay_packet_log_sample_every(); + atomic_init(&relay->packet_log_counter, 0U); + pthread_mutex_init(&relay->state_mu, NULL); + pthread_cond_init(&relay->state_cond, NULL); + return relay; +} + +int udp_relay_serve(udp_relay_t *relay) { + int thread_rc; + int rc; + int errnum; + + if (relay == NULL) { + errno = EINVAL; + return -1; + } + if (udp_relay_is_closed(relay)) { + errno = ECANCELED; + return -1; + } + + pthread_mutex_lock(&relay->state_mu); + relay->worker_done = 0; + relay->worker_rc = 0; + relay->worker_errno = 0; + pthread_mutex_unlock(&relay->state_mu); + + thread_rc = pthread_create(&relay->downstream_thread, NULL, udp_relay_forward_downstream_to_upstream, relay); + if (thread_rc != 0) { + errno = thread_rc; + return -1; + } + relay->downstream_thread_started = 1; + + thread_rc = pthread_create(&relay->upstream_thread, NULL, udp_relay_forward_upstream_to_downstream, relay); + if (thread_rc != 0) { + errno = thread_rc; + udp_relay_close(relay); + udp_relay_join_threads(relay); + return -1; + } + relay->upstream_thread_started = 1; + + pthread_mutex_lock(&relay->state_mu); + while (!relay->worker_done) { + pthread_cond_wait(&relay->state_cond, &relay->state_mu); + } + rc = relay->worker_rc; + errnum = relay->worker_errno; + pthread_mutex_unlock(&relay->state_mu); + + udp_relay_close(relay); + udp_relay_join_threads(relay); + + if (rc != 0 && errnum != 0) { + errno = errnum; + } + return rc; +} + +int udp_relay_close(udp_relay_t *relay) { + int downstream_fd; + int upstream_fd; + + if (relay == NULL) { + return 0; + } + + pthread_mutex_lock(&relay->state_mu); + if (relay->closed) { + pthread_mutex_unlock(&relay->state_mu); + return 0; + } + relay->closed = 1; + downstream_fd = relay->downstream_fd; + upstream_fd = relay->upstream_fd; + relay->downstream_fd = -1; + relay->upstream_fd = -1; + pthread_cond_broadcast(&relay->state_cond); + pthread_mutex_unlock(&relay->state_mu); + + if (downstream_fd >= 0) { + close(downstream_fd); + } + if (upstream_fd >= 0) { + close(upstream_fd); + } + return 0; +} + +void udp_relay_free(udp_relay_t *relay) { + if (relay == NULL) { + return; + } + udp_relay_close(relay); + udp_relay_join_threads(relay); + udp_relay_clear_routes(relay); + pthread_mutex_destroy(&relay->lock); + pthread_mutex_destroy(&relay->log_mu); + pthread_cond_destroy(&relay->state_cond); + pthread_mutex_destroy(&relay->state_mu); + free(relay); +} diff --git a/host/OmniSocketGo_add_camera/src/transport_kcp.c b/host/OmniSocketGo_add_camera/src/transport_kcp.c new file mode 100644 index 0000000..fbb3a51 --- /dev/null +++ b/host/OmniSocketGo_add_camera/src/transport_kcp.c @@ -0,0 +1,2061 @@ +#include "transport_kcp.h" + +#include "ikcp.h" +#include "linux_timestamping.h" + +#include +#include +#include +#include + +#define KCP_RECV_CHUNK_SIZE (32U * 1024U) + +typedef struct kcp_packet_debug_pending { + struct kcp_packet_debug_pending *next; + uint32_t tx_id; + struct sockaddr_storage remote_addr; + socklen_t remote_addr_len; + int packet_bytes; + int has_conv; + uint32_t conv; + kcp_packet_debug_segment_t *segments; + size_t segment_count; + int saw_sched; + int saw_software; +} kcp_packet_debug_pending_t; + +typedef struct kcp_socket_debug_state { + int fd; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + kcp_packet_debug_logger_t *logger; + pthread_mutex_t write_mu; + pthread_mutex_t pending_mu; + pthread_t errqueue_thread; + int errqueue_thread_started; + uint32_t next_tx_id; + kcp_packet_debug_pending_t *pending_head; + atomic_int closed; + atomic_int last_send_errno; +} kcp_socket_debug_state_t; + +typedef struct kcp_session_entry kcp_session_entry_t; +typedef struct kcp_process_sampler kcp_process_sampler_t; + +struct kcp_conn { + ikcpcb *kcp; + int fd; + int is_client; + int owns_socket; + int socket_closed; + atomic_int closed; + struct sockaddr_storage remote_addr; + socklen_t remote_addr_len; + pthread_mutex_t kcp_mu; + pthread_mutex_t close_mu; + pthread_cond_t rx_cond; + pthread_t recv_thread; + int recv_thread_started; + pthread_t update_thread; + int update_thread_started; + pthread_t stats_thread; + 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; + uint64_t pending_out_pkts; + uint64_t pending_in_segs; + uint64_t pending_out_segs; + uint64_t pending_in_errs; + uint64_t pending_kcp_in_errs; + protocol_frame_decoder_t decoder; + int32_t min_srtt_ms; + uint32_t last_feedback_ms; + uint8_t scratch[KCP_RECV_CHUNK_SIZE]; + latency_logger_t *logger; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + kcp_session_stats_logger_t *stats_logger; + int stats_interval_ms; + kcp_process_sampler_t *process_sampler; + kcp_socket_debug_state_t *sock_state; + struct kcp_listener *listener; + struct kcp_conn *accept_next; + struct kcp_conn *process_next; +}; + +struct kcp_listener { + int fd; + int closed; + pthread_mutex_t lock; + pthread_mutex_t accept_mu; + pthread_cond_t accept_cond; + pthread_t recv_thread; + int recv_thread_started; + kcp_session_entry_t *sessions; + kcp_conn_t *accept_head; + kcp_conn_t *accept_tail; + kcp_socket_debug_state_t sock_state; +}; + +struct kcp_session_entry { + uint32_t conv; + kcp_conn_t *conn; + kcp_session_entry_t *next; +}; + +struct kcp_process_sampler { + kcp_process_sampler_t *next; + kcp_session_stats_logger_t *logger; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + int stats_interval_ms; + pthread_mutex_t lock; + pthread_cond_t cond; + pthread_t thread; + int thread_started; + int stopped; + int refcount; + int request_pending; + uint64_t pending_request_id; + uint64_t completed_request_id; + char pending_reason[32]; + kcp_conn_t *members; + uint64_t prev_bytes_sent; + uint64_t prev_bytes_received; + uint64_t prev_in_pkts; + uint64_t prev_out_pkts; + uint64_t prev_in_segs; + 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; + atomic_uint_fast64_t out_pkts; + atomic_uint_fast64_t in_segs; + atomic_uint_fast64_t out_segs; + atomic_uint_fast64_t in_errs; + atomic_uint_fast64_t kcp_in_errs; + atomic_uint_fast64_t curr_estab; +}; + +static pthread_mutex_t g_kcp_process_sampler_mu = PTHREAD_MUTEX_INITIALIZER; +static kcp_process_sampler_t *g_kcp_process_samplers = NULL; + +void kcp_conn_options_init(kcp_conn_options_t *options) { + if (options == NULL) { + return; + } + memset(options, 0, sizeof(*options)); + options->nodelay = KCP_DEFAULT_NODELAY; + options->interval_ms = KCP_DEFAULT_INTERVAL_MS; + options->resend = KCP_DEFAULT_RESEND; + options->nc = KCP_DEFAULT_NC; + options->sndwnd = KCP_DEFAULT_SND_WND; + options->rcvwnd = KCP_DEFAULT_RCV_WND; + options->mtu = KCP_DEFAULT_MTU; +} + +void kcp_conn_options_set_control_defaults(kcp_conn_options_t *options) { + if (options == NULL) { + return; + } + memset(options, 0, sizeof(*options)); + options->nodelay = KCP_CONTROL_NODELAY; + options->interval_ms = KCP_CONTROL_INTERVAL_MS; + options->resend = KCP_CONTROL_RESEND; + options->nc = KCP_CONTROL_NC; + options->sndwnd = KCP_CONTROL_SND_WND; + options->rcvwnd = KCP_CONTROL_RCV_WND; + options->mtu = KCP_CONTROL_MTU; +} + +void kcp_conn_options_set_video_defaults(kcp_conn_options_t *options) { + if (options == NULL) { + return; + } + memset(options, 0, sizeof(*options)); + options->nodelay = KCP_VIDEO_NODELAY; + options->interval_ms = KCP_VIDEO_INTERVAL_MS; + options->resend = KCP_VIDEO_RESEND; + options->nc = KCP_VIDEO_NC; + options->sndwnd = KCP_VIDEO_SND_WND; + options->rcvwnd = KCP_VIDEO_RCV_WND; + 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; + return -1; + } + if (options->interval_ms <= 0 || options->sndwnd <= 0 || options->rcvwnd <= 0 || options->mtu <= 0) { + errno = EINVAL; + return -1; + } + return 0; +} + +static int kcp_conn_apply_options_locked(kcp_conn_t *conn, const kcp_conn_options_t *options) { + if (conn == NULL || conn->kcp == NULL || kcp_conn_validate_options(options) != 0) { + return -1; + } + if (ikcp_wndsize(conn->kcp, options->sndwnd, options->rcvwnd) != 0) { + errno = EINVAL; + return -1; + } + if (ikcp_setmtu(conn->kcp, options->mtu) != 0) { + errno = EINVAL; + return -1; + } + if (ikcp_nodelay(conn->kcp, options->nodelay, options->interval_ms, options->resend, options->nc) != 0) { + errno = EINVAL; + return -1; + } + conn->kcp->stream = 1; + conn->options = *options; + conn->update_interval_ms = options->interval_ms; + return 0; +} + +static void kcp_parse_packet_segments(const uint8_t *packet, size_t len, uint32_t *conv, kcp_packet_debug_segment_t **segments, size_t *segment_count) { + size_t offset = 0; + size_t count = 0; + kcp_packet_debug_segment_t *items = NULL; + + if (conv != NULL) { + *conv = 0; + } + if (segments != NULL) { + *segments = NULL; + } + if (segment_count != NULL) { + *segment_count = 0; + } + if (len < 4) { + return; + } + if (conv != NULL) { + *conv = (uint32_t) ((unsigned char) packet[0] | + ((unsigned char) packet[1] << 8) | + ((unsigned char) packet[2] << 16) | + ((unsigned char) packet[3] << 24)); + } + while (offset + 24U <= len) { + uint32_t seg_len = (uint32_t) ((unsigned char) packet[offset + 20] | + ((unsigned char) packet[offset + 21] << 8) | + ((unsigned char) packet[offset + 22] << 16) | + ((unsigned char) packet[offset + 23] << 24)); + if (offset + 24U + seg_len > len) { + free(items); + return; + } + if (segments != NULL) { + kcp_packet_debug_segment_t *next = (kcp_packet_debug_segment_t *) realloc(items, (count + 1U) * sizeof(*items)); + if (next == NULL) { + free(items); + return; + } + items = next; + items[count].cmd = packet[offset + 4]; + items[count].frg = packet[offset + 5]; + items[count].wnd = (uint16_t) ((unsigned char) packet[offset + 6] | ((unsigned char) packet[offset + 7] << 8)); + items[count].sn = (uint32_t) ((unsigned char) packet[offset + 12] | + ((unsigned char) packet[offset + 13] << 8) | + ((unsigned char) packet[offset + 14] << 16) | + ((unsigned char) packet[offset + 15] << 24)); + items[count].una = (uint32_t) ((unsigned char) packet[offset + 16] | + ((unsigned char) packet[offset + 17] << 8) | + ((unsigned char) packet[offset + 18] << 16) | + ((unsigned char) packet[offset + 19] << 24)); + items[count].len = seg_len; + } + count++; + offset += 24U + seg_len; + } + if (segments != NULL) { + *segments = items; + } else { + free(items); + } + if (segment_count != NULL) { + *segment_count = count; + } +} + +static uint64_t kcp_counter_diff(uint64_t previous, uint64_t current) { + return current < previous ? 0 : current - previous; +} + +static void kcp_conn_update_min_srtt_locked(kcp_conn_t *conn) { + int32_t srtt_ms; + + if (conn == NULL || conn->kcp == NULL) { + return; + } + srtt_ms = conn->kcp->rx_srtt; + if (srtt_ms > 0 && (conn->min_srtt_ms <= 0 || srtt_ms < conn->min_srtt_ms)) { + conn->min_srtt_ms = srtt_ms; + } +} + +static void kcp_conn_note_feedback_locked(kcp_conn_t *conn) { + if (conn == NULL) { + return; + } + conn->last_feedback_ms = omni_now_millis32(); + kcp_conn_update_min_srtt_locked(conn); +} + +static int kcp_process_sampler_matches(const kcp_process_sampler_t *sampler, kcp_session_stats_logger_t *logger, const char *node_role, const char *node_id, int stats_interval_ms) { + if (sampler == NULL) { + return 0; + } + return sampler->logger == logger && + sampler->stats_interval_ms == stats_interval_ms && + strcmp(sampler->node_role, node_role == NULL ? "" : node_role) == 0 && + strcmp(sampler->node_id, node_id == NULL ? "" : node_id) == 0; +} + +static void kcp_process_sampler_record_send(kcp_process_sampler_t *sampler, int packet_bytes, size_t segments) { + if (sampler == NULL) { + return; + } + atomic_fetch_add_explicit(&sampler->bytes_sent, (uint64_t) packet_bytes, memory_order_relaxed); + atomic_fetch_add_explicit(&sampler->out_pkts, 1, memory_order_relaxed); + atomic_fetch_add_explicit(&sampler->out_segs, (uint64_t) segments, memory_order_relaxed); +} + +static void kcp_process_sampler_record_input(kcp_process_sampler_t *sampler, int packet_bytes, size_t segments) { + if (sampler == NULL) { + return; + } + atomic_fetch_add_explicit(&sampler->bytes_received, (uint64_t) packet_bytes, memory_order_relaxed); + atomic_fetch_add_explicit(&sampler->in_pkts, 1, memory_order_relaxed); + atomic_fetch_add_explicit(&sampler->in_segs, (uint64_t) segments, memory_order_relaxed); +} + +static void kcp_process_sampler_record_error(kcp_process_sampler_t *sampler) { + if (sampler == NULL) { + return; + } + atomic_fetch_add_explicit(&sampler->in_errs, 1, memory_order_relaxed); + atomic_fetch_add_explicit(&sampler->kcp_in_errs, 1, memory_order_relaxed); +} + +static void kcp_conn_record_send(kcp_conn_t *conn, int packet_bytes, size_t segments) { + 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; + } + conn->pending_bytes_sent += (uint64_t) packet_bytes; + conn->pending_out_pkts += 1; + conn->pending_out_segs += (uint64_t) segments; +} + +static void kcp_conn_record_input(kcp_conn_t *conn, int packet_bytes, size_t segments) { + if (conn == NULL) { + return; + } + if (conn->process_sampler != NULL) { + kcp_process_sampler_record_input(conn->process_sampler, packet_bytes, segments); + return; + } + conn->pending_bytes_received += (uint64_t) packet_bytes; + conn->pending_in_pkts += 1; + conn->pending_in_segs += (uint64_t) segments; +} + +static void kcp_conn_record_error(kcp_conn_t *conn) { + if (conn == NULL) { + return; + } + if (conn->process_sampler != NULL) { + kcp_process_sampler_record_error(conn->process_sampler); + return; + } + conn->pending_in_errs += 1; + conn->pending_kcp_in_errs += 1; +} + +static void kcp_process_sampler_curr_estab_inc(kcp_process_sampler_t *sampler) { + if (sampler == NULL) { + return; + } + atomic_fetch_add_explicit(&sampler->curr_estab, 1, memory_order_relaxed); +} + +static void kcp_process_sampler_curr_estab_dec(kcp_process_sampler_t *sampler) { + uint_fast64_t current; + + if (sampler == NULL) { + return; + } + current = atomic_load_explicit(&sampler->curr_estab, memory_order_relaxed); + while (current > 0) { + if (atomic_compare_exchange_weak_explicit(&sampler->curr_estab, ¤t, current - 1U, memory_order_relaxed, memory_order_relaxed)) { + return; + } + } +} + +static void kcp_process_sampler_add_conn(kcp_process_sampler_t *sampler, kcp_conn_t *conn) { + if (sampler == NULL || conn == NULL) { + return; + } + pthread_mutex_lock(&sampler->lock); + conn->process_next = sampler->members; + sampler->members = conn; + pthread_mutex_unlock(&sampler->lock); + kcp_process_sampler_curr_estab_inc(sampler); +} + +static void kcp_process_sampler_remove_conn(kcp_process_sampler_t *sampler, kcp_conn_t *conn) { + kcp_conn_t *prev = NULL; + kcp_conn_t *cur; + + if (sampler == NULL || conn == NULL) { + return; + } + pthread_mutex_lock(&sampler->lock); + for (cur = sampler->members; cur != NULL; cur = cur->process_next) { + if (cur == conn) { + if (prev == NULL) { + sampler->members = cur->process_next; + } else { + prev->process_next = cur->process_next; + } + conn->process_next = NULL; + break; + } + prev = cur; + } + pthread_mutex_unlock(&sampler->lock); + if (cur == conn) { + kcp_process_sampler_curr_estab_dec(sampler); + } +} + +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) { + *snd_queue = 0; + } + if (rcv_queue != NULL) { + *rcv_queue = 0; + } + 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; + } + + pthread_mutex_lock(&sampler->lock); + for (conn = sampler->members; conn != NULL; conn = conn->process_next) { + pthread_mutex_lock(&conn->kcp_mu); + if (conn->kcp != NULL) { + if (snd_queue != NULL) { + *snd_queue += conn->kcp->nsnd_que; + } + if (rcv_queue != NULL) { + *rcv_queue += conn->kcp->nrcv_que; + } + 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); + } + pthread_mutex_unlock(&sampler->lock); +} + +static void kcp_process_sampler_log_snapshot(kcp_process_sampler_t *sampler, const char *reason) { + kcp_session_stats_record_t record; + uint64_t bytes_sent; + uint64_t bytes_received; + uint64_t in_pkts; + uint64_t out_pkts; + uint64_t in_segs; + uint64_t out_segs; + uint64_t in_errs; + uint64_t kcp_in_errs; + 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; + } + + bytes_sent = atomic_load_explicit(&sampler->bytes_sent, memory_order_relaxed); + bytes_received = atomic_load_explicit(&sampler->bytes_received, memory_order_relaxed); + in_pkts = atomic_load_explicit(&sampler->in_pkts, memory_order_relaxed); + out_pkts = atomic_load_explicit(&sampler->out_pkts, memory_order_relaxed); + in_segs = atomic_load_explicit(&sampler->in_segs, memory_order_relaxed); + 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, + &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); + snprintf(record.node_role, sizeof(record.node_role), "%s", sampler->node_role); + snprintf(record.node_id, sizeof(record.node_id), "%s", sampler->node_id); + snprintf(record.sample_reason, sizeof(record.sample_reason), "%s", reason == NULL ? "" : reason); + record.ts_unix_nano = omni_now_unix_nano(); + + record.has_bytes_sent = 1; + record.bytes_sent = kcp_counter_diff(sampler->prev_bytes_sent, bytes_sent); + record.has_bytes_received = 1; + record.bytes_received = kcp_counter_diff(sampler->prev_bytes_received, bytes_received); + record.has_in_pkts = 1; + record.in_pkts = kcp_counter_diff(sampler->prev_in_pkts, in_pkts); + record.has_out_pkts = 1; + record.out_pkts = kcp_counter_diff(sampler->prev_out_pkts, out_pkts); + record.has_in_segs = 1; + 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; + record.kcp_in_errs = kcp_counter_diff(sampler->prev_kcp_in_errs, kcp_in_errs); + record.has_ring_buffer_snd_queue = 1; + record.ring_buffer_snd_queue = snd_queue; + record.has_ring_buffer_rcv_queue = 1; + record.ring_buffer_rcv_queue = rcv_queue; + record.has_ring_buffer_snd_buffer = 1; + record.ring_buffer_snd_buffer = snd_buffer; + record.has_curr_estab = 1; + record.curr_estab = atomic_load_explicit(&sampler->curr_estab, memory_order_relaxed); + + sampler->prev_bytes_sent = bytes_sent; + sampler->prev_bytes_received = bytes_received; + sampler->prev_in_pkts = in_pkts; + 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; + + (void) kcp_session_stats_log(sampler->logger, &record); +} + +static void *kcp_process_sampler_thread_main(void *arg) { + kcp_process_sampler_t *sampler = (kcp_process_sampler_t *) arg; + + for (;;) { + int has_request = 0; + uint64_t request_id = 0; + char reason[32]; + struct timespec deadline; + + clock_gettime(CLOCK_REALTIME, &deadline); + deadline.tv_sec += sampler->stats_interval_ms / 1000; + deadline.tv_nsec += (long) (sampler->stats_interval_ms % 1000) * 1000000L; + if (deadline.tv_nsec >= 1000000000L) { + deadline.tv_sec += 1; + deadline.tv_nsec -= 1000000000L; + } + + pthread_mutex_lock(&sampler->lock); + while (!sampler->stopped && !sampler->request_pending) { + int wait_rc = pthread_cond_timedwait(&sampler->cond, &sampler->lock, &deadline); + if (wait_rc == ETIMEDOUT) { + break; + } + } + if (sampler->stopped) { + pthread_mutex_unlock(&sampler->lock); + return NULL; + } + if (sampler->request_pending) { + has_request = 1; + request_id = sampler->pending_request_id; + snprintf(reason, sizeof(reason), "%s", sampler->pending_reason); + sampler->request_pending = 0; + } else { + snprintf(reason, sizeof(reason), "%s", "periodic"); + } + pthread_mutex_unlock(&sampler->lock); + + kcp_process_sampler_log_snapshot(sampler, reason); + + if (has_request) { + pthread_mutex_lock(&sampler->lock); + if (request_id > sampler->completed_request_id) { + sampler->completed_request_id = request_id; + } + pthread_cond_broadcast(&sampler->cond); + pthread_mutex_unlock(&sampler->lock); + } + } +} + +static kcp_process_sampler_t *kcp_process_sampler_acquire(kcp_session_stats_logger_t *logger, const char *node_role, const char *node_id, int stats_interval_ms) { + kcp_process_sampler_t *sampler; + + if (logger == NULL) { + return NULL; + } + + pthread_mutex_lock(&g_kcp_process_sampler_mu); + for (sampler = g_kcp_process_samplers; sampler != NULL; sampler = sampler->next) { + if (kcp_process_sampler_matches(sampler, logger, node_role, node_id, stats_interval_ms)) { + sampler->refcount++; + pthread_mutex_unlock(&g_kcp_process_sampler_mu); + return sampler; + } + } + + sampler = (kcp_process_sampler_t *) calloc(1, sizeof(*sampler)); + if (sampler == NULL) { + pthread_mutex_unlock(&g_kcp_process_sampler_mu); + return NULL; + } + + sampler->logger = logger; + sampler->stats_interval_ms = stats_interval_ms > 0 ? stats_interval_ms : KCP_DEFAULT_STATS_INTERVAL_MS; + sampler->refcount = 1; + snprintf(sampler->node_role, sizeof(sampler->node_role), "%s", node_role == NULL ? "" : node_role); + snprintf(sampler->node_id, sizeof(sampler->node_id), "%s", node_id == NULL ? "" : node_id); + pthread_mutex_init(&sampler->lock, NULL); + pthread_cond_init(&sampler->cond, NULL); + if (pthread_create(&sampler->thread, NULL, kcp_process_sampler_thread_main, sampler) != 0) { + pthread_cond_destroy(&sampler->cond); + pthread_mutex_destroy(&sampler->lock); + free(sampler); + pthread_mutex_unlock(&g_kcp_process_sampler_mu); + return NULL; + } + sampler->thread_started = 1; + sampler->next = g_kcp_process_samplers; + g_kcp_process_samplers = sampler; + pthread_mutex_unlock(&g_kcp_process_sampler_mu); + return sampler; +} + +static void kcp_process_sampler_release(kcp_process_sampler_t *sampler) { + kcp_process_sampler_t **cursor; + + if (sampler == NULL) { + return; + } + + pthread_mutex_lock(&g_kcp_process_sampler_mu); + sampler->refcount--; + if (sampler->refcount > 0) { + pthread_mutex_unlock(&g_kcp_process_sampler_mu); + return; + } + for (cursor = &g_kcp_process_samplers; *cursor != NULL; cursor = &(*cursor)->next) { + if (*cursor == sampler) { + *cursor = sampler->next; + break; + } + } + pthread_mutex_unlock(&g_kcp_process_sampler_mu); + + pthread_mutex_lock(&sampler->lock); + sampler->stopped = 1; + pthread_cond_broadcast(&sampler->cond); + pthread_mutex_unlock(&sampler->lock); + if (sampler->thread_started) { + pthread_join(sampler->thread, NULL); + } + pthread_cond_destroy(&sampler->cond); + pthread_mutex_destroy(&sampler->lock); + free(sampler); +} + +static void kcp_process_sampler_request_sample_and_wait(kcp_process_sampler_t *sampler, const char *reason) { + uint64_t request_id; + + if (sampler == NULL) { + return; + } + pthread_mutex_lock(&sampler->lock); + if (sampler->stopped) { + pthread_mutex_unlock(&sampler->lock); + return; + } + sampler->request_pending = 1; + request_id = ++sampler->pending_request_id; + snprintf(sampler->pending_reason, sizeof(sampler->pending_reason), "%s", reason == NULL ? "" : reason); + pthread_cond_broadcast(&sampler->cond); + while (!sampler->stopped && sampler->completed_request_id < request_id) { + pthread_cond_wait(&sampler->cond, &sampler->lock); + } + pthread_mutex_unlock(&sampler->lock); +} + +static int kcp_socket_debug_log_record(kcp_socket_debug_state_t *state, const char *event_name, const struct sockaddr_storage *remote_addr, socklen_t remote_addr_len, int packet_bytes, int has_tx_id, uint32_t tx_id, int has_conv, uint32_t conv, const kcp_packet_debug_segment_t *segments, size_t segment_count, int64_t ts_unix_nano) { + char local_addr_text[OMNI_MAX_ADDR_TEXT]; + char remote_addr_text[OMNI_MAX_ADDR_TEXT]; + struct sockaddr_storage local_addr; + socklen_t local_addr_len = sizeof(local_addr); + kcp_packet_debug_record_t record; + + if (state->logger == NULL) { + return 0; + } + memset(&record, 0, sizeof(record)); + getsockname(state->fd, (struct sockaddr *) &local_addr, &local_addr_len); + omni_sockaddr_to_string((struct sockaddr *) &local_addr, local_addr_len, local_addr_text, sizeof(local_addr_text)); + omni_sockaddr_to_string((const struct sockaddr *) remote_addr, remote_addr_len, remote_addr_text, sizeof(remote_addr_text)); + snprintf(record.event, sizeof(record.event), "%s", event_name); + snprintf(record.node_role, sizeof(record.node_role), "%s", state->node_role); + snprintf(record.node_id, sizeof(record.node_id), "%s", state->node_id); + snprintf(record.local_addr, sizeof(record.local_addr), "%s", local_addr_text); + snprintf(record.remote_addr, sizeof(record.remote_addr), "%s", remote_addr_text); + record.packet_bytes = packet_bytes; + record.has_udp_tx_id = has_tx_id; + record.udp_tx_id = tx_id; + record.has_kcp_conv = has_conv; + record.kcp_conv = conv; + record.ts_unix_nano = ts_unix_nano; + if (segment_count > 0) { + record.segments = (kcp_packet_debug_segment_t *) calloc(segment_count, sizeof(*record.segments)); + if (record.segments == NULL) { + return -1; + } + memcpy(record.segments, segments, segment_count * sizeof(*segments)); + record.segment_count = segment_count; + } + kcp_packet_debug_log(state->logger, &record); + kcp_packet_debug_record_clear(&record); + return 0; +} + +static void kcp_socket_debug_pending_free(kcp_packet_debug_pending_t *pending) { + while (pending != NULL) { + kcp_packet_debug_pending_t *next = pending->next; + free(pending->segments); + free(pending); + pending = next; + } +} + +static int kcp_socket_debug_reserve_tx(kcp_socket_debug_state_t *state, const struct sockaddr_storage *remote_addr, socklen_t remote_addr_len, const uint8_t *packet, size_t packet_len, uint32_t *out_tx_id) { + kcp_packet_debug_pending_t *pending; + if (state->logger == NULL) { + *out_tx_id = 0; + return 0; + } + pending = (kcp_packet_debug_pending_t *) calloc(1, sizeof(*pending)); + if (pending == NULL) { + return -1; + } + pending->tx_id = state->next_tx_id++; + pending->packet_bytes = (int) packet_len; + memcpy(&pending->remote_addr, remote_addr, sizeof(*remote_addr)); + pending->remote_addr_len = remote_addr_len; + kcp_parse_packet_segments(packet, packet_len, &pending->conv, &pending->segments, &pending->segment_count); + pending->has_conv = packet_len >= 4; + pthread_mutex_lock(&state->pending_mu); + pending->next = state->pending_head; + state->pending_head = pending; + pthread_mutex_unlock(&state->pending_mu); + *out_tx_id = pending->tx_id; + return 0; +} + +static void kcp_socket_debug_rollback_tx(kcp_socket_debug_state_t *state, uint32_t tx_id) { + kcp_packet_debug_pending_t *prev = NULL; + kcp_packet_debug_pending_t *cur; + pthread_mutex_lock(&state->pending_mu); + for (cur = state->pending_head; cur != NULL; cur = cur->next) { + if (cur->tx_id == tx_id) { + if (prev == NULL) { + state->pending_head = cur->next; + } else { + prev->next = cur->next; + } + free(cur->segments); + free(cur); + break; + } + prev = cur; + } + pthread_mutex_unlock(&state->pending_mu); +} + +static void *kcp_socket_debug_errqueue_thread(void *arg) { + kcp_socket_debug_state_t *state = (kcp_socket_debug_state_t *) arg; + uint8_t control[512]; + uint8_t dummy = 0; + struct iovec iov; + struct msghdr msg; + + while (!atomic_load(&state->closed)) { + ssize_t rc; + omni_tx_timestamp_event_t event; + kcp_packet_debug_pending_t *prev = NULL; + kcp_packet_debug_pending_t *cur = NULL; + + memset(&msg, 0, sizeof(msg)); + iov.iov_base = &dummy; + iov.iov_len = sizeof(dummy); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = control; + msg.msg_controllen = sizeof(control); + rc = recvmsg(state->fd, &msg, MSG_ERRQUEUE | MSG_DONTWAIT); + if (rc < 0) { + if (errno == EAGAIN || errno == EWOULDBLOCK || errno == EINTR) { + usleep(10000); + continue; + } + if (atomic_load(&state->closed)) { + return NULL; + } + usleep(10000); + continue; + } + if (linux_timestamping_parse_tx_timestamp(&msg, &event) != 0) { + continue; + } + pthread_mutex_lock(&state->pending_mu); + for (cur = state->pending_head; cur != NULL; cur = cur->next) { + if (cur->tx_id == event.ee_data) { + break; + } + prev = cur; + } + if (cur != NULL) { + if (strcmp(event.event_name, EVENT_A_TX_SCHED) == 0) { + cur->saw_sched = 1; + } else if (strcmp(event.event_name, EVENT_A_TX_SOFTWARE) == 0) { + cur->saw_software = 1; + } + kcp_socket_debug_log_record(state, event.event_name, &cur->remote_addr, cur->remote_addr_len, cur->packet_bytes, 1, cur->tx_id, cur->has_conv, cur->conv, cur->segments, cur->segment_count, event.ts_unix_nano); + if (cur->saw_sched && cur->saw_software) { + if (prev == NULL) { + state->pending_head = cur->next; + } else { + prev->next = cur->next; + } + free(cur->segments); + free(cur); + } + } + pthread_mutex_unlock(&state->pending_mu); + } + return NULL; +} + +static int kcp_socket_debug_init(kcp_socket_debug_state_t *state, int fd, kcp_packet_debug_logger_t *logger, const char *node_role, const char *node_id) { + int thread_rc; + memset(state, 0, sizeof(*state)); + state->fd = fd; + state->logger = logger; + snprintf(state->node_role, sizeof(state->node_role), "%s", node_role == NULL ? "" : node_role); + snprintf(state->node_id, sizeof(state->node_id), "%s", node_id == NULL ? "" : node_id); + pthread_mutex_init(&state->write_mu, NULL); + pthread_mutex_init(&state->pending_mu, NULL); + if (logger != NULL) { + if (linux_timestamping_enable_udp_socket(fd, 1) != 0) { + pthread_mutex_destroy(&state->write_mu); + pthread_mutex_destroy(&state->pending_mu); + return -1; + } + thread_rc = pthread_create(&state->errqueue_thread, NULL, kcp_socket_debug_errqueue_thread, state); + if (thread_rc != 0) { + errno = thread_rc; + pthread_mutex_destroy(&state->write_mu); + pthread_mutex_destroy(&state->pending_mu); + return -1; + } + state->errqueue_thread_started = 1; + } + return 0; +} + +static void kcp_socket_debug_destroy(kcp_socket_debug_state_t *state) { + atomic_store(&state->closed, 1); + if (state->errqueue_thread_started) { + pthread_join(state->errqueue_thread, NULL); + } + kcp_socket_debug_pending_free(state->pending_head); + pthread_mutex_destroy(&state->write_mu); + pthread_mutex_destroy(&state->pending_mu); +} + +static int kcp_socket_send_packet(kcp_socket_debug_state_t *state, const struct sockaddr_storage *remote_addr, socklen_t remote_addr_len, const uint8_t *packet, size_t packet_len) { + uint32_t tx_id = 0; + ssize_t rc; + if (state->logger != NULL && kcp_socket_debug_reserve_tx(state, remote_addr, remote_addr_len, packet, packet_len, &tx_id) != 0) { + atomic_store(&state->last_send_errno, errno != 0 ? errno : EIO); + return -1; + } + pthread_mutex_lock(&state->write_mu); + rc = sendto(state->fd, packet, packet_len, 0, (const struct sockaddr *) remote_addr, remote_addr_len); + pthread_mutex_unlock(&state->write_mu); + if (rc < 0 || (size_t) rc != packet_len) { + if (rc >= 0 && (size_t) rc != packet_len && errno == 0) { + errno = EIO; + } + atomic_store(&state->last_send_errno, errno != 0 ? errno : EIO); + if (state->logger != NULL) { + kcp_socket_debug_rollback_tx(state, tx_id); + } + return -1; + } + atomic_store(&state->last_send_errno, 0); + return 0; +} + +static int kcp_output_callback_impl(const char *buf, int len, struct IKCPCB *kcp, void *user) { + kcp_conn_t *conn = (kcp_conn_t *) user; + size_t segment_count = 0; + (void) kcp; + if (conn == NULL || atomic_load(&conn->closed)) { + return -1; + } + kcp_parse_packet_segments((const uint8_t *) buf, (size_t) len, NULL, NULL, &segment_count); + if (kcp_socket_send_packet(conn->sock_state, &conn->remote_addr, conn->remote_addr_len, (const uint8_t *) buf, (size_t) len) != 0) { + return -1; + } + kcp_conn_record_send(conn, len, segment_count); + return len; +} + +static int kcp_conn_attach_process_sampler(kcp_conn_t *conn) { + kcp_process_sampler_t *next_sampler; + kcp_process_sampler_t *previous_sampler; + uint64_t pending_bytes_sent = 0; + uint64_t pending_bytes_received = 0; + uint64_t pending_in_pkts = 0; + uint64_t pending_out_pkts = 0; + uint64_t pending_in_segs = 0; + uint64_t pending_out_segs = 0; + uint64_t pending_in_errs = 0; + uint64_t pending_kcp_in_errs = 0; + + if (conn == NULL) { + errno = EINVAL; + return -1; + } + + next_sampler = kcp_process_sampler_acquire(conn->stats_logger, conn->node_role, conn->node_id, conn->stats_interval_ms); + if (conn->stats_logger != NULL && next_sampler == NULL) { + return -1; + } + + previous_sampler = conn->process_sampler; + if (previous_sampler == next_sampler) { + return 0; + } + + if (next_sampler != NULL) { + kcp_process_sampler_add_conn(next_sampler, conn); + } + pthread_mutex_lock(&conn->kcp_mu); + previous_sampler = conn->process_sampler; + conn->process_sampler = next_sampler; + pending_bytes_sent = conn->pending_bytes_sent; + pending_bytes_received = conn->pending_bytes_received; + pending_in_pkts = conn->pending_in_pkts; + pending_out_pkts = conn->pending_out_pkts; + pending_in_segs = conn->pending_in_segs; + pending_out_segs = conn->pending_out_segs; + pending_in_errs = conn->pending_in_errs; + pending_kcp_in_errs = conn->pending_kcp_in_errs; + conn->pending_bytes_sent = 0; + conn->pending_bytes_received = 0; + conn->pending_in_pkts = 0; + conn->pending_out_pkts = 0; + conn->pending_in_segs = 0; + conn->pending_out_segs = 0; + conn->pending_in_errs = 0; + conn->pending_kcp_in_errs = 0; + pthread_mutex_unlock(&conn->kcp_mu); + if (next_sampler != NULL) { + atomic_fetch_add_explicit(&next_sampler->bytes_sent, pending_bytes_sent, memory_order_relaxed); + atomic_fetch_add_explicit(&next_sampler->bytes_received, pending_bytes_received, memory_order_relaxed); + atomic_fetch_add_explicit(&next_sampler->in_pkts, pending_in_pkts, memory_order_relaxed); + atomic_fetch_add_explicit(&next_sampler->out_pkts, pending_out_pkts, memory_order_relaxed); + atomic_fetch_add_explicit(&next_sampler->in_segs, pending_in_segs, memory_order_relaxed); + atomic_fetch_add_explicit(&next_sampler->out_segs, pending_out_segs, memory_order_relaxed); + atomic_fetch_add_explicit(&next_sampler->in_errs, pending_in_errs, memory_order_relaxed); + atomic_fetch_add_explicit(&next_sampler->kcp_in_errs, pending_kcp_in_errs, memory_order_relaxed); + } + if (previous_sampler != NULL) { + kcp_process_sampler_remove_conn(previous_sampler, conn); + kcp_process_sampler_release(previous_sampler); + } + return 0; +} + +static void kcp_conn_detach_process_sampler(kcp_conn_t *conn) { + kcp_process_sampler_t *sampler; + + if (conn == NULL || conn->process_sampler == NULL) { + return; + } + + sampler = conn->process_sampler; + conn->process_sampler = NULL; + kcp_process_sampler_remove_conn(sampler, conn); + kcp_process_sampler_release(sampler); +} + +static void kcp_log_session_snapshot(kcp_conn_t *conn, const char *reason) { + kcp_session_stats_record_t record; + struct sockaddr_storage local_addr; + 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; + } + memset(&record, 0, sizeof(record)); + snprintf(record.record_type, sizeof(record.record_type), "%s", KCP_SESSION_STATS_RECORD_SESSION_SAMPLE); + snprintf(record.node_role, sizeof(record.node_role), "%s", conn->node_role); + snprintf(record.node_id, sizeof(record.node_id), "%s", conn->node_id); + getsockname(conn->sock_state->fd, (struct sockaddr *) &local_addr, &local_len); + omni_sockaddr_to_string((struct sockaddr *) &local_addr, local_len, local_text, sizeof(local_text)); + omni_sockaddr_to_string((struct sockaddr *) &conn->remote_addr, conn->remote_addr_len, remote_text, sizeof(remote_text)); + snprintf(record.local_addr, sizeof(record.local_addr), "%s", local_text); + snprintf(record.remote_addr, sizeof(record.remote_addr), "%s", remote_text); + record.has_conv = 1; + record.conv = conn->kcp->conv; + record.ts_unix_nano = omni_now_unix_nano(); + snprintf(record.sample_reason, sizeof(record.sample_reason), "%s", reason); + pthread_mutex_lock(&conn->kcp_mu); + record.has_rto_ms = 1; + record.rto_ms = conn->kcp->rx_rto; + record.has_srtt_ms = 1; + record.srtt_ms = conn->kcp->rx_srtt; + kcp_conn_update_min_srtt_locked(conn); + record.has_min_srtt_ms = conn->min_srtt_ms > 0; + record.min_srtt_ms = conn->min_srtt_ms; + record.has_srttvar_ms = 1; + record.srttvar_ms = conn->kcp->rx_rttval; + record.has_last_feedback_age_ms = conn->last_feedback_ms != 0; + record.last_feedback_age_ms = conn->last_feedback_ms == 0 ? 0 : (omni_now_millis32() - conn->last_feedback_ms); + 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); +} + +static void *kcp_stats_thread_main(void *arg) { + kcp_conn_t *conn = (kcp_conn_t *) arg; + while (!atomic_load(&conn->closed)) { + usleep((useconds_t) conn->stats_interval_ms * 1000U); + if (!atomic_load(&conn->closed)) { + kcp_log_session_snapshot(conn, "periodic"); + } + } + return NULL; +} + +static int kcp_socket_open_bound(const char *listen_addr, const char *bind_device, struct sockaddr_storage *local_addr, socklen_t *local_len) { + int family; + int fd; + if (omni_parse_sockaddr(listen_addr, 1, local_addr, local_len, &family) != 0) { + return -1; + } + fd = socket(family, SOCK_DGRAM, 0); + if (fd < 0) { + return -1; + } + if (bind_device != NULL && bind_device[0] != '\0' && omni_bind_device(fd, bind_device) != 0) { + close(fd); + return -1; + } + if (bind(fd, (struct sockaddr *) local_addr, *local_len) != 0) { + close(fd); + return -1; + } + return fd; +} + +static int kcp_socket_open_dial(const char *server_addr, const char *bind_ip, const char *bind_device, struct sockaddr_storage *remote_addr, socklen_t *remote_len, int *family_out) { + int family; + struct sockaddr_storage local_addr; + socklen_t local_len; + int fd; + if (omni_parse_sockaddr(server_addr, 0, remote_addr, remote_len, &family) != 0) { + return -1; + } + fd = socket(family, SOCK_DGRAM, 0); + if (fd < 0) { + return -1; + } + if (bind_device != NULL && bind_device[0] != '\0' && omni_bind_device(fd, bind_device) != 0) { + close(fd); + return -1; + } + if (bind_ip != NULL && bind_ip[0] != '\0') { + struct addrinfo hints; + struct addrinfo *result = NULL; + memset(&hints, 0, sizeof(hints)); + hints.ai_family = family; + hints.ai_socktype = SOCK_DGRAM; + if (getaddrinfo(bind_ip, "0", &hints, &result) != 0 || result == NULL) { + close(fd); + errno = EINVAL; + return -1; + } + memcpy(&local_addr, result->ai_addr, result->ai_addrlen); + local_len = (socklen_t) result->ai_addrlen; + freeaddrinfo(result); + if (bind(fd, (struct sockaddr *) &local_addr, local_len) != 0) { + close(fd); + return -1; + } + } + if (family_out != NULL) { + *family_out = family; + } + return fd; +} + +static int kcp_sockaddr_equal(const struct sockaddr_storage *left, socklen_t left_len, const struct sockaddr_storage *right, socklen_t right_len) { + char left_text[OMNI_MAX_ADDR_TEXT]; + char right_text[OMNI_MAX_ADDR_TEXT]; + + if (left == NULL || right == NULL) { + return left == right; + } + return strcmp( + omni_sockaddr_to_string((const struct sockaddr *) left, left_len, left_text, sizeof(left_text)), + omni_sockaddr_to_string((const struct sockaddr *) right, right_len, right_text, sizeof(right_text)) + ) == 0; +} + +static void *kcp_client_recv_thread_main(void *arg) { + kcp_conn_t *conn = (kcp_conn_t *) arg; + uint8_t buffer[64 * 1024]; + uint8_t control[512]; + struct sockaddr_storage source; + struct iovec iov; + struct msghdr msg; + ssize_t n; + uint32_t conv = 0; + kcp_packet_debug_segment_t *segments = NULL; + size_t segment_count = 0; + int64_t rx_ts; + + while (!atomic_load(&conn->closed)) { + memset(&msg, 0, sizeof(msg)); + memset(&source, 0, sizeof(source)); + iov.iov_base = buffer; + iov.iov_len = sizeof(buffer); + msg.msg_name = &source; + msg.msg_namelen = sizeof(source); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + if (conn->sock_state->logger != NULL) { + msg.msg_control = control; + msg.msg_controllen = sizeof(control); + } + n = recvmsg(conn->fd, &msg, 0); + if (n < 0) { + if (errno == EINTR) { + continue; + } + if (atomic_load(&conn->closed)) { + return NULL; + } + return NULL; + } + kcp_parse_packet_segments(buffer, (size_t) n, &conv, &segments, &segment_count); + rx_ts = conn->sock_state->logger != NULL ? linux_timestamping_parse_rx_timestamp(&msg) : 0; + if (rx_ts > 0) { + kcp_socket_debug_log_record(conn->sock_state, EVENT_B_RX_SOFTWARE, &source, msg.msg_namelen, (int) n, 0, 0, 1, conv, segments, segment_count, rx_ts); + } + if (!kcp_sockaddr_equal(&source, msg.msg_namelen, &conn->remote_addr, conn->remote_addr_len)) { + free(segments); + segments = NULL; + segment_count = 0; + continue; + } + pthread_mutex_lock(&conn->kcp_mu); + conn->kcp->current = omni_now_millis32(); + if (ikcp_input(conn->kcp, (const char *) buffer, n) != 0) { + kcp_conn_record_error(conn); + } else { + kcp_conn_note_feedback_locked(conn); + kcp_conn_record_input(conn, (int) n, segment_count); + } + pthread_mutex_unlock(&conn->kcp_mu); + pthread_cond_broadcast(&conn->rx_cond); + free(segments); + segments = NULL; + segment_count = 0; + } + return NULL; +} + +static void *kcp_update_thread_main(void *arg) { + kcp_conn_t *conn = (kcp_conn_t *) arg; + while (!atomic_load(&conn->closed)) { + int interval_ms; + pthread_mutex_lock(&conn->kcp_mu); + ikcp_update(conn->kcp, omni_now_millis32()); + interval_ms = conn->update_interval_ms > 0 ? conn->update_interval_ms : KCP_DEFAULT_INTERVAL_MS; + pthread_mutex_unlock(&conn->kcp_mu); + usleep((useconds_t) interval_ms * 1000U); + } + return NULL; +} + +static int kcp_conn_start_stats_thread(kcp_conn_t *conn) { + int thread_rc; + if (conn == NULL || conn->stats_logger == NULL || conn->stats_thread_started) { + return 0; + } + thread_rc = pthread_create(&conn->stats_thread, NULL, kcp_stats_thread_main, conn); + if (thread_rc != 0) { + errno = thread_rc; + return -1; + } + conn->stats_thread_started = 1; + return 0; +} + +static kcp_conn_t *kcp_conn_alloc_common(int fd, const struct sockaddr_storage *remote_addr, socklen_t remote_addr_len, const kcp_conn_options_t *options, kcp_socket_debug_state_t *sock_state, 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 *conn = (kcp_conn_t *) calloc(1, sizeof(*conn)); + uint32_t conv; + int thread_rc; + kcp_conn_options_t effective_options; + + if (conn == NULL) { + errno = ENOMEM; + return NULL; + } + conn->fd = fd; + memcpy(&conn->remote_addr, remote_addr, sizeof(*remote_addr)); + conn->remote_addr_len = remote_addr_len; + pthread_mutex_init(&conn->kcp_mu, NULL); + pthread_mutex_init(&conn->close_mu, NULL); + pthread_cond_init(&conn->rx_cond, NULL); + protocol_frame_decoder_init(&conn->decoder); + conn->logger = logger; + snprintf(conn->node_role, sizeof(conn->node_role), "%s", node_role == NULL ? "" : node_role); + snprintf(conn->node_id, sizeof(conn->node_id), "%s", node_id == NULL ? "" : node_id); + conn->stats_logger = stats_logger; + conn->stats_interval_ms = stats_interval_ms > 0 ? stats_interval_ms : KCP_DEFAULT_STATS_INTERVAL_MS; + kcp_conn_options_init(&effective_options); + if (options != NULL) { + effective_options = *options; + } + conn->options = effective_options; + conn->update_interval_ms = effective_options.interval_ms; + conn->sock_state = sock_state; + if (omni_random_u32(&conv) != 0) { + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + return NULL; + } + conn->kcp = ikcp_create(conv, conn); + if (conn->kcp == NULL) { + errno = ENOMEM; + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + return NULL; + } + ikcp_setoutput(conn->kcp, kcp_output_callback_impl); + if (kcp_conn_apply_options_locked(conn, &effective_options) != 0) { + ikcp_release(conn->kcp); + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + return NULL; + } + if (kcp_conn_attach_process_sampler(conn) != 0) { + ikcp_release(conn->kcp); + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + return NULL; + } + if (kcp_conn_start_stats_thread(conn) != 0) { + kcp_conn_detach_process_sampler(conn); + ikcp_release(conn->kcp); + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + return NULL; + } + thread_rc = pthread_create(&conn->update_thread, NULL, kcp_update_thread_main, conn); + if (thread_rc != 0) { + errno = thread_rc; + if (conn->stats_thread_started) { + atomic_store(&conn->closed, 1); + pthread_join(conn->stats_thread, NULL); + } + kcp_conn_detach_process_sampler(conn); + ikcp_release(conn->kcp); + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + return NULL; + } + conn->update_thread_started = 1; + return conn; +} + +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) { + struct sockaddr_storage remote_addr; + socklen_t remote_len; + int family; + int fd = kcp_socket_open_dial(server_addr, bind_ip, bind_device, &remote_addr, &remote_len, &family); + kcp_conn_t *conn; + kcp_socket_debug_state_t *sock_state; + int thread_rc; + (void) family; + if (fd < 0) { + return NULL; + } + sock_state = (kcp_socket_debug_state_t *) calloc(1, sizeof(*sock_state)); + if (sock_state == NULL) { + errno = ENOMEM; + close(fd); + return NULL; + } + if (kcp_socket_debug_init(sock_state, fd, packet_logger, node_role, node_id) != 0) { + free(sock_state); + close(fd); + return NULL; + } + conn = kcp_conn_alloc_common(fd, &remote_addr, remote_len, options, sock_state, logger, node_role, node_id, stats_logger, stats_interval_ms); + if (conn == NULL) { + kcp_socket_debug_destroy(sock_state); + free(sock_state); + close(fd); + return NULL; + } + conn->is_client = 1; + conn->owns_socket = 1; + thread_rc = pthread_create(&conn->recv_thread, NULL, kcp_client_recv_thread_main, conn); + if (thread_rc != 0) { + errno = thread_rc; + kcp_conn_free(conn); + return NULL; + } + conn->recv_thread_started = 1; + return conn; +} + +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) { + return kcp_conn_dial_with_options(server_addr, bind_ip, bind_device, NULL, packet_logger, logger, node_role, node_id, stats_logger, stats_interval_ms); +} + +static void kcp_listener_enqueue_accept(kcp_listener_t *listener, kcp_conn_t *conn) { + pthread_mutex_lock(&listener->accept_mu); + if (listener->accept_tail == NULL) { + listener->accept_head = conn; + } else { + listener->accept_tail->accept_next = conn; + } + listener->accept_tail = conn; + conn->accept_next = NULL; + pthread_cond_signal(&listener->accept_cond); + pthread_mutex_unlock(&listener->accept_mu); +} + +static kcp_conn_t *kcp_listener_find_session(kcp_listener_t *listener, uint32_t conv) { + kcp_session_entry_t *entry; + for (entry = listener->sessions; entry != NULL; entry = entry->next) { + if (entry->conv == conv) { + return entry->conn; + } + } + return NULL; +} + +static int kcp_listener_add_session(kcp_listener_t *listener, uint32_t conv, kcp_conn_t *conn) { + kcp_session_entry_t *entry = (kcp_session_entry_t *) calloc(1, sizeof(*entry)); + if (entry == NULL) { + return -1; + } + entry->conv = conv; + entry->conn = conn; + entry->next = listener->sessions; + listener->sessions = entry; + return 0; +} + +static void kcp_listener_remove_session(kcp_listener_t *listener, kcp_conn_t *conn) { + kcp_session_entry_t *prev_entry = NULL; + kcp_session_entry_t *entry; + kcp_conn_t *prev_accept = NULL; + kcp_conn_t *accept; + + if (listener == NULL || conn == NULL) { + return; + } + + pthread_mutex_lock(&listener->lock); + for (entry = listener->sessions; entry != NULL; entry = entry->next) { + if (entry->conn == conn) { + if (prev_entry == NULL) { + listener->sessions = entry->next; + } else { + prev_entry->next = entry->next; + } + free(entry); + break; + } + prev_entry = entry; + } + pthread_mutex_unlock(&listener->lock); + + pthread_mutex_lock(&listener->accept_mu); + for (accept = listener->accept_head; accept != NULL; accept = accept->accept_next) { + if (accept == conn) { + if (prev_accept == NULL) { + listener->accept_head = accept->accept_next; + } else { + prev_accept->accept_next = accept->accept_next; + } + if (listener->accept_tail == conn) { + listener->accept_tail = prev_accept; + } + conn->accept_next = NULL; + break; + } + prev_accept = accept; + } + pthread_mutex_unlock(&listener->accept_mu); +} + +static void *kcp_listener_recv_thread_main(void *arg) { + kcp_listener_t *listener = (kcp_listener_t *) arg; + uint8_t buffer[64 * 1024]; + uint8_t control[512]; + struct sockaddr_storage source; + struct iovec iov; + struct msghdr msg; + ssize_t n; + uint32_t conv; + kcp_packet_debug_segment_t *segments = NULL; + size_t segment_count = 0; + int64_t rx_ts; + + while (!listener->closed) { + memset(&msg, 0, sizeof(msg)); + memset(&source, 0, sizeof(source)); + iov.iov_base = buffer; + iov.iov_len = sizeof(buffer); + msg.msg_name = &source; + msg.msg_namelen = sizeof(source); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + if (listener->sock_state.logger != NULL) { + msg.msg_control = control; + msg.msg_controllen = sizeof(control); + } + n = recvmsg(listener->fd, &msg, 0); + if (n < 0) { + if (errno == EINTR) { + continue; + } + if (listener->closed) { + return NULL; + } + return NULL; + } + kcp_parse_packet_segments(buffer, (size_t) n, &conv, &segments, &segment_count); + rx_ts = listener->sock_state.logger != NULL ? linux_timestamping_parse_rx_timestamp(&msg) : 0; + if (rx_ts > 0) { + kcp_socket_debug_log_record(&listener->sock_state, EVENT_B_RX_SOFTWARE, &source, msg.msg_namelen, (int) n, 0, 0, 1, conv, segments, segment_count, rx_ts); + } + pthread_mutex_lock(&listener->lock); + { + kcp_conn_t *conn = kcp_listener_find_session(listener, conv); + if (conn == NULL) { + conn = (kcp_conn_t *) calloc(1, sizeof(*conn)); + if (conn != NULL) { + kcp_conn_options_t accepted_options; + conn->fd = listener->fd; + memcpy(&conn->remote_addr, &source, sizeof(source)); + conn->remote_addr_len = msg.msg_namelen; + pthread_mutex_init(&conn->kcp_mu, NULL); + pthread_mutex_init(&conn->close_mu, NULL); + pthread_cond_init(&conn->rx_cond, NULL); + protocol_frame_decoder_init(&conn->decoder); + snprintf(conn->node_role, sizeof(conn->node_role), "%s", listener->sock_state.node_role); + snprintf(conn->node_id, sizeof(conn->node_id), "%s", listener->sock_state.node_id); + conn->stats_interval_ms = KCP_DEFAULT_STATS_INTERVAL_MS; + conn->sock_state = &listener->sock_state; + conn->listener = listener; + kcp_conn_options_init(&accepted_options); + conn->options = accepted_options; + conn->update_interval_ms = accepted_options.interval_ms; + conn->kcp = ikcp_create(conv, conn); + if (conn->kcp != NULL) { + int update_started = 0; + ikcp_setoutput(conn->kcp, kcp_output_callback_impl); + if (kcp_conn_apply_options_locked(conn, &accepted_options) == 0 && + pthread_create(&conn->update_thread, NULL, kcp_update_thread_main, conn) == 0) { + update_started = 1; + } + if (update_started && kcp_listener_add_session(listener, conv, conn) == 0) { + conn->update_thread_started = 1; + kcp_listener_enqueue_accept(listener, conn); + } else { + atomic_store(&conn->closed, 1); + if (update_started) { + pthread_join(conn->update_thread, NULL); + } + ikcp_release(conn->kcp); + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + conn = NULL; + } + } else { + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + conn = NULL; + } + } + } + if (conn != NULL && conn->kcp != NULL) { + pthread_mutex_lock(&conn->kcp_mu); + conn->kcp->current = omni_now_millis32(); + if (ikcp_input(conn->kcp, (const char *) buffer, n) != 0) { + kcp_conn_record_error(conn); + } else { + kcp_conn_note_feedback_locked(conn); + kcp_conn_record_input(conn, (int) n, segment_count); + } + pthread_mutex_unlock(&conn->kcp_mu); + pthread_cond_broadcast(&conn->rx_cond); + } + } + pthread_mutex_unlock(&listener->lock); + free(segments); + segments = NULL; + segment_count = 0; + } + return NULL; +} + +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) { + struct sockaddr_storage local_addr; + socklen_t local_len; + int fd = kcp_socket_open_bound(listen_addr, bind_device, &local_addr, &local_len); + kcp_listener_t *listener; + if (fd < 0) { + return NULL; + } + listener = (kcp_listener_t *) calloc(1, sizeof(*listener)); + if (listener == NULL) { + close(fd); + return NULL; + } + listener->fd = fd; + pthread_mutex_init(&listener->lock, NULL); + pthread_mutex_init(&listener->accept_mu, NULL); + pthread_cond_init(&listener->accept_cond, NULL); + if (kcp_socket_debug_init(&listener->sock_state, fd, packet_logger, node_role, node_id) != 0) { + kcp_listener_free(listener); + return NULL; + } + if (pthread_create(&listener->recv_thread, NULL, kcp_listener_recv_thread_main, listener) != 0) { + kcp_listener_free(listener); + return NULL; + } + listener->recv_thread_started = 1; + return listener; +} + +kcp_conn_t *kcp_listener_accept(kcp_listener_t *listener) { + kcp_conn_t *conn; + if (listener == NULL) { + errno = EINVAL; + return NULL; + } + pthread_mutex_lock(&listener->accept_mu); + while (!listener->closed && listener->accept_head == NULL) { + pthread_cond_wait(&listener->accept_cond, &listener->accept_mu); + } + if (listener->closed) { + pthread_mutex_unlock(&listener->accept_mu); + errno = ECANCELED; + return NULL; + } + conn = listener->accept_head; + listener->accept_head = conn->accept_next; + if (listener->accept_head == NULL) { + listener->accept_tail = NULL; + } + conn->accept_next = NULL; + pthread_mutex_unlock(&listener->accept_mu); + return conn; +} + +int kcp_conn_configure_runtime(kcp_conn_t *conn, latency_logger_t *logger, const char *node_role, const char *node_id, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms) { + if (conn == NULL) { + errno = EINVAL; + return -1; + } + pthread_mutex_lock(&conn->close_mu); + conn->logger = logger; + if (node_role != NULL) { + snprintf(conn->node_role, sizeof(conn->node_role), "%s", node_role); + } + if (node_id != NULL) { + snprintf(conn->node_id, sizeof(conn->node_id), "%s", node_id); + } + conn->stats_logger = stats_logger; + if (stats_interval_ms > 0) { + conn->stats_interval_ms = stats_interval_ms; + } else if (conn->stats_interval_ms <= 0) { + conn->stats_interval_ms = KCP_DEFAULT_STATS_INTERVAL_MS; + } + if (kcp_conn_attach_process_sampler(conn) != 0) { + pthread_mutex_unlock(&conn->close_mu); + return -1; + } + pthread_mutex_unlock(&conn->close_mu); + if (kcp_conn_start_stats_thread(conn) != 0) { + pthread_mutex_lock(&conn->close_mu); + kcp_conn_detach_process_sampler(conn); + pthread_mutex_unlock(&conn->close_mu); + return -1; + } + return 0; +} + +int kcp_conn_apply_options(kcp_conn_t *conn, const kcp_conn_options_t *options) { + int rc; + + if (conn == NULL || options == NULL) { + errno = EINVAL; + return -1; + } + pthread_mutex_lock(&conn->kcp_mu); + rc = kcp_conn_apply_options_locked(conn, options); + pthread_mutex_unlock(&conn->kcp_mu); + return rc; +} + +int kcp_conn_send(kcp_conn_t *conn, const message_t *msg) { + uint8_t *frame = NULL; + size_t frame_len = 0; + int send_errno = 0; + int kcp_send_rc = 0; + if (conn == NULL || msg == NULL) { + errno = EINVAL; + return -1; + } + if (protocol_encode_message_stream(msg, &frame, &frame_len) != 0) { + return -1; + } + latencylog_log_message_event(conn->logger, conn->node_role, conn->node_id, EVENT_SEND_HANDOFF_BEGIN, msg); + pthread_mutex_lock(&conn->kcp_mu); + atomic_store(&conn->sock_state->last_send_errno, 0); + conn->kcp->current = omni_now_millis32(); + kcp_send_rc = ikcp_send(conn->kcp, (const char *) frame, (int) frame_len); + if (kcp_send_rc < 0) { + pthread_mutex_unlock(&conn->kcp_mu); + errno = kcp_send_rc == -2 ? EMSGSIZE : EINVAL; + free(frame); + return -1; + } + ikcp_flush(conn->kcp); + send_errno = atomic_load(&conn->sock_state->last_send_errno); + pthread_mutex_unlock(&conn->kcp_mu); + if (send_errno != 0) { + errno = send_errno; + free(frame); + return -1; + } + latencylog_log_message_event(conn->logger, conn->node_role, conn->node_id, EVENT_SEND_HANDOFF_END, msg); + free(frame); + return 0; +} + +static void kcp_timespec_deadline_after_ms(struct timespec *deadline, int timeout_ms) { + clock_gettime(CLOCK_REALTIME, deadline); + deadline->tv_sec += timeout_ms / 1000; + deadline->tv_nsec += (long) (timeout_ms % 1000) * 1000000L; + if (deadline->tv_nsec >= 1000000000L) { + deadline->tv_sec += 1; + deadline->tv_nsec -= 1000000000L; + } +} + +int kcp_conn_receive_timed(kcp_conn_t *conn, message_t *out_msg, int timeout_ms) { + uint8_t *frame = NULL; + size_t frame_len = 0; + char err[128]; + int next_rc; + struct timespec deadline; + int use_deadline = timeout_ms > 0; + + if (conn == NULL || out_msg == NULL) { + errno = EINVAL; + return -1; + } + if (use_deadline) { + kcp_timespec_deadline_after_ms(&deadline, timeout_ms); + } + for (;;) { + next_rc = protocol_frame_decoder_next(&conn->decoder, &frame, &frame_len); + if (next_rc < 0) { + return -1; + } + if (next_rc == 1) { + if (protocol_decode_message_stream_payload(frame, frame_len, out_msg, err, sizeof(err)) != 0) { + free(frame); + errno = EPROTO; + return -1; + } + free(frame); + return 0; + } + pthread_mutex_lock(&conn->kcp_mu); + { + int n = ikcp_recv(conn->kcp, (char *) conn->scratch, (int) sizeof(conn->scratch)); + if (n > 0) { + if (protocol_frame_decoder_feed(&conn->decoder, conn->scratch, (size_t) n) != 0) { + pthread_mutex_unlock(&conn->kcp_mu); + return -1; + } + pthread_mutex_unlock(&conn->kcp_mu); + continue; + } + if (atomic_load(&conn->closed)) { + pthread_mutex_unlock(&conn->kcp_mu); + errno = ECANCELED; + return -1; + } + if (timeout_ms == 0) { + pthread_mutex_unlock(&conn->kcp_mu); + return 1; + } + if (timeout_ms < 0) { + pthread_cond_wait(&conn->rx_cond, &conn->kcp_mu); + } else { + int wait_rc = pthread_cond_timedwait(&conn->rx_cond, &conn->kcp_mu, &deadline); + if (wait_rc == ETIMEDOUT) { + pthread_mutex_unlock(&conn->kcp_mu); + return 1; + } + if (wait_rc != 0) { + pthread_mutex_unlock(&conn->kcp_mu); + errno = wait_rc; + return -1; + } + } + } + pthread_mutex_unlock(&conn->kcp_mu); + } +} + +int kcp_conn_receive(kcp_conn_t *conn, message_t *out_msg) { + return kcp_conn_receive_timed(conn, out_msg, -1); +} + +uint32_t kcp_conn_conv(const kcp_conn_t *conn) { + return conn == NULL || conn->kcp == NULL ? 0 : conn->kcp->conv; +} + +int kcp_conn_local_addr(const kcp_conn_t *conn, struct sockaddr_storage *addr, socklen_t *addr_len) { + socklen_t len = sizeof(*addr); + if (conn == NULL || addr == NULL || addr_len == NULL || conn->sock_state == NULL) { + errno = EINVAL; + return -1; + } + if (getsockname(conn->sock_state->fd, (struct sockaddr *) addr, &len) != 0) { + return -1; + } + *addr_len = len; + return 0; +} + +int kcp_conn_remote_addr(const kcp_conn_t *conn, struct sockaddr_storage *addr, socklen_t *addr_len) { + if (conn == NULL || addr == NULL || addr_len == NULL) { + errno = EINVAL; + return -1; + } + if (conn->remote_addr_len == 0) { + errno = ENOTCONN; + return -1; + } + return omni_clone_sockaddr((const struct sockaddr *) &conn->remote_addr, conn->remote_addr_len, addr, addr_len); +} + +void kcp_conn_runtime_stats_snapshot(kcp_conn_t *conn, kcp_runtime_stats_t *out_stats) { + if (out_stats == NULL) { + return; + } + + memset(out_stats, 0, sizeof(*out_stats)); + if (conn == NULL) { + return; + } + + out_stats->connected = atomic_load(&conn->closed) ? 0 : 1; + pthread_mutex_lock(&conn->kcp_mu); + if (conn->kcp != NULL) { + out_stats->conv = conn->kcp->conv; + out_stats->rto_ms = conn->kcp->rx_rto; + out_stats->srtt_ms = conn->kcp->rx_srtt; + kcp_conn_update_min_srtt_locked(conn); + out_stats->min_srtt_ms = conn->min_srtt_ms; + out_stats->srttvar_ms = conn->kcp->rx_rttval; + out_stats->last_feedback_age_ms = conn->last_feedback_ms == 0 ? 0 : (omni_now_millis32() - conn->last_feedback_ms); + out_stats->snd_wnd = conn->kcp->snd_wnd; + out_stats->rmt_wnd = conn->kcp->rmt_wnd; + out_stats->inflight = conn->kcp->snd_nxt - conn->kcp->snd_una; + out_stats->window_limit = conn->kcp->snd_wnd < conn->kcp->rmt_wnd ? conn->kcp->snd_wnd : conn->kcp->rmt_wnd; + out_stats->window_pressure_pct = out_stats->window_limit == 0 + ? 0.0 + : ((double) out_stats->inflight * 100.0) / (double) out_stats->window_limit; + out_stats->snd_queue = conn->kcp->nsnd_que; + out_stats->rcv_queue = conn->kcp->nrcv_que; + out_stats->snd_buffer = conn->kcp->nsnd_buf; + out_stats->out_segs_total = atomic_load_explicit(&conn->total_out_segs, memory_order_relaxed); + out_stats->fast_retrans_total = conn->kcp->fast_retrans_total; + 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_stats->connected = 0; + } + pthread_mutex_unlock(&conn->kcp_mu); +} + +int kcp_conn_close(kcp_conn_t *conn) { + if (conn == NULL) { + return 0; + } + pthread_mutex_lock(&conn->close_mu); + if (!atomic_load(&conn->closed)) { + kcp_log_session_snapshot(conn, "close"); + kcp_process_sampler_request_sample_and_wait(conn->process_sampler, "close"); + 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; + } + pthread_cond_broadcast(&conn->rx_cond); + pthread_mutex_unlock(&conn->kcp_mu); + } + pthread_mutex_unlock(&conn->close_mu); + return 0; +} + +void kcp_conn_free(kcp_conn_t *conn) { + if (conn == NULL) { + return; + } + kcp_conn_close(conn); + if (conn->recv_thread_started) { + pthread_join(conn->recv_thread, NULL); + } + if (conn->update_thread_started) { + pthread_join(conn->update_thread, NULL); + } + if (conn->stats_thread_started) { + pthread_join(conn->stats_thread, NULL); + } + if (conn->listener != NULL && !conn->listener->closed) { + kcp_listener_remove_session(conn->listener, conn); + } + kcp_conn_detach_process_sampler(conn); + if (conn->owns_socket && conn->sock_state != NULL) { + if (!conn->socket_closed) { + close(conn->fd); + conn->socket_closed = 1; + } + kcp_socket_debug_destroy(conn->sock_state); + free(conn->sock_state); + } + if (conn->kcp != NULL) { + ikcp_release(conn->kcp); + } + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); +} + +int kcp_listener_close(kcp_listener_t *listener) { + if (listener == NULL) { + return 0; + } + if (!listener->closed) { + listener->closed = 1; + close(listener->fd); + pthread_cond_broadcast(&listener->accept_cond); + } + return 0; +} + +void kcp_listener_free(kcp_listener_t *listener) { + kcp_session_entry_t *entry; + kcp_session_entry_t *next; + if (listener == NULL) { + return; + } + kcp_listener_close(listener); + if (listener->recv_thread_started) { + pthread_join(listener->recv_thread, NULL); + } + for (entry = listener->sessions; entry != NULL; entry = next) { + next = entry->next; + entry->conn->listener = NULL; + kcp_conn_free(entry->conn); + free(entry); + } + kcp_socket_debug_destroy(&listener->sock_state); + pthread_mutex_destroy(&listener->lock); + pthread_mutex_destroy(&listener->accept_mu); + pthread_cond_destroy(&listener->accept_cond); + free(listener); +} + +int kcp_session_stats_parse_interval_ms(const char *raw, int *out_ms) { + return omni_parse_duration_ms(raw, KCP_DEFAULT_STATS_INTERVAL_MS, out_ms); +} diff --git a/host/OmniSocketGo_add_camera/src/transport_udp.c b/host/OmniSocketGo_add_camera/src/transport_udp.c new file mode 100644 index 0000000..49a0e18 --- /dev/null +++ b/host/OmniSocketGo_add_camera/src/transport_udp.c @@ -0,0 +1,486 @@ +#include "transport_udp.h" + +#include +#include +#include +#include +#include + +typedef struct udp_pending_tx { + struct udp_pending_tx *next; + uint32_t tx_id; + message_t msg; + int bytes_written; + int saw_sched; + int saw_software; +} udp_pending_tx_t; + +struct udp_conn { + int fd; + int connected; + int timestamping_enabled; + latency_logger_t *logger; + tx_timestamp_debug_logger_t *debug_logger; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + pthread_mutex_t write_mu; + pthread_mutex_t pending_mu; + pthread_t errqueue_thread; + int errqueue_thread_started; + uint32_t next_tx_id; + udp_pending_tx_t *pending_head; + uint8_t *recv_buffer; + size_t recv_buffer_cap; + int closed; +}; + +static int udp_open_socket_for_addr(const struct sockaddr *addr, socklen_t addr_len, int bind_device, const char *device) { + int fd; + int reuse = 1; + fd = socket(addr->sa_family, SOCK_DGRAM, 0); + if (fd < 0) { + return -1; + } + setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); + if (bind_device && omni_bind_device(fd, device) != 0) { + close(fd); + return -1; + } + (void) addr_len; + return fd; +} + +static int udp_resolve_ip_only(const char *ip, int family, struct sockaddr_storage *out, socklen_t *out_len) { + struct addrinfo hints; + struct addrinfo *result = NULL; + memset(&hints, 0, sizeof(hints)); + hints.ai_family = family; + hints.ai_socktype = SOCK_DGRAM; + if (getaddrinfo(ip, "0", &hints, &result) != 0 || result == NULL) { + errno = EINVAL; + return -1; + } + memcpy(out, result->ai_addr, result->ai_addrlen); + *out_len = (socklen_t) result->ai_addrlen; + freeaddrinfo(result); + return 0; +} + +static udp_conn_t *udp_conn_alloc(int fd, int connected, int enable_timestamping, latency_logger_t *logger, const char *node_role, const char *node_id, tx_timestamp_debug_logger_t *debug_logger) { + udp_conn_t *conn = (udp_conn_t *) calloc(1, sizeof(*conn)); + if (conn == NULL) { + return NULL; + } + conn->fd = fd; + conn->connected = connected; + conn->timestamping_enabled = enable_timestamping; + conn->logger = logger; + conn->debug_logger = debug_logger; + snprintf(conn->node_role, sizeof(conn->node_role), "%s", node_role == NULL ? "" : node_role); + snprintf(conn->node_id, sizeof(conn->node_id), "%s", node_id == NULL ? "" : node_id); + conn->recv_buffer = (uint8_t *) malloc(OMNI_MAX_FRAME_SIZE); + if (conn->recv_buffer == NULL) { + free(conn); + return NULL; + } + conn->recv_buffer_cap = OMNI_MAX_FRAME_SIZE; + pthread_mutex_init(&conn->write_mu, NULL); + pthread_mutex_init(&conn->pending_mu, NULL); + return conn; +} + +static void udp_pending_destroy(udp_pending_tx_t *pending) { + while (pending != NULL) { + udp_pending_tx_t *next = pending->next; + protocol_message_clear(&pending->msg); + free(pending); + pending = next; + } +} + +static int udp_debug_log_send_chunk(udp_conn_t *conn, const message_t *msg, int bytes_written, uint32_t tx_id) { + tx_timestamp_debug_record_t record; + if (conn->debug_logger == NULL) { + return 0; + } + memset(&record, 0, sizeof(record)); + snprintf(record.record_type, sizeof(record.record_type), "%s", TX_TIMESTAMP_DEBUG_RECORD_SEND_CHUNK); + snprintf(record.node_role, sizeof(record.node_role), "%s", conn->node_role); + snprintf(record.node_id, sizeof(record.node_id), "%s", conn->node_id); + record.message_type = msg->type; + record.message_id = msg->id; + snprintf(record.from, sizeof(record.from), "%s", msg->from); + snprintf(record.to, sizeof(record.to), "%s", msg->to); + snprintf(record.file_name, sizeof(record.file_name), "%s", msg->file_name); + record.body_size = (int) msg->body_len; + record.send_call_index = 0; + record.frame_offset_start = 0; + record.frame_offset_end = bytes_written > 0 ? bytes_written - 1 : 0; + record.bytes_written = bytes_written; + record.expected_tx_id = tx_id; + return tx_timestamp_debug_log(conn->debug_logger, &record); +} + +static void udp_debug_log_errqueue_event(udp_conn_t *conn, const message_t *msg, const omni_tx_timestamp_event_t *event, uint32_t tx_id, int selected) { + tx_timestamp_debug_record_t record; + if (conn->debug_logger == NULL) { + return; + } + memset(&record, 0, sizeof(record)); + snprintf(record.record_type, sizeof(record.record_type), "%s", TX_TIMESTAMP_DEBUG_RECORD_ERRQUEUE_EVENT); + snprintf(record.node_role, sizeof(record.node_role), "%s", conn->node_role); + snprintf(record.node_id, sizeof(record.node_id), "%s", conn->node_id); + record.message_type = msg->type; + record.message_id = msg->id; + snprintf(record.from, sizeof(record.from), "%s", msg->from); + snprintf(record.to, sizeof(record.to), "%s", msg->to); + snprintf(record.file_name, sizeof(record.file_name), "%s", msg->file_name); + record.body_size = (int) msg->body_len; + snprintf(record.phase, sizeof(record.phase), "%s", "background"); + record.read_index = 0; + snprintf(record.event_name, sizeof(record.event_name), "%s", event->event_name); + record.ts_unix_nano = event->ts_unix_nano; + record.ee_info = event->ee_info; + record.ee_data = event->ee_data; + record.expected_tx_id = tx_id; + record.selected_for_latency = selected; + tx_timestamp_debug_log(conn->debug_logger, &record); +} + +static void *udp_errqueue_thread_main(void *arg) { + udp_conn_t *conn = (udp_conn_t *) arg; + uint8_t control[512]; + struct msghdr msg; + struct iovec iov; + uint8_t dummy; + + while (!conn->closed) { + ssize_t rc; + omni_tx_timestamp_event_t event; + udp_pending_tx_t *prev = NULL; + udp_pending_tx_t *cur = NULL; + memset(&msg, 0, sizeof(msg)); + memset(control, 0, sizeof(control)); + dummy = 0; + iov.iov_base = &dummy; + iov.iov_len = sizeof(dummy); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = control; + msg.msg_controllen = sizeof(control); + rc = recvmsg(conn->fd, &msg, MSG_ERRQUEUE | MSG_DONTWAIT); + if (rc < 0) { + if (errno == EAGAIN || errno == EWOULDBLOCK || errno == EINTR) { + usleep(10000); + continue; + } + if (conn->closed) { + return NULL; + } + usleep(10000); + continue; + } + if (linux_timestamping_parse_tx_timestamp(&msg, &event) != 0) { + continue; + } + pthread_mutex_lock(&conn->pending_mu); + cur = NULL; + prev = NULL; + { + udp_pending_tx_t **head = &conn->pending_head; + udp_pending_tx_t *iter = *head; + while (iter != NULL) { + if (iter->tx_id == event.ee_data) { + cur = iter; + break; + } + prev = iter; + iter = iter->next; + } + if (cur != NULL) { + if (strcmp(event.event_name, EVENT_A_TX_SCHED) == 0 && !cur->saw_sched) { + cur->saw_sched = 1; + latencylog_log_message_event_at(conn->logger, conn->node_role, conn->node_id, EVENT_A_TX_SCHED, event.ts_unix_nano, &cur->msg); + } else if (strcmp(event.event_name, EVENT_A_TX_SOFTWARE) == 0 && !cur->saw_software) { + cur->saw_software = 1; + latencylog_log_message_event_at(conn->logger, conn->node_role, conn->node_id, EVENT_A_TX_SOFTWARE, event.ts_unix_nano, &cur->msg); + } + udp_debug_log_errqueue_event(conn, &cur->msg, &event, cur->tx_id, 1); + if (cur->saw_sched && cur->saw_software) { + if (prev == NULL) { + *head = cur->next; + } else { + prev->next = cur->next; + } + protocol_message_clear(&cur->msg); + free(cur); + } + } + } + pthread_mutex_unlock(&conn->pending_mu); + } + return NULL; +} + +static int udp_conn_start_errqueue(udp_conn_t *conn) { + if (!conn->timestamping_enabled) { + return 0; + } + if (pthread_create(&conn->errqueue_thread, NULL, udp_errqueue_thread_main, conn) != 0) { + return -1; + } + conn->errqueue_thread_started = 1; + return 0; +} + +udp_conn_t *udp_conn_dial(const char *server_addr, const char *bind_ip, const char *bind_device, int enable_timestamping, latency_logger_t *logger, const char *node_role, const char *node_id, tx_timestamp_debug_logger_t *debug_logger) { + struct sockaddr_storage remote_addr; + struct sockaddr_storage local_addr; + socklen_t remote_len; + socklen_t local_len; + int family; + int fd; + udp_conn_t *conn; + + if (omni_parse_sockaddr(server_addr, 0, &remote_addr, &remote_len, &family) != 0) { + return NULL; + } + fd = udp_open_socket_for_addr((struct sockaddr *) &remote_addr, remote_len, bind_device != NULL && bind_device[0] != '\0', bind_device); + if (fd < 0) { + return NULL; + } + if (bind_ip != NULL && bind_ip[0] != '\0') { + if (udp_resolve_ip_only(bind_ip, family, &local_addr, &local_len) != 0 || bind(fd, (struct sockaddr *) &local_addr, local_len) != 0) { + close(fd); + return NULL; + } + } + if (connect(fd, (struct sockaddr *) &remote_addr, remote_len) != 0) { + close(fd); + return NULL; + } + if (enable_timestamping && linux_timestamping_enable_udp_socket(fd, 1) != 0) { + close(fd); + return NULL; + } + conn = udp_conn_alloc(fd, 1, enable_timestamping, logger, node_role, node_id, debug_logger); + if (conn == NULL) { + close(fd); + return NULL; + } + if (udp_conn_start_errqueue(conn) != 0) { + udp_conn_free(conn); + return NULL; + } + return conn; +} + +udp_conn_t *udp_conn_bind(const char *listen_addr, const char *bind_device, int enable_timestamping, latency_logger_t *logger, const char *node_role, const char *node_id, tx_timestamp_debug_logger_t *debug_logger) { + struct sockaddr_storage local_addr; + socklen_t local_len; + int family; + int fd; + udp_conn_t *conn; + if (omni_parse_sockaddr(listen_addr, 1, &local_addr, &local_len, &family) != 0) { + return NULL; + } + fd = udp_open_socket_for_addr((struct sockaddr *) &local_addr, local_len, bind_device != NULL && bind_device[0] != '\0', bind_device); + if (fd < 0) { + return NULL; + } + if (bind(fd, (struct sockaddr *) &local_addr, local_len) != 0) { + close(fd); + return NULL; + } + if (enable_timestamping && linux_timestamping_enable_udp_socket(fd, 1) != 0) { + close(fd); + return NULL; + } + conn = udp_conn_alloc(fd, 0, enable_timestamping, logger, node_role, node_id, debug_logger); + if (conn == NULL) { + close(fd); + return NULL; + } + if (udp_conn_start_errqueue(conn) != 0) { + udp_conn_free(conn); + return NULL; + } + return conn; +} + +static int udp_conn_send_inner(udp_conn_t *conn, const message_t *msg, const struct sockaddr *addr, socklen_t addr_len) { + uint8_t *payload = NULL; + size_t payload_len = 0; + ssize_t rc; + udp_pending_tx_t *pending = NULL; + uint32_t tx_id; + + if (protocol_encode_message_datagram(msg, &payload, &payload_len) != 0) { + return -1; + } + pthread_mutex_lock(&conn->write_mu); + latencylog_log_message_event(conn->logger, conn->node_role, conn->node_id, EVENT_SEND_HANDOFF_BEGIN, msg); + tx_id = conn->next_tx_id++; + if (conn->timestamping_enabled) { + pending = (udp_pending_tx_t *) calloc(1, sizeof(*pending)); + if (pending == NULL || protocol_message_copy(&pending->msg, msg) != 0) { + free(payload); + pthread_mutex_unlock(&conn->write_mu); + free(pending); + return -1; + } + pending->tx_id = tx_id; + pending->bytes_written = (int) payload_len; + pthread_mutex_lock(&conn->pending_mu); + pending->next = conn->pending_head; + conn->pending_head = pending; + pthread_mutex_unlock(&conn->pending_mu); + udp_debug_log_send_chunk(conn, msg, (int) payload_len, tx_id); + } + if (addr != NULL) { + rc = sendto(conn->fd, payload, payload_len, 0, addr, addr_len); + } else { + rc = send(conn->fd, payload, payload_len, 0); + } + free(payload); + if (rc < 0 || (size_t) rc != payload_len) { + if (pending != NULL) { + udp_pending_tx_t *prev = NULL; + udp_pending_tx_t *cur; + pthread_mutex_lock(&conn->pending_mu); + for (cur = conn->pending_head; cur != NULL; cur = cur->next) { + if (cur == pending) { + if (prev == NULL) { + conn->pending_head = cur->next; + } else { + prev->next = cur->next; + } + break; + } + prev = cur; + } + pthread_mutex_unlock(&conn->pending_mu); + protocol_message_clear(&pending->msg); + free(pending); + } + pthread_mutex_unlock(&conn->write_mu); + return -1; + } + latencylog_log_message_event(conn->logger, conn->node_role, conn->node_id, EVENT_SEND_HANDOFF_END, msg); + pthread_mutex_unlock(&conn->write_mu); + return 0; +} + +int udp_conn_send(udp_conn_t *conn, const message_t *msg) { + if (conn == NULL || !conn->connected) { + errno = ENOTCONN; + return -1; + } + return udp_conn_send_inner(conn, msg, NULL, 0); +} + +int udp_conn_send_to(udp_conn_t *conn, const message_t *msg, const struct sockaddr *addr, socklen_t addr_len) { + if (conn == NULL || addr == NULL) { + errno = EINVAL; + return -1; + } + return udp_conn_send_inner(conn, msg, addr, addr_len); +} + +int udp_conn_receive(udp_conn_t *conn, message_t *out_msg, struct sockaddr_storage *addr, socklen_t *addr_len) { + uint8_t control[512]; + struct sockaddr_storage source; + struct iovec iov; + struct msghdr msg; + ssize_t n; + int64_t rx_ts = 0; + char err[128]; + + if (conn == NULL || out_msg == NULL) { + errno = EINVAL; + return -1; + } + memset(&msg, 0, sizeof(msg)); + memset(&source, 0, sizeof(source)); + iov.iov_base = conn->recv_buffer; + iov.iov_len = conn->recv_buffer_cap; + msg.msg_name = &source; + msg.msg_namelen = sizeof(source); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + if (conn->timestamping_enabled) { + msg.msg_control = control; + msg.msg_controllen = sizeof(control); + } + 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) { + rx_ts = linux_timestamping_parse_rx_timestamp(&msg); + } + if (protocol_decode_message_datagram(conn->recv_buffer, (size_t) n, out_msg, err, sizeof(err)) != 0) { + errno = EPROTO; + return -1; + } + if (addr != NULL && addr_len != NULL) { + omni_clone_sockaddr((struct sockaddr *) &source, msg.msg_namelen, addr, addr_len); + } + if (rx_ts > 0) { + latencylog_log_message_event_at(conn->logger, conn->node_role, conn->node_id, EVENT_B_RX_SOFTWARE, rx_ts, out_msg); + } + return 0; +} + +int udp_conn_fd(const udp_conn_t *conn) { + return conn == NULL ? -1 : conn->fd; +} + +int udp_conn_local_addr(const udp_conn_t *conn, struct sockaddr_storage *addr, socklen_t *addr_len) { + socklen_t len = sizeof(*addr); + if (conn == NULL || addr == NULL || addr_len == NULL) { + errno = EINVAL; + return -1; + } + if (getsockname(conn->fd, (struct sockaddr *) addr, &len) != 0) { + return -1; + } + *addr_len = len; + return 0; +} + +int udp_conn_close(udp_conn_t *conn) { + if (conn == NULL) { + return 0; + } + 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); + conn->errqueue_thread_started = 0; + } + } + return 0; +} + +void udp_conn_free(udp_conn_t *conn) { + if (conn == NULL) { + return; + } + udp_conn_close(conn); + udp_pending_destroy(conn->pending_head); + free(conn->recv_buffer); + pthread_mutex_destroy(&conn->write_mu); + pthread_mutex_destroy(&conn->pending_mu); + free(conn); +} diff --git a/host/OmniSocketGo_add_camera/src/tx_timestamp_debug.c b/host/OmniSocketGo_add_camera/src/tx_timestamp_debug.c new file mode 100644 index 0000000..3cdf19c --- /dev/null +++ b/host/OmniSocketGo_add_camera/src/tx_timestamp_debug.c @@ -0,0 +1,108 @@ +#include "tx_timestamp_debug.h" + +tx_timestamp_debug_logger_t *tx_timestamp_debug_open_jsonl(const char *path) { + tx_timestamp_debug_logger_t *logger; + FILE *file; + if (path == NULL || path[0] == '\0') { + return NULL; + } + if (omni_ensure_parent_dir(path) != 0) { + return NULL; + } + file = fopen(path, "ab"); + if (file == NULL) { + return NULL; + } + logger = (tx_timestamp_debug_logger_t *) calloc(1, sizeof(*logger)); + if (logger == NULL) { + fclose(file); + return NULL; + } + omni_file_logger_init_path(&logger->file_logger, file, path, 0); + logger->enabled = 1; + return logger; +} + +void tx_timestamp_debug_close(tx_timestamp_debug_logger_t *logger) { + if (logger == NULL) { + return; + } + if (logger->file_logger.file != NULL) { + fclose(logger->file_logger.file); + } + omni_file_logger_destroy(&logger->file_logger); + free(logger); +} + +int tx_timestamp_debug_log(tx_timestamp_debug_logger_t *logger, const tx_timestamp_debug_record_t *record) { + char *line; + char *node_role; + char *node_id; + char *from; + char *to; + char *file_name; + char *phase; + char *event_name; + + if (logger == NULL || record == NULL || !logger->enabled) { + return 0; + } + node_role = omni_json_escape(record->node_role); + node_id = omni_json_escape(record->node_id); + from = omni_json_escape(record->from); + to = omni_json_escape(record->to); + file_name = omni_json_escape(record->file_name); + phase = omni_json_escape(record->phase); + event_name = omni_json_escape(record->event_name); + if (node_role == NULL || node_id == NULL || from == NULL || to == NULL || file_name == NULL || phase == NULL || event_name == NULL) { + free(node_role); + free(node_id); + free(from); + free(to); + free(file_name); + free(phase); + free(event_name); + return -1; + } + line = omni_strdup_printf( + "{\"record_type\":\"%s\",\"node_role\":\"%s\",\"node_id\":\"%s\",\"message_type\":\"%s\",\"message_id\":%" PRIu64 ",\"from\":\"%s\",\"to\":\"%s\",\"file_name\":\"%s\",\"body_size\":%d,\"phase\":\"%s\",\"send_call_index\":%d,\"frame_offset_start\":%d,\"frame_offset_end\":%d,\"bytes_written\":%d,\"expected_tx_id\":%u,\"read_index\":%d,\"event_name\":\"%s\",\"ts_unix_nano\":%" PRId64 ",\"ee_info\":%u,\"ee_data\":%u,\"matched_send_call_index\":%d,\"selected_for_latency\":%d}", + record->record_type, + node_role, + node_id, + protocol_message_type_name(record->message_type), + record->message_id, + from, + to, + file_name, + record->body_size, + phase, + record->send_call_index, + record->frame_offset_start, + record->frame_offset_end, + record->bytes_written, + record->expected_tx_id, + record->read_index, + event_name, + record->ts_unix_nano, + record->ee_info, + record->ee_data, + record->matched_send_call_index, + record->selected_for_latency + ); + free(node_role); + free(node_id); + free(from); + free(to); + free(file_name); + free(phase); + free(event_name); + if (line == NULL) { + return -1; + } + if (omni_file_logger_write_line(&logger->file_logger, line) != 0) { + free(line); + return -1; + } + free(line); + return 0; +} diff --git a/host/OmniSocketGo_add_camera/src/video_pipeline.c b/host/OmniSocketGo_add_camera/src/video_pipeline.c new file mode 100644 index 0000000..026ed3c --- /dev/null +++ b/host/OmniSocketGo_add_camera/src/video_pipeline.c @@ -0,0 +1,1497 @@ +#include "video_pipeline.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#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_HEAD_CAMERA_DEVICE "/dev/video26" +#define VIDEO_DEFAULT_WAIST_CAMERA_DEVICE "/dev/video18" +#define VIDEO_DEFAULT_PEER_ID "peer-b-video" +#define VIDEO_DEFAULT_TARGET_PEER "peer-a-video" +#define VIDEO_SOFT_BACKPRESSURE_SEGMENTS_DEFAULT 64 +#define VIDEO_HARD_BACKPRESSURE_SEGMENTS_DEFAULT 192 +#define VIDEO_HARD_BACKPRESSURE_HOLD_MS_DEFAULT 1000 +#define VIDEO_DEFAULT_FRAME_STALL_RECONNECT_MS 3000 +#define VIDEO_SOFT_BACKPRESSURE_WINDOW_PRESSURE_PCT 90.0 +#define VIDEO_HARD_BACKPRESSURE_WINDOW_PRESSURE_PCT 98.0 +#define VIDEO_SESSION_POLL_INTERVAL_MS 250 + +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; + uint64_t next_frame_seq; +} 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 int env_int_or_default(const char *name, int fallback) { + const char *value = getenv(name); + int parsed; + + if (value == NULL || value[0] == '\0') { + return fallback; + } + parsed = atoi(value); + if (parsed <= 0) { + return fallback; + } + return parsed; +} + +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); +} + +static void video_pipeline_report_progress(const video_pipeline_config_t *config) { + if (config == NULL || config->progress_callback == NULL) { + return; + } + config->progress_callback(config->progress_context); +} + +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->camera_head_device = VIDEO_DEFAULT_HEAD_CAMERA_DEVICE; + config->camera_waist_device = VIDEO_DEFAULT_WAIST_CAMERA_DEVICE; + config->active_camera = NULL; + 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; + config->soft_backpressure_segments = VIDEO_SOFT_BACKPRESSURE_SEGMENTS_DEFAULT; + config->hard_backpressure_segments = VIDEO_HARD_BACKPRESSURE_SEGMENTS_DEFAULT; + config->hard_backpressure_hold_ms = VIDEO_HARD_BACKPRESSURE_HOLD_MS_DEFAULT; + config->frame_stall_reconnect_ms = VIDEO_DEFAULT_FRAME_STALL_RECONNECT_MS; + config->stats_logger = NULL; + config->stage_logger = NULL; + config->stats_interval_ms = 1000; +} + +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->camera_head_device = env_or_default("OMNI_CAMERA_HEAD_DEVICE", config->camera_head_device); + config->camera_waist_device = env_or_default("OMNI_CAMERA_WAIST_DEVICE", config->camera_waist_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); + config->soft_backpressure_segments = env_int_or_default("OMNI_VIDEO_SOFT_BACKPRESSURE_SEGMENTS", config->soft_backpressure_segments); + config->hard_backpressure_segments = env_int_or_default("OMNI_VIDEO_HARD_BACKPRESSURE_SEGMENTS", config->hard_backpressure_segments); + config->hard_backpressure_hold_ms = env_int_or_default("OMNI_VIDEO_HARD_BACKPRESSURE_HOLD_MS", config->hard_backpressure_hold_ms); + config->frame_stall_reconnect_ms = env_int_or_default("OMNI_VIDEO_FRAME_STALL_RECONNECT_MS", config->frame_stall_reconnect_ms); + config->stats_interval_ms = env_int_or_default("BLITZ_KCP_STATS_INTERVAL_MS", config->stats_interval_ms); +} + +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->backpressure_drops = stats->backpressure_drops; + out_stats->backlog_resets = stats->backlog_resets; + out_stats->last_frame_bytes = stats->last_frame_bytes; + out_stats->last_backlog_segments = stats->last_backlog_segments; + out_stats->last_capture_to_send_ms = stats->last_capture_to_send_ms; + out_stats->avg_capture_to_send_ms = stats->avg_capture_to_send_ms; + out_stats->connected = stats->connected; + snprintf(out_stats->last_error, sizeof(out_stats->last_error), "%s", stats->last_error); + snprintf(out_stats->last_backlog_reason, sizeof(out_stats->last_backlog_reason), "%s", stats->last_backlog_reason); + 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, + config->stats_logger, + config->stats_interval_ms + ); + if (sender->client == NULL) { + return -1; + } + return 0; +} + +static int video_sender_drain_pending_messages(video_sender_t *sender) { + int drained = 0; + + if (sender == NULL || sender->client == NULL) { + errno = EINVAL; + return -1; + } + + for (;;) { + message_t msg; + int rc; + + protocol_message_init(&msg); + rc = kcp_client_receive_timed(sender->client, &msg, 1); + if (rc == 1) { + protocol_message_clear(&msg); + return 0; + } + if (rc != 0) { + protocol_message_clear(&msg); + return -1; + } + + // Drain unread server errors so an offline receiver cannot back up the reverse KCP stream. + protocol_message_clear(&msg); + drained += 1; + if (drained >= 8) { + return 0; + } + } +} + +static int video_sender_send_packet( + video_sender_t *sender, + const AVPacket *encoded_pkt, + const video_pipeline_packet_metadata_t *metadata, + uint64_t *out_frame_seq +) { + uint8_t *payload; + size_t payload_len; + uint64_t frame_seq; + int rc; + + if (sender == NULL || sender->client == NULL || encoded_pkt == NULL || metadata == NULL) { + errno = EINVAL; + return -1; + } + + frame_seq = sender->next_frame_seq + 1U; + payload_len = 8U + (size_t) encoded_pkt->size + sizeof(*metadata); + if (video_sender_ensure_buffer_capacity(sender, payload_len) != 0) { + return -1; + } + payload = sender->send_buffer; + + payload[0] = (uint8_t) (frame_seq >> 56); + payload[1] = (uint8_t) (frame_seq >> 48); + payload[2] = (uint8_t) (frame_seq >> 40); + payload[3] = (uint8_t) (frame_seq >> 32); + payload[4] = (uint8_t) (frame_seq >> 24); + payload[5] = (uint8_t) (frame_seq >> 16); + payload[6] = (uint8_t) (frame_seq >> 8); + payload[7] = (uint8_t) frame_seq; + memcpy(payload + 8U, encoded_pkt->data, (size_t) encoded_pkt->size); + memcpy(payload + 8U + (size_t) encoded_pkt->size, metadata, sizeof(*metadata)); + rc = kcp_client_send_binary(sender->client, sender->target_peer, payload, payload_len); + if (rc != 0) { + return rc; + } + sender->next_frame_seq = frame_seq; + if (out_frame_seq != NULL) { + *out_frame_seq = frame_seq; + } + rc = video_sender_drain_pending_messages(sender); + 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 uint32_t video_sender_backlog_segments(const kcp_runtime_stats_t *stats) { + if (stats == NULL) { + return 0; + } + return stats->snd_queue + stats->snd_buffer; +} + +static int video_sender_soft_backpressure_active(const video_pipeline_config_t *config, const kcp_runtime_stats_t *transport) { + if (config == NULL || transport == NULL) { + return 0; + } + return video_sender_backlog_segments(transport) >= (uint32_t) config->soft_backpressure_segments + || transport->window_pressure_pct >= VIDEO_SOFT_BACKPRESSURE_WINDOW_PRESSURE_PCT; +} + +static int video_sender_hard_backpressure_active(const video_pipeline_config_t *config, const kcp_runtime_stats_t *transport) { + if (config == NULL || transport == NULL) { + return 0; + } + return video_sender_backlog_segments(transport) >= (uint32_t) config->hard_backpressure_segments + || transport->window_pressure_pct >= VIDEO_HARD_BACKPRESSURE_WINDOW_PRESSURE_PCT; +} + +static void video_pipeline_note_backpressure( + video_pipeline_stats_t *stats, + const char *reason, + const kcp_runtime_stats_t *transport, + int increment_drop, + int increment_reset +) { + if (stats == NULL) { + return; + } + pthread_mutex_lock(&stats->mutex); + if (increment_drop) { + stats->backpressure_drops += 1; + } + if (increment_reset) { + stats->backlog_resets += 1; + } + if (transport != NULL) { + stats->last_backlog_segments = video_sender_backlog_segments(transport); + stats->transport = *transport; + } else { + stats->last_backlog_segments = 0; + } + snprintf(stats->last_backlog_reason, sizeof(stats->last_backlog_reason), "%s", reason == NULL ? "" : reason); + pthread_mutex_unlock(&stats->mutex); +} + +static void video_pipeline_note_capture_to_send(video_pipeline_stats_t *stats, uint32_t capture_to_send_ms) { + if (stats == NULL) { + return; + } + pthread_mutex_lock(&stats->mutex); + stats->last_capture_to_send_ms = capture_to_send_ms; + if (stats->avg_capture_to_send_ms <= 0.0) { + stats->avg_capture_to_send_ms = (double) capture_to_send_ms; + } else { + stats->avg_capture_to_send_ms = stats->avg_capture_to_send_ms * 0.9 + (double) capture_to_send_ms * 0.1; + } + pthread_mutex_unlock(&stats->mutex); +} + +static int video_stage_logger_should_log(const video_stage_logger_t *logger, uint64_t frame_seq) { + if (logger == NULL || !logger->enabled) { + return 0; + } + if (logger->sample_mod <= 1U) { + return 1; + } + return frame_seq % logger->sample_mod == 0U; +} + +static void video_stage_logger_log_frame( + video_stage_logger_t *logger, + uint64_t frame_seq, + double capture_ms, + double decode_ms, + double scale_ms, + double encode_ms, + double send_ms, + double pipeline_total_ms, + size_t jpeg_bytes, + uint64_t kcp_out_seg_delta, + uint32_t backlog_segments, + double window_pressure_pct, + int32_t video_srtt_ms +) { + char *line; + + if (!video_stage_logger_should_log(logger, frame_seq)) { + return; + } + line = omni_strdup_printf( + "{\"ts_unix_nano\":%" PRId64 ",\"frame_seq\":%" PRIu64 ",\"capture_ms\":%.3f,\"decode_ms\":%.3f,\"scale_ms\":%.3f,\"encode_ms\":%.3f,\"send_ms\":%.3f,\"pipeline_total_ms\":%.3f,\"jpeg_bytes\":%zu,\"kcp_out_seg_delta\":%" PRIu64 ",\"backlog_segments\":%u,\"window_pressure_pct\":%.3f,\"video_srtt_ms\":%d}", + omni_now_unix_nano(), + frame_seq, + capture_ms, + decode_ms, + scale_ms, + encode_ms, + send_ms, + pipeline_total_ms, + jpeg_bytes, + kcp_out_seg_delta, + backlog_segments, + window_pressure_pct, + video_srtt_ms + ); + if (line == NULL) { + return; + } + (void) omni_file_logger_write_line(&logger->file_logger, line); + free(line); +} + +video_stage_logger_t *video_stage_logger_open_jsonl(const char *path, uint64_t sample_mod) { + video_stage_logger_t *logger; + FILE *file; + + if (path == NULL || path[0] == '\0') { + return NULL; + } + if (omni_ensure_parent_dir(path) != 0) { + return NULL; + } + file = fopen(path, "ab"); + if (file == NULL) { + return NULL; + } + logger = (video_stage_logger_t *) calloc(1, sizeof(*logger)); + if (logger == NULL) { + fclose(file); + return NULL; + } + omni_file_logger_init_path(&logger->file_logger, file, path, 0); + logger->enabled = 1; + logger->sample_mod = sample_mod == 0U ? 1U : sample_mod; + return logger; +} + +void video_stage_logger_close(video_stage_logger_t *logger) { + if (logger == NULL) { + return; + } + if (logger->file_logger.file != NULL) { + fclose(logger->file_logger.file); + } + omni_file_logger_destroy(&logger->file_logger); + free(logger); +} + +static int video_server_error_requires_reconnect(const char *message) { + if (message == NULL || message[0] == '\0') { + return 0; + } + return strstr(message, "not registered") != NULL + || strstr(message, "first message must be register") != NULL + || strstr(message, "peer replaced") != NULL + || strstr(message, "timed out waiting for server_register_ok") != NULL + || strstr(message, "failed to acknowledge server heartbeat") != NULL; +} + +static void video_pipeline_update_connection_state( + video_pipeline_stats_t *stats, + const kcp_client_state_t *client_state, + const kcp_runtime_stats_t *transport +) { + if (stats == NULL) { + return; + } + + pthread_mutex_lock(&stats->mutex); + if (transport != NULL) { + stats->transport = *transport; + } + if (client_state != NULL) { + stats->connected = client_state->connected != 0 && client_state->registered != 0; + if (client_state->last_server_error[0] != '\0') { + snprintf(stats->last_error, sizeof(stats->last_error), "%s", client_state->last_server_error); + } + } + pthread_mutex_unlock(&stats->mutex); +} + +static int video_sender_check_session_stale( + video_sender_t *sender, + const video_pipeline_config_t *config, + video_pipeline_stats_t *stats, + kcp_runtime_stats_t *transport_stats, + char *reason, + size_t reason_len +) { + kcp_client_state_t client_state; + + if ( + sender == NULL || sender->client == NULL || config == NULL || stats == NULL || transport_stats == NULL + || reason == NULL || reason_len == 0 + ) { + errno = EINVAL; + return -1; + } + + reason[0] = '\0'; + memset(&client_state, 0, sizeof(client_state)); + kcp_client_runtime_stats_snapshot(sender->client, transport_stats); + kcp_client_state_snapshot(sender->client, &client_state); + video_pipeline_update_connection_state(stats, &client_state, transport_stats); + + if (!transport_stats->connected || !client_state.connected) { + snprintf(reason, reason_len, "video session stale: transport disconnected"); + return 1; + } + if (!client_state.registered) { + snprintf(reason, reason_len, "video session stale: server reported unregistered"); + return 1; + } + if (video_server_error_requires_reconnect(client_state.last_server_error)) { + snprintf(reason, reason_len, "video session stale: server error %.180s", client_state.last_server_error); + return 1; + } + return 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); +} + +typedef struct video_camera_source { + const char *name; + const char *device; + int fd; + video_buffer_t *buffers; + int num_buffers; + int streaming; +} video_camera_source_t; + +static void video_camera_source_cleanup(video_camera_source_t *source) { + enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + + if (source == NULL) { + return; + } + if (source->fd >= 0 && source->streaming) { + (void) ioctl(source->fd, VIDIOC_STREAMOFF, &type); + } + video_pipeline_cleanup_buffers(source->buffers, source->num_buffers); + if (source->fd >= 0) { + close(source->fd); + } + source->fd = -1; + source->buffers = NULL; + source->num_buffers = 0; + source->streaming = 0; +} + +static int video_camera_source_start(video_camera_source_t *source, int width, int height) { + enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + int i; + + source->fd = open_v4l2_device(source->device); + if (source->fd < 0 || init_v4l2_device(source->fd, width, height) < 0 + || init_mmap(source->fd, &source->buffers, &source->num_buffers) < 0) { + return -1; + } + for (i = 0; i < source->num_buffers; 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(source->fd, VIDIOC_QBUF, &buf) < 0) { + return -1; + } + } + if (ioctl(source->fd, VIDIOC_STREAMON, &type) < 0) { + return -1; + } + source->streaming = 1; + fprintf(stderr, "[video_pipeline] camera %s ready on %s\n", source->name, source->device); + return 0; +} + +static void video_camera_source_discard_ready(video_camera_source_t *source) { + struct v4l2_buffer buf; + + if (source == NULL || source->fd < 0) { + return; + } + memset(&buf, 0, sizeof(buf)); + buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + buf.memory = V4L2_MEMORY_MMAP; + if (ioctl(source->fd, VIDIOC_DQBUF, &buf) == 0) { + (void) ioctl(source->fd, VIDIOC_QBUF, &buf); + } +} + +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_camera_source_t cameras[2] = { + {.name = "head", .fd = -1}, + {.name = "waist", .fd = -1} + }; + AVCodecContext *decoder = NULL; + AVCodecContext *encoder = NULL; + struct SwsContext *sws_ctx = NULL; + int frame_index = 0; + int rc = -1; + int sws_src_width = 0; + int sws_src_height = 0; + int sws_src_format = -1; + uint32_t hard_backpressure_since_ms = 0; + uint32_t last_soft_drop_log_ms = 0; + uint32_t last_session_poll_ms = 0; + uint32_t last_successful_send_ms = 0; + uint64_t soft_drops_since_last_send = 0; + int have_sent_frame = 0; + const char *gpsd_host = env_or_default("OMNI_GPSD_HOST", "127.0.0.1"); + int gps_buffer_started = 0; + + 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; + } + + cameras[VIDEO_CAMERA_HEAD].device = config->active_camera == NULL + ? config->camera_device + : config->camera_head_device; + cameras[VIDEO_CAMERA_WAIST].device = config->camera_waist_device; + if (video_camera_source_start(&cameras[VIDEO_CAMERA_HEAD], config->capture_width, config->capture_height) < 0) { + video_pipeline_set_errno_error(stats, "failed to start head camera"); + goto cleanup; + } + if (config->active_camera != NULL + && video_camera_source_start(&cameras[VIDEO_CAMERA_WAIST], config->capture_width, config->capture_height) < 0) { + video_pipeline_set_errno_error(stats, "failed to start waist camera"); + 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; + } + if (gps_buffer_init(gpsd_host) != 0) { + fprintf(stderr, "[video_pipeline] failed to start GPS buffer using %s:2947\n", gpsd_host); + } else { + gps_buffer_started = 1; + } + + pthread_mutex_lock(&stats->mutex); + stats->connected = 1; + stats->last_error[0] = '\0'; + pthread_mutex_unlock(&stats->mutex); + + 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; + kcp_runtime_stats_t transport_stats; + kcp_runtime_stats_t transport_after_send; + int select_rc; + int should_log_stage = 0; + 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; + video_pipeline_packet_metadata_t packet_metadata; + char reconnect_reason[256]; + int frame_number = frame_index + 1; + uint64_t frame_seq = 0; + uint64_t out_segs_before_send = 0; + uint64_t out_segs_after_send = 0; + uint32_t capture_to_send_ms = 0; + int active_camera = config->active_camera == NULL + ? VIDEO_CAMERA_HEAD + : atomic_load(config->active_camera); + video_camera_source_t *active_source; + video_camera_source_t *standby_source; + + if (active_camera != VIDEO_CAMERA_WAIST) { + active_camera = VIDEO_CAMERA_HEAD; + } + active_source = &cameras[active_camera]; + standby_source = config->active_camera == NULL + ? NULL + : &cameras[active_camera == VIDEO_CAMERA_HEAD ? VIDEO_CAMERA_WAIST : VIDEO_CAMERA_HEAD]; + + memset(&transport_stats, 0, sizeof(transport_stats)); + memset(&transport_after_send, 0, sizeof(transport_after_send)); + memset(&packet_metadata, 0, sizeof(packet_metadata)); + reconnect_reason[0] = '\0'; + video_pipeline_report_progress(config); + + if (config->max_frames > 0 && frame_index >= config->max_frames) { + break; + } + total_start_ms = video_pipeline_now_ms(); + + FD_ZERO(&fds); + FD_SET(cameras[VIDEO_CAMERA_HEAD].fd, &fds); + if (cameras[VIDEO_CAMERA_WAIST].fd >= 0) { + FD_SET(cameras[VIDEO_CAMERA_WAIST].fd, &fds); + } + timeout.tv_sec = 2; + timeout.tv_usec = 0; + select_rc = select( + (cameras[VIDEO_CAMERA_HEAD].fd > cameras[VIDEO_CAMERA_WAIST].fd + ? cameras[VIDEO_CAMERA_HEAD].fd + : cameras[VIDEO_CAMERA_WAIST].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 (standby_source != NULL && FD_ISSET(standby_source->fd, &fds)) { + video_camera_source_discard_ready(standby_source); + } + if (!FD_ISSET(active_source->fd, &fds)) { + continue; + } + 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(active_source->fd, VIDIOC_DQBUF, &buf) < 0) { + video_pipeline_set_errno_error(stats, "failed to dequeue V4L2 buffer"); + goto cleanup; + } + capture_end_ms = video_pipeline_now_ms(); + decode_start_ms = capture_end_ms; + + if (decode_mjpeg_frame(decoder, (const uint8_t *) active_source->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(active_source->fd, VIDIOC_QBUF, &buf); + continue; + } + 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(active_source->fd, VIDIOC_QBUF, &buf); + continue; + } + 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(active_source->fd, VIDIOC_QBUF, &buf); + continue; + } + encode_end_ms = video_pipeline_now_ms(); + send_start_ms = encode_end_ms; + + { + gps_video_sample_t gps_sample = get_latest_gps_for_video(); + + packet_metadata.timestamp_ms = (uint64_t) get_realtime_ms(); + packet_metadata.latitude = gps_sample.latitude; + packet_metadata.longitude = gps_sample.longitude; + } + + if ( + last_session_poll_ms == 0 + || omni_now_millis32() - last_session_poll_ms >= VIDEO_SESSION_POLL_INTERVAL_MS + ) { + if (video_sender_drain_pending_messages(&sender) != 0) { + video_pipeline_set_errno_error(stats, "failed to poll video session"); + av_frame_free(&decoded_frame); + av_frame_free(&scaled_frame); + av_packet_free(&encoded_pkt); + (void) ioctl(active_source->fd, VIDIOC_QBUF, &buf); + rc = VIDEO_PIPELINE_RUN_RETRY_IMMEDIATE; + goto cleanup; + } + if ( + video_sender_check_session_stale( + &sender, + config, + stats, + &transport_stats, + reconnect_reason, + sizeof(reconnect_reason) + ) != 0 + ) { + if (reconnect_reason[0] == '\0') { + snprintf(reconnect_reason, sizeof(reconnect_reason), "video session stale: poll failed"); + } + video_pipeline_set_error(stats, reconnect_reason); + fprintf(stderr, "[video_pipeline] %s\n", reconnect_reason); + av_frame_free(&decoded_frame); + av_frame_free(&scaled_frame); + av_packet_free(&encoded_pkt); + (void) ioctl(active_source->fd, VIDIOC_QBUF, &buf); + rc = VIDEO_PIPELINE_RUN_RETRY_IMMEDIATE; + goto cleanup; + } + last_session_poll_ms = omni_now_millis32(); + } else { + kcp_client_runtime_stats_snapshot(sender.client, &transport_stats); + } + if (video_sender_hard_backpressure_active(config, &transport_stats)) { + uint32_t now_ms = omni_now_millis32(); + + if (hard_backpressure_since_ms == 0) { + hard_backpressure_since_ms = now_ms; + } + if (now_ms - hard_backpressure_since_ms >= (uint32_t) config->hard_backpressure_hold_ms) { + char reason[128]; + uint32_t backlog_segments = video_sender_backlog_segments(&transport_stats); + + snprintf( + reason, + sizeof(reason), + "hard_reset backlog=%u snd_queue=%u snd_buffer=%u window_pressure=%.1f%% hold_ms=%d", + backlog_segments, + transport_stats.snd_queue, + transport_stats.snd_buffer, + transport_stats.window_pressure_pct, + config->hard_backpressure_hold_ms + ); + video_pipeline_note_backpressure(stats, reason, &transport_stats, 0, 1); + video_pipeline_set_error(stats, reason); + fprintf( + stderr, + "[video_pipeline] backlog hard reset: backlog=%u snd_queue=%u snd_buffer=%u window_pressure=%.1f%% hold_ms=%d\n", + backlog_segments, + transport_stats.snd_queue, + transport_stats.snd_buffer, + transport_stats.window_pressure_pct, + config->hard_backpressure_hold_ms + ); + av_frame_free(&decoded_frame); + av_frame_free(&scaled_frame); + av_packet_free(&encoded_pkt); + (void) ioctl(active_source->fd, VIDIOC_QBUF, &buf); + rc = VIDEO_PIPELINE_RUN_RETRY_IMMEDIATE; + goto cleanup; + } + } else { + hard_backpressure_since_ms = 0; + } + + if (video_sender_soft_backpressure_active(config, &transport_stats)) { + uint32_t now_ms = omni_now_millis32(); + uint32_t backlog_segments = video_sender_backlog_segments(&transport_stats); + char reason[128]; + + snprintf( + reason, + sizeof(reason), + "soft_drop backlog=%u snd_queue=%u snd_buffer=%u window_pressure=%.1f%% threshold=%d", + backlog_segments, + transport_stats.snd_queue, + transport_stats.snd_buffer, + transport_stats.window_pressure_pct, + config->soft_backpressure_segments + ); + video_pipeline_note_backpressure(stats, reason, &transport_stats, 1, 0); + soft_drops_since_last_send += 1; + if (now_ms - last_soft_drop_log_ms >= 1000U) { + fprintf( + stderr, + "[video_pipeline] soft drop: backlog=%u snd_queue=%u snd_buffer=%u window_pressure=%.1f%% threshold=%d\n", + backlog_segments, + transport_stats.snd_queue, + transport_stats.snd_buffer, + transport_stats.window_pressure_pct, + config->soft_backpressure_segments + ); + last_soft_drop_log_ms = now_ms; + } + if ( + have_sent_frame + && config->frame_stall_reconnect_ms > 0 + && now_ms - last_successful_send_ms >= (uint32_t) config->frame_stall_reconnect_ms + ) { + char stall_reason[192]; + + snprintf( + stall_reason, + sizeof(stall_reason), + "video pipeline stalled: no frames sent for %u ms while soft dropping (%llu drops, backlog=%u, srtt=%d ms)", + now_ms - last_successful_send_ms, + (unsigned long long) soft_drops_since_last_send, + backlog_segments, + transport_stats.srtt_ms + ); + video_pipeline_set_error(stats, stall_reason); + fprintf(stderr, "[video_pipeline] %s\n", stall_reason); + av_frame_free(&decoded_frame); + av_frame_free(&scaled_frame); + av_packet_free(&encoded_pkt); + (void) ioctl(active_source->fd, VIDIOC_QBUF, &buf); + rc = VIDEO_PIPELINE_RUN_RETRY_IMMEDIATE; + goto cleanup; + } + av_frame_free(&decoded_frame); + av_frame_free(&scaled_frame); + av_packet_free(&encoded_pkt); + (void) ioctl(active_source->fd, VIDIOC_QBUF, &buf); + continue; + } + + capture_to_send_ms = send_start_ms <= capture_start_ms + ? 0U + : (uint32_t) (send_start_ms - capture_start_ms + 0.5); + packet_metadata.capture_to_send_ms = capture_to_send_ms; + out_segs_before_send = transport_stats.out_segs_total; + + if (video_sender_send_packet(&sender, encoded_pkt, &packet_metadata, &frame_seq) != 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(active_source->fd, VIDIOC_QBUF, &buf); + goto cleanup; + } + send_end_ms = video_pipeline_now_ms(); + should_log_stage = video_stage_logger_should_log(config->stage_logger, frame_seq); + if (should_log_stage) { + kcp_client_runtime_stats_snapshot(sender.client, &transport_after_send); + out_segs_after_send = transport_after_send.out_segs_total; + } else { + transport_after_send = transport_stats; + out_segs_after_send = out_segs_before_send; + } + video_pipeline_note_capture_to_send(stats, capture_to_send_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; + stats->transport = transport_after_send; + pthread_mutex_unlock(&stats->mutex); + have_sent_frame = 1; + last_successful_send_ms = omni_now_millis32(); + soft_drops_since_last_send = 0; + if (should_log_stage) { + video_stage_logger_log_frame( + config->stage_logger, + frame_seq, + 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, + (size_t) encoded_pkt->size, + out_segs_after_send >= out_segs_before_send ? out_segs_after_send - out_segs_before_send : 0U, + video_sender_backlog_segments(&transport_after_send), + transport_after_send.window_pressure_pct, + transport_after_send.srtt_ms + ); + } + 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(active_source->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 (gps_buffer_started) { + gps_buffer_cleanup(); + } + video_sender_close(&sender); + if (encoder != NULL) { + avcodec_free_context(&encoder); + } + if (decoder != NULL) { + avcodec_free_context(&decoder); + } + sws_freeContext(sws_ctx); + video_camera_source_cleanup(&cameras[VIDEO_CAMERA_HEAD]); + video_camera_source_cleanup(&cameras[VIDEO_CAMERA_WAIST]); + return rc; +} diff --git a/host/OmniSocketGo_add_camera/src/video_pipeline_gps.c b/host/OmniSocketGo_add_camera/src/video_pipeline_gps.c new file mode 100644 index 0000000..e60d6ef --- /dev/null +++ b/host/OmniSocketGo_add_camera/src/video_pipeline_gps.c @@ -0,0 +1,925 @@ +#include "video_pipeline.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#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); +} + +static void video_pipeline_report_progress(const video_pipeline_config_t *config) { + if (config == NULL || config->progress_callback == NULL) { + return; + } + config->progress_callback(config->progress_context); +} + +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; + config->stats_logger = NULL; + config->stats_interval_ms = 1000; +} + +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); + config->stats_interval_ms = env_int_or_default("BLITZ_KCP_STATS_INTERVAL_MS", config->stats_interval_ms); +} + +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, + config->stats_logger, + config->stats_interval_ms + ); + if (sender->client == NULL) { + return -1; + } + return 0; +} + +static int video_sender_drain_pending_messages(video_sender_t *sender) { + if (sender == NULL || sender->client == NULL) { + errno = EINVAL; + return -1; + } + + for (;;) { + message_t msg; + int rc; + + protocol_message_init(&msg); + rc = kcp_client_receive_timed(sender->client, &msg, 1); + if (rc == 1) { + protocol_message_clear(&msg); + return 0; + } + if (rc != 0) { + protocol_message_clear(&msg); + return -1; + } + + // Drain unread server errors so an offline receiver cannot back up the reverse KCP stream. + protocol_message_clear(&msg); + } +} + +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, ×tamp, sizeof(timestamp)); + rc = kcp_client_send_binary(sender->client, sender->target_peer, payload, payload_len); + if (rc != 0) { + return rc; + } + rc = video_sender_drain_pending_messages(sender); + 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; + + video_pipeline_report_progress(config); + + 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; +} diff --git a/host/OmniSocketGo_add_camera/third_party/cjson/cJSON.c b/host/OmniSocketGo_add_camera/third_party/cjson/cJSON.c new file mode 100644 index 0000000..702ea61 --- /dev/null +++ b/host/OmniSocketGo_add_camera/third_party/cjson/cJSON.c @@ -0,0 +1,3302 @@ +/* + Copyright (c) 2009-2017 Dave Gamble and cJSON contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +/* cJSON */ +/* JSON parser in C. */ + +/* disable warnings about old C89 functions in MSVC */ +#if !defined(_CRT_SECURE_NO_DEPRECATE) && defined(_MSC_VER) +#define _CRT_SECURE_NO_DEPRECATE +#endif + +#ifdef __GNUC__ +#pragma GCC visibility push(default) +#endif +#if defined(_MSC_VER) +#pragma warning(push) +/* disable warning about single line comments in system headers */ +#pragma warning(disable : 4001) +#endif + +#include +#include +#include +#include +#include +#include +#include + +#ifdef ENABLE_LOCALES +#include +#endif + +#if defined(_MSC_VER) +#pragma warning(pop) +#endif +#ifdef __GNUC__ +#pragma GCC visibility pop +#endif + +#include "cJSON.h" + +/* define our own boolean type */ +#ifdef true +#undef true +#endif +#define true ((cJSON_bool)1) + +#ifdef false +#undef false +#endif +#define false ((cJSON_bool)0) + +/* define isnan and isinf for ANSI C, if in C99 or above, isnan and isinf has been defined in math.h */ +#ifndef isinf +#define isinf(d) (isnan((d - d)) && !isnan(d)) +#endif +#ifndef isnan +#define isnan(d) (d != d) +#endif + +#ifndef NAN +#ifdef _WIN32 +#define NAN sqrt(-1.0) +#else +#define NAN 0.0 / 0.0 +#endif +#endif + +typedef struct +{ + const unsigned char *json; + size_t position; +} error; +static error global_error = {NULL, 0}; + +CJSON_PUBLIC(const char *) +cJSON_GetErrorPtr(void) +{ + return (const char *)(global_error.json + global_error.position); +} + +CJSON_PUBLIC(char *) +cJSON_GetStringValue(const cJSON *const item) +{ + if (!cJSON_IsString(item)) + { + return NULL; + } + + return item->valuestring; +} + +CJSON_PUBLIC(double) +cJSON_GetNumberValue(const cJSON *const item) +{ + if (!cJSON_IsNumber(item)) + { + return (double)NAN; + } + + return item->valuedouble; +} + +/* This is a safeguard to prevent copy-pasters from using incompatible C and header files */ +#if (CJSON_VERSION_MAJOR != 1) || (CJSON_VERSION_MINOR != 7) || (CJSON_VERSION_PATCH != 19) +#error cJSON.h and cJSON.c have different versions. Make sure that both have the same. +#endif + +CJSON_PUBLIC(const char *) +cJSON_Version(void) +{ + static char version[15]; + sprintf(version, "%i.%i.%i", CJSON_VERSION_MAJOR, CJSON_VERSION_MINOR, CJSON_VERSION_PATCH); + + return version; +} + +/* Case insensitive string comparison, doesn't consider two NULL pointers equal though */ +static int case_insensitive_strcmp(const unsigned char *string1, const unsigned char *string2) +{ + if ((string1 == NULL) || (string2 == NULL)) + { + return 1; + } + + if (string1 == string2) + { + return 0; + } + + for (; tolower(*string1) == tolower(*string2); (void)string1++, string2++) + { + if (*string1 == '\0') + { + return 0; + } + } + + return tolower(*string1) - tolower(*string2); +} + +typedef struct internal_hooks +{ + void *(CJSON_CDECL *allocate)(size_t size); + void(CJSON_CDECL *deallocate)(void *pointer); + void *(CJSON_CDECL *reallocate)(void *pointer, size_t size); +} internal_hooks; + +#if defined(_MSC_VER) +/* work around MSVC error C2322: '...' address of dllimport '...' is not static */ +static void *CJSON_CDECL internal_malloc(size_t size) +{ + return malloc(size); +} +static void CJSON_CDECL internal_free(void *pointer) +{ + free(pointer); +} +static void *CJSON_CDECL internal_realloc(void *pointer, size_t size) +{ + return realloc(pointer, size); +} +#else +#define internal_malloc malloc +#define internal_free free +#define internal_realloc realloc +#endif + +/* strlen of character literals resolved at compile time */ +#define static_strlen(string_literal) (sizeof(string_literal) - sizeof("")) + +static internal_hooks global_hooks = {internal_malloc, internal_free, internal_realloc}; + +static unsigned char *cJSON_strdup(const unsigned char *string, const internal_hooks *const hooks) +{ + size_t length = 0; + unsigned char *copy = NULL; + + if (string == NULL) + { + return NULL; + } + + length = strlen((const char *)string) + sizeof(""); + copy = (unsigned char *)hooks->allocate(length); + if (copy == NULL) + { + return NULL; + } + memcpy(copy, string, length); + + return copy; +} + +CJSON_PUBLIC(void) +cJSON_InitHooks(cJSON_Hooks *hooks) +{ + if (hooks == NULL) + { + /* Reset hooks */ + global_hooks.allocate = malloc; + global_hooks.deallocate = free; + global_hooks.reallocate = realloc; + return; + } + + global_hooks.allocate = malloc; + if (hooks->malloc_fn != NULL) + { + global_hooks.allocate = hooks->malloc_fn; + } + + global_hooks.deallocate = free; + if (hooks->free_fn != NULL) + { + global_hooks.deallocate = hooks->free_fn; + } + + /* use realloc only if both free and malloc are used */ + global_hooks.reallocate = NULL; + if ((global_hooks.allocate == malloc) && (global_hooks.deallocate == free)) + { + global_hooks.reallocate = realloc; + } +} + +/* Internal constructor. */ +static cJSON *cJSON_New_Item(const internal_hooks *const hooks) +{ + cJSON *node = (cJSON *)hooks->allocate(sizeof(cJSON)); + if (node) + { + memset(node, '\0', sizeof(cJSON)); + } + + return node; +} + +/* Delete a cJSON structure. */ +CJSON_PUBLIC(void) +cJSON_Delete(cJSON *item) +{ + cJSON *next = NULL; + while (item != NULL) + { + next = item->next; + if (!(item->type & cJSON_IsReference) && (item->child != NULL)) + { + cJSON_Delete(item->child); + } + if (!(item->type & cJSON_IsReference) && (item->valuestring != NULL)) + { + global_hooks.deallocate(item->valuestring); + item->valuestring = NULL; + } + if (!(item->type & cJSON_StringIsConst) && (item->string != NULL)) + { + global_hooks.deallocate(item->string); + item->string = NULL; + } + global_hooks.deallocate(item); + item = next; + } +} + +/* get the decimal point character of the current locale */ +static unsigned char get_decimal_point(void) +{ +#ifdef ENABLE_LOCALES + struct lconv *lconv = localeconv(); + return (unsigned char)lconv->decimal_point[0]; +#else + return '.'; +#endif +} + +typedef struct +{ + const unsigned char *content; + size_t length; + size_t offset; + size_t depth; /* How deeply nested (in arrays/objects) is the input at the current offset. */ + internal_hooks hooks; +} parse_buffer; + +/* check if the given size is left to read in a given parse buffer (starting with 1) */ +#define can_read(buffer, size) ((buffer != NULL) && (((buffer)->offset + size) <= (buffer)->length)) +/* check if the buffer can be accessed at the given index (starting with 0) */ +#define can_access_at_index(buffer, index) ((buffer != NULL) && (((buffer)->offset + index) < (buffer)->length)) +#define cannot_access_at_index(buffer, index) (!can_access_at_index(buffer, index)) +/* get a pointer to the buffer at the position */ +#define buffer_at_offset(buffer) ((buffer)->content + (buffer)->offset) + +/* Parse the input text to generate a number, and populate the result into item. */ +static cJSON_bool parse_number(cJSON *const item, parse_buffer *const input_buffer) +{ + double number = 0; + unsigned char *after_end = NULL; + unsigned char *number_c_string; + unsigned char decimal_point = get_decimal_point(); + size_t i = 0; + size_t number_string_length = 0; + cJSON_bool has_decimal_point = false; + + if ((input_buffer == NULL) || (input_buffer->content == NULL)) + { + return false; + } + + /* copy the number into a temporary buffer and replace '.' with the decimal point + * of the current locale (for strtod) + * This also takes care of '\0' not necessarily being available for marking the end of the input */ + for (i = 0; can_access_at_index(input_buffer, i); i++) + { + switch (buffer_at_offset(input_buffer)[i]) + { + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + case '+': + case '-': + case 'e': + case 'E': + number_string_length++; + break; + + case '.': + number_string_length++; + has_decimal_point = true; + break; + + default: + goto loop_end; + } + } +loop_end: + /* malloc for temporary buffer, add 1 for '\0' */ + number_c_string = (unsigned char *)input_buffer->hooks.allocate(number_string_length + 1); + if (number_c_string == NULL) + { + return false; /* allocation failure */ + } + + memcpy(number_c_string, buffer_at_offset(input_buffer), number_string_length); + number_c_string[number_string_length] = '\0'; + + if (has_decimal_point) + { + for (i = 0; i < number_string_length; i++) + { + if (number_c_string[i] == '.') + { + /* replace '.' with the decimal point of the current locale (for strtod) */ + number_c_string[i] = decimal_point; + } + } + } + + number = strtod((const char *)number_c_string, (char **)&after_end); + if (number_c_string == after_end) + { + /* free the temporary buffer */ + input_buffer->hooks.deallocate(number_c_string); + return false; /* parse_error */ + } + + item->valuedouble = number; + + /* use saturation in case of overflow */ + if (number >= INT_MAX) + { + item->valueint = INT_MAX; + } + else if (number <= (double)INT_MIN) + { + item->valueint = INT_MIN; + } + else + { + item->valueint = (int)number; + } + + item->type = cJSON_Number; + + input_buffer->offset += (size_t)(after_end - number_c_string); + /* free the temporary buffer */ + input_buffer->hooks.deallocate(number_c_string); + return true; +} + +/* don't ask me, but the original cJSON_SetNumberValue returns an integer or double */ +CJSON_PUBLIC(double) +cJSON_SetNumberHelper(cJSON *object, double number) +{ + if (object == NULL) + { + return (double)NAN; + } + + if (number >= INT_MAX) + { + object->valueint = INT_MAX; + } + else if (number <= (double)INT_MIN) + { + object->valueint = INT_MIN; + } + else + { + object->valueint = (int)number; + } + + return object->valuedouble = number; +} + +/* Note: when passing a NULL valuestring, cJSON_SetValuestring treats this as an error and return NULL */ +CJSON_PUBLIC(char *) +cJSON_SetValuestring(cJSON *object, const char *valuestring) +{ + char *copy = NULL; + size_t v1_len; + size_t v2_len; + /* if object's type is not cJSON_String or is cJSON_IsReference, it should not set valuestring */ + if ((object == NULL) || !(object->type & cJSON_String) || (object->type & cJSON_IsReference)) + { + return NULL; + } + /* return NULL if the object is corrupted or valuestring is NULL */ + if (object->valuestring == NULL || valuestring == NULL) + { + return NULL; + } + + v1_len = strlen(valuestring); + v2_len = strlen(object->valuestring); + + if (v1_len <= v2_len) + { + /* strcpy does not handle overlapping string: [X1, X2] [Y1, Y2] => X2 < Y1 or Y2 < X1 */ + if (!(valuestring + v1_len < object->valuestring || object->valuestring + v2_len < valuestring)) + { + return NULL; + } + strcpy(object->valuestring, valuestring); + return object->valuestring; + } + copy = (char *)cJSON_strdup((const unsigned char *)valuestring, &global_hooks); + if (copy == NULL) + { + return NULL; + } + if (object->valuestring != NULL) + { + cJSON_free(object->valuestring); + } + object->valuestring = copy; + + return copy; +} + +typedef struct +{ + unsigned char *buffer; + size_t length; + size_t offset; + size_t depth; /* current nesting depth (for formatted printing) */ + cJSON_bool noalloc; + cJSON_bool format; /* is this print a formatted print */ + internal_hooks hooks; +} printbuffer; + +/* realloc printbuffer if necessary to have at least "needed" bytes more */ +static unsigned char *ensure(printbuffer *const p, size_t needed) +{ + unsigned char *newbuffer = NULL; + size_t newsize = 0; + + if ((p == NULL) || (p->buffer == NULL)) + { + return NULL; + } + + if ((p->length > 0) && (p->offset >= p->length)) + { + /* make sure that offset is valid */ + return NULL; + } + + if (needed > INT_MAX) + { + /* sizes bigger than INT_MAX are currently not supported */ + return NULL; + } + + needed += p->offset + 1; + if (needed <= p->length) + { + return p->buffer + p->offset; + } + + if (p->noalloc) + { + return NULL; + } + + /* calculate new buffer size */ + if (needed > (INT_MAX / 2)) + { + /* overflow of int, use INT_MAX if possible */ + if (needed <= INT_MAX) + { + newsize = INT_MAX; + } + else + { + return NULL; + } + } + else + { + newsize = needed * 2; + } + + if (p->hooks.reallocate != NULL) + { + /* reallocate with realloc if available */ + newbuffer = (unsigned char *)p->hooks.reallocate(p->buffer, newsize); + if (newbuffer == NULL) + { + p->hooks.deallocate(p->buffer); + p->length = 0; + p->buffer = NULL; + + return NULL; + } + } + else + { + /* otherwise reallocate manually */ + newbuffer = (unsigned char *)p->hooks.allocate(newsize); + if (!newbuffer) + { + p->hooks.deallocate(p->buffer); + p->length = 0; + p->buffer = NULL; + + return NULL; + } + + memcpy(newbuffer, p->buffer, p->offset + 1); + p->hooks.deallocate(p->buffer); + } + p->length = newsize; + p->buffer = newbuffer; + + return newbuffer + p->offset; +} + +/* calculate the new length of the string in a printbuffer and update the offset */ +static void update_offset(printbuffer *const buffer) +{ + const unsigned char *buffer_pointer = NULL; + if ((buffer == NULL) || (buffer->buffer == NULL)) + { + return; + } + buffer_pointer = buffer->buffer + buffer->offset; + + buffer->offset += strlen((const char *)buffer_pointer); +} + +/* securely comparison of floating-point variables */ +static cJSON_bool compare_double(double a, double b) +{ + double maxVal = fabs(a) > fabs(b) ? fabs(a) : fabs(b); + return (fabs(a - b) <= maxVal * DBL_EPSILON); +} + +/* Render the number nicely from the given item into a string. */ +static cJSON_bool print_number(const cJSON *const item, printbuffer *const output_buffer) +{ + unsigned char *output_pointer = NULL; + double d = item->valuedouble; + int length = 0; + size_t i = 0; + unsigned char number_buffer[26] = {0}; /* temporary buffer to print the number into */ + unsigned char decimal_point = get_decimal_point(); + double test = 0.0; + + if (output_buffer == NULL) + { + return false; + } + + /* This checks for NaN and Infinity */ + if (isnan(d) || isinf(d)) + { + length = sprintf((char *)number_buffer, "null"); + } + else if (d == (double)item->valueint) + { + length = sprintf((char *)number_buffer, "%d", item->valueint); + } + else + { + /* Try 15 decimal places of precision to avoid nonsignificant nonzero digits */ + length = sprintf((char *)number_buffer, "%1.15g", d); + + /* Check whether the original double can be recovered */ + if ((sscanf((char *)number_buffer, "%lg", &test) != 1) || !compare_double((double)test, d)) + { + /* If not, print with 17 decimal places of precision */ + length = sprintf((char *)number_buffer, "%1.17g", d); + } + } + + /* sprintf failed or buffer overrun occurred */ + if ((length < 0) || (length > (int)(sizeof(number_buffer) - 1))) + { + return false; + } + + /* reserve appropriate space in the output */ + output_pointer = ensure(output_buffer, (size_t)length + sizeof("")); + if (output_pointer == NULL) + { + return false; + } + + /* copy the printed number to the output and replace locale + * dependent decimal point with '.' */ + for (i = 0; i < ((size_t)length); i++) + { + if (number_buffer[i] == decimal_point) + { + output_pointer[i] = '.'; + continue; + } + + output_pointer[i] = number_buffer[i]; + } + output_pointer[i] = '\0'; + + output_buffer->offset += (size_t)length; + + return true; +} + +/* parse 4 digit hexadecimal number */ +static unsigned parse_hex4(const unsigned char *const input) +{ + unsigned int h = 0; + size_t i = 0; + + for (i = 0; i < 4; i++) + { + /* parse digit */ + if ((input[i] >= '0') && (input[i] <= '9')) + { + h += (unsigned int)input[i] - '0'; + } + else if ((input[i] >= 'A') && (input[i] <= 'F')) + { + h += (unsigned int)10 + input[i] - 'A'; + } + else if ((input[i] >= 'a') && (input[i] <= 'f')) + { + h += (unsigned int)10 + input[i] - 'a'; + } + else /* invalid */ + { + return 0; + } + + if (i < 3) + { + /* shift left to make place for the next nibble */ + h = h << 4; + } + } + + return h; +} + +/* converts a UTF-16 literal to UTF-8 + * A literal can be one or two sequences of the form \uXXXX */ +static unsigned char utf16_literal_to_utf8(const unsigned char *const input_pointer, const unsigned char *const input_end, unsigned char **output_pointer) +{ + long unsigned int codepoint = 0; + unsigned int first_code = 0; + const unsigned char *first_sequence = input_pointer; + unsigned char utf8_length = 0; + unsigned char utf8_position = 0; + unsigned char sequence_length = 0; + unsigned char first_byte_mark = 0; + + if ((input_end - first_sequence) < 6) + { + /* input ends unexpectedly */ + goto fail; + } + + /* get the first utf16 sequence */ + first_code = parse_hex4(first_sequence + 2); + + /* check that the code is valid */ + if (((first_code >= 0xDC00) && (first_code <= 0xDFFF))) + { + goto fail; + } + + /* UTF16 surrogate pair */ + if ((first_code >= 0xD800) && (first_code <= 0xDBFF)) + { + const unsigned char *second_sequence = first_sequence + 6; + unsigned int second_code = 0; + sequence_length = 12; /* \uXXXX\uXXXX */ + + if ((input_end - second_sequence) < 6) + { + /* input ends unexpectedly */ + goto fail; + } + + if ((second_sequence[0] != '\\') || (second_sequence[1] != 'u')) + { + /* missing second half of the surrogate pair */ + goto fail; + } + + /* get the second utf16 sequence */ + second_code = parse_hex4(second_sequence + 2); + /* check that the code is valid */ + if ((second_code < 0xDC00) || (second_code > 0xDFFF)) + { + /* invalid second half of the surrogate pair */ + goto fail; + } + + /* calculate the unicode codepoint from the surrogate pair */ + codepoint = 0x10000 + (((first_code & 0x3FF) << 10) | (second_code & 0x3FF)); + } + else + { + sequence_length = 6; /* \uXXXX */ + codepoint = first_code; + } + + /* encode as UTF-8 + * takes at maximum 4 bytes to encode: + * 11110xxx 10xxxxxx 10xxxxxx 10xxxxxx */ + if (codepoint < 0x80) + { + /* normal ascii, encoding 0xxxxxxx */ + utf8_length = 1; + } + else if (codepoint < 0x800) + { + /* two bytes, encoding 110xxxxx 10xxxxxx */ + utf8_length = 2; + first_byte_mark = 0xC0; /* 11000000 */ + } + else if (codepoint < 0x10000) + { + /* three bytes, encoding 1110xxxx 10xxxxxx 10xxxxxx */ + utf8_length = 3; + first_byte_mark = 0xE0; /* 11100000 */ + } + else if (codepoint <= 0x10FFFF) + { + /* four bytes, encoding 1110xxxx 10xxxxxx 10xxxxxx 10xxxxxx */ + utf8_length = 4; + first_byte_mark = 0xF0; /* 11110000 */ + } + else + { + /* invalid unicode codepoint */ + goto fail; + } + + /* encode as utf8 */ + for (utf8_position = (unsigned char)(utf8_length - 1); utf8_position > 0; utf8_position--) + { + /* 10xxxxxx */ + (*output_pointer)[utf8_position] = (unsigned char)((codepoint | 0x80) & 0xBF); + codepoint >>= 6; + } + /* encode first byte */ + if (utf8_length > 1) + { + (*output_pointer)[0] = (unsigned char)((codepoint | first_byte_mark) & 0xFF); + } + else + { + (*output_pointer)[0] = (unsigned char)(codepoint & 0x7F); + } + + *output_pointer += utf8_length; + + return sequence_length; + +fail: + return 0; +} + +/* Parse the input text into an unescaped cinput, and populate item. */ +static cJSON_bool parse_string(cJSON *const item, parse_buffer *const input_buffer) +{ + const unsigned char *input_pointer = buffer_at_offset(input_buffer) + 1; + const unsigned char *input_end = buffer_at_offset(input_buffer) + 1; + unsigned char *output_pointer = NULL; + unsigned char *output = NULL; + + /* not a string */ + if (buffer_at_offset(input_buffer)[0] != '\"') + { + goto fail; + } + + { + /* calculate approximate size of the output (overestimate) */ + size_t allocation_length = 0; + size_t skipped_bytes = 0; + while (((size_t)(input_end - input_buffer->content) < input_buffer->length) && (*input_end != '\"')) + { + /* is escape sequence */ + if (input_end[0] == '\\') + { + if ((size_t)(input_end + 1 - input_buffer->content) >= input_buffer->length) + { + /* prevent buffer overflow when last input character is a backslash */ + goto fail; + } + skipped_bytes++; + input_end++; + } + input_end++; + } + if (((size_t)(input_end - input_buffer->content) >= input_buffer->length) || (*input_end != '\"')) + { + goto fail; /* string ended unexpectedly */ + } + + /* This is at most how much we need for the output */ + allocation_length = (size_t)(input_end - buffer_at_offset(input_buffer)) - skipped_bytes; + output = (unsigned char *)input_buffer->hooks.allocate(allocation_length + sizeof("")); + if (output == NULL) + { + goto fail; /* allocation failure */ + } + } + + output_pointer = output; + /* loop through the string literal */ + while (input_pointer < input_end) + { + if (*input_pointer != '\\') + { + *output_pointer++ = *input_pointer++; + } + /* escape sequence */ + else + { + unsigned char sequence_length = 2; + if ((input_end - input_pointer) < 1) + { + goto fail; + } + + switch (input_pointer[1]) + { + case 'b': + *output_pointer++ = '\b'; + break; + case 'f': + *output_pointer++ = '\f'; + break; + case 'n': + *output_pointer++ = '\n'; + break; + case 'r': + *output_pointer++ = '\r'; + break; + case 't': + *output_pointer++ = '\t'; + break; + case '\"': + case '\\': + case '/': + *output_pointer++ = input_pointer[1]; + break; + + /* UTF-16 literal */ + case 'u': + sequence_length = utf16_literal_to_utf8(input_pointer, input_end, &output_pointer); + if (sequence_length == 0) + { + /* failed to convert UTF16-literal to UTF-8 */ + goto fail; + } + break; + + default: + goto fail; + } + input_pointer += sequence_length; + } + } + + /* zero terminate the output */ + *output_pointer = '\0'; + + item->type = cJSON_String; + item->valuestring = (char *)output; + + input_buffer->offset = (size_t)(input_end - input_buffer->content); + input_buffer->offset++; + + return true; + +fail: + if (output != NULL) + { + input_buffer->hooks.deallocate(output); + output = NULL; + } + + if (input_pointer != NULL) + { + input_buffer->offset = (size_t)(input_pointer - input_buffer->content); + } + + return false; +} + +/* Render the cstring provided to an escaped version that can be printed. */ +static cJSON_bool print_string_ptr(const unsigned char *const input, printbuffer *const output_buffer) +{ + const unsigned char *input_pointer = NULL; + unsigned char *output = NULL; + unsigned char *output_pointer = NULL; + size_t output_length = 0; + /* numbers of additional characters needed for escaping */ + size_t escape_characters = 0; + + if (output_buffer == NULL) + { + return false; + } + + /* empty string */ + if (input == NULL) + { + output = ensure(output_buffer, sizeof("\"\"")); + if (output == NULL) + { + return false; + } + strcpy((char *)output, "\"\""); + + return true; + } + + /* set "flag" to 1 if something needs to be escaped */ + for (input_pointer = input; *input_pointer; input_pointer++) + { + switch (*input_pointer) + { + case '\"': + case '\\': + case '\b': + case '\f': + case '\n': + case '\r': + case '\t': + /* one character escape sequence */ + escape_characters++; + break; + default: + if (*input_pointer < 32) + { + /* UTF-16 escape sequence uXXXX */ + escape_characters += 5; + } + break; + } + } + output_length = (size_t)(input_pointer - input) + escape_characters; + + output = ensure(output_buffer, output_length + sizeof("\"\"")); + if (output == NULL) + { + return false; + } + + /* no characters have to be escaped */ + if (escape_characters == 0) + { + output[0] = '\"'; + memcpy(output + 1, input, output_length); + output[output_length + 1] = '\"'; + output[output_length + 2] = '\0'; + + return true; + } + + output[0] = '\"'; + output_pointer = output + 1; + /* copy the string */ + for (input_pointer = input; *input_pointer != '\0'; (void)input_pointer++, output_pointer++) + { + if ((*input_pointer > 31) && (*input_pointer != '\"') && (*input_pointer != '\\')) + { + /* normal character, copy */ + *output_pointer = *input_pointer; + } + else + { + /* character needs to be escaped */ + *output_pointer++ = '\\'; + switch (*input_pointer) + { + case '\\': + *output_pointer = '\\'; + break; + case '\"': + *output_pointer = '\"'; + break; + case '\b': + *output_pointer = 'b'; + break; + case '\f': + *output_pointer = 'f'; + break; + case '\n': + *output_pointer = 'n'; + break; + case '\r': + *output_pointer = 'r'; + break; + case '\t': + *output_pointer = 't'; + break; + default: + /* escape and print as unicode codepoint */ + sprintf((char *)output_pointer, "u%04x", *input_pointer); + output_pointer += 4; + break; + } + } + } + output[output_length + 1] = '\"'; + output[output_length + 2] = '\0'; + + return true; +} + +/* Invoke print_string_ptr (which is useful) on an item. */ +static cJSON_bool print_string(const cJSON *const item, printbuffer *const p) +{ + return print_string_ptr((unsigned char *)item->valuestring, p); +} + +/* Predeclare these prototypes. */ +static cJSON_bool parse_value(cJSON *const item, parse_buffer *const input_buffer); +static cJSON_bool print_value(const cJSON *const item, printbuffer *const output_buffer); +static cJSON_bool parse_array(cJSON *const item, parse_buffer *const input_buffer); +static cJSON_bool print_array(const cJSON *const item, printbuffer *const output_buffer); +static cJSON_bool parse_object(cJSON *const item, parse_buffer *const input_buffer); +static cJSON_bool print_object(const cJSON *const item, printbuffer *const output_buffer); + +/* Utility to jump whitespace and cr/lf */ +static parse_buffer *buffer_skip_whitespace(parse_buffer *const buffer) +{ + if ((buffer == NULL) || (buffer->content == NULL)) + { + return NULL; + } + + if (cannot_access_at_index(buffer, 0)) + { + return buffer; + } + + while (can_access_at_index(buffer, 0) && (buffer_at_offset(buffer)[0] <= 32)) + { + buffer->offset++; + } + + if (buffer->offset == buffer->length) + { + buffer->offset--; + } + + return buffer; +} + +/* skip the UTF-8 BOM (byte order mark) if it is at the beginning of a buffer */ +static parse_buffer *skip_utf8_bom(parse_buffer *const buffer) +{ + if ((buffer == NULL) || (buffer->content == NULL) || (buffer->offset != 0)) + { + return NULL; + } + + if (can_access_at_index(buffer, 4) && (strncmp((const char *)buffer_at_offset(buffer), "\xEF\xBB\xBF", 3) == 0)) + { + buffer->offset += 3; + } + + return buffer; +} + +CJSON_PUBLIC(cJSON *) +cJSON_ParseWithOpts(const char *value, const char **return_parse_end, cJSON_bool require_null_terminated) +{ + size_t buffer_length; + + if (NULL == value) + { + return NULL; + } + + /* Adding null character size due to require_null_terminated. */ + buffer_length = strlen(value) + sizeof(""); + + return cJSON_ParseWithLengthOpts(value, buffer_length, return_parse_end, require_null_terminated); +} + +/* Parse an object - create a new root, and populate. */ +CJSON_PUBLIC(cJSON *) +cJSON_ParseWithLengthOpts(const char *value, size_t buffer_length, const char **return_parse_end, cJSON_bool require_null_terminated) +{ + parse_buffer buffer = {0, 0, 0, 0, {0, 0, 0}}; + cJSON *item = NULL; + + /* reset error position */ + global_error.json = NULL; + global_error.position = 0; + + if (value == NULL || 0 == buffer_length) + { + goto fail; + } + + buffer.content = (const unsigned char *)value; + buffer.length = buffer_length; + buffer.offset = 0; + buffer.hooks = global_hooks; + + item = cJSON_New_Item(&global_hooks); + if (item == NULL) /* memory fail */ + { + goto fail; + } + + if (!parse_value(item, buffer_skip_whitespace(skip_utf8_bom(&buffer)))) + { + /* parse failure. ep is set. */ + goto fail; + } + + /* if we require null-terminated JSON without appended garbage, skip and then check for a null terminator */ + if (require_null_terminated) + { + buffer_skip_whitespace(&buffer); + if ((buffer.offset >= buffer.length) || buffer_at_offset(&buffer)[0] != '\0') + { + goto fail; + } + } + if (return_parse_end) + { + *return_parse_end = (const char *)buffer_at_offset(&buffer); + } + + return item; + +fail: + if (item != NULL) + { + cJSON_Delete(item); + } + + if (value != NULL) + { + error local_error; + local_error.json = (const unsigned char *)value; + local_error.position = 0; + + if (buffer.offset < buffer.length) + { + local_error.position = buffer.offset; + } + else if (buffer.length > 0) + { + local_error.position = buffer.length - 1; + } + + if (return_parse_end != NULL) + { + *return_parse_end = (const char *)local_error.json + local_error.position; + } + + global_error = local_error; + } + + return NULL; +} + +/* Default options for cJSON_Parse */ +CJSON_PUBLIC(cJSON *) +cJSON_Parse(const char *value) +{ + return cJSON_ParseWithOpts(value, 0, 0); +} + +CJSON_PUBLIC(cJSON *) +cJSON_ParseWithLength(const char *value, size_t buffer_length) +{ + return cJSON_ParseWithLengthOpts(value, buffer_length, 0, 0); +} + +#define cjson_min(a, b) (((a) < (b)) ? (a) : (b)) + +static unsigned char *print(const cJSON *const item, cJSON_bool format, const internal_hooks *const hooks) +{ + static const size_t default_buffer_size = 256; + printbuffer buffer[1]; + unsigned char *printed = NULL; + + memset(buffer, 0, sizeof(buffer)); + + /* create buffer */ + buffer->buffer = (unsigned char *)hooks->allocate(default_buffer_size); + buffer->length = default_buffer_size; + buffer->format = format; + buffer->hooks = *hooks; + if (buffer->buffer == NULL) + { + goto fail; + } + + /* print the value */ + if (!print_value(item, buffer)) + { + goto fail; + } + update_offset(buffer); + + /* check if reallocate is available */ + if (hooks->reallocate != NULL) + { + printed = (unsigned char *)hooks->reallocate(buffer->buffer, buffer->offset + 1); + if (printed == NULL) + { + goto fail; + } + buffer->buffer = NULL; + } + else /* otherwise copy the JSON over to a new buffer */ + { + printed = (unsigned char *)hooks->allocate(buffer->offset + 1); + if (printed == NULL) + { + goto fail; + } + memcpy(printed, buffer->buffer, cjson_min(buffer->length, buffer->offset + 1)); + printed[buffer->offset] = '\0'; /* just to be sure */ + + /* free the buffer */ + hooks->deallocate(buffer->buffer); + buffer->buffer = NULL; + } + + return printed; + +fail: + if (buffer->buffer != NULL) + { + hooks->deallocate(buffer->buffer); + buffer->buffer = NULL; + } + + if (printed != NULL) + { + hooks->deallocate(printed); + printed = NULL; + } + + return NULL; +} + +/* Render a cJSON item/entity/structure to text. */ +CJSON_PUBLIC(char *) +cJSON_Print(const cJSON *item) +{ + return (char *)print(item, true, &global_hooks); +} + +CJSON_PUBLIC(char *) +cJSON_PrintUnformatted(const cJSON *item) +{ + return (char *)print(item, false, &global_hooks); +} + +CJSON_PUBLIC(char *) +cJSON_PrintBuffered(const cJSON *item, int prebuffer, cJSON_bool fmt) +{ + printbuffer p = {0, 0, 0, 0, 0, 0, {0, 0, 0}}; + + if (prebuffer < 0) + { + return NULL; + } + + p.buffer = (unsigned char *)global_hooks.allocate((size_t)prebuffer); + if (!p.buffer) + { + return NULL; + } + + p.length = (size_t)prebuffer; + p.offset = 0; + p.noalloc = false; + p.format = fmt; + p.hooks = global_hooks; + + if (!print_value(item, &p)) + { + global_hooks.deallocate(p.buffer); + p.buffer = NULL; + return NULL; + } + + return (char *)p.buffer; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_PrintPreallocated(cJSON *item, char *buffer, const int length, const cJSON_bool format) +{ + printbuffer p = {0, 0, 0, 0, 0, 0, {0, 0, 0}}; + + if ((length < 0) || (buffer == NULL)) + { + return false; + } + + p.buffer = (unsigned char *)buffer; + p.length = (size_t)length; + p.offset = 0; + p.noalloc = true; + p.format = format; + p.hooks = global_hooks; + + return print_value(item, &p); +} + +/* Parser core - when encountering text, process appropriately. */ +static cJSON_bool parse_value(cJSON *const item, parse_buffer *const input_buffer) +{ + if ((input_buffer == NULL) || (input_buffer->content == NULL)) + { + return false; /* no input */ + } + + /* parse the different types of values */ + /* null */ + if (can_read(input_buffer, 4) && (strncmp((const char *)buffer_at_offset(input_buffer), "null", 4) == 0)) + { + item->type = cJSON_NULL; + input_buffer->offset += 4; + return true; + } + /* false */ + if (can_read(input_buffer, 5) && (strncmp((const char *)buffer_at_offset(input_buffer), "false", 5) == 0)) + { + item->type = cJSON_False; + input_buffer->offset += 5; + return true; + } + /* true */ + if (can_read(input_buffer, 4) && (strncmp((const char *)buffer_at_offset(input_buffer), "true", 4) == 0)) + { + item->type = cJSON_True; + item->valueint = 1; + input_buffer->offset += 4; + return true; + } + /* string */ + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '\"')) + { + return parse_string(item, input_buffer); + } + /* number */ + if (can_access_at_index(input_buffer, 0) && ((buffer_at_offset(input_buffer)[0] == '-') || ((buffer_at_offset(input_buffer)[0] >= '0') && (buffer_at_offset(input_buffer)[0] <= '9')))) + { + return parse_number(item, input_buffer); + } + /* array */ + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '[')) + { + return parse_array(item, input_buffer); + } + /* object */ + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '{')) + { + return parse_object(item, input_buffer); + } + + return false; +} + +/* Render a value to text. */ +static cJSON_bool print_value(const cJSON *const item, printbuffer *const output_buffer) +{ + unsigned char *output = NULL; + + if ((item == NULL) || (output_buffer == NULL)) + { + return false; + } + + switch ((item->type) & 0xFF) + { + case cJSON_NULL: + output = ensure(output_buffer, 5); + if (output == NULL) + { + return false; + } + strcpy((char *)output, "null"); + return true; + + case cJSON_False: + output = ensure(output_buffer, 6); + if (output == NULL) + { + return false; + } + strcpy((char *)output, "false"); + return true; + + case cJSON_True: + output = ensure(output_buffer, 5); + if (output == NULL) + { + return false; + } + strcpy((char *)output, "true"); + return true; + + case cJSON_Number: + return print_number(item, output_buffer); + + case cJSON_Raw: + { + size_t raw_length = 0; + if (item->valuestring == NULL) + { + return false; + } + + raw_length = strlen(item->valuestring) + sizeof(""); + output = ensure(output_buffer, raw_length); + if (output == NULL) + { + return false; + } + memcpy(output, item->valuestring, raw_length); + return true; + } + + case cJSON_String: + return print_string(item, output_buffer); + + case cJSON_Array: + return print_array(item, output_buffer); + + case cJSON_Object: + return print_object(item, output_buffer); + + default: + return false; + } +} + +/* Build an array from input text. */ +static cJSON_bool parse_array(cJSON *const item, parse_buffer *const input_buffer) +{ + cJSON *head = NULL; /* head of the linked list */ + cJSON *current_item = NULL; + + if (input_buffer->depth >= CJSON_NESTING_LIMIT) + { + return false; /* to deeply nested */ + } + input_buffer->depth++; + + if (buffer_at_offset(input_buffer)[0] != '[') + { + /* not an array */ + goto fail; + } + + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == ']')) + { + /* empty array */ + goto success; + } + + /* check if we skipped to the end of the buffer */ + if (cannot_access_at_index(input_buffer, 0)) + { + input_buffer->offset--; + goto fail; + } + + /* step back to character in front of the first element */ + input_buffer->offset--; + /* loop through the comma separated array elements */ + do + { + /* allocate next item */ + cJSON *new_item = cJSON_New_Item(&(input_buffer->hooks)); + if (new_item == NULL) + { + goto fail; /* allocation failure */ + } + + /* attach next item to list */ + if (head == NULL) + { + /* start the linked list */ + current_item = head = new_item; + } + else + { + /* add to the end and advance */ + current_item->next = new_item; + new_item->prev = current_item; + current_item = new_item; + } + + /* parse next value */ + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (!parse_value(current_item, input_buffer)) + { + goto fail; /* failed to parse value */ + } + buffer_skip_whitespace(input_buffer); + } while (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == ',')); + + if (cannot_access_at_index(input_buffer, 0) || buffer_at_offset(input_buffer)[0] != ']') + { + goto fail; /* expected end of array */ + } + +success: + input_buffer->depth--; + + if (head != NULL) + { + head->prev = current_item; + } + + item->type = cJSON_Array; + item->child = head; + + input_buffer->offset++; + + return true; + +fail: + if (head != NULL) + { + cJSON_Delete(head); + } + + return false; +} + +/* Render an array to text */ +static cJSON_bool print_array(const cJSON *const item, printbuffer *const output_buffer) +{ + unsigned char *output_pointer = NULL; + size_t length = 0; + cJSON *current_element = item->child; + + if (output_buffer == NULL) + { + return false; + } + + if (output_buffer->depth >= CJSON_NESTING_LIMIT) + { + return false; /* nesting is too deep */ + } + + /* Compose the output array. */ + /* opening square bracket */ + output_pointer = ensure(output_buffer, 1); + if (output_pointer == NULL) + { + return false; + } + + *output_pointer = '['; + output_buffer->offset++; + output_buffer->depth++; + + while (current_element != NULL) + { + if (!print_value(current_element, output_buffer)) + { + return false; + } + update_offset(output_buffer); + if (current_element->next) + { + length = (size_t)(output_buffer->format ? 2 : 1); + output_pointer = ensure(output_buffer, length + 1); + if (output_pointer == NULL) + { + return false; + } + *output_pointer++ = ','; + if (output_buffer->format) + { + *output_pointer++ = ' '; + } + *output_pointer = '\0'; + output_buffer->offset += length; + } + current_element = current_element->next; + } + + output_pointer = ensure(output_buffer, 2); + if (output_pointer == NULL) + { + return false; + } + *output_pointer++ = ']'; + *output_pointer = '\0'; + output_buffer->depth--; + + return true; +} + +/* Build an object from the text. */ +static cJSON_bool parse_object(cJSON *const item, parse_buffer *const input_buffer) +{ + cJSON *head = NULL; /* linked list head */ + cJSON *current_item = NULL; + + if (input_buffer->depth >= CJSON_NESTING_LIMIT) + { + return false; /* to deeply nested */ + } + input_buffer->depth++; + + if (cannot_access_at_index(input_buffer, 0) || (buffer_at_offset(input_buffer)[0] != '{')) + { + goto fail; /* not an object */ + } + + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '}')) + { + goto success; /* empty object */ + } + + /* check if we skipped to the end of the buffer */ + if (cannot_access_at_index(input_buffer, 0)) + { + input_buffer->offset--; + goto fail; + } + + /* step back to character in front of the first element */ + input_buffer->offset--; + /* loop through the comma separated array elements */ + do + { + /* allocate next item */ + cJSON *new_item = cJSON_New_Item(&(input_buffer->hooks)); + if (new_item == NULL) + { + goto fail; /* allocation failure */ + } + + /* attach next item to list */ + if (head == NULL) + { + /* start the linked list */ + current_item = head = new_item; + } + else + { + /* add to the end and advance */ + current_item->next = new_item; + new_item->prev = current_item; + current_item = new_item; + } + + if (cannot_access_at_index(input_buffer, 1)) + { + goto fail; /* nothing comes after the comma */ + } + + /* parse the name of the child */ + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (!parse_string(current_item, input_buffer)) + { + goto fail; /* failed to parse name */ + } + buffer_skip_whitespace(input_buffer); + + /* swap valuestring and string, because we parsed the name */ + current_item->string = current_item->valuestring; + current_item->valuestring = NULL; + + if (cannot_access_at_index(input_buffer, 0) || (buffer_at_offset(input_buffer)[0] != ':')) + { + goto fail; /* invalid object */ + } + + /* parse the value */ + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (!parse_value(current_item, input_buffer)) + { + goto fail; /* failed to parse value */ + } + buffer_skip_whitespace(input_buffer); + } while (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == ',')); + + if (cannot_access_at_index(input_buffer, 0) || (buffer_at_offset(input_buffer)[0] != '}')) + { + goto fail; /* expected end of object */ + } + +success: + input_buffer->depth--; + + if (head != NULL) + { + head->prev = current_item; + } + + item->type = cJSON_Object; + item->child = head; + + input_buffer->offset++; + return true; + +fail: + if (head != NULL) + { + cJSON_Delete(head); + } + + return false; +} + +/* Render an object to text. */ +static cJSON_bool print_object(const cJSON *const item, printbuffer *const output_buffer) +{ + unsigned char *output_pointer = NULL; + size_t length = 0; + cJSON *current_item = item->child; + + if (output_buffer == NULL) + { + return false; + } + + if (output_buffer->depth >= CJSON_NESTING_LIMIT) + { + return false; /* nesting is too deep */ + } + + /* Compose the output: */ + length = (size_t)(output_buffer->format ? 2 : 1); /* fmt: {\n */ + output_pointer = ensure(output_buffer, length + 1); + if (output_pointer == NULL) + { + return false; + } + + *output_pointer++ = '{'; + output_buffer->depth++; + if (output_buffer->format) + { + *output_pointer++ = '\n'; + } + output_buffer->offset += length; + + while (current_item) + { + if (output_buffer->format) + { + size_t i; + output_pointer = ensure(output_buffer, output_buffer->depth); + if (output_pointer == NULL) + { + return false; + } + for (i = 0; i < output_buffer->depth; i++) + { + *output_pointer++ = '\t'; + } + output_buffer->offset += output_buffer->depth; + } + + /* print key */ + if (!print_string_ptr((unsigned char *)current_item->string, output_buffer)) + { + return false; + } + update_offset(output_buffer); + + length = (size_t)(output_buffer->format ? 2 : 1); + output_pointer = ensure(output_buffer, length); + if (output_pointer == NULL) + { + return false; + } + *output_pointer++ = ':'; + if (output_buffer->format) + { + *output_pointer++ = '\t'; + } + output_buffer->offset += length; + + /* print value */ + if (!print_value(current_item, output_buffer)) + { + return false; + } + update_offset(output_buffer); + + /* print comma if not last */ + length = ((size_t)(output_buffer->format ? 1 : 0) + (size_t)(current_item->next ? 1 : 0)); + output_pointer = ensure(output_buffer, length + 1); + if (output_pointer == NULL) + { + return false; + } + if (current_item->next) + { + *output_pointer++ = ','; + } + + if (output_buffer->format) + { + *output_pointer++ = '\n'; + } + *output_pointer = '\0'; + output_buffer->offset += length; + + current_item = current_item->next; + } + + output_pointer = ensure(output_buffer, output_buffer->format ? (output_buffer->depth + 1) : 2); + if (output_pointer == NULL) + { + return false; + } + if (output_buffer->format) + { + size_t i; + for (i = 0; i < (output_buffer->depth - 1); i++) + { + *output_pointer++ = '\t'; + } + } + *output_pointer++ = '}'; + *output_pointer = '\0'; + output_buffer->depth--; + + return true; +} + +/* Get Array size/item / object item. */ +CJSON_PUBLIC(int) +cJSON_GetArraySize(const cJSON *array) +{ + cJSON *child = NULL; + size_t size = 0; + + if (array == NULL) + { + return 0; + } + + child = array->child; + + while (child != NULL) + { + size++; + child = child->next; + } + + /* FIXME: Can overflow here. Cannot be fixed without breaking the API */ + + return (int)size; +} + +static cJSON *get_array_item(const cJSON *array, size_t index) +{ + cJSON *current_child = NULL; + + if (array == NULL) + { + return NULL; + } + + current_child = array->child; + while ((current_child != NULL) && (index > 0)) + { + index--; + current_child = current_child->next; + } + + return current_child; +} + +CJSON_PUBLIC(cJSON *) +cJSON_GetArrayItem(const cJSON *array, int index) +{ + if (index < 0) + { + return NULL; + } + + return get_array_item(array, (size_t)index); +} + +static cJSON *get_object_item(const cJSON *const object, const char *const name, const cJSON_bool case_sensitive) +{ + cJSON *current_element = NULL; + + if ((object == NULL) || (name == NULL)) + { + return NULL; + } + + current_element = object->child; + if (case_sensitive) + { + while ((current_element != NULL) && (current_element->string != NULL) && (strcmp(name, current_element->string) != 0)) + { + current_element = current_element->next; + } + } + else + { + while ((current_element != NULL) && (case_insensitive_strcmp((const unsigned char *)name, (const unsigned char *)(current_element->string)) != 0)) + { + current_element = current_element->next; + } + } + + if ((current_element == NULL) || (current_element->string == NULL)) + { + return NULL; + } + + return current_element; +} + +CJSON_PUBLIC(cJSON *) +cJSON_GetObjectItem(const cJSON *const object, const char *const string) +{ + return get_object_item(object, string, false); +} + +CJSON_PUBLIC(cJSON *) +cJSON_GetObjectItemCaseSensitive(const cJSON *const object, const char *const string) +{ + return get_object_item(object, string, true); +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_HasObjectItem(const cJSON *object, const char *string) +{ + return cJSON_GetObjectItem(object, string) ? 1 : 0; +} + +/* Utility for array list handling. */ +static void suffix_object(cJSON *prev, cJSON *item) +{ + prev->next = item; + item->prev = prev; +} + +/* Utility for handling references. */ +static cJSON *create_reference(const cJSON *item, const internal_hooks *const hooks) +{ + cJSON *reference = NULL; + if (item == NULL) + { + return NULL; + } + + reference = cJSON_New_Item(hooks); + if (reference == NULL) + { + return NULL; + } + + memcpy(reference, item, sizeof(cJSON)); + reference->string = NULL; + reference->type |= cJSON_IsReference; + reference->next = reference->prev = NULL; + return reference; +} + +static cJSON_bool add_item_to_array(cJSON *array, cJSON *item) +{ + cJSON *child = NULL; + + if ((item == NULL) || (array == NULL) || (array == item)) + { + return false; + } + + child = array->child; + /* + * To find the last item in array quickly, we use prev in array + */ + if (child == NULL) + { + /* list is empty, start new one */ + array->child = item; + item->prev = item; + item->next = NULL; + } + else + { + /* append to the end */ + if (child->prev) + { + suffix_object(child->prev, item); + array->child->prev = item; + } + } + + return true; +} + +/* Add item to array/object. */ +CJSON_PUBLIC(cJSON_bool) +cJSON_AddItemToArray(cJSON *array, cJSON *item) +{ + return add_item_to_array(array, item); +} + +#if defined(__clang__) || (defined(__GNUC__) && ((__GNUC__ > 4) || ((__GNUC__ == 4) && (__GNUC_MINOR__ > 5)))) +#pragma GCC diagnostic push +#endif +#ifdef __GNUC__ +#pragma GCC diagnostic ignored "-Wcast-qual" +#endif +/* helper function to cast away const */ +static void *cast_away_const(const void *string) +{ + return (void *)string; +} +#if defined(__clang__) || (defined(__GNUC__) && ((__GNUC__ > 4) || ((__GNUC__ == 4) && (__GNUC_MINOR__ > 5)))) +#pragma GCC diagnostic pop +#endif + +static cJSON_bool add_item_to_object(cJSON *const object, const char *const string, cJSON *const item, const internal_hooks *const hooks, const cJSON_bool constant_key) +{ + char *new_key = NULL; + int new_type = cJSON_Invalid; + + if ((object == NULL) || (string == NULL) || (item == NULL) || (object == item)) + { + return false; + } + + if (constant_key) + { + new_key = (char *)cast_away_const(string); + new_type = item->type | cJSON_StringIsConst; + } + else + { + new_key = (char *)cJSON_strdup((const unsigned char *)string, hooks); + if (new_key == NULL) + { + return false; + } + + new_type = item->type & ~cJSON_StringIsConst; + } + + if (!(item->type & cJSON_StringIsConst) && (item->string != NULL)) + { + hooks->deallocate(item->string); + } + + item->string = new_key; + item->type = new_type; + + return add_item_to_array(object, item); +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_AddItemToObject(cJSON *object, const char *string, cJSON *item) +{ + return add_item_to_object(object, string, item, &global_hooks, false); +} + +/* Add an item to an object with constant string as key */ +CJSON_PUBLIC(cJSON_bool) +cJSON_AddItemToObjectCS(cJSON *object, const char *string, cJSON *item) +{ + return add_item_to_object(object, string, item, &global_hooks, true); +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_AddItemReferenceToArray(cJSON *array, cJSON *item) +{ + if (array == NULL) + { + return false; + } + + return add_item_to_array(array, create_reference(item, &global_hooks)); +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_AddItemReferenceToObject(cJSON *object, const char *string, cJSON *item) +{ + if ((object == NULL) || (string == NULL)) + { + return false; + } + + return add_item_to_object(object, string, create_reference(item, &global_hooks), &global_hooks, false); +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddNullToObject(cJSON *const object, const char *const name) +{ + cJSON *null = cJSON_CreateNull(); + if (add_item_to_object(object, name, null, &global_hooks, false)) + { + return null; + } + + cJSON_Delete(null); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddTrueToObject(cJSON *const object, const char *const name) +{ + cJSON *true_item = cJSON_CreateTrue(); + if (add_item_to_object(object, name, true_item, &global_hooks, false)) + { + return true_item; + } + + cJSON_Delete(true_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddFalseToObject(cJSON *const object, const char *const name) +{ + cJSON *false_item = cJSON_CreateFalse(); + if (add_item_to_object(object, name, false_item, &global_hooks, false)) + { + return false_item; + } + + cJSON_Delete(false_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddBoolToObject(cJSON *const object, const char *const name, const cJSON_bool boolean) +{ + cJSON *bool_item = cJSON_CreateBool(boolean); + if (add_item_to_object(object, name, bool_item, &global_hooks, false)) + { + return bool_item; + } + + cJSON_Delete(bool_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddNumberToObject(cJSON *const object, const char *const name, const double number) +{ + cJSON *number_item = cJSON_CreateNumber(number); + if (add_item_to_object(object, name, number_item, &global_hooks, false)) + { + return number_item; + } + + cJSON_Delete(number_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddStringToObject(cJSON *const object, const char *const name, const char *const string) +{ + cJSON *string_item = cJSON_CreateString(string); + if (add_item_to_object(object, name, string_item, &global_hooks, false)) + { + return string_item; + } + + cJSON_Delete(string_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddRawToObject(cJSON *const object, const char *const name, const char *const raw) +{ + cJSON *raw_item = cJSON_CreateRaw(raw); + if (add_item_to_object(object, name, raw_item, &global_hooks, false)) + { + return raw_item; + } + + cJSON_Delete(raw_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddObjectToObject(cJSON *const object, const char *const name) +{ + cJSON *object_item = cJSON_CreateObject(); + if (add_item_to_object(object, name, object_item, &global_hooks, false)) + { + return object_item; + } + + cJSON_Delete(object_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddArrayToObject(cJSON *const object, const char *const name) +{ + cJSON *array = cJSON_CreateArray(); + if (add_item_to_object(object, name, array, &global_hooks, false)) + { + return array; + } + + cJSON_Delete(array); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_DetachItemViaPointer(cJSON *parent, cJSON *const item) +{ + if ((parent == NULL) || (item == NULL) || (item != parent->child && item->prev == NULL)) + { + return NULL; + } + + if (item != parent->child) + { + /* not the first element */ + item->prev->next = item->next; + } + if (item->next != NULL) + { + /* not the last element */ + item->next->prev = item->prev; + } + + if (item == parent->child) + { + /* first element */ + parent->child = item->next; + } + else if (item->next == NULL) + { + /* last element */ + parent->child->prev = item->prev; + } + + /* make sure the detached item doesn't point anywhere anymore */ + item->prev = NULL; + item->next = NULL; + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_DetachItemFromArray(cJSON *array, int which) +{ + if (which < 0) + { + return NULL; + } + + return cJSON_DetachItemViaPointer(array, get_array_item(array, (size_t)which)); +} + +CJSON_PUBLIC(void) +cJSON_DeleteItemFromArray(cJSON *array, int which) +{ + cJSON_Delete(cJSON_DetachItemFromArray(array, which)); +} + +CJSON_PUBLIC(cJSON *) +cJSON_DetachItemFromObject(cJSON *object, const char *string) +{ + cJSON *to_detach = cJSON_GetObjectItem(object, string); + + return cJSON_DetachItemViaPointer(object, to_detach); +} + +CJSON_PUBLIC(cJSON *) +cJSON_DetachItemFromObjectCaseSensitive(cJSON *object, const char *string) +{ + cJSON *to_detach = cJSON_GetObjectItemCaseSensitive(object, string); + + return cJSON_DetachItemViaPointer(object, to_detach); +} + +CJSON_PUBLIC(void) +cJSON_DeleteItemFromObject(cJSON *object, const char *string) +{ + cJSON_Delete(cJSON_DetachItemFromObject(object, string)); +} + +CJSON_PUBLIC(void) +cJSON_DeleteItemFromObjectCaseSensitive(cJSON *object, const char *string) +{ + cJSON_Delete(cJSON_DetachItemFromObjectCaseSensitive(object, string)); +} + +/* Replace array/object items with new ones. */ +CJSON_PUBLIC(cJSON_bool) +cJSON_InsertItemInArray(cJSON *array, int which, cJSON *newitem) +{ + cJSON *after_inserted = NULL; + + if (which < 0 || newitem == NULL) + { + return false; + } + + after_inserted = get_array_item(array, (size_t)which); + if (after_inserted == NULL) + { + return add_item_to_array(array, newitem); + } + + if (after_inserted != array->child && after_inserted->prev == NULL) + { + /* return false if after_inserted is a corrupted array item */ + return false; + } + + newitem->next = after_inserted; + newitem->prev = after_inserted->prev; + after_inserted->prev = newitem; + if (after_inserted == array->child) + { + array->child = newitem; + } + else + { + newitem->prev->next = newitem; + } + return true; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_ReplaceItemViaPointer(cJSON *const parent, cJSON *const item, cJSON *replacement) +{ + if ((parent == NULL) || (parent->child == NULL) || (replacement == NULL) || (item == NULL)) + { + return false; + } + + if (replacement == item) + { + return true; + } + + replacement->next = item->next; + replacement->prev = item->prev; + + if (replacement->next != NULL) + { + replacement->next->prev = replacement; + } + if (parent->child == item) + { + if (parent->child->prev == parent->child) + { + replacement->prev = replacement; + } + parent->child = replacement; + } + else + { /* + * To find the last item in array quickly, we use prev in array. + * We can't modify the last item's next pointer where this item was the parent's child + */ + if (replacement->prev != NULL) + { + replacement->prev->next = replacement; + } + if (replacement->next == NULL) + { + parent->child->prev = replacement; + } + } + + item->next = NULL; + item->prev = NULL; + cJSON_Delete(item); + + return true; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_ReplaceItemInArray(cJSON *array, int which, cJSON *newitem) +{ + if (which < 0) + { + return false; + } + + return cJSON_ReplaceItemViaPointer(array, get_array_item(array, (size_t)which), newitem); +} + +static cJSON_bool replace_item_in_object(cJSON *object, const char *string, cJSON *replacement, cJSON_bool case_sensitive) +{ + if ((replacement == NULL) || (string == NULL)) + { + return false; + } + + /* replace the name in the replacement */ + if (!(replacement->type & cJSON_StringIsConst) && (replacement->string != NULL)) + { + cJSON_free(replacement->string); + } + replacement->string = (char *)cJSON_strdup((const unsigned char *)string, &global_hooks); + if (replacement->string == NULL) + { + return false; + } + + replacement->type &= ~cJSON_StringIsConst; + + return cJSON_ReplaceItemViaPointer(object, get_object_item(object, string, case_sensitive), replacement); +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_ReplaceItemInObject(cJSON *object, const char *string, cJSON *newitem) +{ + return replace_item_in_object(object, string, newitem, false); +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_ReplaceItemInObjectCaseSensitive(cJSON *object, const char *string, cJSON *newitem) +{ + return replace_item_in_object(object, string, newitem, true); +} + +/* Create basic types: */ +CJSON_PUBLIC(cJSON *) +cJSON_CreateNull(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_NULL; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateTrue(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_True; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateFalse(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_False; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateBool(cJSON_bool boolean) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = boolean ? cJSON_True : cJSON_False; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateNumber(double num) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_Number; + item->valuedouble = num; + + /* use saturation in case of overflow */ + if (num >= INT_MAX) + { + item->valueint = INT_MAX; + } + else if (num <= (double)INT_MIN) + { + item->valueint = INT_MIN; + } + else + { + item->valueint = (int)num; + } + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateString(const char *string) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_String; + item->valuestring = (char *)cJSON_strdup((const unsigned char *)string, &global_hooks); + if (!item->valuestring) + { + cJSON_Delete(item); + return NULL; + } + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateStringReference(const char *string) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item != NULL) + { + item->type = cJSON_String | cJSON_IsReference; + item->valuestring = (char *)cast_away_const(string); + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateObjectReference(const cJSON *child) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item != NULL) + { + item->type = cJSON_Object | cJSON_IsReference; + item->child = (cJSON *)cast_away_const(child); + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateArrayReference(const cJSON *child) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item != NULL) + { + item->type = cJSON_Array | cJSON_IsReference; + item->child = (cJSON *)cast_away_const(child); + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateRaw(const char *raw) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_Raw; + item->valuestring = (char *)cJSON_strdup((const unsigned char *)raw, &global_hooks); + if (!item->valuestring) + { + cJSON_Delete(item); + return NULL; + } + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateArray(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_Array; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateObject(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_Object; + } + + return item; +} + +/* Create Arrays: */ +CJSON_PUBLIC(cJSON *) +cJSON_CreateIntArray(const int *numbers, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (numbers == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for (i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateNumber(numbers[i]); + if (!n) + { + cJSON_Delete(a); + return NULL; + } + if (!i) + { + a->child = n; + } + else + { + suffix_object(p, n); + } + p = n; + } + + if (a && a->child) + { + a->child->prev = n; + } + + return a; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateFloatArray(const float *numbers, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (numbers == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for (i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateNumber((double)numbers[i]); + if (!n) + { + cJSON_Delete(a); + return NULL; + } + if (!i) + { + a->child = n; + } + else + { + suffix_object(p, n); + } + p = n; + } + + if (a && a->child) + { + a->child->prev = n; + } + + return a; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateDoubleArray(const double *numbers, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (numbers == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for (i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateNumber(numbers[i]); + if (!n) + { + cJSON_Delete(a); + return NULL; + } + if (!i) + { + a->child = n; + } + else + { + suffix_object(p, n); + } + p = n; + } + + if (a && a->child) + { + a->child->prev = n; + } + + return a; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateStringArray(const char *const *strings, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (strings == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for (i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateString(strings[i]); + if (!n) + { + cJSON_Delete(a); + return NULL; + } + if (!i) + { + a->child = n; + } + else + { + suffix_object(p, n); + } + p = n; + } + + if (a && a->child) + { + a->child->prev = n; + } + + return a; +} + +/* Duplication */ +cJSON *cJSON_Duplicate_rec(const cJSON *item, size_t depth, cJSON_bool recurse); + +CJSON_PUBLIC(cJSON *) +cJSON_Duplicate(const cJSON *item, cJSON_bool recurse) +{ + return cJSON_Duplicate_rec(item, 0, recurse); +} + +cJSON *cJSON_Duplicate_rec(const cJSON *item, size_t depth, cJSON_bool recurse) +{ + cJSON *newitem = NULL; + cJSON *child = NULL; + cJSON *next = NULL; + cJSON *newchild = NULL; + + /* Bail on bad ptr */ + if (!item) + { + goto fail; + } + /* Create new item */ + newitem = cJSON_New_Item(&global_hooks); + if (!newitem) + { + goto fail; + } + /* Copy over all vars */ + newitem->type = item->type & (~cJSON_IsReference); + newitem->valueint = item->valueint; + newitem->valuedouble = item->valuedouble; + if (item->valuestring) + { + newitem->valuestring = (char *)cJSON_strdup((unsigned char *)item->valuestring, &global_hooks); + if (!newitem->valuestring) + { + goto fail; + } + } + if (item->string) + { + newitem->string = (item->type & cJSON_StringIsConst) ? item->string : (char *)cJSON_strdup((unsigned char *)item->string, &global_hooks); + if (!newitem->string) + { + goto fail; + } + } + /* If non-recursive, then we're done! */ + if (!recurse) + { + return newitem; + } + /* Walk the ->next chain for the child. */ + child = item->child; + while (child != NULL) + { + if (depth >= CJSON_CIRCULAR_LIMIT) + { + goto fail; + } + newchild = cJSON_Duplicate_rec(child, depth + 1, true); /* Duplicate (with recurse) each item in the ->next chain */ + if (!newchild) + { + goto fail; + } + if (next != NULL) + { + /* If newitem->child already set, then crosswire ->prev and ->next and move on */ + next->next = newchild; + newchild->prev = next; + next = newchild; + } + else + { + /* Set newitem->child and move to it */ + newitem->child = newchild; + next = newchild; + } + child = child->next; + } + if (newitem && newitem->child) + { + newitem->child->prev = newchild; + } + + return newitem; + +fail: + if (newitem != NULL) + { + cJSON_Delete(newitem); + } + + return NULL; +} + +static void skip_oneline_comment(char **input) +{ + *input += static_strlen("//"); + + for (; (*input)[0] != '\0'; ++(*input)) + { + if ((*input)[0] == '\n') + { + *input += static_strlen("\n"); + return; + } + } +} + +static void skip_multiline_comment(char **input) +{ + *input += static_strlen("/*"); + + for (; (*input)[0] != '\0'; ++(*input)) + { + if (((*input)[0] == '*') && ((*input)[1] == '/')) + { + *input += static_strlen("*/"); + return; + } + } +} + +static void minify_string(char **input, char **output) +{ + (*output)[0] = (*input)[0]; + *input += static_strlen("\""); + *output += static_strlen("\""); + + for (; (*input)[0] != '\0'; (void)++(*input), ++(*output)) + { + (*output)[0] = (*input)[0]; + + if ((*input)[0] == '\"') + { + (*output)[0] = '\"'; + *input += static_strlen("\""); + *output += static_strlen("\""); + return; + } + else if (((*input)[0] == '\\') && ((*input)[1] == '\"')) + { + (*output)[1] = (*input)[1]; + *input += static_strlen("\""); + *output += static_strlen("\""); + } + } +} + +CJSON_PUBLIC(void) +cJSON_Minify(char *json) +{ + char *into = json; + + if (json == NULL) + { + return; + } + + while (json[0] != '\0') + { + switch (json[0]) + { + case ' ': + case '\t': + case '\r': + case '\n': + json++; + break; + + case '/': + if (json[1] == '/') + { + skip_oneline_comment(&json); + } + else if (json[1] == '*') + { + skip_multiline_comment(&json); + } + else + { + json++; + } + break; + + case '\"': + minify_string(&json, (char **)&into); + break; + + default: + into[0] = json[0]; + json++; + into++; + } + } + + /* and null-terminate. */ + *into = '\0'; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsInvalid(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Invalid; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsFalse(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_False; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsTrue(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xff) == cJSON_True; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsBool(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & (cJSON_True | cJSON_False)) != 0; +} +CJSON_PUBLIC(cJSON_bool) +cJSON_IsNull(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_NULL; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsNumber(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Number; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsString(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_String; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsArray(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Array; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsObject(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Object; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsRaw(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Raw; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_Compare(const cJSON *const a, const cJSON *const b, const cJSON_bool case_sensitive) +{ + if ((a == NULL) || (b == NULL) || ((a->type & 0xFF) != (b->type & 0xFF))) + { + return false; + } + + /* check if type is valid */ + switch (a->type & 0xFF) + { + case cJSON_False: + case cJSON_True: + case cJSON_NULL: + case cJSON_Number: + case cJSON_String: + case cJSON_Raw: + case cJSON_Array: + case cJSON_Object: + break; + + default: + return false; + } + + /* identical objects are equal */ + if (a == b) + { + return true; + } + + switch (a->type & 0xFF) + { + /* in these cases and equal type is enough */ + case cJSON_False: + case cJSON_True: + case cJSON_NULL: + return true; + + case cJSON_Number: + if (compare_double(a->valuedouble, b->valuedouble)) + { + return true; + } + return false; + + case cJSON_String: + case cJSON_Raw: + if ((a->valuestring == NULL) || (b->valuestring == NULL)) + { + return false; + } + if (strcmp(a->valuestring, b->valuestring) == 0) + { + return true; + } + + return false; + + case cJSON_Array: + { + cJSON *a_element = a->child; + cJSON *b_element = b->child; + + for (; (a_element != NULL) && (b_element != NULL);) + { + if (!cJSON_Compare(a_element, b_element, case_sensitive)) + { + return false; + } + + a_element = a_element->next; + b_element = b_element->next; + } + + /* one of the arrays is longer than the other */ + if (a_element != b_element) + { + return false; + } + + return true; + } + + case cJSON_Object: + { + cJSON *a_element = NULL; + cJSON *b_element = NULL; + cJSON_ArrayForEach(a_element, a) + { + /* TODO This has O(n^2) runtime, which is horrible! */ + b_element = get_object_item(b, a_element->string, case_sensitive); + if (b_element == NULL) + { + return false; + } + + if (!cJSON_Compare(a_element, b_element, case_sensitive)) + { + return false; + } + } + + /* doing this twice, once on a and b to prevent true comparison if a subset of b + * TODO: Do this the proper way, this is just a fix for now */ + cJSON_ArrayForEach(b_element, b) + { + a_element = get_object_item(a, b_element->string, case_sensitive); + if (a_element == NULL) + { + return false; + } + + if (!cJSON_Compare(b_element, a_element, case_sensitive)) + { + return false; + } + } + + return true; + } + + default: + return false; + } +} + +CJSON_PUBLIC(void *) +cJSON_malloc(size_t size) +{ + return global_hooks.allocate(size); +} + +CJSON_PUBLIC(void) +cJSON_free(void *object) +{ + global_hooks.deallocate(object); + object = NULL; +} \ No newline at end of file diff --git a/host/OmniSocketGo_add_camera/third_party/cjson/cJSON.h b/host/OmniSocketGo_add_camera/third_party/cjson/cJSON.h new file mode 100644 index 0000000..c760c95 --- /dev/null +++ b/host/OmniSocketGo_add_camera/third_party/cjson/cJSON.h @@ -0,0 +1,381 @@ +/* + Copyright (c) 2009-2017 Dave Gamble and cJSON contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#ifndef cJSON__h +#define cJSON__h + +#ifdef __cplusplus +extern "C" +{ +#endif + +#if !defined(__WINDOWS__) && (defined(WIN32) || defined(WIN64) || defined(_MSC_VER) || defined(_WIN32)) +#define __WINDOWS__ +#endif + +#ifdef __WINDOWS__ + + /* When compiling for windows, we specify a specific calling convention to avoid issues where we are being called from a project with a different default calling convention. For windows you have 3 define options: + + CJSON_HIDE_SYMBOLS - Define this in the case where you don't want to ever dllexport symbols + CJSON_EXPORT_SYMBOLS - Define this on library build when you want to dllexport symbols (default) + CJSON_IMPORT_SYMBOLS - Define this if you want to dllimport symbol + + For *nix builds that support visibility attribute, you can define similar behavior by + + setting default visibility to hidden by adding + -fvisibility=hidden (for gcc) + or + -xldscope=hidden (for sun cc) + to CFLAGS + + then using the CJSON_API_VISIBILITY flag to "export" the same symbols the way CJSON_EXPORT_SYMBOLS does + + */ + +#define CJSON_CDECL __cdecl +#define CJSON_STDCALL __stdcall + +/* export symbols by default, this is necessary for copy pasting the C and header file */ +#if !defined(CJSON_HIDE_SYMBOLS) && !defined(CJSON_IMPORT_SYMBOLS) && !defined(CJSON_EXPORT_SYMBOLS) +#define CJSON_EXPORT_SYMBOLS +#endif + +#if defined(CJSON_HIDE_SYMBOLS) +#define CJSON_PUBLIC(type) type CJSON_STDCALL +#elif defined(CJSON_EXPORT_SYMBOLS) +#define CJSON_PUBLIC(type) __declspec(dllexport) type CJSON_STDCALL +#elif defined(CJSON_IMPORT_SYMBOLS) +#define CJSON_PUBLIC(type) __declspec(dllimport) type CJSON_STDCALL +#endif +#else /* !__WINDOWS__ */ +#define CJSON_CDECL +#define CJSON_STDCALL + +#if (defined(__GNUC__) || defined(__SUNPRO_CC) || defined(__SUNPRO_C)) && defined(CJSON_API_VISIBILITY) +#define CJSON_PUBLIC(type) __attribute__((visibility("default"))) type +#else +#define CJSON_PUBLIC(type) type +#endif +#endif + +/* project version */ +#define CJSON_VERSION_MAJOR 1 +#define CJSON_VERSION_MINOR 7 +#define CJSON_VERSION_PATCH 19 + +#include + +/* cJSON Types: */ +#define cJSON_Invalid (0) +#define cJSON_False (1 << 0) +#define cJSON_True (1 << 1) +#define cJSON_NULL (1 << 2) +#define cJSON_Number (1 << 3) +#define cJSON_String (1 << 4) +#define cJSON_Array (1 << 5) +#define cJSON_Object (1 << 6) +#define cJSON_Raw (1 << 7) /* raw json */ + +#define cJSON_IsReference 256 +#define cJSON_StringIsConst 512 + + /* The cJSON structure: */ + typedef struct cJSON + { + /* next/prev allow you to walk array/object chains. Alternatively, use GetArraySize/GetArrayItem/GetObjectItem */ + struct cJSON *next; + struct cJSON *prev; + /* An array or object item will have a child pointer pointing to a chain of the items in the array/object. */ + struct cJSON *child; + + /* The type of the item, as above. */ + int type; + + /* The item's string, if type==cJSON_String and type == cJSON_Raw */ + char *valuestring; + /* writing to valueint is DEPRECATED, use cJSON_SetNumberValue instead */ + int valueint; + /* The item's number, if type==cJSON_Number */ + double valuedouble; + + /* The item's name string, if this item is the child of, or is in the list of subitems of an object. */ + char *string; + } cJSON; + + typedef struct cJSON_Hooks + { + /* malloc/free are CDECL on Windows regardless of the default calling convention of the compiler, so ensure the hooks allow passing those functions directly. */ + void *(CJSON_CDECL *malloc_fn)(size_t sz); + void(CJSON_CDECL *free_fn)(void *ptr); + } cJSON_Hooks; + + typedef int cJSON_bool; + +/* Limits how deeply nested arrays/objects can be before cJSON rejects to parse them. + * This is to prevent stack overflows. */ +#ifndef CJSON_NESTING_LIMIT +#define CJSON_NESTING_LIMIT 1000 +#endif + +/* Limits the length of circular references can be before cJSON rejects to parse them. + * This is to prevent stack overflows. */ +#ifndef CJSON_CIRCULAR_LIMIT +#define CJSON_CIRCULAR_LIMIT 10000 +#endif + + /* returns the version of cJSON as a string */ + CJSON_PUBLIC(const char *) + cJSON_Version(void); + + /* Supply malloc, realloc and free functions to cJSON */ + CJSON_PUBLIC(void) + cJSON_InitHooks(cJSON_Hooks *hooks); + + /* Memory Management: the caller is always responsible to free the results from all variants of cJSON_Parse (with cJSON_Delete) and cJSON_Print (with stdlib free, cJSON_Hooks.free_fn, or cJSON_free as appropriate). The exception is cJSON_PrintPreallocated, where the caller has full responsibility of the buffer. */ + /* Supply a block of JSON, and this returns a cJSON object you can interrogate. */ + CJSON_PUBLIC(cJSON *) + cJSON_Parse(const char *value); + CJSON_PUBLIC(cJSON *) + cJSON_ParseWithLength(const char *value, size_t buffer_length); + /* ParseWithOpts allows you to require (and check) that the JSON is null terminated, and to retrieve the pointer to the final byte parsed. */ + /* If you supply a ptr in return_parse_end and parsing fails, then return_parse_end will contain a pointer to the error so will match cJSON_GetErrorPtr(). */ + CJSON_PUBLIC(cJSON *) + cJSON_ParseWithOpts(const char *value, const char **return_parse_end, cJSON_bool require_null_terminated); + CJSON_PUBLIC(cJSON *) + cJSON_ParseWithLengthOpts(const char *value, size_t buffer_length, const char **return_parse_end, cJSON_bool require_null_terminated); + + /* Render a cJSON entity to text for transfer/storage. */ + CJSON_PUBLIC(char *) + cJSON_Print(const cJSON *item); + /* Render a cJSON entity to text for transfer/storage without any formatting. */ + CJSON_PUBLIC(char *) + cJSON_PrintUnformatted(const cJSON *item); + /* Render a cJSON entity to text using a buffered strategy. prebuffer is a guess at the final size. guessing well reduces reallocation. fmt=0 gives unformatted, =1 gives formatted */ + CJSON_PUBLIC(char *) + cJSON_PrintBuffered(const cJSON *item, int prebuffer, cJSON_bool fmt); + /* Render a cJSON entity to text using a buffer already allocated in memory with given length. Returns 1 on success and 0 on failure. */ + /* NOTE: cJSON is not always 100% accurate in estimating how much memory it will use, so to be safe allocate 5 bytes more than you actually need */ + CJSON_PUBLIC(cJSON_bool) + cJSON_PrintPreallocated(cJSON *item, char *buffer, const int length, const cJSON_bool format); + /* Delete a cJSON entity and all subentities. */ + CJSON_PUBLIC(void) + cJSON_Delete(cJSON *item); + + /* Returns the number of items in an array (or object). */ + CJSON_PUBLIC(int) + cJSON_GetArraySize(const cJSON *array); + /* Retrieve item number "index" from array "array". Returns NULL if unsuccessful. */ + CJSON_PUBLIC(cJSON *) + cJSON_GetArrayItem(const cJSON *array, int index); + /* Get item "string" from object. Case insensitive. */ + CJSON_PUBLIC(cJSON *) + cJSON_GetObjectItem(const cJSON *const object, const char *const string); + CJSON_PUBLIC(cJSON *) + cJSON_GetObjectItemCaseSensitive(const cJSON *const object, const char *const string); + CJSON_PUBLIC(cJSON_bool) + cJSON_HasObjectItem(const cJSON *object, const char *string); + /* For analysing failed parses. This returns a pointer to the parse error. You'll probably need to look a few chars back to make sense of it. Defined when cJSON_Parse() returns 0. 0 when cJSON_Parse() succeeds. */ + CJSON_PUBLIC(const char *) + cJSON_GetErrorPtr(void); + + /* Check item type and return its value */ + CJSON_PUBLIC(char *) + cJSON_GetStringValue(const cJSON *const item); + CJSON_PUBLIC(double) + cJSON_GetNumberValue(const cJSON *const item); + + /* These functions check the type of an item */ + CJSON_PUBLIC(cJSON_bool) + cJSON_IsInvalid(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsFalse(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsTrue(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsBool(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsNull(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsNumber(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsString(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsArray(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsObject(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsRaw(const cJSON *const item); + + /* These calls create a cJSON item of the appropriate type. */ + CJSON_PUBLIC(cJSON *) + cJSON_CreateNull(void); + CJSON_PUBLIC(cJSON *) + cJSON_CreateTrue(void); + CJSON_PUBLIC(cJSON *) + cJSON_CreateFalse(void); + CJSON_PUBLIC(cJSON *) + cJSON_CreateBool(cJSON_bool boolean); + CJSON_PUBLIC(cJSON *) + cJSON_CreateNumber(double num); + CJSON_PUBLIC(cJSON *) + cJSON_CreateString(const char *string); + /* raw json */ + CJSON_PUBLIC(cJSON *) + cJSON_CreateRaw(const char *raw); + CJSON_PUBLIC(cJSON *) + cJSON_CreateArray(void); + CJSON_PUBLIC(cJSON *) + cJSON_CreateObject(void); + + /* Create a string where valuestring references a string so + * it will not be freed by cJSON_Delete */ + CJSON_PUBLIC(cJSON *) + cJSON_CreateStringReference(const char *string); + /* Create an object/array that only references it's elements so + * they will not be freed by cJSON_Delete */ + CJSON_PUBLIC(cJSON *) + cJSON_CreateObjectReference(const cJSON *child); + CJSON_PUBLIC(cJSON *) + cJSON_CreateArrayReference(const cJSON *child); + + /* These utilities create an Array of count items. + * The parameter count cannot be greater than the number of elements in the number array, otherwise array access will be out of bounds.*/ + CJSON_PUBLIC(cJSON *) + cJSON_CreateIntArray(const int *numbers, int count); + CJSON_PUBLIC(cJSON *) + cJSON_CreateFloatArray(const float *numbers, int count); + CJSON_PUBLIC(cJSON *) + cJSON_CreateDoubleArray(const double *numbers, int count); + CJSON_PUBLIC(cJSON *) + cJSON_CreateStringArray(const char *const *strings, int count); + + /* Append item to the specified array/object. */ + CJSON_PUBLIC(cJSON_bool) + cJSON_AddItemToArray(cJSON *array, cJSON *item); + CJSON_PUBLIC(cJSON_bool) + cJSON_AddItemToObject(cJSON *object, const char *string, cJSON *item); + /* Use this when string is definitely const (i.e. a literal, or as good as), and will definitely survive the cJSON object. + * WARNING: When this function was used, make sure to always check that (item->type & cJSON_StringIsConst) is zero before + * writing to `item->string` */ + CJSON_PUBLIC(cJSON_bool) + cJSON_AddItemToObjectCS(cJSON *object, const char *string, cJSON *item); + /* Append reference to item to the specified array/object. Use this when you want to add an existing cJSON to a new cJSON, but don't want to corrupt your existing cJSON. */ + CJSON_PUBLIC(cJSON_bool) + cJSON_AddItemReferenceToArray(cJSON *array, cJSON *item); + CJSON_PUBLIC(cJSON_bool) + cJSON_AddItemReferenceToObject(cJSON *object, const char *string, cJSON *item); + + /* Remove/Detach items from Arrays/Objects. */ + CJSON_PUBLIC(cJSON *) + cJSON_DetachItemViaPointer(cJSON *parent, cJSON *const item); + CJSON_PUBLIC(cJSON *) + cJSON_DetachItemFromArray(cJSON *array, int which); + CJSON_PUBLIC(void) + cJSON_DeleteItemFromArray(cJSON *array, int which); + CJSON_PUBLIC(cJSON *) + cJSON_DetachItemFromObject(cJSON *object, const char *string); + CJSON_PUBLIC(cJSON *) + cJSON_DetachItemFromObjectCaseSensitive(cJSON *object, const char *string); + CJSON_PUBLIC(void) + cJSON_DeleteItemFromObject(cJSON *object, const char *string); + CJSON_PUBLIC(void) + cJSON_DeleteItemFromObjectCaseSensitive(cJSON *object, const char *string); + + /* Update array items. */ + CJSON_PUBLIC(cJSON_bool) + cJSON_InsertItemInArray(cJSON *array, int which, cJSON *newitem); /* Shifts pre-existing items to the right. */ + CJSON_PUBLIC(cJSON_bool) + cJSON_ReplaceItemViaPointer(cJSON *const parent, cJSON *const item, cJSON *replacement); + CJSON_PUBLIC(cJSON_bool) + cJSON_ReplaceItemInArray(cJSON *array, int which, cJSON *newitem); + CJSON_PUBLIC(cJSON_bool) + cJSON_ReplaceItemInObject(cJSON *object, const char *string, cJSON *newitem); + CJSON_PUBLIC(cJSON_bool) + cJSON_ReplaceItemInObjectCaseSensitive(cJSON *object, const char *string, cJSON *newitem); + + /* Duplicate a cJSON item */ + CJSON_PUBLIC(cJSON *) + cJSON_Duplicate(const cJSON *item, cJSON_bool recurse); + /* Duplicate will create a new, identical cJSON item to the one you pass, in new memory that will + * need to be released. With recurse!=0, it will duplicate any children connected to the item. + * The item->next and ->prev pointers are always zero on return from Duplicate. */ + /* Recursively compare two cJSON items for equality. If either a or b is NULL or invalid, they will be considered unequal. + * case_sensitive determines if object keys are treated case sensitive (1) or case insensitive (0) */ + CJSON_PUBLIC(cJSON_bool) + cJSON_Compare(const cJSON *const a, const cJSON *const b, const cJSON_bool case_sensitive); + + /* Minify a strings, remove blank characters(such as ' ', '\t', '\r', '\n') from strings. + * The input pointer json cannot point to a read-only address area, such as a string constant, + * but should point to a readable and writable address area. */ + CJSON_PUBLIC(void) + cJSON_Minify(char *json); + + /* Helper functions for creating and adding items to an object at the same time. + * They return the added item or NULL on failure. */ + CJSON_PUBLIC(cJSON *) + cJSON_AddNullToObject(cJSON *const object, const char *const name); + CJSON_PUBLIC(cJSON *) + cJSON_AddTrueToObject(cJSON *const object, const char *const name); + CJSON_PUBLIC(cJSON *) + cJSON_AddFalseToObject(cJSON *const object, const char *const name); + CJSON_PUBLIC(cJSON *) + cJSON_AddBoolToObject(cJSON *const object, const char *const name, const cJSON_bool boolean); + CJSON_PUBLIC(cJSON *) + cJSON_AddNumberToObject(cJSON *const object, const char *const name, const double number); + CJSON_PUBLIC(cJSON *) + cJSON_AddStringToObject(cJSON *const object, const char *const name, const char *const string); + CJSON_PUBLIC(cJSON *) + cJSON_AddRawToObject(cJSON *const object, const char *const name, const char *const raw); + CJSON_PUBLIC(cJSON *) + cJSON_AddObjectToObject(cJSON *const object, const char *const name); + CJSON_PUBLIC(cJSON *) + cJSON_AddArrayToObject(cJSON *const object, const char *const name); + +/* When assigning an integer value, it needs to be propagated to valuedouble too. */ +#define cJSON_SetIntValue(object, number) ((object) ? (object)->valueint = (object)->valuedouble = (number) : (number)) + /* helper for the cJSON_SetNumberValue macro */ + CJSON_PUBLIC(double) + cJSON_SetNumberHelper(cJSON *object, double number); +#define cJSON_SetNumberValue(object, number) ((object != NULL) ? cJSON_SetNumberHelper(object, (double)number) : (number)) + /* Change the valuestring of a cJSON_String object, only takes effect when type of object is cJSON_String */ + CJSON_PUBLIC(char *) + cJSON_SetValuestring(cJSON *object, const char *valuestring); + +/* If the object is not a boolean type this does nothing and returns cJSON_Invalid else it returns the new type*/ +#define cJSON_SetBoolValue(object, boolValue) ( \ + (object != NULL && ((object)->type & (cJSON_False | cJSON_True))) ? (object)->type = ((object)->type & (~(cJSON_False | cJSON_True))) | ((boolValue) ? cJSON_True : cJSON_False) : cJSON_Invalid) + +/* Macro for iterating over an array or object */ +#define cJSON_ArrayForEach(element, array) for (element = (array != NULL) ? (array)->child : NULL; element != NULL; element = element->next) + + /* malloc/free objects using the malloc/free functions that have been set with cJSON_InitHooks */ + CJSON_PUBLIC(void *) + cJSON_malloc(size_t size); + CJSON_PUBLIC(void) + cJSON_free(void *object); + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file diff --git a/host/OmniSocketGo_add_camera/third_party/kcp/ikcp.c b/host/OmniSocketGo_add_camera/third_party/kcp/ikcp.c new file mode 100644 index 0000000..593ae41 --- /dev/null +++ b/host/OmniSocketGo_add_camera/third_party/kcp/ikcp.c @@ -0,0 +1,1466 @@ +//===================================================================== +// +// KCP - A Better ARQ Protocol Implementation +// skywind3000 (at) gmail.com, 2010-2011 +// +// Features: +// + Average RTT reduce 30% - 40% vs traditional ARQ like tcp. +// + Maximum RTT reduce three times vs tcp. +// + Lightweight, distributed as a single source file. +// +//===================================================================== +#include "ikcp.h" + +#include +#include +#include +#include +#include + +#define IKCP_FASTACK_CONSERVE + +//===================================================================== +// KCP BASIC +//===================================================================== +const IUINT32 IKCP_RTO_NDL = 30; // no delay min rto +const IUINT32 IKCP_RTO_MIN = 100; // normal min rto +const IUINT32 IKCP_RTO_DEF = 200; +const IUINT32 IKCP_RTO_MAX = 60000; +const IUINT32 IKCP_CMD_PUSH = 81; // cmd: push data +const IUINT32 IKCP_CMD_ACK = 82; // cmd: ack +const IUINT32 IKCP_CMD_WASK = 83; // cmd: window probe (ask) +const IUINT32 IKCP_CMD_WINS = 84; // cmd: window size (tell) +const IUINT32 IKCP_ASK_SEND = 1; // need to send IKCP_CMD_WASK +const IUINT32 IKCP_ASK_TELL = 2; // need to send IKCP_CMD_WINS +const IUINT32 IKCP_WND_SND = 32; +const IUINT32 IKCP_WND_RCV = 128; // must >= max fragment size +const IUINT32 IKCP_MTU_DEF = 1400; +const IUINT32 IKCP_ACK_FAST = 3; +const IUINT32 IKCP_INTERVAL = 100; +const IUINT32 IKCP_OVERHEAD = 24; +const IUINT32 IKCP_DEADLINK = 20; +const IUINT32 IKCP_THRESH_INIT = 2; +const IUINT32 IKCP_THRESH_MIN = 2; +const IUINT32 IKCP_PROBE_INIT = 7000; // 7 secs to probe window size +const IUINT32 IKCP_PROBE_LIMIT = 120000; // up to 120 secs to probe window +const IUINT32 IKCP_FASTACK_LIMIT = 5; // max times to trigger fastack + +//--------------------------------------------------------------------- +// encode / decode +//--------------------------------------------------------------------- + +/* encode 8 bits unsigned int */ +static inline char *ikcp_encode8u(char *p, unsigned char c) +{ + *(unsigned char *)p++ = c; + return p; +} + +/* decode 8 bits unsigned int */ +static inline const char *ikcp_decode8u(const char *p, unsigned char *c) +{ + *c = *(unsigned char *)p++; + return p; +} + +/* encode 16 bits unsigned int (lsb) */ +static inline char *ikcp_encode16u(char *p, unsigned short w) +{ +#if IWORDS_BIG_ENDIAN || IWORDS_MUST_ALIGN + *(unsigned char *)(p + 0) = (w & 255); + *(unsigned char *)(p + 1) = (w >> 8); +#else + memcpy(p, &w, 2); +#endif + p += 2; + return p; +} + +/* decode 16 bits unsigned int (lsb) */ +static inline const char *ikcp_decode16u(const char *p, unsigned short *w) +{ +#if IWORDS_BIG_ENDIAN || IWORDS_MUST_ALIGN + *w = *(const unsigned char *)(p + 1); + *w = *(const unsigned char *)(p + 0) + (*w << 8); +#else + memcpy(w, p, 2); +#endif + p += 2; + return p; +} + +/* encode 32 bits unsigned int (lsb) */ +static inline char *ikcp_encode32u(char *p, IUINT32 l) +{ +#if IWORDS_BIG_ENDIAN || IWORDS_MUST_ALIGN + *(unsigned char *)(p + 0) = (unsigned char)((l >> 0) & 0xff); + *(unsigned char *)(p + 1) = (unsigned char)((l >> 8) & 0xff); + *(unsigned char *)(p + 2) = (unsigned char)((l >> 16) & 0xff); + *(unsigned char *)(p + 3) = (unsigned char)((l >> 24) & 0xff); +#else + memcpy(p, &l, 4); +#endif + p += 4; + return p; +} + +/* decode 32 bits unsigned int (lsb) */ +static inline const char *ikcp_decode32u(const char *p, IUINT32 *l) +{ +#if IWORDS_BIG_ENDIAN || IWORDS_MUST_ALIGN + *l = *(const unsigned char *)(p + 3); + *l = *(const unsigned char *)(p + 2) + (*l << 8); + *l = *(const unsigned char *)(p + 1) + (*l << 8); + *l = *(const unsigned char *)(p + 0) + (*l << 8); +#else + memcpy(l, p, 4); +#endif + p += 4; + return p; +} + +static inline IUINT32 _imin_(IUINT32 a, IUINT32 b) +{ + return a <= b ? a : b; +} + +static inline IUINT32 _imax_(IUINT32 a, IUINT32 b) +{ + return a >= b ? a : b; +} + +static inline IUINT32 _ibound_(IUINT32 lower, IUINT32 middle, IUINT32 upper) +{ + return _imin_(_imax_(lower, middle), upper); +} + +static inline long _itimediff(IUINT32 later, IUINT32 earlier) +{ + return ((IINT32)(later - earlier)); +} + +//--------------------------------------------------------------------- +// manage segment +//--------------------------------------------------------------------- +typedef struct IKCPSEG IKCPSEG; + +static void *(*ikcp_malloc_hook)(size_t) = NULL; +static void (*ikcp_free_hook)(void *) = NULL; + +// internal malloc +static void *ikcp_malloc(size_t size) +{ + if (ikcp_malloc_hook) + return ikcp_malloc_hook(size); + return malloc(size); +} + +// internal free +static void ikcp_free(void *ptr) +{ + if (ikcp_free_hook) + { + ikcp_free_hook(ptr); + } + else + { + free(ptr); + } +} + +// redefine allocator +void ikcp_allocator(void *(*new_malloc)(size_t), void (*new_free)(void *)) +{ + ikcp_malloc_hook = new_malloc; + ikcp_free_hook = new_free; +} + +// allocate a new kcp segment +static IKCPSEG *ikcp_segment_new(ikcpcb *kcp, int size) +{ + return (IKCPSEG *)ikcp_malloc(sizeof(IKCPSEG) + size); +} + +// delete a segment +static void ikcp_segment_delete(ikcpcb *kcp, IKCPSEG *seg) +{ + ikcp_free(seg); +} + +// write log +void ikcp_log(ikcpcb *kcp, int mask, const char *fmt, ...) +{ + char buffer[1024]; + va_list argptr; + if ((mask & kcp->logmask) == 0 || kcp->writelog == 0) + return; + va_start(argptr, fmt); + vsprintf(buffer, fmt, argptr); + va_end(argptr); + kcp->writelog(buffer, kcp, kcp->user); +} + +// check log mask +static int ikcp_canlog(const ikcpcb *kcp, int mask) +{ + if ((mask & kcp->logmask) == 0 || kcp->writelog == NULL) + return 0; + return 1; +} + +// output segment +static int ikcp_output(ikcpcb *kcp, const void *data, int size) +{ + assert(kcp); + assert(kcp->output); + if (ikcp_canlog(kcp, IKCP_LOG_OUTPUT)) + { + ikcp_log(kcp, IKCP_LOG_OUTPUT, "[RO] %ld bytes", (long)size); + } + if (size == 0) + return 0; + return kcp->output((const char *)data, size, kcp, kcp->user); +} + +// output queue +void ikcp_qprint(const char *name, const struct IQUEUEHEAD *head) +{ +#if 0 + const struct IQUEUEHEAD *p; + printf("<%s>: [", name); + for (p = head->next; p != head; p = p->next) { + const IKCPSEG *seg = iqueue_entry(p, const IKCPSEG, node); + printf("(%lu %d)", (unsigned long)seg->sn, (int)(seg->ts % 10000)); + if (p->next != head) printf(","); + } + printf("]\n"); +#endif +} + +//--------------------------------------------------------------------- +// create a new kcpcb +//--------------------------------------------------------------------- +ikcpcb *ikcp_create(IUINT32 conv, void *user) +{ + ikcpcb *kcp = (ikcpcb *)ikcp_malloc(sizeof(struct IKCPCB)); + if (kcp == NULL) + return NULL; + kcp->conv = conv; + kcp->user = user; + kcp->snd_una = 0; + kcp->snd_nxt = 0; + kcp->rcv_nxt = 0; + kcp->ts_recent = 0; + kcp->ts_lastack = 0; + kcp->ts_probe = 0; + kcp->probe_wait = 0; + kcp->snd_wnd = IKCP_WND_SND; + kcp->rcv_wnd = IKCP_WND_RCV; + kcp->rmt_wnd = IKCP_WND_RCV; + kcp->cwnd = 0; + kcp->incr = 0; + kcp->probe = 0; + kcp->mtu = IKCP_MTU_DEF; + kcp->mss = kcp->mtu - IKCP_OVERHEAD; + kcp->stream = 0; + + kcp->buffer = (char *)ikcp_malloc((kcp->mtu + IKCP_OVERHEAD) * 3); + if (kcp->buffer == NULL) + { + ikcp_free(kcp); + return NULL; + } + + iqueue_init(&kcp->snd_queue); + iqueue_init(&kcp->rcv_queue); + iqueue_init(&kcp->snd_buf); + iqueue_init(&kcp->rcv_buf); + kcp->nrcv_buf = 0; + kcp->nsnd_buf = 0; + kcp->nrcv_que = 0; + kcp->nsnd_que = 0; + kcp->state = 0; + kcp->acklist = NULL; + kcp->ackblock = 0; + kcp->ackcount = 0; + kcp->rx_srtt = 0; + kcp->rx_rttval = 0; + kcp->rx_rto = IKCP_RTO_DEF; + kcp->rx_minrto = IKCP_RTO_MIN; + kcp->current = 0; + kcp->interval = IKCP_INTERVAL; + kcp->ts_flush = IKCP_INTERVAL; + kcp->nodelay = 0; + kcp->updated = 0; + kcp->logmask = 0; + kcp->ssthresh = IKCP_THRESH_INIT; + kcp->fastresend = 0; + kcp->fastlimit = IKCP_FASTACK_LIMIT; + kcp->nocwnd = 0; + kcp->xmit = 0; + kcp->timeout_retrans_total = 0; + kcp->fast_retrans_total = 0; + kcp->duplicate_recv_total = 0; + kcp->dead_link = IKCP_DEADLINK; + kcp->output = NULL; + kcp->writelog = NULL; + + return kcp; +} + +//--------------------------------------------------------------------- +// release a new kcpcb +//--------------------------------------------------------------------- +void ikcp_release(ikcpcb *kcp) +{ + assert(kcp); + if (kcp) + { + IKCPSEG *seg; + while (!iqueue_is_empty(&kcp->snd_buf)) + { + seg = iqueue_entry(kcp->snd_buf.next, IKCPSEG, node); + iqueue_del(&seg->node); + ikcp_segment_delete(kcp, seg); + } + while (!iqueue_is_empty(&kcp->rcv_buf)) + { + seg = iqueue_entry(kcp->rcv_buf.next, IKCPSEG, node); + iqueue_del(&seg->node); + ikcp_segment_delete(kcp, seg); + } + while (!iqueue_is_empty(&kcp->snd_queue)) + { + seg = iqueue_entry(kcp->snd_queue.next, IKCPSEG, node); + iqueue_del(&seg->node); + ikcp_segment_delete(kcp, seg); + } + while (!iqueue_is_empty(&kcp->rcv_queue)) + { + seg = iqueue_entry(kcp->rcv_queue.next, IKCPSEG, node); + iqueue_del(&seg->node); + ikcp_segment_delete(kcp, seg); + } + if (kcp->buffer) + { + ikcp_free(kcp->buffer); + } + if (kcp->acklist) + { + ikcp_free(kcp->acklist); + } + + kcp->nrcv_buf = 0; + kcp->nsnd_buf = 0; + kcp->nrcv_que = 0; + kcp->nsnd_que = 0; + kcp->ackcount = 0; + kcp->buffer = NULL; + kcp->acklist = NULL; + ikcp_free(kcp); + } +} + +//--------------------------------------------------------------------- +// set output callback, which will be invoked by kcp +//--------------------------------------------------------------------- +void ikcp_setoutput(ikcpcb *kcp, int (*output)(const char *buf, int len, + ikcpcb *kcp, void *user)) +{ + kcp->output = output; +} + +//--------------------------------------------------------------------- +// user/upper level recv: returns size, returns below zero for EAGAIN +//--------------------------------------------------------------------- +int ikcp_recv(ikcpcb *kcp, char *buffer, int len) +{ + struct IQUEUEHEAD *p; + int ispeek = (len < 0) ? 1 : 0; + int peeksize; + int recover = 0; + IKCPSEG *seg; + assert(kcp); + + if (iqueue_is_empty(&kcp->rcv_queue)) + return -1; + + if (len < 0) + len = -len; + + peeksize = ikcp_peeksize(kcp); + + if (peeksize < 0) + return -2; + + if (peeksize > len) + return -3; + + if (kcp->nrcv_que >= kcp->rcv_wnd) + recover = 1; + + // merge fragment + for (len = 0, p = kcp->rcv_queue.next; p != &kcp->rcv_queue;) + { + int fragment; + seg = iqueue_entry(p, IKCPSEG, node); + p = p->next; + + if (buffer) + { + memcpy(buffer, seg->data, seg->len); + buffer += seg->len; + } + + len += seg->len; + fragment = seg->frg; + + if (ikcp_canlog(kcp, IKCP_LOG_RECV)) + { + ikcp_log(kcp, IKCP_LOG_RECV, "recv sn=%lu", (unsigned long)seg->sn); + } + + if (ispeek == 0) + { + iqueue_del(&seg->node); + ikcp_segment_delete(kcp, seg); + kcp->nrcv_que--; + } + + if (fragment == 0) + break; + } + + assert(len == peeksize); + + // move available data from rcv_buf -> rcv_queue + while (!iqueue_is_empty(&kcp->rcv_buf)) + { + seg = iqueue_entry(kcp->rcv_buf.next, IKCPSEG, node); + if (seg->sn == kcp->rcv_nxt && kcp->nrcv_que < kcp->rcv_wnd) + { + iqueue_del(&seg->node); + kcp->nrcv_buf--; + iqueue_add_tail(&seg->node, &kcp->rcv_queue); + kcp->nrcv_que++; + kcp->rcv_nxt++; + } + else + { + break; + } + } + + // fast recover + if (kcp->nrcv_que < kcp->rcv_wnd && recover) + { + // ready to send back IKCP_CMD_WINS in ikcp_flush + // tell remote my window size + kcp->probe |= IKCP_ASK_TELL; + } + + return len; +} + +//--------------------------------------------------------------------- +// peek data size +//--------------------------------------------------------------------- +int ikcp_peeksize(const ikcpcb *kcp) +{ + struct IQUEUEHEAD *p; + IKCPSEG *seg; + int length = 0; + + assert(kcp); + + if (iqueue_is_empty(&kcp->rcv_queue)) + return -1; + + seg = iqueue_entry(kcp->rcv_queue.next, IKCPSEG, node); + if (seg->frg == 0) + return seg->len; + + if (kcp->nrcv_que < seg->frg + 1) + return -1; + + for (p = kcp->rcv_queue.next; p != &kcp->rcv_queue; p = p->next) + { + seg = iqueue_entry(p, IKCPSEG, node); + length += seg->len; + if (seg->frg == 0) + break; + } + + return length; +} + +//--------------------------------------------------------------------- +// user/upper level send, returns below zero for error +//--------------------------------------------------------------------- +int ikcp_send(ikcpcb *kcp, const char *buffer, int len) +{ + IKCPSEG *seg; + int count, i; + int sent = 0; + + assert(kcp->mss > 0); + if (len < 0) + return -1; + + // append to previous segment in streaming mode (if possible) + if (kcp->stream != 0) + { + if (!iqueue_is_empty(&kcp->snd_queue)) + { + IKCPSEG *old = iqueue_entry(kcp->snd_queue.prev, IKCPSEG, node); + if (old->len < kcp->mss) + { + int capacity = kcp->mss - old->len; + int extend = (len < capacity) ? len : capacity; + seg = ikcp_segment_new(kcp, old->len + extend); + assert(seg); + if (seg == NULL) + { + return -2; + } + iqueue_add_tail(&seg->node, &kcp->snd_queue); + memcpy(seg->data, old->data, old->len); + if (buffer) + { + memcpy(seg->data + old->len, buffer, extend); + buffer += extend; + } + seg->len = old->len + extend; + seg->frg = 0; + len -= extend; + iqueue_del_init(&old->node); + ikcp_segment_delete(kcp, old); + sent = extend; + } + } + if (len <= 0) + { + return sent; + } + } + + if (len <= (int)kcp->mss) + count = 1; + else + count = (len + kcp->mss - 1) / kcp->mss; + + if (count >= (int)IKCP_WND_RCV) + { + if (kcp->stream != 0 && sent > 0) + return sent; + return -2; + } + + if (count == 0) + count = 1; + + // fragment + for (i = 0; i < count; i++) + { + int size = len > (int)kcp->mss ? (int)kcp->mss : len; + seg = ikcp_segment_new(kcp, size); + assert(seg); + if (seg == NULL) + { + return -2; + } + if (buffer && len > 0) + { + memcpy(seg->data, buffer, size); + } + seg->len = size; + seg->frg = (kcp->stream == 0) ? (count - i - 1) : 0; + iqueue_init(&seg->node); + iqueue_add_tail(&seg->node, &kcp->snd_queue); + kcp->nsnd_que++; + if (buffer) + { + buffer += size; + } + len -= size; + sent += size; + } + + return sent; +} + +//--------------------------------------------------------------------- +// parse ack +//--------------------------------------------------------------------- +static void ikcp_update_ack(ikcpcb *kcp, IINT32 rtt) +{ + IINT32 rto = 0; + if (kcp->rx_srtt == 0) + { + kcp->rx_srtt = rtt; + kcp->rx_rttval = rtt / 2; + } + else + { + long delta = rtt - kcp->rx_srtt; + if (delta < 0) + delta = -delta; + kcp->rx_rttval = (3 * kcp->rx_rttval + delta) / 4; + kcp->rx_srtt = (7 * kcp->rx_srtt + rtt) / 8; + if (kcp->rx_srtt < 1) + kcp->rx_srtt = 1; + } + rto = kcp->rx_srtt + _imax_(kcp->interval, 4 * kcp->rx_rttval); + kcp->rx_rto = _ibound_(kcp->rx_minrto, rto, IKCP_RTO_MAX); +} + +static void ikcp_shrink_buf(ikcpcb *kcp) +{ + struct IQUEUEHEAD *p = kcp->snd_buf.next; + if (p != &kcp->snd_buf) + { + IKCPSEG *seg = iqueue_entry(p, IKCPSEG, node); + kcp->snd_una = seg->sn; + } + else + { + kcp->snd_una = kcp->snd_nxt; + } +} + +static void ikcp_parse_ack(ikcpcb *kcp, IUINT32 sn) +{ + struct IQUEUEHEAD *p, *next; + + if (_itimediff(sn, kcp->snd_una) < 0 || _itimediff(sn, kcp->snd_nxt) >= 0) + return; + + for (p = kcp->snd_buf.next; p != &kcp->snd_buf; p = next) + { + IKCPSEG *seg = iqueue_entry(p, IKCPSEG, node); + next = p->next; + if (sn == seg->sn) + { + iqueue_del(p); + ikcp_segment_delete(kcp, seg); + kcp->nsnd_buf--; + break; + } + if (_itimediff(sn, seg->sn) < 0) + { + break; + } + } +} + +static void ikcp_parse_una(ikcpcb *kcp, IUINT32 una) +{ + struct IQUEUEHEAD *p, *next; + for (p = kcp->snd_buf.next; p != &kcp->snd_buf; p = next) + { + IKCPSEG *seg = iqueue_entry(p, IKCPSEG, node); + next = p->next; + if (_itimediff(una, seg->sn) > 0) + { + iqueue_del(p); + ikcp_segment_delete(kcp, seg); + kcp->nsnd_buf--; + } + else + { + break; + } + } +} + +static void ikcp_parse_fastack(ikcpcb *kcp, IUINT32 sn, IUINT32 ts) +{ + struct IQUEUEHEAD *p, *next; + + if (_itimediff(sn, kcp->snd_una) < 0 || _itimediff(sn, kcp->snd_nxt) >= 0) + return; + + for (p = kcp->snd_buf.next; p != &kcp->snd_buf; p = next) + { + IKCPSEG *seg = iqueue_entry(p, IKCPSEG, node); + next = p->next; + if (_itimediff(sn, seg->sn) < 0) + { + break; + } + else if (sn != seg->sn) + { +#ifndef IKCP_FASTACK_CONSERVE + seg->fastack++; +#else + if (_itimediff(ts, seg->ts) >= 0) + seg->fastack++; +#endif + } + } +} + +//--------------------------------------------------------------------- +// ack append +//--------------------------------------------------------------------- +static void ikcp_ack_push(ikcpcb *kcp, IUINT32 sn, IUINT32 ts) +{ + IUINT32 newsize = kcp->ackcount + 1; + IUINT32 *ptr; + + if (newsize > kcp->ackblock) + { + IUINT32 *acklist; + IUINT32 newblock; + + for (newblock = 8; newblock < newsize; newblock <<= 1) + ; + acklist = (IUINT32 *)ikcp_malloc(newblock * sizeof(IUINT32) * 2); + + if (acklist == NULL) + { + assert(acklist != NULL); + abort(); + } + + if (kcp->acklist != NULL) + { + IUINT32 x; + for (x = 0; x < kcp->ackcount; x++) + { + acklist[x * 2 + 0] = kcp->acklist[x * 2 + 0]; + acklist[x * 2 + 1] = kcp->acklist[x * 2 + 1]; + } + ikcp_free(kcp->acklist); + } + + kcp->acklist = acklist; + kcp->ackblock = newblock; + } + + ptr = &kcp->acklist[kcp->ackcount * 2]; + ptr[0] = sn; + ptr[1] = ts; + kcp->ackcount++; +} + +static void ikcp_ack_get(const ikcpcb *kcp, int p, IUINT32 *sn, IUINT32 *ts) +{ + if (sn) + sn[0] = kcp->acklist[p * 2 + 0]; + if (ts) + ts[0] = kcp->acklist[p * 2 + 1]; +} + +//--------------------------------------------------------------------- +// parse data +//--------------------------------------------------------------------- +void ikcp_parse_data(ikcpcb *kcp, IKCPSEG *newseg) +{ + struct IQUEUEHEAD *p, *prev; + IUINT32 sn = newseg->sn; + int repeat = 0; + + if (_itimediff(sn, kcp->rcv_nxt + kcp->rcv_wnd) >= 0 || + _itimediff(sn, kcp->rcv_nxt) < 0) + { + ikcp_segment_delete(kcp, newseg); + return; + } + + for (p = kcp->rcv_buf.prev; p != &kcp->rcv_buf; p = prev) + { + IKCPSEG *seg = iqueue_entry(p, IKCPSEG, node); + prev = p->prev; + if (seg->sn == sn) + { + repeat = 1; + break; + } + if (_itimediff(sn, seg->sn) > 0) + { + break; + } + } + + if (repeat == 0) + { + iqueue_init(&newseg->node); + iqueue_add(&newseg->node, p); + kcp->nrcv_buf++; + } + else + { + kcp->duplicate_recv_total++; + ikcp_segment_delete(kcp, newseg); + } + +#if 0 + ikcp_qprint("rcvbuf", &kcp->rcv_buf); + printf("rcv_nxt=%lu\n", kcp->rcv_nxt); +#endif + + // move available data from rcv_buf -> rcv_queue + while (!iqueue_is_empty(&kcp->rcv_buf)) + { + IKCPSEG *seg = iqueue_entry(kcp->rcv_buf.next, IKCPSEG, node); + if (seg->sn == kcp->rcv_nxt && kcp->nrcv_que < kcp->rcv_wnd) + { + iqueue_del(&seg->node); + kcp->nrcv_buf--; + iqueue_add_tail(&seg->node, &kcp->rcv_queue); + kcp->nrcv_que++; + kcp->rcv_nxt++; + } + else + { + break; + } + } + +#if 0 + ikcp_qprint("queue", &kcp->rcv_queue); + printf("rcv_nxt=%lu\n", kcp->rcv_nxt); +#endif + +#if 1 +// printf("snd(buf=%d, queue=%d)\n", kcp->nsnd_buf, kcp->nsnd_que); +// printf("rcv(buf=%d, queue=%d)\n", kcp->nrcv_buf, kcp->nrcv_que); +#endif +} + +//--------------------------------------------------------------------- +// input data +//--------------------------------------------------------------------- +int ikcp_input(ikcpcb *kcp, const char *data, long size) +{ + IUINT32 prev_una = kcp->snd_una; + IUINT32 maxack = 0, latest_ts = 0; + int flag = 0; + + if (ikcp_canlog(kcp, IKCP_LOG_INPUT)) + { + ikcp_log(kcp, IKCP_LOG_INPUT, "[RI] %d bytes", (int)size); + } + + if (data == NULL || (int)size < (int)IKCP_OVERHEAD) + return -1; + + while (1) + { + IUINT32 ts, sn, len, una, conv; + IUINT16 wnd; + IUINT8 cmd, frg; + IKCPSEG *seg; + + if (size < (int)IKCP_OVERHEAD) + break; + + data = ikcp_decode32u(data, &conv); + if (conv != kcp->conv) + return -1; + + data = ikcp_decode8u(data, &cmd); + data = ikcp_decode8u(data, &frg); + data = ikcp_decode16u(data, &wnd); + data = ikcp_decode32u(data, &ts); + data = ikcp_decode32u(data, &sn); + data = ikcp_decode32u(data, &una); + data = ikcp_decode32u(data, &len); + + size -= IKCP_OVERHEAD; + + if ((long)size < (long)len || (int)len < 0) + return -2; + + if (cmd != IKCP_CMD_PUSH && cmd != IKCP_CMD_ACK && + cmd != IKCP_CMD_WASK && cmd != IKCP_CMD_WINS) + return -3; + + kcp->rmt_wnd = wnd; + ikcp_parse_una(kcp, una); + ikcp_shrink_buf(kcp); + + if (cmd == IKCP_CMD_ACK) + { + if (_itimediff(kcp->current, ts) >= 0) + { + ikcp_update_ack(kcp, _itimediff(kcp->current, ts)); + } + ikcp_parse_ack(kcp, sn); + ikcp_shrink_buf(kcp); + if (flag == 0) + { + flag = 1; + maxack = sn; + latest_ts = ts; + } + else + { + if (_itimediff(sn, maxack) > 0) + { +#ifndef IKCP_FASTACK_CONSERVE + maxack = sn; + latest_ts = ts; +#else + if (_itimediff(ts, latest_ts) > 0) + { + maxack = sn; + latest_ts = ts; + } +#endif + } + } + if (ikcp_canlog(kcp, IKCP_LOG_IN_ACK)) + { + ikcp_log(kcp, IKCP_LOG_IN_ACK, + "input ack: sn=%lu rtt=%ld rto=%ld", (unsigned long)sn, + (long)_itimediff(kcp->current, ts), + (long)kcp->rx_rto); + } + } + else if (cmd == IKCP_CMD_PUSH) + { + if (ikcp_canlog(kcp, IKCP_LOG_IN_DATA)) + { + ikcp_log(kcp, IKCP_LOG_IN_DATA, + "input psh: sn=%lu ts=%lu", (unsigned long)sn, (unsigned long)ts); + } + if (_itimediff(sn, kcp->rcv_nxt + kcp->rcv_wnd) < 0) + { + ikcp_ack_push(kcp, sn, ts); + if (_itimediff(sn, kcp->rcv_nxt) >= 0) + { + seg = ikcp_segment_new(kcp, len); + seg->conv = conv; + seg->cmd = cmd; + seg->frg = frg; + seg->wnd = wnd; + seg->ts = ts; + seg->sn = sn; + seg->una = una; + seg->len = len; + + if (len > 0) + { + memcpy(seg->data, data, len); + } + + ikcp_parse_data(kcp, seg); + } + } + } + else if (cmd == IKCP_CMD_WASK) + { + // ready to send back IKCP_CMD_WINS in ikcp_flush + // tell remote my window size + kcp->probe |= IKCP_ASK_TELL; + if (ikcp_canlog(kcp, IKCP_LOG_IN_PROBE)) + { + ikcp_log(kcp, IKCP_LOG_IN_PROBE, "input probe"); + } + } + else if (cmd == IKCP_CMD_WINS) + { + // do nothing + if (ikcp_canlog(kcp, IKCP_LOG_IN_WINS)) + { + ikcp_log(kcp, IKCP_LOG_IN_WINS, + "input wins: %lu", (unsigned long)(wnd)); + } + } + else + { + return -3; + } + + data += len; + size -= len; + } + + if (flag != 0) + { + ikcp_parse_fastack(kcp, maxack, latest_ts); + } + + if (_itimediff(kcp->snd_una, prev_una) > 0) + { + if (kcp->cwnd < kcp->rmt_wnd) + { + IUINT32 mss = kcp->mss; + if (kcp->cwnd < kcp->ssthresh) + { + kcp->cwnd++; + kcp->incr += mss; + } + else + { + if (kcp->incr < mss) + kcp->incr = mss; + kcp->incr += (mss * mss) / kcp->incr + (mss / 16); + if ((kcp->cwnd + 1) * mss <= kcp->incr) + { +#if 1 + kcp->cwnd = (kcp->incr + mss - 1) / ((mss > 0) ? mss : 1); +#else + kcp->cwnd++; +#endif + } + } + if (kcp->cwnd > kcp->rmt_wnd) + { + kcp->cwnd = kcp->rmt_wnd; + kcp->incr = kcp->rmt_wnd * mss; + } + } + } + + return 0; +} + +//--------------------------------------------------------------------- +// ikcp_encode_seg +//--------------------------------------------------------------------- +static char *ikcp_encode_seg(char *ptr, const IKCPSEG *seg) +{ + ptr = ikcp_encode32u(ptr, seg->conv); + ptr = ikcp_encode8u(ptr, (IUINT8)seg->cmd); + ptr = ikcp_encode8u(ptr, (IUINT8)seg->frg); + ptr = ikcp_encode16u(ptr, (IUINT16)seg->wnd); + ptr = ikcp_encode32u(ptr, seg->ts); + ptr = ikcp_encode32u(ptr, seg->sn); + ptr = ikcp_encode32u(ptr, seg->una); + ptr = ikcp_encode32u(ptr, seg->len); + return ptr; +} + +static int ikcp_wnd_unused(const ikcpcb *kcp) +{ + if (kcp->nrcv_que < kcp->rcv_wnd) + { + return kcp->rcv_wnd - kcp->nrcv_que; + } + return 0; +} + +//--------------------------------------------------------------------- +// ikcp_flush +//--------------------------------------------------------------------- +void ikcp_flush(ikcpcb *kcp) +{ + IUINT32 current = kcp->current; + char *buffer = kcp->buffer; + char *ptr = buffer; + int count, size, i; + IUINT32 resent, cwnd; + IUINT32 rtomin; + struct IQUEUEHEAD *p; + int change = 0; + int lost = 0; + IKCPSEG seg; + + // 'ikcp_update' haven't been called. + if (kcp->updated == 0) + return; + + seg.conv = kcp->conv; + seg.cmd = IKCP_CMD_ACK; + seg.frg = 0; + seg.wnd = ikcp_wnd_unused(kcp); + seg.una = kcp->rcv_nxt; + seg.len = 0; + seg.sn = 0; + seg.ts = 0; + + // flush acknowledges + count = kcp->ackcount; + for (i = 0; i < count; i++) + { + size = (int)(ptr - buffer); + if (size + (int)IKCP_OVERHEAD > (int)kcp->mtu) + { + ikcp_output(kcp, buffer, size); + ptr = buffer; + } + ikcp_ack_get(kcp, i, &seg.sn, &seg.ts); + ptr = ikcp_encode_seg(ptr, &seg); + } + + kcp->ackcount = 0; + + // probe window size (if remote window size equals zero) + if (kcp->rmt_wnd == 0) + { + if (kcp->probe_wait == 0) + { + kcp->probe_wait = IKCP_PROBE_INIT; + kcp->ts_probe = kcp->current + kcp->probe_wait; + } + else + { + if (_itimediff(kcp->current, kcp->ts_probe) >= 0) + { + if (kcp->probe_wait < IKCP_PROBE_INIT) + kcp->probe_wait = IKCP_PROBE_INIT; + kcp->probe_wait += kcp->probe_wait / 2; + if (kcp->probe_wait > IKCP_PROBE_LIMIT) + kcp->probe_wait = IKCP_PROBE_LIMIT; + kcp->ts_probe = kcp->current + kcp->probe_wait; + kcp->probe |= IKCP_ASK_SEND; + } + } + } + else + { + kcp->ts_probe = 0; + kcp->probe_wait = 0; + } + + // flush window probing commands + if (kcp->probe & IKCP_ASK_SEND) + { + seg.cmd = IKCP_CMD_WASK; + size = (int)(ptr - buffer); + if (size + (int)IKCP_OVERHEAD > (int)kcp->mtu) + { + ikcp_output(kcp, buffer, size); + ptr = buffer; + } + ptr = ikcp_encode_seg(ptr, &seg); + } + + // flush window probing commands + if (kcp->probe & IKCP_ASK_TELL) + { + seg.cmd = IKCP_CMD_WINS; + size = (int)(ptr - buffer); + if (size + (int)IKCP_OVERHEAD > (int)kcp->mtu) + { + ikcp_output(kcp, buffer, size); + ptr = buffer; + } + ptr = ikcp_encode_seg(ptr, &seg); + } + + kcp->probe = 0; + + // calculate window size + cwnd = _imin_(kcp->snd_wnd, kcp->rmt_wnd); + if (kcp->nocwnd == 0) + cwnd = _imin_(kcp->cwnd, cwnd); + + // move data from snd_queue to snd_buf + while (_itimediff(kcp->snd_nxt, kcp->snd_una + cwnd) < 0) + { + IKCPSEG *newseg; + if (iqueue_is_empty(&kcp->snd_queue)) + break; + + newseg = iqueue_entry(kcp->snd_queue.next, IKCPSEG, node); + + iqueue_del(&newseg->node); + iqueue_add_tail(&newseg->node, &kcp->snd_buf); + kcp->nsnd_que--; + kcp->nsnd_buf++; + + newseg->conv = kcp->conv; + newseg->cmd = IKCP_CMD_PUSH; + newseg->wnd = seg.wnd; + newseg->ts = current; + newseg->sn = kcp->snd_nxt++; + newseg->una = kcp->rcv_nxt; + newseg->resendts = current; + newseg->rto = kcp->rx_rto; + newseg->fastack = 0; + newseg->xmit = 0; + } + + // calculate resent + resent = (kcp->fastresend > 0) ? (IUINT32)kcp->fastresend : 0xffffffff; + rtomin = (kcp->nodelay == 0) ? (kcp->rx_rto >> 3) : 0; + + // flush data segments + for (p = kcp->snd_buf.next; p != &kcp->snd_buf; p = p->next) + { + IKCPSEG *segment = iqueue_entry(p, IKCPSEG, node); + int needsend = 0; + if (segment->xmit == 0) + { + needsend = 1; + segment->xmit++; + segment->rto = kcp->rx_rto; + segment->resendts = current + segment->rto + rtomin; + } + else if (_itimediff(current, segment->resendts) >= 0) + { + needsend = 1; + segment->xmit++; + kcp->xmit++; + kcp->timeout_retrans_total++; + if (kcp->nodelay == 0) + { + segment->rto += _imax_(segment->rto, (IUINT32)kcp->rx_rto); + } + else + { + IINT32 step = (kcp->nodelay < 2) ? ((IINT32)(segment->rto)) : kcp->rx_rto; + segment->rto += step / 2; + } + segment->resendts = current + segment->rto; + lost = 1; + } + else if (segment->fastack >= resent) + { + if ((int)segment->xmit <= kcp->fastlimit || + kcp->fastlimit <= 0) + { + needsend = 1; + segment->xmit++; + kcp->fast_retrans_total++; + segment->fastack = 0; + segment->resendts = current + segment->rto; + change++; + } + } + + if (needsend) + { + int need; + segment->ts = current; + segment->wnd = seg.wnd; + segment->una = kcp->rcv_nxt; + + size = (int)(ptr - buffer); + need = IKCP_OVERHEAD + segment->len; + + if (size + need > (int)kcp->mtu) + { + ikcp_output(kcp, buffer, size); + ptr = buffer; + } + + ptr = ikcp_encode_seg(ptr, segment); + + if (segment->len > 0) + { + memcpy(ptr, segment->data, segment->len); + ptr += segment->len; + } + + if (segment->xmit >= kcp->dead_link) + { + kcp->state = (IUINT32)-1; + } + } + } + + // flash remain segments + size = (int)(ptr - buffer); + if (size > 0) + { + ikcp_output(kcp, buffer, size); + } + + // update ssthresh + if (change) + { + IUINT32 inflight = kcp->snd_nxt - kcp->snd_una; + kcp->ssthresh = inflight / 2; + if (kcp->ssthresh < IKCP_THRESH_MIN) + kcp->ssthresh = IKCP_THRESH_MIN; + kcp->cwnd = kcp->ssthresh + resent; + kcp->incr = kcp->cwnd * kcp->mss; + } + + if (lost) + { + kcp->ssthresh = cwnd / 2; + if (kcp->ssthresh < IKCP_THRESH_MIN) + kcp->ssthresh = IKCP_THRESH_MIN; + kcp->cwnd = 1; + kcp->incr = kcp->mss; + } + + if (kcp->cwnd < 1) + { + kcp->cwnd = 1; + kcp->incr = kcp->mss; + } +} + +//--------------------------------------------------------------------- +// update state (call it repeatedly, every 10ms-100ms), or you can ask +// ikcp_check when to call it again (without ikcp_input/_send calling). +// 'current' - current timestamp in millisec. +//--------------------------------------------------------------------- +void ikcp_update(ikcpcb *kcp, IUINT32 current) +{ + IINT32 slap; + + kcp->current = current; + + if (kcp->updated == 0) + { + kcp->updated = 1; + kcp->ts_flush = kcp->current; + } + + slap = _itimediff(kcp->current, kcp->ts_flush); + + if (slap >= 10000 || slap < -10000) + { + kcp->ts_flush = kcp->current; + slap = 0; + } + + if (slap >= 0) + { + kcp->ts_flush += kcp->interval; + if (_itimediff(kcp->current, kcp->ts_flush) >= 0) + { + kcp->ts_flush = kcp->current + kcp->interval; + } + ikcp_flush(kcp); + } +} + +//--------------------------------------------------------------------- +// Determine when should you invoke ikcp_update: +// returns when you should invoke ikcp_update in millisec, if there +// is no ikcp_input/_send calling. you can call ikcp_update in that +// time, instead of call update repeatly. +// Important to reduce unnacessary ikcp_update invoking. use it to +// schedule ikcp_update (eg. implementing an epoll-like mechanism, +// or optimize ikcp_update when handling massive kcp connections) +//--------------------------------------------------------------------- +IUINT32 ikcp_check(const ikcpcb *kcp, IUINT32 current) +{ + IUINT32 ts_flush = kcp->ts_flush; + IINT32 tm_flush = 0x7fffffff; + IINT32 tm_packet = 0x7fffffff; + IUINT32 minimal = 0; + struct IQUEUEHEAD *p; + + if (kcp->updated == 0) + { + return current; + } + + if (_itimediff(current, ts_flush) >= 10000 || + _itimediff(current, ts_flush) < -10000) + { + ts_flush = current; + } + + if (_itimediff(current, ts_flush) >= 0) + { + return current; + } + + tm_flush = _itimediff(ts_flush, current); + + for (p = kcp->snd_buf.next; p != &kcp->snd_buf; p = p->next) + { + const IKCPSEG *seg = iqueue_entry(p, const IKCPSEG, node); + IINT32 diff = _itimediff(seg->resendts, current); + if (diff <= 0) + { + return current; + } + if (diff < tm_packet) + tm_packet = diff; + } + + minimal = (IUINT32)(tm_packet < tm_flush ? tm_packet : tm_flush); + if (minimal >= kcp->interval) + minimal = kcp->interval; + + return current + minimal; +} + +int ikcp_setmtu(ikcpcb *kcp, int mtu) +{ + char *buffer; + if (mtu < 50 || mtu < (int)IKCP_OVERHEAD) + return -1; + buffer = (char *)ikcp_malloc((mtu + IKCP_OVERHEAD) * 3); + if (buffer == NULL) + return -2; + kcp->mtu = mtu; + kcp->mss = kcp->mtu - IKCP_OVERHEAD; + ikcp_free(kcp->buffer); + kcp->buffer = buffer; + return 0; +} + +int ikcp_interval(ikcpcb *kcp, int interval) +{ + if (interval > 5000) + interval = 5000; + else if (interval < 10) + interval = 10; + kcp->interval = interval; + return 0; +} + +int ikcp_nodelay(ikcpcb *kcp, int nodelay, int interval, int resend, int nc) +{ + if (nodelay >= 0) + { + kcp->nodelay = nodelay; + if (nodelay) + { + kcp->rx_minrto = IKCP_RTO_NDL; + } + else + { + kcp->rx_minrto = IKCP_RTO_MIN; + } + } + if (interval >= 0) + { + if (interval > 5000) + interval = 5000; + else if (interval < 10) + interval = 10; + kcp->interval = interval; + } + if (resend >= 0) + { + kcp->fastresend = resend; + } + if (nc >= 0) + { + kcp->nocwnd = nc; + } + return 0; +} + +int ikcp_wndsize(ikcpcb *kcp, int sndwnd, int rcvwnd) +{ + if (kcp) + { + if (sndwnd > 0) + { + kcp->snd_wnd = sndwnd; + } + if (rcvwnd > 0) + { // must >= max fragment size + kcp->rcv_wnd = _imax_(rcvwnd, IKCP_WND_RCV); + } + } + return 0; +} + +int ikcp_waitsnd(const ikcpcb *kcp) +{ + return kcp->nsnd_buf + kcp->nsnd_que; +} + +// read conv +IUINT32 ikcp_getconv(const void *ptr) +{ + IUINT32 conv; + ikcp_decode32u((const char *)ptr, &conv); + return conv; +} diff --git a/host/OmniSocketGo_add_camera/third_party/kcp/ikcp.h b/host/OmniSocketGo_add_camera/third_party/kcp/ikcp.h new file mode 100644 index 0000000..54106f2 --- /dev/null +++ b/host/OmniSocketGo_add_camera/third_party/kcp/ikcp.h @@ -0,0 +1,421 @@ +//===================================================================== +// +// KCP - A Better ARQ Protocol Implementation +// skywind3000 (at) gmail.com, 2010-2011 +// +// Features: +// + Average RTT reduce 30% - 40% vs traditional ARQ like tcp. +// + Maximum RTT reduce three times vs tcp. +// + Lightweight, distributed as a single source file. +// +//===================================================================== +#ifndef __IKCP_H__ +#define __IKCP_H__ + +#include +#include +#include + +//===================================================================== +// 32BIT INTEGER DEFINITION +//===================================================================== +#ifndef __INTEGER_32_BITS__ +#define __INTEGER_32_BITS__ +#if defined(_WIN64) || defined(WIN64) || defined(__amd64__) || \ + defined(__x86_64) || defined(__x86_64__) || defined(_M_IA64) || \ + defined(_M_AMD64) +typedef unsigned int ISTDUINT32; +typedef int ISTDINT32; +#elif defined(_WIN32) || defined(WIN32) || defined(__i386__) || \ + defined(__i386) || defined(_M_X86) +typedef unsigned long ISTDUINT32; +typedef long ISTDINT32; +#elif defined(__MACOS__) +typedef UInt32 ISTDUINT32; +typedef SInt32 ISTDINT32; +#elif defined(__APPLE__) && defined(__MACH__) +#include +typedef u_int32_t ISTDUINT32; +typedef int32_t ISTDINT32; +#elif defined(__BEOS__) +#include +typedef u_int32_t ISTDUINT32; +typedef int32_t ISTDINT32; +#elif (defined(_MSC_VER) || defined(__BORLANDC__)) && (!defined(__MSDOS__)) +typedef unsigned __int32 ISTDUINT32; +typedef __int32 ISTDINT32; +#elif defined(__GNUC__) +#include +typedef uint32_t ISTDUINT32; +typedef int32_t ISTDINT32; +#else +typedef unsigned long ISTDUINT32; +typedef long ISTDINT32; +#endif +#endif + +//===================================================================== +// Integer Definition +//===================================================================== +#ifndef __IINT8_DEFINED +#define __IINT8_DEFINED +typedef char IINT8; +#endif + +#ifndef __IUINT8_DEFINED +#define __IUINT8_DEFINED +typedef unsigned char IUINT8; +#endif + +#ifndef __IUINT16_DEFINED +#define __IUINT16_DEFINED +typedef unsigned short IUINT16; +#endif + +#ifndef __IINT16_DEFINED +#define __IINT16_DEFINED +typedef short IINT16; +#endif + +#ifndef __IINT32_DEFINED +#define __IINT32_DEFINED +typedef ISTDINT32 IINT32; +#endif + +#ifndef __IUINT32_DEFINED +#define __IUINT32_DEFINED +typedef ISTDUINT32 IUINT32; +#endif + +#ifndef __IINT64_DEFINED +#define __IINT64_DEFINED +#if defined(_MSC_VER) || defined(__BORLANDC__) +typedef __int64 IINT64; +#else +typedef long long IINT64; +#endif +#endif + +#ifndef __IUINT64_DEFINED +#define __IUINT64_DEFINED +#if defined(_MSC_VER) || defined(__BORLANDC__) +typedef unsigned __int64 IUINT64; +#else +typedef unsigned long long IUINT64; +#endif +#endif + +#ifndef INLINE +#if defined(__GNUC__) + +#if (__GNUC__ > 3) || ((__GNUC__ == 3) && (__GNUC_MINOR__ >= 1)) +#define INLINE __inline__ __attribute__((always_inline)) +#else +#define INLINE __inline__ +#endif + +#elif (defined(_MSC_VER) || defined(__BORLANDC__) || defined(__WATCOMC__)) +#define INLINE __inline +#else +#define INLINE +#endif +#endif + +#if (!defined(__cplusplus)) && (!defined(inline)) +#define inline INLINE +#endif + +//===================================================================== +// QUEUE DEFINITION +//===================================================================== +#ifndef __IQUEUE_DEF__ +#define __IQUEUE_DEF__ + +struct IQUEUEHEAD +{ + struct IQUEUEHEAD *next, *prev; +}; + +typedef struct IQUEUEHEAD iqueue_head; + +//--------------------------------------------------------------------- +// queue init +//--------------------------------------------------------------------- +#define IQUEUE_HEAD_INIT(name) {&(name), &(name)} +#define IQUEUE_HEAD(name) \ + struct IQUEUEHEAD name = IQUEUE_HEAD_INIT(name) + +#define IQUEUE_INIT(ptr) ( \ + (ptr)->next = (ptr), (ptr)->prev = (ptr)) + +#define IOFFSETOF(TYPE, MEMBER) ((size_t)&((TYPE *)0)->MEMBER) + +#define ICONTAINEROF(ptr, type, member) ( \ + (type *)(((char *)((type *)ptr)) - IOFFSETOF(type, member))) + +#define IQUEUE_ENTRY(ptr, type, member) ICONTAINEROF(ptr, type, member) + +//--------------------------------------------------------------------- +// queue operation +//--------------------------------------------------------------------- +#define IQUEUE_ADD(node, head) ( \ + (node)->prev = (head), (node)->next = (head)->next, \ + (head)->next->prev = (node), (head)->next = (node)) + +#define IQUEUE_ADD_TAIL(node, head) ( \ + (node)->prev = (head)->prev, (node)->next = (head), \ + (head)->prev->next = (node), (head)->prev = (node)) + +#define IQUEUE_DEL_BETWEEN(p, n) ((n)->prev = (p), (p)->next = (n)) + +#define IQUEUE_DEL(entry) ( \ + (entry)->next->prev = (entry)->prev, \ + (entry)->prev->next = (entry)->next, \ + (entry)->next = 0, (entry)->prev = 0) + +#define IQUEUE_DEL_INIT(entry) \ + do \ + { \ + IQUEUE_DEL(entry); \ + IQUEUE_INIT(entry); \ + } while (0) + +#define IQUEUE_IS_EMPTY(entry) ((entry) == (entry)->next) + +#define iqueue_init IQUEUE_INIT +#define iqueue_entry IQUEUE_ENTRY +#define iqueue_add IQUEUE_ADD +#define iqueue_add_tail IQUEUE_ADD_TAIL +#define iqueue_del IQUEUE_DEL +#define iqueue_del_init IQUEUE_DEL_INIT +#define iqueue_is_empty IQUEUE_IS_EMPTY + +#define IQUEUE_FOREACH(iterator, head, TYPE, MEMBER) \ + for ((iterator) = iqueue_entry((head)->next, TYPE, MEMBER); \ + &((iterator)->MEMBER) != (head); \ + (iterator) = iqueue_entry((iterator)->MEMBER.next, TYPE, MEMBER)) + +#define iqueue_foreach(iterator, head, TYPE, MEMBER) \ + IQUEUE_FOREACH(iterator, head, TYPE, MEMBER) + +#define iqueue_foreach_entry(pos, head) \ + for ((pos) = (head)->next; (pos) != (head); (pos) = (pos)->next) + +#define __iqueue_splice(list, head) \ + do \ + { \ + iqueue_head *first = (list)->next, *last = (list)->prev; \ + iqueue_head *at = (head)->next; \ + (first)->prev = (head), (head)->next = (first); \ + (last)->next = (at), (at)->prev = (last); \ + } while (0) + +#define iqueue_splice(list, head) \ + do \ + { \ + if (!iqueue_is_empty(list)) \ + __iqueue_splice(list, head); \ + } while (0) + +#define iqueue_splice_init(list, head) \ + do \ + { \ + iqueue_splice(list, head); \ + iqueue_init(list); \ + } while (0) + +#ifdef _MSC_VER +#pragma warning(disable : 4311) +#pragma warning(disable : 4312) +#pragma warning(disable : 4996) +#endif + +#endif + +//--------------------------------------------------------------------- +// BYTE ORDER & ALIGNMENT +//--------------------------------------------------------------------- +#ifndef IWORDS_BIG_ENDIAN +#ifdef _BIG_ENDIAN_ +#if _BIG_ENDIAN_ +#define IWORDS_BIG_ENDIAN 1 +#endif +#endif +#ifndef IWORDS_BIG_ENDIAN +#if defined(__hppa__) || \ + defined(__m68k__) || defined(mc68000) || defined(_M_M68K) || \ + (defined(__MIPS__) && defined(__MIPSEB__)) || \ + defined(__ppc__) || defined(__POWERPC__) || defined(_M_PPC) || \ + defined(__sparc__) || defined(__powerpc__) || \ + defined(__mc68000__) || defined(__s390x__) || defined(__s390__) +#define IWORDS_BIG_ENDIAN 1 +#endif +#endif +#ifndef IWORDS_BIG_ENDIAN +#define IWORDS_BIG_ENDIAN 0 +#endif +#endif + +#ifndef IWORDS_MUST_ALIGN +#if defined(__i386__) || defined(__i386) || defined(_i386_) +#define IWORDS_MUST_ALIGN 0 +#elif defined(_M_IX86) || defined(_X86_) || defined(__x86_64__) +#define IWORDS_MUST_ALIGN 0 +#elif defined(__amd64) || defined(__amd64__) +#define IWORDS_MUST_ALIGN 0 +#else +#define IWORDS_MUST_ALIGN 1 +#endif +#endif + +//===================================================================== +// SEGMENT +//===================================================================== +struct IKCPSEG +{ + struct IQUEUEHEAD node; + IUINT32 conv; + IUINT32 cmd; + IUINT32 frg; + IUINT32 wnd; + IUINT32 ts; + IUINT32 sn; + IUINT32 una; + IUINT32 len; + IUINT32 resendts; + IUINT32 rto; + IUINT32 fastack; + IUINT32 xmit; + char data[1]; +}; + +//--------------------------------------------------------------------- +// IKCPCB +//--------------------------------------------------------------------- +struct IKCPCB +{ + IUINT32 conv, mtu, mss, state; + IUINT32 snd_una, snd_nxt, rcv_nxt; + IUINT32 ts_recent, ts_lastack, ssthresh; + 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 nrcv_buf, nsnd_buf; + IUINT32 nrcv_que, nsnd_que; + IUINT32 nodelay, updated; + IUINT32 ts_probe, probe_wait; + IUINT32 dead_link, incr; + struct IQUEUEHEAD snd_queue; + struct IQUEUEHEAD rcv_queue; + struct IQUEUEHEAD snd_buf; + struct IQUEUEHEAD rcv_buf; + IUINT32 *acklist; + IUINT32 ackcount; + IUINT32 ackblock; + IUINT64 timeout_retrans_total; + IUINT64 fast_retrans_total; + IUINT64 duplicate_recv_total; + void *user; + char *buffer; + int fastresend; + int fastlimit; + int nocwnd, stream; + int logmask; + int (*output)(const char *buf, int len, struct IKCPCB *kcp, void *user); + void (*writelog)(const char *log, struct IKCPCB *kcp, void *user); +}; + +typedef struct IKCPCB ikcpcb; + +#define IKCP_LOG_OUTPUT 1 +#define IKCP_LOG_INPUT 2 +#define IKCP_LOG_SEND 4 +#define IKCP_LOG_RECV 8 +#define IKCP_LOG_IN_DATA 16 +#define IKCP_LOG_IN_ACK 32 +#define IKCP_LOG_IN_PROBE 64 +#define IKCP_LOG_IN_WINS 128 +#define IKCP_LOG_OUT_DATA 256 +#define IKCP_LOG_OUT_ACK 512 +#define IKCP_LOG_OUT_PROBE 1024 +#define IKCP_LOG_OUT_WINS 2048 + +#ifdef __cplusplus +extern "C" +{ +#endif + + //--------------------------------------------------------------------- + // interface + //--------------------------------------------------------------------- + + // create a new kcp control object, 'conv' must equal in two endpoint + // from the same connection. 'user' will be passed to the output callback + // output callback can be setup like this: 'kcp->output = my_udp_output' + ikcpcb *ikcp_create(IUINT32 conv, void *user); + + // release kcp control object + void ikcp_release(ikcpcb *kcp); + + // set output callback, which will be invoked by kcp + void ikcp_setoutput(ikcpcb *kcp, int (*output)(const char *buf, int len, + ikcpcb *kcp, void *user)); + + // user/upper level recv: returns size, returns below zero for EAGAIN + int ikcp_recv(ikcpcb *kcp, char *buffer, int len); + + // user/upper level send, returns below zero for error + int ikcp_send(ikcpcb *kcp, const char *buffer, int len); + + // update state (call it repeatedly, every 10ms-100ms), or you can ask + // ikcp_check when to call it again (without ikcp_input/_send calling). + // 'current' - current timestamp in millisec. + void ikcp_update(ikcpcb *kcp, IUINT32 current); + + // Determine when should you invoke ikcp_update: + // returns when you should invoke ikcp_update in millisec, if there + // is no ikcp_input/_send calling. you can call ikcp_update in that + // time, instead of call update repeatly. + // Important to reduce unnacessary ikcp_update invoking. use it to + // schedule ikcp_update (eg. implementing an epoll-like mechanism, + // or optimize ikcp_update when handling massive kcp connections) + IUINT32 ikcp_check(const ikcpcb *kcp, IUINT32 current); + + // when you received a low level packet (eg. UDP packet), call it + int ikcp_input(ikcpcb *kcp, const char *data, long size); + + // flush pending data + void ikcp_flush(ikcpcb *kcp); + + // check the size of next message in the recv queue + int ikcp_peeksize(const ikcpcb *kcp); + + // change MTU size, default is 1400 + int ikcp_setmtu(ikcpcb *kcp, int mtu); + + // set maximum window size: sndwnd=32, rcvwnd=32 by default + int ikcp_wndsize(ikcpcb *kcp, int sndwnd, int rcvwnd); + + // get how many packet is waiting to be sent + int ikcp_waitsnd(const ikcpcb *kcp); + + // fastest: ikcp_nodelay(kcp, 1, 20, 2, 1) + // nodelay: 0:disable(default), 1:enable + // interval: internal update timer interval in millisec, default is 100ms + // resend: 0:disable fast resend(default), 1:enable fast resend + // nc: 0:normal congestion control(default), 1:disable congestion control + int ikcp_nodelay(ikcpcb *kcp, int nodelay, int interval, int resend, int nc); + + void ikcp_log(ikcpcb *kcp, int mask, const char *fmt, ...); + + // setup allocator + void ikcp_allocator(void *(*new_malloc)(size_t), void (*new_free)(void *)); + + // read conv + IUINT32 ikcp_getconv(const void *ptr); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/host/README.md b/host/README.md new file mode 100644 index 0000000..1c13ed0 --- /dev/null +++ b/host/README.md @@ -0,0 +1,13 @@ +# Control-host projects + +The host side is intentionally split into two sibling projects: + +- `OmniSocketGo_add_camera`: C/KCP hub and peer transport, Python bindings, + ROS-control helpers, and startup scripts. +- `robot-command-center`: Django backend plus Vue frontend for video status, + camera switching, and control UI. + +Install/build instructions are in `README_PACKAGE.md`. The host-side package +expects the two project directories to remain siblings after installation. +Do not copy robot binaries to the host; build the C programs on the host +architecture. diff --git a/host/README_PACKAGE.md b/host/README_PACKAGE.md new file mode 100644 index 0000000..0d0e228 --- /dev/null +++ b/host/README_PACKAGE.md @@ -0,0 +1,77 @@ +# OmniSocketGo control-host package + +This archive contains both projects required by the operator computer: + +```text +OmniSocketGo_add_camera/ KCP hub, native/Python transport and launch scripts +robot-command-center/ Django backend and Vue frontend +``` + +It does not contain the robot-side package. + +## Directory placement + +Keep the two extracted projects as sibling directories: + +```text +/ +├── OmniSocketGo_add_camera/ +└── robot-command-center/ +``` + +The supplied host configuration currently points `ROBOT_COMMAND_CENTER_ROOT` +to `/home/ps/Desktop/robot-command-center`. Update +`OmniSocketGo_add_camera/scripts/dev/robot-remote.env.local` if the target host +uses a different path. + +## Install backend and transport dependencies + +```bash +cd OmniSocketGo_add_camera +python3 -m venv .venv +.venv/bin/python -m pip install -r ../requirements.txt +make python-ext PYTHON="$(pwd)/.venv/bin/python" +.venv/bin/python -m pip install --no-build-isolation -e python +make bin/kcpserver +``` + +Alternatively, after confirming the sibling project path: + +```bash +cd OmniSocketGo_add_camera +bash scripts/dev/setup-control-side.sh +``` + +## Install frontend dependencies + +```bash +cd robot-command-center/frontend +npm ci +npm run build +``` + +## Start direct-LAN mode + +Use three terminals from `OmniSocketGo_add_camera`: + +```bash +bash scripts/dev/start-local-hub.sh +``` + +```bash +bash scripts/dev/start-backend.sh +``` + +```bash +bash scripts/dev/start-frontend.sh +``` + +Open `http://127.0.0.1:5173` and verify: + +```bash +curl -s http://127.0.0.1:8001/api/video/status/ | python3 -m json.tool +``` + +For single-public-hub mode, do not start the local hub. Point both the host and +robot `SERVER_ADDR` values at the same public KCP Hub and keep all `relay_via` +values empty. See `OmniSocketGo_add_camera/NETWORK_LINK_STARTUP_GUIDE.md`. diff --git a/host/requirements.txt b/host/requirements.txt new file mode 100644 index 0000000..4562b7e --- /dev/null +++ b/host/requirements.txt @@ -0,0 +1,22 @@ +# OmniSocketGo control-host Python dependencies +Django>=5.2,<6.0 +djangorestframework>=3.17,<4 +django-cors-headers>=4,<5 +channels>=4,<5 +uvicorn>=0.52,<1 +PyYAML>=6,<7 +setuptools>=80 + +# Ubuntu/Debian system dependencies are not pip packages. +# Install them with: +# sudo apt-get update +# sudo apt-get install -y \ +# build-essential pkg-config \ +# python3 python3-dev python3-venv python3-pip \ +# nodejs npm curl +# +# Frontend dependencies are pinned separately by: +# robot-command-center/frontend/package-lock.json +# +# Required Node.js version: +# Node.js ^20.19.0 or >=22.12.0 diff --git a/host/robot-command-center/.gitignore b/host/robot-command-center/.gitignore new file mode 100644 index 0000000..50b3531 --- /dev/null +++ b/host/robot-command-center/.gitignore @@ -0,0 +1,67 @@ +# OS / editor +.DS_Store +Thumbs.db +.idea/ +.vscode/ + +# Python / Django +__pycache__/ +*.py[cod] +*.pyo +*.pyd +.Python +.pytest_cache/ +.mypy_cache/ +.ruff_cache/ +.coverage +.coverage.* +htmlcov/ +.tox/ +.nox/ + +# Python virtual environments +.venv/ +venv/ +env/ +ENV/ + +# Django local data +backend/*.sqlite3 +backend/*.sqlite3-* +backend/db.sqlite3 +backend/media/ +backend/staticfiles/ + +# Environment files +.env +.env.* +!.env.example +backend/.env +backend/.env.* +frontend/.env +frontend/.env.* +!frontend/.env.example + +# Logs +*.log +logs/ + +# Frontend / Node / Vite +frontend/node_modules/ +frontend/dist/ +frontend/.vite/ +frontend/.cache/ +frontend/coverage/ +frontend/npm-debug.log* +frontend/yarn-debug.log* +frontend/yarn-error.log* +frontend/pnpm-debug.log* + +# Local build / temporary files +tmp/ +temp/ +*.tmp + +# Optional local config overrides +config/*.local.yaml +config/*.local.yml diff --git a/host/robot-command-center/backend/config/__init__.py b/host/robot-command-center/backend/config/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/host/robot-command-center/backend/config/asgi.py b/host/robot-command-center/backend/config/asgi.py new file mode 100644 index 0000000..6decc9d --- /dev/null +++ b/host/robot-command-center/backend/config/asgi.py @@ -0,0 +1,31 @@ +""" +ASGI config for config project. + +It exposes the ASGI callable as a module-level variable named ``application``. + +For more information on this file, see +https://docs.djangoproject.com/en/5.2/howto/deployment/asgi/ +""" + +import os + +from channels.auth import AuthMiddlewareStack +from channels.routing import ProtocolTypeRouter, URLRouter +from channels.security.websocket import OriginValidator +from django.conf import settings +from django.core.asgi import get_asgi_application + +os.environ.setdefault('DJANGO_SETTINGS_MODULE', 'config.settings') + +django_asgi_app = get_asgi_application() + +from monitoring.routing import websocket_urlpatterns + + +application = ProtocolTypeRouter({ + "http": django_asgi_app, + "websocket": OriginValidator( + AuthMiddlewareStack(URLRouter(websocket_urlpatterns)), + settings.CONTROL_WS_ALLOWED_ORIGINS, + ), +}) diff --git a/host/robot-command-center/backend/config/settings.py b/host/robot-command-center/backend/config/settings.py new file mode 100644 index 0000000..0f15ca9 --- /dev/null +++ b/host/robot-command-center/backend/config/settings.py @@ -0,0 +1,114 @@ +import os +from pathlib import Path + +BASE_DIR = Path(__file__).resolve().parent.parent + + +def _split_csv_env(name: str) -> list[str]: + value = os.getenv(name, "") + return [item.strip().rstrip("/") for item in value.split(",") if item.strip()] + +SECRET_KEY = 'django-insecure-pk4scm@ifo%mao6l=j0@-$_v+pg-43^hj4a!199^)zivz-_8xu' +DEBUG = True +ALLOWED_HOSTS = ["*"] + +INSTALLED_APPS = [ + 'django.contrib.admin', + 'django.contrib.auth', + 'django.contrib.contenttypes', + 'django.contrib.sessions', + 'django.contrib.messages', + 'django.contrib.staticfiles', + 'corsheaders', + 'rest_framework', + 'channels', + 'monitoring', +] + +MIDDLEWARE = [ + 'django.middleware.security.SecurityMiddleware', + 'corsheaders.middleware.CorsMiddleware', + 'django.contrib.sessions.middleware.SessionMiddleware', + 'django.middleware.common.CommonMiddleware', + 'django.middleware.csrf.CsrfViewMiddleware', + 'django.contrib.auth.middleware.AuthenticationMiddleware', + 'django.contrib.messages.middleware.MessageMiddleware', + 'django.middleware.clickjacking.XFrameOptionsMiddleware', +] + +ROOT_URLCONF = 'config.urls' + +TEMPLATES = [ + { + 'BACKEND': 'django.template.backends.django.DjangoTemplates', + 'DIRS': [], + 'APP_DIRS': True, + 'OPTIONS': { + 'context_processors': [ + 'django.template.context_processors.request', + 'django.contrib.auth.context_processors.auth', + 'django.contrib.messages.context_processors.messages', + ], + }, + }, +] + +WSGI_APPLICATION = 'config.wsgi.application' +ASGI_APPLICATION = 'config.asgi.application' +CHANNEL_LAYERS = { + 'default': { + 'BACKEND': 'channels.layers.InMemoryChannelLayer', + }, +} + +DATABASES = { + 'default': { + 'ENGINE': 'django.db.backends.sqlite3', + 'NAME': BASE_DIR / 'db.sqlite3', + } +} + +AUTH_PASSWORD_VALIDATORS = [ + { + 'NAME': 'django.contrib.auth.password_validation.UserAttributeSimilarityValidator', + }, + { + 'NAME': 'django.contrib.auth.password_validation.MinimumLengthValidator', + }, + { + 'NAME': 'django.contrib.auth.password_validation.CommonPasswordValidator', + }, + { + 'NAME': 'django.contrib.auth.password_validation.NumericPasswordValidator', + }, +] + +LANGUAGE_CODE = 'zh-hans' +TIME_ZONE = 'Asia/Shanghai' + +USE_I18N = True +USE_TZ = True + +STATIC_URL = 'static/' +CORS_ALLOW_ALL_ORIGINS = True +CORS_EXPOSE_HEADERS = [ + 'X-Blitz-Frame-Seq', + 'X-Blitz-Backend-Received-Unix-Ns', + 'X-Blitz-Frame-Hash', + 'X-Blitz-BSide-Capture-To-Send-Ms', +] + +DEFAULT_AUTO_FIELD = 'django.db.models.BigAutoField' + +CONTROL_WS_ALLOWED_ORIGINS = _split_csv_env('CONTROL_WS_ALLOWED_ORIGINS') or [ + 'http://127.0.0.1', + 'http://127.0.0.1:5173', + 'http://127.0.0.1:4173', + 'http://127.0.0.1:8001', + 'https://127.0.0.1', + 'http://localhost:5173', + 'http://localhost:4173', + 'http://localhost', + 'http://localhost:8001', + 'https://localhost', +] diff --git a/host/robot-command-center/backend/config/urls.py b/host/robot-command-center/backend/config/urls.py new file mode 100644 index 0000000..40d6d83 --- /dev/null +++ b/host/robot-command-center/backend/config/urls.py @@ -0,0 +1,7 @@ +from django.contrib import admin +from django.urls import include, path + +urlpatterns = [ + path('admin/', admin.site.urls), + path('api/', include('monitoring.urls')), +] diff --git a/host/robot-command-center/backend/config/wsgi.py b/host/robot-command-center/backend/config/wsgi.py new file mode 100644 index 0000000..e2fbd58 --- /dev/null +++ b/host/robot-command-center/backend/config/wsgi.py @@ -0,0 +1,16 @@ +""" +WSGI config for config project. + +It exposes the WSGI callable as a module-level variable named ``application``. + +For more information on this file, see +https://docs.djangoproject.com/en/5.2/howto/deployment/wsgi/ +""" + +import os + +from django.core.wsgi import get_wsgi_application + +os.environ.setdefault('DJANGO_SETTINGS_MODULE', 'config.settings') + +application = get_wsgi_application() diff --git a/host/robot-command-center/backend/manage.py b/host/robot-command-center/backend/manage.py new file mode 100644 index 0000000..8e7ac79 --- /dev/null +++ b/host/robot-command-center/backend/manage.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python +"""Django's command-line utility for administrative tasks.""" +import os +import sys + + +def main(): + """Run administrative tasks.""" + os.environ.setdefault('DJANGO_SETTINGS_MODULE', 'config.settings') + try: + from django.core.management import execute_from_command_line + except ImportError as exc: + raise ImportError( + "Couldn't import Django. Are you sure it's installed and " + "available on your PYTHONPATH environment variable? Did you " + "forget to activate a virtual environment?" + ) from exc + execute_from_command_line(sys.argv) + + +if __name__ == '__main__': + main() diff --git a/host/robot-command-center/backend/monitoring/__init__.py b/host/robot-command-center/backend/monitoring/__init__.py new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/host/robot-command-center/backend/monitoring/__init__.py @@ -0,0 +1 @@ + diff --git a/host/robot-command-center/backend/monitoring/apps.py b/host/robot-command-center/backend/monitoring/apps.py new file mode 100644 index 0000000..4715afe --- /dev/null +++ b/host/robot-command-center/backend/monitoring/apps.py @@ -0,0 +1,7 @@ +from django.apps import AppConfig + + +class MonitoringConfig(AppConfig): + default_auto_field = "django.db.models.BigAutoField" + name = "monitoring" + diff --git a/host/robot-command-center/backend/monitoring/common.py b/host/robot-command-center/backend/monitoring/common.py new file mode 100644 index 0000000..dd3c4b2 --- /dev/null +++ b/host/robot-command-center/backend/monitoring/common.py @@ -0,0 +1,342 @@ +from __future__ import annotations + +import os +import json +import struct +from datetime import datetime, timezone +from pathlib import Path +import threading +import time +from typing import Any + + +PROJECT_ROOT = Path(__file__).resolve().parents[2] +WORKSPACE_ROOT = PROJECT_ROOT.parent +JPEG_FRAME_DIR = WORKSPACE_ROOT / "RobotDataShow" / "jpeg-frames" +OMNISOCKET_CONFIG_PATH = PROJECT_ROOT / "config" / "omnisocket_demo.yaml" + +VIDEO_SOURCE_MODE = os.getenv("VIDEO_SOURCE_MODE", "auto").strip().lower() +OMNISOCKET_FRAME_FRESH_SECONDS = 2.0 +VIDEO_TIMESTAMP_SAMPLE_SIZE = 10 +VIDEO_TRAILER_ENDIANNESS = "little" +VIDEO_TRAILER_TIMESTAMP_UNIT = "ms" +VIDEO_TRAILER_TIMESTAMP_MULTIPLIER_NS = 1_000_000 +VIDEO_TRAILER_TIMESTAMP_MAX_SKEW_NS = 7 * 24 * 60 * 60 * 1_000_000_000 +VIDEO_TRAILER_COORDINATE_FORMAT = ( + "uint64 timestamp_ms + float64 latitude + float64 longitude + uint32 capture_to_send_ms (little-endian)" +) +VIDEO_TRAILER_STRUCT = struct.Struct(" str: + return datetime.now(timezone.utc).isoformat(timespec="seconds").replace("+00:00", "Z") + + +def parse_simple_yaml_scalar(value: str) -> Any: + if value in {'""', "''"}: + return "" + if len(value) >= 2 and value[0] == value[-1] and value[0] in {'"', "'"}: + return value[1:-1] + if value.lower() == "true": + return True + if value.lower() == "false": + return False + if value and value.lstrip("-").isdigit(): + return int(value) + return value + + +def load_simple_yaml_config(path: Path) -> dict[str, Any]: + parsed: dict[str, Any] = {} + current_section: str | None = None + + with path.open("r", encoding="utf-8") as file: + for raw_line in file: + line = raw_line.split("#", 1)[0].rstrip() + if not line.strip(): + continue + + if not line.startswith(" "): + if not line.endswith(":"): + raise ValueError(f"invalid top-level yaml line: {raw_line.strip()}") + current_section = line[:-1].strip() + parsed[current_section] = {} + continue + + if current_section is None: + raise ValueError(f"yaml key outside section: {raw_line.strip()}") + + stripped = line.strip() + if ":" not in stripped: + raise ValueError(f"invalid yaml key line: {raw_line.strip()}") + + key, value = stripped.split(":", 1) + parsed[current_section][key.strip()] = parse_simple_yaml_scalar(value.strip()) + + return parsed + + +def load_omnisocket_config() -> dict[str, Any]: + config: dict[str, Any] = {} + if OMNISOCKET_CONFIG_PATH.exists(): + try: + try: + import yaml # type: ignore + + with OMNISOCKET_CONFIG_PATH.open("r", encoding="utf-8") as file: + config = yaml.safe_load(file) or {} + except ImportError: + config = load_simple_yaml_config(OMNISOCKET_CONFIG_PATH) + except Exception: + config = {} + + transport_cfg = dict(config.get("transport", {})) + video_receiver_cfg = dict(config.get("video_receiver", {})) + control_sender_cfg = dict(config.get("control_sender", {})) + control_ack_receiver_cfg = dict(config.get("control_ack_receiver", {})) + control_ingress_cfg = dict(config.get("control_ingress", {})) + video_sender_cfg = dict(config.get("video_sender", {})) + telemetry_receiver_cfg = dict(config.get("telemetry_receiver", {})) + + transport_cfg["server_addr"] = os.getenv( + "OMNISOCKET_SERVER_ADDR", + str(transport_cfg.get("server_addr", "127.0.0.1:10909")), + ) + transport_cfg["relay_via"] = os.getenv( + "OMNISOCKET_RELAY_VIA", + str(transport_cfg.get("relay_via", "")), + ) + transport_cfg["bind_ip"] = os.getenv( + "OMNISOCKET_BIND_IP", + str(transport_cfg.get("bind_ip", "")), + ) + transport_cfg["bind_device"] = os.getenv( + "OMNISOCKET_BIND_DEVICE", + str(transport_cfg.get("bind_device", "")), + ) + + video_receiver_cfg["peer_id"] = os.getenv( + "OMNISOCKET_VIDEO_PEER_ID", + str(video_receiver_cfg.get("peer_id", "peer-a-video")), + ) + video_receiver_cfg["buffer_bytes"] = int( + os.getenv( + "OMNISOCKET_BUFFER_BYTES", + str(video_receiver_cfg.get("buffer_bytes", 1024 * 1024)), + ) + ) + + control_sender_cfg["peer_id"] = os.getenv( + "OMNISOCKET_CONTROL_PEER_ID", + str(control_sender_cfg.get("peer_id", "peer-a-ctrl")), + ) + control_sender_cfg["target_peer"] = os.getenv( + "OMNISOCKET_CONTROL_TARGET_PEER", + str(control_sender_cfg.get("target_peer", "peer-b-ctrl")), + ) + + control_ack_receiver_cfg["peer_id"] = os.getenv( + "OMNISOCKET_CONTROL_ACK_RECEIVER_PEER_ID", + str(control_ack_receiver_cfg.get("peer_id", "peer-a-ctrl-ack")), + ) + control_ack_receiver_cfg["expected_sender"] = os.getenv( + "OMNISOCKET_CONTROL_ACK_EXPECTED_SENDER", + str(control_ack_receiver_cfg.get("expected_sender", "peer-b-ctrl-ack")), + ) + + video_sender_cfg["peer_id"] = os.getenv( + "OMNISOCKET_VIDEO_SENDER_PEER_ID", + str(video_sender_cfg.get("peer_id", "peer-b-video")), + ) + video_sender_cfg["target_peer"] = os.getenv( + "OMNISOCKET_VIDEO_TARGET_PEER_ID", + str(video_sender_cfg.get("target_peer", "peer-a-video")), + ) + + control_ingress_cfg["native_udp_bind"] = os.getenv( + "OMNISOCKET_CONTROL_NATIVE_UDP_BIND", + str(control_ingress_cfg.get("native_udp_bind", "127.0.0.1:10921")), + ) + control_ingress_cfg["source_lease_ms"] = int( + os.getenv( + "OMNISOCKET_CONTROL_SOURCE_LEASE_MS", + str(control_ingress_cfg.get("source_lease_ms", 300)), + ) + ) + control_ingress_cfg["send_rate_hz"] = float( + os.getenv( + "OMNISOCKET_CONTROL_SEND_RATE_HZ", + str(control_ingress_cfg.get("send_rate_hz", 20.0)), + ) + ) + control_ingress_cfg["zero_burst_packets"] = int( + os.getenv( + "OMNISOCKET_CONTROL_ZERO_BURST_PACKETS", + str(control_ingress_cfg.get("zero_burst_packets", 3)), + ) + ) + + telemetry_receiver_cfg["peer_id"] = os.getenv( + "OMNISOCKET_TELEMETRY_PEER_ID", + str(telemetry_receiver_cfg.get("peer_id", "peer-a-telemetry")), + ) + telemetry_receiver_cfg["interval_ms"] = int( + os.getenv( + "OMNISOCKET_TELEMETRY_INTERVAL_MS", + str(telemetry_receiver_cfg.get("interval_ms", 500)), + ) + ) + telemetry_receiver_cfg["stale_after_ms"] = int( + os.getenv( + "OMNISOCKET_TELEMETRY_STALE_AFTER_MS", + str(telemetry_receiver_cfg.get("stale_after_ms", telemetry_receiver_cfg["interval_ms"] * 3)), + ) + ) + + return { + "transport": transport_cfg, + "video_receiver": video_receiver_cfg, + "control_sender": control_sender_cfg, + "control_ack_receiver": control_ack_receiver_cfg, + "control_ingress": control_ingress_cfg, + "video_sender": video_sender_cfg, + "telemetry_receiver": telemetry_receiver_cfg, + } + + +class JsonlRunLogger: + def __init__(self, stem_env: str, default_stem: str) -> None: + explicit_path = os.getenv(stem_env, "").strip() + self._path = Path(explicit_path) if explicit_path else ( + BLITZ_RUN_DIR / f"{default_stem}.{BLITZ_INSTANCE_ID}.jsonl" if BLITZ_RUN_DIR is not None else None + ) + self._lock = threading.Lock() + self._file = None + self._buffered_bytes = 0 + self._current_bytes = 0 + self._flush_bytes = self._positive_int_env("BLITZ_JSONL_FLUSH_BYTES", 262144) + self._flush_interval_ms = self._positive_int_env("BLITZ_JSONL_FLUSH_INTERVAL_MS", 1000) + self._max_bytes = self._positive_int_env("BLITZ_JSONL_ROTATE_BYTES", 134217728) + self._max_files = self._positive_int_env("BLITZ_JSONL_ROTATE_FILES", 8) + self._last_flush_monotonic_ms = self._now_ms() + + if self._path is not None: + try: + self._path.parent.mkdir(parents=True, exist_ok=True) + self._file = self._path.open("a", encoding="utf-8") + self._current_bytes = self._path.stat().st_size if self._path.exists() else 0 + except OSError: + self._file = None + + @property + def path(self) -> str | None: + return str(self._path) if self._path is not None else None + + @property + def enabled(self) -> bool: + return self._file is not None + + def write(self, payload: dict[str, Any]) -> None: + if self._file is None: + return + line = json.dumps(payload, ensure_ascii=False, separators=(",", ":")) + line_bytes = len(line.encode("utf-8")) + 1 + with self._lock: + if self._file is None: + return + try: + self._file.write(line) + self._file.write("\n") + self._buffered_bytes += line_bytes + self._current_bytes += line_bytes + now_ms = self._now_ms() + if ( + self._buffered_bytes >= self._flush_bytes + or (self._flush_interval_ms > 0 and now_ms - self._last_flush_monotonic_ms >= self._flush_interval_ms) + ): + self._flush_locked(now_ms) + if self._max_bytes > 0 and self._max_files > 0 and self._current_bytes >= self._max_bytes: + self._rotate_locked() + except OSError: + if self._file is not None: + try: + self._file.close() + except OSError: + pass + self._file = None + + def close(self) -> None: + with self._lock: + if self._file is not None: + try: + self._flush_locked(self._now_ms()) + except OSError: + pass + self._file.close() + self._file = None + + def _flush_locked(self, now_ms: int) -> None: + if self._file is None: + return + self._file.flush() + self._buffered_bytes = 0 + self._last_flush_monotonic_ms = now_ms + + def _rotate_locked(self) -> None: + if self._path is None or self._file is None or self._max_files <= 0: + return + self._flush_locked(self._now_ms()) + self._file.close() + self._file = None + + oldest = self._path.with_name(f"{self._path.name}.{self._max_files}") + if oldest.exists(): + oldest.unlink() + + for index in range(self._max_files - 1, 0, -1): + src = self._path.with_name(f"{self._path.name}.{index}") + if src.exists(): + dst = self._path.with_name(f"{self._path.name}.{index + 1}") + src.replace(dst) + + if self._path.exists(): + rotated = self._path.with_name(f"{self._path.name}.1") + self._path.replace(rotated) + + self._file = self._path.open("a", encoding="utf-8") + self._buffered_bytes = 0 + self._current_bytes = self._path.stat().st_size if self._path.exists() else 0 + self._last_flush_monotonic_ms = self._now_ms() + + @staticmethod + def _now_ms() -> int: + return int(time.monotonic() * 1000) + + @staticmethod + def _positive_int_env(name: str, default: int) -> int: + raw = os.getenv(name, "").strip() + try: + value = int(raw) + except ValueError: + return default + return value if value > 0 else default + + +def parse_host_port(bind_addr: str) -> tuple[str, int]: + host, port_text = bind_addr.rsplit(":", 1) + host = host.strip() or "127.0.0.1" + port = int(port_text) + if port <= 0 or port > 65535: + raise ValueError(f"invalid port in bind address: {bind_addr}") + return host, port diff --git a/host/robot-command-center/backend/monitoring/consumers.py b/host/robot-command-center/backend/monitoring/consumers.py new file mode 100644 index 0000000..1dc67f9 --- /dev/null +++ b/host/robot-command-center/backend/monitoring/consumers.py @@ -0,0 +1,42 @@ +from __future__ import annotations + +import json + +from channels.generic.websocket import WebsocketConsumer + +from .common import CONTROL_PACKET_SIZE, CONTROL_SOURCE_WEB +from .services import control_arbiter, native_control_ingress + + +class ControlConsumer(WebsocketConsumer): + def connect(self) -> None: + control_arbiter.ensure_started() + native_control_ingress.ensure_started() + self.accept() + self.send( + text_data=json.dumps( + { + "type": "ready", + "packet_bytes": CONTROL_PACKET_SIZE, + } + ) + ) + + def receive(self, text_data: str | None = None, bytes_data: bytes | None = None) -> None: + if bytes_data is None: + self.send(text_data=json.dumps({"type": "error", "detail": "binary control payload required"})) + return + + if len(bytes_data) != CONTROL_PACKET_SIZE: + self.send( + text_data=json.dumps( + { + "type": "error", + "detail": f"expected {CONTROL_PACKET_SIZE} bytes, got {len(bytes_data)}", + } + ) + ) + return + + control_arbiter.ingest_command(CONTROL_SOURCE_WEB, bytes_data) + diff --git a/host/robot-command-center/backend/monitoring/control.py b/host/robot-command-center/backend/monitoring/control.py new file mode 100644 index 0000000..c61326d --- /dev/null +++ b/host/robot-command-center/backend/monitoring/control.py @@ -0,0 +1,951 @@ +from __future__ import annotations + +import json +import socket +import sys +import threading +import time +from typing import Any + +from .common import ( + CONTROL_PACKET_SIZE, + CONTROL_SOURCE_NATIVE_UDP, + CONTROL_SOURCE_PRIORITY, + JsonlRunLogger, + ZERO_CONTROL_PAYLOAD, + WORKSPACE_ROOT, + load_omnisocket_config, + parse_host_port, +) +from .video import safe_kcp_stats + + +def _payload_preview(payload: bytes, limit: int = 160) -> str: + if not payload: + return "" + preview = payload[:limit].decode("utf-8", errors="replace") + if len(payload) > limit: + return f"{preview}..." + return preview + + +class ControlAckTracker: + def __init__(self) -> None: + self._lock = threading.Lock() + self._event_logger = JsonlRunLogger("BLITZ_A_CONTROL_EVENTS_LOG_PATH", "a-control-events") + self._ack_logger = JsonlRunLogger("BLITZ_A_CONTROL_ACKS_LOG_PATH", "a-control-acks") + self._pending: dict[int, dict[str, Any]] = {} + self._latest_estimate: dict[str, Any] = { + "ack_available": False, + "updated_at": None, + "received_mono_ns": 0, + "control_loop_rtt_ms": None, + "b_recv_to_persist_ms": None, + "control_oneway_network_est_ms": None, + "control_to_persist_est_ms": None, + "sample_reason": None, + } + + def register_send( + self, + *, + message_id: int, + issued_at_unix_ns: int, + issued_at_mono_ns: int, + source: str, + payload: bytes, + send_call_latency_us: int, + ) -> None: + event = { + "ts_unix_nano": issued_at_unix_ns, + "message_id": message_id, + "issued_at_unix_ns": issued_at_unix_ns, + "issued_at_mono_ns": issued_at_mono_ns, + "source": source, + "command_signature": payload.hex(), + "payload_size": len(payload), + "send_call_latency_us": send_call_latency_us, + } + with self._lock: + self._pending[message_id] = event + self._prune_locked(issued_at_mono_ns) + self._event_logger.write(event) + + def handle_ack(self, ack_payload: dict[str, Any], received_unix_ns: int, received_mono_ns: int) -> str: + try: + message_id = int(ack_payload["message_id"]) + except (KeyError, TypeError, ValueError): + return "invalid_message_id" + + with self._lock: + event = self._pending.pop(message_id, None) + self._prune_locked(received_mono_ns) + + if event is None: + return "pending_missing" + + try: + control_loop_rtt_ms = round((received_unix_ns - int(event["issued_at_unix_ns"])) / 1_000_000.0, 3) + b_recv_to_persist_ms = round(float(ack_payload.get("b_recv_to_persist_us", 0)) / 1000.0, 3) + except (TypeError, ValueError): + return "invalid_timing" + + control_oneway_network_est_ms = round(max(0.0, (control_loop_rtt_ms - b_recv_to_persist_ms) / 2.0), 3) + control_to_persist_est_ms = round(control_oneway_network_est_ms + b_recv_to_persist_ms, 3) + ack_record = { + "ts_unix_nano": received_unix_ns, + "received_unix_ns": received_unix_ns, + "received_mono_ns": received_mono_ns, + "message_id": message_id, + "ack_phase": str(ack_payload.get("ack_phase") or "persist_end"), + "sample_reason": str(ack_payload.get("sample_reason") or ""), + "b_recv_to_persist_us": ack_payload.get("b_recv_to_persist_us"), + "unix_send_ok": bool(ack_payload.get("unix_send_ok", False)), + "issued_at_unix_ns": event["issued_at_unix_ns"], + "source": event["source"], + "control_loop_rtt_ms": control_loop_rtt_ms, + "b_recv_to_persist_ms": b_recv_to_persist_ms, + "control_oneway_network_est_ms": control_oneway_network_est_ms, + "control_to_persist_est_ms": control_to_persist_est_ms, + } + self._ack_logger.write(ack_record) + with self._lock: + self._latest_estimate = { + "ack_available": True, + "updated_at": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime(received_unix_ns / 1_000_000_000)), + "received_mono_ns": received_mono_ns, + "control_loop_rtt_ms": control_loop_rtt_ms, + "b_recv_to_persist_ms": b_recv_to_persist_ms, + "control_oneway_network_est_ms": control_oneway_network_est_ms, + "control_to_persist_est_ms": control_to_persist_est_ms, + "sample_reason": ack_record["sample_reason"], + } + return "accepted" + + def get_latest_estimate(self) -> dict[str, Any]: + with self._lock: + estimate = dict(self._latest_estimate) + if int(estimate.get("received_mono_ns", 0) or 0) > 0 and time.monotonic_ns() - int(estimate["received_mono_ns"]) > 10_000_000_000: + estimate["ack_available"] = False + estimate["control_loop_rtt_ms"] = None + estimate["b_recv_to_persist_ms"] = None + estimate["control_oneway_network_est_ms"] = None + estimate["control_to_persist_est_ms"] = None + estimate["sample_reason"] = None + estimate.pop("received_mono_ns", None) + return estimate + + def close(self) -> None: + self._event_logger.close() + self._ack_logger.close() + + def _prune_locked(self, now_mono_ns: int) -> None: + stale_ids = [ + message_id + for message_id, event in self._pending.items() + if now_mono_ns - int(event.get("issued_at_mono_ns", 0)) > 60_000_000_000 + ] + for message_id in stale_ids: + self._pending.pop(message_id, None) + + +class OmniSocketControlSender: + def __init__(self, ack_tracker: ControlAckTracker) -> None: + self._lock = threading.Lock() + self._camera_command_lock = threading.Lock() + self._camera_condition = threading.Condition(self._lock) + self._ack_tracker = ack_tracker + self._session = None + self._session_cls = None + self._msg_type_text = None + self._msg_type_error = None + self._control_defaults: dict[str, Any] = {} + self._started = False + self._drain_thread: threading.Thread | None = None + self._closing = threading.Event() + self._target_peer = "" + self._send_count = 0 + self._send_errors = 0 + self._drain_errors = 0 + self._last_error = "" + self._reconnect_count = 0 + self._ever_connected = False + self._registered = False + self._supports_send_with_id = False + self._supports_send_text = False + self._requested_camera: str | None = None + self._active_camera: str | None = None + self._camera_ack_revision = 0 + self._camera_command_count = 0 + self._camera_ack_count = 0 + self._camera_updated_at: str | None = None + self._camera_last_error = "" + self._load_backend() + + def _load_backend(self) -> None: + try: + self._import_backend() + except Exception as error: # pragma: no cover - optional runtime dependency + self._last_error = f"omnisocket import failed: {error}" + + def _import_backend(self) -> None: + try: + from omnisocket import CONTROL_DEFAULTS, MSG_TYPE_ERROR, MSG_TYPE_TEXT, Session # type: ignore + except ImportError: + python_dir = WORKSPACE_ROOT / "OmniSocketGo" / "python" + if python_dir.exists(): + sys.path.insert(0, str(python_dir)) + from omnisocket import CONTROL_DEFAULTS, MSG_TYPE_ERROR, MSG_TYPE_TEXT, Session # type: ignore + + self._session_cls = Session + self._msg_type_text = MSG_TYPE_TEXT + self._msg_type_error = MSG_TYPE_ERROR + self._control_defaults = dict(CONTROL_DEFAULTS) + self._supports_send_text = hasattr(Session, "send_text") + + def _connect_session(self): + assert self._session_cls is not None + + config = load_omnisocket_config() + transport_cfg = config.get("transport", {}) + control_cfg = config.get("control_sender", {}) + + session = self._session_cls() + session.connect( + server_addr=str(transport_cfg.get("server_addr", "127.0.0.1:10909")), + peer_id=str(control_cfg.get("peer_id", "peer-a-ctrl")), + relay_via=str(transport_cfg.get("relay_via", "")), + bind_ip=str(transport_cfg.get("bind_ip", "")), + bind_device=str(transport_cfg.get("bind_device", "")), + **self._control_defaults, + ) + target_peer = str(control_cfg.get("target_peer", "peer-b-ctrl")) + return session, target_peer + + def ensure_started(self) -> None: + if self._session_cls is None: + return + + with self._lock: + if self._closing.is_set(): + return + if self._started and self._session is not None: + return + session, target_peer = self._connect_session() + self._session = session + self._target_peer = target_peer + self._closing.clear() + self._started = True + self._last_error = "" + self._registered = bool(dict(session.stats()).get("registered", 0)) + self._supports_send_with_id = hasattr(session, "send_with_id") + self._supports_send_text = hasattr(session, "send_text") + if self._ever_connected: + self._reconnect_count += 1 + else: + self._ever_connected = True + self._drain_thread = threading.Thread( + target=self._drain_loop, + name="omnisocket-control-drain", + daemon=True, + ) + self._drain_thread.start() + + def _reset_session(self, session: Any | None) -> None: + with self._lock: + if session is not None and session is not self._session: + return + current = self._session + self._session = None + self._started = False + self._registered = False + self._supports_send_with_id = False + if current is not None: + try: + current.close() + except Exception: + pass + + def send_payload(self, payload: bytes, *, source: str) -> None: + if len(payload) != CONTROL_PACKET_SIZE: + raise ValueError(f"expected {CONTROL_PACKET_SIZE} bytes, got {len(payload)}") + self.ensure_started() + with self._lock: + session = self._session + target_peer = self._target_peer + supports_send_with_id = self._supports_send_with_id + + if session is None: + raise RuntimeError("control session is not available") + + try: + issued_at_unix_ns = time.time_ns() + issued_at_mono_ns = time.monotonic_ns() + send_started_ns = time.perf_counter_ns() + message_id: int | None = None + if supports_send_with_id: + message_id = int(session.send_with_id(to=target_peer, data=payload)) + else: + session.send(to=target_peer, data=payload) + send_call_latency_us = max(0, int((time.perf_counter_ns() - send_started_ns) / 1000)) + except Exception as error: + with self._lock: + self._send_errors += 1 + self._last_error = str(error) + self._reset_session(session) + raise + + if message_id is not None: + self._ack_tracker.register_send( + message_id=message_id, + issued_at_unix_ns=issued_at_unix_ns, + issued_at_mono_ns=issued_at_mono_ns, + source=source, + payload=payload, + send_call_latency_us=send_call_latency_us, + ) + with self._lock: + self._send_count += 1 + + def send_zero_burst(self, count: int) -> None: + for _ in range(max(0, count)): + try: + self.send_payload(ZERO_CONTROL_PAYLOAD, source="zero_burst") + except Exception: + return + + def select_camera(self, camera: str, *, timeout: float = 2.0) -> dict[str, Any]: + normalized = camera.strip().lower() + if normalized not in {"head", "waist"}: + raise ValueError("camera must be 'head' or 'waist'") + + with self._camera_command_lock: + self.ensure_started() + with self._lock: + session = self._session + target_peer = self._target_peer + supports_send_text = self._supports_send_text + previous_revision = self._camera_ack_revision + + if session is None: + raise RuntimeError("control session is not available") + if not supports_send_text: + raise RuntimeError("omnisocket extension does not support text messages; rebuild it with make python-ext") + + try: + session.send_text(to=target_peer, text=f"camera:{normalized}") + except Exception as error: + with self._lock: + self._send_errors += 1 + self._last_error = str(error) + self._camera_last_error = str(error) + self._reset_session(session) + raise + + deadline = time.monotonic() + max(0.1, timeout) + with self._camera_condition: + self._send_count += 1 + self._camera_command_count += 1 + self._requested_camera = normalized + self._camera_last_error = "" + while self._camera_ack_revision == previous_revision: + remaining = deadline - time.monotonic() + if remaining <= 0: + self._camera_last_error = "robot camera confirmation timed out" + break + self._camera_condition.wait(timeout=remaining) + confirmed = self._camera_ack_revision != previous_revision and self._active_camera == normalized + + status = self.get_camera_status() + status["confirmed"] = confirmed + return status + + def get_camera_status(self) -> dict[str, Any]: + session_stats = self.session_stats() + with self._lock: + return { + "available": self._session_cls is not None and self._supports_send_text, + "connected": self._session is not None, + "registered": bool(session_stats.get("registered", 0)), + "requested_camera": self._requested_camera, + "active_camera": self._active_camera, + "command_count": self._camera_command_count, + "ack_count": self._camera_ack_count, + "updated_at": self._camera_updated_at, + "last_error": self._camera_last_error or self._last_error, + } + + def _drain_loop(self) -> None: + while not self._closing.is_set(): + with self._lock: + session = self._session + if session is None: + return + + try: + result = session.recv(timeout_ms=100) + except Exception as error: + last_server_error = "" + try: + last_server_error = str(dict(session.stats()).get("last_server_error", "") or "") + except Exception: + last_server_error = "" + with self._lock: + self._drain_errors += 1 + self._registered = False + self._last_error = last_server_error or str(error) + if not self._closing.is_set(): + self._reset_session(session) + return + + if result is None: + try: + stats = dict(session.stats()) + except Exception: + stats = {} + with self._lock: + self._registered = bool(stats.get("registered", 0)) + if stats.get("last_server_error"): + self._last_error = str(stats.get("last_server_error")) + continue + + from_peer, msg_type, payload = result + if msg_type == self._msg_type_error: + text = payload.decode("utf-8", errors="replace") + try: + stats = dict(session.stats()) + except Exception: + stats = {} + with self._lock: + self._last_error = f"server error from {from_peer}: {text}" + self._registered = bool(stats.get("registered", 0)) + continue + + if msg_type == self._msg_type_text: + with self._lock: + expected_peer = self._target_peer + if expected_peer and from_peer != expected_peer: + continue + try: + message = json.loads(payload.decode("utf-8")) + except (UnicodeDecodeError, json.JSONDecodeError): + continue + camera = str(message.get("camera") or "").lower() if isinstance(message, dict) else "" + if isinstance(message, dict) and message.get("type") == "camera.selected" and camera in {"head", "waist"}: + with self._camera_condition: + self._active_camera = camera + self._camera_ack_revision += 1 + self._camera_ack_count += 1 + self._camera_updated_at = time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()) + self._camera_last_error = "" + self._camera_condition.notify_all() + + def session_stats(self) -> dict[str, Any]: + with self._lock: + session = self._session + if session is None: + return {"connected": 0, "registered": 0, "last_server_error": self._last_error} + try: + return dict(session.stats()) + except Exception: + return {"connected": 0, "registered": 0, "last_server_error": self._last_error} + + def session_kcp_stats(self) -> dict[str, Any]: + with self._lock: + session = self._session + return safe_kcp_stats(session) + + def get_status(self) -> dict[str, Any]: + config = load_omnisocket_config() + control_cfg = config.get("control_sender", {}) + session_stats = self.session_stats() + with self._lock: + return { + "backend_ready": self._session_cls is not None, + "started": self._started, + "connected": self._session is not None, + "registered": bool(session_stats.get("registered", 0)), + "peer_id": str(control_cfg.get("peer_id", "")), + "target_peer": str(control_cfg.get("target_peer", "")), + "send_count": self._send_count, + "send_errors": self._send_errors, + "drain_errors": self._drain_errors, + "reconnect_count": self._reconnect_count, + "last_server_error": str(session_stats.get("last_server_error", "") or ""), + "last_error": self._last_error, + } + + def close(self) -> None: + self._closing.set() + self.send_zero_burst(1) + self._reset_session(None) + drain_thread = self._drain_thread + if drain_thread is not None and drain_thread.is_alive(): + drain_thread.join(timeout=0.5) + + +class OmniSocketControlAckReceiver: + def __init__(self, ack_tracker: ControlAckTracker) -> None: + self._ack_tracker = ack_tracker + self._lock = threading.Lock() + self._thread: threading.Thread | None = None + self._started = False + self._session = None + self._session_cls = None + self._msg_type_text = None + self._msg_type_error = None + self._control_defaults: dict[str, Any] = {} + self._closing = threading.Event() + self._last_error = "" + self._last_server_error = "" + self._registered = False + self._reconnect_count = 0 + self._ever_connected = False + self._received_messages = 0 + self._received_bytes = 0 + self._accepted_count = 0 + self._pending_missing_count = 0 + self._invalid_message_id_count = 0 + self._invalid_timing_count = 0 + self._unexpected_message_type_count = 0 + self._unexpected_sender_count = 0 + self._sender_mismatch_accepted_count = 0 + self._payload_decode_errors = 0 + self._last_msg_type: int | None = None + self._last_from_peer = "" + self._last_payload_preview = "" + self._last_ack_result = "" + self._load_backend() + + def _load_backend(self) -> None: + try: + self._import_backend() + except Exception as error: # pragma: no cover + self._last_error = f"omnisocket import failed: {error}" + + def _import_backend(self) -> None: + try: + from omnisocket import CONTROL_DEFAULTS, MSG_TYPE_ERROR, MSG_TYPE_TEXT, Session # type: ignore + except ImportError: + python_dir = WORKSPACE_ROOT / "OmniSocketGo" / "python" + if python_dir.exists(): + sys.path.insert(0, str(python_dir)) + from omnisocket import CONTROL_DEFAULTS, MSG_TYPE_ERROR, MSG_TYPE_TEXT, Session # type: ignore + + self._session_cls = Session + self._msg_type_text = MSG_TYPE_TEXT + self._msg_type_error = MSG_TYPE_ERROR + self._control_defaults = dict(CONTROL_DEFAULTS) + + def _connect_session(self): + assert self._session_cls is not None + + config = load_omnisocket_config() + transport_cfg = config.get("transport", {}) + ack_cfg = config.get("control_ack_receiver", {}) + + session = self._session_cls() + session.connect( + server_addr=str(transport_cfg.get("server_addr", "127.0.0.1:10909")), + peer_id=str(ack_cfg.get("peer_id", "peer-a-ctrl-ack")), + relay_via=str(transport_cfg.get("relay_via", "")), + bind_ip=str(transport_cfg.get("bind_ip", "")), + bind_device=str(transport_cfg.get("bind_device", "")), + **self._control_defaults, + ) + return session, str(ack_cfg.get("expected_sender", "peer-b-ctrl-ack")) + + def ensure_started(self) -> None: + if self._session_cls is None: + return + with self._lock: + if self._started or self._closing.is_set(): + return + self._started = True + self._thread = threading.Thread(target=self._run, name="omnisocket-control-ack", daemon=True) + self._thread.start() + + @staticmethod + def _looks_like_control_ack(ack_payload: Any) -> bool: + if not isinstance(ack_payload, dict): + return False + if "message_id" not in ack_payload: + return False + return any(field in ack_payload for field in ("ack_phase", "b_recv_to_persist_us", "unix_send_ok", "sample_reason")) + + @staticmethod + def _sender_matches(expected_sender: str, from_peer: str) -> bool: + normalized_expected = expected_sender.strip() + normalized_from = from_peer.strip() + if not normalized_expected: + return True + return normalized_from == normalized_expected + + def _run(self) -> None: + while not self._closing.is_set(): + expected_sender = "" + try: + session, expected_sender = self._connect_session() + with self._lock: + self._session = session + self._last_error = "" + if self._ever_connected: + self._reconnect_count += 1 + else: + self._ever_connected = True + + while not self._closing.is_set(): + result = session.recv(timeout_ms=1000) + if result is None: + try: + session_stats = dict(session.stats()) + except Exception: + session_stats = {} + with self._lock: + self._registered = bool(session_stats.get("registered", 0)) + self._last_server_error = str(session_stats.get("last_server_error", "") or "") + continue + from_peer, msg_type, payload = result + with self._lock: + self._received_messages += 1 + self._received_bytes += len(payload) + self._last_from_peer = str(from_peer or "") + self._last_msg_type = int(msg_type) + self._last_payload_preview = _payload_preview(payload) + try: + session_stats = dict(session.stats()) + except Exception: + session_stats = {} + self._registered = bool(session_stats.get("registered", 0)) + self._last_server_error = str(session_stats.get("last_server_error", "") or "") + if msg_type == self._msg_type_error: + with self._lock: + self._last_error = f"ack session error from {from_peer}: {payload.decode('utf-8', errors='replace')}" + self._last_ack_result = "server_error" + continue + if msg_type != self._msg_type_text: + with self._lock: + self._unexpected_message_type_count += 1 + self._last_ack_result = "unexpected_message_type" + continue + try: + ack_payload = json.loads(payload.decode("utf-8")) + except (UnicodeDecodeError, json.JSONDecodeError): + with self._lock: + self._payload_decode_errors += 1 + self._last_ack_result = "payload_decode_error" + continue + sender_matches = self._sender_matches(expected_sender, from_peer) + if not sender_matches: + with self._lock: + self._unexpected_sender_count += 1 + if not self._looks_like_control_ack(ack_payload): + with self._lock: + self._last_ack_result = "unexpected_sender" + continue + ack_result = self._ack_tracker.handle_ack(ack_payload, time.time_ns(), time.monotonic_ns()) + with self._lock: + self._last_ack_result = ack_result + if ack_result == "accepted": + self._accepted_count += 1 + if not sender_matches: + self._sender_mismatch_accepted_count += 1 + elif ack_result == "pending_missing": + self._pending_missing_count += 1 + elif ack_result == "invalid_message_id": + self._invalid_message_id_count += 1 + elif ack_result == "invalid_timing": + self._invalid_timing_count += 1 + except Exception as error: # pragma: no cover + if not self._closing.is_set(): + with self._lock: + self._last_error = str(error) + time.sleep(2) + finally: + if self._session is not None: + try: + self._session.close() + except Exception: + pass + with self._lock: + self._session = None + if self._closing.is_set(): + self._started = False + + def get_status(self) -> dict[str, Any]: + config = load_omnisocket_config().get("control_ack_receiver", {}) + with self._lock: + session = self._session + if session is not None: + try: + session_stats = dict(session.stats()) + except Exception: + session_stats = {} + else: + session_stats = {} + with self._lock: + return { + "backend_ready": self._session_cls is not None, + "started": self._started, + "connected": self._session is not None, + "registered": bool(session_stats.get("registered", 0) or self._registered), + "peer_id": str(config.get("peer_id", "")), + "expected_sender": str(config.get("expected_sender", "")), + "recv_calls": int(session_stats.get("recv_calls", 0)), + "recv_bytes": int(session_stats.get("recv_bytes", 0)), + "recv_timeouts": int(session_stats.get("recv_timeouts", 0)), + "recv_errors": int(session_stats.get("recv_errors", 0)), + "received_messages": self._received_messages, + "received_bytes": self._received_bytes, + "accepted_count": self._accepted_count, + "pending_missing_count": self._pending_missing_count, + "invalid_message_id_count": self._invalid_message_id_count, + "invalid_timing_count": self._invalid_timing_count, + "unexpected_message_type_count": self._unexpected_message_type_count, + "unexpected_sender_count": self._unexpected_sender_count, + "sender_mismatch_accepted_count": self._sender_mismatch_accepted_count, + "payload_decode_errors": self._payload_decode_errors, + "last_msg_type": self._last_msg_type, + "last_from_peer": self._last_from_peer, + "last_payload_preview": self._last_payload_preview, + "last_ack_result": self._last_ack_result, + "reconnect_count": self._reconnect_count, + "last_server_error": str(session_stats.get("last_server_error", "") or self._last_server_error), + "last_error": self._last_error, + } + + def close(self) -> None: + self._closing.set() + with self._lock: + session = self._session + if session is not None: + try: + session.close() + except Exception: + pass + thread = self._thread + if thread is not None and thread.is_alive(): + thread.join(timeout=0.5) + + +class ControlArbiter: + def __init__(self, sender: OmniSocketControlSender) -> None: + self._sender = sender + self._lock = threading.Lock() + self._thread: threading.Thread | None = None + self._closing = threading.Event() + self._started = False + self._source_lease_ms = 300 + self._send_rate_hz = 20.0 + self._zero_burst_packets = 3 + self._latest_by_source: dict[str, tuple[bytes, float]] = {} + self._packet_counts = {source: 0 for source in CONTROL_SOURCE_PRIORITY} + self._last_payload = ZERO_CONTROL_PAYLOAD + self._last_sent_at = 0.0 + self._active_source: str | None = None + self._last_error = "" + + def _load_config(self) -> None: + cfg = load_omnisocket_config().get("control_ingress", {}) + self._source_lease_ms = max(50, int(cfg.get("source_lease_ms", 300))) + self._send_rate_hz = max(1.0, float(cfg.get("send_rate_hz", 20.0))) + self._zero_burst_packets = max(1, int(cfg.get("zero_burst_packets", 3))) + + def ensure_started(self) -> None: + self._load_config() + with self._lock: + if self._closing.is_set(): + return + if self._started: + return + self._started = True + self._thread = threading.Thread( + target=self._send_loop, + name="control-arbiter", + daemon=True, + ) + self._thread.start() + + def ingest_command(self, source: str, payload: bytes) -> None: + if source not in CONTROL_SOURCE_PRIORITY: + raise ValueError(f"unsupported control source: {source}") + if len(payload) != CONTROL_PACKET_SIZE: + raise ValueError(f"expected {CONTROL_PACKET_SIZE} bytes, got {len(payload)}") + + self.ensure_started() + now = time.monotonic() + with self._lock: + self._latest_by_source[source] = (payload, now) + self._packet_counts[source] += 1 + + def _resolve_active_locked(self, now: float) -> tuple[str | None, bytes, int]: + lease_seconds = self._source_lease_ms / 1000.0 + expired_sources = [ + source + for source, (_, updated_at) in self._latest_by_source.items() + if (now - updated_at) > lease_seconds + ] + for source in expired_sources: + self._latest_by_source.pop(source, None) + + for source in CONTROL_SOURCE_PRIORITY: + entry = self._latest_by_source.get(source) + if entry is None: + continue + payload, updated_at = entry + remaining_ms = max(0, int((lease_seconds - (now - updated_at)) * 1000)) + return source, payload, remaining_ms + + return None, ZERO_CONTROL_PAYLOAD, 0 + + def _send_loop(self) -> None: + interval = 1.0 / max(self._send_rate_hz, 1.0) + previous_active: str | None = None + + while not self._closing.is_set(): + now = time.monotonic() + with self._lock: + active_source, payload, _lease_ms = self._resolve_active_locked(now) + self._active_source = active_source + self._last_payload = payload + + if previous_active is not None and active_source is None: + try: + self._sender.send_zero_burst(self._zero_burst_packets) + except Exception as error: + with self._lock: + self._last_error = str(error) + elif active_source is not None: + try: + self._sender.send_payload(payload, source=active_source) + with self._lock: + self._last_sent_at = time.monotonic() + self._last_error = "" + except Exception as error: + with self._lock: + self._last_error = str(error) + + previous_active = active_source + self._closing.wait(interval) + + try: + self._sender.send_zero_burst(self._zero_burst_packets) + except Exception: + pass + + def get_status(self) -> dict[str, Any]: + self.ensure_started() + now = time.monotonic() + with self._lock: + active_source, _payload, lease_ms = self._resolve_active_locked(now) + return { + "active_source": active_source, + "control_lease_remaining_ms": lease_ms, + "packet_counts": dict(self._packet_counts), + "send_rate_hz": self._send_rate_hz, + "source_lease_ms": self._source_lease_ms, + "zero_burst_packets": self._zero_burst_packets, + "last_error": self._last_error, + "last_sent_at_monotonic": self._last_sent_at, + } + + def close(self) -> None: + self._closing.set() + thread = self._thread + if thread is not None and thread.is_alive(): + thread.join(timeout=0.5) + + +class NativeUdpControlIngress: + def __init__(self, arbiter: ControlArbiter) -> None: + self._arbiter = arbiter + self._lock = threading.Lock() + self._thread: threading.Thread | None = None + self._closing = threading.Event() + self._started = False + self._bind_addr = "127.0.0.1:10921" + self._packets_received = 0 + self._invalid_packets = 0 + self._last_sender = "" + self._last_error = "" + + def ensure_started(self) -> None: + bind_addr = str(load_omnisocket_config().get("control_ingress", {}).get("native_udp_bind", "127.0.0.1:10921")) + with self._lock: + self._bind_addr = bind_addr + if self._closing.is_set(): + return + if self._thread is not None and self._thread.is_alive(): + return + self._started = True + self._thread = threading.Thread( + target=self._run, + name="native-udp-control-ingress", + daemon=True, + ) + self._thread.start() + + def _run(self) -> None: + try: + try: + host, port = parse_host_port(self._bind_addr) + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + sock.bind((host, port)) + sock.settimeout(0.1) + except Exception as error: + with self._lock: + self._last_error = str(error) + return + + with sock: + while not self._closing.is_set(): + try: + payload, sender_addr = sock.recvfrom(CONTROL_PACKET_SIZE + 64) + except socket.timeout: + continue + except OSError as error: + with self._lock: + if not self._closing.is_set(): + self._last_error = str(error) + return + + with self._lock: + self._last_sender = f"{sender_addr[0]}:{sender_addr[1]}" + + if len(payload) != CONTROL_PACKET_SIZE: + with self._lock: + self._invalid_packets += 1 + continue + + try: + self._arbiter.ingest_command(CONTROL_SOURCE_NATIVE_UDP, payload) + except Exception as error: + with self._lock: + self._last_error = str(error) + continue + + with self._lock: + self._packets_received += 1 + finally: + with self._lock: + self._started = False + self._thread = None + + def get_status(self) -> dict[str, Any]: + self.ensure_started() + with self._lock: + return { + "started": self._started, + "bind_addr": self._bind_addr, + "packets_received": self._packets_received, + "invalid_packets": self._invalid_packets, + "last_sender": self._last_sender, + "last_error": self._last_error, + } + + def close(self) -> None: + self._closing.set() + thread = self._thread + if thread is not None and thread.is_alive(): + thread.join(timeout=0.5) diff --git a/host/robot-command-center/backend/monitoring/routing.py b/host/robot-command-center/backend/monitoring/routing.py new file mode 100644 index 0000000..1da9004 --- /dev/null +++ b/host/robot-command-center/backend/monitoring/routing.py @@ -0,0 +1,9 @@ +from django.urls import re_path + +from .consumers import ControlConsumer + + +websocket_urlpatterns = [ + re_path(r"^ws/control/$", ControlConsumer.as_asgi()), +] + diff --git a/host/robot-command-center/backend/monitoring/services.py b/host/robot-command-center/backend/monitoring/services.py new file mode 100644 index 0000000..602a959 --- /dev/null +++ b/host/robot-command-center/backend/monitoring/services.py @@ -0,0 +1,53 @@ +from __future__ import annotations + +import atexit + +from .control import ControlAckTracker, ControlArbiter, NativeUdpControlIngress, OmniSocketControlAckReceiver, OmniSocketControlSender +from .telemetry import GpsDataService, HubTelemetryReceiver, NetworkTelemetryService +from .video import OmniSocketVideoReceiver, VideoDisplayProbeStore, VideoFrameService + + +_video_receiver = OmniSocketVideoReceiver() +_control_ack_tracker = ControlAckTracker() +_control_sender = OmniSocketControlSender(_control_ack_tracker) +_control_ack_receiver = OmniSocketControlAckReceiver(_control_ack_tracker) +_hub_telemetry_receiver = HubTelemetryReceiver() +_video_display_probe_store = VideoDisplayProbeStore() + +control_arbiter = ControlArbiter(_control_sender) +camera_control_service = _control_sender +native_control_ingress = NativeUdpControlIngress(control_arbiter) + +video_service = VideoFrameService(_video_receiver, _video_display_probe_store) +gps_service = GpsDataService(_video_receiver) +network_service = NetworkTelemetryService( + _video_receiver, + _control_sender, + _control_ack_tracker, + _control_ack_receiver, + control_arbiter, + native_control_ingress, + _hub_telemetry_receiver, + _video_display_probe_store, +) + + +def shutdown_monitoring_services() -> None: + for closer in ( + network_service.close, + native_control_ingress.close, + control_arbiter.close, + _control_ack_receiver.close, + _control_ack_tracker.close, + _hub_telemetry_receiver.close, + _video_display_probe_store.close, + _video_receiver.close, + _control_sender.close, + ): + try: + closer() + except Exception: + pass + + +atexit.register(shutdown_monitoring_services) diff --git a/host/robot-command-center/backend/monitoring/telemetry.py b/host/robot-command-center/backend/monitoring/telemetry.py new file mode 100644 index 0000000..a476f99 --- /dev/null +++ b/host/robot-command-center/backend/monitoring/telemetry.py @@ -0,0 +1,903 @@ +from __future__ import annotations + +from collections import deque +import json +import os +import sys +import threading +import time +from datetime import datetime, timezone +from pathlib import Path +from typing import Any + +from .common import ( + VIDEO_TRAILER_COORDINATE_FORMAT, + WORKSPACE_ROOT, + load_omnisocket_config, + utc_iso_now, +) +from .control import ControlAckTracker, ControlArbiter, NativeUdpControlIngress, OmniSocketControlAckReceiver, OmniSocketControlSender +from .video import FrameTrailerMetadata, OmniSocketVideoReceiver, VideoDisplayProbeStore + + +LOCAL_SAMPLE_INTERVAL_MS = 500 +TREND_HISTORY_SIZE = 10 +TREND_WINDOW_SIZE = 5 +BLITZ_RUNTIME_DIR = Path(os.getenv("BLITZ_RUNTIME_DIR", "/run/blitz-robot")) +WATCHDOG_STATUS_PATH = BLITZ_RUNTIME_DIR / "watchdog.status.json" +WATCHDOG_STATUS_STALE_MS = max(int(os.getenv("BLITZ_HEALTH_STALE_SEC", "15")), 1) * 1000 +WATCHDOG_FAULT_REASON_MAP: dict[str, tuple[str, str | None]] = { + "": ("none", None), + "none": ("none", None), + "camera_missing": ("video_pipeline_stalled", "degraded"), + "camera_recovered": ("video_session_recovering", "recovering"), + "camera-reappeared-escalated": ("video_session_recovering", "recovering"), + "bside_status_stale": ("video_session_recovering", "recovering"), + "bside-unhealthy-escalated": ("video_session_recovering", "recovering"), + "ros_receiver_unhealthy": ("control_session_recovering", "recovering"), + "ros-unhealthy": ("control_session_recovering", "recovering"), + "network_or_robot_unreachable": ("network_or_robot_unreachable", "recovering"), + "network-recovered-ros-unhealthy": ("control_session_recovering", "recovering"), + "network-recovered-escalated": ("network_or_robot_unreachable", "recovering"), +} + + +def _utc_from_epoch(epoch_seconds: float | None) -> str | None: + if epoch_seconds is None or epoch_seconds <= 0.0: + return None + return datetime.fromtimestamp(epoch_seconds, timezone.utc).isoformat(timespec="seconds").replace("+00:00", "Z") + + +def _coerce_int(value: Any, default: int = 0) -> int: + try: + if value is None: + return default + return int(value) + except (TypeError, ValueError): + return default + + +def _coerce_float(value: Any, default: float = 0.0) -> float: + try: + if value is None: + return default + return float(value) + except (TypeError, ValueError): + return default + + +def _load_optional_json(path: Path) -> dict[str, Any] | None: + try: + if not path.exists(): + return None + with path.open("r", encoding="utf-8") as file: + payload = json.load(file) + if isinstance(payload, dict): + return payload + except Exception: + return None + return None + + +class GpsDataService: + def __init__(self, receiver: OmniSocketVideoReceiver) -> None: + self._receiver = receiver + + def get_latest(self) -> dict[str, Any]: + metadata = self._receiver.get_latest_frame_metadata() + if metadata is None: + return self._build_waiting_payload() + return self._build_payload_from_metadata(metadata) + + def _build_waiting_payload(self) -> dict[str, Any]: + return { + "has_fix": False, + "utc_time": "--:--:--", + "latitude": None, + "longitude": None, + "satellites": None, + "altitude_m": None, + "coordinate_system": "WGS84", + "source_sentence": "VIDEO_TRAILER", + "raw_coordinate_format": VIDEO_TRAILER_COORDINATE_FORMAT, + "source_mode": "video-frame-trailer-waiting", + "updated_at": "", + } + + def _build_payload_from_metadata(self, metadata: FrameTrailerMetadata) -> dict[str, Any]: + updated_at = _utc_from_epoch(metadata.received_at) or "" + if not metadata.has_gps_fix: + return { + "has_fix": False, + "utc_time": "--:--:--", + "latitude": None, + "longitude": None, + "raw_latitude_hex": f"0x{metadata.raw_latitude_hex}", + "raw_longitude_hex": f"0x{metadata.raw_longitude_hex}", + "satellites": None, + "altitude_m": None, + "coordinate_system": "WGS84", + "source_sentence": "VIDEO_TRAILER", + "raw_coordinate_format": VIDEO_TRAILER_COORDINATE_FORMAT, + "source_mode": "video-frame-trailer-no-fix", + "updated_at": updated_at, + } + + timestamp_seconds = metadata.timestamp_ns / 1_000_000_000 + return { + "has_fix": True, + "utc_time": datetime.fromtimestamp(timestamp_seconds, timezone.utc).strftime("%H:%M:%S"), + "latitude": round(metadata.latitude, 6) if metadata.latitude is not None else None, + "longitude": round(metadata.longitude, 6) if metadata.longitude is not None else None, + "raw_latitude_hex": f"0x{metadata.raw_latitude_hex}", + "raw_longitude_hex": f"0x{metadata.raw_longitude_hex}", + "satellites": None, + "altitude_m": None, + "coordinate_system": "WGS84", + "source_sentence": "VIDEO_TRAILER", + "raw_coordinate_format": VIDEO_TRAILER_COORDINATE_FORMAT, + "source_mode": "video-frame-trailer", + "updated_at": updated_at, + } + + +class KcpTrendTracker: + def __init__(self) -> None: + self._lock = threading.Lock() + self._samples: dict[str, deque[dict[str, Any]]] = {} + + def _normalize(self, stats: dict[str, Any] | None) -> dict[str, Any]: + raw = dict(stats or {}) + snd_wnd = _coerce_int(raw.get("snd_wnd")) + rmt_wnd = _coerce_int(raw.get("rmt_wnd")) + inflight = _coerce_int(raw.get("inflight")) + window_limit = _coerce_int(raw.get("window_limit"), min(snd_wnd, rmt_wnd) if snd_wnd and rmt_wnd else 0) + return { + "connected": _coerce_int(raw.get("connected")), + "conv": _coerce_int(raw.get("conv")), + "rto_ms": _coerce_int(raw.get("rto_ms")), + "srtt_ms": _coerce_int(raw.get("srtt_ms")), + "min_srtt_ms": _coerce_int(raw.get("min_srtt_ms")), + "srttvar_ms": _coerce_int(raw.get("srttvar_ms")), + "last_feedback_age_ms": _coerce_int(raw.get("last_feedback_age_ms")), + "snd_wnd": snd_wnd, + "rmt_wnd": rmt_wnd, + "inflight": inflight, + "window_limit": window_limit, + "window_pressure_pct": round(_coerce_float(raw.get("window_pressure_pct")), 3), + "snd_queue": _coerce_int(raw.get("snd_queue")), + "rcv_queue": _coerce_int(raw.get("rcv_queue")), + "snd_buffer": _coerce_int(raw.get("snd_buffer")), + "out_segs_total": _coerce_int(raw.get("out_segs_total")), + "retrans_total": _coerce_int(raw.get("retrans_total")), + "fast_retrans_total": _coerce_int(raw.get("fast_retrans_total")), + "lost_total": _coerce_int(raw.get("lost_total")), + "repeat_total": _coerce_int(raw.get("repeat_total")), + "xmit_total": _coerce_int(raw.get("xmit_total")), + } + + def add_sample(self, key: str, stats: dict[str, Any] | None) -> None: + sample = { + "ts_monotonic": time.monotonic(), + "updated_at": utc_iso_now(), + "stats": self._normalize(stats), + } + with self._lock: + history = self._samples.setdefault(key, deque(maxlen=TREND_HISTORY_SIZE)) + history.append(sample) + + def latest_updated_at(self, key: str) -> str | None: + with self._lock: + history = self._samples.get(key) + if not history: + return None + return str(history[-1].get("updated_at") or "") + + def describe(self, key: str, current_stats: dict[str, Any] | None) -> dict[str, Any]: + current = self._normalize(current_stats) + with self._lock: + history = list(self._samples.get(key, ())) + + timeline = history + [{"stats": current, "updated_at": utc_iso_now()}] + previous = timeline[-2]["stats"] if len(timeline) >= 2 else None + trend_window = [entry["stats"] for entry in timeline[-TREND_WINDOW_SIZE:]] + deadband = max(2.0, 0.05 * float(max(current.get("window_limit", 0), 1))) + + snd_queue_delta = 0 + snd_buffer_delta = 0 + retrans_delta = 0 + fast_retrans_delta = 0 + lost_delta = 0 + repeat_delta = 0 + out_segs_delta = 0 + if previous is not None: + snd_queue_delta = max(0, current["snd_queue"] - _coerce_int(previous.get("snd_queue"))) + snd_buffer_delta = max(0, current["snd_buffer"] - _coerce_int(previous.get("snd_buffer"))) + retrans_delta = max(0, current["retrans_total"] - _coerce_int(previous.get("retrans_total"))) + fast_retrans_delta = max(0, current["fast_retrans_total"] - _coerce_int(previous.get("fast_retrans_total"))) + lost_delta = max(0, current["lost_total"] - _coerce_int(previous.get("lost_total"))) + repeat_delta = max(0, current["repeat_total"] - _coerce_int(previous.get("repeat_total"))) + out_segs_delta = max(0, current["out_segs_total"] - _coerce_int(previous.get("out_segs_total"))) + + def classify(field: str) -> str: + if len(trend_window) < 2: + return "stable" + oldest = float(_coerce_int(trend_window[0].get(field))) + newest = float(_coerce_int(trend_window[-1].get(field))) + delta = newest - oldest + if abs(delta) < deadband: + return "stable" + return "rising" if delta > 0 else "falling" + + repair_rate_pct = 0.0 + if out_segs_delta > 0: + repair_rate_pct = round((retrans_delta / out_segs_delta) * 100.0, 3) + + return { + "kcp": current, + "trend": { + "snd_queue_delta": snd_queue_delta, + "snd_buffer_delta": snd_buffer_delta, + "snd_queue_trend": classify("snd_queue"), + "snd_buffer_trend": classify("snd_buffer"), + "retrans_delta": retrans_delta, + "fast_retrans_delta": fast_retrans_delta, + "lost_delta": lost_delta, + "repeat_delta": repeat_delta, + "out_segs_delta": out_segs_delta, + "repair_rate_pct": repair_rate_pct, + }, + } + + +class HubTelemetryReceiver: + def __init__(self) -> None: + self._lock = threading.Lock() + self._thread: threading.Thread | None = None + self._started = False + self._session = None + self._session_cls = None + self._msg_type_text = None + self._msg_type_error = None + self._telemetry_defaults: dict[str, Any] = {} + self._latest_snapshot: dict[str, Any] | None = None + self._last_error = "" + self._last_received_wall = 0.0 + self._last_received_monotonic = 0.0 + self._reconnect_count = 0 + self._ever_connected = False + self._closing = threading.Event() + self._load_backend() + + def _load_backend(self) -> None: + try: + self._import_backend() + except Exception as error: # pragma: no cover - optional runtime dependency + self._last_error = f"omnisocket import failed: {error}" + + def _import_backend(self) -> None: + try: + from omnisocket import MSG_TYPE_ERROR, MSG_TYPE_TEXT, Session, TELEMETRY_DEFAULTS # type: ignore + except ImportError: + python_dir = WORKSPACE_ROOT / "OmniSocketGo" / "python" + if python_dir.exists(): + sys.path.insert(0, str(python_dir)) + from omnisocket import MSG_TYPE_ERROR, MSG_TYPE_TEXT, Session, TELEMETRY_DEFAULTS # type: ignore + + self._msg_type_error = MSG_TYPE_ERROR + self._msg_type_text = MSG_TYPE_TEXT + self._session_cls = Session + self._telemetry_defaults = dict(TELEMETRY_DEFAULTS) + + def _connect_session(self): + assert self._session_cls is not None + + config = load_omnisocket_config() + transport_cfg = config.get("transport", {}) + telemetry_cfg = config.get("telemetry_receiver", {}) + + session = self._session_cls() + session.connect( + server_addr=str(transport_cfg.get("server_addr", "127.0.0.1:10909")), + peer_id=str(telemetry_cfg.get("peer_id", "peer-a-telemetry")), + relay_via=str(transport_cfg.get("relay_via", "")), + bind_ip=str(transport_cfg.get("bind_ip", "")), + bind_device=str(transport_cfg.get("bind_device", "")), + **self._telemetry_defaults, + ) + return session + + def ensure_started(self) -> None: + if self._session_cls is None: + return + + with self._lock: + if self._started or self._closing.is_set(): + return + self._started = True + self._thread = threading.Thread( + target=self._run, + name="hub-telemetry-receiver", + daemon=True, + ) + self._thread.start() + + def _run(self) -> None: + while not self._closing.is_set(): + try: + session = self._connect_session() + with self._lock: + self._session = session + self._last_error = "" + if self._ever_connected: + self._reconnect_count += 1 + else: + self._ever_connected = True + + while not self._closing.is_set(): + result = session.recv(timeout_ms=1000) + if result is None: + continue + + from_peer, msg_type, payload = result + if msg_type == self._msg_type_error: + with self._lock: + self._last_error = f"hub error from {from_peer}: {payload.decode('utf-8', errors='replace')}" + continue + if msg_type != self._msg_type_text: + continue + + snapshot = json.loads(payload.decode("utf-8")) + if snapshot.get("type") != "hub_kcp_snapshot": + continue + + now_wall = time.time() + now_mono = time.monotonic() + with self._lock: + self._latest_snapshot = snapshot + self._last_received_wall = now_wall + self._last_received_monotonic = now_mono + self._last_error = "" + except Exception as error: # pragma: no cover - runtime integration path + if not self._closing.is_set(): + session_error = "" + if self._session is not None: + try: + session_error = str(dict(self._session.stats()).get("last_server_error", "") or "") + except Exception: + session_error = "" + with self._lock: + self._last_error = session_error or str(error) + finally: + with self._lock: + session = self._session + self._session = None + if self._closing.is_set(): + self._started = False + if session is not None: + try: + session.close() + except Exception: + pass + if not self._closing.is_set(): + time.sleep(2) + + def get_snapshot(self) -> dict[str, Any]: + self.ensure_started() + cfg = load_omnisocket_config().get("telemetry_receiver", {}) + stale_after_ms = max(500, int(cfg.get("stale_after_ms", 1500))) + + with self._lock: + received_monotonic = self._last_received_monotonic + received_wall = self._last_received_wall + snapshot = self._latest_snapshot + connected = self._session is not None + last_error = self._last_error + reconnect_count = self._reconnect_count + if self._session is not None: + try: + session_stats = dict(self._session.stats()) + except Exception: + session_stats = {} + else: + session_stats = {} + + stale = True + if received_monotonic > 0.0: + stale = (time.monotonic() - received_monotonic) * 1000.0 > stale_after_ms + + return { + "connected": connected, + "updated_at": _utc_from_epoch(received_wall), + "received_at_monotonic": received_monotonic, + "stale": stale, + "peer_id": str(cfg.get("peer_id", "peer-a-telemetry")), + "snapshot": snapshot or {"sessions": []}, + "last_error": last_error, + "registered": bool(session_stats.get("registered", 0)), + "last_server_error": str(session_stats.get("last_server_error", "") or ""), + "reconnect_count": reconnect_count, + } + + def close(self) -> None: + self._closing.set() + with self._lock: + session = self._session + if session is not None: + try: + session.close() + except Exception: + pass + thread = self._thread + if thread is not None and thread.is_alive(): + thread.join(timeout=0.5) + + +class NetworkTelemetryService: + def __init__( + self, + video_receiver: OmniSocketVideoReceiver, + control_sender: OmniSocketControlSender, + control_ack_tracker: ControlAckTracker, + control_ack_receiver: OmniSocketControlAckReceiver, + control_arbiter: ControlArbiter, + native_ingress: NativeUdpControlIngress, + hub_receiver: HubTelemetryReceiver, + video_display_probe_store: VideoDisplayProbeStore, + ) -> None: + self._video_receiver = video_receiver + self._control_sender = control_sender + self._control_ack_tracker = control_ack_tracker + self._control_ack_receiver = control_ack_receiver + self._control_arbiter = control_arbiter + self._native_ingress = native_ingress + self._hub_receiver = hub_receiver + self._video_display_probe_store = video_display_probe_store + self._trend_tracker = KcpTrendTracker() + self._rate_lock = threading.Lock() + self._last_rate_sample: tuple[float, int, int] | None = None + self._sample_thread: threading.Thread | None = None + self._sample_started = False + self._last_remote_snapshot_at = 0.0 + self._closing = threading.Event() + + def _ensure_started(self) -> None: + self._video_receiver.ensure_started() + self._control_arbiter.ensure_started() + self._control_ack_receiver.ensure_started() + self._native_ingress.ensure_started() + self._hub_receiver.ensure_started() + with self._rate_lock: + if self._sample_started or self._closing.is_set(): + return + self._sample_started = True + self._sample_thread = threading.Thread( + target=self._sample_loop, + name="network-telemetry-sampler", + daemon=True, + ) + self._sample_thread.start() + + def _sample_loop(self) -> None: + interval_seconds = LOCAL_SAMPLE_INTERVAL_MS / 1000.0 + while not self._closing.is_set(): + try: + self._trend_tracker.add_sample("a_to_d.video", self._video_receiver.session_kcp_stats()) + self._trend_tracker.add_sample("a_to_d.control", self._control_sender.session_kcp_stats()) + except Exception: + pass + time.sleep(interval_seconds) + + def _compute_rates(self, send_bytes: int, recv_bytes: int) -> tuple[float, float]: + now = time.monotonic() + with self._rate_lock: + previous = self._last_rate_sample + self._last_rate_sample = (now, send_bytes, recv_bytes) + + if previous is None: + return 0.0, 0.0 + + prev_time, prev_send, prev_recv = previous + elapsed = now - prev_time + if elapsed <= 0.0: + return 0.0, 0.0 + + tx_kbps = max(0.0, ((send_bytes - prev_send) * 8.0) / elapsed / 1000.0) + rx_kbps = max(0.0, ((recv_bytes - prev_recv) * 8.0) / elapsed / 1000.0) + return tx_kbps, rx_kbps + + def _ingest_remote_snapshot(self, telemetry_state: dict[str, Any]) -> None: + received_at = float(telemetry_state.get("received_at_monotonic") or 0.0) + if received_at <= 0.0 or received_at <= self._last_remote_snapshot_at: + return + + snapshot = telemetry_state.get("snapshot") or {} + sessions = snapshot.get("sessions") or [] + for session in sessions: + peer_id = str(session.get("peer_id", "")).strip() + if not peer_id: + continue + self._trend_tracker.add_sample(f"hub::{peer_id}", session) + self._last_remote_snapshot_at = received_at + + def _build_session_payload( + self, + trend_key: str, + peer_id: str, + app_stats: dict[str, Any] | None, + current_kcp: dict[str, Any] | None, + updated_at: str | None, + stale: bool, + ) -> dict[str, Any]: + described = self._trend_tracker.describe(trend_key, current_kcp) + connected = bool(described["kcp"].get("connected")) + if app_stats is not None and "registered" in app_stats: + connected = bool(app_stats.get("registered")) + return { + "peer_id": peer_id, + "connected": connected, + "updated_at": updated_at, + "stale": stale, + "app": app_stats, + "kcp": described["kcp"], + "trend": described["trend"], + } + + def _build_link(self, source: str, updated_at: str | None, stale: bool, sessions: dict[str, dict[str, Any]]) -> dict[str, Any]: + session_items = list(sessions.values()) + active_sessions = [session for session in session_items if session.get("connected") and not session.get("stale")] + retrans_sum = sum(_coerce_int(session.get("trend", {}).get("retrans_delta")) for session in active_sessions) + out_segs_sum = sum(_coerce_int(session.get("trend", {}).get("out_segs_delta")) for session in active_sessions) + repair_rate_pct = round((retrans_sum / out_segs_sum) * 100.0, 3) if out_segs_sum > 0 else 0.0 + + return { + "source": source, + "updated_at": updated_at, + "stale": stale, + "aggregate": { + "online_sessions": len(active_sessions), + "max_window_pressure_pct": max( + (_coerce_float(session.get("kcp", {}).get("window_pressure_pct")) for session in active_sessions), + default=0.0, + ), + "sum_snd_queue": sum(_coerce_int(session.get("kcp", {}).get("snd_queue")) for session in active_sessions), + "sum_snd_buffer": sum(_coerce_int(session.get("kcp", {}).get("snd_buffer")) for session in active_sessions), + "sum_retrans_delta": retrans_sum, + "sum_out_segs_delta": out_segs_sum, + "repair_rate_pct": repair_rate_pct, + }, + "sessions": sessions, + } + + def _pick_primary_session(self, links: dict[str, dict[str, Any]]) -> dict[str, Any] | None: + candidates = ( + links["a_to_d"]["sessions"]["control"], + links["a_to_d"]["sessions"]["video"], + links["d_to_b"]["sessions"]["control"], + links["d_to_b"]["sessions"]["video"], + ) + for session in candidates: + if session.get("connected") and not session.get("stale"): + return session + return None + + def _derive_robot_health( + self, + *, + video_receiver_status: dict[str, Any], + local_control_registered: bool, + remote_control_fresh: bool, + remote_video_fresh: bool, + telemetry_state: dict[str, Any], + watchdog_status: dict[str, Any] | None, + ) -> dict[str, Any]: + if watchdog_status is not None: + explicit_health = self._derive_robot_health_from_watchdog(watchdog_status) + if explicit_health is not None: + return explicit_health + + has_recent_frame = bool(video_receiver_status.get("has_recent_frame")) + telemetry_connected = bool(telemetry_state.get("connected")) + telemetry_stale = bool(telemetry_state.get("stale", True)) + + if has_recent_frame and remote_control_fresh and remote_video_fresh: + fault_reason = "none" + recovery_state = "ok" + elif not remote_control_fresh and not remote_video_fresh and not has_recent_frame: + fault_reason = "network_or_robot_unreachable" + recovery_state = "recovering" if telemetry_connected and not telemetry_stale else "degraded" + elif remote_control_fresh and not remote_video_fresh: + fault_reason = "video_session_recovering" + recovery_state = "recovering" + elif not remote_control_fresh and local_control_registered: + fault_reason = "control_session_recovering" + recovery_state = "recovering" + elif remote_control_fresh and not has_recent_frame: + fault_reason = "video_pipeline_stalled" + recovery_state = "degraded" + else: + fault_reason = "unknown" + recovery_state = "degraded" + + return { + "fault_reason": fault_reason, + "recovery_state": recovery_state, + "confidence": "derived", + "updated_at": utc_iso_now(), + } + + def _derive_robot_health_from_watchdog(self, watchdog_status: dict[str, Any]) -> dict[str, Any] | None: + updated_at_epoch_ms = _coerce_int(watchdog_status.get("updated_at_epoch_ms")) + if updated_at_epoch_ms <= 0: + return None + + now_epoch_ms = int(time.time() * 1000) + if now_epoch_ms - updated_at_epoch_ms > WATCHDOG_STATUS_STALE_MS: + return None + + raw_fault_reason = str(watchdog_status.get("fault_reason", "") or "") + raw_recovery_state = str(watchdog_status.get("recovery_state", "") or "") + normalized_fault_reason = "unknown" + normalized_recovery_state = raw_recovery_state or "degraded" + mapped_health = WATCHDOG_FAULT_REASON_MAP.get(raw_fault_reason) + + if mapped_health is not None: + normalized_fault_reason, recovery_override = mapped_health + if recovery_override is not None: + normalized_recovery_state = recovery_override + if raw_recovery_state == "backoff": + normalized_recovery_state = "backoff" + + return { + "fault_reason": normalized_fault_reason, + "recovery_state": normalized_recovery_state, + "confidence": "derived", + "updated_at": _utc_from_epoch(updated_at_epoch_ms / 1000.0) or utc_iso_now(), + } + + def _derive_latency_estimate( + self, + *, + links: dict[str, dict[str, Any]], + video_receiver_status: dict[str, Any], + display_probe_status: dict[str, Any], + ) -> dict[str, Any]: + a_to_d_control_raw = links["a_to_d"]["sessions"]["control"]["kcp"].get("srtt_ms") + d_to_b_control_raw = links["d_to_b"]["sessions"]["control"]["kcp"].get("srtt_ms") + a_to_d_control_min_raw = links["a_to_d"]["sessions"]["control"]["kcp"].get("min_srtt_ms") + d_to_b_control_min_raw = links["d_to_b"]["sessions"]["control"]["kcp"].get("min_srtt_ms") + a_to_d_video_raw = links["a_to_d"]["sessions"]["video"]["kcp"].get("srtt_ms") + d_to_b_video_raw = links["d_to_b"]["sessions"]["video"]["kcp"].get("srtt_ms") + + a_to_d_control = _coerce_float(a_to_d_control_raw) if a_to_d_control_raw is not None else None + d_to_b_control = _coerce_float(d_to_b_control_raw) if d_to_b_control_raw is not None else None + a_to_d_control_min = _coerce_float(a_to_d_control_min_raw) if a_to_d_control_min_raw is not None else None + d_to_b_control_min = _coerce_float(d_to_b_control_min_raw) if d_to_b_control_min_raw is not None else None + a_to_d_video = _coerce_float(a_to_d_video_raw) if a_to_d_video_raw is not None else None + d_to_b_video = _coerce_float(d_to_b_video_raw) if d_to_b_video_raw is not None else None + ack_estimate = self._control_ack_tracker.get_latest_estimate() + capture_to_send_raw = video_receiver_status.get("latest_capture_to_send_ms") + request_to_paint_raw = display_probe_status.get("request_to_paint_ms") + capture_to_send_ms = _coerce_float(capture_to_send_raw) if capture_to_send_raw is not None else None + request_to_paint_ms = _coerce_float(request_to_paint_raw) if request_to_paint_raw is not None else None + video_network_oneway_est_ms = ( + round((a_to_d_video + d_to_b_video) / 2.0, 3) + if a_to_d_video is not None and d_to_b_video is not None + else None + ) + video_partial_est_ms = None + if capture_to_send_ms is not None and video_network_oneway_est_ms is not None: + video_partial_est_ms = round(capture_to_send_ms + video_network_oneway_est_ms, 3) + video_e2e_est_ms = None + if video_partial_est_ms is not None and request_to_paint_ms is not None: + video_e2e_est_ms = round(video_partial_est_ms + request_to_paint_ms, 3) + + return { + "control_loop_rtt_ms": ack_estimate.get("control_loop_rtt_ms"), + "control_to_persist_est_ms": ack_estimate.get("control_to_persist_est_ms"), + "control_oneway_srtt_est_ms": ( + round((a_to_d_control + d_to_b_control) / 2.0, 3) + if a_to_d_control is not None and d_to_b_control is not None + else None + ), + "control_oneway_bestcase_est_ms": ( + round((a_to_d_control_min + d_to_b_control_min) / 2.0, 3) + if a_to_d_control_min is not None and d_to_b_control_min is not None + else None + ), + "video_network_oneway_est_ms": video_network_oneway_est_ms, + "video_partial_est_ms": video_partial_est_ms, + "video_e2e_est_ms": video_e2e_est_ms, + "estimate_method": { + "control": "ack_loop" if ack_estimate.get("ack_available") else "srtt_fallback", + "video": "capture_to_send+srtt/2+request_to_paint" if video_e2e_est_ms is not None else "capture_to_send+srtt/2", + }, + "clock_sync_required": False, + "assumptions": [ + "control one-way estimate uses ACK loop when available", + "video one-way estimate uses per-leg SRTT and local paint timing", + ], + "confidence": { + "control": "derived_ack" if ack_estimate.get("ack_available") else "fallback_srtt", + "video": "derived_local_probe" if video_e2e_est_ms is not None else "partial_without_probe", + }, + } + + def get_latest(self) -> dict[str, Any]: + self._ensure_started() + + config = load_omnisocket_config() + video_receiver_cfg = config.get("video_receiver", {}) + control_sender_cfg = config.get("control_sender", {}) + video_sender_cfg = config.get("video_sender", {}) + + video_app = self._video_receiver.session_stats() + control_app = self._control_sender.session_stats() + video_kcp = self._video_receiver.session_kcp_stats() + control_kcp = self._control_sender.session_kcp_stats() + video_receiver_status = self._video_receiver.get_status() + arbiter_status = self._control_arbiter.get_status() + ingress_status = self._native_ingress.get_status() + sender_status = self._control_sender.get_status() + ack_receiver_status = self._control_ack_receiver.get_status() + ack_status = self._control_ack_tracker.get_latest_estimate() + telemetry_state = self._hub_receiver.get_snapshot() + display_probe_status = self._video_display_probe_store.get_status() + + total_send_bytes = int(video_app.get("send_bytes", 0)) + int(control_app.get("send_bytes", 0)) + total_recv_bytes = int(video_app.get("recv_bytes", 0)) + int(control_app.get("recv_bytes", 0)) + tx_kbps, rx_kbps = self._compute_rates(total_send_bytes, total_recv_bytes) + + local_updated_at = utc_iso_now() + local_sessions = { + "video": self._build_session_payload( + "a_to_d.video", + str(video_receiver_cfg.get("peer_id", "peer-a-video")), + video_app, + video_kcp, + local_updated_at, + False, + ), + "control": self._build_session_payload( + "a_to_d.control", + str(control_sender_cfg.get("peer_id", "peer-a-ctrl")), + control_app, + control_kcp, + local_updated_at, + False, + ), + } + + remote_snapshot = telemetry_state.get("snapshot") or {} + remote_sessions_by_peer = { + str(session.get("peer_id", "")).strip(): session + for session in remote_snapshot.get("sessions", []) or [] + if str(session.get("peer_id", "")).strip() + } + remote_updated_at = telemetry_state.get("updated_at") + remote_stale = bool(telemetry_state.get("stale", True)) + remote_sessions = { + "video": self._build_session_payload( + f"hub::{str(video_sender_cfg.get('peer_id', 'peer-b-video'))}", + str(video_sender_cfg.get("peer_id", "peer-b-video")), + None, + remote_sessions_by_peer.get(str(video_sender_cfg.get("peer_id", "peer-b-video")), {}), + remote_updated_at, + remote_stale, + ), + "control": self._build_session_payload( + f"hub::{str(control_sender_cfg.get('target_peer', 'peer-b-ctrl'))}", + str(control_sender_cfg.get("target_peer", "peer-b-ctrl")), + None, + remote_sessions_by_peer.get(str(control_sender_cfg.get("target_peer", "peer-b-ctrl")), {}), + remote_updated_at, + remote_stale, + ), + } + self._video_receiver.update_remote_video_srtt( + _coerce_int(remote_sessions["video"]["kcp"].get("srtt_ms")) if remote_sessions["video"]["kcp"].get("srtt_ms") is not None else None + ) + + links = { + "a_to_d": self._build_link("local-a-side", local_updated_at, False, local_sessions), + "d_to_b": self._build_link("hub-telemetry", remote_updated_at, remote_stale, remote_sessions), + } + + primary_session = self._pick_primary_session(links) + primary_kcp = dict(primary_session.get("kcp", {})) if primary_session is not None else {} + self._ingest_remote_snapshot(telemetry_state) + + fresh_connected_sessions = ( + links["a_to_d"]["aggregate"]["online_sessions"] + links["d_to_b"]["aggregate"]["online_sessions"] + ) + latency_ms = primary_kcp.get("srtt_ms") if primary_session is not None else None + jitter_ms = primary_kcp.get("srttvar_ms") if primary_session is not None else None + local_control_registered = bool(control_app.get("registered", 0)) + remote_control_fresh = bool(remote_sessions["control"].get("connected")) and not bool(remote_sessions["control"].get("stale")) + remote_video_fresh = bool(remote_sessions["video"].get("connected")) and not bool(remote_sessions["video"].get("stale")) + watchdog_status = _load_optional_json(WATCHDOG_STATUS_PATH) + robot_health = self._derive_robot_health( + video_receiver_status=video_receiver_status, + local_control_registered=local_control_registered, + remote_control_fresh=remote_control_fresh, + remote_video_fresh=remote_video_fresh, + telemetry_state=telemetry_state, + watchdog_status=watchdog_status, + ) + latency_estimate = self._derive_latency_estimate( + links=links, + video_receiver_status=video_receiver_status, + display_probe_status=display_probe_status, + ) + + if local_control_registered and remote_control_fresh: + peer_status = "online" + elif local_control_registered or bool(local_sessions["video"].get("connected")): + peer_status = "degraded" + elif sender_status.get("backend_ready"): + peer_status = "idle" + else: + peer_status = "backend-unavailable" + + return { + "peer_status": peer_status, + "latency_ms": latency_ms, + "jitter_ms": jitter_ms, + "packet_loss_pct": None, + "tx_kbps": round(tx_kbps, 3), + "rx_kbps": round(rx_kbps, 3), + "transport": "OmniSocket / kcp", + "source_mode": "omnisocket-live" if fresh_connected_sessions > 0 else "omnisocket-idle", + "updated_at": utc_iso_now(), + "active_control_source": arbiter_status["active_source"], + "control_lease_remaining_ms": arbiter_status["control_lease_remaining_ms"], + "combined": { + "connected_sessions": fresh_connected_sessions, + "send_bytes": total_send_bytes, + "recv_bytes": total_recv_bytes, + "tx_kbps": round(tx_kbps, 3), + "rx_kbps": round(rx_kbps, 3), + }, + "sessions": { + "video": { + "app": video_app, + "kcp": local_sessions["video"]["kcp"], + }, + "control": { + "app": control_app, + "kcp": local_sessions["control"]["kcp"], + }, + }, + "links": links, + "latency_estimate": latency_estimate, + "video_freshness": video_receiver_status.get("freshness", {}), + "control_ack_status": { + **ack_status, + "receiver": ack_receiver_status, + }, + "telemetry_receiver": { + "hub_connected": bool(telemetry_state.get("connected")), + "hub_updated_at": telemetry_state.get("updated_at"), + "hub_stale": remote_stale, + "last_error": telemetry_state.get("last_error", ""), + "peer_id": telemetry_state.get("peer_id", ""), + "registered": bool(telemetry_state.get("registered", False)), + "last_server_error": str(telemetry_state.get("last_server_error", "") or ""), + "reconnect_count": int(telemetry_state.get("reconnect_count", 0)), + }, + "robot_health": robot_health, + "ingress": { + "native_udp": ingress_status, + }, + "control": { + "arbiter": arbiter_status, + "sender": sender_status, + "ack_receiver": ack_receiver_status, + }, + } + + def close(self) -> None: + self._closing.set() + thread = self._sample_thread + if thread is not None and thread.is_alive(): + thread.join(timeout=0.5) diff --git a/host/robot-command-center/backend/monitoring/urls.py b/host/robot-command-center/backend/monitoring/urls.py new file mode 100644 index 0000000..a4145ef --- /dev/null +++ b/host/robot-command-center/backend/monitoring/urls.py @@ -0,0 +1,16 @@ +from django.urls import path + +from . import views + + +urlpatterns = [ + path("dashboard/", views.dashboard_snapshot, name="dashboard-snapshot"), + path("gps/latest/", views.gps_latest, name="gps-latest"), + path("network/latest/", views.network_latest, name="network-latest"), + path("clock/calibrate/", views.clock_calibration, name="clock-calibration"), + path("video/status/", views.video_status, name="video-status"), + path("video/camera/", views.video_camera, name="video-camera"), + path("video/frame/", views.video_frame, name="video-frame"), + path("video/display-probe/", views.video_display_probe, name="video-display-probe"), + path("video/stream/", views.video_stream, name="video-stream"), +] diff --git a/host/robot-command-center/backend/monitoring/video.py b/host/robot-command-center/backend/monitoring/video.py new file mode 100644 index 0000000..9dc24b4 --- /dev/null +++ b/host/robot-command-center/backend/monitoring/video.py @@ -0,0 +1,751 @@ +from __future__ import annotations + +from collections import deque +from dataclasses import dataclass +import hashlib +import math +import struct +import sys +import threading +import time +from typing import Any, Iterator + +from .common import ( + JPEG_FRAME_DIR, + OMNISOCKET_CONFIG_PATH, + OMNISOCKET_FRAME_FRESH_SECONDS, + JsonlRunLogger, + VIDEO_SOURCE_MODE, + VIDEO_TIMESTAMP_SAMPLE_SIZE, + VIDEO_TRAILER_BYTES, + VIDEO_TRAILER_ENDIANNESS, + VIDEO_TRAILER_STRUCT, + VIDEO_TRAILER_TIMESTAMP_MAX_SKEW_NS, + VIDEO_TRAILER_TIMESTAMP_MULTIPLIER_NS, + VIDEO_TRAILER_TIMESTAMP_UNIT, + WORKSPACE_ROOT, + load_omnisocket_config, +) + + +def safe_kcp_stats(session: Any) -> dict[str, Any]: + if session is None or not hasattr(session, "kcp_stats"): + return {} + try: + return dict(session.kcp_stats()) + except Exception: + return {} + + +class VideoDisplayProbeStore: + def __init__(self) -> None: + self._lock = threading.Lock() + self._logger = JsonlRunLogger("BLITZ_A_VIDEO_DISPLAY_PROBE_LOG_PATH", "a-video-display-probe") + self._latest: VideoDisplayProbeStatus = VideoDisplayProbeStatus( + updated_at=None, + frame_seq=None, + frame_hash="", + input_to_next_fresh_frame_ms=None, + input_to_next_changed_frame_ms=None, + input_to_next_paint_ms=None, + request_to_paint_ms=None, + response_to_paint_ms=None, + backend_to_request_ms=None, + backend_to_request_ms_raw=None, + backend_to_paint_ms=None, + backend_to_paint_ms_raw=None, + browser_backend_clock_offset_ms=None, + browser_backend_clock_rtt_ms=None, + browser_backend_clock_sample_count=0, + browser_backend_clock_calibrated_at=None, + ) + + def record_event(self, payload: dict[str, Any]) -> None: + backend_received_unix_ns = payload.get("backend_received_unix_ns") + request_started_unix_ms = payload.get("request_started_unix_ms") + response_received_unix_ms = payload.get("response_received_unix_ms") + paint_unix_ms = payload.get("paint_unix_ms") + browser_backend_clock_offset_ms = self._coerce_float(payload.get("browser_backend_clock_offset_ms")) + browser_backend_clock_rtt_ms = self._coerce_float(payload.get("browser_backend_clock_rtt_ms")) + browser_backend_clock_sample_count = self._coerce_int(payload.get("browser_backend_clock_sample_count")) + browser_backend_clock_calibrated_at = self._coerce_text(payload.get("browser_backend_clock_calibrated_at")) + request_to_paint_ms = self._duration_ms(paint_unix_ms, request_started_unix_ms, clamp_floor_zero=True) + response_to_paint_ms = self._duration_ms(paint_unix_ms, response_received_unix_ms, clamp_floor_zero=True) + backend_received_unix_ms = None + try: + if backend_received_unix_ns is not None: + backend_received_unix_ms = int(backend_received_unix_ns) / 1_000_000.0 + except (TypeError, ValueError): + backend_received_unix_ms = None + backend_received_browser_unix_ms = None + if backend_received_unix_ms is not None and browser_backend_clock_offset_ms is not None: + backend_received_browser_unix_ms = round(backend_received_unix_ms + browser_backend_clock_offset_ms, 3) + backend_to_request_ms_raw = self._duration_ms(request_started_unix_ms, backend_received_unix_ms, clamp_floor_zero=False) + backend_to_paint_ms_raw = self._duration_ms(paint_unix_ms, backend_received_unix_ms, clamp_floor_zero=False) + backend_to_request_ms = self._duration_ms( + request_started_unix_ms, + backend_received_browser_unix_ms, + clamp_floor_zero=True, + ) + backend_to_paint_ms = self._duration_ms( + paint_unix_ms, + backend_received_browser_unix_ms, + clamp_floor_zero=True, + ) + + status = VideoDisplayProbeStatus( + updated_at=self._coerce_text(payload.get("updated_at")), + frame_seq=int(payload["frame_seq"]) if payload.get("frame_seq") is not None else None, + frame_hash=str(payload.get("frame_hash") or ""), + input_to_next_fresh_frame_ms=self._coerce_float(payload.get("input_to_next_fresh_frame_ms")), + input_to_next_changed_frame_ms=self._coerce_float(payload.get("input_to_next_changed_frame_ms")), + input_to_next_paint_ms=self._coerce_float(payload.get("input_to_next_paint_ms")), + request_to_paint_ms=request_to_paint_ms, + response_to_paint_ms=response_to_paint_ms, + backend_to_request_ms=backend_to_request_ms, + backend_to_request_ms_raw=backend_to_request_ms_raw, + backend_to_paint_ms=backend_to_paint_ms, + backend_to_paint_ms_raw=backend_to_paint_ms_raw, + browser_backend_clock_offset_ms=browser_backend_clock_offset_ms, + browser_backend_clock_rtt_ms=browser_backend_clock_rtt_ms, + browser_backend_clock_sample_count=browser_backend_clock_sample_count, + browser_backend_clock_calibrated_at=browser_backend_clock_calibrated_at, + ) + logged_payload = dict(payload) + logged_payload.update( + { + "request_to_paint_ms": request_to_paint_ms, + "response_to_paint_ms": response_to_paint_ms, + "backend_received_browser_unix_ms": backend_received_browser_unix_ms, + "backend_to_request_ms": backend_to_request_ms, + "backend_to_request_ms_raw": backend_to_request_ms_raw, + "backend_to_paint_ms": backend_to_paint_ms, + "backend_to_paint_ms_raw": backend_to_paint_ms_raw, + } + ) + with self._lock: + self._latest = status + self._logger.write(logged_payload) + + def get_status(self) -> dict[str, Any]: + with self._lock: + latest = self._latest + return { + "updated_at": latest.updated_at, + "frame_seq": latest.frame_seq, + "frame_hash": latest.frame_hash, + "input_to_next_fresh_frame_ms": latest.input_to_next_fresh_frame_ms, + "input_to_next_changed_frame_ms": latest.input_to_next_changed_frame_ms, + "input_to_next_paint_ms": latest.input_to_next_paint_ms, + "request_to_paint_ms": latest.request_to_paint_ms, + "response_to_paint_ms": latest.response_to_paint_ms, + "backend_to_request_ms": latest.backend_to_request_ms, + "backend_to_request_ms_raw": latest.backend_to_request_ms_raw, + "backend_to_paint_ms": latest.backend_to_paint_ms, + "backend_to_paint_ms_raw": latest.backend_to_paint_ms_raw, + "browser_backend_clock_offset_ms": latest.browser_backend_clock_offset_ms, + "browser_backend_clock_rtt_ms": latest.browser_backend_clock_rtt_ms, + "browser_backend_clock_sample_count": latest.browser_backend_clock_sample_count, + "browser_backend_clock_calibrated_at": latest.browser_backend_clock_calibrated_at, + } + + def close(self) -> None: + self._logger.close() + + @staticmethod + def _coerce_float(value: Any) -> float | None: + try: + if value is None: + return None + return round(float(value), 3) + except (TypeError, ValueError): + return None + + @staticmethod + def _coerce_int(value: Any) -> int: + try: + if value is None: + return 0 + return int(value) + except (TypeError, ValueError): + return 0 + + @staticmethod + def _coerce_text(value: Any) -> str | None: + text = str(value or "").strip() + return text or None + + @classmethod + def _duration_ms(cls, end_ms: Any, start_ms: Any, *, clamp_floor_zero: bool) -> float | None: + end_value = cls._coerce_float(end_ms) + start_value = cls._coerce_float(start_ms) + if end_value is None or start_value is None: + return None + delta = round(end_value - start_value, 3) + if clamp_floor_zero: + delta = max(0.0, delta) + return delta + + +@dataclass(frozen=True) +class FrameTrailerMetadata: + timestamp_ns: int + latitude: float | None + longitude: float | None + capture_to_send_ms: int + raw_latitude_hex: str + raw_longitude_hex: str + received_at: float + + @property + def has_gps_fix(self) -> bool: + return self.latitude is not None and self.longitude is not None + + +@dataclass(frozen=True) +class VideoDisplayProbeStatus: + updated_at: str | None + frame_seq: int | None + frame_hash: str + input_to_next_fresh_frame_ms: float | None + input_to_next_changed_frame_ms: float | None + input_to_next_paint_ms: float | None + request_to_paint_ms: float | None + response_to_paint_ms: float | None + backend_to_request_ms: float | None + backend_to_request_ms_raw: float | None + backend_to_paint_ms: float | None + backend_to_paint_ms_raw: float | None + browser_backend_clock_offset_ms: float | None + browser_backend_clock_rtt_ms: float | None + browser_backend_clock_sample_count: int + browser_backend_clock_calibrated_at: str | None + + +class OmniSocketVideoReceiver: + def __init__(self) -> None: + self._lock = threading.Lock() + self._thread: threading.Thread | None = None + self._started = False + self._session = None + self._session_cls = None + self._binary_msg_type = None + self._video_defaults: dict[str, Any] = {} + self._latest_frame: bytes | None = None + self._latest_received_at = 0.0 + self._latest_backend_received_unix_ns = 0 + self._latest_backend_received_mono_ns = 0 + self._latest_sequence: int | None = None + self._latest_metadata: FrameTrailerMetadata | None = None + self._latest_sender_clock_delta_ms_raw: float | None = None + self._latest_timestamp_unit: str | None = None + self._latest_timestamp_endianness: str | None = None + self._sender_clock_delta_samples_ms_raw: deque[float] = deque(maxlen=VIDEO_TIMESTAMP_SAMPLE_SIZE) + self._latest_frame_hash = "" + self._latest_frame_bytes = 0 + self._last_frame_hash = "" + self._last_sequence: int | None = None + self._last_backend_received_mono_ns = 0 + self._interarrival_ms_samples: deque[float] = deque(maxlen=120) + self._repeat_samples: deque[int] = deque(maxlen=120) + self._skip_samples: deque[int] = deque(maxlen=120) + self._freeze_samples_ms: deque[float] = deque(maxlen=120) + self._current_stale_frame_run_length = 0 + self._latest_remote_video_srtt_ms: int | None = None + self._frames_received = 0 + self._last_error = "" + self._reconnect_count = 0 + self._ever_connected = False + self._closing = threading.Event() + self._frame_recv_logger = JsonlRunLogger("BLITZ_A_VIDEO_FRAME_RECV_LOG_PATH", "a-video-frame-recv") + self._load_backend() + + def _load_backend(self) -> None: + try: + self._import_backend() + except Exception as error: # pragma: no cover - optional runtime dependency + self._last_error = f"omnisocket import failed: {error}" + + def _import_backend(self) -> None: + try: + from omnisocket import MSG_TYPE_BINARY, Session, VIDEO_DEFAULTS # type: ignore + except ImportError: + python_dir = WORKSPACE_ROOT / "OmniSocketGo" / "python" + if python_dir.exists(): + sys.path.insert(0, str(python_dir)) + from omnisocket import MSG_TYPE_BINARY, Session, VIDEO_DEFAULTS # type: ignore + + self._binary_msg_type = MSG_TYPE_BINARY + self._session_cls = Session + self._video_defaults = dict(VIDEO_DEFAULTS) + + def ensure_started(self) -> None: + if self._session_cls is None or self._binary_msg_type is None: + return + + with self._lock: + if self._started or self._closing.is_set(): + return + self._started = True + self._thread = threading.Thread( + target=self._run, + name="omnisocket-video-receiver", + daemon=True, + ) + self._thread.start() + + def _connect_session(self): + assert self._session_cls is not None + + config = load_omnisocket_config() + transport_cfg = config.get("transport", {}) + video_cfg = config.get("video_receiver", {}) + + session = self._session_cls() + session.connect( + server_addr=str(transport_cfg.get("server_addr", "127.0.0.1:10909")), + peer_id=str(video_cfg.get("peer_id", "peer-a-video")), + relay_via=str(transport_cfg.get("relay_via", "")), + bind_ip=str(transport_cfg.get("bind_ip", "")), + bind_device=str(transport_cfg.get("bind_device", "")), + **self._video_defaults, + ) + return session, int(video_cfg.get("buffer_bytes", 1024 * 1024)) + + def _extract_jpeg_payload(self, frame: bytes) -> bytes | None: + if frame.startswith(b"\xff\xd8"): + return frame + if len(frame) > 8 and frame[8:10] == b"\xff\xd8": + return frame[8:] + return None + + def _split_jpeg_frame_and_trailer(self, frame: bytes) -> tuple[bytes, bytes] | None: + jpeg_payload = self._extract_jpeg_payload(frame) + if jpeg_payload is None: + return None + + if jpeg_payload.endswith(b"\xff\xd9"): + return jpeg_payload, b"" + + if ( + len(jpeg_payload) >= VIDEO_TRAILER_BYTES + 2 + and jpeg_payload[-(VIDEO_TRAILER_BYTES + 2) : -VIDEO_TRAILER_BYTES] == b"\xff\xd9" + ): + return jpeg_payload[:-VIDEO_TRAILER_BYTES], jpeg_payload[-VIDEO_TRAILER_BYTES:] + + eoi_index = jpeg_payload.rfind(b"\xff\xd9") + if eoi_index < 0: + return jpeg_payload, b"" + + trailer_start = eoi_index + 2 + return jpeg_payload[:trailer_start], jpeg_payload[trailer_start:] + + def _extract_jpeg_frame(self, frame: bytes) -> bytes | None: + split_payload = self._split_jpeg_frame_and_trailer(frame) + if split_payload is None: + return None + jpeg_frame, _ = split_payload + return jpeg_frame + + def _extract_sequence(self, frame: bytes) -> int | None: + if len(frame) >= 8 and not frame.startswith(b"\xff\xd8"): + return int.from_bytes(frame[:8], "big") + return None + + def _extract_frame_tail(self, frame: bytes) -> bytes: + split_payload = self._split_jpeg_frame_and_trailer(frame) + if split_payload is None: + return b"" + _, trailer = split_payload + return trailer + + def _extract_frame_metadata(self, frame: bytes, received_at: float | None = None) -> FrameTrailerMetadata | None: + trailer = self._extract_frame_tail(frame) + if len(trailer) != VIDEO_TRAILER_BYTES: + return None + + try: + timestamp_ms, latitude, longitude, capture_to_send_ms = VIDEO_TRAILER_STRUCT.unpack(trailer) + except struct.error: + return None + + if timestamp_ms <= 0: + return None + + timestamp_ns = timestamp_ms * VIDEO_TRAILER_TIMESTAMP_MULTIPLIER_NS + if abs(time.time_ns() - timestamp_ns) > VIDEO_TRAILER_TIMESTAMP_MAX_SKEW_NS: + return None + + gps_fix_available = ( + math.isfinite(latitude) + and math.isfinite(longitude) + and (-90.0 <= latitude <= 90.0) + and (-180.0 <= longitude <= 180.0) + and not (abs(latitude) < 1e-9 and abs(longitude) < 1e-9) + ) + + return FrameTrailerMetadata( + timestamp_ns=timestamp_ns, + latitude=latitude if gps_fix_available else None, + longitude=longitude if gps_fix_available else None, + capture_to_send_ms=int(capture_to_send_ms), + raw_latitude_hex=trailer[8:16].hex(), + raw_longitude_hex=trailer[16:24].hex(), + received_at=received_at if received_at is not None else time.time(), + ) + + def _has_fresh_frame_locked(self) -> bool: + return self._latest_frame is not None and ( + time.time() - self._latest_received_at <= OMNISOCKET_FRAME_FRESH_SECONDS + ) + + def update_remote_video_srtt(self, srtt_ms: int | None) -> None: + with self._lock: + self._latest_remote_video_srtt_ms = srtt_ms + + def _freshness_payload_locked(self) -> dict[str, Any]: + interarrival_samples = list(self._interarrival_ms_samples) + repeat_samples = list(self._repeat_samples) + skip_samples = list(self._skip_samples) + freeze_samples_ms = list(self._freeze_samples_ms) + + inter_frame_avg_ms = round(sum(interarrival_samples) / len(interarrival_samples), 3) if interarrival_samples else None + if interarrival_samples: + ordered = sorted(interarrival_samples) + p95_index = min(len(ordered) - 1, max(0, math.ceil(len(ordered) * 0.95) - 1)) + inter_frame_p95_ms = round(ordered[p95_index], 3) + else: + inter_frame_p95_ms = None + + repeated_frame_ratio = round(sum(repeat_samples) / len(repeat_samples), 4) if repeat_samples else 0.0 + total_skip = sum(skip_samples) + expected_frames = len(skip_samples) + total_skip + skip_ratio = round(total_skip / expected_frames, 4) if expected_frames > 0 else 0.0 + longest_freeze_ms = round(max(freeze_samples_ms), 3) if freeze_samples_ms else 0.0 + return { + "inter_frame_avg_ms": inter_frame_avg_ms, + "inter_frame_p95_ms": inter_frame_p95_ms, + "repeated_frame_ratio": repeated_frame_ratio, + "skip_ratio": skip_ratio, + "longest_freeze_ms": longest_freeze_ms, + "stale_frame_run_length": self._current_stale_frame_run_length, + "relative_freshness_lag_frames": self._current_stale_frame_run_length + (skip_samples[-1] if skip_samples else 0), + } + + def _frame_headers_locked(self) -> dict[str, str]: + capture_to_send_ms = self._latest_metadata.capture_to_send_ms if self._latest_metadata is not None else None + headers: dict[str, str] = {} + if self._latest_sequence is not None: + headers["X-Blitz-Frame-Seq"] = str(self._latest_sequence) + if self._latest_backend_received_unix_ns > 0: + headers["X-Blitz-Backend-Received-Unix-Ns"] = str(self._latest_backend_received_unix_ns) + if self._latest_frame_hash: + headers["X-Blitz-Frame-Hash"] = self._latest_frame_hash + if capture_to_send_ms is not None: + headers["X-Blitz-BSide-Capture-To-Send-Ms"] = str(capture_to_send_ms) + return headers + + def get_latest_frame_headers(self) -> dict[str, str]: + snapshot = self.get_latest_frame_snapshot() + return snapshot[1] if snapshot is not None else {} + + def _run(self) -> None: + while not self._closing.is_set(): + try: + session, buffer_bytes = self._connect_session() + with self._lock: + self._session = session + self._last_error = "" + if self._ever_connected: + self._reconnect_count += 1 + else: + self._ever_connected = True + buffer = bytearray(buffer_bytes) + + while not self._closing.is_set(): + meta = session.recv_into(buffer, timeout_ms=1000) + if meta is None: + continue + if meta.get("msg_type") != self._binary_msg_type: + continue + + frame = bytes(buffer[: meta["body_len"]]) + jpeg_frame = self._extract_jpeg_frame(frame) + if jpeg_frame is None: + self._last_error = "received non-JPEG binary frame" + continue + + received_at = time.time() + received_unix_ns = time.time_ns() + received_mono_ns = time.monotonic_ns() + frame_metadata = self._extract_frame_metadata(frame, received_at=received_at) + sender_clock_delta_ms_raw = None + if frame_metadata is not None: + sender_clock_delta_ms_raw = round((received_unix_ns - frame_metadata.timestamp_ns) / 1_000_000, 3) + unit = VIDEO_TRAILER_TIMESTAMP_UNIT + endianness = VIDEO_TRAILER_ENDIANNESS + else: + unit = None + endianness = None + frame_sequence = self._extract_sequence(frame) + frame_hash = hashlib.blake2s(jpeg_frame, digest_size=8).hexdigest() + local_kcp = safe_kcp_stats(session) if self._frame_recv_logger.enabled else {} + frame_log_record: dict[str, Any] | None = None + + with self._lock: + interarrival_ms = None + if self._last_backend_received_mono_ns > 0: + interarrival_ms = round((received_mono_ns - self._last_backend_received_mono_ns) / 1_000_000, 3) + self._interarrival_ms_samples.append(interarrival_ms) + + sequence_gap = 0 + if frame_sequence is not None and self._last_sequence is not None and frame_sequence > self._last_sequence: + sequence_gap = max(0, frame_sequence - self._last_sequence - 1) + + repeat_flag = bool(self._last_frame_hash) and frame_hash == self._last_frame_hash + self._repeat_samples.append(1 if repeat_flag else 0) + self._skip_samples.append(sequence_gap) + if repeat_flag: + self._current_stale_frame_run_length += 1 + else: + self._current_stale_frame_run_length = 0 + if interarrival_ms is not None and repeat_flag: + self._freeze_samples_ms.append(interarrival_ms) + elif interarrival_ms is not None: + self._freeze_samples_ms.append(0.0) + + self._latest_frame = jpeg_frame + self._latest_received_at = received_at + self._latest_backend_received_unix_ns = received_unix_ns + self._latest_backend_received_mono_ns = received_mono_ns + self._latest_sequence = frame_sequence + self._last_sequence = frame_sequence + self._latest_metadata = frame_metadata + self._latest_sender_clock_delta_ms_raw = sender_clock_delta_ms_raw + self._latest_timestamp_unit = unit + self._latest_timestamp_endianness = endianness + self._latest_frame_hash = frame_hash + self._latest_frame_bytes = len(jpeg_frame) + self._last_frame_hash = frame_hash + self._last_backend_received_mono_ns = received_mono_ns + if sender_clock_delta_ms_raw is not None: + self._sender_clock_delta_samples_ms_raw.append(sender_clock_delta_ms_raw) + self._frames_received += 1 + + if self._frame_recv_logger.enabled: + frame_log_record = { + "ts_unix_nano": received_unix_ns, + "frame_seq": frame_sequence, + "backend_received_unix_ns": received_unix_ns, + "backend_received_mono_ns": received_mono_ns, + "jpeg_bytes": len(jpeg_frame), + "interarrival_ms": interarrival_ms, + "sequence_gap": sequence_gap, + "repeat_flag": repeat_flag, + "skip_count": sequence_gap, + "frame_hash": frame_hash, + "a_to_d_video_srtt_ms": local_kcp.get("srtt_ms"), + "d_to_b_video_srtt_ms": self._latest_remote_video_srtt_ms, + "b_side_capture_to_send_ms": frame_metadata.capture_to_send_ms if frame_metadata is not None else None, + "sender_clock_delta_ms_raw": sender_clock_delta_ms_raw, + } + if frame_log_record is not None: + self._frame_recv_logger.write(frame_log_record) + except Exception as error: # pragma: no cover - runtime integration path + if not self._closing.is_set(): + session_error = "" + if self._session is not None: + try: + session_error = str(dict(self._session.stats()).get("last_server_error", "") or "") + except Exception: + session_error = "" + self._last_error = session_error or str(error) + time.sleep(2) + finally: + if self._session is not None: + try: + self._session.close() + except Exception: + pass + with self._lock: + self._session = None + if self._closing.is_set(): + self._started = False + + def get_latest_frame(self) -> bytes | None: + snapshot = self.get_latest_frame_snapshot() + return snapshot[0] if snapshot is not None else None + + def get_latest_frame_snapshot(self) -> tuple[bytes, dict[str, str]] | None: + self.ensure_started() + with self._lock: + if not self._has_fresh_frame_locked(): + return None + if self._latest_frame is None: + return None + return self._latest_frame, self._frame_headers_locked() + + def get_latest_frame_metadata(self) -> FrameTrailerMetadata | None: + self.ensure_started() + with self._lock: + if not self._has_fresh_frame_locked(): + return None + return self._latest_metadata + + def session_stats(self) -> dict[str, Any]: + self.ensure_started() + with self._lock: + session = self._session + if session is None: + return {"connected": 0, "registered": 0, "last_server_error": self._last_error} + try: + return dict(session.stats()) + except Exception: + return {"connected": 0, "registered": 0, "last_server_error": self._last_error} + + def session_kcp_stats(self) -> dict[str, Any]: + self.ensure_started() + with self._lock: + session = self._session + return safe_kcp_stats(session) + + def get_status(self) -> dict[str, Any]: + self.ensure_started() + config = load_omnisocket_config() + transport_cfg = config.get("transport", {}) + video_cfg = config.get("video_receiver", {}) + session_stats = self.session_stats() + with self._lock: + has_recent_frame = self._has_fresh_frame_locked() + freshness_status = self._freshness_payload_locked() + if has_recent_frame and self._latest_sender_clock_delta_ms_raw is not None: + timing_status = { + "available": True, + "sender_clock_delta_ms_raw": self._latest_sender_clock_delta_ms_raw, + "sender_clock_delta_samples_ms_raw": list(reversed(self._sender_clock_delta_samples_ms_raw)), + "sample_count": len(self._sender_clock_delta_samples_ms_raw), + "sample_window_size": VIDEO_TIMESTAMP_SAMPLE_SIZE, + "timestamp_unit": self._latest_timestamp_unit, + "timestamp_endianness": self._latest_timestamp_endianness, + "unsynced_clock": True, + } + else: + timing_status = { + "available": False, + "sender_clock_delta_ms_raw": None, + "sender_clock_delta_samples_ms_raw": [], + "sample_count": 0, + "sample_window_size": VIDEO_TIMESTAMP_SAMPLE_SIZE, + "timestamp_unit": None, + "timestamp_endianness": None, + "unsynced_clock": True, + } + return { + "backend_ready": self._session_cls is not None, + "mode": VIDEO_SOURCE_MODE, + "connected": self._session is not None, + "registered": bool(session_stats.get("registered", 0)), + "has_recent_frame": has_recent_frame, + "frames_received": self._frames_received, + "latest_sequence": self._latest_sequence, + "latest_frame_hash": self._latest_frame_hash, + "latest_backend_received_unix_ns": self._latest_backend_received_unix_ns or None, + "latest_backend_received_mono_ns": self._latest_backend_received_mono_ns or None, + "latest_frame_bytes": self._latest_frame_bytes, + "latest_capture_to_send_ms": self._latest_metadata.capture_to_send_ms if self._latest_metadata is not None else None, + "reconnect_count": self._reconnect_count, + "last_server_error": str(session_stats.get("last_server_error", "") or ""), + "last_error": self._last_error, + "config_path": str(OMNISOCKET_CONFIG_PATH), + "server_addr": str(transport_cfg.get("server_addr", "")), + "relay_via": str(transport_cfg.get("relay_via", "")), + "peer_id": str(video_cfg.get("peer_id", "")), + "buffer_bytes": int(video_cfg.get("buffer_bytes", 0)), + "timing": timing_status, + "freshness": freshness_status, + } + + def close(self) -> None: + self._closing.set() + with self._lock: + session = self._session + if session is not None: + try: + session.close() + except Exception: + pass + thread = self._thread + if thread is not None and thread.is_alive(): + thread.join(timeout=0.5) + self._frame_recv_logger.close() + + +class VideoFrameService: + def __init__(self, receiver: OmniSocketVideoReceiver, display_probe_store: VideoDisplayProbeStore) -> None: + self._receiver = receiver + self._display_probe_store = display_probe_store + + def get_status(self) -> dict[str, Any]: + receiver_status = self._receiver.get_status() + receiver_frame = self._receiver.get_latest_frame() + display_probe_status = self._display_probe_store.get_status() + + if receiver_frame is not None: + return { + "available": True, + "source_mode": "omnisocket-jpeg-live", + "frame_count": receiver_status["frames_received"], + "fps": 30, + "frame_dir": str(JPEG_FRAME_DIR), + "source_detail": f"peer stream active, frames={receiver_status['frames_received']}", + "receiver": receiver_status, + "timing": receiver_status["timing"], + "freshness": receiver_status.get("freshness", {}), + "display_probe": display_probe_status, + } + + wait_detail = receiver_status["last_error"] or ( + "waiting for live OmniSocket JPEG frames; check the hub, sender, and receiver configuration" + ) + return { + "available": False, + "source_mode": "omnisocket-waiting", + "frame_count": receiver_status["frames_received"], + "fps": 30, + "frame_dir": str(JPEG_FRAME_DIR), + "source_detail": wait_detail, + "receiver": receiver_status, + "timing": receiver_status["timing"], + "freshness": receiver_status.get("freshness", {}), + "display_probe": display_probe_status, + } + + def get_next_frame(self) -> bytes: + receiver_frame = self._receiver.get_latest_frame() + if receiver_frame is not None: + return receiver_frame + raise RuntimeError("no live OmniSocket JPEG frame is currently available") + + def get_next_frame_with_headers(self) -> tuple[bytes, dict[str, str]]: + snapshot = self._receiver.get_latest_frame_snapshot() + if snapshot is not None: + return snapshot + raise RuntimeError("no live OmniSocket JPEG frame is currently available") + + def get_latest_frame_headers(self) -> dict[str, str]: + return self._receiver.get_latest_frame_headers() + + def record_display_probe(self, payload: dict[str, Any]) -> None: + self._display_probe_store.record_event(payload) + + def iter_mjpeg(self, fps: float = 6.0) -> Iterator[bytes]: + frame_interval = 1.0 / max(1.0, min(fps, 30.0)) + while True: + frame = self.get_next_frame() + header = ( + b"--frame\r\n" + b"Content-Type: image/jpeg\r\n" + + f"Content-Length: {len(frame)}\r\n\r\n".encode("ascii") + ) + yield header + frame + b"\r\n" + time.sleep(frame_interval) diff --git a/host/robot-command-center/backend/monitoring/views.py b/host/robot-command-center/backend/monitoring/views.py new file mode 100644 index 0000000..4fd2f57 --- /dev/null +++ b/host/robot-command-center/backend/monitoring/views.py @@ -0,0 +1,123 @@ +from __future__ import annotations + +import json +import time + +from django.views.decorators.csrf import csrf_exempt +from django.http import HttpResponse, StreamingHttpResponse +from rest_framework.decorators import api_view +from rest_framework.response import Response + +from .services import camera_control_service, gps_service, network_service, video_service + + +@api_view(["GET"]) +def dashboard_snapshot(request): + return Response( + { + "gps": gps_service.get_latest(), + "network": network_service.get_latest(), + "video": video_service.get_status(), + } + ) + + +@api_view(["GET"]) +def gps_latest(request): + return Response(gps_service.get_latest()) + + +@api_view(["GET"]) +def network_latest(request): + return Response(network_service.get_latest()) + + +@api_view(["GET"]) +def clock_calibration(request): + server_received_unix_ns = time.time_ns() + server_sent_unix_ns = time.time_ns() + response = Response( + { + "server_received_unix_ms": round(server_received_unix_ns / 1_000_000.0, 3), + "server_sent_unix_ms": round(server_sent_unix_ns / 1_000_000.0, 3), + } + ) + response["Cache-Control"] = "no-store, no-cache, must-revalidate, max-age=0" + return response + + +@api_view(["GET"]) +def video_status(request): + return Response(video_service.get_status()) + + +@csrf_exempt +@api_view(["GET", "POST"]) +def video_camera(request): + if request.method == "GET": + return Response(camera_control_service.get_camera_status()) + + camera = str(request.data.get("camera") or "").strip().lower() + if camera not in {"head", "waist"}: + return Response({"detail": "camera must be 'head' or 'waist'"}, status=400) + + try: + result = camera_control_service.select_camera(camera) + except (OSError, RuntimeError) as error: + return Response({"detail": str(error)}, status=503) + return Response(result, status=200 if result.get("confirmed") else 504) + + +def video_frame(request): + try: + frame, headers = video_service.get_next_frame_with_headers() + except (FileNotFoundError, RuntimeError) as error: + status = video_service.get_status() + return HttpResponse( + status.get("source_detail") or str(error), + status=503, + content_type="text/plain; charset=utf-8", + ) + + response = HttpResponse(frame, content_type="image/jpeg") + response["Cache-Control"] = "no-store, no-cache, must-revalidate, max-age=0" + for key, value in headers.items(): + response[key] = value + return response + + +def video_stream(request): + status = video_service.get_status() + if not status["available"]: + return HttpResponse( + status.get("source_detail") or f"JPEG frame directory not found: {status['frame_dir']}", + status=503, + content_type="text/plain; charset=utf-8", + ) + + try: + fps = float(request.GET.get("fps", status["fps"])) + except (TypeError, ValueError): + fps = float(status["fps"]) + + response = StreamingHttpResponse( + video_service.iter_mjpeg(fps=fps), + content_type="multipart/x-mixed-replace; boundary=frame", + ) + response["Cache-Control"] = "no-store, no-cache, must-revalidate, max-age=0" + return response + + +@csrf_exempt +@api_view(["POST"]) +def video_display_probe(request): + try: + payload = json.loads(request.body.decode("utf-8")) + except (UnicodeDecodeError, json.JSONDecodeError): + return Response({"detail": "invalid json"}, status=400) + + if not isinstance(payload, dict): + return Response({"detail": "expected json object"}, status=400) + + video_service.record_display_probe(payload) + return Response({"ok": True}) diff --git a/host/robot-command-center/config/omnisocket_demo.yaml b/host/robot-command-center/config/omnisocket_demo.yaml new file mode 100644 index 0000000..27ff328 --- /dev/null +++ b/host/robot-command-center/config/omnisocket_demo.yaml @@ -0,0 +1,34 @@ +transport: + server_addr: "" + relay_via: "106.55.173.235:10909" + bind_ip: "" + bind_device: "" + +video_receiver: + peer_id: "peer-a-video" + buffer_bytes: 1048576 + +control_sender: + peer_id: "peer-a-ctrl" + target_peer: "peer-b-ctrl" + +control_ack_receiver: + peer_id: "peer-a-ctrl-ack" + expected_sender: "peer-b-ctrl-ack" + +control_ingress: + native_udp_bind: "127.0.0.1:10921" + source_lease_ms: 300 + send_rate_hz: 20.0 + zero_burst_packets: 3 + +telemetry_receiver: + peer_id: "peer-a-telemetry" + interval_ms: 500 + stale_after_ms: 1500 + +video_sender: + peer_id: "peer-b-video" + target_peer: "peer-a-video" + frame_bytes: 65536 + frame_interval_ms: 33 diff --git a/host/robot-command-center/frontend/.editorconfig b/host/robot-command-center/frontend/.editorconfig new file mode 100644 index 0000000..3b510aa --- /dev/null +++ b/host/robot-command-center/frontend/.editorconfig @@ -0,0 +1,8 @@ +[*.{js,jsx,mjs,cjs,ts,tsx,mts,cts,vue,css,scss,sass,less,styl}] +charset = utf-8 +indent_size = 2 +indent_style = space +insert_final_newline = true +trim_trailing_whitespace = true +end_of_line = lf +max_line_length = 100 diff --git a/host/robot-command-center/frontend/.gitattributes b/host/robot-command-center/frontend/.gitattributes new file mode 100644 index 0000000..6313b56 --- /dev/null +++ b/host/robot-command-center/frontend/.gitattributes @@ -0,0 +1 @@ +* text=auto eol=lf diff --git a/host/robot-command-center/frontend/.gitignore b/host/robot-command-center/frontend/.gitignore new file mode 100644 index 0000000..cd68f14 --- /dev/null +++ b/host/robot-command-center/frontend/.gitignore @@ -0,0 +1,39 @@ +# Logs +logs +*.log +npm-debug.log* +yarn-debug.log* +yarn-error.log* +pnpm-debug.log* +lerna-debug.log* + +node_modules +.DS_Store +dist +dist-ssr +coverage +*.local + +# Editor directories and files +.vscode/* +!.vscode/extensions.json +.idea +*.suo +*.ntvs* +*.njsproj +*.sln +*.sw? + +*.tsbuildinfo + +.eslintcache + +# Cypress +/cypress/videos/ +/cypress/screenshots/ + +# Vitest +__screenshots__/ + +# Vite +*.timestamp-*-*.mjs diff --git a/host/robot-command-center/frontend/.oxlintrc.json b/host/robot-command-center/frontend/.oxlintrc.json new file mode 100644 index 0000000..d5648b9 --- /dev/null +++ b/host/robot-command-center/frontend/.oxlintrc.json @@ -0,0 +1,10 @@ +{ + "$schema": "./node_modules/oxlint/configuration_schema.json", + "plugins": ["eslint", "typescript", "unicorn", "oxc", "vue"], + "env": { + "browser": true + }, + "categories": { + "correctness": "error" + } +} diff --git a/host/robot-command-center/frontend/.prettierrc.json b/host/robot-command-center/frontend/.prettierrc.json new file mode 100644 index 0000000..29a2402 --- /dev/null +++ b/host/robot-command-center/frontend/.prettierrc.json @@ -0,0 +1,6 @@ +{ + "$schema": "https://json.schemastore.org/prettierrc", + "semi": false, + "singleQuote": true, + "printWidth": 100 +} diff --git a/host/robot-command-center/frontend/README.md b/host/robot-command-center/frontend/README.md new file mode 100644 index 0000000..f190e00 --- /dev/null +++ b/host/robot-command-center/frontend/README.md @@ -0,0 +1,48 @@ +# frontend + +This template should help get you started developing with Vue 3 in Vite. + +## Recommended IDE Setup + +[VS Code](https://code.visualstudio.com/) + [Vue (Official)](https://marketplace.visualstudio.com/items?itemName=Vue.volar) (and disable Vetur). + +## Recommended Browser Setup + +- Chromium-based browsers (Chrome, Edge, Brave, etc.): + - [Vue.js devtools](https://chromewebstore.google.com/detail/vuejs-devtools/nhdogjmejiglipccpnnnanhbledajbpd) + - [Turn on Custom Object Formatter in Chrome DevTools](http://bit.ly/object-formatters) +- Firefox: + - [Vue.js devtools](https://addons.mozilla.org/en-US/firefox/addon/vue-js-devtools/) + - [Turn on Custom Object Formatter in Firefox DevTools](https://fxdx.dev/firefox-devtools-custom-object-formatters/) + +## Type Support for `.vue` Imports in TS + +TypeScript cannot handle type information for `.vue` imports by default, so we replace the `tsc` CLI with `vue-tsc` for type checking. In editors, we need [Volar](https://marketplace.visualstudio.com/items?itemName=Vue.volar) to make the TypeScript language service aware of `.vue` types. + +## Customize configuration + +See [Vite Configuration Reference](https://vite.dev/config/). + +## Project Setup + +```sh +npm install +``` + +### Compile and Hot-Reload for Development + +```sh +npm run dev +``` + +### Type-Check, Compile and Minify for Production + +```sh +npm run build +``` + +### Lint with [ESLint](https://eslint.org/) + +```sh +npm run lint +``` diff --git a/host/robot-command-center/frontend/env.d.ts b/host/robot-command-center/frontend/env.d.ts new file mode 100644 index 0000000..11f02fe --- /dev/null +++ b/host/robot-command-center/frontend/env.d.ts @@ -0,0 +1 @@ +/// diff --git a/host/robot-command-center/frontend/eslint.config.ts b/host/robot-command-center/frontend/eslint.config.ts new file mode 100644 index 0000000..89fdc89 --- /dev/null +++ b/host/robot-command-center/frontend/eslint.config.ts @@ -0,0 +1,26 @@ +import { globalIgnores } from 'eslint/config' +import { defineConfigWithVueTs, vueTsConfigs } from '@vue/eslint-config-typescript' +import pluginVue from 'eslint-plugin-vue' +import pluginOxlint from 'eslint-plugin-oxlint' +import skipFormatting from 'eslint-config-prettier/flat' + +// To allow more languages other than `ts` in `.vue` files, uncomment the following lines: +// import { configureVueProject } from '@vue/eslint-config-typescript' +// configureVueProject({ scriptLangs: ['ts', 'tsx'] }) +// More info at https://github.com/vuejs/eslint-config-typescript/#advanced-setup + +export default defineConfigWithVueTs( + { + name: 'app/files-to-lint', + files: ['**/*.{vue,ts,mts,tsx}'], + }, + + globalIgnores(['**/dist/**', '**/dist-ssr/**', '**/coverage/**']), + + ...pluginVue.configs['flat/essential'], + vueTsConfigs.recommended, + + ...pluginOxlint.buildFromOxlintConfigFile('.oxlintrc.json'), + + skipFormatting, +) diff --git a/host/robot-command-center/frontend/index.html b/host/robot-command-center/frontend/index.html new file mode 100644 index 0000000..9e5fc8f --- /dev/null +++ b/host/robot-command-center/frontend/index.html @@ -0,0 +1,13 @@ + + + + + + + Vite App + + +
+ + + diff --git a/host/robot-command-center/frontend/package-lock.json b/host/robot-command-center/frontend/package-lock.json new file mode 100644 index 0000000..3a683ff --- /dev/null +++ b/host/robot-command-center/frontend/package-lock.json @@ -0,0 +1,5032 @@ +{ + "name": "frontend", + "version": "0.0.0", + "lockfileVersion": 3, + "requires": true, + "packages": { + "": { + "name": "frontend", + "version": "0.0.0", + "dependencies": { + "pinia": "^3.0.4", + "vue": "^3.5.31", + "vue-router": "^5.0.4" + }, + "devDependencies": { + "@tsconfig/node24": "^24.0.4", + "@types/node": "^24.12.0", + "@vitejs/plugin-vue": "^6.0.5", + "@vue/eslint-config-typescript": "^14.7.0", + "@vue/tsconfig": "^0.9.1", + "eslint": "^10.1.0", + "eslint-config-prettier": "^10.1.8", + "eslint-plugin-oxlint": "~1.57.0", + "eslint-plugin-vue": "~10.8.0", + "jiti": "^2.6.1", + "npm-run-all2": "^8.0.4", + "oxlint": "~1.57.0", + "prettier": "3.8.1", + "typescript": "~6.0.0", + "vite": "^8.0.3", + "vite-plugin-vue-devtools": "^8.1.1", + "vue-tsc": "^3.2.6" + }, + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@babel/code-frame": { + "version": "7.29.0", + "resolved": "https://registry.npmjs.org/@babel/code-frame/-/code-frame-7.29.0.tgz", + "integrity": "sha512-9NhCeYjq9+3uxgdtp20LSiJXJvN0FeCtNGpJxuMFZ1Kv3cWUNb6DOhJwUvcVCzKGR66cw4njwM6hrJLqgOwbcw==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/helper-validator-identifier": "^7.28.5", + "js-tokens": "^4.0.0", + "picocolors": "^1.1.1" + }, + "engines": { + "node": ">=6.9.0" + } + }, + "node_modules/@babel/compat-data": { + "version": "7.29.0", + "resolved": "https://registry.npmjs.org/@babel/compat-data/-/compat-data-7.29.0.tgz", + "integrity": "sha512-T1NCJqT/j9+cn8fvkt7jtwbLBfLC/1y1c7NtCeXFRgzGTsafi68MRv8yzkYSapBnFA6L3U2VSc02ciDzoAJhJg==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=6.9.0" + } + }, + "node_modules/@babel/core": { + "version": "7.29.0", + "resolved": "https://registry.npmjs.org/@babel/core/-/core-7.29.0.tgz", + "integrity": "sha512-CGOfOJqWjg2qW/Mb6zNsDm+u5vFQ8DxXfbM09z69p5Z6+mE1ikP2jUXw+j42Pf1XTYED2Rni5f95npYeuwMDQA==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/code-frame": "^7.29.0", + "@babel/generator": "^7.29.0", + "@babel/helper-compilation-targets": "^7.28.6", + "@babel/helper-module-transforms": "^7.28.6", + "@babel/helpers": "^7.28.6", + "@babel/parser": "^7.29.0", + "@babel/template": "^7.28.6", + "@babel/traverse": "^7.29.0", + "@babel/types": "^7.29.0", + "@jridgewell/remapping": "^2.3.5", + "convert-source-map": "^2.0.0", + "debug": "^4.1.0", + "gensync": "^1.0.0-beta.2", + "json5": "^2.2.3", + "semver": "^6.3.1" + }, + "engines": { + "node": ">=6.9.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/babel" + } + }, + "node_modules/@babel/core/node_modules/semver": { + "version": "6.3.1", + "resolved": "https://registry.npmjs.org/semver/-/semver-6.3.1.tgz", + "integrity": "sha512-BR7VvDCVHO+q2xBEWskxS6DJE1qRnb7DxzUrogb71CWoSficBxYsiAGd+Kl0mmq/MprG9yArRkyrQxTO6XjMzA==", + "dev": true, + "license": "ISC", + "bin": { + "semver": "bin/semver.js" + } + }, + "node_modules/@babel/generator": { + "version": "7.29.1", + "resolved": "https://registry.npmjs.org/@babel/generator/-/generator-7.29.1.tgz", + "integrity": "sha512-qsaF+9Qcm2Qv8SRIMMscAvG4O3lJ0F1GuMo5HR/Bp02LopNgnZBC/EkbevHFeGs4ls/oPz9v+Bsmzbkbe+0dUw==", + "license": "MIT", + "dependencies": { + "@babel/parser": "^7.29.0", + "@babel/types": "^7.29.0", + "@jridgewell/gen-mapping": "^0.3.12", + "@jridgewell/trace-mapping": "^0.3.28", + "jsesc": "^3.0.2" + }, + "engines": { + "node": ">=6.9.0" + } + }, + "node_modules/@babel/helper-annotate-as-pure": { + "version": "7.27.3", + "resolved": "https://registry.npmjs.org/@babel/helper-annotate-as-pure/-/helper-annotate-as-pure-7.27.3.tgz", + "integrity": "sha512-fXSwMQqitTGeHLBC08Eq5yXz2m37E4pJX1qAU1+2cNedz/ifv/bVXft90VeSav5nFO61EcNgwr0aJxbyPaWBPg==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/types": "^7.27.3" + }, + "engines": { + "node": ">=6.9.0" + } + }, + "node_modules/@babel/helper-compilation-targets": { + "version": "7.28.6", + "resolved": "https://registry.npmjs.org/@babel/helper-compilation-targets/-/helper-compilation-targets-7.28.6.tgz", + "integrity": "sha512-JYtls3hqi15fcx5GaSNL7SCTJ2MNmjrkHXg4FSpOA/grxK8KwyZ5bubHsCq8FXCkua6xhuaaBit+3b7+VZRfcA==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/compat-data": "^7.28.6", + "@babel/helper-validator-option": "^7.27.1", + "browserslist": "^4.24.0", + "lru-cache": "^5.1.1", + "semver": "^6.3.1" + }, + "engines": { + "node": ">=6.9.0" + } + }, + "node_modules/@babel/helper-compilation-targets/node_modules/semver": { + "version": "6.3.1", + "resolved": "https://registry.npmjs.org/semver/-/semver-6.3.1.tgz", + "integrity": "sha512-BR7VvDCVHO+q2xBEWskxS6DJE1qRnb7DxzUrogb71CWoSficBxYsiAGd+Kl0mmq/MprG9yArRkyrQxTO6XjMzA==", + "dev": true, + "license": "ISC", + "bin": { + "semver": "bin/semver.js" + } + }, + "node_modules/@babel/helper-create-class-features-plugin": { + "version": "7.28.6", + "resolved": "https://registry.npmjs.org/@babel/helper-create-class-features-plugin/-/helper-create-class-features-plugin-7.28.6.tgz", + "integrity": "sha512-dTOdvsjnG3xNT9Y0AUg1wAl38y+4Rl4sf9caSQZOXdNqVn+H+HbbJ4IyyHaIqNR6SW9oJpA/RuRjsjCw2IdIow==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/helper-annotate-as-pure": "^7.27.3", + "@babel/helper-member-expression-to-functions": "^7.28.5", + "@babel/helper-optimise-call-expression": "^7.27.1", + "@babel/helper-replace-supers": "^7.28.6", + "@babel/helper-skip-transparent-expression-wrappers": "^7.27.1", + "@babel/traverse": "^7.28.6", + "semver": "^6.3.1" + }, + "engines": { + "node": ">=6.9.0" + }, + "peerDependencies": { + "@babel/core": "^7.0.0" + } + }, + "node_modules/@babel/helper-create-class-features-plugin/node_modules/semver": { + "version": "6.3.1", + "resolved": "https://registry.npmjs.org/semver/-/semver-6.3.1.tgz", + "integrity": "sha512-BR7VvDCVHO+q2xBEWskxS6DJE1qRnb7DxzUrogb71CWoSficBxYsiAGd+Kl0mmq/MprG9yArRkyrQxTO6XjMzA==", + "dev": true, + "license": "ISC", + "bin": { + "semver": "bin/semver.js" + } + }, + "node_modules/@babel/helper-globals": { + "version": "7.28.0", + "resolved": "https://registry.npmjs.org/@babel/helper-globals/-/helper-globals-7.28.0.tgz", + "integrity": "sha512-+W6cISkXFa1jXsDEdYA8HeevQT/FULhxzR99pxphltZcVaugps53THCeiWA8SguxxpSp3gKPiuYfSWopkLQ4hw==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=6.9.0" + } + }, + "node_modules/@babel/helper-member-expression-to-functions": { + "version": "7.28.5", + "resolved": "https://registry.npmjs.org/@babel/helper-member-expression-to-functions/-/helper-member-expression-to-functions-7.28.5.tgz", + "integrity": "sha512-cwM7SBRZcPCLgl8a7cY0soT1SptSzAlMH39vwiRpOQkJlh53r5hdHwLSCZpQdVLT39sZt+CRpNwYG4Y2v77atg==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/traverse": "^7.28.5", + "@babel/types": "^7.28.5" + }, + "engines": { + "node": ">=6.9.0" + } + }, + "node_modules/@babel/helper-module-imports": { + "version": "7.28.6", + "resolved": "https://registry.npmjs.org/@babel/helper-module-imports/-/helper-module-imports-7.28.6.tgz", + "integrity": "sha512-l5XkZK7r7wa9LucGw9LwZyyCUscb4x37JWTPz7swwFE/0FMQAGpiWUZn8u9DzkSBWEcK25jmvubfpw2dnAMdbw==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/traverse": "^7.28.6", + "@babel/types": "^7.28.6" + }, + "engines": { + "node": ">=6.9.0" + } + }, + "node_modules/@babel/helper-module-transforms": { + "version": "7.28.6", + "resolved": "https://registry.npmjs.org/@babel/helper-module-transforms/-/helper-module-transforms-7.28.6.tgz", + "integrity": "sha512-67oXFAYr2cDLDVGLXTEABjdBJZ6drElUSI7WKp70NrpyISso3plG9SAGEF6y7zbha/wOzUByWWTJvEDVNIUGcA==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/helper-module-imports": "^7.28.6", + "@babel/helper-validator-identifier": "^7.28.5", + "@babel/traverse": "^7.28.6" + }, + "engines": { + "node": ">=6.9.0" + }, + "peerDependencies": { + "@babel/core": "^7.0.0" + } + }, + "node_modules/@babel/helper-optimise-call-expression": { + "version": "7.27.1", + "resolved": "https://registry.npmjs.org/@babel/helper-optimise-call-expression/-/helper-optimise-call-expression-7.27.1.tgz", + "integrity": "sha512-URMGH08NzYFhubNSGJrpUEphGKQwMQYBySzat5cAByY1/YgIRkULnIy3tAMeszlL/so2HbeilYloUmSpd7GdVw==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/types": "^7.27.1" + }, + "engines": { + "node": ">=6.9.0" + } + }, + "node_modules/@babel/helper-plugin-utils": { + "version": "7.28.6", + "resolved": "https://registry.npmjs.org/@babel/helper-plugin-utils/-/helper-plugin-utils-7.28.6.tgz", + "integrity": "sha512-S9gzZ/bz83GRysI7gAD4wPT/AI3uCnY+9xn+Mx/KPs2JwHJIz1W8PZkg2cqyt3RNOBM8ejcXhV6y8Og7ly/Dug==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=6.9.0" + } + }, + "node_modules/@babel/helper-replace-supers": { + "version": "7.28.6", + "resolved": "https://registry.npmjs.org/@babel/helper-replace-supers/-/helper-replace-supers-7.28.6.tgz", + "integrity": "sha512-mq8e+laIk94/yFec3DxSjCRD2Z0TAjhVbEJY3UQrlwVo15Lmt7C2wAUbK4bjnTs4APkwsYLTahXRraQXhb1WCg==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/helper-member-expression-to-functions": "^7.28.5", + "@babel/helper-optimise-call-expression": "^7.27.1", + "@babel/traverse": "^7.28.6" + }, + "engines": { + "node": ">=6.9.0" + }, + "peerDependencies": { + "@babel/core": "^7.0.0" + } + }, + "node_modules/@babel/helper-skip-transparent-expression-wrappers": { + "version": "7.27.1", + "resolved": "https://registry.npmjs.org/@babel/helper-skip-transparent-expression-wrappers/-/helper-skip-transparent-expression-wrappers-7.27.1.tgz", + "integrity": "sha512-Tub4ZKEXqbPjXgWLl2+3JpQAYBJ8+ikpQ2Ocj/q/r0LwE3UhENh7EUabyHjz2kCEsrRY83ew2DQdHluuiDQFzg==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/traverse": "^7.27.1", + "@babel/types": "^7.27.1" + }, + "engines": { + "node": ">=6.9.0" + } + }, + "node_modules/@babel/helper-string-parser": { + "version": "7.27.1", + "resolved": "https://registry.npmjs.org/@babel/helper-string-parser/-/helper-string-parser-7.27.1.tgz", + "integrity": "sha512-qMlSxKbpRlAridDExk92nSobyDdpPijUq2DW6oDnUqd0iOGxmQjyqhMIihI9+zv4LPyZdRje2cavWPbCbWm3eA==", + "license": "MIT", + "engines": { + "node": ">=6.9.0" + } + }, + "node_modules/@babel/helper-validator-identifier": { + "version": "7.28.5", + "resolved": "https://registry.npmjs.org/@babel/helper-validator-identifier/-/helper-validator-identifier-7.28.5.tgz", + "integrity": "sha512-qSs4ifwzKJSV39ucNjsvc6WVHs6b7S03sOh2OcHF9UHfVPqWWALUsNUVzhSBiItjRZoLHx7nIarVjqKVusUZ1Q==", + "license": "MIT", + "engines": { + "node": ">=6.9.0" + } + }, + "node_modules/@babel/helper-validator-option": { + "version": "7.27.1", + "resolved": "https://registry.npmjs.org/@babel/helper-validator-option/-/helper-validator-option-7.27.1.tgz", + "integrity": "sha512-YvjJow9FxbhFFKDSuFnVCe2WxXk1zWc22fFePVNEaWJEu8IrZVlda6N0uHwzZrUM1il7NC9Mlp4MaJYbYd9JSg==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=6.9.0" + } + }, + "node_modules/@babel/helpers": { + "version": "7.29.2", + "resolved": "https://registry.npmjs.org/@babel/helpers/-/helpers-7.29.2.tgz", + "integrity": "sha512-HoGuUs4sCZNezVEKdVcwqmZN8GoHirLUcLaYVNBK2J0DadGtdcqgr3BCbvH8+XUo4NGjNl3VOtSjEKNzqfFgKw==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/template": "^7.28.6", + "@babel/types": "^7.29.0" + }, + "engines": { + "node": ">=6.9.0" + } + }, + "node_modules/@babel/parser": { + "version": "7.29.2", + "resolved": "https://registry.npmjs.org/@babel/parser/-/parser-7.29.2.tgz", + "integrity": "sha512-4GgRzy/+fsBa72/RZVJmGKPmZu9Byn8o4MoLpmNe1m8ZfYnz5emHLQz3U4gLud6Zwl0RZIcgiLD7Uq7ySFuDLA==", + "license": "MIT", + "dependencies": { + "@babel/types": "^7.29.0" + }, + "bin": { + "parser": "bin/babel-parser.js" + }, + "engines": { + "node": ">=6.0.0" + } + }, + "node_modules/@babel/plugin-proposal-decorators": { + "version": "7.29.0", + "resolved": "https://registry.npmjs.org/@babel/plugin-proposal-decorators/-/plugin-proposal-decorators-7.29.0.tgz", + "integrity": "sha512-CVBVv3VY/XRMxRYq5dwr2DS7/MvqPm23cOCjbwNnVrfOqcWlnefua1uUs0sjdKOGjvPUG633o07uWzJq4oI6dA==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/helper-create-class-features-plugin": "^7.28.6", + "@babel/helper-plugin-utils": "^7.28.6", + "@babel/plugin-syntax-decorators": "^7.28.6" + }, + "engines": { + "node": ">=6.9.0" + }, + "peerDependencies": { + "@babel/core": "^7.0.0-0" + } + }, + "node_modules/@babel/plugin-syntax-decorators": { + "version": "7.28.6", + "resolved": "https://registry.npmjs.org/@babel/plugin-syntax-decorators/-/plugin-syntax-decorators-7.28.6.tgz", + "integrity": "sha512-71EYI0ONURHJBL4rSFXnITXqXrrY8q4P0q006DPfN+Rk+ASM+++IBXem/ruokgBZR8YNEWZ8R6B+rCb8VcUTqA==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/helper-plugin-utils": "^7.28.6" + }, + "engines": { + "node": ">=6.9.0" + }, + "peerDependencies": { + "@babel/core": "^7.0.0-0" + } + }, + "node_modules/@babel/plugin-syntax-import-attributes": { + "version": "7.28.6", + "resolved": "https://registry.npmjs.org/@babel/plugin-syntax-import-attributes/-/plugin-syntax-import-attributes-7.28.6.tgz", + "integrity": "sha512-jiLC0ma9XkQT3TKJ9uYvlakm66Pamywo+qwL+oL8HJOvc6TWdZXVfhqJr8CCzbSGUAbDOzlGHJC1U+vRfLQDvw==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/helper-plugin-utils": "^7.28.6" + }, + "engines": { + "node": ">=6.9.0" + }, + "peerDependencies": { + "@babel/core": "^7.0.0-0" + } + }, + "node_modules/@babel/plugin-syntax-import-meta": { + "version": "7.10.4", + "resolved": "https://registry.npmjs.org/@babel/plugin-syntax-import-meta/-/plugin-syntax-import-meta-7.10.4.tgz", + "integrity": "sha512-Yqfm+XDx0+Prh3VSeEQCPU81yC+JWZ2pDPFSS4ZdpfZhp4MkFMaDC1UqseovEKwSUpnIL7+vK+Clp7bfh0iD7g==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/helper-plugin-utils": "^7.10.4" + }, + "peerDependencies": { + "@babel/core": "^7.0.0-0" + } + }, + "node_modules/@babel/plugin-syntax-jsx": { + "version": "7.28.6", + "resolved": "https://registry.npmjs.org/@babel/plugin-syntax-jsx/-/plugin-syntax-jsx-7.28.6.tgz", + "integrity": "sha512-wgEmr06G6sIpqr8YDwA2dSRTE3bJ+V0IfpzfSY3Lfgd7YWOaAdlykvJi13ZKBt8cZHfgH1IXN+CL656W3uUa4w==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/helper-plugin-utils": "^7.28.6" + }, + "engines": { + "node": ">=6.9.0" + }, + "peerDependencies": { + "@babel/core": "^7.0.0-0" + } + }, + "node_modules/@babel/plugin-syntax-typescript": { + "version": "7.28.6", + "resolved": "https://registry.npmjs.org/@babel/plugin-syntax-typescript/-/plugin-syntax-typescript-7.28.6.tgz", + "integrity": "sha512-+nDNmQye7nlnuuHDboPbGm00Vqg3oO8niRRL27/4LYHUsHYh0zJ1xWOz0uRwNFmM1Avzk8wZbc6rdiYhomzv/A==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/helper-plugin-utils": "^7.28.6" + }, + "engines": { + "node": ">=6.9.0" + }, + "peerDependencies": { + "@babel/core": "^7.0.0-0" + } + }, + "node_modules/@babel/plugin-transform-typescript": { + "version": "7.28.6", + "resolved": "https://registry.npmjs.org/@babel/plugin-transform-typescript/-/plugin-transform-typescript-7.28.6.tgz", + "integrity": "sha512-0YWL2RFxOqEm9Efk5PvreamxPME8OyY0wM5wh5lHjF+VtVhdneCWGzZeSqzOfiobVqQaNCd2z0tQvnI9DaPWPw==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/helper-annotate-as-pure": "^7.27.3", + "@babel/helper-create-class-features-plugin": "^7.28.6", + "@babel/helper-plugin-utils": "^7.28.6", + "@babel/helper-skip-transparent-expression-wrappers": "^7.27.1", + "@babel/plugin-syntax-typescript": "^7.28.6" + }, + "engines": { + "node": ">=6.9.0" + }, + "peerDependencies": { + "@babel/core": "^7.0.0-0" + } + }, + "node_modules/@babel/template": { + "version": "7.28.6", + "resolved": "https://registry.npmjs.org/@babel/template/-/template-7.28.6.tgz", + "integrity": "sha512-YA6Ma2KsCdGb+WC6UpBVFJGXL58MDA6oyONbjyF/+5sBgxY/dwkhLogbMT2GXXyU84/IhRw/2D1Os1B/giz+BQ==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/code-frame": "^7.28.6", + "@babel/parser": "^7.28.6", + "@babel/types": "^7.28.6" + }, + "engines": { + "node": ">=6.9.0" + } + }, + "node_modules/@babel/traverse": { + "version": "7.29.0", + "resolved": "https://registry.npmjs.org/@babel/traverse/-/traverse-7.29.0.tgz", + "integrity": "sha512-4HPiQr0X7+waHfyXPZpWPfWL/J7dcN1mx9gL6WdQVMbPnF3+ZhSMs8tCxN7oHddJE9fhNE7+lxdnlyemKfJRuA==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/code-frame": "^7.29.0", + "@babel/generator": "^7.29.0", + "@babel/helper-globals": "^7.28.0", + "@babel/parser": "^7.29.0", + "@babel/template": "^7.28.6", + "@babel/types": "^7.29.0", + "debug": "^4.3.1" + }, + "engines": { + "node": ">=6.9.0" + } + }, + "node_modules/@babel/types": { + "version": "7.29.0", + "resolved": "https://registry.npmjs.org/@babel/types/-/types-7.29.0.tgz", + "integrity": "sha512-LwdZHpScM4Qz8Xw2iKSzS+cfglZzJGvofQICy7W7v4caru4EaAmyUuO6BGrbyQ2mYV11W0U8j5mBhd14dd3B0A==", + "license": "MIT", + "dependencies": { + "@babel/helper-string-parser": "^7.27.1", + "@babel/helper-validator-identifier": "^7.28.5" + }, + "engines": { + "node": ">=6.9.0" + } + }, + "node_modules/@emnapi/core": { + "version": "1.9.1", + "resolved": "https://registry.npmjs.org/@emnapi/core/-/core-1.9.1.tgz", + "integrity": "sha512-mukuNALVsoix/w1BJwFzwXBN/dHeejQtuVzcDsfOEsdpCumXb/E9j8w11h5S54tT1xhifGfbbSm/ICrObRb3KA==", + "dev": true, + "license": "MIT", + "optional": true, + "peer": true, + "dependencies": { + "@emnapi/wasi-threads": "1.2.0", + "tslib": "^2.4.0" + } + }, + "node_modules/@emnapi/runtime": { + "version": "1.9.1", + "resolved": "https://registry.npmjs.org/@emnapi/runtime/-/runtime-1.9.1.tgz", + "integrity": "sha512-VYi5+ZVLhpgK4hQ0TAjiQiZ6ol0oe4mBx7mVv7IflsiEp0OWoVsp/+f9Vc1hOhE0TtkORVrI1GvzyreqpgWtkA==", + "dev": true, + "license": "MIT", + "optional": true, + "peer": true, + "dependencies": { + "tslib": "^2.4.0" + } + }, + "node_modules/@emnapi/wasi-threads": { + "version": "1.2.0", + "resolved": "https://registry.npmjs.org/@emnapi/wasi-threads/-/wasi-threads-1.2.0.tgz", + "integrity": "sha512-N10dEJNSsUx41Z6pZsXU8FjPjpBEplgH24sfkmITrBED1/U2Esum9F3lfLrMjKHHjmi557zQn7kR9R+XWXu5Rg==", + "dev": true, + "license": "MIT", + "optional": true, + "peer": true, + "dependencies": { + "tslib": "^2.4.0" + } + }, + "node_modules/@eslint-community/eslint-utils": { + "version": "4.9.1", + "resolved": "https://registry.npmjs.org/@eslint-community/eslint-utils/-/eslint-utils-4.9.1.tgz", + "integrity": "sha512-phrYmNiYppR7znFEdqgfWHXR6NCkZEK7hwWDHZUjit/2/U0r6XvkDl0SYnoM51Hq7FhCGdLDT6zxCCOY1hexsQ==", + "dev": true, + "license": "MIT", + "dependencies": { + "eslint-visitor-keys": "^3.4.3" + }, + "engines": { + "node": "^12.22.0 || ^14.17.0 || >=16.0.0" + }, + "funding": { + "url": "https://opencollective.com/eslint" + }, + "peerDependencies": { + "eslint": "^6.0.0 || ^7.0.0 || >=8.0.0" + } + }, + "node_modules/@eslint-community/regexpp": { + "version": "4.12.2", + "resolved": "https://registry.npmjs.org/@eslint-community/regexpp/-/regexpp-4.12.2.tgz", + "integrity": "sha512-EriSTlt5OC9/7SXkRSCAhfSxxoSUgBm33OH+IkwbdpgoqsSsUg7y3uh+IICI/Qg4BBWr3U2i39RpmycbxMq4ew==", + "dev": true, + "license": "MIT", + "engines": { + "node": "^12.0.0 || ^14.0.0 || >=16.0.0" + } + }, + "node_modules/@eslint/config-array": { + "version": "0.23.3", + "resolved": "https://registry.npmjs.org/@eslint/config-array/-/config-array-0.23.3.tgz", + "integrity": "sha512-j+eEWmB6YYLwcNOdlwQ6L2OsptI/LO6lNBuLIqe5R7RetD658HLoF+Mn7LzYmAWWNNzdC6cqP+L6r8ujeYXWLw==", + "dev": true, + "license": "Apache-2.0", + "dependencies": { + "@eslint/object-schema": "^3.0.3", + "debug": "^4.3.1", + "minimatch": "^10.2.4" + }, + "engines": { + "node": "^20.19.0 || ^22.13.0 || >=24" + } + }, + "node_modules/@eslint/config-helpers": { + "version": "0.5.3", + "resolved": "https://registry.npmjs.org/@eslint/config-helpers/-/config-helpers-0.5.3.tgz", + "integrity": "sha512-lzGN0onllOZCGroKJmRwY6QcEHxbjBw1gwB8SgRSqK8YbbtEXMvKynsXc3553ckIEBxsbMBU7oOZXKIPGZNeZw==", + "dev": true, + "license": "Apache-2.0", + "dependencies": { + "@eslint/core": "^1.1.1" + }, + "engines": { + "node": "^20.19.0 || ^22.13.0 || >=24" + } + }, + "node_modules/@eslint/core": { + "version": "1.1.1", + "resolved": "https://registry.npmjs.org/@eslint/core/-/core-1.1.1.tgz", + "integrity": "sha512-QUPblTtE51/7/Zhfv8BDwO0qkkzQL7P/aWWbqcf4xWLEYn1oKjdO0gglQBB4GAsu7u6wjijbCmzsUTy6mnk6oQ==", + "dev": true, + "license": "Apache-2.0", + "dependencies": { + "@types/json-schema": "^7.0.15" + }, + "engines": { + "node": "^20.19.0 || ^22.13.0 || >=24" + } + }, + "node_modules/@eslint/object-schema": { + "version": "3.0.3", + "resolved": "https://registry.npmjs.org/@eslint/object-schema/-/object-schema-3.0.3.tgz", + "integrity": "sha512-iM869Pugn9Nsxbh/YHRqYiqd23AmIbxJOcpUMOuWCVNdoQJ5ZtwL6h3t0bcZzJUlC3Dq9jCFCESBZnX0GTv7iQ==", + "dev": true, + "license": "Apache-2.0", + "engines": { + "node": "^20.19.0 || ^22.13.0 || >=24" + } + }, + "node_modules/@eslint/plugin-kit": { + "version": "0.6.1", + "resolved": "https://registry.npmjs.org/@eslint/plugin-kit/-/plugin-kit-0.6.1.tgz", + "integrity": "sha512-iH1B076HoAshH1mLpHMgwdGeTs0CYwL0SPMkGuSebZrwBp16v415e9NZXg2jtrqPVQjf6IANe2Vtlr5KswtcZQ==", + "dev": true, + "license": "Apache-2.0", + "dependencies": { + "@eslint/core": "^1.1.1", + "levn": "^0.4.1" + }, + "engines": { + "node": "^20.19.0 || ^22.13.0 || >=24" + } + }, + "node_modules/@humanfs/core": { + "version": "0.19.1", + "resolved": "https://registry.npmjs.org/@humanfs/core/-/core-0.19.1.tgz", + "integrity": "sha512-5DyQ4+1JEUzejeK1JGICcideyfUbGixgS9jNgex5nqkW+cY7WZhxBigmieN5Qnw9ZosSNVC9KQKyb+GUaGyKUA==", + "dev": true, + "license": "Apache-2.0", + "engines": { + "node": ">=18.18.0" + } + }, + "node_modules/@humanfs/node": { + "version": "0.16.7", + "resolved": "https://registry.npmjs.org/@humanfs/node/-/node-0.16.7.tgz", + "integrity": "sha512-/zUx+yOsIrG4Y43Eh2peDeKCxlRt/gET6aHfaKpuq267qXdYDFViVHfMaLyygZOnl0kGWxFIgsBy8QFuTLUXEQ==", + "dev": true, + "license": "Apache-2.0", + "dependencies": { + "@humanfs/core": "^0.19.1", + "@humanwhocodes/retry": "^0.4.0" + }, + "engines": { + "node": ">=18.18.0" + } + }, + "node_modules/@humanwhocodes/module-importer": { + "version": "1.0.1", + "resolved": "https://registry.npmjs.org/@humanwhocodes/module-importer/-/module-importer-1.0.1.tgz", + "integrity": "sha512-bxveV4V8v5Yb4ncFTT3rPSgZBOpCkjfK0y4oVVVJwIuDVBRMDXrPyXRL988i5ap9m9bnyEEjWfm5WkBmtffLfA==", + "dev": true, + "license": "Apache-2.0", + "engines": { + "node": ">=12.22" + }, + "funding": { + "type": "github", + "url": "https://github.com/sponsors/nzakas" + } + }, + "node_modules/@humanwhocodes/retry": { + "version": "0.4.3", + "resolved": "https://registry.npmjs.org/@humanwhocodes/retry/-/retry-0.4.3.tgz", + "integrity": "sha512-bV0Tgo9K4hfPCek+aMAn81RppFKv2ySDQeMoSZuvTASywNTnVJCArCZE2FWqpvIatKu7VMRLWlR1EazvVhDyhQ==", + "dev": true, + "license": "Apache-2.0", + "engines": { + "node": ">=18.18" + }, + "funding": { + "type": "github", + "url": "https://github.com/sponsors/nzakas" + } + }, + "node_modules/@jridgewell/gen-mapping": { + "version": "0.3.13", + "resolved": "https://registry.npmjs.org/@jridgewell/gen-mapping/-/gen-mapping-0.3.13.tgz", + "integrity": "sha512-2kkt/7niJ6MgEPxF0bYdQ6etZaA+fQvDcLKckhy1yIQOzaoKjBBjSj63/aLVjYE3qhRt5dvM+uUyfCg6UKCBbA==", + "license": "MIT", + "dependencies": { + "@jridgewell/sourcemap-codec": "^1.5.0", + "@jridgewell/trace-mapping": "^0.3.24" + } + }, + "node_modules/@jridgewell/remapping": { + "version": "2.3.5", + "resolved": "https://registry.npmjs.org/@jridgewell/remapping/-/remapping-2.3.5.tgz", + "integrity": "sha512-LI9u/+laYG4Ds1TDKSJW2YPrIlcVYOwi2fUC6xB43lueCjgxV4lffOCZCtYFiH6TNOX+tQKXx97T4IKHbhyHEQ==", + "license": "MIT", + "dependencies": { + "@jridgewell/gen-mapping": "^0.3.5", + "@jridgewell/trace-mapping": "^0.3.24" + } + }, + "node_modules/@jridgewell/resolve-uri": { + "version": "3.1.2", + "resolved": "https://registry.npmjs.org/@jridgewell/resolve-uri/-/resolve-uri-3.1.2.tgz", + "integrity": "sha512-bRISgCIjP20/tbWSPWMEi54QVPRZExkuD9lJL+UIxUKtwVJA8wW1Trb1jMs1RFXo1CBTNZ/5hpC9QvmKWdopKw==", + "license": "MIT", + "engines": { + "node": ">=6.0.0" + } + }, + "node_modules/@jridgewell/sourcemap-codec": { + "version": "1.5.5", + "resolved": "https://registry.npmjs.org/@jridgewell/sourcemap-codec/-/sourcemap-codec-1.5.5.tgz", + "integrity": "sha512-cYQ9310grqxueWbl+WuIUIaiUaDcj7WOq5fVhEljNVgRfOUhY9fy2zTvfoqWsnebh8Sl70VScFbICvJnLKB0Og==", + "license": "MIT" + }, + "node_modules/@jridgewell/trace-mapping": { + "version": "0.3.31", + "resolved": "https://registry.npmjs.org/@jridgewell/trace-mapping/-/trace-mapping-0.3.31.tgz", + "integrity": "sha512-zzNR+SdQSDJzc8joaeP8QQoCQr8NuYx2dIIytl1QeBEZHJ9uW6hebsrYgbz8hJwUQao3TWCMtmfV8Nu1twOLAw==", + "license": "MIT", + "dependencies": { + "@jridgewell/resolve-uri": "^3.1.0", + "@jridgewell/sourcemap-codec": "^1.4.14" + } + }, + "node_modules/@napi-rs/wasm-runtime": { + "version": "1.1.2", + "resolved": "https://registry.npmjs.org/@napi-rs/wasm-runtime/-/wasm-runtime-1.1.2.tgz", + "integrity": "sha512-sNXv5oLJ7ob93xkZ1XnxisYhGYXfaG9f65/ZgYuAu3qt7b3NadcOEhLvx28hv31PgX8SZJRYrAIPQilQmFpLVw==", + "dev": true, + "license": "MIT", + "optional": true, + "dependencies": { + "@tybys/wasm-util": "^0.10.1" + }, + "funding": { + "type": "github", + "url": "https://github.com/sponsors/Brooooooklyn" + }, + "peerDependencies": { + "@emnapi/core": "^1.7.1", + "@emnapi/runtime": "^1.7.1" + } + }, + "node_modules/@nodelib/fs.scandir": { + "version": "2.1.5", + "resolved": "https://registry.npmjs.org/@nodelib/fs.scandir/-/fs.scandir-2.1.5.tgz", + "integrity": "sha512-vq24Bq3ym5HEQm2NKCr3yXDwjc7vTsEThRDnkp2DK9p1uqLR+DHurm/NOTo0KG7HYHU7eppKZj3MyqYuMBf62g==", + "dev": true, + "license": "MIT", + "dependencies": { + "@nodelib/fs.stat": "2.0.5", + "run-parallel": "^1.1.9" + }, + "engines": { + "node": ">= 8" + } + }, + "node_modules/@nodelib/fs.stat": { + "version": "2.0.5", + "resolved": "https://registry.npmjs.org/@nodelib/fs.stat/-/fs.stat-2.0.5.tgz", + "integrity": "sha512-RkhPPp2zrqDAQA/2jNhnztcPAlv64XdhIp7a7454A5ovI7Bukxgt7MX7udwAu3zg1DcpPU0rz3VV1SeaqvY4+A==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">= 8" + } + }, + "node_modules/@nodelib/fs.walk": { + "version": "1.2.8", + "resolved": "https://registry.npmjs.org/@nodelib/fs.walk/-/fs.walk-1.2.8.tgz", + "integrity": "sha512-oGB+UxlgWcgQkgwo8GcEGwemoTFt3FIO9ababBmaGwXIoBKZ+GTy0pP185beGg7Llih/NSHSV2XAs1lnznocSg==", + "dev": true, + "license": "MIT", + "dependencies": { + "@nodelib/fs.scandir": "2.1.5", + "fastq": "^1.6.0" + }, + "engines": { + "node": ">= 8" + } + }, + "node_modules/@oxc-project/types": { + "version": "0.122.0", + "resolved": "https://registry.npmjs.org/@oxc-project/types/-/types-0.122.0.tgz", + "integrity": "sha512-oLAl5kBpV4w69UtFZ9xqcmTi+GENWOcPF7FCrczTiBbmC0ibXxCwyvZGbO39rCVEuLGAZM84DH0pUIyyv/YJzA==", + "dev": true, + "license": "MIT", + "funding": { + "url": "https://github.com/sponsors/Boshen" + } + }, + "node_modules/@oxlint/binding-android-arm-eabi": { + "version": "1.57.0", + "resolved": "https://registry.npmjs.org/@oxlint/binding-android-arm-eabi/-/binding-android-arm-eabi-1.57.0.tgz", + "integrity": "sha512-C7EiyfAJG4B70496eV543nKiq5cH0o/xIh/ufbjQz3SIvHhlDDsyn+mRFh+aW8KskTyUpyH2LGWL8p2oN6bl1A==", + "cpu": [ + "arm" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "android" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@oxlint/binding-android-arm64": { + "version": "1.57.0", + "resolved": "https://registry.npmjs.org/@oxlint/binding-android-arm64/-/binding-android-arm64-1.57.0.tgz", + "integrity": "sha512-9i80AresjZ/FZf5xK8tKFbhQnijD4s1eOZw6/FHUwD59HEZbVLRc2C88ADYJfLZrF5XofWDiRX/Ja9KefCLy7w==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "android" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@oxlint/binding-darwin-arm64": { + "version": "1.57.0", + "resolved": "https://registry.npmjs.org/@oxlint/binding-darwin-arm64/-/binding-darwin-arm64-1.57.0.tgz", + "integrity": "sha512-0eUfhRz5L2yKa9I8k3qpyl37XK3oBS5BvrgdVIx599WZK63P8sMbg+0s4IuxmIiZuBK68Ek+Z+gcKgeYf0otsg==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "darwin" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@oxlint/binding-darwin-x64": { + "version": "1.57.0", + "resolved": "https://registry.npmjs.org/@oxlint/binding-darwin-x64/-/binding-darwin-x64-1.57.0.tgz", + "integrity": "sha512-UvrSuzBaYOue+QMAcuDITe0k/Vhj6KZGjfnI6x+NkxBTke/VoM7ZisaxgNY0LWuBkTnd1OmeQfEQdQ48fRjkQg==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "darwin" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@oxlint/binding-freebsd-x64": { + "version": "1.57.0", + "resolved": "https://registry.npmjs.org/@oxlint/binding-freebsd-x64/-/binding-freebsd-x64-1.57.0.tgz", + "integrity": "sha512-wtQq0dCoiw4bUwlsNVDJJ3pxJA218fOezpgtLKrbQqUtQJcM9yP8z+I9fu14aHg0uyAxIY+99toL6uBa2r7nxA==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "freebsd" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@oxlint/binding-linux-arm-gnueabihf": { + "version": "1.57.0", + "resolved": "https://registry.npmjs.org/@oxlint/binding-linux-arm-gnueabihf/-/binding-linux-arm-gnueabihf-1.57.0.tgz", + "integrity": "sha512-qxFWl2BBBFcT4djKa+OtMdnLgoHEJXpqjyGwz8OhW35ImoCwR5qtAGqApNYce5260FQqoAHW8S8eZTjiX67Tsg==", + "cpu": [ + "arm" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@oxlint/binding-linux-arm-musleabihf": { + "version": "1.57.0", + "resolved": "https://registry.npmjs.org/@oxlint/binding-linux-arm-musleabihf/-/binding-linux-arm-musleabihf-1.57.0.tgz", + "integrity": "sha512-SQoIsBU7J0bDW15/f0/RvxHfY3Y0+eB/caKBQtNFbuerTiA6JCYx9P1MrrFTwY2dTm/lMgTSgskvCEYk2AtG/Q==", + "cpu": [ + "arm" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@oxlint/binding-linux-arm64-gnu": { + "version": "1.57.0", + "resolved": "https://registry.npmjs.org/@oxlint/binding-linux-arm64-gnu/-/binding-linux-arm64-gnu-1.57.0.tgz", + "integrity": "sha512-jqxYd1W6WMeozsCmqe9Rzbu3SRrGTyGDAipRlRggetyYbUksJqJKvUNTQtZR/KFoJPb+grnSm5SHhdWrywv3RQ==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@oxlint/binding-linux-arm64-musl": { + "version": "1.57.0", + "resolved": "https://registry.npmjs.org/@oxlint/binding-linux-arm64-musl/-/binding-linux-arm64-musl-1.57.0.tgz", + "integrity": "sha512-i66WyEPVEvq9bxRUCJ/MP5EBfnTDN3nhwEdFZFTO5MmLLvzngfWEG3NSdXQzTT3vk5B9i6C2XSIYBh+aG6uqyg==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@oxlint/binding-linux-ppc64-gnu": { + "version": "1.57.0", + "resolved": "https://registry.npmjs.org/@oxlint/binding-linux-ppc64-gnu/-/binding-linux-ppc64-gnu-1.57.0.tgz", + "integrity": "sha512-oMZDCwz4NobclZU3pH+V1/upVlJZiZvne4jQP+zhJwt+lmio4XXr4qG47CehvrW1Lx2YZiIHuxM2D4YpkG3KVA==", + "cpu": [ + "ppc64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@oxlint/binding-linux-riscv64-gnu": { + "version": "1.57.0", + "resolved": "https://registry.npmjs.org/@oxlint/binding-linux-riscv64-gnu/-/binding-linux-riscv64-gnu-1.57.0.tgz", + "integrity": "sha512-uoBnjJ3MMEBbfnWC1jSFr7/nSCkcQYa72NYoNtLl1imshDnWSolYCjzb8LVCwYCCfLJXD+0gBLD7fyC14c0+0g==", + "cpu": [ + "riscv64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@oxlint/binding-linux-riscv64-musl": { + "version": "1.57.0", + "resolved": "https://registry.npmjs.org/@oxlint/binding-linux-riscv64-musl/-/binding-linux-riscv64-musl-1.57.0.tgz", + "integrity": "sha512-BdrwD7haPZ8a9KrZhKJRSj6jwCor+Z8tHFZ3PT89Y3Jq5v3LfMfEePeAmD0LOTWpiTmzSzdmyw9ijneapiVHKQ==", + "cpu": [ + "riscv64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@oxlint/binding-linux-s390x-gnu": { + "version": "1.57.0", + "resolved": "https://registry.npmjs.org/@oxlint/binding-linux-s390x-gnu/-/binding-linux-s390x-gnu-1.57.0.tgz", + "integrity": "sha512-BNs+7ZNsRstVg2tpNxAXfMX/Iv5oZh204dVyb8Z37+/gCh+yZqNTlg6YwCLIMPSk5wLWIGOaQjT0GUOahKYImw==", + "cpu": [ + "s390x" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@oxlint/binding-linux-x64-gnu": { + "version": "1.57.0", + "resolved": "https://registry.npmjs.org/@oxlint/binding-linux-x64-gnu/-/binding-linux-x64-gnu-1.57.0.tgz", + "integrity": "sha512-AghS18w+XcENcAX0+BQGLiqjpqpaxKJa4cWWP0OWNLacs27vHBxu7TYkv9LUSGe5w8lOJHeMxcYfZNOAPqw2bg==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@oxlint/binding-linux-x64-musl": { + "version": "1.57.0", + "resolved": "https://registry.npmjs.org/@oxlint/binding-linux-x64-musl/-/binding-linux-x64-musl-1.57.0.tgz", + "integrity": "sha512-E/FV3GB8phu/Rpkhz5T96hAiJlGzn91qX5yj5gU754P5cmVGXY1Jw/VSjDSlZBCY3VHjsVLdzgdkJaomEmcNOg==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@oxlint/binding-openharmony-arm64": { + "version": "1.57.0", + "resolved": "https://registry.npmjs.org/@oxlint/binding-openharmony-arm64/-/binding-openharmony-arm64-1.57.0.tgz", + "integrity": "sha512-xvZ2yZt0nUVfU14iuGv3V25jpr9pov5N0Wr28RXnHFxHCRxNDMtYPHV61gGLhN9IlXM96gI4pyYpLSJC5ClLCQ==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "openharmony" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@oxlint/binding-win32-arm64-msvc": { + "version": "1.57.0", + "resolved": "https://registry.npmjs.org/@oxlint/binding-win32-arm64-msvc/-/binding-win32-arm64-msvc-1.57.0.tgz", + "integrity": "sha512-Z4D8Pd0AyHBKeazhdIXeUUy5sIS3Mo0veOlzlDECg6PhRRKgEsBJCCV1n+keUZtQ04OP+i7+itS3kOykUyNhDg==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "win32" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@oxlint/binding-win32-ia32-msvc": { + "version": "1.57.0", + "resolved": "https://registry.npmjs.org/@oxlint/binding-win32-ia32-msvc/-/binding-win32-ia32-msvc-1.57.0.tgz", + "integrity": "sha512-StOZ9nFMVKvevicbQfql6Pouu9pgbeQnu60Fvhz2S6yfMaii+wnueLnqQ5I1JPgNF0Syew4voBlAaHD13wH6tw==", + "cpu": [ + "ia32" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "win32" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@oxlint/binding-win32-x64-msvc": { + "version": "1.57.0", + "resolved": "https://registry.npmjs.org/@oxlint/binding-win32-x64-msvc/-/binding-win32-x64-msvc-1.57.0.tgz", + "integrity": "sha512-6PuxhYgth8TuW0+ABPOIkGdBYw+qYGxgIdXPHSVpiCDm+hqTTWCmC739St1Xni0DJBt8HnSHTG67i1y6gr8qrA==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "win32" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@polka/url": { + "version": "1.0.0-next.29", + "resolved": "https://registry.npmjs.org/@polka/url/-/url-1.0.0-next.29.tgz", + "integrity": "sha512-wwQAWhWSuHaag8c4q/KN/vCoeOJYshAIvMQwD4GpSb3OiZklFfvAgmj0VCBBImRpuF/aFgIRzllXlVX93Jevww==", + "dev": true, + "license": "MIT" + }, + "node_modules/@rolldown/binding-android-arm64": { + "version": "1.0.0-rc.12", + "resolved": "https://registry.npmjs.org/@rolldown/binding-android-arm64/-/binding-android-arm64-1.0.0-rc.12.tgz", + "integrity": "sha512-pv1y2Fv0JybcykuiiD3qBOBdz6RteYojRFY1d+b95WVuzx211CRh+ytI/+9iVyWQ6koTh5dawe4S/yRfOFjgaA==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "android" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@rolldown/binding-darwin-arm64": { + "version": "1.0.0-rc.12", + "resolved": "https://registry.npmjs.org/@rolldown/binding-darwin-arm64/-/binding-darwin-arm64-1.0.0-rc.12.tgz", + "integrity": "sha512-cFYr6zTG/3PXXF3pUO+umXxt1wkRK/0AYT8lDwuqvRC+LuKYWSAQAQZjCWDQpAH172ZV6ieYrNnFzVVcnSflAg==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "darwin" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@rolldown/binding-darwin-x64": { + "version": "1.0.0-rc.12", + "resolved": "https://registry.npmjs.org/@rolldown/binding-darwin-x64/-/binding-darwin-x64-1.0.0-rc.12.tgz", + "integrity": "sha512-ZCsYknnHzeXYps0lGBz8JrF37GpE9bFVefrlmDrAQhOEi4IOIlcoU1+FwHEtyXGx2VkYAvhu7dyBf75EJQffBw==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "darwin" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@rolldown/binding-freebsd-x64": { + "version": "1.0.0-rc.12", + "resolved": "https://registry.npmjs.org/@rolldown/binding-freebsd-x64/-/binding-freebsd-x64-1.0.0-rc.12.tgz", + "integrity": "sha512-dMLeprcVsyJsKolRXyoTH3NL6qtsT0Y2xeuEA8WQJquWFXkEC4bcu1rLZZSnZRMtAqwtrF/Ib9Ddtpa/Gkge9Q==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "freebsd" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@rolldown/binding-linux-arm-gnueabihf": { + "version": "1.0.0-rc.12", + "resolved": "https://registry.npmjs.org/@rolldown/binding-linux-arm-gnueabihf/-/binding-linux-arm-gnueabihf-1.0.0-rc.12.tgz", + "integrity": "sha512-YqWjAgGC/9M1lz3GR1r1rP79nMgo3mQiiA+Hfo+pvKFK1fAJ1bCi0ZQVh8noOqNacuY1qIcfyVfP6HoyBRZ85Q==", + "cpu": [ + "arm" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@rolldown/binding-linux-arm64-gnu": { + "version": "1.0.0-rc.12", + "resolved": "https://registry.npmjs.org/@rolldown/binding-linux-arm64-gnu/-/binding-linux-arm64-gnu-1.0.0-rc.12.tgz", + "integrity": "sha512-/I5AS4cIroLpslsmzXfwbe5OmWvSsrFuEw3mwvbQ1kDxJ822hFHIx+vsN/TAzNVyepI/j/GSzrtCIwQPeKCLIg==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@rolldown/binding-linux-arm64-musl": { + "version": "1.0.0-rc.12", + "resolved": "https://registry.npmjs.org/@rolldown/binding-linux-arm64-musl/-/binding-linux-arm64-musl-1.0.0-rc.12.tgz", + "integrity": "sha512-V6/wZztnBqlx5hJQqNWwFdxIKN0m38p8Jas+VoSfgH54HSj9tKTt1dZvG6JRHcjh6D7TvrJPWFGaY9UBVOaWPw==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@rolldown/binding-linux-ppc64-gnu": { + "version": "1.0.0-rc.12", + "resolved": "https://registry.npmjs.org/@rolldown/binding-linux-ppc64-gnu/-/binding-linux-ppc64-gnu-1.0.0-rc.12.tgz", + "integrity": "sha512-AP3E9BpcUYliZCxa3w5Kwj9OtEVDYK6sVoUzy4vTOJsjPOgdaJZKFmN4oOlX0Wp0RPV2ETfmIra9x1xuayFB7g==", + "cpu": [ + "ppc64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@rolldown/binding-linux-s390x-gnu": { + "version": "1.0.0-rc.12", + "resolved": "https://registry.npmjs.org/@rolldown/binding-linux-s390x-gnu/-/binding-linux-s390x-gnu-1.0.0-rc.12.tgz", + "integrity": "sha512-nWwpvUSPkoFmZo0kQazZYOrT7J5DGOJ/+QHHzjvNlooDZED8oH82Yg67HvehPPLAg5fUff7TfWFHQS8IV1n3og==", + "cpu": [ + "s390x" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@rolldown/binding-linux-x64-gnu": { + "version": "1.0.0-rc.12", + "resolved": "https://registry.npmjs.org/@rolldown/binding-linux-x64-gnu/-/binding-linux-x64-gnu-1.0.0-rc.12.tgz", + "integrity": "sha512-RNrafz5bcwRy+O9e6P8Z/OCAJW/A+qtBczIqVYwTs14pf4iV1/+eKEjdOUta93q2TsT/FI0XYDP3TCky38LMAg==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@rolldown/binding-linux-x64-musl": { + "version": "1.0.0-rc.12", + "resolved": "https://registry.npmjs.org/@rolldown/binding-linux-x64-musl/-/binding-linux-x64-musl-1.0.0-rc.12.tgz", + "integrity": "sha512-Jpw/0iwoKWx3LJ2rc1yjFrj+T7iHZn2JDg1Yny1ma0luviFS4mhAIcd1LFNxK3EYu3DHWCps0ydXQ5i/rrJ2ig==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@rolldown/binding-openharmony-arm64": { + "version": "1.0.0-rc.12", + "resolved": "https://registry.npmjs.org/@rolldown/binding-openharmony-arm64/-/binding-openharmony-arm64-1.0.0-rc.12.tgz", + "integrity": "sha512-vRugONE4yMfVn0+7lUKdKvN4D5YusEiPilaoO2sgUWpCvrncvWgPMzK00ZFFJuiPgLwgFNP5eSiUlv2tfc+lpA==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "openharmony" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@rolldown/binding-wasm32-wasi": { + "version": "1.0.0-rc.12", + "resolved": "https://registry.npmjs.org/@rolldown/binding-wasm32-wasi/-/binding-wasm32-wasi-1.0.0-rc.12.tgz", + "integrity": "sha512-ykGiLr/6kkiHc0XnBfmFJuCjr5ZYKKofkx+chJWDjitX+KsJuAmrzWhwyOMSHzPhzOHOy7u9HlFoa5MoAOJ/Zg==", + "cpu": [ + "wasm32" + ], + "dev": true, + "license": "MIT", + "optional": true, + "dependencies": { + "@napi-rs/wasm-runtime": "^1.1.1" + }, + "engines": { + "node": ">=14.0.0" + } + }, + "node_modules/@rolldown/binding-win32-arm64-msvc": { + "version": "1.0.0-rc.12", + "resolved": "https://registry.npmjs.org/@rolldown/binding-win32-arm64-msvc/-/binding-win32-arm64-msvc-1.0.0-rc.12.tgz", + "integrity": "sha512-5eOND4duWkwx1AzCxadcOrNeighiLwMInEADT0YM7xeEOOFcovWZCq8dadXgcRHSf3Ulh1kFo/qvzoFiCLOL1Q==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "win32" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@rolldown/binding-win32-x64-msvc": { + "version": "1.0.0-rc.12", + "resolved": "https://registry.npmjs.org/@rolldown/binding-win32-x64-msvc/-/binding-win32-x64-msvc-1.0.0-rc.12.tgz", + "integrity": "sha512-PyqoipaswDLAZtot351MLhrlrh6lcZPo2LSYE+VDxbVk24LVKAGOuE4hb8xZQmrPAuEtTZW8E6D2zc5EUZX4Lw==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "win32" + ], + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/@rolldown/pluginutils": { + "version": "1.0.0-rc.2", + "resolved": "https://registry.npmjs.org/@rolldown/pluginutils/-/pluginutils-1.0.0-rc.2.tgz", + "integrity": "sha512-izyXV/v+cHiRfozX62W9htOAvwMo4/bXKDrQ+vom1L1qRuexPock/7VZDAhnpHCLNejd3NJ6hiab+tO0D44Rgw==", + "dev": true, + "license": "MIT" + }, + "node_modules/@tsconfig/node24": { + "version": "24.0.4", + "resolved": "https://registry.npmjs.org/@tsconfig/node24/-/node24-24.0.4.tgz", + "integrity": "sha512-2A933l5P5oCbv6qSxHs7ckKwobs8BDAe9SJ/Xr2Hy+nDlwmLE1GhFh/g/vXGRZWgxBg9nX/5piDtHR9Dkw/XuA==", + "dev": true, + "license": "MIT" + }, + "node_modules/@tybys/wasm-util": { + "version": "0.10.1", + "resolved": "https://registry.npmjs.org/@tybys/wasm-util/-/wasm-util-0.10.1.tgz", + "integrity": "sha512-9tTaPJLSiejZKx+Bmog4uSubteqTvFrVrURwkmHixBo0G4seD0zUxp98E1DzUBJxLQ3NPwXrGKDiVjwx/DpPsg==", + "dev": true, + "license": "MIT", + "optional": true, + "dependencies": { + "tslib": "^2.4.0" + } + }, + "node_modules/@types/esrecurse": { + "version": "4.3.1", + "resolved": "https://registry.npmjs.org/@types/esrecurse/-/esrecurse-4.3.1.tgz", + "integrity": "sha512-xJBAbDifo5hpffDBuHl0Y8ywswbiAp/Wi7Y/GtAgSlZyIABppyurxVueOPE8LUQOxdlgi6Zqce7uoEpqNTeiUw==", + "dev": true, + "license": "MIT" + }, + "node_modules/@types/estree": { + "version": "1.0.8", + "resolved": "https://registry.npmjs.org/@types/estree/-/estree-1.0.8.tgz", + "integrity": "sha512-dWHzHa2WqEXI/O1E9OjrocMTKJl2mSrEolh1Iomrv6U+JuNwaHXsXx9bLu5gG7BUWFIN0skIQJQ/L1rIex4X6w==", + "dev": true, + "license": "MIT" + }, + "node_modules/@types/json-schema": { + "version": "7.0.15", + "resolved": "https://registry.npmjs.org/@types/json-schema/-/json-schema-7.0.15.tgz", + "integrity": "sha512-5+fP8P8MFNC+AyZCDxrB2pkZFPGzqQWUzpSeuuVLvm8VMcorNYavBqoFcxK8bQz4Qsbn4oUEEem4wDLfcysGHA==", + "dev": true, + "license": "MIT" + }, + "node_modules/@types/node": { + "version": "24.12.0", + "resolved": "https://registry.npmjs.org/@types/node/-/node-24.12.0.tgz", + "integrity": "sha512-GYDxsZi3ChgmckRT9HPU0WEhKLP08ev/Yfcq2AstjrDASOYCSXeyjDsHg4v5t4jOj7cyDX3vmprafKlWIG9MXQ==", + "dev": true, + "license": "MIT", + "dependencies": { + "undici-types": "~7.16.0" + } + }, + "node_modules/@typescript-eslint/eslint-plugin": { + "version": "8.58.0", + "resolved": "https://registry.npmjs.org/@typescript-eslint/eslint-plugin/-/eslint-plugin-8.58.0.tgz", + "integrity": "sha512-RLkVSiNuUP1C2ROIWfqX+YcUfLaSnxGE/8M+Y57lopVwg9VTYYfhuz15Yf1IzCKgZj6/rIbYTmJCUSqr76r0Wg==", + "dev": true, + "license": "MIT", + "dependencies": { + "@eslint-community/regexpp": "^4.12.2", + "@typescript-eslint/scope-manager": "8.58.0", + "@typescript-eslint/type-utils": "8.58.0", + "@typescript-eslint/utils": "8.58.0", + "@typescript-eslint/visitor-keys": "8.58.0", + "ignore": "^7.0.5", + "natural-compare": "^1.4.0", + "ts-api-utils": "^2.5.0" + }, + "engines": { + "node": "^18.18.0 || ^20.9.0 || >=21.1.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/typescript-eslint" + }, + "peerDependencies": { + "@typescript-eslint/parser": "^8.58.0", + "eslint": "^8.57.0 || ^9.0.0 || ^10.0.0", + "typescript": ">=4.8.4 <6.1.0" + } + }, + "node_modules/@typescript-eslint/eslint-plugin/node_modules/ignore": { + "version": "7.0.5", + "resolved": "https://registry.npmjs.org/ignore/-/ignore-7.0.5.tgz", + "integrity": "sha512-Hs59xBNfUIunMFgWAbGX5cq6893IbWg4KnrjbYwX3tx0ztorVgTDA6B2sxf8ejHJ4wz8BqGUMYlnzNBer5NvGg==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">= 4" + } + }, + "node_modules/@typescript-eslint/parser": { + "version": "8.58.0", + "resolved": "https://registry.npmjs.org/@typescript-eslint/parser/-/parser-8.58.0.tgz", + "integrity": "sha512-rLoGZIf9afaRBYsPUMtvkDWykwXwUPL60HebR4JgTI8mxfFe2cQTu3AGitANp4b9B2QlVru6WzjgB2IzJKiCSA==", + "dev": true, + "license": "MIT", + "dependencies": { + "@typescript-eslint/scope-manager": "8.58.0", + "@typescript-eslint/types": "8.58.0", + "@typescript-eslint/typescript-estree": "8.58.0", + "@typescript-eslint/visitor-keys": "8.58.0", + "debug": "^4.4.3" + }, + "engines": { + "node": "^18.18.0 || ^20.9.0 || >=21.1.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/typescript-eslint" + }, + "peerDependencies": { + "eslint": "^8.57.0 || ^9.0.0 || ^10.0.0", + "typescript": ">=4.8.4 <6.1.0" + } + }, + "node_modules/@typescript-eslint/project-service": { + "version": "8.58.0", + "resolved": "https://registry.npmjs.org/@typescript-eslint/project-service/-/project-service-8.58.0.tgz", + "integrity": "sha512-8Q/wBPWLQP1j16NxoPNIKpDZFMaxl7yWIoqXWYeWO+Bbd2mjgvoF0dxP2jKZg5+x49rgKdf7Ck473M8PC3V9lg==", + "dev": true, + "license": "MIT", + "dependencies": { + "@typescript-eslint/tsconfig-utils": "^8.58.0", + "@typescript-eslint/types": "^8.58.0", + "debug": "^4.4.3" + }, + "engines": { + "node": "^18.18.0 || ^20.9.0 || >=21.1.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/typescript-eslint" + }, + "peerDependencies": { + "typescript": ">=4.8.4 <6.1.0" + } + }, + "node_modules/@typescript-eslint/scope-manager": { + "version": "8.58.0", + "resolved": "https://registry.npmjs.org/@typescript-eslint/scope-manager/-/scope-manager-8.58.0.tgz", + "integrity": "sha512-W1Lur1oF50FxSnNdGp3Vs6P+yBRSmZiw4IIjEeYxd8UQJwhUF0gDgDD/W/Tgmh73mxgEU3qX0Bzdl/NGuSPEpQ==", + "dev": true, + "license": "MIT", + "dependencies": { + "@typescript-eslint/types": "8.58.0", + "@typescript-eslint/visitor-keys": "8.58.0" + }, + "engines": { + "node": "^18.18.0 || ^20.9.0 || >=21.1.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/typescript-eslint" + } + }, + "node_modules/@typescript-eslint/tsconfig-utils": { + "version": "8.58.0", + "resolved": "https://registry.npmjs.org/@typescript-eslint/tsconfig-utils/-/tsconfig-utils-8.58.0.tgz", + "integrity": "sha512-doNSZEVJsWEu4htiVC+PR6NpM+pa+a4ClH9INRWOWCUzMst/VA9c4gXq92F8GUD1rwhNvRLkgjfYtFXegXQF7A==", + "dev": true, + "license": "MIT", + "engines": { + "node": "^18.18.0 || ^20.9.0 || >=21.1.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/typescript-eslint" + }, + "peerDependencies": { + "typescript": ">=4.8.4 <6.1.0" + } + }, + "node_modules/@typescript-eslint/type-utils": { + "version": "8.58.0", + "resolved": "https://registry.npmjs.org/@typescript-eslint/type-utils/-/type-utils-8.58.0.tgz", + "integrity": "sha512-aGsCQImkDIqMyx1u4PrVlbi/krmDsQUs4zAcCV6M7yPcPev+RqVlndsJy9kJ8TLihW9TZ0kbDAzctpLn5o+lOg==", + "dev": true, + "license": "MIT", + "dependencies": { + "@typescript-eslint/types": "8.58.0", + "@typescript-eslint/typescript-estree": "8.58.0", + "@typescript-eslint/utils": "8.58.0", + "debug": "^4.4.3", + "ts-api-utils": "^2.5.0" + }, + "engines": { + "node": "^18.18.0 || ^20.9.0 || >=21.1.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/typescript-eslint" + }, + "peerDependencies": { + "eslint": "^8.57.0 || ^9.0.0 || ^10.0.0", + "typescript": ">=4.8.4 <6.1.0" + } + }, + "node_modules/@typescript-eslint/types": { + "version": "8.58.0", + "resolved": "https://registry.npmjs.org/@typescript-eslint/types/-/types-8.58.0.tgz", + "integrity": "sha512-O9CjxypDT89fbHxRfETNoAnHj/i6IpRK0CvbVN3qibxlLdo5p5hcLmUuCCrHMpxiWSwKyI8mCP7qRNYuOJ0Uww==", + "dev": true, + "license": "MIT", + "engines": { + "node": "^18.18.0 || ^20.9.0 || >=21.1.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/typescript-eslint" + } + }, + "node_modules/@typescript-eslint/typescript-estree": { + "version": "8.58.0", + "resolved": "https://registry.npmjs.org/@typescript-eslint/typescript-estree/-/typescript-estree-8.58.0.tgz", + "integrity": "sha512-7vv5UWbHqew/dvs+D3e1RvLv1v2eeZ9txRHPnEEBUgSNLx5ghdzjHa0sgLWYVKssH+lYmV0JaWdoubo0ncGYLA==", + "dev": true, + "license": "MIT", + "dependencies": { + "@typescript-eslint/project-service": "8.58.0", + "@typescript-eslint/tsconfig-utils": "8.58.0", + "@typescript-eslint/types": "8.58.0", + "@typescript-eslint/visitor-keys": "8.58.0", + "debug": "^4.4.3", + "minimatch": "^10.2.2", + "semver": "^7.7.3", + "tinyglobby": "^0.2.15", + "ts-api-utils": "^2.5.0" + }, + "engines": { + "node": "^18.18.0 || ^20.9.0 || >=21.1.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/typescript-eslint" + }, + "peerDependencies": { + "typescript": ">=4.8.4 <6.1.0" + } + }, + "node_modules/@typescript-eslint/utils": { + "version": "8.58.0", + "resolved": "https://registry.npmjs.org/@typescript-eslint/utils/-/utils-8.58.0.tgz", + "integrity": "sha512-RfeSqcFeHMHlAWzt4TBjWOAtoW9lnsAGiP3GbaX9uVgTYYrMbVnGONEfUCiSss+xMHFl+eHZiipmA8WkQ7FuNA==", + "dev": true, + "license": "MIT", + "dependencies": { + "@eslint-community/eslint-utils": "^4.9.1", + "@typescript-eslint/scope-manager": "8.58.0", + "@typescript-eslint/types": "8.58.0", + "@typescript-eslint/typescript-estree": "8.58.0" + }, + "engines": { + "node": "^18.18.0 || ^20.9.0 || >=21.1.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/typescript-eslint" + }, + "peerDependencies": { + "eslint": "^8.57.0 || ^9.0.0 || ^10.0.0", + "typescript": ">=4.8.4 <6.1.0" + } + }, + "node_modules/@typescript-eslint/visitor-keys": { + "version": "8.58.0", + "resolved": "https://registry.npmjs.org/@typescript-eslint/visitor-keys/-/visitor-keys-8.58.0.tgz", + "integrity": "sha512-XJ9UD9+bbDo4a4epraTwG3TsNPeiB9aShrUneAVXy8q4LuwowN+qu89/6ByLMINqvIMeI9H9hOHQtg/ijrYXzQ==", + "dev": true, + "license": "MIT", + "dependencies": { + "@typescript-eslint/types": "8.58.0", + "eslint-visitor-keys": "^5.0.0" + }, + "engines": { + "node": "^18.18.0 || ^20.9.0 || >=21.1.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/typescript-eslint" + } + }, + "node_modules/@typescript-eslint/visitor-keys/node_modules/eslint-visitor-keys": { + "version": "5.0.1", + "resolved": "https://registry.npmjs.org/eslint-visitor-keys/-/eslint-visitor-keys-5.0.1.tgz", + "integrity": "sha512-tD40eHxA35h0PEIZNeIjkHoDR4YjjJp34biM0mDvplBe//mB+IHCqHDGV7pxF+7MklTvighcCPPZC7ynWyjdTA==", + "dev": true, + "license": "Apache-2.0", + "engines": { + "node": "^20.19.0 || ^22.13.0 || >=24" + }, + "funding": { + "url": "https://opencollective.com/eslint" + } + }, + "node_modules/@vitejs/plugin-vue": { + "version": "6.0.5", + "resolved": "https://registry.npmjs.org/@vitejs/plugin-vue/-/plugin-vue-6.0.5.tgz", + "integrity": "sha512-bL3AxKuQySfk1iGcBsQnoRVexTPJq0Z/ixFVM8OhVJAP6ZXXXLtM7NFKWhLl30Kg7uTBqIaPXbh+nuQCuBDedg==", + "dev": true, + "license": "MIT", + "dependencies": { + "@rolldown/pluginutils": "1.0.0-rc.2" + }, + "engines": { + "node": "^20.19.0 || >=22.12.0" + }, + "peerDependencies": { + "vite": "^5.0.0 || ^6.0.0 || ^7.0.0 || ^8.0.0", + "vue": "^3.2.25" + } + }, + "node_modules/@volar/language-core": { + "version": "2.4.28", + "resolved": "https://registry.npmjs.org/@volar/language-core/-/language-core-2.4.28.tgz", + "integrity": "sha512-w4qhIJ8ZSitgLAkVay6AbcnC7gP3glYM3fYwKV3srj8m494E3xtrCv6E+bWviiK/8hs6e6t1ij1s2Endql7vzQ==", + "dev": true, + "license": "MIT", + "dependencies": { + "@volar/source-map": "2.4.28" + } + }, + "node_modules/@volar/source-map": { + "version": "2.4.28", + "resolved": "https://registry.npmjs.org/@volar/source-map/-/source-map-2.4.28.tgz", + "integrity": "sha512-yX2BDBqJkRXfKw8my8VarTyjv48QwxdJtvRgUpNE5erCsgEUdI2DsLbpa+rOQVAJYshY99szEcRDmyHbF10ggQ==", + "dev": true, + "license": "MIT" + }, + "node_modules/@volar/typescript": { + "version": "2.4.28", + "resolved": "https://registry.npmjs.org/@volar/typescript/-/typescript-2.4.28.tgz", + "integrity": "sha512-Ja6yvWrbis2QtN4ClAKreeUZPVYMARDYZl9LMEv1iQ1QdepB6wn0jTRxA9MftYmYa4DQ4k/DaSZpFPUfxl8giw==", + "dev": true, + "license": "MIT", + "dependencies": { + "@volar/language-core": "2.4.28", + "path-browserify": "^1.0.1", + "vscode-uri": "^3.0.8" + } + }, + "node_modules/@vue-macros/common": { + "version": "3.1.2", + "resolved": "https://registry.npmjs.org/@vue-macros/common/-/common-3.1.2.tgz", + "integrity": "sha512-h9t4ArDdniO9ekYHAD95t9AZcAbb19lEGK+26iAjUODOIJKmObDNBSe4+6ELQAA3vtYiFPPBtHh7+cQCKi3Dng==", + "license": "MIT", + "dependencies": { + "@vue/compiler-sfc": "^3.5.22", + "ast-kit": "^2.1.2", + "local-pkg": "^1.1.2", + "magic-string-ast": "^1.0.2", + "unplugin-utils": "^0.3.0" + }, + "engines": { + "node": ">=20.19.0" + }, + "funding": { + "url": "https://github.com/sponsors/vue-macros" + }, + "peerDependencies": { + "vue": "^2.7.0 || ^3.2.25" + }, + "peerDependenciesMeta": { + "vue": { + "optional": true + } + } + }, + "node_modules/@vue/babel-helper-vue-transform-on": { + "version": "1.5.0", + "resolved": "https://registry.npmjs.org/@vue/babel-helper-vue-transform-on/-/babel-helper-vue-transform-on-1.5.0.tgz", + "integrity": "sha512-0dAYkerNhhHutHZ34JtTl2czVQHUNWv6xEbkdF5W+Yrv5pCWsqjeORdOgbtW2I9gWlt+wBmVn+ttqN9ZxR5tzA==", + "dev": true, + "license": "MIT" + }, + "node_modules/@vue/babel-plugin-jsx": { + "version": "1.5.0", + "resolved": "https://registry.npmjs.org/@vue/babel-plugin-jsx/-/babel-plugin-jsx-1.5.0.tgz", + "integrity": "sha512-mneBhw1oOqCd2247O0Yw/mRwC9jIGACAJUlawkmMBiNmL4dGA2eMzuNZVNqOUfYTa6vqmND4CtOPzmEEEqLKFw==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/helper-module-imports": "^7.27.1", + "@babel/helper-plugin-utils": "^7.27.1", + "@babel/plugin-syntax-jsx": "^7.27.1", + "@babel/template": "^7.27.2", + "@babel/traverse": "^7.28.0", + "@babel/types": "^7.28.2", + "@vue/babel-helper-vue-transform-on": "1.5.0", + "@vue/babel-plugin-resolve-type": "1.5.0", + "@vue/shared": "^3.5.18" + }, + "peerDependencies": { + "@babel/core": "^7.0.0-0" + }, + "peerDependenciesMeta": { + "@babel/core": { + "optional": true + } + } + }, + "node_modules/@vue/babel-plugin-resolve-type": { + "version": "1.5.0", + "resolved": "https://registry.npmjs.org/@vue/babel-plugin-resolve-type/-/babel-plugin-resolve-type-1.5.0.tgz", + "integrity": "sha512-Wm/60o+53JwJODm4Knz47dxJnLDJ9FnKnGZJbUUf8nQRAtt6P+undLUAVU3Ha33LxOJe6IPoifRQ6F/0RrU31w==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/code-frame": "^7.27.1", + "@babel/helper-module-imports": "^7.27.1", + "@babel/helper-plugin-utils": "^7.27.1", + "@babel/parser": "^7.28.0", + "@vue/compiler-sfc": "^3.5.18" + }, + "funding": { + "url": "https://github.com/sponsors/sxzz" + }, + "peerDependencies": { + "@babel/core": "^7.0.0-0" + } + }, + "node_modules/@vue/compiler-core": { + "version": "3.5.31", + "resolved": "https://registry.npmjs.org/@vue/compiler-core/-/compiler-core-3.5.31.tgz", + "integrity": "sha512-k/ueL14aNIEy5Onf0OVzR8kiqF/WThgLdFhxwa4e/KF/0qe38IwIdofoSWBTvvxQOesaz6riAFAUaYjoF9fLLQ==", + "license": "MIT", + "dependencies": { + "@babel/parser": "^7.29.2", + "@vue/shared": "3.5.31", + "entities": "^7.0.1", + "estree-walker": "^2.0.2", + "source-map-js": "^1.2.1" + } + }, + "node_modules/@vue/compiler-dom": { + "version": "3.5.31", + "resolved": "https://registry.npmjs.org/@vue/compiler-dom/-/compiler-dom-3.5.31.tgz", + "integrity": "sha512-BMY/ozS/xxjYqRFL+tKdRpATJYDTTgWSo0+AJvJNg4ig+Hgb0dOsHPXvloHQ5hmlivUqw1Yt2pPIqp4e0v1GUw==", + "license": "MIT", + "dependencies": { + "@vue/compiler-core": "3.5.31", + "@vue/shared": "3.5.31" + } + }, + "node_modules/@vue/compiler-sfc": { + "version": "3.5.31", + "resolved": "https://registry.npmjs.org/@vue/compiler-sfc/-/compiler-sfc-3.5.31.tgz", + "integrity": "sha512-M8wpPgR9UJ8MiRGjppvx9uWJfLV7A/T+/rL8s/y3QG3u0c2/YZgff3d6SuimKRIhcYnWg5fTfDMlz2E6seUW8Q==", + "license": "MIT", + "dependencies": { + "@babel/parser": "^7.29.2", + "@vue/compiler-core": "3.5.31", + "@vue/compiler-dom": "3.5.31", + "@vue/compiler-ssr": "3.5.31", + "@vue/shared": "3.5.31", + "estree-walker": "^2.0.2", + "magic-string": "^0.30.21", + "postcss": "^8.5.8", + "source-map-js": "^1.2.1" + } + }, + "node_modules/@vue/compiler-ssr": { + "version": "3.5.31", + "resolved": "https://registry.npmjs.org/@vue/compiler-ssr/-/compiler-ssr-3.5.31.tgz", + "integrity": "sha512-h0xIMxrt/LHOvJKMri+vdYT92BrK3HFLtDqq9Pr/lVVfE4IyKZKvWf0vJFW10Yr6nX02OR4MkJwI0c1HDa1hog==", + "license": "MIT", + "dependencies": { + "@vue/compiler-dom": "3.5.31", + "@vue/shared": "3.5.31" + } + }, + "node_modules/@vue/devtools-api": { + "version": "7.7.9", + "resolved": "https://registry.npmjs.org/@vue/devtools-api/-/devtools-api-7.7.9.tgz", + "integrity": "sha512-kIE8wvwlcZ6TJTbNeU2HQNtaxLx3a84aotTITUuL/4bzfPxzajGBOoqjMhwZJ8L9qFYDU/lAYMEEm11dnZOD6g==", + "license": "MIT", + "dependencies": { + "@vue/devtools-kit": "^7.7.9" + } + }, + "node_modules/@vue/devtools-core": { + "version": "8.1.1", + "resolved": "https://registry.npmjs.org/@vue/devtools-core/-/devtools-core-8.1.1.tgz", + "integrity": "sha512-bCCsSABp1/ot4j8xJEycM6Mtt2wbuucfByr6hMgjbYhrtlscOJypZKvy8f1FyWLYrLTchB5Qz216Lm92wfbq0A==", + "dev": true, + "license": "MIT", + "dependencies": { + "@vue/devtools-kit": "^8.1.1", + "@vue/devtools-shared": "^8.1.1" + }, + "peerDependencies": { + "vue": "^3.0.0" + } + }, + "node_modules/@vue/devtools-core/node_modules/@vue/devtools-kit": { + "version": "8.1.1", + "resolved": "https://registry.npmjs.org/@vue/devtools-kit/-/devtools-kit-8.1.1.tgz", + "integrity": "sha512-gVBaBv++i+adg4JpH71k9ppl4soyR7Y2McEqO5YNgv0BI1kMZ7BDX5gnwkZ5COYgiCyhejZG+yGNrBAjj6Coqg==", + "dev": true, + "license": "MIT", + "dependencies": { + "@vue/devtools-shared": "^8.1.1", + "birpc": "^2.6.1", + "hookable": "^5.5.3", + "perfect-debounce": "^2.0.0" + } + }, + "node_modules/@vue/devtools-core/node_modules/@vue/devtools-shared": { + "version": "8.1.1", + "resolved": "https://registry.npmjs.org/@vue/devtools-shared/-/devtools-shared-8.1.1.tgz", + "integrity": "sha512-+h4ttmJYl/txpxHKaoZcaKpC+pvckgLzIDiSQlaQ7kKthKh8KuwoLW2D8hPJEnqKzXOvu15UHEoGyngAXCz0EQ==", + "dev": true, + "license": "MIT" + }, + "node_modules/@vue/devtools-core/node_modules/perfect-debounce": { + "version": "2.1.0", + "resolved": "https://registry.npmjs.org/perfect-debounce/-/perfect-debounce-2.1.0.tgz", + "integrity": "sha512-LjgdTytVFXeUgtHZr9WYViYSM/g8MkcTPYDlPa3cDqMirHjKiSZPYd6DoL7pK8AJQr+uWkQvCjHNdiMqsrJs+g==", + "dev": true, + "license": "MIT" + }, + "node_modules/@vue/devtools-kit": { + "version": "7.7.9", + "resolved": "https://registry.npmjs.org/@vue/devtools-kit/-/devtools-kit-7.7.9.tgz", + "integrity": "sha512-PyQ6odHSgiDVd4hnTP+aDk2X4gl2HmLDfiyEnn3/oV+ckFDuswRs4IbBT7vacMuGdwY/XemxBoh302ctbsptuA==", + "license": "MIT", + "dependencies": { + "@vue/devtools-shared": "^7.7.9", + "birpc": "^2.3.0", + "hookable": "^5.5.3", + "mitt": "^3.0.1", + "perfect-debounce": "^1.0.0", + "speakingurl": "^14.0.1", + "superjson": "^2.2.2" + } + }, + "node_modules/@vue/devtools-shared": { + "version": "7.7.9", + "resolved": "https://registry.npmjs.org/@vue/devtools-shared/-/devtools-shared-7.7.9.tgz", + "integrity": "sha512-iWAb0v2WYf0QWmxCGy0seZNDPdO3Sp5+u78ORnyeonS6MT4PC7VPrryX2BpMJrwlDeaZ6BD4vP4XKjK0SZqaeA==", + "license": "MIT", + "dependencies": { + "rfdc": "^1.4.1" + } + }, + "node_modules/@vue/eslint-config-typescript": { + "version": "14.7.0", + "resolved": "https://registry.npmjs.org/@vue/eslint-config-typescript/-/eslint-config-typescript-14.7.0.tgz", + "integrity": "sha512-iegbMINVc+seZ/QxtzWiOBozctrHiF2WvGedruu2EbLujg9VuU0FQiNcN2z1ycuaoKKpF4m2qzB5HDEMKbxtIg==", + "dev": true, + "license": "MIT", + "dependencies": { + "@typescript-eslint/utils": "^8.56.0", + "fast-glob": "^3.3.3", + "typescript-eslint": "^8.56.0", + "vue-eslint-parser": "^10.4.0" + }, + "engines": { + "node": "^18.18.0 || ^20.9.0 || >=21.1.0" + }, + "peerDependencies": { + "eslint": "^9.10.0 || ^10.0.0", + "eslint-plugin-vue": "^9.28.0 || ^10.0.0", + "typescript": ">=4.8.4" + }, + "peerDependenciesMeta": { + "typescript": { + "optional": true + } + } + }, + "node_modules/@vue/language-core": { + "version": "3.2.6", + "resolved": "https://registry.npmjs.org/@vue/language-core/-/language-core-3.2.6.tgz", + "integrity": "sha512-xYYYX3/aVup576tP/23sEUpgiEnujrENaoNRbaozC1/MA9I6EGFQRJb4xrt/MmUCAGlxTKL2RmT8JLTPqagCkg==", + "dev": true, + "license": "MIT", + "dependencies": { + "@volar/language-core": "2.4.28", + "@vue/compiler-dom": "^3.5.0", + "@vue/shared": "^3.5.0", + "alien-signals": "^3.0.0", + "muggle-string": "^0.4.1", + "path-browserify": "^1.0.1", + "picomatch": "^4.0.2" + } + }, + "node_modules/@vue/language-core/node_modules/picomatch": { + "version": "4.0.4", + "resolved": "https://registry.npmjs.org/picomatch/-/picomatch-4.0.4.tgz", + "integrity": "sha512-QP88BAKvMam/3NxH6vj2o21R6MjxZUAd6nlwAS/pnGvN9IVLocLHxGYIzFhg6fUQ+5th6P4dv4eW9jX3DSIj7A==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=12" + }, + "funding": { + "url": "https://github.com/sponsors/jonschlinkert" + } + }, + "node_modules/@vue/reactivity": { + "version": "3.5.31", + "resolved": "https://registry.npmjs.org/@vue/reactivity/-/reactivity-3.5.31.tgz", + "integrity": "sha512-DtKXxk9E/KuVvt8VxWu+6Luc9I9ETNcqR1T1oW1gf02nXaZ1kuAx58oVu7uX9XxJR0iJCro6fqBLw9oSBELo5g==", + "license": "MIT", + "dependencies": { + "@vue/shared": "3.5.31" + } + }, + "node_modules/@vue/runtime-core": { + "version": "3.5.31", + "resolved": "https://registry.npmjs.org/@vue/runtime-core/-/runtime-core-3.5.31.tgz", + "integrity": "sha512-AZPmIHXEAyhpkmN7aWlqjSfYynmkWlluDNPHMCZKFHH+lLtxP/30UJmoVhXmbDoP1Ng0jG0fyY2zCj1PnSSA6Q==", + "license": "MIT", + "dependencies": { + "@vue/reactivity": "3.5.31", + "@vue/shared": "3.5.31" + } + }, + "node_modules/@vue/runtime-dom": { + "version": "3.5.31", + "resolved": "https://registry.npmjs.org/@vue/runtime-dom/-/runtime-dom-3.5.31.tgz", + "integrity": "sha512-xQJsNRmGPeDCJq/u813tyonNgWBFjzfVkBwDREdEWndBnGdHLHgkwNBQxLtg4zDrzKTEcnikUy1UUNecb3lJ6g==", + "license": "MIT", + "dependencies": { + "@vue/reactivity": "3.5.31", + "@vue/runtime-core": "3.5.31", + "@vue/shared": "3.5.31", + "csstype": "^3.2.3" + } + }, + "node_modules/@vue/server-renderer": { + "version": "3.5.31", + "resolved": "https://registry.npmjs.org/@vue/server-renderer/-/server-renderer-3.5.31.tgz", + "integrity": "sha512-GJuwRvMcdZX/CriUnyIIOGkx3rMV3H6sOu0JhdKbduaeCji6zb60iOGMY7tFoN24NfsUYoFBhshZtGxGpxO4iA==", + "license": "MIT", + "dependencies": { + "@vue/compiler-ssr": "3.5.31", + "@vue/shared": "3.5.31" + }, + "peerDependencies": { + "vue": "3.5.31" + } + }, + "node_modules/@vue/shared": { + "version": "3.5.31", + "resolved": "https://registry.npmjs.org/@vue/shared/-/shared-3.5.31.tgz", + "integrity": "sha512-nBxuiuS9Lj5bPkPbWogPUnjxxWpkRniX7e5UBQDWl6Fsf4roq9wwV+cR7ezQ4zXswNvPIlsdj1slcLB7XCsRAw==", + "license": "MIT" + }, + "node_modules/@vue/tsconfig": { + "version": "0.9.1", + "resolved": "https://registry.npmjs.org/@vue/tsconfig/-/tsconfig-0.9.1.tgz", + "integrity": "sha512-buvjm+9NzLCJL29KY1j1991YYJ5e6275OiK+G4jtmfIb+z4POywbdm0wXusT9adVWqe0xqg70TbI7+mRx4uU9w==", + "dev": true, + "license": "MIT", + "peerDependencies": { + "typescript": ">= 5.8", + "vue": "^3.4.0" + }, + "peerDependenciesMeta": { + "typescript": { + "optional": true + }, + "vue": { + "optional": true + } + } + }, + "node_modules/acorn": { + "version": "8.16.0", + "resolved": "https://registry.npmjs.org/acorn/-/acorn-8.16.0.tgz", + "integrity": "sha512-UVJyE9MttOsBQIDKw1skb9nAwQuR5wuGD3+82K6JgJlm/Y+KI92oNsMNGZCYdDsVtRHSak0pcV5Dno5+4jh9sw==", + "license": "MIT", + "bin": { + "acorn": "bin/acorn" + }, + "engines": { + "node": ">=0.4.0" + } + }, + "node_modules/acorn-jsx": { + "version": "5.3.2", + "resolved": "https://registry.npmjs.org/acorn-jsx/-/acorn-jsx-5.3.2.tgz", + "integrity": "sha512-rq9s+JNhf0IChjtDXxllJ7g41oZk5SlXtp0LHwyA5cejwn7vKmKp4pPri6YEePv2PU65sAsegbXtIinmDFDXgQ==", + "dev": true, + "license": "MIT", + "peerDependencies": { + "acorn": "^6.0.0 || ^7.0.0 || ^8.0.0" + } + }, + "node_modules/ajv": { + "version": "6.14.0", + "resolved": "https://registry.npmjs.org/ajv/-/ajv-6.14.0.tgz", + "integrity": "sha512-IWrosm/yrn43eiKqkfkHis7QioDleaXQHdDVPKg0FSwwd/DuvyX79TZnFOnYpB7dcsFAMmtFztZuXPDvSePkFw==", + "dev": true, + "license": "MIT", + "dependencies": { + "fast-deep-equal": "^3.1.1", + "fast-json-stable-stringify": "^2.0.0", + "json-schema-traverse": "^0.4.1", + "uri-js": "^4.2.2" + }, + "funding": { + "type": "github", + "url": "https://github.com/sponsors/epoberezkin" + } + }, + "node_modules/alien-signals": { + "version": "3.1.2", + "resolved": "https://registry.npmjs.org/alien-signals/-/alien-signals-3.1.2.tgz", + "integrity": "sha512-d9dYqZTS90WLiU0I5c6DHj/HcKkF8ZyGN3G5x8wSbslulz70KOxaqCT0hQCo9KOyhVqzqGojvNdJXoTumZOtcw==", + "dev": true, + "license": "MIT" + }, + "node_modules/ansi-styles": { + "version": "6.2.3", + "resolved": "https://registry.npmjs.org/ansi-styles/-/ansi-styles-6.2.3.tgz", + "integrity": "sha512-4Dj6M28JB+oAH8kFkTLUo+a2jwOFkuqb3yucU0CANcRRUbxS0cP0nZYCGjcc3BNXwRIsUVmDGgzawme7zvJHvg==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=12" + }, + "funding": { + "url": "https://github.com/chalk/ansi-styles?sponsor=1" + } + }, + "node_modules/ansis": { + "version": "4.2.0", + "resolved": "https://registry.npmjs.org/ansis/-/ansis-4.2.0.tgz", + "integrity": "sha512-HqZ5rWlFjGiV0tDm3UxxgNRqsOTniqoKZu0pIAfh7TZQMGuZK+hH0drySty0si0QXj1ieop4+SkSfPZBPPkHig==", + "dev": true, + "license": "ISC", + "engines": { + "node": ">=14" + } + }, + "node_modules/ast-kit": { + "version": "2.2.0", + "resolved": "https://registry.npmjs.org/ast-kit/-/ast-kit-2.2.0.tgz", + "integrity": "sha512-m1Q/RaVOnTp9JxPX+F+Zn7IcLYMzM8kZofDImfsKZd8MbR+ikdOzTeztStWqfrqIxZnYWryyI9ePm3NGjnZgGw==", + "license": "MIT", + "dependencies": { + "@babel/parser": "^7.28.5", + "pathe": "^2.0.3" + }, + "engines": { + "node": ">=20.19.0" + }, + "funding": { + "url": "https://github.com/sponsors/sxzz" + } + }, + "node_modules/ast-walker-scope": { + "version": "0.8.3", + "resolved": "https://registry.npmjs.org/ast-walker-scope/-/ast-walker-scope-0.8.3.tgz", + "integrity": "sha512-cbdCP0PGOBq0ASG+sjnKIoYkWMKhhz+F/h9pRexUdX2Hd38+WOlBkRKlqkGOSm0YQpcFMQBJeK4WspUAkwsEdg==", + "license": "MIT", + "dependencies": { + "@babel/parser": "^7.28.4", + "ast-kit": "^2.1.3" + }, + "engines": { + "node": ">=20.19.0" + }, + "funding": { + "url": "https://github.com/sponsors/sxzz" + } + }, + "node_modules/balanced-match": { + "version": "4.0.4", + "resolved": "https://registry.npmjs.org/balanced-match/-/balanced-match-4.0.4.tgz", + "integrity": "sha512-BLrgEcRTwX2o6gGxGOCNyMvGSp35YofuYzw9h1IMTRmKqttAZZVU67bdb9Pr2vUHA8+j3i2tJfjO6C6+4myGTA==", + "dev": true, + "license": "MIT", + "engines": { + "node": "18 || 20 || >=22" + } + }, + "node_modules/baseline-browser-mapping": { + "version": "2.10.12", + "resolved": "https://registry.npmjs.org/baseline-browser-mapping/-/baseline-browser-mapping-2.10.12.tgz", + "integrity": "sha512-qyq26DxfY4awP2gIRXhhLWfwzwI+N5Nxk6iQi8EFizIaWIjqicQTE4sLnZZVdeKPRcVNoJOkkpfzoIYuvCKaIQ==", + "dev": true, + "license": "Apache-2.0", + "bin": { + "baseline-browser-mapping": "dist/cli.cjs" + }, + "engines": { + "node": ">=6.0.0" + } + }, + "node_modules/birpc": { + "version": "2.9.0", + "resolved": "https://registry.npmjs.org/birpc/-/birpc-2.9.0.tgz", + "integrity": "sha512-KrayHS5pBi69Xi9JmvoqrIgYGDkD6mcSe/i6YKi3w5kekCLzrX4+nawcXqrj2tIp50Kw/mT/s3p+GVK0A0sKxw==", + "license": "MIT", + "funding": { + "url": "https://github.com/sponsors/antfu" + } + }, + "node_modules/boolbase": { + "version": "1.0.0", + "resolved": "https://registry.npmjs.org/boolbase/-/boolbase-1.0.0.tgz", + "integrity": "sha512-JZOSA7Mo9sNGB8+UjSgzdLtokWAky1zbztM3WRLCbZ70/3cTANmQmOdR7y2g+J0e2WXywy1yS468tY+IruqEww==", + "dev": true, + "license": "ISC" + }, + "node_modules/brace-expansion": { + "version": "5.0.5", + "resolved": "https://registry.npmjs.org/brace-expansion/-/brace-expansion-5.0.5.tgz", + "integrity": "sha512-VZznLgtwhn+Mact9tfiwx64fA9erHH/MCXEUfB/0bX/6Fz6ny5EGTXYltMocqg4xFAQZtnO3DHWWXi8RiuN7cQ==", + "dev": true, + "license": "MIT", + "dependencies": { + "balanced-match": "^4.0.2" + }, + "engines": { + "node": "18 || 20 || >=22" + } + }, + "node_modules/braces": { + "version": "3.0.3", + "resolved": "https://registry.npmjs.org/braces/-/braces-3.0.3.tgz", + "integrity": "sha512-yQbXgO/OSZVD2IsiLlro+7Hf6Q18EJrKSEsdoMzKePKXct3gvD8oLcOQdIzGupr5Fj+EDe8gO/lxc1BzfMpxvA==", + "dev": true, + "license": "MIT", + "dependencies": { + "fill-range": "^7.1.1" + }, + "engines": { + "node": ">=8" + } + }, + "node_modules/browserslist": { + "version": "4.28.1", + "resolved": "https://registry.npmjs.org/browserslist/-/browserslist-4.28.1.tgz", + "integrity": "sha512-ZC5Bd0LgJXgwGqUknZY/vkUQ04r8NXnJZ3yYi4vDmSiZmC/pdSN0NbNRPxZpbtO4uAfDUAFffO8IZoM3Gj8IkA==", + "dev": true, + "funding": [ + { + "type": "opencollective", + "url": "https://opencollective.com/browserslist" + }, + { + "type": "tidelift", + "url": "https://tidelift.com/funding/github/npm/browserslist" + }, + { + "type": "github", + "url": "https://github.com/sponsors/ai" + } + ], + "license": "MIT", + "dependencies": { + "baseline-browser-mapping": "^2.9.0", + "caniuse-lite": "^1.0.30001759", + "electron-to-chromium": "^1.5.263", + "node-releases": "^2.0.27", + "update-browserslist-db": "^1.2.0" + }, + "bin": { + "browserslist": "cli.js" + }, + "engines": { + "node": "^6 || ^7 || ^8 || ^9 || ^10 || ^11 || ^12 || >=13.7" + } + }, + "node_modules/bundle-name": { + "version": "4.1.0", + "resolved": "https://registry.npmjs.org/bundle-name/-/bundle-name-4.1.0.tgz", + "integrity": "sha512-tjwM5exMg6BGRI+kNmTntNsvdZS1X8BFYS6tnJ2hdH0kVxM6/eVZ2xy+FqStSWvYmtfFMDLIxurorHwDKfDz5Q==", + "dev": true, + "license": "MIT", + "dependencies": { + "run-applescript": "^7.0.0" + }, + "engines": { + "node": ">=18" + }, + "funding": { + "url": "https://github.com/sponsors/sindresorhus" + } + }, + "node_modules/caniuse-lite": { + "version": "1.0.30001782", + "resolved": "https://registry.npmjs.org/caniuse-lite/-/caniuse-lite-1.0.30001782.tgz", + "integrity": "sha512-dZcaJLJeDMh4rELYFw1tvSn1bhZWYFOt468FcbHHxx/Z/dFidd1I6ciyFdi3iwfQCyOjqo9upF6lGQYtMiJWxw==", + "dev": true, + "funding": [ + { + "type": "opencollective", + "url": "https://opencollective.com/browserslist" + }, + { + "type": "tidelift", + "url": "https://tidelift.com/funding/github/npm/caniuse-lite" + }, + { + "type": "github", + "url": "https://github.com/sponsors/ai" + } + ], + "license": "CC-BY-4.0" + }, + "node_modules/chokidar": { + "version": "5.0.0", + "resolved": "https://registry.npmjs.org/chokidar/-/chokidar-5.0.0.tgz", + "integrity": "sha512-TQMmc3w+5AxjpL8iIiwebF73dRDF4fBIieAqGn9RGCWaEVwQ6Fb2cGe31Yns0RRIzii5goJ1Y7xbMwo1TxMplw==", + "license": "MIT", + "dependencies": { + "readdirp": "^5.0.0" + }, + "engines": { + "node": ">= 20.19.0" + }, + "funding": { + "url": "https://paulmillr.com/funding/" + } + }, + "node_modules/confbox": { + "version": "0.2.4", + "resolved": "https://registry.npmjs.org/confbox/-/confbox-0.2.4.tgz", + "integrity": "sha512-ysOGlgTFbN2/Y6Cg3Iye8YKulHw+R2fNXHrgSmXISQdMnomY6eNDprVdW9R5xBguEqI954+S6709UyiO7B+6OQ==", + "license": "MIT" + }, + "node_modules/convert-source-map": { + "version": "2.0.0", + "resolved": "https://registry.npmjs.org/convert-source-map/-/convert-source-map-2.0.0.tgz", + "integrity": "sha512-Kvp459HrV2FEJ1CAsi1Ku+MY3kasH19TFykTz2xWmMeq6bk2NU3XXvfJ+Q61m0xktWwt+1HSYf3JZsTms3aRJg==", + "dev": true, + "license": "MIT" + }, + "node_modules/copy-anything": { + "version": "4.0.5", + "resolved": "https://registry.npmjs.org/copy-anything/-/copy-anything-4.0.5.tgz", + "integrity": "sha512-7Vv6asjS4gMOuILabD3l739tsaxFQmC+a7pLZm02zyvs8p977bL3zEgq3yDk5rn9B0PbYgIv++jmHcuUab4RhA==", + "license": "MIT", + "dependencies": { + "is-what": "^5.2.0" + }, + "engines": { + "node": ">=18" + }, + "funding": { + "url": "https://github.com/sponsors/mesqueeb" + } + }, + "node_modules/cross-spawn": { + "version": "7.0.6", + "resolved": "https://registry.npmjs.org/cross-spawn/-/cross-spawn-7.0.6.tgz", + "integrity": "sha512-uV2QOWP2nWzsy2aMp8aRibhi9dlzF5Hgh5SHaB9OiTGEyDTiJJyx0uy51QXdyWbtAHNua4XJzUKca3OzKUd3vA==", + "dev": true, + "license": "MIT", + "dependencies": { + "path-key": "^3.1.0", + "shebang-command": "^2.0.0", + "which": "^2.0.1" + }, + "engines": { + "node": ">= 8" + } + }, + "node_modules/cssesc": { + "version": "3.0.0", + "resolved": "https://registry.npmjs.org/cssesc/-/cssesc-3.0.0.tgz", + "integrity": "sha512-/Tb/JcjK111nNScGob5MNtsntNM1aCNUDipB/TkwZFhyDrrE47SOx/18wF2bbjgc3ZzCSKW1T5nt5EbFoAz/Vg==", + "dev": true, + "license": "MIT", + "bin": { + "cssesc": "bin/cssesc" + }, + "engines": { + "node": ">=4" + } + }, + "node_modules/csstype": { + "version": "3.2.3", + "resolved": "https://registry.npmjs.org/csstype/-/csstype-3.2.3.tgz", + "integrity": "sha512-z1HGKcYy2xA8AGQfwrn0PAy+PB7X/GSj3UVJW9qKyn43xWa+gl5nXmU4qqLMRzWVLFC8KusUX8T/0kCiOYpAIQ==", + "license": "MIT" + }, + "node_modules/debug": { + "version": "4.4.3", + "resolved": "https://registry.npmjs.org/debug/-/debug-4.4.3.tgz", + "integrity": "sha512-RGwwWnwQvkVfavKVt22FGLw+xYSdzARwm0ru6DhTVA3umU5hZc28V3kO4stgYryrTlLpuvgI9GiijltAjNbcqA==", + "dev": true, + "license": "MIT", + "dependencies": { + "ms": "^2.1.3" + }, + "engines": { + "node": ">=6.0" + }, + "peerDependenciesMeta": { + "supports-color": { + "optional": true + } + } + }, + "node_modules/deep-is": { + "version": "0.1.4", + "resolved": "https://registry.npmjs.org/deep-is/-/deep-is-0.1.4.tgz", + "integrity": "sha512-oIPzksmTg4/MriiaYGO+okXDT7ztn/w3Eptv/+gSIdMdKsJo0u4CfYNFJPy+4SKMuCqGw2wxnA+URMg3t8a/bQ==", + "dev": true, + "license": "MIT" + }, + "node_modules/default-browser": { + "version": "5.5.0", + "resolved": "https://registry.npmjs.org/default-browser/-/default-browser-5.5.0.tgz", + "integrity": "sha512-H9LMLr5zwIbSxrmvikGuI/5KGhZ8E2zH3stkMgM5LpOWDutGM2JZaj460Udnf1a+946zc7YBgrqEWwbk7zHvGw==", + "dev": true, + "license": "MIT", + "dependencies": { + "bundle-name": "^4.1.0", + "default-browser-id": "^5.0.0" + }, + "engines": { + "node": ">=18" + }, + "funding": { + "url": "https://github.com/sponsors/sindresorhus" + } + }, + "node_modules/default-browser-id": { + "version": "5.0.1", + "resolved": "https://registry.npmjs.org/default-browser-id/-/default-browser-id-5.0.1.tgz", + "integrity": "sha512-x1VCxdX4t+8wVfd1so/9w+vQ4vx7lKd2Qp5tDRutErwmR85OgmfX7RlLRMWafRMY7hbEiXIbudNrjOAPa/hL8Q==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=18" + }, + "funding": { + "url": "https://github.com/sponsors/sindresorhus" + } + }, + "node_modules/define-lazy-prop": { + "version": "3.0.0", + "resolved": "https://registry.npmjs.org/define-lazy-prop/-/define-lazy-prop-3.0.0.tgz", + "integrity": "sha512-N+MeXYoqr3pOgn8xfyRPREN7gHakLYjhsHhWGT3fWAiL4IkAt0iDw14QiiEm2bE30c5XX5q0FtAA3CK5f9/BUg==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=12" + }, + "funding": { + "url": "https://github.com/sponsors/sindresorhus" + } + }, + "node_modules/detect-libc": { + "version": "2.1.2", + "resolved": "https://registry.npmjs.org/detect-libc/-/detect-libc-2.1.2.tgz", + "integrity": "sha512-Btj2BOOO83o3WyH59e8MgXsxEQVcarkUOpEYrubB0urwnN10yQ364rsiByU11nZlqWYZm05i/of7io4mzihBtQ==", + "dev": true, + "license": "Apache-2.0", + "engines": { + "node": ">=8" + } + }, + "node_modules/electron-to-chromium": { + "version": "1.5.329", + "resolved": "https://registry.npmjs.org/electron-to-chromium/-/electron-to-chromium-1.5.329.tgz", + "integrity": "sha512-/4t+AS1l4S3ZC0Ja7PHFIWeBIxGA3QGqV8/yKsP36v7NcyUCl+bIcmw6s5zVuMIECWwBrAK/6QLzTmbJChBboQ==", + "dev": true, + "license": "ISC" + }, + "node_modules/entities": { + "version": "7.0.1", + "resolved": "https://registry.npmjs.org/entities/-/entities-7.0.1.tgz", + "integrity": "sha512-TWrgLOFUQTH994YUyl1yT4uyavY5nNB5muff+RtWaqNVCAK408b5ZnnbNAUEWLTCpum9w6arT70i1XdQ4UeOPA==", + "license": "BSD-2-Clause", + "engines": { + "node": ">=0.12" + }, + "funding": { + "url": "https://github.com/fb55/entities?sponsor=1" + } + }, + "node_modules/error-stack-parser-es": { + "version": "1.0.5", + "resolved": "https://registry.npmjs.org/error-stack-parser-es/-/error-stack-parser-es-1.0.5.tgz", + "integrity": "sha512-5qucVt2XcuGMcEGgWI7i+yZpmpByQ8J1lHhcL7PwqCwu9FPP3VUXzT4ltHe5i2z9dePwEHcDVOAfSnHsOlCXRA==", + "dev": true, + "license": "MIT", + "funding": { + "url": "https://github.com/sponsors/antfu" + } + }, + "node_modules/escalade": { + "version": "3.2.0", + "resolved": "https://registry.npmjs.org/escalade/-/escalade-3.2.0.tgz", + "integrity": "sha512-WUj2qlxaQtO4g6Pq5c29GTcWGDyd8itL8zTlipgECz3JesAiiOKotd8JU6otB3PACgG6xkJUyVhboMS+bje/jA==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=6" + } + }, + "node_modules/escape-string-regexp": { + "version": "4.0.0", + "resolved": "https://registry.npmjs.org/escape-string-regexp/-/escape-string-regexp-4.0.0.tgz", + "integrity": "sha512-TtpcNJ3XAzx3Gq8sWRzJaVajRs0uVxA2YAkdb1jm2YkPz4G6egUFAyA3n5vtEIZefPk5Wa4UXbKuS5fKkJWdgA==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=10" + }, + "funding": { + "url": "https://github.com/sponsors/sindresorhus" + } + }, + "node_modules/eslint": { + "version": "10.1.0", + "resolved": "https://registry.npmjs.org/eslint/-/eslint-10.1.0.tgz", + "integrity": "sha512-S9jlY/ELKEUwwQnqWDO+f+m6sercqOPSqXM5Go94l7DOmxHVDgmSFGWEzeE/gwgTAr0W103BWt0QLe/7mabIvA==", + "dev": true, + "license": "MIT", + "dependencies": { + "@eslint-community/eslint-utils": "^4.8.0", + "@eslint-community/regexpp": "^4.12.2", + "@eslint/config-array": "^0.23.3", + "@eslint/config-helpers": "^0.5.3", + "@eslint/core": "^1.1.1", + "@eslint/plugin-kit": "^0.6.1", + "@humanfs/node": "^0.16.6", + "@humanwhocodes/module-importer": "^1.0.1", + "@humanwhocodes/retry": "^0.4.2", + "@types/estree": "^1.0.6", + "ajv": "^6.14.0", + "cross-spawn": "^7.0.6", + "debug": "^4.3.2", + "escape-string-regexp": "^4.0.0", + "eslint-scope": "^9.1.2", + "eslint-visitor-keys": "^5.0.1", + "espree": "^11.2.0", + "esquery": "^1.7.0", + "esutils": "^2.0.2", + "fast-deep-equal": "^3.1.3", + "file-entry-cache": "^8.0.0", + "find-up": "^5.0.0", + "glob-parent": "^6.0.2", + "ignore": "^5.2.0", + "imurmurhash": "^0.1.4", + "is-glob": "^4.0.0", + "json-stable-stringify-without-jsonify": "^1.0.1", + "minimatch": "^10.2.4", + "natural-compare": "^1.4.0", + "optionator": "^0.9.3" + }, + "bin": { + "eslint": "bin/eslint.js" + }, + "engines": { + "node": "^20.19.0 || ^22.13.0 || >=24" + }, + "funding": { + "url": "https://eslint.org/donate" + }, + "peerDependencies": { + "jiti": "*" + }, + "peerDependenciesMeta": { + "jiti": { + "optional": true + } + } + }, + "node_modules/eslint-config-prettier": { + "version": "10.1.8", + "resolved": "https://registry.npmjs.org/eslint-config-prettier/-/eslint-config-prettier-10.1.8.tgz", + "integrity": "sha512-82GZUjRS0p/jganf6q1rEO25VSoHH0hKPCTrgillPjdI/3bgBhAE1QzHrHTizjpRvy6pGAvKjDJtk2pF9NDq8w==", + "dev": true, + "license": "MIT", + "bin": { + "eslint-config-prettier": "bin/cli.js" + }, + "funding": { + "url": "https://opencollective.com/eslint-config-prettier" + }, + "peerDependencies": { + "eslint": ">=7.0.0" + } + }, + "node_modules/eslint-plugin-oxlint": { + "version": "1.57.0", + "resolved": "https://registry.npmjs.org/eslint-plugin-oxlint/-/eslint-plugin-oxlint-1.57.0.tgz", + "integrity": "sha512-+c1ZqIKq6pJ/BzZkpFxkuk+40EFXSb57t8AytjEnCqeCW6WecHzeBOIukfq6nHOxIrzX+uJ0ulN70Fj8YaR50g==", + "dev": true, + "license": "MIT", + "dependencies": { + "jsonc-parser": "^3.3.1" + }, + "peerDependencies": { + "oxlint": "~1.57.0" + } + }, + "node_modules/eslint-plugin-vue": { + "version": "10.8.0", + "resolved": "https://registry.npmjs.org/eslint-plugin-vue/-/eslint-plugin-vue-10.8.0.tgz", + "integrity": "sha512-f1J/tcbnrpgC8suPN5AtdJ5MQjuXbSU9pGRSSYAuF3SHoiYCOdEX6O22pLaRyLHXvDcOe+O5ENgc1owQ587agA==", + "dev": true, + "license": "MIT", + "dependencies": { + "@eslint-community/eslint-utils": "^4.4.0", + "natural-compare": "^1.4.0", + "nth-check": "^2.1.1", + "postcss-selector-parser": "^7.1.0", + "semver": "^7.6.3", + "xml-name-validator": "^4.0.0" + }, + "engines": { + "node": "^18.18.0 || ^20.9.0 || >=21.1.0" + }, + "peerDependencies": { + "@stylistic/eslint-plugin": "^2.0.0 || ^3.0.0 || ^4.0.0 || ^5.0.0", + "@typescript-eslint/parser": "^7.0.0 || ^8.0.0", + "eslint": "^8.57.0 || ^9.0.0 || ^10.0.0", + "vue-eslint-parser": "^10.0.0" + }, + "peerDependenciesMeta": { + "@stylistic/eslint-plugin": { + "optional": true + }, + "@typescript-eslint/parser": { + "optional": true + } + } + }, + "node_modules/eslint-scope": { + "version": "9.1.2", + "resolved": "https://registry.npmjs.org/eslint-scope/-/eslint-scope-9.1.2.tgz", + "integrity": "sha512-xS90H51cKw0jltxmvmHy2Iai1LIqrfbw57b79w/J7MfvDfkIkFZ+kj6zC3BjtUwh150HsSSdxXZcsuv72miDFQ==", + "dev": true, + "license": "BSD-2-Clause", + "dependencies": { + "@types/esrecurse": "^4.3.1", + "@types/estree": "^1.0.8", + "esrecurse": "^4.3.0", + "estraverse": "^5.2.0" + }, + "engines": { + "node": "^20.19.0 || ^22.13.0 || >=24" + }, + "funding": { + "url": "https://opencollective.com/eslint" + } + }, + "node_modules/eslint-visitor-keys": { + "version": "3.4.3", + "resolved": "https://registry.npmjs.org/eslint-visitor-keys/-/eslint-visitor-keys-3.4.3.tgz", + "integrity": "sha512-wpc+LXeiyiisxPlEkUzU6svyS1frIO3Mgxj1fdy7Pm8Ygzguax2N3Fa/D/ag1WqbOprdI+uY6wMUl8/a2G+iag==", + "dev": true, + "license": "Apache-2.0", + "engines": { + "node": "^12.22.0 || ^14.17.0 || >=16.0.0" + }, + "funding": { + "url": "https://opencollective.com/eslint" + } + }, + "node_modules/eslint/node_modules/eslint-visitor-keys": { + "version": "5.0.1", + "resolved": "https://registry.npmjs.org/eslint-visitor-keys/-/eslint-visitor-keys-5.0.1.tgz", + "integrity": "sha512-tD40eHxA35h0PEIZNeIjkHoDR4YjjJp34biM0mDvplBe//mB+IHCqHDGV7pxF+7MklTvighcCPPZC7ynWyjdTA==", + "dev": true, + "license": "Apache-2.0", + "engines": { + "node": "^20.19.0 || ^22.13.0 || >=24" + }, + "funding": { + "url": "https://opencollective.com/eslint" + } + }, + "node_modules/espree": { + "version": "11.2.0", + "resolved": "https://registry.npmjs.org/espree/-/espree-11.2.0.tgz", + "integrity": "sha512-7p3DrVEIopW1B1avAGLuCSh1jubc01H2JHc8B4qqGblmg5gI9yumBgACjWo4JlIc04ufug4xJ3SQI8HkS/Rgzw==", + "dev": true, + "license": "BSD-2-Clause", + "dependencies": { + "acorn": "^8.16.0", + "acorn-jsx": "^5.3.2", + "eslint-visitor-keys": "^5.0.1" + }, + "engines": { + "node": "^20.19.0 || ^22.13.0 || >=24" + }, + "funding": { + "url": "https://opencollective.com/eslint" + } + }, + "node_modules/espree/node_modules/eslint-visitor-keys": { + "version": "5.0.1", + "resolved": "https://registry.npmjs.org/eslint-visitor-keys/-/eslint-visitor-keys-5.0.1.tgz", + "integrity": "sha512-tD40eHxA35h0PEIZNeIjkHoDR4YjjJp34biM0mDvplBe//mB+IHCqHDGV7pxF+7MklTvighcCPPZC7ynWyjdTA==", + "dev": true, + "license": "Apache-2.0", + "engines": { + "node": "^20.19.0 || ^22.13.0 || >=24" + }, + "funding": { + "url": "https://opencollective.com/eslint" + } + }, + "node_modules/esquery": { + "version": "1.7.0", + "resolved": "https://registry.npmjs.org/esquery/-/esquery-1.7.0.tgz", + "integrity": "sha512-Ap6G0WQwcU/LHsvLwON1fAQX9Zp0A2Y6Y/cJBl9r/JbW90Zyg4/zbG6zzKa2OTALELarYHmKu0GhpM5EO+7T0g==", + "dev": true, + "license": "BSD-3-Clause", + "dependencies": { + "estraverse": "^5.1.0" + }, + "engines": { + "node": ">=0.10" + } + }, + "node_modules/esrecurse": { + "version": "4.3.0", + "resolved": "https://registry.npmjs.org/esrecurse/-/esrecurse-4.3.0.tgz", + "integrity": "sha512-KmfKL3b6G+RXvP8N1vr3Tq1kL/oCFgn2NYXEtqP8/L3pKapUA4G8cFVaoF3SU323CD4XypR/ffioHmkti6/Tag==", + "dev": true, + "license": "BSD-2-Clause", + "dependencies": { + "estraverse": "^5.2.0" + }, + "engines": { + "node": ">=4.0" + } + }, + "node_modules/estraverse": { + "version": "5.3.0", + "resolved": "https://registry.npmjs.org/estraverse/-/estraverse-5.3.0.tgz", + "integrity": "sha512-MMdARuVEQziNTeJD8DgMqmhwR11BRQ/cBP+pLtYdSTnf3MIO8fFeiINEbX36ZdNlfU/7A9f3gUw49B3oQsvwBA==", + "dev": true, + "license": "BSD-2-Clause", + "engines": { + "node": ">=4.0" + } + }, + "node_modules/estree-walker": { + "version": "2.0.2", + "resolved": "https://registry.npmjs.org/estree-walker/-/estree-walker-2.0.2.tgz", + "integrity": "sha512-Rfkk/Mp/DL7JVje3u18FxFujQlTNR2q6QfMSMB7AvCBx91NGj/ba3kCfza0f6dVDbw7YlRf/nDrn7pQrCCyQ/w==", + "license": "MIT" + }, + "node_modules/esutils": { + "version": "2.0.3", + "resolved": "https://registry.npmjs.org/esutils/-/esutils-2.0.3.tgz", + "integrity": "sha512-kVscqXk4OCp68SZ0dkgEKVi6/8ij300KBWTJq32P/dYeWTSwK41WyTxalN1eRmA5Z9UU/LX9D7FWSmV9SAYx6g==", + "dev": true, + "license": "BSD-2-Clause", + "engines": { + "node": ">=0.10.0" + } + }, + "node_modules/exsolve": { + "version": "1.0.8", + "resolved": "https://registry.npmjs.org/exsolve/-/exsolve-1.0.8.tgz", + "integrity": "sha512-LmDxfWXwcTArk8fUEnOfSZpHOJ6zOMUJKOtFLFqJLoKJetuQG874Uc7/Kki7zFLzYybmZhp1M7+98pfMqeX8yA==", + "license": "MIT" + }, + "node_modules/fast-deep-equal": { + "version": "3.1.3", + "resolved": "https://registry.npmjs.org/fast-deep-equal/-/fast-deep-equal-3.1.3.tgz", + "integrity": "sha512-f3qQ9oQy9j2AhBe/H9VC91wLmKBCCU/gDOnKNAYG5hswO7BLKj09Hc5HYNz9cGI++xlpDCIgDaitVs03ATR84Q==", + "dev": true, + "license": "MIT" + }, + "node_modules/fast-glob": { + "version": "3.3.3", + "resolved": "https://registry.npmjs.org/fast-glob/-/fast-glob-3.3.3.tgz", + "integrity": "sha512-7MptL8U0cqcFdzIzwOTHoilX9x5BrNqye7Z/LuC7kCMRio1EMSyqRK3BEAUD7sXRq4iT4AzTVuZdhgQ2TCvYLg==", + "dev": true, + "license": "MIT", + "dependencies": { + "@nodelib/fs.stat": "^2.0.2", + "@nodelib/fs.walk": "^1.2.3", + "glob-parent": "^5.1.2", + "merge2": "^1.3.0", + "micromatch": "^4.0.8" + }, + "engines": { + "node": ">=8.6.0" + } + }, + "node_modules/fast-glob/node_modules/glob-parent": { + "version": "5.1.2", + "resolved": "https://registry.npmjs.org/glob-parent/-/glob-parent-5.1.2.tgz", + "integrity": "sha512-AOIgSQCepiJYwP3ARnGx+5VnTu2HBYdzbGP45eLw1vr3zB3vZLeyed1sC9hnbcOc9/SrMyM5RPQrkGz4aS9Zow==", + "dev": true, + "license": "ISC", + "dependencies": { + "is-glob": "^4.0.1" + }, + "engines": { + "node": ">= 6" + } + }, + "node_modules/fast-json-stable-stringify": { + "version": "2.1.0", + "resolved": "https://registry.npmjs.org/fast-json-stable-stringify/-/fast-json-stable-stringify-2.1.0.tgz", + "integrity": "sha512-lhd/wF+Lk98HZoTCtlVraHtfh5XYijIjalXck7saUtuanSDyLMxnHhSXEDJqHxD7msR8D0uCmqlkwjCV8xvwHw==", + "dev": true, + "license": "MIT" + }, + "node_modules/fast-levenshtein": { + "version": "2.0.6", + "resolved": "https://registry.npmjs.org/fast-levenshtein/-/fast-levenshtein-2.0.6.tgz", + "integrity": "sha512-DCXu6Ifhqcks7TZKY3Hxp3y6qphY5SJZmrWMDrKcERSOXWQdMhU9Ig/PYrzyw/ul9jOIyh0N4M0tbC5hodg8dw==", + "dev": true, + "license": "MIT" + }, + "node_modules/fastq": { + "version": "1.20.1", + "resolved": "https://registry.npmjs.org/fastq/-/fastq-1.20.1.tgz", + "integrity": "sha512-GGToxJ/w1x32s/D2EKND7kTil4n8OVk/9mycTc4VDza13lOvpUZTGX3mFSCtV9ksdGBVzvsyAVLM6mHFThxXxw==", + "dev": true, + "license": "ISC", + "dependencies": { + "reusify": "^1.0.4" + } + }, + "node_modules/file-entry-cache": { + "version": "8.0.0", + "resolved": "https://registry.npmjs.org/file-entry-cache/-/file-entry-cache-8.0.0.tgz", + "integrity": "sha512-XXTUwCvisa5oacNGRP9SfNtYBNAMi+RPwBFmblZEF7N7swHYQS6/Zfk7SRwx4D5j3CH211YNRco1DEMNVfZCnQ==", + "dev": true, + "license": "MIT", + "dependencies": { + "flat-cache": "^4.0.0" + }, + "engines": { + "node": ">=16.0.0" + } + }, + "node_modules/fill-range": { + "version": "7.1.1", + "resolved": "https://registry.npmjs.org/fill-range/-/fill-range-7.1.1.tgz", + "integrity": "sha512-YsGpe3WHLK8ZYi4tWDg2Jy3ebRz2rXowDxnld4bkQB00cc/1Zw9AWnC0i9ztDJitivtQvaI9KaLyKrc+hBW0yg==", + "dev": true, + "license": "MIT", + "dependencies": { + "to-regex-range": "^5.0.1" + }, + "engines": { + "node": ">=8" + } + }, + "node_modules/find-up": { + "version": "5.0.0", + "resolved": "https://registry.npmjs.org/find-up/-/find-up-5.0.0.tgz", + "integrity": "sha512-78/PXT1wlLLDgTzDs7sjq9hzz0vXD+zn+7wypEe4fXQxCmdmqfGsEPQxmiCSQI3ajFV91bVSsvNtrJRiW6nGng==", + "dev": true, + "license": "MIT", + "dependencies": { + "locate-path": "^6.0.0", + "path-exists": "^4.0.0" + }, + "engines": { + "node": ">=10" + }, + "funding": { + "url": "https://github.com/sponsors/sindresorhus" + } + }, + "node_modules/flat-cache": { + "version": "4.0.1", + "resolved": "https://registry.npmjs.org/flat-cache/-/flat-cache-4.0.1.tgz", + "integrity": "sha512-f7ccFPK3SXFHpx15UIGyRJ/FJQctuKZ0zVuN3frBo4HnK3cay9VEW0R6yPYFHC0AgqhukPzKjq22t5DmAyqGyw==", + "dev": true, + "license": "MIT", + "dependencies": { + "flatted": "^3.2.9", + "keyv": "^4.5.4" + }, + "engines": { + "node": ">=16" + } + }, + "node_modules/flatted": { + "version": "3.4.2", + "resolved": "https://registry.npmjs.org/flatted/-/flatted-3.4.2.tgz", + "integrity": "sha512-PjDse7RzhcPkIJwy5t7KPWQSZ9cAbzQXcafsetQoD7sOJRQlGikNbx7yZp2OotDnJyrDcbyRq3Ttb18iYOqkxA==", + "dev": true, + "license": "ISC" + }, + "node_modules/fsevents": { + "version": "2.3.3", + "resolved": "https://registry.npmjs.org/fsevents/-/fsevents-2.3.3.tgz", + "integrity": "sha512-5xoDfX+fL7faATnagmWPpbFtwh/R77WmMMqqHGS65C3vvB0YHrgF+B1YmZ3441tMj5n63k0212XNoJwzlhffQw==", + "dev": true, + "hasInstallScript": true, + "license": "MIT", + "optional": true, + "os": [ + "darwin" + ], + "engines": { + "node": "^8.16.0 || ^10.6.0 || >=11.0.0" + } + }, + "node_modules/gensync": { + "version": "1.0.0-beta.2", + "resolved": "https://registry.npmjs.org/gensync/-/gensync-1.0.0-beta.2.tgz", + "integrity": "sha512-3hN7NaskYvMDLQY55gnW3NQ+mesEAepTqlg+VEbj7zzqEMBVNhzcGYYeqFo/TlYz6eQiFcp1HcsCZO+nGgS8zg==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=6.9.0" + } + }, + "node_modules/glob-parent": { + "version": "6.0.2", + "resolved": "https://registry.npmjs.org/glob-parent/-/glob-parent-6.0.2.tgz", + "integrity": "sha512-XxwI8EOhVQgWp6iDL+3b0r86f4d6AX6zSU55HfB4ydCEuXLXc5FcYeOu+nnGftS4TEju/11rt4KJPTMgbfmv4A==", + "dev": true, + "license": "ISC", + "dependencies": { + "is-glob": "^4.0.3" + }, + "engines": { + "node": ">=10.13.0" + } + }, + "node_modules/hookable": { + "version": "5.5.3", + "resolved": "https://registry.npmjs.org/hookable/-/hookable-5.5.3.tgz", + "integrity": "sha512-Yc+BQe8SvoXH1643Qez1zqLRmbA5rCL+sSmk6TVos0LWVfNIB7PGncdlId77WzLGSIB5KaWgTaNTs2lNVEI6VQ==", + "license": "MIT" + }, + "node_modules/ignore": { + "version": "5.3.2", + "resolved": "https://registry.npmjs.org/ignore/-/ignore-5.3.2.tgz", + "integrity": "sha512-hsBTNUqQTDwkWtcdYI2i06Y/nUBEsNEDJKjWdigLvegy8kDuJAS8uRlpkkcQpyEXL0Z/pjDy5HBmMjRCJ2gq+g==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">= 4" + } + }, + "node_modules/imurmurhash": { + "version": "0.1.4", + "resolved": "https://registry.npmjs.org/imurmurhash/-/imurmurhash-0.1.4.tgz", + "integrity": "sha512-JmXMZ6wuvDmLiHEml9ykzqO6lwFbof0GG4IkcGaENdCRDDmMVnny7s5HsIgHCbaq0w2MyPhDqkhTUgS2LU2PHA==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=0.8.19" + } + }, + "node_modules/is-docker": { + "version": "3.0.0", + "resolved": "https://registry.npmjs.org/is-docker/-/is-docker-3.0.0.tgz", + "integrity": "sha512-eljcgEDlEns/7AXFosB5K/2nCM4P7FQPkGc/DWLy5rmFEWvZayGrik1d9/QIY5nJ4f9YsVvBkA6kJpHn9rISdQ==", + "dev": true, + "license": "MIT", + "bin": { + "is-docker": "cli.js" + }, + "engines": { + "node": "^12.20.0 || ^14.13.1 || >=16.0.0" + }, + "funding": { + "url": "https://github.com/sponsors/sindresorhus" + } + }, + "node_modules/is-extglob": { + "version": "2.1.1", + "resolved": "https://registry.npmjs.org/is-extglob/-/is-extglob-2.1.1.tgz", + "integrity": "sha512-SbKbANkN603Vi4jEZv49LeVJMn4yGwsbzZworEoyEiutsN3nJYdbO36zfhGJ6QEDpOZIFkDtnq5JRxmvl3jsoQ==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=0.10.0" + } + }, + "node_modules/is-glob": { + "version": "4.0.3", + "resolved": "https://registry.npmjs.org/is-glob/-/is-glob-4.0.3.tgz", + "integrity": "sha512-xelSayHH36ZgE7ZWhli7pW34hNbNl8Ojv5KVmkJD4hBdD3th8Tfk9vYasLM+mXWOZhFkgZfxhLSnrwRr4elSSg==", + "dev": true, + "license": "MIT", + "dependencies": { + "is-extglob": "^2.1.1" + }, + "engines": { + "node": ">=0.10.0" + } + }, + "node_modules/is-inside-container": { + "version": "1.0.0", + "resolved": "https://registry.npmjs.org/is-inside-container/-/is-inside-container-1.0.0.tgz", + "integrity": "sha512-KIYLCCJghfHZxqjYBE7rEy0OBuTd5xCHS7tHVgvCLkx7StIoaxwNW3hCALgEUjFfeRk+MG/Qxmp/vtETEF3tRA==", + "dev": true, + "license": "MIT", + "dependencies": { + "is-docker": "^3.0.0" + }, + "bin": { + "is-inside-container": "cli.js" + }, + "engines": { + "node": ">=14.16" + }, + "funding": { + "url": "https://github.com/sponsors/sindresorhus" + } + }, + "node_modules/is-number": { + "version": "7.0.0", + "resolved": "https://registry.npmjs.org/is-number/-/is-number-7.0.0.tgz", + "integrity": "sha512-41Cifkg6e8TylSpdtTpeLVMqvSBEVzTttHvERD741+pnZ8ANv0004MRL43QKPDlK9cGvNp6NZWZUBlbGXYxxng==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=0.12.0" + } + }, + "node_modules/is-what": { + "version": "5.5.0", + "resolved": "https://registry.npmjs.org/is-what/-/is-what-5.5.0.tgz", + "integrity": "sha512-oG7cgbmg5kLYae2N5IVd3jm2s+vldjxJzK1pcu9LfpGuQ93MQSzo0okvRna+7y5ifrD+20FE8FvjusyGaz14fw==", + "license": "MIT", + "engines": { + "node": ">=18" + }, + "funding": { + "url": "https://github.com/sponsors/mesqueeb" + } + }, + "node_modules/is-wsl": { + "version": "3.1.1", + "resolved": "https://registry.npmjs.org/is-wsl/-/is-wsl-3.1.1.tgz", + "integrity": "sha512-e6rvdUCiQCAuumZslxRJWR/Doq4VpPR82kqclvcS0efgt430SlGIk05vdCN58+VrzgtIcfNODjozVielycD4Sw==", + "dev": true, + "license": "MIT", + "dependencies": { + "is-inside-container": "^1.0.0" + }, + "engines": { + "node": ">=16" + }, + "funding": { + "url": "https://github.com/sponsors/sindresorhus" + } + }, + "node_modules/isexe": { + "version": "2.0.0", + "resolved": "https://registry.npmjs.org/isexe/-/isexe-2.0.0.tgz", + "integrity": "sha512-RHxMLp9lnKHGHRng9QFhRCMbYAcVpn69smSGcq3f36xjgVVWThj4qqLbTLlq7Ssj8B+fIQ1EuCEGI2lKsyQeIw==", + "dev": true, + "license": "ISC" + }, + "node_modules/jiti": { + "version": "2.6.1", + "resolved": "https://registry.npmjs.org/jiti/-/jiti-2.6.1.tgz", + "integrity": "sha512-ekilCSN1jwRvIbgeg/57YFh8qQDNbwDb9xT/qu2DAHbFFZUicIl4ygVaAvzveMhMVr3LnpSKTNnwt8PoOfmKhQ==", + "dev": true, + "license": "MIT", + "bin": { + "jiti": "lib/jiti-cli.mjs" + } + }, + "node_modules/js-tokens": { + "version": "4.0.0", + "resolved": "https://registry.npmjs.org/js-tokens/-/js-tokens-4.0.0.tgz", + "integrity": "sha512-RdJUflcE3cUzKiMqQgsCu06FPu9UdIJO0beYbPhHN4k6apgJtifcoCtT9bcxOpYBtpD2kCM6Sbzg4CausW/PKQ==", + "dev": true, + "license": "MIT" + }, + "node_modules/jsesc": { + "version": "3.1.0", + "resolved": "https://registry.npmjs.org/jsesc/-/jsesc-3.1.0.tgz", + "integrity": "sha512-/sM3dO2FOzXjKQhJuo0Q173wf2KOo8t4I8vHy6lF9poUp7bKT0/NHE8fPX23PwfhnykfqnC2xRxOnVw5XuGIaA==", + "license": "MIT", + "bin": { + "jsesc": "bin/jsesc" + }, + "engines": { + "node": ">=6" + } + }, + "node_modules/json-buffer": { + "version": "3.0.1", + "resolved": "https://registry.npmjs.org/json-buffer/-/json-buffer-3.0.1.tgz", + "integrity": "sha512-4bV5BfR2mqfQTJm+V5tPPdf+ZpuhiIvTuAB5g8kcrXOZpTT/QwwVRWBywX1ozr6lEuPdbHxwaJlm9G6mI2sfSQ==", + "dev": true, + "license": "MIT" + }, + "node_modules/json-parse-even-better-errors": { + "version": "4.0.0", + "resolved": "https://registry.npmjs.org/json-parse-even-better-errors/-/json-parse-even-better-errors-4.0.0.tgz", + "integrity": "sha512-lR4MXjGNgkJc7tkQ97kb2nuEMnNCyU//XYVH0MKTGcXEiSudQ5MKGKen3C5QubYy0vmq+JGitUg92uuywGEwIA==", + "dev": true, + "license": "MIT", + "engines": { + "node": "^18.17.0 || >=20.5.0" + } + }, + "node_modules/json-schema-traverse": { + "version": "0.4.1", + "resolved": "https://registry.npmjs.org/json-schema-traverse/-/json-schema-traverse-0.4.1.tgz", + "integrity": "sha512-xbbCH5dCYU5T8LcEhhuh7HJ88HXuW3qsI3Y0zOZFKfZEHcpWiHU/Jxzk629Brsab/mMiHQti9wMP+845RPe3Vg==", + "dev": true, + "license": "MIT" + }, + "node_modules/json-stable-stringify-without-jsonify": { + "version": "1.0.1", + "resolved": "https://registry.npmjs.org/json-stable-stringify-without-jsonify/-/json-stable-stringify-without-jsonify-1.0.1.tgz", + "integrity": "sha512-Bdboy+l7tA3OGW6FjyFHWkP5LuByj1Tk33Ljyq0axyzdk9//JSi2u3fP1QSmd1KNwq6VOKYGlAu87CisVir6Pw==", + "dev": true, + "license": "MIT" + }, + "node_modules/json5": { + "version": "2.2.3", + "resolved": "https://registry.npmjs.org/json5/-/json5-2.2.3.tgz", + "integrity": "sha512-XmOWe7eyHYH14cLdVPoyg+GOH3rYX++KpzrylJwSW98t3Nk+U8XOl8FWKOgwtzdb8lXGf6zYwDUzeHMWfxasyg==", + "license": "MIT", + "bin": { + "json5": "lib/cli.js" + }, + "engines": { + "node": ">=6" + } + }, + "node_modules/jsonc-parser": { + "version": "3.3.1", + "resolved": "https://registry.npmjs.org/jsonc-parser/-/jsonc-parser-3.3.1.tgz", + "integrity": "sha512-HUgH65KyejrUFPvHFPbqOY0rsFip3Bo5wb4ngvdi1EpCYWUQDC5V+Y7mZws+DLkr4M//zQJoanu1SP+87Dv1oQ==", + "dev": true, + "license": "MIT" + }, + "node_modules/keyv": { + "version": "4.5.4", + "resolved": "https://registry.npmjs.org/keyv/-/keyv-4.5.4.tgz", + "integrity": "sha512-oxVHkHR/EJf2CNXnWxRLW6mg7JyCCUcG0DtEGmL2ctUo1PNTin1PUil+r/+4r5MpVgC/fn1kjsx7mjSujKqIpw==", + "dev": true, + "license": "MIT", + "dependencies": { + "json-buffer": "3.0.1" + } + }, + "node_modules/kolorist": { + "version": "1.8.0", + "resolved": "https://registry.npmjs.org/kolorist/-/kolorist-1.8.0.tgz", + "integrity": "sha512-Y+60/zizpJ3HRH8DCss+q95yr6145JXZo46OTpFvDZWLfRCE4qChOyk1b26nMaNpfHHgxagk9dXT5OP0Tfe+dQ==", + "dev": true, + "license": "MIT" + }, + "node_modules/levn": { + "version": "0.4.1", + "resolved": "https://registry.npmjs.org/levn/-/levn-0.4.1.tgz", + "integrity": "sha512-+bT2uH4E5LGE7h/n3evcS/sQlJXCpIp6ym8OWJ5eV6+67Dsql/LaaT7qJBAt2rzfoa/5QBGBhxDix1dMt2kQKQ==", + "dev": true, + "license": "MIT", + "dependencies": { + "prelude-ls": "^1.2.1", + "type-check": "~0.4.0" + }, + "engines": { + "node": ">= 0.8.0" + } + }, + "node_modules/lightningcss": { + "version": "1.32.0", + "resolved": "https://registry.npmjs.org/lightningcss/-/lightningcss-1.32.0.tgz", + "integrity": "sha512-NXYBzinNrblfraPGyrbPoD19C1h9lfI/1mzgWYvXUTe414Gz/X1FD2XBZSZM7rRTrMA8JL3OtAaGifrIKhQ5yQ==", + "dev": true, + "license": "MPL-2.0", + "dependencies": { + "detect-libc": "^2.0.3" + }, + "engines": { + "node": ">= 12.0.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/parcel" + }, + "optionalDependencies": { + "lightningcss-android-arm64": "1.32.0", + "lightningcss-darwin-arm64": "1.32.0", + "lightningcss-darwin-x64": "1.32.0", + "lightningcss-freebsd-x64": "1.32.0", + "lightningcss-linux-arm-gnueabihf": "1.32.0", + "lightningcss-linux-arm64-gnu": "1.32.0", + "lightningcss-linux-arm64-musl": "1.32.0", + "lightningcss-linux-x64-gnu": "1.32.0", + "lightningcss-linux-x64-musl": "1.32.0", + "lightningcss-win32-arm64-msvc": "1.32.0", + "lightningcss-win32-x64-msvc": "1.32.0" + } + }, + "node_modules/lightningcss-android-arm64": { + "version": "1.32.0", + "resolved": "https://registry.npmjs.org/lightningcss-android-arm64/-/lightningcss-android-arm64-1.32.0.tgz", + "integrity": "sha512-YK7/ClTt4kAK0vo6w3X+Pnm0D2cf2vPHbhOXdoNti1Ga0al1P4TBZhwjATvjNwLEBCnKvjJc2jQgHXH0NEwlAg==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MPL-2.0", + "optional": true, + "os": [ + "android" + ], + "engines": { + "node": ">= 12.0.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/parcel" + } + }, + "node_modules/lightningcss-darwin-arm64": { + "version": "1.32.0", + "resolved": "https://registry.npmjs.org/lightningcss-darwin-arm64/-/lightningcss-darwin-arm64-1.32.0.tgz", + "integrity": "sha512-RzeG9Ju5bag2Bv1/lwlVJvBE3q6TtXskdZLLCyfg5pt+HLz9BqlICO7LZM7VHNTTn/5PRhHFBSjk5lc4cmscPQ==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MPL-2.0", + "optional": true, + "os": [ + "darwin" + ], + "engines": { + "node": ">= 12.0.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/parcel" + } + }, + "node_modules/lightningcss-darwin-x64": { + "version": "1.32.0", + "resolved": "https://registry.npmjs.org/lightningcss-darwin-x64/-/lightningcss-darwin-x64-1.32.0.tgz", + "integrity": "sha512-U+QsBp2m/s2wqpUYT/6wnlagdZbtZdndSmut/NJqlCcMLTWp5muCrID+K5UJ6jqD2BFshejCYXniPDbNh73V8w==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MPL-2.0", + "optional": true, + "os": [ + "darwin" + ], + "engines": { + "node": ">= 12.0.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/parcel" + } + }, + "node_modules/lightningcss-freebsd-x64": { + "version": "1.32.0", + "resolved": "https://registry.npmjs.org/lightningcss-freebsd-x64/-/lightningcss-freebsd-x64-1.32.0.tgz", + "integrity": "sha512-JCTigedEksZk3tHTTthnMdVfGf61Fky8Ji2E4YjUTEQX14xiy/lTzXnu1vwiZe3bYe0q+SpsSH/CTeDXK6WHig==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MPL-2.0", + "optional": true, + "os": [ + "freebsd" + ], + "engines": { + "node": ">= 12.0.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/parcel" + } + }, + "node_modules/lightningcss-linux-arm-gnueabihf": { + "version": "1.32.0", + "resolved": "https://registry.npmjs.org/lightningcss-linux-arm-gnueabihf/-/lightningcss-linux-arm-gnueabihf-1.32.0.tgz", + "integrity": "sha512-x6rnnpRa2GL0zQOkt6rts3YDPzduLpWvwAF6EMhXFVZXD4tPrBkEFqzGowzCsIWsPjqSK+tyNEODUBXeeVHSkw==", + "cpu": [ + "arm" + ], + "dev": true, + "license": "MPL-2.0", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">= 12.0.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/parcel" + } + }, + "node_modules/lightningcss-linux-arm64-gnu": { + "version": "1.32.0", + "resolved": "https://registry.npmjs.org/lightningcss-linux-arm64-gnu/-/lightningcss-linux-arm64-gnu-1.32.0.tgz", + "integrity": "sha512-0nnMyoyOLRJXfbMOilaSRcLH3Jw5z9HDNGfT/gwCPgaDjnx0i8w7vBzFLFR1f6CMLKF8gVbebmkUN3fa/kQJpQ==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MPL-2.0", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">= 12.0.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/parcel" + } + }, + "node_modules/lightningcss-linux-arm64-musl": { + "version": "1.32.0", + "resolved": "https://registry.npmjs.org/lightningcss-linux-arm64-musl/-/lightningcss-linux-arm64-musl-1.32.0.tgz", + "integrity": "sha512-UpQkoenr4UJEzgVIYpI80lDFvRmPVg6oqboNHfoH4CQIfNA+HOrZ7Mo7KZP02dC6LjghPQJeBsvXhJod/wnIBg==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MPL-2.0", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">= 12.0.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/parcel" + } + }, + "node_modules/lightningcss-linux-x64-gnu": { + "version": "1.32.0", + "resolved": "https://registry.npmjs.org/lightningcss-linux-x64-gnu/-/lightningcss-linux-x64-gnu-1.32.0.tgz", + "integrity": "sha512-V7Qr52IhZmdKPVr+Vtw8o+WLsQJYCTd8loIfpDaMRWGUZfBOYEJeyJIkqGIDMZPwPx24pUMfwSxxI8phr/MbOA==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MPL-2.0", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">= 12.0.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/parcel" + } + }, + "node_modules/lightningcss-linux-x64-musl": { + "version": "1.32.0", + "resolved": "https://registry.npmjs.org/lightningcss-linux-x64-musl/-/lightningcss-linux-x64-musl-1.32.0.tgz", + "integrity": "sha512-bYcLp+Vb0awsiXg/80uCRezCYHNg1/l3mt0gzHnWV9XP1W5sKa5/TCdGWaR/zBM2PeF/HbsQv/j2URNOiVuxWg==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MPL-2.0", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">= 12.0.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/parcel" + } + }, + "node_modules/lightningcss-win32-arm64-msvc": { + "version": "1.32.0", + "resolved": "https://registry.npmjs.org/lightningcss-win32-arm64-msvc/-/lightningcss-win32-arm64-msvc-1.32.0.tgz", + "integrity": "sha512-8SbC8BR40pS6baCM8sbtYDSwEVQd4JlFTOlaD3gWGHfThTcABnNDBda6eTZeqbofalIJhFx0qKzgHJmcPTnGdw==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MPL-2.0", + "optional": true, + "os": [ + "win32" + ], + "engines": { + "node": ">= 12.0.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/parcel" + } + }, + "node_modules/lightningcss-win32-x64-msvc": { + "version": "1.32.0", + "resolved": "https://registry.npmjs.org/lightningcss-win32-x64-msvc/-/lightningcss-win32-x64-msvc-1.32.0.tgz", + "integrity": "sha512-Amq9B/SoZYdDi1kFrojnoqPLxYhQ4Wo5XiL8EVJrVsB8ARoC1PWW6VGtT0WKCemjy8aC+louJnjS7U18x3b06Q==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MPL-2.0", + "optional": true, + "os": [ + "win32" + ], + "engines": { + "node": ">= 12.0.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/parcel" + } + }, + "node_modules/local-pkg": { + "version": "1.1.2", + "resolved": "https://registry.npmjs.org/local-pkg/-/local-pkg-1.1.2.tgz", + "integrity": "sha512-arhlxbFRmoQHl33a0Zkle/YWlmNwoyt6QNZEIJcqNbdrsix5Lvc4HyyI3EnwxTYlZYc32EbYrQ8SzEZ7dqgg9A==", + "license": "MIT", + "dependencies": { + "mlly": "^1.7.4", + "pkg-types": "^2.3.0", + "quansync": "^0.2.11" + }, + "engines": { + "node": ">=14" + }, + "funding": { + "url": "https://github.com/sponsors/antfu" + } + }, + "node_modules/locate-path": { + "version": "6.0.0", + "resolved": "https://registry.npmjs.org/locate-path/-/locate-path-6.0.0.tgz", + "integrity": "sha512-iPZK6eYjbxRu3uB4/WZ3EsEIMJFMqAoopl3R+zuq0UjcAm/MO6KCweDgPfP3elTztoKP3KtnVHxTn2NHBSDVUw==", + "dev": true, + "license": "MIT", + "dependencies": { + "p-locate": "^5.0.0" + }, + "engines": { + "node": ">=10" + }, + "funding": { + "url": "https://github.com/sponsors/sindresorhus" + } + }, + "node_modules/lru-cache": { + "version": "5.1.1", + "resolved": "https://registry.npmjs.org/lru-cache/-/lru-cache-5.1.1.tgz", + "integrity": "sha512-KpNARQA3Iwv+jTA0utUVVbrh+Jlrr1Fv0e56GGzAFOXN7dk/FviaDW8LHmK52DlcH4WP2n6gI8vN1aesBFgo9w==", + "dev": true, + "license": "ISC", + "dependencies": { + "yallist": "^3.0.2" + } + }, + "node_modules/magic-string": { + "version": "0.30.21", + "resolved": "https://registry.npmjs.org/magic-string/-/magic-string-0.30.21.tgz", + "integrity": "sha512-vd2F4YUyEXKGcLHoq+TEyCjxueSeHnFxyyjNp80yg0XV4vUhnDer/lvvlqM/arB5bXQN5K2/3oinyCRyx8T2CQ==", + "license": "MIT", + "dependencies": { + "@jridgewell/sourcemap-codec": "^1.5.5" + } + }, + "node_modules/magic-string-ast": { + "version": "1.0.3", + "resolved": "https://registry.npmjs.org/magic-string-ast/-/magic-string-ast-1.0.3.tgz", + "integrity": "sha512-CvkkH1i81zl7mmb94DsRiFeG9V2fR2JeuK8yDgS8oiZSFa++wWLEgZ5ufEOyLHbvSbD1gTRKv9NdX69Rnvr9JA==", + "license": "MIT", + "dependencies": { + "magic-string": "^0.30.19" + }, + "engines": { + "node": ">=20.19.0" + }, + "funding": { + "url": "https://github.com/sponsors/sxzz" + } + }, + "node_modules/memorystream": { + "version": "0.3.1", + "resolved": "https://registry.npmjs.org/memorystream/-/memorystream-0.3.1.tgz", + "integrity": "sha512-S3UwM3yj5mtUSEfP41UZmt/0SCoVYUcU1rkXv+BQ5Ig8ndL4sPoJNBUJERafdPb5jjHJGuMgytgKvKIf58XNBw==", + "dev": true, + "engines": { + "node": ">= 0.10.0" + } + }, + "node_modules/merge2": { + "version": "1.4.1", + "resolved": "https://registry.npmjs.org/merge2/-/merge2-1.4.1.tgz", + "integrity": "sha512-8q7VEgMJW4J8tcfVPy8g09NcQwZdbwFEqhe/WZkoIzjn/3TGDwtOCYtXGxA3O8tPzpczCCDgv+P2P5y00ZJOOg==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">= 8" + } + }, + "node_modules/micromatch": { + "version": "4.0.8", + "resolved": "https://registry.npmjs.org/micromatch/-/micromatch-4.0.8.tgz", + "integrity": "sha512-PXwfBhYu0hBCPw8Dn0E+WDYb7af3dSLVWKi3HGv84IdF4TyFoC0ysxFd0Goxw7nSv4T/PzEJQxsYsEiFCKo2BA==", + "dev": true, + "license": "MIT", + "dependencies": { + "braces": "^3.0.3", + "picomatch": "^2.3.1" + }, + "engines": { + "node": ">=8.6" + } + }, + "node_modules/minimatch": { + "version": "10.2.5", + "resolved": "https://registry.npmjs.org/minimatch/-/minimatch-10.2.5.tgz", + "integrity": "sha512-MULkVLfKGYDFYejP07QOurDLLQpcjk7Fw+7jXS2R2czRQzR56yHRveU5NDJEOviH+hETZKSkIk5c+T23GjFUMg==", + "dev": true, + "license": "BlueOak-1.0.0", + "dependencies": { + "brace-expansion": "^5.0.5" + }, + "engines": { + "node": "18 || 20 || >=22" + }, + "funding": { + "url": "https://github.com/sponsors/isaacs" + } + }, + "node_modules/mitt": { + "version": "3.0.1", + "resolved": "https://registry.npmjs.org/mitt/-/mitt-3.0.1.tgz", + "integrity": "sha512-vKivATfr97l2/QBCYAkXYDbrIWPM2IIKEl7YPhjCvKlG3kE2gm+uBo6nEXK3M5/Ffh/FLpKExzOQ3JJoJGFKBw==", + "license": "MIT" + }, + "node_modules/mlly": { + "version": "1.8.2", + "resolved": "https://registry.npmjs.org/mlly/-/mlly-1.8.2.tgz", + "integrity": "sha512-d+ObxMQFmbt10sretNDytwt85VrbkhhUA/JBGm1MPaWJ65Cl4wOgLaB1NYvJSZ0Ef03MMEU/0xpPMXUIQ29UfA==", + "license": "MIT", + "dependencies": { + "acorn": "^8.16.0", + "pathe": "^2.0.3", + "pkg-types": "^1.3.1", + "ufo": "^1.6.3" + } + }, + "node_modules/mlly/node_modules/confbox": { + "version": "0.1.8", + "resolved": "https://registry.npmjs.org/confbox/-/confbox-0.1.8.tgz", + "integrity": "sha512-RMtmw0iFkeR4YV+fUOSucriAQNb9g8zFR52MWCtl+cCZOFRNL6zeB395vPzFhEjjn4fMxXudmELnl/KF/WrK6w==", + "license": "MIT" + }, + "node_modules/mlly/node_modules/pkg-types": { + "version": "1.3.1", + "resolved": "https://registry.npmjs.org/pkg-types/-/pkg-types-1.3.1.tgz", + "integrity": "sha512-/Jm5M4RvtBFVkKWRu2BLUTNP8/M2a+UwuAX+ae4770q1qVGtfjG+WTCupoZixokjmHiry8uI+dlY8KXYV5HVVQ==", + "license": "MIT", + "dependencies": { + "confbox": "^0.1.8", + "mlly": "^1.7.4", + "pathe": "^2.0.1" + } + }, + "node_modules/mrmime": { + "version": "2.0.1", + "resolved": "https://registry.npmjs.org/mrmime/-/mrmime-2.0.1.tgz", + "integrity": "sha512-Y3wQdFg2Va6etvQ5I82yUhGdsKrcYox6p7FfL1LbK2J4V01F9TGlepTIhnK24t7koZibmg82KGglhA1XK5IsLQ==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=10" + } + }, + "node_modules/ms": { + "version": "2.1.3", + "resolved": "https://registry.npmjs.org/ms/-/ms-2.1.3.tgz", + "integrity": "sha512-6FlzubTLZG3J2a/NVCAleEhjzq5oxgHyaCU9yYXvcLsvoVaHJq/s5xXI6/XXP6tz7R9xAOtHnSO/tXtF3WRTlA==", + "dev": true, + "license": "MIT" + }, + "node_modules/muggle-string": { + "version": "0.4.1", + "resolved": "https://registry.npmjs.org/muggle-string/-/muggle-string-0.4.1.tgz", + "integrity": "sha512-VNTrAak/KhO2i8dqqnqnAHOa3cYBwXEZe9h+D5h/1ZqFSTEFHdM65lR7RoIqq3tBBYavsOXV84NoHXZ0AkPyqQ==", + "license": "MIT" + }, + "node_modules/nanoid": { + "version": "3.3.11", + "resolved": "https://registry.npmjs.org/nanoid/-/nanoid-3.3.11.tgz", + "integrity": "sha512-N8SpfPUnUp1bK+PMYW8qSWdl9U+wwNWI4QKxOYDy9JAro3WMX7p2OeVRF9v+347pnakNevPmiHhNmZ2HbFA76w==", + "funding": [ + { + "type": "github", + "url": "https://github.com/sponsors/ai" + } + ], + "license": "MIT", + "bin": { + "nanoid": "bin/nanoid.cjs" + }, + "engines": { + "node": "^10 || ^12 || ^13.7 || ^14 || >=15.0.1" + } + }, + "node_modules/natural-compare": { + "version": "1.4.0", + "resolved": "https://registry.npmjs.org/natural-compare/-/natural-compare-1.4.0.tgz", + "integrity": "sha512-OWND8ei3VtNC9h7V60qff3SVobHr996CTwgxubgyQYEpg290h9J0buyECNNJexkFm5sOajh5G116RYA1c8ZMSw==", + "dev": true, + "license": "MIT" + }, + "node_modules/node-releases": { + "version": "2.0.36", + "resolved": "https://registry.npmjs.org/node-releases/-/node-releases-2.0.36.tgz", + "integrity": "sha512-TdC8FSgHz8Mwtw9g5L4gR/Sh9XhSP/0DEkQxfEFXOpiul5IiHgHan2VhYYb6agDSfp4KuvltmGApc8HMgUrIkA==", + "dev": true, + "license": "MIT" + }, + "node_modules/npm-normalize-package-bin": { + "version": "4.0.0", + "resolved": "https://registry.npmjs.org/npm-normalize-package-bin/-/npm-normalize-package-bin-4.0.0.tgz", + "integrity": "sha512-TZKxPvItzai9kN9H/TkmCtx/ZN/hvr3vUycjlfmH0ootY9yFBzNOpiXAdIn1Iteqsvk4lQn6B5PTrt+n6h8k/w==", + "dev": true, + "license": "ISC", + "engines": { + "node": "^18.17.0 || >=20.5.0" + } + }, + "node_modules/npm-run-all2": { + "version": "8.0.4", + "resolved": "https://registry.npmjs.org/npm-run-all2/-/npm-run-all2-8.0.4.tgz", + "integrity": "sha512-wdbB5My48XKp2ZfJUlhnLVihzeuA1hgBnqB2J9ahV77wLS+/YAJAlN8I+X3DIFIPZ3m5L7nplmlbhNiFDmXRDA==", + "dev": true, + "license": "MIT", + "dependencies": { + "ansi-styles": "^6.2.1", + "cross-spawn": "^7.0.6", + "memorystream": "^0.3.1", + "picomatch": "^4.0.2", + "pidtree": "^0.6.0", + "read-package-json-fast": "^4.0.0", + "shell-quote": "^1.7.3", + "which": "^5.0.0" + }, + "bin": { + "npm-run-all": "bin/npm-run-all/index.js", + "npm-run-all2": "bin/npm-run-all/index.js", + "run-p": "bin/run-p/index.js", + "run-s": "bin/run-s/index.js" + }, + "engines": { + "node": "^20.5.0 || >=22.0.0", + "npm": ">= 10" + } + }, + "node_modules/npm-run-all2/node_modules/isexe": { + "version": "3.1.5", + "resolved": "https://registry.npmjs.org/isexe/-/isexe-3.1.5.tgz", + "integrity": "sha512-6B3tLtFqtQS4ekarvLVMZ+X+VlvQekbe4taUkf/rhVO3d/h0M2rfARm/pXLcPEsjjMsFgrFgSrhQIxcSVrBz8w==", + "dev": true, + "license": "BlueOak-1.0.0", + "engines": { + "node": ">=18" + } + }, + "node_modules/npm-run-all2/node_modules/picomatch": { + "version": "4.0.4", + "resolved": "https://registry.npmjs.org/picomatch/-/picomatch-4.0.4.tgz", + "integrity": "sha512-QP88BAKvMam/3NxH6vj2o21R6MjxZUAd6nlwAS/pnGvN9IVLocLHxGYIzFhg6fUQ+5th6P4dv4eW9jX3DSIj7A==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=12" + }, + "funding": { + "url": "https://github.com/sponsors/jonschlinkert" + } + }, + "node_modules/npm-run-all2/node_modules/which": { + "version": "5.0.0", + "resolved": "https://registry.npmjs.org/which/-/which-5.0.0.tgz", + "integrity": "sha512-JEdGzHwwkrbWoGOlIHqQ5gtprKGOenpDHpxE9zVR1bWbOtYRyPPHMe9FaP6x61CmNaTThSkb0DAJte5jD+DmzQ==", + "dev": true, + "license": "ISC", + "dependencies": { + "isexe": "^3.1.1" + }, + "bin": { + "node-which": "bin/which.js" + }, + "engines": { + "node": "^18.17.0 || >=20.5.0" + } + }, + "node_modules/nth-check": { + "version": "2.1.1", + "resolved": "https://registry.npmjs.org/nth-check/-/nth-check-2.1.1.tgz", + "integrity": "sha512-lqjrjmaOoAnWfMmBPL+XNnynZh2+swxiX3WUE0s4yEHI6m+AwrK2UZOimIRl3X/4QctVqS8AiZjFqyOGrMXb/w==", + "dev": true, + "license": "BSD-2-Clause", + "dependencies": { + "boolbase": "^1.0.0" + }, + "funding": { + "url": "https://github.com/fb55/nth-check?sponsor=1" + } + }, + "node_modules/ohash": { + "version": "2.0.11", + "resolved": "https://registry.npmjs.org/ohash/-/ohash-2.0.11.tgz", + "integrity": "sha512-RdR9FQrFwNBNXAr4GixM8YaRZRJ5PUWbKYbE5eOsrwAjJW0q2REGcf79oYPsLyskQCZG1PLN+S/K1V00joZAoQ==", + "dev": true, + "license": "MIT" + }, + "node_modules/open": { + "version": "10.2.0", + "resolved": "https://registry.npmjs.org/open/-/open-10.2.0.tgz", + "integrity": "sha512-YgBpdJHPyQ2UE5x+hlSXcnejzAvD0b22U2OuAP+8OnlJT+PjWPxtgmGqKKc+RgTM63U9gN0YzrYc71R2WT/hTA==", + "dev": true, + "license": "MIT", + "dependencies": { + "default-browser": "^5.2.1", + "define-lazy-prop": "^3.0.0", + "is-inside-container": "^1.0.0", + "wsl-utils": "^0.1.0" + }, + "engines": { + "node": ">=18" + }, + "funding": { + "url": "https://github.com/sponsors/sindresorhus" + } + }, + "node_modules/optionator": { + "version": "0.9.4", + "resolved": "https://registry.npmjs.org/optionator/-/optionator-0.9.4.tgz", + "integrity": "sha512-6IpQ7mKUxRcZNLIObR0hz7lxsapSSIYNZJwXPGeF0mTVqGKFIXj1DQcMoT22S3ROcLyY/rz0PWaWZ9ayWmad9g==", + "dev": true, + "license": "MIT", + "dependencies": { + "deep-is": "^0.1.3", + "fast-levenshtein": "^2.0.6", + "levn": "^0.4.1", + "prelude-ls": "^1.2.1", + "type-check": "^0.4.0", + "word-wrap": "^1.2.5" + }, + "engines": { + "node": ">= 0.8.0" + } + }, + "node_modules/oxlint": { + "version": "1.57.0", + "resolved": "https://registry.npmjs.org/oxlint/-/oxlint-1.57.0.tgz", + "integrity": "sha512-DGFsuBX5MFZX9yiDdtKjTrYPq45CZ8Fft6qCltJITYZxfwYjVdGf/6wycGYTACloauwIPxUnYhBVeZbHvleGhw==", + "dev": true, + "license": "MIT", + "bin": { + "oxlint": "bin/oxlint" + }, + "engines": { + "node": "^20.19.0 || >=22.12.0" + }, + "funding": { + "url": "https://github.com/sponsors/Boshen" + }, + "optionalDependencies": { + "@oxlint/binding-android-arm-eabi": "1.57.0", + "@oxlint/binding-android-arm64": "1.57.0", + "@oxlint/binding-darwin-arm64": "1.57.0", + "@oxlint/binding-darwin-x64": "1.57.0", + "@oxlint/binding-freebsd-x64": "1.57.0", + "@oxlint/binding-linux-arm-gnueabihf": "1.57.0", + "@oxlint/binding-linux-arm-musleabihf": "1.57.0", + "@oxlint/binding-linux-arm64-gnu": "1.57.0", + "@oxlint/binding-linux-arm64-musl": "1.57.0", + "@oxlint/binding-linux-ppc64-gnu": "1.57.0", + "@oxlint/binding-linux-riscv64-gnu": "1.57.0", + "@oxlint/binding-linux-riscv64-musl": "1.57.0", + "@oxlint/binding-linux-s390x-gnu": "1.57.0", + "@oxlint/binding-linux-x64-gnu": "1.57.0", + "@oxlint/binding-linux-x64-musl": "1.57.0", + "@oxlint/binding-openharmony-arm64": "1.57.0", + "@oxlint/binding-win32-arm64-msvc": "1.57.0", + "@oxlint/binding-win32-ia32-msvc": "1.57.0", + "@oxlint/binding-win32-x64-msvc": "1.57.0" + }, + "peerDependencies": { + "oxlint-tsgolint": ">=0.15.0" + }, + "peerDependenciesMeta": { + "oxlint-tsgolint": { + "optional": true + } + } + }, + "node_modules/p-limit": { + "version": "3.1.0", + "resolved": "https://registry.npmjs.org/p-limit/-/p-limit-3.1.0.tgz", + "integrity": "sha512-TYOanM3wGwNGsZN2cVTYPArw454xnXj5qmWF1bEoAc4+cU/ol7GVh7odevjp1FNHduHc3KZMcFduxU5Xc6uJRQ==", + "dev": true, + "license": "MIT", + "dependencies": { + "yocto-queue": "^0.1.0" + }, + "engines": { + "node": ">=10" + }, + "funding": { + "url": "https://github.com/sponsors/sindresorhus" + } + }, + "node_modules/p-locate": { + "version": "5.0.0", + "resolved": "https://registry.npmjs.org/p-locate/-/p-locate-5.0.0.tgz", + "integrity": "sha512-LaNjtRWUBY++zB5nE/NwcaoMylSPk+S+ZHNB1TzdbMJMny6dynpAGt7X/tl/QYq3TIeE6nxHppbo2LGymrG5Pw==", + "dev": true, + "license": "MIT", + "dependencies": { + "p-limit": "^3.0.2" + }, + "engines": { + "node": ">=10" + }, + "funding": { + "url": "https://github.com/sponsors/sindresorhus" + } + }, + "node_modules/path-browserify": { + "version": "1.0.1", + "resolved": "https://registry.npmjs.org/path-browserify/-/path-browserify-1.0.1.tgz", + "integrity": "sha512-b7uo2UCUOYZcnF/3ID0lulOJi/bafxa1xPe7ZPsammBSpjSWQkjNxlt635YGS2MiR9GjvuXCtz2emr3jbsz98g==", + "dev": true, + "license": "MIT" + }, + "node_modules/path-exists": { + "version": "4.0.0", + "resolved": "https://registry.npmjs.org/path-exists/-/path-exists-4.0.0.tgz", + "integrity": "sha512-ak9Qy5Q7jYb2Wwcey5Fpvg2KoAc/ZIhLSLOSBmRmygPsGwkVVt0fZa0qrtMz+m6tJTAHfZQ8FnmB4MG4LWy7/w==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=8" + } + }, + "node_modules/path-key": { + "version": "3.1.1", + "resolved": "https://registry.npmjs.org/path-key/-/path-key-3.1.1.tgz", + "integrity": "sha512-ojmeN0qd+y0jszEtoY48r0Peq5dwMEkIlCOu6Q5f41lfkswXuKtYrhgoTpLnyIcHm24Uhqx+5Tqm2InSwLhE6Q==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=8" + } + }, + "node_modules/pathe": { + "version": "2.0.3", + "resolved": "https://registry.npmjs.org/pathe/-/pathe-2.0.3.tgz", + "integrity": "sha512-WUjGcAqP1gQacoQe+OBJsFA7Ld4DyXuUIjZ5cc75cLHvJ7dtNsTugphxIADwspS+AraAUePCKrSVtPLFj/F88w==", + "license": "MIT" + }, + "node_modules/perfect-debounce": { + "version": "1.0.0", + "resolved": "https://registry.npmjs.org/perfect-debounce/-/perfect-debounce-1.0.0.tgz", + "integrity": "sha512-xCy9V055GLEqoFaHoC1SoLIaLmWctgCUaBaWxDZ7/Zx4CTyX7cJQLJOok/orfjZAh9kEYpjJa4d0KcJmCbctZA==", + "license": "MIT" + }, + "node_modules/picocolors": { + "version": "1.1.1", + "resolved": "https://registry.npmjs.org/picocolors/-/picocolors-1.1.1.tgz", + "integrity": "sha512-xceH2snhtb5M9liqDsmEw56le376mTZkEX/jEb/RxNFyegNul7eNslCXP9FDj/Lcu0X8KEyMceP2ntpaHrDEVA==", + "license": "ISC" + }, + "node_modules/picomatch": { + "version": "2.3.2", + "resolved": "https://registry.npmjs.org/picomatch/-/picomatch-2.3.2.tgz", + "integrity": "sha512-V7+vQEJ06Z+c5tSye8S+nHUfI51xoXIXjHQ99cQtKUkQqqO1kO/KCJUfZXuB47h/YBlDhah2H3hdUGXn8ie0oA==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=8.6" + }, + "funding": { + "url": "https://github.com/sponsors/jonschlinkert" + } + }, + "node_modules/pidtree": { + "version": "0.6.0", + "resolved": "https://registry.npmjs.org/pidtree/-/pidtree-0.6.0.tgz", + "integrity": "sha512-eG2dWTVw5bzqGRztnHExczNxt5VGsE6OwTeCG3fdUf9KBsZzO3R5OIIIzWR+iZA0NtZ+RDVdaoE2dK1cn6jH4g==", + "dev": true, + "license": "MIT", + "bin": { + "pidtree": "bin/pidtree.js" + }, + "engines": { + "node": ">=0.10" + } + }, + "node_modules/pinia": { + "version": "3.0.4", + "resolved": "https://registry.npmjs.org/pinia/-/pinia-3.0.4.tgz", + "integrity": "sha512-l7pqLUFTI/+ESXn6k3nu30ZIzW5E2WZF/LaHJEpoq6ElcLD+wduZoB2kBN19du6K/4FDpPMazY2wJr+IndBtQw==", + "license": "MIT", + "dependencies": { + "@vue/devtools-api": "^7.7.7" + }, + "funding": { + "url": "https://github.com/sponsors/posva" + }, + "peerDependencies": { + "typescript": ">=4.5.0", + "vue": "^3.5.11" + }, + "peerDependenciesMeta": { + "typescript": { + "optional": true + } + } + }, + "node_modules/pkg-types": { + "version": "2.3.0", + "resolved": "https://registry.npmjs.org/pkg-types/-/pkg-types-2.3.0.tgz", + "integrity": "sha512-SIqCzDRg0s9npO5XQ3tNZioRY1uK06lA41ynBC1YmFTmnY6FjUjVt6s4LoADmwoig1qqD0oK8h1p/8mlMx8Oig==", + "license": "MIT", + "dependencies": { + "confbox": "^0.2.2", + "exsolve": "^1.0.7", + "pathe": "^2.0.3" + } + }, + "node_modules/postcss": { + "version": "8.5.8", + "resolved": "https://registry.npmjs.org/postcss/-/postcss-8.5.8.tgz", + "integrity": "sha512-OW/rX8O/jXnm82Ey1k44pObPtdblfiuWnrd8X7GJ7emImCOstunGbXUpp7HdBrFQX6rJzn3sPT397Wp5aCwCHg==", + "funding": [ + { + "type": "opencollective", + "url": "https://opencollective.com/postcss/" + }, + { + "type": "tidelift", + "url": "https://tidelift.com/funding/github/npm/postcss" + }, + { + "type": "github", + "url": "https://github.com/sponsors/ai" + } + ], + "license": "MIT", + "dependencies": { + "nanoid": "^3.3.11", + "picocolors": "^1.1.1", + "source-map-js": "^1.2.1" + }, + "engines": { + "node": "^10 || ^12 || >=14" + } + }, + "node_modules/postcss-selector-parser": { + "version": "7.1.1", + "resolved": "https://registry.npmjs.org/postcss-selector-parser/-/postcss-selector-parser-7.1.1.tgz", + "integrity": "sha512-orRsuYpJVw8LdAwqqLykBj9ecS5/cRHlI5+nvTo8LcCKmzDmqVORXtOIYEEQuL9D4BxtA1lm5isAqzQZCoQ6Eg==", + "dev": true, + "license": "MIT", + "dependencies": { + "cssesc": "^3.0.0", + "util-deprecate": "^1.0.2" + }, + "engines": { + "node": ">=4" + } + }, + "node_modules/prelude-ls": { + "version": "1.2.1", + "resolved": "https://registry.npmjs.org/prelude-ls/-/prelude-ls-1.2.1.tgz", + "integrity": "sha512-vkcDPrRZo1QZLbn5RLGPpg/WmIQ65qoWWhcGKf/b5eplkkarX0m9z8ppCat4mlOqUsWpyNuYgO3VRyrYHSzX5g==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">= 0.8.0" + } + }, + "node_modules/prettier": { + "version": "3.8.1", + "resolved": "https://registry.npmjs.org/prettier/-/prettier-3.8.1.tgz", + "integrity": "sha512-UOnG6LftzbdaHZcKoPFtOcCKztrQ57WkHDeRD9t/PTQtmT0NHSeWWepj6pS0z/N7+08BHFDQVUrfmfMRcZwbMg==", + "dev": true, + "license": "MIT", + "bin": { + "prettier": "bin/prettier.cjs" + }, + "engines": { + "node": ">=14" + }, + "funding": { + "url": "https://github.com/prettier/prettier?sponsor=1" + } + }, + "node_modules/punycode": { + "version": "2.3.1", + "resolved": "https://registry.npmjs.org/punycode/-/punycode-2.3.1.tgz", + "integrity": "sha512-vYt7UD1U9Wg6138shLtLOvdAu+8DsC/ilFtEVHcH+wydcSpNE20AfSOduf6MkRFahL5FY7X1oU7nKVZFtfq8Fg==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=6" + } + }, + "node_modules/quansync": { + "version": "0.2.11", + "resolved": "https://registry.npmjs.org/quansync/-/quansync-0.2.11.tgz", + "integrity": "sha512-AifT7QEbW9Nri4tAwR5M/uzpBuqfZf+zwaEM/QkzEjj7NBuFD2rBuy0K3dE+8wltbezDV7JMA0WfnCPYRSYbXA==", + "funding": [ + { + "type": "individual", + "url": "https://github.com/sponsors/antfu" + }, + { + "type": "individual", + "url": "https://github.com/sponsors/sxzz" + } + ], + "license": "MIT" + }, + "node_modules/queue-microtask": { + "version": "1.2.3", + "resolved": "https://registry.npmjs.org/queue-microtask/-/queue-microtask-1.2.3.tgz", + "integrity": "sha512-NuaNSa6flKT5JaSYQzJok04JzTL1CA6aGhv5rfLW3PgqA+M2ChpZQnAC8h8i4ZFkBS8X5RqkDBHA7r4hej3K9A==", + "dev": true, + "funding": [ + { + "type": "github", + "url": "https://github.com/sponsors/feross" + }, + { + "type": "patreon", + "url": "https://www.patreon.com/feross" + }, + { + "type": "consulting", + "url": "https://feross.org/support" + } + ], + "license": "MIT" + }, + "node_modules/read-package-json-fast": { + "version": "4.0.0", + "resolved": "https://registry.npmjs.org/read-package-json-fast/-/read-package-json-fast-4.0.0.tgz", + "integrity": "sha512-qpt8EwugBWDw2cgE2W+/3oxC+KTez2uSVR8JU9Q36TXPAGCaozfQUs59v4j4GFpWTaw0i6hAZSvOmu1J0uOEUg==", + "dev": true, + "license": "ISC", + "dependencies": { + "json-parse-even-better-errors": "^4.0.0", + "npm-normalize-package-bin": "^4.0.0" + }, + "engines": { + "node": "^18.17.0 || >=20.5.0" + } + }, + "node_modules/readdirp": { + "version": "5.0.0", + "resolved": "https://registry.npmjs.org/readdirp/-/readdirp-5.0.0.tgz", + "integrity": "sha512-9u/XQ1pvrQtYyMpZe7DXKv2p5CNvyVwzUB6uhLAnQwHMSgKMBR62lc7AHljaeteeHXn11XTAaLLUVZYVZyuRBQ==", + "license": "MIT", + "engines": { + "node": ">= 20.19.0" + }, + "funding": { + "type": "individual", + "url": "https://paulmillr.com/funding/" + } + }, + "node_modules/reusify": { + "version": "1.1.0", + "resolved": "https://registry.npmjs.org/reusify/-/reusify-1.1.0.tgz", + "integrity": "sha512-g6QUff04oZpHs0eG5p83rFLhHeV00ug/Yf9nZM6fLeUrPguBTkTQOdpAWWspMh55TZfVQDPaN3NQJfbVRAxdIw==", + "dev": true, + "license": "MIT", + "engines": { + "iojs": ">=1.0.0", + "node": ">=0.10.0" + } + }, + "node_modules/rfdc": { + "version": "1.4.1", + "resolved": "https://registry.npmjs.org/rfdc/-/rfdc-1.4.1.tgz", + "integrity": "sha512-q1b3N5QkRUWUl7iyylaaj3kOpIT0N2i9MqIEQXP73GVsN9cw3fdx8X63cEmWhJGi2PPCF23Ijp7ktmd39rawIA==", + "license": "MIT" + }, + "node_modules/rolldown": { + "version": "1.0.0-rc.12", + "resolved": "https://registry.npmjs.org/rolldown/-/rolldown-1.0.0-rc.12.tgz", + "integrity": "sha512-yP4USLIMYrwpPHEFB5JGH1uxhcslv6/hL0OyvTuY+3qlOSJvZ7ntYnoWpehBxufkgN0cvXxppuTu5hHa/zPh+A==", + "dev": true, + "license": "MIT", + "dependencies": { + "@oxc-project/types": "=0.122.0", + "@rolldown/pluginutils": "1.0.0-rc.12" + }, + "bin": { + "rolldown": "bin/cli.mjs" + }, + "engines": { + "node": "^20.19.0 || >=22.12.0" + }, + "optionalDependencies": { + "@rolldown/binding-android-arm64": "1.0.0-rc.12", + "@rolldown/binding-darwin-arm64": "1.0.0-rc.12", + "@rolldown/binding-darwin-x64": "1.0.0-rc.12", + "@rolldown/binding-freebsd-x64": "1.0.0-rc.12", + "@rolldown/binding-linux-arm-gnueabihf": "1.0.0-rc.12", + "@rolldown/binding-linux-arm64-gnu": "1.0.0-rc.12", + "@rolldown/binding-linux-arm64-musl": "1.0.0-rc.12", + "@rolldown/binding-linux-ppc64-gnu": "1.0.0-rc.12", + "@rolldown/binding-linux-s390x-gnu": "1.0.0-rc.12", + "@rolldown/binding-linux-x64-gnu": "1.0.0-rc.12", + "@rolldown/binding-linux-x64-musl": "1.0.0-rc.12", + "@rolldown/binding-openharmony-arm64": "1.0.0-rc.12", + "@rolldown/binding-wasm32-wasi": "1.0.0-rc.12", + "@rolldown/binding-win32-arm64-msvc": "1.0.0-rc.12", + "@rolldown/binding-win32-x64-msvc": "1.0.0-rc.12" + } + }, + "node_modules/rolldown/node_modules/@rolldown/pluginutils": { + "version": "1.0.0-rc.12", + "resolved": "https://registry.npmjs.org/@rolldown/pluginutils/-/pluginutils-1.0.0-rc.12.tgz", + "integrity": "sha512-HHMwmarRKvoFsJorqYlFeFRzXZqCt2ETQlEDOb9aqssrnVBB1/+xgTGtuTrIk5vzLNX1MjMtTf7W9z3tsSbrxw==", + "dev": true, + "license": "MIT" + }, + "node_modules/run-applescript": { + "version": "7.1.0", + "resolved": "https://registry.npmjs.org/run-applescript/-/run-applescript-7.1.0.tgz", + "integrity": "sha512-DPe5pVFaAsinSaV6QjQ6gdiedWDcRCbUuiQfQa2wmWV7+xC9bGulGI8+TdRmoFkAPaBXk8CrAbnlY2ISniJ47Q==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=18" + }, + "funding": { + "url": "https://github.com/sponsors/sindresorhus" + } + }, + "node_modules/run-parallel": { + "version": "1.2.0", + "resolved": "https://registry.npmjs.org/run-parallel/-/run-parallel-1.2.0.tgz", + "integrity": "sha512-5l4VyZR86LZ/lDxZTR6jqL8AFE2S0IFLMP26AbjsLVADxHdhB/c0GUsH+y39UfCi3dzz8OlQuPmnaJOMoDHQBA==", + "dev": true, + "funding": [ + { + "type": "github", + "url": "https://github.com/sponsors/feross" + }, + { + "type": "patreon", + "url": "https://www.patreon.com/feross" + }, + { + "type": "consulting", + "url": "https://feross.org/support" + } + ], + "license": "MIT", + "dependencies": { + "queue-microtask": "^1.2.2" + } + }, + "node_modules/scule": { + "version": "1.3.0", + "resolved": "https://registry.npmjs.org/scule/-/scule-1.3.0.tgz", + "integrity": "sha512-6FtHJEvt+pVMIB9IBY+IcCJ6Z5f1iQnytgyfKMhDKgmzYG+TeH/wx1y3l27rshSbLiSanrR9ffZDrEsmjlQF2g==", + "license": "MIT" + }, + "node_modules/semver": { + "version": "7.7.4", + "resolved": "https://registry.npmjs.org/semver/-/semver-7.7.4.tgz", + "integrity": "sha512-vFKC2IEtQnVhpT78h1Yp8wzwrf8CM+MzKMHGJZfBtzhZNycRFnXsHk6E5TxIkkMsgNS7mdX3AGB7x2QM2di4lA==", + "dev": true, + "license": "ISC", + "bin": { + "semver": "bin/semver.js" + }, + "engines": { + "node": ">=10" + } + }, + "node_modules/shebang-command": { + "version": "2.0.0", + "resolved": "https://registry.npmjs.org/shebang-command/-/shebang-command-2.0.0.tgz", + "integrity": "sha512-kHxr2zZpYtdmrN1qDjrrX/Z1rR1kG8Dx+gkpK1G4eXmvXswmcE1hTWBWYUzlraYw1/yZp6YuDY77YtvbN0dmDA==", + "dev": true, + "license": "MIT", + "dependencies": { + "shebang-regex": "^3.0.0" + }, + "engines": { + "node": ">=8" + } + }, + "node_modules/shebang-regex": { + "version": "3.0.0", + "resolved": "https://registry.npmjs.org/shebang-regex/-/shebang-regex-3.0.0.tgz", + "integrity": "sha512-7++dFhtcx3353uBaq8DDR4NuxBetBzC7ZQOhmTQInHEd6bSrXdiEyzCvG07Z44UYdLShWUyXt5M/yhz8ekcb1A==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=8" + } + }, + "node_modules/shell-quote": { + "version": "1.8.3", + "resolved": "https://registry.npmjs.org/shell-quote/-/shell-quote-1.8.3.tgz", + "integrity": "sha512-ObmnIF4hXNg1BqhnHmgbDETF8dLPCggZWBjkQfhZpbszZnYur5DUljTcCHii5LC3J5E0yeO/1LIMyH+UvHQgyw==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">= 0.4" + }, + "funding": { + "url": "https://github.com/sponsors/ljharb" + } + }, + "node_modules/sirv": { + "version": "3.0.2", + "resolved": "https://registry.npmjs.org/sirv/-/sirv-3.0.2.tgz", + "integrity": "sha512-2wcC/oGxHis/BoHkkPwldgiPSYcpZK3JU28WoMVv55yHJgcZ8rlXvuG9iZggz+sU1d4bRgIGASwyWqjxu3FM0g==", + "dev": true, + "license": "MIT", + "dependencies": { + "@polka/url": "^1.0.0-next.24", + "mrmime": "^2.0.0", + "totalist": "^3.0.0" + }, + "engines": { + "node": ">=18" + } + }, + "node_modules/source-map-js": { + "version": "1.2.1", + "resolved": "https://registry.npmjs.org/source-map-js/-/source-map-js-1.2.1.tgz", + "integrity": "sha512-UXWMKhLOwVKb728IUtQPXxfYU+usdybtUrK/8uGE8CQMvrhOpwvzDBwj0QhSL7MQc7vIsISBG8VQ8+IDQxpfQA==", + "license": "BSD-3-Clause", + "engines": { + "node": ">=0.10.0" + } + }, + "node_modules/speakingurl": { + "version": "14.0.1", + "resolved": "https://registry.npmjs.org/speakingurl/-/speakingurl-14.0.1.tgz", + "integrity": "sha512-1POYv7uv2gXoyGFpBCmpDVSNV74IfsWlDW216UPjbWufNf+bSU6GdbDsxdcxtfwb4xlI3yxzOTKClUosxARYrQ==", + "license": "BSD-3-Clause", + "engines": { + "node": ">=0.10.0" + } + }, + "node_modules/superjson": { + "version": "2.2.6", + "resolved": "https://registry.npmjs.org/superjson/-/superjson-2.2.6.tgz", + "integrity": "sha512-H+ue8Zo4vJmV2nRjpx86P35lzwDT3nItnIsocgumgr0hHMQ+ZGq5vrERg9kJBo5AWGmxZDhzDo+WVIJqkB0cGA==", + "license": "MIT", + "dependencies": { + "copy-anything": "^4" + }, + "engines": { + "node": ">=16" + } + }, + "node_modules/tinyglobby": { + "version": "0.2.15", + "resolved": "https://registry.npmjs.org/tinyglobby/-/tinyglobby-0.2.15.tgz", + "integrity": "sha512-j2Zq4NyQYG5XMST4cbs02Ak8iJUdxRM0XI5QyxXuZOzKOINmWurp3smXu3y5wDcJrptwpSjgXHzIQxR0omXljQ==", + "license": "MIT", + "dependencies": { + "fdir": "^6.5.0", + "picomatch": "^4.0.3" + }, + "engines": { + "node": ">=12.0.0" + }, + "funding": { + "url": "https://github.com/sponsors/SuperchupuDev" + } + }, + "node_modules/tinyglobby/node_modules/fdir": { + "version": "6.5.0", + "resolved": "https://registry.npmjs.org/fdir/-/fdir-6.5.0.tgz", + "integrity": "sha512-tIbYtZbucOs0BRGqPJkshJUYdL+SDH7dVM8gjy+ERp3WAUjLEFJE+02kanyHtwjWOnwrKYBiwAmM0p4kLJAnXg==", + "license": "MIT", + "engines": { + "node": ">=12.0.0" + }, + "peerDependencies": { + "picomatch": "^3 || ^4" + }, + "peerDependenciesMeta": { + "picomatch": { + "optional": true + } + } + }, + "node_modules/tinyglobby/node_modules/picomatch": { + "version": "4.0.4", + "resolved": "https://registry.npmjs.org/picomatch/-/picomatch-4.0.4.tgz", + "integrity": "sha512-QP88BAKvMam/3NxH6vj2o21R6MjxZUAd6nlwAS/pnGvN9IVLocLHxGYIzFhg6fUQ+5th6P4dv4eW9jX3DSIj7A==", + "license": "MIT", + "engines": { + "node": ">=12" + }, + "funding": { + "url": "https://github.com/sponsors/jonschlinkert" + } + }, + "node_modules/to-regex-range": { + "version": "5.0.1", + "resolved": "https://registry.npmjs.org/to-regex-range/-/to-regex-range-5.0.1.tgz", + "integrity": "sha512-65P7iz6X5yEr1cwcgvQxbbIw7Uk3gOy5dIdtZ4rDveLqhrdJP+Li/Hx6tyK0NEb+2GCyneCMJiGqrADCSNk8sQ==", + "dev": true, + "license": "MIT", + "dependencies": { + "is-number": "^7.0.0" + }, + "engines": { + "node": ">=8.0" + } + }, + "node_modules/totalist": { + "version": "3.0.1", + "resolved": "https://registry.npmjs.org/totalist/-/totalist-3.0.1.tgz", + "integrity": "sha512-sf4i37nQ2LBx4m3wB74y+ubopq6W/dIzXg0FDGjsYnZHVa1Da8FH853wlL2gtUhg+xJXjfk3kUZS3BRoQeoQBQ==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=6" + } + }, + "node_modules/ts-api-utils": { + "version": "2.5.0", + "resolved": "https://registry.npmjs.org/ts-api-utils/-/ts-api-utils-2.5.0.tgz", + "integrity": "sha512-OJ/ibxhPlqrMM0UiNHJ/0CKQkoKF243/AEmplt3qpRgkW8VG7IfOS41h7V8TjITqdByHzrjcS/2si+y4lIh8NA==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=18.12" + }, + "peerDependencies": { + "typescript": ">=4.8.4" + } + }, + "node_modules/tslib": { + "version": "2.8.1", + "resolved": "https://registry.npmjs.org/tslib/-/tslib-2.8.1.tgz", + "integrity": "sha512-oJFu94HQb+KVduSUQL7wnpmqnfmLsOA/nAh6b6EH0wCEoK0/mPeXU6c3wKDV83MkOuHPRHtSXKKU99IBazS/2w==", + "dev": true, + "license": "0BSD", + "optional": true + }, + "node_modules/type-check": { + "version": "0.4.0", + "resolved": "https://registry.npmjs.org/type-check/-/type-check-0.4.0.tgz", + "integrity": "sha512-XleUoc9uwGXqjWwXaUTZAmzMcFZ5858QA2vvx1Ur5xIcixXIP+8LnFDgRplU30us6teqdlskFfu+ae4K79Ooew==", + "dev": true, + "license": "MIT", + "dependencies": { + "prelude-ls": "^1.2.1" + }, + "engines": { + "node": ">= 0.8.0" + } + }, + "node_modules/typescript": { + "version": "6.0.2", + "resolved": "https://registry.npmjs.org/typescript/-/typescript-6.0.2.tgz", + "integrity": "sha512-bGdAIrZ0wiGDo5l8c++HWtbaNCWTS4UTv7RaTH/ThVIgjkveJt83m74bBHMJkuCbslY8ixgLBVZJIOiQlQTjfQ==", + "devOptional": true, + "license": "Apache-2.0", + "bin": { + "tsc": "bin/tsc", + "tsserver": "bin/tsserver" + }, + "engines": { + "node": ">=14.17" + } + }, + "node_modules/typescript-eslint": { + "version": "8.58.0", + "resolved": "https://registry.npmjs.org/typescript-eslint/-/typescript-eslint-8.58.0.tgz", + "integrity": "sha512-e2TQzKfaI85fO+F3QywtX+tCTsu/D3WW5LVU6nz8hTFKFZ8yBJ6mSYRpXqdR3mFjPWmO0eWsTa5f+UpAOe/FMA==", + "dev": true, + "license": "MIT", + "dependencies": { + "@typescript-eslint/eslint-plugin": "8.58.0", + "@typescript-eslint/parser": "8.58.0", + "@typescript-eslint/typescript-estree": "8.58.0", + "@typescript-eslint/utils": "8.58.0" + }, + "engines": { + "node": "^18.18.0 || ^20.9.0 || >=21.1.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/typescript-eslint" + }, + "peerDependencies": { + "eslint": "^8.57.0 || ^9.0.0 || ^10.0.0", + "typescript": ">=4.8.4 <6.1.0" + } + }, + "node_modules/ufo": { + "version": "1.6.3", + "resolved": "https://registry.npmjs.org/ufo/-/ufo-1.6.3.tgz", + "integrity": "sha512-yDJTmhydvl5lJzBmy/hyOAA0d+aqCBuwl818haVdYCRrWV84o7YyeVm4QlVHStqNrrJSTb6jKuFAVqAFsr+K3Q==", + "license": "MIT" + }, + "node_modules/undici-types": { + "version": "7.16.0", + "resolved": "https://registry.npmjs.org/undici-types/-/undici-types-7.16.0.tgz", + "integrity": "sha512-Zz+aZWSj8LE6zoxD+xrjh4VfkIG8Ya6LvYkZqtUQGJPZjYl53ypCaUwWqo7eI0x66KBGeRo+mlBEkMSeSZ38Nw==", + "dev": true, + "license": "MIT" + }, + "node_modules/unplugin": { + "version": "3.0.0", + "resolved": "https://registry.npmjs.org/unplugin/-/unplugin-3.0.0.tgz", + "integrity": "sha512-0Mqk3AT2TZCXWKdcoaufeXNukv2mTrEZExeXlHIOZXdqYoHHr4n51pymnwV8x2BOVxwXbK2HLlI7usrqMpycdg==", + "license": "MIT", + "dependencies": { + "@jridgewell/remapping": "^2.3.5", + "picomatch": "^4.0.3", + "webpack-virtual-modules": "^0.6.2" + }, + "engines": { + "node": "^20.19.0 || >=22.12.0" + } + }, + "node_modules/unplugin-utils": { + "version": "0.3.1", + "resolved": "https://registry.npmjs.org/unplugin-utils/-/unplugin-utils-0.3.1.tgz", + "integrity": "sha512-5lWVjgi6vuHhJ526bI4nlCOmkCIF3nnfXkCMDeMJrtdvxTs6ZFCM8oNufGTsDbKv/tJ/xj8RpvXjRuPBZJuJog==", + "license": "MIT", + "dependencies": { + "pathe": "^2.0.3", + "picomatch": "^4.0.3" + }, + "engines": { + "node": ">=20.19.0" + }, + "funding": { + "url": "https://github.com/sponsors/sxzz" + } + }, + "node_modules/unplugin-utils/node_modules/picomatch": { + "version": "4.0.4", + "resolved": "https://registry.npmjs.org/picomatch/-/picomatch-4.0.4.tgz", + "integrity": "sha512-QP88BAKvMam/3NxH6vj2o21R6MjxZUAd6nlwAS/pnGvN9IVLocLHxGYIzFhg6fUQ+5th6P4dv4eW9jX3DSIj7A==", + "license": "MIT", + "engines": { + "node": ">=12" + }, + "funding": { + "url": "https://github.com/sponsors/jonschlinkert" + } + }, + "node_modules/unplugin/node_modules/picomatch": { + "version": "4.0.4", + "resolved": "https://registry.npmjs.org/picomatch/-/picomatch-4.0.4.tgz", + "integrity": "sha512-QP88BAKvMam/3NxH6vj2o21R6MjxZUAd6nlwAS/pnGvN9IVLocLHxGYIzFhg6fUQ+5th6P4dv4eW9jX3DSIj7A==", + "license": "MIT", + "engines": { + "node": ">=12" + }, + "funding": { + "url": "https://github.com/sponsors/jonschlinkert" + } + }, + "node_modules/update-browserslist-db": { + "version": "1.2.3", + "resolved": "https://registry.npmjs.org/update-browserslist-db/-/update-browserslist-db-1.2.3.tgz", + "integrity": "sha512-Js0m9cx+qOgDxo0eMiFGEueWztz+d4+M3rGlmKPT+T4IS/jP4ylw3Nwpu6cpTTP8R1MAC1kF4VbdLt3ARf209w==", + "dev": true, + "funding": [ + { + "type": "opencollective", + "url": "https://opencollective.com/browserslist" + }, + { + "type": "tidelift", + "url": "https://tidelift.com/funding/github/npm/browserslist" + }, + { + "type": "github", + "url": "https://github.com/sponsors/ai" + } + ], + "license": "MIT", + "dependencies": { + "escalade": "^3.2.0", + "picocolors": "^1.1.1" + }, + "bin": { + "update-browserslist-db": "cli.js" + }, + "peerDependencies": { + "browserslist": ">= 4.21.0" + } + }, + "node_modules/uri-js": { + "version": "4.4.1", + "resolved": "https://registry.npmjs.org/uri-js/-/uri-js-4.4.1.tgz", + "integrity": "sha512-7rKUyy33Q1yc98pQ1DAmLtwX109F7TIfWlW1Ydo8Wl1ii1SeHieeh0HHfPeL2fMXK6z0s8ecKs9frCuLJvndBg==", + "dev": true, + "license": "BSD-2-Clause", + "dependencies": { + "punycode": "^2.1.0" + } + }, + "node_modules/util-deprecate": { + "version": "1.0.2", + "resolved": "https://registry.npmjs.org/util-deprecate/-/util-deprecate-1.0.2.tgz", + "integrity": "sha512-EPD5q1uXyFxJpCrLnCc1nHnq3gOa6DZBocAIiI2TaSCA7VCJ1UJDMagCzIkXNsUYfD1daK//LTEQ8xiIbrHtcw==", + "dev": true, + "license": "MIT" + }, + "node_modules/vite": { + "version": "8.0.3", + "resolved": "https://registry.npmjs.org/vite/-/vite-8.0.3.tgz", + "integrity": "sha512-B9ifbFudT1TFhfltfaIPgjo9Z3mDynBTJSUYxTjOQruf/zHH+ezCQKcoqO+h7a9Pw9Nm/OtlXAiGT1axBgwqrQ==", + "dev": true, + "license": "MIT", + "dependencies": { + "lightningcss": "^1.32.0", + "picomatch": "^4.0.4", + "postcss": "^8.5.8", + "rolldown": "1.0.0-rc.12", + "tinyglobby": "^0.2.15" + }, + "bin": { + "vite": "bin/vite.js" + }, + "engines": { + "node": "^20.19.0 || >=22.12.0" + }, + "funding": { + "url": "https://github.com/vitejs/vite?sponsor=1" + }, + "optionalDependencies": { + "fsevents": "~2.3.3" + }, + "peerDependencies": { + "@types/node": "^20.19.0 || >=22.12.0", + "@vitejs/devtools": "^0.1.0", + "esbuild": "^0.27.0", + "jiti": ">=1.21.0", + "less": "^4.0.0", + "sass": "^1.70.0", + "sass-embedded": "^1.70.0", + "stylus": ">=0.54.8", + "sugarss": "^5.0.0", + "terser": "^5.16.0", + "tsx": "^4.8.1", + "yaml": "^2.4.2" + }, + "peerDependenciesMeta": { + "@types/node": { + "optional": true + }, + "@vitejs/devtools": { + "optional": true + }, + "esbuild": { + "optional": true + }, + "jiti": { + "optional": true + }, + "less": { + "optional": true + }, + "sass": { + "optional": true + }, + "sass-embedded": { + "optional": true + }, + "stylus": { + "optional": true + }, + "sugarss": { + "optional": true + }, + "terser": { + "optional": true + }, + "tsx": { + "optional": true + }, + "yaml": { + "optional": true + } + } + }, + "node_modules/vite-plugin-vue-devtools": { + "version": "8.1.1", + "resolved": "https://registry.npmjs.org/vite-plugin-vue-devtools/-/vite-plugin-vue-devtools-8.1.1.tgz", + "integrity": "sha512-9qTpOmZ2vHpvlI9hdVXAQ1Ry4I8GcBArU7aPi0qfIaV7fQIXy0L1nb6X4mFY2Gw0dYshHuLbIl0Ulb572SCjsQ==", + "dev": true, + "license": "MIT", + "dependencies": { + "@vue/devtools-core": "^8.1.1", + "@vue/devtools-kit": "^8.1.1", + "@vue/devtools-shared": "^8.1.1", + "sirv": "^3.0.2", + "vite-plugin-inspect": "^11.3.3", + "vite-plugin-vue-inspector": "^5.3.2" + }, + "engines": { + "node": ">=v14.21.3" + }, + "peerDependencies": { + "vite": "^6.0.0 || ^7.0.0 || ^8.0.0" + } + }, + "node_modules/vite-plugin-vue-devtools/node_modules/@vue/devtools-kit": { + "version": "8.1.1", + "resolved": "https://registry.npmjs.org/@vue/devtools-kit/-/devtools-kit-8.1.1.tgz", + "integrity": "sha512-gVBaBv++i+adg4JpH71k9ppl4soyR7Y2McEqO5YNgv0BI1kMZ7BDX5gnwkZ5COYgiCyhejZG+yGNrBAjj6Coqg==", + "dev": true, + "license": "MIT", + "dependencies": { + "@vue/devtools-shared": "^8.1.1", + "birpc": "^2.6.1", + "hookable": "^5.5.3", + "perfect-debounce": "^2.0.0" + } + }, + "node_modules/vite-plugin-vue-devtools/node_modules/@vue/devtools-shared": { + "version": "8.1.1", + "resolved": "https://registry.npmjs.org/@vue/devtools-shared/-/devtools-shared-8.1.1.tgz", + "integrity": "sha512-+h4ttmJYl/txpxHKaoZcaKpC+pvckgLzIDiSQlaQ7kKthKh8KuwoLW2D8hPJEnqKzXOvu15UHEoGyngAXCz0EQ==", + "dev": true, + "license": "MIT" + }, + "node_modules/vite-plugin-vue-devtools/node_modules/perfect-debounce": { + "version": "2.1.0", + "resolved": "https://registry.npmjs.org/perfect-debounce/-/perfect-debounce-2.1.0.tgz", + "integrity": "sha512-LjgdTytVFXeUgtHZr9WYViYSM/g8MkcTPYDlPa3cDqMirHjKiSZPYd6DoL7pK8AJQr+uWkQvCjHNdiMqsrJs+g==", + "dev": true, + "license": "MIT" + }, + "node_modules/vite-plugin-vue-devtools/node_modules/vite-plugin-inspect": { + "version": "11.3.3", + "resolved": "https://registry.npmjs.org/vite-plugin-inspect/-/vite-plugin-inspect-11.3.3.tgz", + "integrity": "sha512-u2eV5La99oHoYPHE6UvbwgEqKKOQGz86wMg40CCosP6q8BkB6e5xPneZfYagK4ojPJSj5anHCrnvC20DpwVdRA==", + "dev": true, + "license": "MIT", + "dependencies": { + "ansis": "^4.1.0", + "debug": "^4.4.1", + "error-stack-parser-es": "^1.0.5", + "ohash": "^2.0.11", + "open": "^10.2.0", + "perfect-debounce": "^2.0.0", + "sirv": "^3.0.1", + "unplugin-utils": "^0.3.0", + "vite-dev-rpc": "^1.1.0" + }, + "engines": { + "node": ">=14" + }, + "funding": { + "url": "https://github.com/sponsors/antfu" + }, + "peerDependencies": { + "vite": "^6.0.0 || ^7.0.0-0" + }, + "peerDependenciesMeta": { + "@nuxt/kit": { + "optional": true + } + } + }, + "node_modules/vite-plugin-vue-devtools/node_modules/vite-plugin-inspect/node_modules/vite-dev-rpc": { + "version": "1.1.0", + "resolved": "https://registry.npmjs.org/vite-dev-rpc/-/vite-dev-rpc-1.1.0.tgz", + "integrity": "sha512-pKXZlgoXGoE8sEKiKJSng4hI1sQ4wi5YT24FCrwrLt6opmkjlqPPVmiPWWJn8M8byMxRGzp1CrFuqQs4M/Z39A==", + "dev": true, + "license": "MIT", + "dependencies": { + "birpc": "^2.4.0", + "vite-hot-client": "^2.1.0" + }, + "funding": { + "url": "https://github.com/sponsors/antfu" + }, + "peerDependencies": { + "vite": "^2.9.0 || ^3.0.0-0 || ^4.0.0-0 || ^5.0.0-0 || ^6.0.1 || ^7.0.0-0" + } + }, + "node_modules/vite-plugin-vue-devtools/node_modules/vite-plugin-inspect/node_modules/vite-dev-rpc/node_modules/vite-hot-client": { + "version": "2.1.0", + "resolved": "https://registry.npmjs.org/vite-hot-client/-/vite-hot-client-2.1.0.tgz", + "integrity": "sha512-7SpgZmU7R+dDnSmvXE1mfDtnHLHQSisdySVR7lO8ceAXvM0otZeuQQ6C8LrS5d/aYyP/QZ0hI0L+dIPrm4YlFQ==", + "dev": true, + "license": "MIT", + "funding": { + "url": "https://github.com/sponsors/antfu" + }, + "peerDependencies": { + "vite": "^2.6.0 || ^3.0.0 || ^4.0.0 || ^5.0.0-0 || ^6.0.0-0 || ^7.0.0-0" + } + }, + "node_modules/vite-plugin-vue-inspector": { + "version": "5.4.0", + "resolved": "https://registry.npmjs.org/vite-plugin-vue-inspector/-/vite-plugin-vue-inspector-5.4.0.tgz", + "integrity": "sha512-Iq/024CydcE46FZqWPU4t4lw4uYOdLnFSO1RNxJVt2qY9zxIjmnkBqhHnYaReWM82kmNnaXs7OkfgRrV2GEjyw==", + "dev": true, + "license": "MIT", + "dependencies": { + "@babel/core": "^7.23.0", + "@babel/plugin-proposal-decorators": "^7.23.0", + "@babel/plugin-syntax-import-attributes": "^7.22.5", + "@babel/plugin-syntax-import-meta": "^7.10.4", + "@babel/plugin-transform-typescript": "^7.22.15", + "@vue/babel-plugin-jsx": "^1.1.5", + "@vue/compiler-dom": "^3.3.4", + "kolorist": "^1.8.0", + "magic-string": "^0.30.4" + }, + "peerDependencies": { + "vite": "^3.0.0 || ^4.0.0 || ^5.0.0 || ^6.0.0 || ^7.0.0 || ^8.0.0" + } + }, + "node_modules/vite/node_modules/picomatch": { + "version": "4.0.4", + "resolved": "https://registry.npmjs.org/picomatch/-/picomatch-4.0.4.tgz", + "integrity": "sha512-QP88BAKvMam/3NxH6vj2o21R6MjxZUAd6nlwAS/pnGvN9IVLocLHxGYIzFhg6fUQ+5th6P4dv4eW9jX3DSIj7A==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=12" + }, + "funding": { + "url": "https://github.com/sponsors/jonschlinkert" + } + }, + "node_modules/vscode-uri": { + "version": "3.1.0", + "resolved": "https://registry.npmjs.org/vscode-uri/-/vscode-uri-3.1.0.tgz", + "integrity": "sha512-/BpdSx+yCQGnCvecbyXdxHDkuk55/G3xwnC0GqY4gmQ3j+A+g8kzzgB4Nk/SINjqn6+waqw3EgbVF2QKExkRxQ==", + "dev": true, + "license": "MIT" + }, + "node_modules/vue": { + "version": "3.5.31", + "resolved": "https://registry.npmjs.org/vue/-/vue-3.5.31.tgz", + "integrity": "sha512-iV/sU9SzOlmA/0tygSmjkEN6Jbs3nPoIPFhCMLD2STrjgOU8DX7ZtzMhg4ahVwf5Rp9KoFzcXeB1ZrVbLBp5/Q==", + "license": "MIT", + "dependencies": { + "@vue/compiler-dom": "3.5.31", + "@vue/compiler-sfc": "3.5.31", + "@vue/runtime-dom": "3.5.31", + "@vue/server-renderer": "3.5.31", + "@vue/shared": "3.5.31" + }, + "peerDependencies": { + "typescript": "*" + }, + "peerDependenciesMeta": { + "typescript": { + "optional": true + } + } + }, + "node_modules/vue-eslint-parser": { + "version": "10.4.0", + "resolved": "https://registry.npmjs.org/vue-eslint-parser/-/vue-eslint-parser-10.4.0.tgz", + "integrity": "sha512-Vxi9pJdbN3ZnVGLODVtZ7y4Y2kzAAE2Cm0CZ3ZDRvydVYxZ6VrnBhLikBsRS+dpwj4Jv4UCv21PTEwF5rQ9WXg==", + "dev": true, + "license": "MIT", + "dependencies": { + "debug": "^4.4.0", + "eslint-scope": "^8.2.0 || ^9.0.0", + "eslint-visitor-keys": "^4.2.0 || ^5.0.0", + "espree": "^10.3.0 || ^11.0.0", + "esquery": "^1.6.0", + "semver": "^7.6.3" + }, + "engines": { + "node": "^18.18.0 || ^20.9.0 || >=21.1.0" + }, + "funding": { + "url": "https://github.com/sponsors/mysticatea" + }, + "peerDependencies": { + "eslint": "^8.57.0 || ^9.0.0 || ^10.0.0" + } + }, + "node_modules/vue-eslint-parser/node_modules/eslint-visitor-keys": { + "version": "5.0.1", + "resolved": "https://registry.npmjs.org/eslint-visitor-keys/-/eslint-visitor-keys-5.0.1.tgz", + "integrity": "sha512-tD40eHxA35h0PEIZNeIjkHoDR4YjjJp34biM0mDvplBe//mB+IHCqHDGV7pxF+7MklTvighcCPPZC7ynWyjdTA==", + "dev": true, + "license": "Apache-2.0", + "engines": { + "node": "^20.19.0 || ^22.13.0 || >=24" + }, + "funding": { + "url": "https://opencollective.com/eslint" + } + }, + "node_modules/vue-router": { + "version": "5.0.4", + "resolved": "https://registry.npmjs.org/vue-router/-/vue-router-5.0.4.tgz", + "integrity": "sha512-lCqDLCI2+fKVRl2OzXuzdSWmxXFLQRxQbmHugnRpTMyYiT+hNaycV0faqG5FBHDXoYrZ6MQcX87BvbY8mQ20Bg==", + "license": "MIT", + "dependencies": { + "@babel/generator": "^7.28.6", + "@vue-macros/common": "^3.1.1", + "@vue/devtools-api": "^8.0.6", + "ast-walker-scope": "^0.8.3", + "chokidar": "^5.0.0", + "json5": "^2.2.3", + "local-pkg": "^1.1.2", + "magic-string": "^0.30.21", + "mlly": "^1.8.0", + "muggle-string": "^0.4.1", + "pathe": "^2.0.3", + "picomatch": "^4.0.3", + "scule": "^1.3.0", + "tinyglobby": "^0.2.15", + "unplugin": "^3.0.0", + "unplugin-utils": "^0.3.1", + "yaml": "^2.8.2" + }, + "funding": { + "url": "https://github.com/sponsors/posva" + }, + "peerDependencies": { + "@pinia/colada": ">=0.21.2", + "@vue/compiler-sfc": "^3.5.17", + "pinia": "^3.0.4", + "vue": "^3.5.0" + }, + "peerDependenciesMeta": { + "@pinia/colada": { + "optional": true + }, + "@vue/compiler-sfc": { + "optional": true + }, + "pinia": { + "optional": true + } + } + }, + "node_modules/vue-router/node_modules/@vue/devtools-api": { + "version": "8.1.1", + "resolved": "https://registry.npmjs.org/@vue/devtools-api/-/devtools-api-8.1.1.tgz", + "integrity": "sha512-bsDMJ07b3GN1puVwJb/fyFnj/U2imyswK5UQVLZwVl7O05jDrt6BHxeG5XffmOOdasOj/bOmIjxJvGPxU7pcqw==", + "license": "MIT", + "dependencies": { + "@vue/devtools-kit": "^8.1.1" + } + }, + "node_modules/vue-router/node_modules/@vue/devtools-kit": { + "version": "8.1.1", + "resolved": "https://registry.npmjs.org/@vue/devtools-kit/-/devtools-kit-8.1.1.tgz", + "integrity": "sha512-gVBaBv++i+adg4JpH71k9ppl4soyR7Y2McEqO5YNgv0BI1kMZ7BDX5gnwkZ5COYgiCyhejZG+yGNrBAjj6Coqg==", + "license": "MIT", + "dependencies": { + "@vue/devtools-shared": "^8.1.1", + "birpc": "^2.6.1", + "hookable": "^5.5.3", + "perfect-debounce": "^2.0.0" + } + }, + "node_modules/vue-router/node_modules/@vue/devtools-shared": { + "version": "8.1.1", + "resolved": "https://registry.npmjs.org/@vue/devtools-shared/-/devtools-shared-8.1.1.tgz", + "integrity": "sha512-+h4ttmJYl/txpxHKaoZcaKpC+pvckgLzIDiSQlaQ7kKthKh8KuwoLW2D8hPJEnqKzXOvu15UHEoGyngAXCz0EQ==", + "license": "MIT" + }, + "node_modules/vue-router/node_modules/perfect-debounce": { + "version": "2.1.0", + "resolved": "https://registry.npmjs.org/perfect-debounce/-/perfect-debounce-2.1.0.tgz", + "integrity": "sha512-LjgdTytVFXeUgtHZr9WYViYSM/g8MkcTPYDlPa3cDqMirHjKiSZPYd6DoL7pK8AJQr+uWkQvCjHNdiMqsrJs+g==", + "license": "MIT" + }, + "node_modules/vue-router/node_modules/picomatch": { + "version": "4.0.4", + "resolved": "https://registry.npmjs.org/picomatch/-/picomatch-4.0.4.tgz", + "integrity": "sha512-QP88BAKvMam/3NxH6vj2o21R6MjxZUAd6nlwAS/pnGvN9IVLocLHxGYIzFhg6fUQ+5th6P4dv4eW9jX3DSIj7A==", + "license": "MIT", + "engines": { + "node": ">=12" + }, + "funding": { + "url": "https://github.com/sponsors/jonschlinkert" + } + }, + "node_modules/vue-tsc": { + "version": "3.2.6", + "resolved": "https://registry.npmjs.org/vue-tsc/-/vue-tsc-3.2.6.tgz", + "integrity": "sha512-gYW/kWI0XrwGzd0PKc7tVB/qpdeAkIZLNZb10/InizkQjHjnT8weZ/vBarZoj4kHKbUTZT/bAVgoOr8x4NsQ/Q==", + "dev": true, + "license": "MIT", + "dependencies": { + "@volar/typescript": "2.4.28", + "@vue/language-core": "3.2.6" + }, + "bin": { + "vue-tsc": "bin/vue-tsc.js" + }, + "peerDependencies": { + "typescript": ">=5.0.0" + } + }, + "node_modules/webpack-virtual-modules": { + "version": "0.6.2", + "resolved": "https://registry.npmjs.org/webpack-virtual-modules/-/webpack-virtual-modules-0.6.2.tgz", + "integrity": "sha512-66/V2i5hQanC51vBQKPH4aI8NMAcBW59FVBs+rC7eGHupMyfn34q7rZIE+ETlJ+XTevqfUhVVBgSUNSW2flEUQ==", + "license": "MIT" + }, + "node_modules/which": { + "version": "2.0.2", + "resolved": "https://registry.npmjs.org/which/-/which-2.0.2.tgz", + "integrity": "sha512-BLI3Tl1TW3Pvl70l3yq3Y64i+awpwXqsGBYWkkqMtnbXgrMD+yj7rhW0kuEDxzJaYXGjEW5ogapKNMEKNMjibA==", + "dev": true, + "license": "ISC", + "dependencies": { + "isexe": "^2.0.0" + }, + "bin": { + "node-which": "bin/node-which" + }, + "engines": { + "node": ">= 8" + } + }, + "node_modules/word-wrap": { + "version": "1.2.5", + "resolved": "https://registry.npmjs.org/word-wrap/-/word-wrap-1.2.5.tgz", + "integrity": "sha512-BN22B5eaMMI9UMtjrGd5g5eCYPpCPDUy0FJXbYsaT5zYxjFOckS53SQDE3pWkVoWpHXVb3BrYcEN4Twa55B5cA==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=0.10.0" + } + }, + "node_modules/wsl-utils": { + "version": "0.1.0", + "resolved": "https://registry.npmjs.org/wsl-utils/-/wsl-utils-0.1.0.tgz", + "integrity": "sha512-h3Fbisa2nKGPxCpm89Hk33lBLsnaGBvctQopaBSOW/uIs6FTe1ATyAnKFJrzVs9vpGdsTe73WF3V4lIsk4Gacw==", + "dev": true, + "license": "MIT", + "dependencies": { + "is-wsl": "^3.1.0" + }, + "engines": { + "node": ">=18" + }, + "funding": { + "url": "https://github.com/sponsors/sindresorhus" + } + }, + "node_modules/xml-name-validator": { + "version": "4.0.0", + "resolved": "https://registry.npmjs.org/xml-name-validator/-/xml-name-validator-4.0.0.tgz", + "integrity": "sha512-ICP2e+jsHvAj2E2lIHxa5tjXRlKDJo4IdvPvCXbXQGdzSfmSpNVyIKMvoZHjDY9DP0zV17iI85o90vRFXNccRw==", + "dev": true, + "license": "Apache-2.0", + "engines": { + "node": ">=12" + } + }, + "node_modules/yallist": { + "version": "3.1.1", + "resolved": "https://registry.npmjs.org/yallist/-/yallist-3.1.1.tgz", + "integrity": "sha512-a4UGQaWPH59mOXUYnAG2ewncQS4i4F43Tv3JoAM+s2VDAmS9NsK8GpDMLrCHPksFT7h3K6TOoUNn2pb7RoXx4g==", + "dev": true, + "license": "ISC" + }, + "node_modules/yaml": { + "version": "2.8.3", + "resolved": "https://registry.npmjs.org/yaml/-/yaml-2.8.3.tgz", + "integrity": "sha512-AvbaCLOO2Otw/lW5bmh9d/WEdcDFdQp2Z2ZUH3pX9U2ihyUY0nvLv7J6TrWowklRGPYbB/IuIMfYgxaCPg5Bpg==", + "license": "ISC", + "bin": { + "yaml": "bin.mjs" + }, + "engines": { + "node": ">= 14.6" + }, + "funding": { + "url": "https://github.com/sponsors/eemeli" + } + }, + "node_modules/yocto-queue": { + "version": "0.1.0", + "resolved": "https://registry.npmjs.org/yocto-queue/-/yocto-queue-0.1.0.tgz", + "integrity": "sha512-rVksvsnNCdJ/ohGc6xgPwyN8eheCxsiLM8mxuE/t/mOVqJewPuO1miLpTHQiRgTKCLexL4MeAFVagts7HmNZ2Q==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=10" + }, + "funding": { + "url": "https://github.com/sponsors/sindresorhus" + } + } + } +} diff --git a/host/robot-command-center/frontend/package.json b/host/robot-command-center/frontend/package.json new file mode 100644 index 0000000..6078c25 --- /dev/null +++ b/host/robot-command-center/frontend/package.json @@ -0,0 +1,44 @@ +{ + "name": "frontend", + "version": "0.0.0", + "private": true, + "type": "module", + "scripts": { + "dev": "vite", + "build": "run-p type-check \"build-only {@}\" --", + "preview": "vite preview", + "build-only": "vite build", + "type-check": "vue-tsc --build", + "lint": "run-s lint:*", + "lint:oxlint": "oxlint . --fix", + "lint:eslint": "eslint . --fix --cache", + "format": "prettier --write --experimental-cli src/" + }, + "dependencies": { + "pinia": "^3.0.4", + "vue": "^3.5.31", + "vue-router": "^5.0.4" + }, + "devDependencies": { + "@tsconfig/node24": "^24.0.4", + "@types/node": "^24.12.0", + "@vitejs/plugin-vue": "^6.0.5", + "@vue/eslint-config-typescript": "^14.7.0", + "@vue/tsconfig": "^0.9.1", + "eslint": "^10.1.0", + "eslint-config-prettier": "^10.1.8", + "eslint-plugin-oxlint": "~1.57.0", + "eslint-plugin-vue": "~10.8.0", + "jiti": "^2.6.1", + "npm-run-all2": "^8.0.4", + "oxlint": "~1.57.0", + "prettier": "3.8.1", + "typescript": "~6.0.0", + "vite": "^8.0.3", + "vite-plugin-vue-devtools": "^8.1.1", + "vue-tsc": "^3.2.6" + }, + "engines": { + "node": "^20.19.0 || >=22.12.0" + } +} diff --git a/host/robot-command-center/frontend/public/favicon.ico b/host/robot-command-center/frontend/public/favicon.ico new file mode 100644 index 0000000..df36fcf Binary files /dev/null and b/host/robot-command-center/frontend/public/favicon.ico differ diff --git a/host/robot-command-center/frontend/src/App.vue b/host/robot-command-center/frontend/src/App.vue new file mode 100644 index 0000000..53f54f1 --- /dev/null +++ b/host/robot-command-center/frontend/src/App.vue @@ -0,0 +1,218 @@ + + + + + diff --git a/host/robot-command-center/frontend/src/components/ControlFeedback.vue b/host/robot-command-center/frontend/src/components/ControlFeedback.vue new file mode 100644 index 0000000..326a11f --- /dev/null +++ b/host/robot-command-center/frontend/src/components/ControlFeedback.vue @@ -0,0 +1,544 @@ + + + + + diff --git a/host/robot-command-center/frontend/src/components/ControlPanel.vue b/host/robot-command-center/frontend/src/components/ControlPanel.vue new file mode 100644 index 0000000..318455e --- /dev/null +++ b/host/robot-command-center/frontend/src/components/ControlPanel.vue @@ -0,0 +1,323 @@ + + + + + diff --git a/host/robot-command-center/frontend/src/components/GpsMapPanel.vue b/host/robot-command-center/frontend/src/components/GpsMapPanel.vue new file mode 100644 index 0000000..76252e1 --- /dev/null +++ b/host/robot-command-center/frontend/src/components/GpsMapPanel.vue @@ -0,0 +1,483 @@ + + + + + diff --git a/host/robot-command-center/frontend/src/components/NetworkPanel.vue b/host/robot-command-center/frontend/src/components/NetworkPanel.vue new file mode 100644 index 0000000..3d83a09 --- /dev/null +++ b/host/robot-command-center/frontend/src/components/NetworkPanel.vue @@ -0,0 +1,435 @@ + + + + + diff --git a/host/robot-command-center/frontend/src/components/VideoPanel.vue b/host/robot-command-center/frontend/src/components/VideoPanel.vue new file mode 100644 index 0000000..b831172 --- /dev/null +++ b/host/robot-command-center/frontend/src/components/VideoPanel.vue @@ -0,0 +1,808 @@ + + + + + diff --git a/host/robot-command-center/frontend/src/composables/useControlInterface.ts b/host/robot-command-center/frontend/src/composables/useControlInterface.ts new file mode 100644 index 0000000..40a1747 --- /dev/null +++ b/host/robot-command-center/frontend/src/composables/useControlInterface.ts @@ -0,0 +1,748 @@ +import { computed, onMounted, onUnmounted, ref } from 'vue' + +import { buildControlWebSocketUrl } from '@/lib/api' +import { t } from '@/lib/locale' + +type SocketState = 'connecting' | 'open' | 'closed' +type ControlInputMode = 'keyboard' | 'gamepad' +type ControlSource = 'keyboard' | 'gamepad' | 'idle' +type CommandTuple = [number, number, number, number, number, number] + +type KeyFeedback = { + code: string + label: string + pressed: boolean +} + +type ButtonFeedback = { + label: string + pressed: boolean +} + +type ControlTuning = { + forward: number + strafe: number + turn: number + turbo: number +} + +const TRACKED_KEYS = ['KeyW', 'KeyS', 'KeyA', 'KeyD', 'KeyQ', 'KeyE', 'ShiftLeft', 'ShiftRight', 'Space'] +const KEY_LABELS: Record = { + KeyW: 'W', + KeyS: 'S', + KeyA: 'A', + KeyD: 'D', + KeyQ: 'Q', + KeyE: 'E', + ShiftLeft: 'Shift', + ShiftRight: 'Shift', + Space: 'Space', +} +const GAMEPAD_BUTTON_LABELS = ['A', 'B', 'X', 'Y', 'LB', 'RB', 'LT', 'RT', 'Back', 'Start', 'LS', 'RS'] + +const ZERO_COMMAND: CommandTuple = [0, 0, 0, 0, 0, 0] +const GAMEPAD_DEADZONE = 0.14 +const COMMAND_SEND_INTERVAL_MS = 50 +const DEFAULT_CONTROL_TUNING: ControlTuning = { + forward: 0.8, + strafe: 0.15, + turn: 0.4, + turbo: 1.5, +} +const CONTROL_INPUT_MODE_STORAGE_KEY = 'robot-command-center.control-input-mode' +const CONTROL_TUNING_STORAGE_KEY = 'robot-command-center.control-tuning' +const MIN_AXIS_SPEED = 0.05 +const MAX_AXIS_SPEED = 3 +const MIN_TURBO_MULTIPLIER = 1 +const MAX_TURBO_MULTIPLIER = 3 + +const pressedKeys = ref>(new Set()) +const socketState = ref('connecting') +const lastServerMessageOverride = ref('') +const lastServerMessagePreset = ref<'waiting' | 'live'>('waiting') +const gamepadSupported = ref(false) +const gamepadConnected = ref(false) +const gamepadNameRaw = ref('') +const gamepadIndex = ref(null) +const gamepadMapping = ref('') +const gamepadAxes = ref([0, 0, 0, 0]) +const gamepadButtonPressed = ref(Array.from({ length: GAMEPAD_BUTTON_LABELS.length }, () => false)) +const activeSource = ref('idle') +const operatorInputSequence = ref(0) +const lastOperatorInputPerfMs = ref(0) + +function clampValue(value: number, min: number, max: number) { + return Math.min(max, Math.max(min, value)) +} + +function sanitizeAxisSpeed(value: unknown, fallback: number) { + const numericValue = typeof value === 'number' ? value : Number(value) + if (!Number.isFinite(numericValue)) { + return fallback + } + return roundValue(clampValue(numericValue, MIN_AXIS_SPEED, MAX_AXIS_SPEED)) +} + +function sanitizeTurboMultiplier(value: unknown, fallback: number) { + const numericValue = typeof value === 'number' ? value : Number(value) + if (!Number.isFinite(numericValue)) { + return fallback + } + return roundValue(clampValue(numericValue, MIN_TURBO_MULTIPLIER, MAX_TURBO_MULTIPLIER)) +} + +function normalizeControlTuning(raw?: Partial): ControlTuning { + return { + forward: sanitizeAxisSpeed(raw?.forward, DEFAULT_CONTROL_TUNING.forward), + strafe: sanitizeAxisSpeed(raw?.strafe, DEFAULT_CONTROL_TUNING.strafe), + turn: sanitizeAxisSpeed(raw?.turn, DEFAULT_CONTROL_TUNING.turn), + turbo: sanitizeTurboMultiplier(raw?.turbo, DEFAULT_CONTROL_TUNING.turbo), + } +} + +function normalizeControlInputMode(raw: unknown): ControlInputMode { + return raw === 'gamepad' ? 'gamepad' : 'keyboard' +} + +function loadPersistedControlTuning() { + if (typeof window === 'undefined') { + return DEFAULT_CONTROL_TUNING + } + + let raw: string | null = null + + try { + raw = window.localStorage.getItem(CONTROL_TUNING_STORAGE_KEY) + } catch { + return DEFAULT_CONTROL_TUNING + } + + if (raw == null) { + return DEFAULT_CONTROL_TUNING + } + + try { + return normalizeControlTuning(JSON.parse(raw) as Partial) + } catch { + return DEFAULT_CONTROL_TUNING + } +} + +function loadPersistedControlInputMode() { + if (typeof window === 'undefined') { + return normalizeControlInputMode(null) + } + + try { + return normalizeControlInputMode(window.localStorage.getItem(CONTROL_INPUT_MODE_STORAGE_KEY)) + } catch { + return normalizeControlInputMode(null) + } +} + +const controlInputMode = ref(loadPersistedControlInputMode()) +const initialControlTuning = loadPersistedControlTuning() +const forwardSpeed = ref(initialControlTuning.forward) +const strafeSpeed = ref(initialControlTuning.strafe) +const turnSpeed = ref(initialControlTuning.turn) +const turboMultiplier = ref(initialControlTuning.turbo) + +let socket: WebSocket | null = null +let sendTimer: number | null = null +let reconnectTimer: number | null = null +let gamepadTimer: number | null = null +let manualClose = false +let consumerCount = 0 +let lastGamepadSignature = '' +let lastCommandSignature = '' + +function noteOperatorInput() { + operatorInputSequence.value += 1 + lastOperatorInputPerfMs.value = performance.now() +} + +function normalizeAxis(raw: number) { + if (Math.abs(raw) < GAMEPAD_DEADZONE) { + return 0 + } + const sign = raw >= 0 ? 1 : -1 + return sign * ((Math.abs(raw) - GAMEPAD_DEADZONE) / (1 - GAMEPAD_DEADZONE)) +} + +function roundValue(value: number) { + return Math.round(value * 1000) / 1000 +} + +function persistControlTuning() { + if (typeof window === 'undefined') { + return + } + + try { + window.localStorage.setItem( + CONTROL_TUNING_STORAGE_KEY, + JSON.stringify({ + forward: forwardSpeed.value, + strafe: strafeSpeed.value, + turn: turnSpeed.value, + turbo: turboMultiplier.value, + }), + ) + } catch { + // Ignore storage failures so tuning still works for the current session. + } +} + +function persistControlInputMode() { + if (typeof window === 'undefined') { + return + } + + try { + window.localStorage.setItem(CONTROL_INPUT_MODE_STORAGE_KEY, controlInputMode.value) + } catch { + // Ignore storage failures so mode switching still works for the current session. + } +} + +function setControlInputMode(next: ControlInputMode) { + const resolved = normalizeControlInputMode(next) + const previous = controlInputMode.value + + if (resolved === previous) { + return + } + + controlInputMode.value = resolved + persistControlInputMode() + + if (previous === 'keyboard') { + pressedKeys.value = new Set() + } + + refreshSendLoop(true) +} + +function setControlTuning(next: Partial) { + const resolved = normalizeControlTuning({ + forward: next.forward ?? forwardSpeed.value, + strafe: next.strafe ?? strafeSpeed.value, + turn: next.turn ?? turnSpeed.value, + turbo: next.turbo ?? turboMultiplier.value, + }) + const changed = + resolved.forward !== forwardSpeed.value || + resolved.strafe !== strafeSpeed.value || + resolved.turn !== turnSpeed.value || + resolved.turbo !== turboMultiplier.value + + forwardSpeed.value = resolved.forward + strafeSpeed.value = resolved.strafe + turnSpeed.value = resolved.turn + turboMultiplier.value = resolved.turbo + persistControlTuning() + + if (changed) { + refreshSendLoop(true) + } +} + +function resetControlTuning() { + setControlTuning(DEFAULT_CONTROL_TUNING) +} + +function packCommand(values: CommandTuple) { + const buffer = new ArrayBuffer(24) + const view = new DataView(buffer) + values.forEach((value, index) => view.setFloat32(index * 4, value, true)) + return buffer +} + +function isZeroCommand(values: CommandTuple) { + return values.every((value) => Math.abs(value) < 0.0001) +} + +function commandSignature(values: CommandTuple, source: ControlSource) { + return `${source}:${values.map((value) => value.toFixed(3)).join(',')}` +} + +function activeTurnAxis() { + const axis2 = normalizeAxis(gamepadAxes.value[2] ?? 0) + const axis3 = normalizeAxis(gamepadAxes.value[3] ?? 0) + return Math.abs(axis2) >= Math.abs(axis3) ? axis2 : axis3 +} + +function keyboardCommandValues(): CommandTuple { + const keys = pressedKeys.value + const turbo = keys.has('ShiftLeft') || keys.has('ShiftRight') ? turboMultiplier.value : 1 + + let lx = 0 + let ly = 0 + let az = 0 + + if (keys.has('KeyW')) lx += forwardSpeed.value + if (keys.has('KeyS')) lx -= forwardSpeed.value + if (keys.has('KeyA')) ly += strafeSpeed.value + if (keys.has('KeyD')) ly -= strafeSpeed.value + if (keys.has('KeyQ')) az += turnSpeed.value + if (keys.has('KeyE')) az -= turnSpeed.value + + if (keys.has('Space')) { + return ZERO_COMMAND + } + + return [ + roundValue(lx * turbo), + roundValue(ly * turbo), + 0, + 0, + 0, + roundValue(az * turbo), + ] +} + +function gamepadCommandValues(): CommandTuple { + if (!gamepadConnected.value) { + return ZERO_COMMAND + } + + const buttons = gamepadButtonPressed.value + const turbo = buttons[5] ? turboMultiplier.value : 1 + + if (buttons[0]) { + return ZERO_COMMAND + } + + const lx = roundValue(-normalizeAxis(gamepadAxes.value[1] ?? 0) * forwardSpeed.value * turbo) + const ly = roundValue(-normalizeAxis(gamepadAxes.value[0] ?? 0) * strafeSpeed.value * turbo) + const az = roundValue(-activeTurnAxis() * turnSpeed.value * turbo) + + return [lx, ly, 0, 0, 0, az] +} + +function keyboardActiveRaw() { + return pressedKeys.value.size > 0 +} + +function keyboardActive() { + return controlInputMode.value === 'keyboard' && keyboardActiveRaw() +} + +function gamepadActiveRaw() { + if (!gamepadConnected.value) { + return false + } + return !isZeroCommand(gamepadCommandValues()) || gamepadButtonPressed.value.some(Boolean) +} + +function gamepadActiveInternal() { + return controlInputMode.value === 'gamepad' && gamepadActiveRaw() +} + +function resolvedSource(): ControlSource { + if (controlInputMode.value === 'keyboard' && keyboardActiveRaw()) { + return 'keyboard' + } + if (controlInputMode.value === 'gamepad' && gamepadActiveRaw()) { + return 'gamepad' + } + return 'idle' +} + +function resolvedCommandValues(): CommandTuple { + const source = resolvedSource() + activeSource.value = source + if (source === 'keyboard') { + return keyboardCommandValues() + } + if (source === 'gamepad') { + return gamepadCommandValues() + } + return ZERO_COMMAND +} + +function stopSendLoop() { + if (sendTimer != null) { + window.clearInterval(sendTimer) + sendTimer = null + } +} + +function sendCurrentCommand() { + if (socket == null || socket.readyState !== WebSocket.OPEN) { + return + } + socket.send(packCommand(resolvedCommandValues())) +} + +function refreshSendLoop(force = false, noteInput = true) { + const source = resolvedSource() + const values = resolvedCommandValues() + const signature = commandSignature(values, source) + + if (!force && signature === lastCommandSignature) { + return + } + lastCommandSignature = signature + if (noteInput) { + noteOperatorInput() + } + + stopSendLoop() + if (socket == null || socket.readyState !== WebSocket.OPEN) { + return + } + + sendCurrentCommand() + if (isZeroCommand(values)) { + return + } + + sendTimer = window.setInterval(() => { + sendCurrentCommand() + }, COMMAND_SEND_INTERVAL_MS) +} + +function clearKeyboardCommands() { + pressedKeys.value = new Set() + refreshSendLoop() +} + +function handleKeydown(event: KeyboardEvent) { + if (!TRACKED_KEYS.includes(event.code)) { + return + } + if (controlInputMode.value !== 'keyboard') { + return + } + if (event.target instanceof HTMLElement) { + const tag = event.target.tagName + if (tag === 'INPUT' || tag === 'TEXTAREA' || tag === 'SELECT') { + return + } + } + + event.preventDefault() + const next = new Set(pressedKeys.value) + next.add(event.code) + pressedKeys.value = next + refreshSendLoop() +} + +function handleKeyup(event: KeyboardEvent) { + if (!TRACKED_KEYS.includes(event.code)) { + return + } + if (controlInputMode.value !== 'keyboard') { + return + } + + event.preventDefault() + const next = new Set(pressedKeys.value) + next.delete(event.code) + pressedKeys.value = next + refreshSendLoop() +} + +function resetGamepadState() { + gamepadConnected.value = false + gamepadNameRaw.value = '' + gamepadIndex.value = null + gamepadMapping.value = '' + gamepadAxes.value = [0, 0, 0, 0] + gamepadButtonPressed.value = Array.from({ length: GAMEPAD_BUTTON_LABELS.length }, () => false) +} + +function pollGamepadState() { + gamepadSupported.value = typeof navigator !== 'undefined' && typeof navigator.getGamepads === 'function' + if (!gamepadSupported.value) { + resetGamepadState() + if (controlInputMode.value === 'gamepad') { + refreshSendLoop() + } + return + } + + const pad = Array.from(navigator.getGamepads()).find((entry): entry is Gamepad => entry != null) + + if (pad == null) { + if (gamepadConnected.value) { + resetGamepadState() + lastGamepadSignature = '' + if (controlInputMode.value === 'gamepad') { + refreshSendLoop() + } + } + return + } + + const axes = Array.from({ length: 4 }, (_, index) => roundValue(normalizeAxis(pad.axes[index] ?? 0))) + const buttons = GAMEPAD_BUTTON_LABELS.map((_, index) => Boolean(pad.buttons[index]?.pressed)) + const signature = `${pad.index}:${pad.id}:${pad.mapping}:${axes.join(',')}:${buttons.map((pressed) => (pressed ? '1' : '0')).join('')}` + + if (signature === lastGamepadSignature) { + return + } + + lastGamepadSignature = signature + gamepadConnected.value = true + gamepadNameRaw.value = pad.id || '' + gamepadIndex.value = pad.index + gamepadMapping.value = pad.mapping || '' + gamepadAxes.value = axes + gamepadButtonPressed.value = buttons + if (controlInputMode.value === 'gamepad') { + refreshSendLoop() + } +} + +function connectSocket() { + if (socket != null && (socket.readyState === WebSocket.OPEN || socket.readyState === WebSocket.CONNECTING)) { + return + } + + manualClose = false + socketState.value = 'connecting' + socket = new WebSocket(buildControlWebSocketUrl()) + socket.binaryType = 'arraybuffer' + + socket.onopen = () => { + socketState.value = 'open' + lastServerMessagePreset.value = 'live' + lastServerMessageOverride.value = '' + refreshSendLoop(true, false) + } + + socket.onmessage = (event) => { + if (typeof event.data === 'string') { + lastServerMessageOverride.value = event.data + } + } + + socket.onclose = () => { + socketState.value = 'closed' + lastServerMessagePreset.value = 'waiting' + lastServerMessageOverride.value = '' + stopSendLoop() + socket = null + if (manualClose) { + return + } + if (reconnectTimer != null) { + window.clearTimeout(reconnectTimer) + } + reconnectTimer = window.setTimeout(() => { + connectSocket() + }, 1000) + } +} + +function disconnectSocket() { + manualClose = true + stopSendLoop() + if (reconnectTimer != null) { + window.clearTimeout(reconnectTimer) + reconnectTimer = null + } + socket?.close() + socket = null +} + +function startGamepadLoop() { + if (gamepadTimer != null) { + window.clearInterval(gamepadTimer) + } + pollGamepadState() + gamepadTimer = window.setInterval(() => { + pollGamepadState() + }, COMMAND_SEND_INTERVAL_MS) +} + +function stopGamepadLoop() { + if (gamepadTimer != null) { + window.clearInterval(gamepadTimer) + gamepadTimer = null + } +} + +function attachGlobalListeners() { + connectSocket() + startGamepadLoop() + window.addEventListener('keydown', handleKeydown) + window.addEventListener('keyup', handleKeyup) + window.addEventListener('blur', clearKeyboardCommands) + window.addEventListener('gamepadconnected', pollGamepadState) + window.addEventListener('gamepaddisconnected', pollGamepadState) +} + +function detachGlobalListeners() { + window.removeEventListener('keydown', handleKeydown) + window.removeEventListener('keyup', handleKeyup) + window.removeEventListener('blur', clearKeyboardCommands) + window.removeEventListener('gamepadconnected', pollGamepadState) + window.removeEventListener('gamepaddisconnected', pollGamepadState) + clearKeyboardCommands() + stopGamepadLoop() + disconnectSocket() +} + +function mountConsumer() { + consumerCount += 1 + if (consumerCount === 1) { + attachGlobalListeners() + } +} + +function unmountConsumer() { + consumerCount = Math.max(consumerCount - 1, 0) + if (consumerCount === 0) { + detachGlobalListeners() + } +} + +const socketLabel = computed(() => { + if (socketState.value === 'open') return t('control.socket.open') + if (socketState.value === 'connecting') return t('control.socket.connecting') + return t('control.socket.reconnecting') +}) + +const activeSourceLabel = computed(() => { + if (activeSource.value === 'keyboard') return t('common.keyboard') + if (activeSource.value === 'gamepad') return t('common.gamepad') + return t('common.idle') +}) + +const controlInputModeLabel = computed(() => { + if (controlInputMode.value === 'gamepad') return t('common.gamepad') + return t('common.keyboard') +}) + +const lastServerMessage = computed(() => { + if (lastServerMessageOverride.value) { + return lastServerMessageOverride.value + } + return lastServerMessagePreset.value === 'live' ? t('control.server.live') : t('control.server.waiting') +}) + +const commandValues = computed(() => { + const [lx, ly, lz, ax, ay, az] = resolvedCommandValues() + return { lx, ly, lz, ax, ay, az } +}) + +const commandLabel = computed(() => { + const { lx, ly, az } = commandValues.value + return `lx=${lx.toFixed(2)} ly=${ly.toFixed(2)} az=${az.toFixed(2)}` +}) + +const commandMagnitude = computed(() => { + const { lx, ly, az } = commandValues.value + const limits = controlLimits.value + return Math.min( + 1, + Math.max( + Math.abs(lx) / Math.max(limits.forward, MIN_AXIS_SPEED), + Math.abs(ly) / Math.max(limits.strafe, MIN_AXIS_SPEED), + Math.abs(az) / Math.max(limits.turn, MIN_AXIS_SPEED), + ), + ) +}) + +const pressedKeysLabel = computed(() => Array.from(pressedKeys.value).sort().join(', ') || t('common.none')) + +const keyboardKeys = computed(() => + TRACKED_KEYS.map((code) => ({ + code, + label: code === 'Space' ? t('control.key.stop') : (KEY_LABELS[code] ?? code), + pressed: pressedKeys.value.has(code), + })), +) + +const keyboardTurbo = computed( + () => controlInputMode.value === 'keyboard' && (pressedKeys.value.has('ShiftLeft') || pressedKeys.value.has('ShiftRight')), +) +const controlTuning = computed(() => ({ + forward: forwardSpeed.value, + strafe: strafeSpeed.value, + turn: turnSpeed.value, + turbo: turboMultiplier.value, +})) +const controlLimits = computed(() => ({ + forward: roundValue(forwardSpeed.value * turboMultiplier.value), + strafe: roundValue(strafeSpeed.value * turboMultiplier.value), + turn: roundValue(turnSpeed.value * turboMultiplier.value), +})) + +const gamepadButtons = computed(() => + GAMEPAD_BUTTON_LABELS.map((label, index) => ({ + label, + pressed: gamepadButtonPressed.value[index] ?? false, + })), +) + +const gamepadName = computed(() => { + if (!gamepadConnected.value) { + return t('control.gamepad.none') + } + return gamepadNameRaw.value || t('control.gamepad.unnamed') +}) + +const gamepadLeftStick = computed(() => ({ + x: gamepadAxes.value[0] ?? 0, + y: gamepadAxes.value[1] ?? 0, +})) + +const gamepadRightStick = computed(() => ({ + x: activeTurnAxis(), + y: gamepadAxes.value[3] ?? 0, +})) + +export function useControlInterface() { + onMounted(() => { + mountConsumer() + }) + + onUnmounted(() => { + unmountConsumer() + }) + + return { + controlInputMode, + controlInputModeLabel, + setControlInputMode, + socketState, + socketLabel, + lastServerMessage, + activeSource, + activeSourceLabel, + commandValues, + commandLabel, + commandMagnitude, + controlTuning, + controlLimits, + setControlTuning, + resetControlTuning, + pressedKeysLabel, + keyboardKeys, + keyboardTurbo, + keyboardActive: computed(() => keyboardActive()), + gamepadSupported, + gamepadConnected, + gamepadName, + gamepadIndex, + gamepadMapping, + gamepadButtons, + gamepadLeftStick, + gamepadRightStick, + gamepadAxes, + gamepadActive: computed(() => gamepadActiveInternal()), + operatorInputSequence, + lastOperatorInputPerfMs, + } +} + +export function useOperatorInputTelemetry() { + return { + operatorInputSequence, + lastOperatorInputPerfMs, + } +} diff --git a/host/robot-command-center/frontend/src/composables/useMonitoringData.ts b/host/robot-command-center/frontend/src/composables/useMonitoringData.ts new file mode 100644 index 0000000..054cb26 --- /dev/null +++ b/host/robot-command-center/frontend/src/composables/useMonitoringData.ts @@ -0,0 +1,67 @@ +import { computed, onMounted, onUnmounted, ref } from 'vue' + +import { fetchDashboardSnapshot } from '@/lib/api' +import { t } from '@/lib/locale' +import type { GpsTelemetry, NetworkTelemetry, VideoStatus } from '@/types' + +type UseMonitoringDataOptions = { + refreshIntervalMs?: number +} + +export function useMonitoringData(options: UseMonitoringDataOptions = {}) { + const gps = ref(null) + const network = ref(null) + const video = ref(null) + const loading = ref(true) + const errorMessage = ref('') + const refreshIntervalMs = Math.max(200, options.refreshIntervalMs ?? 2000) + + let refreshTimer: number | null = null + + async function refreshDashboard() { + try { + const snapshot = await fetchDashboardSnapshot() + gps.value = snapshot.gps + network.value = snapshot.network + video.value = snapshot.video + errorMessage.value = '' + } catch (error) { + errorMessage.value = error instanceof Error ? error.message : t('common.requestFailed', { status: '-', statusText: '' }) + } finally { + loading.value = false + } + } + + const headerStatus = computed(() => { + if (errorMessage.value) { + return errorMessage.value + } + if (loading.value) { + return t('monitoring.loading') + } + return t('monitoring.connected') + }) + + onMounted(() => { + refreshDashboard().catch(() => undefined) + refreshTimer = window.setInterval(() => { + refreshDashboard().catch(() => undefined) + }, refreshIntervalMs) + }) + + onUnmounted(() => { + if (refreshTimer != null) { + window.clearInterval(refreshTimer) + } + }) + + return { + gps, + network, + video, + loading, + errorMessage, + headerStatus, + refreshDashboard, + } +} diff --git a/host/robot-command-center/frontend/src/lib/api.ts b/host/robot-command-center/frontend/src/lib/api.ts new file mode 100644 index 0000000..3ab2f45 --- /dev/null +++ b/host/robot-command-center/frontend/src/lib/api.ts @@ -0,0 +1,84 @@ +import type { CameraName, CameraSelectionStatus, DashboardSnapshot, VideoStatus } from '@/types' +import { t } from '@/lib/locale' + +const envBaseUrl = import.meta.env.VITE_API_BASE_URL as string | undefined + +export const API_BASE = (envBaseUrl?.trim() || 'http://127.0.0.1:8001').replace(/\/$/, '') + +async function fetchJson(path: string): Promise { + const response = await fetch(`${API_BASE}${path}`) + if (!response.ok) { + throw new Error(t('common.requestFailed', { status: response.status, statusText: response.statusText })) + } + return response.json() as Promise +} + +export function fetchDashboardSnapshot() { + return fetchJson('/api/dashboard/') +} + +export function fetchVideoStatus() { + return fetchJson('/api/video/status/') +} + +export function fetchCameraStatus() { + return fetchJson('/api/video/camera/') +} + +export async function selectCamera(camera: CameraName) { + const response = await fetch(`${API_BASE}/api/video/camera/`, { + method: 'POST', + headers: { + 'Content-Type': 'application/json', + }, + body: JSON.stringify({ camera }), + }) + const payload = (await response.json()) as CameraSelectionStatus & { detail?: string } + if (!response.ok) { + throw new Error(payload.detail || payload.last_error || t('common.requestFailed', { + status: response.status, + statusText: response.statusText, + })) + } + return payload +} + +export async function fetchClockCalibrationSample() { + const response = await fetch(`${API_BASE}/api/clock/calibrate/`, { + cache: 'no-store', + }) + if (!response.ok) { + throw new Error(`clock calibration failed: ${response.status} ${response.statusText}`) + } + return response.json() as Promise<{ + server_received_unix_ms: number + server_sent_unix_ms: number + }> +} + +export function buildVideoFrameUrl(frameKey: number) { + return `${API_BASE}/api/video/frame/?frame=${frameKey}&t=${Date.now()}` +} + +export async function postVideoDisplayProbe(payload: Record) { + const response = await fetch(`${API_BASE}/api/video/display-probe/`, { + method: 'POST', + headers: { + 'Content-Type': 'application/json', + }, + body: JSON.stringify(payload), + }) + if (!response.ok) { + throw new Error(`display probe post failed: ${response.status} ${response.statusText}`) + } +} + +export function buildControlWebSocketUrl() { + const url = new URL(API_BASE, window.location.origin) + const basePath = url.pathname.replace(/\/$/, '') + url.protocol = url.protocol === 'https:' ? 'wss:' : 'ws:' + url.pathname = `${basePath}/ws/control/` + url.search = '' + url.hash = '' + return url.toString() +} diff --git a/host/robot-command-center/frontend/src/lib/locale.ts b/host/robot-command-center/frontend/src/lib/locale.ts new file mode 100644 index 0000000..6f3e788 --- /dev/null +++ b/host/robot-command-center/frontend/src/lib/locale.ts @@ -0,0 +1,520 @@ +import { computed, readonly, ref } from 'vue' + +export type Locale = 'zh-CN' | 'en-US' + +const LOCALE_STORAGE_KEY = 'robot-command-center.locale' +const DEFAULT_LOCALE: Locale = 'zh-CN' + +const zhCNMessages = { + 'common.loading': '加载中', + 'common.waiting': '等待中', + 'common.unavailable': '不可用', + 'common.unknown': '未知', + 'common.none': '无', + 'common.na': 'n/a', + 'common.yes': '是', + 'common.no': '否', + 'common.keyboard': '键盘', + 'common.gamepad': '手柄', + 'common.idle': '空闲', + 'common.control': '控制', + 'common.video': '视频', + 'common.online': '在线', + 'common.offline': '离线', + 'common.fresh': '新鲜', + 'common.stale': '过期', + 'common.stable': '稳定', + 'common.rising': '上升', + 'common.falling': '下降', + 'common.selected': '已选中', + 'common.standby': '待命', + 'common.turbo': '加速', + 'common.ackLoop': 'ACK 闭环', + 'common.srttFallback': 'SRTT 回退', + 'common.requestFailed': '请求失败: {status} {statusText}', + 'app.brandTitle': '机器人指挥中心', + 'app.brandSubtitle': '远程机器人控制台', + 'app.nav.overview': '概览', + 'app.nav.video': '视频', + 'app.nav.map': '地图定位', + 'app.nav.network': '网络状态', + 'app.localeToggle': 'English', + 'dashboard.eyebrow': '概览', + 'dashboard.title': '机器人指挥中心', + 'dashboard.description': 'A 端统一后台进程持续刷新视频、控制仲裁和链路遥测。', + 'networkView.eyebrow': '网络', + 'networkView.title': '网络遥测', + 'networkView.description': '查看 A <-> D 与 D <-> B 两段链路的实时队列、重传、窗口压力和延迟估计。', + 'videoView.eyebrow': '视频', + 'videoView.title': '视频监控', + 'videoView.description': '查看机器人实时 JPEG 视频流、画面新鲜度和端到端延迟估计。', + 'mapView.eyebrow': '地图', + 'mapView.title': '地图定位', + 'mapView.description': '查看机器人最新 GPS 数据，并按需使用高德地图做坐标转换和展示。', + 'monitoring.loading': '正在连接 Django 后端并加载实时监控数据...', + 'monitoring.connected': '仪表盘已连接。视频、GPS 和会话遥测正在持续刷新。', + 'control.socket.open': 'WebSocket 已连接', + 'control.socket.connecting': '连接中', + 'control.socket.reconnecting': '重连中', + 'control.server.waiting': '等待控制链路就绪', + 'control.server.live': '控制链路已建立', + 'control.gamepad.none': '未检测到手柄', + 'control.gamepad.unnamed': '未命名手柄', + 'control.gamepad.unknownMapping': '未知映射', + 'control.key.stop': '停止', + 'controlPanel.eyebrow': '控制', + 'controlPanel.title': '控制反馈', + 'controlPanel.resetDefaults': '恢复默认', + 'controlPanel.inputModeEyebrow': '输入模式', + 'controlPanel.inputModeCopy': '同一时刻只能有一种本地输入模式控制页面。', + 'controlPanel.keyboardDetail': '使用 W/S、A/D、Q/E、Shift 和 Space。', + 'controlPanel.gamepadDetail': '仅使用浏览器识别到的手柄。', + 'controlPanel.forward': '前进', + 'controlPanel.strafe': '横移', + 'controlPanel.turn': '转向', + 'controlPanel.turbo': '加速', + 'controlPanel.keyboardHint': '键盘映射: W/S 前后, A/D 横移, Q/E 转向, Shift 加速, Space 停止。', + 'controlPanel.tuningHint': '速度调节由两种本地输入模式共享，并保存在当前浏览器中。', + 'controlPanel.gamepadHint': '手柄模式下左摇杆控制移动，右摇杆控制转向，RB 加速，A 发送停止。', + 'controlFeedback.modeChip': '{mode} 模式', + 'controlFeedback.forward': '前进', + 'controlFeedback.strafe': '横移', + 'controlFeedback.turn': '转向', + 'controlFeedback.tuningSummary': '调参: 前进 {forward} m/s, 横移 {strafe} m/s, 转向 {turn} rad/s, 加速 x{turbo}', + 'controlFeedback.keyboard': '键盘', + 'controlFeedback.gamepad': '手柄', + 'controlFeedback.waitingForController': '等待手柄接入', + 'controlFeedback.gamepadMeta': '#{index} / 映射={mapping}', + 'controlFeedback.gamepadHint': '左摇杆控制移动，右摇杆控制转向，RB 加速，A 停止。', + 'controlFeedback.leftStick': '左摇杆', + 'controlFeedback.rightStick': '右摇杆', + 'controlFeedback.outgoingCommand': '当前发出命令: {command}', + 'videoPanel.eyebrow': '视频', + 'videoPanel.title': '实时视频', + 'videoPanel.frameAlt': '机器人实时画面', + 'videoPanel.waitingFrames': '等待实时视频帧', + 'videoPanel.camera.head': '头部相机', + 'videoPanel.camera.waist': '腰部相机', + 'videoPanel.camera.switching': '正在切换相机…', + 'videoPanel.camera.switchFailed': '相机切换失败：{error}', + 'videoPanel.mode.loading': '加载中', + 'videoPanel.mode.live': '{fps} FPS 实时', + 'videoPanel.stats.frames': '帧数', + 'videoPanel.stats.latestSeq': '最新序号', + 'videoPanel.stats.videoE2E': '视频端到端估计', + 'videoPanel.stats.paintDelay': '绘制延迟', + 'videoPanel.section.pipeline': '流水线估计', + 'videoPanel.section.freshness': '新鲜度', + 'videoPanel.section.operator': '操作员闭环', + 'videoPanel.captureToSend': '采集到发送', + 'videoPanel.networkOneWay': '网络单程', + 'videoPanel.partialEstimate': '部分估计', + 'videoPanel.endToEndEstimate': '端到端估计', + 'videoPanel.interFrameAvg': '帧间平均', + 'videoPanel.interFrameP95': '帧间 p95', + 'videoPanel.repeatedRatio': '重复比例', + 'videoPanel.skipRatio': '跳帧比例', + 'videoPanel.longestFreeze': '最长卡顿', + 'videoPanel.lagFrames': '落后帧数', + 'videoPanel.inputToNextSeq': '输入到下一新序号', + 'videoPanel.inputToChangedFrame': '输入到下一变化帧', + 'videoPanel.inputToPaint': '输入到下一次绘制', + 'videoPanel.displayProbeRequestToPaint': '显示探针请求到绘制', + 'videoPanel.senderClockDelta': '发送端时钟差', + 'videoPanel.timing.waiting': '等待中', + 'videoPanel.timing.noTrailer': '正在等待第一帧带有效 trailer 的视频数据', + 'videoPanel.timing.rawHint': '这里只显示发送端原始时钟差，设备时钟未同步', + 'videoPanel.noSourceDetail': '暂无实时视频详情', + 'networkPanel.eyebrow': '网络', + 'networkPanel.title': '双段链路遥测', + 'networkPanel.controlLoopRtt': '控制闭环 RTT', + 'networkPanel.controlToPersist': '控制到持久化', + 'networkPanel.controlSrttOneWay': '控制单程 SRTT', + 'networkPanel.videoOneWayEst': '视频单程估计', + 'networkPanel.txRate': '发送速率', + 'networkPanel.rxRate': '接收速率', + 'networkPanel.robotFault': '机器人故障', + 'networkPanel.recoveryState': '恢复状态', + 'networkPanel.healthConfidence': '健康置信度', + 'networkPanel.healthUpdated': '健康更新时间', + 'networkPanel.transport': '传输', + 'networkPanel.activeControl': '当前控制源', + 'networkPanel.lease': '租约', + 'networkPanel.ackMode': 'ACK 模式', + 'networkPanel.ackUpdated': 'ACK 更新时间', + 'networkPanel.telemetryPeer': '遥测 Peer', + 'networkPanel.telemetryRegistered': '遥测已注册', + 'networkPanel.hubFreshness': 'Hub 新鲜度', + 'networkPanel.hubState': 'Hub 状态', + 'networkPanel.telemetryReconnects': '遥测重连次数', + 'networkPanel.hubError': 'Hub 错误', + 'networkPanel.telemetrySessionError': '遥测会话错误', + 'networkPanel.online': '在线', + 'networkPanel.maxPressure': '最大压力', + 'networkPanel.queued': '排队量', + 'networkPanel.inFlightBuffer': '在途缓冲', + 'networkPanel.retransDelta': '重传增量', + 'networkPanel.repairRate': '修复率', + 'networkPanel.updated': '更新时间', + 'networkPanel.srtt': 'SRTT', + 'networkPanel.rttvar': 'RTTVAR', + 'networkPanel.rto': 'RTO', + 'networkPanel.sndWnd': '发送窗口', + 'networkPanel.rmtWnd': '远端窗口', + 'networkPanel.inflight': '在途', + 'networkPanel.windowLimit': '窗口上限', + 'networkPanel.pressure': '压力', + 'networkPanel.sndQueue': '发送队列', + 'networkPanel.sndBuffer': '发送缓冲', + 'networkPanel.queueDelta': '队列增量', + 'networkPanel.bufferDelta': '缓冲增量', + 'networkPanel.retrans': '重传', + 'networkPanel.fastRetrans': '快速重传', + 'networkPanel.lost': '丢失', + 'networkPanel.repeat': '重复', + 'networkPanel.appBytes': '应用字节', + 'networkPanel.registered': '已注册', + 'networkPanel.serverError': '服务端错误', + 'networkPanel.combined': '总计', + 'networkPanel.videoE2E': '视频端到端估计', + 'networkPanel.controlEstimateConfidence': '控制估计置信度', + 'networkPanel.videoFreshness': '视频新鲜度', + 'networkPanel.videoFreshnessRepeat': '重复', + 'networkPanel.videoFreshnessSkip': '跳帧', + 'networkPanel.videoFreshnessFreeze': '卡顿', + 'networkPanel.nativeUdp': '原生 UDP', + 'networkPanel.controlSender': '控制发送端', + 'networkPanel.ackReceiver': 'ACK 接收端', + 'networkPanel.controlReconnects': '控制重连次数', + 'networkPanel.controlSessionError': '控制会话错误', + 'networkPanel.loadingPeer': '加载中', + 'networkPanel.unassigned': '未分配', + 'gpsMap.eyebrow': 'GPS', + 'gpsMap.title': '地图定位', + 'gpsMap.intro': '这里展示机器人最新的 GPS 定位，并在需要时调用高德地图做坐标转换。', + 'gpsMap.keyPlaceholder': '高德 Web 端 Key', + 'gpsMap.jscodePlaceholder': '安全密钥 jscode', + 'gpsMap.loadMap': '加载地图', + 'gpsMap.stopMap': '停止加载', + 'gpsMap.status.waitingInit': '等待加载高德地图。', + 'gpsMap.status.fillCredentials': '请先填写高德 Key 和安全密钥 jscode。', + 'gpsMap.status.loading': '正在加载高德地图...', + 'gpsMap.status.loaded': '地图已加载。', + 'gpsMap.status.stopped': '已停止高德地图加载与坐标转换。需要时再点击“加载地图”即可。', + 'gpsMap.status.waitingGps': '等待 GPS 数据。', + 'gpsMap.status.noFix': 'GPS 在线，但当前还没有有效定位。', + 'gpsMap.status.convertFailed': 'GPS 坐标转换失败。', + 'gpsMap.status.refreshedSource': '地图已刷新，数据源: {source}', + 'gpsMap.status.restoredConfig': '已恢复高德配置。地图不会自动加载，按需点击“加载地图”。', + 'gpsMap.status.loadFailed': '地图加载失败。', + 'gpsMap.mapPlaceholder': '高德地图当前未加载。点击上方“加载地图”后才会开始请求地图与坐标转换服务。', + 'gpsMap.wgs84': 'WGS84 坐标', + 'gpsMap.gcj02': '高德 GCJ-02', + 'gpsMap.rawLatHex': '纬度原始 8 字节', + 'gpsMap.rawLonHex': '经度原始 8 字节', + 'gpsMap.utcTime': 'UTC 时间', + 'gpsMap.satAltitude': '卫星 / 海拔', + 'gpsMap.coordMeta': '坐标系 / 格式', + 'gpsMap.lastUpdated': '最近刷新', + 'gpsMap.noValue': '暂无', + 'gpsMap.noValidFix': '暂无有效定位', + 'gpsMap.infoTitle': '机器人 GPS 定位', + 'gpsMap.infoSatellites': '卫星数', + 'gpsMap.infoAltitude': '海拔', +} as const + +export type MessageKey = keyof typeof zhCNMessages + +const enUSMessages: Record = { + 'common.loading': 'Loading', + 'common.waiting': 'Waiting', + 'common.unavailable': 'Unavailable', + 'common.unknown': 'Unknown', + 'common.none': 'None', + 'common.na': 'n/a', + 'common.yes': 'Yes', + 'common.no': 'No', + 'common.keyboard': 'Keyboard', + 'common.gamepad': 'Gamepad', + 'common.idle': 'Idle', + 'common.control': 'Control', + 'common.video': 'Video', + 'common.online': 'Online', + 'common.offline': 'Offline', + 'common.fresh': 'Fresh', + 'common.stale': 'Stale', + 'common.stable': 'Stable', + 'common.rising': 'Rising', + 'common.falling': 'Falling', + 'common.selected': 'Selected', + 'common.standby': 'Standby', + 'common.turbo': 'Turbo', + 'common.ackLoop': 'ACK loop', + 'common.srttFallback': 'SRTT fallback', + 'common.requestFailed': 'Request failed: {status} {statusText}', + 'app.brandTitle': 'Robot Command Center', + 'app.brandSubtitle': 'Remote robot command console', + 'app.nav.overview': 'Overview', + 'app.nav.video': 'Video', + 'app.nav.map': 'Map', + 'app.nav.network': 'Network', + 'app.localeToggle': '中文', + 'dashboard.eyebrow': 'Overview', + 'dashboard.title': 'Robot Command Center', + 'dashboard.description': 'The A-side unified backend keeps video, control arbitration, and live transport telemetry refreshed.', + 'networkView.eyebrow': 'Network', + 'networkView.title': 'Network Telemetry', + 'networkView.description': 'Inspect queueing, retransmissions, window pressure, and latency estimates for the A <-> D and D <-> B legs.', + 'videoView.eyebrow': 'Video', + 'videoView.title': 'Video Monitor', + 'videoView.description': 'Inspect the live robot JPEG stream, freshness metrics, and end-to-end latency estimates.', + 'mapView.eyebrow': 'Map', + 'mapView.title': 'Map Positioning', + 'mapView.description': 'Inspect the latest robot GPS fix and use AMap for coordinate conversion when needed.', + 'monitoring.loading': 'Connecting to the Django backend and loading live monitoring data...', + 'monitoring.connected': 'Dashboard connected. Video, GPS, and session telemetry are refreshing continuously.', + 'control.socket.open': 'WebSocket open', + 'control.socket.connecting': 'Connecting', + 'control.socket.reconnecting': 'Reconnecting', + 'control.server.waiting': 'Waiting for control link', + 'control.server.live': 'Control link live', + 'control.gamepad.none': 'No gamepad detected', + 'control.gamepad.unnamed': 'Unnamed gamepad', + 'control.gamepad.unknownMapping': 'unknown', + 'control.key.stop': 'Stop', + 'controlPanel.eyebrow': 'Control', + 'controlPanel.title': 'Control Feedback', + 'controlPanel.resetDefaults': 'Reset Defaults', + 'controlPanel.inputModeEyebrow': 'Input Mode', + 'controlPanel.inputModeCopy': 'Only one local input mode can control the page at a time.', + 'controlPanel.keyboardDetail': 'Use W/S, A/D, Q/E, Shift, and Space.', + 'controlPanel.gamepadDetail': 'Use the browser-detected controller only.', + 'controlPanel.forward': 'Forward', + 'controlPanel.strafe': 'Strafe', + 'controlPanel.turn': 'Turn', + 'controlPanel.turbo': 'Turbo', + 'controlPanel.keyboardHint': 'Keyboard mapping: W/S forward-back, A/D strafe, Q/E turn, Shift turbo, Space stop.', + 'controlPanel.tuningHint': 'Speed tuning is shared by both local input modes and saved in this browser.', + 'controlPanel.gamepadHint': 'Gamepad mode uses the left stick to drive, the right stick to turn, RB to boost, and A to stop.', + 'controlFeedback.modeChip': '{mode} mode', + 'controlFeedback.forward': 'Forward', + 'controlFeedback.strafe': 'Strafe', + 'controlFeedback.turn': 'Turn', + 'controlFeedback.tuningSummary': 'Tuning: fwd {forward} m/s, strafe {strafe} m/s, turn {turn} rad/s, turbo x{turbo}', + 'controlFeedback.keyboard': 'Keyboard', + 'controlFeedback.gamepad': 'Gamepad', + 'controlFeedback.waitingForController': 'Waiting for controller', + 'controlFeedback.gamepadMeta': '#{index} / mapping={mapping}', + 'controlFeedback.gamepadHint': 'Left stick drives, right stick turns, RB boosts, A stops.', + 'controlFeedback.leftStick': 'Left stick', + 'controlFeedback.rightStick': 'Right stick', + 'controlFeedback.outgoingCommand': 'Outgoing command: {command}', + 'videoPanel.eyebrow': 'Video', + 'videoPanel.title': 'Live Video', + 'videoPanel.frameAlt': 'Robot live frame', + 'videoPanel.waitingFrames': 'waiting for live video frames', + 'videoPanel.camera.head': 'Head camera', + 'videoPanel.camera.waist': 'Waist camera', + 'videoPanel.camera.switching': 'Switching camera…', + 'videoPanel.camera.switchFailed': 'Camera switch failed: {error}', + 'videoPanel.mode.loading': 'loading', + 'videoPanel.mode.live': '{fps} FPS live', + 'videoPanel.stats.frames': 'Frames', + 'videoPanel.stats.latestSeq': 'Latest Seq', + 'videoPanel.stats.videoE2E': 'Video E2E Est.', + 'videoPanel.stats.paintDelay': 'Paint Delay', + 'videoPanel.section.pipeline': 'Pipeline Estimate', + 'videoPanel.section.freshness': 'Freshness', + 'videoPanel.section.operator': 'Operator Loop', + 'videoPanel.captureToSend': 'Capture to send', + 'videoPanel.networkOneWay': 'Network one-way', + 'videoPanel.partialEstimate': 'Partial estimate', + 'videoPanel.endToEndEstimate': 'End-to-end estimate', + 'videoPanel.interFrameAvg': 'Inter-frame avg', + 'videoPanel.interFrameP95': 'Inter-frame p95', + 'videoPanel.repeatedRatio': 'Repeated ratio', + 'videoPanel.skipRatio': 'Skip ratio', + 'videoPanel.longestFreeze': 'Longest freeze', + 'videoPanel.lagFrames': 'Lag frames', + 'videoPanel.inputToNextSeq': 'Input to next seq', + 'videoPanel.inputToChangedFrame': 'Input to changed frame', + 'videoPanel.inputToPaint': 'Input to paint', + 'videoPanel.displayProbeRequestToPaint': 'Display probe request-to-paint', + 'videoPanel.senderClockDelta': 'Sender Clock Delta', + 'videoPanel.timing.waiting': 'waiting', + 'videoPanel.timing.noTrailer': 'waiting for the first valid video trailer', + 'videoPanel.timing.rawHint': 'raw sender clock delta only, unsynced clocks', + 'videoPanel.noSourceDetail': 'no live video detail available', + 'networkPanel.eyebrow': 'Network', + 'networkPanel.title': 'Dual-Leg Telemetry', + 'networkPanel.controlLoopRtt': 'Control Loop RTT', + 'networkPanel.controlToPersist': 'Control to Persist', + 'networkPanel.controlSrttOneWay': 'Control SRTT One-way', + 'networkPanel.videoOneWayEst': 'Video One-way Est.', + 'networkPanel.txRate': 'TX Rate', + 'networkPanel.rxRate': 'RX Rate', + 'networkPanel.robotFault': 'Robot Fault', + 'networkPanel.recoveryState': 'Recovery State', + 'networkPanel.healthConfidence': 'Health Confidence', + 'networkPanel.healthUpdated': 'Health Updated', + 'networkPanel.transport': 'Transport', + 'networkPanel.activeControl': 'Active Control', + 'networkPanel.lease': 'Lease', + 'networkPanel.ackMode': 'ACK Mode', + 'networkPanel.ackUpdated': 'ACK Updated', + 'networkPanel.telemetryPeer': 'Telemetry Peer', + 'networkPanel.telemetryRegistered': 'Telemetry Registered', + 'networkPanel.hubFreshness': 'Hub Freshness', + 'networkPanel.hubState': 'Hub State', + 'networkPanel.telemetryReconnects': 'Telemetry Reconnects', + 'networkPanel.hubError': 'Hub Error', + 'networkPanel.telemetrySessionError': 'Telemetry Session Error', + 'networkPanel.online': 'Online', + 'networkPanel.maxPressure': 'Max Pressure', + 'networkPanel.queued': 'Queued', + 'networkPanel.inFlightBuffer': 'In Flight Buffer', + 'networkPanel.retransDelta': 'Retrans Delta', + 'networkPanel.repairRate': 'Repair Rate', + 'networkPanel.updated': 'Updated', + 'networkPanel.srtt': 'SRTT', + 'networkPanel.rttvar': 'RTTVAR', + 'networkPanel.rto': 'RTO', + 'networkPanel.sndWnd': 'SND WND', + 'networkPanel.rmtWnd': 'RMT WND', + 'networkPanel.inflight': 'Inflight', + 'networkPanel.windowLimit': 'Window Limit', + 'networkPanel.pressure': 'Pressure', + 'networkPanel.sndQueue': 'SND Queue', + 'networkPanel.sndBuffer': 'SND Buffer', + 'networkPanel.queueDelta': 'Queue Delta', + 'networkPanel.bufferDelta': 'Buffer Delta', + 'networkPanel.retrans': 'Retrans', + 'networkPanel.fastRetrans': 'Fast Retrans', + 'networkPanel.lost': 'Lost', + 'networkPanel.repeat': 'Repeat', + 'networkPanel.appBytes': 'App Bytes', + 'networkPanel.registered': 'Registered', + 'networkPanel.serverError': 'Server Error', + 'networkPanel.combined': 'Combined', + 'networkPanel.videoE2E': 'Video E2E Est.', + 'networkPanel.controlEstimateConfidence': 'Control Estimate Confidence', + 'networkPanel.videoFreshness': 'Video Freshness', + 'networkPanel.videoFreshnessRepeat': 'repeat', + 'networkPanel.videoFreshnessSkip': 'skip', + 'networkPanel.videoFreshnessFreeze': 'freeze', + 'networkPanel.nativeUdp': 'Native UDP', + 'networkPanel.controlSender': 'Control Sender', + 'networkPanel.ackReceiver': 'ACK Receiver', + 'networkPanel.controlReconnects': 'Control Reconnects', + 'networkPanel.controlSessionError': 'Control Session Error', + 'networkPanel.loadingPeer': 'loading', + 'networkPanel.unassigned': 'unassigned', + 'gpsMap.eyebrow': 'GPS', + 'gpsMap.title': 'Map Positioning', + 'gpsMap.intro': 'This panel displays the latest robot GPS fix and uses AMap for coordinate conversion when needed.', + 'gpsMap.keyPlaceholder': 'AMap Web Key', + 'gpsMap.jscodePlaceholder': 'Security jscode', + 'gpsMap.loadMap': 'Load Map', + 'gpsMap.stopMap': 'Stop Loading', + 'gpsMap.status.waitingInit': 'Waiting to load AMap.', + 'gpsMap.status.fillCredentials': 'Please enter the AMap key and security jscode first.', + 'gpsMap.status.loading': 'Loading AMap...', + 'gpsMap.status.loaded': 'Map loaded.', + 'gpsMap.status.stopped': 'Stopped AMap loading and coordinate conversion. Click "Load Map" again when needed.', + 'gpsMap.status.waitingGps': 'Waiting for GPS data.', + 'gpsMap.status.noFix': 'GPS is online, but there is no valid fix yet.', + 'gpsMap.status.convertFailed': 'GPS coordinate conversion failed.', + 'gpsMap.status.refreshedSource': 'Map refreshed, source: {source}', + 'gpsMap.status.restoredConfig': 'Recovered saved AMap config. The map will not auto-load; click "Load Map" when needed.', + 'gpsMap.status.loadFailed': 'Map loading failed.', + 'gpsMap.mapPlaceholder': 'AMap is not loaded right now. Click "Load Map" above before requesting map and coordinate conversion services.', + 'gpsMap.wgs84': 'WGS84 Coordinates', + 'gpsMap.gcj02': 'AMap GCJ-02', + 'gpsMap.rawLatHex': 'Raw Latitude 8 Bytes', + 'gpsMap.rawLonHex': 'Raw Longitude 8 Bytes', + 'gpsMap.utcTime': 'UTC Time', + 'gpsMap.satAltitude': 'Satellites / Altitude', + 'gpsMap.coordMeta': 'Coordinate System / Format', + 'gpsMap.lastUpdated': 'Last Updated', + 'gpsMap.noValue': 'Unavailable', + 'gpsMap.noValidFix': 'No valid fix', + 'gpsMap.infoTitle': 'Robot GPS Position', + 'gpsMap.infoSatellites': 'Satellites', + 'gpsMap.infoAltitude': 'Altitude', +} + +const messages: Record> = { + 'zh-CN': zhCNMessages, + 'en-US': enUSMessages, +} + +function normalizeLocale(raw: unknown): Locale { + return raw === 'en-US' ? 'en-US' : DEFAULT_LOCALE +} + +function loadStoredLocale(): Locale { + if (typeof window === 'undefined') { + return DEFAULT_LOCALE + } + try { + return normalizeLocale(window.localStorage.getItem(LOCALE_STORAGE_KEY)) + } catch { + return DEFAULT_LOCALE + } +} + +const localeState = ref(loadStoredLocale()) + +function storeLocale(locale: Locale) { + if (typeof window === 'undefined') { + return + } + try { + window.localStorage.setItem(LOCALE_STORAGE_KEY, locale) + } catch { + // Ignore storage failures; locale still works for current session. + } +} + +function interpolate(template: string, params?: Record) { + if (!params) { + return template + } + return template.replace(/\{(\w+)\}/g, (_, key: string) => String(params[key] ?? '')) +} + +export function t(key: MessageKey, params?: Record) { + const template = messages[localeState.value][key] ?? key + return interpolate(template, params) +} + +export function formatDateTime(value?: string | null) { + if (!value) { + return t('common.unavailable') + } + return new Date(value).toLocaleString(localeState.value, { hour12: false }) +} + +export function setLocale(locale: Locale) { + const next = normalizeLocale(locale) + if (localeState.value === next) { + return + } + localeState.value = next + storeLocale(next) +} + +export function toggleLocale() { + setLocale(localeState.value === 'zh-CN' ? 'en-US' : 'zh-CN') +} + +export function useLocale() { + return { + locale: readonly(localeState), + setLocale, + toggleLocale, + t, + formatDateTime, + nextLocaleLabel: computed(() => t('app.localeToggle')), + } +} diff --git a/host/robot-command-center/frontend/src/main.ts b/host/robot-command-center/frontend/src/main.ts new file mode 100644 index 0000000..fda1e6e --- /dev/null +++ b/host/robot-command-center/frontend/src/main.ts @@ -0,0 +1,12 @@ +import { createApp } from 'vue' +import { createPinia } from 'pinia' + +import App from './App.vue' +import router from './router' + +const app = createApp(App) + +app.use(createPinia()) +app.use(router) + +app.mount('#app') diff --git a/host/robot-command-center/frontend/src/router/index.ts b/host/robot-command-center/frontend/src/router/index.ts new file mode 100644 index 0000000..1c991cb --- /dev/null +++ b/host/robot-command-center/frontend/src/router/index.ts @@ -0,0 +1,34 @@ +import { createRouter, createWebHistory } from 'vue-router' + +import DashboardView from '@/views/DashboardView.vue' +import MapView from '@/views/MapView.vue' +import NetworkView from '@/views/NetworkView.vue' +import VideoView from '@/views/VideoView.vue' + +const router = createRouter({ + history: createWebHistory(import.meta.env.BASE_URL), + routes: [ + { + path: '/', + name: 'dashboard', + component: DashboardView, + }, + { + path: '/video', + name: 'video', + component: VideoView, + }, + { + path: '/map', + name: 'map', + component: MapView, + }, + { + path: '/network', + name: 'network', + component: NetworkView, + }, + ], +}) + +export default router diff --git a/host/robot-command-center/frontend/src/stores/counter.ts b/host/robot-command-center/frontend/src/stores/counter.ts new file mode 100644 index 0000000..b6757ba --- /dev/null +++ b/host/robot-command-center/frontend/src/stores/counter.ts @@ -0,0 +1,12 @@ +import { ref, computed } from 'vue' +import { defineStore } from 'pinia' + +export const useCounterStore = defineStore('counter', () => { + const count = ref(0) + const doubleCount = computed(() => count.value * 2) + function increment() { + count.value++ + } + + return { count, doubleCount, increment } +}) diff --git a/host/robot-command-center/frontend/src/types.ts b/host/robot-command-center/frontend/src/types.ts new file mode 100644 index 0000000..3654382 --- /dev/null +++ b/host/robot-command-center/frontend/src/types.ts @@ -0,0 +1,339 @@ +export interface GpsTelemetry { + has_fix: boolean + utc_time: string + latitude: number | null + longitude: number | null + raw_latitude_hex?: string + raw_longitude_hex?: string + satellites: number | null + altitude_m: number | null + coordinate_system: string + source_sentence: string + raw_coordinate_format: string + source_mode: string + updated_at: string +} + +export interface SessionAppStats { + connected: number + registered?: number + send_calls?: number + send_bytes?: number + send_errors?: number + recv_calls?: number + recv_bytes?: number + recv_timeouts?: number + recv_errors?: number + last_server_error?: string +} + +export interface SessionKcpStats { + connected?: number + conv?: number + rto_ms?: number + srtt_ms?: number + min_srtt_ms?: number + srttvar_ms?: number + last_feedback_age_ms?: number + snd_wnd?: number + rmt_wnd?: number + inflight?: number + window_limit?: number + window_pressure_pct?: number + snd_queue?: number + rcv_queue?: number + snd_buffer?: number + out_segs_total?: number + retrans_total?: number + fast_retrans_total?: number + lost_total?: number + repeat_total?: number + xmit_total?: number +} + +export interface SessionTelemetry { + app: SessionAppStats + kcp: SessionKcpStats +} + +export interface SessionTrendStats { + snd_queue_delta: number + snd_buffer_delta: number + snd_queue_trend: string + snd_buffer_trend: string + retrans_delta: number + fast_retrans_delta: number + lost_delta: number + repeat_delta: number + out_segs_delta: number + repair_rate_pct: number +} + +export interface LinkSessionTelemetry { + peer_id: string + connected: boolean + updated_at: string | null + stale: boolean + app: SessionAppStats | null + kcp: SessionKcpStats + trend: SessionTrendStats +} + +export interface LinkAggregateTelemetry { + online_sessions: number + max_window_pressure_pct: number + sum_snd_queue: number + sum_snd_buffer: number + sum_retrans_delta: number + sum_out_segs_delta: number + repair_rate_pct: number +} + +export interface LinkTelemetry { + source: string + updated_at: string | null + stale: boolean + aggregate: LinkAggregateTelemetry + sessions: { + control: LinkSessionTelemetry + video: LinkSessionTelemetry + } +} + +export interface NativeUdpIngress { + started: boolean + bind_addr: string + packets_received: number + invalid_packets: number + last_sender: string + last_error: string +} + +export interface ControlArbiterStatus { + active_source: string | null + control_lease_remaining_ms: number + packet_counts: Record + send_rate_hz: number + source_lease_ms: number + zero_burst_packets: number + last_error: string + last_sent_at_monotonic: number +} + +export interface ControlSenderStatus { + backend_ready: boolean + started: boolean + connected: boolean + registered: boolean + peer_id: string + target_peer: string + send_count: number + send_errors: number + drain_errors: number + reconnect_count: number + last_server_error: string + last_error: string +} + +export interface ControlAckReceiverStatus { + backend_ready: boolean + started: boolean + connected: boolean + peer_id: string + expected_sender: string + reconnect_count: number + last_error: string +} + +export interface TelemetryReceiverStatus { + hub_connected: boolean + hub_updated_at: string | null + hub_stale: boolean + last_error: string + peer_id: string + registered: boolean + last_server_error: string + reconnect_count: number +} + +export interface RobotHealthStatus { + fault_reason: string + recovery_state: string + confidence: string + updated_at: string +} + +export interface VideoFreshnessStatus { + inter_frame_avg_ms: number | null + inter_frame_p95_ms: number | null + repeated_frame_ratio: number + skip_ratio: number + longest_freeze_ms: number + stale_frame_run_length: number + relative_freshness_lag_frames: number +} + +export interface LatencyEstimateStatus { + control_loop_rtt_ms: number | null + control_to_persist_est_ms: number | null + control_oneway_srtt_est_ms: number | null + control_oneway_bestcase_est_ms: number | null + video_network_oneway_est_ms: number | null + video_partial_est_ms: number | null + video_e2e_est_ms: number | null + estimate_method: { + control: string + video: string + } + clock_sync_required: boolean + assumptions: string[] + confidence: { + control: string + video: string + } +} + +export interface ControlAckStatus { + ack_available: boolean + updated_at: string | null + control_loop_rtt_ms: number | null + b_recv_to_persist_ms: number | null + control_oneway_network_est_ms: number | null + control_to_persist_est_ms: number | null + sample_reason: string | null + receiver: ControlAckReceiverStatus +} + +export interface NetworkTelemetry { + peer_status: string + latency_ms: number | null + jitter_ms: number | null + packet_loss_pct: number | null + tx_kbps: number + rx_kbps: number + transport: string + source_mode: string + updated_at: string + active_control_source: string | null + control_lease_remaining_ms: number + combined: { + connected_sessions: number + send_bytes: number + recv_bytes: number + tx_kbps: number + rx_kbps: number + } + sessions: { + video: SessionTelemetry + control: SessionTelemetry + } + links: { + a_to_d: LinkTelemetry + d_to_b: LinkTelemetry + } + latency_estimate: LatencyEstimateStatus + video_freshness: VideoFreshnessStatus + control_ack_status: ControlAckStatus + telemetry_receiver: TelemetryReceiverStatus + robot_health: RobotHealthStatus + ingress: { + native_udp: NativeUdpIngress + } + control: { + arbiter: ControlArbiterStatus + sender: ControlSenderStatus + ack_receiver: ControlAckReceiverStatus + } +} + +export interface VideoStatus { + available: boolean + source_mode: string + frame_count: number + fps: number + frame_dir: string + source_detail?: string + timing?: { + available: boolean + sender_clock_delta_ms_raw: number | null + sender_clock_delta_samples_ms_raw: number[] + sample_count: number + sample_window_size: number + timestamp_unit: string | null + timestamp_endianness: string | null + unsynced_clock: boolean + } + freshness?: VideoFreshnessStatus + display_probe?: { + updated_at: string | null + frame_seq: number | null + frame_hash: string + input_to_next_fresh_frame_ms: number | null + input_to_next_changed_frame_ms: number | null + input_to_next_paint_ms: number | null + request_to_paint_ms: number | null + response_to_paint_ms: number | null + backend_to_request_ms: number | null + backend_to_request_ms_raw: number | null + backend_to_paint_ms: number | null + backend_to_paint_ms_raw: number | null + browser_backend_clock_offset_ms: number | null + browser_backend_clock_rtt_ms: number | null + browser_backend_clock_sample_count: number + browser_backend_clock_calibrated_at: string | null + } + receiver?: { + backend_ready: boolean + mode: string + connected: boolean + registered: boolean + has_recent_frame: boolean + frames_received: number + latest_sequence: number | null + latest_frame_hash?: string + latest_backend_received_unix_ns?: number | null + latest_backend_received_mono_ns?: number | null + latest_frame_bytes?: number + latest_capture_to_send_ms?: number | null + reconnect_count: number + last_server_error: string + last_error: string + config_path: string + server_addr?: string + relay_via?: string + peer_id?: string + buffer_bytes?: number + timing?: { + available: boolean + sender_clock_delta_ms_raw: number | null + sender_clock_delta_samples_ms_raw: number[] + sample_count: number + sample_window_size: number + timestamp_unit: string | null + timestamp_endianness: string | null + unsynced_clock: boolean + } + freshness?: VideoFreshnessStatus + } +} + +export type CameraName = 'head' | 'waist' + +export interface CameraSelectionStatus { + available: boolean + connected: boolean + registered: boolean + requested_camera: CameraName | null + active_camera: CameraName | null + command_count: number + ack_count: number + updated_at: string | null + last_error: string + confirmed?: boolean +} + +export interface DashboardSnapshot { + gps: GpsTelemetry + network: NetworkTelemetry + video: VideoStatus +} diff --git a/host/robot-command-center/frontend/src/views/DashboardView.vue b/host/robot-command-center/frontend/src/views/DashboardView.vue new file mode 100644 index 0000000..1aad0bb --- /dev/null +++ b/host/robot-command-center/frontend/src/views/DashboardView.vue @@ -0,0 +1,109 @@ + + + + + diff --git a/host/robot-command-center/frontend/src/views/MapView.vue b/host/robot-command-center/frontend/src/views/MapView.vue new file mode 100644 index 0000000..316db32 --- /dev/null +++ b/host/robot-command-center/frontend/src/views/MapView.vue @@ -0,0 +1,72 @@ + + + + + diff --git a/host/robot-command-center/frontend/src/views/NetworkView.vue b/host/robot-command-center/frontend/src/views/NetworkView.vue new file mode 100644 index 0000000..c89eb3f --- /dev/null +++ b/host/robot-command-center/frontend/src/views/NetworkView.vue @@ -0,0 +1,72 @@ + + + + + diff --git a/host/robot-command-center/frontend/src/views/VideoView.vue b/host/robot-command-center/frontend/src/views/VideoView.vue new file mode 100644 index 0000000..b9806ab --- /dev/null +++ b/host/robot-command-center/frontend/src/views/VideoView.vue @@ -0,0 +1,70 @@ + + + + + diff --git a/host/robot-command-center/frontend/tsconfig.app.json b/host/robot-command-center/frontend/tsconfig.app.json new file mode 100644 index 0000000..c0f2d86 --- /dev/null +++ b/host/robot-command-center/frontend/tsconfig.app.json @@ -0,0 +1,18 @@ +{ + "extends": "@vue/tsconfig/tsconfig.dom.json", + "include": ["env.d.ts", "src/**/*", "src/**/*.vue"], + "exclude": ["src/**/__tests__/*"], + "compilerOptions": { + // Extra safety for array and object lookups, but may have false positives. + "noUncheckedIndexedAccess": true, + + // Path mapping for cleaner imports. + "paths": { + "@/*": ["./src/*"] + }, + + // `vue-tsc --build` produces a .tsbuildinfo file for incremental type-checking. + // Specified here to keep it out of the root directory. + "tsBuildInfoFile": "./node_modules/.tmp/tsconfig.app.tsbuildinfo" + } +} diff --git a/host/robot-command-center/frontend/tsconfig.json b/host/robot-command-center/frontend/tsconfig.json new file mode 100644 index 0000000..66b5e57 --- /dev/null +++ b/host/robot-command-center/frontend/tsconfig.json @@ -0,0 +1,11 @@ +{ + "files": [], + "references": [ + { + "path": "./tsconfig.node.json" + }, + { + "path": "./tsconfig.app.json" + } + ] +} diff --git a/host/robot-command-center/frontend/tsconfig.node.json b/host/robot-command-center/frontend/tsconfig.node.json new file mode 100644 index 0000000..c9b2bad --- /dev/null +++ b/host/robot-command-center/frontend/tsconfig.node.json @@ -0,0 +1,27 @@ +// TSConfig for modules that run in Node.js environment via either transpilation or type-stripping. +{ + "extends": "@tsconfig/node24/tsconfig.json", + "include": [ + "vite.config.*", + "vitest.config.*", + "cypress.config.*", + "playwright.config.*", + "eslint.config.*" + ], + "compilerOptions": { + // Most tools use transpilation instead of Node.js's native type-stripping. + // Bundler mode provides a smoother developer experience. + "module": "preserve", + "moduleResolution": "bundler", + + // Include Node.js types and avoid accidentally including other `@types/*` packages. + "types": ["node"], + + // Disable emitting output during `vue-tsc --build`, which is used for type-checking only. + "noEmit": true, + + // `vue-tsc --build` produces a .tsbuildinfo file for incremental type-checking. + // Specified here to keep it out of the root directory. + "tsBuildInfoFile": "./node_modules/.tmp/tsconfig.node.tsbuildinfo" + } +} diff --git a/host/robot-command-center/frontend/vite.config.ts b/host/robot-command-center/frontend/vite.config.ts new file mode 100644 index 0000000..4217010 --- /dev/null +++ b/host/robot-command-center/frontend/vite.config.ts @@ -0,0 +1,18 @@ +import { fileURLToPath, URL } from 'node:url' + +import { defineConfig } from 'vite' +import vue from '@vitejs/plugin-vue' +import vueDevTools from 'vite-plugin-vue-devtools' + +// https://vite.dev/config/ +export default defineConfig({ + plugins: [ + vue(), + vueDevTools(), + ], + resolve: { + alias: { + '@': fileURLToPath(new URL('./src', import.meta.url)) + }, + }, +}) diff --git a/release/OmniSocketGo_deployment-20260808.tar.gz.sha256 b/release/OmniSocketGo_deployment-20260808.tar.gz.sha256 new file mode 100644 index 0000000..c2091ca --- /dev/null +++ b/release/OmniSocketGo_deployment-20260808.tar.gz.sha256 @@ -0,0 +1 @@ +1088bab1b4d59fb2b4434d31520210f81bb6fce202ca9aefd5a5ad24abebd62d OmniSocketGo_deployment-20260808.tar.gz diff --git a/release/README.md b/release/README.md new file mode 100644 index 0000000..4ef1326 --- /dev/null +++ b/release/README.md @@ -0,0 +1,16 @@ +# Extracted release contents + +The source tree in this repository was extracted from the original +`OmniSocketGo_deployment-20260808` bundle. The original bundle contained the +host release and the direct-V4L2 robot release. Their original README and +SHA256 manifests are retained here as `README_original_bundle.md` and +`SHA256SUMS_original_bundle`. + +The ROS 2 robot release is included separately under +`robot/ros2/OmniSocketGo_robot_ros`; it is the ROS 2 camera-ownership variant +validated after the 2026-08-08 bundle was produced. + +The compressed archives themselves are not committed because their contents +are already present as source files and the repository ignores `*.tar.gz`. +The top-level archive checksum file is retained for provenance only; verify +the original archives before distributing them separately. diff --git a/release/README_original_bundle.md b/release/README_original_bundle.md new file mode 100644 index 0000000..17ca25c --- /dev/null +++ b/release/README_original_bundle.md @@ -0,0 +1,77 @@ +# OmniSocketGo deployment bundle — 2026-08-08 + +This delivery bundle contains two independent source archives. + +## Archive roles + +### `OmniSocketGo_robot-20260808.tar.gz` + +Transfer this archive to the physical robot. It contains: + +- `b_side_omnid` source code; +- head/waist camera switching; +- serial-number based `/dev/video*` discovery; +- camera occupancy reporting and known-service release logic; +- direct-LAN and robot-side startup scripts; +- robot-side `requirements.txt` and installation notes. + +Build it on the robot. Do not copy x86_64 host binaries to an ARM64 robot. + +### `OmniSocketGo_host-20260808.tar.gz` + +Install this archive on the operator/control computer. It contains: + +- `OmniSocketGo_add_camera` for the local/public KCP transport; +- `robot-command-center` Django backend; +- `robot-command-center` Vue frontend, including camera switching and waist + image rotation; +- host-side `requirements.txt` and installation notes. + +This archive can also supply the source needed to build `bin/kcpserver` on a +future public relay server. The public server only needs the KCP Hub binary and +does not need `robot-command-center`. + +## Verify downloads + +Run from the directory containing this README: + +```bash +sha256sum -c SHA256SUMS +``` + +## Extract + +Robot: + +```bash +tar -xzf OmniSocketGo_robot-20260808.tar.gz +cd OmniSocketGo_robot-20260808 +``` + +Control host: + +```bash +tar -xzf OmniSocketGo_host-20260808.tar.gz +cd OmniSocketGo_host-20260808 +``` + +Read each archive's `README_PACKAGE.md` and `requirements.txt` before building. + +## Important configuration boundary + +```text +Physical robot: + OmniSocketGo_robot only + +Control host: + OmniSocketGo_add_camera + robot-command-center + +Future public KCP Hub: + build and run bin/kcpserver from OmniSocketGo_add_camera source +``` + +Git does not automatically copy local uncommitted changes to the robot. These +archives capture the current working files, including the local camera and +frontend improvements, while excluding Git metadata, logs, virtual environments, +compiled binaries, frontend `node_modules` and generated frontend `dist` files. diff --git a/release/SHA256SUMS_original_bundle b/release/SHA256SUMS_original_bundle new file mode 100644 index 0000000..b3f26de --- /dev/null +++ b/release/SHA256SUMS_original_bundle @@ -0,0 +1,2 @@ +fad748aa9170fe8c5f9588816764c4171e5f3fc357b776dfef9d89102844fc5c OmniSocketGo_robot-20260808.tar.gz +beb04254fd47e851528b8553120a3ce4fa122f5307d377d7aac97ef18b9cafe0 OmniSocketGo_host-20260808.tar.gz diff --git a/robot/README.md b/robot/README.md new file mode 100644 index 0000000..9e6b264 --- /dev/null +++ b/robot/README.md @@ -0,0 +1,21 @@ +# Robot releases + +There are two camera acquisition releases. Choose one for a run: + +## `v4l2` + +`robot/v4l2/OmniSocketGo_robot` opens `/dev/video*` directly. It is the +2026-08-08 robot package extracted from the deployment bundle. Use it when +testing direct MJPEG/V4L2 access and make sure no Orbbec/proc_manager camera +process owns the selected device. + +## `ros2` + +`robot/ros2/OmniSocketGo_robot_ros` subscribes to the Orbbec ROS 2 RGB topics +through `omnisocket_camera_bridge`. The bridge keeps the existing KCP payload +format while avoiding a second V4L2 opener. It is the path to use when local +RGB/depth consumers and `proc_manager` must continue running. + +The two releases use the same transport peer IDs and daemon entry points; do +not run both simultaneously on one robot. See each release README and the +root `NETWORK_LINK_STARTUP_GUIDE.md` for startup and network configuration. diff --git a/robot/ros2/OmniSocketGo_robot_ros/.gitignore b/robot/ros2/OmniSocketGo_robot_ros/.gitignore new file mode 100644 index 0000000..261c24a --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/.gitignore @@ -0,0 +1,37 @@ +bin/* +inbox/* +*.jsonl +*.html +peer-b-latency.* + + +*.bin +.vscode/settings.json +*.log +root@117.78.11.244 + +c/bin + +*__pycache__* + +/python/build +/python/omnisocket.egg-info + +*.so* + +/.venv + +**/build/ + +ros2/install/ +ros2/log/ + +ros-control-py/install +ros-control-py/log +scripts/boot/modem_network_info.json + +logs/ + +# Machine-specific runtime configuration. +/scripts/dev/robot-remote.env.local +/scripts/boot/robot-boot.env.local diff --git a/robot/ros2/OmniSocketGo_robot_ros/Makefile b/robot/ros2/OmniSocketGo_robot_ros/Makefile new file mode 100644 index 0000000..aa288ca --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/Makefile @@ -0,0 +1,113 @@ +CC ?= gcc +CFLAGS ?= -std=c11 -Wall -Wextra -O2 -pthread -D_GNU_SOURCE +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 + +COMMON_SRCS := \ + $(SRC_DIR)/omni_common.c \ + $(SRC_DIR)/protocol.c \ + $(SRC_DIR)/latencylog.c \ + $(SRC_DIR)/tx_timestamp_debug.c \ + $(SRC_DIR)/kcp_packet_debug.c \ + $(SRC_DIR)/kcp_session_stats.c \ + $(SRC_DIR)/linux_timestamping.c \ + $(SRC_DIR)/interactive.c \ + $(SRC_DIR)/transport_udp.c \ + $(SRC_DIR)/transport_kcp.c \ + $(SRC_DIR)/server_udp_relay.c \ + $(SRC_DIR)/server_udp_hub.c \ + $(SRC_DIR)/server_kcp_hub.c \ + $(SRC_DIR)/peer_udp_client.c \ + $(SRC_DIR)/peer_kcp_client.c \ + third_party/cjson/cJSON.c \ + third_party/kcp/ikcp.c + +TARGETS := \ + $(BIN_DIR)/udpserver \ + $(BIN_DIR)/udppeer \ + $(BIN_DIR)/udpping \ + $(BIN_DIR)/udprelay \ + $(BIN_DIR)/kcpserver \ + $(BIN_DIR)/kcppeer \ + $(BIN_DIR)/kcpping + +CAMERA_VIDEO_SENDER := $(BIN_DIR)/camera_video_sender +FFMPEG_PIPELINE_COMMON_SRCS := \ + $(SRC_DIR)/video_pipeline.c \ + $(SRC_DIR)/ros_image_shm.c \ + $(SRC_DIR)/gps_buffer.c \ + $(SRC_DIR)/omni_common.c \ + $(SRC_DIR)/protocol.c \ + $(SRC_DIR)/latencylog.c \ + $(SRC_DIR)/kcp_packet_debug.c \ + $(SRC_DIR)/kcp_session_stats.c \ + $(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 + +CAMERA_VIDEO_SENDER_SRCS := \ + $(CMD_DIR)/v1_camera_pipeline_ifdef.c \ + $(FFMPEG_PIPELINE_COMMON_SRCS) + +B_SIDE_OMNID := $(BIN_DIR)/b_side_omnid +B_SIDE_OMNID_SRCS := \ + $(CMD_DIR)/b_side_omnid.c \ + $(FFMPEG_PIPELINE_COMMON_SRCS) + +all: $(TARGETS) + +$(BIN_DIR): + mkdir -p $(BIN_DIR) + +$(BIN_DIR)/udpserver: $(CMD_DIR)/udpserver.c $(COMMON_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) -o $@ $^ $(LDFLAGS) + +$(BIN_DIR)/udppeer: $(CMD_DIR)/udppeer.c $(COMMON_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) -o $@ $^ $(LDFLAGS) + +$(BIN_DIR)/udpping: $(CMD_DIR)/udpping.c $(COMMON_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) -o $@ $^ $(LDFLAGS) + +$(BIN_DIR)/udprelay: $(CMD_DIR)/udprelay.c $(COMMON_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) -o $@ $^ $(LDFLAGS) + +$(BIN_DIR)/kcpserver: $(CMD_DIR)/kcpserver.c $(COMMON_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) -o $@ $^ $(LDFLAGS) + +$(BIN_DIR)/kcppeer: $(CMD_DIR)/kcppeer.c $(COMMON_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) -o $@ $^ $(LDFLAGS) + +$(BIN_DIR)/kcpping: $(CMD_DIR)/kcpping.c $(COMMON_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) -o $@ $^ $(LDFLAGS) + +$(CAMERA_VIDEO_SENDER): $(CAMERA_VIDEO_SENDER_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) $$(pkg-config --cflags libavformat libavcodec libavutil libswscale) -o $@ $^ $(LDFLAGS) $$(pkg-config --libs libavformat libavcodec libavutil libswscale) -lm + +camera_video_sender: $(CAMERA_VIDEO_SENDER) + +$(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) -lm + +b_side_omnid: $(B_SIDE_OMNID) + +clean: + rm -rf $(BIN_DIR) + +python-ext: + cd python && $(PYTHON) setup.py build_ext --inplace + +python-install: + cd python && $(PYTHON) -m pip install -e . + +.PHONY: all clean python-ext python-install camera_video_sender b_side_omnid diff --git a/robot/ros2/OmniSocketGo_robot_ros/README.md b/robot/ros2/OmniSocketGo_robot_ros/README.md new file mode 100644 index 0000000..1eac1df --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/README.md @@ -0,0 +1,132 @@ +# OmniSocketGo_robot_ros + +Linux-only C11 implementation of the UDP/KCP transport stack from `OmniSocketGo`. + +This subtree is intentionally standalone. The Go code stays in place as the behavior reference, while the C implementation builds its own binaries under `c/bin/`. + +## Build + +```bash +make -j$(nproc) +``` + +Build outputs: + +- `./bin/udpserver` +- `./bin/udppeer` +- `./bin/udpping` +- `./bin/udprelay` +- `./bin/kcpserver` +- `./bin/kcppeer` +- `./bin/kcpping` + +Python extension build: + +```bash +make python-ext +make python-install +``` + +## 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 \ + -telemetry-interval 1000ms \ + -kcp-session-stats-log logs/d-kcp-stats.jsonl \ + -kcp-session-stats-interval 1000ms +``` + +For multi-hour runs, keep `-latency-log` and `-kcp-ts-debug-log` off unless you are collecting a short repro trace. + +Relay `C` runs a raw UDP forwarder to `D`: + +```bash +./bin/kcpserver -mode=relay -listen 0.0.0.0:10909 -relay-remote 172.21.32.15:10909 +``` + +Peer `A` dials `D` through relay `C`: + +```bash +./bin/kcppeer -id peer-a -server 172.21.32.15:10909 -relay-via 106.55.173.235:10909 \ + -inbox-dir inbox/a \ + -latency-log logs/a-latency.jsonl \ + -kcp-ts-debug-log logs/a-kcp-ts.jsonl \ + -kcp-session-stats-log logs/a-kcp-stats.jsonl +``` + +Peer `B` dials `D` directly: + +```bash +./bin/kcppeer -id peer-b -server 81.70.156.140:10909 \ + -inbox-dir inbox/b \ + -latency-log logs/b-latency.jsonl \ + -kcp-ts-debug-log logs/b-kcp-ts.jsonl \ + -kcp-session-stats-log logs/b-kcp-stats.jsonl +``` + +Optional ping / echo tools: + +```bash +./bin/kcpping -id peer-a -server 106.55.173.235:10909 -echo +./bin/kcpping -id peer-b -server 81.70.156.140:10909 -to peer-a -count 20 -interval 100ms +./bin/udpserver -listen 0.0.0.0:9001 +./bin/udppeer -id peer-a -server 127.0.0.1:9001 +./bin/udpping -id pinger -server 127.0.0.1:9001 -to peer-a -count 20 +``` + +Python control/video demos use two KCP sessions: + +- `peer-a-ctrl <-> peer-b-ctrl` for small binary control packets +- `peer-b-video -> peer-a-video` for larger binary video frames + +Example demo entry points: + +- `udp_keyboard_sender.py` +- `udp_xbox_sender.py` +- `udp_fsm_controller.py` +- `omnisocket_video_sender.py` +- `omnisocket_video_receiver.py` +- `scripts/kcp_control_benchmark.py` + +Python `recv_into()` note: + +- The writable buffer must be large enough for the full incoming payload. +- If the buffer is too small, `recv_into()` reports the required size but the current frame has already been consumed and is lost. +- For the video demo, keep `video_receiver.buffer_bytes >= video_sender.frame_bytes`. + +## Interactive Commands + +`udppeer` and `kcppeer` support the same interactive shell: + +```text +help +text peer-b hello +text peer-a hi +file peer-a /tmp/test125.bin +quit +``` + +## Notes + +- The C project targets Linux only. +- It preserves the Go wire format for UDP datagrams and KCP stream frames. +- It now supports `binary` payload messages in addition to `text`, `file`, `register`, and `error`. +- Python `Session.recv_into()` is a zero-copy receive helper for already-sized buffers; it does not retain oversized frames for a retry. +- It keeps runtime JSONL logging, UDP TX timestamp debug, KCP packet debug, and KCP session stats. +- Offline `latencysummary` and HTML chart generation are intentionally not migrated. +- No automated C tests are included in this subtree; validation is expected to happen on Linux via `make` and manual smoke tests. + +## ROS 2 robot camera mode + +`OmniSocketGo_robot_ros` adds a ROS 2-native robot camera path. The Orbbec +driver/proc_manager owns `/dev/video*`; `omnisocket_camera_bridge` subscribes to +the RGB topics and exposes latest-frame shared-memory slots to `b_side_omnid`. +The C daemon keeps the existing MJPEG/KCP wire format and never opens V4L2 in +the default mode. See: + +- `docs/ROS2_CAMERA_FORWARDING.md` for build, startup, switching, and service ownership; +- `docs/ROS_CAMERA_INTERFACES.md` for the RGB/depth/CameraInfo/metadata topics; +- `ros2/README.md` for building the bridge package. diff --git a/robot/ros2/OmniSocketGo_robot_ros/ROBOT_LAN_README.md b/robot/ros2/OmniSocketGo_robot_ros/ROBOT_LAN_README.md new file mode 100644 index 0000000..edcc115 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/ROBOT_LAN_README.md @@ -0,0 +1,112 @@ +# OmniSocketGo Robot LAN Package + +This package is preconfigured for a robot connected directly by Ethernet to +the operator computer at `192.168.41.144`. + +## Network topology + +```text +robot Ethernet (for example 192.168.41.145/24) + -> UDP 192.168.41.144:10909 + -> local KCP hub on the operator computer +``` + +All relay settings are empty. Video uses +`peer-b-video -> peer-a-video`; control uses `peer-a-ctrl -> peer-b-ctrl`. + +In the legacy V4L2 mode both cameras are opened and kept streaming: + +- head: `/dev/video26` +- waist: `/dev/video18` + +Only the camera selected by `OMNI_CAMERA_ACTIVE` is encoded and transmitted. +The default is `head`. The ROS2-native default does not open these devices; +it subscribes to the corresponding Orbbec RGB topics instead. + +## Robot preparation + +On Ubuntu, install the build/runtime dependencies if they are not already +available: + +```bash +sudo apt-get install build-essential pkg-config \ + libavformat-dev libavcodec-dev libavutil-dev libswscale-dev \ + v4l-utils psmisc python3-colcon-common-extensions \ + ros-jazzy-rclpy ros-jazzy-sensor-msgs +``` + +Configure the robot Ethernet interface in the same subnet as the computer, +for example `192.168.41.145/24`, and verify: + +```bash +ping -c 3 192.168.41.144 +``` + +Then build and check the package: + +```bash +make b_side_omnid +./check-robot-lan.sh +``` + +## Start + +If the robot boot watchdog is already managing an older daemon, stop it first +so it cannot reopen the cameras: + +```bash +sudo systemctl stop blitz-watchdog.service blitz-b-side-omnid.service +``` + +Start the LAN sender: + +```bash +./start-robot-lan.sh +``` + +The camera preflight may stop only the known `orbbec_head.service` and +`orbbec_waist.service` units to release the two devices. It refuses to stop the +whole `proc_manager.service`. + +Successful dual-camera initialization prints: + +```text +[video_pipeline] camera head ready on /dev/video26 +[video_pipeline] camera waist ready on /dev/video18 +``` + +The periodic daemon line should then report `video registered=1`, with +`frames` increasing. For a development start, inspect: + +```bash +python3 -m json.tool logs/runtime/b-side-omnid.status.json +``` + +Expected fields include `video_connected: true`, an increasing +`video_frames_sent`, an empty `video_last_error`, and +`video_active_camera: head`. + +## Select the waist camera at startup + +Edit `scripts/dev/robot-remote.env.local` and set: + +```bash +OMNI_CAMERA_ACTIVE="waist" +``` + +Then restart `start-robot-lan.sh`. Runtime text commands `camera:head` and +`camera:waist` sent from `peer-a-ctrl` also switch the active input without +reopening either camera. + +## Important + +For this direct-LAN validation, use `start-robot-lan.sh`. Do not install the +existing 5G-oriented `blitz-robot.target` boot chain until its modem policy has +been adapted for the target robot. +# ROS 2 相机输入 + +本 ROS-native 版本默认使用 `OMNI_CAMERA_SOURCE=ros2`。头部/腰部 RGB 由 +`omnisocket_camera_bridge` 订阅 ROS 2 图像 topic 后写入共享内存，视频 C +管线不再直接打开 `/dev/video*`。启动与服务占用策略见 +`docs/ROS2_CAMERA_FORWARDING.md`，RGB+深度接口见 +`docs/ROS_CAMERA_INTERFACES.md`。 diff --git a/robot/ros2/OmniSocketGo_robot_ros/check-robot-lan.sh b/robot/ros2/OmniSocketGo_robot_ros/check-robot-lan.sh new file mode 100644 index 0000000..ce08dca --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/check-robot-lan.sh @@ -0,0 +1,60 @@ +#!/usr/bin/env bash +set -euo pipefail + +PROJECT_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${PROJECT_ROOT}/scripts/dev/load-env.sh" + +failed=0 + +check_empty_relay() { + local name="$1" + local value="${!name:-}" + if [[ -n "${value}" ]]; then + echo "[FAIL] ${name} must be empty, got: ${value}" >&2 + failed=1 + else + echo "[ OK ] ${name}=" + fi +} + +check_camera() { + local name="$1" + local device="$2" + if [[ -e "${device}" ]]; then + echo "[ OK ] ${name} camera: ${device} -> $(readlink -f "${device}")" + else + echo "[FAIL] ${name} camera is missing: ${device}" >&2 + failed=1 + fi +} + +echo "Robot video target: ${OMNI_VIDEO_SERVER_ADDR}" +echo "Robot control target: ${OMNI_CONTROL_SERVER_ADDR}" +check_empty_relay ROBOT_SIDE_OMNISOCKET_RELAY_VIA +check_empty_relay OMNI_VIDEO_RELAY_VIA +check_empty_relay OMNI_CONTROL_RELAY_VIA +check_camera head "${OMNI_CAMERA_HEAD_DEVICE}" +check_camera waist "${OMNI_CAMERA_WAIST_DEVICE}" + +if pkg-config --exists libavformat libavcodec libavutil libswscale; then + echo "[ OK ] FFmpeg development libraries" +else + echo "[FAIL] FFmpeg development libraries are missing" >&2 + failed=1 +fi + +if [[ -x "${PROJECT_ROOT}/bin/b_side_omnid" ]]; then + echo "[ OK ] bin/b_side_omnid is built" +else + echo "[WARN] bin/b_side_omnid is not built; run: make b_side_omnid" +fi + +server_host="${OMNI_VIDEO_SERVER_ADDR%:*}" +if command -v ping >/dev/null 2>&1 && ping -c 1 -W 1 "${server_host}" >/dev/null 2>&1; then + echo "[ OK ] computer is reachable: ${server_host}" +else + echo "[WARN] cannot ping ${server_host}; verify the direct Ethernet addresses" +fi + +exit "${failed}" diff --git a/robot/ros2/OmniSocketGo_robot_ros/cmd/b_side_omnid.c b/robot/ros2/OmniSocketGo_robot_ros/cmd/b_side_omnid.c new file mode 100644 index 0000000..40ac2fb --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/cmd/b_side_omnid.c @@ -0,0 +1,1331 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cJSON.h" +#include "control_protocol.h" +#include "latencylog.h" +#include "protocol.h" +#include "video_pipeline.h" + +#define CONTROL_DEFAULT_PEER_ID "peer-b-ctrl" +#define CONTROL_DEFAULT_EXPECTED_SENDER "peer-a-ctrl" +#define CONTROL_ACK_DEFAULT_PEER_ID "peer-b-ctrl-ack" +#define CONTROL_ACK_DEFAULT_TARGET_PEER "peer-a-ctrl-ack" +#define CONTROL_DEFAULT_UNIX_SOCKET "/tmp/omnisocket-b-side-cmd.sock" +#define CONTROL_DEFAULT_SERVER_IDLE_RECONNECT_MS 3000 +#define DEFAULT_RUNTIME_DIR "/run/blitz-robot" +#define DEFAULT_STATUS_FILE_NAME "b-side-omnid.status.json" +#define DEFAULT_VIDEO_THREAD_FAULT_FILE "fault-injection-bside-video-thread-stall" +#define DEFAULT_CONTROL_THREAD_FAULT_FILE "fault-injection-bside-control-thread-stall" +#define DEFAULT_THREAD_HEARTBEAT_TIMEOUT_SEC 15 +#define DEFAULT_KCP_STATS_INTERVAL_MS 1000 +#define DEFAULT_CONTROL_LATENCY_SAMPLE_MOD 100 +#define DEFAULT_CONTROL_ACK_SAMPLE_MOD 10 +#define EXIT_CODE_VIDEO_THREAD_STALLED 101 +#define EXIT_CODE_CONTROL_THREAD_STALLED 102 + +typedef struct unix_dgram_client { + int fd; + char bind_path[108]; + char dest_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; + uint64_t reconnect_count; + uint32_t server_idle_ms; + int ever_connected; + int registered; + char last_error[256]; + char last_reconnect_reason[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; + atomic_int active_camera; + 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_ack_peer_id; + const char *control_ack_target_peer; + const char *control_unix_socket; + int control_server_idle_reconnect_ms; + const char *runtime_dir; + int heartbeat_timeout_sec; + int stats_interval_ms; + uint64_t control_latency_sample_mod; + uint64_t control_ack_sample_mod; + char status_file_path[512]; + char video_thread_fault_file[512]; + char control_thread_fault_file[512]; + atomic_long video_thread_heartbeat_epoch_sec; + atomic_long control_thread_heartbeat_epoch_sec; + atomic_int control_ack_shutdown_requested; + kcp_session_stats_logger_t *stats_logger; + latency_logger_t *control_latency_logger; + video_stage_logger_t *video_stage_logger; + unix_dgram_client_t unix_client; + control_bridge_stats_t control_stats; + pthread_mutex_t control_ack_mutex; + pthread_t control_ack_thread; + kcp_client_t *control_ack_client; + int control_ack_thread_started; + int control_ack_connect_requested; + int control_ack_connect_inflight; +} daemon_state_t; + +static void control_message_body_to_cstr(const message_t *msg, char *buffer, size_t buffer_len); + +static const char *camera_name(int camera) { + return camera == VIDEO_CAMERA_WAIST ? "waist" : "head"; +} + +static int handle_camera_select_message(daemon_state_t *state, const message_t *msg) { + char body[64]; + int selected; + + if (state == NULL || msg == NULL || msg->type != MSG_TYPE_TEXT) { + return 0; + } + control_message_body_to_cstr(msg, body, sizeof(body)); + if (strcmp(body, "camera:head") == 0 || strcmp(body, "camera.select=head") == 0) { + selected = VIDEO_CAMERA_HEAD; + } else if (strcmp(body, "camera:waist") == 0 || strcmp(body, "camera.select=waist") == 0) { + selected = VIDEO_CAMERA_WAIST; + } else { + return 0; + } + atomic_store(&state->active_camera, selected); + fprintf(stderr, "[b_side_omnid] active camera switched to %s\n", camera_name(selected)); + return 1; +} + +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 env_int_or_default(const char *name, int fallback) { + const char *value = getenv(name); + int parsed; + + if (value == NULL || value[0] == '\0') { + return fallback; + } + parsed = atoi(value); + if (parsed <= 0) { + return fallback; + } + return parsed; +} + +static uint64_t env_u64_or_default(const char *name, uint64_t fallback) { + const char *value = getenv(name); + unsigned long long parsed = 0ULL; + char *endptr = NULL; + + if (value == NULL || value[0] == '\0') { + return fallback; + } + parsed = strtoull(value, &endptr, 10); + if (endptr == value || *endptr != '\0' || parsed == 0ULL) { + return fallback; + } + return (uint64_t) parsed; +} + +static int64_t realtime_epoch_ms(void) { + struct timespec ts; + + clock_gettime(CLOCK_REALTIME, &ts); + return (int64_t) ts.tv_sec * 1000 + ts.tv_nsec / 1000000; +} + +static long realtime_epoch_sec(void) { + return (long) time(NULL); +} + +static void update_thread_heartbeat(atomic_long *heartbeat) { + if (heartbeat == NULL) { + return; + } + atomic_store(heartbeat, realtime_epoch_sec()); +} + +static int should_log_control_latency(const daemon_state_t *state, const message_t *msg) { + uint64_t sample_mod; + + if (state == NULL || state->control_latency_logger == NULL || msg == NULL) { + return 0; + } + sample_mod = state->control_latency_sample_mod; + if (sample_mod <= 1U) { + return 1; + } + return msg->id % sample_mod == 0U; +} + +static int should_send_control_ack(const daemon_state_t *state, const message_t *msg) { + uint64_t sample_mod; + + if (state == NULL || msg == NULL) { + return 0; + } + sample_mod = state->control_ack_sample_mod; + if (sample_mod <= 1U) { + return 1; + } + return msg->id % sample_mod == 0U; +} + +static void video_pipeline_heartbeat_progress(void *context) { + update_thread_heartbeat((atomic_long *) context); +} + +static int ensure_runtime_dir(const char *runtime_dir) { + struct stat st; + + if (runtime_dir == NULL || runtime_dir[0] == '\0') { + errno = EINVAL; + return -1; + } + if (stat(runtime_dir, &st) == 0) { + if (S_ISDIR(st.st_mode)) { + return 0; + } + errno = ENOTDIR; + return -1; + } + if (errno != ENOENT) { + return -1; + } + if (mkdir(runtime_dir, 0775) != 0 && errno != EEXIST) { + return -1; + } + return 0; +} + +static int path_exists(const char *path) { + return path != NULL && path[0] != '\0' && access(path, F_OK) == 0; +} + +static int consume_fault_flag(const char *path) { + if (!path_exists(path)) { + return 0; + } + unlink(path); + return 1; +} + +static void maybe_inject_thread_stall(daemon_state_t *state, const char *fault_path, const char *thread_name) { + if (state == NULL || fault_path == NULL || thread_name == NULL) { + return; + } + if (!consume_fault_flag(fault_path)) { + return; + } + fprintf( + stderr, + "[b_side_omnid] fault injection requested for %s thread, sleeping past %d second heartbeat timeout\n", + thread_name, + state->heartbeat_timeout_sec + ); + sleep((unsigned int) state->heartbeat_timeout_sec + 2U); +} + +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 unix_dgram_client_close(unix_dgram_client_t *client); +static void control_bridge_stats_snapshot(control_bridge_stats_t *stats, control_bridge_stats_t *out_stats); +static void close_control_ack_client(kcp_client_t **client_ptr); + +static int control_ack_enabled(const daemon_state_t *state) { + return state != NULL + && state->control_ack_peer_id != NULL + && state->control_ack_peer_id[0] != '\0' + && state->control_ack_target_peer != NULL + && state->control_ack_target_peer[0] != '\0'; +} + +static int control_ack_manager_init(daemon_state_t *state) { + int rc; + + if (state == NULL) { + errno = EINVAL; + return -1; + } + rc = pthread_mutex_init(&state->control_ack_mutex, NULL); + if (rc != 0) { + errno = rc; + return -1; + } + atomic_init(&state->control_ack_shutdown_requested, 0); + state->control_ack_client = NULL; + state->control_ack_thread_started = 0; + state->control_ack_connect_requested = 0; + state->control_ack_connect_inflight = 0; + return 0; +} + +static void control_ack_manager_reset(daemon_state_t *state, int request_connect) { + kcp_client_t *client = NULL; + + if (state == NULL) { + return; + } + pthread_mutex_lock(&state->control_ack_mutex); + client = state->control_ack_client; + state->control_ack_client = NULL; + state->control_ack_connect_requested = request_connect && control_ack_enabled(state) && state->control_ack_thread_started; + pthread_mutex_unlock(&state->control_ack_mutex); + close_control_ack_client(&client); +} + +static void control_ack_manager_destroy(daemon_state_t *state) { + if (state == NULL) { + return; + } + atomic_store(&state->control_ack_shutdown_requested, 1); + if (state->control_ack_thread_started) { + pthread_join(state->control_ack_thread, NULL); + state->control_ack_thread_started = 0; + } + control_ack_manager_reset(state, 0); + pthread_mutex_destroy(&state->control_ack_mutex); +} + +static int write_status_json_atomic(const char *path, cJSON *root) { + char *json; + char temp_path[640]; + FILE *file; + size_t json_len; + + if (path == NULL || root == NULL) { + errno = EINVAL; + return -1; + } + + json = cJSON_PrintUnformatted(root); + if (json == NULL) { + errno = ENOMEM; + return -1; + } + + snprintf(temp_path, sizeof(temp_path), "%s.tmp.%ld", path, (long) getpid()); + file = fopen(temp_path, "wb"); + if (file == NULL) { + cJSON_free(json); + return -1; + } + + json_len = strlen(json); + if (fwrite(json, 1, json_len, file) != json_len || fflush(file) != 0) { + int saved_errno = errno; + + fclose(file); + unlink(temp_path); + cJSON_free(json); + errno = saved_errno; + return -1; + } + if (fclose(file) != 0) { + int saved_errno = errno; + + unlink(temp_path); + cJSON_free(json); + errno = saved_errno; + return -1; + } + if (rename(temp_path, path) != 0) { + int saved_errno = errno; + + unlink(temp_path); + cJSON_free(json); + errno = saved_errno; + return -1; + } + + cJSON_free(json); + return 0; +} + +static int write_daemon_status_file(daemon_state_t *state) { + cJSON *root; + video_pipeline_stats_t video_stats; + control_bridge_stats_t control_stats; + int rc; + + if (state == NULL) { + errno = EINVAL; + return -1; + } + if (ensure_runtime_dir(state->runtime_dir) != 0) { + return -1; + } + + 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); + + root = cJSON_CreateObject(); + if (root == NULL) { + errno = ENOMEM; + return -1; + } + + cJSON_AddNumberToObject(root, "updated_at_epoch_ms", (double) realtime_epoch_ms()); + cJSON_AddNumberToObject(root, "pid", (double) getpid()); + cJSON_AddNumberToObject(root, "video_thread_heartbeat_epoch_ms", (double) atomic_load(&state->video_thread_heartbeat_epoch_sec) * 1000.0); + cJSON_AddNumberToObject(root, "control_thread_heartbeat_epoch_ms", (double) atomic_load(&state->control_thread_heartbeat_epoch_sec) * 1000.0); + cJSON_AddBoolToObject(root, "video_connected", video_stats.connected != 0); + cJSON_AddNumberToObject(root, "video_frames_sent", (double) video_stats.frames_sent); + cJSON_AddNumberToObject(root, "video_send_errors", (double) video_stats.send_errors); + cJSON_AddNumberToObject(root, "video_backlog_resets", (double) video_stats.backlog_resets); + cJSON_AddNumberToObject(root, "video_last_capture_to_send_ms", (double) video_stats.last_capture_to_send_ms); + cJSON_AddNumberToObject(root, "video_avg_capture_to_send_ms", video_stats.avg_capture_to_send_ms); + cJSON_AddStringToObject( + root, + "video_input_source", + state->video_config.input_mode == VIDEO_INPUT_ROS2 ? "ros2" : "v4l2" + ); + cJSON_AddStringToObject(root, "video_active_camera", camera_name(atomic_load(&state->active_camera))); + cJSON_AddStringToObject(root, "video_last_error", video_stats.last_error); + cJSON_AddBoolToObject(root, "control_registered", control_stats.registered != 0); + cJSON_AddNumberToObject(root, "control_reconnect_count", (double) control_stats.reconnect_count); + cJSON_AddNumberToObject(root, "control_unix_send_errors", (double) control_stats.unix_send_errors); + cJSON_AddStringToObject(root, "control_last_error", control_stats.last_error); + + rc = write_status_json_atomic(state->status_file_path, root); + cJSON_Delete(root); + return rc; +} + +static int thread_heartbeat_expired(atomic_long *heartbeat, int timeout_sec, long now_sec) { + long heartbeat_sec; + + if (heartbeat == NULL || timeout_sec <= 0) { + return 0; + } + heartbeat_sec = atomic_load(heartbeat); + if (heartbeat_sec <= 0) { + return 0; + } + return now_sec - heartbeat_sec > timeout_sec; +} + +static void exit_if_thread_stalled(daemon_state_t *state) { + long now_sec; + + if (state == NULL || state->heartbeat_timeout_sec <= 0) { + return; + } + now_sec = realtime_epoch_sec(); + if (thread_heartbeat_expired(&state->video_thread_heartbeat_epoch_sec, state->heartbeat_timeout_sec, now_sec)) { + fprintf(stderr, "[b_side_omnid] video thread heartbeat stalled for more than %d seconds\n", state->heartbeat_timeout_sec); + fflush(stderr); + exit(EXIT_CODE_VIDEO_THREAD_STALLED); + } + if (thread_heartbeat_expired(&state->control_thread_heartbeat_epoch_sec, state->heartbeat_timeout_sec, now_sec)) { + fprintf(stderr, "[b_side_omnid] control thread heartbeat stalled for more than %d seconds\n", state->heartbeat_timeout_sec); + fflush(stderr); + exit(EXIT_CODE_CONTROL_THREAD_STALLED); + } +} + +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_reconnect_reason(control_bridge_stats_t *stats, const char *message) { + if (stats == NULL) { + return; + } + pthread_mutex_lock(&stats->mutex); + snprintf(stats->last_reconnect_reason, sizeof(stats->last_reconnect_reason), "%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->reconnect_count = stats->reconnect_count; + out_stats->server_idle_ms = stats->server_idle_ms; + out_stats->registered = stats->registered; + snprintf(out_stats->last_error, sizeof(out_stats->last_error), "%s", stats->last_error); + snprintf(out_stats->last_reconnect_reason, sizeof(out_stats->last_reconnect_reason), "%s", stats->last_reconnect_reason); + out_stats->transport = stats->transport; + pthread_mutex_unlock(&stats->mutex); +} + +static int control_server_error_requires_reconnect(const char *message) { + if (message == NULL || message[0] == '\0') { + return 0; + } + return strstr(message, "not registered") != NULL + || strstr(message, "first message must be register") != NULL + || strstr(message, "peer replaced") != NULL + || strstr(message, "timed out waiting for server_register_ok") != NULL; +} + +static void control_message_body_to_cstr(const message_t *msg, char *buffer, size_t buffer_len) { + size_t copy_len; + + if (buffer == NULL || buffer_len == 0) { + return; + } + buffer[0] = '\0'; + if (msg == NULL || msg->body == NULL || msg->body_len == 0) { + return; + } + copy_len = msg->body_len < (buffer_len - 1U) ? msg->body_len : (buffer_len - 1U); + memcpy(buffer, msg->body, copy_len); + buffer[copy_len] = '\0'; +} + +static kcp_client_t *connect_control_ack_client(const daemon_state_t *state) { + kcp_conn_options_t options; + + if (state == NULL || state->control_ack_peer_id == NULL || state->control_ack_peer_id[0] == '\0') { + errno = EINVAL; + return NULL; + } + kcp_conn_options_set_control_defaults(&options); + return kcp_client_dial_with_options( + state->control_server_addr, + state->control_relay_via, + state->control_ack_peer_id, + state->control_bind_ip, + state->control_bind_device, + &options, + NULL, + NULL, + state->stats_logger, + state->stats_interval_ms + ); +} + +static void close_control_ack_client(kcp_client_t **client_ptr) { + if (client_ptr == NULL || *client_ptr == NULL) { + return; + } + kcp_client_close(*client_ptr); + kcp_client_free(*client_ptr); + *client_ptr = NULL; +} + +static void control_ack_manager_request_connect(daemon_state_t *state) { + if (state == NULL || !control_ack_enabled(state) || !state->control_ack_thread_started) { + return; + } + pthread_mutex_lock(&state->control_ack_mutex); + if (state->control_ack_client == NULL) { + state->control_ack_connect_requested = 1; + } + pthread_mutex_unlock(&state->control_ack_mutex); +} + +static void *control_ack_thread_main(void *arg) { + daemon_state_t *state = (daemon_state_t *) arg; + + while (!atomic_load(&state->control_ack_shutdown_requested) && !*state->stop_requested) { + kcp_client_t *client = NULL; + int connect_failed = 0; + int should_connect = 0; + + pthread_mutex_lock(&state->control_ack_mutex); + if (state->control_ack_connect_requested && state->control_ack_client == NULL && !state->control_ack_connect_inflight) { + state->control_ack_connect_inflight = 1; + should_connect = 1; + } + pthread_mutex_unlock(&state->control_ack_mutex); + + if (!should_connect) { + usleep(200000); + continue; + } + + client = connect_control_ack_client(state); + connect_failed = client == NULL; + + pthread_mutex_lock(&state->control_ack_mutex); + state->control_ack_connect_inflight = 0; + if ( + client != NULL + && state->control_ack_connect_requested + && state->control_ack_client == NULL + && !atomic_load(&state->control_ack_shutdown_requested) + && !*state->stop_requested + ) { + state->control_ack_client = client; + state->control_ack_connect_requested = 0; + client = NULL; + } + pthread_mutex_unlock(&state->control_ack_mutex); + + if (client != NULL) { + close_control_ack_client(&client); + } + if (connect_failed && !atomic_load(&state->control_ack_shutdown_requested) && !*state->stop_requested) { + sleep(1); + } + } + return NULL; +} + +static void maybe_send_control_ack( + daemon_state_t *state, + const message_t *msg, + int64_t recv_unix_nano, + int64_t persist_end_unix_nano, + const char *sample_reason +) { + kcp_client_t *ack_client = NULL; + kcp_client_t *client_to_close = NULL; + char *payload = NULL; + int send_rc = -1; + + if ( + state == NULL || msg == NULL || recv_unix_nano <= 0 || persist_end_unix_nano <= recv_unix_nano + || !control_ack_enabled(state) || !state->control_ack_thread_started + ) { + return; + } + + payload = omni_strdup_printf( + "{\"message_id\":%" PRIu64 ",\"ack_phase\":\"persist_end\",\"b_recv_to_persist_us\":%" PRId64 ",\"unix_send_ok\":true,\"sample_reason\":\"%s\"}", + msg->id, + (persist_end_unix_nano - recv_unix_nano) / 1000, + sample_reason == NULL ? "sample_mod" : sample_reason + ); + if (payload == NULL) { + return; + } + + pthread_mutex_lock(&state->control_ack_mutex); + ack_client = state->control_ack_client; + if (ack_client == NULL) { + state->control_ack_connect_requested = 1; + pthread_mutex_unlock(&state->control_ack_mutex); + free(payload); + return; + } + send_rc = kcp_client_send_text(ack_client, state->control_ack_target_peer, payload); + if (send_rc != 0) { + client_to_close = state->control_ack_client; + state->control_ack_client = NULL; + state->control_ack_connect_requested = 1; + } + pthread_mutex_unlock(&state->control_ack_mutex); + + free(payload); + if (client_to_close != NULL) { + close_control_ack_client(&client_to_close); + } +} + +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_path, sizeof(client->dest_path), "%s", dest_path); + 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 int unix_dgram_client_reopen(unix_dgram_client_t *client) { + char dest_path[sizeof(client->dest_path)]; + + if (client == NULL || client->dest_path[0] == '\0') { + errno = EINVAL; + return -1; + } + snprintf(dest_path, sizeof(dest_path), "%s", client->dest_path); + unix_dgram_client_close(client); + return unix_dgram_client_init(client, dest_path); +} + +static int unix_dgram_client_should_reopen(int error_code) { + return error_code == ENOENT || error_code == ECONNREFUSED || error_code == EBADF || error_code == ENOTCONN; +} + +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) { + update_thread_heartbeat(&state->video_thread_heartbeat_epoch_sec); + maybe_inject_thread_stall(state, state->video_thread_fault_file, "video"); + int video_rc = video_pipeline_run(&state->video_config, &state->video_stats, state->stop_requested); + update_thread_heartbeat(&state->video_thread_heartbeat_epoch_sec); + + if (video_rc == 0) { + break; + } + if (video_rc == VIDEO_PIPELINE_RUN_RETRY_IMMEDIATE) { + continue; + } + 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; + int reconnect_immediately = 0; + + update_thread_heartbeat(&state->control_thread_heartbeat_epoch_sec); + maybe_inject_thread_stall(state, state->control_thread_fault_file, "control"); + 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, + state->stats_logger, + state->stats_interval_ms + ); + if (client == NULL) { + control_bridge_set_errno_error(&state->control_stats, "failed to connect control session"); + sleep(1); + continue; + } + + { + kcp_client_state_t client_state; + + memset(&client_state, 0, sizeof(client_state)); + kcp_client_state_snapshot(client, &client_state); + pthread_mutex_lock(&state->control_stats.mutex); + if (state->control_stats.ever_connected) { + state->control_stats.reconnect_count += 1; + } else { + state->control_stats.ever_connected = 1; + } + state->control_stats.registered = client_state.registered; + state->control_stats.server_idle_ms = client_state.server_idle_ms; + state->control_stats.last_reconnect_reason[0] = '\0'; + snprintf(state->control_stats.last_error, sizeof(state->control_stats.last_error), "%s", client_state.last_server_error); + kcp_client_runtime_stats_snapshot(client, &state->control_stats.transport); + pthread_mutex_unlock(&state->control_stats.mutex); + } + control_ack_manager_request_connect(state); + + while (!*state->stop_requested) { + message_t msg; + int rc; + kcp_client_state_t client_state; + int ack_sampled = 0; + int log_control_latency = 0; + int64_t recv_unix_nano = 0; + int64_t persist_begin_unix_nano = 0; + int64_t persist_end_unix_nano = 0; + + update_thread_heartbeat(&state->control_thread_heartbeat_epoch_sec); + protocol_message_init(&msg); + rc = kcp_client_receive_timed(client, &msg, 100); + update_thread_heartbeat(&state->control_thread_heartbeat_epoch_sec); + if (rc == 1) { + char reconnect_reason[256]; + + protocol_message_clear(&msg); + memset(&client_state, 0, sizeof(client_state)); + kcp_client_state_snapshot(client, &client_state); + pthread_mutex_lock(&state->control_stats.mutex); + state->control_stats.registered = client_state.registered; + state->control_stats.server_idle_ms = client_state.server_idle_ms; + snprintf(state->control_stats.last_error, sizeof(state->control_stats.last_error), "%s", client_state.last_server_error); + kcp_client_runtime_stats_snapshot(client, &state->control_stats.transport); + pthread_mutex_unlock(&state->control_stats.mutex); + if (!client_state.registered) { + snprintf(reconnect_reason, sizeof(reconnect_reason), "control session stale: server reported unregistered"); + } else if ( + state->control_server_idle_reconnect_ms > 0 + && client_state.server_idle_ms >= (uint32_t) state->control_server_idle_reconnect_ms + ) { + snprintf( + reconnect_reason, + sizeof(reconnect_reason), + "control session stale: server idle timeout (%u ms >= %d ms)", + client_state.server_idle_ms, + state->control_server_idle_reconnect_ms + ); + } else if (control_server_error_requires_reconnect(client_state.last_server_error)) { + snprintf( + reconnect_reason, + sizeof(reconnect_reason), + "control session stale: server error %.180s", + client_state.last_server_error + ); + } else { + reconnect_reason[0] = '\0'; + } + if (reconnect_reason[0] != '\0') { + control_bridge_set_error(&state->control_stats, reconnect_reason); + control_bridge_set_reconnect_reason(&state->control_stats, reconnect_reason); + fprintf(stderr, "[b_side_omnid] %s\n", reconnect_reason); + reconnect_immediately = 1; + break; + } + continue; + } + if (rc != 0) { + memset(&client_state, 0, sizeof(client_state)); + kcp_client_state_snapshot(client, &client_state); + pthread_mutex_lock(&state->control_stats.mutex); + state->control_stats.registered = client_state.registered; + state->control_stats.server_idle_ms = client_state.server_idle_ms; + kcp_client_runtime_stats_snapshot(client, &state->control_stats.transport); + pthread_mutex_unlock(&state->control_stats.mutex); + if (client_state.last_server_error[0] != '\0') { + control_bridge_set_error(&state->control_stats, client_state.last_server_error); + if (control_server_error_requires_reconnect(client_state.last_server_error)) { + control_bridge_set_reconnect_reason(&state->control_stats, client_state.last_server_error); + reconnect_immediately = 1; + } + } else { + control_bridge_set_errno_error(&state->control_stats, "control receive loop stopped"); + } + protocol_message_clear(&msg); + break; + } + + if (msg.type == MSG_TYPE_ERROR && strcmp(msg.from, SERVER_PEER_ID) == 0) { + char server_error[256]; + + control_message_body_to_cstr(&msg, server_error, sizeof(server_error)); + control_bridge_set_error(&state->control_stats, server_error); + if (control_server_error_requires_reconnect(server_error)) { + char reconnect_reason[256]; + + snprintf(reconnect_reason, sizeof(reconnect_reason), "control session stale: server error %.180s", server_error); + control_bridge_set_reconnect_reason(&state->control_stats, reconnect_reason); + fprintf(stderr, "[b_side_omnid] %s\n", reconnect_reason); + reconnect_immediately = 1; + protocol_message_clear(&msg); + break; + } + protocol_message_clear(&msg); + continue; + } + 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 (handle_camera_select_message(state, &msg)) { + pthread_mutex_lock(&state->control_stats.mutex); + state->control_stats.packets_forwarded += 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; + } + + ack_sampled = should_send_control_ack(state, &msg); + log_control_latency = ack_sampled || should_log_control_latency(state, &msg); + if (log_control_latency) { + recv_unix_nano = omni_now_unix_nano(); + persist_begin_unix_nano = recv_unix_nano; + latencylog_log_message_event_at( + state->control_latency_logger, + OMNI_NODE_ROLE_PEER, + state->control_peer_id, + EVENT_B_APP_RECV, + recv_unix_nano, + &msg + ); + latencylog_log_message_event_at( + state->control_latency_logger, + OMNI_NODE_ROLE_PEER, + state->control_peer_id, + EVENT_B_PERSIST_BEGIN, + persist_begin_unix_nano, + &msg + ); + } + + if (unix_dgram_client_send(&state->unix_client, msg.body, msg.body_len) != 0) { + int send_errno = errno; + int recovered = 0; + + if (unix_dgram_client_should_reopen(send_errno) && unix_dgram_client_reopen(&state->unix_client) == 0) { + recovered = unix_dgram_client_send(&state->unix_client, msg.body, msg.body_len) == 0; + } + if (recovered) { + memset(&client_state, 0, sizeof(client_state)); + kcp_client_state_snapshot(client, &client_state); + pthread_mutex_lock(&state->control_stats.mutex); + state->control_stats.packets_forwarded += 1; + state->control_stats.registered = client_state.registered; + state->control_stats.server_idle_ms = client_state.server_idle_ms; + kcp_client_runtime_stats_snapshot(client, &state->control_stats.transport); + pthread_mutex_unlock(&state->control_stats.mutex); + if (log_control_latency) { + persist_end_unix_nano = omni_now_unix_nano(); + latencylog_log_message_event_at( + state->control_latency_logger, + OMNI_NODE_ROLE_PEER, + state->control_peer_id, + EVENT_B_PERSIST_END, + persist_end_unix_nano, + &msg + ); + } + if (ack_sampled) { + maybe_send_control_ack(state, &msg, recv_unix_nano, persist_end_unix_nano, "sample_mod"); + } + protocol_message_clear(&msg); + continue; + } + errno = send_errno; + 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; + } + + memset(&client_state, 0, sizeof(client_state)); + kcp_client_state_snapshot(client, &client_state); + pthread_mutex_lock(&state->control_stats.mutex); + state->control_stats.packets_forwarded += 1; + state->control_stats.registered = client_state.registered; + state->control_stats.server_idle_ms = client_state.server_idle_ms; + kcp_client_runtime_stats_snapshot(client, &state->control_stats.transport); + pthread_mutex_unlock(&state->control_stats.mutex); + if (log_control_latency) { + persist_end_unix_nano = omni_now_unix_nano(); + latencylog_log_message_event_at( + state->control_latency_logger, + OMNI_NODE_ROLE_PEER, + state->control_peer_id, + EVENT_B_PERSIST_END, + persist_end_unix_nano, + &msg + ); + } + if (ack_sampled) { + maybe_send_control_ack(state, &msg, recv_unix_nano, persist_end_unix_nano, "sample_mod"); + } + protocol_message_clear(&msg); + } + + pthread_mutex_lock(&state->control_stats.mutex); + state->control_stats.registered = 0; + state->control_stats.server_idle_ms = 0; + pthread_mutex_unlock(&state->control_stats.mutex); + control_ack_manager_reset(state, 0); + kcp_client_close(client); + kcp_client_free(client); + if (!*state->stop_requested && !reconnect_immediately) { + 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 registered=%d frames=%llu bytes=%llu drops=%llu resets=%llu backlog=%u cap2send=%ums avg=%.1fms reason=%s srtt=%dms | control registered=%d idle=%ums reconnects=%llu forwarded=%llu invalid=%llu unix_err=%llu srtt=%dms last_reconnect=%s\n", + video_stats.connected, + (unsigned long long) video_stats.frames_sent, + (unsigned long long) video_stats.bytes_sent, + (unsigned long long) video_stats.backpressure_drops, + (unsigned long long) video_stats.backlog_resets, + video_stats.last_backlog_segments, + video_stats.last_capture_to_send_ms, + video_stats.avg_capture_to_send_ms, + video_stats.last_backlog_reason[0] == '\0' ? "-" : video_stats.last_backlog_reason, + video_stats.transport.srtt_ms, + control_stats.registered, + control_stats.server_idle_ms, + (unsigned long long) control_stats.reconnect_count, + (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, + control_stats.last_reconnect_reason[0] == '\0' ? "-" : control_stats.last_reconnect_reason + ); +} + +int main(void) { + daemon_state_t state; + pthread_t video_thread; + pthread_t control_thread; + long initial_heartbeat; + + 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); + atomic_init( + &state.active_camera, + strcmp(env_or_default("OMNI_CAMERA_ACTIVE", "head"), "waist") == 0 + ? VIDEO_CAMERA_WAIST + : VIDEO_CAMERA_HEAD + ); + state.video_config.active_camera = &state.active_camera; + 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_ack_peer_id = env_or_default("OMNI_CONTROL_ACK_PEER_ID", CONTROL_ACK_DEFAULT_PEER_ID); + state.control_ack_target_peer = env_or_default("OMNI_CONTROL_ACK_TARGET_PEER", CONTROL_ACK_DEFAULT_TARGET_PEER); + state.control_unix_socket = env_or_default("OMNI_CONTROL_UNIX_SOCKET_PATH", CONTROL_DEFAULT_UNIX_SOCKET); + state.runtime_dir = env_or_default("BLITZ_RUNTIME_DIR", DEFAULT_RUNTIME_DIR); + state.heartbeat_timeout_sec = env_int_or_default( + "BLITZ_OMNID_THREAD_HEARTBEAT_TIMEOUT_SEC", + DEFAULT_THREAD_HEARTBEAT_TIMEOUT_SEC + ); + state.stats_interval_ms = env_int_or_default("BLITZ_KCP_STATS_INTERVAL_MS", DEFAULT_KCP_STATS_INTERVAL_MS); + state.control_latency_sample_mod = env_u64_or_default("BLITZ_CONTROL_LATENCY_LOG_SAMPLE_MOD", DEFAULT_CONTROL_LATENCY_SAMPLE_MOD); + state.control_ack_sample_mod = env_u64_or_default("BLITZ_CONTROL_ACK_SAMPLE_MOD", DEFAULT_CONTROL_ACK_SAMPLE_MOD); + state.video_config.progress_callback = video_pipeline_heartbeat_progress; + state.video_config.progress_context = &state.video_thread_heartbeat_epoch_sec; + state.video_config.stats_logger = NULL; + state.video_config.stage_logger = NULL; + state.video_config.stats_interval_ms = state.stats_interval_ms; + state.control_server_idle_reconnect_ms = env_int_or_default( + "OMNI_CONTROL_SERVER_IDLE_RECONNECT_MS", + CONTROL_DEFAULT_SERVER_IDLE_RECONNECT_MS + ); + snprintf(state.status_file_path, sizeof(state.status_file_path), "%s/%s", state.runtime_dir, DEFAULT_STATUS_FILE_NAME); + snprintf( + state.video_thread_fault_file, + sizeof(state.video_thread_fault_file), + "%s/%s", + state.runtime_dir, + DEFAULT_VIDEO_THREAD_FAULT_FILE + ); + snprintf( + state.control_thread_fault_file, + sizeof(state.control_thread_fault_file), + "%s/%s", + state.runtime_dir, + DEFAULT_CONTROL_THREAD_FAULT_FILE + ); + initial_heartbeat = realtime_epoch_sec(); + atomic_init(&state.video_thread_heartbeat_epoch_sec, initial_heartbeat); + atomic_init(&state.control_thread_heartbeat_epoch_sec, initial_heartbeat); + + 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 (control_ack_manager_init(&state) != 0) { + perror("control_ack_manager_init"); + control_bridge_stats_destroy(&state.control_stats); + 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_ack_manager_destroy(&state); + 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_ack_manager_destroy(&state); + control_bridge_stats_destroy(&state.control_stats); + video_pipeline_stats_destroy(&state.video_stats); + return 1; + } + + { + const char *stats_log_path = getenv("BLITZ_KCP_STATS_LOG_PATH"); + const char *latency_log_path = getenv("BLITZ_CONTROL_LATENCY_LOG_PATH"); + const char *video_stage_log_path = getenv("BLITZ_VIDEO_STAGE_LOG_PATH"); + int latency_enabled = env_int_or_default("BLITZ_CONTROL_LATENCY_LOG_ENABLED", 1); + int video_stage_log_enabled = env_int_or_default("BLITZ_VIDEO_STAGE_LOG_ENABLED", 1); + uint64_t video_stage_log_sample_mod = env_u64_or_default("BLITZ_VIDEO_STAGE_LOG_SAMPLE_MOD", 10); + + if (stats_log_path != NULL && stats_log_path[0] != '\0') { + state.stats_logger = kcp_session_stats_open_jsonl(stats_log_path); + if (state.stats_logger == NULL) { + fprintf(stderr, "[b_side_omnid] warning: failed to open KCP stats log %s\n", stats_log_path); + } + } + if (latency_enabled && latency_log_path != NULL && latency_log_path[0] != '\0') { + state.control_latency_logger = latencylog_open_jsonl(latency_log_path); + if (state.control_latency_logger == NULL) { + fprintf(stderr, "[b_side_omnid] warning: failed to open control latency log %s\n", latency_log_path); + } + } + if (video_stage_log_enabled && video_stage_log_path != NULL && video_stage_log_path[0] != '\0') { + state.video_stage_logger = video_stage_logger_open_jsonl(video_stage_log_path, video_stage_log_sample_mod); + if (state.video_stage_logger == NULL) { + fprintf(stderr, "[b_side_omnid] warning: failed to open video stage log %s\n", video_stage_log_path); + } + } + state.video_config.stats_logger = state.stats_logger; + state.video_config.stage_logger = state.video_stage_logger; + state.video_config.stats_interval_ms = state.stats_interval_ms; + } + + if (control_ack_enabled(&state)) { + if (pthread_create(&state.control_ack_thread, NULL, control_ack_thread_main, &state) != 0) { + fprintf(stderr, "[b_side_omnid] warning: failed to start async control ACK manager, ACK sampling disabled\n"); + } else { + state.control_ack_thread_started = 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_ack_manager_destroy(&state); + control_bridge_stats_destroy(&state.control_stats); + video_pipeline_stats_destroy(&state.video_stats); + latencylog_close(state.control_latency_logger); + video_stage_logger_close(state.video_stage_logger); + kcp_session_stats_close(state.stats_logger); + 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_ack_manager_destroy(&state); + control_bridge_stats_destroy(&state.control_stats); + video_pipeline_stats_destroy(&state.video_stats); + latencylog_close(state.control_latency_logger); + video_stage_logger_close(state.video_stage_logger); + kcp_session_stats_close(state.stats_logger); + return 1; + } + + while (!g_stop_requested) { + sleep(1); + print_stats(&state); + if (write_daemon_status_file(&state) != 0) { + fprintf(stderr, "[b_side_omnid] failed to write status file %s: %s\n", state.status_file_path, strerror(errno)); + } + exit_if_thread_stalled(&state); + } + + pthread_join(video_thread, NULL); + pthread_join(control_thread, NULL); + unix_dgram_client_close(&state.unix_client); + control_ack_manager_destroy(&state); + control_bridge_stats_destroy(&state.control_stats); + video_pipeline_stats_destroy(&state.video_stats); + latencylog_close(state.control_latency_logger); + video_stage_logger_close(state.video_stage_logger); + kcp_session_stats_close(state.stats_logger); + return 0; +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/cmd/kcppeer.c b/robot/ros2/OmniSocketGo_robot_ros/cmd/kcppeer.c new file mode 100644 index 0000000..e71981c --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/cmd/kcppeer.c @@ -0,0 +1,352 @@ +#include "cli_parse.h" +#include "interactive.h" +#include "peer_kcp_client.h" + +#include +#include + +typedef struct kcppeer_receive_ctx { + kcp_client_t *client; + const char *inbox_dir; + volatile int stop_requested; + int rc; +} kcppeer_receive_ctx_t; + +static void kcppeer_usage(FILE *out) { + fprintf(out, "usage: kcppeer [-id peer-a] [-server 127.0.0.1:9002] [-relay-via addr]\n"); + fprintf(out, " [-to peer] [-text msg | -file path] [-bind-ip ip] [-bind-device dev]\n"); + fprintf(out, " [-inbox-dir dir] [-latency-log path] [-kcp-ts-debug-log path]\n"); + fprintf(out, " [-kcp-session-stats-log path] [-kcp-session-stats-interval 100ms]\n"); + fprintf(out, " [-interactive[=true|false]]\n"); +} + +static void *kcppeer_receive_thread_main(void *arg) { + kcppeer_receive_ctx_t *ctx = (kcppeer_receive_ctx_t *) arg; + + for (;;) { + message_t msg; + char persisted_path[512]; + + protocol_message_init(&msg); + if (kcp_client_receive(ctx->client, &msg) != 0) { + protocol_message_clear(&msg); + ctx->rc = ctx->stop_requested ? 0 : -1; + return NULL; + } + + switch (msg.type) { + case MSG_TYPE_TEXT: + if (kcp_client_persist_message(ctx->client, &msg, ctx->inbox_dir, persisted_path, sizeof(persisted_path)) != 0) { + fprintf(stderr, "kcppeer: persist text from %s to %s failed\n", msg.from, msg.to); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + fprintf(stderr, "received text from %s to %s and persisted to %s\n", msg.from, msg.to, persisted_path); + break; + case MSG_TYPE_FILE: + if (kcp_client_persist_message(ctx->client, &msg, ctx->inbox_dir, persisted_path, sizeof(persisted_path)) != 0) { + fprintf(stderr, "kcppeer: persist file from %s to %s failed\n", msg.from, msg.to); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + fprintf(stderr, "received file from %s to %s: %s (%lu bytes) -> %s\n", msg.from, msg.to, msg.file_name, (unsigned long) msg.body_len, persisted_path); + break; + case MSG_TYPE_BINARY: + if (kcp_client_persist_message(ctx->client, &msg, ctx->inbox_dir, persisted_path, sizeof(persisted_path)) != 0) { + fprintf(stderr, "kcppeer: persist binary payload from %s to %s failed\n", msg.from, msg.to); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + fprintf(stderr, "received binary payload from %s to %s (%lu bytes) -> %s\n", msg.from, msg.to, (unsigned long) msg.body_len, persisted_path); + break; + case MSG_TYPE_ERROR: + fprintf(stderr, "received error from %s to %s: %.*s\n", msg.from, msg.to, (int) msg.body_len, msg.body == NULL ? "" : (const char *) msg.body); + break; + default: + fprintf(stderr, "received unexpected message type %s from %s\n", protocol_message_type_name(msg.type), msg.from); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + protocol_message_clear(&msg); + } +} + +int main(int argc, char **argv) { + const char *peer_id = "peer-a"; + const char *server_addr = "127.0.0.1:9002"; + const char *relay_via = ""; + const char *actual_dial_target; + const char *target_peer = ""; + const char *text = ""; + const char *file_path = ""; + const char *bind_ip = ""; + const char *bind_device = ""; + const char *inbox_dir = "inbox"; + const char *latency_log_path = ""; + const char *packet_log_path = ""; + const char *stats_log_path = ""; + const char *stats_interval_raw = ""; + int stats_interval_ms = KCP_DEFAULT_STATS_INTERVAL_MS; + int interactive = 1; + latency_logger_t *latency_logger = NULL; + kcp_packet_debug_logger_t *packet_logger = NULL; + kcp_session_stats_logger_t *stats_logger = NULL; + kcp_client_t *client = NULL; + kcppeer_receive_ctx_t receive_ctx; + pthread_t receive_thread; + int receive_thread_started = 0; + int i; + int rc = 1; + + memset(&receive_ctx, 0, sizeof(receive_ctx)); + + for (i = 1; i < argc; ++i) { + const char *value = NULL; + int handled; + + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-id", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -id requires a value\n"); + return 1; + } else if (handled) { + peer_id = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-server", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -server requires a value\n"); + return 1; + } else if (handled) { + server_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-relay-via", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -relay-via requires a value\n"); + return 1; + } else if (handled) { + relay_via = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-to", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -to requires a value\n"); + return 1; + } else if (handled) { + target_peer = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-text", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -text requires a value\n"); + return 1; + } else if (handled) { + text = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-file", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -file requires a value\n"); + return 1; + } else if (handled) { + file_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-bind-ip", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -bind-ip requires a value\n"); + return 1; + } else if (handled) { + bind_ip = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-bind-device", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -bind-device requires a value\n"); + return 1; + } else if (handled) { + bind_device = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-inbox-dir", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -inbox-dir requires a value\n"); + return 1; + } else if (handled) { + inbox_dir = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-latency-log", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -latency-log requires a value\n"); + return 1; + } else if (handled) { + latency_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-kcp-ts-debug-log", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -kcp-ts-debug-log requires a value\n"); + return 1; + } else if (handled) { + packet_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-kcp-session-stats-log", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -kcp-session-stats-log requires a value\n"); + return 1; + } else if (handled) { + stats_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-kcp-session-stats-interval", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -kcp-session-stats-interval requires a value\n"); + return 1; + } else if (handled) { + stats_interval_raw = value; + continue; + } + if ((handled = cli_parse_bool_flag(argv[i], "-interactive", &interactive)) < 0) { + fprintf(stderr, "kcppeer: invalid -interactive value\n"); + return 1; + } else if (handled) { + continue; + } + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + kcppeer_usage(stdout); + return 0; + } + fprintf(stderr, "kcppeer: unknown argument %s\n", argv[i]); + kcppeer_usage(stderr); + return 1; + } + + if (text[0] != '\0' && file_path[0] != '\0') { + fprintf(stderr, "kcppeer: only one of -text or -file may be specified\n"); + return 1; + } + if ((text[0] != '\0' || file_path[0] != '\0') && target_peer[0] == '\0') { + fprintf(stderr, "kcppeer: flag -to is required when sending text or file\n"); + return 1; + } + if (kcp_session_stats_parse_interval_ms(stats_interval_raw, &stats_interval_ms) != 0) { + fprintf(stderr, "kcppeer: invalid -kcp-session-stats-interval value %s\n", stats_interval_raw); + return 1; + } + + if (latency_log_path[0] != '\0') { + latency_logger = latencylog_open_jsonl(latency_log_path); + if (latency_logger == NULL) { + fprintf(stderr, "kcppeer: open latency logger %s failed\n", latency_log_path); + goto cleanup; + } + } + if (packet_log_path[0] != '\0') { + packet_logger = kcp_packet_debug_open_jsonl(packet_log_path); + if (packet_logger == NULL) { + fprintf(stderr, "kcppeer: open kcp packet debug logger %s failed\n", packet_log_path); + goto cleanup; + } + } + if (stats_log_path[0] != '\0') { + stats_logger = kcp_session_stats_open_jsonl(stats_log_path); + if (stats_logger == NULL) { + fprintf(stderr, "kcppeer: open kcp session stats logger %s failed\n", stats_log_path); + goto cleanup; + } + } + + actual_dial_target = relay_via[0] != '\0' ? relay_via : server_addr; + client = kcp_client_dial(server_addr, relay_via, peer_id, bind_ip, bind_device, latency_logger, packet_logger, stats_logger, stats_interval_ms); + if (client == NULL) { + int saved_errno = errno; + const char *reason = saved_errno != 0 ? strerror(saved_errno) : "unknown error"; + if (relay_via[0] != '\0') { + fprintf(stderr, "kcppeer: dial target %s failed (logical server %s): %s (errno=%d)\n", actual_dial_target, server_addr, reason, saved_errno); + } else { + fprintf(stderr, "kcppeer: dial kcp server %s failed: %s (errno=%d)\n", server_addr, reason, saved_errno); + } + goto cleanup; + } + if (relay_via[0] != '\0') { + fprintf(stderr, "opened KCP session as %s; logical server=%s, actual dial target=%s via relay; registration confirmed\n", kcp_client_id(client), server_addr, actual_dial_target); + } else { + fprintf(stderr, "opened KCP session as %s; logical server=%s, actual dial target=%s; registration confirmed\n", kcp_client_id(client), server_addr, actual_dial_target); + } + + receive_ctx.client = client; + receive_ctx.inbox_dir = inbox_dir; + if (pthread_create(&receive_thread, NULL, kcppeer_receive_thread_main, &receive_ctx) != 0) { + fprintf(stderr, "kcppeer: create receive thread failed\n"); + goto cleanup; + } + receive_thread_started = 1; + + if (target_peer[0] != '\0' && text[0] != '\0') { + if (kcp_client_send_text(client, target_peer, text) != 0) { + fprintf(stderr, "kcppeer: send text to %s failed\n", target_peer); + goto cleanup; + } + fprintf(stderr, "sent text to %s\n", target_peer); + } + if (target_peer[0] != '\0' && file_path[0] != '\0') { + if (kcp_client_send_file_path(client, target_peer, file_path) != 0) { + fprintf(stderr, "kcppeer: send file %s to %s failed\n", file_path, target_peer); + goto cleanup; + } + fprintf(stderr, "sent file %s to %s\n", file_path, target_peer); + } + + if (interactive) { + char line[2048]; + char prompt[128]; + + snprintf(prompt, sizeof(prompt), "%s> ", kcp_client_id(client)); + interactive_print_help(stdout, "KCP"); + while (fputs(prompt, stdout) >= 0 && fflush(stdout) == 0 && fgets(line, sizeof(line), stdin) != NULL) { + interactive_command_t command; + char err[128]; + + omni_trim_newline(line); + if (interactive_parse_command(line, &command, err, sizeof(err)) != 0) { + if (strstr(err, "empty command") == NULL) { + fprintf(stderr, "%s\n", err); + } + continue; + } + if (command.type == INTERACTIVE_CMD_HELP) { + interactive_print_help(stdout, "KCP"); + continue; + } + if (command.type == INTERACTIVE_CMD_QUIT) { + break; + } + if (command.type == INTERACTIVE_CMD_TEXT) { + if (kcp_client_send_text(client, command.to, command.value) != 0) { + fprintf(stderr, "kcppeer: send text to %s failed\n", command.to); + continue; + } + fprintf(stderr, "sent text to %s\n", command.to); + continue; + } + if (command.type == INTERACTIVE_CMD_FILE) { + if (kcp_client_send_file_path(client, command.to, command.value) != 0) { + fprintf(stderr, "kcppeer: send file %s to %s failed\n", command.value, command.to); + continue; + } + fprintf(stderr, "sent file %s to %s\n", command.value, command.to); + continue; + } + } + } + + rc = 0; + +cleanup: + receive_ctx.stop_requested = 1; + kcp_client_close(client); + if (receive_thread_started) { + pthread_join(receive_thread, NULL); + if (rc == 0 && receive_ctx.rc != 0) { + rc = 1; + } + } + kcp_client_free(client); + kcp_session_stats_close(stats_logger); + kcp_packet_debug_close(packet_logger); + latencylog_close(latency_logger); + return rc; +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/cmd/kcpping.c b/robot/ros2/OmniSocketGo_robot_ros/cmd/kcpping.c new file mode 100644 index 0000000..42442d8 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/cmd/kcpping.c @@ -0,0 +1,788 @@ +#include "cli_parse.h" +#include "peer_kcp_client.h" + +#include "cJSON.h" + +#include +#include + +typedef struct kcp_ping_message_node { + struct kcp_ping_message_node *next; + message_t msg; +} kcp_ping_message_node_t; + +typedef struct kcp_ping_receiver_ctx { + kcp_client_t *client; + pthread_mutex_t mu; + kcp_ping_message_node_t *head; + kcp_ping_message_node_t *tail; + volatile int stop_requested; + int closed; + int rc; +} kcp_ping_receiver_ctx_t; + +typedef struct kcp_pending_ping { + struct kcp_pending_ping *next; + uint64_t seq; + int64_t deadline_ns; +} kcp_pending_ping_t; + +typedef struct kcp_ping_tracker { + kcp_pending_ping_t *pending; + int pending_count; + int sent; + int duplicates; + uint64_t max_seq_sent; + int64_t *samples_ns; + size_t sample_count; + size_t sample_cap; +} kcp_ping_tracker_t; + +static volatile sig_atomic_t g_kcpping_stop = 0; + +static void kcpping_on_signal(int signo) { + (void) signo; + g_kcpping_stop = 1; +} + +static void kcpping_usage(FILE *out) { + fprintf(out, "usage: kcpping [-id pinger] [-server 127.0.0.1:9002] [-to peer] [-echo]\n"); + fprintf(out, " [-count 100] [-interval 100ms] [-size 64] [-timeout 3s]\n"); + fprintf(out, " [-bind-ip ip] [-bind-device dev] [-latency-log path]\n"); +} + +static int kcp_ping_compare_i64(const void *left, const void *right) { + const int64_t *a = (const int64_t *) left; + const int64_t *b = (const int64_t *) right; + if (*a < *b) { + return -1; + } + if (*a > *b) { + return 1; + } + return 0; +} + +static double kcp_ping_sqrt(double value) { + double x = value; + int i; + + if (value <= 0.0) { + return 0.0; + } + if (x < 1.0) { + x = 1.0; + } + for (i = 0; i < 16; ++i) { + x = 0.5 * (x + value / x); + } + return x; +} + +static int kcp_ping_build_payload(uint64_t seq, int64_t ts_ns, int size, char **out_body, size_t *out_len) { + cJSON *root = NULL; + char *json = NULL; + char *pad = NULL; + size_t base_len; + size_t pad_len; + + *out_body = NULL; + *out_len = 0; + + root = cJSON_CreateObject(); + if (root == NULL) { + return -1; + } + cJSON_AddNumberToObject(root, "seq", (double) seq); + cJSON_AddNumberToObject(root, "ts_ns", (double) ts_ns); + cJSON_AddStringToObject(root, "pad", ""); + json = cJSON_PrintUnformatted(root); + cJSON_Delete(root); + if (json == NULL) { + return -1; + } + base_len = strlen(json); + cJSON_free(json); + if ((int) base_len > size) { + errno = EMSGSIZE; + return -1; + } + + pad_len = (size_t) size - base_len; + pad = (char *) malloc(pad_len + 1U); + if (pad == NULL) { + return -1; + } + memset(pad, 'A', pad_len); + pad[pad_len] = '\0'; + + root = cJSON_CreateObject(); + if (root == NULL) { + free(pad); + return -1; + } + cJSON_AddNumberToObject(root, "seq", (double) seq); + cJSON_AddNumberToObject(root, "ts_ns", (double) ts_ns); + cJSON_AddStringToObject(root, "pad", pad); + free(pad); + json = cJSON_PrintUnformatted(root); + cJSON_Delete(root); + if (json == NULL) { + return -1; + } + if ((int) strlen(json) != size) { + cJSON_free(json); + errno = EINVAL; + return -1; + } + *out_body = json; + *out_len = (size_t) size; + return 0; +} + +static int kcp_ping_parse_payload(const uint8_t *body, size_t body_len, uint64_t *seq, int64_t *ts_ns) { + char *text; + cJSON *root; + const cJSON *seq_item; + const cJSON *ts_item; + + if (body == NULL || seq == NULL || ts_ns == NULL) { + errno = EINVAL; + return -1; + } + text = (char *) malloc(body_len + 1U); + if (text == NULL) { + return -1; + } + memcpy(text, body, body_len); + text[body_len] = '\0'; + root = cJSON_Parse(text); + free(text); + if (root == NULL) { + errno = EPROTO; + return -1; + } + seq_item = cJSON_GetObjectItemCaseSensitive(root, "seq"); + ts_item = cJSON_GetObjectItemCaseSensitive(root, "ts_ns"); + if (!cJSON_IsNumber(seq_item) || !cJSON_IsNumber(ts_item) || seq_item->valuedouble <= 0 || ts_item->valuedouble <= 0) { + cJSON_Delete(root); + errno = EPROTO; + return -1; + } + *seq = (uint64_t) seq_item->valuedouble; + *ts_ns = (int64_t) ts_item->valuedouble; + cJSON_Delete(root); + return 0; +} + +static void kcp_ping_receiver_ctx_init(kcp_ping_receiver_ctx_t *ctx, kcp_client_t *client) { + memset(ctx, 0, sizeof(*ctx)); + ctx->client = client; + pthread_mutex_init(&ctx->mu, NULL); +} + +static void kcp_ping_receiver_ctx_destroy(kcp_ping_receiver_ctx_t *ctx) { + kcp_ping_message_node_t *node; + kcp_ping_message_node_t *next; + + if (ctx == NULL) { + return; + } + for (node = ctx->head; node != NULL; node = next) { + next = node->next; + protocol_message_clear(&node->msg); + free(node); + } + pthread_mutex_destroy(&ctx->mu); +} + +static void *kcpping_receive_thread_main(void *arg) { + kcp_ping_receiver_ctx_t *ctx = (kcp_ping_receiver_ctx_t *) arg; + + for (;;) { + message_t msg; + kcp_ping_message_node_t *node; + + protocol_message_init(&msg); + if (kcp_client_receive(ctx->client, &msg) != 0) { + protocol_message_clear(&msg); + pthread_mutex_lock(&ctx->mu); + ctx->rc = ctx->stop_requested ? 0 : -1; + ctx->closed = 1; + pthread_mutex_unlock(&ctx->mu); + return NULL; + } + + node = (kcp_ping_message_node_t *) calloc(1, sizeof(*node)); + if (node == NULL) { + protocol_message_clear(&msg); + pthread_mutex_lock(&ctx->mu); + ctx->rc = -1; + ctx->closed = 1; + pthread_mutex_unlock(&ctx->mu); + return NULL; + } + node->msg = msg; + + pthread_mutex_lock(&ctx->mu); + if (ctx->tail == NULL) { + ctx->head = node; + } else { + ctx->tail->next = node; + } + ctx->tail = node; + pthread_mutex_unlock(&ctx->mu); + } +} + +static int kcp_ping_receiver_pop(kcp_ping_receiver_ctx_t *ctx, message_t *out_msg) { + kcp_ping_message_node_t *node; + + pthread_mutex_lock(&ctx->mu); + node = ctx->head; + if (node != NULL) { + ctx->head = node->next; + if (ctx->head == NULL) { + ctx->tail = NULL; + } + } + pthread_mutex_unlock(&ctx->mu); + + if (node == NULL) { + return 0; + } + *out_msg = node->msg; + free(node); + return 1; +} + +static int kcp_ping_receiver_status(kcp_ping_receiver_ctx_t *ctx, int *closed, int *rc) { + pthread_mutex_lock(&ctx->mu); + *closed = ctx->closed; + *rc = ctx->rc; + pthread_mutex_unlock(&ctx->mu); + return 0; +} + +static void kcp_ping_tracker_init(kcp_ping_tracker_t *tracker) { + memset(tracker, 0, sizeof(*tracker)); +} + +static void kcp_ping_tracker_destroy(kcp_ping_tracker_t *tracker) { + kcp_pending_ping_t *pending; + kcp_pending_ping_t *next; + + for (pending = tracker->pending; pending != NULL; pending = next) { + next = pending->next; + free(pending); + } + free(tracker->samples_ns); +} + +static int kcp_ping_tracker_mark_sent(kcp_ping_tracker_t *tracker, uint64_t seq, int64_t sent_at_ns, int64_t timeout_ns) { + kcp_pending_ping_t *pending = (kcp_pending_ping_t *) calloc(1, sizeof(*pending)); + if (pending == NULL) { + return -1; + } + pending->seq = seq; + pending->deadline_ns = sent_at_ns + timeout_ns; + pending->next = tracker->pending; + tracker->pending = pending; + tracker->pending_count++; + tracker->sent++; + tracker->max_seq_sent = seq; + return 0; +} + +static kcp_pending_ping_t *kcp_ping_tracker_find_pending(kcp_ping_tracker_t *tracker, uint64_t seq, kcp_pending_ping_t **out_prev) { + kcp_pending_ping_t *prev = NULL; + kcp_pending_ping_t *cur; + + for (cur = tracker->pending; cur != NULL; cur = cur->next) { + if (cur->seq == seq) { + if (out_prev != NULL) { + *out_prev = prev; + } + return cur; + } + prev = cur; + } + if (out_prev != NULL) { + *out_prev = NULL; + } + return NULL; +} + +static int kcp_ping_tracker_add_sample(kcp_ping_tracker_t *tracker, int64_t rtt_ns) { + int64_t *next_samples; + size_t next_cap; + + if (tracker->sample_count == tracker->sample_cap) { + next_cap = tracker->sample_cap == 0 ? 16U : tracker->sample_cap * 2U; + next_samples = (int64_t *) realloc(tracker->samples_ns, next_cap * sizeof(*next_samples)); + if (next_samples == NULL) { + return -1; + } + tracker->samples_ns = next_samples; + tracker->sample_cap = next_cap; + } + tracker->samples_ns[tracker->sample_count++] = rtt_ns; + return 0; +} + +static int kcp_ping_tracker_observe_reply(kcp_ping_tracker_t *tracker, uint64_t seq, int64_t sent_ts_ns, int64_t received_ts_ns, int *disposition, int64_t *rtt_ns) { + kcp_pending_ping_t *prev = NULL; + kcp_pending_ping_t *pending; + + if (seq == 0 || seq > tracker->max_seq_sent) { + *disposition = 2; + *rtt_ns = 0; + return 0; + } + pending = kcp_ping_tracker_find_pending(tracker, seq, &prev); + if (pending == NULL) { + tracker->duplicates++; + *disposition = 1; + *rtt_ns = 0; + return 0; + } + if (prev == NULL) { + tracker->pending = pending->next; + } else { + prev->next = pending->next; + } + tracker->pending_count--; + free(pending); + + *rtt_ns = received_ts_ns - sent_ts_ns; + if (*rtt_ns < 0) { + *rtt_ns = 0; + } + if (kcp_ping_tracker_add_sample(tracker, *rtt_ns) != 0) { + return -1; + } + *disposition = 0; + return 0; +} + +static void kcp_ping_tracker_expire(kcp_ping_tracker_t *tracker, int64_t now_ns, FILE *out) { + kcp_pending_ping_t *prev = NULL; + kcp_pending_ping_t *cur = tracker->pending; + + while (cur != NULL) { + if (cur->deadline_ns <= now_ns) { + kcp_pending_ping_t *next = cur->next; + fprintf(out, "seq=%" PRIu64 " timeout\n", cur->seq); + if (prev == NULL) { + tracker->pending = next; + } else { + prev->next = next; + } + free(cur); + tracker->pending_count--; + cur = next; + continue; + } + prev = cur; + cur = cur->next; + } +} + +static int64_t kcp_ping_percentile_ns(const int64_t *sorted, size_t count, double percentile) { + size_t index; + double raw_index; + + if (count == 0) { + return 0; + } + if (percentile <= 0.0) { + return sorted[0]; + } + if (percentile >= 1.0) { + return sorted[count - 1]; + } + raw_index = percentile * (double) count; + index = (size_t) raw_index; + if ((double) index < raw_index) { + index++; + } + if (index > 0) { + index--; + } + if (index >= count) { + index = count - 1; + } + return sorted[index]; +} + +static void kcp_ping_print_summary(FILE *out, const char *target, const kcp_ping_tracker_t *tracker) { + int received = (int) tracker->sample_count; + double loss_pct = tracker->sent == 0 ? 0.0 : ((double) (tracker->sent - received) * 100.0 / (double) tracker->sent); + + fprintf(out, "--- %s kcp ping statistics ---\n", target); + fprintf(out, "%d packets transmitted, %d received, %d duplicates, %.2f%% packet loss\n", tracker->sent, received, tracker->duplicates, loss_pct); + if (tracker->sample_count == 0) { + fprintf(out, "rtt min/avg/max/p50/p95/p99 = n/a/n/a/n/a/n/a/n/a/n/a, stddev=n/a\n"); + return; + } + + { + int64_t *sorted = (int64_t *) malloc(tracker->sample_count * sizeof(*sorted)); + size_t i; + double sum = 0.0; + double variance = 0.0; + double avg; + int64_t min_ns; + int64_t max_ns; + int64_t p50_ns; + int64_t p95_ns; + int64_t p99_ns; + + if (sorted == NULL) { + fprintf(out, "rtt summary unavailable: memory allocation failed\n"); + return; + } + memcpy(sorted, tracker->samples_ns, tracker->sample_count * sizeof(*sorted)); + qsort(sorted, tracker->sample_count, sizeof(*sorted), kcp_ping_compare_i64); + for (i = 0; i < tracker->sample_count; ++i) { + sum += (double) sorted[i]; + } + avg = sum / (double) tracker->sample_count; + for (i = 0; i < tracker->sample_count; ++i) { + double delta = (double) sorted[i] - avg; + variance += delta * delta; + } + variance /= (double) tracker->sample_count; + + min_ns = sorted[0]; + max_ns = sorted[tracker->sample_count - 1]; + p50_ns = kcp_ping_percentile_ns(sorted, tracker->sample_count, 0.50); + p95_ns = kcp_ping_percentile_ns(sorted, tracker->sample_count, 0.95); + p99_ns = kcp_ping_percentile_ns(sorted, tracker->sample_count, 0.99); + + fprintf( + out, + "rtt min/avg/max/p50/p95/p99 = %.2fms/%.2fms/%.2fms/%.2fms/%.2fms/%.2fms, stddev=%.2fms\n", + (double) min_ns / 1000000.0, + avg / 1000000.0, + (double) max_ns / 1000000.0, + (double) p50_ns / 1000000.0, + (double) p95_ns / 1000000.0, + (double) p99_ns / 1000000.0, + kcp_ping_sqrt(variance) / 1000000.0 + ); + free(sorted); + } +} + +static int kcp_ping_expiry_poll_ms(int timeout_ms) { + int interval = timeout_ms / 4; + if (interval < 10) { + return 10; + } + if (interval > 100) { + return 100; + } + return interval; +} + +int main(int argc, char **argv) { + const char *peer_id = "pinger"; + const char *server_addr = "127.0.0.1:9002"; + const char *target_peer = ""; + const char *bind_ip = ""; + const char *bind_device = ""; + const char *latency_log_path = ""; + int echo_mode = 0; + int count = 100; + int interval_ms = 100; + int size = 64; + int timeout_ms = 3000; + latency_logger_t *latency_logger = NULL; + kcp_client_t *client = NULL; + kcp_ping_receiver_ctx_t receiver_ctx; + pthread_t receiver_thread; + int receiver_ctx_initialized = 0; + int receiver_thread_started = 0; + kcp_ping_tracker_t tracker; + int i; + int rc = 1; + + kcp_ping_tracker_init(&tracker); + memset(&receiver_ctx, 0, sizeof(receiver_ctx)); + + for (i = 1; i < argc; ++i) { + const char *value = NULL; + int handled; + + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-id", &value)) < 0) { + fprintf(stderr, "kcpping: flag -id requires a value\n"); + return 1; + } else if (handled) { + peer_id = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-server", &value)) < 0) { + fprintf(stderr, "kcpping: flag -server requires a value\n"); + return 1; + } else if (handled) { + server_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-to", &value)) < 0) { + fprintf(stderr, "kcpping: flag -to requires a value\n"); + return 1; + } else if (handled) { + target_peer = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-count", &value)) < 0) { + fprintf(stderr, "kcpping: flag -count requires a value\n"); + return 1; + } else if (handled) { + count = atoi(value); + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-interval", &value)) < 0) { + fprintf(stderr, "kcpping: flag -interval requires a value\n"); + return 1; + } else if (handled) { + if (omni_parse_duration_ms(value, interval_ms, &interval_ms) != 0) { + fprintf(stderr, "kcpping: invalid -interval value %s\n", value); + return 1; + } + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-size", &value)) < 0) { + fprintf(stderr, "kcpping: flag -size requires a value\n"); + return 1; + } else if (handled) { + size = atoi(value); + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-timeout", &value)) < 0) { + fprintf(stderr, "kcpping: flag -timeout requires a value\n"); + return 1; + } else if (handled) { + if (omni_parse_duration_ms(value, timeout_ms, &timeout_ms) != 0) { + fprintf(stderr, "kcpping: invalid -timeout value %s\n", value); + return 1; + } + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-bind-ip", &value)) < 0) { + fprintf(stderr, "kcpping: flag -bind-ip requires a value\n"); + return 1; + } else if (handled) { + bind_ip = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-bind-device", &value)) < 0) { + fprintf(stderr, "kcpping: flag -bind-device requires a value\n"); + return 1; + } else if (handled) { + bind_device = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-latency-log", &value)) < 0) { + fprintf(stderr, "kcpping: flag -latency-log requires a value\n"); + return 1; + } else if (handled) { + latency_log_path = value; + continue; + } + if ((handled = cli_parse_bool_flag(argv[i], "-echo", &echo_mode)) < 0) { + fprintf(stderr, "kcpping: invalid -echo value\n"); + return 1; + } else if (handled) { + continue; + } + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + kcpping_usage(stdout); + return 0; + } + fprintf(stderr, "kcpping: unknown argument %s\n", argv[i]); + kcpping_usage(stderr); + return 1; + } + + if (peer_id[0] == '\0' || server_addr[0] == '\0') { + fprintf(stderr, "kcpping: flags -id and -server are required\n"); + return 1; + } + if (!echo_mode && target_peer[0] == '\0') { + fprintf(stderr, "kcpping: flag -to is required unless -echo is set\n"); + return 1; + } + if (count < 0 || interval_ms <= 0 || size <= 0 || timeout_ms <= 0) { + fprintf(stderr, "kcpping: invalid numeric flag value\n"); + return 1; + } + + signal(SIGINT, kcpping_on_signal); + + if (latency_log_path[0] != '\0') { + latency_logger = latencylog_open_jsonl(latency_log_path); + if (latency_logger == NULL) { + fprintf(stderr, "kcpping: open latency logger %s failed\n", latency_log_path); + goto cleanup; + } + } + client = kcp_client_dial(server_addr, NULL, peer_id, bind_ip, bind_device, latency_logger, NULL, NULL, KCP_DEFAULT_STATS_INTERVAL_MS); + if (client == NULL) { + fprintf(stderr, "kcpping: dial kcp server %s failed\n", server_addr); + goto cleanup; + } + + if (echo_mode) { + while (!g_kcpping_stop) { + message_t msg; + + protocol_message_init(&msg); + if (kcp_client_receive(client, &msg) != 0) { + protocol_message_clear(&msg); + if (g_kcpping_stop) { + break; + } + fprintf(stderr, "kcpping: receive failed in echo mode\n"); + goto cleanup; + } + if (msg.type == MSG_TYPE_TEXT) { + char *text = (char *) malloc(msg.body_len + 1U); + if (text == NULL) { + protocol_message_clear(&msg); + goto cleanup; + } + memcpy(text, msg.body, msg.body_len); + text[msg.body_len] = '\0'; + if (kcp_client_send_text(client, msg.from, text) != 0) { + free(text); + protocol_message_clear(&msg); + fprintf(stderr, "kcpping: echo send back to %s failed\n", msg.from); + goto cleanup; + } + free(text); + } else if (msg.type == MSG_TYPE_ERROR) { + fprintf(stderr, "server error: %.*s\n", (int) msg.body_len, msg.body == NULL ? "" : (const char *) msg.body); + } else { + fprintf(stderr, "unexpected message type %s from %s ignored\n", protocol_message_type_name(msg.type), msg.from); + } + protocol_message_clear(&msg); + } + rc = 0; + goto cleanup; + } + + fprintf(stdout, "KCP PING %s via %s (payload=%d bytes, KCP)\n", target_peer, server_addr, size); + kcp_ping_receiver_ctx_init(&receiver_ctx, client); + receiver_ctx_initialized = 1; + if (pthread_create(&receiver_thread, NULL, kcpping_receive_thread_main, &receiver_ctx) != 0) { + fprintf(stderr, "kcpping: create receive thread failed\n"); + goto cleanup; + } + receiver_thread_started = 1; + + { + uint64_t next_seq = 1; + int stop_sending = 0; + int64_t next_send_at_ns = omni_now_unix_nano(); + int poll_ms = kcp_ping_expiry_poll_ms(timeout_ms); + int64_t timeout_ns = (int64_t) timeout_ms * 1000000LL; + + while (!g_kcpping_stop || tracker.pending_count > 0 || !stop_sending) { + int64_t now_ns = omni_now_unix_nano(); + message_t msg; + int popped; + int receiver_closed; + int receiver_status_rc; + + if (!stop_sending && now_ns >= next_send_at_ns) { + char *payload = NULL; + size_t payload_len = 0; + + if (count > 0 && tracker.sent >= count) { + stop_sending = 1; + } else { + if (kcp_ping_build_payload(next_seq, now_ns, size, &payload, &payload_len) != 0) { + fprintf(stderr, "kcpping: build payload for seq=%" PRIu64 " failed\n", next_seq); + free(payload); + goto cleanup; + } + if (kcp_client_send_text(client, target_peer, payload) != 0) { + fprintf(stderr, "kcpping: send ping seq=%" PRIu64 " failed\n", next_seq); + free(payload); + goto cleanup; + } + free(payload); + if (kcp_ping_tracker_mark_sent(&tracker, next_seq, now_ns, timeout_ns) != 0) { + goto cleanup; + } + next_seq++; + next_send_at_ns = now_ns + (int64_t) interval_ms * 1000000LL; + if (count > 0 && tracker.sent >= count) { + stop_sending = 1; + } + } + } + + kcp_ping_tracker_expire(&tracker, now_ns, stdout); + + do { + popped = kcp_ping_receiver_pop(&receiver_ctx, &msg); + if (popped == 1) { + if (msg.type == MSG_TYPE_TEXT) { + uint64_t seq; + int64_t sent_ts_ns; + int disposition; + int64_t rtt_ns; + + if (kcp_ping_parse_payload(msg.body, msg.body_len, &seq, &sent_ts_ns) != 0) { + fprintf(stderr, "ignore non-ping text message from %s\n", msg.from); + } else if (kcp_ping_tracker_observe_reply(&tracker, seq, sent_ts_ns, omni_now_unix_nano(), &disposition, &rtt_ns) != 0) { + protocol_message_clear(&msg); + goto cleanup; + } else if (disposition == 0) { + fprintf(stdout, "seq=%" PRIu64 " rtt=%.2fms\n", seq, (double) rtt_ns / 1000000.0); + } else if (disposition == 1) { + fprintf(stderr, "seq=%" PRIu64 " duplicate or late reply ignored\n", seq); + } else { + fprintf(stderr, "seq=%" PRIu64 " unexpected reply ignored\n", seq); + } + } else if (msg.type == MSG_TYPE_ERROR) { + fprintf(stderr, "server error: %.*s\n", (int) msg.body_len, msg.body == NULL ? "" : (const char *) msg.body); + } else { + fprintf(stderr, "unexpected message type %s from %s ignored\n", protocol_message_type_name(msg.type), msg.from); + } + protocol_message_clear(&msg); + } + } while (popped == 1); + + kcp_ping_receiver_status(&receiver_ctx, &receiver_closed, &receiver_status_rc); + if (receiver_closed && receiver_status_rc != 0) { + fprintf(stderr, "kcpping: receive loop failed\n"); + goto cleanup; + } + if ((g_kcpping_stop || stop_sending) && tracker.pending_count == 0) { + break; + } + usleep((useconds_t) poll_ms * 1000U); + } + } + + kcp_ping_print_summary(stdout, target_peer, &tracker); + rc = 0; + +cleanup: + receiver_ctx.stop_requested = 1; + kcp_client_close(client); + if (receiver_thread_started) { + pthread_join(receiver_thread, NULL); + kcp_ping_receiver_ctx_destroy(&receiver_ctx); + } else if (receiver_ctx_initialized) { + kcp_ping_receiver_ctx_destroy(&receiver_ctx); + } + kcp_client_free(client); + latencylog_close(latency_logger); + kcp_ping_tracker_destroy(&tracker); + return rc; +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/cmd/kcpserver.c b/robot/ros2/OmniSocketGo_robot_ros/cmd/kcpserver.c new file mode 100644 index 0000000..afcf8f4 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/cmd/kcpserver.c @@ -0,0 +1,253 @@ +#include "cli_parse.h" +#include "server_kcp_hub.h" +#include "server_udp_relay.h" + +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"); +} + +int main(int argc, char **argv) { + const char *mode = "hub"; + const char *listen_addr = ":9002"; + const char *bind_device = ""; + const char *latency_log_path = ""; + 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; + + latency_logger_t *latency_logger = NULL; + kcp_packet_debug_logger_t *packet_logger = NULL; + kcp_session_stats_logger_t *stats_logger = NULL; + kcp_listener_t *listener = NULL; + kcp_hub_t *hub = NULL; + udp_relay_t *relay = NULL; + + for (i = 1; i < argc; ++i) { + const char *value = NULL; + int handled; + + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-mode", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -mode requires a value\n"); + return 1; + } else if (handled) { + mode = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-listen", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -listen requires a value\n"); + return 1; + } else if (handled) { + listen_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-bind-device", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -bind-device requires a value\n"); + return 1; + } else if (handled) { + bind_device = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-latency-log", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -latency-log requires a value\n"); + return 1; + } else if (handled) { + latency_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-kcp-ts-debug-log", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -kcp-ts-debug-log requires a value\n"); + return 1; + } else if (handled) { + packet_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-kcp-session-stats-log", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -kcp-session-stats-log requires a value\n"); + return 1; + } else if (handled) { + stats_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-kcp-session-stats-interval", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -kcp-session-stats-interval requires a value\n"); + return 1; + } else if (handled) { + 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; + } else if (handled) { + relay_listen_alias = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-relay-remote", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -relay-remote requires a value\n"); + return 1; + } else if (handled) { + relay_remote_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-relay-peer", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -relay-peer requires a value\n"); + return 1; + } else if (handled) { + relay_peer_alias = value; + continue; + } + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + kcpserver_usage(stdout); + return 0; + } + fprintf(stderr, "kcpserver: unknown argument %s\n", argv[i]); + kcpserver_usage(stderr); + return 1; + } + + if (kcp_session_stats_parse_interval_ms(stats_interval_raw, &stats_interval_ms) != 0) { + 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"); + return 1; + } + if (relay_remote_addr[0] == '\0' && relay_peer_alias[0] != '\0') { + relay_remote_addr = relay_peer_alias; + } + if (relay_peer_alias[0] != '\0') { + fprintf(stderr, "warning: flag -relay-peer is deprecated; use -relay-remote instead\n"); + } + if (relay_listen_alias[0] != '\0') { + if (strcmp(mode, "relay") != 0) { + fprintf(stderr, "kcpserver: flag -relay-listen may only be used in relay mode\n"); + return 1; + } + if (listen_addr[0] != '\0' && strcmp(listen_addr, ":9002") != 0 && strcmp(listen_addr, relay_listen_alias) != 0) { + fprintf(stderr, "kcpserver: flags -listen and -relay-listen must match when both are set in relay mode\n"); + return 1; + } + listen_addr = relay_listen_alias; + fprintf(stderr, "warning: flag -relay-listen is deprecated; use -listen with -mode=relay instead\n"); + } + + if (strcmp(mode, "hub") == 0) { + if (relay_remote_addr[0] != '\0') { + fprintf(stderr, "kcpserver: flag -relay-remote may only be used in relay mode\n"); + return 1; + } + if (latency_log_path[0] != '\0') { + latency_logger = latencylog_open_jsonl(latency_log_path); + if (latency_logger == NULL) { + fprintf(stderr, "kcpserver: open latency logger %s failed\n", latency_log_path); + goto cleanup; + } + } + if (packet_log_path[0] != '\0') { + packet_logger = kcp_packet_debug_open_jsonl(packet_log_path); + if (packet_logger == NULL) { + fprintf(stderr, "kcpserver: open packet debug logger %s failed\n", packet_log_path); + goto cleanup; + } + } + if (stats_log_path[0] != '\0') { + stats_logger = kcp_session_stats_open_jsonl(stats_log_path); + if (stats_logger == NULL) { + fprintf(stderr, "kcpserver: open session stats logger %s failed\n", stats_log_path); + goto cleanup; + } + } + listener = kcp_listener_listen(listen_addr, bind_device, packet_logger, OMNI_NODE_ROLE_SERVER, "hub"); + if (listener == NULL) { + fprintf(stderr, "kcpserver: listen on %s failed\n", listen_addr); + goto cleanup; + } + hub = kcp_hub_new(latency_logger, stats_logger, stats_interval_ms); + if (hub == NULL) { + 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"); + goto cleanup; + } + rc = 0; + goto cleanup; + } + + 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; + } + if (relay_remote_addr[0] == '\0') { + fprintf(stderr, "kcpserver: flag -relay-remote is required in relay mode\n"); + return 1; + } + relay = udp_relay_open(listen_addr, relay_remote_addr); + if (relay == NULL) { + fprintf(stderr, "kcpserver: open udp relay %s -> %s failed\n", listen_addr, relay_remote_addr); + goto cleanup; + } + fprintf(stderr, "udp relay listening on %s and forwarding to %s\n", listen_addr, relay_remote_addr); + if (udp_relay_serve(relay) != 0) { + fprintf(stderr, "kcpserver: udp relay stopped with error\n"); + goto cleanup; + } + rc = 0; + goto cleanup; + } + + fprintf(stderr, "kcpserver: unsupported -mode=%s; want hub or relay\n", mode); + +cleanup: + udp_relay_free(relay); + kcp_hub_free(hub); + kcp_listener_free(listener); + kcp_session_stats_close(stats_logger); + kcp_packet_debug_close(packet_logger); + latencylog_close(latency_logger); + return rc; +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/cmd/udppeer.c b/robot/ros2/OmniSocketGo_robot_ros/cmd/udppeer.c new file mode 100644 index 0000000..e5656ae --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/cmd/udppeer.c @@ -0,0 +1,291 @@ +#include "cli_parse.h" +#include "interactive.h" +#include "peer_udp_client.h" + +#include + +typedef struct udppeer_receive_ctx { + udp_client_t *client; + const char *inbox_dir; + volatile int stop_requested; + int rc; +} udppeer_receive_ctx_t; + +static void udppeer_usage(FILE *out) { + fprintf(out, "usage: udppeer [-id peer-a] [-server 127.0.0.1:9001] [-to peer] [-text msg | -file path]\n"); + fprintf(out, " [-bind-ip ip] [-inbox-dir dir] [-latency-log path] [-tx-ts-debug-log path]\n"); + fprintf(out, " [-interactive[=true|false]]\n"); +} + +static void *udppeer_receive_thread_main(void *arg) { + udppeer_receive_ctx_t *ctx = (udppeer_receive_ctx_t *) arg; + + for (;;) { + message_t msg; + char persisted_path[512]; + + protocol_message_init(&msg); + if (udp_client_receive(ctx->client, &msg) != 0) { + protocol_message_clear(&msg); + ctx->rc = ctx->stop_requested ? 0 : -1; + return NULL; + } + + switch (msg.type) { + case MSG_TYPE_TEXT: + if (udp_client_persist_message(ctx->client, &msg, ctx->inbox_dir, persisted_path, sizeof(persisted_path)) != 0) { + fprintf(stderr, "udppeer: persist text from %s to %s failed\n", msg.from, msg.to); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + fprintf(stderr, "received text from %s to %s and persisted to %s\n", msg.from, msg.to, persisted_path); + break; + case MSG_TYPE_FILE: + if (udp_client_persist_message(ctx->client, &msg, ctx->inbox_dir, persisted_path, sizeof(persisted_path)) != 0) { + fprintf(stderr, "udppeer: persist file from %s to %s failed\n", msg.from, msg.to); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + fprintf(stderr, "received file from %s to %s: %s (%lu bytes) -> %s\n", msg.from, msg.to, msg.file_name, (unsigned long) msg.body_len, persisted_path); + break; + case MSG_TYPE_BINARY: + if (udp_client_persist_message(ctx->client, &msg, ctx->inbox_dir, persisted_path, sizeof(persisted_path)) != 0) { + fprintf(stderr, "udppeer: persist binary payload from %s to %s failed\n", msg.from, msg.to); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + fprintf(stderr, "received binary payload from %s to %s (%lu bytes) -> %s\n", msg.from, msg.to, (unsigned long) msg.body_len, persisted_path); + break; + case MSG_TYPE_ERROR: + fprintf(stderr, "received error from %s to %s: %.*s\n", msg.from, msg.to, (int) msg.body_len, msg.body == NULL ? "" : (const char *) msg.body); + break; + default: + fprintf(stderr, "received unexpected message type %s from %s\n", protocol_message_type_name(msg.type), msg.from); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + protocol_message_clear(&msg); + } +} + +int main(int argc, char **argv) { + const char *peer_id = "peer-a"; + const char *server_addr = "127.0.0.1:9001"; + const char *target_peer = ""; + const char *text = ""; + const char *file_path = ""; + const char *bind_ip = ""; + const char *inbox_dir = "inbox"; + const char *latency_log_path = ""; + const char *tx_debug_log_path = ""; + int interactive = 1; + latency_logger_t *latency_logger = NULL; + tx_timestamp_debug_logger_t *debug_logger = NULL; + udp_client_t *client = NULL; + udppeer_receive_ctx_t receive_ctx; + pthread_t receive_thread; + int receive_thread_started = 0; + int i; + int rc = 1; + + memset(&receive_ctx, 0, sizeof(receive_ctx)); + + for (i = 1; i < argc; ++i) { + const char *value = NULL; + int handled; + + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-id", &value)) < 0) { + fprintf(stderr, "udppeer: flag -id requires a value\n"); + return 1; + } else if (handled) { + peer_id = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-server", &value)) < 0) { + fprintf(stderr, "udppeer: flag -server requires a value\n"); + return 1; + } else if (handled) { + server_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-to", &value)) < 0) { + fprintf(stderr, "udppeer: flag -to requires a value\n"); + return 1; + } else if (handled) { + target_peer = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-text", &value)) < 0) { + fprintf(stderr, "udppeer: flag -text requires a value\n"); + return 1; + } else if (handled) { + text = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-file", &value)) < 0) { + fprintf(stderr, "udppeer: flag -file requires a value\n"); + return 1; + } else if (handled) { + file_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-bind-ip", &value)) < 0) { + fprintf(stderr, "udppeer: flag -bind-ip requires a value\n"); + return 1; + } else if (handled) { + bind_ip = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-inbox-dir", &value)) < 0) { + fprintf(stderr, "udppeer: flag -inbox-dir requires a value\n"); + return 1; + } else if (handled) { + inbox_dir = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-latency-log", &value)) < 0) { + fprintf(stderr, "udppeer: flag -latency-log requires a value\n"); + return 1; + } else if (handled) { + latency_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-tx-ts-debug-log", &value)) < 0) { + fprintf(stderr, "udppeer: flag -tx-ts-debug-log requires a value\n"); + return 1; + } else if (handled) { + tx_debug_log_path = value; + continue; + } + if ((handled = cli_parse_bool_flag(argv[i], "-interactive", &interactive)) < 0) { + fprintf(stderr, "udppeer: invalid -interactive value\n"); + return 1; + } else if (handled) { + continue; + } + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + udppeer_usage(stdout); + return 0; + } + fprintf(stderr, "udppeer: unknown argument %s\n", argv[i]); + udppeer_usage(stderr); + return 1; + } + + if (text[0] != '\0' && file_path[0] != '\0') { + fprintf(stderr, "udppeer: only one of -text or -file may be specified\n"); + return 1; + } + if ((text[0] != '\0' || file_path[0] != '\0') && target_peer[0] == '\0') { + fprintf(stderr, "udppeer: flag -to is required when sending text or file\n"); + return 1; + } + + if (latency_log_path[0] != '\0') { + latency_logger = latencylog_open_jsonl(latency_log_path); + if (latency_logger == NULL) { + fprintf(stderr, "udppeer: open latency logger %s failed\n", latency_log_path); + goto cleanup; + } + } + if (tx_debug_log_path[0] != '\0') { + debug_logger = tx_timestamp_debug_open_jsonl(tx_debug_log_path); + if (debug_logger == NULL) { + fprintf(stderr, "udppeer: open tx timestamp debug logger %s failed\n", tx_debug_log_path); + goto cleanup; + } + } + + client = udp_client_dial(server_addr, peer_id, bind_ip, latency_logger, debug_logger, tx_debug_log_path[0] != '\0'); + if (client == NULL) { + fprintf(stderr, "udppeer: dial udp server %s failed\n", server_addr); + goto cleanup; + } + fprintf(stderr, "connected to %s as %s (UDP)\n", server_addr, udp_client_id(client)); + + receive_ctx.client = client; + receive_ctx.inbox_dir = inbox_dir; + if (pthread_create(&receive_thread, NULL, udppeer_receive_thread_main, &receive_ctx) != 0) { + fprintf(stderr, "udppeer: create receive thread failed\n"); + goto cleanup; + } + receive_thread_started = 1; + + if (target_peer[0] != '\0' && text[0] != '\0') { + if (udp_client_send_text(client, target_peer, text) != 0) { + fprintf(stderr, "udppeer: send text to %s failed\n", target_peer); + goto cleanup; + } + fprintf(stderr, "sent text to %s\n", target_peer); + } + if (target_peer[0] != '\0' && file_path[0] != '\0') { + if (udp_client_send_file_path(client, target_peer, file_path) != 0) { + fprintf(stderr, "udppeer: send file %s to %s failed\n", file_path, target_peer); + goto cleanup; + } + fprintf(stderr, "sent file %s to %s\n", file_path, target_peer); + } + + if (interactive) { + char line[2048]; + char prompt[128]; + + snprintf(prompt, sizeof(prompt), "%s> ", udp_client_id(client)); + interactive_print_help(stdout, "UDP"); + while (fputs(prompt, stdout) >= 0 && fflush(stdout) == 0 && fgets(line, sizeof(line), stdin) != NULL) { + interactive_command_t command; + char err[128]; + + omni_trim_newline(line); + if (interactive_parse_command(line, &command, err, sizeof(err)) != 0) { + if (strstr(err, "empty command") == NULL) { + fprintf(stderr, "%s\n", err); + } + continue; + } + if (command.type == INTERACTIVE_CMD_HELP) { + interactive_print_help(stdout, "UDP"); + continue; + } + if (command.type == INTERACTIVE_CMD_QUIT) { + break; + } + if (command.type == INTERACTIVE_CMD_TEXT) { + if (udp_client_send_text(client, command.to, command.value) != 0) { + fprintf(stderr, "udppeer: send text to %s failed\n", command.to); + continue; + } + fprintf(stderr, "sent text to %s\n", command.to); + continue; + } + if (command.type == INTERACTIVE_CMD_FILE) { + if (udp_client_send_file_path(client, command.to, command.value) != 0) { + fprintf(stderr, "udppeer: send file %s to %s failed\n", command.value, command.to); + continue; + } + fprintf(stderr, "sent file %s to %s\n", command.value, command.to); + continue; + } + } + } + + rc = 0; + +cleanup: + receive_ctx.stop_requested = 1; + udp_client_close(client); + if (receive_thread_started) { + pthread_join(receive_thread, NULL); + if (rc == 0 && receive_ctx.rc != 0) { + rc = 1; + } + } + udp_client_free(client); + tx_timestamp_debug_close(debug_logger); + latencylog_close(latency_logger); + return rc; +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/cmd/udpping.c b/robot/ros2/OmniSocketGo_robot_ros/cmd/udpping.c new file mode 100644 index 0000000..1b652da --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/cmd/udpping.c @@ -0,0 +1,780 @@ +#include "cli_parse.h" +#include "peer_udp_client.h" + +#include "cJSON.h" + +#include +#include + +typedef struct ping_message_node { + struct ping_message_node *next; + message_t msg; +} ping_message_node_t; + +typedef struct ping_receiver_ctx { + udp_client_t *client; + pthread_mutex_t mu; + ping_message_node_t *head; + ping_message_node_t *tail; + volatile int stop_requested; + int closed; + int rc; +} ping_receiver_ctx_t; + +typedef struct pending_ping { + struct pending_ping *next; + uint64_t seq; + int64_t deadline_ns; +} pending_ping_t; + +typedef struct ping_tracker { + pending_ping_t *pending; + int pending_count; + int sent; + int duplicates; + uint64_t max_seq_sent; + int64_t *samples_ns; + size_t sample_count; + size_t sample_cap; +} ping_tracker_t; + +static volatile sig_atomic_t g_udpping_stop = 0; + +static void udpping_on_signal(int signo) { + (void) signo; + g_udpping_stop = 1; +} + +static void udpping_usage(FILE *out) { + fprintf(out, "usage: udpping [-id pinger] [-server 127.0.0.1:9001] [-to peer] [-echo]\n"); + fprintf(out, " [-count 100] [-interval 100ms] [-size 64] [-timeout 3s]\n"); + fprintf(out, " [-bind-ip ip] [-latency-log path]\n"); +} + +static int ping_compare_i64(const void *left, const void *right) { + const int64_t *a = (const int64_t *) left; + const int64_t *b = (const int64_t *) right; + if (*a < *b) { + return -1; + } + if (*a > *b) { + return 1; + } + return 0; +} + +static double ping_sqrt(double value) { + double x = value; + int i; + + if (value <= 0.0) { + return 0.0; + } + if (x < 1.0) { + x = 1.0; + } + for (i = 0; i < 16; ++i) { + x = 0.5 * (x + value / x); + } + return x; +} + +static int ping_build_payload(uint64_t seq, int64_t ts_ns, int size, char **out_body, size_t *out_len) { + cJSON *root = NULL; + char *json = NULL; + char *pad = NULL; + size_t base_len; + size_t pad_len; + + *out_body = NULL; + *out_len = 0; + + root = cJSON_CreateObject(); + if (root == NULL) { + return -1; + } + cJSON_AddNumberToObject(root, "seq", (double) seq); + cJSON_AddNumberToObject(root, "ts_ns", (double) ts_ns); + cJSON_AddStringToObject(root, "pad", ""); + json = cJSON_PrintUnformatted(root); + cJSON_Delete(root); + if (json == NULL) { + return -1; + } + base_len = strlen(json); + cJSON_free(json); + if ((int) base_len > size) { + errno = EMSGSIZE; + return -1; + } + + pad_len = (size_t) size - base_len; + pad = (char *) malloc(pad_len + 1U); + if (pad == NULL) { + return -1; + } + memset(pad, 'A', pad_len); + pad[pad_len] = '\0'; + + root = cJSON_CreateObject(); + if (root == NULL) { + free(pad); + return -1; + } + cJSON_AddNumberToObject(root, "seq", (double) seq); + cJSON_AddNumberToObject(root, "ts_ns", (double) ts_ns); + cJSON_AddStringToObject(root, "pad", pad); + free(pad); + json = cJSON_PrintUnformatted(root); + cJSON_Delete(root); + if (json == NULL) { + return -1; + } + if ((int) strlen(json) != size) { + cJSON_free(json); + errno = EINVAL; + return -1; + } + *out_body = json; + *out_len = (size_t) size; + return 0; +} + +static int ping_parse_payload(const uint8_t *body, size_t body_len, uint64_t *seq, int64_t *ts_ns) { + char *text; + cJSON *root; + const cJSON *seq_item; + const cJSON *ts_item; + + if (body == NULL || seq == NULL || ts_ns == NULL) { + errno = EINVAL; + return -1; + } + text = (char *) malloc(body_len + 1U); + if (text == NULL) { + return -1; + } + memcpy(text, body, body_len); + text[body_len] = '\0'; + root = cJSON_Parse(text); + free(text); + if (root == NULL) { + errno = EPROTO; + return -1; + } + seq_item = cJSON_GetObjectItemCaseSensitive(root, "seq"); + ts_item = cJSON_GetObjectItemCaseSensitive(root, "ts_ns"); + if (!cJSON_IsNumber(seq_item) || !cJSON_IsNumber(ts_item) || seq_item->valuedouble <= 0 || ts_item->valuedouble <= 0) { + cJSON_Delete(root); + errno = EPROTO; + return -1; + } + *seq = (uint64_t) seq_item->valuedouble; + *ts_ns = (int64_t) ts_item->valuedouble; + cJSON_Delete(root); + return 0; +} + +static void ping_receiver_ctx_init(ping_receiver_ctx_t *ctx, udp_client_t *client) { + memset(ctx, 0, sizeof(*ctx)); + ctx->client = client; + pthread_mutex_init(&ctx->mu, NULL); +} + +static void ping_receiver_ctx_destroy(ping_receiver_ctx_t *ctx) { + ping_message_node_t *node; + ping_message_node_t *next; + + if (ctx == NULL) { + return; + } + for (node = ctx->head; node != NULL; node = next) { + next = node->next; + protocol_message_clear(&node->msg); + free(node); + } + pthread_mutex_destroy(&ctx->mu); +} + +static void *udpping_receive_thread_main(void *arg) { + ping_receiver_ctx_t *ctx = (ping_receiver_ctx_t *) arg; + + for (;;) { + message_t msg; + ping_message_node_t *node; + + protocol_message_init(&msg); + if (udp_client_receive(ctx->client, &msg) != 0) { + protocol_message_clear(&msg); + pthread_mutex_lock(&ctx->mu); + ctx->rc = ctx->stop_requested ? 0 : -1; + ctx->closed = 1; + pthread_mutex_unlock(&ctx->mu); + return NULL; + } + + node = (ping_message_node_t *) calloc(1, sizeof(*node)); + if (node == NULL) { + protocol_message_clear(&msg); + pthread_mutex_lock(&ctx->mu); + ctx->rc = -1; + ctx->closed = 1; + pthread_mutex_unlock(&ctx->mu); + return NULL; + } + node->msg = msg; + + pthread_mutex_lock(&ctx->mu); + if (ctx->tail == NULL) { + ctx->head = node; + } else { + ctx->tail->next = node; + } + ctx->tail = node; + pthread_mutex_unlock(&ctx->mu); + } +} + +static int ping_receiver_pop(ping_receiver_ctx_t *ctx, message_t *out_msg) { + ping_message_node_t *node; + + pthread_mutex_lock(&ctx->mu); + node = ctx->head; + if (node != NULL) { + ctx->head = node->next; + if (ctx->head == NULL) { + ctx->tail = NULL; + } + } + pthread_mutex_unlock(&ctx->mu); + + if (node == NULL) { + return 0; + } + *out_msg = node->msg; + free(node); + return 1; +} + +static int ping_receiver_status(ping_receiver_ctx_t *ctx, int *closed, int *rc) { + pthread_mutex_lock(&ctx->mu); + *closed = ctx->closed; + *rc = ctx->rc; + pthread_mutex_unlock(&ctx->mu); + return 0; +} + +static void ping_tracker_init(ping_tracker_t *tracker) { + memset(tracker, 0, sizeof(*tracker)); +} + +static void ping_tracker_destroy(ping_tracker_t *tracker) { + pending_ping_t *pending; + pending_ping_t *next; + + for (pending = tracker->pending; pending != NULL; pending = next) { + next = pending->next; + free(pending); + } + free(tracker->samples_ns); +} + +static int ping_tracker_mark_sent(ping_tracker_t *tracker, uint64_t seq, int64_t sent_at_ns, int64_t timeout_ns) { + pending_ping_t *pending = (pending_ping_t *) calloc(1, sizeof(*pending)); + if (pending == NULL) { + return -1; + } + pending->seq = seq; + pending->deadline_ns = sent_at_ns + timeout_ns; + pending->next = tracker->pending; + tracker->pending = pending; + tracker->pending_count++; + tracker->sent++; + tracker->max_seq_sent = seq; + return 0; +} + +static pending_ping_t *ping_tracker_find_pending(ping_tracker_t *tracker, uint64_t seq, pending_ping_t **out_prev) { + pending_ping_t *prev = NULL; + pending_ping_t *cur; + + for (cur = tracker->pending; cur != NULL; cur = cur->next) { + if (cur->seq == seq) { + if (out_prev != NULL) { + *out_prev = prev; + } + return cur; + } + prev = cur; + } + if (out_prev != NULL) { + *out_prev = NULL; + } + return NULL; +} + +static int ping_tracker_add_sample(ping_tracker_t *tracker, int64_t rtt_ns) { + int64_t *next_samples; + size_t next_cap; + + if (tracker->sample_count == tracker->sample_cap) { + next_cap = tracker->sample_cap == 0 ? 16U : tracker->sample_cap * 2U; + next_samples = (int64_t *) realloc(tracker->samples_ns, next_cap * sizeof(*next_samples)); + if (next_samples == NULL) { + return -1; + } + tracker->samples_ns = next_samples; + tracker->sample_cap = next_cap; + } + tracker->samples_ns[tracker->sample_count++] = rtt_ns; + return 0; +} + +static int ping_tracker_observe_reply(ping_tracker_t *tracker, uint64_t seq, int64_t sent_ts_ns, int64_t received_ts_ns, int *disposition, int64_t *rtt_ns) { + pending_ping_t *prev = NULL; + pending_ping_t *pending; + + if (seq == 0 || seq > tracker->max_seq_sent) { + *disposition = 2; + *rtt_ns = 0; + return 0; + } + pending = ping_tracker_find_pending(tracker, seq, &prev); + if (pending == NULL) { + tracker->duplicates++; + *disposition = 1; + *rtt_ns = 0; + return 0; + } + if (prev == NULL) { + tracker->pending = pending->next; + } else { + prev->next = pending->next; + } + tracker->pending_count--; + free(pending); + + *rtt_ns = received_ts_ns - sent_ts_ns; + if (*rtt_ns < 0) { + *rtt_ns = 0; + } + if (ping_tracker_add_sample(tracker, *rtt_ns) != 0) { + return -1; + } + *disposition = 0; + return 0; +} + +static void ping_tracker_expire(ping_tracker_t *tracker, int64_t now_ns, FILE *out) { + pending_ping_t *prev = NULL; + pending_ping_t *cur = tracker->pending; + + while (cur != NULL) { + if (cur->deadline_ns <= now_ns) { + pending_ping_t *next = cur->next; + fprintf(out, "seq=%" PRIu64 " timeout\n", cur->seq); + if (prev == NULL) { + tracker->pending = next; + } else { + prev->next = next; + } + free(cur); + tracker->pending_count--; + cur = next; + continue; + } + prev = cur; + cur = cur->next; + } +} + +static int64_t ping_percentile_ns(const int64_t *sorted, size_t count, double percentile) { + size_t index; + double raw_index; + + if (count == 0) { + return 0; + } + if (percentile <= 0.0) { + return sorted[0]; + } + if (percentile >= 1.0) { + return sorted[count - 1]; + } + raw_index = percentile * (double) count; + index = (size_t) raw_index; + if ((double) index < raw_index) { + index++; + } + if (index > 0) { + index--; + } + if (index >= count) { + index = count - 1; + } + return sorted[index]; +} + +static void ping_print_summary(FILE *out, const char *target, const ping_tracker_t *tracker) { + int received = (int) tracker->sample_count; + double loss_pct = tracker->sent == 0 ? 0.0 : ((double) (tracker->sent - received) * 100.0 / (double) tracker->sent); + + fprintf(out, "--- %s udp ping statistics ---\n", target); + fprintf(out, "%d packets transmitted, %d received, %d duplicates, %.2f%% packet loss\n", tracker->sent, received, tracker->duplicates, loss_pct); + if (tracker->sample_count == 0) { + fprintf(out, "rtt min/avg/max/p50/p95/p99 = n/a/n/a/n/a/n/a/n/a/n/a, stddev=n/a\n"); + return; + } + + { + int64_t *sorted = (int64_t *) malloc(tracker->sample_count * sizeof(*sorted)); + size_t i; + double sum = 0.0; + double variance = 0.0; + double avg; + int64_t min_ns; + int64_t max_ns; + int64_t p50_ns; + int64_t p95_ns; + int64_t p99_ns; + + if (sorted == NULL) { + fprintf(out, "rtt summary unavailable: memory allocation failed\n"); + return; + } + memcpy(sorted, tracker->samples_ns, tracker->sample_count * sizeof(*sorted)); + qsort(sorted, tracker->sample_count, sizeof(*sorted), ping_compare_i64); + for (i = 0; i < tracker->sample_count; ++i) { + sum += (double) sorted[i]; + } + avg = sum / (double) tracker->sample_count; + for (i = 0; i < tracker->sample_count; ++i) { + double delta = (double) sorted[i] - avg; + variance += delta * delta; + } + variance /= (double) tracker->sample_count; + + min_ns = sorted[0]; + max_ns = sorted[tracker->sample_count - 1]; + p50_ns = ping_percentile_ns(sorted, tracker->sample_count, 0.50); + p95_ns = ping_percentile_ns(sorted, tracker->sample_count, 0.95); + p99_ns = ping_percentile_ns(sorted, tracker->sample_count, 0.99); + + fprintf( + out, + "rtt min/avg/max/p50/p95/p99 = %.2fms/%.2fms/%.2fms/%.2fms/%.2fms/%.2fms, stddev=%.2fms\n", + (double) min_ns / 1000000.0, + avg / 1000000.0, + (double) max_ns / 1000000.0, + (double) p50_ns / 1000000.0, + (double) p95_ns / 1000000.0, + (double) p99_ns / 1000000.0, + ping_sqrt(variance) / 1000000.0 + ); + free(sorted); + } +} + +static int ping_expiry_poll_ms(int timeout_ms) { + int interval = timeout_ms / 4; + if (interval < 10) { + return 10; + } + if (interval > 100) { + return 100; + } + return interval; +} + +int main(int argc, char **argv) { + const char *peer_id = "pinger"; + const char *server_addr = "127.0.0.1:9001"; + const char *target_peer = ""; + const char *bind_ip = ""; + const char *latency_log_path = ""; + int echo_mode = 0; + int count = 100; + int interval_ms = 100; + int size = 64; + int timeout_ms = 3000; + latency_logger_t *latency_logger = NULL; + udp_client_t *client = NULL; + ping_receiver_ctx_t receiver_ctx; + pthread_t receiver_thread; + int receiver_ctx_initialized = 0; + int receiver_thread_started = 0; + ping_tracker_t tracker; + int i; + int rc = 1; + + ping_tracker_init(&tracker); + memset(&receiver_ctx, 0, sizeof(receiver_ctx)); + + for (i = 1; i < argc; ++i) { + const char *value = NULL; + int handled; + + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-id", &value)) < 0) { + fprintf(stderr, "udpping: flag -id requires a value\n"); + return 1; + } else if (handled) { + peer_id = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-server", &value)) < 0) { + fprintf(stderr, "udpping: flag -server requires a value\n"); + return 1; + } else if (handled) { + server_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-to", &value)) < 0) { + fprintf(stderr, "udpping: flag -to requires a value\n"); + return 1; + } else if (handled) { + target_peer = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-count", &value)) < 0) { + fprintf(stderr, "udpping: flag -count requires a value\n"); + return 1; + } else if (handled) { + count = atoi(value); + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-interval", &value)) < 0) { + fprintf(stderr, "udpping: flag -interval requires a value\n"); + return 1; + } else if (handled) { + if (omni_parse_duration_ms(value, interval_ms, &interval_ms) != 0) { + fprintf(stderr, "udpping: invalid -interval value %s\n", value); + return 1; + } + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-size", &value)) < 0) { + fprintf(stderr, "udpping: flag -size requires a value\n"); + return 1; + } else if (handled) { + size = atoi(value); + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-timeout", &value)) < 0) { + fprintf(stderr, "udpping: flag -timeout requires a value\n"); + return 1; + } else if (handled) { + if (omni_parse_duration_ms(value, timeout_ms, &timeout_ms) != 0) { + fprintf(stderr, "udpping: invalid -timeout value %s\n", value); + return 1; + } + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-bind-ip", &value)) < 0) { + fprintf(stderr, "udpping: flag -bind-ip requires a value\n"); + return 1; + } else if (handled) { + bind_ip = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-latency-log", &value)) < 0) { + fprintf(stderr, "udpping: flag -latency-log requires a value\n"); + return 1; + } else if (handled) { + latency_log_path = value; + continue; + } + if ((handled = cli_parse_bool_flag(argv[i], "-echo", &echo_mode)) < 0) { + fprintf(stderr, "udpping: invalid -echo value\n"); + return 1; + } else if (handled) { + continue; + } + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + udpping_usage(stdout); + return 0; + } + fprintf(stderr, "udpping: unknown argument %s\n", argv[i]); + udpping_usage(stderr); + return 1; + } + + if (peer_id[0] == '\0' || server_addr[0] == '\0') { + fprintf(stderr, "udpping: flags -id and -server are required\n"); + return 1; + } + if (!echo_mode && target_peer[0] == '\0') { + fprintf(stderr, "udpping: flag -to is required unless -echo is set\n"); + return 1; + } + if (count < 0 || interval_ms <= 0 || size <= 0 || timeout_ms <= 0) { + fprintf(stderr, "udpping: invalid numeric flag value\n"); + return 1; + } + + signal(SIGINT, udpping_on_signal); + + if (latency_log_path[0] != '\0') { + latency_logger = latencylog_open_jsonl(latency_log_path); + if (latency_logger == NULL) { + fprintf(stderr, "udpping: open latency logger %s failed\n", latency_log_path); + goto cleanup; + } + } + client = udp_client_dial(server_addr, peer_id, bind_ip, latency_logger, NULL, 0); + if (client == NULL) { + fprintf(stderr, "udpping: dial udp server %s failed\n", server_addr); + goto cleanup; + } + + if (echo_mode) { + while (!g_udpping_stop) { + message_t msg; + + protocol_message_init(&msg); + if (udp_client_receive(client, &msg) != 0) { + protocol_message_clear(&msg); + if (g_udpping_stop) { + break; + } + fprintf(stderr, "udpping: receive failed in echo mode\n"); + goto cleanup; + } + if (msg.type == MSG_TYPE_TEXT) { + char *text = (char *) malloc(msg.body_len + 1U); + if (text == NULL) { + protocol_message_clear(&msg); + goto cleanup; + } + memcpy(text, msg.body, msg.body_len); + text[msg.body_len] = '\0'; + if (udp_client_send_text(client, msg.from, text) != 0) { + free(text); + protocol_message_clear(&msg); + fprintf(stderr, "udpping: echo send back to %s failed\n", msg.from); + goto cleanup; + } + free(text); + } else if (msg.type == MSG_TYPE_ERROR) { + fprintf(stderr, "server error: %.*s\n", (int) msg.body_len, msg.body == NULL ? "" : (const char *) msg.body); + } else { + fprintf(stderr, "unexpected message type %s from %s ignored\n", protocol_message_type_name(msg.type), msg.from); + } + protocol_message_clear(&msg); + } + rc = 0; + goto cleanup; + } + + fprintf(stdout, "UDP PING %s via %s (payload=%d bytes, UDP)\n", target_peer, server_addr, size); + ping_receiver_ctx_init(&receiver_ctx, client); + receiver_ctx_initialized = 1; + if (pthread_create(&receiver_thread, NULL, udpping_receive_thread_main, &receiver_ctx) != 0) { + fprintf(stderr, "udpping: create receive thread failed\n"); + goto cleanup; + } + receiver_thread_started = 1; + + { + uint64_t next_seq = 1; + int stop_sending = 0; + int64_t next_send_at_ns = omni_now_unix_nano(); + int poll_ms = ping_expiry_poll_ms(timeout_ms); + int64_t timeout_ns = (int64_t) timeout_ms * 1000000LL; + + while (!g_udpping_stop || tracker.pending_count > 0 || !stop_sending) { + int64_t now_ns = omni_now_unix_nano(); + message_t msg; + int popped; + int receiver_closed; + int receiver_status_rc; + + if (!stop_sending && now_ns >= next_send_at_ns) { + char *payload = NULL; + size_t payload_len = 0; + + if (count > 0 && tracker.sent >= count) { + stop_sending = 1; + } else { + if (ping_build_payload(next_seq, now_ns, size, &payload, &payload_len) != 0) { + fprintf(stderr, "udpping: build payload for seq=%" PRIu64 " failed\n", next_seq); + free(payload); + goto cleanup; + } + if (udp_client_send_text(client, target_peer, payload) != 0) { + fprintf(stderr, "udpping: send ping seq=%" PRIu64 " failed\n", next_seq); + free(payload); + goto cleanup; + } + free(payload); + if (ping_tracker_mark_sent(&tracker, next_seq, now_ns, timeout_ns) != 0) { + goto cleanup; + } + next_seq++; + next_send_at_ns = now_ns + (int64_t) interval_ms * 1000000LL; + if (count > 0 && tracker.sent >= count) { + stop_sending = 1; + } + } + } + + ping_tracker_expire(&tracker, now_ns, stdout); + + do { + popped = ping_receiver_pop(&receiver_ctx, &msg); + if (popped == 1) { + if (msg.type == MSG_TYPE_TEXT) { + uint64_t seq; + int64_t sent_ts_ns; + int disposition; + int64_t rtt_ns; + + if (ping_parse_payload(msg.body, msg.body_len, &seq, &sent_ts_ns) != 0) { + fprintf(stderr, "ignore non-ping text message from %s\n", msg.from); + } else if (ping_tracker_observe_reply(&tracker, seq, sent_ts_ns, omni_now_unix_nano(), &disposition, &rtt_ns) != 0) { + protocol_message_clear(&msg); + goto cleanup; + } else if (disposition == 0) { + fprintf(stdout, "seq=%" PRIu64 " rtt=%.2fms\n", seq, (double) rtt_ns / 1000000.0); + } else if (disposition == 1) { + fprintf(stderr, "seq=%" PRIu64 " duplicate or late reply ignored\n", seq); + } else { + fprintf(stderr, "seq=%" PRIu64 " unexpected reply ignored\n", seq); + } + } else if (msg.type == MSG_TYPE_ERROR) { + fprintf(stderr, "server error: %.*s\n", (int) msg.body_len, msg.body == NULL ? "" : (const char *) msg.body); + } else { + fprintf(stderr, "unexpected message type %s from %s ignored\n", protocol_message_type_name(msg.type), msg.from); + } + protocol_message_clear(&msg); + } + } while (popped == 1); + + ping_receiver_status(&receiver_ctx, &receiver_closed, &receiver_status_rc); + if (receiver_closed && receiver_status_rc != 0) { + fprintf(stderr, "udpping: receive loop failed\n"); + goto cleanup; + } + if ((g_udpping_stop || stop_sending) && tracker.pending_count == 0) { + break; + } + usleep((useconds_t) poll_ms * 1000U); + } + } + + ping_print_summary(stdout, target_peer, &tracker); + rc = 0; + +cleanup: + receiver_ctx.stop_requested = 1; + udp_client_close(client); + if (receiver_thread_started) { + pthread_join(receiver_thread, NULL); + ping_receiver_ctx_destroy(&receiver_ctx); + } else if (receiver_ctx_initialized) { + ping_receiver_ctx_destroy(&receiver_ctx); + } + udp_client_free(client); + latencylog_close(latency_logger); + ping_tracker_destroy(&tracker); + return rc; +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/cmd/udprelay.c b/robot/ros2/OmniSocketGo_robot_ros/cmd/udprelay.c new file mode 100644 index 0000000..57cf5b2 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/cmd/udprelay.c @@ -0,0 +1,59 @@ +#include "cli_parse.h" +#include "server_udp_relay.h" + +static void udprelay_usage(FILE *out) { + fprintf(out, "usage: udprelay [-listen addr] [-upstream addr]\n"); +} + +int main(int argc, char **argv) { + const char *listen_addr = ":9003"; + const char *upstream_addr = "127.0.0.1:9002"; + udp_relay_t *relay = NULL; + int i; + int rc = 1; + + for (i = 1; i < argc; ++i) { + const char *value = NULL; + int handled; + + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-listen", &value)) < 0) { + fprintf(stderr, "udprelay: flag -listen requires a value\n"); + return 1; + } else if (handled) { + listen_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-upstream", &value)) < 0) { + fprintf(stderr, "udprelay: flag -upstream requires a value\n"); + return 1; + } else if (handled) { + upstream_addr = value; + continue; + } + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + udprelay_usage(stdout); + return 0; + } + fprintf(stderr, "udprelay: unknown argument %s\n", argv[i]); + udprelay_usage(stderr); + return 1; + } + + relay = udp_relay_open(listen_addr, upstream_addr); + if (relay == NULL) { + fprintf(stderr, "udprelay: open relay %s -> %s failed\n", listen_addr, upstream_addr); + goto cleanup; + } + + fprintf(stderr, "udp relay listening on %s, upstream %s\n", listen_addr, upstream_addr); + if (udp_relay_serve(relay) != 0) { + fprintf(stderr, "udprelay: relay serve failed\n"); + goto cleanup; + } + + rc = 0; + +cleanup: + udp_relay_free(relay); + return rc; +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/cmd/udpserver.c b/robot/ros2/OmniSocketGo_robot_ros/cmd/udpserver.c new file mode 100644 index 0000000..978fd36 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/cmd/udpserver.c @@ -0,0 +1,88 @@ +#include "cli_parse.h" +#include "server_udp_hub.h" + +static void udpserver_usage(FILE *out) { + fprintf(out, "usage: udpserver [-listen addr] [-latency-log path] [-tx-ts-debug-log path]\n"); +} + +int main(int argc, char **argv) { + const char *listen_addr = ":9001"; + const char *latency_log_path = ""; + const char *tx_debug_log_path = ""; + latency_logger_t *latency_logger = NULL; + tx_timestamp_debug_logger_t *debug_logger = NULL; + udp_hub_t *hub = NULL; + int enable_timestamping = 0; + int i; + int rc = 1; + + for (i = 1; i < argc; ++i) { + const char *value = NULL; + int handled; + + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-listen", &value)) < 0) { + fprintf(stderr, "udpserver: flag -listen requires a value\n"); + return 1; + } else if (handled) { + listen_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-latency-log", &value)) < 0) { + fprintf(stderr, "udpserver: flag -latency-log requires a value\n"); + return 1; + } else if (handled) { + latency_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-tx-ts-debug-log", &value)) < 0) { + fprintf(stderr, "udpserver: flag -tx-ts-debug-log requires a value\n"); + return 1; + } else if (handled) { + tx_debug_log_path = value; + continue; + } + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + udpserver_usage(stdout); + return 0; + } + fprintf(stderr, "udpserver: unknown argument %s\n", argv[i]); + udpserver_usage(stderr); + return 1; + } + + if (latency_log_path[0] != '\0') { + latency_logger = latencylog_open_jsonl(latency_log_path); + if (latency_logger == NULL) { + fprintf(stderr, "udpserver: open latency logger %s failed\n", latency_log_path); + goto cleanup; + } + } + if (tx_debug_log_path[0] != '\0') { + debug_logger = tx_timestamp_debug_open_jsonl(tx_debug_log_path); + if (debug_logger == NULL) { + fprintf(stderr, "udpserver: open tx timestamp debug logger %s failed\n", tx_debug_log_path); + goto cleanup; + } + enable_timestamping = 1; + } + + hub = udp_hub_open(listen_addr, latency_logger, debug_logger, enable_timestamping); + if (hub == NULL) { + fprintf(stderr, "udpserver: listen on %s failed\n", listen_addr); + goto cleanup; + } + + fprintf(stderr, "udp server listening on %s\n", listen_addr); + if (udp_hub_serve(hub) != 0) { + fprintf(stderr, "udpserver: serve failed\n"); + goto cleanup; + } + + rc = 0; + +cleanup: + udp_hub_free(hub); + tx_timestamp_debug_close(debug_logger); + latencylog_close(latency_logger); + return rc; +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/cmd/v1_camera_pipeline_ifdef.c b/robot/ros2/OmniSocketGo_robot_ros/cmd/v1_camera_pipeline_ifdef.c new file mode 100644 index 0000000..093ee6e --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/cmd/v1_camera_pipeline_ifdef.c @@ -0,0 +1,35 @@ +#include +#include + +#include "video_pipeline.h" + +int main(void) { + video_pipeline_config_t config; + video_pipeline_stats_t stats; + + video_pipeline_config_init(&config); + video_pipeline_config_load_env(&config); + if (getenv("OMNI_VIDEO_DEBUG_TIMING") == NULL) { + config.enable_timing_logs = 1; + } + if (video_pipeline_stats_init(&stats) != 0) { + perror("video_pipeline_stats_init"); + return 1; + } + + for (;;) { + int rc = video_pipeline_run(&config, &stats, NULL); + + if (rc == 0) { + break; + } + if (rc != VIDEO_PIPELINE_RUN_RETRY_IMMEDIATE) { + perror("video_pipeline_run"); + video_pipeline_stats_destroy(&stats); + return 1; + } + } + + video_pipeline_stats_destroy(&stats); + return 0; +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/config/omnisocket_demo.yaml b/robot/ros2/OmniSocketGo_robot_ros/config/omnisocket_demo.yaml new file mode 100644 index 0000000..80542f0 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/config/omnisocket_demo.yaml @@ -0,0 +1,41 @@ +transport: + server_addr: "127.0.0.1:10909" + relay_via: "" + bind_ip: "" + bind_device: "" + +control_sender: + peer_id: "peer-a-ctrl" + target_peer: "peer-b-ctrl" + joy_topic: "/xbox_data" + deadzone: 0.10 + analog_epsilon: 0.01 + dpad_threshold: 0.50 + trigger_pressed_threshold: -0.50 + +control_receiver: + peer_id: "peer-b-ctrl" + +motion: + initial_lift: 0.89 + lift_step: 0.05 + max_surge: 1.0 + max_sway: 0.5 + max_spin: 0.5 + max_lift: 0.90 + min_lift: 0.65 + surge_step: 0.1 + sway_step: 0.1 + spin_step: 0.1 + +video_sender: + peer_id: "peer-b-video" + target_peer: "peer-a-video" + frame_bytes: 30720 + frame_interval_ms: 66 + +video_receiver: + peer_id: "peer-a-video" + # recv_into() requires a buffer large enough for the whole frame. + # If buffer_bytes is smaller than video_sender.frame_bytes, the oversize frame is dropped. + buffer_bytes: 65536 diff --git a/robot/ros2/OmniSocketGo_robot_ros/docs/ROS2_CAMERA_FORWARDING.md b/robot/ros2/OmniSocketGo_robot_ros/docs/ROS2_CAMERA_FORWARDING.md new file mode 100644 index 0000000..435c642 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/docs/ROS2_CAMERA_FORWARDING.md @@ -0,0 +1,117 @@ +# ROS 2 相机转发说明 + +## 设计 + +```text +Orbbec 驱动 / proc_manager + │ ROS 2 Image topic + ▼ +omnisocket_camera_bridge + │ /dev/shm/omnisocket-rgb-{head,waist} + ▼ +b_side_omnid（OmniSocketGo 视频编码与 KCP 发送） + │ 原有视频包协议 + ▼ +远端 OmniSocketGo/robot-command-center +``` + +设备只由驱动节点打开。ROS 2 桥接节点订阅 `sensor_msgs/msg/Image`，每路仅保留一帧最新图像写入共享内存；C 端读取共享内存后继续使用原有缩放、MJPEG 编码和网络发送逻辑。因此切换摄像头不会再次打开或关闭 `/dev/video*`。 + +## 首次构建 + +在机器人上： + +```bash +sudo apt-get install python3-colcon-common-extensions \ + ros-jazzy-rclpy ros-jazzy-sensor-msgs + +cd ~/OmniSocketGo_robot_ros/ros2 +source /opt/ros/jazzy/setup.bash +colcon build +source install/setup.bash + +cd ~/OmniSocketGo_robot_ros +make b_side_omnid +``` + +如果环境不是 Jazzy，把 `/opt/ros/jazzy` 换成实际 ROS 2 发行版目录。 + +## 启动 + +推荐使用项目启动脚本。它会： + +1. 检查 `proc_manager.service`； +2. `proc_manager` 已运行且已经提供 RGB topic 时，只等待并复用这些 topic； +3. `proc_manager` 已运行但没有占用头/腰设备时，按配置启动 `orbbec_head.service` 和 `orbbec_waist.service`；如果设备已被占用则拒绝重复启动； +4. `proc_manager` 未运行时，同样按配置启动两个相机服务，再等待 topic； +5. 启动 ROS 2 RGB 桥； +6. 启动 `b_side_omnid`。 + +```bash +cd ~/OmniSocketGo_robot_ros +source scripts/dev/load-env.sh +source scripts/dev/robot-remote.env +source scripts/dev/robot-remote.env.local # 如有本机覆盖 +scripts/dev/start-b-side-omnid.sh +``` + +默认变量： + +```bash +OMNI_CAMERA_SOURCE=ros2 +OMNI_CAMERA_ACTIVE=head # head 或 waist +OMNI_PROC_MANAGER_AUTO_START=0 +OMNI_CAMERA_START_SERVICES=1 +OMNI_ROS2_BRIDGE_AUTO_START=1 +``` + +`OMNI_PROC_MANAGER_AUTO_START=1` 可让脚本在服务未运行时尝试启动 `proc_manager`；需要 sudo 权限。脚本只在确认头部/腰部设备节点空闲时启动 `orbbec_head/waist.service`，不会停止现有服务或抢占已被占用的设备。 + +## 单独启动桥接节点 + +```bash +source /opt/ros/jazzy/setup.bash +source ~/OmniSocketGo_robot_ros/ros2/install/setup.bash +ros2 run omnisocket_camera_bridge omnisocket_ros_camera_bridge \ + --ros-args \ + -p head_topic:=/ob_camera_head/color/image_raw \ + -p waist_topic:=/ob_camera_waist/color/image_raw \ + -p head_shm:=/dev/shm/omnisocket-rgb-head \ + -p waist_shm:=/dev/shm/omnisocket-rgb-waist +``` + +先确认桥接节点有帧： + +```bash +ros2 topic hz /ob_camera_head/color/image_raw +ros2 topic hz /ob_camera_waist/color/image_raw +ls -lh /dev/shm/omnisocket-rgb-head /dev/shm/omnisocket-rgb-waist +``` + +查看 `logs/runtime/b-side-omnid.status.json`，应看到 `video_input_source: ros2`、递增的 +`video_frames_sent`，以及当前的 `video_active_camera`。这可以确认 C +视频管线正在读取 ROS2 桥接帧，而不是打开 `/dev/video*`。 + +## 切换头部/腰部 + +视频控制通道仍使用原有文本命令： + +```text +camera:head +camera:waist +``` + +两路 ROS 2 订阅线程持续运行，切换只改变 C 端读取和发送的共享内存槽，不会重新连接相机或触碰 `/dev/video*`。 + +## 兼容旧 V4L2 模式 + +仅在没有 ROS 2 图像 topic 的临时测试环境使用： + +```bash +OMNI_CAMERA_SOURCE=v4l2 \ +OMNI_CAMERA_HEAD_DEVICE=/dev/video26 \ +OMNI_CAMERA_WAIST_DEVICE=/dev/video18 \ +scripts/dev/start-b-side-omnid.sh +``` + +ROS-native 项目默认不走这条路径。 diff --git a/robot/ros2/OmniSocketGo_robot_ros/docs/ROS_CAMERA_INTERFACES.md b/robot/ros2/OmniSocketGo_robot_ros/docs/ROS_CAMERA_INTERFACES.md new file mode 100644 index 0000000..6b6ab0e --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/docs/ROS_CAMERA_INTERFACES.md @@ -0,0 +1,44 @@ +# ROS 2 相机接口（RGB + 深度） + +本项目的 ROS 2 相机输入来自 Orbbec 节点，不再直接打开 `/dev/video*`。头部和腰部相机分别发布下面的接口。 + +## 头部相机 + +| 数据 | Topic | 消息类型 | +|---|---|---| +| 彩色图像 | `/ob_camera_head/color/image_raw` | `sensor_msgs/msg/Image` | +| 彩色内参 | `/ob_camera_head/color/camera_info` | `sensor_msgs/msg/CameraInfo` | +| 彩色元数据 | `/ob_camera_head/color/metadata` | `orbbec_camera_msgs/msg/Metadata` | +| 深度图像 | `/ob_camera_head/depth/image_raw` | `sensor_msgs/msg/Image` | +| 深度内参 | `/ob_camera_head/depth/camera_info` | `sensor_msgs/msg/CameraInfo` | +| 深度元数据 | `/ob_camera_head/depth/metadata` | `orbbec_camera_msgs/msg/Metadata` | +| 深度到彩色变换 | `/ob_camera_head/depth_to_color` | 由 Orbbec 驱动发布 | + +## 腰部相机 + +| 数据 | Topic | 消息类型 | +|---|---|---| +| 彩色图像 | `/ob_camera_waist/color/image_raw` | `sensor_msgs/msg/Image` | +| 彩色内参 | `/ob_camera_waist/color/camera_info` | `sensor_msgs/msg/CameraInfo` | +| 彩色元数据 | `/ob_camera_waist/color/metadata` | `orbbec_camera_msgs/msg/Metadata` | +| 深度图像 | `/ob_camera_waist/depth/image_raw` | `sensor_msgs/msg/Image` | +| 深度内参 | `/ob_camera_waist/depth/camera_info` | `sensor_msgs/msg/CameraInfo` | +| 深度元数据 | `/ob_camera_waist/depth/metadata` | `orbbec_camera_msgs/msg/Metadata` | +| 深度到彩色变换 | `/ob_camera_waist/depth_to_color` | 由 Orbbec 驱动发布 | + +默认 RGB 分辨率由相机驱动决定，常见为 `1280x720`、`rgb8`。实际值请以运行时消息为准： + +```bash +ros2 topic echo /ob_camera_head/color/image_raw --once +ros2 topic echo /ob_camera_head/depth/image_raw --once +ros2 topic echo /ob_camera_head/color/camera_info --once +``` + +应用节点可以同时订阅 RGB、深度、CameraInfo 和元数据。RGB 转发桥只订阅彩色图像；深度和标定接口保留在 ROS 2 内，供本地感知、对齐和测距节点使用。不要把深度图像通过当前视频编码通道发送，除非另行设计深度压缩协议。 + +## QoS 与同步建议 + +- 图像订阅使用 `BEST_EFFORT`、队列深度 2，优先获取最新帧，避免实时转发被旧帧阻塞。 +- RGB 与深度需要配对时，应使用消息时间戳进行近似同步，而不是假设两个回调严格交错。 +- `depth_to_color` 和 `CameraInfo` 必须随相机配置一起使用；不能用另一台相机的内参替代。 +- `/dev/video*` 只由 Orbbec/`proc_manager` 负责，应用节点不再调用 V4L2 抢占设备。 diff --git a/robot/ros2/OmniSocketGo_robot_ros/go/README.md b/robot/ros2/OmniSocketGo_robot_ros/go/README.md new file mode 100644 index 0000000..770f75f --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/go/README.md @@ -0,0 +1,94 @@ +# OmniSocketGo + +Linux only. Go 1.22. + +如果目标机器只运行 `server`，只需要编译并拷贝 `server` 二进制。 +如果目标机器只运行 `peer`，只需要编译并拷贝 `peer` 二进制。 + +`go build ./cmd/server` 和 `go build ./cmd/peer` 会把各自依赖到的功能一起编译进最终二进制，不需要再单独编译 `cmd/internal/...` 包。 + +- `server` 二进制会包含它依赖到的转发、协议、传输等代码 +- `peer` 二进制会包含它依赖到的注册、交互发送、接收落盘、协议、传输等代码 +- 只有没有被这个可执行程序引用的其他命令，才不在该二进制里，比如 `cmd/latencysummary` + +## Build + +按目标架构分别编译。 + mkdir -p bin + go build -o bin/server ./cmd/server + go build -o bin/peer ./cmd/peer + go build -o bin/latencysummary ./cmd/latencysummary + +### Linux amd64 + +```bash +CGO_ENABLED=0 GOOS=linux GOARCH=amd64 go build -o bin/server-linux-amd64 ./cmd/server +``` + +### Linux arm64 + +```bash +CGO_ENABLED=0 GOOS=linux GOARCH=arm64 go build -o bin/peer-linux-arm64 ./cmd/peer +``` + +### Linux armv7 + +```bash +CGO_ENABLED=0 GOOS=linux GOARCH=arm GOARM=7 go build -o bin/server-linux-armv7 ./cmd/server +CGO_ENABLED=0 GOOS=linux GOARCH=arm GOARM=7 go build -o bin/peer-linux-armv7 ./cmd/peer +``` + + + +## Run On Different Machines + +`server D` 所在机器监听 `0.0.0.0:10909`。 + +```bash +go run cmd/kcpserver/ -listen 0.0.0.0:10909 +-kcp-ts-debug-log logs/d-kcp-ts.jsonl -kcp-session-stats-log logs/d-kcp-stats.jsonl +``` + +`relay server C` 所在机器 + +```bash +go run ./cmd/kcpserver/ -mode=relay -listen 0.0.0.0:10909 -relay-remote 172.21.32.15:10909 + +2>&1 | tee logs/c.stdout.log +``` + +### peer-a (A) + +```bash +go run ./cmd/kcppeer/ -id peer-a -server 172.21.32.15:10909 -relay-via 106.55.173.235:10909 -inbox-dir inbox/a + +-latency-log logs/a-latency.jsonl -kcp-ts-debug-log logs/a-kcp-ts.jsonl -kcp-session-stats-log logs/a-kcp-stats.jsonl + +go run ./cmd/kcpping/ -id peer-a -server 106.55.173.235:10909 -echo +``` + +### peer-b (B) + +```bash +go run ./cmd/kcppeer/ -id peer-b -server 81.70.156.140:10909 -inbox-dir inbox/b + +-latency-log logs/b-latency.jsonl -kcp-ts-debug-log logs/b-kcp-ts.jsonl -kcp-session-stats-log logs/b-kcp-stats.jsonl + +go run ./cmd/kcpping -id peer-b -server 81.70.156.140:10909 -to peer-a -count 20 -interval 100ms +``` + +## Interactive Commands + +`peer` 启动后可以在终端里持续使用同一条长连接发送多次消息。 + +```text +help +text peer-b hello +text peer-a hi +file peer-a /tmp/test125.bin +file peer-a /tmp/test5.bin +quit +``` +### 自动化拉取更新汇总数据 +cd /home/limingjie/LMJ_Work/OmniSocketGo +./scripts/refresh-latency-summary.sh \ No newline at end of file diff --git a/robot/ros2/OmniSocketGo_robot_ros/go/change_to_c.md b/robot/ros2/OmniSocketGo_robot_ros/go/change_to_c.md new file mode 100644 index 0000000..675c8a4 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/go/change_to_c.md @@ -0,0 +1,465 @@ +OmniSocketGo -> OmniSocketC 转换计划 + + Context + + 将现有的 Go 语言实现的 UDP/KCP 传输层项目 (OmniSocketGo) 转换为纯 C 语言项目，运行在 Linux 系统上。 + + 原项目架构：A(Jetson) <-> C(relay cloud) <-> D(hub cloud) <-> B(host) + - B <-> D：KCP 链路 + - D <-> C：UDP relay 转发 + - C <-> A：KCP 链路（A 通过 relay C 连接到 hub D） + - 最终目的：B 和 A 之间双向传输数据 + + 转换要求： + - 只保留 UDP 和 KCP，不需要 TCP + - 不需要写测试 + - 完整实现协议层、传输层、日志事件系统 + - Linux only + + 项目位置 + + OmniSocketGo/c/ — 作为当前 Go 项目的子目录 + + 项目结构 + + c/ + ├── Makefile + ├── README.md + ├── include/ + │ ├── protocol.h # 协议消息定义 + 编解码 + │ ├── transport_kcp.h # KCP 连接封装 + │ ├── transport_udp.h # UDP 连接封装（含 Linux timestamping） + │ ├── linux_timestamping.h # Linux SO_TIMESTAMPING 底层实现 + │ ├── kcp_packet_debug.h # KCP packet-level kernel timestamp debug logger + │ ├── kcp_session_stats.h # KCP session stats (RTO/SRTT) logger + │ ├── tx_timestamp_debug.h # TX errqueue timestamp debug logger + │ ├── server_kcp_hub.h # KCP Hub (D 节点) + │ ├── server_udp_relay.h # UDP Relay (C 节点) + │ ├── peer_kcp_client.h # KCP Peer Client (A/B 节点) + │ ├── latencylog.h # 延迟日志事件系统 + │ ├── interactive.h # 交互式命令行 + │ └── cJSON.h # JSON 库 (第三方轻量级) + ├── src/ + │ ├── protocol.c + │ ├── transport_kcp.c + │ ├── transport_udp.c + │ ├── linux_timestamping.c + │ ├── kcp_packet_debug.c + │ ├── kcp_session_stats.c + │ ├── tx_timestamp_debug.c + │ ├── server_kcp_hub.c + │ ├── server_udp_relay.c + │ ├── peer_kcp_client.c + │ ├── latencylog.c + │ ├── interactive.c + │ └── cJSON.c + ├── cmd/ + │ ├── kcpserver.c # 主程序: KCP Hub 或 UDP Relay + │ ├── kcppeer.c # 主程序: KCP Peer (A/B) + │ └── kcpping.c # 主程序: KCP Ping 工具 + └── third_party/ + └── kcp/ + ├── ikcp.h # KCP 协议核心实现 (github.com/skywind3000/kcp) + └── ikcp.c + + 依赖说明 + + - KCP: 使用 skywind3000/kcp 的原始 C 实现 (ikcp.h/ikcp.c)，替代 Go 的 xtaci/kcp-go/v5 + - JSON: 使用 cJSON (DaveGamble/cJSON) 替代 Go 的 encoding/json + - 线程: 使用 pthread 替代 Go goroutine + - 同步: 使用 pthread_mutex/pthread_rwlock 替代 Go sync.Mutex/sync.RWMutex + + 模块实现计划 + + 1. 第三方库集成 + + - 下载 ikcp.h/ikcp.c (skywind3000/kcp) + - 下载 cJSON.h/cJSON.c (DaveGamble/cJSON) + + 2. protocol.h / protocol.c + + 对应 Go: cmd/internal/protocol/message.go + codec.go + + // 消息类型 + typedef enum { + MSG_TYPE_TEXT = 0, + MSG_TYPE_FILE = 1, + MSG_TYPE_REGISTER = 2, + MSG_TYPE_ERROR = 3, + } message_type_t; + + // 消息结构 + typedef struct { + message_type_t type; + uint64_t id; + char from[64]; + char to[64]; + char file_name[256]; + uint8_t *body; + int body_len; + } message_t; + + #define MAX_FRAME_SIZE (8 * 1024 * 1024) + #define SERVER_PEER_ID "server" + + 核心函数： + - int protocol_encode_message(const message_t *msg, uint8_t **out, int *out_len) — 编码消息为 [4B headerLen][header JSON][body] + - int protocol_decode_message(const uint8_t *data, int data_len, message_t *msg) — 解码 + - int protocol_write_frame(int fd, const uint8_t *payload, int payload_len) — 写带长度前缀的帧 (用于 KCP stream) + - int protocol_read_frame(int fd, uint8_t **payload, int *payload_len) — 读帧 + - int protocol_write_message(int fd, const message_t *msg) — 完整编码+写帧 + - int protocol_read_message(int fd, message_t *msg) — 读帧+解码 + - int protocol_validate_message(const message_t *msg) — 校验 + - void message_free(message_t *msg) — 释放 body 内存 + + 注意: KCP session 在 stream 模式下行为类似 TCP，需要 [4B frameLen] 前缀来分帧。 + + 3. latencylog.h / latencylog.c + + 对应 Go: cmd/internal/latencylog/logger.go + + // 事件名常量 + #define EVENT_A_APP_PREP_BEGIN "A_APP_PREP_BEGIN" + #define EVENT_SEND_HANDOFF_BEGIN "send_handoff_begin" + #define EVENT_SEND_HANDOFF_END "send_handoff_end" + #define EVENT_B_APP_RECV "B_APP_RECV" + #define EVENT_B_PERSIST_BEGIN "B_PERSIST_BEGIN" + #define EVENT_B_PERSIST_END "B_PERSIST_END" + // ... 其他事件 + + typedef struct { + int64_t ts_unix_nano; + char node_role[16]; + char node_id[64]; + char event[32]; + message_type_t message_type; + uint64_t message_id; + char from[64]; + char to[64]; + char file_name[256]; + int body_size; + } latency_event_t; + + typedef struct latency_logger latency_logger_t; + + 核心函数： + - latency_logger_t *latencylog_new_jsonl(const char *path) — 创建 JSONL 文件日志器 + - void latencylog_log_event(latency_logger_t *logger, const latency_event_t *event) — 写事件 + - void latencylog_log_message_event(latency_logger_t *logger, const char *node_role, const char *node_id, const char *event_name, const message_t + *msg) — 为业务消息记事件 + - void latencylog_close(latency_logger_t *logger) — 关闭 + - int latencylog_is_business_message(const message_t *msg) — 判断是否业务消息 + + 4. transport_kcp.h / transport_kcp.c + + 对应 Go: cmd/internal/transport/kcp.go + kcp_packet_conn.go + + KCP 连接封装，底层用 raw ikcp + UDP socket： + + typedef struct kcp_conn { + ikcpcb *kcp; + int udp_fd; + struct sockaddr_in remote_addr; + pthread_mutex_t write_mu; + pthread_t recv_thread; // 底层 UDP -> ikcp_input 的线程 + latency_logger_t *logger; + char node_role[16]; + char node_id[64]; + int closed; + } kcp_conn_t; + + 核心函数： + - kcp_conn_t *kcp_conn_dial(const char *server_addr, const char *bind_ip, const char *bind_device) — 客户端拨号 + - kcp_conn_t *kcp_conn_accept(int udp_fd, struct sockaddr_in *remote, uint32_t conv) — 服务端接受 + - int kcp_conn_send(kcp_conn_t *conn, const message_t *msg) — 发送消息 + - int kcp_conn_receive(kcp_conn_t *conn, message_t *msg) — 接收消息 + - void kcp_conn_close(kcp_conn_t *conn) — 关闭 + + KCP 配置参数（与 Go 版一致）： + #define KCP_NODELAY 1 + #define KCP_INTERVAL 10 + #define KCP_RESEND 2 + #define KCP_NC 1 + #define KCP_WND_SIZE 256 + #define KCP_MTU 1400 + + KCP 底层架构说明： + Go 版使用 kcp-go 库，该库内部维护了一个 Listener 来多路复用一个 UDP socket 上的多个 KCP session（通过 conv ID 区分）。在 C 中需要自行实现： + - 服务端：一个 UDP socket 监听，一个接收线程读取所有 UDP 包，根据 conv ID 分发到对应的 ikcpcb + - 客户端：一个 UDP socket，一个 ikcpcb，一个后台线程负责 UDP recv -> ikcp_input + + 5. transport_udp.h / transport_udp.c + + 对应 Go: cmd/internal/transport/udp.go + udp_linux.go + + typedef struct udp_conn { + int fd; + struct sockaddr_in peer_addr; + syscall_rawconn_t raw; // syscall.RawConn 等价 + int linux_timestamping_enabled; + latency_logger_t *logger; + tx_timestamp_debug_logger_t *tx_debug_logger; + uint32_t tx_packet_seq; + // pending TX records for errqueue correlation + struct udp_tx_pending *pending_tx; + char node_role[16]; + char node_id[64]; + pthread_mutex_t write_mu; + } udp_conn_t; + + 完整实现 Linux SO_TIMESTAMPING： + - TX: SOF_TIMESTAMPING_TX_SCHED + SOF_TIMESTAMPING_TX_SOFTWARE + OPT_ID + - RX: SOF_TIMESTAMPING_RX_SOFTWARE + - errqueue 采集: recvmsg(MSG_ERRQUEUE) 读取 SCM_TIMESTAMPING 控制消息 + - TX timestamp debug logger: 记录 send_chunk / errqueue_event 到 JSONL + - 对应 Go 文件: udp_linux.go, tx_timestamp_debug.go + + 同时为 KCP packet conn 实现类似的 timestamping： + - 对应 Go 文件: kcp_packet_conn_linux.go, kcp_packet_debug.go + - KCP 底层 UDP 包的 TX/RX kernel timestamp 记录 + + KCP session stats 完整实现： + - session-level: conv, RTO, SRTT, SRTTVar 周期采样 + - 对应 Go 文件: kcp_session_stats.go + + 6. server_kcp_hub.h / server_kcp_hub.c + + 对应 Go: cmd/internal/server/kcp_hub.go + + typedef struct { + pthread_rwlock_t lock; + // peer_id -> kcp_conn_t* 的哈希表 + struct peer_entry *peers; // 简单链表或哈希表 + int peer_count; + latency_logger_t *logger; + // relay 相关 + int relay_udp_fd; + struct sockaddr_in relay_peer_addr; + int relay_peer_known; + } kcp_hub_t; + + 核心函数： + - kcp_hub_t *kcp_hub_new(latency_logger_t *logger) — 创建 hub + - int kcp_hub_serve_session(kcp_hub_t *hub, kcp_conn_t *conn) — 处理新会话（注册 + 转发循环） + - void kcp_hub_set_relay(kcp_hub_t *hub, int udp_fd, struct sockaddr_in *peer_addr) — 配置 relay + - int kcp_hub_serve_relay(kcp_hub_t *hub) — relay 接收循环 + - void kcp_hub_free(kcp_hub_t *hub) — 释放 + + 服务端 KCP listener 实现： + - 主 UDP socket 监听 + - 收到新 conv ID 时创建新 ikcpcb + - 用 pthread 为每个 session 创建处理线程 + + 7. server_udp_relay.h / server_udp_relay.c + + 对应 Go: cmd/internal/server/udp_relay.go + + typedef struct { + int downstream_fd; // 监听端 + int upstream_fd; // 连接到 hub D 的 UDP + struct sockaddr_in upstream_addr; + struct sockaddr_in client_addr; + int client_known; + pthread_mutex_t lock; + } udp_relay_t; + + 核心函数： + - udp_relay_t *udp_relay_new(int listen_fd, struct sockaddr_in *upstream_addr) — 创建 + - int udp_relay_serve(udp_relay_t *relay) — 双向转发循环（两个线程） + - void udp_relay_close(udp_relay_t *relay) — 关闭 + + 8. peer_kcp_client.h / peer_kcp_client.c + + 对应 Go: cmd/internal/peer/kcp_client.go + persist.go + + typedef struct { + char id[64]; + kcp_conn_t *conn; + latency_logger_t *logger; + uint64_t next_msg_id; // atomic + pthread_mutex_t id_mu; + } kcp_client_t; + + 核心函数： + - kcp_client_t *kcp_client_dial(const char *server_addr, const char *peer_id, ...) — 连接并注册 + - int kcp_client_send_text(kcp_client_t *c, const char *to, const char *text) — 发文本 + - int kcp_client_send_file(kcp_client_t *c, const char *to, const char *path) — 发文件 + - int kcp_client_receive(kcp_client_t *c, message_t *msg) — 接收 + - int kcp_client_persist_message(kcp_client_t *c, const message_t *msg, const char *inbox_dir) — 持久化 + - void kcp_client_close(kcp_client_t *c) — 关闭 + + 9. interactive.h / interactive.c + + 对应 Go: cmd/kcppeer/interactive.go + + 交互式命令行 REPL： + - help / text / file / quit + - int run_interactive_shell(kcp_client_t *client) — 运行交互循环 + + 10. cmd/kcpserver.c + + 对应 Go: cmd/kcpserver/main.go + + 用法: + kcpserver -listen 0.0.0.0:10909 # hub 模式 + kcpserver -mode relay -listen 0.0.0.0:10909 -relay-remote 172.21.32.15:10909 # relay 模式 + + - 解析命令行参数 (getopt) + - hub 模式：创建 KCP listener -> 接受连接 -> kcp_hub_serve_session + - relay 模式：创建 UDP relay -> udp_relay_serve + + 11. cmd/kcppeer.c + + 对应 Go: cmd/kcppeer/main.go + + 用法: + kcppeer -id peer-a -server 172.21.32.15:10909 -relay-via 106.55.173.235:10909 -inbox-dir inbox/a + kcppeer -id peer-b -server 81.70.156.140:10909 -inbox-dir inbox/b + + - 连接到 KCP server + - 启动接收线程 + - 运行交互式 shell 或单次发送 + + 12. cmd/kcpping.c + + 对应 Go: cmd/kcpping/main.go + platform_linux.go + + KCP ping 工具： + - ping 模式: 发 JSON payload, 计算 RTT + - echo 模式: 回弹文本消息 + - 统计: min/avg/max/p50/p95/p99/stddev + + KCP session 多路复用实现（核心难点） + + Go 版的 kcp-go 库在一个 UDP socket 上透明地多路复用多个 KCP session。C 版需要手动实现： + + typedef struct kcp_listener { + int udp_fd; + pthread_t recv_thread; + pthread_mutex_t sessions_lock; + // conv -> kcp_session 的哈希表 + struct kcp_session_entry *sessions; + // 新会话通知队列 + kcp_conn_t **accept_queue; + int accept_queue_head, accept_queue_tail, accept_queue_cap; + pthread_mutex_t accept_lock; + pthread_cond_t accept_cond; + } kcp_listener_t; + + - kcp_listener_t *kcp_listen(const char *addr, const char *bind_device) — 创建 listener + - kcp_conn_t *kcp_accept(kcp_listener_t *listener) — 阻塞等待新会话 + - 内部 recv_thread 循环读 UDP 包，解析前 4 字节 conv ID，分发到对应 ikcpcb + - 未知 conv ID 时创建新 session 并放入 accept_queue + + 编译 + + CC = gcc + CFLAGS = -Wall -Wextra -O2 -pthread -D_GNU_SOURCE + LDFLAGS = -lpthread + + SRCS = src/protocol.c src/transport_kcp.c src/transport_udp.c \ + src/server_kcp_hub.c src/server_udp_relay.c \ + src/peer_kcp_client.c src/latencylog.c src/interactive.c \ + src/cJSON.c third_party/kcp/ikcp.c + + all: kcpserver kcppeer kcpping + + kcpserver: cmd/kcpserver.c $(SRCS) + $(CC) $(CFLAGS) -Iinclude -Ithird_party/kcp -o $@ $^ $(LDFLAGS + + kcppeer: cmd/kcppeer.c $(SRCS) + $(CC) $(CFLAGS) -Iinclude -Ithird_party/kcp -o $@ $^ $(LDFLAGS + + kcpping: cmd/kcpping.c $(SRCS) + $(CC) $(CFLAGS) -Iinclude -Ithird_party/kcp -o $@ $^ $(LDFLAGS + + 验证方法 + + 1. 编译: make all 无错误无警告 + 2. 单机测试: + - 启动 hub: ./kcpserver -listen 0.0.0.0:10909 + - 启动 peer-a: ./kcppeer -id peer-a -server 127.0.0.1:10909 -inbox-dir inbox/a + - 启动 peer-b: ./kcppeer -id peer-b -server 127.0.0.1:10909 -inbox-dir inbox/b + - peer-b shell 中: text peer-a hello + - 验证 peer-a 收到消息并落盘到 inbox/a/ + 3. 跨机器 relay 测试: + - D 机器: ./kcpserver -listen 0.0.0.0:10909 + - C 机器: ./kcpserver -mode relay -listen 0.0.0.0:10909 -relay-remote :10909 + - A 机器: ./kcppeer -id peer-a -server :10909 -relay-via :10909 -inbox-dir inbox/a + - B 机器: ./kcppeer -id peer-b -server :10909 -inbox-dir inbox/b + 4. kcpping 测试: + - echo 端: ./kcpping -id peer-a -server :10909 -echo + - ping 端: ./kcpping -id peer-b -server :10909 -to peer-a -count 20 -interval 100 + + 实现顺序 + + 1. 集成第三方库 (ikcp, cJSON) + 2. protocol 模块 (消息编解码) + 3. latencylog 模块 (日志事件) + 4. transport_kcp 模块 (KCP 连接 + listener 多路复用) + 5. transport_udp 模块 (UDP 连接，简化 timestamping) + 6. server_udp_relay 模块 (C 节点 relay) + 7. server_kcp_hub 模块 (D 节点 hub) + 8. peer_kcp_client 模块 (A/B 节点 peer + persist) + 9. interactive 模块 (交互 shell) + 10. cmd/kcpserver.c 主程序 + 11. cmd/kcppeer.c 主程序 + 12. cmd/kcpping.c 主程序 + 13. Makefile + README + 14. 编译测试 + + 简化决策 + + - 不实现 TCP 传输: 去除 transport/tcp.go, server/hub.go(TCP版), peer/client.go(TCP版) 等 TCP 相关代码 + - 不写测试: 去除所有 _test.go 对应的测试代码 + - 完整实现 Linux timestamping: 完整移植 SO_TIMESTAMPING 的 TX/RX timestamp 采集，包括 errqueue TX sched/software timestamp 和 RX software + timestamp，以及对应的 debug logger (KCPPacketDebugLogger, TXTimestampDebugLogger) + - 完整实现 KCP session stats: 包括 session-level RTO/SRTT 采样和 JSONL 记录 + - 不实现 latency summary/chart: 不实现 latencysummary 工具和 HTML chart 生成（这是离线分析工具，不属于核心传输功能） + - peer 哈希表: 使用简单链表实现，hub 连接数不多时性能足够 + + +# OmniSocketGo -> OmniSocketC 全量 UDP/KCP 迁移计划 + +## Summary +- 在仓库新增 `c/` 子项目，作为 Linux-only、C11、`make` 驱动的独立实现；现有 Go 项目保留不动，作为行为对照。 +- 迁移范围按“全量 Go 对齐，但去掉 TCP 和离线 summary/chart”执行：保留 UDP/KCP 协议、纯 UDP 程序族、KCP 程序族、运行时 JSONL 日志、Linux timestamping、KCP packet debug、KCP session stats、以及 KCP hub-to-hub 内部 relay 能力。 +- 你当前草案需要修正的关键点有 5 个：`protocol_*frame(int fd, ...)` 不适合 KCP；KCP 必须补齐 `ikcp_update/check` 调度与 conv 多路复用；纯 UDP 程序族不能省略；`latencysummary`/HTML chart 本次不迁移；Makefile 需要修正链接目标并统一输出到 `c/bin/`。 + +## Public Interfaces +- 新增二进制：`kcpserver`、`kcppeer`、`kcpping`、`udpserver`、`udppeer`、`udpping`、`udprelay`。 +- `kcpserver` 保留当前 Go 旗标语义：`-mode=hub|relay`、`-listen`、`-bind-device`、`-relay-remote`、deprecated relay aliases、`-latency-log`、`-kcp-ts-debug-log`、`-kcp-session-stats-log`、`-kcp-session-stats-interval`。 +- `kcppeer` 保留当前 Go 旗标语义：`-id`、`-server`、`-relay-via`、`-to`、`-text`、`-file`、`-bind-ip`、`-bind-device`、`-inbox-dir`、`-interactive`、`-latency-log`、`-kcp-ts-debug-log`、`-kcp-session-stats-log`、`-kcp-session-stats-interval`。 +- `kcpping`、`udpserver`、`udppeer`、`udpping`、`udprelay` 的参数与输出行为对齐当前 Go 入口；`udpserver` 默认不开 Linux timestamping，只有设置 `-tx-ts-debug-log` 时才启用。 +- 协议层改为内存接口，不再设计 fd 风格 API：`message_t`、datagram 编解码、stream frame 编解码、增量 frame feed。 +- 运行时日志层保留当前 JSON 字段和事件名；server/hub 继续作为 black-box relay，不新增端到端业务事件。 +- 内部网络 API 包括：`udp_conn_t`、`kcp_conn_t`、`kcp_listener_t`、`udp_hub_t`、`kcp_hub_t`、`udp_relay_t`、`udp_client_t`、`kcp_client_t`；KCP hub-to-hub relay 只做库级能力，不新增额外 CLI。 + +## Implementation Changes +- 目录固定为 `c/include`、`c/src`、`c/cmd`、`c/third_party/{ikcp,cjson}`、`c/bin`、`c/README.md`、`c/Makefile`。 +- 第三方依赖直接 vendoring 到仓库：`ikcp` 用于 KCP 核心，`cJSON` 同时用于协议头、ping payload、运行时日志。 +- 协议规则完全保留：`text/file/register/error`、`ServerPeerID`、`8 MiB` 限制、UTF-8 校验、`file_name` 约束、`register/error` 来源与目标约束。 +- 线上 wire format 完全保留：UDP datagram 为 `[4B headerLen][header JSON][body]`；KCP stream 为 `[4B frameLen][4B headerLen][header JSON][body]`。 +- inbox 持久化完全保留：文本追加写 `messages.log` JSONL；文件落盘为 `--`。 +- UDP 传输层实现 connected/unconnected 两种发送模式，保留 register/forward 消息收发、Linux SO_TIMESTAMPING、TX errqueue 关联、JSONL debug 记录。 +- KCP 客户端连接采用“一连接一 UDP socket + 一 `ikcpcb` + 一接收线程 + 一 update 线程 + 一阻塞接收缓冲区/条件变量”模型。 +- KCP 服务端监听采用“单 listener UDP socket + 单 listener RX 线程 + conv->session 表 + accept 队列”模型；每个 session 拥有自己的 `ikcpcb`、update 线程、接收缓冲区和关闭状态，发送通过 listener 共享 socket 和写锁完成。 +- `kcpserver` 的 relay 模式保持为原始 UDP 端口转发，不解码协议；`udprelay` 同样保持透明字节转发。 +- 纯 UDP hub、KCP hub、双 peer、双 ping 工具、两套 interactive shell 全部对齐现有 Go 行为。 +- KCP hub 保留“先本地投递，再尝试 relay”的策略；未知目标、重复注册、已注册 peer 再发 `register/error`、过大 relay 消息等错误路径全部保留。 +- Linux 观测能力完整迁移：业务事件 JSONL、UDP TX debug、KCP packet debug、KCP session/process stats；不迁移 `latencysummary` 与 HTML chart。 + +## Acceptance +- 在 Linux 上执行 `make` 能无缺失符号地构建 7 个二进制，并输出到 `c/bin/`。 +- 纯 UDP 冒烟通过：`udpserver` + 两个 `udppeer` 可双向收发文本和文件，`udpping` 的 echo/ping 正常。 +- 单 hub KCP 冒烟通过：`kcpserver` + 两个 `kcppeer` 可双向收发文本和文件，`kcpping` 的 echo/ping 正常。 +- README 目标拓扑通过：D 跑 `kcpserver -mode=hub`，C 跑 `kcpserver -mode=relay`，A 用 `-relay-via C` 连 D，B 直连 D，A/B 双向传输正常。 +- 全量 Go 对齐场景通过：两个 KCP hub 通过内部 raw UDP relay API 互通，跨 hub 文本、文件、错误回送行为与当前 Go 一致。 +- 负路径通过：重复注册被拒、未注册 UDP sender 被拒、未知目标返回 `error`、已注册 peer 发送 `register/error` 被拒、oversize relayed message 在实际 `WriteTo` 前被拒、`bind-ip`/`bind-device` 非法值在启动时失败。 +- 打开任一日志旗标后，生成的 JSONL 记录字段名、事件名、时间戳语义与现有运行时日志一致，并在 Linux 支持的情况下出现非零 kernel timestamps。 + +## Assumptions +- 默认编译器为 `gcc`/`clang`，编译参数基线为 `-std=c11 -Wall -Wextra -O2 -pthread -D_GNU_SOURCE`。 +- 本次不迁移任何 Go 测试文件，也不为 C 版编写自动化测试；验证仅靠 Linux 构建和手工场景回归。 +- 本次不迁移 TCP 入口，也不迁移 `latencysummary`/HTML chart。 +- hub-to-hub relay 在 C 版中实现为内部库能力，保持与当前 Go 仓库一致的范围，不额外扩展新的公共命令。 diff --git a/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/latencylog/logger.go b/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/latencylog/logger.go new file mode 100644 index 0000000..f1f1f31 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/latencylog/logger.go @@ -0,0 +1,166 @@ +package latencylog + +import ( + "encoding/json" + "os" + "path/filepath" + "sync" + "time" + + "omnisocketgo/cmd/internal/protocol" +) + +const ( + NodeRolePeer = "peer" //客户端节点 + NodeRoleServer = "server" //云端转发节点 +) + +// 记录的消息事件的类型常量。 +const ( + EventAAppPrepBegin = "A_APP_PREP_BEGIN" // A 端应用开始准备这条消息 + EventATXSched = "A_TX_SCHED" // A 端进入 Linux qdisc 之前 + EventATXSoftware = "A_TX_SOFTWARE" // A 端即将交给网卡驱动 + EventATXHardware = "A_TX_HARDWARE" // A 端网卡真正发出到物理介质 + EventBRXHardware = "B_RX_HARDWARE" // B 端网卡真正从物理介质收到 + EventBRXSoftware = "B_RX_SOFTWARE" // B 端驱动把数据交给 Linux 接收栈 + EventBAppRecv = "B_APP_RECV" // B 端应用真正读到完整消息 + EventBPersistBegin = "B_PERSIST_BEGIN" // B 端开始写盘 + EventBPersistEnd = "B_PERSIST_END" // B 端写盘完成 + + EventSendHandoffBegin = "send_handoff_begin" // 调试事件：应用把消息交给传输层开始 + EventSendHandoffEnd = "send_handoff_end" // 调试事件：应用把消息交给传输层结束 +) + +// Event 是一条时延时间戳日志记录。 +type Event struct { + TsUnixNano int64 `json:"ts_unix_nano"` + NodeRole string `json:"node_role"` + NodeID string `json:"node_id"` + Event string `json:"event"` + MessageType protocol.MessageType `json:"message_type"` + MessageID uint64 `json:"message_id"` + From string `json:"from"` + To string `json:"to"` + FileName string `json:"file_name,omitempty"` + BodySize int `json:"body_size"` +} + +// Logger 负责接收事件并将其写入外部介质。 +type Logger interface { + LogEvent(Event) error +} + +// NoopLogger 是默认的空实现。 +type NoopLogger struct{} + +// LogEvent 对空日志实现始终返回 nil。 +func (NoopLogger) LogEvent(Event) error { + return nil +} + +// JSONLLogger 以 JSONL 形式追加写日志文件。 +type JSONLLogger struct { + mu sync.Mutex + closeOnce sync.Once + closeErr error + file *os.File +} + +// NewJSONLLogger 创建一个线程安全的 JSONL 文件日志器。 +func NewJSONLLogger(path string) (*JSONLLogger, error) { + dir := filepath.Dir(path) + if err := os.MkdirAll(dir, 0o755); err != nil { + return nil, err + } + + file, err := os.OpenFile(path, os.O_CREATE|os.O_WRONLY|os.O_APPEND, 0o644) + if err != nil { + return nil, err + } + + return &JSONLLogger{file: file}, nil +} + +// LogEvent 以单行 JSON 的形式追加一条事件。 +func (l *JSONLLogger) LogEvent(event Event) error { + line, err := json.Marshal(event) + if err != nil { + return err + } + + l.mu.Lock() + defer l.mu.Unlock() + + if _, err := l.file.Write(append(line, '\n')); err != nil { + return err + } + + return nil +} + +// Close 关闭底层文件；重复调用是安全的。 +func (l *JSONLLogger) Close() error { + l.closeOnce.Do(func() { + l.closeErr = l.file.Close() + }) + + return l.closeErr +} + +// IsBusinessMessage 判断消息是否属于要参与 A-C-B 时延分析的业务消息。 +func IsBusinessMessage(msg protocol.Message) bool { + switch msg.Type { + case protocol.MessageTypeText, protocol.MessageTypeFile: + return true + default: + return false + } +} + +// NewMessageEvent 用当前 UTC 时间为一条业务消息构造事件。 +func NewMessageEvent(nodeRole, nodeID, eventName string, msg protocol.Message) Event { + return NewMessageEventAt(time.Now().UTC().UnixNano(), nodeRole, nodeID, eventName, msg) +} + +// NewMessageEventAt 用指定的 UnixNano 时间为一条业务消息构造事件。 +func NewMessageEventAt(tsUnixNano int64, nodeRole, nodeID, eventName string, msg protocol.Message) Event { + return Event{ + TsUnixNano: tsUnixNano, + NodeRole: nodeRole, + NodeID: nodeID, + Event: eventName, + MessageType: msg.Type, + MessageID: msg.ID, + From: msg.From, + To: msg.To, + FileName: msg.FileName, + BodySize: len(msg.Body), + } +} + +// LogBestEffort 写一条事件，失败时静默忽略，避免打断主收发流程。 +func LogBestEffort(logger Logger, event Event) { + if logger == nil { + return + } + + _ = logger.LogEvent(event) +} + +// LogMessageEvent 为业务消息构造并写入一条事件。 +func LogMessageEvent(logger Logger, nodeRole, nodeID, eventName string, msg protocol.Message) { + if !IsBusinessMessage(msg) { + return + } + + LogBestEffort(logger, NewMessageEvent(nodeRole, nodeID, eventName, msg)) +} + +// LogMessageEventAt 为业务消息写入一条指定时间戳的事件。 +func LogMessageEventAt(logger Logger, nodeRole, nodeID, eventName string, tsUnixNano int64, msg protocol.Message) { + if !IsBusinessMessage(msg) { + return + } + + LogBestEffort(logger, NewMessageEventAt(tsUnixNano, nodeRole, nodeID, eventName, msg)) +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/latencylog/logger_test.go b/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/latencylog/logger_test.go new file mode 100644 index 0000000..d1850fe --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/latencylog/logger_test.go @@ -0,0 +1,131 @@ +package latencylog + +import ( + "bufio" + "encoding/json" + "os" + "path/filepath" + "sync" + "testing" + + "omnisocketgo/cmd/internal/protocol" +) + +func TestJSONLLoggerWritesOneEventPerLine(t *testing.T) { + path := filepath.Join(t.TempDir(), "latency.jsonl") + + logger, err := NewJSONLLogger(path) + if err != nil { + t.Fatalf("NewJSONLLogger() error = %v", err) + } + t.Cleanup(func() { + _ = logger.Close() + }) + + event := Event{ + TsUnixNano: 123, + NodeRole: NodeRolePeer, + NodeID: "peer-a", + Event: EventAAppPrepBegin, + MessageType: protocol.MessageTypeText, + MessageID: 1, + From: "peer-a", + To: "peer-b", + BodySize: 5, + } + if err := logger.LogEvent(event); err != nil { + t.Fatalf("LogEvent() error = %v", err) + } + + file, err := os.Open(path) + if err != nil { + t.Fatalf("os.Open() error = %v", err) + } + defer file.Close() + + scanner := bufio.NewScanner(file) + if !scanner.Scan() { + t.Fatal("expected one JSONL line, got none") + } + + var got Event + if err := json.Unmarshal(scanner.Bytes(), &got); err != nil { + t.Fatalf("json.Unmarshal() error = %v", err) + } + if got != event { + t.Fatalf("event mismatch: got %+v want %+v", got, event) + } + if scanner.Scan() { + t.Fatal("expected exactly one JSONL line") + } + if err := scanner.Err(); err != nil { + t.Fatalf("scanner.Err() = %v", err) + } +} + +func TestJSONLLoggerHandlesConcurrentWrites(t *testing.T) { + path := filepath.Join(t.TempDir(), "latency.jsonl") + + logger, err := NewJSONLLogger(path) + if err != nil { + t.Fatalf("NewJSONLLogger() error = %v", err) + } + t.Cleanup(func() { + _ = logger.Close() + }) + + const total = 32 + + var wg sync.WaitGroup + for i := 0; i < total; i++ { + i := i + wg.Add(1) + go func() { + defer wg.Done() + + err := logger.LogEvent(Event{ + TsUnixNano: int64(i + 1), + NodeRole: NodeRoleServer, + NodeID: protocol.ServerPeerID, + Event: EventBAppRecv, + MessageType: protocol.MessageTypeFile, + MessageID: uint64(i + 1), + From: "peer-a", + To: "peer-b", + FileName: "payload.bin", + BodySize: 3, + }) + if err != nil { + t.Errorf("LogEvent() error = %v", err) + } + }() + } + wg.Wait() + + file, err := os.Open(path) + if err != nil { + t.Fatalf("os.Open() error = %v", err) + } + defer file.Close() + + scanner := bufio.NewScanner(file) + var count int + seen := make(map[uint64]bool, total) + for scanner.Scan() { + var event Event + if err := json.Unmarshal(scanner.Bytes(), &event); err != nil { + t.Fatalf("json.Unmarshal() error = %v", err) + } + count++ + seen[event.MessageID] = true + } + if err := scanner.Err(); err != nil { + t.Fatalf("scanner.Err() = %v", err) + } + if count != total { + t.Fatalf("line count = %d, want %d", count, total) + } + if len(seen) != total { + t.Fatalf("unique message count = %d, want %d", len(seen), total) + } +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/latencylog/summary.go b/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/latencylog/summary.go new file mode 100644 index 0000000..dd825d1 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/latencylog/summary.go @@ -0,0 +1,457 @@ +package latencylog + +import ( + "bufio" + "encoding/json" + "fmt" + "os" + "path/filepath" + "sort" + + "omnisocketgo/cmd/internal/protocol" +) + +// Summary 是针对单条消息的时延的规则列表。 +var requiredTimestampNames = []string{ + EventAAppPrepBegin, // A 端应用开始准备这条消息 + EventATXSched, // A 端进入 Linux qdisc 之前 + EventATXSoftware, // A 端即将交给网卡驱动 + EventBRXSoftware, // B 端网卡驱动把数据交给 Linux 接收栈 + EventBAppRecv, // B 端应用真正读到完整消息 + EventBPersistEnd, // B 端写盘完成 +} + +// Summary 是针对单条消息的时延整理结果。 +type Summary struct { + MessageType protocol.MessageType `json:"message_type"` //消息类型 + MessageID uint64 `json:"message_id"` //消息ID + From string `json:"from"` //发送方 + To string `json:"to"` //接收方 + FileName string `json:"file_name,omitempty"` //文件名（仅文件消息） + BodySize int `json:"body_size"` //消息体大小（字节数） + Timestamps map[string]int64 `json:"timestamps"` //事件时间戳，key 是事件名称，value 是 UnixNano 时间戳 + + AProcessingLatencyNS *int64 `json:"a_processing_latency_ns,omitempty"` // A 处理时延：A_TX_SCHED - A_APP_PREP_BEGIN + AQueueLatencyNS *int64 `json:"a_queue_latency_ns,omitempty"` // A 排队时延：A_TX_SOFTWARE - A_TX_SCHED + ABTransportPropagationNS *int64 `json:"a_b_transport_propagation_ns,omitempty"` // A-B 传输+传播时延近似：B_APP_RECV - A_TX_SOFTWARE + BKernelReceivePathLatencyNS *int64 `json:"b_kernel_receive_path_latency_ns,omitempty"` // B 内核接收路径近似：B_APP_RECV - B_RX_SOFTWARE + BProcessingLatencyNS *int64 `json:"b_processing_latency_ns,omitempty"` // B 处理时延：B_PERSIST_END - B_APP_RECV + EndToEndLatencyNS *int64 `json:"end_to_end_latency_ns,omitempty"` // 端到端时延：B_PERSIST_END - A_APP_PREP_BEGIN + AProcessingBitrateBPS *float64 `json:"a_processing_bitrate_bps,omitempty"` // A 处理阶段近似比特率：(BodySize * 8) / A 处理时延（秒） + ABTransportPropagationBitrateBPS *float64 `json:"a_b_transport_propagation_bitrate_bps,omitempty"` // A-B 传输+传播阶段近似比特率：(BodySize * 8) / A-B 传输+传播时延（秒） + EndToEndBitrateBPS *float64 `json:"end_to_end_bitrate_bps,omitempty"` // 端到端近似比特率：(BodySize * 8) / 端到端时延（秒） + ApproxRTTNS *int64 `json:"approx_rtt_ns,omitempty"` // 近似 RTT：首条反向应答的 B_APP_RECV - 当前请求的 A_TX_SOFTWARE + MissingTimestamps []string `json:"missing_timestamps,omitempty"` // 缺失的时间戳列表，包含 requiredTimestampNames 中但在原始事件中没有的事件名称 +} + +// LoadEventsFromFiles 从JSONL 原始日志文件中加载事件。 +type messageKey struct { + MessageType protocol.MessageType //消息类型 + MessageID uint64 //消息ID + From string //发送方 + To string //接收方 +} + +// LoadEventsFromFiles 从多个 JSONL 原始日志文件中加载事件。 +func LoadEventsFromFiles(paths []string) ([]Event, error) { + var events []Event + for _, path := range paths { + fileEvents, err := LoadEventsFromFile(path) + if err != nil { + return nil, err + } + events = append(events, fileEvents...) + } + + return events, nil +} + +// LoadEventsFromFilesWithSharedMaxOffset 从多个 JSONL 原始日志文件中加载事件， +// 并按每个输入文件的最大 message_id 计算共享截断点。 +func LoadEventsFromFilesWithSharedMaxOffset(paths []string, sharedMaxOffset uint64) ([]Event, *uint64, error) { + eventsByFile := make([][]Event, 0, len(paths)) + var minMaxMessageID uint64 + hasSharedMax := false + + for _, path := range paths { + fileEvents, err := LoadEventsFromFile(path) + if err != nil { + return nil, nil, err + } + + eventsByFile = append(eventsByFile, fileEvents) + + fileMaxMessageID, ok := maxBusinessMessageID(fileEvents) + if !ok { + return nil, nil, nil + } + if !hasSharedMax || fileMaxMessageID < minMaxMessageID { + minMaxMessageID = fileMaxMessageID + hasSharedMax = true + } + } + + if !hasSharedMax { + return nil, nil, nil + } + + cutoff, ok := subtractUint64(minMaxMessageID, sharedMaxOffset) + if !ok { + return []Event{}, nil, nil + } + + var events []Event + for _, fileEvents := range eventsByFile { + events = append(events, filterEventsByMaxMessageID(fileEvents, cutoff)...) + } + + return events, &cutoff, nil +} + +// LoadEventsFromFile 从单个 JSONL 原始日志文件中加载事件。 +func LoadEventsFromFile(path string) ([]Event, error) { + file, err := os.Open(path) + if err != nil { + return nil, fmt.Errorf("latencylog: open raw log %s: %w", path, err) + } + defer file.Close() + + var events []Event + scanner := bufio.NewScanner(file) + for scanner.Scan() { + if len(scanner.Bytes()) == 0 { + continue + } + + var event Event + if err := json.Unmarshal(scanner.Bytes(), &event); err != nil { //解析 JSONL 行失败，返回错误 + return nil, fmt.Errorf("latencylog: decode event from %s: %w", path, err) + } + events = append(events, event) + } + if err := scanner.Err(); err != nil { + return nil, fmt.Errorf("latencylog: scan raw log %s: %w", path, err) + } + + return events, nil +} + +// SummarizeEvents 将原始事件整理成按消息分组的时延结果。 +func SummarizeEvents(events []Event) []Summary { + grouped := make(map[messageKey]*Summary) + + for _, event := range events { + if !IsBusinessEvent(event) { + continue + } + + key := messageKey{ + MessageType: event.MessageType, + MessageID: event.MessageID, + From: event.From, + To: event.To, + } + + summary, ok := grouped[key] + if !ok { + summary = &Summary{ + MessageType: event.MessageType, + MessageID: event.MessageID, + From: event.From, + To: event.To, + FileName: event.FileName, + BodySize: event.BodySize, + Timestamps: make(map[string]int64), + } + grouped[key] = summary + } + + if summary.FileName == "" { + summary.FileName = event.FileName + } + if event.BodySize > 0 { + summary.BodySize = event.BodySize + } + + if existing, exists := summary.Timestamps[event.Event]; !exists || event.TsUnixNano < existing { + summary.Timestamps[event.Event] = event.TsUnixNano + } + } + + summaryPointers := make([]*Summary, 0, len(grouped)) + for _, summary := range grouped { + completeSummary(summary) //补全时延指标和缺失时间戳信息 + summaryPointers = append(summaryPointers, summary) + } + assignApproxRTTs(summaryPointers) + + summaries := make([]Summary, 0, len(summaryPointers)) + for _, summary := range summaryPointers { + summaries = append(summaries, *summary) + } + //对整理结果进行排序，先按发送方、再按接收方、再按消息 ID、最后按消息类型排序，保证输出的稳定性和可读性。 + sort.Slice(summaries, func(i, j int) bool { + if summaries[i].From != summaries[j].From { + return summaries[i].From < summaries[j].From + } + if summaries[i].To != summaries[j].To { + return summaries[i].To < summaries[j].To + } + if summaries[i].MessageID != summaries[j].MessageID { + return summaries[i].MessageID < summaries[j].MessageID + } + return summaries[i].MessageType < summaries[j].MessageType + }) + + return summaries +} + +// WriteSummariesJSONL 将整理结果写成 JSONL 汇总文件。 +func WriteSummariesJSONL(path string, summaries []Summary) error { + if err := os.MkdirAll(filepath.Dir(path), 0o755); err != nil { + return fmt.Errorf("latencylog: create summary dir for %s: %w", path, err) + } + + file, err := os.OpenFile(path, os.O_CREATE|os.O_WRONLY|os.O_TRUNC, 0o644) + if err != nil { + return fmt.Errorf("latencylog: open summary file %s: %w", path, err) + } + defer file.Close() + + writer := bufio.NewWriter(file) + for _, summary := range summaries { //将每条整理结果编码成 JSONL 行并写入文件 + line, err := json.Marshal(summary) + if err != nil { + return fmt.Errorf("latencylog: encode summary for message %d: %w", summary.MessageID, err) + } + if _, err := writer.Write(append(line, '\n')); err != nil { + return fmt.Errorf("latencylog: write summary file %s: %w", path, err) + } + } + + if err := writer.Flush(); err != nil { //将缓冲区内容写入文件 + return fmt.Errorf("latencylog: flush summary file %s: %w", path, err) + } + + return nil +} + +// completeSummary 根据事件时间戳计算时延指标，并找出缺失的时间戳。 +func completeSummary(summary *Summary) { + summary.MissingTimestamps = missingTimestampNames(summary.Timestamps) + + if value := subtractIfPresent(summary.Timestamps, EventATXSched, EventAAppPrepBegin); value != nil { + summary.AProcessingLatencyNS = value + } + if value := subtractIfPresent(summary.Timestamps, EventATXSoftware, EventATXSched); value != nil { + summary.AQueueLatencyNS = value + } + if value := subtractIfPresent(summary.Timestamps, EventBAppRecv, EventATXSoftware); value != nil { + summary.ABTransportPropagationNS = value + } + if value := subtractIfPresent(summary.Timestamps, EventBAppRecv, EventBRXSoftware); value != nil { + summary.BKernelReceivePathLatencyNS = value + } + if value := subtractIfPresent(summary.Timestamps, EventBPersistEnd, EventBAppRecv); value != nil { + summary.BProcessingLatencyNS = value + } + if value := subtractIfPresent(summary.Timestamps, EventBPersistEnd, EventAAppPrepBegin); value != nil { + summary.EndToEndLatencyNS = value + } + + summary.AProcessingBitrateBPS = calculateBitrateBPS(summary.BodySize, summary.AProcessingLatencyNS) + summary.ABTransportPropagationBitrateBPS = calculateBitrateBPS(summary.BodySize, summary.ABTransportPropagationNS) + summary.EndToEndBitrateBPS = calculateBitrateBPS(summary.BodySize, summary.EndToEndLatencyNS) +} + +type routeKey struct { + From string + To string +} + +func assignApproxRTTs(summaries []*Summary) { + grouped := make(map[routeKey][]*Summary) + for _, summary := range summaries { + grouped[routeKey{From: summary.From, To: summary.To}] = append(grouped[routeKey{From: summary.From, To: summary.To}], summary) + } + + for key, requests := range grouped { + replies := grouped[routeKey{From: key.To, To: key.From}] + if len(replies) == 0 { + continue + } + + assignApproxRTTsForRoute( + sortSummariesByTimestamp(requests, EventBAppRecv), + sortSummariesByTimestamp(replies, EventATXSoftware), + ) + } +} + +func assignApproxRTTsForRoute(requests, replies []*Summary) { + replyIndex := 0 + for _, request := range requests { + requestReceivedAtResponder, ok := request.Timestamps[EventBAppRecv] + if !ok { + continue + } + + for replyIndex < len(replies) { + reply := replies[replyIndex] + replySentAtResponder, ok := reply.Timestamps[EventATXSoftware] + if !ok { + replyIndex++ + continue + } + if replySentAtResponder < requestReceivedAtResponder { + replyIndex++ + continue + } + + if value := subtractSummaryTimestamps(reply, EventBAppRecv, request, EventATXSoftware); value != nil { + request.ApproxRTTNS = value + } + replyIndex++ + break + } + } +} + +func sortSummariesByTimestamp(summaries []*Summary, eventName string) []*Summary { + sorted := append([]*Summary(nil), summaries...) + sort.SliceStable(sorted, func(i, j int) bool { + leftTS, leftOK := sorted[i].Timestamps[eventName] + rightTS, rightOK := sorted[j].Timestamps[eventName] + switch { + case leftOK && rightOK: + if leftTS != rightTS { + return leftTS < rightTS + } + case leftOK: + return true + case rightOK: + return false + } + + if sorted[i].MessageID != sorted[j].MessageID { + return sorted[i].MessageID < sorted[j].MessageID + } + if sorted[i].From != sorted[j].From { + return sorted[i].From < sorted[j].From + } + if sorted[i].To != sorted[j].To { + return sorted[i].To < sorted[j].To + } + + return sorted[i].MessageType < sorted[j].MessageType + }) + return sorted +} + +// 返回 requiredTimestampNames 中哪些在给定的 timestamps 中缺失。 +func missingTimestampNames(timestamps map[string]int64) []string { + var missing []string + for _, name := range requiredTimestampNames { + if _, ok := timestamps[name]; !ok { + missing = append(missing, name) + } + } + + return missing +} + +// 如果 timestamps 中同时存在 endName 和 beginName，则返回它们的差值；否则返回 nil。 +func subtractIfPresent(timestamps map[string]int64, endName, beginName string) *int64 { + end, ok := timestamps[endName] + if !ok { + return nil + } + begin, ok := timestamps[beginName] + if !ok { + return nil + } + + value := end - begin + return &value +} + +func subtractSummaryTimestamps(endSummary *Summary, endName string, beginSummary *Summary, beginName string) *int64 { + end, ok := endSummary.Timestamps[endName] + if !ok { + return nil + } + begin, ok := beginSummary.Timestamps[beginName] + if !ok { + return nil + } + + value := end - begin + return &value +} + +// 除法函数，如果 bodySize <= 0 或 latencyNS 不存在或 <= 0，则返回 nil；否则返回 bodySize / latencyNS 的结果。 +func calculateBitrateBPS(bodySize int, latencyNS *int64) *float64 { + if bodySize <= 0 || latencyNS == nil || *latencyNS <= 0 { + return nil + } + + value := float64(bodySize) * 8 * 1_000_000_000 / float64(*latencyNS) + return &value +} + +// 最大 message_id 计算函数 +func maxBusinessMessageID(events []Event) (uint64, bool) { + var maxMessageID uint64 + hasBusinessMessage := false + + for _, event := range events { + if !IsBusinessEvent(event) { + continue + } + if !hasBusinessMessage || event.MessageID > maxMessageID { + maxMessageID = event.MessageID + hasBusinessMessage = true + } + } + + return maxMessageID, hasBusinessMessage +} + +// 根据 message_id 截断事件列表的函数 +func filterEventsByMaxMessageID(events []Event, maxMessageID uint64) []Event { + filtered := make([]Event, 0, len(events)) + for _, event := range events { + if event.MessageID > maxMessageID { + continue + } + filtered = append(filtered, event) + } + + return filtered +} + +func subtractUint64(value, offset uint64) (uint64, bool) { + if offset > value { + return 0, false + } + + return value - offset, true +} + +// 判断事件是否是业务相关的时延事件（其中一项） +func IsBusinessEvent(event Event) bool { + switch event.Event { + case EventAAppPrepBegin, + EventATXSched, + EventATXSoftware, + EventATXHardware, + EventBRXHardware, + EventBRXSoftware, + EventBAppRecv, + EventBPersistBegin, + EventBPersistEnd: + return true + default: + return false + } +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/latencylog/summary_chart.go b/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/latencylog/summary_chart.go new file mode 100644 index 0000000..a37a4c2 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/latencylog/summary_chart.go @@ -0,0 +1,498 @@ +package latencylog + +import ( + "bufio" + "fmt" + "html/template" + "os" + "path/filepath" + "strings" + + "omnisocketgo/cmd/internal/protocol" +) + +const summaryChartHTMLTemplate = ` + + + + + Latency Summary Chart + + + +
+

Latency Summary

+

A simple per-message end-to-end latency chart generated from summarized JSONL records.

+ +
+
+
Messages
+
{{.TotalMessages}}
+
+
+
With End-To-End
+
{{.MessagesWithEndToEnd}}
+
+
+
Average End-To-End
+
{{.AverageEndToEnd}}
+
+
+
Max End-To-End
+
{{.MaxEndToEnd}}
+
+
+ +
+ {{range .Legend}} + + + {{.Label}} + + {{end}} +
+ + {{if .Rows}} +
+ {{range .Rows}} +
+
+

{{.Title}}

+
{{.EndToEnd}}
+
+
{{.Subtitle}}
+
{{.ApproxRTT}}
+ {{if .RatioMetrics}} +
+ {{range .RatioMetrics}} + {{.Label}} {{.Value}} + {{end}} +
+ {{end}} +
+ {{range .Segments}} +
+ {{end}} +
+ {{if .Segments}} +
+ {{range .Segments}} + + + {{.Label}} {{.Value}} + + {{end}} +
+ {{end}} + {{if .MissingTimestamps}} +
Missing timestamps: {{.MissingTimestamps}}
+ {{end}} +
+ {{end}} +
+ {{else}} +
No summarized messages were available for chart rendering.
+ {{end}} +
+ + +` + +type summaryChartPage struct { + TotalMessages int + MessagesWithEndToEnd int + AverageEndToEnd string + MaxEndToEnd string + Legend []summaryChartLegendItem + Rows []summaryChartRow +} + +type summaryChartLegendItem struct { + Label string + Color string +} + +type summaryChartRow struct { + Title string + Subtitle string + EndToEnd string + ApproxRTT string + MissingTimestamps string + RatioMetrics []summaryChartValue + Segments []summaryChartSegment +} + +type summaryChartSegment struct { + Label string + Value string + Color string + WidthPercent float64 +} + +type summaryChartValue struct { + Label string + Value string +} + +type summaryChartSegmentMetric struct { + label string + value *int64 + color string +} + +// WriteSummariesHTMLChart 将整理结果写成一个可直接在浏览器中打开的简单 HTML 图表。 +func WriteSummariesHTMLChart(path string, summaries []Summary) error { + if err := os.MkdirAll(filepath.Dir(path), 0o755); err != nil { + return fmt.Errorf("latencylog: create chart dir for %s: %w", path, err) + } + + file, err := os.OpenFile(path, os.O_CREATE|os.O_WRONLY|os.O_TRUNC, 0o644) + if err != nil { + return fmt.Errorf("latencylog: open chart file %s: %w", path, err) + } + defer file.Close() + + page := buildSummaryChartPage(summaries) + tmpl, err := template.New("summary-chart").Parse(summaryChartHTMLTemplate) + if err != nil { + return fmt.Errorf("latencylog: parse chart template: %w", err) + } + + writer := bufio.NewWriter(file) + if err := tmpl.Execute(writer, page); err != nil { + return fmt.Errorf("latencylog: render chart %s: %w", path, err) + } + if err := writer.Flush(); err != nil { + return fmt.Errorf("latencylog: flush chart %s: %w", path, err) + } + + return nil +} + +func buildSummaryChartPage(summaries []Summary) summaryChartPage { + page := summaryChartPage{ + TotalMessages: len(summaries), + Legend: []summaryChartLegendItem{ + {Label: "A processing", Color: "var(--a-proc)"}, + {Label: "A queue", Color: "var(--a-queue)"}, + {Label: "A-B transport + propagation", Color: "var(--transport)"}, + {Label: "B processing", Color: "var(--b-proc)"}, + {Label: "Unknown / missing", Color: "var(--unknown)"}, + }, + Rows: make([]summaryChartRow, 0, len(summaries)), + } + + var ( + endToEndValues []int64 + totalEndToEnd int64 + maxEndToEnd int64 + ) + + for _, summary := range summaries { + page.Rows = append(page.Rows, buildSummaryChartRow(summary)) + + if summary.EndToEndLatencyNS == nil { + continue + } + endToEnd := *summary.EndToEndLatencyNS + endToEndValues = append(endToEndValues, endToEnd) + totalEndToEnd += endToEnd + if endToEnd > maxEndToEnd { + maxEndToEnd = endToEnd + } + } + + page.MessagesWithEndToEnd = len(endToEndValues) + page.AverageEndToEnd = "n/a" + page.MaxEndToEnd = "n/a" + if len(endToEndValues) > 0 { + page.AverageEndToEnd = formatLatencyNS(totalEndToEnd / int64(len(endToEndValues))) + page.MaxEndToEnd = formatLatencyNS(maxEndToEnd) + } + + return page +} + +func buildSummaryChartRow(summary Summary) summaryChartRow { + row := summaryChartRow{ + Title: buildSummaryChartTitle(summary), + Subtitle: buildSummaryChartSubtitle(summary), + EndToEnd: "End-to-end: n/a", + ApproxRTT: "Approx RTT: n/a", + MissingTimestamps: strings.Join(summary.MissingTimestamps, ", "), + } + if summary.ApproxRTTNS != nil && *summary.ApproxRTTNS > 0 { + row.ApproxRTT = fmt.Sprintf("Approx RTT: %s", formatLatencyNS(*summary.ApproxRTTNS)) + } + + ratioMetrics := []struct { + label string + value *float64 + }{ + {label: "A processing bitrate", value: summary.AProcessingBitrateBPS}, + {label: "A-B transport + propagation bitrate", value: summary.ABTransportPropagationBitrateBPS}, + {label: "End-to-end bitrate", value: summary.EndToEndBitrateBPS}, + } + for _, metric := range ratioMetrics { + if metric.value == nil || *metric.value <= 0 { + continue + } + row.RatioMetrics = append(row.RatioMetrics, summaryChartValue{ + Label: metric.label, + Value: formatBitrateBPS(*metric.value), + }) + } + + if summary.EndToEndLatencyNS == nil || *summary.EndToEndLatencyNS <= 0 { + return row + } + + total := *summary.EndToEndLatencyNS + row.EndToEnd = fmt.Sprintf("End-to-end: %s", formatLatencyNS(total)) + + metrics := []summaryChartSegmentMetric{ + {label: "A processing", value: summary.AProcessingLatencyNS, color: "var(--a-proc)"}, + {label: "A queue", value: summary.AQueueLatencyNS, color: "var(--a-queue)"}, + {label: "A-B transport + propagation", value: summary.ABTransportPropagationNS, color: "var(--transport)"}, + {label: "B processing", value: summary.BProcessingLatencyNS, color: "var(--b-proc)"}, + } + + var knownTotal int64 + for _, metric := range metrics { + if metric.value == nil || *metric.value <= 0 { + continue + } + knownTotal += *metric.value + } + + scaleTotal := total + if knownTotal > scaleTotal { + scaleTotal = knownTotal + } + if scaleTotal <= 0 { + return row + } + + for _, metric := range metrics { + if metric.value == nil || *metric.value <= 0 { + continue + } + row.Segments = append(row.Segments, summaryChartSegment{ + Label: metric.label, + Value: formatLatencyNS(*metric.value), + Color: metric.color, + WidthPercent: float64(*metric.value) * 100 / float64(scaleTotal), + }) + } + + if remaining := total - knownTotal; remaining > 0 { + row.Segments = append(row.Segments, summaryChartSegment{ + Label: "Unknown / missing", + Value: formatLatencyNS(remaining), + Color: "var(--unknown)", + WidthPercent: float64(remaining) * 100 / float64(scaleTotal), + }) + } + + return row +} + +func buildSummaryChartTitle(summary Summary) string { + if summary.MessageType == protocol.MessageTypeFile && summary.FileName != "" { + return fmt.Sprintf("%s #%d (%s)", summary.MessageType, summary.MessageID, summary.FileName) + } + + return fmt.Sprintf("%s #%d", summary.MessageType, summary.MessageID) +} + +func buildSummaryChartSubtitle(summary Summary) string { + parts := []string{ + fmt.Sprintf("%s -> %s", summary.From, summary.To), + fmt.Sprintf("%d bytes", summary.BodySize), + } + + if summary.MessageType == protocol.MessageTypeFile && summary.FileName != "" { + parts = append(parts, fmt.Sprintf("file: %s", summary.FileName)) + } + + return strings.Join(parts, " | ") +} + +func formatLatencyNS(ns int64) string { + return fmt.Sprintf("%.3f ms", float64(ns)/1_000_000) +} + +func formatBitrateBPS(bitsPerSecond float64) string { + return fmt.Sprintf("%.3f Mb/s", bitsPerSecond/1_000_000) +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/latencylog/summary_chart_test.go b/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/latencylog/summary_chart_test.go new file mode 100644 index 0000000..d9f41ec --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/latencylog/summary_chart_test.go @@ -0,0 +1,79 @@ +package latencylog + +import ( + "os" + "path/filepath" + "strings" + "testing" + + "omnisocketgo/cmd/internal/protocol" +) + +func TestWriteSummariesHTMLChart(t *testing.T) { + aProcessing := int64(20_000_000) + aQueue := int64(10_000_000) + transport := int64(40_000_000) + bProcessing := int64(30_000_000) + endToEnd := int64(100_000_000) + aProcessingBitrate := float64(5) * 8 * 1_000_000_000 / float64(aProcessing) + transportBitrate := float64(5) * 8 * 1_000_000_000 / float64(transport) + endToEndBitrate := float64(5) * 8 * 1_000_000_000 / float64(endToEnd) + + summaries := []Summary{ + { + MessageType: protocol.MessageTypeText, + MessageID: 7, + From: "peer-a", + To: "peer-b", + BodySize: 5, + AProcessingLatencyNS: &aProcessing, + AQueueLatencyNS: &aQueue, + ABTransportPropagationNS: &transport, + BProcessingLatencyNS: &bProcessing, + EndToEndLatencyNS: &endToEnd, + AProcessingBitrateBPS: &aProcessingBitrate, + ABTransportPropagationBitrateBPS: &transportBitrate, + EndToEndBitrateBPS: &endToEndBitrate, + ApproxRTTNS: &endToEnd, + }, + { + MessageType: protocol.MessageTypeFile, + MessageID: 8, + From: "peer-b", + To: "peer-a", + FileName: "payload.bin", + BodySize: 128, + MissingTimestamps: []string{EventBRXSoftware}, + }, + } + + path := filepath.Join(t.TempDir(), "charts", "latency-summary.html") + if err := WriteSummariesHTMLChart(path, summaries); err != nil { + t.Fatalf("WriteSummariesHTMLChart() error = %v", err) + } + + data, err := os.ReadFile(path) + if err != nil { + t.Fatalf("os.ReadFile() error = %v", err) + } + + content := string(data) + for _, want := range []string{ + "Latency Summary", + "text #7", + "peer-a -> peer-b | 5 bytes", + "End-to-end: 100.000 ms", + "Approx RTT: 100.000 ms", + "A processing bitrate 0.002 Mb/s", + "A-B transport + propagation bitrate 0.001 Mb/s", + "End-to-end bitrate 0.000 Mb/s", + "A processing 20.000 ms", + "A-B transport + propagation 40.000 ms", + "file #8 (payload.bin)", + "Missing timestamps: B_RX_SOFTWARE", + } { + if !strings.Contains(content, want) { + t.Fatalf("chart content missing %q\n%s", want, content) + } + } +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/latencylog/summary_test.go b/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/latencylog/summary_test.go new file mode 100644 index 0000000..f1bb8da --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/latencylog/summary_test.go @@ -0,0 +1,399 @@ +package latencylog + +import ( + "bufio" + "encoding/json" + "os" + "path/filepath" + "reflect" + "sort" + "testing" + + "omnisocketgo/cmd/internal/protocol" +) + +func TestSummarizeEventsComputesLatencyMetrics(t *testing.T) { + events := []Event{ + {TsUnixNano: 100, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 120, Event: EventATXSched, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 140, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 180, Event: EventBRXSoftware, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 220, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 230, Event: EventBPersistBegin, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 260, Event: EventBPersistEnd, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + } + + summaries := SummarizeEvents(events) + if len(summaries) != 1 { + t.Fatalf("summary count = %d, want 1", len(summaries)) + } + + summary := summaries[0] + if got := ptrValue(summary.AProcessingLatencyNS); got != 20 { + t.Fatalf("AProcessingLatencyNS = %d, want 20", got) + } + if got := ptrValue(summary.AQueueLatencyNS); got != 20 { + t.Fatalf("AQueueLatencyNS = %d, want 20", got) + } + if got := ptrValue(summary.ABTransportPropagationNS); got != 80 { + t.Fatalf("ABTransportPropagationNS = %d, want 80", got) + } + if got := ptrValue(summary.BKernelReceivePathLatencyNS); got != 40 { + t.Fatalf("BKernelReceivePathLatencyNS = %d, want 40", got) + } + if got := ptrValue(summary.BProcessingLatencyNS); got != 40 { + t.Fatalf("BProcessingLatencyNS = %d, want 40", got) + } + if got := ptrValue(summary.EndToEndLatencyNS); got != 160 { + t.Fatalf("EndToEndLatencyNS = %d, want 160", got) + } + if got := ptrValueFloat(summary.AProcessingBitrateBPS); got != 128_000_000_000 { + t.Fatalf("AProcessingBitrateBPS = %v, want 128000000000", got) + } + if got := ptrValueFloat(summary.ABTransportPropagationBitrateBPS); got != 32_000_000_000 { + t.Fatalf("ABTransportPropagationBitrateBPS = %v, want 32000000000", got) + } + if got := ptrValueFloat(summary.EndToEndBitrateBPS); got != 16_000_000_000 { + t.Fatalf("EndToEndBitrateBPS = %v, want 16000000000", got) + } + if got := summary.Timestamps[EventBRXSoftware]; got != 180 { + t.Fatalf("timestamps[%q] = %d, want 180", EventBRXSoftware, got) + } + if len(summary.MissingTimestamps) != 0 { + t.Fatalf("MissingTimestamps = %v, want empty", summary.MissingTimestamps) + } +} + +func TestSummarizeEventsComputesApproxRTTByPairingReverseMessages(t *testing.T) { + events := []Event{ + {TsUnixNano: 100, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b"}, + {TsUnixNano: 110, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b"}, + {TsUnixNano: 180, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b"}, + {TsUnixNano: 120, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b"}, + {TsUnixNano: 140, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b"}, + {TsUnixNano: 190, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b"}, + {TsUnixNano: 200, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 11, From: "peer-b", To: "peer-a"}, + {TsUnixNano: 210, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 11, From: "peer-b", To: "peer-a"}, + {TsUnixNano: 260, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 11, From: "peer-b", To: "peer-a"}, + {TsUnixNano: 220, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 12, From: "peer-b", To: "peer-a"}, + {TsUnixNano: 230, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 12, From: "peer-b", To: "peer-a"}, + {TsUnixNano: 310, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 12, From: "peer-b", To: "peer-a"}, + } + + summaries := SummarizeEvents(events) + if len(summaries) != 4 { + t.Fatalf("summary count = %d, want 4", len(summaries)) + } + + gotByMessageID := make(map[uint64]Summary, len(summaries)) + for _, summary := range summaries { + gotByMessageID[summary.MessageID] = summary + } + + if got := ptrValue(gotByMessageID[1].ApproxRTTNS); got != 150 { + t.Fatalf("message 1 ApproxRTTNS = %d, want 150", got) + } + if got := ptrValue(gotByMessageID[2].ApproxRTTNS); got != 170 { + t.Fatalf("message 2 ApproxRTTNS = %d, want 170", got) + } + if gotByMessageID[11].ApproxRTTNS != nil { + t.Fatalf("message 11 ApproxRTTNS = %d, want nil", ptrValue(gotByMessageID[11].ApproxRTTNS)) + } + if gotByMessageID[12].ApproxRTTNS != nil { + t.Fatalf("message 12 ApproxRTTNS = %d, want nil", ptrValue(gotByMessageID[12].ApproxRTTNS)) + } +} + +func TestSummarizeEventsReportsMissingTimestamps(t *testing.T) { + events := []Event{ + {TsUnixNano: 100, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b"}, + {TsUnixNano: 240, Event: EventBPersistEnd, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b"}, + } + + summaries := SummarizeEvents(events) + if len(summaries) != 1 { + t.Fatalf("summary count = %d, want 1", len(summaries)) + } + + wantMissing := []string{EventATXSched, EventATXSoftware, EventBRXSoftware, EventBAppRecv} + if !reflect.DeepEqual(summaries[0].MissingTimestamps, wantMissing) { + t.Fatalf("MissingTimestamps = %v, want %v", summaries[0].MissingTimestamps, wantMissing) + } + if summaries[0].AProcessingLatencyNS != nil { + t.Fatalf("AProcessingLatencyNS = %v, want nil", ptrValue(summaries[0].AProcessingLatencyNS)) + } + if summaries[0].EndToEndLatencyNS == nil { + t.Fatal("EndToEndLatencyNS = nil, want non-nil because endpoints are present") + } +} + +func TestLoadAndWriteSummaryFiles(t *testing.T) { + rawPath := filepath.Join(t.TempDir(), "raw.jsonl") + rawLogger, err := NewJSONLLogger(rawPath) + if err != nil { + t.Fatalf("NewJSONLLogger() error = %v", err) + } + t.Cleanup(func() { + _ = rawLogger.Close() + }) + + for _, event := range []Event{ + {TsUnixNano: 100, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 120, Event: EventATXSched, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 140, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 180, Event: EventBRXSoftware, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 220, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 260, Event: EventBPersistEnd, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 320}, + } { + if err := rawLogger.LogEvent(event); err != nil { + t.Fatalf("LogEvent() error = %v", err) + } + } + + events, err := LoadEventsFromFile(rawPath) + if err != nil { + t.Fatalf("LoadEventsFromFile() error = %v", err) + } + + summaryPath := filepath.Join(t.TempDir(), "summary.jsonl") + if err := WriteSummariesJSONL(summaryPath, SummarizeEvents(events)); err != nil { + t.Fatalf("WriteSummariesJSONL() error = %v", err) + } + + file, err := os.Open(summaryPath) + if err != nil { + t.Fatalf("os.Open() error = %v", err) + } + defer file.Close() + + scanner := bufio.NewScanner(file) + if !scanner.Scan() { + t.Fatal("expected one summary line, got none") + } + + var summary Summary + if err := json.Unmarshal(scanner.Bytes(), &summary); err != nil { + t.Fatalf("json.Unmarshal() error = %v", err) + } + if summary.MessageID != 3 { + t.Fatalf("MessageID = %d, want 3", summary.MessageID) + } + if got := ptrValue(summary.BKernelReceivePathLatencyNS); got != 40 { + t.Fatalf("BKernelReceivePathLatencyNS = %d, want 40", got) + } + if got := ptrValue(summary.EndToEndLatencyNS); got != 160 { + t.Fatalf("EndToEndLatencyNS = %d, want 160", got) + } + if got := ptrValueFloat(summary.EndToEndBitrateBPS); got != 16_000_000_000 { + t.Fatalf("EndToEndBitrateBPS = %v, want 16000000000", got) + } +} + +func TestLoadEventsFromFilesWithSharedMaxOffsetFiltersToSharedCutoff(t *testing.T) { + t.Parallel() + + testCases := []struct { + name string + firstMessageIDs []uint64 + secondMessageIDs []uint64 + offset uint64 + wantCutoff *uint64 + wantMessageIDs []uint64 + }{ + { + name: "same max message id rolls back one", + firstMessageIDs: []uint64{1, 2, 3, 4, 5, 6, 7}, + secondMessageIDs: []uint64{1, 2, 3, 4, 5, 6, 7}, + offset: 1, + wantCutoff: uint64Ptr(6), + wantMessageIDs: []uint64{1, 2, 3, 4, 5, 6}, + }, + { + name: "smaller input max wins before rollback", + firstMessageIDs: []uint64{1, 2, 3, 4, 5, 6, 7, 8, 9}, + secondMessageIDs: []uint64{1, 2, 3, 4, 5, 6, 7}, + offset: 1, + wantCutoff: uint64Ptr(6), + wantMessageIDs: []uint64{1, 2, 3, 4, 5, 6}, + }, + { + name: "not enough shared messages yields empty result", + firstMessageIDs: []uint64{1}, + secondMessageIDs: []uint64{1}, + offset: 1, + wantCutoff: uint64Ptr(0), + wantMessageIDs: nil, + }, + } + + for _, tt := range testCases { + tt := tt + t.Run(tt.name, func(t *testing.T) { + t.Parallel() + + tempDir := t.TempDir() + firstPath := filepath.Join(tempDir, "first.jsonl") + secondPath := filepath.Join(tempDir, "second.jsonl") + writeEventsJSONL(t, firstPath, testEventsForMessageIDs(tt.firstMessageIDs, "peer-a", "peer-b")) + writeEventsJSONL(t, secondPath, testEventsForMessageIDs(tt.secondMessageIDs, "peer-b", "peer-a")) + + events, cutoff, err := LoadEventsFromFilesWithSharedMaxOffset([]string{firstPath, secondPath}, tt.offset) + if err != nil { + t.Fatalf("LoadEventsFromFilesWithSharedMaxOffset() error = %v", err) + } + if !reflect.DeepEqual(cutoff, tt.wantCutoff) { + t.Fatalf("cutoff = %v, want %v", cutoff, tt.wantCutoff) + } + + if got := businessMessageIDs(events); !reflect.DeepEqual(got, tt.wantMessageIDs) { + t.Fatalf("message IDs = %v, want %v", got, tt.wantMessageIDs) + } + }) + } +} + +func TestLoadEventsFromFilesWithSharedMaxOffsetPreservesEarlierSummaries(t *testing.T) { + tempDir := t.TempDir() + firstPath := filepath.Join(tempDir, "first.jsonl") + secondPath := filepath.Join(tempDir, "second.jsonl") + + writeEventsJSONL(t, firstPath, []Event{ + {TsUnixNano: 100, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 120, Event: EventATXSched, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 140, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 180, Event: EventBRXSoftware, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 220, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 260, Event: EventBPersistEnd, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 300, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b", BodySize: 160}, + {TsUnixNano: 330, Event: EventATXSched, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b", BodySize: 160}, + {TsUnixNano: 360, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b", BodySize: 160}, + {TsUnixNano: 390, Event: EventBRXSoftware, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b", BodySize: 160}, + {TsUnixNano: 420, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b", BodySize: 160}, + {TsUnixNano: 470, Event: EventBPersistEnd, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b", BodySize: 160}, + {TsUnixNano: 500, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 80}, + {TsUnixNano: 520, Event: EventATXSched, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 80}, + {TsUnixNano: 540, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 80}, + {TsUnixNano: 560, Event: EventBRXSoftware, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 80}, + {TsUnixNano: 580, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 80}, + {TsUnixNano: 600, Event: EventBPersistEnd, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 80}, + {TsUnixNano: 700, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 4, From: "peer-a", To: "peer-b", BodySize: 40}, + }) + writeEventsJSONL(t, secondPath, []Event{ + {TsUnixNano: 90, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 95, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 150, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 290, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 295, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 350, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 490, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 495, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 550, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 690, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 4, From: "peer-b", To: "peer-a", BodySize: 20}, + }) + + events, cutoff, err := LoadEventsFromFilesWithSharedMaxOffset([]string{firstPath, secondPath}, 1) + if err != nil { + t.Fatalf("LoadEventsFromFilesWithSharedMaxOffset() error = %v", err) + } + if !reflect.DeepEqual(cutoff, uint64Ptr(3)) { + t.Fatalf("cutoff = %v, want %v", cutoff, uint64Ptr(3)) + } + + summaries := SummarizeEvents(events) + if got := len(summaries); got != 6 { + t.Fatalf("summary count = %d, want 6", got) + } + + for _, summary := range summaries { + if summary.MessageID == 4 { + t.Fatalf("message 4 should have been truncated from summaries: %+v", summary) + } + } + + var forwardMessageTwo Summary + found := false + for _, summary := range summaries { + if summary.From == "peer-a" && summary.To == "peer-b" && summary.MessageID == 2 { + forwardMessageTwo = summary + found = true + break + } + } + if !found { + t.Fatal("summary for message 2 peer-a -> peer-b not found") + } + if got := ptrValue(forwardMessageTwo.EndToEndLatencyNS); got != 170 { + t.Fatalf("message 2 EndToEndLatencyNS = %d, want 170", got) + } + if got := ptrValue(forwardMessageTwo.ApproxRTTNS); got != 190 { + t.Fatalf("message 2 ApproxRTTNS = %d, want 190", got) + } +} + +func ptrValue(value *int64) int64 { + if value == nil { + return 0 + } + return *value +} + +func ptrValueFloat(value *float64) float64 { + if value == nil { + return 0 + } + return *value +} + +func uint64Ptr(value uint64) *uint64 { + return &value +} + +func businessMessageIDs(events []Event) []uint64 { + seen := make(map[uint64]struct{}) + var ids []uint64 + + for _, event := range events { + if !IsBusinessEvent(event) { + continue + } + if _, ok := seen[event.MessageID]; ok { + continue + } + seen[event.MessageID] = struct{}{} + ids = append(ids, event.MessageID) + } + + sort.Slice(ids, func(i, j int) bool { + return ids[i] < ids[j] + }) + return ids +} + +func testEventsForMessageIDs(messageIDs []uint64, from, to string) []Event { + events := make([]Event, 0, len(messageIDs)*2) + for _, messageID := range messageIDs { + events = append(events, + Event{TsUnixNano: int64(messageID*100 + 10), Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: messageID, From: from, To: to, BodySize: 32}, + Event{TsUnixNano: int64(messageID*100 + 20), Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: messageID, From: from, To: to, BodySize: 32}, + ) + } + + return events +} + +func writeEventsJSONL(t *testing.T, path string, events []Event) { + t.Helper() + + file, err := os.OpenFile(path, os.O_CREATE|os.O_WRONLY|os.O_TRUNC, 0o644) + if err != nil { + t.Fatalf("os.OpenFile(%s) error = %v", path, err) + } + defer file.Close() + + encoder := json.NewEncoder(file) + for _, event := range events { + if err := encoder.Encode(event); err != nil { + t.Fatalf("encoder.Encode(%s) error = %v", path, err) + } + } +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/protocol/codec.go b/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/protocol/codec.go new file mode 100644 index 0000000..fef658b --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/protocol/codec.go @@ -0,0 +1,279 @@ +package protocol + +import ( + "encoding/binary" + "encoding/json" + "errors" + "fmt" + "io" + "unicode/utf8" +) + +// MaxFrameSize 用于限制单个帧的最大长度， +// 避免异常对端通过伪造超大长度值导致接收方无上限分配内存。 +const MaxFrameSize = 8 * 1024 * 1024 // 先临时设置传输的视频帧不超过8MB + +var ( + ErrInvalidFrameLength = errors.New("protocol: invalid frame length") // 表示帧长度非法，例如长度为 0。 + ErrFrameTooLarge = errors.New("protocol: frame too large") // 表示帧长度超过允许的上限。 + ErrInvalidMessageType = errors.New("protocol: invalid message type") // 表示消息类型不是当前协议支持的类型。 + ErrMissingFrom = errors.New("protocol: missing from") // 表示消息缺少发送方标识。 + ErrMissingTo = errors.New("protocol: missing to") // 表示消息缺少接收方标识。 + ErrMissingFileName = errors.New("protocol: missing file name") // 表示 file 消息缺少文件名。 + ErrUnexpectedFileName = errors.New("protocol: unexpected file name") // 表示 text 消息错误地携带了文件名。 + ErrInvalidTextBody = errors.New("protocol: invalid text body") // 表示 text 消息正文不是合法 UTF-8。 + ErrUnexpectedBody = errors.New("protocol: unexpected body") // 表示某些控制消息不允许携带正文。 + ErrInvalidRegisterTarget = errors.New("protocol: invalid register target") // 表示 register 消息没有发往 server。 + ErrInvalidErrorSource = errors.New("protocol: invalid error source") // 表示 error 消息不是由 server 发出。 + ErrInvalidHeaderLength = errors.New("protocol: invalid header length") // 表示 header 长度字段为 0、越界或无法完整切分。 + ErrInvalidHeaderJSON = errors.New("protocol: invalid header json") // 表示 header JSON 无法解析，可能是格式错误或缺少必要字段。 + ErrInvalidContentLength = errors.New("protocol: invalid content length") // 表示头部记录的正文长度与实际正文不一致。 +) + +// 应用层消息：[4字节 frameLength][4字节 headerLen][header JSON（下面自定义的Message头）][body bytes] +// 写了 tag：JSON 字段名是你指定的 type；不写 tag：JSON 字段名默认是 Go 字段名 Type +type messageHeader struct { + Type MessageType `json:"type"` + ID uint64 `json:"id"` + From string `json:"from"` + To string `json:"to"` + FileName string `json:"file_name,omitempty"` + ContentLength int `json:"content_length"` +} + +// EncodeMessage 将逻辑消息编码为帧内字节格式： +// 1. 4 字节大端序 header 长度 +// 2. header JSON +// 3. 原始 body 字节 +func EncodeMessage(msg Message) ([]byte, error) { + if err := validateMessage(msg); err != nil { + return nil, err + } + + header := messageHeader{ + Type: msg.Type, + ID: msg.ID, + From: msg.From, + To: msg.To, + FileName: msg.FileName, + ContentLength: len(msg.Body), + } + + headerPayload, err := json.Marshal(header) + if err != nil { + return nil, fmt.Errorf("protocol: encode header: %w", err) + } + // 创建一个新的字节切片来存储完整的帧内容，避免直接在 headerPayload 上修改导致数据混乱。 + payload := make([]byte, 4+len(headerPayload)+len(msg.Body)) + // 在 payload 前 4 字节写入 header 长度，后续内容依次是 header JSON（第五个字节开始） 和 body。 + binary.BigEndian.PutUint32(payload[:4], uint32(len(headerPayload))) + copy(payload[4:], headerPayload) + copy(payload[4+len(headerPayload):], msg.Body) + + //检查整个帧长度是否合法，避免上层调用者构造的消息过大导致发送失败。 + if len(payload) > MaxFrameSize { + return nil, ErrFrameTooLarge + } + + return payload, nil +} + +// DecodeMessage 将帧内字节格式还原为 Message。 +func DecodeMessage(data []byte) (Message, error) { + if len(data) > MaxFrameSize { + return Message{}, ErrFrameTooLarge + } + if len(data) < 4 { + return Message{}, ErrInvalidHeaderLength + } + + headerLen := int(binary.BigEndian.Uint32(data[:4])) + if headerLen == 0 || headerLen > len(data)-4 { + return Message{}, ErrInvalidHeaderLength + } + + headerPayload := data[4 : 4+headerLen] + body := data[4+headerLen:] + + var header messageHeader + if err := json.Unmarshal(headerPayload, &header); err != nil { + return Message{}, fmt.Errorf("protocol: decode header: %w", errors.Join(ErrInvalidHeaderJSON, err)) + } + + if header.ContentLength < 0 || header.ContentLength != len(body) { + return Message{}, ErrInvalidContentLength + } + + bodyCopy := make([]byte, len(body)) + copy(bodyCopy, body) + + msg := Message{ + Type: header.Type, + ID: header.ID, + From: header.From, + To: header.To, + FileName: header.FileName, + Body: bodyCopy, + } + + if err := validateMessage(msg); err != nil { + return Message{}, err + } + + return msg, nil +} + +// WriteFrame 向流中写入一个带长度前缀的帧。 +// TCP帧格式如下： +// 1. 4 字节大端序长度 +// 2. 后续 payload 内容 +// +// TCP 是字节流协议，没有天然的消息边界。 +// 增加显式长度前缀后，接收方就知道一条完整消息应该读取多少字节， +// 从而解决粘包和拆包问题。 +func WriteFrame(w io.Writer, payload []byte) error { + size := len(payload) + //空帧 + if size == 0 { + return ErrInvalidFrameLength + } + //帧过大 + if size > MaxFrameSize { + return ErrFrameTooLarge + } + + var header [4]byte + binary.BigEndian.PutUint32(header[:], uint32(size)) + + // 先写长度头，接收方才能根据长度一次性读取完整消息体。 + if err := writeFull(w, header[:]); err != nil { + return err + } + + return writeFull(w, payload) +} + +// ReadFrame 从流中读取一个完整的长度前缀帧。 +// 它会先读取固定 4 字节长度头，校验长度是否合法， +// 再使用 io.ReadFull 按长度读取完整消息体， +// 这样即使底层 TCP 发生分段读取，也不会把半条消息暴露给上层。 +func ReadFrame(r io.Reader) ([]byte, error) { + var header [4]byte + if _, err := io.ReadFull(r, header[:]); err != nil { + return nil, err + } + + size := binary.BigEndian.Uint32(header[:]) + // 长度为 0 的帧被认为是非法输入，而不是合法的空消息。 + if size == 0 { + return nil, ErrInvalidFrameLength + } + // 长度超过上限的帧会被拒绝，避免接收方无上限分配内存。 + if size > MaxFrameSize { + return nil, ErrFrameTooLarge + } + + payload := make([]byte, int(size)) + if _, err := io.ReadFull(r, payload); err != nil { + return nil, err + } + + return payload, nil +} + +// WriteMessage 是给上层直接使用的完整发送路径： +// 把一条结构化消息完整编码并发送出去”的总入口。 +// Message -> header+body -> 长度前缀帧 -> io.Writer。 +func WriteMessage(w io.Writer, msg Message) error { + payload, err := EncodeMessage(msg) + if err != nil { + return fmt.Errorf("protocol: encode message: %w", err) + } + + if err := WriteFrame(w, payload); err != nil { + return fmt.Errorf("protocol: write frame: %w", err) + } + + return nil +} + +// ReadMessage 是给上层直接使用的完整接收路径： +// io.Reader -> 长度前缀帧 -> header+body -> Message。 +func ReadMessage(r io.Reader) (Message, error) { + payload, err := ReadFrame(r) + if err != nil { + return Message{}, fmt.Errorf("protocol: read frame: %w", err) + } + + msg, err := DecodeMessage(payload) + if err != nil { + return Message{}, fmt.Errorf("protocol: decode message: %w", err) + } + + return msg, nil +} + +// validateMessage 检查 Message 传输的类型（只接受 text 和 file ）。 +func validateMessage(msg Message) error { + if msg.From == "" { + return ErrMissingFrom + } + if msg.To == "" { + return ErrMissingTo + } + + switch msg.Type { + case MessageTypeText: + if msg.FileName != "" { + return ErrUnexpectedFileName + } + if !utf8.Valid(msg.Body) { + return ErrInvalidTextBody + } + case MessageTypeFile: + if msg.FileName == "" { + return ErrMissingFileName + } + case MessageTypeRegister: + if msg.To != ServerPeerID { + return ErrInvalidRegisterTarget + } + if msg.FileName != "" { + return ErrUnexpectedFileName + } + if len(msg.Body) != 0 { + return ErrUnexpectedBody + } + case MessageTypeError: + if msg.From != ServerPeerID { + return ErrInvalidErrorSource + } + if msg.FileName != "" { + return ErrUnexpectedFileName + } + if !utf8.Valid(msg.Body) { + return ErrInvalidTextBody + } + default: + return ErrInvalidMessageType + } + + return nil +} + +// writeFull 会持续写入，直到所有字节都写完或者底层返回错误。 +// 这样可以避免某些 Writer 发生部分写入时破坏帧格式。 +func writeFull(w io.Writer, data []byte) error { + for len(data) > 0 { + n, err := w.Write(data) + if err != nil { + return err + } + if n == 0 { + return io.ErrShortWrite + } + data = data[n:] + } + + return nil +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/protocol/codec_test.go b/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/protocol/codec_test.go new file mode 100644 index 0000000..b229b36 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/protocol/codec_test.go @@ -0,0 +1,507 @@ +package protocol + +import ( + "bytes" + "encoding/binary" + "encoding/json" + "errors" + "reflect" + "strings" + "testing" +) + +// TestEncodeDecodeMessageTextASCII 验证 ASCII 文本可以按 text 消息往返编解码。 +func TestEncodeDecodeMessageTextASCII(t *testing.T) { + original := Message{ + Type: MessageTypeText, + ID: 42, + From: "peer-a", + To: "peer-b", + Body: []byte("hello"), + } + + data, err := EncodeMessage(original) + if err != nil { + t.Fatalf("EncodeMessage() error = %v", err) + } + + decoded, err := DecodeMessage(data) + if err != nil { + t.Fatalf("DecodeMessage() error = %v", err) + } + + if !reflect.DeepEqual(decoded, original) { + t.Fatalf("round trip mismatch: got %+v want %+v", decoded, original) + } +} + +// TestEncodeDecodeMessageTextUTF8 验证 text 消息允许合法 UTF-8， +// 从而天然兼容 ASCII 之外的普通文本。 +func TestEncodeDecodeMessageTextUTF8(t *testing.T) { + original := Message{ + Type: MessageTypeText, + ID: 43, + From: "peer-a", + To: "peer-b", + Body: []byte("你好, world"), + } + + data, err := EncodeMessage(original) + if err != nil { + t.Fatalf("EncodeMessage() error = %v", err) + } + + decoded, err := DecodeMessage(data) + if err != nil { + t.Fatalf("DecodeMessage() error = %v", err) + } + + if !reflect.DeepEqual(decoded, original) { + t.Fatalf("round trip mismatch: got %+v want %+v", decoded, original) + } +} + +// TestEncodeDecodeMessageFile 验证 file 消息会保留文件名和原始二进制正文。 +func TestEncodeDecodeMessageFile(t *testing.T) { + original := Message{ + Type: MessageTypeFile, + ID: 44, + From: "peer-a", + To: "peer-b", + FileName: "data.bin", + Body: []byte{0x00, 0xff, 0x10, 0x7f}, + } + + data, err := EncodeMessage(original) + if err != nil { + t.Fatalf("EncodeMessage() error = %v", err) + } + + decoded, err := DecodeMessage(data) + if err != nil { + t.Fatalf("DecodeMessage() error = %v", err) + } + + if !reflect.DeepEqual(decoded, original) { + t.Fatalf("round trip mismatch: got %+v want %+v", decoded, original) + } +} + +// TestEncodeDecodeMessageRegister 验证 register 控制消息也能正常编解码。 +func TestEncodeDecodeMessageRegister(t *testing.T) { + original := Message{ + Type: MessageTypeRegister, + ID: 45, + From: "peer-a", + To: ServerPeerID, + Body: []byte{}, + } + + data, err := EncodeMessage(original) + if err != nil { + t.Fatalf("EncodeMessage() error = %v", err) + } + + decoded, err := DecodeMessage(data) + if err != nil { + t.Fatalf("DecodeMessage() error = %v", err) + } + + if !reflect.DeepEqual(decoded, original) { + t.Fatalf("round trip mismatch: got %+v want %+v", decoded, original) + } +} + +// TestEncodeDecodeMessageError 验证 error 控制消息会保留 UTF-8 错误文本。 +func TestEncodeDecodeMessageError(t *testing.T) { + original := Message{ + Type: MessageTypeError, + ID: 46, + From: ServerPeerID, + To: "peer-a", + Body: []byte("unknown target"), + } + + data, err := EncodeMessage(original) + if err != nil { + t.Fatalf("EncodeMessage() error = %v", err) + } + + decoded, err := DecodeMessage(data) + if err != nil { + t.Fatalf("DecodeMessage() error = %v", err) + } + + if !reflect.DeepEqual(decoded, original) { + t.Fatalf("round trip mismatch: got %+v want %+v", decoded, original) + } +} + +// TestWriteReadFrame 单独验证最底层的长度前缀帧逻辑， +// 不依赖 Message 结构，方便确认 TCP 粘包拆包问题是否被正确处理。 +func TestWriteReadFrame(t *testing.T) { + var buf bytes.Buffer + payload := []byte("header+body") + + if err := WriteFrame(&buf, payload); err != nil { + t.Fatalf("WriteFrame() error = %v", err) + } + + got, err := ReadFrame(&buf) + if err != nil { + t.Fatalf("ReadFrame() error = %v", err) + } + + if !bytes.Equal(got, payload) { + t.Fatalf("payload mismatch: got %q want %q", got, payload) + } +} + +// TestWriteReadMessageAllowsEmptyBody 验证空文本和空文件都可以正常通过协议层， +// 因为外层帧非空的前提下，空正文是合法业务内容。 +func TestWriteReadMessageAllowsEmptyBody(t *testing.T) { + tests := []struct { + name string + message Message + }{ + { + name: "empty text", + message: Message{ + Type: MessageTypeText, + ID: 1, + From: "peer-a", + To: "peer-b", + Body: []byte(""), + }, + }, + { + name: "empty file", + message: Message{ + Type: MessageTypeFile, + ID: 2, + From: "peer-a", + To: "peer-b", + FileName: "empty.txt", + Body: []byte{}, + }, + }, + } + + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + var buf bytes.Buffer + + if err := WriteMessage(&buf, tt.message); err != nil { + t.Fatalf("WriteMessage() error = %v", err) + } + + got, err := ReadMessage(&buf) + if err != nil { + t.Fatalf("ReadMessage() error = %v", err) + } + + if !reflect.DeepEqual(got, tt.message) { + t.Fatalf("round trip mismatch: got %+v want %+v", got, tt.message) + } + }) + } +} + +// TestWriteReadMessageRejectsInvalidMessages 验证协议层会在编码前拦住明显非法的消息。 +func TestWriteReadMessageRejectsInvalidMessages(t *testing.T) { + tests := []struct { + name string + message Message + wantErr error + }{ + { + name: "invalid type", + message: Message{ + Type: MessageType("unknown"), + ID: 1, + From: "peer-a", + To: "peer-b", + }, + wantErr: ErrInvalidMessageType, + }, + { + name: "missing from", + message: Message{ + Type: MessageTypeText, + ID: 2, + To: "peer-b", + }, + wantErr: ErrMissingFrom, + }, + { + name: "missing to", + message: Message{ + Type: MessageTypeText, + ID: 3, + From: "peer-a", + }, + wantErr: ErrMissingTo, + }, + { + name: "text with file name", + message: Message{ + Type: MessageTypeText, + ID: 4, + From: "peer-a", + To: "peer-b", + FileName: "bad.txt", + Body: []byte("hello"), + }, + wantErr: ErrUnexpectedFileName, + }, + { + name: "text with invalid utf8", + message: Message{ + Type: MessageTypeText, + ID: 5, + From: "peer-a", + To: "peer-b", + Body: []byte{0xff, 0xfe}, + }, + wantErr: ErrInvalidTextBody, + }, + { + name: "file without file name", + message: Message{ + Type: MessageTypeFile, + ID: 6, + From: "peer-a", + To: "peer-b", + Body: []byte{0x01}, + }, + wantErr: ErrMissingFileName, + }, + { + name: "register with wrong target", + message: Message{ + Type: MessageTypeRegister, + ID: 7, + From: "peer-a", + To: "peer-b", + }, + wantErr: ErrInvalidRegisterTarget, + }, + { + name: "register with body", + message: Message{ + Type: MessageTypeRegister, + ID: 8, + From: "peer-a", + To: ServerPeerID, + Body: []byte("unexpected"), + }, + wantErr: ErrUnexpectedBody, + }, + { + name: "error with wrong source", + message: Message{ + Type: MessageTypeError, + ID: 9, + From: "peer-a", + To: "peer-b", + Body: []byte("bad"), + }, + wantErr: ErrInvalidErrorSource, + }, + { + name: "error with file name", + message: Message{ + Type: MessageTypeError, + ID: 10, + From: ServerPeerID, + To: "peer-a", + FileName: "bad.txt", + Body: []byte("bad"), + }, + wantErr: ErrUnexpectedFileName, + }, + } + + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + _, err := EncodeMessage(tt.message) + if !errors.Is(err, tt.wantErr) { + t.Fatalf("EncodeMessage() error = %v, want %v", err, tt.wantErr) + } + }) + } +} + +// TestReadFrameRejectsInvalidLength 验证长度为 0 的帧会被当成非法输入， +// 而不是被当成一条合法的空消息。 +func TestReadFrameRejectsInvalidLength(t *testing.T) { + var buf bytes.Buffer + + if err := binary.Write(&buf, binary.BigEndian, uint32(0)); err != nil { + t.Fatalf("binary.Write() error = %v", err) + } + + _, err := ReadFrame(&buf) + if !errors.Is(err, ErrInvalidFrameLength) { + t.Fatalf("ReadFrame() error = %v, want %v", err, ErrInvalidFrameLength) + } +} + +// TestReadFrameRejectsTooLargeFrame 验证超大帧会在分配消息体前被拒绝， +// 从而保证最大长度限制真正生效。 +func TestReadFrameRejectsTooLargeFrame(t *testing.T) { + var buf bytes.Buffer + + if err := binary.Write(&buf, binary.BigEndian, uint32(MaxFrameSize+1)); err != nil { + t.Fatalf("binary.Write() error = %v", err) + } + + _, err := ReadFrame(&buf) + if !errors.Is(err, ErrFrameTooLarge) { + t.Fatalf("ReadFrame() error = %v, want %v", err, ErrFrameTooLarge) + } +} + +// TestWriteFrameRejectsEmptyPayload 验证写入端和读取端的约束保持一致： +// 既然读取端不接受 0 长度帧，写入端也不应该产生这种帧。 +func TestWriteFrameRejectsEmptyPayload(t *testing.T) { + var buf bytes.Buffer + + err := WriteFrame(&buf, nil) + if !errors.Is(err, ErrInvalidFrameLength) { + t.Fatalf("WriteFrame() error = %v, want %v", err, ErrInvalidFrameLength) + } +} + +// TestDecodeMessageRejectsInvalidHeaderLength 验证无法切出完整头部时会被立即拒绝。 +func TestDecodeMessageRejectsInvalidHeaderLength(t *testing.T) { + tests := []struct { + name string + data []byte + }{ + { + name: "too short for header len", + data: []byte{0x00, 0x00, 0x00}, + }, + { + name: "zero header len", + data: []byte{0x00, 0x00, 0x00, 0x00}, + }, + { + name: "header len exceeds payload", + data: []byte{0x00, 0x00, 0x00, 0x10, '{', '}'}, + }, + } + + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + _, err := DecodeMessage(tt.data) + if !errors.Is(err, ErrInvalidHeaderLength) { + t.Fatalf("DecodeMessage() error = %v, want %v", err, ErrInvalidHeaderLength) + } + }) + } +} + +// TestDecodeMessageRejectsInvalidHeaderJSON 验证头部 JSON 非法时能返回明确错误。 +func TestDecodeMessageRejectsInvalidHeaderJSON(t *testing.T) { + data := append([]byte{0x00, 0x00, 0x00, 0x09}, []byte("{invalid}")...) + + _, err := DecodeMessage(data) + if !errors.Is(err, ErrInvalidHeaderJSON) { + t.Fatalf("DecodeMessage() error = %v, want %v", err, ErrInvalidHeaderJSON) + } +} + +// TestDecodeMessageRejectsContentLengthMismatch 验证头部声明长度和实际正文不一致时会失败。 +func TestDecodeMessageRejectsContentLengthMismatch(t *testing.T) { + headerPayload, err := json.Marshal(messageHeader{ + Type: MessageTypeText, + ID: 7, + From: "peer-a", + To: "peer-b", + ContentLength: 10, + }) + if err != nil { + t.Fatalf("json.Marshal() error = %v", err) + } + + var data bytes.Buffer + if err := binary.Write(&data, binary.BigEndian, uint32(len(headerPayload))); err != nil { + t.Fatalf("binary.Write() error = %v", err) + } + if _, err := data.Write(headerPayload); err != nil { + t.Fatalf("data.Write(headerPayload) error = %v", err) + } + if _, err := data.Write([]byte("hello")); err != nil { + t.Fatalf("data.Write(body) error = %v", err) + } + + _, err = DecodeMessage(data.Bytes()) + if !errors.Is(err, ErrInvalidContentLength) { + t.Fatalf("DecodeMessage() error = %v, want %v", err, ErrInvalidContentLength) + } +} + +// TestReadMultipleMessages 模拟同一条流中连续写入 text 和 file， +// 验证读取端每次都能严格停在当前帧边界，不会串包。 +func TestReadMultipleMessages(t *testing.T) { + var buf bytes.Buffer + + first := Message{ + Type: MessageTypeText, + ID: 1, + From: "peer-a", + To: "peer-b", + Body: []byte("hello"), + } + + second := Message{ + Type: MessageTypeFile, + ID: 2, + From: "peer-b", + To: "peer-a", + FileName: "payload.bin", + Body: []byte{0x01, 0x02, 0x03}, + } + + if err := WriteMessage(&buf, first); err != nil { + t.Fatalf("WriteMessage(first) error = %v", err) + } + if err := WriteMessage(&buf, second); err != nil { + t.Fatalf("WriteMessage(second) error = %v", err) + } + + gotFirst, err := ReadMessage(&buf) + if err != nil { + t.Fatalf("ReadMessage(first) error = %v", err) + } + gotSecond, err := ReadMessage(&buf) + if err != nil { + t.Fatalf("ReadMessage(second) error = %v", err) + } + + if !reflect.DeepEqual(gotFirst, first) { + t.Fatalf("first message mismatch: got %+v want %+v", gotFirst, first) + } + if !reflect.DeepEqual(gotSecond, second) { + t.Fatalf("second message mismatch: got %+v want %+v", gotSecond, second) + } +} + +// TestReadMessageWrapsDecodeError 验证 ReadMessage 在返回错误时会保留解码阶段上下文。 +func TestReadMessageWrapsDecodeError(t *testing.T) { + var buf bytes.Buffer + + if err := WriteFrame(&buf, append([]byte{0x00, 0x00, 0x00, 0x09}, []byte("{invalid}")...)); err != nil { + t.Fatalf("WriteFrame() error = %v", err) + } + + _, err := ReadMessage(&buf) + if err == nil { + t.Fatal("ReadMessage() error = nil, want non-nil") + } + if !strings.Contains(err.Error(), "decode message") { + t.Fatalf("ReadMessage() error = %v, want wrapped decode error", err) + } +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/protocol/message.go b/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/protocol/message.go new file mode 100644 index 0000000..5f5d28b --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/go/cmd/internal/protocol/message.go @@ -0,0 +1,33 @@ +package protocol + +// MessageType 表示一条消息的传输类型。 +// v1 只区分普通文本和文件两类负载。 +type MessageType string + +const ( + // MessageTypeText 表示正文按 UTF-8 文本解释，天然兼容 ASCII。 + MessageTypeText MessageType = "text" + // MessageTypeFile 表示正文是原始文件字节。 + MessageTypeFile MessageType = "file" + // MessageTypeRegister 表示 peer 向 server 显式注册自己的身份。 + MessageTypeRegister MessageType = "register" + // MessageTypeError 表示 server 向 peer 返回错误信息。 + MessageTypeError MessageType = "error" +) + +// ServerPeerID 是协议中约定的 server 端固定标识。 +const ServerPeerID = "server" + +// Message 是 peer 和 server 共用的传输消息结构。 +// 头部元信息会被编码为 JSON，Body 则作为原始字节拼接在头部之后。 +type Message struct { + Type MessageType `json:"type"` // 消息类型，只允许 text 或 file。 + ID uint64 `json:"id"` // 由发送方生成，用于追踪消息。 + From string `json:"from"` // 发送方标识。 + To string `json:"to"` // 接收方标识。 + + // FileName 仅在 Type 为 file 时使用。 + FileName string `json:"file_name,omitempty"` + // Body 是真正传输的正文内容，不进入头部 JSON。 + Body []byte `json:"-"` +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/go/cmd/latencysummary/main.go b/robot/ros2/OmniSocketGo_robot_ros/go/cmd/latencysummary/main.go new file mode 100644 index 0000000..1e5eac4 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/go/cmd/latencysummary/main.go @@ -0,0 +1,67 @@ +package main + +import ( + "flag" + "log" + "path/filepath" + "strings" + + "omnisocketgo/cmd/internal/latencylog" +) + +type stringListFlag []string + +func (f *stringListFlag) String() string { + return "" +} + +func (f *stringListFlag) Set(value string) error { + *f = append(*f, value) + return nil +} + +func main() { + var inputPaths stringListFlag + outputPath := flag.String("output", "latency-summary.jsonl", "output JSONL file for summarized latency metrics") + // shared-max-offset 是一个可选参数，用于在对齐输入文件的 per-file max message_id 后，排除掉最新的共享 message_id 以外的记录。它指定了要排除的共享 message_id 的数量。 + sharedMaxOffset := flag.Uint64("shared-max-offset", 1, "number of newest shared message IDs to exclude after aligning inputs by per-file max message_id") + flag.Var(&inputPaths, "input", "raw latency JSONL file path; can be provided multiple times") + flag.Parse() + + if len(inputPaths) == 0 { + log.Fatal("at least one -input raw latency log file is required") + } + + events, sharedMaxMessageID, err := latencylog.LoadEventsFromFilesWithSharedMaxOffset(inputPaths, *sharedMaxOffset) + if err != nil { + log.Fatalf("load raw latency logs: %v", err) + } + // sharedMaxMessageID 可能为 nil，表示没有可用的共享 message_id 截止值（例如因为输入文件中没有共享消息）。在这种情况下，我们将继续处理所有事件，但会记录一个警告。 + if sharedMaxMessageID != nil { + log.Printf("using shared message_id cutoff <= %d (shared-max-offset=%d)", *sharedMaxMessageID, *sharedMaxOffset) + } else { + log.Printf("no shared message_id cutoff available after applying shared-max-offset=%d", *sharedMaxOffset) + } + + summaries := latencylog.SummarizeEvents(events) + if err := latencylog.WriteSummariesJSONL(*outputPath, summaries); err != nil { + log.Fatalf("write latency summary: %v", err) + } + + chartPath := replaceFileExt(*outputPath, ".html") + if err := latencylog.WriteSummariesHTMLChart(chartPath, summaries); err != nil { + log.Fatalf("write latency chart: %v", err) + } + + log.Printf("wrote %d summarized message records to %s", len(summaries), *outputPath) + log.Printf("wrote simple latency chart to %s", chartPath) +} + +func replaceFileExt(path, ext string) string { + currentExt := filepath.Ext(path) + if currentExt == "" { + return path + ext + } + + return strings.TrimSuffix(path, currentExt) + ext +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/go/go.mod b/robot/ros2/OmniSocketGo_robot_ros/go/go.mod new file mode 100644 index 0000000..8a2d2c0 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/go/go.mod @@ -0,0 +1,16 @@ +module omnisocketgo + +go 1.24.0 + +require github.com/xtaci/kcp-go/v5 v5.6.70 + +require ( + github.com/klauspost/cpuid/v2 v2.2.6 // indirect + github.com/klauspost/reedsolomon v1.12.0 // indirect + github.com/pkg/errors v0.9.1 // indirect + github.com/tjfoc/gmsm v1.4.1 // indirect + golang.org/x/crypto v0.45.0 // indirect + golang.org/x/net v0.47.0 // indirect + golang.org/x/sys v0.38.0 // indirect + golang.org/x/time v0.14.0 // indirect +) diff --git a/robot/ros2/OmniSocketGo_robot_ros/go/go.sum b/robot/ros2/OmniSocketGo_robot_ros/go/go.sum new file mode 100644 index 0000000..1876ec0 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/go/go.sum @@ -0,0 +1,98 @@ +cloud.google.com/go v0.26.0/go.mod h1:aQUYkXzVsufM+DwF1aE+0xfcU+56JwCaLick0ClmMTw= +github.com/BurntSushi/toml v0.3.1/go.mod h1:xHWCNGjB5oqiDr8zfno3MHue2Ht5sIBksp03qcyfWMU= +github.com/census-instrumentation/opencensus-proto v0.2.1/go.mod h1:f6KPmirojxKA12rnyqOA5BBL4O983OfeGPqjHWSTneU= +github.com/client9/misspell v0.3.4/go.mod h1:qj6jICC3Q7zFZvVWo7KLAzC3yx5G7kyvSDkc90ppPyw= +github.com/cncf/udpa/go v0.0.0-20191209042840-269d4d468f6f/go.mod h1:M8M6+tZqaGXZJjfX53e64911xZQV5JYwmTeXPW+k8Sc= +github.com/davecgh/go-spew v1.1.0 h1:ZDRjVQ15GmhC3fiQ8ni8+OwkZQO4DARzQgrnXU1Liz8= +github.com/davecgh/go-spew v1.1.0/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38= +github.com/envoyproxy/go-control-plane v0.9.0/go.mod h1:YTl/9mNaCwkRvm6d1a2C3ymFceY/DCBVvsKhRF0iEA4= +github.com/envoyproxy/go-control-plane v0.9.4/go.mod h1:6rpuAdCZL397s3pYoYcLgu1mIlRU8Am5FuJP05cCM98= +github.com/envoyproxy/protoc-gen-validate v0.1.0/go.mod h1:iSmxcyjqTsJpI2R4NaDN7+kN2VEUnK/pcBlmesArF7c= +github.com/golang/glog v0.0.0-20160126235308-23def4e6c14b/go.mod h1:SBH7ygxi8pfUlaOkMMuAQtPIUF8ecWP5IEl/CR7VP2Q= +github.com/golang/mock v1.1.1/go.mod h1:oTYuIxOrZwtPieC+H1uAHpcLFnEyAGVDL/k47Jfbm0A= +github.com/golang/protobuf v1.2.0/go.mod h1:6lQm79b+lXiMfvg/cZm0SGofjICqVBUtrP5yJMmIC1U= +github.com/golang/protobuf v1.3.2/go.mod h1:6lQm79b+lXiMfvg/cZm0SGofjICqVBUtrP5yJMmIC1U= +github.com/golang/protobuf v1.3.3/go.mod h1:vzj43D7+SQXF/4pzW/hwtAqwc6iTitCiVSaWz5lYuqw= +github.com/golang/protobuf v1.4.0-rc.1/go.mod h1:ceaxUfeHdC40wWswd/P6IGgMaK3YpKi5j83Wpe3EHw8= +github.com/golang/protobuf v1.4.0-rc.1.0.20200221234624-67d41d38c208/go.mod h1:xKAWHe0F5eneWXFV3EuXVDTCmh+JuBKY0li0aMyXATA= +github.com/golang/protobuf v1.4.0-rc.2/go.mod h1:LlEzMj4AhA7rCAGe4KMBDvJI+AwstrUpVNzEA03Pprs= +github.com/golang/protobuf v1.4.0-rc.4.0.20200313231945-b860323f09d0/go.mod h1:WU3c8KckQ9AFe+yFwt9sWVRKCVIyN9cPHBJSNnbL67w= +github.com/golang/protobuf v1.4.0/go.mod h1:jodUvKwWbYaEsadDk5Fwe5c77LiNKVO9IDvqG2KuDX0= +github.com/golang/protobuf v1.4.2/go.mod h1:oDoupMAO8OvCJWAcko0GGGIgR6R6ocIYbsSw735rRwI= +github.com/google/go-cmp v0.2.0/go.mod h1:oXzfMopK8JAjlY9xF4vHSVASa0yLyX7SntLO5aqRK0M= +github.com/google/go-cmp v0.3.0/go.mod h1:8QqcDgzrUqlUb/G2PQTWiueGozuR1884gddMywk6iLU= +github.com/google/go-cmp v0.3.1/go.mod h1:8QqcDgzrUqlUb/G2PQTWiueGozuR1884gddMywk6iLU= +github.com/google/go-cmp v0.4.0/go.mod h1:v8dTdLbMG2kIc/vJvl+f65V22dbkXbowE6jgT/gNBxE= +github.com/klauspost/cpuid/v2 v2.2.6 h1:ndNyv040zDGIDh8thGkXYjnFtiN02M1PVVF+JE/48xc= +github.com/klauspost/cpuid/v2 v2.2.6/go.mod h1:Lcz8mBdAVJIBVzewtcLocK12l3Y+JytZYpaMropDUws= +github.com/klauspost/reedsolomon v1.12.0 h1:I5FEp3xSwVCcEh3F5A7dofEfhXdF/bWhQWPH+XwBFno= +github.com/klauspost/reedsolomon v1.12.0/go.mod h1:EPLZJeh4l27pUGC3aXOjheaoh1I9yut7xTURiW3LQ9Y= +github.com/pkg/errors v0.9.1 h1:FEBLx1zS214owpjy7qsBeixbURkuhQAwrK5UwLGTwt4= +github.com/pkg/errors v0.9.1/go.mod h1:bwawxfHBFNV+L2hUp1rHADufV3IMtnDRdf1r5NINEl0= +github.com/pmezard/go-difflib v1.0.0 h1:4DBwDE0NGyQoBHbLQYPwSUPoCMWR5BEzIk/f1lZbAQM= +github.com/pmezard/go-difflib v1.0.0/go.mod h1:iKH77koFhYxTK1pcRnkKkqfTogsbg7gZNVY4sRDYZ/4= +github.com/prometheus/client_model v0.0.0-20190812154241-14fe0d1b01d4/go.mod h1:xMI15A0UPsDsEKsMN9yxemIoYk6Tm2C1GtYGdfGttqA= +github.com/stretchr/testify v1.6.1 h1:hDPOHmpOpP40lSULcqw7IrRb/u7w6RpDC9399XyoNd0= +github.com/stretchr/testify v1.6.1/go.mod h1:6Fq8oRcR53rry900zMqJjRRixrwX3KX962/h/Wwjteg= +github.com/tjfoc/gmsm v1.4.1 h1:aMe1GlZb+0bLjn+cKTPEvvn9oUEBlJitaZiiBwsbgho= +github.com/tjfoc/gmsm v1.4.1/go.mod h1:j4INPkHWMrhJb38G+J6W4Tw0AbuN8Thu3PbdVYhVcTE= +github.com/xtaci/kcp-go/v5 v5.6.70 h1:AYX0QZl6PqmNj2IdYGZGuBfZuDUkUfl+eHYNijCqaO0= +github.com/xtaci/kcp-go/v5 v5.6.70/go.mod h1:9O3D8WR+cyyUjGiTILYfg17vn72otWuXK2AFfqIe6CM= +github.com/xtaci/lossyconn v0.0.0-20190602105132-8df528c0c9ae h1:J0GxkO96kL4WF+AIT3M4mfUVinOCPgf2uUWYFUzN0sM= +github.com/xtaci/lossyconn v0.0.0-20190602105132-8df528c0c9ae/go.mod h1:gXtu8J62kEgmN++bm9BVICuT/e8yiLI2KFobd/TRFsE= +golang.org/x/crypto v0.0.0-20190308221718-c2843e01d9a2/go.mod h1:djNgcEr1/C05ACkg1iLfiJU5Ep61QUkGW8qpdssI0+w= +golang.org/x/crypto v0.0.0-20200622213623-75b288015ac9/go.mod h1:LzIPMQfyMNhhGPhUkYOs5KpL4U8rLKemX1yGLhDgUto= +golang.org/x/crypto v0.0.0-20201012173705-84dcc777aaee/go.mod h1:LzIPMQfyMNhhGPhUkYOs5KpL4U8rLKemX1yGLhDgUto= +golang.org/x/crypto v0.45.0 h1:jMBrvKuj23MTlT0bQEOBcAE0mjg8mK9RXFhRH6nyF3Q= +golang.org/x/crypto v0.45.0/go.mod h1:XTGrrkGJve7CYK7J8PEww4aY7gM3qMCElcJQ8n8JdX4= +golang.org/x/exp v0.0.0-20190121172915-509febef88a4/go.mod h1:CJ0aWSM057203Lf6IL+f9T1iT9GByDxfZKAQTCR3kQA= +golang.org/x/lint v0.0.0-20181026193005-c67002cb31c3/go.mod h1:UVdnD1Gm6xHRNCYTkRU2/jEulfH38KcIWyp/GAMgvoE= +golang.org/x/lint v0.0.0-20190227174305-5b3e6a55c961/go.mod h1:wehouNa3lNwaWXcvxsM5YxQ5yQlVC4a0KAMCusXpPoU= +golang.org/x/lint v0.0.0-20190313153728-d0100b6bd8b3/go.mod h1:6SW0HCj/g11FgYtHlgUYUwCkIfeOF89ocIRzGO/8vkc= +golang.org/x/net v0.0.0-20180724234803-3673e40ba225/go.mod h1:mL1N/T3taQHkDXs73rZJwtUhF3w3ftmwwsq0BUmARs4= +golang.org/x/net v0.0.0-20180826012351-8a410e7b638d/go.mod h1:mL1N/T3taQHkDXs73rZJwtUhF3w3ftmwwsq0BUmARs4= +golang.org/x/net v0.0.0-20190213061140-3a22650c66bd/go.mod h1:mL1N/T3taQHkDXs73rZJwtUhF3w3ftmwwsq0BUmARs4= +golang.org/x/net v0.0.0-20190311183353-d8887717615a/go.mod h1:t9HGtf8HONx5eT2rtn7q6eTqICYqUVnKs3thJo3Qplg= +golang.org/x/net v0.0.0-20190404232315-eb5bcb51f2a3/go.mod h1:t9HGtf8HONx5eT2rtn7q6eTqICYqUVnKs3thJo3Qplg= +golang.org/x/net v0.0.0-20201010224723-4f7140c49acb/go.mod h1:sp8m0HH+o8qH0wwXwYZr8TS3Oi6o0r6Gce1SSxlDquU= +golang.org/x/net v0.47.0 h1:Mx+4dIFzqraBXUugkia1OOvlD6LemFo1ALMHjrXDOhY= +golang.org/x/net v0.47.0/go.mod h1:/jNxtkgq5yWUGYkaZGqo27cfGZ1c5Nen03aYrrKpVRU= +golang.org/x/oauth2 v0.0.0-20180821212333-d2e6202438be/go.mod h1:N/0e6XlmueqKjAGxoOufVs8QHGRruUQn6yWY3a++T0U= +golang.org/x/sync v0.0.0-20180314180146-1d60e4601c6f/go.mod h1:RxMgew5VJxzue5/jJTE5uejpjVlOe/izrB70Jof72aM= +golang.org/x/sync v0.0.0-20181108010431-42b317875d0f/go.mod h1:RxMgew5VJxzue5/jJTE5uejpjVlOe/izrB70Jof72aM= +golang.org/x/sync v0.0.0-20190423024810-112230192c58/go.mod h1:RxMgew5VJxzue5/jJTE5uejpjVlOe/izrB70Jof72aM= +golang.org/x/sys v0.0.0-20180830151530-49385e6e1522/go.mod h1:STP8DvDyc/dI5b8T5hshtkjS+E42TnysNCUPdjciGhY= +golang.org/x/sys v0.0.0-20190215142949-d0b11bdaac8a/go.mod h1:STP8DvDyc/dI5b8T5hshtkjS+E42TnysNCUPdjciGhY= +golang.org/x/sys v0.0.0-20190412213103-97732733099d/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs= +golang.org/x/sys v0.0.0-20200930185726-fdedc70b468f/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs= +golang.org/x/sys v0.5.0/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= +golang.org/x/sys v0.38.0 h1:3yZWxaJjBmCWXqhN1qh02AkOnCQ1poK6oF+a7xWL6Gc= +golang.org/x/sys v0.38.0/go.mod h1:OgkHotnGiDImocRcuBABYBEXf8A9a87e/uXjp9XT3ks= +golang.org/x/text v0.3.0/go.mod h1:NqM8EUOU14njkJ3fqMW+pc6Ldnwhi/IjpwHt7yyuwOQ= +golang.org/x/text v0.3.3/go.mod h1:5Zoc/QRtKVWzQhOtBMvqHzDpF6irO9z98xDceosuGiQ= +golang.org/x/time v0.14.0 h1:MRx4UaLrDotUKUdCIqzPC48t1Y9hANFKIRpNx+Te8PI= +golang.org/x/time v0.14.0/go.mod h1:eL/Oa2bBBK0TkX57Fyni+NgnyQQN4LitPmob2Hjnqw4= +golang.org/x/tools v0.0.0-20180917221912-90fa682c2a6e/go.mod h1:n7NCudcB/nEzxVGmLbDWY5pfWTLqBcC2KZ6jyYvM4mQ= +golang.org/x/tools v0.0.0-20190114222345-bf090417da8b/go.mod h1:n7NCudcB/nEzxVGmLbDWY5pfWTLqBcC2KZ6jyYvM4mQ= +golang.org/x/tools v0.0.0-20190226205152-f727befe758c/go.mod h1:9Yl7xja0Znq3iFh3HoIrodX9oNMXvdceNzlUR8zjMvY= +golang.org/x/tools v0.0.0-20190311212946-11955173bddd/go.mod h1:LCzVGOaR6xXOjkQ3onu1FJEFr0SW1gC7cKk1uF8kGRs= +golang.org/x/tools v0.0.0-20190524140312-2c0ae7006135/go.mod h1:RgjU9mgBXZiqYHBnxXauZ1Gv1EHHAz9KjViQ78xBX0Q= +golang.org/x/xerrors v0.0.0-20191204190536-9bdfabe68543/go.mod h1:I/5z698sn9Ka8TeJc9MKroUUfqBBauWjQqLJ2OPfmY0= +google.golang.org/appengine v1.1.0/go.mod h1:EbEs0AVv82hx2wNQdGPgUI5lhzA/G0D9YwlJXL52JkM= +google.golang.org/appengine v1.4.0/go.mod h1:xpcJRLb0r/rnEns0DIKYYv+WjYCduHsrkT7/EB5XEv4= +google.golang.org/genproto v0.0.0-20180817151627-c66870c02cf8/go.mod h1:JiN7NxoALGmiZfu7CAH4rXhgtRTLTxftemlI0sWmxmc= +google.golang.org/genproto v0.0.0-20190819201941-24fa4b261c55/go.mod h1:DMBHOl98Agz4BDEuKkezgsaosCRResVns1a3J2ZsMNc= +google.golang.org/grpc v1.19.0/go.mod h1:mqu4LbDTu4XGKhr4mRzUsmM4RtVoemTSY81AxZiDr8c= +google.golang.org/grpc v1.23.0/go.mod h1:Y5yQAOtifL1yxbo5wqy6BxZv8vAUGQwXBOALyacEbxg= +google.golang.org/grpc v1.25.1/go.mod h1:c3i+UQWmh7LiEpx4sFZnkU36qjEYZ0imhYfXVyQciAY= +google.golang.org/grpc v1.31.0/go.mod h1:N36X2cJ7JwdamYAgDz+s+rVMFjt3numwzf/HckM8pak= +google.golang.org/protobuf v0.0.0-20200109180630-ec00e32a8dfd/go.mod h1:DFci5gLYBciE7Vtevhsrf46CRTquxDuWsQurQQe4oz8= +google.golang.org/protobuf v0.0.0-20200221191635-4d8936d0db64/go.mod h1:kwYJMbMJ01Woi6D6+Kah6886xMZcty6N08ah7+eCXa0= +google.golang.org/protobuf v0.0.0-20200228230310-ab0ca4ff8a60/go.mod h1:cfTl7dwQJ+fmap5saPgwCLgHXTUD7jkjRqWcaiX5VyM= +google.golang.org/protobuf v1.20.1-0.20200309200217-e05f789c0967/go.mod h1:A+miEFZTKqfCUM6K7xSMQL9OKL/b6hQv+e19PK+JZNE= +google.golang.org/protobuf v1.21.0/go.mod h1:47Nbq4nVaFHyn7ilMalzfO3qCViNmqZ2kzikPIcrTAo= +google.golang.org/protobuf v1.23.0/go.mod h1:EGpADcykh3NcUnDUJcl1+ZksZNG86OlYog2l/sGQquU= +gopkg.in/yaml.v3 v3.0.1 h1:fxVm/GzAzEWqLHuvctI91KS9hhNmmWOoWu0XTYJS7CA= +gopkg.in/yaml.v3 v3.0.1/go.mod h1:K4uyk7z7BCEPqu6E+C64Yfv1cQ7kz7rIZviUmN+EgEM= +honnef.co/go/tools v0.0.0-20190102054323-c2f93a96b099/go.mod h1:rf3lG4BRIbNafJWhAfAdb/ePZxsR/4RtNHQocxwk9r4= +honnef.co/go/tools v0.0.0-20190523083050-ea95bdfd59fc/go.mod h1:rf3lG4BRIbNafJWhAfAdb/ePZxsR/4RtNHQocxwk9r4= diff --git a/robot/ros2/OmniSocketGo_robot_ros/include/cli_parse.h b/robot/ros2/OmniSocketGo_robot_ros/include/cli_parse.h new file mode 100644 index 0000000..11bbf78 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/include/cli_parse.h @@ -0,0 +1,57 @@ +#ifndef OMNI_CLI_PARSE_H +#define OMNI_CLI_PARSE_H + +#include "omni_common.h" + +static int cli_parse_bool_text(const char *raw, int *out_value) { + if (raw == NULL || out_value == NULL) { + errno = EINVAL; + return -1; + } + if (strcmp(raw, "1") == 0 || strcmp(raw, "true") == 0 || strcmp(raw, "yes") == 0 || strcmp(raw, "on") == 0) { + *out_value = 1; + return 0; + } + if (strcmp(raw, "0") == 0 || strcmp(raw, "false") == 0 || strcmp(raw, "no") == 0 || strcmp(raw, "off") == 0) { + *out_value = 0; + return 0; + } + errno = EINVAL; + return -1; +} + +static int cli_parse_value_flag(int argc, char **argv, int *index, const char *arg, const char *flag, const char **out_value) { + size_t flag_len = strlen(flag); + + if (strcmp(arg, flag) == 0) { + if (*index + 1 >= argc) { + errno = EINVAL; + return -1; + } + *out_value = argv[++(*index)]; + return 1; + } + if (strncmp(arg, flag, flag_len) == 0 && arg[flag_len] == '=') { + *out_value = arg + flag_len + 1; + return 1; + } + return 0; +} + +static int cli_parse_bool_flag(const char *arg, const char *flag, int *out_value) { + size_t flag_len = strlen(flag); + + if (strcmp(arg, flag) == 0) { + *out_value = 1; + return 1; + } + if (strncmp(arg, flag, flag_len) == 0 && arg[flag_len] == '=') { + if (cli_parse_bool_text(arg + flag_len + 1, out_value) != 0) { + return -1; + } + return 1; + } + return 0; +} + +#endif diff --git a/robot/ros2/OmniSocketGo_robot_ros/include/control_protocol.h b/robot/ros2/OmniSocketGo_robot_ros/include/control_protocol.h new file mode 100644 index 0000000..c589f1e --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/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 diff --git a/robot/ros2/OmniSocketGo_robot_ros/include/gps_buffer.h b/robot/ros2/OmniSocketGo_robot_ros/include/gps_buffer.h new file mode 100644 index 0000000..f38db88 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/include/gps_buffer.h @@ -0,0 +1,16 @@ +#ifndef GPS_BUFFER_H +#define GPS_BUFFER_H + +#include + +typedef struct gps_video_sample { + double latitude; + double longitude; +} gps_video_sample_t; + +gps_video_sample_t get_latest_gps_for_video(void); + + +int gps_buffer_init(const char* host); +void gps_buffer_cleanup(void); +#endif diff --git a/robot/ros2/OmniSocketGo_robot_ros/include/interactive.h b/robot/ros2/OmniSocketGo_robot_ros/include/interactive.h new file mode 100644 index 0000000..775f456 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/include/interactive.h @@ -0,0 +1,30 @@ +#ifndef OMNI_INTERACTIVE_H +#define OMNI_INTERACTIVE_H + +#include "omni_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef enum interactive_command_type { + INTERACTIVE_CMD_HELP = 0, + INTERACTIVE_CMD_QUIT = 1, + INTERACTIVE_CMD_TEXT = 2, + INTERACTIVE_CMD_FILE = 3 +} interactive_command_type_t; + +typedef struct interactive_command { + interactive_command_type_t type; + char to[OMNI_MAX_PEER_ID]; + char value[1024]; +} interactive_command_t; + +int interactive_parse_command(const char *line, interactive_command_t *command, char *err, size_t err_len); +void interactive_print_help(FILE *out, const char *transport_name); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/ros2/OmniSocketGo_robot_ros/include/kcp_packet_debug.h b/robot/ros2/OmniSocketGo_robot_ros/include/kcp_packet_debug.h new file mode 100644 index 0000000..45632ee --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/include/kcp_packet_debug.h @@ -0,0 +1,49 @@ +#ifndef OMNI_KCP_PACKET_DEBUG_H +#define OMNI_KCP_PACKET_DEBUG_H + +#include "omni_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct kcp_packet_debug_segment { + uint8_t cmd; + uint32_t sn; + uint32_t una; + uint8_t frg; + uint16_t wnd; + uint32_t len; +} kcp_packet_debug_segment_t; + +typedef struct kcp_packet_debug_record { + char event[OMNI_MAX_EVENT_NAME]; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + char local_addr[OMNI_MAX_ADDR_TEXT]; + char remote_addr[OMNI_MAX_ADDR_TEXT]; + int packet_bytes; + int has_udp_tx_id; + uint32_t udp_tx_id; + int has_kcp_conv; + uint32_t kcp_conv; + int64_t ts_unix_nano; + kcp_packet_debug_segment_t *segments; + size_t segment_count; +} kcp_packet_debug_record_t; + +typedef struct kcp_packet_debug_logger { + omni_file_logger_t file_logger; + int enabled; +} kcp_packet_debug_logger_t; + +kcp_packet_debug_logger_t *kcp_packet_debug_open_jsonl(const char *path); +void kcp_packet_debug_close(kcp_packet_debug_logger_t *logger); +int kcp_packet_debug_log(kcp_packet_debug_logger_t *logger, const kcp_packet_debug_record_t *record); +void kcp_packet_debug_record_clear(kcp_packet_debug_record_t *record); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/ros2/OmniSocketGo_robot_ros/include/kcp_session_stats.h b/robot/ros2/OmniSocketGo_robot_ros/include/kcp_session_stats.h new file mode 100644 index 0000000..166f237 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/include/kcp_session_stats.h @@ -0,0 +1,92 @@ +#ifndef OMNI_KCP_SESSION_STATS_H +#define OMNI_KCP_SESSION_STATS_H + +#include "omni_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define KCP_SESSION_STATS_RECORD_SESSION_SAMPLE "session_sample" +#define KCP_SESSION_STATS_RECORD_PROCESS_SAMPLE "process_snmp_sample" + +typedef struct kcp_session_stats_record { + char record_type[32]; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + char local_addr[OMNI_MAX_ADDR_TEXT]; + char remote_addr[OMNI_MAX_ADDR_TEXT]; + int has_conv; + uint32_t conv; + int64_t ts_unix_nano; + char sample_reason[32]; + int has_rto_ms; + uint32_t rto_ms; + int has_srtt_ms; + int32_t srtt_ms; + int has_min_srtt_ms; + int32_t min_srtt_ms; + int has_srttvar_ms; + int32_t srttvar_ms; + int has_last_feedback_age_ms; + uint32_t last_feedback_age_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; + uint64_t bytes_received; + int has_in_pkts; + uint64_t in_pkts; + int has_out_pkts; + uint64_t out_pkts; + int has_in_segs; + uint64_t in_segs; + int has_out_segs; + uint64_t out_segs; + int has_retrans_segs; + uint64_t retrans_segs; + int has_fast_retrans_segs; + uint64_t fast_retrans_segs; + int has_early_retrans_segs; + uint64_t early_retrans_segs; + int has_lost_segs; + uint64_t lost_segs; + int has_repeat_segs; + uint64_t repeat_segs; + int has_in_errs; + uint64_t in_errs; + int has_kcp_in_errs; + uint64_t kcp_in_errs; + int has_ring_buffer_snd_queue; + uint64_t ring_buffer_snd_queue; + int has_ring_buffer_rcv_queue; + uint64_t ring_buffer_rcv_queue; + int has_ring_buffer_snd_buffer; + uint64_t ring_buffer_snd_buffer; + int has_curr_estab; + uint64_t curr_estab; +} kcp_session_stats_record_t; + +typedef struct kcp_session_stats_logger { + omni_file_logger_t file_logger; + int enabled; +} kcp_session_stats_logger_t; + +kcp_session_stats_logger_t *kcp_session_stats_open_jsonl(const char *path); +void kcp_session_stats_close(kcp_session_stats_logger_t *logger); +int kcp_session_stats_log(kcp_session_stats_logger_t *logger, const kcp_session_stats_record_t *record); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/ros2/OmniSocketGo_robot_ros/include/latencylog.h b/robot/ros2/OmniSocketGo_robot_ros/include/latencylog.h new file mode 100644 index 0000000..809f515 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/include/latencylog.h @@ -0,0 +1,51 @@ +#ifndef OMNI_LATENCYLOG_H +#define OMNI_LATENCYLOG_H + +#include "protocol.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define EVENT_A_APP_PREP_BEGIN "A_APP_PREP_BEGIN" +#define EVENT_A_TX_SCHED "A_TX_SCHED" +#define EVENT_A_TX_SOFTWARE "A_TX_SOFTWARE" +#define EVENT_A_TX_HARDWARE "A_TX_HARDWARE" +#define EVENT_B_RX_HARDWARE "B_RX_HARDWARE" +#define EVENT_B_RX_SOFTWARE "B_RX_SOFTWARE" +#define EVENT_B_APP_RECV "B_APP_RECV" +#define EVENT_B_PERSIST_BEGIN "B_PERSIST_BEGIN" +#define EVENT_B_PERSIST_END "B_PERSIST_END" +#define EVENT_SEND_HANDOFF_BEGIN "send_handoff_begin" +#define EVENT_SEND_HANDOFF_END "send_handoff_end" + +typedef struct latency_event { + int64_t ts_unix_nano; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + char event[OMNI_MAX_EVENT_NAME]; + message_type_t message_type; + uint64_t message_id; + char from[OMNI_MAX_PEER_ID]; + char to[OMNI_MAX_PEER_ID]; + char file_name[OMNI_MAX_FILE_NAME]; + int body_size; +} latency_event_t; + +typedef struct latency_logger { + omni_file_logger_t file_logger; + int enabled; +} latency_logger_t; + +latency_logger_t *latencylog_open_jsonl(const char *path); +void latencylog_close(latency_logger_t *logger); +int latencylog_log_event(latency_logger_t *logger, const latency_event_t *event); +int latencylog_is_business_message(const message_t *msg); +void latencylog_log_message_event(latency_logger_t *logger, const char *node_role, const char *node_id, const char *event_name, const message_t *msg); +void latencylog_log_message_event_at(latency_logger_t *logger, const char *node_role, const char *node_id, const char *event_name, int64_t ts_unix_nano, const message_t *msg); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/ros2/OmniSocketGo_robot_ros/include/linux_timestamping.h b/robot/ros2/OmniSocketGo_robot_ros/include/linux_timestamping.h new file mode 100644 index 0000000..0cd2572 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/include/linux_timestamping.h @@ -0,0 +1,25 @@ +#ifndef OMNI_LINUX_TIMESTAMPING_H +#define OMNI_LINUX_TIMESTAMPING_H + +#include "omni_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct omni_tx_timestamp_event { + char event_name[OMNI_MAX_EVENT_NAME]; + int64_t ts_unix_nano; + uint32_t ee_info; + uint32_t ee_data; +} omni_tx_timestamp_event_t; + +int linux_timestamping_enable_udp_socket(int fd, int enable_rx); +int64_t linux_timestamping_parse_rx_timestamp(const struct msghdr *msg); +int linux_timestamping_parse_tx_timestamp(const struct msghdr *msg, omni_tx_timestamp_event_t *out_event); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/ros2/OmniSocketGo_robot_ros/include/omni_common.h b/robot/ros2/OmniSocketGo_robot_ros/include/omni_common.h new file mode 100644 index 0000000..09b8432 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/include/omni_common.h @@ -0,0 +1,78 @@ +#ifndef OMNI_COMMON_H +#define OMNI_COMMON_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define OMNI_NODE_ROLE_PEER "peer" +#define OMNI_NODE_ROLE_SERVER "server" + +#define OMNI_MAX_PEER_ID 64 +#define OMNI_MAX_NODE_ROLE 16 +#define OMNI_MAX_EVENT_NAME 64 +#define OMNI_MAX_FILE_NAME 256 +#define OMNI_MAX_ADDR_TEXT 128 +#define OMNI_MAX_FRAME_SIZE (8U * 1024U * 1024U) + +#define OMNI_ARRAY_LEN(x) (sizeof(x) / sizeof((x)[0])) + +typedef struct omni_file_logger { + FILE *file; + pthread_mutex_t mutex; + char path[PATH_MAX]; + size_t current_bytes; + size_t buffered_bytes; + size_t flush_bytes; + size_t max_bytes; + int flush_interval_ms; + int max_files; + int immediate_flush; + uint64_t last_flush_monotonic_ms; +} omni_file_logger_t; + +int64_t omni_now_unix_nano(void); +uint32_t omni_now_millis32(void); + +int omni_set_nonblocking(int fd, int enabled); +int omni_parse_sockaddr(const char *raw, int passive, struct sockaddr_storage *addr, socklen_t *addr_len, int *family_out); +int omni_clone_sockaddr(const struct sockaddr *src, socklen_t src_len, struct sockaddr_storage *dst, socklen_t *dst_len); +const char *omni_sockaddr_to_string(const struct sockaddr *addr, socklen_t addr_len, char *buffer, size_t buffer_len); + +int omni_bind_device(int fd, const char *device); +int omni_ensure_dir(const char *path); +int omni_ensure_parent_dir(const char *path); +int omni_read_file(const char *path, uint8_t **out, size_t *out_len); +int omni_write_full_fd(int fd, const uint8_t *data, size_t len); +int omni_append_file(const char *path, const uint8_t *data, size_t len); +int omni_write_file(const char *path, const uint8_t *data, size_t len); +int omni_random_u32(uint32_t *out); + +char *omni_strdup(const char *src); +char *omni_strdup_printf(const char *fmt, ...); +char *omni_json_escape(const char *src); +char *omni_json_escape_bytes(const uint8_t *src, size_t len); +int omni_utf8_valid(const uint8_t *data, size_t len); +void omni_trim_newline(char *line); +int omni_parse_duration_ms(const char *raw, int default_ms, int *out_ms); +double omni_duration_ms_to_ns(double ms); +const char *omni_path_base_name(const char *path); + +void omni_file_logger_init(omni_file_logger_t *logger, FILE *file); +void omni_file_logger_init_path(omni_file_logger_t *logger, FILE *file, const char *path, int immediate_flush); +void omni_file_logger_destroy(omni_file_logger_t *logger); +int omni_file_logger_write_line(omni_file_logger_t *logger, const char *line); + +#endif diff --git a/robot/ros2/OmniSocketGo_robot_ros/include/peer_kcp_client.h b/robot/ros2/OmniSocketGo_robot_ros/include/peer_kcp_client.h new file mode 100644 index 0000000..426ec3d --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/include/peer_kcp_client.h @@ -0,0 +1,46 @@ +#ifndef OMNI_PEER_KCP_CLIENT_H +#define OMNI_PEER_KCP_CLIENT_H + +#include "transport_kcp.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct kcp_client kcp_client_t; +typedef struct kcp_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; +} kcp_client_recv_meta_t; +typedef struct kcp_client_state { + int connected; + int registered; + uint32_t server_idle_ms; + char last_server_error[256]; +} kcp_client_state_t; + +kcp_client_t *kcp_client_dial_with_options(const char *server_addr, const char *dial_addr, const char *peer_id, const char *bind_ip, const char *bind_device, const kcp_conn_options_t *options, latency_logger_t *logger, kcp_packet_debug_logger_t *packet_logger, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms); +kcp_client_t *kcp_client_dial(const char *server_addr, const char *dial_addr, const char *peer_id, const char *bind_ip, const char *bind_device, latency_logger_t *logger, kcp_packet_debug_logger_t *packet_logger, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms); +const char *kcp_client_id(const kcp_client_t *client); +int kcp_client_send_text(kcp_client_t *client, const char *to, const char *text); +int kcp_client_send_binary(kcp_client_t *client, const char *to, const void *data, size_t data_len); +int kcp_client_send_binary_with_id(kcp_client_t *client, const char *to, const void *data, size_t data_len, uint64_t *out_id); +int kcp_client_send_file_path(kcp_client_t *client, const char *to, const char *path); +int kcp_client_receive_timed(kcp_client_t *client, message_t *out_msg, int timeout_ms); +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); +void kcp_client_state_snapshot(kcp_client_t *client, kcp_client_state_t *out_state); +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); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/ros2/OmniSocketGo_robot_ros/include/peer_udp_client.h b/robot/ros2/OmniSocketGo_robot_ros/include/peer_udp_client.h new file mode 100644 index 0000000..937e49e --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/include/peer_udp_client.h @@ -0,0 +1,37 @@ +#ifndef OMNI_PEER_UDP_CLIENT_H +#define OMNI_PEER_UDP_CLIENT_H + +#include "transport_udp.h" + +#ifdef __cplusplus +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); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/ros2/OmniSocketGo_robot_ros/include/protocol.h b/robot/ros2/OmniSocketGo_robot_ros/include/protocol.h new file mode 100644 index 0000000..a6c64ad --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/include/protocol.h @@ -0,0 +1,62 @@ +#ifndef OMNI_PROTOCOL_H +#define OMNI_PROTOCOL_H + +#include "omni_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef enum message_type { + MSG_TYPE_TEXT = 0, + MSG_TYPE_FILE = 1, + MSG_TYPE_REGISTER = 2, + MSG_TYPE_ERROR = 3, + MSG_TYPE_BINARY = 4, + MSG_TYPE_INVALID = 255 +} message_type_t; + +#define SERVER_PEER_ID "server" + +typedef struct message { + 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]; + uint8_t *body; + size_t body_len; +} message_t; + +typedef struct protocol_frame_decoder { + uint8_t *buffer; + size_t len; + size_t cap; +} protocol_frame_decoder_t; + +const char *protocol_message_type_name(message_type_t type); +int protocol_message_type_from_name(const char *raw, message_type_t *out); + +void protocol_message_init(message_t *msg); +void protocol_message_clear(message_t *msg); +int protocol_message_copy(message_t *dst, const message_t *src); + +int protocol_validate_message(const message_t *msg, char *err, size_t err_len); + +int protocol_encode_message_datagram(const message_t *msg, uint8_t **out, size_t *out_len); +int protocol_decode_message_datagram(const uint8_t *data, size_t data_len, message_t *out_msg, char *err, size_t err_len); + +int protocol_encode_message_stream(const message_t *msg, uint8_t **out, size_t *out_len); +int protocol_decode_message_stream_payload(const uint8_t *payload, size_t payload_len, message_t *out_msg, char *err, size_t err_len); + +void protocol_frame_decoder_init(protocol_frame_decoder_t *decoder); +void protocol_frame_decoder_reset(protocol_frame_decoder_t *decoder); +void protocol_frame_decoder_destroy(protocol_frame_decoder_t *decoder); +int protocol_frame_decoder_feed(protocol_frame_decoder_t *decoder, const uint8_t *data, size_t data_len); +int protocol_frame_decoder_next(protocol_frame_decoder_t *decoder, uint8_t **payload, size_t *payload_len); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/ros2/OmniSocketGo_robot_ros/include/ros_image_shm.h b/robot/ros2/OmniSocketGo_robot_ros/include/ros_image_shm.h new file mode 100644 index 0000000..1e661f6 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/include/ros_image_shm.h @@ -0,0 +1,77 @@ +#ifndef OMNI_ROS_IMAGE_SHM_H +#define OMNI_ROS_IMAGE_SHM_H + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define ROS_IMAGE_SHM_MAGIC 0x52494d47U /* "RIMG" */ +#define ROS_IMAGE_SHM_VERSION 1U +#define ROS_IMAGE_SHM_HEADER_BYTES 64U +#define ROS_IMAGE_SHM_DEFAULT_MAX_FRAME_BYTES (1920U * 1080U * 4U) + +enum ros_image_encoding { + ROS_IMAGE_ENCODING_RGB8 = 1, + ROS_IMAGE_ENCODING_BGR8 = 2, + ROS_IMAGE_ENCODING_RGBA8 = 3, + ROS_IMAGE_ENCODING_BGRA8 = 4, + ROS_IMAGE_ENCODING_MONO8 = 5 +}; + +/* This layout is also used by the Python ROS2 bridge (struct format = 201112L +_Static_assert(sizeof(ros_image_shm_header_t) == ROS_IMAGE_SHM_HEADER_BYTES, "ROS image SHM header size mismatch"); +#endif + +int ros_image_shm_open( + ros_image_shm_source_t *source, + const char *path, + size_t max_frame_bytes +); + +void ros_image_shm_close(ros_image_shm_source_t *source); + +/* + * Wait for a newer frame and copy it into caller-owned storage. + * Returns 1 for a frame, 0 for timeout, and -1 for an invalid/error frame. + */ +int ros_image_shm_read_latest( + ros_image_shm_source_t *source, + uint8_t *destination, + size_t destination_bytes, + ros_image_shm_header_t *header, + int timeout_ms +); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/ros2/OmniSocketGo_robot_ros/include/server_kcp_hub.h b/robot/ros2/OmniSocketGo_robot_ros/include/server_kcp_hub.h new file mode 100644 index 0000000..37140df --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/include/server_kcp_hub.h @@ -0,0 +1,27 @@ +#ifndef OMNI_SERVER_KCP_HUB_H +#define OMNI_SERVER_KCP_HUB_H + +#include "transport_kcp.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct kcp_hub kcp_hub_t; + +kcp_hub_t *kcp_hub_new(latency_logger_t *logger, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms); +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); +void kcp_hub_free(kcp_hub_t *hub); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/ros2/OmniSocketGo_robot_ros/include/server_udp_hub.h b/robot/ros2/OmniSocketGo_robot_ros/include/server_udp_hub.h new file mode 100644 index 0000000..7baed3c --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/include/server_udp_hub.h @@ -0,0 +1,21 @@ +#ifndef OMNI_SERVER_UDP_HUB_H +#define OMNI_SERVER_UDP_HUB_H + +#include "transport_udp.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct udp_hub udp_hub_t; + +udp_hub_t *udp_hub_open(const char *listen_addr, latency_logger_t *logger, tx_timestamp_debug_logger_t *debug_logger, int enable_timestamping); +int udp_hub_serve(udp_hub_t *hub); +int udp_hub_close(udp_hub_t *hub); +void udp_hub_free(udp_hub_t *hub); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/ros2/OmniSocketGo_robot_ros/include/server_udp_relay.h b/robot/ros2/OmniSocketGo_robot_ros/include/server_udp_relay.h new file mode 100644 index 0000000..1c7728e --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/include/server_udp_relay.h @@ -0,0 +1,21 @@ +#ifndef OMNI_SERVER_UDP_RELAY_H +#define OMNI_SERVER_UDP_RELAY_H + +#include "omni_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct udp_relay udp_relay_t; + +udp_relay_t *udp_relay_open(const char *listen_addr, const char *upstream_addr); +int udp_relay_serve(udp_relay_t *relay); +int udp_relay_close(udp_relay_t *relay); +void udp_relay_free(udp_relay_t *relay); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/ros2/OmniSocketGo_robot_ros/include/transport_kcp.h b/robot/ros2/OmniSocketGo_robot_ros/include/transport_kcp.h new file mode 100644 index 0000000..f9e1bbc --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/include/transport_kcp.h @@ -0,0 +1,117 @@ +#ifndef OMNI_TRANSPORT_KCP_H +#define OMNI_TRANSPORT_KCP_H + +#include "kcp_packet_debug.h" +#include "kcp_session_stats.h" +#include "latencylog.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define KCP_DEFAULT_NODELAY 1 +#define KCP_DEFAULT_INTERVAL_MS 10 +#define KCP_DEFAULT_RESEND 2 +#define KCP_DEFAULT_NC 1 +#define KCP_DEFAULT_SND_WND 256 +#define KCP_DEFAULT_RCV_WND 256 +#define KCP_DEFAULT_MTU 1400 +#define KCP_DEFAULT_STATS_INTERVAL_MS 100 + +#define KCP_CONTROL_NODELAY 1 +#define KCP_CONTROL_INTERVAL_MS 5 +#define KCP_CONTROL_RESEND 2 +#define KCP_CONTROL_NC 1 +#define KCP_CONTROL_SND_WND 32 +#define KCP_CONTROL_RCV_WND 32 +#define KCP_CONTROL_MTU 1400 + +#define KCP_VIDEO_NODELAY 1 +#define KCP_VIDEO_INTERVAL_MS 10 +#define KCP_VIDEO_RESEND 2 +#define KCP_VIDEO_NC 1 +#define KCP_VIDEO_SND_WND 256 +#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 +#define KCP_NC KCP_DEFAULT_NC +#define KCP_WND_SIZE KCP_DEFAULT_SND_WND +#define KCP_MTU KCP_DEFAULT_MTU + +typedef struct kcp_conn kcp_conn_t; +typedef struct kcp_listener kcp_listener_t; +typedef struct kcp_runtime_stats { + int connected; + uint32_t conv; + uint32_t rto_ms; + int32_t srtt_ms; + int32_t min_srtt_ms; + int32_t srttvar_ms; + uint32_t last_feedback_age_ms; + uint32_t snd_wnd; + uint32_t rmt_wnd; + uint32_t inflight; + uint32_t window_limit; + double window_pressure_pct; + uint32_t snd_queue; + uint32_t rcv_queue; + uint32_t snd_buffer; + uint64_t out_segs_total; + uint64_t retrans_total; + uint64_t fast_retrans_total; + uint64_t lost_total; + uint64_t repeat_total; + uint32_t xmit_total; +} kcp_runtime_stats_t; +typedef struct kcp_conn_options { + int nodelay; + int interval_ms; + int resend; + int nc; + int sndwnd; + int rcvwnd; + int mtu; +} kcp_conn_options_t; + +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); +int kcp_conn_configure_runtime(kcp_conn_t *conn, latency_logger_t *logger, const char *node_role, const char *node_id, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms); +int kcp_conn_apply_options(kcp_conn_t *conn, const kcp_conn_options_t *options); +int kcp_conn_send(kcp_conn_t *conn, const message_t *msg); +int kcp_conn_receive_timed(kcp_conn_t *conn, message_t *out_msg, int timeout_ms); +int kcp_conn_receive(kcp_conn_t *conn, message_t *out_msg); +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_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); +int kcp_listener_close(kcp_listener_t *listener); +void kcp_listener_free(kcp_listener_t *listener); + +int kcp_session_stats_parse_interval_ms(const char *raw, int *out_ms); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/ros2/OmniSocketGo_robot_ros/include/transport_udp.h b/robot/ros2/OmniSocketGo_robot_ros/include/transport_udp.h new file mode 100644 index 0000000..54e7155 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/include/transport_udp.h @@ -0,0 +1,30 @@ +#ifndef OMNI_TRANSPORT_UDP_H +#define OMNI_TRANSPORT_UDP_H + +#include "latencylog.h" +#include "linux_timestamping.h" +#include "tx_timestamp_debug.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct udp_conn udp_conn_t; + +udp_conn_t *udp_conn_dial(const char *server_addr, const char *bind_ip, const char *bind_device, int enable_timestamping, latency_logger_t *logger, const char *node_role, const char *node_id, tx_timestamp_debug_logger_t *debug_logger); +udp_conn_t *udp_conn_bind(const char *listen_addr, const char *bind_device, int enable_timestamping, latency_logger_t *logger, const char *node_role, const char *node_id, tx_timestamp_debug_logger_t *debug_logger); + +int udp_conn_send(udp_conn_t *conn, const message_t *msg); +int udp_conn_send_to(udp_conn_t *conn, const message_t *msg, const struct sockaddr *addr, socklen_t addr_len); +int udp_conn_receive(udp_conn_t *conn, message_t *out_msg, struct sockaddr_storage *addr, socklen_t *addr_len); + +int udp_conn_fd(const udp_conn_t *conn); +int udp_conn_local_addr(const udp_conn_t *conn, struct sockaddr_storage *addr, socklen_t *addr_len); +int udp_conn_close(udp_conn_t *conn); +void udp_conn_free(udp_conn_t *conn); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/ros2/OmniSocketGo_robot_ros/include/tx_timestamp_debug.h b/robot/ros2/OmniSocketGo_robot_ros/include/tx_timestamp_debug.h new file mode 100644 index 0000000..c5795ca --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/include/tx_timestamp_debug.h @@ -0,0 +1,51 @@ +#ifndef OMNI_TX_TIMESTAMP_DEBUG_H +#define OMNI_TX_TIMESTAMP_DEBUG_H + +#include "protocol.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define TX_TIMESTAMP_DEBUG_RECORD_SEND_CHUNK "send_chunk" +#define TX_TIMESTAMP_DEBUG_RECORD_ERRQUEUE_EVENT "errqueue_event" + +typedef struct tx_timestamp_debug_record { + char record_type[32]; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + message_type_t message_type; + uint64_t message_id; + char from[OMNI_MAX_PEER_ID]; + char to[OMNI_MAX_PEER_ID]; + char file_name[OMNI_MAX_FILE_NAME]; + int body_size; + char phase[32]; + int send_call_index; + int frame_offset_start; + int frame_offset_end; + int bytes_written; + uint32_t expected_tx_id; + int read_index; + char event_name[OMNI_MAX_EVENT_NAME]; + int64_t ts_unix_nano; + uint32_t ee_info; + uint32_t ee_data; + int matched_send_call_index; + int selected_for_latency; +} tx_timestamp_debug_record_t; + +typedef struct tx_timestamp_debug_logger { + omni_file_logger_t file_logger; + int enabled; +} tx_timestamp_debug_logger_t; + +tx_timestamp_debug_logger_t *tx_timestamp_debug_open_jsonl(const char *path); +void tx_timestamp_debug_close(tx_timestamp_debug_logger_t *logger); +int tx_timestamp_debug_log(tx_timestamp_debug_logger_t *logger, const tx_timestamp_debug_record_t *record); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/ros2/OmniSocketGo_robot_ros/include/video_pipeline.h b/robot/ros2/OmniSocketGo_robot_ros/include/video_pipeline.h new file mode 100644 index 0000000..1101405 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/include/video_pipeline.h @@ -0,0 +1,115 @@ +#ifndef OMNI_VIDEO_PIPELINE_H +#define OMNI_VIDEO_PIPELINE_H + +#include +#include +#include +#include +#include + +#include "gps_buffer.h" +#include "omni_common.h" +#include "peer_kcp_client.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined(__GNUC__) +typedef struct __attribute__((packed)) video_pipeline_packet_metadata { +#else +typedef struct video_pipeline_packet_metadata { +#endif + uint64_t timestamp_ms; + double latitude; + double longitude; + uint32_t capture_to_send_ms; +} video_pipeline_packet_metadata_t; + +typedef struct video_stage_logger { + omni_file_logger_t file_logger; + int enabled; + uint64_t sample_mod; +} video_stage_logger_t; + +typedef void (*video_pipeline_progress_fn)(void *context); + +typedef enum video_input_mode { + VIDEO_INPUT_ROS2 = 0, + VIDEO_INPUT_V4L2 = 1 +} video_input_mode_t; + +#if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L +_Static_assert(sizeof(video_pipeline_packet_metadata_t) == 28, "video trailer metadata must be 28 bytes"); +#endif + +typedef struct video_pipeline_config { + video_input_mode_t input_mode; + const char *camera_device; + const char *camera_head_device; + const char *camera_waist_device; + const char *ros_head_shm; + const char *ros_waist_shm; + size_t ros_max_frame_bytes; + atomic_int *active_camera; + 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; + int soft_backpressure_segments; + int hard_backpressure_segments; + int hard_backpressure_hold_ms; + int frame_stall_reconnect_ms; + kcp_session_stats_logger_t *stats_logger; + video_stage_logger_t *stage_logger; + int stats_interval_ms; + video_pipeline_progress_fn progress_callback; + void *progress_context; +} video_pipeline_config_t; + +enum { + VIDEO_CAMERA_HEAD = 0, + VIDEO_CAMERA_WAIST = 1 +}; + +typedef struct video_pipeline_stats { + pthread_mutex_t mutex; + uint64_t frames_sent; + uint64_t bytes_sent; + uint64_t send_errors; + uint64_t backpressure_drops; + uint64_t backlog_resets; + uint64_t last_frame_bytes; + uint32_t last_backlog_segments; + uint32_t last_capture_to_send_ms; + double avg_capture_to_send_ms; + int connected; + char last_error[256]; + char last_backlog_reason[128]; + kcp_runtime_stats_t transport; +} video_pipeline_stats_t; + +#define VIDEO_PIPELINE_RUN_RETRY_IMMEDIATE 2 + +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); +video_stage_logger_t *video_stage_logger_open_jsonl(const char *path, uint64_t sample_mod); +void video_stage_logger_close(video_stage_logger_t *logger); +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 diff --git a/robot/ros2/OmniSocketGo_robot_ros/python/omnisocket/__init__.py b/robot/ros2/OmniSocketGo_robot_ros/python/omnisocket/__init__.py new file mode 100644 index 0000000..2b23277 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/python/omnisocket/__init__.py @@ -0,0 +1,57 @@ +try: + from ._omnisocket import ( + MSG_TYPE_BINARY, + MSG_TYPE_ERROR, + MSG_TYPE_FILE, + MSG_TYPE_REGISTER, + MSG_TYPE_TEXT, + Session, + UdpSession, + ) +except ImportError as exc: + raise ImportError( + "omnisocket extension is not built; run `make python-ext` on a Linux host first" + ) from exc + +CONTROL_DEFAULTS = { + "nodelay": 1, + "interval_ms": 5, + "resend": 2, + "nc": 1, + "sndwnd": 32, + "rcvwnd": 32, + "mtu": 1400, +} + +VIDEO_DEFAULTS = { + "nodelay": 1, + "interval_ms": 10, + "resend": 2, + "nc": 1, + "sndwnd": 256, + "rcvwnd": 256, + "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", + "MSG_TYPE_FILE", + "MSG_TYPE_REGISTER", + "MSG_TYPE_TEXT", + "Session", + "UdpSession", +] diff --git a/robot/ros2/OmniSocketGo_robot_ros/python/omnisocket/_omnisocket.c b/robot/ros2/OmniSocketGo_robot_ros/python/omnisocket/_omnisocket.c new file mode 100644 index 0000000..1ba6830 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/python/omnisocket/_omnisocket.c @@ -0,0 +1,684 @@ +#define PY_SSIZE_T_CLEAN +#include + +#include "omnisocket_client.h" + +typedef struct PyOmniSession { + PyObject_HEAD + 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" +); + +PyDoc_STRVAR( + PyOmniSession_recv_into_doc, + "recv_into(buffer, timeout_ms=-1) -> dict | None\n" + "\n" + "The writable buffer must be large enough for the full message body.\n" + "If it is too small, BufferError reports the required size but the\n" + "current frame has already been consumed and is lost." +); + +static PyObject *build_recv_result(const message_t *msg) { + PyObject *body = NULL; + PyObject *result = NULL; + + 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,s:i,s:s}", + "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, + "registered", + stats->registered, + "last_server_error", + stats->last_server_error + ); +} + +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("min_srtt_ms", PyLong_FromLong(stats->min_srtt_ms)); + SET_KCP_STAT("srttvar_ms", PyLong_FromLong(stats->srttvar_ms)); + SET_KCP_STAT("last_feedback_age_ms", PyLong_FromUnsignedLong(stats->last_feedback_age_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; + (void) args; + (void) kwargs; + + self = (PyOmniSession *) type->tp_alloc(type, 0); + if (self == NULL) { + return NULL; + } + if (omnisocket_session_init(&self->session) != 0) { + type->tp_free((PyObject *) self); + return PyErr_SetFromErrno(PyExc_OSError); + } + return (PyObject *) self; +} + +static void PyOmniSession_dealloc(PyOmniSession *self) { + omnisocket_session_destroy(&self->session); + Py_TYPE(self)->tp_free((PyObject *) self); +} + +static PyObject *PyOmniSession_connect(PyOmniSession *self, PyObject *args, PyObject *kwargs) { + const char *server_addr; + const char *peer_id; + const char *relay_via = ""; + const char *bind_ip = ""; + const char *bind_device = ""; + int nodelay = KCP_DEFAULT_NODELAY; + int interval_ms = KCP_DEFAULT_INTERVAL_MS; + int resend = KCP_DEFAULT_RESEND; + int nc = KCP_DEFAULT_NC; + int sndwnd = KCP_DEFAULT_SND_WND; + int rcvwnd = KCP_DEFAULT_RCV_WND; + int mtu = KCP_DEFAULT_MTU; + int stats_interval_ms = KCP_DEFAULT_STATS_INTERVAL_MS; + kcp_conn_options_t options; + int rc; + + static char *kwlist[] = { + "server_addr", + "peer_id", + "relay_via", + "bind_ip", + "bind_device", + "nodelay", + "interval_ms", + "resend", + "nc", + "sndwnd", + "rcvwnd", + "mtu", + "stats_interval_ms", + NULL + }; + + if (!PyArg_ParseTupleAndKeywords( + args, + kwargs, + "ss|sssiiiiiiii", + kwlist, + &server_addr, + &peer_id, + &relay_via, + &bind_ip, + &bind_device, + &nodelay, + &interval_ms, + &resend, + &nc, + &sndwnd, + &rcvwnd, + &mtu, + &stats_interval_ms)) { + return NULL; + } + + kcp_conn_options_init(&options); + options.nodelay = nodelay; + options.interval_ms = interval_ms; + options.resend = resend; + options.nc = nc; + options.sndwnd = sndwnd; + options.rcvwnd = rcvwnd; + options.mtu = mtu; + + Py_BEGIN_ALLOW_THREADS + rc = omnisocket_session_connect( + &self->session, + server_addr, + relay_via, + peer_id, + bind_ip, + bind_device, + &options, + stats_interval_ms + ); + Py_END_ALLOW_THREADS + + if (rc != 0) { + return PyErr_SetFromErrno(PyExc_OSError); + } + Py_RETURN_NONE; +} + +static PyObject *PyOmniSession_close(PyOmniSession *self, PyObject *Py_UNUSED(ignored)) { + int rc; + + Py_BEGIN_ALLOW_THREADS + rc = omnisocket_session_close(&self->session); + Py_END_ALLOW_THREADS + + if (rc != 0) { + return PyErr_SetFromErrno(PyExc_OSError); + } + Py_RETURN_NONE; +} + +static PyObject *PyOmniSession_send(PyOmniSession *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_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 *PyOmniSession_send_with_id(PyOmniSession *self, PyObject *args, PyObject *kwargs) { + const char *to; + Py_buffer payload; + int rc; + uint64_t message_id = 0; + 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_session_send_with_id(&self->session, to, payload.buf, (size_t) payload.len, &message_id); + Py_END_ALLOW_THREADS + + PyBuffer_Release(&payload); + if (rc != 0) { + return PyErr_SetFromErrno(PyExc_OSError); + } + return PyLong_FromUnsignedLongLong((unsigned long long) message_id); +} + +static PyObject *PyOmniSession_recv(PyOmniSession *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_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 *PyOmniSession_recv_into(PyOmniSession *self, PyObject *args, PyObject *kwargs) { + PyObject *buffer_obj; + Py_buffer view; + int timeout_ms = -1; + int rc; + kcp_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_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 *PyOmniSession_stats(PyOmniSession *self, PyObject *Py_UNUSED(ignored)) { + omnisocket_session_stats_t stats; + + memset(&stats, 0, sizeof(stats)); + omnisocket_session_stats_snapshot(&self->session, &stats); + return build_stats_dict(&stats); +} + +static PyObject *PyOmniSession_kcp_stats(PyOmniSession *self, PyObject *Py_UNUSED(ignored)) { + omnisocket_session_kcp_stats_t stats; + + memset(&stats, 0, sizeof(stats)); + omnisocket_session_kcp_stats_snapshot(&self->session, &stats); + return build_kcp_stats_dict(&stats); +} + +static PyMethodDef PyOmniSession_methods[] = { + {"connect", (PyCFunction) PyOmniSession_connect, METH_VARARGS | METH_KEYWORDS, NULL}, + {"close", (PyCFunction) PyOmniSession_close, METH_NOARGS, NULL}, + {"send", (PyCFunction) PyOmniSession_send, METH_VARARGS | METH_KEYWORDS, NULL}, + {"send_with_id", (PyCFunction) PyOmniSession_send_with_id, METH_VARARGS | METH_KEYWORDS, NULL}, + {"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_stats", (PyCFunction) PyOmniSession_kcp_stats, METH_NOARGS, NULL}, + {NULL, NULL, 0, NULL} +}; + +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", + .m_size = -1, +}; + +PyMODINIT_FUNC PyInit__omnisocket(void) { + PyObject *module; + + PyOmniSessionType.tp_name = "omnisocket.Session"; + PyOmniSessionType.tp_basicsize = sizeof(PyOmniSession); + PyOmniSessionType.tp_flags = Py_TPFLAGS_DEFAULT; + PyOmniSessionType.tp_new = PyOmniSession_new; + PyOmniSessionType.tp_dealloc = (destructor) PyOmniSession_dealloc; + PyOmniSessionType.tp_methods = PyOmniSession_methods; + + if (PyType_Ready(&PyOmniSessionType) < 0) { + 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; + } + + Py_INCREF(&PyOmniSessionType); + if (PyModule_AddObject(module, "Session", (PyObject *) &PyOmniSessionType) != 0) { + Py_DECREF(&PyOmniSessionType); + Py_DECREF(module); + 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 || + PyModule_AddIntConstant(module, "MSG_TYPE_ERROR", MSG_TYPE_ERROR) != 0 || + PyModule_AddIntConstant(module, "MSG_TYPE_BINARY", MSG_TYPE_BINARY) != 0) { + Py_DECREF(module); + return NULL; + } + + return module; +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/python/omnisocket/omnisocket_client.c b/robot/ros2/OmniSocketGo_robot_ros/python/omnisocket/omnisocket_client.c new file mode 100644 index 0000000..3dd0a0f --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/python/omnisocket/omnisocket_client.c @@ -0,0 +1,572 @@ +#include "omnisocket_client.h" + +static void omnisocket_session_sync_client_state_locked(omnisocket_session_t *session, kcp_client_t *client) { + kcp_client_state_t client_state; + + if (session == NULL) { + return; + } + memset(&client_state, 0, sizeof(client_state)); + if (client != NULL) { + kcp_client_state_snapshot(client, &client_state); + } + session->stats.connected = client_state.connected; + session->stats.registered = client_state.registered; + snprintf( + session->stats.last_server_error, + sizeof(session->stats.last_server_error), + "%s", + client_state.last_server_error + ); +} + +static void omnisocket_session_mark_disconnected_locked(omnisocket_session_t *session) { + if (session == NULL) { + return; + } + session->stats.connected = 0; + session->stats.registered = 0; +} + +int omnisocket_session_init(omnisocket_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_session_destroy(omnisocket_session_t *session) { + if (session == NULL) { + return; + } + (void) omnisocket_session_close(session); + pthread_cond_destroy(&session->idle_cond); + pthread_mutex_destroy(&session->mutex); +} + +static int omnisocket_session_begin_client_op(omnisocket_session_t *session, kcp_client_t **out_client) { + if (session == NULL || out_client == NULL) { + errno = EINVAL; + return -1; + } + + 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_session_connect( + omnisocket_session_t *session, + const char *server_addr, + const char *relay_via, + const char *peer_id, + const char *bind_ip, + const char *bind_device, + const kcp_conn_options_t *options, + int stats_interval_ms +) { + kcp_client_t *client; + + if (session == NULL || server_addr == NULL || peer_id == NULL) { + errno = EINVAL; + return -1; + } + + pthread_mutex_lock(&session->mutex); + while (session->closing) { + pthread_cond_wait(&session->idle_cond, &session->mutex); + } + if (session->client != NULL) { + pthread_mutex_unlock(&session->mutex); + errno = EISCONN; + return -1; + } + client = kcp_client_dial_with_options( + server_addr, + relay_via, + peer_id, + bind_ip, + bind_device, + options, + NULL, + NULL, + NULL, + stats_interval_ms + ); + if (client == NULL) { + pthread_mutex_unlock(&session->mutex); + return -1; + } + session->client = client; + omnisocket_session_sync_client_state_locked(session, client); + pthread_mutex_unlock(&session->mutex); + return 0; +} + +int omnisocket_session_close(omnisocket_session_t *session) { + kcp_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; + } + omnisocket_session_mark_disconnected_locked(session); + pthread_mutex_unlock(&session->mutex); + + if (client != NULL) { + kcp_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); + kcp_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_session_send(omnisocket_session_t *session, const char *to, const void *data, size_t data_len) { + return omnisocket_session_send_with_id(session, to, data, data_len, NULL); +} + +int omnisocket_session_send_with_id( + omnisocket_session_t *session, + const char *to, + const void *data, + size_t data_len, + uint64_t *out_message_id +) { + kcp_client_t *client; + int rc; + + if (session == NULL || to == NULL || (data == NULL && data_len > 0)) { + errno = EINVAL; + return -1; + } + + if (omnisocket_session_begin_client_op(session, &client) != 0) { + return -1; + } + rc = kcp_client_send_binary_with_id(client, to, data, data_len, out_message_id); + 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; + } + omnisocket_session_sync_client_state_locked(session, client); + 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; +} + +int omnisocket_session_recv(omnisocket_session_t *session, message_t *out_msg, int timeout_ms) { + kcp_client_t *client; + int rc; + + if (session == NULL || out_msg == NULL) { + errno = EINVAL; + return -1; + } + + if (omnisocket_session_begin_client_op(session, &client) != 0) { + return -1; + } + rc = kcp_client_receive_timed(client, out_msg, timeout_ms); + pthread_mutex_lock(&session->mutex); + if (rc == 0) { + session->stats.recv_calls += 1; + session->stats.recv_bytes += (uint64_t) out_msg->body_len; + } else if (rc == 1) { + session->stats.recv_timeouts += 1; + } else { + session->stats.recv_errors += 1; + } + omnisocket_session_sync_client_state_locked(session, client); + 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; +} + +int omnisocket_session_recv_into( + omnisocket_session_t *session, + void *buffer, + size_t buffer_len, + kcp_client_recv_meta_t *out_meta, + int timeout_ms +) { + kcp_client_t *client; + int rc; + + if (session == NULL || out_meta == NULL || (buffer == NULL && buffer_len > 0)) { + errno = EINVAL; + return -1; + } + + if (omnisocket_session_begin_client_op(session, &client) != 0) { + return -1; + } + rc = kcp_client_receive_binary_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; + } + omnisocket_session_sync_client_state_locked(session, client); + 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_session_stats_snapshot(omnisocket_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); +} + +void omnisocket_session_kcp_stats_snapshot(omnisocket_session_t *session, omnisocket_session_kcp_stats_t *out_stats) { + kcp_runtime_stats_t runtime_stats; + + 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->min_srtt_ms = runtime_stats.min_srtt_ms; + out_stats->srttvar_ms = runtime_stats.srttvar_ms; + out_stats->last_feedback_age_ms = runtime_stats.last_feedback_age_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; + } + + 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); + while (session->closing) { + pthread_cond_wait(&session->idle_cond, &session->mutex); + } + 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; + session->stats.registered = 1; + session->stats.last_server_error[0] = '\0'; + 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; + session->stats.registered = 0; + pthread_mutex_unlock(&session->mutex); + + if (client != NULL) { + 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; + } + + if (omnisocket_udp_session_begin_client_op(session, &client) != 0) { + 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; + } + if (session->closing && session->active_ops == 0) { + pthread_cond_broadcast(&session->idle_cond); + } + pthread_mutex_unlock(&session->mutex); + return rc; +} + +int omnisocket_udp_session_recv(omnisocket_udp_session_t *session, message_t *out_msg, int timeout_ms) { + udp_client_t *client; + int rc; + + if (session == NULL || out_msg == NULL) { + errno = EINVAL; + return -1; + } + + if (omnisocket_udp_session_begin_client_op(session, &client) != 0) { + return -1; + } + rc = udp_client_receive_timed(client, out_msg, timeout_ms); + pthread_mutex_lock(&session->mutex); + if (rc == 0) { + session->stats.recv_calls += 1; + session->stats.recv_bytes += (uint64_t) out_msg->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; +} + +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 +) { + 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); +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/python/omnisocket/omnisocket_client.h b/robot/ros2/OmniSocketGo_robot_ros/python/omnisocket/omnisocket_client.h new file mode 100644 index 0000000..b5e0db8 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/python/omnisocket/omnisocket_client.h @@ -0,0 +1,118 @@ +#ifndef OMNISOCKET_PY_CLIENT_H +#define OMNISOCKET_PY_CLIENT_H + +#include "peer_kcp_client.h" +#include "peer_udp_client.h" + +typedef struct omnisocket_session_stats { + uint64_t send_calls; + uint64_t send_bytes; + uint64_t send_errors; + uint64_t recv_calls; + uint64_t recv_bytes; + uint64_t recv_timeouts; + uint64_t recv_errors; + int connected; + int registered; + char last_server_error[256]; +} omnisocket_session_stats_t; + +typedef struct omnisocket_session_kcp_stats { + int connected; + uint32_t conv; + uint32_t rto_ms; + int32_t srtt_ms; + int32_t min_srtt_ms; + int32_t srttvar_ms; + uint32_t last_feedback_age_ms; + uint32_t snd_wnd; + uint32_t rmt_wnd; + uint32_t inflight; + uint32_t window_limit; + double window_pressure_pct; + uint32_t snd_queue; + uint32_t rcv_queue; + uint32_t snd_buffer; + uint64_t out_segs_total; + uint64_t retrans_total; + uint64_t fast_retrans_total; + uint64_t lost_total; + uint64_t repeat_total; + uint32_t xmit_total; +} omnisocket_session_kcp_stats_t; + +typedef struct omnisocket_session { + pthread_mutex_t mutex; + pthread_cond_t idle_cond; + kcp_client_t *client; + size_t active_ops; + int closing; + 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); + +int omnisocket_session_connect( + omnisocket_session_t *session, + const char *server_addr, + const char *relay_via, + const char *peer_id, + const char *bind_ip, + const char *bind_device, + const kcp_conn_options_t *options, + int stats_interval_ms +); +int omnisocket_session_close(omnisocket_session_t *session); +int omnisocket_session_send(omnisocket_session_t *session, const char *to, const void *data, size_t data_len); +int omnisocket_session_send_with_id( + omnisocket_session_t *session, + const char *to, + const void *data, + size_t data_len, + uint64_t *out_message_id +); +int omnisocket_session_recv(omnisocket_session_t *session, message_t *out_msg, int timeout_ms); +int omnisocket_session_recv_into( + omnisocket_session_t *session, + void *buffer, + size_t buffer_len, + kcp_client_recv_meta_t *out_meta, + int timeout_ms +); +void omnisocket_session_stats_snapshot(omnisocket_session_t *session, omnisocket_session_stats_t *out_stats); +void omnisocket_session_kcp_stats_snapshot(omnisocket_session_t *session, omnisocket_session_kcp_stats_t *out_stats); + +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 diff --git a/robot/ros2/OmniSocketGo_robot_ros/python/setup.py b/robot/ros2/OmniSocketGo_robot_ros/python/setup.py new file mode 100644 index 0000000..f302f32 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/python/setup.py @@ -0,0 +1,58 @@ +from pathlib import Path +import sys + +from setuptools import Extension, setup + + +ROOT = Path(__file__).resolve().parent.parent +PY_ROOT = Path(__file__).resolve().parent + +if sys.platform != "linux": + raise RuntimeError("omnisocket Python extension can only be built on Linux") + + +COMMON_SOURCES = [ + ROOT / "src" / "omni_common.c", + ROOT / "src" / "protocol.c", + ROOT / "src" / "latencylog.c", + ROOT / "src" / "tx_timestamp_debug.c", + ROOT / "src" / "kcp_packet_debug.c", + ROOT / "src" / "kcp_session_stats.c", + ROOT / "src" / "linux_timestamping.c", + ROOT / "src" / "interactive.c", + ROOT / "src" / "transport_udp.c", + ROOT / "src" / "transport_kcp.c", + ROOT / "src" / "server_udp_relay.c", + ROOT / "src" / "server_udp_hub.c", + ROOT / "src" / "server_kcp_hub.c", + ROOT / "src" / "peer_udp_client.c", + ROOT / "src" / "peer_kcp_client.c", + ROOT / "third_party" / "cjson" / "cJSON.c", + ROOT / "third_party" / "kcp" / "ikcp.c", +] + + +setup( + name="omnisocket", + version="0.1.0", + packages=["omnisocket"], + ext_modules=[ + Extension( + "omnisocket._omnisocket", + sources=[ + str(PY_ROOT / "omnisocket" / "_omnisocket.c"), + str(PY_ROOT / "omnisocket" / "omnisocket_client.c"), + *[str(path) for path in COMMON_SOURCES], + ], + include_dirs=[ + str(ROOT / "include"), + str(ROOT / "third_party" / "cjson"), + str(ROOT / "third_party" / "kcp"), + str(PY_ROOT / "omnisocket"), + ], + define_macros=[("_GNU_SOURCE", None)], + extra_compile_args=["-std=c11", "-O2", "-pthread"], + extra_link_args=["-pthread"], + ) + ], +) diff --git a/robot/ros2/OmniSocketGo_robot_ros/python/tests/test_sessions.py b/robot/ros2/OmniSocketGo_robot_ros/python/tests/test_sessions.py new file mode 100644 index 0000000..e9785dc --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/python/tests/test_sessions.py @@ -0,0 +1,318 @@ +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) + + +@contextmanager +def _run_relay(listen_addr: str, remote_addr: str): + binary = ROOT / 'bin' / 'kcpserver' + if not binary.exists(): + pytest.skip(f'{binary} is not built') + + process = subprocess.Popen( + [str(binary), '-mode', 'relay', '-listen', listen_addr, '-relay-remote', remote_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, relay_via: 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) + if relay_via: + kwargs['relay_via'] = relay_via + 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['registered'] == 1 + assert receiver_stats['registered'] == 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_kcp_duplicate_peer_new_instance_wins() -> None: + port = _reserve_port() + listen_addr = f'127.0.0.1:{port}' + shared_peer_id = 'pytest-kcp-shared-peer' + sender_id = 'pytest-kcp-unique-sender' + + with _run_server('kcpserver', listen_addr): + original = _connect_with_retry(Session, transport='kcp', server_addr=listen_addr, peer_id=shared_peer_id) + sender = _connect_with_retry(Session, transport='kcp', server_addr=listen_addr, peer_id=sender_id) + replacement = None + + try: + replacement = _connect_with_retry(Session, transport='kcp', server_addr=listen_addr, peer_id=shared_peer_id) + replacement_stats = replacement.stats() + assert replacement_stats['connected'] == 1 + assert replacement_stats['registered'] == 1 + + with pytest.raises(OSError): + original.recv(timeout_ms=1000) + + payload = b'registered-replacement' + sender.send(to=shared_peer_id, data=payload) + from_peer, msg_type, recv_payload = replacement.recv(timeout_ms=1000) + assert from_peer == sender_id + assert msg_type == MSG_TYPE_BINARY + assert recv_payload == payload + finally: + original.close() + sender.close() + if replacement is not None: + replacement.close() + + +def test_kcp_idle_video_peers_survive_without_receive_loop() -> None: + port = _reserve_port() + listen_addr = f'127.0.0.1:{port}' + sender_id = 'peer-b-video' + receiver_id = 'pytest-kcp-video-idle-receiver' + + with _run_server('kcpserver', listen_addr): + sender = _connect_with_retry(Session, transport='kcp', server_addr=listen_addr, peer_id=sender_id) + receiver = _connect_with_retry(Session, transport='kcp', server_addr=listen_addr, peer_id=receiver_id) + + try: + time.sleep(5.0) + + payload = b'idle-video-session-still-alive' + 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 + finally: + sender.close() + receiver.close() + + +def test_kcp_peer_a_video_stale_receiver_is_evicted() -> None: + port = _reserve_port() + listen_addr = f'127.0.0.1:{port}' + receiver_id = 'peer-a-video' + + with _run_server('kcpserver', listen_addr): + receiver = _connect_with_retry(Session, transport='kcp', server_addr=listen_addr, peer_id=receiver_id) + + try: + time.sleep(5.0) + with pytest.raises(OSError): + receiver.recv(timeout_ms=1000) + finally: + receiver.close() + + +def test_kcp_relay_routes_multiple_sessions_by_conv() -> None: + hub_port = _reserve_port() + relay_port = _reserve_port() + hub_addr = f'127.0.0.1:{hub_port}' + relay_addr = f'127.0.0.1:{relay_port}' + + with _run_server('kcpserver', hub_addr): + with _run_relay(relay_addr, hub_addr): + sender = _connect_with_retry(Session, transport='kcp', server_addr=hub_addr, peer_id='pytest-relay-sender', relay_via=relay_addr) + receiver = _connect_with_retry(Session, transport='kcp', server_addr=hub_addr, peer_id='pytest-relay-receiver', relay_via=relay_addr) + chatter = _connect_with_retry(Session, transport='kcp', server_addr=hub_addr, peer_id='pytest-relay-chatter', relay_via=relay_addr) + + try: + chatter.send(to='pytest-relay-sender', data=b'chatter-primes-last-client') + from_peer, msg_type, recv_payload = sender.recv(timeout_ms=1000) + assert from_peer == 'pytest-relay-chatter' + assert msg_type == MSG_TYPE_BINARY + assert recv_payload == b'chatter-primes-last-client' + + sender.send(to='pytest-relay-receiver', data=b'relay-video-frame') + from_peer, msg_type, recv_payload = receiver.recv(timeout_ms=1000) + assert from_peer == 'pytest-relay-sender' + assert msg_type == MSG_TYPE_BINARY + assert recv_payload == b'relay-video-frame' + finally: + sender.close() + receiver.close() + chatter.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) diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-c/Makefile b/robot/ros2/OmniSocketGo_robot_ros/ros-control-c/Makefile new file mode 100644 index 0000000..26b692e --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/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) diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-c/README.md b/robot/ros2/OmniSocketGo_robot_ros/ros-control-c/README.md new file mode 100644 index 0000000..42603d0 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/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 ` 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 +``` diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-c/Robot Remote Control via UDP - Implementation Plan.md b/robot/ros2/OmniSocketGo_robot_ros/ros-control-c/Robot Remote Control via UDP - Implementation Plan.md new file mode 100644 index 0000000..350852b --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/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 , -p , -l , -a + + --- + 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 , -p , -d , -l , -a , -z + + --- + 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 \ No newline at end of file diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-c/common/protocol.h b/robot/ros2/OmniSocketGo_robot_ros/ros-control-c/common/protocol.h new file mode 100644 index 0000000..91adf5b --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/ros-control-c/common/protocol.h @@ -0,0 +1,26 @@ +#ifndef PROTOCOL_H +#define PROTOCOL_H + +#include + +#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 */ diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-c/common/teleop_transport.c b/robot/ros2/OmniSocketGo_robot_ros/ros-control-c/common/teleop_transport.c new file mode 100644 index 0000000..c5d875d --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/ros-control-c/common/teleop_transport.c @@ -0,0 +1,300 @@ +#include "teleop_transport.h" + +#include +#include +#include +#include +#include +#include + +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; + } +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-c/common/teleop_transport.h b/robot/ros2/OmniSocketGo_robot_ros/ros-control-c/common/teleop_transport.h new file mode 100644 index 0000000..6061aff --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/ros-control-c/common/teleop_transport.h @@ -0,0 +1,59 @@ +#ifndef TELEOP_TRANSPORT_H +#define TELEOP_TRANSPORT_H + +#include +#include + +#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 */ diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-c/remote/gamepad_controller.c b/robot/ros2/OmniSocketGo_robot_ros/ros-control-c/remote/gamepad_controller.c new file mode 100644 index 0000000..db96133 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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; +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-c/remote/keyboard_controller.c b/robot/ros2/OmniSocketGo_robot_ros/ros-control-c/remote/keyboard_controller.c new file mode 100644 index 0000000..239dad8 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/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 +#include +#include +#include +#include +#include +#include +#include +#include + +#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; +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-c/robot/udp_ros_bridge.py b/robot/ros2/OmniSocketGo_robot_ros/ros-control-c/robot/udp_ros_bridge.py new file mode 100644 index 0000000..1b435d8 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/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 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() diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/README.md b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/README.md new file mode 100644 index 0000000..be5c392 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/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 消息、错误长度消息都会被丢弃并记录日志 diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/ROS2 Teleop over UDP.md b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/ROS2 Teleop over UDP.md new file mode 100644 index 0000000..976d719 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/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 模式有效 diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/config/xbox_twist_joy.yaml b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/config/xbox_twist_joy.yaml new file mode 100644 index 0000000..c48735e --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/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 diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/launch/keyboard_sender.launch.py b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/launch/keyboard_sender.launch.py new file mode 100644 index 0000000..49d7667 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/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), + }], + ), + ]) diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/launch/robot_udp_receiver.launch.py b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/launch/robot_udp_receiver.launch.py new file mode 100644 index 0000000..4a9d1e5 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/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), + }], + ), + ]) diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/launch/xbox_to_udp.launch.py b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/launch/xbox_to_udp.launch.py new file mode 100644 index 0000000..d9038d0 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/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), + }], + ), + ]) diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/package.xml b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/package.xml new file mode 100644 index 0000000..fc70b79 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/package.xml @@ -0,0 +1,25 @@ + + + udp_teleop_bridge + 0.1.0 + ROS 2 OmniSocket UDP/KCP bridge for teleop TwistStamped commands. + + Codex + MIT + + ament_python + + ament_index_python + geometry_msgs + joy + launch + launch_ros + rclpy + rosidl_runtime_py + teleop_twist_joy + teleop_twist_keyboard + + + ament_python + + diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/resource/udp_teleop_bridge b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/resource/udp_teleop_bridge new file mode 100644 index 0000000..9cc185f --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/resource/udp_teleop_bridge @@ -0,0 +1 @@ +udp_teleop_bridge diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/setup.cfg b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/setup.cfg new file mode 100644 index 0000000..8f79a94 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/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 diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/setup.py b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/setup.py new file mode 100644 index 0000000..ae42c8d --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/setup.py @@ -0,0 +1,34 @@ +from setuptools import find_packages, setup + + +package_name = 'udp_teleop_bridge' + + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', [f'resource/{package_name}']), + (f'share/{package_name}', ['package.xml']), + (f'share/{package_name}/launch', [ + 'launch/keyboard_sender.launch.py', + 'launch/robot_udp_receiver.launch.py', + 'launch/xbox_to_udp.launch.py', + ]), + (f'share/{package_name}/config', ['config/xbox_twist_joy.yaml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Codex', + maintainer_email='codex@example.com', + description='ROS 2 OmniSocket UDP/KCP bridge for teleop TwistStamped commands.', + license='MIT', + 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', + ], + }, +) diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/test/test_protocol.py b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/test/test_protocol.py new file mode 100644 index 0000000..87cfbe1 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/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') diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/__init__.py b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/__init__.py new file mode 100644 index 0000000..094feaf --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/__init__.py @@ -0,0 +1 @@ +"""OmniSocket teleop bridge package.""" diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/cmd_vel_udp_sender.py b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/cmd_vel_udp_sender.py new file mode 100644 index 0000000..34a5ce1 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/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() diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/omni_transport.py b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/omni_transport.py new file mode 100644 index 0000000..3978ac3 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/omni_transport.py @@ -0,0 +1,101 @@ +"""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 send_with_id(self, *, to: str, data: bytes) -> int: + if not hasattr(self._session, 'send_with_id'): + self._session.send(to=to, data=data) + raise RuntimeError('send_with_id is not available on this omnisocket build') + return int(self._session.send_with_id(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', +] diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/protocol.py b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/protocol.py new file mode 100644 index 0000000..44f4641 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/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)) diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/topic_status_reader.py b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/topic_status_reader.py new file mode 100644 index 0000000..8d0f8d1 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/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() diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/udp_cmd_vel_receiver.py b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/udp_cmd_vel_receiver.py new file mode 100644 index 0000000..8eac4fb --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/udp_cmd_vel_receiver.py @@ -0,0 +1,480 @@ +"""ROS 2 node that receives OmniSocket teleop packets and republishes TwistStamped.""" + +from __future__ import annotations + +import json +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 = self._create_transport() + + 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._runtime_dir = os.getenv('BLITZ_RUNTIME_DIR', '/run/blitz-robot').strip() or '/run/blitz-robot' + self._status_path = os.path.join(self._runtime_dir, 'ros-receiver.status.json') + self._transport_reconnect_count = 0 + self._recv_thread_heartbeat_epoch_ms = self._now_epoch_ms() + self._runtime_last_error = '' + + self.create_timer(1.0 / self._publish_rate_hz, self._publish_tick) + self.create_timer(1.0, self._write_status_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 _close_unix_socket(self) -> None: + if self._unix_socket is not None: + try: + self._unix_socket.close() + except OSError: + pass + self._unix_socket = None + + def _create_transport(self): + from .omni_transport import OmniTransport + + return OmniTransport( + transport=self._transport_name, + server_addr=self._server_addr, + relay_via=self._relay_via, + peer_id=self._peer_id, + ) + + def _reconnect_transport(self) -> bool: + while not self._closing.is_set() and rclpy.ok(): + current_transport = self._transport + if current_transport is not None: + try: + current_transport.close() + except OSError: + pass + try: + self._transport = self._create_transport() + self._transport_reconnect_count += 1 + self._set_runtime_last_error('') + if self._should_log('transport_reconnected', 1.0): + self.get_logger().info( + 'Reconnected OmniSocket transport %s://%s as %s' + % (self._transport_name, self._server_addr, self._peer_id) + ) + return True + except OSError as exc: + self._transport = None + self._set_runtime_last_error(str(exc)) + if self._should_log('transport_reconnect_error', 2.0): + self.get_logger().error(f'Failed to reconnect OmniSocket transport: {exc}') + time.sleep(0.5) + return False + + def _rebind_unix_socket(self) -> bool: + while not self._closing.is_set() and rclpy.ok(): + self._close_unix_socket() + try: + self._setup_unix_socket() + self._transport_reconnect_count += 1 + self._set_runtime_last_error('') + if self._should_log('unix_rebound', 1.0): + self.get_logger().info(f'Rebound unix datagram socket at {self._local_socket_path}') + return True + except OSError as exc: + self._set_runtime_last_error(str(exc)) + if self._should_log('unix_rebind_error', 2.0): + self.get_logger().error(f'Failed to rebind unix datagram socket: {exc}') + time.sleep(0.5) + return False + + 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 _now_epoch_ms(self) -> int: + return time.time_ns() // 1_000_000 + + def _update_recv_heartbeat(self) -> None: + with self._lock: + self._recv_thread_heartbeat_epoch_ms = self._now_epoch_ms() + + def _last_packet_age_ms(self) -> int | None: + with self._lock: + last_packet_monotonic = self._last_packet_monotonic + if last_packet_monotonic is None: + return None + return max(0, int((time.monotonic() - last_packet_monotonic) * 1000.0)) + + def _socket_bound(self) -> bool: + if self._transport_name == 'unix_dgram': + return self._unix_socket is not None and os.path.exists(self._local_socket_path) + return self._transport is not None + + def _set_runtime_last_error(self, message: str) -> None: + self._runtime_last_error = message + + def _status_payload(self) -> dict[str, object]: + with self._lock: + recv_thread_heartbeat_epoch_ms = self._recv_thread_heartbeat_epoch_ms + return { + 'updated_at_epoch_ms': self._now_epoch_ms(), + 'pid': os.getpid(), + 'recv_thread_heartbeat_epoch_ms': recv_thread_heartbeat_epoch_ms, + 'transport': self._transport_name, + 'local_socket_path': self._local_socket_path, + 'socket_bound': self._socket_bound(), + 'transport_reconnect_count': self._transport_reconnect_count, + 'last_packet_age_ms': self._last_packet_age_ms(), + 'last_error': self._runtime_last_error, + } + + def _write_status_tick(self) -> None: + payload = self._status_payload() + if self._transport_name == 'unix_dgram': + if self._unix_socket is None: + payload['last_error'] = self._runtime_last_error or 'unix datagram socket is not bound' + else: + if self._transport is None: + payload['last_error'] = self._runtime_last_error or 'OmniSocket transport is not connected' + try: + os.makedirs(self._runtime_dir, exist_ok=True) + temp_path = f'{self._status_path}.tmp.{os.getpid()}' + with open(temp_path, 'w', encoding='utf-8') as handle: + json.dump(payload, handle, ensure_ascii=True, separators=(',', ':')) + os.replace(temp_path, self._status_path) + except OSError as exc: + if self._should_log('status_write_error', 5.0): + self.get_logger().warning(f'Failed to write receiver status file: {exc}') + + 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(): + self._update_recv_heartbeat() + try: + assert self._transport is not None + meta = self._transport.recv_into(buffer=self._recv_buffer, timeout_ms=100) + except BufferError as exc: + self._set_runtime_last_error(str(exc)) + if self._should_log('buffer_error', 2.0): + self.get_logger().warning(f'Dropped oversized OmniSocket frame: {exc}') + continue + except OSError as exc: + self._set_runtime_last_error(str(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}') + if not self._reconnect_transport(): + return + continue + + self._update_recv_heartbeat() + if meta is None: + continue + self._set_runtime_last_error('') + + 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._set_runtime_last_error(f'server error message from {from_peer}') + self._handle_error_message(from_peer, body_len) + continue + + if self._expected_sender and from_peer != self._expected_sender: + self._set_runtime_last_error(f'unexpected sender {from_peer}') + 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: + self._set_runtime_last_error(f'unexpected message type {msg_type}') + 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: + self._set_runtime_last_error(f'invalid payload size {body_len}') + 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: + self._set_runtime_last_error(str(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() + self._set_runtime_last_error('') + + def _recv_loop_unix_dgram(self) -> None: + assert self._unix_socket is not None + + while not self._closing.is_set() and rclpy.ok(): + self._update_recv_heartbeat() + try: + payload = self._unix_socket.recv(DEFAULT_RECV_BUFFER_BYTES) + except socket.timeout: + if not os.path.exists(self._local_socket_path): + self._set_runtime_last_error('unix datagram socket path disappeared') + if self._should_log('unix_socket_missing', 2.0): + self.get_logger().warning( + f'Unix datagram socket path disappeared, rebinding {self._local_socket_path}' + ) + if not self._rebind_unix_socket(): + return + continue + except OSError as exc: + self._set_runtime_last_error(str(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}') + if not self._rebind_unix_socket(): + return + continue + + self._update_recv_heartbeat() + if len(payload) != PACKET_SIZE: + self._set_runtime_last_error(f'invalid unix datagram payload size {len(payload)}') + 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: + self._set_runtime_last_error(str(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() + self._set_runtime_last_error('') + + 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._close_unix_socket() + except OSError as exc: + if self._should_log('unix_close_error', 2.0): + self.get_logger().warning(f'Closing unix socket failed: {exc}') + 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() diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros2/README.md b/robot/ros2/OmniSocketGo_robot_ros/ros2/README.md new file mode 100644 index 0000000..0c9c0e1 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/ros2/README.md @@ -0,0 +1,23 @@ +# omnisocket_camera_bridge + +这是 OmniSocketGo robot 端的 ROS 2 RGB 桥接包。它订阅头部和腰部的 `sensor_msgs/msg/Image`，把最新帧写入固定大小的共享内存文件，供 C 视频管线读取。 + +```bash +cd ~/OmniSocketGo_robot_ros/ros2 +source /opt/ros/jazzy/setup.bash +colcon build +source install/setup.bash +ros2 run omnisocket_camera_bridge omnisocket_ros_camera_bridge +``` + +参数： + +```text +head_topic /ob_camera_head/color/image_raw +waist_topic /ob_camera_waist/color/image_raw +head_shm /dev/shm/omnisocket-rgb-head +waist_shm /dev/shm/omnisocket-rgb-waist +max_frame_bytes 8294400 +``` + +支持 `rgb8`、`bgr8`、`rgba8`、`bgra8` 和 `mono8`。带行填充的 ROS 图像会被压缩为连续行后再写入共享内存；C 端据消息编码转换为 FFmpeg 像素格式。该包不访问 V4L2 设备。 diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros2/omnisocket_camera_bridge/omnisocket_camera_bridge/__init__.py b/robot/ros2/OmniSocketGo_robot_ros/ros2/omnisocket_camera_bridge/omnisocket_camera_bridge/__init__.py new file mode 100644 index 0000000..af41fca --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/ros2/omnisocket_camera_bridge/omnisocket_camera_bridge/__init__.py @@ -0,0 +1 @@ +"""ROS2 RGB bridge used by OmniSocketGo_robot_ros.""" diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros2/omnisocket_camera_bridge/omnisocket_camera_bridge/ros_camera_bridge.py b/robot/ros2/OmniSocketGo_robot_ros/ros2/omnisocket_camera_bridge/omnisocket_camera_bridge/ros_camera_bridge.py new file mode 100644 index 0000000..7795669 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/ros2/omnisocket_camera_bridge/omnisocket_camera_bridge/ros_camera_bridge.py @@ -0,0 +1,217 @@ +#!/usr/bin/env python3 +"""Bridge ROS2 RGB images into the shared-memory input used by b_side_omnid. + +The Orbbec ROS2 driver remains the only process that opens the physical camera. +This node only subscribes to sensor_msgs/Image and publishes the latest frame for +the C transport daemon. A bounded latest-frame slot is intentional: old video +frames are discarded instead of increasing end-to-end latency. +""" + +import mmap +import os +import struct +import threading +from typing import Optional + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy +from sensor_msgs.msg import Image + + +MAGIC = 0x52494D47 # RIMG +VERSION = 1 +HEADER_BYTES = 64 +HEADER_FORMAT = " bool: + info = _encoding_info(msg.encoding) + if info is None: + return False + encoding, bytes_per_pixel = info + + width = int(msg.width) + height = int(msg.height) + step = int(msg.step) + row_bytes = width * bytes_per_pixel + if width <= 0 or height <= 0 or step < row_bytes: + return False + + data = bytes(msg.data) + required = step * (height - 1) + row_bytes + if len(data) < required: + return False + + # ROS Image permits row padding. The C side consumes packed rows. + if step == row_bytes: + payload = data[: row_bytes * height] + else: + payload = b"".join( + data[row * step : row * step + row_bytes] for row in range(height) + ) + if len(payload) > self.max_frame_bytes: + return False + + timestamp_ns = int(msg.header.stamp.sec) * 1_000_000_000 + int( + msg.header.stamp.nanosec + ) + with self.lock: + odd_sequence = self.sequence + 1 + if odd_sequence % 2 == 0: + odd_sequence += 1 + # Odd sequence means the slot is being written. + struct.pack_into( + HEADER_FORMAT, + self.mapping, + 0, + odd_sequence, + MAGIC, + VERSION, + width, + height, + row_bytes, + encoding, + len(payload), + 0, + timestamp_ns, + 0, + 0, + ) + self.mapping[HEADER_BYTES : HEADER_BYTES + len(payload)] = payload + even_sequence = odd_sequence + 1 + struct.pack_into(" {head_shm}, " + f"waist={waist_topic} -> {waist_shm}, max={max_frame_bytes} bytes" + ) + + def _write(self, slot: SharedImageSlot, msg: Image, label: str): + if _encoding_info(msg.encoding) is None: + if msg.encoding not in self._unsupported: + self._unsupported.add(msg.encoding) + self.get_logger().error( + f"unsupported {label} image encoding: {msg.encoding}" + ) + return + if not slot.write(msg): + self.get_logger().warning( + f"discarded invalid or oversized {label} image frame" + ) + + def _head_callback(self, msg: Image): + self._write(self.head_slot, msg, "head") + + def _waist_callback(self, msg: Image): + self._write(self.waist_slot, msg, "waist") + + def destroy_node(self): + self.head_slot.close() + self.waist_slot.close() + super().destroy_node() + + +def main(args: Optional[list] = None): + rclpy.init(args=args) + node = None + try: + node = RosCameraBridge() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + if node is not None: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros2/omnisocket_camera_bridge/package.xml b/robot/ros2/OmniSocketGo_robot_ros/ros2/omnisocket_camera_bridge/package.xml new file mode 100644 index 0000000..3711734 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/ros2/omnisocket_camera_bridge/package.xml @@ -0,0 +1,16 @@ + + + omnisocket_camera_bridge + 0.1.0 + ROS2 RGB image bridge for OmniSocketGo_robot_ros. + OmniSocketGo maintainers + Proprietary + + ament_python + rclpy + sensor_msgs + + + ament_python + + diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros2/omnisocket_camera_bridge/resource/omnisocket_camera_bridge b/robot/ros2/OmniSocketGo_robot_ros/ros2/omnisocket_camera_bridge/resource/omnisocket_camera_bridge new file mode 100644 index 0000000..e69de29 diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros2/omnisocket_camera_bridge/setup.cfg b/robot/ros2/OmniSocketGo_robot_ros/ros2/omnisocket_camera_bridge/setup.cfg new file mode 100644 index 0000000..65d2686 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/ros2/omnisocket_camera_bridge/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/omnisocket_camera_bridge +[install] +install_scripts=$base/lib/omnisocket_camera_bridge diff --git a/robot/ros2/OmniSocketGo_robot_ros/ros2/omnisocket_camera_bridge/setup.py b/robot/ros2/OmniSocketGo_robot_ros/ros2/omnisocket_camera_bridge/setup.py new file mode 100644 index 0000000..d471ecc --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/ros2/omnisocket_camera_bridge/setup.py @@ -0,0 +1,20 @@ +from setuptools import find_packages, setup + +package_name = "omnisocket_camera_bridge" + +setup( + name=package_name, + version="0.1.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + entry_points={ + "console_scripts": [ + "omnisocket_ros_camera_bridge = omnisocket_camera_bridge.ros_camera_bridge:main", + ], + }, +) diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/BACDauto_test.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/BACDauto_test.sh new file mode 100644 index 0000000..70f7300 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/BACDauto_test.sh @@ -0,0 +1,296 @@ +#!/bin/bash + +LOCAL_REPO_DIR="/home/limingjie/LMJ_Work/RobotCompetition/OmniSocketGo" +KCP_PEER_BIN="./bin/kcppeer" +OUTPUT_ROOT="/home/limingjie/LMJ_Work/RobotCompetition/KCPData/BDAClogs" +PEERB_POLL_INTERVAL_SEC=5 +PEERB_MAX_POLLS=180 +PEER_A_EXIT_WAIT_SEC=5 + +require_local_binary() { + if [ ! -x "$1" ]; then + echo "ERROR: 缺少可执行文件 $1" + exit 1 + fi +} + +cleanup_remote_peerb() { + ssh omni-peer bash -s <<'EOF' +pids=$(ps -eo pid=,args= | awk '/[b]in\/kcppeer -id peer-b/ {print $1}') +if [ -n "$pids" ]; then + kill $pids 2>/dev/null || true +fi +pids=$(ps -eo pid=,args= | awk '/\/tmp\/peerb_batch\.sh/ {print $1}') +if [ -n "$pids" ]; then + kill $pids 2>/dev/null || true +fi +rm -f /tmp/peerb_batch_done /tmp/peerb_batch.sh /tmp/peerb_commands +EOF +} + +echo "=== 开始自动化测试 ===" + +cd "$LOCAL_REPO_DIR" +require_local_binary "$KCP_PEER_BIN" + +echo ">>> 0. 清理上次残留进程..." +pkill -f 'bin/kcppeer -id peer-a' 2>/dev/null || true +cleanup_remote_peerb || exit 1 + +rm -rf logs +rm -rf inbox/a +mkdir -p logs inbox/a + +# 1. 清理残留 & 启动 Server D 和 Relay C +echo ">>> 1. 启动 Server D 和 Relay C..." + +ssh bj-txy bash -s <<'EOF' +pkill -f kcpserver 2>/dev/null || true +pkill -f 'bin/kcpserver' 2>/dev/null || true +sleep 1 +cd /home/ubuntu/OmniSocketGo +rm -rf logs +mkdir -p logs +if [ ! -x ./bin/kcpserver ]; then + echo "ERROR: 缺少 ./bin/kcpserver" + 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 || true +pkill -f 'bin/kcpserver' 2>/dev/null || true +sleep 1 +cd /home/ubuntu/OmniSocketGo +rm -rf logs +mkdir -p logs +if [ ! -x ./bin/kcpserver ]; then + echo "ERROR: 缺少 ./bin/kcpserver" + exit 1 +fi +setsid ./bin/kcpserver -mode=relay -listen 0.0.0.0:10909 -relay-remote 172.21.32.15:10909 > relay_console.log 2>&1 /dev/null; then + echo " Server D 就绪 (${i}s)" + break + fi + if [ "$i" -eq 60 ]; then + echo " ERROR: Server D 60s 内未就绪，退出" + exit 1 + fi + sleep 1 +done + +# 等待 relay C 端口就绪 +echo " 等待 Relay C 端口就绪..." +for i in $(seq 1 60); do + if ssh sz-txy "ss -ulnp | grep -q 10909" 2>/dev/null; then + echo " Relay C 就绪 (${i}s)" + break + fi + if [ "$i" -eq 60 ]; then + echo " ERROR: Relay C 60s 内未就绪，退出" + exit 1 + fi + sleep 1 +done + +# 2. 启动本地 Peer-A +echo ">>> 2. 启动本地 Peer-A..." +PEER_A_CMD_FIFO="/tmp/peera_commands_$$" +rm -f "$PEER_A_CMD_FIFO" +mkfifo "$PEER_A_CMD_FIFO" +nohup "$KCP_PEER_BIN" \ + -id peer-a \ + -server 172.21.32.15:10909 \ + -relay-via 106.55.173.235:10909 \ + -inbox-dir inbox/a \ + -latency-log logs/a-latency.jsonl \ + -kcp-ts-debug-log logs/a-kcp-ts.jsonl \ + -kcp-session-stats-log logs/a-kcp-stats.jsonl \ + < "$PEER_A_CMD_FIFO" > logs/peera_console.log 2>&1 & +PEER_A_PID=$! +exec 4>"$PEER_A_CMD_FIFO" + +# 等待 peer-a 注册成功 +echo " 等待 Peer-A 注册..." +for i in $(seq 1 30); do + if grep -Eq "opened KCP session as peer-a|connected to .* as peer-a( \\(KCP\\))?" logs/peera_console.log 2>/dev/null; then + echo " Peer-A 就绪 (${i}s)" + break + fi + if [ "$i" -eq 30 ]; then + echo " WARNING: Peer-A 30s 内未就绪" + fi + sleep 1 +done + +# 3. 在远端后台启动 peer-b 整个发送流程，不依赖长 SSH 连接 +echo ">>> 3. 启动远端 Peer-B 并执行 50 轮打流测试..." +ssh omni-peer "cd /home/boll/LMJWork/OmniSocketGo && rm -rf logs inbox/b && mkdir -p logs inbox/b" + +PEERB_DONE_FLAG="/tmp/peerb_batch_done" +PEERB_BATCH_SCRIPT="/tmp/peerb_batch.sh" + +# 把整个发送脚本写到远端，setsid 后台执行 +ssh omni-peer bash -s <<'DEPLOY_SCRIPT' +DONE_FLAG="/tmp/peerb_batch_done" +BATCH_SCRIPT="/tmp/peerb_batch.sh" +rm -f "$DONE_FLAG" + +cat > "$BATCH_SCRIPT" <<'INNER_EOF' +#!/bin/bash +cd /home/boll/LMJWork/OmniSocketGo + +CMD_FIFO=/tmp/peerb_commands +DONE_FLAG="/tmp/peerb_batch_done" +STATUS="error" +rm -f "$CMD_FIFO" "$DONE_FLAG" +mkfifo "$CMD_FIFO" + +finish() { + local status_to_write="$STATUS" + rm -f "$CMD_FIFO" + printf '%s\n' "$status_to_write" > "$DONE_FLAG" +} + +trap finish EXIT + +if [ ! -x ./bin/kcppeer ]; then + echo "ERROR: 缺少 ./bin/kcppeer" > logs/peerb_console.log + exit 1 +fi + +# 启动 peer-b +./bin/kcppeer \ + -id peer-b \ + -server 81.70.156.140:10909 \ + -inbox-dir inbox/b \ + -latency-log logs/b-latency.jsonl \ + -kcp-ts-debug-log logs/b-kcp-ts.jsonl \ + -kcp-session-stats-log logs/b-kcp-stats.jsonl \ + < "$CMD_FIFO" > logs/peerb_console.log 2>&1 & +PEER_B_PID=$! + +exec 3>"$CMD_FIFO" + +# 等 peer-b 就绪 +for i in $(seq 1 60); do + if grep -Eq "opened KCP session as peer-b|connected to .* as peer-b( \\(KCP\\))?" logs/peerb_console.log 2>/dev/null; then + break + fi + sleep 1 +done + +# 50 轮发送 +for i in $(seq 1 50); do + echo "file peer-a /home/boll/test30.bin" >&3 + sleep 1 + echo "file peer-a /home/boll/test5.bin" >&3 + sleep 1 +done + +sleep 5 +echo "quit" >&3 || true +exec 3>&- + +peer_b_exited=0 +for i in $(seq 1 15); do + if ! kill -0 $PEER_B_PID 2>/dev/null; then + peer_b_exited=1 + break + fi + sleep 1 +done + +if [ "$peer_b_exited" -eq 0 ]; then + kill $PEER_B_PID 2>/dev/null || true + sleep 1 +fi + +if kill -0 $PEER_B_PID 2>/dev/null; then + kill -9 $PEER_B_PID 2>/dev/null || true +fi + +wait $PEER_B_PID 2>/dev/null || true + +# 写完成标记 +STATUS="done" +INNER_EOF + +chmod +x "$BATCH_SCRIPT" +setsid bash "$BATCH_SCRIPT" /dev/null 2>&1 & +echo "peer-b batch launched in background" +DEPLOY_SCRIPT + +# 本地轮询等待远端完成（短 SSH 连接，不怕断开） +echo " 等待 peer-b 发送完成（预计 ~110 秒）..." +for i in $(seq 1 "$PEERB_MAX_POLLS"); do + PEERB_STATUS=$(ssh omni-peer "cat /tmp/peerb_batch_done 2>/dev/null || true") + if [ "$PEERB_STATUS" = "done" ]; then + ELAPSED_SEC=$(( (i - 1) * PEERB_POLL_INTERVAL_SEC )) + echo " peer-b 发送完成（约 ${ELAPSED_SEC}s）" + break + fi + if [ "$PEERB_STATUS" = "error" ]; then + echo " ERROR: peer-b 后台任务启动失败，请检查远端 logs/peerb_console.log" + exit 1 + fi + if [ "$i" -eq "$PEERB_MAX_POLLS" ]; then + echo " ERROR: peer-b $((PEERB_MAX_POLLS * PEERB_POLL_INTERVAL_SEC))s 内未完成" + exit 1 + fi + # 每 5 秒查一次，减少 SSH 连接频率 + sleep "$PEERB_POLL_INTERVAL_SEC" +done + +# 4. 清理 +echo ">>> 4. 清理所有进程..." +sleep 2 +echo "quit" >&4 || true +exec 4>&- +for i in $(seq 1 "$PEER_A_EXIT_WAIT_SEC"); do + if ! kill -0 "$PEER_A_PID" 2>/dev/null; then + break + fi + sleep 1 +done +if kill -0 "$PEER_A_PID" 2>/dev/null; then + kill "$PEER_A_PID" 2>/dev/null || true +fi +wait "$PEER_A_PID" 2>/dev/null || true +rm -f "$PEER_A_CMD_FIFO" +ssh bj-txy "pkill -f kcpserver || true; pkill -f 'bin/kcpserver' || true" +ssh sz-txy "pkill -f kcpserver || true; pkill -f 'bin/kcpserver' || true" + +# 获取当前时间戳，格式为 YYYYMMDD_HHMMSS +TIMESTAMP=$(date +"%Y%m%d_%H%M%S") +OUTPUT_DIR="$OUTPUT_ROOT/$TIMESTAMP" +# 5. 拉取数据 & 生成报告 +echo ">>> 5. 拉取数据并生成汇总报告..." +mkdir -p "$OUTPUT_DIR" +scp -o ServerAliveInterval=15 -P 10022 boll@175.178.116.187:/home/boll/LMJWork/OmniSocketGo/logs/b-latency.jsonl "$LOCAL_REPO_DIR/logs/b-latency.jsonl" || exit 1 + +(cd "$LOCAL_REPO_DIR/go" && go run ./cmd/latencysummary \ + -input /home/limingjie/LMJ_Work/RobotCompetition/OmniSocketGo/logs/a-latency.jsonl \ + -input /home/limingjie/LMJ_Work/RobotCompetition/OmniSocketGo/logs/b-latency.jsonl \ + -output /home/limingjie/LMJ_Work/RobotCompetition/OmniSocketGo/logs/latency-summary.jsonl) || exit 1 +cd "$LOCAL_REPO_DIR/.." || exit 1 +mv "$LOCAL_REPO_DIR/logs/a-latency.jsonl" "$OUTPUT_DIR/a-latency.jsonl" || exit 1 +mv "$LOCAL_REPO_DIR/logs/b-latency.jsonl" "$OUTPUT_DIR/b-latency.jsonl" || exit 1 +mv "$LOCAL_REPO_DIR/logs/latency-summary.jsonl" "$OUTPUT_DIR/latency-summary.jsonl" || exit 1 +if [ -f "$LOCAL_REPO_DIR/logs/latency-summary.html" ]; then + mv "$LOCAL_REPO_DIR/logs/latency-summary.html" "$OUTPUT_DIR/latency-summary.html" || exit 1 +fi + +echo "=== 测试完成！===" diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/BDAanto_test.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/BDAanto_test.sh new file mode 100644 index 0000000..dab1169 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/BDAanto_test.sh @@ -0,0 +1,259 @@ +#!/bin/bash + +LOCAL_REPO_DIR="/home/limingjie/LMJ_Work/RobotCompetition/OmniSocketGo" +KCP_PEER_BIN="./bin/kcppeer" +OUTPUT_ROOT="/home/limingjie/LMJ_Work/RobotCompetition/KCPData/BCAlogs/" +PEERB_POLL_INTERVAL_SEC=5 +PEERB_MAX_POLLS=180 +PEER_A_EXIT_WAIT_SEC=5 + +require_local_binary() { + if [ ! -x "$1" ]; then + echo "ERROR: 缺少可执行文件 $1" + exit 1 + fi +} + +cleanup_remote_peerb() { + ssh omni-peer bash -s <<'EOF' +pids=$(ps -eo pid=,args= | awk '/[b]in\/kcppeer -id peer-b/ {print $1}') +if [ -n "$pids" ]; then + kill $pids 2>/dev/null || true +fi +pids=$(ps -eo pid=,args= | awk '/\/tmp\/peerb_batch\.sh/ {print $1}') +if [ -n "$pids" ]; then + kill $pids 2>/dev/null || true +fi +rm -f /tmp/peerb_batch_done /tmp/peerb_batch.sh /tmp/peerb_commands +EOF +} + +echo "=== 开始自动化测试 ===" + +cd "$LOCAL_REPO_DIR" +require_local_binary "$KCP_PEER_BIN" + +echo ">>> 0. 清理上次残留进程..." +pkill -f 'bin/kcppeer -id peer-a' 2>/dev/null || true +cleanup_remote_peerb || exit 1 + +rm -rf logs +rm -rf inbox/a +mkdir -p logs inbox/a + +# 1. 清理残留 & 启动 Server D +echo ">>> 1. 启动 Server D..." + +ssh bj-txy bash -s <<'EOF' +pkill -f kcpserver 2>/dev/null || true +pkill -f 'bin/kcpserver' 2>/dev/null || true +sleep 1 +cd /home/ubuntu/OmniSocketGo +rm -rf logs +mkdir -p logs +if [ ! -x ./bin/kcpserver ]; then + echo "ERROR: 缺少 ./bin/kcpserver" + 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; then + echo " Server D 就绪 (${i}s)" + break + fi + if [ "$i" -eq 60 ]; then + echo " ERROR: Server D 60s 内未就绪，退出" + exit 1 + fi + sleep 1 +done + +# 2. 启动本地 Peer-A +echo ">>> 2. 启动本地 Peer-A..." +PEER_A_CMD_FIFO="/tmp/peera_commands_$$" +rm -f "$PEER_A_CMD_FIFO" +mkfifo "$PEER_A_CMD_FIFO" +nohup "$KCP_PEER_BIN" \ + -id peer-a \ + -server 81.70.156.140:10909 \ + -inbox-dir inbox/a \ + -latency-log logs/a-latency.jsonl \ + -kcp-ts-debug-log logs/a-kcp-ts.jsonl \ + -kcp-session-stats-log logs/a-kcp-stats.jsonl \ + < "$PEER_A_CMD_FIFO" > logs/peera_console.log 2>&1 & +PEER_A_PID=$! +exec 4>"$PEER_A_CMD_FIFO" + +# 等待 peer-a 注册成功 +echo " 等待 Peer-A 注册..." +for i in $(seq 1 30); do + if grep -Eq "opened KCP session as peer-a|connected to .* as peer-a( \\(KCP\\))?" logs/peera_console.log 2>/dev/null; then + echo " Peer-A 就绪 (${i}s)" + break + fi + if [ "$i" -eq 30 ]; then + echo " WARNING: Peer-A 30s 内未就绪" + fi + sleep 1 +done + +# 3. 在远端后台启动 peer-b 整个发送流程，不依赖长 SSH 连接 +echo ">>> 3. 启动远端 Peer-B 并执行 50 轮打流测试..." +ssh omni-peer "cd /home/boll/LMJWork/OmniSocketGo && rm -rf logs inbox/b && mkdir -p logs inbox/b" + +PEERB_DONE_FLAG="/tmp/peerb_batch_done" +PEERB_BATCH_SCRIPT="/tmp/peerb_batch.sh" + +# 把整个发送脚本写到远端，setsid 后台执行 +ssh omni-peer bash -s <<'DEPLOY_SCRIPT' +DONE_FLAG="/tmp/peerb_batch_done" +BATCH_SCRIPT="/tmp/peerb_batch.sh" +rm -f "$DONE_FLAG" + +cat > "$BATCH_SCRIPT" <<'INNER_EOF' +#!/bin/bash +cd /home/boll/LMJWork/OmniSocketGo + +CMD_FIFO=/tmp/peerb_commands +DONE_FLAG="/tmp/peerb_batch_done" +rm -f "$CMD_FIFO" "$DONE_FLAG" +mkfifo "$CMD_FIFO" + +if [ ! -x ./bin/kcppeer ]; then + echo "ERROR: 缺少 ./bin/kcppeer" > logs/peerb_console.log + echo "error" > "$DONE_FLAG" + exit 1 +fi + +# 启动 peer-b +./bin/kcppeer \ + -id peer-b \ + -server 81.70.156.140:10909 \ + -inbox-dir inbox/b \ + -latency-log logs/b-latency.jsonl \ + -kcp-ts-debug-log logs/b-kcp-ts.jsonl \ + -kcp-session-stats-log logs/b-kcp-stats.jsonl \ + < "$CMD_FIFO" > logs/peerb_console.log 2>&1 & +PEER_B_PID=$! + +exec 3>"$CMD_FIFO" + +# 等 peer-b 就绪 +for i in $(seq 1 60); do + if grep -Eq "opened KCP session as peer-b|connected to .* as peer-b( \\(KCP\\))?" logs/peerb_console.log 2>/dev/null; then + break + fi + sleep 1 +done + +# 50 轮发送 +for i in $(seq 1 50); do + echo "file peer-a /home/boll/test30.bin" >&3 + sleep 1 + echo "file peer-a /home/boll/test5.bin" >&3 + sleep 1 +done + +sleep 5 +echo "quit" >&3 +exec 3>&- +rm -f "$CMD_FIFO" + +peer_b_exited=0 +for i in $(seq 1 15); do + if ! kill -0 $PEER_B_PID 2>/dev/null; then + peer_b_exited=1 + break + fi + sleep 1 +done + +if [ "$peer_b_exited" -eq 0 ]; then + kill $PEER_B_PID 2>/dev/null || true + sleep 1 +fi + +if kill -0 $PEER_B_PID 2>/dev/null; then + kill -9 $PEER_B_PID 2>/dev/null || true +fi + +wait $PEER_B_PID 2>/dev/null || true + +# 写完成标记 +echo "done" > "$DONE_FLAG" +INNER_EOF + +chmod +x "$BATCH_SCRIPT" +setsid bash "$BATCH_SCRIPT" /dev/null 2>&1 & +echo "peer-b batch launched in background" +DEPLOY_SCRIPT + +# 本地轮询等待远端完成（短 SSH 连接，不怕断开） +echo " 等待 peer-b 发送完成（预计 ~110 秒）..." +for i in $(seq 1 "$PEERB_MAX_POLLS"); do + PEERB_STATUS=$(ssh omni-peer "cat /tmp/peerb_batch_done 2>/dev/null || true") + if [ "$PEERB_STATUS" = "done" ]; then + ELAPSED_SEC=$(( (i - 1) * PEERB_POLL_INTERVAL_SEC )) + echo " peer-b 发送完成（约 ${ELAPSED_SEC}s）" + break + fi + if [ "$PEERB_STATUS" = "error" ]; then + echo " ERROR: peer-b 后台任务启动失败，请检查远端 logs/peerb_console.log" + exit 1 + fi + if [ "$i" -eq "$PEERB_MAX_POLLS" ]; then + echo " ERROR: peer-b $((PEERB_MAX_POLLS * PEERB_POLL_INTERVAL_SEC))s 内未完成" + exit 1 + fi + sleep "$PEERB_POLL_INTERVAL_SEC" +done + +# 4. 清理 +echo ">>> 4. 清理所有进程..." +sleep 2 +echo "quit" >&4 || true +exec 4>&- +for i in $(seq 1 "$PEER_A_EXIT_WAIT_SEC"); do + if ! kill -0 "$PEER_A_PID" 2>/dev/null; then + break + fi + sleep 1 +done +if kill -0 "$PEER_A_PID" 2>/dev/null; then + kill "$PEER_A_PID" 2>/dev/null || true +fi +wait "$PEER_A_PID" 2>/dev/null || true +rm -f "$PEER_A_CMD_FIFO" +ssh bj-txy "pkill -f kcpserver || true; pkill -f 'bin/kcpserver' || true" + +# 获取当前时间戳，格式为 YYYYMMDD_HHMMSS +TIMESTAMP=$(date +"%Y%m%d_%H%M%S") +OUTPUT_DIR="$OUTPUT_ROOT/$TIMESTAMP" + +# 5. 拉取数据 & 生成报告 +echo ">>> 5. 拉取数据并生成汇总报告..." +mkdir -p "$OUTPUT_DIR" +scp -o ServerAliveInterval=15 -P 10022 boll@175.178.116.187:/home/boll/LMJWork/OmniSocketGo/logs/b-latency.jsonl "$LOCAL_REPO_DIR/logs/b-latency.jsonl" || exit 1 + +(cd "$LOCAL_REPO_DIR/go" && go run ./cmd/latencysummary \ + -input "$LOCAL_REPO_DIR/logs/a-latency.jsonl" \ + -input "$LOCAL_REPO_DIR/logs/b-latency.jsonl" \ + -output "$LOCAL_REPO_DIR/logs/latency-summary.jsonl") || exit 1 + +cd "$LOCAL_REPO_DIR/.." || exit 1 +mv "$LOCAL_REPO_DIR/logs/a-latency.jsonl" "$OUTPUT_DIR/a-latency.jsonl" || exit 1 +mv "$LOCAL_REPO_DIR/logs/b-latency.jsonl" "$OUTPUT_DIR/b-latency.jsonl" || exit 1 +mv "$LOCAL_REPO_DIR/logs/latency-summary.jsonl" "$OUTPUT_DIR/latency-summary.jsonl" || exit 1 +if [ -f "$LOCAL_REPO_DIR/logs/latency-summary.html" ]; then + mv "$LOCAL_REPO_DIR/logs/latency-summary.html" "$OUTPUT_DIR/latency-summary.html" || exit 1 +fi + +echo "=== 测试完成！===" diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/5g-dial.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/5g-dial.sh new file mode 100644 index 0000000..e2c07c7 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/5g-dial.sh @@ -0,0 +1,180 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="5g-dial" + +append_route_targets() { + local raw_list="$1" + local target + + if [[ -z "${raw_list}" ]]; then + return 0 + fi + + for target in ${raw_list//,/ }; do + if [[ -z "${target}" ]]; then + continue + fi + dial_cmd+=(--route-target "${target}") + done +} + +read_detected_interface() { + local info_json="$1" + + if [[ ! -f "${info_json}" ]]; then + return 1 + fi + + python3 -c 'import json, sys; print((json.load(open(sys.argv[1], encoding="utf-8")).get("interface") or "").strip())' "${info_json}" +} + +disable_interfaces() { + local raw_list="$1" + local iface + local nmcli_available=0 + + if [[ -z "${raw_list}" ]]; then + return 0 + fi + if command -v nmcli >/dev/null 2>&1; then + nmcli_available=1 + fi + + for iface in ${raw_list//,/ }; do + if [[ -z "${iface}" ]]; then + continue + fi + blitz_log "${STEP}" "disable-interface" "start" "iface=${iface}" 0 + if [[ "${nmcli_available}" -eq 1 ]]; then + nmcli device disconnect "${iface}" >/dev/null 2>&1 || true + fi + if ip link show dev "${iface}" >/dev/null 2>&1; then + if ip link set dev "${iface}" down; then + blitz_log "${STEP}" "disable-interface" "success" "iface=${iface}" 0 + else + rc=$? + blitz_log "${STEP}" "disable-interface" "failure" "iface=${iface}" "${rc}" + return "${rc}" + fi + else + blitz_log "${STEP}" "disable-interface" "success" "iface=${iface} not present, skipping" 0 + fi + done +} + +wait_for_serial() { + local serial_port="$1" + local timeout_sec="$2" + local waited=0 + + while (( waited < timeout_sec )); do + if [[ -e "${serial_port}" ]]; then + blitz_log "${STEP}" "wait-serial" "success" "serial_port=${serial_port} waited_sec=${waited}" 0 + return 0 + fi + if (( waited == 0 || waited % 5 == 0 )); then + blitz_log "${STEP}" "wait-serial" "waiting" "serial_port=${serial_port} waited_sec=${waited}" 0 + fi + sleep 1 + waited=$(( waited + 1 )) + done + + blitz_log "${STEP}" "wait-serial" "failure" "serial_port=${serial_port} timeout_sec=${timeout_sec}" 1 + return 1 +} + +wait_for_route() { + local target_ip="$1" + local timeout_sec="$2" + local expected_interface="${3:-}" + local waited=0 + local route_output + + while (( waited < timeout_sec )); do + route_output="$(blitz_route_ready "${target_ip}" "${expected_interface}" || true)" + if [[ -n "${route_output}" ]]; then + blitz_log "${STEP}" "route-check" "success" "target_ip=${target_ip} interface=${expected_interface:-auto} route=${route_output}" 0 + return 0 + fi + if (( waited == 0 || waited % 5 == 0 )); then + blitz_log "${STEP}" "route-check" "waiting" "target_ip=${target_ip} interface=${expected_interface:-auto} waited_sec=${waited}" 0 + fi + sleep 1 + waited=$(( waited + 1 )) + done + + blitz_log "${STEP}" "route-check" "failure" "target_ip=${target_ip} interface=${expected_interface:-auto} timeout_sec=${timeout_sec}" 1 + return 1 +} + +blitz_load_boot_env +blitz_require_root "${STEP}" +blitz_require_command ip "${STEP}" +blitz_require_command python3 "${STEP}" +blitz_require_file "${BLITZ_5G_DIAL_DIR}/rndis_dial.py" "${STEP}" + +if [[ -z "${BLITZ_TIME_SERVER_IP}" ]]; then + blitz_log "${STEP}" "precheck" "failure" "BLITZ_TIME_SERVER_IP is empty and no fallback could be derived" 1 + exit 1 +fi + +disable_interfaces "${BLITZ_5G_DISABLE_INTERFACES:-}" + +if [[ -n "${BLITZ_5G_INTERFACE:-}" ]]; then + route_output="$(blitz_route_ready "${BLITZ_TIME_SERVER_IP}" "${BLITZ_5G_INTERFACE}" || true)" + if [[ -n "${route_output}" ]]; then + blitz_log "${STEP}" "dial" "already_up" "target_ip=${BLITZ_TIME_SERVER_IP} interface=${BLITZ_5G_INTERFACE} route=${route_output}" 0 + exit 0 + fi +else + blitz_log "${STEP}" "route-check" "info" "BLITZ_5G_INTERFACE is empty, skipping pre-dial route shortcut and using auto-detect mode" 0 +fi + +wait_for_serial "${BLITZ_5G_SERIAL_PORT}" "${BLITZ_5G_SERIAL_WAIT_SEC}" + +dial_cmd=( + python3 + rndis_dial.py + --serial-port "${BLITZ_5G_SERIAL_PORT}" + --modem-subnet "${BLITZ_5G_MODEM_SUBNET}" +) +if [[ -n "${BLITZ_5G_INTERFACE:-}" ]]; then + dial_cmd+=(--interface "${BLITZ_5G_INTERFACE}") +fi +case "${BLITZ_5G_SKIP_DHCP:-0}" in + 1|true|TRUE|yes|YES) + dial_cmd+=(--skip-dhcp) + ;; +esac +case "${BLITZ_5G_REMOVE_DEFAULT_ROUTE:-1}" in + 1|true|TRUE|yes|YES) + dial_cmd+=(--remove-default-route --gateway "${BLITZ_5G_GATEWAY}" --route-target "${BLITZ_TIME_SERVER_IP}") + append_route_targets "${BLITZ_5G_ROUTE_TARGETS:-}" + ;; +esac + +pushd "${BLITZ_5G_DIAL_DIR}" >/dev/null +blitz_run "${STEP}" "dial" "${dial_cmd[@]}" +popd >/dev/null + +resolved_interface="${BLITZ_5G_INTERFACE:-}" +if [[ -z "${resolved_interface}" ]]; then + resolved_interface="$(read_detected_interface "${BLITZ_5G_INFO_JSON}" || true)" + if [[ -n "${resolved_interface}" ]]; then + blitz_log "${STEP}" "resolve-interface" "success" "resolved interface from ${BLITZ_5G_INFO_JSON}: ${resolved_interface}" 0 + else + blitz_log "${STEP}" "resolve-interface" "failure" "failed to read detected interface from ${BLITZ_5G_INFO_JSON}" 1 + fi +fi + +if [[ -n "${resolved_interface}" ]]; then + wait_for_route "${BLITZ_TIME_SERVER_IP}" "${BLITZ_5G_ROUTE_WAIT_SEC}" "${resolved_interface}" + blitz_log "${STEP}" "complete" "success" "5G dial completed and route is ready on ${resolved_interface}" 0 +else + blitz_log "${STEP}" "complete" "success" "5G dial completed but route wait was skipped because no interface could be resolved; refer to rndis_dial.py logs" 0 +fi diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/README.md b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/README.md new file mode 100644 index 0000000..ab75d33 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/README.md @@ -0,0 +1,219 @@ +# Robot B-Side Boot Chain + +This directory contains the robot-side boot and recovery scripts. + +Normal usage is: + +```bash +sudo bash scripts/boot/install-systemd.sh +sudo systemctl start blitz-robot.target +``` + +After installation, `blitz-robot.target` is enabled and will start automatically on reboot. + +To stop the chain now and disable boot-time autostart for future reboots: + +```bash +sudo bash scripts/boot/disable-systemd.sh +``` + +## Current Startup Order + +The current cold-start chain is: + +1. `blitz-boot-gate.service` +2. `blitz-5g-dial.service` +3. `blitz-ros-receiver.service` +4. `blitz-b-side-omnid.service` +5. `blitz-watchdog.service` + +There is no longer any automatic time-sync step in the boot chain. + +## What Each Script Does + +- `robot-boot.env`: default boot configuration +- `robot-boot.env.local`: machine-local overrides +- `common.sh`: shared env loading, logging, and helper functions +- `boot-gate.sh`: fixed startup delay gate +- `5g-dial.sh`: brings up the 5G modem path and verifies routing +- `start-ros-receiver-service.sh`: boot wrapper for ROS receiver +- `wait-for-unix-socket.sh`: waits for the ROS receiver unix socket +- `start-b-side-omnid-service.sh`: boot wrapper for `b_side_omnid` +- `blitz-watchdog.sh`: runtime health watchdog and recovery orchestrator +- `blitz-fault-inject.sh`: fault injection entrypoint +- `install-systemd.sh`: installs systemd units into `/etc/systemd/system` +- `disable-systemd.sh`: stops the boot chain and disables autostart + +## Important Configuration + +Most machine-specific overrides should go into: + +```text +scripts/boot/robot-boot.env.local +``` + +Typical settings: + +```bash +BLITZ_BOOT_DELAY_SEC="30" +BLITZ_LOG_FILE="/var/log/blitz-robot/startup.log" +BLITZ_RUNTIME_DIR="/run/blitz-robot" + +BLITZ_5G_DIAL_DIR="${OMNISOCKETGO_ROOT}/scripts/boot" +BLITZ_5G_SERIAL_PORT="/dev/ttyUSB2" +BLITZ_5G_INTERFACE="" +BLITZ_5G_MODEM_SUBNET="192.168.224.0/22" +BLITZ_5G_GATEWAY="192.168.225.1" +BLITZ_5G_REMOVE_DEFAULT_ROUTE="1" +BLITZ_5G_ROUTE_TARGETS="106.55.173.235" +BLITZ_5G_INFO_JSON="${OMNISOCKETGO_ROOT}/scripts/boot/modem_network_info.json" + +BLITZ_TIME_SERVER_IP="81.70.156.140" + +BLITZ_ROS_USER="nvidia" +BLITZ_ROS_SOCKET_WAIT_SEC="20" +BLITZ_WATCHDOG_INTERVAL_SEC="5" +BLITZ_HEALTH_STALE_SEC="15" +BLITZ_OMNID_THREAD_HEARTBEAT_TIMEOUT_SEC="15" +BLITZ_NETWORK_FAIL_THRESHOLD="3" +BLITZ_NETWORK_RECOVERY_COOLDOWN_SEC="30" +BLITZ_GPS_MONITOR_ENABLED="1" +BLITZ_GPS_DEVICE_GLOB="/dev/ttyCH341USB*" +BLITZ_GPS_CHECK_INTERVAL_SEC="10" +BLITZ_GPS_RESTART_UNITS="gpsd.socket gpsd.service" +BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION="0" +``` + +`BLITZ_TIME_SERVER_IP` is still used, but only as the 5G route/ping health-check target. It is no longer used for automatic clock synchronization. + +If `BLITZ_TIME_SERVER_IP` is left empty, the scripts fall back to the host part of `ROBOT_SIDE_OMNISOCKET_SERVER_ADDR`. + +## Install Or Upgrade + +Run: + +```bash +sudo bash scripts/boot/install-systemd.sh +sudo systemctl daemon-reload +sudo systemctl restart blitz-robot.target +``` + +`install-systemd.sh` will also remove any old `blitz-time-sync.service` unit left over from earlier versions. + +## Disable Autostart + +To stop the currently running services and disable autostart for future reboots: + +```bash +sudo bash scripts/boot/disable-systemd.sh +``` + +To re-enable later: + +```bash +sudo bash scripts/boot/install-systemd.sh +sudo systemctl start blitz-robot.target +``` + +## Logs + +All boot-chain and watchdog logs are appended to: + +```text +/var/log/blitz-robot/startup.log +``` + +Follow the log live: + +```bash +sudo tail -f /var/log/blitz-robot/startup.log +``` + +Check service state: + +```bash +sudo systemctl status blitz-robot.target +sudo systemctl status blitz-5g-dial.service +sudo systemctl status blitz-ros-receiver.service +sudo systemctl status blitz-b-side-omnid.service +sudo systemctl status blitz-watchdog.service +``` + +Check systemd journal: + +```bash +sudo journalctl -u blitz-robot.target -u blitz-5g-dial.service \ + -u blitz-ros-receiver.service -u blitz-b-side-omnid.service \ + -u blitz-watchdog.service -f +``` + +## Runtime Status Files + +The runtime status directory is: + +```text +/run/blitz-robot +``` + +Key files: + +- `b-side-omnid.status.json` +- `ros-receiver.status.json` +- `watchdog.status.json` + +`watchdog.status.json` now also records `gps_ok` and `gps_device_present` so you can quickly tell whether the GPS USB serial node is currently visible and whether the last `gpsd` reconnect attempt succeeded. + +Pretty-print them: + +```bash +sudo python3 -m json.tool /run/blitz-robot/watchdog.status.json +sudo python3 -m json.tool /run/blitz-robot/b-side-omnid.status.json +sudo python3 -m json.tool /run/blitz-robot/ros-receiver.status.json +``` + +## Fault Injection + +Available test commands: + +```bash +sudo bash scripts/boot/blitz-fault-inject.sh bside-crash +sudo bash scripts/boot/blitz-fault-inject.sh bside-process-freeze +sudo bash scripts/boot/blitz-fault-inject.sh bside-video-thread-stall +sudo bash scripts/boot/blitz-fault-inject.sh bside-control-thread-stall +sudo bash scripts/boot/blitz-fault-inject.sh ros-crash +sudo bash scripts/boot/blitz-fault-inject.sh ros-freeze +``` + +For synthetic network fault injection, first enable it in `robot-boot.env.local`: + +```bash +BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION="1" +``` + +Then restart watchdog and inject: + +```bash +sudo systemctl restart blitz-watchdog.service +sudo bash scripts/boot/blitz-fault-inject.sh network-down on +sudo bash scripts/boot/blitz-fault-inject.sh network-down off +``` + +## Recovery Behavior Summary + +- If `b_side_omnid` dies or its status file goes stale, watchdog first tries a targeted `b_side` restart. +- If ROS receiver dies, loses its socket, or its heartbeat goes stale, watchdog performs an ordered full restart: + - stop `b_side` + - restart ROS receiver + - wait for unix socket + - start `b_side` +- If network checks fail repeatedly, watchdog stops `b_side`, runs `5g-dial.sh`, waits for route recovery, and then restores services. +- While 5G is healthy, watchdog keeps every host route listed by `BLITZ_TIME_SERVER_IP` and `BLITZ_5G_ROUTE_TARGETS` pinned to the resolved 5G interface. When 5G becomes unhealthy, watchdog deletes those host routes so traffic can fall back to the remaining default network path. If that fallback path is still reachable, watchdog keeps `b_side_omnid` running instead of treating it as a full network outage. +- Whenever watchdog changes or restores those host routes, it logs `route-path` lines for each target so you can see which interface Linux currently chooses for `81.70.156.140`, `106.55.173.235`, and any other configured 5G-pinned target. +- If GPS monitoring is enabled, watchdog checks `BLITZ_GPS_DEVICE_GLOB` every `BLITZ_GPS_CHECK_INTERVAL_SEC` seconds. When the GPS serial device disappears and later reappears, watchdog restarts the units in `BLITZ_GPS_RESTART_UNITS` so `gpsd` can bind to the new device node again. +- Camera disappearance is logged as degraded state. Reappearance triggers a `b_side` restart after the device is stable. + +## Notes + +- `time-sync.sh` and `blitz-time-sync.service` are intentionally removed from the automatic boot path. +- `b_side_omnid` must already be built before boot-time startup. +- `bin/b_side_omnid` missing, ROS env missing, or modem script missing will all show up in `startup.log`. diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/blitz-5g-link-logger.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/blitz-5g-link-logger.sh new file mode 100644 index 0000000..bfcdfcd --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/blitz-5g-link-logger.sh @@ -0,0 +1,137 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="5g-link-logger" + +resolve_target_ip() { + if [[ -n "${BLITZ_TIME_SERVER_IP:-}" ]]; then + printf '%s\n' "${BLITZ_TIME_SERVER_IP}" + return 0 + fi + + for candidate in ${BLITZ_5G_ROUTE_TARGETS//,/ }; do + if [[ -n "${candidate}" ]]; then + printf '%s\n' "${candidate}" + return 0 + fi + done + return 1 +} + +emit_sample_json() { + local interface_name="${1:-}" + local target_ip="${2:-}" + + python3 - "${interface_name}" "${target_ip}" <<'PY' +import json +import subprocess +import sys +import time + +interface_name = sys.argv[1] +target_ip = sys.argv[2] + +payload = { + "ts_unix_ms": time.time_ns() // 1_000_000, + "interface": interface_name, + "target_ip": target_ip, + "link_present": False, + "route_output": "", + "route_ok": False, + "probe_ok": False, + "ping_rtt_ms": None, + "rx_bytes": 0, + "tx_bytes": 0, + "rx_packets": 0, + "tx_packets": 0, + "rx_errors": 0, + "tx_errors": 0, + "rx_drops": 0, + "tx_drops": 0, +} + +if interface_name: + try: + output = subprocess.check_output( + ["ip", "-j", "-s", "link", "show", "dev", interface_name], + text=True, + stderr=subprocess.DEVNULL, + ) + stats = json.loads(output) + if stats: + item = stats[0] + payload["link_present"] = True + rx = item.get("stats64", {}).get("rx", {}) + tx = item.get("stats64", {}).get("tx", {}) + if not rx and not tx: + rx = item.get("stats", {}).get("rx", {}) + tx = item.get("stats", {}).get("tx", {}) + payload["rx_bytes"] = int(rx.get("bytes") or 0) + payload["tx_bytes"] = int(tx.get("bytes") or 0) + payload["rx_packets"] = int(rx.get("packets") or 0) + payload["tx_packets"] = int(tx.get("packets") or 0) + payload["rx_errors"] = int(rx.get("errors") or 0) + payload["tx_errors"] = int(tx.get("errors") or 0) + payload["rx_drops"] = int(rx.get("dropped") or 0) + payload["tx_drops"] = int(tx.get("dropped") or 0) + except Exception: + pass + +if target_ip: + try: + route = subprocess.check_output( + ["ip", "route", "get", target_ip], + text=True, + stderr=subprocess.STDOUT, + ).strip() + payload["route_output"] = route.splitlines()[0] if route else "" + payload["route_ok"] = bool(payload["route_output"]) and ( + not interface_name or f" dev {interface_name}" in payload["route_output"] + ) + except Exception as exc: + payload["route_output"] = str(exc) + + ping_cmd = ["ping", "-c", "1", "-W", "2", target_ip] + if interface_name: + ping_cmd[1:1] = ["-I", interface_name] + ping = subprocess.run(ping_cmd, capture_output=True, text=True) + payload["probe_ok"] = ping.returncode == 0 + output = (ping.stdout or "") + "\n" + (ping.stderr or "") + for token in output.replace("\n", " ").split(): + if token.startswith("time="): + value = token.split("=", 1)[1].rstrip("ms") + try: + payload["ping_rtt_ms"] = float(value) + except ValueError: + pass + break + +print(json.dumps(payload, separators=(",", ":"), ensure_ascii=False)) +PY +} + +if [[ "${OMNI_BOOT_MODE:-0}" == "1" ]]; then + blitz_load_boot_env + blitz_require_run_context +fi + +if [[ -z "${BLITZ_RUN_DIR:-}" && -f "${BLITZ_RUN_CONTEXT_FILE:-}" ]]; then + blitz_load_run_context_env || true +fi +blitz_ensure_instance_id + +export BLITZ_5G_LINK_LOG_PATH="${BLITZ_5G_LINK_LOG_PATH:-${BLITZ_RUN_DIR}/b-5g-link-quality.${BLITZ_INSTANCE_ID}.jsonl}" +target_ip="$(resolve_target_ip || true)" + +blitz_log "${STEP}" "start" "start" "path=${BLITZ_5G_LINK_LOG_PATH} interval_sec=${BLITZ_5G_LINK_LOG_INTERVAL_SEC}" 0 + +while true; do + interface_name="$(blitz_resolve_5g_interface || true)" + line="$(emit_sample_json "${interface_name}" "${target_ip}")" + blitz_jsonl_append_line "${BLITZ_5G_LINK_LOG_PATH}" "${line}" + sleep "${BLITZ_5G_LINK_LOG_INTERVAL_SEC}" +done diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/blitz-fault-inject.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/blitz-fault-inject.sh new file mode 100644 index 0000000..8ec1b2f --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/blitz-fault-inject.sh @@ -0,0 +1,139 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="fault-inject" +B_SIDE_SERVICE="blitz-b-side-omnid.service" +ROS_SERVICE="blitz-ros-receiver.service" + +main_pid_for_service() { + local service_name="$1" + systemctl show --property MainPID --value "${service_name}" +} + +wait_for_service_pid_change() { + local service_name="$1" + local previous_pid="$2" + local timeout_sec="${3:-10}" + local waited=0 + local current_pid="" + + while (( waited < timeout_sec )); do + current_pid="$(main_pid_for_service "${service_name}")" + if [[ -n "${current_pid}" && "${current_pid}" != "0" && "${current_pid}" != "${previous_pid}" ]]; then + printf '%s\n' "${current_pid}" + return 0 + fi + sleep 1 + waited=$(( waited + 1 )) + done + + return 1 +} + +require_running_pid() { + local service_name="$1" + local pid + + pid="$(main_pid_for_service "${service_name}")" + if [[ -z "${pid}" || "${pid}" == "0" ]]; then + blitz_log "${STEP}" "lookup-pid" "failure" "service=${service_name}" 1 + exit 1 + fi + printf '%s\n' "${pid}" +} + +write_fault_flag() { + local flag_name="$1" + local flag_path="${BLITZ_RUNTIME_DIR}/${flag_name}" + printf '%s\n' "$(date +%s)" > "${flag_path}" + blitz_log "${STEP}" "flag-on" "success" "path=${flag_path}" 0 +} + +clear_fault_flag() { + local flag_name="$1" + local flag_path="${BLITZ_RUNTIME_DIR}/${flag_name}" + rm -f "${flag_path}" + blitz_log "${STEP}" "flag-off" "success" "path=${flag_path}" 0 +} + +blitz_load_boot_env +blitz_require_root "${STEP}" +blitz_prepare_runtime_dir + +case "${1:-}" in + bside-crash) + target_pid="$(require_running_pid "${B_SIDE_SERVICE}")" + blitz_log "${STEP}" "bside-crash" "start" "service=${B_SIDE_SERVICE} pid=${target_pid}" 0 + kill -9 "${target_pid}" + if restarted_pid="$(wait_for_service_pid_change "${B_SIDE_SERVICE}" "${target_pid}")"; then + blitz_log "${STEP}" "bside-crash" "success" "old_pid=${target_pid} new_pid=${restarted_pid}" 0 + else + blitz_log "${STEP}" "bside-crash" "failure" "old_pid=${target_pid} restart_not_observed_within=10s" 1 + exit 1 + fi + ;; + bside-process-freeze) + target_pid="$(require_running_pid "${B_SIDE_SERVICE}")" + blitz_log "${STEP}" "bside-process-freeze" "start" "service=${B_SIDE_SERVICE} pid=${target_pid}" 0 + kill -STOP "${target_pid}" + blitz_log "${STEP}" "bside-process-freeze" "success" "service=${B_SIDE_SERVICE} pid=${target_pid}" 0 + ;; + bside-video-thread-stall) + write_fault_flag "fault-injection-bside-video-thread-stall" + ;; + bside-control-thread-stall) + write_fault_flag "fault-injection-bside-control-thread-stall" + ;; + ros-crash) + target_pid="$(require_running_pid "${ROS_SERVICE}")" + blitz_log "${STEP}" "ros-crash" "start" "service=${ROS_SERVICE} pid=${target_pid}" 0 + kill -9 "${target_pid}" + if restarted_pid="$(wait_for_service_pid_change "${ROS_SERVICE}" "${target_pid}")"; then + blitz_log "${STEP}" "ros-crash" "success" "old_pid=${target_pid} new_pid=${restarted_pid}" 0 + else + blitz_log "${STEP}" "ros-crash" "failure" "old_pid=${target_pid} restart_not_observed_within=10s" 1 + exit 1 + fi + ;; + ros-freeze) + target_pid="$(require_running_pid "${ROS_SERVICE}")" + blitz_log "${STEP}" "ros-freeze" "start" "service=${ROS_SERVICE} pid=${target_pid}" 0 + kill -STOP "${target_pid}" + blitz_log "${STEP}" "ros-freeze" "success" "service=${ROS_SERVICE} pid=${target_pid}" 0 + ;; + network-down) + if [[ "${BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION}" != "1" ]]; then + blitz_log "${STEP}" "network-down" "failure" "set BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION=1 first" 1 + exit 1 + fi + case "${2:-}" in + on) + write_fault_flag "fault-injection-network-down" + ;; + off) + clear_fault_flag "fault-injection-network-down" + ;; + *) + echo "usage: $0 network-down on|off" >&2 + exit 2 + ;; + esac + ;; + *) + cat <<'EOF' +usage: + blitz-fault-inject.sh bside-crash + blitz-fault-inject.sh bside-process-freeze + blitz-fault-inject.sh bside-video-thread-stall + blitz-fault-inject.sh bside-control-thread-stall + blitz-fault-inject.sh ros-crash + blitz-fault-inject.sh ros-freeze + blitz-fault-inject.sh network-down on|off +EOF + exit 2 + ;; +esac diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/blitz-incident-capture-launch.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/blitz-incident-capture-launch.sh new file mode 100644 index 0000000..0bd788b --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/blitz-incident-capture-launch.sh @@ -0,0 +1,50 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="incident-launch" +incident_id="" +args=() +timeout_bin="" + +while (($# > 0)); do + case "$1" in + --incident-id) + incident_id="${2:-}" + shift 2 + ;; + *) + args+=("$1") + shift + ;; + esac +done + +blitz_load_boot_env +blitz_require_root "${STEP}" +blitz_require_command systemd-run "${STEP}" +blitz_require_command timeout "${STEP}" +timeout_bin="$(command -v timeout)" + +if [[ -z "${incident_id}" ]]; then + incident_id="$(blitz_new_incident_id)" +fi + +unit_name="blitz-incident-${incident_id//[^A-Za-z0-9_.-]/-}" + +systemd-run \ + --quiet \ + --collect \ + --unit "${unit_name}" \ + --property=Type=oneshot \ + --property="StandardOutput=append:${BLITZ_LOG_FILE}" \ + --property="StandardError=append:${BLITZ_LOG_FILE}" \ + "${timeout_bin}" "${BLITZ_INCIDENT_TOTAL_TIMEOUT_SEC}s" \ + /bin/bash "${SCRIPT_DIR}/blitz-incident-capture.sh" \ + --incident-id "${incident_id}" \ + "${args[@]}" + +printf '%s\n' "${incident_id}" diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/blitz-incident-capture.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/blitz-incident-capture.sh new file mode 100644 index 0000000..c6bcdfd --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/blitz-incident-capture.sh @@ -0,0 +1,131 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="incident-capture" +incident_id="" +incident_source="" +incident_reason="" +incident_unit="" +incident_result="" +incident_exit_status="" + +run_capture() { + local output_path="$1" + shift + + if command -v timeout >/dev/null 2>&1; then + timeout "${BLITZ_INCIDENT_COMMAND_TIMEOUT_SEC}s" "$@" > "${output_path}" 2>&1 || true + else + "$@" > "${output_path}" 2>&1 || true + fi +} + +while (($# > 0)); do + case "$1" in + --incident-id) + incident_id="${2:-}" + shift 2 + ;; + --source) + incident_source="${2:-}" + shift 2 + ;; + --reason) + incident_reason="${2:-}" + shift 2 + ;; + --unit) + incident_unit="${2:-}" + shift 2 + ;; + --result) + incident_result="${2:-}" + shift 2 + ;; + --exit-status) + incident_exit_status="${2:-}" + shift 2 + ;; + *) + blitz_log "${STEP}" "parse-arg" "failure" "unknown argument: $1" 2 + exit 2 + ;; + esac +done + +if [[ -n "${incident_result}" && "${incident_result}" == "success" ]]; then + exit 0 +fi + +blitz_load_boot_env +blitz_load_run_context_env || true +blitz_prepare_runtime_dir +blitz_prepare_run_root + +if [[ -z "${incident_id}" ]]; then + incident_id="$(blitz_new_incident_id)" +fi + +incident_dir="${BLITZ_RUN_ROOT}/incidents/${incident_id}" +mkdir -p "${incident_dir}" + +python3 - "${incident_dir}/incident.json" "${incident_id}" "${BLITZ_RUN_ID:-}" "${incident_source}" "${incident_reason}" "${incident_unit}" "${incident_result}" "${incident_exit_status}" "${BLITZ_RUN_DIR:-}" "${HOSTNAME:-$(hostname)}" <<'PY' +import json +import sys +import time + +path, incident_id, run_id, source, reason, unit, result, exit_status, run_dir, hostname = sys.argv[1:10] +payload = { + "incident_id": incident_id, + "run_id": run_id, + "source": source, + "fault_reason": reason, + "unit": unit, + "service_result": result, + "exit_status": exit_status, + "run_dir": run_dir, + "hostname": hostname, + "captured_at": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()), +} +with open(path, "w", encoding="utf-8") as handle: + json.dump(payload, handle, ensure_ascii=False, indent=2, sort_keys=True) +PY + +for status_file in \ + "${BLITZ_RUNTIME_DIR}/watchdog.status.json" \ + "${BLITZ_RUNTIME_DIR}/b-side-omnid.status.json" \ + "${BLITZ_RUNTIME_DIR}/ros-receiver.status.json" +do + if [[ -f "${status_file}" ]]; then + cp -f "${status_file}" "${incident_dir}/$(basename "${status_file}")" + fi +done + +if [[ -f "${BLITZ_LOG_FILE}" ]]; then + tail -n 400 "${BLITZ_LOG_FILE}" > "${incident_dir}/startup.log.tail" +fi + +run_capture "${incident_dir}/systemctl-status.txt" \ + systemctl status blitz-robot.target blitz-run-context.service blitz-5g-dial.service blitz-5g-link-logger.service blitz-ros-receiver.service blitz-b-side-omnid.service blitz-watchdog.service +run_capture "${incident_dir}/journal.txt" \ + journalctl --no-pager --since "5 minutes ago" -u blitz-run-context.service -u blitz-5g-dial.service -u blitz-5g-link-logger.service -u blitz-ros-receiver.service -u blitz-b-side-omnid.service -u blitz-watchdog.service +run_capture "${incident_dir}/ip-addr.txt" ip addr +run_capture "${incident_dir}/ip-route.txt" ip route +run_capture "${incident_dir}/ss-uapn.txt" ss -uapn +run_capture "${incident_dir}/ss-xlp.txt" ss -xlp + +if [[ -f "${BLITZ_5G_INFO_JSON:-}" ]]; then + cp -f "${BLITZ_5G_INFO_JSON}" "${incident_dir}/$(basename "${BLITZ_5G_INFO_JSON}")" +fi + +if [[ -n "${BLITZ_RUN_DIR:-}" && -d "${BLITZ_RUN_DIR}" ]]; then + while IFS= read -r -d '' jsonl; do + tail -n 200 "${jsonl}" > "${incident_dir}/tail-$(basename "${jsonl}")" + done < <(find "${BLITZ_RUN_DIR}" -maxdepth 1 -type f -name '*.jsonl' -print0 2>/dev/null) +fi + +blitz_log "${STEP}" "complete" "success" "incident_id=${incident_id} path=${incident_dir}" 0 diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/blitz-run-context.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/blitz-run-context.sh new file mode 100644 index 0000000..b159722 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/blitz-run-context.sh @@ -0,0 +1,22 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="run-context" + +on_error() { + local rc="$?" + blitz_log "${STEP}" "error" "failure" "line=${1:-unknown} cmd=${BASH_COMMAND:-unknown}" "${rc}" + exit "${rc}" +} + +trap 'on_error "${LINENO}"' ERR + +blitz_load_boot_env +blitz_require_root "${STEP}" +blitz_require_command python3 "${STEP}" +blitz_init_run_context +blitz_log "${STEP}" "complete" "success" "run_id=${BLITZ_RUN_ID} run_dir=${BLITZ_RUN_DIR}" 0 diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/blitz-watchdog.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/blitz-watchdog.sh new file mode 100644 index 0000000..758d521 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/blitz-watchdog.sh @@ -0,0 +1,971 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="watchdog" +B_SIDE_SERVICE="blitz-b-side-omnid.service" +ROS_SERVICE="blitz-ros-receiver.service" +B_SIDE_STATUS_FILE="" +ROS_STATUS_FILE="" +WATCHDOG_STATUS_FILE="" +NETWORK_FAULT_FILE="" +WATCHDOG_EVENT_LOG="" +WATCHDOG_SAMPLE_LOG="" +WATCHDOG_EVENT_LOG_FAILURE_REPORTED=0 +WATCHDOG_SAMPLE_LOG_FAILURE_REPORTED=0 +CAMERA_MISSING_PREV=0 +CAMERA_RECOVERY_STABLE_COUNT=0 +NETWORK_FAIL_COUNT=0 +NETWORK_COOLDOWN_UNTIL=0 +BACKOFF_UNTIL=0 +LAST_ACTION="none" +LAST_ACTION_EPOCH_MS=0 +FULL_RESTART_WINDOW_START=0 +FULL_RESTART_WINDOW_COUNT=0 +NETWORK_LAST_INTERFACE="" +NETWORK_ROUTE_INTERFACE_LAST_KNOWN="" +NETWORK_PRIMARY_LAST_RETRY_SEC=0 +GPS_LAST_CHECK_SEC=0 +GPS_DEVICE_PRESENT_PREV=-1 +GPS_DEVICE_PRESENT_STATE=1 +GPS_STACK_ACTIVE_STATE=1 +LAST_REPORTED_FAULT_REASON="" +LAST_REPORTED_RECOVERY_STATE="" +declare -A TARGETED_RESTART_WINDOW_START=() +declare -A TARGETED_RESTART_WINDOW_COUNT=() + +now_epoch_sec() { + date +%s +} + +now_epoch_ms() { + date +%s%3N +} + +service_is_active() { + systemctl is-active --quiet "$1" +} + +gps_monitor_enabled() { + [[ "${BLITZ_GPS_MONITOR_ENABLED:-0}" == "1" ]] +} + +gps_stack_active() { + local units=() + local unit + + read -r -a units <<< "${BLITZ_GPS_RESTART_UNITS:-}" + if (( ${#units[@]} == 0 )); then + return 1 + fi + + for unit in "${units[@]}"; do + if service_is_active "${unit}"; then + return 0 + fi + done + return 1 +} + +restart_gps_stack() { + local reason="$1" + local devices="$2" + local units=() + local rc + + read -r -a units <<< "${BLITZ_GPS_RESTART_UNITS:-}" + if (( ${#units[@]} == 0 )); then + GPS_STACK_ACTIVE_STATE=0 + blitz_log "${STEP}" "gps-reconnect" "failure" "reason=${reason} devices=${devices} units=empty" 1 + return 1 + fi + + set_last_action "gps-reconnect" + blitz_log "${STEP}" "gps-reconnect" "start" "reason=${reason} devices=${devices} units=${BLITZ_GPS_RESTART_UNITS}" 0 + if systemctl restart "${units[@]}"; then + GPS_STACK_ACTIVE_STATE=1 + blitz_log "${STEP}" "gps-reconnect" "success" "reason=${reason} devices=${devices} units=${BLITZ_GPS_RESTART_UNITS}" 0 + return 0 + fi + + rc=$? + GPS_STACK_ACTIVE_STATE=0 + blitz_log "${STEP}" "gps-reconnect" "failure" "reason=${reason} devices=${devices} units=${BLITZ_GPS_RESTART_UNITS}" "${rc}" + return "${rc}" +} + +check_gps_health() { + local now_sec="$1" + local check_interval_sec="${BLITZ_GPS_CHECK_INTERVAL_SEC:-10}" + local device_glob="${BLITZ_GPS_DEVICE_GLOB:-}" + local previous_present="${GPS_DEVICE_PRESENT_PREV}" + local recovery_reason="" + local device_summary="" + local -a devices=() + + if ! gps_monitor_enabled; then + GPS_DEVICE_PRESENT_STATE=1 + GPS_STACK_ACTIVE_STATE=1 + return 0 + fi + + if (( check_interval_sec < 1 )); then + check_interval_sec=1 + fi + if (( GPS_LAST_CHECK_SEC != 0 && now_sec - GPS_LAST_CHECK_SEC < check_interval_sec )); then + if (( GPS_DEVICE_PRESENT_STATE == 1 && GPS_STACK_ACTIVE_STATE == 1 )); then + return 0 + fi + return 1 + fi + GPS_LAST_CHECK_SEC="${now_sec}" + + mapfile -t devices < <(compgen -G "${device_glob}" || true) + if (( ${#devices[@]} == 0 )); then + GPS_DEVICE_PRESENT_STATE=0 + GPS_STACK_ACTIVE_STATE=0 + if (( previous_present != 0 )); then + blitz_log "${STEP}" "gps-device-check" "failure" "state=missing glob=${device_glob}" 1 + fi + GPS_DEVICE_PRESENT_PREV=0 + return 1 + fi + + device_summary="$(IFS=,; printf '%s' "${devices[*]}")" + GPS_DEVICE_PRESENT_STATE=1 + GPS_DEVICE_PRESENT_PREV=1 + + if (( previous_present == 0 )); then + blitz_log "${STEP}" "gps-device-check" "success" "state=reappeared devices=${device_summary}" 0 + recovery_reason="device-reappeared" + elif ! gps_stack_active; then + recovery_reason="gpsd-inactive" + fi + + if [[ -n "${recovery_reason}" ]]; then + if restart_gps_stack "${recovery_reason}" "${device_summary}"; then + return 0 + fi + return 1 + fi + + GPS_STACK_ACTIVE_STATE=1 + return 0 +} + +status_file_fresh() { + local path="$1" + local max_age_sec="$2" + local now_sec + local mtime_sec + + if [[ ! -f "${path}" ]]; then + return 1 + fi + now_sec="$(now_epoch_sec)" + mtime_sec="$(stat -c %Y "${path}" 2>/dev/null || echo 0)" + (( now_sec - mtime_sec <= max_age_sec )) +} + +ros_receiver_status_fresh() { + local path="$1" + local max_age_sec="$2" + local now_epoch_ms_value + + now_epoch_ms_value="$(now_epoch_ms)" + python3 - "${path}" "${now_epoch_ms_value}" "${max_age_sec}" <<'PY' +import json +import sys + +path = sys.argv[1] +now_epoch_ms = int(sys.argv[2]) +max_age_ms = int(sys.argv[3]) * 1000 + +try: + with open(path, "r", encoding="utf-8") as handle: + payload = json.load(handle) +except Exception: + raise SystemExit(1) + +heartbeat_ms = int(payload.get("recv_thread_heartbeat_epoch_ms") or 0) +socket_bound = bool(payload.get("socket_bound")) + +if heartbeat_ms <= 0 or not socket_bound: + raise SystemExit(1) + +raise SystemExit(0 if now_epoch_ms - heartbeat_ms <= max_age_ms else 1) +PY +} + +ros_receiver_healthy() { + local max_age_sec="$1" + + service_is_active "${ROS_SERVICE}" \ + && [[ -S "${ROBOT_RECEIVER_LOCAL_SOCKET_PATH}" ]] \ + && status_file_fresh "${ROS_STATUS_FILE}" "${max_age_sec}" \ + && ros_receiver_status_fresh "${ROS_STATUS_FILE}" "${max_age_sec}" +} + +write_watchdog_status() { + local fault_reason="$1" + local recovery_state="$2" + local network_ok="$3" + local camera_ok="$4" + local ros_ok="$5" + local bside_ok="$6" + local gps_ok="$7" + local gps_device_present="$8" + local tmp_file + + tmp_file="${WATCHDOG_STATUS_FILE}.tmp.$$" + cat > "${tmp_file}" <&1)"; then + if (( WATCHDOG_EVENT_LOG_FAILURE_REPORTED == 0 )); then + blitz_log "${STEP}" "watchdog-event-log" "failure" "path=${WATCHDOG_EVENT_LOG} detail=${line}" 0 || true + WATCHDOG_EVENT_LOG_FAILURE_REPORTED=1 + fi + return 0 + fi + if ! blitz_jsonl_append_line "${WATCHDOG_EVENT_LOG}" "${line}"; then + if (( WATCHDOG_EVENT_LOG_FAILURE_REPORTED == 0 )); then + blitz_log "${STEP}" "watchdog-event-log" "failure" "path=${WATCHDOG_EVENT_LOG} detail=append-failed" 0 || true + WATCHDOG_EVENT_LOG_FAILURE_REPORTED=1 + fi + return 0 + fi + WATCHDOG_EVENT_LOG_FAILURE_REPORTED=0 +} + +watchdog_append_sample() { + local line="" + + [[ -n "${WATCHDOG_SAMPLE_LOG}" ]] || return 0 + if ! line="$(watchdog_emit_json "$@" 2>&1)"; then + if (( WATCHDOG_SAMPLE_LOG_FAILURE_REPORTED == 0 )); then + blitz_log "${STEP}" "watchdog-sample-log" "failure" "path=${WATCHDOG_SAMPLE_LOG} detail=${line}" 0 || true + WATCHDOG_SAMPLE_LOG_FAILURE_REPORTED=1 + fi + return 0 + fi + if ! blitz_jsonl_append_line "${WATCHDOG_SAMPLE_LOG}" "${line}"; then + if (( WATCHDOG_SAMPLE_LOG_FAILURE_REPORTED == 0 )); then + blitz_log "${STEP}" "watchdog-sample-log" "failure" "path=${WATCHDOG_SAMPLE_LOG} detail=append-failed" 0 || true + WATCHDOG_SAMPLE_LOG_FAILURE_REPORTED=1 + fi + return 0 + fi + WATCHDOG_SAMPLE_LOG_FAILURE_REPORTED=0 +} + +watchdog_record_state_transition() { + local fault_reason="$1" + local recovery_state="$2" + + if [[ "${fault_reason}" == "${LAST_REPORTED_FAULT_REASON}" && "${recovery_state}" == "${LAST_REPORTED_RECOVERY_STATE}" ]]; then + return 0 + fi + watchdog_append_event "event" "state-transition" "${fault_reason}" "${recovery_state}" "" "" + LAST_REPORTED_FAULT_REASON="${fault_reason}" + LAST_REPORTED_RECOVERY_STATE="${recovery_state}" +} + +watchdog_launch_incident() { + local reason="$1" + local unit_name="$2" + + blitz_launch_incident_capture \ + --source watchdog \ + --reason "${reason}" \ + --unit "${unit_name}" \ + --result failure \ + --exit-status 1 2>/dev/null || true +} + +set_last_action() { + LAST_ACTION="$1" + LAST_ACTION_EPOCH_MS="$(now_epoch_ms)" +} + +targeted_restart_total() { + local total=0 + local key + + for key in "${!TARGETED_RESTART_WINDOW_COUNT[@]}"; do + total=$(( total + TARGETED_RESTART_WINDOW_COUNT["${key}"] )) + done + printf '%s\n' "${total}" +} + +register_targeted_restart() { + local fault_key="$1" + local now_sec + local window_start + local count + + now_sec="$(now_epoch_sec)" + window_start="${TARGETED_RESTART_WINDOW_START["${fault_key}"]:-0}" + count="${TARGETED_RESTART_WINDOW_COUNT["${fault_key}"]:-0}" + if (( window_start == 0 || now_sec - window_start > 60 )); then + window_start="${now_sec}" + count=1 + else + count=$(( count + 1 )) + fi + TARGETED_RESTART_WINDOW_START["${fault_key}"]="${window_start}" + TARGETED_RESTART_WINDOW_COUNT["${fault_key}"]="${count}" + (( count >= 2 )) +} + +record_full_restart() { + local now_sec + + now_sec="$(now_epoch_sec)" + if (( FULL_RESTART_WINDOW_START == 0 || now_sec - FULL_RESTART_WINDOW_START > 600 )); then + FULL_RESTART_WINDOW_START="${now_sec}" + FULL_RESTART_WINDOW_COUNT=1 + else + FULL_RESTART_WINDOW_COUNT=$(( FULL_RESTART_WINDOW_COUNT + 1 )) + fi + if (( FULL_RESTART_WINDOW_COUNT >= 3 )); then + BACKOFF_UNTIL=$(( now_sec + 60 )) + watchdog_append_event "event" "backoff-enter" "backoff" "backoff" "full_restart_count=${FULL_RESTART_WINDOW_COUNT}" "" + fi +} + +restart_bside_targeted() { + local fault_key="$1" + local reason="$2" + local rc + local incident_id="" + + if register_targeted_restart "${fault_key}"; then + blitz_log "${STEP}" "escalate-full-restart" "start" "reason=${reason}" 0 + watchdog_append_event "event" "escalate-full-restart" "${reason}-escalated" "recovering" "fault_key=${fault_key}" "" + full_restart_stack "${reason}-escalated" + return 0 + fi + + incident_id="$(watchdog_launch_incident "${reason}" "${B_SIDE_SERVICE}")" + set_last_action "restart-bside" + RECOVERY_ACTION_TAKEN=1 + blitz_log "${STEP}" "restart-bside" "start" "reason=${reason}" 0 + watchdog_append_event "event" "restart-bside-start" "${reason}" "recovering" "fault_key=${fault_key}" "${incident_id}" + if systemctl restart "${B_SIDE_SERVICE}"; then + blitz_log "${STEP}" "restart-bside" "success" "reason=${reason}" 0 + watchdog_append_event "event" "restart-bside-success" "${reason}" "recovering" "fault_key=${fault_key}" "${incident_id}" + return 0 + fi + + rc=$? + blitz_log "${STEP}" "restart-bside" "failure" "reason=${reason}" "${rc}" + watchdog_append_event "event" "restart-bside-failure" "${reason}" "recovering" "fault_key=${fault_key} rc=${rc}" "${incident_id}" + return "${rc}" +} + +full_restart_stack() { + local reason="$1" + local rc + local incident_id="" + + incident_id="$(watchdog_launch_incident "${reason}" "blitz-robot.target")" + set_last_action "full-restart" + RECOVERY_ACTION_TAKEN=1 + recovery_state="recovering" + fault_reason="${reason}" + + blitz_log "${STEP}" "full-restart-stop-bside" "start" "reason=${reason}" 0 + watchdog_append_event "event" "full-restart-start" "${reason}" "recovering" "" "${incident_id}" + systemctl stop "${B_SIDE_SERVICE}" || true + + if systemctl restart "${ROS_SERVICE}"; then + blitz_log "${STEP}" "full-restart-restart-ros" "success" "reason=${reason}" 0 + else + rc=$? + blitz_log "${STEP}" "full-restart-restart-ros" "failure" "reason=${reason}" "${rc}" + record_full_restart + return "${rc}" + fi + + if bash "${BOOT_SCRIPT_DIR}/wait-for-unix-socket.sh" --step "${STEP}" --timeout "${BLITZ_ROS_SOCKET_WAIT_SEC}"; then + : + else + rc=$? + blitz_log "${STEP}" "full-restart-wait-socket" "failure" "reason=${reason}" "${rc}" + record_full_restart + return "${rc}" + fi + + if systemctl start "${B_SIDE_SERVICE}"; then + blitz_log "${STEP}" "full-restart-start-bside" "success" "reason=${reason}" 0 + else + rc=$? + blitz_log "${STEP}" "full-restart-start-bside" "failure" "reason=${reason}" "${rc}" + watchdog_append_event "event" "full-restart-failure" "${reason}" "recovering" "stage=start-bside rc=${rc}" "${incident_id}" + record_full_restart + return "${rc}" + fi + watchdog_append_event "event" "full-restart-success" "${reason}" "recovering" "" "${incident_id}" + record_full_restart +} + +network_fault_injected() { + [[ "${BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION}" == "1" && -f "${NETWORK_FAULT_FILE}" ]] +} + +resolve_network_interface() { + NETWORK_LAST_INTERFACE="$(blitz_resolve_5g_interface || true)" + if [[ -n "${NETWORK_LAST_INTERFACE}" ]]; then + NETWORK_ROUTE_INTERFACE_LAST_KNOWN="${NETWORK_LAST_INTERFACE}" + return 0 + fi + return 1 +} + +network_route_targets() { + local target + + if [[ -n "${BLITZ_TIME_SERVER_IP:-}" ]]; then + printf '%s\n' "${BLITZ_TIME_SERVER_IP}" + fi + for target in ${BLITZ_5G_ROUTE_TARGETS//,/ }; do + if [[ -n "${target}" && "${target}" != "${BLITZ_TIME_SERVER_IP:-}" ]]; then + printf '%s\n' "${target}" + fi + done +} + +log_target_route_paths() { + local action="$1" + local target + local route_output + + while IFS= read -r target; do + [[ -n "${target}" ]] || continue + route_output="$(ip route get "${target}" 2>&1 | head -n 1 || true)" + if [[ -z "${route_output}" ]]; then + route_output="unresolved" + fi + blitz_log "${STEP}" "route-path" "info" "action=${action} target=${target} route=${route_output}" 0 + done < <(network_route_targets) +} + +route_output_uses_interface() { + local route_output="$1" + local interface_name="$2" + + [[ -n "${interface_name}" ]] || return 1 + [[ "${route_output}" == *" dev ${interface_name} "* || "${route_output}" == *" dev ${interface_name}" ]] +} + +route_output_uses_gateway() { + local route_output="$1" + local gateway="$2" + + [[ -n "${gateway}" ]] || return 1 + [[ "${route_output}" == *"via ${gateway}"* ]] +} + +route_is_desired_target_route() { + local route_output="$1" + local interface_name="$2" + local gateway="$3" + + route_output_uses_interface "${route_output}" "${interface_name}" \ + && route_output_uses_gateway "${route_output}" "${gateway}" +} + +route_is_managed_5g_route() { + local route_output="$1" + local interface_name="${2:-}" + local gateway="${3:-}" + + if route_output_uses_interface "${route_output}" "${interface_name}"; then + return 0 + fi + if route_output_uses_gateway "${route_output}" "${gateway}"; then + return 0 + fi + if route_output_uses_gateway "${route_output}" "${BLITZ_5G_GATEWAY:-}"; then + return 0 + fi + return 1 +} + +resolve_route_cleanup_interface() { + local interface_name="" + local info_json="${BLITZ_5G_INFO_JSON:-}" + + if [[ -n "${NETWORK_LAST_INTERFACE}" ]]; then + printf '%s\n' "${NETWORK_LAST_INTERFACE}" + return 0 + fi + if [[ -n "${NETWORK_ROUTE_INTERFACE_LAST_KNOWN}" ]]; then + printf '%s\n' "${NETWORK_ROUTE_INTERFACE_LAST_KNOWN}" + return 0 + fi + + interface_name="$(blitz_read_5g_info_interface "${info_json}" || true)" + if [[ -n "${interface_name}" ]]; then + printf '%s\n' "${interface_name}" + return 0 + fi + return 1 +} + +resolve_network_gateway() { + local interface_name="$1" + local default_route + local gateway="" + local tokens=() + local index + + default_route="$(ip -o route show default dev "${interface_name}" 2>/dev/null | head -n 1 || true)" + if [[ -n "${default_route}" ]]; then + read -r -a tokens <<< "${default_route}" + for (( index=0; index<${#tokens[@]}-1; index++ )); do + if [[ "${tokens[index]}" == "via" ]]; then + gateway="${tokens[index + 1]}" + break + fi + done + fi + + if [[ -n "${gateway}" ]]; then + printf '%s\n' "${gateway}" + return 0 + fi + if [[ -n "${BLITZ_5G_GATEWAY:-}" ]]; then + printf '%s\n' "${BLITZ_5G_GATEWAY}" + return 0 + fi + return 1 +} + +sync_target_routes_to_5g() { + local interface_name="$1" + local gateway="${2:-}" + local route_output="" + local updated=0 + local target + local rc + + if [[ -z "${interface_name}" ]]; then + return 1 + fi + + if [[ -z "${gateway}" ]]; then + gateway="$(resolve_network_gateway "${interface_name}" || true)" + fi + if [[ -z "${gateway}" ]]; then + blitz_log "${STEP}" "route-sync-gateway" "failure" "interface=${interface_name}" 1 + return 1 + fi + + while IFS= read -r target; do + [[ -n "${target}" ]] || continue + route_output="$(ip route show "${target}/32" 2>/dev/null | head -n 1 || true)" + if [[ -n "${route_output}" ]] && route_is_desired_target_route "${route_output}" "${interface_name}" "${gateway}"; then + continue + fi + if ip route replace "${target}/32" via "${gateway}" dev "${interface_name}"; then + updated=1 + blitz_log "${STEP}" "route-sync-target" "success" "target=${target} interface=${interface_name} gateway=${gateway}" 0 + else + rc=$? + blitz_log "${STEP}" "route-sync-target" "failure" "target=${target} interface=${interface_name} gateway=${gateway}" "${rc}" + return "${rc}" + fi + done < <(network_route_targets) + + if (( updated == 1 )); then + NETWORK_ROUTE_INTERFACE_LAST_KNOWN="${interface_name}" + log_target_route_paths "sync-to-5g" + fi + return 0 +} + +clear_target_routes_from_5g() { + local interface_name="${1:-}" + local gateway="${2:-}" + local route_output="" + local target + local removed_any=0 + local rc + + if [[ -z "${interface_name}" ]]; then + interface_name="$(resolve_route_cleanup_interface || true)" + fi + if [[ -z "${gateway}" && -n "${interface_name}" ]]; then + gateway="$(resolve_network_gateway "${interface_name}" || true)" + fi + if [[ -z "${gateway}" ]]; then + gateway="${BLITZ_5G_GATEWAY:-}" + fi + + while IFS= read -r target; do + [[ -n "${target}" ]] || continue + route_output="$(ip route show "${target}/32" 2>/dev/null | head -n 1 || true)" + if [[ -z "${route_output}" ]] || ! route_is_managed_5g_route "${route_output}" "${interface_name}" "${gateway}"; then + continue + fi + if ip route del "${target}/32"; then + removed_any=1 + blitz_log "${STEP}" "route-clear-target" "success" "target=${target} interface=${interface_name:-unknown} gateway=${gateway:-unknown}" 0 + else + rc=$? + blitz_log "${STEP}" "route-clear-target" "failure" "target=${target} interface=${interface_name:-unknown} gateway=${gateway:-unknown}" "${rc}" + return "${rc}" + fi + done < <(network_route_targets) + + if (( removed_any == 1 )); then + blitz_log "${STEP}" "route-clear" "success" "interface=${interface_name:-unknown} gateway=${gateway:-unknown}" 0 + log_target_route_paths "clear-from-5g" + fi + return 0 +} + +repair_network_routes() { + local interface_name="$1" + local gateway="" + local route_output + + if [[ -z "${interface_name}" ]]; then + return 1 + fi + + gateway="$(resolve_network_gateway "${interface_name}" || true)" + if [[ -z "${gateway}" ]]; then + blitz_log "${STEP}" "route-repair-gateway" "failure" "interface=${interface_name}" 1 + return 1 + fi + + if ! sync_target_routes_to_5g "${interface_name}" "${gateway}"; then + clear_target_routes_from_5g "${interface_name}" "${gateway}" || true + return 1 + fi + + route_output="$(blitz_route_ready "${BLITZ_TIME_SERVER_IP}" "${interface_name}" || true)" + if [[ -z "${route_output}" ]]; then + clear_target_routes_from_5g "${interface_name}" "${gateway}" || true + blitz_log "${STEP}" "route-repair-postcheck" "failure" "interface=${interface_name} gateway=${gateway}" 1 + return 1 + fi + + if ! ping -I "${interface_name}" -c 1 -W 2 "${BLITZ_TIME_SERVER_IP}" >/dev/null 2>&1; then + clear_target_routes_from_5g "${interface_name}" "${gateway}" || true + blitz_log "${STEP}" "route-repair-probe" "failure" "interface=${interface_name} target=${BLITZ_TIME_SERVER_IP}" 1 + return 1 + fi + + blitz_log "${STEP}" "route-repair-postcheck" "success" "interface=${interface_name} gateway=${gateway} route=${route_output}" 0 + return 0 +} + +network_is_healthy() { + local route_output + + NETWORK_LAST_INTERFACE="" + if network_fault_injected; then + return 1 + fi + if ! resolve_network_interface; then + return 1 + fi + route_output="$(blitz_route_ready "${BLITZ_TIME_SERVER_IP}" "${NETWORK_LAST_INTERFACE}" || true)" + if [[ -z "${route_output}" ]]; then + return 1 + fi + ping -I "${NETWORK_LAST_INTERFACE}" -c 1 -W 2 "${BLITZ_TIME_SERVER_IP}" >/dev/null 2>&1 +} + +fallback_network_is_healthy() { + local route_output + + if [[ -z "${BLITZ_TIME_SERVER_IP:-}" ]]; then + return 1 + fi + + route_output="$(blitz_route_ready "${BLITZ_TIME_SERVER_IP}" || true)" + if [[ -z "${route_output}" ]]; then + return 1 + fi + + ping -c 1 -W 2 "${BLITZ_TIME_SERVER_IP}" >/dev/null 2>&1 +} + +wait_for_network_recovery() { + local timeout_sec="$1" + local waited=0 + + while (( waited < timeout_sec )); do + if network_is_healthy; then + blitz_log "${STEP}" "network-postcheck" "success" "interface=${NETWORK_LAST_INTERFACE} waited_sec=${waited}" 0 + return 0 + fi + if (( waited == 0 || waited % 5 == 0 )); then + blitz_log "${STEP}" "network-postcheck" "waiting" "interface=${NETWORK_LAST_INTERFACE:-unresolved} waited_sec=${waited}" 0 + fi + sleep 1 + waited=$(( waited + 1 )) + done + + blitz_log "${STEP}" "network-postcheck" "failure" "interface=${NETWORK_LAST_INTERFACE:-unresolved} timeout_sec=${timeout_sec}" 1 + return 1 +} + +perform_network_recovery() { + local rc=0 + local incident_id="" + + if resolve_network_interface && repair_network_routes "${NETWORK_LAST_INTERFACE}"; then + set_last_action "route-repair" + RECOVERY_ACTION_TAKEN=1 + NETWORK_COOLDOWN_UNTIL=$(( $(now_epoch_sec) + BLITZ_NETWORK_RECOVERY_COOLDOWN_SEC )) + NETWORK_FAIL_COUNT=0 + blitz_log "${STEP}" "network-recovery" "success" "mode=route-repair interface=${NETWORK_LAST_INTERFACE}" 0 + watchdog_append_event "event" "route-repair-success" "network_or_robot_unreachable" "recovering" "interface=${NETWORK_LAST_INTERFACE}" "" + return 0 + fi + + incident_id="$(watchdog_launch_incident "network-recovery" "blitz-5g-dial.service")" + set_last_action "network-recovery" + RECOVERY_ACTION_TAKEN=1 + blitz_log "${STEP}" "network-recovery" "start" "fail_count=${NETWORK_FAIL_COUNT}" 0 + watchdog_append_event "event" "network-recovery-start" "network_or_robot_unreachable" "recovering" "fail_count=${NETWORK_FAIL_COUNT}" "${incident_id}" + systemctl stop "${B_SIDE_SERVICE}" || true + + if bash "${BOOT_SCRIPT_DIR}/5g-dial.sh"; then + : + else + rc=$? + blitz_log "${STEP}" "network-redial" "failure" "fail_count=${NETWORK_FAIL_COUNT} script=${BOOT_SCRIPT_DIR}/5g-dial.sh" "${rc}" + watchdog_append_event "event" "network-recovery-failure" "network_or_robot_unreachable" "recovering" "stage=redial rc=${rc}" "${incident_id}" + return "${rc}" + fi + + if wait_for_network_recovery "${BLITZ_5G_ROUTE_WAIT_SEC}"; then + : + else + rc=$? + blitz_log "${STEP}" "network-recovery" "failure" "fail_count=${NETWORK_FAIL_COUNT} interface=${NETWORK_LAST_INTERFACE:-unresolved}" "${rc}" + watchdog_append_event "event" "network-recovery-failure" "network_or_robot_unreachable" "recovering" "stage=postcheck rc=${rc}" "${incident_id}" + return "${rc}" + fi + + NETWORK_COOLDOWN_UNTIL=$(( $(now_epoch_sec) + BLITZ_NETWORK_RECOVERY_COOLDOWN_SEC )) + NETWORK_FAIL_COUNT=0 + watchdog_append_event "event" "network-recovery-success" "network_or_robot_unreachable" "recovering" "interface=${NETWORK_LAST_INTERFACE:-unresolved}" "${incident_id}" + if ros_receiver_healthy "${BLITZ_HEALTH_STALE_SEC}"; then + restart_bside_targeted "network" "network-recovered" + return 0 + fi + full_restart_stack "network-recovered-ros-unhealthy" + return 0 +} + +blitz_load_boot_env +blitz_require_root "${STEP}" +blitz_require_command systemctl "${STEP}" +blitz_require_command stat "${STEP}" +blitz_require_command ping "${STEP}" +blitz_require_command python3 "${STEP}" +blitz_prepare_runtime_dir +blitz_require_run_context + +B_SIDE_STATUS_FILE="${BLITZ_RUNTIME_DIR}/b-side-omnid.status.json" +ROS_STATUS_FILE="${BLITZ_RUNTIME_DIR}/ros-receiver.status.json" +WATCHDOG_STATUS_FILE="${BLITZ_RUNTIME_DIR}/watchdog.status.json" +NETWORK_FAULT_FILE="${BLITZ_RUNTIME_DIR}/fault-injection-network-down" +WATCHDOG_EVENT_LOG="${BLITZ_RUN_DIR}/watchdog-events.jsonl" +WATCHDOG_SAMPLE_LOG="${BLITZ_RUN_DIR}/watchdog-samples.jsonl" + +while true; do + fault_reason="none" + recovery_state="ok" + network_ok=1 + camera_ok=1 + ros_ok=1 + bside_ok=1 + gps_ok=1 + gps_device_present=1 + RECOVERY_ACTION_TAKEN=0 + now_sec="$(now_epoch_sec)" + + if gps_monitor_enabled; then + gps_device_present="${GPS_DEVICE_PRESENT_STATE}" + if (( GPS_DEVICE_PRESENT_STATE == 0 || GPS_STACK_ACTIVE_STATE == 0 )); then + gps_ok=0 + fi + fi + + if (( BACKOFF_UNTIL > now_sec )); then + fault_reason="backoff" + recovery_state="backoff" + watchdog_record_state_transition "${fault_reason}" "${recovery_state}" + write_watchdog_status "${fault_reason}" "${recovery_state}" 0 0 0 0 "${gps_ok}" "${gps_device_present}" + watchdog_append_sample "sample" "loop" "${fault_reason}" "${recovery_state}" "" "" 0 0 0 0 "${gps_ok}" "${gps_device_present}" + sleep "${BLITZ_WATCHDOG_INTERVAL_SEC}" + continue + fi + + if (( NETWORK_COOLDOWN_UNTIL > now_sec )); then + recovery_state="recovering" + elif ! network_is_healthy; then + clear_target_routes_from_5g || true + if fallback_network_is_healthy; then + NETWORK_FAIL_COUNT=0 + fault_reason="network_fallback_active" + recovery_state="degraded" + blitz_log "${STEP}" "network-check" "fallback" "interface=${NETWORK_LAST_INTERFACE:-unresolved} target=${BLITZ_TIME_SERVER_IP}" 0 + if (( NETWORK_PRIMARY_LAST_RETRY_SEC == 0 || now_sec - NETWORK_PRIMARY_LAST_RETRY_SEC >= 10 )); then + NETWORK_PRIMARY_LAST_RETRY_SEC="${now_sec}" + if resolve_network_interface && repair_network_routes "${NETWORK_LAST_INTERFACE}"; then + NETWORK_PRIMARY_LAST_RETRY_SEC=0 + fault_reason="none" + recovery_state="ok" + blitz_log "${STEP}" "network-check" "primary-restored" "interface=${NETWORK_LAST_INTERFACE} target=${BLITZ_TIME_SERVER_IP}" 0 + log_target_route_paths "primary-restored" + fi + fi + else + network_ok=0 + NETWORK_FAIL_COUNT=$(( NETWORK_FAIL_COUNT + 1 )) + fault_reason="network_or_robot_unreachable" + recovery_state="recovering" + blitz_log "${STEP}" "network-check" "failure" "count=${NETWORK_FAIL_COUNT} interface=${NETWORK_LAST_INTERFACE:-unresolved}" 1 + if (( NETWORK_FAIL_COUNT >= BLITZ_NETWORK_FAIL_THRESHOLD )); then + perform_network_recovery || true + fi + fi + else + NETWORK_PRIMARY_LAST_RETRY_SEC=0 + NETWORK_FAIL_COUNT=0 + sync_target_routes_to_5g "${NETWORK_LAST_INTERFACE}" || true + fi + + if check_gps_health "${now_sec}"; then + gps_ok=1 + else + gps_ok=0 + gps_device_present="${GPS_DEVICE_PRESENT_STATE}" + if [[ "${fault_reason}" == "none" ]]; then + if (( GPS_DEVICE_PRESENT_STATE == 0 )); then + fault_reason="gps_device_missing" + else + fault_reason="gps_reconnect_failed" + fi + recovery_state="degraded" + fi + fi + gps_device_present="${GPS_DEVICE_PRESENT_STATE}" + + if [[ ! -e "${OMNI_CAMERA_DEVICE}" ]]; then + camera_ok=0 + fault_reason="camera_missing" + recovery_state="degraded" + CAMERA_MISSING_PREV=1 + CAMERA_RECOVERY_STABLE_COUNT=0 + elif (( RECOVERY_ACTION_TAKEN == 0 && CAMERA_MISSING_PREV == 1 )); then + CAMERA_RECOVERY_STABLE_COUNT=$(( CAMERA_RECOVERY_STABLE_COUNT + 1 )) + recovery_state="recovering" + fault_reason="camera_recovered" + if (( CAMERA_RECOVERY_STABLE_COUNT >= 2 )); then + restart_bside_targeted "camera" "camera-reappeared" || true + CAMERA_MISSING_PREV=0 + CAMERA_RECOVERY_STABLE_COUNT=0 + fi + else + CAMERA_RECOVERY_STABLE_COUNT=0 + fi + + if (( RECOVERY_ACTION_TAKEN == 0 )) && { ! service_is_active "${B_SIDE_SERVICE}" || ! status_file_fresh "${B_SIDE_STATUS_FILE}" "${BLITZ_HEALTH_STALE_SEC}"; }; then + bside_ok=0 + fault_reason="bside_status_stale" + recovery_state="recovering" + restart_bside_targeted "bside" "bside-unhealthy" || true + fi + + if (( RECOVERY_ACTION_TAKEN == 0 )) && ! ros_receiver_healthy "${BLITZ_HEALTH_STALE_SEC}"; then + ros_ok=0 + fault_reason="ros_receiver_unhealthy" + recovery_state="recovering" + full_restart_stack "ros-unhealthy" || true + fi + + watchdog_record_state_transition "${fault_reason}" "${recovery_state}" + write_watchdog_status "${fault_reason}" "${recovery_state}" "${network_ok}" "${camera_ok}" "${ros_ok}" "${bside_ok}" "${gps_ok}" "${gps_device_present}" + watchdog_append_sample "sample" "loop" "${fault_reason}" "${recovery_state}" "" "" "${network_ok}" "${camera_ok}" "${ros_ok}" "${bside_ok}" "${gps_ok}" "${gps_device_present}" + sleep "${BLITZ_WATCHDOG_INTERVAL_SEC}" +done diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/boot-gate.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/boot-gate.sh new file mode 100644 index 0000000..ef22e0f --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/boot-gate.sh @@ -0,0 +1,15 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="boot-gate" + +blitz_load_boot_env + +blitz_log "${STEP}" "start" "start" "delay_sec=${BLITZ_BOOT_DELAY_SEC}" 0 +blitz_log "${STEP}" "delay" "start" "sleep ${BLITZ_BOOT_DELAY_SEC}s before starting Blitz services" 0 +sleep "${BLITZ_BOOT_DELAY_SEC}" +blitz_log "${STEP}" "delay" "success" "boot gate released after ${BLITZ_BOOT_DELAY_SEC}s" 0 diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/common.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/common.sh new file mode 100644 index 0000000..61a2205 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/common.sh @@ -0,0 +1,661 @@ +#!/usr/bin/env bash +set -euo pipefail + +BOOT_SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +DEV_SCRIPT_DIR="$(cd "${BOOT_SCRIPT_DIR}/../dev" && pwd)" + +source_with_nounset_off() { + set +u + # shellcheck disable=SC1090 + source "$1" + set -u +} + +blitz_host_from_addr() { + local value="${1:-}" + + if [[ -z "${value}" ]]; then + return 1 + fi + if [[ "${value}" == \[*\]:* ]]; then + value="${value#\[}" + printf '%s\n' "${value%%]:*}" + return 0 + fi + printf '%s\n' "${value%%:*}" +} + +blitz_load_boot_env() { + local env_file + local default_time_server + local dev_run_root + local dev_runtime_dir + + if [[ "${BLITZ_BOOT_ENV_LOADED:-0}" == "1" ]]; then + return 0 + fi + + export BLITZ_BOOT_LOADING_ENV="1" + # shellcheck disable=SC1091 + source "${DEV_SCRIPT_DIR}/load-env.sh" + unset BLITZ_BOOT_LOADING_ENV + + for env_file in \ + "${BOOT_SCRIPT_DIR}/robot-boot.env" \ + "${BOOT_SCRIPT_DIR}/robot-boot.env.local" + do + if [[ -f "${env_file}" ]]; then + set -a + # shellcheck disable=SC1090 + source "${env_file}" + set +a + fi + done + + if declare -F normalize_loaded_env_vars >/dev/null 2>&1; then + normalize_loaded_env_vars + fi + + dev_run_root="${OMNISOCKETGO_ROOT}/logs" + dev_runtime_dir="${dev_run_root}/runtime" + + if [[ -z "${BLITZ_RUN_ROOT:-}" || "${BLITZ_RUN_ROOT}" == "${dev_run_root}" ]]; then + export BLITZ_RUN_ROOT="/var/log/blitz-robot" + fi + if [[ -z "${BLITZ_RUNTIME_DIR:-}" || "${BLITZ_RUNTIME_DIR}" == "${dev_runtime_dir}" ]]; then + export BLITZ_RUNTIME_DIR="/run/blitz-robot" + fi + if [[ -z "${BLITZ_RUN_CONTEXT_FILE:-}" || "${BLITZ_RUN_CONTEXT_FILE}" == "${dev_runtime_dir}/run-context.env" ]]; then + export BLITZ_RUN_CONTEXT_FILE="${BLITZ_RUNTIME_DIR}/run-context.env" + fi + if [[ -z "${BLITZ_RUN_ID_FILE:-}" || "${BLITZ_RUN_ID_FILE}" == "${dev_runtime_dir}/run-id" ]]; then + export BLITZ_RUN_ID_FILE="${BLITZ_RUNTIME_DIR}/run-id" + fi + if [[ -z "${BLITZ_CURRENT_RUN_LINK:-}" || "${BLITZ_CURRENT_RUN_LINK}" == "${dev_run_root}/current" ]]; then + export BLITZ_CURRENT_RUN_LINK="${BLITZ_RUN_ROOT}/current" + fi + + default_time_server="$(blitz_host_from_addr "${ROBOT_SIDE_OMNISOCKET_SERVER_ADDR:-}" || true)" + + export BLITZ_BOOT_DELAY_SEC="${BLITZ_BOOT_DELAY_SEC:-30}" + export BLITZ_RUN_ROOT="${BLITZ_RUN_ROOT:-/var/log/blitz-robot}" + export BLITZ_LOG_FILE="${BLITZ_LOG_FILE:-/var/log/blitz-robot/startup.log}" + export BLITZ_RUNTIME_DIR="${BLITZ_RUNTIME_DIR:-/run/blitz-robot}" + export BLITZ_RUN_CONTEXT_FILE="${BLITZ_RUN_CONTEXT_FILE:-${BLITZ_RUNTIME_DIR}/run-context.env}" + export BLITZ_RUN_ID_FILE="${BLITZ_RUN_ID_FILE:-${BLITZ_RUNTIME_DIR}/run-id}" + export BLITZ_CURRENT_RUN_LINK="${BLITZ_CURRENT_RUN_LINK:-${BLITZ_RUN_ROOT}/current}" + export BLITZ_5G_DIAL_DIR="${BLITZ_5G_DIAL_DIR:-${BOOT_SCRIPT_DIR}}" + export BLITZ_5G_SERIAL_PORT="${BLITZ_5G_SERIAL_PORT:-/dev/ttyUSB7}" + export BLITZ_5G_INTERFACE="${BLITZ_5G_INTERFACE:-}" + export BLITZ_5G_MODEM_SUBNET="${BLITZ_5G_MODEM_SUBNET:-192.168.224.0/22}" + export BLITZ_5G_GATEWAY="${BLITZ_5G_GATEWAY:-192.168.225.1}" + export BLITZ_5G_SKIP_DHCP="${BLITZ_5G_SKIP_DHCP:-0}" + export BLITZ_5G_REMOVE_DEFAULT_ROUTE="${BLITZ_5G_REMOVE_DEFAULT_ROUTE:-1}" + export BLITZ_5G_ROUTE_TARGETS="${BLITZ_5G_ROUTE_TARGETS:-106.55.173.235}" + export BLITZ_5G_INFO_JSON="${BLITZ_5G_INFO_JSON:-${BLITZ_5G_DIAL_DIR}/modem_network_info.json}" + export BLITZ_5G_DISABLE_INTERFACES="${BLITZ_5G_DISABLE_INTERFACES:-}" + export BLITZ_5G_SERIAL_WAIT_SEC="${BLITZ_5G_SERIAL_WAIT_SEC:-60}" + export BLITZ_5G_ROUTE_WAIT_SEC="${BLITZ_5G_ROUTE_WAIT_SEC:-30}" + export BLITZ_TIME_SERVER_IP="${BLITZ_TIME_SERVER_IP:-${default_time_server}}" + export BLITZ_ROS_USER="${BLITZ_ROS_USER:-nvidia}" + export BLITZ_ROS_SOCKET_WAIT_SEC="${BLITZ_ROS_SOCKET_WAIT_SEC:-20}" + export BLITZ_WATCHDOG_INTERVAL_SEC="${BLITZ_WATCHDOG_INTERVAL_SEC:-5}" + export BLITZ_HEALTH_STALE_SEC="${BLITZ_HEALTH_STALE_SEC:-15}" + export BLITZ_OMNID_THREAD_HEARTBEAT_TIMEOUT_SEC="${BLITZ_OMNID_THREAD_HEARTBEAT_TIMEOUT_SEC:-15}" + export BLITZ_KCP_STATS_INTERVAL_MS="${BLITZ_KCP_STATS_INTERVAL_MS:-1000}" + export BLITZ_CONTROL_LATENCY_LOG_ENABLED="${BLITZ_CONTROL_LATENCY_LOG_ENABLED:-1}" + export BLITZ_CONTROL_LATENCY_LOG_SAMPLE_MOD="${BLITZ_CONTROL_LATENCY_LOG_SAMPLE_MOD:-100}" + export BLITZ_5G_LINK_LOG_INTERVAL_SEC="${BLITZ_5G_LINK_LOG_INTERVAL_SEC:-5}" + export BLITZ_JSONL_FLUSH_INTERVAL_MS="${BLITZ_JSONL_FLUSH_INTERVAL_MS:-1000}" + export BLITZ_JSONL_FLUSH_BYTES="${BLITZ_JSONL_FLUSH_BYTES:-262144}" + export BLITZ_JSONL_ROTATE_BYTES="${BLITZ_JSONL_ROTATE_BYTES:-134217728}" + export BLITZ_JSONL_ROTATE_FILES="${BLITZ_JSONL_ROTATE_FILES:-8}" + export BLITZ_INCIDENT_COMMAND_TIMEOUT_SEC="${BLITZ_INCIDENT_COMMAND_TIMEOUT_SEC:-5}" + export BLITZ_INCIDENT_TOTAL_TIMEOUT_SEC="${BLITZ_INCIDENT_TOTAL_TIMEOUT_SEC:-30}" + export BLITZ_NETWORK_FAIL_THRESHOLD="${BLITZ_NETWORK_FAIL_THRESHOLD:-3}" + export BLITZ_NETWORK_RECOVERY_COOLDOWN_SEC="${BLITZ_NETWORK_RECOVERY_COOLDOWN_SEC:-30}" + export BLITZ_GPS_MONITOR_ENABLED="${BLITZ_GPS_MONITOR_ENABLED:-1}" + export BLITZ_GPS_DEVICE_GLOB="${BLITZ_GPS_DEVICE_GLOB:-/dev/ttyCH341USB*}" + export BLITZ_GPS_CHECK_INTERVAL_SEC="${BLITZ_GPS_CHECK_INTERVAL_SEC:-10}" + export BLITZ_GPS_RESTART_UNITS="${BLITZ_GPS_RESTART_UNITS:-gpsd.socket gpsd.service}" + export BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION="${BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION:-0}" + export BLITZ_BOOT_ENV_LOADED="1" +} + +blitz_timestamp() { + date '+%Y-%m-%d %H:%M:%S%z' +} + +blitz_sanitize_detail() { + local detail="${1:-}" + + detail="${detail//$'\n'/ ; }" + detail="${detail//$'\r'/ }" + printf '%s' "${detail}" +} + +blitz_log() { + local step="${1:-unknown-step}" + local action="${2:-unknown-action}" + local result="${3:-info}" + local details="${4:-}" + local exit_code="${5:-0}" + + printf '%s | %s | %s | %s | %s | %s\n' \ + "$(blitz_timestamp)" \ + "${step}" \ + "${action}" \ + "${result}" \ + "$(blitz_sanitize_detail "${details}")" \ + "${exit_code}" +} + +blitz_join_cmd() { + local cmd=() + local arg + + for arg in "$@"; do + cmd+=("$(printf '%q' "${arg}")") + done + printf '%s' "${cmd[*]}" +} + +blitz_require_command() { + local command_name="$1" + local step="${2:-precheck}" + + if command -v "${command_name}" >/dev/null 2>&1; then + blitz_log "${step}" "require-command" "success" "command=${command_name}" 0 + return 0 + fi + + blitz_log "${step}" "require-command" "failure" "missing command: ${command_name}" 127 + return 127 +} + +blitz_require_file() { + local path="$1" + local step="${2:-precheck}" + + if [[ -f "${path}" ]]; then + blitz_log "${step}" "require-file" "success" "path=${path}" 0 + return 0 + fi + + blitz_log "${step}" "require-file" "failure" "missing file: ${path}" 1 + return 1 +} + +blitz_require_executable() { + local path="$1" + local step="${2:-precheck}" + + if [[ -x "${path}" ]]; then + blitz_log "${step}" "require-executable" "success" "path=${path}" 0 + return 0 + fi + + blitz_log "${step}" "require-executable" "failure" "missing executable: ${path}" 1 + return 1 +} + +blitz_require_root() { + local step="${1:-precheck}" + + if [[ "${EUID}" -eq 0 ]]; then + blitz_log "${step}" "require-root" "success" "uid=${EUID}" 0 + return 0 + fi + + blitz_log "${step}" "require-root" "failure" "root privileges are required" 1 + return 1 +} + +blitz_run() { + local step="$1" + local action="$2" + local rc + shift 2 + + blitz_log "${step}" "${action}" "start" "$(blitz_join_cmd "$@")" 0 + if "$@"; then + blitz_log "${step}" "${action}" "success" "$(blitz_join_cmd "$@")" 0 + return 0 + else + rc=$? + fi + + blitz_log "${step}" "${action}" "failure" "$(blitz_join_cmd "$@")" "${rc}" + return "${rc}" +} + +blitz_route_ready() { + local target_ip="$1" + local expected_interface="${2:-}" + local route_output + + route_output="$(ip route get "${target_ip}" 2>&1 || true)" + if [[ -z "${route_output}" ]]; then + return 1 + fi + if [[ "${route_output}" == *"unreachable"* || "${route_output}" == *"prohibit"* ]]; then + return 1 + fi + if [[ -n "${expected_interface}" && "${route_output}" != *" dev ${expected_interface} "* && "${route_output}" != *" dev ${expected_interface}" ]]; then + return 1 + fi + + printf '%s\n' "${route_output}" + return 0 +} + +blitz_interface_exists() { + local interface_name="${1:-}" + + if [[ -z "${interface_name}" ]]; then + return 1 + fi + ip link show dev "${interface_name}" >/dev/null 2>&1 +} + +blitz_read_5g_info_interface() { + local info_json="$1" + + if [[ -z "${info_json}" || ! -f "${info_json}" ]]; then + return 1 + fi + + python3 - "${info_json}" <<'PY' +import json +import sys + +path = sys.argv[1] + +try: + with open(path, "r", encoding="utf-8") as handle: + payload = json.load(handle) +except Exception: + raise SystemExit(1) + +interface = str(payload.get("interface") or "").strip() +if not interface: + raise SystemExit(1) + +print(interface) +PY +} + +blitz_detect_5g_interface_from_subnet() { + local modem_subnet="${1:-${BLITZ_5G_MODEM_SUBNET:-}}" + + if [[ -z "${modem_subnet}" ]]; then + return 1 + fi + + python3 - "${modem_subnet}" <<'PY' +import ipaddress +import json +import subprocess +import sys + +subnet = ipaddress.ip_network(sys.argv[1], strict=False) +skip = {"lo", "docker0", "l4tbr0"} + +def priority(name: str) -> tuple[int, str]: + if name.startswith("enx"): + return (0, name) + if name.startswith("wwan"): + return (1, name) + if name.startswith("usb"): + return (2, name) + if name.startswith("eth"): + return (3, name) + return (9, name) + +try: + output = subprocess.check_output(["ip", "-j", "-4", "addr", "show"], text=True) + payload = json.loads(output) +except Exception: + raise SystemExit(1) + +candidates = [] +for item in payload: + ifname = str(item.get("ifname") or "").strip() + if not ifname or ifname in skip: + continue + for addr in item.get("addr_info") or []: + if addr.get("family") != "inet": + continue + local = addr.get("local") + prefixlen = addr.get("prefixlen") + if not local or prefixlen is None: + continue + try: + iface = ipaddress.ip_interface(f"{local}/{prefixlen}") + except ValueError: + continue + if iface.ip in subnet: + candidates.append((priority(ifname), ifname)) + break + +if not candidates: + raise SystemExit(1) + +candidates.sort(key=lambda item: item[0]) +print(candidates[0][1]) +PY +} + +blitz_refresh_5g_info_json() { + local interface_name="$1" + local info_json="${2:-${BLITZ_5G_INFO_JSON:-}}" + + if [[ -z "${interface_name}" || -z "${info_json}" ]]; then + return 1 + fi + + python3 - "${interface_name}" "${info_json}" <<'PY' +import json +import os +import subprocess +import sys + +interface_name = sys.argv[1] +path = sys.argv[2] + +try: + output = subprocess.check_output(["ip", "-j", "addr", "show", "dev", interface_name], text=True) + payload = json.loads(output) +except Exception: + raise SystemExit(1) + +if not payload: + raise SystemExit(1) + +item = payload[0] +ipv4 = [] +ipv6 = [] +for addr in item.get("addr_info") or []: + local = addr.get("local") + prefixlen = addr.get("prefixlen") + family = addr.get("family") + if not local or prefixlen is None: + continue + entry = f"{local}/{prefixlen}" + if family == "inet": + ipv4.append(entry) + elif family == "inet6": + ipv6.append(entry) + +data = { + "interface": interface_name, + "ipv4": ipv4, + "ipv6": ipv6, +} + +parent = os.path.dirname(path) +if parent: + os.makedirs(parent, exist_ok=True) +temp_path = f"{path}.tmp.{os.getpid()}" +with open(temp_path, "w", encoding="utf-8") as handle: + json.dump(data, handle, ensure_ascii=False, indent=2) +os.replace(temp_path, path) +PY +} + +blitz_resolve_5g_interface() { + local explicit_interface="${BLITZ_5G_INTERFACE:-}" + local info_json="${BLITZ_5G_INFO_JSON:-}" + local recorded_interface="" + local detected_interface="" + + if [[ -n "${explicit_interface}" ]]; then + if blitz_interface_exists "${explicit_interface}"; then + printf '%s\n' "${explicit_interface}" + return 0 + fi + return 1 + fi + + recorded_interface="$(blitz_read_5g_info_interface "${info_json}" || true)" + if [[ -n "${recorded_interface}" ]] && blitz_interface_exists "${recorded_interface}"; then + printf '%s\n' "${recorded_interface}" + return 0 + fi + + detected_interface="$(blitz_detect_5g_interface_from_subnet || true)" + if [[ -n "${detected_interface}" ]]; then + if [[ "${detected_interface}" != "${recorded_interface}" ]]; then + blitz_refresh_5g_info_json "${detected_interface}" "${info_json}" >/dev/null 2>&1 || true + fi + printf '%s\n' "${detected_interface}" + return 0 + fi + + return 1 +} + +blitz_prepare_runtime_dir() { + local runtime_dir + + blitz_load_boot_env + runtime_dir="${BLITZ_RUNTIME_DIR}" + + mkdir -p "${runtime_dir}" + if [[ "${EUID}" -eq 0 ]]; then + chown "root:${BLITZ_ROS_USER}" "${runtime_dir}" + chmod 0775 "${runtime_dir}" + else + chmod 0775 "${runtime_dir}" 2>/dev/null || true + fi + blitz_log "runtime-dir" "prepare" "success" "path=${runtime_dir}" 0 +} + +blitz_prepare_run_root() { + local run_root + local run_dir + local incidents_dir + + blitz_load_boot_env + run_root="${BLITZ_RUN_ROOT}" + run_dir="${run_root}/runs" + incidents_dir="${run_root}/incidents" + + mkdir -p "${run_dir}" "${incidents_dir}" + if [[ "${EUID}" -eq 0 ]]; then + chown -R "root:${BLITZ_ROS_USER}" "${run_root}" 2>/dev/null || true + chmod 0775 "${run_root}" "${run_dir}" "${incidents_dir}" 2>/dev/null || true + fi +} + +blitz_load_run_context_env() { + local context_file="${1:-${BLITZ_RUN_CONTEXT_FILE:-}}" + + if [[ -z "${context_file}" || ! -f "${context_file}" ]]; then + return 1 + fi + + set -a + # shellcheck disable=SC1090 + source "${context_file}" + set +a + return 0 +} + +blitz_read_run_id() { + local run_id_file="${BLITZ_RUN_ID_FILE:-}" + + if [[ -z "${run_id_file}" || ! -f "${run_id_file}" ]]; then + return 1 + fi + tr -d '\r\n' < "${run_id_file}" +} + +blitz_utc_compact_timestamp() { + date -u '+%Y%m%dT%H%M%SZ' +} + +blitz_new_run_id() { + printf '%s\n' "$(blitz_utc_compact_timestamp)" +} + +blitz_new_incident_id() { + local prefix="${1:-incident}" + printf '%s-%s-%d\n' "${prefix}" "$(blitz_utc_compact_timestamp)" "$$" +} + +blitz_new_instance_id() { + printf '%s-%d\n' "$(blitz_utc_compact_timestamp)" "$$" +} + +blitz_git_commit() { + git -C "${OMNISOCKETGO_ROOT}" rev-parse HEAD 2>/dev/null || true +} + +blitz_git_dirty_flag() { + if git -C "${OMNISOCKETGO_ROOT}" diff --quiet --ignore-submodules=dirty >/dev/null 2>&1; then + printf '0\n' + return 0 + fi + printf '1\n' +} + +blitz_write_run_context() { + local run_id="$1" + local run_dir="$2" + local boot_id="$3" + local context_file="${BLITZ_RUN_CONTEXT_FILE}" + local id_file="${BLITZ_RUN_ID_FILE}" + local temp_context + local temp_info + local commit_hash + local dirty_flag + local started_at + + commit_hash="$(blitz_git_commit)" + dirty_flag="$(blitz_git_dirty_flag)" + started_at="$(date -u '+%Y-%m-%dT%H:%M:%SZ')" + temp_context="${context_file}.tmp.$$" + temp_info="${run_dir}/run-info.json.tmp.$$" + + mkdir -p "${run_dir}" + printf '%s\n' "${run_id}" > "${id_file}" + + cat > "${temp_context}" </dev/null || echo 0)" + if (( size < max_bytes )); then + return 0 + fi + + for (( index=max_files; index>=1; index-- )); do + if [[ "${index}" -eq "${max_files}" ]]; then + rm -f "${path}.${index}" + fi + if [[ -f "${path}.${index}" ]]; then + mv -f "${path}.${index}" "${path}.$(( index + 1 ))" + fi + done + mv -f "${path}" "${path}.1" +} + +blitz_jsonl_append_line() { + local path="$1" + local line="$2" + + mkdir -p "$(dirname "${path}")" + blitz_jsonl_rotate_if_needed "${path}" + printf '%s\n' "${line}" >> "${path}" +} + +blitz_launch_incident_capture() { + local launch_script="${BOOT_SCRIPT_DIR}/blitz-incident-capture-launch.sh" + + if [[ ! -f "${launch_script}" ]]; then + return 1 + fi + /bin/bash "${launch_script}" "$@" >/dev/null 2>&1 || return 1 +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/disable-systemd.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/disable-systemd.sh new file mode 100644 index 0000000..e2f6601 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/disable-systemd.sh @@ -0,0 +1,53 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="disable" +SYSTEMD_DEST_DIR="/etc/systemd/system" +UNITS=( + "blitz-watchdog.service" + "blitz-5g-link-logger.service" + "blitz-b-side-omnid.service" + "blitz-ros-receiver.service" + "blitz-5g-dial.service" + "blitz-run-context.service" + "blitz-boot-gate.service" + "blitz-robot.target" +) + +stop_unit_if_present() { + local unit_name="$1" + local unit_path="${SYSTEMD_DEST_DIR}/${unit_name}" + + if [[ ! -f "${unit_path}" ]]; then + return 0 + fi + blitz_run "${STEP}" "stop-unit" systemctl stop "${unit_name}" || true +} + +disable_unit_if_present() { + local unit_name="$1" + local unit_path="${SYSTEMD_DEST_DIR}/${unit_name}" + + if [[ ! -f "${unit_path}" ]]; then + return 0 + fi + blitz_run "${STEP}" "disable-unit" systemctl disable "${unit_name}" || true +} + +blitz_load_boot_env +blitz_require_root "${STEP}" +blitz_require_command systemctl "${STEP}" + +for unit_name in "${UNITS[@]}"; do + stop_unit_if_present "${unit_name}" +done + +for unit_name in "${UNITS[@]}"; do + disable_unit_if_present "${unit_name}" +done + +blitz_log "${STEP}" "complete" "success" "boot chain stopped and disabled; next reboot will not auto-start blitz services" 0 diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/install-systemd.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/install-systemd.sh new file mode 100644 index 0000000..00145a7 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/install-systemd.sh @@ -0,0 +1,71 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +SYSTEMD_TEMPLATE_DIR="${SCRIPT_DIR}/systemd" +SYSTEMD_DEST_DIR="/etc/systemd/system" + +render_template() { + local template_path="$1" + local output_path="$2" + + sed \ + -e "s|@OMNISOCKETGO_ROOT@|${OMNISOCKETGO_ROOT}|g" \ + -e "s|@BLITZ_LOG_FILE@|${BLITZ_LOG_FILE}|g" \ + -e "s|@BLITZ_ROS_USER@|${BLITZ_ROS_USER}|g" \ + "${template_path}" > "${output_path}" +} + +install_unit() { + local template_name="$1" + local temp_output + + temp_output="$(mktemp)" + render_template "${SYSTEMD_TEMPLATE_DIR}/${template_name}" "${temp_output}" + install -m 0644 "${temp_output}" "${SYSTEMD_DEST_DIR}/${template_name%.in}" + rm -f "${temp_output}" + blitz_log "install" "install-unit" "success" "unit=${SYSTEMD_DEST_DIR}/${template_name%.in}" 0 +} + +remove_unit_if_present() { + local unit_name="$1" + local unit_path="${SYSTEMD_DEST_DIR}/${unit_name}" + + if [[ ! -f "${unit_path}" ]]; then + return 0 + fi + + systemctl disable --now "${unit_name}" >/dev/null 2>&1 || true + rm -f "${unit_path}" + blitz_log "install" "remove-unit" "success" "unit=${unit_path}" 0 +} + +blitz_load_boot_env +blitz_require_root "install" +blitz_require_command install "install" +blitz_require_command systemctl "install" + +mkdir -p "${SYSTEMD_DEST_DIR}" +install -d -m 0755 "$(dirname "${BLITZ_LOG_FILE}")" +touch "${BLITZ_LOG_FILE}" +chmod 0644 "${BLITZ_LOG_FILE}" +blitz_log "install" "prepare-log-file" "success" "log_file=${BLITZ_LOG_FILE}" 0 +blitz_prepare_runtime_dir +blitz_prepare_run_root + +install_unit "blitz-boot-gate.service.in" +install_unit "blitz-run-context.service.in" +install_unit "blitz-5g-dial.service.in" +install_unit "blitz-5g-link-logger.service.in" +install_unit "blitz-ros-receiver.service.in" +install_unit "blitz-b-side-omnid.service.in" +install_unit "blitz-watchdog.service.in" +install_unit "blitz-robot.target.in" +remove_unit_if_present "blitz-time-sync.service" + +blitz_run "install" "daemon-reload" systemctl daemon-reload +blitz_run "install" "enable-target" systemctl enable blitz-robot.target +blitz_log "install" "complete" "success" "run systemctl start blitz-robot.target to launch immediately" 0 diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/prepare-runtime-dir.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/prepare-runtime-dir.sh new file mode 100644 index 0000000..c2b954a --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/prepare-runtime-dir.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="runtime-dir" + +blitz_load_boot_env +blitz_prepare_runtime_dir +blitz_log "${STEP}" "complete" "success" "runtime_dir=${BLITZ_RUNTIME_DIR}" 0 diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/rndis_dial.py b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/rndis_dial.py new file mode 100644 index 0000000..956b871 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/rndis_dial.py @@ -0,0 +1,854 @@ +#!/usr/bin/env python3 +"""RM520N-GL RNDIS 自动拨号脚本。 + +流程: +1. 检测 USB 设备是否存在 +2. 打开 AT 口并检查 SIM 状态 +3. 配置 RNDIS 模式: AT+QCFG="usbnet",3 +4. 重启模块: AT+CFUN=1,1 +5. 等待模块重新枚举并识别 5G 网卡 +6. 如果网卡还没有 IPv4, 自动尝试 DHCP + +用法: + sudo python3 rndis_dial.py + sudo python3 rndis_dial.py --serial-port /dev/ttyUSB7 + sudo python3 rndis_dial.py --interface eth0 #指定网口 +""" + +from __future__ import annotations + +import argparse +import errno +import ipaddress +import json +import os +import select +import shlex +import shutil +import subprocess +import sys +import termios +import time +import tty + +USB_ID = "2c7c:0801" +DEFAULT_SERIAL_PORT = "/dev/ttyUSB7" #串口设备节点 +DEFAULT_BAUD_RATE = 115200 +CHECK_INTERVAL = 2 +SERIAL_READ_TIMEOUT = 0.2 +SERIAL_POLL_INTERVAL = 0.1 +SERIAL_SETTLE_DELAY = 0.3 +AT_SYNC_RETRIES = 3 +AT_SYNC_TIMEOUT = 2.5 +# 示例地址 192.168.225.38/22 所在网段。 +# 拨号成功后会用这个网段来最终确认哪个接口是 5G 模组。 +DEFAULT_MODEM_SUBNET = "192.168.224.0/22" +DEFAULT_MODEM_GATEWAY = "192.168.225.1" +DEFAULT_PUBLIC_TARGETS = ("81.70.156.140", "106.55.173.235") +DEFAULT_INFO_JSON = "modem_network_info.json" +SKIP_INTERFACES = {"lo", "docker0", "l4tbr0"} +BAUD_RATE_MAP = { + 9600: termios.B9600, + 19200: termios.B19200, + 38400: termios.B38400, + 57600: termios.B57600, + 115200: termios.B115200, +} + + +def run_cmd(cmd, timeout=30, check=False): + print(f"[CMD] {format_shell_cmd(cmd)}") + result = subprocess.run( + cmd, + capture_output=True, + text=True, + timeout=timeout, + check=False, + ) + output = (result.stdout or "") + (result.stderr or "") + if check and result.returncode != 0: + raise RuntimeError(f"命令执行失败: {' '.join(cmd)}\n{output.strip()}") + return result.returncode, output.strip() + + +def format_shell_cmd(cmd): + """把命令参数格式化成可直接阅读的 shell 形式。""" + return " ".join(shlex.quote(part) for part in cmd) + + +def parse_ipv4_address(value): + try: + return str(ipaddress.IPv4Address(value)) + except ipaddress.AddressValueError as exc: + raise argparse.ArgumentTypeError(f"无效的 IPv4 地址: {value}") from exc + + +def dedupe_keep_order(values): + seen = set() + result = [] + for value in values: + if value in seen: + continue + seen.add(value) + result.append(value) + return result + + +def require_root(): + if os.geteuid() != 0: + print("[FAIL] 请使用 sudo 运行此脚本") + sys.exit(1) + + +def require_commands(): + missing = [cmd for cmd in ("lsusb", "ip") if shutil.which(cmd) is None] + if missing: + print(f"[FAIL] 缺少系统命令: {', '.join(missing)}") + sys.exit(1) + + +def usb_device_present(): + # 1. 第一次检测 lsusb，确认模块已经被系统识别。 + """通过 lsusb 检查模块是否已经被系统识别。""" + code, output = run_cmd(["lsusb"], timeout=10) + if code != 0: + return False, output + + for line in output.splitlines(): + if USB_ID in line: + return True, line.strip() + return False, output + + +def wait_for_usb_device(expected_present, timeout): + """等待模块 USB 设备下线或重新上线。""" + deadline = time.time() + timeout + last_seen = "" + while time.time() < deadline: + present, detail = usb_device_present() + last_seen = detail + if present == expected_present: + return True, detail + time.sleep(CHECK_INTERVAL) + return False, last_seen + + +def wait_for_path(path, timeout): + """等待串口节点或其他路径重新出现。""" + deadline = time.time() + timeout + while time.time() < deadline: + if os.path.exists(path): + return True + time.sleep(1) + return False + + +def normalize_serial_output(text): + """整理串口原始输出，便于后续匹配关键字。""" + cleaned = text.replace("\r", "\n") + return "\n".join(line for line in cleaned.splitlines() if line.strip()).strip() + + +def serial_response_complete(text): + if not text: + return False + + for line in reversed(text.splitlines()): + stripped = line.strip() + if stripped == "OK": + return True + if "ERROR" in stripped: + return True + return False + + +class RawSerialSession: + """使用 Python 标准库直接控制 Linux 串口，尽量贴近 stty/raw 行为。""" + + def __init__(self, port, baudrate): + if baudrate not in BAUD_RATE_MAP: + raise RuntimeError(f"不支持的波特率: {baudrate}") + + self.port = port + self.fd = None + self._original_attrs = None + + try: + self.fd = os.open(port, os.O_RDWR | os.O_NOCTTY | os.O_NONBLOCK) + self._original_attrs = termios.tcgetattr(self.fd) + tty.setraw(self.fd, when=termios.TCSANOW) + + attrs = termios.tcgetattr(self.fd) + attrs[0] = 0 + attrs[1] = 0 + attrs[2] &= ~(termios.PARENB | termios.CSTOPB | termios.CSIZE) + attrs[2] |= termios.CS8 | termios.CLOCAL | termios.CREAD + attrs[3] = 0 + attrs[4] = BAUD_RATE_MAP[baudrate] + attrs[5] = BAUD_RATE_MAP[baudrate] + attrs[6][termios.VMIN] = 0 + attrs[6][termios.VTIME] = 0 + termios.tcsetattr(self.fd, termios.TCSANOW, attrs) + termios.tcflush(self.fd, termios.TCIOFLUSH) + except OSError as exc: + self.close() + raise RuntimeError(f"无法打开串口 {port}: {exc}") from exc + + @property + def is_open(self): + return self.fd is not None + + def reset_input_buffer(self): + if self.fd is not None: + termios.tcflush(self.fd, termios.TCIFLUSH) + + def reset_output_buffer(self): + if self.fd is not None: + termios.tcflush(self.fd, termios.TCOFLUSH) + + def write(self, data): + if self.fd is None: + raise OSError("串口未打开") + + sent = 0 + while sent < len(data): + try: + written = os.write(self.fd, data[sent:]) + except BlockingIOError: + time.sleep(SERIAL_POLL_INTERVAL) + continue + if written <= 0: + raise OSError("串口写入返回 0 字节") + sent += written + + def flush(self): + if self.fd is not None: + termios.tcdrain(self.fd) + + def read_chunk(self, timeout, size=4096): + if self.fd is None: + return b"" + + ready, _, _ = select.select([self.fd], [], [], timeout) + if not ready: + return b"" + + try: + return os.read(self.fd, size) + except BlockingIOError: + return b"" + + def close(self): + if self.fd is None: + return + + fd = self.fd + self.fd = None + + if self._original_attrs is not None: + try: + termios.tcsetattr(fd, termios.TCSANOW, self._original_attrs) + except termios.error: + pass + os.close(fd) + + +def read_serial_output(session, timeout, allow_disconnect=False): + """在给定时间窗口内读取 AT 响应，直到出现结束标记或超时。""" + deadline = time.time() + timeout + chunks = [] + saw_terminal_line = False + last_data_time = None + + while time.time() < deadline: + try: + chunk = session.read_chunk(timeout=min(SERIAL_READ_TIMEOUT, max(deadline - time.time(), 0))) + except OSError as exc: + if allow_disconnect and exc.errno in (errno.EIO, errno.ENODEV, errno.EBADF): + break + raise RuntimeError(f"读取串口响应失败: {exc}") from exc + + if chunk: + chunks.append(chunk.decode(errors="ignore")) + last_data_time = time.time() + current_text = normalize_serial_output("".join(chunks)) + if serial_response_complete(current_text): + saw_terminal_line = True + continue + + if saw_terminal_line and last_data_time is not None and time.time() - last_data_time >= SERIAL_SETTLE_DELAY: + break + + time.sleep(SERIAL_POLL_INTERVAL) + + return normalize_serial_output("".join(chunks)) + + +def open_serial_session(port): + """打开 AT 串口会话，后续在同一连接里顺序发送多条命令。""" + ser = RawSerialSession(port=port, baudrate=DEFAULT_BAUD_RATE) + time.sleep(0.2) + ser.reset_input_buffer() + ser.reset_output_buffer() + return ser + + +def execute_serial_step(ser, command, expect=None, timeout=3, allow_disconnect=False): + """在当前串口会话里发送一条 AT 命令并校验响应。""" + print(f"[AT] {command}") + try: + ser.reset_input_buffer() + ser.write((command + "\r").encode()) + ser.flush() + except OSError as exc: + raise RuntimeError(f"AT 命令 `{command}` 发送失败: {exc}") from exc + + response = read_serial_output(ser, timeout=timeout, allow_disconnect=allow_disconnect) + + if response: + print(response) + else: + print("(无响应)") + + if "ERROR" in response: + raise RuntimeError(f"AT 命令 `{command}` 执行失败: {response}") + if expect and expect not in response and not allow_disconnect: + raise RuntimeError(f"AT 命令 `{command}` 响应异常: {response or '空响应'}") + return response + + +def synchronize_at_channel(ser): + """某些模组 AT 口在刚打开时需要先用 AT 做一次预热。""" + last_error = None + + for attempt in range(1, AT_SYNC_RETRIES + 1): + try: + print(f"[INFO] 预热 AT 通道，第 {attempt} 次") + response = execute_serial_step(ser, "AT", expect="OK", timeout=AT_SYNC_TIMEOUT) + if "OK" in response: + return + except RuntimeError as exc: + last_error = exc + time.sleep(0.5) + + if last_error is not None: + raise RuntimeError( + "AT 通道预热失败，请确认串口是否是 AT 命令口，例如 /dev/ttyUSB2" + ) from last_error + raise RuntimeError("AT 通道预热失败") + + +def run_serial_steps(port, steps): + """在同一个串口会话里顺序执行多条 AT 命令。""" + ser = None + + try: + ser = open_serial_session(port) + synchronize_at_channel(ser) + for step in steps: + execute_serial_step( + ser, + step["command"], + expect=step.get("expect"), + timeout=step.get("timeout", 3), + allow_disconnect=step.get("allow_disconnect", False), + ) + finally: + if ser is not None and ser.is_open: + ser.close() + +def configure_rndis(port): + # 2. 用 Python 串口库在同一会话里顺序执行拨号相关 AT 命令。 + """切换到 RNDIS 模式并触发模块重启。""" + if not wait_for_path(port, timeout=30): + raise RuntimeError(f"串口不存在: {port}") + + print(f"[OK] 串口已打开: {port}") + run_serial_steps( + port, + [ + {"command": "AT+CPIN?", "expect": "READY", "timeout": 4}, + {"command": 'AT+QCFG="usbnet",3', "expect": "OK", "timeout": 5}, + {"command": "AT+CFUN=1,1", "timeout": 4, "allow_disconnect": True}, + ], + ) + + +def get_interfaces(): + """列出当前系统中的接口，过滤明显无关的本地接口。""" + interfaces = [] + try: + for name in os.listdir("/sys/class/net"): + if name in SKIP_INTERFACES or is_usb_gadget(name): + continue + interfaces.append(name) + except FileNotFoundError: + return [] + return sorted(interfaces) + + +def is_usb_gadget(iface): + """过滤 Jetson 自己暴露出去的 gadget 网卡。""" + sysfs_path = f"/sys/class/net/{iface}" + if not os.path.exists(sysfs_path): + return False + return "/gadget/" in os.path.realpath(sysfs_path) + + +def is_usb_network_interface(iface): + """判断接口是否来自 USB 设备。""" + device_path = f"/sys/class/net/{iface}/device" + if not os.path.exists(device_path): + return False + real_path = os.path.realpath(device_path) + return "/usb" in real_path + + +def get_ipv4_addrs(): + """返回所有接口的 IPv4/CIDR 信息。""" + code, output = run_cmd(["ip", "-o", "-4", "addr", "show"], timeout=10) + if code != 0: + return {} + + ipv4_addrs = {} + for line in output.splitlines(): + parts = line.split() + if len(parts) >= 4: + iface = parts[1] + ipv4_addrs.setdefault(iface, []).append(parts[3]) + return ipv4_addrs + + +def get_ipv6_addrs(): + """返回所有接口的 IPv6/CIDR 信息。""" + code, output = run_cmd(["ip", "-o", "-6", "addr", "show"], timeout=10) + if code != 0: + return {} + + ipv6_addrs = {} + for line in output.splitlines(): + parts = line.split() + if len(parts) >= 4: + iface = parts[1] + ipv6_addrs.setdefault(iface, []).append(parts[3]) + return ipv6_addrs + + +def interface_priority(iface): + if iface.startswith("wwan"): + return 0 + if iface.startswith("enx"): + return 1 + if iface.startswith("usb"): + return 2 + return 10 + + +def list_usb_network_candidates(explicit_iface=None): + """列出拨号前可尝试的 USB 网卡候选项。 + + 这里不靠固定网口名确认 5G 模组，只是在还没有 IP 的时候先缩小范围。 + 真正确认模组接口，会在 DHCP 之后根据 IP 网段判断。 + """ + candidates = [] + + for iface in get_interfaces(): + if explicit_iface and iface != explicit_iface: + continue + if not is_usb_network_interface(iface): + continue + candidates.append((interface_priority(iface), iface)) + + if not candidates: + return [] + + candidates.sort() + return [iface for _, iface in candidates] + + +def ip_in_subnet(ip_cidr, subnet): + """判断接口地址是否落在指定网段内。""" + try: + return ipaddress.ip_interface(ip_cidr).ip in ipaddress.ip_network(subnet, strict=False) + except ValueError: + return False + + +def find_interface_by_subnet(modem_subnet, explicit_iface=None): + """拨号成功后，通过 IP 网段确认 5G 模组网卡。""" + candidates = [] + for iface, addrs in get_ipv4_addrs().items(): + if iface in SKIP_INTERFACES or is_usb_gadget(iface): + continue + if not is_usb_network_interface(iface): + continue + if explicit_iface and iface != explicit_iface: + continue + + matched_addrs = [addr for addr in addrs if ip_in_subnet(addr, modem_subnet)] + if matched_addrs: + candidates.append((interface_priority(iface), iface, matched_addrs)) + + if not candidates: + return None, [] + + candidates.sort() + _, iface, matched_addrs = candidates[0] + return iface, matched_addrs + + +def wait_for_usb_candidates(explicit_iface=None, timeout=90): + """等待模块枚举出 USB 网卡候选项。""" + deadline = time.time() + timeout + while time.time() < deadline: + candidates = list_usb_network_candidates(explicit_iface=explicit_iface) + if candidates: + return candidates + time.sleep(CHECK_INTERVAL) + return [] + + +def bring_interface_up(iface): + code, output = run_cmd(["ip", "link", "set", "dev", iface, "up"], timeout=10) + if code != 0: + raise RuntimeError(f"拉起网卡失败: {iface}\n{output}") + + +def renew_dhcp(iface): + dhclient = shutil.which("dhclient") + udhcpc = shutil.which("udhcpc") + + if dhclient: + print(f"[INFO] 使用 dhclient 为 {iface} 获取 IP") + code, output = run_cmd(["dhclient", "-1", "-v", iface], timeout=45) + return code == 0, output + + if udhcpc: + print(f"[INFO] 使用 udhcpc 为 {iface} 获取 IP") + code, output = run_cmd(["udhcpc", "-n", "-q", "-i", iface], timeout=45) + return code == 0, output + + return False, "系统中未找到 dhclient 或 udhcpc" + + +def get_default_routes(iface): + code, output = run_cmd(["ip", "-o", "route", "show", "default", "dev", iface], timeout=10) + if code != 0: + return [] + return [line.strip() for line in output.splitlines() if line.strip()] + + +def resolve_gateway(iface, fallback_gateway): + for route in get_default_routes(iface): + tokens = route.split() + for index, token in enumerate(tokens[:-1]): + if token == "via": + gateway = tokens[index + 1] + print(f"[INFO] 从默认路由检测到 {iface} 网关: {gateway}") + return gateway + + print(f"[INFO] 未从默认路由检测到 {iface} 网关，回退到 {fallback_gateway}") + return fallback_gateway + + +def delete_default_routes(iface): + removed = 0 + + while True: + routes = get_default_routes(iface) + if not routes: + return removed + + deleted_this_round = False + for route in routes: + cmd = ["ip", "route", "del", *route.split()] + code, output = run_cmd(cmd, timeout=10) + if code != 0: + code, output = run_cmd(["ip", "route", "del", "default", "dev", iface], timeout=10) + if code != 0: + raise RuntimeError(f"删除默认路由失败: {iface}\n{output}") + removed += 1 + deleted_this_round = True + + if not deleted_this_round: + raise RuntimeError(f"未能删除 {iface} 的默认路由") + + +def install_host_routes(iface, gateway, targets): + for target in dedupe_keep_order(targets): + cmd = ["ip", "route", "replace", f"{target}/32", "via", gateway, "dev", iface] + code, output = run_cmd(cmd, timeout=10) + if code != 0: + raise RuntimeError(f"添加主机路由失败: {target} via {gateway} dev {iface}\n{output}") + + print(f"[OK] 已添加主机路由: {target}/32 via {gateway} dev {iface}") + + +def enforce_route_policy(iface, fallback_gateway, route_targets): + gateway = resolve_gateway(iface, fallback_gateway) + removed = delete_default_routes(iface) + print(f"[OK] 已删除 {iface} 上的 {removed} 条默认路由") + + if route_targets: + install_host_routes(iface, gateway, route_targets) + else: + print(f"[WARN] {iface} 未配置任何主机路由目标，5G 将不再承载公网流量") + + +def ensure_ipv4(iface): + """为指定接口申请 IPv4 地址。""" + ipv4_addrs = get_ipv4_addrs().get(iface, []) + if ipv4_addrs: + return ipv4_addrs + + bring_interface_up(iface) + ok, output = renew_dhcp(iface) + if output: + print(output) + if not ok: + return [] + + return get_ipv4_addrs().get(iface, []) + + +def acquire_modem_interface(modem_subnet, explicit_iface=None): + """通过 DHCP + IP 网段识别真正的模组接口。""" + iface, matched_addrs = find_interface_by_subnet( + modem_subnet, + explicit_iface=explicit_iface, + ) + if iface: + return iface, matched_addrs + + candidates = list_usb_network_candidates(explicit_iface=explicit_iface) + if not candidates: + raise RuntimeError("未找到可尝试 DHCP 的 USB 网卡候选项") + + print(f"[INFO] 当前 USB 网卡候选项: {', '.join(candidates)}") + + for iface in candidates: + print(f"[INFO] 尝试为 {iface} 获取 IPv4") + ensure_ipv4(iface) + + matched_iface, matched_addrs = find_interface_by_subnet( + modem_subnet, + explicit_iface=explicit_iface, + ) + if matched_iface: + return matched_iface, matched_addrs + + return None, [] + + +def print_interface_status(iface): + # 3. 拨号成功后，打印 ip/ifconfig，确认模组网口和地址。 + print(f"[OK] 检测到 5G 网卡: {iface}") + + code, output = run_cmd(["ip", "-4", "addr", "show", "dev", iface], timeout=10) + if code == 0 and output: + print(output) + + if shutil.which("ifconfig"): + code, ifconfig_output = run_cmd(["ifconfig", iface], timeout=10) + if code == 0 and ifconfig_output: + print("\n===== ifconfig =====") + print(ifconfig_output) + + +def save_interface_info(iface, output_file=DEFAULT_INFO_JSON): + """把网口名称、IPv4、IPv6 保存到 JSON 文件。""" + data = { + "interface": iface, + "ipv4": get_ipv4_addrs().get(iface, []), + "ipv6": get_ipv6_addrs().get(iface, []), + } + + with open(output_file, "w", encoding="utf-8") as json_file: + json.dump(data, json_file, ensure_ascii=False, indent=2) + + print(f"[OK] 网口信息已保存到 {output_file}") + + +def ping_target(iface, target, count=3, timeout=15): + """通过指定网口 ping 一个目标。""" + code, output = run_cmd( + ["ping", "-I", iface, "-c", str(count), "-W", "3", target], + timeout=timeout, + ) + return code == 0, output + + +def print_ping_summary(output): + """只打印 ping 的关键结果。""" + for line in output.splitlines(): + if "packets transmitted" in line or "rtt " in line or "Destination " in line: + print(line) + + +def verify_connectivity(iface, gateway=DEFAULT_MODEM_GATEWAY, targets=DEFAULT_PUBLIC_TARGETS, retry_interval=3, max_wait=45): + # 4. 最后先 ping 模组网关，再重试公网连通性。 + """先测模组网关，再轮询公网目标地址。""" + ok, output = ping_target(iface, gateway, count=3, timeout=15) + if ok: + print(f"[OK] {iface} 可到达模组网关 {gateway}") + print_ping_summary(output) + else: + print(f"[WARN] {iface} 无法到达模组网关 {gateway}") + if output: + print(output) + return False + + deadline = time.time() + max_wait + attempt = 1 + while True: + for target in targets: + ok, output = ping_target(iface, target, count=3, timeout=15) + if ok: + print(f"[OK] {iface} 可通过 {target}") + print_ping_summary(output) + return True + + print(f"[WARN] 第 {attempt} 次 Ping {target} 失败") + if output: + print_ping_summary(output) + + if time.time() >= deadline: + print(f"[WARN] {iface} 在 {max_wait} 秒内仍无法连通 {', '.join(targets)}") + return False + + attempt += 1 + time.sleep(retry_interval) + + +def ping_via_interface(iface, targets=DEFAULT_PUBLIC_TARGETS): + """保留原调用点，内部走完整连通性检查。""" + return verify_connectivity(iface, targets=targets) + + +def parse_args(): + parser = argparse.ArgumentParser(description="RM520N-GL RNDIS 自动拨号脚本") + parser.add_argument( + "--serial-port", + default=DEFAULT_SERIAL_PORT, + help=f"AT 串口路径，默认 {DEFAULT_SERIAL_PORT}", + ) + parser.add_argument( + "--interface", + help="指定期望的 5G 网卡名，例如 eth0", + ) + parser.add_argument( + "--modem-subnet", + default=DEFAULT_MODEM_SUBNET, + help=f"拨号成功后用于识别模组接口的 IPv4 网段，默认 {DEFAULT_MODEM_SUBNET}", + ) + parser.add_argument( + "--gateway", + type=parse_ipv4_address, + default=DEFAULT_MODEM_GATEWAY, + help=f"5G 模组网关地址，默认 {DEFAULT_MODEM_GATEWAY}", + ) + parser.add_argument( + "--skip-dhcp", + action="store_true", + help="只等待 USB 网卡出现，不主动申请 IPv4", + ) + parser.add_argument( + "--remove-default-route", + action="store_true", + help="拨号成功后删除 5G 接口上的默认路由，只保留显式主机路由", + ) + parser.add_argument( + "--route-target", + action="append", + default=[], + type=parse_ipv4_address, + help="拨号完成后通过 5G 接口保留的 IPv4 主机路由目标，可重复传入", + ) + return parser.parse_args() + + +def main(): + args = parse_args() + require_root() + require_commands() + + print("===== RM520N-GL RNDIS 自动拨号 =====") + print(f"[INFO] 目标模组网段: {args.modem_subnet}") + + #1.检测 lsusb，确认是否识别到模块 + present, detail = usb_device_present() + if not present: + print(f"[FAIL] 未检测到模块 USB 设备 {USB_ID}") + if detail: + print(detail) + sys.exit(1) + + print(f"[OK] 检测到 USB 设备: {detail}") + print(f"[INFO] 使用 AT 口: {args.serial_port}") + + #2.进行 Python 串口拨号 + try: + configure_rndis(args.serial_port) + + print("[INFO] 已发送 AT+CFUN=1,1，等待模块重启") + disappeared, _ = wait_for_usb_device(expected_present=False, timeout=25) + if disappeared: + print("[OK] 模块已下线，继续等待重新枚举") + else: + print("[WARN] 未观察到模块下线，继续等待重新枚举") + + reappeared, detail = wait_for_usb_device(expected_present=True, timeout=90) + if not reappeared: + print(f"[FAIL] 模块重启后未重新枚举: {USB_ID}") + sys.exit(1) + + print(f"[OK] 模块已重新枚举: {detail}") + + candidates = wait_for_usb_candidates(explicit_iface=args.interface, timeout=90) + if not candidates: + print("[FAIL] 未检测到 5G 模组枚举出的 USB 网卡") + sys.exit(1) + + if args.skip_dhcp: + print(f"[INFO] 当前 USB 网卡候选项: {', '.join(candidates)}") + iface, ipv4_addrs = find_interface_by_subnet( + args.modem_subnet, + explicit_iface=args.interface, + ) + if not iface: + print(f"[WARN] 当前还没有接口拿到目标网段 {args.modem_subnet} 的地址") + sys.exit(1) + else: + iface, ipv4_addrs = acquire_modem_interface( + args.modem_subnet, + explicit_iface=args.interface, + ) + if not iface: + print(f"[FAIL] 未找到落在目标网段 {args.modem_subnet} 内的模组接口") + sys.exit(1) + + print_interface_status(iface) + + if ipv4_addrs: + for addr in ipv4_addrs: + print(f"[OK] {iface} 已获取 IPv4: {addr}") + save_interface_info(iface) + route_targets = dedupe_keep_order(args.route_target) + if args.remove_default_route: + enforce_route_policy(iface, args.gateway, route_targets) + + connectivity_targets = route_targets or list(DEFAULT_PUBLIC_TARGETS) + ping_via_interface(iface, targets=connectivity_targets) + print(f"[DONE] RNDIS 拨号完成，可执行: sudo python3 speed_test.py {iface}") + return + + print(f"[WARN] {iface} 已出现，但还没有 IPv4 地址") + print(f"[INFO] 可手动检查: ip addr show {iface}") + sys.exit(1) + except (RuntimeError, subprocess.TimeoutExpired) as exc: + print(f"[FAIL] {exc}") + sys.exit(1) + + +if __name__ == "__main__": + main() diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/robot-boot.env b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/robot-boot.env new file mode 100644 index 0000000..152a737 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/robot-boot.env @@ -0,0 +1,60 @@ +# Boot-time settings for the robot-side autostart chain. +# Override machine-specific values in robot-boot.env.local. + +BLITZ_BOOT_DELAY_SEC="30" +BLITZ_RUN_ROOT="/var/log/blitz-robot" +BLITZ_LOG_FILE="/var/log/blitz-robot/startup.log" +BLITZ_RUNTIME_DIR="/run/blitz-robot" +BLITZ_RUN_CONTEXT_FILE="${BLITZ_RUNTIME_DIR}/run-context.env" +BLITZ_RUN_ID_FILE="${BLITZ_RUNTIME_DIR}/run-id" +BLITZ_CURRENT_RUN_LINK="${BLITZ_RUN_ROOT}/current" + +BLITZ_5G_DIAL_DIR="${OMNISOCKETGO_ROOT}/scripts/boot" +BLITZ_5G_SERIAL_PORT="/dev/ttyUSB2" +BLITZ_5G_INTERFACE="" +BLITZ_5G_MODEM_SUBNET="192.168.224.0/22" +BLITZ_5G_GATEWAY="192.168.225.1" +BLITZ_5G_SKIP_DHCP="0" +BLITZ_5G_REMOVE_DEFAULT_ROUTE="1" +BLITZ_5G_ROUTE_TARGETS="106.55.173.235" +BLITZ_5G_INFO_JSON="${OMNISOCKETGO_ROOT}/scripts/boot/modem_network_info.json" +BLITZ_5G_SERIAL_WAIT_SEC="60" +BLITZ_5G_ROUTE_WAIT_SEC="30" + +# Leave empty to fall back to the host part of ROBOT_SIDE_OMNISOCKET_SERVER_ADDR. +BLITZ_TIME_SERVER_IP="81.70.156.140" + +BLITZ_ROS_USER="nvidia" +BLITZ_ROS_SOCKET_WAIT_SEC="20" +BLITZ_WATCHDOG_INTERVAL_SEC="5" +BLITZ_HEALTH_STALE_SEC="15" +BLITZ_OMNID_THREAD_HEARTBEAT_TIMEOUT_SEC="15" +BLITZ_KCP_STATS_INTERVAL_MS="1000" +BLITZ_CONTROL_LATENCY_LOG_ENABLED="1" +BLITZ_CONTROL_LATENCY_LOG_SAMPLE_MOD="100" +BLITZ_CONTROL_ACK_SAMPLE_MOD="10" +BLITZ_VIDEO_STAGE_LOG_ENABLED="1" +BLITZ_VIDEO_STAGE_LOG_SAMPLE_MOD="10" +BLITZ_5G_LINK_LOG_INTERVAL_SEC="5" +BLITZ_JSONL_FLUSH_INTERVAL_MS="1000" +BLITZ_JSONL_FLUSH_BYTES="262144" +BLITZ_JSONL_ROTATE_BYTES="134217728" +BLITZ_JSONL_ROTATE_FILES="8" +# Log one normal relay packet out of every N packets. Drop events still log immediately. +OMNI_RELAY_PACKET_LOG_SAMPLE_EVERY="200" +BLITZ_INCIDENT_COMMAND_TIMEOUT_SEC="5" +BLITZ_INCIDENT_TOTAL_TIMEOUT_SEC="30" +BLITZ_NETWORK_FAIL_THRESHOLD="3" +BLITZ_NETWORK_RECOVERY_COOLDOWN_SEC="30" +BLITZ_GPS_MONITOR_ENABLED="1" +BLITZ_GPS_DEVICE_GLOB="/dev/ttyCH341USB*" +BLITZ_GPS_CHECK_INTERVAL_SEC="10" +BLITZ_GPS_RESTART_UNITS="gpsd.socket gpsd.service" +BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION="0" + +OMNI_CAMERA_DEVICE="/dev/v4l/by-path/platform-a80aa10000.usb-usb-0:3.2:1.4-video-index0" + +# Boot units run b_side_omnid as root directly, so nested sudo must stay off. +B_SIDE_OMNID_USE_SUDO="0" +OMNI_CONTROL_ACK_PEER_ID="peer-b-ctrl-ack" +OMNI_CONTROL_ACK_TARGET_PEER="peer-a-ctrl-ack" diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/start-5g-link-logger-service.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/start-5g-link-logger-service.sh new file mode 100644 index 0000000..ea2c051 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/start-5g-link-logger-service.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}/common.sh" + +STEP="5g-link-logger-service" + +blitz_load_boot_env +blitz_require_run_context + +export OMNI_BOOT_MODE="1" +export BLITZ_INSTANCE_ID="${BLITZ_INSTANCE_ID:-$(blitz_new_instance_id)}" +export BLITZ_5G_LINK_LOG_PATH="${BLITZ_5G_LINK_LOG_PATH:-${BLITZ_RUN_DIR}/b-5g-link-quality.${BLITZ_INSTANCE_ID}.jsonl}" + +blitz_log "${STEP}" "start" "start" "exec bash ${OMNISOCKETGO_ROOT}/scripts/boot/blitz-5g-link-logger.sh" 0 +exec bash "${OMNISOCKETGO_ROOT}/scripts/boot/blitz-5g-link-logger.sh" diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/start-b-side-omnid-service.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/start-b-side-omnid-service.sh new file mode 100644 index 0000000..53eea06 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/start-b-side-omnid-service.sh @@ -0,0 +1,19 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="b-side-omnid" + +blitz_load_boot_env +blitz_require_run_context + +blitz_require_executable "${OMNISOCKETGO_ROOT}/bin/b_side_omnid" "${STEP}" + +export OMNI_BOOT_MODE="1" +export B_SIDE_OMNID_USE_SUDO="0" + +blitz_log "${STEP}" "start" "start" "exec bash ${OMNISOCKETGO_ROOT}/scripts/dev/start-b-side-omnid.sh" 0 +exec bash "${OMNISOCKETGO_ROOT}/scripts/dev/start-b-side-omnid.sh" diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/start-ros-receiver-service.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/start-ros-receiver-service.sh new file mode 100644 index 0000000..8bea80a --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/start-ros-receiver-service.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}/common.sh" + +STEP="ros-receiver" + +blitz_load_boot_env +blitz_require_run_context + +blitz_require_file "/opt/ros/${ROS_DISTRO}/setup.bash" "${STEP}" +blitz_require_file "${ROS_CONTROL_PY_DIR}/install/setup.bash" "${STEP}" + +export OMNI_BOOT_MODE="1" +blitz_log "${STEP}" "start" "start" "exec bash ${OMNISOCKETGO_ROOT}/scripts/dev/start-ros-receiver.sh" 0 +exec bash "${OMNISOCKETGO_ROOT}/scripts/dev/start-ros-receiver.sh" diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-5g-dial.service.in b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-5g-dial.service.in new file mode 100644 index 0000000..02a5c64 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-5g-dial.service.in @@ -0,0 +1,15 @@ +[Unit] +Description=Blitz robot 5G dial +PartOf=blitz-robot.target +After=blitz-run-context.service +Requires=blitz-run-context.service + +[Service] +Type=oneshot +RemainAfterExit=yes +ExecStart=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/5g-dial.sh +StandardOutput=append:@BLITZ_LOG_FILE@ +StandardError=append:@BLITZ_LOG_FILE@ + +[Install] +WantedBy=blitz-robot.target diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-5g-link-logger.service.in b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-5g-link-logger.service.in new file mode 100644 index 0000000..81b810b --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-5g-link-logger.service.in @@ -0,0 +1,19 @@ +[Unit] +Description=Blitz robot 5G link logger +PartOf=blitz-robot.target +After=blitz-run-context.service blitz-5g-dial.service +Requires=blitz-run-context.service +Wants=blitz-run-context.service blitz-5g-dial.service + +[Service] +Type=simple +EnvironmentFile=-/run/blitz-robot/run-context.env +ExecStartPre=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/prepare-runtime-dir.sh +ExecStart=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/start-5g-link-logger-service.sh +Restart=always +RestartSec=5 +StandardOutput=append:@BLITZ_LOG_FILE@ +StandardError=append:@BLITZ_LOG_FILE@ + +[Install] +WantedBy=blitz-robot.target diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-b-side-omnid.service.in b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-b-side-omnid.service.in new file mode 100644 index 0000000..bce9b11 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-b-side-omnid.service.in @@ -0,0 +1,20 @@ +[Unit] +Description=Blitz robot b-side omnid +PartOf=blitz-robot.target +After=blitz-run-context.service blitz-5g-dial.service blitz-ros-receiver.service +Requires=blitz-run-context.service +Wants=blitz-run-context.service blitz-5g-dial.service blitz-ros-receiver.service + +[Service] +Type=simple +EnvironmentFile=-/run/blitz-robot/run-context.env +ExecStartPre=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/prepare-runtime-dir.sh +ExecStart=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/start-b-side-omnid-service.sh +ExecStopPost=/bin/bash -lc 'if [[ "${SERVICE_RESULT:-success}" != "success" ]]; then exec /bin/bash "@OMNISOCKETGO_ROOT@/scripts/boot/blitz-incident-capture-launch.sh" --source exec-stop-post --unit "%n" --result "${SERVICE_RESULT:-}" --exit-status "${EXIT_STATUS:-}" --reason b-side-service-exit; fi' +Restart=always +RestartSec=2 +StandardOutput=append:@BLITZ_LOG_FILE@ +StandardError=append:@BLITZ_LOG_FILE@ + +[Install] +WantedBy=blitz-robot.target diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-boot-gate.service.in b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-boot-gate.service.in new file mode 100644 index 0000000..5f918ef --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-boot-gate.service.in @@ -0,0 +1,15 @@ +[Unit] +Description=Blitz robot boot gate +PartOf=blitz-robot.target +After=multi-user.target network-online.target +Wants=network-online.target + +[Service] +Type=oneshot +RemainAfterExit=yes +ExecStart=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/boot-gate.sh +StandardOutput=append:@BLITZ_LOG_FILE@ +StandardError=append:@BLITZ_LOG_FILE@ + +[Install] +WantedBy=blitz-robot.target diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-robot.target.in b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-robot.target.in new file mode 100644 index 0000000..7590c67 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-robot.target.in @@ -0,0 +1,13 @@ +[Unit] +Description=Blitz robot boot chain +Wants=blitz-boot-gate.service +Wants=blitz-run-context.service +Wants=blitz-5g-dial.service +Wants=blitz-5g-link-logger.service +Wants=blitz-ros-receiver.service +Wants=blitz-b-side-omnid.service +Wants=blitz-watchdog.service +After=multi-user.target + +[Install] +WantedBy=multi-user.target diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-ros-receiver.service.in b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-ros-receiver.service.in new file mode 100644 index 0000000..634b19b --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-ros-receiver.service.in @@ -0,0 +1,23 @@ +[Unit] +Description=Blitz robot ROS receiver +PartOf=blitz-robot.target +After=blitz-run-context.service blitz-5g-dial.service +Requires=blitz-run-context.service +Wants=blitz-run-context.service blitz-5g-dial.service + +[Service] +Type=simple +User=@BLITZ_ROS_USER@ +PermissionsStartOnly=true +EnvironmentFile=-/run/blitz-robot/run-context.env +ExecStartPre=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/prepare-runtime-dir.sh +ExecStart=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/start-ros-receiver-service.sh +ExecStartPost=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/wait-for-unix-socket.sh --step ros-receiver +ExecStopPost=/bin/bash -lc 'if [[ "${SERVICE_RESULT:-success}" != "success" ]]; then exec /bin/bash "@OMNISOCKETGO_ROOT@/scripts/boot/blitz-incident-capture-launch.sh" --source exec-stop-post --unit "%n" --result "${SERVICE_RESULT:-}" --exit-status "${EXIT_STATUS:-}" --reason ros-service-exit; fi' +Restart=always +RestartSec=2 +StandardOutput=append:@BLITZ_LOG_FILE@ +StandardError=append:@BLITZ_LOG_FILE@ + +[Install] +WantedBy=blitz-robot.target diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-run-context.service.in b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-run-context.service.in new file mode 100644 index 0000000..2ace077 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-run-context.service.in @@ -0,0 +1,15 @@ +[Unit] +Description=Blitz robot run context +PartOf=blitz-robot.target +After=blitz-boot-gate.service +Requires=blitz-boot-gate.service + +[Service] +Type=oneshot +RemainAfterExit=yes +ExecStart=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/blitz-run-context.sh +StandardOutput=append:@BLITZ_LOG_FILE@ +StandardError=append:@BLITZ_LOG_FILE@ + +[Install] +WantedBy=blitz-robot.target diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-watchdog.service.in b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-watchdog.service.in new file mode 100644 index 0000000..882d5b7 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/systemd/blitz-watchdog.service.in @@ -0,0 +1,19 @@ +[Unit] +Description=Blitz robot health watchdog +PartOf=blitz-robot.target +After=blitz-run-context.service blitz-b-side-omnid.service blitz-ros-receiver.service +Requires=blitz-run-context.service +Wants=blitz-run-context.service blitz-b-side-omnid.service blitz-ros-receiver.service + +[Service] +Type=simple +EnvironmentFile=-/run/blitz-robot/run-context.env +ExecStartPre=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/prepare-runtime-dir.sh +ExecStart=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/blitz-watchdog.sh +Restart=always +RestartSec=5 +StandardOutput=append:@BLITZ_LOG_FILE@ +StandardError=append:@BLITZ_LOG_FILE@ + +[Install] +WantedBy=blitz-robot.target diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/wait-for-unix-socket.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/wait-for-unix-socket.sh new file mode 100644 index 0000000..2d4d411 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/boot/wait-for-unix-socket.sh @@ -0,0 +1,49 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="ros-receiver" +SOCKET_PATH="" +TIMEOUT_SEC="" + +while [[ $# -gt 0 ]]; do + case "$1" in + --path) + SOCKET_PATH="$2" + shift 2 + ;; + --timeout) + TIMEOUT_SEC="$2" + shift 2 + ;; + --step) + STEP="$2" + shift 2 + ;; + *) + blitz_log "${STEP}" "wait-socket-arg" "failure" "unknown argument: $1" 2 + exit 2 + ;; + esac +done + +blitz_load_boot_env + +SOCKET_PATH="${SOCKET_PATH:-${ROBOT_RECEIVER_LOCAL_SOCKET_PATH}}" +TIMEOUT_SEC="${TIMEOUT_SEC:-${BLITZ_ROS_SOCKET_WAIT_SEC}}" + +blitz_log "${STEP}" "wait-socket" "start" "path=${SOCKET_PATH} timeout_sec=${TIMEOUT_SEC}" 0 + +for (( waited=0; waited< TIMEOUT_SEC; waited++ )); do + if [[ -S "${SOCKET_PATH}" ]]; then + blitz_log "${STEP}" "wait-socket" "success" "path=${SOCKET_PATH} waited_sec=${waited}" 0 + exit 0 + fi + sleep 1 +done + +blitz_log "${STEP}" "wait-socket" "failure" "path=${SOCKET_PATH} timeout_sec=${TIMEOUT_SEC}" 1 +exit 1 diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/README.md b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/README.md new file mode 100644 index 0000000..48c27f7 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/README.md @@ -0,0 +1,194 @@ +# 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`. +`start-backend.sh` and `start-frontend.sh` need that repo; `start-ros-receiver.sh` and `start-b-side-omnid.sh` do not. + +## 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 +- `prepare-camera-device.sh`: reports V4L2 owners; it never stops a service in this project +- `apply-camera-controls.sh`: applies the camera preset before `b_side_omnid` starts +- `start-backend.sh`: starts Django ASGI with `uvicorn` +- `log-network-summary.py`: polls the backend `network/latest` API and appends compact JSONL snapshots +- `start-frontend.sh`: starts the Vite dev server +- `start-ros-receiver.sh`: starts the ROS2 `udp_teleop_bridge` receiver +- `start-b-side-omnid.sh`: applies camera controls, then 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 +``` + +When you launch via `start-*.sh`, you do not need to manually `export` the variables from +`robot-remote.env` or `robot-remote.env.local`. `load-env.sh` loads those files with `set -a`, +so the variables are exported automatically for the child process. Manual `export` is only needed +if you bypass these scripts and start binaries directly from a clean shell. + +## 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_CAMERA_HEAD_DEVICE` and `OMNI_CAMERA_WAIST_DEVICE` select the two warm camera inputs used by `b_side_omnid` +- `OMNI_CAMERA_ACTIVE=head|waist` selects the camera sent at startup + +`b_side_omnid` keeps both configured cameras streaming and only decodes/encodes/sends the selected input. Send a text control message with body `camera:head` or `camera:waist` to `peer-b-ctrl` to switch without reopening either camera. The normal fixed-size binary robot control packets are unchanged. +- `OMNI_CAMERA_OCCUPANCY_POLICY` +- `OMNI_CAMERA_RELEASE_SERVICE` +- `OMNI_CAMERA_PROFILE` +- `OMNI_CAMERA_BRIGHTNESS` +- `OMNI_CAMERA_CUSTOM_CTRL` +- `OMNI_CAMERA_VERIFY` +- `OMNI_VIDEO_PEER_ID` +- `OMNI_CONTROL_PEER_ID` +- `OMNI_VIDEO_SOFT_BACKPRESSURE_SEGMENTS` +- `OMNI_VIDEO_HARD_BACKPRESSURE_SEGMENTS` +- `OMNI_VIDEO_HARD_BACKPRESSURE_HOLD_MS` +- `OMNI_CONTROL_SERVER_IDLE_RECONNECT_MS` +- `OMNI_VIDEO_MAX_FRAME_AGE_MS` +- `OMNISOCKET_TELEMETRY_PEER_ID` +- `OMNISOCKET_TELEMETRY_INTERVAL_MS` +- `OMNISOCKET_TELEMETRY_STALE_AFTER_MS` +- `OMNI_NETWORK_SUMMARY_LOG_ENABLED` +- `OMNI_NETWORK_SUMMARY_LOG_PATH` +- `OMNI_NETWORK_SUMMARY_LOG_INTERVAL_MS` + +Camera presets use `v4l2-ctl` from `v4l-utils` on the robot side. + +Role mapping: + +- `start-backend.sh` uses the `CONTROL_SIDE_*` address pair +- `start-b-side-omnid.sh` uses the `ROBOT_SIDE_*` address pair +- `start-b-side-omnid.sh` also applies the `OMNI_CAMERA_*` preset before the daemon opens the camera +- `start-b-side-omnid.sh` checks ROS2 topics/services in ROS2 mode; legacy V4L2 mode runs a non-destructive preflight +- `start-ros-receiver.sh` defaults to the robot-side address pair, but with `transport=unix_dgram` it usually does not need the server address + +New repair knobs: + +- `OMNI_VIDEO_SOFT_BACKPRESSURE_SEGMENTS`, `OMNI_VIDEO_HARD_BACKPRESSURE_SEGMENTS`, and `OMNI_VIDEO_HARD_BACKPRESSURE_HOLD_MS` are used by `b_side_omnid` +- `OMNI_CONTROL_SERVER_IDLE_RECONNECT_MS` is used by `b_side_omnid` +- `OMNI_VIDEO_MAX_FRAME_AGE_MS` is used by `start-backend.sh` on the A-side backend, not by `b_side_omnid` +- `OMNISOCKET_TELEMETRY_INTERVAL_MS` and `OMNISOCKET_TELEMETRY_STALE_AFTER_MS` tune the backend's D-side telemetry freshness window +- `OMNI_NETWORK_SUMMARY_LOG_*` controls the A-side JSONL summary logger that polls `GET /api/network/latest/` + +Default long-run network logging: + +- A-side starts a compact JSONL logger by default at `${OMNISOCKETGO_ROOT}/logs/a-network-summary.jsonl` +- The default A-side polling interval is `2000 ms` +- For D-side long runs, prefer: + +```bash +./bin/kcpserver -listen 0.0.0.0:10909 \ + -telemetry-peer peer-a-telemetry \ + -telemetry-interval 1000ms \ + -kcp-session-stats-log logs/d-kcp-stats.jsonl \ + -kcp-session-stats-interval 1000ms +``` + +- Keep `-latency-log` and `-kcp-ts-debug-log` off by default for multi-hour runs +- Do not continuously redirect relay `C` stderr to a file unless you are reproducing a short issue window + +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" +OMNI_NETWORK_SUMMARY_LOG_INTERVAL_MS="5000" +``` + +Camera occupancy handling is deliberately non-destructive in this project. `check` only reports +owners and fails if the device is busy. The old `release-known` policy is removed and cannot stop +any service. In ROS2 mode use `ensure-ros-camera-services.sh`; it checks `proc_manager.service` +and starts the two Orbbec services only when the expected topics are absent and both camera +devices are free: + +```bash +OMNI_CAMERA_DEVICE="/dev/video18" +OMNI_CAMERA_SOURCE="ros2" +OMNI_PROC_MANAGER_SERVICE="proc_manager.service" +OMNI_CAMERA_START_SERVICES="1" +``` + +Run the preflight without starting the daemon: + +```bash +bash scripts/dev/prepare-camera-device.sh +``` + +If a remaining owner belongs to `proc_manager.service`, inspect it with `ros2 component list` and +unload only the camera component. Stopping the complete process manager can interrupt unrelated +robot functions. + +Default camera behavior is the `night` preset: + +```bash +OMNI_CAMERA_PROFILE="night" +# Optional per-machine tweak: +OMNI_CAMERA_BRIGHTNESS="8" +``` + +To switch to a daytime preset with brightness only: + +```bash +OMNI_CAMERA_PROFILE="day" +OMNI_CAMERA_BRIGHTNESS="8" +``` + +To send the raw `v4l2-ctl --set-ctrl=...` payload yourself: + +```bash +OMNI_CAMERA_PROFILE="custom" +OMNI_CAMERA_CUSTOM_CTRL="brightness=8,auto_exposure=1,exposure_time_absolute=800,gain=64" +OMNI_CAMERA_VERIFY="1" +``` +# ROS2 相机模式说明 + +在 `OmniSocketGo_robot_ros` 中，默认 `OMNI_CAMERA_SOURCE=ros2`。此模式不调用 +`prepare-camera-device.sh`，不会停止 `orbbec_head.service`、 +`orbbec_waist.service`，也不会直接打开 `/dev/video*`。相机服务检查和 +启动回退由 `ensure-ros-camera-services.sh` 完成。机器人当前 setuptools 版本下请使用 +`colcon build`（不要加 `--symlink-install`）。 diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/aggregate-latency-estimates.py b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/aggregate-latency-estimates.py new file mode 100644 index 0000000..e490d3c --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/aggregate-latency-estimates.py @@ -0,0 +1,292 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import argparse +from datetime import datetime, timezone +import html +import json +from pathlib import Path +from typing import Any + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="Aggregate run logs into control/video latency estimate outputs.") + parser.add_argument("--run-dir", required=True, help="Run directory containing JSONL logs.") + parser.add_argument("--output-dir", help="Output directory. Defaults to --run-dir.") + return parser.parse_args() + + +def iter_jsonl(path: Path) -> list[dict[str, Any]]: + records: list[dict[str, Any]] = [] + if not path.exists(): + return records + with path.open("r", encoding="utf-8") as handle: + for raw_line in handle: + line = raw_line.strip() + if not line: + continue + try: + payload = json.loads(line) + except json.JSONDecodeError: + continue + if isinstance(payload, dict): + records.append(payload) + return records + + +def load_glob_jsonl(run_dir: Path, pattern: str) -> list[dict[str, Any]]: + records: list[dict[str, Any]] = [] + for path in sorted(run_dir.glob(pattern)): + records.extend(iter_jsonl(path)) + return records + + +def write_jsonl(path: Path, records: list[dict[str, Any]]) -> None: + with path.open("w", encoding="utf-8") as handle: + for record in records: + handle.write(json.dumps(record, ensure_ascii=False, separators=(",", ":"))) + handle.write("\n") + + +def parse_unix_ms(value: Any) -> int | None: + if value is None: + return None + if isinstance(value, (int, float)): + return int(value) + text = str(value).strip() + if not text: + return None + if text.endswith("Z"): + text = f"{text[:-1]}+00:00" + try: + return int(datetime.fromisoformat(text).astimezone(timezone.utc).timestamp() * 1000) + except ValueError: + return None + + +def flatten_net_epoch(samples: list[dict[str, Any]]) -> list[dict[str, Any]]: + flattened: list[dict[str, Any]] = [] + for sample in samples: + links = sample.get("links") or {} + a_to_d = (links.get("a_to_d") or {}).get("sessions") or {} + d_to_b = (links.get("d_to_b") or {}).get("sessions") or {} + a_control = (a_to_d.get("control") or {}).get("kcp") or {} + d_control = (d_to_b.get("control") or {}).get("kcp") or {} + a_video = (a_to_d.get("video") or {}).get("kcp") or {} + d_video = (d_to_b.get("video") or {}).get("kcp") or {} + flattened.append( + { + "updated_at": sample.get("updated_at"), + "a_to_d_control_srtt_ms": a_control.get("srtt_ms"), + "a_to_d_control_min_srtt_ms": a_control.get("min_srtt_ms"), + "d_to_b_control_srtt_ms": d_control.get("srtt_ms"), + "d_to_b_control_min_srtt_ms": d_control.get("min_srtt_ms"), + "a_to_d_video_srtt_ms": a_video.get("srtt_ms"), + "a_to_d_video_min_srtt_ms": a_video.get("min_srtt_ms"), + "d_to_b_video_srtt_ms": d_video.get("srtt_ms"), + "d_to_b_video_min_srtt_ms": d_video.get("min_srtt_ms"), + "a_to_d_control_feedback_age_ms": a_control.get("last_feedback_age_ms"), + "d_to_b_control_feedback_age_ms": d_control.get("last_feedback_age_ms"), + "a_to_d_video_feedback_age_ms": a_video.get("last_feedback_age_ms"), + "d_to_b_video_feedback_age_ms": d_video.get("last_feedback_age_ms"), + "a_to_d_control_retrans_delta": ((a_to_d.get("control") or {}).get("trend") or {}).get("retrans_delta"), + "d_to_b_control_retrans_delta": ((d_to_b.get("control") or {}).get("trend") or {}).get("retrans_delta"), + "a_to_d_video_retrans_delta": ((a_to_d.get("video") or {}).get("trend") or {}).get("retrans_delta"), + "d_to_b_video_retrans_delta": ((d_to_b.get("video") or {}).get("trend") or {}).get("retrans_delta"), + "a_to_d_video_window_pressure_pct": a_video.get("window_pressure_pct"), + "d_to_b_video_window_pressure_pct": d_video.get("window_pressure_pct"), + "robot_health": sample.get("robot_health"), + } + ) + return flattened + + +def aggregate_control_estimates( + network_samples: list[dict[str, Any]], + control_events: list[dict[str, Any]], + control_acks: list[dict[str, Any]], +) -> list[dict[str, Any]]: + if control_acks: + return control_acks + + fallback: list[dict[str, Any]] = [] + for sample in network_samples: + estimate = sample.get("latency_estimate") or {} + fallback.append( + { + "updated_at": sample.get("updated_at"), + "estimate_method": "srtt_fallback", + "control_loop_rtt_ms": estimate.get("control_loop_rtt_ms"), + "control_to_persist_est_ms": estimate.get("control_to_persist_est_ms"), + "control_oneway_srtt_est_ms": estimate.get("control_oneway_srtt_est_ms"), + "control_oneway_bestcase_est_ms": estimate.get("control_oneway_bestcase_est_ms"), + "source_event_count": len(control_events), + } + ) + return fallback + + +def aggregate_video_estimates( + network_samples: list[dict[str, Any]], + frame_recv_records: list[dict[str, Any]], + display_probe_records: list[dict[str, Any]], +) -> list[dict[str, Any]]: + network_timeline = sorted( + ( + (updated_at_ms, sample.get("latency_estimate") or {}) + for sample in network_samples + for updated_at_ms in [parse_unix_ms(sample.get("updated_at"))] + if updated_at_ms is not None + ), + key=lambda item: item[0], + ) + probes_by_seq = { + int(record["frame_seq"]): record + for record in display_probe_records + if record.get("frame_seq") is not None + } + estimates: list[dict[str, Any]] = [] + timeline_index = 0 + + for record in frame_recv_records: + frame_seq = record.get("frame_seq") + if frame_seq is None: + continue + probe = probes_by_seq.get(int(frame_seq)) + backend_received_unix_ns = record.get("backend_received_unix_ns") + backend_received_unix_ms = None + try: + if backend_received_unix_ns is not None: + backend_received_unix_ms = int(int(backend_received_unix_ns) / 1_000_000) + except (TypeError, ValueError): + backend_received_unix_ms = None + + latency_estimate: dict[str, Any] = {} + if backend_received_unix_ms is not None and network_timeline: + while timeline_index + 1 < len(network_timeline) and network_timeline[timeline_index + 1][0] <= backend_received_unix_ms: + timeline_index += 1 + if network_timeline[timeline_index][0] <= backend_received_unix_ms: + latency_estimate = network_timeline[timeline_index][1] + + network_oneway = latency_estimate.get("video_network_oneway_est_ms") + capture_to_send = record.get("b_side_capture_to_send_ms") + partial_est = None + if capture_to_send is not None or network_oneway is not None: + partial_est = round(float(capture_to_send or 0.0) + float(network_oneway or 0.0), 3) + request_to_paint_ms = None + if probe is not None and probe.get("request_to_paint_ms") is not None: + request_to_paint_ms = round(float(probe["request_to_paint_ms"]), 3) + elif probe is not None and probe.get("request_started_unix_ms") is not None and probe.get("paint_unix_ms") is not None: + request_to_paint_ms = round(float(probe["paint_unix_ms"]) - float(probe["request_started_unix_ms"]), 3) + video_e2e_est_ms = round(partial_est + request_to_paint_ms, 3) if partial_est is not None and request_to_paint_ms is not None else None + estimates.append( + { + "frame_seq": frame_seq, + "backend_received_unix_ns": record.get("backend_received_unix_ns"), + "frame_hash": record.get("frame_hash"), + "estimate_method": "capture_to_send+srtt/2+request_to_paint" if video_e2e_est_ms is not None else "capture_to_send+srtt/2", + "video_network_oneway_est_ms": network_oneway, + "b_side_capture_to_send_ms": capture_to_send, + "request_to_paint_ms": request_to_paint_ms, + "response_to_paint_ms": probe.get("response_to_paint_ms") if probe is not None else None, + "backend_to_request_ms": probe.get("backend_to_request_ms") if probe is not None else None, + "backend_to_request_ms_raw": probe.get("backend_to_request_ms_raw") if probe is not None else None, + "backend_to_paint_ms": probe.get("backend_to_paint_ms") if probe is not None else None, + "backend_to_paint_ms_raw": probe.get("backend_to_paint_ms_raw") if probe is not None else None, + "browser_backend_clock_offset_ms": probe.get("browser_backend_clock_offset_ms") if probe is not None else None, + "browser_backend_clock_rtt_ms": probe.get("browser_backend_clock_rtt_ms") if probe is not None else None, + "video_partial_est_ms": partial_est, + "video_e2e_est_ms": video_e2e_est_ms, + "sequence_gap": record.get("sequence_gap"), + "repeat_flag": record.get("repeat_flag"), + "sender_clock_delta_ms_raw": record.get("sender_clock_delta_ms_raw"), + } + ) + return estimates + + +def write_html_summary( + path: Path, + *, + net_epochs: list[dict[str, Any]], + control_estimates: list[dict[str, Any]], + video_estimates: list[dict[str, Any]], +) -> None: + latest_control = control_estimates[-1] if control_estimates else {} + latest_video = video_estimates[-1] if video_estimates else {} + latest_net = net_epochs[-1] if net_epochs else {} + html_text = f""" + + + + Latency Estimates + + + +

Latency Estimates

+
+
+

Control

+

loop RTT: {html.escape(str(latest_control.get("control_loop_rtt_ms")))}

+

to persist: {html.escape(str(latest_control.get("control_to_persist_est_ms")))}

+

method: {html.escape(str(latest_control.get("estimate_method")))}

+

samples: {len(control_estimates)}

+
+
+

Video

+

network one-way: {html.escape(str(latest_video.get("video_network_oneway_est_ms")))}

+

partial: {html.escape(str(latest_video.get("video_partial_est_ms")))}

+

end-to-end: {html.escape(str(latest_video.get("video_e2e_est_ms")))}

+

samples: {len(video_estimates)}

+
+
+

Net Epoch

+

a→d control srtt: {html.escape(str(latest_net.get("a_to_d_control_srtt_ms")))}

+

d→b control srtt: {html.escape(str(latest_net.get("d_to_b_control_srtt_ms")))}

+

a→d video srtt: {html.escape(str(latest_net.get("a_to_d_video_srtt_ms")))}

+

d→b video srtt: {html.escape(str(latest_net.get("d_to_b_video_srtt_ms")))}

+
+
+ + +""" + path.write_text(html_text, encoding="utf-8") + + +def main() -> int: + args = parse_args() + run_dir = Path(args.run_dir).resolve() + output_dir = Path(args.output_dir).resolve() if args.output_dir else run_dir + output_dir.mkdir(parents=True, exist_ok=True) + + network_samples = load_glob_jsonl(run_dir, "a-network-summary.*.jsonl") + control_events = load_glob_jsonl(run_dir, "a-control-events.*.jsonl") + control_acks = load_glob_jsonl(run_dir, "a-control-acks.*.jsonl") + frame_recv_records = load_glob_jsonl(run_dir, "a-video-frame-recv.*.jsonl") + display_probe_records = load_glob_jsonl(run_dir, "a-video-display-probe.*.jsonl") + + net_epochs = flatten_net_epoch(network_samples) + control_estimates = aggregate_control_estimates(network_samples, control_events, control_acks) + video_estimates = aggregate_video_estimates(network_samples, frame_recv_records, display_probe_records) + + write_jsonl(output_dir / "net-epoch-summary.jsonl", net_epochs) + write_jsonl(output_dir / "control-latency-estimates.jsonl", control_estimates) + write_jsonl(output_dir / "video-latency-estimates.jsonl", video_estimates) + write_html_summary( + output_dir / "latency-estimates.html", + net_epochs=net_epochs, + control_estimates=control_estimates, + video_estimates=video_estimates, + ) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/apply-camera-controls.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/apply-camera-controls.sh new file mode 100644 index 0000000..6b32c81 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/apply-camera-controls.sh @@ -0,0 +1,111 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" + +camera_device="${OMNI_CAMERA_DEVICE}" +camera_profile="${OMNI_CAMERA_PROFILE}" +camera_brightness="${OMNI_CAMERA_BRIGHTNESS}" +camera_custom_ctrl="${OMNI_CAMERA_CUSTOM_CTRL}" +camera_verify="${OMNI_CAMERA_VERIFY}" + +is_truthy() { + case "${1:-0}" in + 1|true|TRUE|yes|YES|on|ON) + return 0 + ;; + *) + return 1 + ;; + esac +} + +require_v4l2_ctl() { + if command -v v4l2-ctl >/dev/null 2>&1; then + return 0 + fi + + echo "Missing required command: v4l2-ctl. Install v4l-utils on the robot side before starting b_side_omnid." >&2 + exit 1 +} + +run_v4l2_ctl() { + v4l2-ctl -d "${camera_device}" "$@" +} + +set_ctrl() { + local ctrl="$1" + + echo "[camera-controls] set ${camera_device} ${ctrl}" + run_v4l2_ctl "--set-ctrl=${ctrl}" +} + +verify_ctrl() { + local ctrl="$1" + + echo "[camera-controls] verify ${camera_device} ${ctrl}" + run_v4l2_ctl "--get-ctrl=${ctrl}" +} + +needs_v4l2_ctl=0 + +case "${camera_profile}" in + night) + needs_v4l2_ctl=1 + ;; + day) + if [[ -n "${camera_brightness}" ]]; then + needs_v4l2_ctl=1 + fi + ;; + custom) + if [[ -z "${camera_custom_ctrl}" ]]; then + echo "OMNI_CAMERA_CUSTOM_CTRL must be non-empty when OMNI_CAMERA_PROFILE=custom." >&2 + exit 1 + fi + needs_v4l2_ctl=1 + ;; + *) + echo "Unsupported OMNI_CAMERA_PROFILE: ${camera_profile}. Expected one of: night, day, custom." >&2 + exit 1 + ;; +esac + +if is_truthy "${camera_verify}"; then + needs_v4l2_ctl=1 +fi + +if [[ "${needs_v4l2_ctl}" == "0" ]]; then + echo "[camera-controls] profile=${camera_profile}; no camera controls requested" + exit 0 +fi + +require_v4l2_ctl + +case "${camera_profile}" in + night) + set_ctrl "auto_exposure=1" + set_ctrl "exposure_time_absolute=800" + set_ctrl "gain=64" + if [[ -n "${camera_brightness}" ]]; then + set_ctrl "brightness=${camera_brightness}" + fi + ;; + day) + if [[ -n "${camera_brightness}" ]]; then + set_ctrl "brightness=${camera_brightness}" + fi + ;; + custom) + set_ctrl "${camera_custom_ctrl}" + ;; +esac + +if is_truthy "${camera_verify}"; then + verify_ctrl "auto_exposure" + verify_ctrl "exposure_time_absolute" + verify_ctrl "gain" + verify_ctrl "brightness" +fi diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/ensure-ros-camera-services.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/ensure-ros-camera-services.sh new file mode 100644 index 0000000..57258fd --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/ensure-ros-camera-services.sh @@ -0,0 +1,135 @@ +#!/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 [[ -f "/opt/ros/${ROS_DISTRO}/setup.bash" ]]; then + # shellcheck disable=SC1091 + set +u + source "/opt/ros/${ROS_DISTRO}/setup.bash" + set -u +fi +if [[ -f "${HOME}/xos/setup.bash" ]]; then + # shellcheck disable=SC1091 + set +u + source "${HOME}/xos/setup.bash" + set -u +fi + +service_active() { + systemctl is-active --quiet "$1" +} + +start_service_if_allowed() { + local service="$1" + local auto_start="${2:-0}" + + if service_active "${service}"; then + echo "[ros-camera-services] ${service} is active" >&2 + return 0 + fi + if [[ "${auto_start}" != "1" ]]; then + echo "[ros-camera-services] ${service} is inactive; automatic start is disabled" >&2 + return 1 + fi + echo "[ros-camera-services] starting ${service}" >&2 + sudo systemctl start "${service}" + service_active "${service}" +} + +topic_exists() { + local topic="$1" + ros2 topic list 2>/dev/null | grep -Fxq "${topic}" +} + +wait_for_topics() { + local timeout_sec="${1:-20}" + local deadline=$((SECONDS + timeout_sec)) + + while (( SECONDS < deadline )); do + if topic_exists "${OMNI_ROS2_HEAD_RGB_TOPIC}" \ + && topic_exists "${OMNI_ROS2_WAIST_RGB_TOPIC}"; then + echo "[ros-camera-services] RGB topics are available" >&2 + return 0 + fi + sleep 1 + done + return 1 +} + +camera_devices_are_free() { + local device + local busy=0 + + if ! command -v fuser >/dev/null 2>&1; then + echo "[ros-camera-services] fuser is required to verify camera ownership" >&2 + return 1 + fi + for device in "${OMNI_CAMERA_HEAD_DEVICE}" "${OMNI_CAMERA_WAIST_DEVICE}"; do + if [[ ! -e "${device}" ]]; then + echo "[ros-camera-services] camera device is missing: ${device}" >&2 + busy=1 + continue + fi + if fuser "${device}" >/dev/null 2>&1; then + echo "[ros-camera-services] camera device is busy: ${device}" >&2 + fuser -v "${device}" 2>&1 || true + busy=1 + fi + done + return "${busy}" +} + +if ! command -v systemctl >/dev/null 2>&1; then + echo "[ros-camera-services] systemctl is required on the robot" >&2 + exit 1 +fi +if ! command -v ros2 >/dev/null 2>&1; then + echo "[ros-camera-services] ros2 is not in PATH; source the ROS2 and robot setup files first" >&2 + exit 1 +fi + +if ! service_active "${OMNI_PROC_MANAGER_SERVICE}"; then + echo "[ros-camera-services] ${OMNI_PROC_MANAGER_SERVICE} is not active" >&2 + if [[ "${OMNI_PROC_MANAGER_AUTO_START:-0}" == "1" ]]; then + start_service_if_allowed "${OMNI_PROC_MANAGER_SERVICE}" "${OMNI_PROC_MANAGER_AUTO_START}" + else + echo "[ros-camera-services] proc_manager auto-start disabled; using the explicit camera-service fallback if allowed" >&2 + fi +fi + +# proc_manager owns the ROS camera components when it is active. Never start +# separate camera services on top of it: that would create a second device +# owner and reproduce EBUSY. The individual services are only a fallback when +# proc_manager is inactive and explicitly allowed by the environment. +if wait_for_topics 20; then + exit 0 +fi + +if service_active "${OMNI_PROC_MANAGER_SERVICE}"; then + echo "[ros-camera-services] proc_manager is active but the expected RGB topics are missing" >&2 + if ! camera_devices_are_free; then + echo "[ros-camera-services] refusing to start ${OMNI_CAMERA_HEAD_SERVICE}/${OMNI_CAMERA_WAIST_SERVICE}: a camera device is already owned" >&2 + exit 1 + fi + echo "[ros-camera-services] proc_manager does not own the head/waist devices; starting the configured camera services" >&2 +fi + +if [[ "${OMNI_CAMERA_START_SERVICES:-1}" != "1" ]]; then + echo "[ros-camera-services] individual camera service fallback is disabled" >&2 + exit 1 +fi + +if ! camera_devices_are_free; then + echo "[ros-camera-services] refusing to start camera services while a device is busy" >&2 + exit 1 +fi + +start_service_if_allowed "${OMNI_CAMERA_HEAD_SERVICE}" "${OMNI_CAMERA_START_SERVICES:-1}" +start_service_if_allowed "${OMNI_CAMERA_WAIST_SERVICE}" "${OMNI_CAMERA_START_SERVICES:-1}" +if ! wait_for_topics 20; then + echo "[ros-camera-services] camera services started but RGB topics did not appear" >&2 + exit 1 +fi diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/load-env.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/load-env.sh new file mode 100644 index 0000000..fc0413d --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/load-env.sh @@ -0,0 +1,342 @@ +#!/usr/bin/env bash +set -euo pipefail + +LOAD_ENV_SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +DEFAULT_OMNISOCKETGO_ROOT="$(cd "${LOAD_ENV_SCRIPT_DIR}/../.." && pwd)" + +die() { + echo "$*" >&2 + return 1 2>/dev/null || exit 1 +} + +normalize_loaded_env_vars() { + local var_name + local value + + for var_name in $(compgen -A variable); do + case "${var_name}" in + BACKEND_*|BLITZ_*|B_SIDE_*|CONTROL_*|FRONTEND_*|OMNI_*|PYTHON3_BIN|PYTHON_VENV_PATH|ROBOT_*|ROS_DISTRO|VITE_*) + value="${!var_name}" + if [[ "${value}" == *$'\r' ]]; then + printf -v "${var_name}" '%s' "${value%$'\r'}" + export "${var_name}" + fi + ;; + esac + done +} + +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" ]] +} + +require_robot_command_center_root() { + 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 ${LOAD_ENV_SCRIPT_DIR}/robot-remote.env.local if needed." + fi +} + +export OMNISOCKETGO_ROOT="${OMNISOCKETGO_ROOT:-${DEFAULT_OMNISOCKETGO_ROOT}}" + +omni_camera_device_was_set=0 +omni_camera_profile_was_set=0 +omni_camera_brightness_was_set=0 +omni_camera_custom_ctrl_was_set=0 +omni_camera_verify_was_set=0 + +if [[ "${OMNI_CAMERA_DEVICE+x}" == "x" ]]; then + omni_camera_device_was_set=1 + preserved_omni_camera_device="${OMNI_CAMERA_DEVICE}" +fi +if [[ "${OMNI_CAMERA_PROFILE+x}" == "x" ]]; then + omni_camera_profile_was_set=1 + preserved_omni_camera_profile="${OMNI_CAMERA_PROFILE}" +fi +if [[ "${OMNI_CAMERA_BRIGHTNESS+x}" == "x" ]]; then + omni_camera_brightness_was_set=1 + preserved_omni_camera_brightness="${OMNI_CAMERA_BRIGHTNESS}" +fi +if [[ "${OMNI_CAMERA_CUSTOM_CTRL+x}" == "x" ]]; then + omni_camera_custom_ctrl_was_set=1 + preserved_omni_camera_custom_ctrl="${OMNI_CAMERA_CUSTOM_CTRL}" +fi +if [[ "${OMNI_CAMERA_VERIFY+x}" == "x" ]]; then + omni_camera_verify_was_set=1 + preserved_omni_camera_verify="${OMNI_CAMERA_VERIFY}" +fi + +ENV_FILES=( + "${LOAD_ENV_SCRIPT_DIR}/robot-remote.env" + "${LOAD_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 + +normalize_loaded_env_vars + +if [[ "${omni_camera_device_was_set}" == "1" ]]; then + export OMNI_CAMERA_DEVICE="${preserved_omni_camera_device}" +fi +if [[ "${omni_camera_profile_was_set}" == "1" ]]; then + export OMNI_CAMERA_PROFILE="${preserved_omni_camera_profile}" +fi +if [[ "${omni_camera_brightness_was_set}" == "1" ]]; then + export OMNI_CAMERA_BRIGHTNESS="${preserved_omni_camera_brightness}" +fi +if [[ "${omni_camera_custom_ctrl_was_set}" == "1" ]]; then + export OMNI_CAMERA_CUSTOM_CTRL="${preserved_omni_camera_custom_ctrl}" +fi +if [[ "${omni_camera_verify_was_set}" == "1" ]]; then + export OMNI_CAMERA_VERIFY="${preserved_omni_camera_verify}" +fi + +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 + +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 OMNISOCKET_TELEMETRY_PEER_ID="${OMNISOCKET_TELEMETRY_PEER_ID:-peer-a-telemetry}" +export OMNISOCKET_TELEMETRY_INTERVAL_MS="${OMNISOCKET_TELEMETRY_INTERVAL_MS:-1000}" +export OMNISOCKET_TELEMETRY_STALE_AFTER_MS="${OMNISOCKET_TELEMETRY_STALE_AFTER_MS:-3000}" +export OMNI_NETWORK_SUMMARY_LOG_ENABLED="${OMNI_NETWORK_SUMMARY_LOG_ENABLED:-1}" +export OMNI_NETWORK_SUMMARY_LOG_PATH="${OMNI_NETWORK_SUMMARY_LOG_PATH:-${OMNISOCKETGO_ROOT}/logs/a-network-summary.jsonl}" +export OMNI_NETWORK_SUMMARY_LOG_INTERVAL_MS="${OMNI_NETWORK_SUMMARY_LOG_INTERVAL_MS:-1000}" +export OMNI_NETWORK_SUMMARY_LOG_REQUEST_TIMEOUT_SEC="${OMNI_NETWORK_SUMMARY_LOG_REQUEST_TIMEOUT_SEC:-3}" +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_CAMERA_DEVICE="${OMNI_CAMERA_DEVICE:-/dev/video0}" +export OMNI_CAMERA_HEAD_DEVICE="${OMNI_CAMERA_HEAD_DEVICE:-/dev/video26}" +export OMNI_CAMERA_WAIST_DEVICE="${OMNI_CAMERA_WAIST_DEVICE:-/dev/video18}" +export OMNI_CAMERA_SOURCE="${OMNI_CAMERA_SOURCE:-ros2}" +export OMNI_CAMERA_ACTIVE="${OMNI_CAMERA_ACTIVE:-head}" +export OMNI_PROC_MANAGER_SERVICE="${OMNI_PROC_MANAGER_SERVICE:-proc_manager.service}" +export OMNI_PROC_MANAGER_AUTO_START="${OMNI_PROC_MANAGER_AUTO_START:-0}" +export OMNI_CAMERA_START_SERVICES="${OMNI_CAMERA_START_SERVICES:-1}" +export OMNI_CAMERA_HEAD_SERVICE="${OMNI_CAMERA_HEAD_SERVICE:-orbbec_head.service}" +export OMNI_CAMERA_WAIST_SERVICE="${OMNI_CAMERA_WAIST_SERVICE:-orbbec_waist.service}" +export OMNI_ROS2_HEAD_RGB_TOPIC="${OMNI_ROS2_HEAD_RGB_TOPIC:-/ob_camera_head/color/image_raw}" +export OMNI_ROS2_WAIST_RGB_TOPIC="${OMNI_ROS2_WAIST_RGB_TOPIC:-/ob_camera_waist/color/image_raw}" +export OMNI_ROS2_HEAD_SHM="${OMNI_ROS2_HEAD_SHM:-/dev/shm/omnisocket-rgb-head}" +export OMNI_ROS2_WAIST_SHM="${OMNI_ROS2_WAIST_SHM:-/dev/shm/omnisocket-rgb-waist}" +export OMNI_ROS2_MAX_FRAME_BYTES="${OMNI_ROS2_MAX_FRAME_BYTES:-8294400}" +export OMNI_ROS2_BRIDGE_ROOT="${OMNI_ROS2_BRIDGE_ROOT:-${OMNISOCKETGO_ROOT}/ros2}" +export OMNI_ROS2_BRIDGE_SETUP="${OMNI_ROS2_BRIDGE_SETUP:-${OMNI_ROS2_BRIDGE_ROOT}/install/setup.bash}" +export OMNI_ROS2_BRIDGE_AUTO_START="${OMNI_ROS2_BRIDGE_AUTO_START:-1}" +export OMNI_ROS2_BRIDGE_LOG_PATH="${OMNI_ROS2_BRIDGE_LOG_PATH:-${OMNISOCKETGO_ROOT}/logs/ros-camera-bridge.log}" +export OMNI_CAMERA_PROFILE="${OMNI_CAMERA_PROFILE:-night}" +export OMNI_CAMERA_BRIGHTNESS="${OMNI_CAMERA_BRIGHTNESS:-}" +export OMNI_CAMERA_CUSTOM_CTRL="${OMNI_CAMERA_CUSTOM_CTRL:-}" +export OMNI_CAMERA_VERIFY="${OMNI_CAMERA_VERIFY:-0}" +export OMNI_GPSD_HOST="${OMNI_GPSD_HOST:-127.0.0.1}" +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 OMNI_CONTROL_ACK_PEER_ID="${OMNI_CONTROL_ACK_PEER_ID:-peer-b-ctrl-ack}" +export OMNI_CONTROL_ACK_TARGET_PEER="${OMNI_CONTROL_ACK_TARGET_PEER:-peer-a-ctrl-ack}" +export B_SIDE_OMNID_USE_SUDO="${B_SIDE_OMNID_USE_SUDO:-1}" +export BLITZ_RUNTIME_DIR="${BLITZ_RUNTIME_DIR:-${OMNISOCKETGO_ROOT}/logs/runtime}" +export BLITZ_RUN_ROOT="${BLITZ_RUN_ROOT:-${OMNISOCKETGO_ROOT}/logs}" +export BLITZ_RUN_CONTEXT_FILE="${BLITZ_RUN_CONTEXT_FILE:-${BLITZ_RUNTIME_DIR}/run-context.env}" +export BLITZ_RUN_ID_FILE="${BLITZ_RUN_ID_FILE:-${BLITZ_RUNTIME_DIR}/run-id}" +export BLITZ_CURRENT_RUN_LINK="${BLITZ_CURRENT_RUN_LINK:-${BLITZ_RUN_ROOT}/current}" +export BLITZ_5G_INTERFACE="${BLITZ_5G_INTERFACE:-}" +export BLITZ_5G_MODEM_SUBNET="${BLITZ_5G_MODEM_SUBNET:-192.168.224.0/22}" +export BLITZ_5G_GATEWAY="${BLITZ_5G_GATEWAY:-192.168.225.1}" +export BLITZ_5G_ROUTE_TARGETS="${BLITZ_5G_ROUTE_TARGETS:-106.55.173.235}" +export BLITZ_5G_INFO_JSON="${BLITZ_5G_INFO_JSON:-${OMNISOCKETGO_ROOT}/scripts/boot/modem_network_info.json}" +export BLITZ_TIME_SERVER_IP="${BLITZ_TIME_SERVER_IP:-}" +export BLITZ_KCP_STATS_INTERVAL_MS="${BLITZ_KCP_STATS_INTERVAL_MS:-1000}" +export BLITZ_CONTROL_LATENCY_LOG_ENABLED="${BLITZ_CONTROL_LATENCY_LOG_ENABLED:-1}" +export BLITZ_CONTROL_LATENCY_LOG_SAMPLE_MOD="${BLITZ_CONTROL_LATENCY_LOG_SAMPLE_MOD:-100}" +export BLITZ_CONTROL_ACK_SAMPLE_MOD="${BLITZ_CONTROL_ACK_SAMPLE_MOD:-10}" +export BLITZ_VIDEO_STAGE_LOG_ENABLED="${BLITZ_VIDEO_STAGE_LOG_ENABLED:-1}" +export BLITZ_VIDEO_STAGE_LOG_SAMPLE_MOD="${BLITZ_VIDEO_STAGE_LOG_SAMPLE_MOD:-10}" +export BLITZ_5G_LINK_LOG_INTERVAL_SEC="${BLITZ_5G_LINK_LOG_INTERVAL_SEC:-5}" +export BLITZ_JSONL_FLUSH_INTERVAL_MS="${BLITZ_JSONL_FLUSH_INTERVAL_MS:-1000}" +export BLITZ_JSONL_FLUSH_BYTES="${BLITZ_JSONL_FLUSH_BYTES:-262144}" +export BLITZ_JSONL_ROTATE_BYTES="${BLITZ_JSONL_ROTATE_BYTES:-134217728}" +export BLITZ_JSONL_ROTATE_FILES="${BLITZ_JSONL_ROTATE_FILES:-8}" + +blitz_dev_utc_compact_timestamp() { + date -u '+%Y%m%dT%H%M%SZ' +} + +blitz_dev_git_commit() { + git -C "${OMNISOCKETGO_ROOT}" rev-parse HEAD 2>/dev/null || true +} + +blitz_dev_git_dirty_flag() { + if git -C "${OMNISOCKETGO_ROOT}" diff --quiet --ignore-submodules=dirty >/dev/null 2>&1; then + printf '0\n' + return 0 + fi + printf '1\n' +} + +blitz_dev_prepare_dirs() { + mkdir -p "${BLITZ_RUNTIME_DIR}" "${BLITZ_RUN_ROOT}/runs" "${BLITZ_RUN_ROOT}/incidents" +} + +blitz_dev_write_run_info() { + local run_dir="$1" + local run_id="$2" + local boot_id="$3" + local tmp_info="${run_dir}/run-info.json.tmp.$$" + local started_at + local commit_hash + local dirty_flag + + started_at="$(date -u '+%Y-%m-%dT%H:%M:%SZ')" + commit_hash="$(blitz_dev_git_commit)" + dirty_flag="$(blitz_dev_git_dirty_flag)" + + python3 - "${tmp_info}" "${run_id}" "${run_dir}" "${boot_id}" "${started_at}" "${commit_hash}" "${dirty_flag}" "${HOSTNAME:-$(hostname)}" <<'PY' +import json +import os +import sys + +path, run_id, run_dir, boot_id, started_at, commit_hash, dirty_flag, hostname = sys.argv[1:9] +payload = { + "run_id": run_id, + "run_dir": run_dir, + "boot_id": boot_id, + "started_at": started_at, + "hostname": hostname, + "git_commit": commit_hash, + "git_dirty": dirty_flag == "1", + "env": { + key: os.environ.get(key, "") + for key in sorted(os.environ) + if key.startswith(("BLITZ_", "OMNI_", "ROBOT_RECEIVER_")) + }, +} +with open(path, "w", encoding="utf-8") as handle: + json.dump(payload, handle, ensure_ascii=False, indent=2, sort_keys=True) +PY + mv -f "${tmp_info}" "${run_dir}/run-info.json" +} + +blitz_dev_init_run_context() { + local run_id="${1:-$(blitz_dev_utc_compact_timestamp)}" + local boot_id="dev-$(blitz_dev_utc_compact_timestamp)" + local run_dir="${BLITZ_RUN_ROOT}/runs/${run_id}" + local tmp_context="${BLITZ_RUN_CONTEXT_FILE}.tmp.$$" + + blitz_dev_prepare_dirs + mkdir -p "${run_dir}" + export BLITZ_RUN_ID="${run_id}" + export BLITZ_RUN_DIR="${run_dir}" + export BLITZ_BOOT_ID="${boot_id}" + printf '%s\n' "${run_id}" > "${BLITZ_RUN_ID_FILE}" + cat > "${tmp_context}" < None: + del signum, frame + global STOP_REQUESTED + STOP_REQUESTED = True + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="Poll /api/network/latest/ and append JSONL snapshots.") + parser.add_argument("--url", required=True, help="HTTP endpoint that returns the network summary JSON.") + parser.add_argument("--output", required=True, help="Output JSONL path.") + parser.add_argument( + "--interval-ms", + type=int, + default=2000, + help="Polling interval in milliseconds. Default: 2000.", + ) + parser.add_argument( + "--request-timeout-sec", + type=float, + default=3.0, + help="Single request timeout in seconds. Default: 3.0.", + ) + return parser.parse_args() + + +def sleep_with_stop(seconds: float) -> None: + deadline = time.monotonic() + max(0.0, seconds) + while not STOP_REQUESTED: + remaining = deadline - time.monotonic() + if remaining <= 0.0: + return + time.sleep(min(remaining, 0.2)) + + +def fetch_json(url: str, timeout_sec: float) -> str: + request = urllib.request.Request( + url, + headers={ + "Accept": "application/json", + "Cache-Control": "no-cache", + }, + method="GET", + ) + # This logger always polls the local backend. Ignore HTTP_PROXY/HTTPS_PROXY + # so a developer proxy cannot turn a 127.0.0.1 request into a 502. + with LOCAL_HTTP_OPENER.open(request, timeout=timeout_sec) as response: + charset = response.headers.get_content_charset("utf-8") + payload = response.read().decode(charset) + parsed = json.loads(payload) + return json.dumps(parsed, separators=(",", ":"), ensure_ascii=False) + + +def main() -> int: + args = parse_args() + interval_sec = max(args.interval_ms, 200) / 1000.0 + output_path = Path(args.output) + last_error_log_monotonic = 0.0 + + signal.signal(signal.SIGINT, handle_signal) + signal.signal(signal.SIGTERM, handle_signal) + + output_path.parent.mkdir(parents=True, exist_ok=True) + + with output_path.open("a", encoding="utf-8") as output_file: + while not STOP_REQUESTED: + started = time.monotonic() + try: + line = fetch_json(args.url, args.request_timeout_sec) + except (TimeoutError, urllib.error.URLError, urllib.error.HTTPError, json.JSONDecodeError) as error: + now = time.monotonic() + if now - last_error_log_monotonic >= 10.0: + print(f"[network-summary] poll failed: {error}", file=sys.stderr) + last_error_log_monotonic = now + else: + output_file.write(line) + output_file.write("\n") + output_file.flush() + + elapsed = time.monotonic() - started + sleep_with_stop(max(0.0, interval_sec - elapsed)) + + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/prepare-camera-device.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/prepare-camera-device.sh new file mode 100644 index 0000000..b9ca30f --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/prepare-camera-device.sh @@ -0,0 +1,82 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" + +camera_device="${OMNI_CAMERA_DEVICE}" +occupancy_policy="${OMNI_CAMERA_OCCUPANCY_POLICY:-check}" + +die() { + echo "[camera-preflight] $*" >&2 + exit 1 +} + +if [[ "${OMNI_CAMERA_SOURCE:-ros2}" == "ros2" ]]; then + die "ROS2 camera mode is active; do not inspect or release /dev/video*. Use ensure-ros-camera-services.sh instead." +fi + +camera_pids() { + fuser "${camera_device}" 2>/dev/null \ + | tr ' ' '\n' \ + | grep -E '^[0-9]+$' \ + | sort -nu \ + || true +} + +pid_service() { + local pid="$1" + local service + + service="$(sed -nE 's#.*[/:]([^/:]+\.service)$#\1#p' "/proc/${pid}/cgroup" 2>/dev/null | head -1)" + printf '%s' "${service:-unknown}" +} + +report_owners() { + local pid + local comm + local service + local found=0 + + while read -r pid; do + [[ -n "${pid}" ]] || continue + found=1 + comm="$(cat "/proc/${pid}/comm" 2>/dev/null || printf 'unknown')" + service="$(pid_service "${pid}")" + echo "[camera-preflight] owner pid=${pid} command=${comm} service=${service}" >&2 + done < <(camera_pids) + + if [[ "${found}" == "1" ]]; then + return 0 + fi + return 1 +} + +if [[ ! -e "${camera_device}" ]]; then + die "camera device does not exist: ${camera_device}" +fi + +if ! command -v fuser >/dev/null 2>&1; then + die "missing required command: fuser (install the psmisc package)" +fi + +resolved_device="$(readlink -f "${camera_device}" 2>/dev/null || printf '%s' "${camera_device}")" +echo "[camera-preflight] checking ${camera_device} (${resolved_device}) policy=${occupancy_policy}" >&2 + +if ! report_owners; then + echo "[camera-preflight] ${camera_device} is free" >&2 + exit 0 +fi + +case "${occupancy_policy}" in + check) + die "${camera_device} is busy; no process was stopped" + ;; + release-known) + die "release-known is intentionally removed in OmniSocketGo_robot_ros; stop the owning service manually and use policy=check" + ;; + *) + die "unsupported OMNI_CAMERA_OCCUPANCY_POLICY=${occupancy_policy}; expected check" + ;; +esac diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/reset-run-context.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/reset-run-context.sh new file mode 100644 index 0000000..4b9233d --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/reset-run-context.sh @@ -0,0 +1,10 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +export BLITZ_SKIP_DEV_RUN_CONTEXT_INIT="1" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" + +blitz_dev_reset_run_context +printf 'run_id=%s\nrun_dir=%s\n' "${BLITZ_RUN_ID}" "${BLITZ_RUN_DIR}" diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/robot-remote.env b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/robot-remote.env new file mode 100644 index 0000000..145d7f1 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/robot-remote.env @@ -0,0 +1,90 @@ +# 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="192.168.41.144:10909" # Local LAN hub +CONTROL_SIDE_OMNISOCKET_RELAY_VIA="" # No relay + +ROBOT_SIDE_OMNISOCKET_SERVER_ADDR="192.168.41.144:10909" # Local LAN hub +ROBOT_SIDE_OMNISOCKET_RELAY_VIA="" # Direct LAN +# Log one normal relay packet out of every N packets. Drop events still log immediately. +OMNI_RELAY_PACKET_LOG_SAMPLE_EVERY="200" + +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" +OMNISOCKET_TELEMETRY_PEER_ID="peer-a-telemetry" +OMNISOCKET_TELEMETRY_INTERVAL_MS="1000" +OMNISOCKET_TELEMETRY_STALE_AFTER_MS="3000" +OMNI_NETWORK_SUMMARY_LOG_ENABLED="1" +OMNI_NETWORK_SUMMARY_LOG_PATH="${OMNISOCKETGO_ROOT}/logs/a-network-summary.jsonl" +OMNI_NETWORK_SUMMARY_LOG_INTERVAL_MS="1000" +OMNI_NETWORK_SUMMARY_LOG_REQUEST_TIMEOUT_SEC="3" + +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_GPSD_HOST="127.0.0.1" +OMNI_CAMERA_HEAD_DEVICE="/dev/video26" +OMNI_CAMERA_WAIST_DEVICE="/dev/video18" +OMNI_CAMERA_SOURCE="ros2" +OMNI_CAMERA_ACTIVE="head" +OMNI_PROC_MANAGER_SERVICE="proc_manager.service" +OMNI_PROC_MANAGER_AUTO_START="0" +OMNI_CAMERA_START_SERVICES="1" +OMNI_CAMERA_HEAD_SERVICE="orbbec_head.service" +OMNI_CAMERA_WAIST_SERVICE="orbbec_waist.service" +OMNI_ROS2_HEAD_RGB_TOPIC="/ob_camera_head/color/image_raw" +OMNI_ROS2_WAIST_RGB_TOPIC="/ob_camera_waist/color/image_raw" +OMNI_ROS2_HEAD_SHM="/dev/shm/omnisocket-rgb-head" +OMNI_ROS2_WAIST_SHM="/dev/shm/omnisocket-rgb-waist" +OMNI_ROS2_MAX_FRAME_BYTES="8294400" +OMNI_ROS2_BRIDGE_AUTO_START="1" +OMNI_CAMERA_PROFILE="day" +OMNI_CAMERA_BRIGHTNESS="" +OMNI_CAMERA_CUSTOM_CTRL="" +OMNI_CAMERA_VERIFY="0" +OMNI_VIDEO_SERVER_ADDR="${ROBOT_SIDE_OMNISOCKET_SERVER_ADDR}" +OMNI_VIDEO_RELAY_VIA="${ROBOT_SIDE_OMNISOCKET_RELAY_VIA}" +OMNI_VIDEO_SOFT_BACKPRESSURE_SEGMENTS="256" +OMNI_VIDEO_HARD_BACKPRESSURE_SEGMENTS="1024" +OMNI_VIDEO_HARD_BACKPRESSURE_HOLD_MS="5000" +OMNI_VIDEO_FRAME_STALL_RECONNECT_MS="30000" +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}" +OMNI_CONTROL_ACK_PEER_ID="peer-b-ctrl-ack" +OMNI_CONTROL_ACK_TARGET_PEER="peer-a-ctrl-ack" +BLITZ_CONTROL_ACK_SAMPLE_MOD="10" +BLITZ_VIDEO_STAGE_LOG_ENABLED="1" +BLITZ_VIDEO_STAGE_LOG_SAMPLE_MOD="10" +OMNI_CONTROL_SERVER_IDLE_RECONNECT_MS="30000" + +# A-side backend video freshness guard. Used by scripts/dev/start-backend.sh. +OMNI_VIDEO_MAX_FRAME_AGE_MS="1000" + +B_SIDE_OMNID_USE_SUDO="1" diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/start-5g-link-logger.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/start-5g-link-logger.sh new file mode 100644 index 0000000..093928f --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/start-5g-link-logger.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" + +blitz_dev_prepare_5g_logging_env +exec bash "${OMNISOCKETGO_ROOT}/scripts/boot/blitz-5g-link-logger.sh" diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/start-b-side-omnid.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/start-b-side-omnid.sh new file mode 100644 index 0000000..9850cb7 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/start-b-side-omnid.sh @@ -0,0 +1,125 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" +blitz_dev_prepare_bside_logging_env + +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}" + +logger_pid="" +bridge_pid="" + +cleanup() { + if [[ -n "${logger_pid}" ]]; then + kill "${logger_pid}" 2>/dev/null || true + wait "${logger_pid}" 2>/dev/null || true + fi + if [[ -n "${bridge_pid}" ]]; then + kill "${bridge_pid}" 2>/dev/null || true + wait "${bridge_pid}" 2>/dev/null || true + fi +} + +start_5g_link_logger_if_needed() { + if [[ "${OMNI_5G_LINK_LOG_ENABLED:-1}" != "1" ]]; then + echo "[start-b-side-omnid] 5G link logger disabled" >&2 + return 0 + fi + if [[ "${OMNI_BOOT_MODE:-0}" == "1" ]]; then + return 0 + fi + bash "${SCRIPT_DIR}/start-5g-link-logger.sh" & + logger_pid=$! + echo "[start-b-side-omnid] 5G link logger -> ${BLITZ_5G_LINK_LOG_PATH:-unset}" >&2 +} + +start_ros_camera_bridge_if_needed() { + if [[ "${OMNI_ROS2_BRIDGE_AUTO_START:-1}" != "1" ]]; then + echo "[start-b-side-omnid] ROS2 camera bridge auto-start disabled" >&2 + return 0 + fi + if pgrep -af 'omnisocket_ros_camera_bridge' >/dev/null 2>&1; then + echo "[start-b-side-omnid] ROS2 camera bridge already running" >&2 + return 0 + fi + if [[ ! -f "${OMNI_ROS2_BRIDGE_SETUP}" ]]; then + echo "[start-b-side-omnid] missing ROS2 bridge setup: ${OMNI_ROS2_BRIDGE_SETUP}" >&2 + echo "[start-b-side-omnid] build it with: cd ${OMNI_ROS2_BRIDGE_ROOT} && colcon build" >&2 + return 1 + fi + mkdir -p "$(dirname "${OMNI_ROS2_BRIDGE_LOG_PATH}")" + ( + if [[ -f "/opt/ros/${ROS_DISTRO}/setup.bash" ]]; then + # shellcheck disable=SC1091 + set +u + source "/opt/ros/${ROS_DISTRO}/setup.bash" + set -u + fi + # shellcheck disable=SC1091 + set +u + source "${OMNI_ROS2_BRIDGE_SETUP}" + set -u + exec ros2 run omnisocket_camera_bridge omnisocket_ros_camera_bridge \ + --ros-args \ + -p "head_topic:=${OMNI_ROS2_HEAD_RGB_TOPIC}" \ + -p "waist_topic:=${OMNI_ROS2_WAIST_RGB_TOPIC}" \ + -p "head_shm:=${OMNI_ROS2_HEAD_SHM}" \ + -p "waist_shm:=${OMNI_ROS2_WAIST_SHM}" \ + -p "max_frame_bytes:=${OMNI_ROS2_MAX_FRAME_BYTES}" + ) >>"${OMNI_ROS2_BRIDGE_LOG_PATH}" 2>&1 & + bridge_pid=$! + sleep 2 + if ! kill -0 "${bridge_pid}" 2>/dev/null; then + echo "[start-b-side-omnid] ROS2 camera bridge exited; inspect ${OMNI_ROS2_BRIDGE_LOG_PATH}" >&2 + return 1 + fi + echo "[start-b-side-omnid] ROS2 camera bridge pid=${bridge_pid}" >&2 +} + +if [[ ! -x "./bin/b_side_omnid" ]]; then + if [[ "${OMNI_BOOT_MODE:-0}" == "1" ]]; then + echo "Missing ./bin/b_side_omnid in boot mode; build it before enabling the autostart service." >&2 + exit 1 + fi + make b_side_omnid +fi + +launch_b_side_omnid() { + trap cleanup EXIT INT TERM + start_5g_link_logger_if_needed + if [[ "${OMNI_CAMERA_SOURCE}" == "ros2" ]]; then + bash "${SCRIPT_DIR}/ensure-ros-camera-services.sh" + start_ros_camera_bridge_if_needed + else + OMNI_CAMERA_DEVICE="${OMNI_CAMERA_HEAD_DEVICE}" \ + OMNI_CAMERA_OCCUPANCY_POLICY=check \ + bash "${SCRIPT_DIR}/prepare-camera-device.sh" + OMNI_CAMERA_DEVICE="${OMNI_CAMERA_WAIST_DEVICE}" \ + OMNI_CAMERA_OCCUPANCY_POLICY=check \ + bash "${SCRIPT_DIR}/prepare-camera-device.sh" + OMNI_CAMERA_DEVICE="${OMNI_CAMERA_HEAD_DEVICE}" bash "${SCRIPT_DIR}/apply-camera-controls.sh" + OMNI_CAMERA_DEVICE="${OMNI_CAMERA_WAIST_DEVICE}" bash "${SCRIPT_DIR}/apply-camera-controls.sh" + fi + ./bin/b_side_omnid +} + +if [[ "${OMNI_CAMERA_SOURCE}" == "ros2" ]]; then + # ROS2/DDS must run as the robot user; the C daemon no longer needs root + # because it reads shared memory instead of opening /dev/video*. + export B_SIDE_OMNID_USE_SUDO=0 +fi + +if [[ "${B_SIDE_OMNID_USE_SUDO}" == "1" && "${EUID}" -ne 0 ]]; then + exec sudo -E bash -lc 'cd "$1" && export B_SIDE_OMNID_USE_SUDO=0 && exec bash "$2"' _ "${OMNISOCKETGO_ROOT}" "${SCRIPT_DIR}/start-b-side-omnid.sh" +fi + +launch_b_side_omnid diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/start-backend.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/start-backend.sh new file mode 100644 index 0000000..bc8e4d4 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/start-backend.sh @@ -0,0 +1,60 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" +require_robot_command_center_root +blitz_dev_prepare_backend_logging_env + +if [[ ! -x "${PYTHON_VENV_PATH}/bin/python" ]]; then + echo "[start-backend] creating or repairing virtualenv at ${PYTHON_VENV_PATH}" >&2 + "${PYTHON3_BIN}" -m venv "${PYTHON_VENV_PATH}" +fi + +if [[ ! -x "${PYTHON_VENV_PATH}/bin/python" ]]; then + echo "[start-backend] virtualenv is incomplete: missing ${PYTHON_VENV_PATH}/bin/python" >&2 + exit 1 +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}" + +logger_pid="" + +cleanup() { + if [[ -n "${logger_pid}" ]]; then + kill "${logger_pid}" 2>/dev/null || true + wait "${logger_pid}" 2>/dev/null || true + fi +} + +start_network_summary_logger() { + local logger_url + local logger_dir + + if [[ "${OMNI_NETWORK_SUMMARY_LOG_ENABLED}" != "1" ]]; then + return + fi + + logger_url="http://127.0.0.1:${BACKEND_PORT}/api/network/latest/" + logger_dir="$(dirname "${OMNI_NETWORK_SUMMARY_LOG_PATH}")" + mkdir -p "${logger_dir}" + + python "${SCRIPT_DIR}/log-network-summary.py" \ + --url "${logger_url}" \ + --output "${OMNI_NETWORK_SUMMARY_LOG_PATH}" \ + --interval-ms "${OMNI_NETWORK_SUMMARY_LOG_INTERVAL_MS}" \ + --request-timeout-sec "${OMNI_NETWORK_SUMMARY_LOG_REQUEST_TIMEOUT_SEC}" & + logger_pid=$! + echo "[start-backend] network summary logger -> ${OMNI_NETWORK_SUMMARY_LOG_PATH} (${OMNI_NETWORK_SUMMARY_LOG_INTERVAL_MS} ms)" >&2 +} + +trap cleanup EXIT INT TERM + +start_network_summary_logger +python -m uvicorn config.asgi:application --host "${BACKEND_HOST}" --port "${BACKEND_PORT}" diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/start-dev-tmux.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/start-dev-tmux.sh new file mode 100644 index 0000000..e9c2fe2 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/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}" diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/start-frontend.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/start-frontend.sh new file mode 100644 index 0000000..b33a87a --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/start-frontend.sh @@ -0,0 +1,10 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" +require_robot_command_center_root + +cd "${FRONTEND_DIR}" +exec npm run dev -- --host "${FRONTEND_HOST}" --port "${FRONTEND_PORT}" diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/start-local-hub.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/start-local-hub.sh new file mode 100644 index 0000000..3ca0c0f --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/start-local-hub.sh @@ -0,0 +1,22 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" + +hub_binary="${OMNISOCKETGO_ROOT}/bin/kcpserver" +hub_listen_addr="${LOCAL_HUB_LISTEN_ADDR:-0.0.0.0:10909}" +telemetry_peer_id="${LOCAL_HUB_TELEMETRY_PEER_ID:-peer-a-telemetry}" +telemetry_interval="${LOCAL_HUB_TELEMETRY_INTERVAL:-1000ms}" + +if [[ ! -x "${hub_binary}" ]]; then + echo "[start-local-hub] missing executable ${hub_binary}; run: make bin/kcpserver" >&2 + exit 1 +fi + +echo "[start-local-hub] listen=${hub_listen_addr} relay=disabled telemetry_peer=${telemetry_peer_id}" >&2 +exec "${hub_binary}" \ + -listen "${hub_listen_addr}" \ + -telemetry-peer "${telemetry_peer_id}" \ + -telemetry-interval "${telemetry_interval}" diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/start-ros-receiver.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/start-ros-receiver.sh new file mode 100644 index 0000000..6c90630 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/dev/start-ros-receiver.sh @@ -0,0 +1,50 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" + +source_with_nounset_off() { + set +u + # shellcheck disable=SC1090 + source "$1" + set -u +} + +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" +if [[ ! -f "/opt/ros/${ROS_DISTRO}/setup.bash" ]]; then + echo "Missing ROS distro setup: /opt/ros/${ROS_DISTRO}/setup.bash" >&2 + exit 1 +fi +source_with_nounset_off "/opt/ros/${ROS_DISTRO}/setup.bash" + +cd "${ROS_CONTROL_PY_DIR}" +if [[ ! -f "install/setup.bash" ]]; then + echo "Missing ROS workspace setup: ${ROS_CONTROL_PY_DIR}/install/setup.bash" >&2 + exit 1 +fi +source_with_nounset_off "install/setup.bash" + +launch_args=( + "transport:=${ROBOT_RECEIVER_TRANSPORT}" + "peer_id:=${ROBOT_RECEIVER_PEER_ID}" + "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}" +) + +if [[ -n "${ROBOT_RECEIVER_SERVER_ADDR}" ]]; then + launch_args+=("server_addr:=${ROBOT_RECEIVER_SERVER_ADDR}") +fi + +if [[ -n "${ROBOT_RECEIVER_RELAY_VIA}" ]]; then + launch_args+=("relay_via:=${ROBOT_RECEIVER_RELAY_VIA}") +fi + +if [[ -n "${ROBOT_RECEIVER_EXPECTED_SENDER}" ]]; then + launch_args+=("expected_sender:=${ROBOT_RECEIVER_EXPECTED_SENDER}") +fi + +exec ros2 launch udp_teleop_bridge robot_udp_receiver.launch.py "${launch_args[@]}" diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/kcp_control_benchmark.py b/robot/ros2/OmniSocketGo_robot_ros/scripts/kcp_control_benchmark.py new file mode 100644 index 0000000..90c5b23 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/kcp_control_benchmark.py @@ -0,0 +1,76 @@ +"""Send high-rate control packets to benchmark the KCP control session.""" + +from __future__ import annotations + +import argparse +from pathlib import Path +import sys +import time + +ROOT = Path(__file__).resolve().parents[1] +sys.path.insert(0, str(ROOT)) + +import yaml + +from omnisocket_control import make_control_packet + +try: + from omnisocket import CONTROL_DEFAULTS, Session +except ImportError: + sys.path.insert(0, str(ROOT / "python")) + from omnisocket import CONTROL_DEFAULTS, Session + + +def load_config() -> dict: + config_path = ROOT / "config" / "omnisocket_demo.yaml" + if not config_path.exists(): + return {} + with config_path.open("r", encoding="utf-8") as file: + return yaml.safe_load(file) or {} + + +def main() -> None: + parser = argparse.ArgumentParser() + parser.add_argument("--rate", type=float, default=200.0, help="send rate in Hz") + parser.add_argument("--count", type=int, default=1000, help="packets to send") + args = parser.parse_args() + + config = load_config() + transport_cfg = config.get("transport", {}) + sender_cfg = config.get("control_sender", {}) + + session = Session() + session.connect( + server_addr=str(transport_cfg.get("server_addr", "127.0.0.1:10909")), + peer_id=str(sender_cfg.get("peer_id", "peer-a-ctrl")), + relay_via=str(transport_cfg.get("relay_via", "")), + bind_ip=str(transport_cfg.get("bind_ip", "")), + bind_device=str(transport_cfg.get("bind_device", "")), + **CONTROL_DEFAULTS, + ) + + target_peer = str(sender_cfg.get("target_peer", "peer-b-ctrl")) + spacing = 1.0 / args.rate if args.rate > 0 else 0.0 + start = time.perf_counter() + + try: + for seq_id in range(args.count): + packet = make_control_packet(seq_id, "set_surge", drive_value=0.25) + session.send(to=target_peer, data=packet.encode()) + if spacing > 0: + target = start + (seq_id + 1) * spacing + remaining = target - time.perf_counter() + if remaining > 0: + time.sleep(remaining) + finally: + elapsed = time.perf_counter() - start + print( + f"sent {args.count} control packets in {elapsed:.3f}s " + f"({(args.count / elapsed) if elapsed > 0 else 0.0:.1f} pkt/s)" + ) + print(f"stats={session.stats()}") + session.close() + + +if __name__ == "__main__": + main() diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/refresh-latency-summary.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/refresh-latency-summary.sh new file mode 100644 index 0000000..6734e65 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/refresh-latency-summary.sh @@ -0,0 +1,136 @@ +#!/usr/bin/env bash + +set -u +set -o pipefail + +repo_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" +remote_source="boll@175.178.116.187:/home/boll/LMJWork/OmniSocketGo/peer-b-latency.jsonl" +local_peer_a="$repo_dir/peer-a-latency.jsonl" +local_peer_b="$repo_dir/peer-b-latency.jsonl" +summary_output="$repo_dir/latency-summary.jsonl" +chart_output="$repo_dir/latency-summary.html" +latency_binary="$repo_dir/bin/latencysummary" +go_cache_dir="${GOCACHE:-/tmp/omnisocketgo-go-build}" +poll_interval_seconds=1 + +remote_tmp="" +summary_tmp="" +chart_tmp="" + +log() { + printf '[%s] %s\n' "$(date '+%Y-%m-%d %H:%M:%S')" "$*" +} + +cleanup_temp_file() { + local path="$1" + if [[ -n "$path" && -e "$path" ]]; then + rm -f "$path" + fi +} + +cleanup() { + cleanup_temp_file "$remote_tmp" + cleanup_temp_file "$summary_tmp" + cleanup_temp_file "$chart_tmp" +} + +handle_interrupt() { + log "received interrupt signal, stopping refresh loop" + cleanup + exit 130 +} + +handle_terminate() { + log "received terminate signal, stopping refresh loop" + cleanup + exit 143 +} + +trap cleanup EXIT +trap handle_interrupt INT +trap handle_terminate TERM + +cd "$repo_dir" || exit 1 + +mkdir -p "$repo_dir/bin" +mkdir -p "$go_cache_dir" +if ! GOCACHE="$go_cache_dir" go build -o "$latency_binary" ./cmd/latencysummary; then + log "build failed; exiting" + exit 1 +fi + +log "starting 1-second refresh loop" + +while true; do + remote_tmp="$(mktemp "$repo_dir/peer-b-latency.jsonl.tmp.XXXXXX")" || exit 1 + if scp -P 10022 "$remote_source" "$remote_tmp"; then + if mv -f "$remote_tmp" "$local_peer_b"; then + remote_tmp="" + else + status=$? + log "failed to replace $(basename "$local_peer_b") after scp (exit $status)" + cleanup_temp_file "$remote_tmp" + remote_tmp="" + sleep "$poll_interval_seconds" + continue + fi + else + status=$? + log "scp refresh failed (exit $status)" + cleanup_temp_file "$remote_tmp" + remote_tmp="" + sleep "$poll_interval_seconds" + continue + fi + + summary_tmp="$(mktemp "$repo_dir/latency-summary.tmp.XXXXXX.jsonl")" || exit 1 + chart_tmp="${summary_tmp%.jsonl}.html" + if "$latency_binary" \ + -input "$local_peer_a" \ + -input "$local_peer_b" \ + -shared-max-offset 1 \ + -output "$summary_tmp"; then + if [[ ! -f "$summary_tmp" || ! -f "$chart_tmp" ]]; then + log "summary succeeded but temporary outputs are incomplete" + cleanup_temp_file "$summary_tmp" + cleanup_temp_file "$chart_tmp" + summary_tmp="" + chart_tmp="" + sleep "$poll_interval_seconds" + continue + fi + + if ! mv -f "$summary_tmp" "$summary_output"; then + status=$? + log "failed to replace $(basename "$summary_output") (exit $status)" + cleanup_temp_file "$summary_tmp" + cleanup_temp_file "$chart_tmp" + summary_tmp="" + chart_tmp="" + sleep "$poll_interval_seconds" + continue + fi + summary_tmp="" + + if ! mv -f "$chart_tmp" "$chart_output"; then + status=$? + log "failed to replace $(basename "$chart_output") (exit $status)" + cleanup_temp_file "$chart_tmp" + chart_tmp="" + sleep "$poll_interval_seconds" + continue + fi + chart_tmp="" + else + status=$? + log "latency summary refresh failed (exit $status)" + cleanup_temp_file "$summary_tmp" + cleanup_temp_file "$chart_tmp" + summary_tmp="" + chart_tmp="" + sleep "$poll_interval_seconds" + continue + fi + + sleep "$poll_interval_seconds" +done diff --git a/robot/ros2/OmniSocketGo_robot_ros/scripts/run-kcp-batch-test.sh b/robot/ros2/OmniSocketGo_robot_ros/scripts/run-kcp-batch-test.sh new file mode 100644 index 0000000..29c29d7 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/scripts/run-kcp-batch-test.sh @@ -0,0 +1,1202 @@ +#!/usr/bin/env bash + +set -euo pipefail + +script_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +repo_dir="$(cd "$script_dir/.." && pwd)" +script_name="$(basename "$0")" + +run_mode="direct" +server_ssh="" +peerb_ssh="" +relay_ssh="" +server_addr="" +relay_addr="" +relay_remote="" +log_prefix="" +listen_addr="0.0.0.0:10909" +relay_listen_addr="0.0.0.0:10909" +server_workdir="$repo_dir" +peerb_workdir="$repo_dir" +relay_workdir="$repo_dir" +local_workdir="$repo_dir" +ready_timeout=60 +send_interval=1 +drain_wait=5 +repeat_count=1 + +declare -a peerb_files=() + +server_started=0 +relay_started=0 +peer_b_started=0 +peer_a_pid="" + +usage() { + printf 'Usage:\n' + printf ' %s --mode --server-ssh --peerb-ssh \\\n' "$script_name" + printf ' --server-addr --log-prefix --file [options]\n' + printf '\n' + printf 'Modes:\n' + printf ' direct peer-a -> hub(server) <- peer-b (default)\n' + printf ' relay peer-a -> relay(C) -> hub(D) <- peer-b\n' + printf '\n' + printf 'Required arguments:\n' + printf ' --server-ssh SSH target for the hub server machine\n' + printf ' --peerb-ssh SSH target for the peer-b machine\n' + printf ' --server-addr Hub server IP (combined with listen port for peers)\n' + printf ' --log-prefix Log directory prefix; logs go under logs/\n' + printf ' --file Existing file path on peer-b; repeat for multiple files\n' + printf '\n' + printf 'Relay mode arguments (required when --mode=relay):\n' + printf ' --relay-ssh SSH target for the relay server machine\n' + printf ' --relay-addr Relay server IP (combined with relay listen port for peer-a)\n' + printf ' --relay-remote Hub address from relay perspective (relay -relay-remote)\n' + printf '\n' + printf 'Options:\n' + printf ' --mode Run mode (default: %s)\n' "$run_mode" + printf ' --listen-addr Hub server listen address (default: %s)\n' "$listen_addr" + printf ' --relay-listen-addr Relay server listen address (default: %s)\n' "$relay_listen_addr" + printf ' --server-workdir Hub server-side workdir (default: %s)\n' "$server_workdir" + printf ' --relay-workdir Relay server-side workdir (default: %s)\n' "$relay_workdir" + printf ' --peerb-workdir Peer-b-side workdir (default: %s)\n' "$peerb_workdir" + printf ' --local-workdir Local peer-a workdir (default: %s)\n' "$local_workdir" + printf ' --ready-timeout Startup wait timeout (default: %s)\n' "$ready_timeout" + printf ' --repeat Repeat the full --file list this many rounds (default: %s)\n' "$repeat_count" + printf ' --send-interval Delay between file commands (default: %s)\n' "$send_interval" + printf ' --drain-wait Wait after the last file before quit (default: %s)\n' "$drain_wait" + printf ' -h, --help Show this help\n' + printf '\n' + printf 'Example (direct mode):\n' + printf ' %s \\\n' "$script_name" + printf ' --mode direct \\\n' + printf ' --server-ssh root@server-host \\\n' + printf ' --peerb-ssh root@peer-b-host \\\n' + printf ' --server-addr 203.0.113.10 \\\n' + printf ' --log-prefix case01- \\\n' + printf ' --repeat 30 \\\n' + printf ' --file /tmp/test125.bin\n' + printf '\n' + printf 'Example (relay mode):\n' + printf ' %s \\\n' "$script_name" + printf ' --mode relay \\\n' + printf ' --server-ssh root@hub-host \\\n' + printf ' --relay-ssh root@relay-host \\\n' + printf ' --peerb-ssh root@peer-b-host \\\n' + printf ' --server-addr 152.136.164.246 \\\n' + printf ' --relay-addr 139.199.57.110 \\\n' + printf ' --relay-remote 172.21.0.13:10909 \\\n' + printf ' --log-prefix case01- \\\n' + printf ' --repeat 30 \\\n' + printf ' --file /tmp/test125.bin\n' +} + +log() { + printf '[%s] %s\n' "$(date '+%Y-%m-%d %H:%M:%S')" "$*" +} + +die() { + printf >&2 '[%s] error: %s\n' "$(date '+%Y-%m-%d %H:%M:%S')" "$*" + exit 1 +} + +join_path() { + local base="${1%/}" + printf '%s/%s' "$base" "$2" +} + +build_quoted_command() { + local out_var="$1" + shift + + local command="" + local part="" + local quoted="" + for part in "$@"; do + printf -v quoted '%q' "$part" + if [[ -n "$command" ]]; then + command+=" " + fi + command+="$quoted" + done + + printf -v "$out_var" '%s' "$command" +} + +run_remote_script() { + local target="$1" + local script="$2" + shift 2 + + local parts=("env") + local assignment="" + for assignment in "$@"; do + parts+=("$assignment") + done + parts+=("bash" "-s" "--") + + local remote_cmd="" + build_quoted_command remote_cmd "${parts[@]}" + ssh -T "$target" "$remote_cmd" <<<"$script" +} + +validate_positive_integer() { + local name="$1" + local value="$2" + + if [[ ! "$value" =~ ^[1-9][0-9]*$ ]]; then + die "$name must be a positive integer, got: $value" + fi +} + +validate_sleep_value() { + local name="$1" + local value="$2" + + if [[ ! "$value" =~ ^([0-9]+([.][0-9]+)?|[.][0-9]+)$ ]]; then + die "$name must be a non-negative number understood by sleep, got: $value" + fi +} + +dump_local_log_head() { + local path="$1" + + if [[ -f "$path" ]]; then + sed -n '1,120p' "$path" >&2 || true + fi +} + +dump_remote_log_head() { + local target="$1" + local log_file="$2" + local label="$3" + local script="" + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ -f "$LOG_FILE" ]]; then + sed -n '1,120p' "$LOG_FILE" +fi +EOF +)" + + log "showing $label log head from $target" + run_remote_script "$target" "$script" "LOG_FILE=$log_file" || true +} + +check_local_dependencies() { + command -v ssh >/dev/null 2>&1 || die "ssh is required" + command -v scp >/dev/null 2>&1 || die "scp is required" + command -v go >/dev/null 2>&1 || die "go is required for local peer-a" +} + +copy_remote_file_to_local() { + local remote_source="$1" + local local_dest="$2" + local local_dir="" + local local_tmp="" + + local_dir="$(dirname "$local_dest")" + mkdir -p "$local_dir" + local_tmp="$(mktemp "$local_dir/.copy.tmp.XXXXXX")" + + if scp "$remote_source" "$local_tmp"; then + mv -f "$local_tmp" "$local_dest" + else + local status=$? + rm -f "$local_tmp" + return "$status" + fi +} + +remove_local_log_dir() { + if [[ -e "$local_log_dir" ]]; then + log "removing local log dir: $local_log_dir" + rm -rf "$local_log_dir" + fi +} + +remove_remote_log_dir() { + local target="$1" + local log_dir="$2" + local label="$3" + local pid_file="${4:-}" + local script="" + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ -n "${PID_FILE:-}" && -f "$PID_FILE" ]]; then + existing_pid="$(<"$PID_FILE")" + if [[ -n "$existing_pid" ]] && kill -0 "$existing_pid" 2>/dev/null; then + printf >&2 'refusing to remove log dir while process %s is still running\n' "$existing_pid" + exit 1 + fi +fi + +rm -rf "$LOG_DIR" +EOF +)" + + log "removing $label log dir on $target: $log_dir" + run_remote_script "$target" "$script" \ + "LOG_DIR=$log_dir" \ + "PID_FILE=$pid_file" +} + +clean_log_directories() { + remove_local_log_dir + remove_remote_log_dir "$server_ssh" "$server_log_dir" "server" "$server_pid_file" + remove_remote_log_dir "$peerb_ssh" "$peerb_log_dir" "peer-b" + if [[ "$run_mode" == "relay" ]]; then + remove_remote_log_dir "$relay_ssh" "$relay_log_dir" "relay" "$relay_pid_file" + fi +} + +truncate_local_file() { + local path="$1" + local dir="" + + dir="$(dirname "$path")" + mkdir -p "$dir" + : > "$path" +} + +truncate_remote_file() { + local target="$1" + local path="$2" + local script="" + + script="$(cat <<'EOF' +set -euo pipefail + +mkdir -p "$(dirname "$FILE_PATH")" +: > "$FILE_PATH" +EOF +)" + + run_remote_script "$target" "$script" "FILE_PATH=$path" +} + +reset_logs_after_probe() { + log "resetting peer logs after connectivity probe" + + rm -f "$local_peer_a_messages_log" + truncate_local_file "$local_peer_a_stdout_log" + truncate_local_file "$local_peer_a_latency_log" + truncate_local_file "$local_peer_a_ts_debug_log" + truncate_local_file "$local_peer_a_session_stats_log" + + truncate_remote_file "$peerb_ssh" "$peerb_stdout_log" + truncate_remote_file "$peerb_ssh" "$peerb_latency_log" + truncate_remote_file "$peerb_ssh" "$peerb_ts_debug_log" + truncate_remote_file "$peerb_ssh" "$peerb_session_stats_log" +} + +fetch_remote_peer_b_logs() { + log "copying peer-b latency log from $peerb_ssh:$peerb_latency_log to $local_peer_b_latency_log" + copy_remote_file_to_local "$peerb_ssh:$peerb_latency_log" "$local_peer_b_latency_log" +} + +run_local_latency_summary() { + [[ -f "$local_peer_a_latency_log" ]] || die "local peer-a latency log not found: $local_peer_a_latency_log" + [[ -f "$local_peer_b_latency_log" ]] || die "local peer-b latency log not found: $local_peer_b_latency_log" + + log "generating local latency summary: $local_kcp_latency_summary_log" + ( + cd "$repo_dir" + exec go run ./cmd/latencysummary \ + -input "$local_peer_a_latency_log" \ + -input "$local_peer_b_latency_log" \ + -output "$local_kcp_latency_summary_log" + ) +} + +check_remote_peerb_files() { + local script="" + local file="" + + script="$(cat <<'EOF' +set -euo pipefail + +cd "$PEERB_WORKDIR" +if [[ ! -f "$FILE_PATH" ]]; then + printf >&2 'peer-b file not found: %s\n' "$FILE_PATH" + exit 1 +fi +EOF +)" + + for file in "${peerb_files[@]}"; do + log "checking peer-b file exists: $file" + run_remote_script "$peerb_ssh" "$script" \ + "PEERB_WORKDIR=$peerb_workdir" \ + "FILE_PATH=$file" + done +} + +start_remote_server() { + local script="" + + script="$(cat <<'EOF' +export PATH="$PATH:/usr/local/go/bin:$HOME/go/bin" +set -euo pipefail + +cd "$SERVER_WORKDIR" +mkdir -p "$LOG_DIR" + +if [[ -f "$PID_FILE" ]]; then + existing_pid="$(<"$PID_FILE")" + if [[ -n "$existing_pid" ]] && kill -0 "$existing_pid" 2>/dev/null; then + printf >&2 'server already running with pid %s\n' "$existing_pid" + exit 1 + fi +fi + +: > "$STDOUT_LOG" +setsid go run ./cmd/kcpserver/ \ + -listen "$LISTEN_ADDR" \ + >>"$STDOUT_LOG" 2>&1 "$PID_FILE" +EOF +)" + + log "starting remote kcpserver (hub) on $server_ssh" + run_remote_script "$server_ssh" "$script" \ + "SERVER_WORKDIR=$server_workdir" \ + "LOG_DIR=$server_log_dir" \ + "PID_FILE=$server_pid_file" \ + "STDOUT_LOG=$server_stdout_log" \ + "LISTEN_ADDR=$listen_addr" + + server_started=1 +} + +wait_for_remote_server_ready() { + local pattern="kcp hub listening" + local script="" + local start_time="$SECONDS" + local status=0 + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ -f "$LOG_FILE" ]] && grep -Fq -- "$READY_PATTERN" "$LOG_FILE"; then + exit 0 +fi + +if [[ -f "$PID_FILE" ]]; then + pid="$(<"$PID_FILE")" + if [[ -n "$pid" ]] && kill -0 "$pid" 2>/dev/null; then + exit 10 + fi +fi + +exit 20 +EOF +)" + + while (( SECONDS - start_time < ready_timeout )); do + status=0 + run_remote_script "$server_ssh" "$script" \ + "LOG_FILE=$server_stdout_log" \ + "READY_PATTERN=$pattern" \ + "PID_FILE=$server_pid_file" || status=$? + + case "$status" in + 0) + log "remote server is ready" + return 0 + ;; + 10) + sleep 1 + ;; + 20) + log "remote server exited before readiness" + dump_remote_log_head "$server_ssh" "$server_stdout_log" "server" + return 1 + ;; + *) + log "remote server readiness check failed with status $status" + dump_remote_log_head "$server_ssh" "$server_stdout_log" "server" + return 1 + ;; + esac + done + + log "timed out waiting for remote server readiness after ${ready_timeout}s" + dump_remote_log_head "$server_ssh" "$server_stdout_log" "server" + return 1 +} + +stop_remote_server() { + local script="" + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ ! -f "$PID_FILE" ]]; then + exit 0 +fi + +pid="$(<"$PID_FILE")" +if [[ -z "$pid" ]]; then + rm -f "$PID_FILE" + exit 0 +fi + +# Kill the entire process group (setsid creates a new group with pid == pgid). +kill -- -"$pid" 2>/dev/null || kill "$pid" 2>/dev/null || true +for _ in 1 2 3 4 5; do + if ! kill -0 "$pid" 2>/dev/null; then + rm -f "$PID_FILE" + exit 0 + fi + sleep 1 +done + +kill -9 -- -"$pid" 2>/dev/null || kill -9 "$pid" 2>/dev/null || true +rm -f "$PID_FILE" +EOF +)" + + run_remote_script "$server_ssh" "$script" "PID_FILE=$server_pid_file" +} + +start_remote_relay() { + local script="" + + script="$(cat <<'EOF' +export PATH="$PATH:/usr/local/go/bin:$HOME/go/bin" +set -euo pipefail + +cd "$RELAY_WORKDIR" +mkdir -p "$LOG_DIR" + +if [[ -f "$PID_FILE" ]]; then + existing_pid="$(<"$PID_FILE")" + if [[ -n "$existing_pid" ]] && kill -0 "$existing_pid" 2>/dev/null; then + printf >&2 'relay already running with pid %s\n' "$existing_pid" + exit 1 + fi +fi + +: > "$STDOUT_LOG" +setsid go run ./cmd/kcpserver/ \ + -mode=relay \ + -listen "$LISTEN_ADDR" \ + -relay-remote "$RELAY_REMOTE" \ + >>"$STDOUT_LOG" 2>&1 "$PID_FILE" +EOF +)" + + log "starting remote relay on $relay_ssh" + run_remote_script "$relay_ssh" "$script" \ + "RELAY_WORKDIR=$relay_workdir" \ + "LOG_DIR=$relay_log_dir" \ + "PID_FILE=$relay_pid_file" \ + "STDOUT_LOG=$relay_stdout_log" \ + "LISTEN_ADDR=$relay_listen_addr" \ + "RELAY_REMOTE=$relay_remote" + + relay_started=1 +} + +wait_for_remote_relay_ready() { + local pattern="udp relay listening" + local script="" + local start_time="$SECONDS" + local status=0 + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ -f "$LOG_FILE" ]] && grep -Fq -- "$READY_PATTERN" "$LOG_FILE"; then + exit 0 +fi + +if [[ -f "$PID_FILE" ]]; then + pid="$(<"$PID_FILE")" + if [[ -n "$pid" ]] && kill -0 "$pid" 2>/dev/null; then + exit 10 + fi +fi + +exit 20 +EOF +)" + + while (( SECONDS - start_time < ready_timeout )); do + status=0 + run_remote_script "$relay_ssh" "$script" \ + "LOG_FILE=$relay_stdout_log" \ + "READY_PATTERN=$pattern" \ + "PID_FILE=$relay_pid_file" || status=$? + + case "$status" in + 0) + log "remote relay is ready" + return 0 + ;; + 10) + sleep 1 + ;; + 20) + log "remote relay exited before readiness" + dump_remote_log_head "$relay_ssh" "$relay_stdout_log" "relay" + return 1 + ;; + *) + log "remote relay readiness check failed with status $status" + dump_remote_log_head "$relay_ssh" "$relay_stdout_log" "relay" + return 1 + ;; + esac + done + + log "timed out waiting for remote relay readiness after ${ready_timeout}s" + dump_remote_log_head "$relay_ssh" "$relay_stdout_log" "relay" + return 1 +} + +stop_remote_relay() { + local script="" + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ ! -f "$PID_FILE" ]]; then + exit 0 +fi + +pid="$(<"$PID_FILE")" +if [[ -z "$pid" ]]; then + rm -f "$PID_FILE" + exit 0 +fi + +kill -- -"$pid" 2>/dev/null || kill "$pid" 2>/dev/null || true +for _ in 1 2 3 4 5; do + if ! kill -0 "$pid" 2>/dev/null; then + rm -f "$PID_FILE" + exit 0 + fi + sleep 1 +done + +kill -9 -- -"$pid" 2>/dev/null || kill -9 "$pid" 2>/dev/null || true +rm -f "$PID_FILE" +EOF +)" + + run_remote_script "$relay_ssh" "$script" "PID_FILE=$relay_pid_file" +} + +start_local_peer_a() { + log "starting local peer-a" + mkdir -p "$local_log_dir" "$local_peer_a_inbox" + : > "$local_peer_a_stdout_log" + + local peer_a_args=( + -id peer-a + -server "$server_connect_addr" + -inbox-dir "$local_peer_a_inbox" + -latency-log "$local_peer_a_latency_log" + -kcp-ts-debug-log "$local_peer_a_ts_debug_log" + -kcp-session-stats-log "$local_peer_a_session_stats_log" + -interactive=false + ) + + if [[ "$run_mode" == "relay" ]]; then + peer_a_args+=(-relay-via "$relay_connect_addr") + fi + + ( + cd "$local_workdir" + exec go run ./cmd/kcppeer "${peer_a_args[@]}" \ + >>"$local_peer_a_stdout_log" 2>&1 + ) & + + peer_a_pid="$!" +} + +wait_for_local_peer_a_ready() { + local pattern="opened KCP session as peer-a" + local start_time="$SECONDS" + + while (( SECONDS - start_time < ready_timeout )); do + if [[ -f "$local_peer_a_stdout_log" ]] && grep -Fq -- "$pattern" "$local_peer_a_stdout_log"; then + log "local peer-a is ready" + return 0 + fi + + if [[ -n "$peer_a_pid" ]] && ! kill -0 "$peer_a_pid" 2>/dev/null; then + log "local peer-a exited before readiness" + dump_local_log_head "$local_peer_a_stdout_log" + return 1 + fi + + sleep 1 + done + + log "timed out waiting for local peer-a readiness after ${ready_timeout}s" + dump_local_log_head "$local_peer_a_stdout_log" + return 1 +} + +stop_local_peer_a() { + if [[ -z "$peer_a_pid" ]]; then + return 0 + fi + + if kill -0 "$peer_a_pid" 2>/dev/null; then + kill "$peer_a_pid" 2>/dev/null || true + wait "$peer_a_pid" 2>/dev/null || true + else + wait "$peer_a_pid" 2>/dev/null || true + fi + + peer_a_pid="" +} + +start_remote_peer_b() { + local script="" + + script="$(cat <<'EOF' +export PATH="$PATH:/usr/local/go/bin:$HOME/go/bin" +set -euo pipefail + +cd "$PEERB_WORKDIR" +mkdir -p "$LOG_DIR" "$INBOX_DIR" + +if [[ -f "$PID_FILE" ]]; then + existing_pid="$(<"$PID_FILE")" + if [[ -n "$existing_pid" ]] && kill -0 "$existing_pid" 2>/dev/null; then + printf >&2 'peer-b already running with pid %s\n' "$existing_pid" + exit 1 + fi +fi + +: > "$STDOUT_LOG" +: > "$COMMAND_FILE" + + peer_b_cmd="$(cat <<'INNER' + tail -n +1 -f "$COMMAND_FILE" | exec go run ./cmd/kcppeer/ \ + -id peer-b \ + -server "$SERVER_ADDR" \ + -inbox-dir "$INBOX_DIR" \ + -latency-log "$LATENCY_LOG" \ + -kcp-ts-debug-log "$TS_DEBUG_LOG" \ + -kcp-session-stats-log "$SESSION_STATS_LOG" +INNER +)" + +nohup setsid bash -lc "$peer_b_cmd" >>"$STDOUT_LOG" 2>&1 "$PID_FILE" +EOF +)" + + log "starting remote peer-b on $peerb_ssh" + run_remote_script "$peerb_ssh" "$script" \ + "PEERB_WORKDIR=$peerb_workdir" \ + "LOG_DIR=$peerb_log_dir" \ + "INBOX_DIR=$peerb_inbox_dir" \ + "STDOUT_LOG=$peerb_stdout_log" \ + "COMMAND_FILE=$peerb_command_file" \ + "PID_FILE=$peerb_pid_file" \ + "SERVER_ADDR=$server_connect_addr" \ + "LATENCY_LOG=$peerb_latency_log" \ + "TS_DEBUG_LOG=$peerb_ts_debug_log" \ + "SESSION_STATS_LOG=$peerb_session_stats_log" + + peer_b_started=1 +} + +wait_for_remote_peer_b_ready() { + local pattern="opened KCP session as peer-b" + local script="" + local start_time="$SECONDS" + local status=0 + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ -f "$LOG_FILE" ]] && grep -Fq -- "$READY_PATTERN" "$LOG_FILE"; then + exit 0 +fi + +if [[ -f "$PID_FILE" ]]; then + pid="$(<"$PID_FILE")" + if [[ -n "$pid" ]] && kill -0 "$pid" 2>/dev/null; then + exit 10 + fi +fi + +exit 20 +EOF +)" + + while (( SECONDS - start_time < ready_timeout )); do + status=0 + run_remote_script "$peerb_ssh" "$script" \ + "LOG_FILE=$peerb_stdout_log" \ + "READY_PATTERN=$pattern" \ + "PID_FILE=$peerb_pid_file" || status=$? + + case "$status" in + 0) + log "remote peer-b is ready" + return 0 + ;; + 10) + sleep 1 + ;; + 20) + log "remote peer-b exited before readiness" + dump_remote_log_head "$peerb_ssh" "$peerb_stdout_log" "peer-b" + return 1 + ;; + *) + log "remote peer-b readiness check failed with status $status" + dump_remote_log_head "$peerb_ssh" "$peerb_stdout_log" "peer-b" + return 1 + ;; + esac + done + + log "timed out waiting for remote peer-b readiness after ${ready_timeout}s" + dump_remote_log_head "$peerb_ssh" "$peerb_stdout_log" "peer-b" + return 1 +} + +probe_peer_b_to_local_peer_a() { + local marker="" + local command_line="" + local quoted_command="" + local script="" + local start_time="$SECONDS" + + marker="probe-$(date +%s)-$$" + printf -v command_line 'text peer-a %s' "$marker" + printf -v quoted_command '%q' "$command_line" + + script="$(cat <> "\$COMMAND_FILE" +EOF +)" + + log "probing peer-b -> peer-a message delivery before batch" + run_remote_script "$peerb_ssh" "$script" "COMMAND_FILE=$peerb_command_file" + + while (( SECONDS - start_time < ready_timeout )); do + if [[ -f "$local_peer_a_messages_log" ]] && grep -Fq -- "$marker" "$local_peer_a_messages_log"; then + log "peer-b -> peer-a probe succeeded" + reset_logs_after_probe + return 0 + fi + + if [[ -n "$peer_a_pid" ]] && ! kill -0 "$peer_a_pid" 2>/dev/null; then + log "local peer-a exited during connectivity probe" + dump_local_log_head "$local_peer_a_stdout_log" + return 1 + fi + + sleep 1 + done + + log "timed out waiting for peer-b -> peer-a probe delivery after ${ready_timeout}s" + dump_local_log_head "$local_peer_a_stdout_log" + dump_remote_log_head "$peerb_ssh" "$peerb_stdout_log" "peer-b" + return 1 +} + +run_remote_peer_b_batch() { + local script="" + local batch_commands="" + local round=0 + local i=0 + local send_index=0 + local total_sends=$(( ${#peerb_files[@]} * repeat_count )) + local file="" + local command_line="" + local quoted_command="" + local quoted_sleep="" + + for (( round = 1; round <= repeat_count; round++ )); do + for (( i = 0; i < ${#peerb_files[@]}; i++ )); do + file="${peerb_files[$i]}" + send_index=$(( send_index + 1 )) + log "queueing peer-b -> peer-a file (round $round/$repeat_count, send $send_index/$total_sends): $file" + printf -v command_line 'file peer-a %s' "$file" + printf -v quoted_command '%q' "$command_line" + batch_commands+="printf '%s\n' ${quoted_command} >> \"\$COMMAND_FILE\""$'\n' + if (( send_index < total_sends )); then + printf -v quoted_sleep '%q' "$send_interval" + batch_commands+="sleep ${quoted_sleep}"$'\n' + fi + done + done + printf -v quoted_sleep '%q' "$drain_wait" + batch_commands+="sleep ${quoted_sleep}"$'\n' + batch_commands+="printf '%s\n' quit >> \"\$COMMAND_FILE\""$'\n' + + script="$(cat <&2 'peer-b pid file not found: %s\n' "\$PID_FILE" + exit 1 +fi + +pid="\$(<"\$PID_FILE")" +if [[ -z "\$pid" ]] || ! kill -0 "\$pid" 2>/dev/null; then + printf >&2 'peer-b is not running\n' + exit 1 +fi + +$batch_commands + +for (( i = 0; i < READY_TIMEOUT; i++ )); do + if ! kill -0 "\$pid" 2>/dev/null; then + rm -f "\$PID_FILE" "\$COMMAND_FILE" + exit 0 + fi + sleep 1 +done + +printf >&2 'peer-b did not exit after quit within %s seconds\n' "\$READY_TIMEOUT" +exit 1 +EOF +)" + + log "sending ${#peerb_files[@]} files across $repeat_count rounds ($total_sends sends total) from peer-b" + run_remote_script "$peerb_ssh" "$script" \ + "PID_FILE=$peerb_pid_file" \ + "COMMAND_FILE=$peerb_command_file" \ + "READY_TIMEOUT=$ready_timeout" + + peer_b_started=0 +} + +stop_remote_peer_b() { + local script="" + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ ! -f "$PID_FILE" ]]; then + rm -f "$COMMAND_FILE" + exit 0 +fi + +pid="$(<"$PID_FILE")" +if [[ -z "$pid" ]]; then + rm -f "$PID_FILE" "$COMMAND_FILE" + exit 0 +fi + +if kill -0 "$pid" 2>/dev/null; then + printf 'quit\n' >> "$COMMAND_FILE" 2>/dev/null || true + for _ in 1 2 3 4 5; do + if ! kill -0 "$pid" 2>/dev/null; then + rm -f "$PID_FILE" "$COMMAND_FILE" + exit 0 + fi + sleep 1 + done + kill -- -"$pid" 2>/dev/null || kill "$pid" 2>/dev/null || true + for _ in 1 2 3 4 5; do + if ! kill -0 "$pid" 2>/dev/null; then + rm -f "$PID_FILE" "$COMMAND_FILE" + exit 0 + fi + sleep 1 + done + kill -9 -- -"$pid" 2>/dev/null || kill -9 "$pid" 2>/dev/null || true +fi + +rm -f "$PID_FILE" "$COMMAND_FILE" +EOF +)" + + run_remote_script "$peerb_ssh" "$script" \ + "PID_FILE=$peerb_pid_file" \ + "COMMAND_FILE=$peerb_command_file" +} + +cleanup() { + local exit_code="$?" + + trap - EXIT INT TERM + + if [[ -n "$peer_a_pid" ]]; then + log "stopping local peer-a" + stop_local_peer_a + fi + + if (( peer_b_started == 1 )); then + log "stopping remote peer-b on $peerb_ssh" + stop_remote_peer_b || true + fi + + if (( relay_started == 1 )); then + log "stopping remote relay on $relay_ssh" + stop_remote_relay || true + fi + + if (( server_started == 1 )); then + log "stopping remote server on $server_ssh" + stop_remote_server || true + fi + + exit "$exit_code" +} + +handle_interrupt() { + log "received interrupt signal" + exit 130 +} + +handle_terminate() { + log "received terminate signal" + exit 143 +} + +while [[ $# -gt 0 ]]; do + case "$1" in + --mode) + [[ $# -ge 2 ]] || die "--mode requires a value" + run_mode="$2" + shift 2 + ;; + --server-ssh) + [[ $# -ge 2 ]] || die "--server-ssh requires a value" + server_ssh="$2" + shift 2 + ;; + --peerb-ssh) + [[ $# -ge 2 ]] || die "--peerb-ssh requires a value" + peerb_ssh="$2" + shift 2 + ;; + --relay-ssh) + [[ $# -ge 2 ]] || die "--relay-ssh requires a value" + relay_ssh="$2" + shift 2 + ;; + --server-addr) + [[ $# -ge 2 ]] || die "--server-addr requires a value" + server_addr="$2" + shift 2 + ;; + --relay-addr) + [[ $# -ge 2 ]] || die "--relay-addr requires a value" + relay_addr="$2" + shift 2 + ;; + --relay-remote) + [[ $# -ge 2 ]] || die "--relay-remote requires a value" + relay_remote="$2" + shift 2 + ;; + --log-prefix) + [[ $# -ge 2 ]] || die "--log-prefix requires a value" + log_prefix="$2" + shift 2 + ;; + --listen-addr) + [[ $# -ge 2 ]] || die "--listen-addr requires a value" + listen_addr="$2" + shift 2 + ;; + --relay-listen-addr) + [[ $# -ge 2 ]] || die "--relay-listen-addr requires a value" + relay_listen_addr="$2" + shift 2 + ;; + --server-workdir) + [[ $# -ge 2 ]] || die "--server-workdir requires a value" + server_workdir="$2" + shift 2 + ;; + --relay-workdir) + [[ $# -ge 2 ]] || die "--relay-workdir requires a value" + relay_workdir="$2" + shift 2 + ;; + --peerb-workdir) + [[ $# -ge 2 ]] || die "--peerb-workdir requires a value" + peerb_workdir="$2" + shift 2 + ;; + --local-workdir) + [[ $# -ge 2 ]] || die "--local-workdir requires a value" + local_workdir="$2" + shift 2 + ;; + --ready-timeout) + [[ $# -ge 2 ]] || die "--ready-timeout requires a value" + ready_timeout="$2" + shift 2 + ;; + --repeat) + [[ $# -ge 2 ]] || die "--repeat requires a value" + repeat_count="$2" + shift 2 + ;; + --send-interval) + [[ $# -ge 2 ]] || die "--send-interval requires a value" + send_interval="$2" + shift 2 + ;; + --drain-wait) + [[ $# -ge 2 ]] || die "--drain-wait requires a value" + drain_wait="$2" + shift 2 + ;; + --file) + [[ $# -ge 2 ]] || die "--file requires a value" + peerb_files+=("$2") + shift 2 + ;; + -h|--help) + usage + exit 0 + ;; + *) + die "unknown argument: $1" + ;; + esac +done + +[[ "$run_mode" == "direct" || "$run_mode" == "relay" ]] || die "--mode must be 'direct' or 'relay', got: $run_mode" +[[ -n "$server_ssh" ]] || die "--server-ssh is required" +[[ -n "$peerb_ssh" ]] || die "--peerb-ssh is required" +[[ -n "$server_addr" ]] || die "--server-addr is required" +[[ -n "$log_prefix" ]] || die "--log-prefix is required" +(( ${#peerb_files[@]} > 0 )) || die "at least one --file is required" + +if [[ "$run_mode" == "relay" ]]; then + [[ -n "$relay_ssh" ]] || die "--relay-ssh is required in relay mode" + [[ -n "$relay_addr" ]] || die "--relay-addr is required in relay mode" + [[ -n "$relay_remote" ]] || die "--relay-remote is required in relay mode" +fi + +validate_positive_integer "--ready-timeout" "$ready_timeout" +validate_positive_integer "--repeat" "$repeat_count" +validate_sleep_value "--send-interval" "$send_interval" +validate_sleep_value "--drain-wait" "$drain_wait" + +check_local_dependencies + +# Extract ports and build peer connection addresses. +server_port="${listen_addr##*:}" +server_connect_addr="${server_addr}:${server_port}" + +relay_connect_addr="" +if [[ "$run_mode" == "relay" ]]; then + relay_port="${relay_listen_addr##*:}" + relay_connect_addr="${relay_addr}:${relay_port}" +fi + +log_dir_name="${log_prefix}logs" +inbox_dir_name="${log_prefix}inbox" + +local_log_dir="$(join_path "$local_workdir" "$log_dir_name")" +local_peer_a_inbox="$(join_path "$local_workdir" "$inbox_dir_name/peer-a")" +local_peer_a_messages_log="$(join_path "$local_peer_a_inbox" "messages.log")" +local_peer_a_stdout_log="$(join_path "$local_log_dir" "peer-a.stdout.log")" +local_peer_a_latency_log="$(join_path "$local_log_dir" "peer-a-kcp-latency.jsonl")" +local_peer_a_ts_debug_log="$(join_path "$local_log_dir" "peer-a-kcp-packet-debug.jsonl")" +local_peer_a_session_stats_log="$(join_path "$local_log_dir" "peer-a-kcp-session-stats.jsonl")" +local_peer_b_stdout_log="$(join_path "$local_log_dir" "peer-b.stdout.log")" +local_peer_b_latency_log="$(join_path "$local_log_dir" "peer-b-kcp-latency.jsonl")" +local_peer_b_ts_debug_log="$(join_path "$local_log_dir" "peer-b-kcp-packet-debug.jsonl")" +local_peer_b_session_stats_log="$(join_path "$local_log_dir" "peer-b-kcp-session-stats.jsonl")" +local_kcp_latency_summary_log="$(join_path "$local_log_dir" "kcp-latency-summary.jsonl")" + +server_log_dir="$(join_path "$server_workdir" "$log_dir_name")" +server_pid_file="$(join_path "$server_log_dir" "server.pid")" +server_stdout_log="$(join_path "$server_log_dir" "server.stdout.log")" + +relay_log_dir="" +relay_pid_file="" +relay_stdout_log="" +if [[ "$run_mode" == "relay" ]]; then + relay_log_dir="$(join_path "$relay_workdir" "$log_dir_name")" + relay_pid_file="$(join_path "$relay_log_dir" "relay.pid")" + relay_stdout_log="$(join_path "$relay_log_dir" "relay.stdout.log")" +fi + +peerb_log_dir="$(join_path "$peerb_workdir" "$log_dir_name")" +peerb_inbox_dir="$(join_path "$peerb_workdir" "$inbox_dir_name/peer-b")" +peerb_stdout_log="$(join_path "$peerb_log_dir" "peer-b.stdout.log")" +peerb_latency_log="$(join_path "$peerb_log_dir" "peer-b-kcp-latency.jsonl")" +peerb_ts_debug_log="$(join_path "$peerb_log_dir" "peer-b-kcp-packet-debug.jsonl")" +peerb_session_stats_log="$(join_path "$peerb_log_dir" "peer-b-kcp-session-stats.jsonl")" +peerb_pid_file="$(join_path "$peerb_log_dir" "peer-b.pid")" +peerb_command_file="$(join_path "$peerb_log_dir" "peer-b.commands")" + +trap cleanup EXIT +trap handle_interrupt INT +trap handle_terminate TERM + +clean_log_directories + +mkdir -p "$local_log_dir" "$local_peer_a_inbox" + +log "run mode: $run_mode" +log "local peer-a logs: $local_log_dir" +log "remote server logs: $server_log_dir" +if [[ "$run_mode" == "relay" ]]; then + log "remote relay logs: $relay_log_dir" +fi +log "remote peer-b logs: $peerb_log_dir" + +check_remote_peerb_files +start_remote_server +wait_for_remote_server_ready + +if [[ "$run_mode" == "relay" ]]; then + start_remote_relay + wait_for_remote_relay_ready +fi + +start_local_peer_a +start_remote_peer_b +wait_for_local_peer_a_ready +wait_for_remote_peer_b_ready +probe_peer_b_to_local_peer_a +run_remote_peer_b_batch + +log "batch send completed" + +if [[ -n "$peer_a_pid" ]]; then + log "stopping local peer-a after batch" + stop_local_peer_a +fi + +if (( relay_started == 1 )); then + log "stopping remote relay on $relay_ssh after batch" + if stop_remote_relay; then + relay_started=0 + else + log "failed to stop remote relay cleanly; cleanup will retry" + fi +fi + +if (( server_started == 1 )); then + log "stopping remote server on $server_ssh after batch" + if stop_remote_server; then + server_started=0 + else + log "failed to stop remote server cleanly; cleanup will retry" + fi +fi + +fetch_remote_peer_b_logs +run_local_latency_summary diff --git a/robot/ros2/OmniSocketGo_robot_ros/src/gps_buffer.c b/robot/ros2/OmniSocketGo_robot_ros/src/gps_buffer.c new file mode 100644 index 0000000..9d65b91 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/src/gps_buffer.c @@ -0,0 +1,333 @@ +#include "gps_buffer.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include // 确保包含 errno + +// 全局共享变量 +static gps_video_sample_t g_current_gps_data = {0.0, 0.0}; +static volatile int g_running = 0; +static pthread_t g_gps_thread; +static pthread_mutex_t g_gps_mutex = PTHREAD_MUTEX_INITIALIZER; + +static double normalize_coordinate(double coordinate) { + return round(coordinate * 1000000.0) / 1000000.0; +} + +static void store_gps(double latitude, double longitude) { + pthread_mutex_lock(&g_gps_mutex); + g_current_gps_data.latitude = normalize_coordinate(latitude); + g_current_gps_data.longitude = normalize_coordinate(longitude); + pthread_mutex_unlock(&g_gps_mutex); +} + +static void clear_gps(void) { + pthread_mutex_lock(&g_gps_mutex); + g_current_gps_data.latitude = 0.0; + g_current_gps_data.longitude = 0.0; + pthread_mutex_unlock(&g_gps_mutex); +} + +static gps_video_sample_t load_gps(void) { + gps_video_sample_t sample; + + pthread_mutex_lock(&g_gps_mutex); + sample = g_current_gps_data; + pthread_mutex_unlock(&g_gps_mutex); + return sample; +} + +static void gps_sleep_before_retry(void) { + int retry_ms = 1000; + int step_ms = 100; + int elapsed_ms = 0; + + while (g_running && elapsed_ms < retry_ms) { + usleep((useconds_t) step_ms * 1000U); + elapsed_ms += step_ms; + } +} + +// 将经纬度规范化为 double，保留 6 位小数。 +static int normalize_gps(double latitude, double longitude, gps_video_sample_t* sample) { + if (!isfinite(latitude) || !isfinite(longitude)) { + return -1; + } + // 过滤掉 0,0 这种无效坐标 + if (fabs(latitude) < 1e-6 && fabs(longitude) < 1e-6) { + return -1; + } + + if (sample == NULL) { + return -1; + } + + sample->latitude = normalize_coordinate(latitude); + sample->longitude = normalize_coordinate(longitude); + return 0; +} + +// ================================================================= +// 以下是借鉴 gps_parse.c 实现的底层解析函数 +// ================================================================= + +// 1. 辅助函数：在 JSON 字符串中查找键对应的值的起始位置 +static const char* find_json_value(const char* json, const char* key) { + char pattern[64]; + int written; + const char* position; + + if (json == NULL || key == NULL) return NULL; + + // 构建搜索模式: "key": + written = snprintf(pattern, sizeof(pattern), "\"%s\":", key); + if (written < 0 || (size_t)written >= sizeof(pattern)) { + return NULL; + } + + position = strstr(json, pattern); + if (position == NULL) { + return NULL; + } + + // 跳过 "key": + position += written; + + // 跳过可能存在的空格 + while (*position == ' ' || *position == '\t') { + position++; + } + + return position; +} + +// 2. 解析函数：从 JSON 字符串中提取 Double 类型的值 +static int json_extract_double(const char* json, const char* key, double* value) { + const char* position; + char* endptr = NULL; + double parsed; + + position = find_json_value(json, key); + if (position == NULL) { + return 0; // 键不存在 + } + + // 确保当前位置是数字或负号 + if (*position != '-' && !(*position >= '0' && *position <= '9')) { + return 0; + } + + // 重置 errno 以检测错误 + errno = 0; + parsed = strtod(position, &endptr); + + // 检查转换是否成功 + if (errno != 0 || endptr == position || !isfinite(parsed)) { + return 0; + } + + *value = parsed; + return 1; +} + +// 3. 解析函数：从 JSON 字符串中提取 Int 类型的值 +static int json_extract_int(const char* json, const char* key, int* value) { + double dval; + if (json_extract_double(json, key, &dval)) { + *value = (int)dval; + return 1; + } + return 0; +} + +// 4. 检查是否为 TPV (定位数据) 包 +static int is_tpv_class(const char* json) { + char class_buf[32] = {0}; + const char* pos = find_json_value(json, "class"); + if (pos == NULL || *pos != '"') return 0; + + // 简单提取 class 的值 (TPV/SKY/DEVICES) + sscanf(pos, "\"%31[^\"]\"", class_buf); + return (strcmp(class_buf, "TPV") == 0); +} + +// ================================================================= +// 后台线程函数：负责连接 gpsd 并更新全局变量 +// ================================================================= +void* gps_update_thread(void* arg) { + const char* host = (const char*)arg; + const char* gpsd_host = (host != NULL && host[0] != '\0') ? host : "127.0.0.1"; + + while (g_running) { + int sockfd = -1; + struct addrinfo hints; + struct addrinfo *res = NULL; + struct addrinfo *rp = NULL; + int s; + char buffer[4096]; + size_t offset = 0; + + // 1. 解析地址并连接 gpsd (默认端口 2947) + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_UNSPEC; // 兼容 IPv4/IPv6 + hints.ai_socktype = SOCK_STREAM; + + s = getaddrinfo(gpsd_host, "2947", &hints, &res); + if (s != 0) { + fprintf(stderr, "GPS线程: 解析 gpsd 地址失败 %s:2947: %s\n", gpsd_host, gai_strerror(s)); + gps_sleep_before_retry(); + continue; + } + + // 尝试连接每一个解析出来的地址 + for (rp = res; rp != NULL; rp = rp->ai_next) { + sockfd = socket(rp->ai_family, rp->ai_socktype, rp->ai_protocol); + if (sockfd == -1) { + continue; + } + + if (connect(sockfd, rp->ai_addr, rp->ai_addrlen) != -1) { + break; + } + close(sockfd); + sockfd = -1; + } + + freeaddrinfo(res); + + if (sockfd < 0) { + fprintf(stderr, "GPS线程: 无法连接到 %s:2947，1 秒后重试\n", gpsd_host); + gps_sleep_before_retry(); + continue; + } + + printf("GPS线程: 已连接到 gpsd %s\n", gpsd_host); + + // 2. 发送 WATCH 命令，开启 JSON 流 + { + const char* watch_cmd = "?WATCH={\"enable\":true,\"json\":true};\n"; + + if (send(sockfd, watch_cmd, strlen(watch_cmd), 0) < 0) { + perror("GPS线程: 发送 WATCH 命令失败"); + close(sockfd); + gps_sleep_before_retry(); + continue; + } + } + + // 3. 主循环：读取并解析数据流 + // 注意：gpsd 数据是以 \n 结尾的，不能直接用固定长度 recv + while (g_running) { + ssize_t len = recv(sockfd, buffer + offset, sizeof(buffer) - 1 - offset, 0); + + if (len <= 0) { + break; + } + + offset += (size_t) len; + buffer[offset] = '\0'; // 确保字符串结束 + + // 查找换行符 \n，因为一条完整的 JSON 消息以 \n 结尾 + char* start = buffer; + char* end; + + while ((end = memchr(start, '\n', (buffer + offset) - start)) != NULL) { + *end = '\0'; // 临时截断，形成独立字符串 + + // --- 核心解析逻辑 --- + // 1. 检查是否为 TPV 数据包 + if (is_tpv_class(start)) { + double lat = 0.0; + double lon = 0.0; + int mode = 0; + int has_fix = 0; + + // 2. 提取定位模式 (mode: 1=无定位, 2=2D, 3=3D) + if (json_extract_int(start, "mode", &mode)) { + has_fix = (mode >= 2); + } + + // 3. 如果有定位，提取经纬度 + if (has_fix) { + int got_lat = json_extract_double(start, "lat", &lat); + int got_lon = json_extract_double(start, "lon", &lon); + + if (got_lat && got_lon) { + gps_video_sample_t sample; + + // 4. 更新全局共享变量，使用 double 直接携带经纬度。 + if (normalize_gps(lat, lon, &sample) == 0) { + store_gps(sample.latitude, sample.longitude); + } + // 调试：取消注释可查看实时经纬度 + // printf("更新GPS: lat=%.6f, lon=%.6f\n", lat, lon); + } + } + // 如果无定位，这里不操作，保持上一次的有效值 + } + // --- 解析结束 --- + + // 移动指针到下一条消息 + start = end + 1; + } + + // 处理完所有完整消息后，将剩余未处理的数据移到缓冲区头部 + if (start < buffer + offset) { + size_t remaining = (size_t) ((buffer + offset) - start); + memmove(buffer, start, remaining); + offset = remaining; + } else { + offset = 0; // 缓冲区已清空 + } + } + + close(sockfd); + if (g_running) { + fprintf(stderr, "GPS线程: 连接断开，1 秒后重连...\n"); + gps_sleep_before_retry(); + } + } + + return NULL; +} + +// ================================================================= +// 接口函数实现 +// ================================================================= +gps_video_sample_t get_latest_gps_for_video(void) { + return load_gps(); +} + +int gps_buffer_init(const char* host) { + if (g_running) return 0; + + g_running = 1; + clear_gps(); + pthread_attr_t attr; + pthread_attr_init(&attr); + pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED); + // 创建后台线程 + if (pthread_create(&g_gps_thread, &attr, gps_update_thread, (void*)host) != 0) { + g_running = 0; + pthread_attr_destroy(&attr); // 清理属性 + perror("无法创建 GPS 线程"); + return -1; + } + pthread_attr_destroy(&attr); // 清理属性 + return 0; +} + +void gps_buffer_cleanup(void) { + g_running = 0; + // 等待线程结束 + + usleep(10000); // 等待 100ms 让后台线程有机会处理退出标志 +} + + +//gcc main.c video_pipeline_run.c gps_buffer.c -lpthread -lm -o my_app 请确保在编译命令中链接 pthread 和 m (math) 库 diff --git a/robot/ros2/OmniSocketGo_robot_ros/src/interactive.c b/robot/ros2/OmniSocketGo_robot_ros/src/interactive.c new file mode 100644 index 0000000..14f5a89 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/src/interactive.c @@ -0,0 +1,77 @@ +#include "interactive.h" + +#include + +static void interactive_skip_spaces(const char **cursor) { + while (**cursor != '\0' && isspace((unsigned char) **cursor)) { + (*cursor)++; + } +} + +int interactive_parse_command(const char *line, interactive_command_t *command, char *err, size_t err_len) { + const char *cursor = line; + char action[16]; + size_t action_len = 0; + size_t to_len = 0; + size_t value_len; + + if (line == NULL || command == NULL) { + snprintf(err, err_len, "interactive: invalid command"); + return -1; + } + memset(command, 0, sizeof(*command)); + interactive_skip_spaces(&cursor); + while (*cursor != '\0' && !isspace((unsigned char) *cursor) && action_len + 1 < sizeof(action)) { + action[action_len++] = *cursor++; + } + action[action_len] = '\0'; + if (action_len == 0) { + snprintf(err, err_len, "interactive: empty command"); + return -1; + } + if (strcmp(action, "help") == 0) { + command->type = INTERACTIVE_CMD_HELP; + return 0; + } + if (strcmp(action, "quit") == 0) { + command->type = INTERACTIVE_CMD_QUIT; + return 0; + } + + interactive_skip_spaces(&cursor); + while (*cursor != '\0' && !isspace((unsigned char) *cursor) && to_len + 1 < sizeof(command->to)) { + command->to[to_len++] = *cursor++; + } + command->to[to_len] = '\0'; + interactive_skip_spaces(&cursor); + if (command->to[0] == '\0' || *cursor == '\0') { + snprintf(err, err_len, "interactive: missing target or value"); + return -1; + } + + value_len = strlen(cursor); + if (value_len >= sizeof(command->value)) { + snprintf(err, err_len, "interactive: value too long"); + return -1; + } + snprintf(command->value, sizeof(command->value), "%s", cursor); + + if (strcmp(action, "text") == 0) { + command->type = INTERACTIVE_CMD_TEXT; + return 0; + } + if (strcmp(action, "file") == 0) { + command->type = INTERACTIVE_CMD_FILE; + return 0; + } + snprintf(err, err_len, "interactive: unknown command %s", action); + return -1; +} + +void interactive_print_help(FILE *out, const char *transport_name) { + fprintf(out, "interactive mode commands (%s):\n", transport_name); + fprintf(out, " help show this help\n"); + fprintf(out, " text send one text message\n"); + fprintf(out, " file send one file\n"); + fprintf(out, " quit exit this process\n"); +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/src/kcp_packet_debug.c b/robot/ros2/OmniSocketGo_robot_ros/src/kcp_packet_debug.c new file mode 100644 index 0000000..87540f8 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/src/kcp_packet_debug.c @@ -0,0 +1,166 @@ +#include "kcp_packet_debug.h" + +kcp_packet_debug_logger_t *kcp_packet_debug_open_jsonl(const char *path) { + kcp_packet_debug_logger_t *logger; + FILE *file; + if (path == NULL || path[0] == '\0') { + return NULL; + } + if (omni_ensure_parent_dir(path) != 0) { + return NULL; + } + file = fopen(path, "ab"); + if (file == NULL) { + return NULL; + } + logger = (kcp_packet_debug_logger_t *) calloc(1, sizeof(*logger)); + if (logger == NULL) { + fclose(file); + return NULL; + } + omni_file_logger_init_path(&logger->file_logger, file, path, 0); + logger->enabled = 1; + return logger; +} + +void kcp_packet_debug_close(kcp_packet_debug_logger_t *logger) { + if (logger == NULL) { + return; + } + if (logger->file_logger.file != NULL) { + fclose(logger->file_logger.file); + } + omni_file_logger_destroy(&logger->file_logger); + free(logger); +} + +void kcp_packet_debug_record_clear(kcp_packet_debug_record_t *record) { + if (record == NULL) { + return; + } + free(record->segments); + memset(record, 0, sizeof(*record)); +} + +int kcp_packet_debug_log(kcp_packet_debug_logger_t *logger, const kcp_packet_debug_record_t *record) { + char *event = NULL; + char *node_role = NULL; + char *node_id = NULL; + char *local_addr = NULL; + char *remote_addr = NULL; + char *segments_json = NULL; + char *tx_id_text = NULL; + char *conv_text = NULL; + char *line = NULL; + size_t i; + size_t cap = 128U; + size_t len = 0U; + + if (logger == NULL || record == NULL || !logger->enabled) { + return 0; + } + + event = omni_json_escape(record->event); + node_role = omni_json_escape(record->node_role); + node_id = omni_json_escape(record->node_id); + local_addr = omni_json_escape(record->local_addr); + remote_addr = omni_json_escape(record->remote_addr); + if (event == NULL || node_role == NULL || node_id == NULL || local_addr == NULL || remote_addr == NULL) { + free(event); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + return -1; + } + + segments_json = (char *) malloc(cap); + if (segments_json == NULL) { + free(event); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + return -1; + } + segments_json[len++] = '['; + for (i = 0; i < record->segment_count; ++i) { + int written; + while (len + 96U > cap) { + char *next = (char *) realloc(segments_json, cap * 2U); + if (next == NULL) { + free(event); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + free(segments_json); + return -1; + } + segments_json = next; + cap *= 2U; + } + written = snprintf( + segments_json + len, + cap - len, + "%s{\"cmd\":%u,\"sn\":%u,\"una\":%u,\"frg\":%u,\"wnd\":%u,\"len\":%u}", + i == 0 ? "" : ",", + record->segments[i].cmd, + record->segments[i].sn, + record->segments[i].una, + record->segments[i].frg, + record->segments[i].wnd, + record->segments[i].len + ); + len += (size_t) written; + } + segments_json[len++] = ']'; + segments_json[len] = '\0'; + + tx_id_text = record->has_udp_tx_id ? omni_strdup_printf("%u", record->udp_tx_id) : omni_strdup("null"); + conv_text = record->has_kcp_conv ? omni_strdup_printf("%u", record->kcp_conv) : omni_strdup("null"); + if (tx_id_text == NULL || conv_text == NULL) { + free(event); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + free(segments_json); + free(tx_id_text); + free(conv_text); + return -1; + } + + line = omni_strdup_printf( + "{\"event\":\"%s\",\"node_role\":\"%s\",\"node_id\":\"%s\",\"local_addr\":\"%s\",\"remote_addr\":\"%s\",\"packet_bytes\":%d,\"udp_tx_id\":%s,\"kcp_conv\":%s,\"segments\":%s,\"ts_unix_nano\":%" PRId64 "}", + event, + node_role, + node_id, + local_addr, + remote_addr, + record->packet_bytes, + tx_id_text, + conv_text, + segments_json, + record->ts_unix_nano + ); + + free(event); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + free(segments_json); + free(tx_id_text); + free(conv_text); + + if (line == NULL) { + return -1; + } + if (omni_file_logger_write_line(&logger->file_logger, line) != 0) { + free(line); + return -1; + } + free(line); + return 0; +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/src/kcp_session_stats.c b/robot/ros2/OmniSocketGo_robot_ros/src/kcp_session_stats.c new file mode 100644 index 0000000..c0b8ef8 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/src/kcp_session_stats.c @@ -0,0 +1,286 @@ +#include "kcp_session_stats.h" + +static int kcp_session_stats_append(char **line, size_t *len, const char *suffix) { + size_t suffix_len; + char *next; + + if (line == NULL || len == NULL || suffix == NULL) { + errno = EINVAL; + return -1; + } + suffix_len = strlen(suffix); + next = (char *) realloc(*line, *len + suffix_len + 1U); + if (next == NULL) { + return -1; + } + memcpy(next + *len, suffix, suffix_len + 1U); + *line = next; + *len += suffix_len; + return 0; +} + +static int kcp_session_stats_appendf(char **line, size_t *len, const char *fmt, ...) { + va_list args; + va_list copy; + int needed; + char *buffer; + + if (line == NULL || len == NULL || fmt == NULL) { + errno = EINVAL; + return -1; + } + + va_start(args, fmt); + va_copy(copy, args); + needed = vsnprintf(NULL, 0, fmt, copy); + va_end(copy); + if (needed < 0) { + va_end(args); + return -1; + } + + buffer = (char *) malloc((size_t) needed + 1U); + if (buffer == NULL) { + va_end(args); + return -1; + } + vsnprintf(buffer, (size_t) needed + 1U, fmt, args); + va_end(args); + + if (kcp_session_stats_append(line, len, buffer) != 0) { + free(buffer); + return -1; + } + free(buffer); + return 0; +} + +kcp_session_stats_logger_t *kcp_session_stats_open_jsonl(const char *path) { + kcp_session_stats_logger_t *logger; + FILE *file; + if (path == NULL || path[0] == '\0') { + return NULL; + } + if (omni_ensure_parent_dir(path) != 0) { + return NULL; + } + file = fopen(path, "ab"); + if (file == NULL) { + return NULL; + } + logger = (kcp_session_stats_logger_t *) calloc(1, sizeof(*logger)); + if (logger == NULL) { + fclose(file); + return NULL; + } + omni_file_logger_init_path(&logger->file_logger, file, path, 0); + logger->enabled = 1; + return logger; +} + +void kcp_session_stats_close(kcp_session_stats_logger_t *logger) { + if (logger == NULL) { + return; + } + if (logger->file_logger.file != NULL) { + fclose(logger->file_logger.file); + } + omni_file_logger_destroy(&logger->file_logger); + free(logger); +} + +int kcp_session_stats_log(kcp_session_stats_logger_t *logger, const kcp_session_stats_record_t *record) { + char *record_type = NULL; + char *node_role = NULL; + char *node_id = NULL; + char *local_addr = NULL; + char *remote_addr = NULL; + char *sample_reason = NULL; + char *line = NULL; + size_t line_len = 0; + + if (logger == NULL || record == NULL || !logger->enabled) { + return 0; + } + record_type = omni_json_escape(record->record_type); + node_role = omni_json_escape(record->node_role); + node_id = omni_json_escape(record->node_id); + local_addr = omni_json_escape(record->local_addr); + remote_addr = omni_json_escape(record->remote_addr); + sample_reason = omni_json_escape(record->sample_reason); + if (record_type == NULL || node_role == NULL || node_id == NULL || local_addr == NULL || remote_addr == NULL || sample_reason == NULL) { + free(record_type); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + free(sample_reason); + return -1; + } + line = omni_strdup(""); + if (line == NULL) { + free(record_type); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + free(sample_reason); + return -1; + } + + if (kcp_session_stats_appendf(&line, &line_len, "{\"record_type\":\"%s\",\"node_role\":\"%s\",\"node_id\":\"%s\",\"ts_unix_nano\":%" PRId64 ",\"sample_reason\":\"%s\"", + record_type, + node_role, + node_id, + record->ts_unix_nano, + sample_reason) != 0) { + goto cleanup; + } + if (record->local_addr[0] != '\0' && + kcp_session_stats_appendf(&line, &line_len, ",\"local_addr\":\"%s\"", local_addr) != 0) { + goto cleanup; + } + if (record->remote_addr[0] != '\0' && + kcp_session_stats_appendf(&line, &line_len, ",\"remote_addr\":\"%s\"", remote_addr) != 0) { + goto cleanup; + } + if (record->has_conv && + kcp_session_stats_appendf(&line, &line_len, ",\"conv\":%u", record->conv) != 0) { + goto cleanup; + } + if (record->has_rto_ms && + kcp_session_stats_appendf(&line, &line_len, ",\"rto_ms\":%u", record->rto_ms) != 0) { + goto cleanup; + } + if (record->has_srtt_ms && + kcp_session_stats_appendf(&line, &line_len, ",\"srtt_ms\":%d", record->srtt_ms) != 0) { + goto cleanup; + } + if (record->has_min_srtt_ms && + kcp_session_stats_appendf(&line, &line_len, ",\"min_srtt_ms\":%d", record->min_srtt_ms) != 0) { + goto cleanup; + } + if (record->has_srttvar_ms && + kcp_session_stats_appendf(&line, &line_len, ",\"srttvar_ms\":%d", record->srttvar_ms) != 0) { + goto cleanup; + } + if (record->has_last_feedback_age_ms && + kcp_session_stats_appendf(&line, &line_len, ",\"last_feedback_age_ms\":%u", record->last_feedback_age_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; + } + if (record->has_bytes_received && + kcp_session_stats_appendf(&line, &line_len, ",\"bytes_received\":%" PRIu64, record->bytes_received) != 0) { + goto cleanup; + } + if (record->has_in_pkts && + kcp_session_stats_appendf(&line, &line_len, ",\"in_pkts\":%" PRIu64, record->in_pkts) != 0) { + goto cleanup; + } + if (record->has_out_pkts && + kcp_session_stats_appendf(&line, &line_len, ",\"out_pkts\":%" PRIu64, record->out_pkts) != 0) { + goto cleanup; + } + if (record->has_in_segs && + kcp_session_stats_appendf(&line, &line_len, ",\"in_segs\":%" PRIu64, record->in_segs) != 0) { + goto cleanup; + } + if (record->has_out_segs && + kcp_session_stats_appendf(&line, &line_len, ",\"out_segs\":%" PRIu64, record->out_segs) != 0) { + goto cleanup; + } + if (record->has_retrans_segs && + kcp_session_stats_appendf(&line, &line_len, ",\"retrans_segs\":%" PRIu64, record->retrans_segs) != 0) { + goto cleanup; + } + if (record->has_fast_retrans_segs && + kcp_session_stats_appendf(&line, &line_len, ",\"fast_retrans_segs\":%" PRIu64, record->fast_retrans_segs) != 0) { + goto cleanup; + } + if (record->has_early_retrans_segs && + kcp_session_stats_appendf(&line, &line_len, ",\"early_retrans_segs\":%" PRIu64, record->early_retrans_segs) != 0) { + goto cleanup; + } + if (record->has_lost_segs && + kcp_session_stats_appendf(&line, &line_len, ",\"lost_segs\":%" PRIu64, record->lost_segs) != 0) { + goto cleanup; + } + if (record->has_repeat_segs && + kcp_session_stats_appendf(&line, &line_len, ",\"repeat_segs\":%" PRIu64, record->repeat_segs) != 0) { + goto cleanup; + } + if (record->has_in_errs && + kcp_session_stats_appendf(&line, &line_len, ",\"in_errs\":%" PRIu64, record->in_errs) != 0) { + goto cleanup; + } + if (record->has_kcp_in_errs && + kcp_session_stats_appendf(&line, &line_len, ",\"kcp_in_errs\":%" PRIu64, record->kcp_in_errs) != 0) { + goto cleanup; + } + if (record->has_ring_buffer_snd_queue && + kcp_session_stats_appendf(&line, &line_len, ",\"ring_buffer_snd_queue\":%" PRIu64, record->ring_buffer_snd_queue) != 0) { + goto cleanup; + } + if (record->has_ring_buffer_rcv_queue && + kcp_session_stats_appendf(&line, &line_len, ",\"ring_buffer_rcv_queue\":%" PRIu64, record->ring_buffer_rcv_queue) != 0) { + goto cleanup; + } + if (record->has_ring_buffer_snd_buffer && + kcp_session_stats_appendf(&line, &line_len, ",\"ring_buffer_snd_buffer\":%" PRIu64, record->ring_buffer_snd_buffer) != 0) { + goto cleanup; + } + if (record->has_curr_estab && + kcp_session_stats_appendf(&line, &line_len, ",\"curr_estab\":%" PRIu64, record->curr_estab) != 0) { + goto cleanup; + } + if (kcp_session_stats_append(&line, &line_len, "}") != 0) { + goto cleanup; + } + + free(record_type); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + free(sample_reason); + + if (omni_file_logger_write_line(&logger->file_logger, line) != 0) { + free(line); + return -1; + } + free(line); + return 0; + +cleanup: + free(record_type); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + free(sample_reason); + free(line); + return -1; +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/src/latencylog.c b/robot/ros2/OmniSocketGo_robot_ros/src/latencylog.c new file mode 100644 index 0000000..3a3314e --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/src/latencylog.c @@ -0,0 +1,130 @@ +#include "latencylog.h" + +static void latencylog_fill_event(latency_event_t *event, const char *node_role, const char *node_id, const char *event_name, int64_t ts_unix_nano, const message_t *msg) { + memset(event, 0, sizeof(*event)); + event->ts_unix_nano = ts_unix_nano; + snprintf(event->node_role, sizeof(event->node_role), "%s", node_role == NULL ? "" : node_role); + snprintf(event->node_id, sizeof(event->node_id), "%s", node_id == NULL ? "" : node_id); + snprintf(event->event, sizeof(event->event), "%s", event_name == NULL ? "" : event_name); + event->message_type = msg->type; + event->message_id = msg->id; + snprintf(event->from, sizeof(event->from), "%s", msg->from); + snprintf(event->to, sizeof(event->to), "%s", msg->to); + snprintf(event->file_name, sizeof(event->file_name), "%s", msg->file_name); + event->body_size = (int) msg->body_len; +} + +latency_logger_t *latencylog_open_jsonl(const char *path) { + latency_logger_t *logger; + FILE *file; + if (path == NULL || path[0] == '\0') { + return NULL; + } + if (omni_ensure_parent_dir(path) != 0) { + return NULL; + } + file = fopen(path, "ab"); + if (file == NULL) { + return NULL; + } + logger = (latency_logger_t *) calloc(1, sizeof(*logger)); + if (logger == NULL) { + fclose(file); + return NULL; + } + omni_file_logger_init_path(&logger->file_logger, file, path, 0); + logger->enabled = 1; + return logger; +} + +void latencylog_close(latency_logger_t *logger) { + if (logger == NULL) { + return; + } + if (logger->file_logger.file != NULL) { + fclose(logger->file_logger.file); + } + omni_file_logger_destroy(&logger->file_logger); + free(logger); +} + +int latencylog_log_event(latency_logger_t *logger, const latency_event_t *event) { + char *node_role = NULL; + char *node_id = NULL; + char *event_name = NULL; + char *from = NULL; + char *to = NULL; + char *file_name = NULL; + char *line = NULL; + + if (logger == NULL || event == NULL || !logger->enabled) { + return 0; + } + + node_role = omni_json_escape(event->node_role); + node_id = omni_json_escape(event->node_id); + event_name = omni_json_escape(event->event); + from = omni_json_escape(event->from); + to = omni_json_escape(event->to); + file_name = omni_json_escape(event->file_name); + if (node_role == NULL || node_id == NULL || event_name == NULL || from == NULL || to == NULL || file_name == NULL) { + free(node_role); + free(node_id); + free(event_name); + free(from); + free(to); + free(file_name); + return -1; + } + + line = omni_strdup_printf( + "{\"ts_unix_nano\":%" PRId64 ",\"node_role\":\"%s\",\"node_id\":\"%s\",\"event\":\"%s\",\"message_type\":\"%s\",\"message_id\":%" PRIu64 ",\"from\":\"%s\",\"to\":\"%s\",\"file_name\":\"%s\",\"body_size\":%d}", + event->ts_unix_nano, + node_role, + node_id, + event_name, + protocol_message_type_name(event->message_type), + event->message_id, + from, + to, + file_name, + event->body_size + ); + + free(node_role); + free(node_id); + free(event_name); + free(from); + free(to); + free(file_name); + + if (line == NULL) { + return -1; + } + if (omni_file_logger_write_line(&logger->file_logger, line) != 0) { + free(line); + return -1; + } + free(line); + return 0; +} + +int latencylog_is_business_message(const message_t *msg) { + if (msg == NULL) { + return 0; + } + return msg->type == MSG_TYPE_TEXT || msg->type == MSG_TYPE_FILE || msg->type == MSG_TYPE_BINARY; +} + +void latencylog_log_message_event(latency_logger_t *logger, const char *node_role, const char *node_id, const char *event_name, const message_t *msg) { + latencylog_log_message_event_at(logger, node_role, node_id, event_name, omni_now_unix_nano(), msg); +} + +void latencylog_log_message_event_at(latency_logger_t *logger, const char *node_role, const char *node_id, const char *event_name, int64_t ts_unix_nano, const message_t *msg) { + latency_event_t event; + if (!latencylog_is_business_message(msg)) { + return; + } + latencylog_fill_event(&event, node_role, node_id, event_name, ts_unix_nano, msg); + (void) latencylog_log_event(logger, &event); +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/src/linux_timestamping.c b/robot/ros2/OmniSocketGo_robot_ros/src/linux_timestamping.c new file mode 100644 index 0000000..0a5c910 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/src/linux_timestamping.c @@ -0,0 +1,103 @@ +#include "linux_timestamping.h" +#include "latencylog.h" + +#ifdef __linux__ +#include +#include +#include +#include + +static int64_t linux_timespec_to_ns(const struct timespec *ts) { + if (ts == NULL) { + return 0; + } + return (int64_t) ts->tv_sec * 1000000000LL + ts->tv_nsec; +} + +int linux_timestamping_enable_udp_socket(int fd, int enable_rx) { + int flags = SOF_TIMESTAMPING_SOFTWARE | SOF_TIMESTAMPING_TX_SCHED | SOF_TIMESTAMPING_TX_SOFTWARE | SOF_TIMESTAMPING_OPT_ID | SOF_TIMESTAMPING_OPT_TSONLY; + if (enable_rx) { + flags |= SOF_TIMESTAMPING_RX_SOFTWARE; + } + return setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING, &flags, sizeof(flags)); +} + +int64_t linux_timestamping_parse_rx_timestamp(const struct msghdr *msg) { + struct cmsghdr *cmsg; + const struct scm_timestamping *timestamps; + if (msg == NULL) { + return 0; + } + for (cmsg = CMSG_FIRSTHDR((struct msghdr *) msg); cmsg != NULL; cmsg = CMSG_NXTHDR((struct msghdr *) msg, cmsg)) { + if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_TIMESTAMPING) { + timestamps = (const struct scm_timestamping *) CMSG_DATA(cmsg); + if (timestamps->ts[0].tv_sec != 0 || timestamps->ts[0].tv_nsec != 0) { + return linux_timespec_to_ns(×tamps->ts[0]); + } + } + } + return 0; +} + +int linux_timestamping_parse_tx_timestamp(const struct msghdr *msg, omni_tx_timestamp_event_t *out_event) { + struct cmsghdr *cmsg; + const struct scm_timestamping *timestamps = NULL; + const struct sock_extended_err *sock_err = NULL; + int64_t timestamp_ns = 0; + + if (msg == NULL || out_event == NULL) { + errno = EINVAL; + return -1; + } + memset(out_event, 0, sizeof(*out_event)); + + for (cmsg = CMSG_FIRSTHDR((struct msghdr *) msg); cmsg != NULL; cmsg = CMSG_NXTHDR((struct msghdr *) msg, cmsg)) { + if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_TIMESTAMPING) { + timestamps = (const struct scm_timestamping *) CMSG_DATA(cmsg); + } else if ((cmsg->cmsg_level == SOL_IP && cmsg->cmsg_type == IP_RECVERR) || + (cmsg->cmsg_level == SOL_IPV6 && cmsg->cmsg_type == IPV6_RECVERR)) { + sock_err = (const struct sock_extended_err *) CMSG_DATA(cmsg); + } + } + if (timestamps == NULL || sock_err == NULL) { + errno = EAGAIN; + return -1; + } + if (timestamps->ts[0].tv_sec != 0 || timestamps->ts[0].tv_nsec != 0) { + timestamp_ns = linux_timespec_to_ns(×tamps->ts[0]); + snprintf(out_event->event_name, sizeof(out_event->event_name), "%s", EVENT_A_TX_SOFTWARE); + } else if (timestamps->ts[1].tv_sec != 0 || timestamps->ts[1].tv_nsec != 0) { + timestamp_ns = linux_timespec_to_ns(×tamps->ts[1]); + snprintf(out_event->event_name, sizeof(out_event->event_name), "%s", EVENT_A_TX_SCHED); + } else { + errno = EAGAIN; + return -1; + } + out_event->ts_unix_nano = timestamp_ns; + out_event->ee_info = sock_err->ee_info; + out_event->ee_data = sock_err->ee_data; + return 0; +} + +#else + +int linux_timestamping_enable_udp_socket(int fd, int enable_rx) { + (void) fd; + (void) enable_rx; + errno = ENOTSUP; + return -1; +} + +int64_t linux_timestamping_parse_rx_timestamp(const struct msghdr *msg) { + (void) msg; + return 0; +} + +int linux_timestamping_parse_tx_timestamp(const struct msghdr *msg, omni_tx_timestamp_event_t *out_event) { + (void) msg; + (void) out_event; + errno = ENOTSUP; + return -1; +} + +#endif diff --git a/robot/ros2/OmniSocketGo_robot_ros/src/omni_common.c b/robot/ros2/OmniSocketGo_robot_ros/src/omni_common.c new file mode 100644 index 0000000..fe2b1a8 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/src/omni_common.c @@ -0,0 +1,795 @@ +#include "omni_common.h" + +#include +#include +#include +#include +#include + +int64_t omni_now_unix_nano(void) { + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + return (int64_t) ts.tv_sec * 1000000000LL + ts.tv_nsec; +} + +uint32_t omni_now_millis32(void) { + struct timespec ts; + uint64_t ms; + clock_gettime(CLOCK_MONOTONIC, &ts); + ms = (uint64_t) ts.tv_sec * 1000ULL + (uint64_t) (ts.tv_nsec / 1000000L); + return (uint32_t) (ms & 0xffffffffu); +} + +int omni_set_nonblocking(int fd, int enabled) { + int flags = fcntl(fd, F_GETFL, 0); + if (flags < 0) { + return -1; + } + if (enabled) { + flags |= O_NONBLOCK; + } else { + flags &= ~O_NONBLOCK; + } + return fcntl(fd, F_SETFL, flags); +} + +int omni_parse_sockaddr(const char *raw, int passive, struct sockaddr_storage *addr, socklen_t *addr_len, int *family_out) { + struct addrinfo hints; + struct addrinfo *result = NULL; + char host_copy[OMNI_MAX_ADDR_TEXT]; + char port_copy[32]; + const char *host = NULL; + const char *service = NULL; + const char *last_colon; + size_t host_len; + + if (raw == NULL || addr == NULL || addr_len == NULL) { + errno = EINVAL; + return -1; + } + + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_DGRAM; + hints.ai_flags = passive ? AI_PASSIVE : 0; + + last_colon = strrchr(raw, ':'); + if (last_colon == NULL) { + host = passive ? NULL : raw; + service = passive ? raw : "0"; + } else { + host_len = (size_t) (last_colon - raw); + if (host_len >= sizeof(host_copy)) { + errno = ENAMETOOLONG; + return -1; + } + memcpy(host_copy, raw, host_len); + host_copy[host_len] = '\0'; + snprintf(port_copy, sizeof(port_copy), "%s", last_colon + 1); + host = host_len == 0 ? NULL : host_copy; + service = port_copy; + } + + if (getaddrinfo(host, service, &hints, &result) != 0 || result == NULL) { + errno = EINVAL; + return -1; + } + memcpy(addr, result->ai_addr, result->ai_addrlen); + *addr_len = (socklen_t) result->ai_addrlen; + if (family_out != NULL) { + *family_out = result->ai_family; + } + freeaddrinfo(result); + return 0; +} + +int omni_clone_sockaddr(const struct sockaddr *src, socklen_t src_len, struct sockaddr_storage *dst, socklen_t *dst_len) { + if (src == NULL || dst == NULL || dst_len == NULL || src_len > sizeof(*dst)) { + errno = EINVAL; + return -1; + } + memset(dst, 0, sizeof(*dst)); + memcpy(dst, src, src_len); + *dst_len = src_len; + return 0; +} + +const char *omni_sockaddr_to_string(const struct sockaddr *addr, socklen_t addr_len, char *buffer, size_t buffer_len) { + char host[NI_MAXHOST]; + char service[NI_MAXSERV]; + + if (buffer == NULL || buffer_len == 0) { + return ""; + } + if (addr == NULL) { + snprintf(buffer, buffer_len, ""); + return buffer; + } + if (getnameinfo(addr, addr_len, host, sizeof(host), service, sizeof(service), NI_NUMERICHOST | NI_NUMERICSERV) != 0) { + snprintf(buffer, buffer_len, ""); + return buffer; + } + if (addr->sa_family == AF_INET6) { + snprintf(buffer, buffer_len, "[%s]:%s", host, service); + } else { + snprintf(buffer, buffer_len, "%s:%s", host, service); + } + return buffer; +} + +int omni_bind_device(int fd, const char *device) { +#ifdef __linux__ + if (device == NULL || device[0] == '\0') { + return 0; + } + return setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE, device, (socklen_t) strlen(device)); +#else + (void) fd; + (void) device; + errno = ENOTSUP; + return -1; +#endif +} + +static int omni_mkdir_single(const char *path) { + if (mkdir(path, 0755) == 0 || errno == EEXIST) { + return 0; + } + return -1; +} + +int omni_ensure_dir(const char *path) { + char tmp[PATH_MAX]; + size_t i; + + if (path == NULL || path[0] == '\0') { + return 0; + } + if (strlen(path) >= sizeof(tmp)) { + errno = ENAMETOOLONG; + return -1; + } + snprintf(tmp, sizeof(tmp), "%s", path); + for (i = 1; tmp[i] != '\0'; ++i) { + if (tmp[i] == '/') { + tmp[i] = '\0'; + if (tmp[0] != '\0' && omni_mkdir_single(tmp) != 0) { + return -1; + } + tmp[i] = '/'; + } + } + return omni_mkdir_single(tmp); +} + +int omni_ensure_parent_dir(const char *path) { + char tmp[PATH_MAX]; + char *slash; + + if (path == NULL || path[0] == '\0') { + return 0; + } + if (strlen(path) >= sizeof(tmp)) { + errno = ENAMETOOLONG; + return -1; + } + snprintf(tmp, sizeof(tmp), "%s", path); + slash = strrchr(tmp, '/'); + if (slash == NULL) { + return 0; + } + if (slash == tmp) { + return omni_mkdir_single("/"); + } + *slash = '\0'; + return omni_ensure_dir(tmp); +} + +int omni_read_file(const char *path, uint8_t **out, size_t *out_len) { + FILE *file; + long size; + uint8_t *buffer; + if (out == NULL || out_len == NULL) { + errno = EINVAL; + return -1; + } + *out = NULL; + *out_len = 0; + file = fopen(path, "rb"); + if (file == NULL) { + return -1; + } + if (fseek(file, 0, SEEK_END) != 0) { + fclose(file); + return -1; + } + size = ftell(file); + if (size < 0) { + fclose(file); + return -1; + } + if (fseek(file, 0, SEEK_SET) != 0) { + fclose(file); + return -1; + } + buffer = (uint8_t *) malloc((size_t) size); + if (size > 0 && buffer == NULL) { + fclose(file); + errno = ENOMEM; + return -1; + } + if ((size_t) size > 0 && fread(buffer, 1, (size_t) size, file) != (size_t) size) { + free(buffer); + fclose(file); + errno = EIO; + return -1; + } + fclose(file); + *out = buffer; + *out_len = (size_t) size; + return 0; +} + +int omni_write_full_fd(int fd, const uint8_t *data, size_t len) { + ssize_t written; + while (len > 0) { + written = write(fd, data, len); + if (written < 0) { + if (errno == EINTR) { + continue; + } + return -1; + } + if (written == 0) { + errno = EIO; + return -1; + } + data += written; + len -= (size_t) written; + } + return 0; +} + +static int omni_write_file_internal(const char *path, const uint8_t *data, size_t len, const char *mode) { + FILE *file; + if (omni_ensure_parent_dir(path) != 0) { + return -1; + } + file = fopen(path, mode); + if (file == NULL) { + return -1; + } + if (len > 0 && fwrite(data, 1, len, file) != len) { + fclose(file); + errno = EIO; + return -1; + } + if (fclose(file) != 0) { + return -1; + } + return 0; +} + +int omni_append_file(const char *path, const uint8_t *data, size_t len) { + return omni_write_file_internal(path, data, len, "ab"); +} + +int omni_write_file(const char *path, const uint8_t *data, size_t len) { + return omni_write_file_internal(path, data, len, "wb"); +} + +int omni_random_u32(uint32_t *out) { + uint8_t *cursor; + size_t remaining; + int fd; + + if (out == NULL) { + errno = EINVAL; + return -1; + } + + fd = open("/dev/urandom", O_RDONLY); + if (fd < 0) { + return -1; + } + + cursor = (uint8_t *) out; + remaining = sizeof(*out); + while (remaining > 0) { + ssize_t n = read(fd, cursor, remaining); + if (n < 0) { + if (errno == EINTR) { + continue; + } + close(fd); + return -1; + } + if (n == 0) { + close(fd); + errno = EIO; + return -1; + } + cursor += n; + remaining -= (size_t) n; + } + close(fd); + + if (*out == 0) { + *out = 1; + } + return 0; +} + +char *omni_strdup(const char *src) { + size_t len; + char *dst; + if (src == NULL) { + return NULL; + } + len = strlen(src); + dst = (char *) malloc(len + 1U); + if (dst == NULL) { + return NULL; + } + memcpy(dst, src, len + 1U); + return dst; +} + +char *omni_strdup_printf(const char *fmt, ...) { + va_list args; + va_list copy; + int needed; + char *buffer; + va_start(args, fmt); + va_copy(copy, args); + needed = vsnprintf(NULL, 0, fmt, copy); + va_end(copy); + if (needed < 0) { + va_end(args); + return NULL; + } + buffer = (char *) malloc((size_t) needed + 1U); + if (buffer == NULL) { + va_end(args); + return NULL; + } + vsnprintf(buffer, (size_t) needed + 1U, fmt, args); + va_end(args); + return buffer; +} + +char *omni_json_escape_bytes(const uint8_t *src, size_t len) { + size_t i; + size_t out_len = 0; + char *out; + char *cursor; + + if (src == NULL) { + if (len == 0) { + return omni_strdup(""); + } + errno = EINVAL; + return NULL; + } + + for (i = 0; i < len; ++i) { + switch (src[i]) { + case '\\': + case '"': + case '\b': + case '\f': + case '\n': + case '\r': + case '\t': + out_len += 2; + break; + default: + out_len += src[i] < 0x20 ? 6U : 1U; + break; + } + } + out = (char *) malloc(out_len + 1U); + if (out == NULL) { + return NULL; + } + cursor = out; + for (i = 0; i < len; ++i) { + switch (src[i]) { + case '\\': + *cursor++ = '\\'; + *cursor++ = '\\'; + break; + case '"': + *cursor++ = '\\'; + *cursor++ = '"'; + break; + case '\b': + *cursor++ = '\\'; + *cursor++ = 'b'; + break; + case '\f': + *cursor++ = '\\'; + *cursor++ = 'f'; + break; + case '\n': + *cursor++ = '\\'; + *cursor++ = 'n'; + break; + case '\r': + *cursor++ = '\\'; + *cursor++ = 'r'; + break; + case '\t': + *cursor++ = '\\'; + *cursor++ = 't'; + break; + default: + if (src[i] < 0x20) { + snprintf(cursor, 7, "\\u%04x", src[i]); + cursor += 6; + } else { + *cursor++ = (char) src[i]; + } + break; + } + } + *cursor = '\0'; + return out; +} + +char *omni_json_escape(const char *src) { + if (src == NULL) { + return omni_strdup(""); + } + return omni_json_escape_bytes((const uint8_t *) src, strlen(src)); +} + +int omni_utf8_valid(const uint8_t *data, size_t len) { + size_t i = 0; + uint8_t c; + while (i < len) { + c = data[i]; + if (c <= 0x7f) { + i++; + continue; + } + if ((c & 0xe0) == 0xc0) { + if (i + 1 >= len || (data[i + 1] & 0xc0) != 0x80 || c < 0xc2) { + return 0; + } + i += 2; + continue; + } + if ((c & 0xf0) == 0xe0) { + if (i + 2 >= len || (data[i + 1] & 0xc0) != 0x80 || (data[i + 2] & 0xc0) != 0x80) { + return 0; + } + if (c == 0xe0 && data[i + 1] < 0xa0) { + return 0; + } + if (c == 0xed && data[i + 1] >= 0xa0) { + return 0; + } + i += 3; + continue; + } + if ((c & 0xf8) == 0xf0) { + if (i + 3 >= len || (data[i + 1] & 0xc0) != 0x80 || (data[i + 2] & 0xc0) != 0x80 || (data[i + 3] & 0xc0) != 0x80) { + return 0; + } + if (c == 0xf0 && data[i + 1] < 0x90) { + return 0; + } + if (c > 0xf4 || (c == 0xf4 && data[i + 1] >= 0x90)) { + return 0; + } + i += 4; + continue; + } + return 0; + } + return 1; +} + +void omni_trim_newline(char *line) { + size_t len; + if (line == NULL) { + return; + } + len = strlen(line); + while (len > 0 && (line[len - 1] == '\n' || line[len - 1] == '\r')) { + line[--len] = '\0'; + } +} + +int omni_parse_duration_ms(const char *raw, int default_ms, int *out_ms) { + char *endptr; + long value; + if (out_ms == NULL) { + errno = EINVAL; + return -1; + } + if (raw == NULL || raw[0] == '\0') { + *out_ms = default_ms; + return 0; + } + value = strtol(raw, &endptr, 10); + if (endptr == raw || value <= 0) { + errno = EINVAL; + return -1; + } + if (*endptr == '\0' || strcmp(endptr, "ms") == 0) { + *out_ms = (int) value; + return 0; + } + if (strcmp(endptr, "s") == 0) { + *out_ms = (int) (value * 1000L); + return 0; + } + errno = EINVAL; + return -1; +} + +double omni_duration_ms_to_ns(double ms) { + return ms * 1000000.0; +} + +const char *omni_path_base_name(const char *path) { + const char *slash; + + if (path == NULL) { + return ""; + } + slash = strrchr(path, '/'); + return slash == NULL ? path : slash + 1; +} + +static uint64_t omni_now_monotonic_ms64(void) { + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + return (uint64_t) ts.tv_sec * 1000ULL + (uint64_t) (ts.tv_nsec / 1000000L); +} + +static int omni_positive_int_env(const char *name, int default_value) { + const char *raw = getenv(name); + long parsed; + char *endptr = NULL; + + if (raw == NULL || raw[0] == '\0') { + return default_value; + } + parsed = strtol(raw, &endptr, 10); + if (endptr == raw || *endptr != '\0' || parsed <= 0) { + return default_value; + } + return (int) parsed; +} + +static size_t omni_positive_size_env(const char *name, size_t default_value) { + const char *raw = getenv(name); + unsigned long long parsed; + char *endptr = NULL; + + if (raw == NULL || raw[0] == '\0') { + return default_value; + } + parsed = strtoull(raw, &endptr, 10); + if (endptr == raw || *endptr != '\0' || parsed == 0ULL) { + return default_value; + } + return (size_t) parsed; +} + +static int omni_file_logger_flush_locked(omni_file_logger_t *logger, uint64_t now_ms) { + if (logger == NULL || logger->file == NULL) { + errno = EINVAL; + return -1; + } + if (fflush(logger->file) != 0) { + return -1; + } + logger->buffered_bytes = 0U; + logger->last_flush_monotonic_ms = now_ms; + return 0; +} + +static int omni_build_rotated_path(char *buffer, size_t buffer_len, const char *path, int suffix) { + size_t path_len; + int written; + + if (buffer == NULL || buffer_len == 0U || path == NULL || path[0] == '\0') { + errno = EINVAL; + return -1; + } + path_len = strlen(path); + if (path_len + 16U >= buffer_len) { + errno = ENAMETOOLONG; + return -1; + } + memcpy(buffer, path, path_len); + written = snprintf(buffer + path_len, buffer_len - path_len, ".%d", suffix); + if (written < 0 || (size_t) written >= buffer_len - path_len) { + errno = ENAMETOOLONG; + return -1; + } + return 0; +} + +static int omni_file_logger_reopen_append_locked(omni_file_logger_t *logger) { + struct stat st; + FILE *file; + + if (logger == NULL || logger->path[0] == '\0') { + errno = EINVAL; + return -1; + } + + file = fopen(logger->path, "ab"); + if (file == NULL) { + return -1; + } + + logger->file = file; + logger->current_bytes = 0U; + if (stat(logger->path, &st) == 0) { + logger->current_bytes = (size_t) st.st_size; + } + logger->buffered_bytes = 0U; + logger->last_flush_monotonic_ms = omni_now_monotonic_ms64(); + return 0; +} + +static int omni_file_logger_recover_after_rotate_locked(omni_file_logger_t *logger, const char *rotated_current_path) { + int reopen_errno; + + if (omni_file_logger_reopen_append_locked(logger) == 0) { + return 0; + } + + reopen_errno = errno; + if (rotated_current_path != NULL && rotated_current_path[0] != '\0') { + if (rename(rotated_current_path, logger->path) == 0) { + if (omni_file_logger_reopen_append_locked(logger) == 0) { + return 0; + } + } + } + + errno = reopen_errno; + return -1; +} + +static int omni_file_logger_rotate_locked(omni_file_logger_t *logger) { + int index; + int saved_errno = 0; + int should_recover = 0; + char rotated_current_path[PATH_MAX]; + char from_path[PATH_MAX]; + char to_path[PATH_MAX]; + + if (logger == NULL || logger->path[0] == '\0' || logger->max_bytes == 0U || logger->max_files <= 0) { + return 0; + } + rotated_current_path[0] = '\0'; + if (logger->file != NULL) { + if (omni_file_logger_flush_locked(logger, omni_now_monotonic_ms64()) != 0) { + return -1; + } + should_recover = 1; + if (fclose(logger->file) != 0) { + logger->file = NULL; + saved_errno = errno; + goto recover; + } + logger->file = NULL; + } + + if (omni_build_rotated_path(from_path, sizeof(from_path), logger->path, logger->max_files) != 0) { + saved_errno = errno; + goto recover; + } + unlink(from_path); + for (index = logger->max_files - 1; index >= 1; --index) { + if (omni_build_rotated_path(from_path, sizeof(from_path), logger->path, index) != 0 || + omni_build_rotated_path(to_path, sizeof(to_path), logger->path, index + 1) != 0) { + saved_errno = errno; + goto recover; + } + if (rename(from_path, to_path) != 0 && errno != ENOENT) { + saved_errno = errno; + goto recover; + } + } + if (omni_build_rotated_path(to_path, sizeof(to_path), logger->path, 1) != 0) { + saved_errno = errno; + goto recover; + } + if (rename(logger->path, to_path) != 0 && errno != ENOENT) { + saved_errno = errno; + goto recover; + } + snprintf(rotated_current_path, sizeof(rotated_current_path), "%s", to_path); + + if (omni_file_logger_reopen_append_locked(logger) != 0) { + saved_errno = errno; + goto recover; + } + return 0; + +recover: + if (should_recover) { + int recover_errno = saved_errno != 0 ? saved_errno : errno; + if (omni_file_logger_recover_after_rotate_locked(logger, rotated_current_path) == 0) { + errno = recover_errno; + } else if (saved_errno != 0) { + errno = saved_errno; + } + } else if (saved_errno != 0) { + errno = saved_errno; + } + return -1; +} + +void omni_file_logger_init(omni_file_logger_t *logger, FILE *file) { + memset(logger, 0, sizeof(*logger)); + logger->file = file; + pthread_mutex_init(&logger->mutex, NULL); + logger->flush_bytes = 1U; + logger->flush_interval_ms = 0; + logger->immediate_flush = 1; + logger->last_flush_monotonic_ms = omni_now_monotonic_ms64(); +} + +void omni_file_logger_init_path(omni_file_logger_t *logger, FILE *file, const char *path, int immediate_flush) { + struct stat st; + + omni_file_logger_init(logger, file); + if (path != NULL && path[0] != '\0') { + snprintf(logger->path, sizeof(logger->path), "%s", path); + if (stat(path, &st) == 0) { + logger->current_bytes = (size_t) st.st_size; + } + } + logger->flush_bytes = omni_positive_size_env("BLITZ_JSONL_FLUSH_BYTES", 262144U); + logger->flush_interval_ms = omni_positive_int_env("BLITZ_JSONL_FLUSH_INTERVAL_MS", 1000); + logger->max_bytes = omni_positive_size_env("BLITZ_JSONL_ROTATE_BYTES", 134217728U); + logger->max_files = omni_positive_int_env("BLITZ_JSONL_ROTATE_FILES", 8); + logger->immediate_flush = immediate_flush != 0; +} + +void omni_file_logger_destroy(omni_file_logger_t *logger) { + pthread_mutex_destroy(&logger->mutex); +} + +int omni_file_logger_write_line(omni_file_logger_t *logger, const char *line) { + int rc = 0; + size_t line_len; + uint64_t now_ms; + if (logger == NULL || logger->file == NULL || line == NULL) { + errno = EINVAL; + return -1; + } + line_len = strlen(line) + 1U; + now_ms = omni_now_monotonic_ms64(); + pthread_mutex_lock(&logger->mutex); + if (fputs(line, logger->file) == EOF || fputc('\n', logger->file) == EOF) { + rc = -1; + } else { + logger->current_bytes += line_len; + logger->buffered_bytes += line_len; + if (logger->immediate_flush || + logger->buffered_bytes >= logger->flush_bytes || + (logger->flush_interval_ms > 0 && now_ms - logger->last_flush_monotonic_ms >= (uint64_t) logger->flush_interval_ms)) { + if (omni_file_logger_flush_locked(logger, now_ms) != 0) { + rc = -1; + } + } + if (rc == 0 && logger->max_bytes > 0U && logger->current_bytes >= logger->max_bytes) { + if (omni_file_logger_rotate_locked(logger) != 0) { + rc = -1; + } + } + } + pthread_mutex_unlock(&logger->mutex); + return rc; +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/src/peer_kcp_client.c b/robot/ros2/OmniSocketGo_robot_ros/src/peer_kcp_client.c new file mode 100644 index 0000000..733f076 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/src/peer_kcp_client.c @@ -0,0 +1,675 @@ +#include "peer_kcp_client.h" + +#include +#include +#include +#include + +#define KCP_CLIENT_REGISTER_TIMEOUT_MS 3000 +#define KCP_CLIENT_CTRL_REGISTER_OK "{\"type\":\"server_register_ok\"}" +#define KCP_CLIENT_CTRL_PEER_REPLACED "{\"type\":\"server_peer_replaced\",\"reason\":\"new_instance_wins\"}" +#define KCP_CLIENT_CTRL_HEARTBEAT "{\"type\":\"server_heartbeat\"}" +#define KCP_CLIENT_CTRL_HEARTBEAT_ACK "{\"type\":\"server_heartbeat_ack\"}" + +struct kcp_client { + char id[OMNI_MAX_PEER_ID]; + char server_addr[OMNI_MAX_ADDR_TEXT]; + kcp_conn_t *conn; + latency_logger_t *logger; + pthread_mutex_t state_mu; + uint64_t next_message_id; + int registered; + uint32_t last_server_activity_ms; + char last_server_error[256]; +}; + +static int kcp_client_next_message_id(kcp_client_t *client, uint64_t *out_id) { + if (client == NULL || out_id == NULL) { + errno = EINVAL; + return -1; + } + pthread_mutex_lock(&client->state_mu); + *out_id = ++client->next_message_id; + pthread_mutex_unlock(&client->state_mu); + return 0; +} + +static void kcp_client_set_registered(kcp_client_t *client, int registered) { + if (client == NULL) { + return; + } + pthread_mutex_lock(&client->state_mu); + client->registered = registered != 0; + pthread_mutex_unlock(&client->state_mu); +} + +static void kcp_client_touch_server_activity(kcp_client_t *client) { + if (client == NULL) { + return; + } + pthread_mutex_lock(&client->state_mu); + client->last_server_activity_ms = omni_now_millis32(); + pthread_mutex_unlock(&client->state_mu); +} + +static void kcp_client_set_last_server_error(kcp_client_t *client, const char *message) { + if (client == NULL) { + return; + } + pthread_mutex_lock(&client->state_mu); + snprintf(client->last_server_error, sizeof(client->last_server_error), "%s", message == NULL ? "" : message); + pthread_mutex_unlock(&client->state_mu); +} + +static void kcp_client_clear_last_server_error(kcp_client_t *client) { + kcp_client_set_last_server_error(client, ""); +} + +static int kcp_client_server_error_invalidates_registration(const char *message) { + if (message == NULL || message[0] == '\0') { + return 0; + } + return strstr(message, "not registered") != NULL + || strstr(message, "first message must be register") != NULL + || strstr(message, "peer replaced") != NULL + || strstr(message, "timed out waiting for server_register_ok") != NULL; +} + +static int kcp_client_is_registered(kcp_client_t *client) { + int registered; + + if (client == NULL) { + return 0; + } + pthread_mutex_lock(&client->state_mu); + registered = client->registered; + pthread_mutex_unlock(&client->state_mu); + return registered; +} + +static int kcp_client_text_body_equals(const message_t *msg, const char *payload) { + size_t expected_len; + + if (msg == NULL || payload == NULL || msg->body == NULL) { + return 0; + } + expected_len = strlen(payload); + return msg->body_len == expected_len && memcmp(msg->body, payload, expected_len) == 0; +} + +static void kcp_client_copy_server_error_body(const message_t *msg, char *buffer, size_t buffer_len) { + size_t copy_len; + + if (buffer == NULL || buffer_len == 0) { + return; + } + buffer[0] = '\0'; + if (msg == NULL || msg->body == NULL || msg->body_len == 0) { + return; + } + copy_len = msg->body_len < (buffer_len - 1U) ? msg->body_len : (buffer_len - 1U); + memcpy(buffer, msg->body, copy_len); + buffer[copy_len] = '\0'; +} + +static int kcp_client_registration_errno_from_message(const char *message) { + if (message == NULL || message[0] == '\0') { + return ECONNREFUSED; + } + if (strstr(message, "duplicate peer id") != NULL) { + return EEXIST; + } + if (strstr(message, "first message must be register") != NULL) { + return EPROTO; + } + return ECONNREFUSED; +} + +static int kcp_client_send_text_internal(kcp_client_t *client, const char *to, const char *text, int log_business_event) { + message_t msg; + uint64_t id; + + if (client == NULL || to == NULL || text == NULL || client->conn == NULL) { + errno = EINVAL; + return -1; + } + + protocol_message_init(&msg); + if (kcp_client_next_message_id(client, &id) != 0) { + return -1; + } + msg.type = MSG_TYPE_TEXT; + msg.id = id; + snprintf(msg.from, sizeof(msg.from), "%s", client->id); + snprintf(msg.to, sizeof(msg.to), "%s", to); + msg.body = (uint8_t *) omni_strdup(text); + if (msg.body == NULL) { + return -1; + } + msg.body_len = strlen((const char *) msg.body); + if (log_business_event) { + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_A_APP_PREP_BEGIN, &msg); + } + if (kcp_conn_send(client->conn, &msg) != 0) { + protocol_message_clear(&msg); + return -1; + } + protocol_message_clear(&msg); + return 0; +} + +static int kcp_client_send_business_preflight(kcp_client_t *client) { + if (client == NULL || client->conn == NULL) { + errno = ENOTCONN; + return -1; + } + if (!kcp_client_is_registered(client)) { + errno = ENOTCONN; + return -1; + } + return 0; +} + +static int kcp_client_handle_reserved_server_message(kcp_client_t *client, const message_t *msg) { + if (client == NULL || msg == NULL) { + errno = EINVAL; + return -1; + } + if (msg->type != MSG_TYPE_TEXT || strcmp(msg->from, SERVER_PEER_ID) != 0) { + return 0; + } + kcp_client_touch_server_activity(client); + if (kcp_client_text_body_equals(msg, KCP_CLIENT_CTRL_REGISTER_OK)) { + kcp_client_set_registered(client, 1); + kcp_client_clear_last_server_error(client); + return 1; + } + if (kcp_client_text_body_equals(msg, KCP_CLIENT_CTRL_HEARTBEAT)) { + if (kcp_client_send_text_internal(client, SERVER_PEER_ID, KCP_CLIENT_CTRL_HEARTBEAT_ACK, 0) != 0) { + kcp_client_set_registered(client, 0); + kcp_client_set_last_server_error(client, "failed to acknowledge server heartbeat"); + (void) kcp_conn_close(client->conn); + return -1; + } + return 1; + } + if (kcp_client_text_body_equals(msg, KCP_CLIENT_CTRL_HEARTBEAT_ACK)) { + return 1; + } + if (kcp_client_text_body_equals(msg, KCP_CLIENT_CTRL_PEER_REPLACED)) { + kcp_client_set_registered(client, 0); + kcp_client_set_last_server_error(client, "server peer replaced this session"); + (void) kcp_conn_close(client->conn); + errno = ECONNRESET; + return -1; + } + return 0; +} + +static int kcp_client_remaining_timeout_ms(int original_timeout_ms, uint32_t start_ms) { + uint32_t elapsed_ms; + + if (original_timeout_ms < 0) { + return -1; + } + elapsed_ms = omni_now_millis32() - start_ms; + if (elapsed_ms >= (uint32_t) original_timeout_ms) { + return 0; + } + return original_timeout_ms - (int) elapsed_ms; +} + +static int kcp_client_wait_for_register_ok(kcp_client_t *client) { + uint32_t start_ms; + + if (client == NULL || client->conn == NULL) { + errno = EINVAL; + return -1; + } + + start_ms = omni_now_millis32(); + for (;;) { + message_t msg; + int rc; + int remaining_timeout_ms = kcp_client_remaining_timeout_ms(KCP_CLIENT_REGISTER_TIMEOUT_MS, start_ms); + + if (remaining_timeout_ms <= 0) { + kcp_client_set_registered(client, 0); + kcp_client_set_last_server_error(client, "timed out waiting for server_register_ok"); + (void) kcp_conn_close(client->conn); + errno = ETIMEDOUT; + return -1; + } + + protocol_message_init(&msg); + rc = kcp_conn_receive_timed(client->conn, &msg, remaining_timeout_ms); + if (rc == 1) { + protocol_message_clear(&msg); + kcp_client_set_registered(client, 0); + kcp_client_set_last_server_error(client, "timed out waiting for server_register_ok"); + (void) kcp_conn_close(client->conn); + errno = ETIMEDOUT; + return -1; + } + if (rc != 0) { + protocol_message_clear(&msg); + kcp_client_set_registered(client, 0); + return -1; + } + if (msg.type == MSG_TYPE_ERROR && strcmp(msg.from, SERVER_PEER_ID) == 0) { + char error_text[256]; + + kcp_client_copy_server_error_body(&msg, error_text, sizeof(error_text)); + kcp_client_touch_server_activity(client); + kcp_client_set_registered(client, 0); + kcp_client_set_last_server_error(client, error_text); + protocol_message_clear(&msg); + (void) kcp_conn_close(client->conn); + errno = kcp_client_registration_errno_from_message(error_text); + return -1; + } + rc = kcp_client_handle_reserved_server_message(client, &msg); + protocol_message_clear(&msg); + if (rc < 0) { + return -1; + } + if (rc > 0 && kcp_client_is_registered(client)) { + return 0; + } + + kcp_client_set_registered(client, 0); + kcp_client_set_last_server_error(client, "unexpected message while waiting for server_register_ok"); + (void) kcp_conn_close(client->conn); + errno = EPROTO; + return -1; + } +} + +static int kcp_client_receive_business_timed(kcp_client_t *client, message_t *out_msg, int timeout_ms) { + uint32_t start_ms; + + if (client == NULL || out_msg == NULL || client->conn == NULL) { + errno = EINVAL; + return -1; + } + + start_ms = omni_now_millis32(); + protocol_message_init(out_msg); + for (;;) { + int rc; + int reserved_rc; + int effective_timeout_ms = timeout_ms < 0 ? -1 : kcp_client_remaining_timeout_ms(timeout_ms, start_ms); + + if (timeout_ms >= 0 && effective_timeout_ms <= 0) { + return 1; + } + protocol_message_clear(out_msg); + rc = kcp_conn_receive_timed(client->conn, out_msg, effective_timeout_ms); + if (rc != 0) { + if (rc != 1) { + kcp_client_set_registered(client, 0); + } + return rc; + } + + if (strcmp(out_msg->from, SERVER_PEER_ID) == 0) { + kcp_client_touch_server_activity(client); + } + reserved_rc = kcp_client_handle_reserved_server_message(client, out_msg); + if (reserved_rc < 0) { + protocol_message_clear(out_msg); + return -1; + } + if (reserved_rc > 0) { + protocol_message_clear(out_msg); + continue; + } + if (out_msg->type == MSG_TYPE_ERROR && strcmp(out_msg->from, SERVER_PEER_ID) == 0) { + char error_text[256]; + + kcp_client_copy_server_error_body(out_msg, error_text, sizeof(error_text)); + kcp_client_set_last_server_error(client, error_text); + if (kcp_client_server_error_invalidates_registration(error_text)) { + kcp_client_set_registered(client, 0); + } + } + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_B_APP_RECV, out_msg); + return 0; + } +} + +static int kcp_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) { + return -1; + } + if (msg->type == MSG_TYPE_TEXT) { + char *body = omni_json_escape_bytes(msg->body, msg->body_len); + char *from = omni_json_escape(msg->from); + char *to = omni_json_escape(msg->to); + char *line; + + if (body == NULL || from == NULL || to == NULL) { + free(body); + free(from); + free(to); + return -1; + } + snprintf(path, sizeof(path), "%s/messages.log", inbox_dir); + line = omni_strdup_printf( + "{\"message_type\":\"%s\",\"message_id\":%" PRIu64 ",\"from\":\"%s\",\"to\":\"%s\",\"body\":\"%s\"}\n", + protocol_message_type_name(msg->type), + msg->id, + from, + to, + body + ); + free(body); + free(from); + free(to); + if (line == NULL) { + return -1; + } + if (omni_append_file(path, (const uint8_t *) line, strlen(line)) != 0) { + free(line); + return -1; + } + free(line); + } else if (msg->type == MSG_TYPE_FILE) { + const char *file_name = omni_path_base_name(msg->file_name); + if (file_name[0] == '\0') { + file_name = "unnamed"; + } + snprintf(path, sizeof(path), "%s/%s-%" PRIu64 "-%s", inbox_dir, msg->from, msg->id, file_name); + if (omni_write_file(path, msg->body, msg->body_len) != 0) { + return -1; + } + } else if (msg->type == MSG_TYPE_BINARY) { + snprintf(path, sizeof(path), "%s/%s-%" PRIu64 ".bin", inbox_dir, msg->from, msg->id); + if (omni_write_file(path, msg->body, msg->body_len) != 0) { + return -1; + } + } else { + errno = EINVAL; + return -1; + } + + if (out_path != NULL && out_path_len > 0) { + snprintf(out_path, out_path_len, "%s", path); + } + return 0; +} + +static void kcp_client_fill_recv_meta(kcp_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); +} + +kcp_client_t *kcp_client_dial_with_options(const char *server_addr, const char *dial_addr, const char *peer_id, const char *bind_ip, const char *bind_device, const kcp_conn_options_t *options, latency_logger_t *logger, kcp_packet_debug_logger_t *packet_logger, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms) { + kcp_client_t *client; + const char *actual_dial_addr = (dial_addr != NULL && dial_addr[0] != '\0') ? dial_addr : server_addr; + message_t register_msg; + int saved_errno = 0; + + client = (kcp_client_t *) calloc(1, sizeof(*client)); + if (client == NULL) { + return NULL; + } + snprintf(client->id, sizeof(client->id), "%s", peer_id); + snprintf(client->server_addr, sizeof(client->server_addr), "%s", server_addr == NULL ? "" : server_addr); + pthread_mutex_init(&client->state_mu, NULL); + client->last_server_activity_ms = omni_now_millis32(); + client->logger = logger; + client->conn = kcp_conn_dial_with_options(actual_dial_addr, bind_ip, bind_device, options, packet_logger, logger, OMNI_NODE_ROLE_PEER, peer_id, stats_logger, stats_interval_ms); + if (client->conn == NULL) { + saved_errno = errno; + kcp_client_free(client); + errno = saved_errno; + return NULL; + } + + protocol_message_init(®ister_msg); + register_msg.type = MSG_TYPE_REGISTER; + register_msg.id = 0; + snprintf(register_msg.from, sizeof(register_msg.from), "%s", peer_id); + snprintf(register_msg.to, sizeof(register_msg.to), "%s", SERVER_PEER_ID); + if (kcp_conn_send(client->conn, ®ister_msg) != 0) { + saved_errno = errno; + kcp_client_free(client); + errno = saved_errno; + return NULL; + } + if (kcp_client_wait_for_register_ok(client) != 0) { + saved_errno = errno; + kcp_client_free(client); + errno = saved_errno; + return NULL; + } + return client; +} + +kcp_client_t *kcp_client_dial(const char *server_addr, const char *dial_addr, const char *peer_id, const char *bind_ip, const char *bind_device, latency_logger_t *logger, kcp_packet_debug_logger_t *packet_logger, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms) { + return kcp_client_dial_with_options(server_addr, dial_addr, peer_id, bind_ip, bind_device, NULL, logger, packet_logger, stats_logger, stats_interval_ms); +} + +const char *kcp_client_id(const kcp_client_t *client) { + return client == NULL ? "" : client->id; +} + +int kcp_client_send_text(kcp_client_t *client, const char *to, const char *text) { + if (client == NULL || to == NULL || text == NULL) { + errno = EINVAL; + return -1; + } + if (kcp_client_send_business_preflight(client) != 0) { + return -1; + } + return kcp_client_send_text_internal(client, to, text, 1); +} + +int kcp_client_send_binary(kcp_client_t *client, const char *to, const void *data, size_t data_len) { + return kcp_client_send_binary_with_id(client, to, data, data_len, NULL); +} + +int kcp_client_send_binary_with_id( + kcp_client_t *client, + const char *to, + const void *data, + size_t data_len, + uint64_t *out_id +) { + message_t msg; + uint64_t id; + + if (client == NULL || to == NULL || (data == NULL && data_len > 0)) { + errno = EINVAL; + return -1; + } + if (kcp_client_send_business_preflight(client) != 0) { + return -1; + } + protocol_message_init(&msg); + if (kcp_client_next_message_id(client, &id) != 0) { + return -1; + } + 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 (kcp_conn_send(client->conn, &msg) != 0) { + protocol_message_clear(&msg); + return -1; + } + if (out_id != NULL) { + *out_id = id; + } + protocol_message_clear(&msg); + return 0; +} + +int kcp_client_send_file_path(kcp_client_t *client, const char *to, const char *path) { + message_t msg; + uint64_t id; + uint8_t *body = NULL; + size_t body_len = 0; + const char *base_name = strrchr(path, '/'); + + if (client == NULL || to == NULL || path == NULL) { + errno = EINVAL; + return -1; + } + if (kcp_client_send_business_preflight(client) != 0) { + return -1; + } + if (omni_read_file(path, &body, &body_len) != 0) { + return -1; + } + protocol_message_init(&msg); + if (kcp_client_next_message_id(client, &id) != 0) { + free(body); + return -1; + } + msg.type = MSG_TYPE_FILE; + msg.id = id; + snprintf(msg.from, sizeof(msg.from), "%s", client->id); + snprintf(msg.to, sizeof(msg.to), "%s", to); + snprintf(msg.file_name, sizeof(msg.file_name), "%s", base_name == NULL ? path : base_name + 1); + msg.body = body; + msg.body_len = body_len; + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_A_APP_PREP_BEGIN, &msg); + if (kcp_conn_send(client->conn, &msg) != 0) { + protocol_message_clear(&msg); + return -1; + } + protocol_message_clear(&msg); + return 0; +} + +int kcp_client_receive_timed(kcp_client_t *client, message_t *out_msg, int timeout_ms) { + if (client == NULL || out_msg == NULL) { + errno = EINVAL; + return -1; + } + return kcp_client_receive_business_timed(client, out_msg, timeout_ms); +} + +int kcp_client_receive(kcp_client_t *client, message_t *out_msg) { + if (kcp_client_receive_timed(client, out_msg, -1) != 0) { + return -1; + } + return 0; +} + +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) { + 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 = kcp_client_receive_timed(client, &msg, timeout_ms); + if (rc != 0) { + protocol_message_clear(&msg); + return rc; + } + + kcp_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 kcp_client_persist_message(kcp_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; + return -1; + } + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_B_PERSIST_BEGIN, msg); + if (kcp_client_persist_message_to_disk(msg, inbox_dir, out_path, out_path_len) != 0) { + return -1; + } + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_B_PERSIST_END, msg); + return 0; +} + +void kcp_client_state_snapshot(kcp_client_t *client, kcp_client_state_t *out_state) { + kcp_runtime_stats_t runtime_stats; + + if (out_state == NULL) { + return; + } + memset(out_state, 0, sizeof(*out_state)); + if (client == NULL) { + return; + } + memset(&runtime_stats, 0, sizeof(runtime_stats)); + if (client->conn != NULL) { + kcp_conn_runtime_stats_snapshot(client->conn, &runtime_stats); + out_state->connected = runtime_stats.connected; + } + pthread_mutex_lock(&client->state_mu); + out_state->registered = client->registered; + out_state->server_idle_ms = client->last_server_activity_ms == 0 + ? 0 + : (omni_now_millis32() - client->last_server_activity_ms); + snprintf(out_state->last_server_error, sizeof(out_state->last_server_error), "%s", client->last_server_error); + pthread_mutex_unlock(&client->state_mu); +} + +void kcp_client_runtime_stats_snapshot(kcp_client_t *client, kcp_runtime_stats_t *out_stats) { + if (out_stats == NULL) { + return; + } + + 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) { + if (client == NULL) { + return 0; + } + kcp_client_set_registered(client, 0); + return kcp_conn_close(client->conn); +} + +void kcp_client_free(kcp_client_t *client) { + if (client == NULL) { + return; + } + kcp_conn_free(client->conn); + pthread_mutex_destroy(&client->state_mu); + free(client); +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/src/peer_udp_client.c b/robot/ros2/OmniSocketGo_robot_ros/src/peer_udp_client.c new file mode 100644 index 0000000..9e617ab --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/src/peer_udp_client.c @@ -0,0 +1,297 @@ +#include "peer_udp_client.h" + +#include +#include +#include + +struct udp_client { + char id[OMNI_MAX_PEER_ID]; + udp_conn_t *conn; + latency_logger_t *logger; + pthread_mutex_t id_mu; + uint64_t next_message_id; +}; + +static int client_next_message_id(udp_client_t *client, uint64_t *out_id) { + pthread_mutex_lock(&client->id_mu); + *out_id = ++client->next_message_id; + pthread_mutex_unlock(&client->id_mu); + 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) { + return -1; + } + if (msg->type == MSG_TYPE_TEXT) { + char *body = omni_json_escape_bytes(msg->body, msg->body_len); + char *from = omni_json_escape(msg->from); + char *to = omni_json_escape(msg->to); + char *line; + if (body == NULL || from == NULL || to == NULL) { + free(body); + free(from); + free(to); + return -1; + } + snprintf(path, sizeof(path), "%s/messages.log", inbox_dir); + line = omni_strdup_printf("{\"message_type\":\"%s\",\"message_id\":%" PRIu64 ",\"from\":\"%s\",\"to\":\"%s\",\"body\":\"%s\"}\n", protocol_message_type_name(msg->type), msg->id, from, to, body); + free(body); + free(from); + free(to); + if (line == NULL) { + return -1; + } + if (omni_append_file(path, (const uint8_t *) line, strlen(line)) != 0) { + free(line); + return -1; + } + free(line); + } else if (msg->type == MSG_TYPE_FILE) { + const char *file_name = omni_path_base_name(msg->file_name); + if (file_name[0] == '\0') { + file_name = "unnamed"; + } + snprintf(path, sizeof(path), "%s/%s-%" PRIu64 "-%s", inbox_dir, msg->from, msg->id, file_name); + if (omni_write_file(path, msg->body, msg->body_len) != 0) { + return -1; + } + } else if (msg->type == MSG_TYPE_BINARY) { + snprintf(path, sizeof(path), "%s/%s-%" PRIu64 ".bin", inbox_dir, msg->from, msg->id); + if (omni_write_file(path, msg->body, msg->body_len) != 0) { + return -1; + } + } else { + errno = EINVAL; + return -1; + } + if (out_path != NULL && out_path_len > 0) { + snprintf(out_path, out_path_len, "%s", path); + } + return 0; +} + +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)); + if (client == NULL) { + return NULL; + } + 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, bind_device, enable_timestamping, logger, OMNI_NODE_ROLE_PEER, peer_id, debug_logger); + if (client->conn == NULL) { + udp_client_free(client); + return NULL; + } + protocol_message_init(®ister_msg); + register_msg.type = MSG_TYPE_REGISTER; + register_msg.id = 0; + snprintf(register_msg.from, sizeof(register_msg.from), "%s", peer_id); + snprintf(register_msg.to, sizeof(register_msg.to), "%s", SERVER_PEER_ID); + if (udp_conn_send(client->conn, ®ister_msg) != 0) { + udp_client_free(client); + return NULL; + } + 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; +} + +int udp_client_send_text(udp_client_t *client, const char *to, const char *text) { + message_t msg; + uint64_t id; + protocol_message_init(&msg); + client_next_message_id(client, &id); + msg.type = MSG_TYPE_TEXT; + msg.id = id; + snprintf(msg.from, sizeof(msg.from), "%s", client->id); + snprintf(msg.to, sizeof(msg.to), "%s", to); + msg.body = (uint8_t *) omni_strdup(text); + if (msg.body == NULL) { + return -1; + } + msg.body_len = strlen((const char *) msg.body); + 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_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; + uint8_t *body = NULL; + size_t body_len = 0; + const char *base_name = strrchr(path, '/'); + if (omni_read_file(path, &body, &body_len) != 0) { + return -1; + } + protocol_message_init(&msg); + client_next_message_id(client, &id); + msg.type = MSG_TYPE_FILE; + msg.id = id; + snprintf(msg.from, sizeof(msg.from), "%s", client->id); + snprintf(msg.to, sizeof(msg.to), "%s", to); + snprintf(msg.file_name, sizeof(msg.file_name), "%s", base_name == NULL ? path : base_name + 1); + msg.body = body; + msg.body_len = body_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_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; + } + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_B_APP_RECV, 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; + return -1; + } + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_B_PERSIST_BEGIN, msg); + if (client_persist_message_to_disk(msg, inbox_dir, out_path, out_path_len) != 0) { + return -1; + } + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_B_PERSIST_END, msg); + return 0; +} + +int udp_client_close(udp_client_t *client) { + return client == NULL ? 0 : udp_conn_close(client->conn); +} + +void udp_client_free(udp_client_t *client) { + if (client == NULL) { + return; + } + udp_conn_free(client->conn); + pthread_mutex_destroy(&client->id_mu); + free(client); +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/src/protocol.c b/robot/ros2/OmniSocketGo_robot_ros/src/protocol.c new file mode 100644 index 0000000..d82d045 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/src/protocol.c @@ -0,0 +1,415 @@ +#include "protocol.h" + +#include "cJSON.h" + +#include + +static const char *protocol_message_type_table[] = { + "text", + "file", + "register", + "error", + "binary" +}; + +const char *protocol_message_type_name(message_type_t type) { + if ((int) type < 0 || (size_t) type >= OMNI_ARRAY_LEN(protocol_message_type_table)) { + return "invalid"; + } + return protocol_message_type_table[type]; +} + +int protocol_message_type_from_name(const char *raw, message_type_t *out) { + size_t i; + if (raw == NULL || out == NULL) { + return -1; + } + for (i = 0; i < OMNI_ARRAY_LEN(protocol_message_type_table); ++i) { + if (strcmp(raw, protocol_message_type_table[i]) == 0) { + *out = (message_type_t) i; + return 0; + } + } + return -1; +} + +void protocol_message_init(message_t *msg) { + if (msg == NULL) { + return; + } + memset(msg, 0, sizeof(*msg)); + msg->type = MSG_TYPE_INVALID; +} + +void protocol_message_clear(message_t *msg) { + if (msg == NULL) { + return; + } + free(msg->body); + protocol_message_init(msg); +} + +int protocol_message_copy(message_t *dst, const message_t *src) { + if (dst == NULL || src == NULL) { + errno = EINVAL; + return -1; + } + protocol_message_clear(dst); + memcpy(dst, src, sizeof(*dst)); + dst->body = NULL; + if (src->body_len > 0) { + dst->body = (uint8_t *) malloc(src->body_len); + if (dst->body == NULL) { + protocol_message_init(dst); + errno = ENOMEM; + return -1; + } + memcpy(dst->body, src->body, src->body_len); + } + return 0; +} + +static int protocol_set_err(char *err, size_t err_len, const char *fmt, ...) { + va_list args; + if (err != NULL && err_len > 0) { + va_start(args, fmt); + vsnprintf(err, err_len, fmt, args); + va_end(args); + } + return -1; +} + +int protocol_validate_message(const message_t *msg, char *err, size_t err_len) { + if (msg == NULL) { + return protocol_set_err(err, err_len, "protocol: nil message"); + } + if (msg->from[0] == '\0') { + return protocol_set_err(err, err_len, "protocol: missing from"); + } + if (msg->to[0] == '\0') { + return protocol_set_err(err, err_len, "protocol: missing to"); + } + switch (msg->type) { + case MSG_TYPE_TEXT: + if (msg->file_name[0] != '\0') { + return protocol_set_err(err, err_len, "protocol: unexpected file name"); + } + if (!omni_utf8_valid(msg->body, msg->body_len)) { + return protocol_set_err(err, err_len, "protocol: invalid text body"); + } + break; + case MSG_TYPE_FILE: + if (msg->file_name[0] == '\0') { + return protocol_set_err(err, err_len, "protocol: missing file name"); + } + break; + case MSG_TYPE_BINARY: + if (msg->file_name[0] != '\0') { + return protocol_set_err(err, err_len, "protocol: unexpected file name"); + } + break; + case MSG_TYPE_REGISTER: + if (strcmp(msg->to, SERVER_PEER_ID) != 0) { + return protocol_set_err(err, err_len, "protocol: invalid register target"); + } + if (msg->file_name[0] != '\0') { + return protocol_set_err(err, err_len, "protocol: unexpected file name"); + } + if (msg->body_len != 0) { + return protocol_set_err(err, err_len, "protocol: unexpected body"); + } + break; + case MSG_TYPE_ERROR: + if (strcmp(msg->from, SERVER_PEER_ID) != 0) { + return protocol_set_err(err, err_len, "protocol: invalid error source"); + } + if (msg->file_name[0] != '\0') { + return protocol_set_err(err, err_len, "protocol: unexpected file name"); + } + if (!omni_utf8_valid(msg->body, msg->body_len)) { + return protocol_set_err(err, err_len, "protocol: invalid text body"); + } + break; + default: + return protocol_set_err(err, err_len, "protocol: invalid message type"); + } + return 0; +} + +static int protocol_build_header_json(const message_t *msg, char **out_json, size_t *out_len) { + cJSON *root; + char *json; + + root = cJSON_CreateObject(); + if (root == NULL) { + errno = ENOMEM; + return -1; + } + cJSON_AddStringToObject(root, "type", protocol_message_type_name(msg->type)); + cJSON_AddNumberToObject(root, "id", (double) msg->id); + cJSON_AddStringToObject(root, "from", msg->from); + cJSON_AddStringToObject(root, "to", msg->to); + if (msg->file_name[0] != '\0') { + cJSON_AddStringToObject(root, "file_name", msg->file_name); + } + cJSON_AddNumberToObject(root, "content_length", (double) msg->body_len); + json = cJSON_PrintUnformatted(root); + cJSON_Delete(root); + if (json == NULL) { + errno = ENOMEM; + return -1; + } + *out_len = strlen(json); + *out_json = json; + return 0; +} + +int protocol_encode_message_datagram(const message_t *msg, uint8_t **out, size_t *out_len) { + uint8_t *buffer; + char *header_json; + size_t header_len; + uint32_t net_header_len; + char err[128]; + + if (out == NULL || out_len == NULL) { + errno = EINVAL; + return -1; + } + *out = NULL; + *out_len = 0; + if (protocol_validate_message(msg, err, sizeof(err)) != 0) { + errno = EINVAL; + return -1; + } + if (protocol_build_header_json(msg, &header_json, &header_len) != 0) { + return -1; + } + if (4U + header_len + msg->body_len > OMNI_MAX_FRAME_SIZE) { + cJSON_free(header_json); + errno = EMSGSIZE; + return -1; + } + buffer = (uint8_t *) malloc(4U + header_len + msg->body_len); + if (buffer == NULL) { + cJSON_free(header_json); + errno = ENOMEM; + return -1; + } + net_header_len = htonl((uint32_t) header_len); + memcpy(buffer, &net_header_len, 4); + memcpy(buffer + 4, header_json, header_len); + if (msg->body_len > 0) { + memcpy(buffer + 4 + header_len, msg->body, msg->body_len); + } + cJSON_free(header_json); + *out = buffer; + *out_len = 4U + header_len + msg->body_len; + return 0; +} + +static int protocol_copy_string_field(char *dst, size_t dst_len, const cJSON *object, const char *field, int required, char *err, size_t err_len) { + const cJSON *item = cJSON_GetObjectItemCaseSensitive(object, field); + if (item == NULL) { + if (required) { + return protocol_set_err(err, err_len, "protocol: missing %s", field); + } + dst[0] = '\0'; + return 0; + } + if (!cJSON_IsString(item) || item->valuestring == NULL) { + return protocol_set_err(err, err_len, "protocol: invalid %s", field); + } + snprintf(dst, dst_len, "%s", item->valuestring); + return 0; +} + +static int protocol_copy_u64_field(uint64_t *dst, const cJSON *object, const char *field, int required, char *err, size_t err_len) { + const cJSON *item = cJSON_GetObjectItemCaseSensitive(object, field); + if (item == NULL) { + if (required) { + return protocol_set_err(err, err_len, "protocol: missing %s", field); + } + *dst = 0; + return 0; + } + if (!cJSON_IsNumber(item)) { + return protocol_set_err(err, err_len, "protocol: invalid %s", field); + } + *dst = (uint64_t) item->valuedouble; + return 0; +} + +int protocol_decode_message_datagram(const uint8_t *data, size_t data_len, message_t *out_msg, char *err, size_t err_len) { + uint32_t net_header_len; + uint32_t header_len; + char *header_text = NULL; + cJSON *header = NULL; + const cJSON *type_item; + uint64_t content_length = 0; + + if (data == NULL || out_msg == NULL || data_len < 4U) { + return protocol_set_err(err, err_len, "protocol: invalid datagram"); + } + if (data_len > OMNI_MAX_FRAME_SIZE) { + return protocol_set_err(err, err_len, "protocol: frame too large"); + } + + protocol_message_clear(out_msg); + + memcpy(&net_header_len, data, 4); + header_len = ntohl(net_header_len); + if (header_len == 0 || (size_t) header_len > data_len - 4U) { + return protocol_set_err(err, err_len, "protocol: invalid header length"); + } + header_text = (char *) malloc((size_t) header_len + 1U); + if (header_text == NULL) { + errno = ENOMEM; + return -1; + } + memcpy(header_text, data + 4, header_len); + header_text[header_len] = '\0'; + header = cJSON_Parse(header_text); + free(header_text); + if (header == NULL || !cJSON_IsObject(header)) { + if (header != NULL) { + cJSON_Delete(header); + } + return protocol_set_err(err, err_len, "protocol: invalid header json"); + } + type_item = cJSON_GetObjectItemCaseSensitive(header, "type"); + if (type_item == NULL || !cJSON_IsString(type_item) || protocol_message_type_from_name(type_item->valuestring, &out_msg->type) != 0) { + cJSON_Delete(header); + return protocol_set_err(err, err_len, "protocol: invalid message type"); + } + if (protocol_copy_u64_field(&out_msg->id, header, "id", 1, err, err_len) != 0 || + protocol_copy_string_field(out_msg->from, sizeof(out_msg->from), header, "from", 1, err, err_len) != 0 || + protocol_copy_string_field(out_msg->to, sizeof(out_msg->to), header, "to", 1, err, err_len) != 0 || + protocol_copy_string_field(out_msg->file_name, sizeof(out_msg->file_name), header, "file_name", 0, err, err_len) != 0 || + protocol_copy_u64_field(&content_length, header, "content_length", 1, err, err_len) != 0) { + cJSON_Delete(header); + protocol_message_clear(out_msg); + return -1; + } + cJSON_Delete(header); + if ((size_t) content_length != data_len - 4U - (size_t) header_len) { + protocol_message_clear(out_msg); + return protocol_set_err(err, err_len, "protocol: invalid content length"); + } + out_msg->body_len = (size_t) content_length; + if (out_msg->body_len > 0) { + out_msg->body = (uint8_t *) malloc(out_msg->body_len); + if (out_msg->body == NULL) { + protocol_message_clear(out_msg); + errno = ENOMEM; + return -1; + } + memcpy(out_msg->body, data + 4U + header_len, out_msg->body_len); + } + if (protocol_validate_message(out_msg, err, err_len) != 0) { + protocol_message_clear(out_msg); + return -1; + } + return 0; +} + +int protocol_encode_message_stream(const message_t *msg, uint8_t **out, size_t *out_len) { + uint8_t *payload; + uint8_t *buffer; + size_t payload_len; + uint32_t net_len; + + if (protocol_encode_message_datagram(msg, &payload, &payload_len) != 0) { + return -1; + } + buffer = (uint8_t *) malloc(payload_len + 4U); + if (buffer == NULL) { + free(payload); + errno = ENOMEM; + return -1; + } + net_len = htonl((uint32_t) payload_len); + memcpy(buffer, &net_len, 4); + memcpy(buffer + 4, payload, payload_len); + free(payload); + *out = buffer; + *out_len = payload_len + 4U; + return 0; +} + +int protocol_decode_message_stream_payload(const uint8_t *payload, size_t payload_len, message_t *out_msg, char *err, size_t err_len) { + return protocol_decode_message_datagram(payload, payload_len, out_msg, err, err_len); +} + +void protocol_frame_decoder_init(protocol_frame_decoder_t *decoder) { + memset(decoder, 0, sizeof(*decoder)); +} + +void protocol_frame_decoder_reset(protocol_frame_decoder_t *decoder) { + decoder->len = 0; +} + +void protocol_frame_decoder_destroy(protocol_frame_decoder_t *decoder) { + free(decoder->buffer); + memset(decoder, 0, sizeof(*decoder)); +} + +int protocol_frame_decoder_feed(protocol_frame_decoder_t *decoder, const uint8_t *data, size_t data_len) { + uint8_t *next_buffer; + size_t next_cap; + if (decoder->len + data_len > OMNI_MAX_FRAME_SIZE * 2U) { + errno = EMSGSIZE; + return -1; + } + if (decoder->len + data_len > decoder->cap) { + next_cap = decoder->cap == 0 ? 4096U : decoder->cap; + while (next_cap < decoder->len + data_len) { + next_cap *= 2U; + } + next_buffer = (uint8_t *) realloc(decoder->buffer, next_cap); + if (next_buffer == NULL) { + errno = ENOMEM; + return -1; + } + decoder->buffer = next_buffer; + decoder->cap = next_cap; + } + memcpy(decoder->buffer + decoder->len, data, data_len); + decoder->len += data_len; + return 0; +} + +int protocol_frame_decoder_next(protocol_frame_decoder_t *decoder, uint8_t **payload, size_t *payload_len) { + uint32_t net_len; + uint32_t frame_len; + uint8_t *frame; + + if (payload == NULL || payload_len == NULL) { + errno = EINVAL; + return -1; + } + *payload = NULL; + *payload_len = 0; + if (decoder->len < 4U) { + return 0; + } + memcpy(&net_len, decoder->buffer, 4); + frame_len = ntohl(net_len); + if (frame_len == 0 || frame_len > OMNI_MAX_FRAME_SIZE) { + errno = EMSGSIZE; + return -1; + } + if (decoder->len < 4U + frame_len) { + return 0; + } + frame = (uint8_t *) malloc(frame_len); + if (frame == NULL) { + errno = ENOMEM; + return -1; + } + memcpy(frame, decoder->buffer + 4, frame_len); + memmove(decoder->buffer, decoder->buffer + 4U + frame_len, decoder->len - 4U - frame_len); + decoder->len -= 4U + frame_len; + *payload = frame; + *payload_len = frame_len; + return 1; +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/src/ros_image_shm.c b/robot/ros2/OmniSocketGo_robot_ros/src/ros_image_shm.c new file mode 100644 index 0000000..0dd935b --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/src/ros_image_shm.c @@ -0,0 +1,167 @@ +#include "ros_image_shm.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +static uint64_t monotonic_ms(void) { + struct timespec ts; + + if (clock_gettime(CLOCK_MONOTONIC, &ts) != 0) { + return 0; + } + return (uint64_t) ts.tv_sec * 1000U + (uint64_t) ts.tv_nsec / 1000000U; +} + +static uint64_t load_sequence(const ros_image_shm_source_t *source) { + const uint64_t *sequence; + + sequence = (const uint64_t *) source->mapping; + return __atomic_load_n(sequence, __ATOMIC_ACQUIRE); +} + +static int validate_header( + const ros_image_shm_header_t *header, + size_t max_frame_bytes, + size_t destination_bytes +) { + size_t required_bytes; + + if (header == NULL + || header->magic != ROS_IMAGE_SHM_MAGIC + || header->version != ROS_IMAGE_SHM_VERSION + || header->width == 0 + || header->height == 0 + || header->stride == 0 + || header->data_bytes == 0 + || header->data_bytes > max_frame_bytes + || header->data_bytes > destination_bytes + ) { + errno = EPROTO; + return -1; + } + + required_bytes = (size_t) header->stride * (size_t) header->height; + if (required_bytes < header->data_bytes || required_bytes > max_frame_bytes) { + errno = EPROTO; + return -1; + } + return 0; +} + +int ros_image_shm_open( + ros_image_shm_source_t *source, + const char *path, + size_t max_frame_bytes +) { + struct stat st; + + if (source == NULL || path == NULL || path[0] == '\0' || max_frame_bytes == 0) { + errno = EINVAL; + return -1; + } + memset(source, 0, sizeof(*source)); + source->fd = -1; + source->max_frame_bytes = max_frame_bytes; + snprintf(source->path, sizeof(source->path), "%s", path); + + source->fd = open(path, O_RDWR | O_CLOEXEC); + if (source->fd < 0) { + return -1; + } + if (fstat(source->fd, &st) != 0) { + ros_image_shm_close(source); + return -1; + } + if ((size_t) st.st_size < ROS_IMAGE_SHM_HEADER_BYTES + max_frame_bytes) { + errno = EINVAL; + ros_image_shm_close(source); + return -1; + } + source->mapping_bytes = (size_t) st.st_size; + source->mapping = mmap(NULL, source->mapping_bytes, PROT_READ, MAP_SHARED, source->fd, 0); + if (source->mapping == MAP_FAILED) { + source->mapping = NULL; + ros_image_shm_close(source); + return -1; + } + return 0; +} + +void ros_image_shm_close(ros_image_shm_source_t *source) { + if (source == NULL) { + return; + } + if (source->mapping != NULL) { + munmap(source->mapping, source->mapping_bytes); + } + if (source->fd >= 0) { + close(source->fd); + } + source->mapping = NULL; + source->mapping_bytes = 0; + source->fd = -1; + source->last_sequence = 0; +} + +int ros_image_shm_read_latest( + ros_image_shm_source_t *source, + uint8_t *destination, + size_t destination_bytes, + ros_image_shm_header_t *header, + int timeout_ms +) { + uint64_t deadline; + + if (source == NULL || source->mapping == NULL || destination == NULL || header == NULL + || destination_bytes == 0 || timeout_ms < 0) { + errno = EINVAL; + return -1; + } + deadline = monotonic_ms() + (uint64_t) timeout_ms; + + for (;;) { + ros_image_shm_header_t snapshot; + uint64_t sequence_before; + uint64_t sequence_after; + + sequence_before = load_sequence(source); + if (sequence_before != 0 && (sequence_before & 1U) == 0U + && sequence_before != source->last_sequence) { + memcpy(&snapshot, source->mapping, sizeof(snapshot)); + if (validate_header(&snapshot, source->max_frame_bytes, destination_bytes) != 0) { + /* A writer may have updated the header between the sequence checks. */ + sequence_after = load_sequence(source); + if (sequence_after != sequence_before) { + continue; + } + return -1; + } + memcpy(destination, + (const uint8_t *) source->mapping + ROS_IMAGE_SHM_HEADER_BYTES, + snapshot.data_bytes); + sequence_after = load_sequence(source); + if (sequence_after == sequence_before) { + *header = snapshot; + source->last_sequence = sequence_before; + return 1; + } + continue; + } + + if (monotonic_ms() >= deadline) { + errno = ETIMEDOUT; + return 0; + } + struct timespec pause_time = {0, 1000000L}; + nanosleep(&pause_time, NULL); + sched_yield(); + } +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/src/server_kcp_hub.c b/robot/ros2/OmniSocketGo_robot_ros/src/server_kcp_hub.c new file mode 100644 index 0000000..cdcf2f3 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/src/server_kcp_hub.c @@ -0,0 +1,1136 @@ +#include "server_kcp_hub.h" + +#include "cJSON.h" + +#include +#include +#include +#include +#include + +#define KCP_RELAY_MAX_DATAGRAM_SIZE (60 * 1024) +#define KCP_HUB_MAINTENANCE_INTERVAL_MS 250 +#define KCP_HUB_DEFAULT_TELEMETRY_INTERVAL_MS 500 +#define KCP_HUB_DEFAULT_HEARTBEAT_INTERVAL_MS 1000 +#define KCP_HUB_DEFAULT_LEASE_TIMEOUT_MS 4000 +#define KCP_HUB_TELEMETRY_NODE_ID "hub-telemetry" +#define KCP_HUB_DEFAULT_NODE_ID "hub" +#define KCP_HUB_CTRL_REGISTER_OK "{\"type\":\"server_register_ok\"}" +#define KCP_HUB_CTRL_PEER_REPLACED "{\"type\":\"server_peer_replaced\",\"reason\":\"new_instance_wins\"}" +#define KCP_HUB_CTRL_HEARTBEAT "{\"type\":\"server_heartbeat\"}" +#define KCP_HUB_CTRL_HEARTBEAT_ACK "{\"type\":\"server_heartbeat_ack\"}" + +typedef struct kcp_peer_entry { + struct kcp_peer_entry *next; + char peer_id[OMNI_MAX_PEER_ID]; + kcp_conn_t *conn; + uint32_t last_seen_ms; + uint32_t last_heartbeat_sent_ms; +} kcp_peer_entry_t; + +typedef struct kcp_session_thread_ctx { + kcp_hub_t *hub; + kcp_conn_t *conn; +} kcp_session_thread_ctx_t; + +typedef struct kcp_hub_pending_action { + struct kcp_hub_pending_action *next; + char peer_id[OMNI_MAX_PEER_ID]; + kcp_conn_t *conn; +} kcp_hub_pending_action_t; + +struct kcp_hub { + pthread_rwlock_t lock; + kcp_peer_entry_t *peers; + 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; + int heartbeat_interval_ms; + int lease_timeout_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; + 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_send_server_text(kcp_conn_t *conn, const char *to, const char *payload); +static void kcp_hub_touch_peer(kcp_hub_t *hub, const char *peer_id, kcp_conn_t *conn); +static void kcp_hub_run_maintenance(kcp_hub_t *hub); + +static uint32_t kcp_hub_now_ms(void) { + return omni_now_millis32(); +} + +static uint32_t kcp_hub_elapsed_ms(uint32_t now_ms, uint32_t then_ms) { + return now_ms - then_ms; +} + +static int kcp_hub_text_body_equals(const message_t *msg, const char *payload) { + size_t expected_len; + + if (msg == NULL || payload == NULL) { + return 0; + } + expected_len = strlen(payload); + return msg->body_len == expected_len && msg->body != NULL && memcmp(msg->body, payload, expected_len) == 0; +} + +static int kcp_hub_append_pending_action(kcp_hub_pending_action_t **head, const char *peer_id, kcp_conn_t *conn) { + kcp_hub_pending_action_t *action; + + if (head == NULL || peer_id == NULL || conn == NULL) { + errno = EINVAL; + return -1; + } + action = (kcp_hub_pending_action_t *) calloc(1, sizeof(*action)); + if (action == NULL) { + return -1; + } + snprintf(action->peer_id, sizeof(action->peer_id), "%s", peer_id); + action->conn = conn; + action->next = *head; + *head = action; + return 0; +} + +static void kcp_hub_free_pending_actions(kcp_hub_pending_action_t *head) { + while (head != NULL) { + kcp_hub_pending_action_t *next = head->next; + free(head); + head = next; + } +} + +static int kcp_hub_peer_is_telemetry(const char *peer_id) { + return kcp_hub_peer_id_has_suffix(peer_id, "-telemetry"); +} + +static int kcp_hub_peer_is_video_receiver(const char *peer_id) { + return peer_id != NULL && strcmp(peer_id, "peer-a-video") == 0; +} + +static int kcp_hub_peer_uses_server_lease(const char *peer_id) { + if (peer_id == NULL || peer_id[0] == '\0') { + return 0; + } + return kcp_hub_peer_id_has_suffix(peer_id, "-ctrl") + || kcp_hub_peer_is_telemetry(peer_id) + || kcp_hub_peer_is_video_receiver(peer_id); +} + +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; + + if (hub == NULL || peer_id == NULL || peer_id[0] == '\0') { + return; + } + + pthread_rwlock_wrlock(&hub->lock); + for (entry = hub->peers; entry != NULL; entry = entry->next) { + if (strcmp(entry->peer_id, peer_id) == 0 && entry->conn == conn) { + if (prev == NULL) { + hub->peers = entry->next; + } else { + prev->next = entry->next; + } + free(entry); + break; + } + prev = entry; + } + pthread_rwlock_unlock(&hub->lock); +} + +static kcp_peer_entry_t *kcp_hub_find_peer(kcp_hub_t *hub, const char *peer_id) { + kcp_peer_entry_t *entry; + for (entry = hub->peers; entry != NULL; entry = entry->next) { + if (strcmp(entry->peer_id, peer_id) == 0) { + return entry; + } + } + return NULL; +} + +static int kcp_hub_peer_id_has_suffix(const char *peer_id, const char *suffix) { + size_t peer_len; + size_t suffix_len; + + if (peer_id == NULL || suffix == NULL) { + return 0; + } + peer_len = strlen(peer_id); + suffix_len = strlen(suffix); + return peer_len >= suffix_len && strcmp(peer_id + peer_len - suffix_len, suffix) == 0; +} + +static int kcp_hub_configure_peer_transport(kcp_conn_t *conn, const char *peer_id) { + kcp_conn_options_t options; + + if (conn == NULL || peer_id == NULL) { + errno = EINVAL; + return -1; + } + if (kcp_hub_peer_id_has_suffix(peer_id, "-ctrl")) { + kcp_conn_options_set_control_defaults(&options); + return kcp_conn_apply_options(conn, &options); + } + if (kcp_hub_peer_id_has_suffix(peer_id, "-video")) { + 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 void kcp_hub_touch_peer_locked(kcp_hub_t *hub, const char *peer_id, kcp_conn_t *conn) { + kcp_peer_entry_t *entry; + + if (hub == NULL || peer_id == NULL || peer_id[0] == '\0') { + return; + } + entry = kcp_hub_find_peer(hub, peer_id); + if (entry != NULL && (conn == NULL || entry->conn == conn)) { + entry->last_seen_ms = kcp_hub_now_ms(); + } +} + +static void kcp_hub_touch_peer(kcp_hub_t *hub, const char *peer_id, kcp_conn_t *conn) { + if (hub == NULL || peer_id == NULL || peer_id[0] == '\0') { + return; + } + pthread_rwlock_wrlock(&hub->lock); + kcp_hub_touch_peer_locked(hub, peer_id, conn); + pthread_rwlock_unlock(&hub->lock); +} + +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, "min_srtt_ms", (double) stats->min_srtt_ms) == NULL || + cJSON_AddNumberToObject(object, "srttvar_ms", (double) stats->srttvar_ms) == NULL || + cJSON_AddNumberToObject(object, "last_feedback_age_ms", (double) stats->last_feedback_age_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; + uint32_t last_telemetry_push_ms = 0; + + while (!atomic_load(&hub->closed)) { + int interval_ms = KCP_HUB_DEFAULT_TELEMETRY_INTERVAL_MS; + uint32_t now_ms = kcp_hub_now_ms(); + int telemetry_enabled = 0; + + pthread_rwlock_rdlock(&hub->lock); + telemetry_enabled = hub->telemetry_peer_id[0] != '\0'; + if (telemetry_enabled && hub->telemetry_interval_ms > 0) { + interval_ms = hub->telemetry_interval_ms; + } + pthread_rwlock_unlock(&hub->lock); + + if (telemetry_enabled && (last_telemetry_push_ms == 0 || kcp_hub_elapsed_ms(now_ms, last_telemetry_push_ms) >= (uint32_t) interval_ms)) { + (void) kcp_hub_push_telemetry_snapshot(hub); + last_telemetry_push_ms = now_ms; + } + kcp_hub_run_maintenance(hub); + if (atomic_load(&hub->closed)) { + break; + } + usleep((useconds_t) KCP_HUB_MAINTENANCE_INTERVAL_MS * 1000U); + } + return NULL; +} + +static int kcp_hub_send_server_text(kcp_conn_t *conn, const char *to, const char *payload) { + message_t msg; + + 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", (to == NULL || to[0] == '\0') ? "unknown" : to); + msg.body = (uint8_t *) omni_strdup(payload == NULL ? "" : payload); + if (msg.body == NULL) { + return -1; + } + msg.body_len = strlen((const char *) msg.body); + if (kcp_conn_send(conn, &msg) != 0) { + protocol_message_clear(&msg); + return -1; + } + protocol_message_clear(&msg); + return 0; +} + +static int kcp_hub_send_server_error(kcp_conn_t *conn, const char *to, const char *message) { + message_t msg; + protocol_message_init(&msg); + msg.type = MSG_TYPE_ERROR; + snprintf(msg.from, sizeof(msg.from), "%s", SERVER_PEER_ID); + snprintf(msg.to, sizeof(msg.to), "%s", (to == NULL || to[0] == '\0') ? "unknown" : to); + msg.body = (uint8_t *) omni_strdup(message == NULL ? "" : message); + if (msg.body == NULL) { + return -1; + } + msg.body_len = strlen((const char *) msg.body); + if (kcp_conn_send(conn, &msg) != 0) { + protocol_message_clear(&msg); + return -1; + } + protocol_message_clear(&msg); + return 0; +} + +static int kcp_hub_sockaddr_equal(const struct sockaddr *left, socklen_t left_len, const struct sockaddr *right, socklen_t right_len) { + char left_text[OMNI_MAX_ADDR_TEXT]; + char right_text[OMNI_MAX_ADDR_TEXT]; + + if (left == NULL || right == NULL) { + return left == right; + } + return strcmp( + omni_sockaddr_to_string(left, left_len, left_text, sizeof(left_text)), + omni_sockaddr_to_string(right, right_len, right_text, sizeof(right_text)) + ) == 0; +} + +static int kcp_hub_accept_relay_peer(kcp_hub_t *hub, const struct sockaddr *addr, socklen_t addr_len) { + int accepted = 0; + + pthread_rwlock_wrlock(&hub->lock); + if (hub->relay_peer_addr_len == 0 && hub->relay_learn_peer) { + omni_clone_sockaddr(addr, addr_len, &hub->relay_peer_addr, &hub->relay_peer_addr_len); + accepted = 1; + } else if (hub->relay_peer_addr_len == 0) { + accepted = 1; + } else { + accepted = kcp_hub_sockaddr_equal((const struct sockaddr *) &hub->relay_peer_addr, hub->relay_peer_addr_len, addr, addr_len); + } + pthread_rwlock_unlock(&hub->lock); + return accepted; +} + +static int kcp_hub_forward_to_relay(kcp_hub_t *hub, const message_t *msg, int *relay_status) { + uint8_t *payload = NULL; + size_t payload_len = 0; + struct sockaddr_storage relay_addr; + socklen_t relay_addr_len = 0; + int relay_fd = -1; + int relay_configured = 0; + + if (relay_status != NULL) { + *relay_status = 0; + } + if (protocol_encode_message_datagram(msg, &payload, &payload_len) != 0) { + return -1; + } + if (payload_len > KCP_RELAY_MAX_DATAGRAM_SIZE) { + free(payload); + errno = EMSGSIZE; + if (relay_status != NULL) { + *relay_status = 3; + } + return -1; + } + + pthread_rwlock_rdlock(&hub->lock); + relay_fd = hub->relay_fd; + relay_configured = hub->relay_configured; + if (hub->relay_peer_addr_len > 0) { + omni_clone_sockaddr((const struct sockaddr *) &hub->relay_peer_addr, hub->relay_peer_addr_len, &relay_addr, &relay_addr_len); + } + pthread_rwlock_unlock(&hub->lock); + + if (!relay_configured || relay_fd < 0) { + free(payload); + errno = ENOTCONN; + if (relay_status != NULL) { + *relay_status = 1; + } + return -1; + } + if (relay_addr_len == 0) { + free(payload); + errno = EDESTADDRREQ; + if (relay_status != NULL) { + *relay_status = 2; + } + return -1; + } + if (sendto(relay_fd, payload, payload_len, 0, (struct sockaddr *) &relay_addr, relay_addr_len) < 0) { + free(payload); + return -1; + } + free(payload); + return 0; +} + +static int kcp_hub_forward_relay_server_error(kcp_hub_t *hub, const char *to, const char *message) { + message_t msg; + int rc; + + protocol_message_init(&msg); + msg.type = MSG_TYPE_ERROR; + snprintf(msg.from, sizeof(msg.from), "%s", SERVER_PEER_ID); + snprintf(msg.to, sizeof(msg.to), "%s", (to == NULL || to[0] == '\0') ? "unknown" : to); + msg.body = (uint8_t *) omni_strdup(message == NULL ? "" : message); + if (msg.body == NULL) { + return -1; + } + msg.body_len = strlen((const char *) msg.body); + rc = kcp_hub_forward_to_relay(hub, &msg, NULL); + protocol_message_clear(&msg); + return rc; +} + +static int kcp_hub_deliver_to_local_peer(kcp_hub_t *hub, const message_t *msg) { + kcp_conn_t *target_conn = NULL; + int rc; + + pthread_rwlock_rdlock(&hub->lock); + { + kcp_peer_entry_t *entry = kcp_hub_find_peer(hub, msg->to); + if (entry != NULL) { + target_conn = entry->conn; + } + } + pthread_rwlock_unlock(&hub->lock); + + if (target_conn == NULL) { + errno = ENOENT; + return -1; + } + rc = kcp_conn_send(target_conn, msg); + if (rc != 0) { + kcp_hub_unregister(hub, msg->to, target_conn); + kcp_conn_close(target_conn); + return -1; + } + return 0; +} + +static int kcp_hub_deliver_relayed_message(kcp_hub_t *hub, const message_t *msg) { + char *error_text; + + if (kcp_hub_deliver_to_local_peer(hub, msg) == 0) { + return 0; + } + if (errno != ENOENT) { + if (msg->type == MSG_TYPE_ERROR) { + return 0; + } + error_text = omni_strdup_printf("failed to forward to %s", msg->to); + if (error_text == NULL) { + return -1; + } + if (kcp_hub_forward_relay_server_error(hub, msg->from, error_text) != 0) { + free(error_text); + return -1; + } + free(error_text); + return 0; + } + + if (msg->type == MSG_TYPE_ERROR) { + return 0; + } + + error_text = omni_strdup_printf("unknown target: %s", msg->to); + if (error_text == NULL) { + return -1; + } + if (kcp_hub_forward_relay_server_error(hub, msg->from, error_text) != 0) { + free(error_text); + return -1; + } + free(error_text); + return 0; +} + +static int kcp_hub_handle_peer_message(kcp_hub_t *hub, const char *peer_id, kcp_conn_t *conn, message_t *msg) { + char *error_text = NULL; + int relay_status = 0; + + kcp_hub_touch_peer(hub, peer_id, conn); + switch (msg->type) { + case MSG_TYPE_TEXT: + if (strcmp(msg->to, SERVER_PEER_ID) == 0) { + if (kcp_hub_text_body_equals(msg, KCP_HUB_CTRL_HEARTBEAT_ACK)) { + return 0; + } + if (kcp_hub_send_server_error(conn, peer_id, "unsupported server control message") != 0) { + return -1; + } + errno = EPROTO; + return -1; + } + snprintf(msg->from, sizeof(msg->from), "%s", peer_id); + if (kcp_hub_deliver_to_local_peer(hub, msg) == 0) { + return 0; + } + if (errno != ENOENT) { + error_text = omni_strdup_printf("failed to forward to %s", msg->to); + if (error_text == NULL) { + return -1; + } + if (kcp_hub_send_server_error(conn, peer_id, error_text) != 0) { + free(error_text); + return -1; + } + free(error_text); + return 0; + } + if (kcp_hub_forward_to_relay(hub, msg, &relay_status) == 0) { + return 0; + } + if (relay_status == 1) { + error_text = omni_strdup_printf("unknown target: %s", msg->to); + } else if (relay_status == 2) { + error_text = omni_strdup("failed to relay to remote peer"); + } else if (relay_status == 3) { + error_text = omni_strdup("message too large for relay udp"); + } else { + error_text = omni_strdup("failed to relay to remote peer"); + } + if (error_text == NULL) { + return -1; + } + if (kcp_hub_send_server_error(conn, peer_id, error_text) != 0) { + free(error_text); + return -1; + } + free(error_text); + return 0; + case MSG_TYPE_FILE: + case MSG_TYPE_BINARY: + snprintf(msg->from, sizeof(msg->from), "%s", peer_id); + if (kcp_hub_deliver_to_local_peer(hub, msg) == 0) { + return 0; + } + if (errno != ENOENT) { + error_text = omni_strdup_printf("failed to forward to %s", msg->to); + if (error_text == NULL) { + return -1; + } + if (kcp_hub_send_server_error(conn, peer_id, error_text) != 0) { + free(error_text); + return -1; + } + free(error_text); + return 0; + } + if (kcp_hub_forward_to_relay(hub, msg, &relay_status) == 0) { + return 0; + } + if (relay_status == 1) { + error_text = omni_strdup_printf("unknown target: %s", msg->to); + } else if (relay_status == 2) { + error_text = omni_strdup("failed to relay to remote peer"); + } else if (relay_status == 3) { + error_text = omni_strdup("message too large for relay udp"); + } else { + error_text = omni_strdup("failed to relay to remote peer"); + } + if (error_text == NULL) { + return -1; + } + if (kcp_hub_send_server_error(conn, peer_id, error_text) != 0) { + free(error_text); + return -1; + } + free(error_text); + return 0; + case MSG_TYPE_REGISTER: + case MSG_TYPE_ERROR: + if (kcp_hub_send_server_error(conn, peer_id, "registered peers can only send text, file, or binary messages") != 0) { + return -1; + } + errno = EPROTO; + return -1; + default: + error_text = omni_strdup_printf("unsupported message type: %s", protocol_message_type_name(msg->type)); + if (error_text == NULL) { + return -1; + } + if (kcp_hub_send_server_error(conn, peer_id, error_text) != 0) { + free(error_text); + return -1; + } + free(error_text); + errno = EPROTO; + return -1; + } +} + +static int kcp_hub_commit_registered_conn( + kcp_hub_t *hub, + const char *peer_id, + kcp_conn_t *conn, + uint32_t now_ms, + kcp_conn_t **out_old_conn +) { + kcp_peer_entry_t *entry; + + if (hub == NULL || peer_id == NULL || peer_id[0] == '\0' || conn == NULL) { + errno = EINVAL; + return -1; + } + if (out_old_conn != NULL) { + *out_old_conn = NULL; + } + + pthread_rwlock_wrlock(&hub->lock); + entry = kcp_hub_find_peer(hub, peer_id); + if (entry != NULL) { + if (out_old_conn != NULL) { + *out_old_conn = entry->conn; + } + entry->conn = conn; + entry->last_seen_ms = now_ms; + entry->last_heartbeat_sent_ms = 0; + pthread_rwlock_unlock(&hub->lock); + return 0; + } + + entry = (kcp_peer_entry_t *) calloc(1, sizeof(*entry)); + if (entry == NULL) { + pthread_rwlock_unlock(&hub->lock); + return -1; + } + snprintf(entry->peer_id, sizeof(entry->peer_id), "%s", peer_id); + entry->conn = conn; + entry->last_seen_ms = now_ms; + entry->last_heartbeat_sent_ms = 0; + entry->next = hub->peers; + hub->peers = entry; + pthread_rwlock_unlock(&hub->lock); + return 0; +} + +static int kcp_hub_register_conn(kcp_hub_t *hub, kcp_conn_t *conn, char *peer_id, size_t peer_id_len) { + message_t msg; + kcp_conn_t *old_conn = NULL; + uint32_t now_ms; + + protocol_message_init(&msg); + if (kcp_conn_receive(conn, &msg) != 0) { + protocol_message_clear(&msg); + return -1; + } + if (msg.type != MSG_TYPE_REGISTER) { + kcp_hub_send_server_error(conn, msg.from, "first message must be register"); + protocol_message_clear(&msg); + errno = EPROTO; + return -1; + } + + snprintf(peer_id, peer_id_len, "%s", msg.from); + if (kcp_hub_send_server_text(conn, msg.from, KCP_HUB_CTRL_REGISTER_OK) != 0) { + protocol_message_clear(&msg); + return -1; + } + + now_ms = kcp_hub_now_ms(); + if (kcp_hub_commit_registered_conn(hub, msg.from, conn, now_ms, &old_conn) != 0) { + protocol_message_clear(&msg); + return -1; + } + + if (old_conn != NULL && old_conn != conn) { + (void) kcp_hub_send_server_text(old_conn, msg.from, KCP_HUB_CTRL_PEER_REPLACED); + kcp_conn_close(old_conn); + } + protocol_message_clear(&msg); + return 0; +} + +static void *kcp_hub_session_thread_main(void *arg) { + kcp_session_thread_ctx_t *ctx = (kcp_session_thread_ctx_t *) arg; + kcp_hub_serve_session(ctx->hub, ctx->conn); + free(ctx); + return NULL; +} + +kcp_hub_t *kcp_hub_new(latency_logger_t *logger, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms) { + kcp_hub_t *hub = (kcp_hub_t *) calloc(1, sizeof(*hub)); + if (hub == NULL) { + return NULL; + } + pthread_rwlock_init(&hub->lock, NULL); + 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->heartbeat_interval_ms = KCP_HUB_DEFAULT_HEARTBEAT_INTERVAL_MS; + hub->lease_timeout_ms = KCP_HUB_DEFAULT_LEASE_TIMEOUT_MS; + hub->relay_fd = -1; + atomic_init(&hub->closed, 0); + if (pthread_create(&hub->telemetry_thread, NULL, kcp_hub_telemetry_thread_main, hub) != 0) { + pthread_rwlock_destroy(&hub->lock); + free(hub); + return NULL; + } + hub->telemetry_thread_started = 1; + return hub; +} + +int kcp_hub_serve_listener(kcp_hub_t *hub, kcp_listener_t *listener) { + if (hub == NULL || listener == NULL) { + errno = EINVAL; + return -1; + } + 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 (atomic_load(&hub->closed)) { + return 0; + } + return -1; + } + ctx = (kcp_session_thread_ctx_t *) calloc(1, sizeof(*ctx)); + if (ctx == NULL) { + kcp_conn_close(conn); + kcp_conn_free(conn); + return -1; + } + ctx->hub = hub; + ctx->conn = conn; + if (pthread_create(&thread, NULL, kcp_hub_session_thread_main, ctx) != 0) { + free(ctx); + kcp_conn_close(conn); + kcp_conn_free(conn); + return -1; + } + pthread_detach(thread); + } + return 0; +} + +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) { + errno = EINVAL; + return -1; + } + peer_id[0] = '\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_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; + } + + for (;;) { + message_t msg; + protocol_message_init(&msg); + if (kcp_conn_receive(conn, &msg) != 0) { + protocol_message_clear(&msg); + rc = -1; + break; + } + if (kcp_hub_handle_peer_message(hub, peer_id, conn, &msg) != 0) { + protocol_message_clear(&msg); + rc = -1; + break; + } + protocol_message_clear(&msg); + } + + kcp_hub_unregister(hub, peer_id, conn); + kcp_conn_close(conn); + kcp_conn_free(conn); + return rc; +} + +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) { + if (hub == NULL || relay_fd < 0) { + errno = EINVAL; + return -1; + } + pthread_rwlock_wrlock(&hub->lock); + hub->relay_fd = relay_fd; + hub->relay_configured = 1; + hub->relay_learn_peer = learn_peer; + hub->relay_peer_addr_len = 0; + if (peer_addr != NULL && peer_addr_len > 0) { + omni_clone_sockaddr(peer_addr, peer_addr_len, &hub->relay_peer_addr, &hub->relay_peer_addr_len); + } + pthread_rwlock_unlock(&hub->lock); + return 0; +} + +int kcp_hub_set_telemetry(kcp_hub_t *hub, const char *peer_id, int interval_ms) { + 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; + pthread_rwlock_unlock(&hub->lock); + return 0; +} + +static void kcp_hub_run_maintenance(kcp_hub_t *hub) { + kcp_hub_pending_action_t *heartbeat_actions = NULL; + kcp_hub_pending_action_t *close_actions = NULL; + uint32_t now_ms; + int heartbeat_interval_ms; + int lease_timeout_ms; + + if (hub == NULL) { + return; + } + + now_ms = kcp_hub_now_ms(); + heartbeat_interval_ms = KCP_HUB_DEFAULT_HEARTBEAT_INTERVAL_MS; + lease_timeout_ms = KCP_HUB_DEFAULT_LEASE_TIMEOUT_MS; + + pthread_rwlock_wrlock(&hub->lock); + if (hub->heartbeat_interval_ms > 0) { + heartbeat_interval_ms = hub->heartbeat_interval_ms; + } + if (hub->lease_timeout_ms > 0) { + lease_timeout_ms = hub->lease_timeout_ms; + } + { + kcp_peer_entry_t *prev = NULL; + kcp_peer_entry_t *entry = hub->peers; + + while (entry != NULL) { + kcp_peer_entry_t *next = entry->next; + uint32_t idle_ms = kcp_hub_elapsed_ms(now_ms, entry->last_seen_ms); + int uses_server_lease = kcp_hub_peer_uses_server_lease(entry->peer_id); + + if (entry->conn == NULL || entry->peer_id[0] == '\0') { + prev = entry; + entry = next; + continue; + } + if (uses_server_lease && lease_timeout_ms > 0 && idle_ms >= (uint32_t) lease_timeout_ms) { + if (prev == NULL) { + hub->peers = next; + } else { + prev->next = next; + } + (void) kcp_hub_append_pending_action(&close_actions, entry->peer_id, entry->conn); + free(entry); + entry = next; + continue; + } + if ( + uses_server_lease + && + heartbeat_interval_ms > 0 + && idle_ms >= (uint32_t) heartbeat_interval_ms + && (entry->last_heartbeat_sent_ms == 0 || kcp_hub_elapsed_ms(now_ms, entry->last_heartbeat_sent_ms) >= (uint32_t) heartbeat_interval_ms) + ) { + entry->last_heartbeat_sent_ms = now_ms; + (void) kcp_hub_append_pending_action(&heartbeat_actions, entry->peer_id, entry->conn); + } + prev = entry; + entry = next; + } + } + pthread_rwlock_unlock(&hub->lock); + + while (heartbeat_actions != NULL) { + kcp_hub_pending_action_t *next = heartbeat_actions->next; + if (kcp_hub_send_server_text(heartbeat_actions->conn, heartbeat_actions->peer_id, KCP_HUB_CTRL_HEARTBEAT) != 0) { + kcp_hub_unregister(hub, heartbeat_actions->peer_id, heartbeat_actions->conn); + kcp_conn_close(heartbeat_actions->conn); + } + free(heartbeat_actions); + heartbeat_actions = next; + } + + while (close_actions != NULL) { + kcp_hub_pending_action_t *next = close_actions->next; + kcp_conn_close(close_actions->conn); + free(close_actions); + close_actions = next; + } + + kcp_hub_free_pending_actions(heartbeat_actions); + kcp_hub_free_pending_actions(close_actions); +} + +int kcp_hub_serve_relay(kcp_hub_t *hub) { + uint8_t buffer[KCP_RELAY_MAX_DATAGRAM_SIZE]; + + if (hub == NULL) { + errno = EINVAL; + return -1; + } + while (!atomic_load(&hub->closed)) { + struct sockaddr_storage source; + socklen_t source_len = sizeof(source); + ssize_t n; + message_t msg; + char err[128]; + int relay_fd; + + pthread_rwlock_rdlock(&hub->lock); + relay_fd = hub->relay_fd; + pthread_rwlock_unlock(&hub->lock); + if (relay_fd < 0) { + errno = ENOTCONN; + return -1; + } + + n = recvfrom(relay_fd, buffer, sizeof(buffer), 0, (struct sockaddr *) &source, &source_len); + if (n < 0) { + if (atomic_load(&hub->closed)) { + return 0; + } + if (errno == EINTR) { + continue; + } + return -1; + } + if (!kcp_hub_accept_relay_peer(hub, (struct sockaddr *) &source, source_len)) { + continue; + } + + protocol_message_init(&msg); + if (protocol_decode_message_datagram(buffer, (size_t) n, &msg, err, sizeof(err)) != 0) { + protocol_message_clear(&msg); + continue; + } + if (msg.type != MSG_TYPE_TEXT && msg.type != MSG_TYPE_FILE && msg.type != MSG_TYPE_BINARY && msg.type != MSG_TYPE_ERROR) { + protocol_message_clear(&msg); + continue; + } + (void) kcp_hub_deliver_relayed_message(hub, &msg); + protocol_message_clear(&msg); + } + return 0; +} + +int kcp_hub_close(kcp_hub_t *hub) { + if (hub == NULL) { + return 0; + } + if (!atomic_exchange(&hub->closed, 1)) { + if (hub->relay_fd >= 0) { + close(hub->relay_fd); + hub->relay_fd = -1; + } + } + return 0; +} + +void kcp_hub_free(kcp_hub_t *hub) { + kcp_peer_entry_t *entry; + kcp_peer_entry_t *next; + + if (hub == NULL) { + 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) { + kcp_conn_close(entry->conn); + } + free(entry); + } + pthread_rwlock_destroy(&hub->lock); + free(hub); +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/src/server_udp_hub.c b/robot/ros2/OmniSocketGo_robot_ros/src/server_udp_hub.c new file mode 100644 index 0000000..faf67ed --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/src/server_udp_hub.c @@ -0,0 +1,181 @@ +#include "server_udp_hub.h" + +#include + +typedef struct udp_peer_entry { + struct udp_peer_entry *next; + char peer_id[OMNI_MAX_PEER_ID]; + struct sockaddr_storage addr; + socklen_t addr_len; +} udp_peer_entry_t; + +struct udp_hub { + udp_conn_t *conn; + pthread_rwlock_t lock; + udp_peer_entry_t *peers; +}; + +static int udp_addr_equal(const struct sockaddr_storage *a, socklen_t a_len, const struct sockaddr_storage *b, socklen_t b_len) { + return a_len == b_len && memcmp(a, b, a_len) == 0; +} + +static udp_peer_entry_t *udp_hub_find_by_id(udp_hub_t *hub, const char *peer_id) { + udp_peer_entry_t *entry; + for (entry = hub->peers; entry != NULL; entry = entry->next) { + if (strcmp(entry->peer_id, peer_id) == 0) { + return entry; + } + } + return NULL; +} + +static udp_peer_entry_t *udp_hub_find_by_addr(udp_hub_t *hub, const struct sockaddr_storage *addr, socklen_t addr_len) { + udp_peer_entry_t *entry; + for (entry = hub->peers; entry != NULL; entry = entry->next) { + if (udp_addr_equal(&entry->addr, entry->addr_len, addr, addr_len)) { + return entry; + } + } + return NULL; +} + +static int udp_hub_send_error(udp_hub_t *hub, const struct sockaddr_storage *addr, socklen_t addr_len, const char *to, const char *message) { + message_t msg; + protocol_message_init(&msg); + msg.type = MSG_TYPE_ERROR; + msg.id = 0; + snprintf(msg.from, sizeof(msg.from), "%s", SERVER_PEER_ID); + snprintf(msg.to, sizeof(msg.to), "%s", to == NULL || to[0] == '\0' ? "unknown" : to); + msg.body = (uint8_t *) omni_strdup(message); + msg.body_len = msg.body == NULL ? 0 : strlen((const char *) msg.body); + if (msg.body == NULL) { + return -1; + } + if (udp_conn_send_to(hub->conn, &msg, (const struct sockaddr *) addr, addr_len) != 0) { + protocol_message_clear(&msg); + return -1; + } + protocol_message_clear(&msg); + return 0; +} + +udp_hub_t *udp_hub_open(const char *listen_addr, latency_logger_t *logger, tx_timestamp_debug_logger_t *debug_logger, int enable_timestamping) { + udp_hub_t *hub = (udp_hub_t *) calloc(1, sizeof(*hub)); + if (hub == NULL) { + return NULL; + } + hub->conn = udp_conn_bind(listen_addr, NULL, enable_timestamping, logger, OMNI_NODE_ROLE_SERVER, "hub", debug_logger); + if (hub->conn == NULL) { + free(hub); + return NULL; + } + pthread_rwlock_init(&hub->lock, NULL); + return hub; +} + +int udp_hub_serve(udp_hub_t *hub) { + message_t msg; + struct sockaddr_storage addr; + socklen_t addr_len; + udp_peer_entry_t *sender; + udp_peer_entry_t *target; + udp_peer_entry_t *entry; + + if (hub == NULL) { + errno = EINVAL; + return -1; + } + + protocol_message_init(&msg); + for (;;) { + protocol_message_clear(&msg); + if (udp_conn_receive(hub->conn, &msg, &addr, &addr_len) != 0) { + return -1; + } + + if (msg.type == MSG_TYPE_REGISTER) { + pthread_rwlock_wrlock(&hub->lock); + entry = udp_hub_find_by_id(hub, msg.from); + if (entry == NULL) { + entry = (udp_peer_entry_t *) calloc(1, sizeof(*entry)); + if (entry == NULL) { + pthread_rwlock_unlock(&hub->lock); + protocol_message_clear(&msg); + return -1; + } + snprintf(entry->peer_id, sizeof(entry->peer_id), "%s", msg.from); + entry->next = hub->peers; + hub->peers = entry; + } + memcpy(&entry->addr, &addr, sizeof(addr)); + entry->addr_len = addr_len; + pthread_rwlock_unlock(&hub->lock); + continue; + } + if (msg.type != MSG_TYPE_TEXT && msg.type != MSG_TYPE_FILE && msg.type != MSG_TYPE_BINARY) { + if (msg.type == MSG_TYPE_ERROR) { + udp_hub_send_error(hub, &addr, addr_len, msg.from, "peers cannot send error messages"); + } else { + char *error_text = omni_strdup_printf("unsupported message type: %s", protocol_message_type_name(msg.type)); + if (error_text != NULL) { + udp_hub_send_error(hub, &addr, addr_len, msg.from, error_text); + free(error_text); + } + } + continue; + } + + pthread_rwlock_rdlock(&hub->lock); + sender = udp_hub_find_by_addr(hub, &addr, addr_len); + if (sender == NULL) { + pthread_rwlock_unlock(&hub->lock); + udp_hub_send_error(hub, &addr, addr_len, msg.from, "not registered; send register first"); + continue; + } + snprintf(msg.from, sizeof(msg.from), "%s", sender->peer_id); + target = udp_hub_find_by_id(hub, msg.to); + if (target == NULL) { + char *error_text; + pthread_rwlock_unlock(&hub->lock); + error_text = omni_strdup_printf("unknown target: %s", msg.to); + if (error_text != NULL) { + udp_hub_send_error(hub, &addr, addr_len, sender->peer_id, error_text); + free(error_text); + } + continue; + } + if (udp_conn_send_to(hub->conn, &msg, (const struct sockaddr *) &target->addr, target->addr_len) != 0) { + char *error_text; + pthread_rwlock_unlock(&hub->lock); + error_text = omni_strdup_printf("failed to forward to %s", msg.to); + if (error_text != NULL) { + udp_hub_send_error(hub, &addr, addr_len, sender->peer_id, error_text); + free(error_text); + } + continue; + } + pthread_rwlock_unlock(&hub->lock); + } +} + +int udp_hub_close(udp_hub_t *hub) { + if (hub == NULL) { + return 0; + } + return udp_conn_close(hub->conn); +} + +void udp_hub_free(udp_hub_t *hub) { + udp_peer_entry_t *entry; + udp_peer_entry_t *next; + if (hub == NULL) { + return; + } + udp_conn_free(hub->conn); + for (entry = hub->peers; entry != NULL; entry = next) { + next = entry->next; + free(entry); + } + pthread_rwlock_destroy(&hub->lock); + free(hub); +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/src/server_udp_relay.c b/robot/ros2/OmniSocketGo_robot_ros/src/server_udp_relay.c new file mode 100644 index 0000000..c562df1 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/src/server_udp_relay.c @@ -0,0 +1,613 @@ +#include "server_udp_relay.h" + +#include +#include +#include +#include +#include + +#define UDP_RELAY_BUF_SIZE (64U * 1024U) +#define UDP_RELAY_ROUTE_TIMEOUT_MS 30000U +#define UDP_RELAY_DEFAULT_PACKET_LOG_SAMPLE_EVERY 200U + +struct udp_relay { + int downstream_fd; + int upstream_fd; + struct sockaddr_storage upstream_addr; + socklen_t upstream_addr_len; + char downstream_local_addr[OMNI_MAX_ADDR_TEXT]; + char upstream_local_addr[OMNI_MAX_ADDR_TEXT]; + struct sockaddr_storage client_addr; + socklen_t client_addr_len; + int has_client; + uint32_t client_last_seen_ms; + struct udp_relay_route *routes; + pthread_mutex_t lock; + pthread_mutex_t log_mu; + unsigned int packet_log_sample_every; + atomic_ullong packet_log_counter; + pthread_mutex_t state_mu; + pthread_cond_t state_cond; + pthread_t downstream_thread; + int downstream_thread_started; + pthread_t upstream_thread; + int upstream_thread_started; + int worker_done; + int worker_rc; + int worker_errno; + int closed; +}; + +typedef struct udp_relay_route { + struct udp_relay_route *next; + uint32_t conv; + struct sockaddr_storage client_addr; + socklen_t client_addr_len; + uint32_t last_seen_ms; +} udp_relay_route_t; + +static uint32_t udp_relay_now_ms(void) { + return omni_now_millis32(); +} + +static uint32_t udp_relay_elapsed_ms(uint32_t now_ms, uint32_t then_ms) { + return now_ms - then_ms; +} + +static unsigned int udp_relay_packet_log_sample_every(void) { + const char *raw = getenv("OMNI_RELAY_PACKET_LOG_SAMPLE_EVERY"); + unsigned long parsed; + char *endptr = NULL; + + if (raw == NULL || raw[0] == '\0') { + return UDP_RELAY_DEFAULT_PACKET_LOG_SAMPLE_EVERY; + } + parsed = strtoul(raw, &endptr, 10); + if (endptr == raw || *endptr != '\0') { + return UDP_RELAY_DEFAULT_PACKET_LOG_SAMPLE_EVERY; + } + return (unsigned int) parsed; +} + +static int udp_relay_event_should_always_log(const char *event_name) { + return event_name != NULL && strstr(event_name, "_drop_") != NULL; +} + +static int udp_relay_should_log_packet(udp_relay_t *relay, const char *event_name) { + unsigned long long seq; + + if (relay == NULL) { + return 0; + } + if (udp_relay_event_should_always_log(event_name)) { + return 1; + } + if (relay->packet_log_sample_every == 0U) { + return 0; + } + if (relay->packet_log_sample_every == 1U) { + return 1; + } + seq = atomic_fetch_add_explicit(&relay->packet_log_counter, 1U, memory_order_relaxed) + 1U; + return (seq % (unsigned long long) relay->packet_log_sample_every) == 0U; +} + +static void udp_relay_parse_kcp_summary(const uint8_t *packet, size_t len, int *has_conv, uint32_t *conv, size_t *segment_count) { + size_t offset = 0; + size_t count = 0; + + if (has_conv != NULL) { + *has_conv = 0; + } + if (conv != NULL) { + *conv = 0; + } + if (segment_count != NULL) { + *segment_count = 0; + } + if (packet == NULL || len < 4U) { + return; + } + if (has_conv != NULL) { + *has_conv = 1; + } + if (conv != NULL) { + *conv = (uint32_t) ((unsigned char) packet[0] | + ((unsigned char) packet[1] << 8) | + ((unsigned char) packet[2] << 16) | + ((unsigned char) packet[3] << 24)); + } + while (offset + 24U <= len) { + uint32_t seg_len = (uint32_t) ((unsigned char) packet[offset + 20] | + ((unsigned char) packet[offset + 21] << 8) | + ((unsigned char) packet[offset + 22] << 16) | + ((unsigned char) packet[offset + 23] << 24)); + if (offset + 24U + seg_len > len) { + return; + } + count++; + offset += 24U + seg_len; + } + if (segment_count != NULL) { + *segment_count = count; + } +} + +static void udp_relay_print_packet(udp_relay_t *relay, const char *event_name, const char *local_addr, const struct sockaddr_storage *remote_addr, socklen_t remote_addr_len, const uint8_t *packet, size_t packet_len) { + char remote_addr_text[OMNI_MAX_ADDR_TEXT]; + int64_t ts_unix_nano; + int has_conv = 0; + uint32_t conv = 0; + size_t segment_count = 0; + + if (relay == NULL) { + return; + } + if (!udp_relay_should_log_packet(relay, event_name)) { + return; + } + + if (remote_addr != NULL && remote_addr_len > 0) { + omni_sockaddr_to_string((const struct sockaddr *) remote_addr, remote_addr_len, remote_addr_text, sizeof(remote_addr_text)); + } else { + remote_addr_text[0] = '\0'; + } + ts_unix_nano = omni_now_unix_nano(); + udp_relay_parse_kcp_summary(packet, packet_len, &has_conv, &conv, &segment_count); + + pthread_mutex_lock(&relay->log_mu); + if (has_conv) { + fprintf(stderr, "[relay] ts=%" PRId64 " event=%s local=%s remote=%s bytes=%zu conv=%" PRIu32 " segs=%zu\n", + ts_unix_nano, + event_name == NULL ? "" : event_name, + local_addr == NULL ? "" : local_addr, + remote_addr_text, + packet_len, + conv, + segment_count); + } else { + fprintf(stderr, "[relay] ts=%" PRId64 " event=%s local=%s remote=%s bytes=%zu\n", + ts_unix_nano, + event_name == NULL ? "" : event_name, + local_addr == NULL ? "" : local_addr, + remote_addr_text, + packet_len); + } + fflush(stderr); + pthread_mutex_unlock(&relay->log_mu); +} + +static int udp_relay_is_closed(udp_relay_t *relay) { + int closed; + + pthread_mutex_lock(&relay->state_mu); + closed = relay->closed; + pthread_mutex_unlock(&relay->state_mu); + return closed; +} + +static void udp_relay_note_result(udp_relay_t *relay, int rc, int errnum) { + pthread_mutex_lock(&relay->state_mu); + if (!relay->worker_done) { + relay->worker_done = 1; + relay->worker_rc = rc; + relay->worker_errno = errnum; + pthread_cond_signal(&relay->state_cond); + } + pthread_mutex_unlock(&relay->state_mu); +} + +static void udp_relay_record_client(udp_relay_t *relay, const struct sockaddr_storage *addr, socklen_t addr_len) { + pthread_mutex_lock(&relay->lock); + memcpy(&relay->client_addr, addr, sizeof(*addr)); + relay->client_addr_len = addr_len; + relay->has_client = 1; + relay->client_last_seen_ms = udp_relay_now_ms(); + pthread_mutex_unlock(&relay->lock); +} + +static void udp_relay_prune_routes_locked(udp_relay_t *relay, uint32_t now_ms) { + udp_relay_route_t *prev = NULL; + udp_relay_route_t *route; + + if (relay == NULL) { + return; + } + + route = relay->routes; + while (route != NULL) { + udp_relay_route_t *next = route->next; + + if (udp_relay_elapsed_ms(now_ms, route->last_seen_ms) >= UDP_RELAY_ROUTE_TIMEOUT_MS) { + if (prev == NULL) { + relay->routes = next; + } else { + prev->next = next; + } + free(route); + route = next; + continue; + } + + prev = route; + route = next; + } + + if (relay->has_client && udp_relay_elapsed_ms(now_ms, relay->client_last_seen_ms) >= UDP_RELAY_ROUTE_TIMEOUT_MS) { + relay->has_client = 0; + relay->client_addr_len = 0; + memset(&relay->client_addr, 0, sizeof(relay->client_addr)); + } +} + +static int udp_relay_record_route(udp_relay_t *relay, uint32_t conv, const struct sockaddr_storage *addr, socklen_t addr_len) { + udp_relay_route_t *route; + uint32_t now_ms; + + if (relay == NULL || addr == NULL || addr_len == 0) { + errno = EINVAL; + return -1; + } + + now_ms = udp_relay_now_ms(); + pthread_mutex_lock(&relay->lock); + udp_relay_prune_routes_locked(relay, now_ms); + for (route = relay->routes; route != NULL; route = route->next) { + if (route->conv == conv) { + memcpy(&route->client_addr, addr, sizeof(*addr)); + route->client_addr_len = addr_len; + route->last_seen_ms = now_ms; + pthread_mutex_unlock(&relay->lock); + return 0; + } + } + + route = (udp_relay_route_t *) calloc(1, sizeof(*route)); + if (route == NULL) { + pthread_mutex_unlock(&relay->lock); + return -1; + } + route->conv = conv; + memcpy(&route->client_addr, addr, sizeof(*addr)); + route->client_addr_len = addr_len; + route->last_seen_ms = now_ms; + route->next = relay->routes; + relay->routes = route; + pthread_mutex_unlock(&relay->lock); + return 0; +} + +static int udp_relay_copy_client(udp_relay_t *relay, struct sockaddr_storage *addr, socklen_t *addr_len) { + int has_client; + uint32_t now_ms; + + now_ms = udp_relay_now_ms(); + pthread_mutex_lock(&relay->lock); + udp_relay_prune_routes_locked(relay, now_ms); + has_client = relay->has_client; + if (has_client) { + memcpy(addr, &relay->client_addr, sizeof(*addr)); + *addr_len = relay->client_addr_len; + } + pthread_mutex_unlock(&relay->lock); + return has_client; +} + +static int udp_relay_copy_route(udp_relay_t *relay, uint32_t conv, struct sockaddr_storage *addr, socklen_t *addr_len) { + udp_relay_route_t *route; + uint32_t now_ms; + + now_ms = udp_relay_now_ms(); + pthread_mutex_lock(&relay->lock); + udp_relay_prune_routes_locked(relay, now_ms); + for (route = relay->routes; route != NULL; route = route->next) { + if (route->conv == conv) { + memcpy(addr, &route->client_addr, sizeof(*addr)); + *addr_len = route->client_addr_len; + pthread_mutex_unlock(&relay->lock); + return 1; + } + } + pthread_mutex_unlock(&relay->lock); + return 0; +} + +static void udp_relay_clear_routes(udp_relay_t *relay) { + udp_relay_route_t *route; + udp_relay_route_t *next; + + if (relay == NULL) { + return; + } + + pthread_mutex_lock(&relay->lock); + route = relay->routes; + relay->routes = NULL; + pthread_mutex_unlock(&relay->lock); + + while (route != NULL) { + next = route->next; + free(route); + route = next; + } +} + +static void *udp_relay_forward_downstream_to_upstream(void *arg) { + udp_relay_t *relay = (udp_relay_t *) arg; + uint8_t buffer[UDP_RELAY_BUF_SIZE]; + + for (;;) { + struct sockaddr_storage source; + socklen_t source_len = sizeof(source); + ssize_t n = recvfrom(relay->downstream_fd, buffer, sizeof(buffer), 0, (struct sockaddr *) &source, &source_len); + int has_conv = 0; + uint32_t conv = 0; + + if (n < 0) { + int errnum = errno; + if (errnum == EINTR) { + continue; + } + if (udp_relay_is_closed(relay)) { + udp_relay_note_result(relay, 0, 0); + } else { + udp_relay_note_result(relay, -1, errnum); + } + return NULL; + } + + udp_relay_record_client(relay, &source, source_len); + udp_relay_parse_kcp_summary(buffer, (size_t) n, &has_conv, &conv, NULL); + if (has_conv) { + (void) udp_relay_record_route(relay, conv, &source, source_len); + } + udp_relay_print_packet(relay, "relay_downstream_rx", relay->downstream_local_addr, &source, source_len, buffer, (size_t) n); + for (;;) { + if (send(relay->upstream_fd, buffer, (size_t) n, 0) >= 0) { + udp_relay_print_packet(relay, "relay_upstream_tx", relay->upstream_local_addr, &relay->upstream_addr, relay->upstream_addr_len, buffer, (size_t) n); + break; + } + { + int errnum = errno; + if (errnum == EINTR) { + continue; + } + if (udp_relay_is_closed(relay)) { + udp_relay_note_result(relay, 0, 0); + } else { + udp_relay_note_result(relay, -1, errnum); + } + return NULL; + } + } + } +} + +static void *udp_relay_forward_upstream_to_downstream(void *arg) { + udp_relay_t *relay = (udp_relay_t *) arg; + uint8_t buffer[UDP_RELAY_BUF_SIZE]; + + for (;;) { + struct sockaddr_storage client_addr; + socklen_t client_addr_len = 0; + ssize_t n = recv(relay->upstream_fd, buffer, sizeof(buffer), 0); + int has_conv = 0; + uint32_t conv = 0; + + if (n < 0) { + int errnum = errno; + if (errnum == EINTR) { + continue; + } + if (udp_relay_is_closed(relay)) { + udp_relay_note_result(relay, 0, 0); + } else { + udp_relay_note_result(relay, -1, errnum); + } + return NULL; + } + + udp_relay_print_packet(relay, "relay_upstream_rx", relay->upstream_local_addr, &relay->upstream_addr, relay->upstream_addr_len, buffer, (size_t) n); + udp_relay_parse_kcp_summary(buffer, (size_t) n, &has_conv, &conv, NULL); + if (has_conv && !udp_relay_copy_route(relay, conv, &client_addr, &client_addr_len)) { + udp_relay_print_packet(relay, "relay_upstream_drop_unknown_conv", relay->upstream_local_addr, &relay->upstream_addr, relay->upstream_addr_len, buffer, (size_t) n); + continue; + } + if (!has_conv && !udp_relay_copy_client(relay, &client_addr, &client_addr_len)) { + udp_relay_print_packet(relay, "relay_upstream_drop_no_client", relay->upstream_local_addr, &relay->upstream_addr, relay->upstream_addr_len, buffer, (size_t) n); + continue; + } + + for (;;) { + if (sendto(relay->downstream_fd, buffer, (size_t) n, 0, (struct sockaddr *) &client_addr, client_addr_len) >= 0) { + udp_relay_print_packet(relay, "relay_downstream_tx", relay->downstream_local_addr, &client_addr, client_addr_len, buffer, (size_t) n); + break; + } + { + int errnum = errno; + if (errnum == EINTR) { + continue; + } + if (udp_relay_is_closed(relay)) { + udp_relay_note_result(relay, 0, 0); + } else { + udp_relay_note_result(relay, -1, errnum); + } + return NULL; + } + } + } +} + +static void udp_relay_join_threads(udp_relay_t *relay) { + if (relay->downstream_thread_started) { + pthread_join(relay->downstream_thread, NULL); + relay->downstream_thread_started = 0; + } + if (relay->upstream_thread_started) { + pthread_join(relay->upstream_thread, NULL); + relay->upstream_thread_started = 0; + } +} + +udp_relay_t *udp_relay_open(const char *listen_addr, const char *upstream_addr) { + struct sockaddr_storage listen_ss; + struct sockaddr_storage upstream_ss; + struct sockaddr_storage downstream_local_ss; + struct sockaddr_storage upstream_local_ss; + socklen_t listen_len; + socklen_t upstream_len; + socklen_t downstream_local_len = sizeof(downstream_local_ss); + socklen_t upstream_local_len = sizeof(upstream_local_ss); + int family; + int fd_listen = -1; + int fd_upstream = -1; + udp_relay_t *relay = NULL; + + if (omni_parse_sockaddr(listen_addr, 1, &listen_ss, &listen_len, &family) != 0 || + omni_parse_sockaddr(upstream_addr, 0, &upstream_ss, &upstream_len, &family) != 0) { + return NULL; + } + fd_listen = socket(listen_ss.ss_family, SOCK_DGRAM, 0); + if (fd_listen < 0) { + return NULL; + } + if (bind(fd_listen, (struct sockaddr *) &listen_ss, listen_len) != 0) { + close(fd_listen); + return NULL; + } + fd_upstream = socket(upstream_ss.ss_family, SOCK_DGRAM, 0); + if (fd_upstream < 0) { + close(fd_listen); + return NULL; + } + if (connect(fd_upstream, (struct sockaddr *) &upstream_ss, upstream_len) != 0) { + close(fd_upstream); + close(fd_listen); + return NULL; + } + relay = (udp_relay_t *) calloc(1, sizeof(*relay)); + if (relay == NULL) { + close(fd_upstream); + close(fd_listen); + return NULL; + } + relay->downstream_fd = fd_listen; + relay->upstream_fd = fd_upstream; + memcpy(&relay->upstream_addr, &upstream_ss, sizeof(upstream_ss)); + relay->upstream_addr_len = upstream_len; + if (getsockname(fd_listen, (struct sockaddr *) &downstream_local_ss, &downstream_local_len) == 0) { + omni_sockaddr_to_string((const struct sockaddr *) &downstream_local_ss, downstream_local_len, relay->downstream_local_addr, sizeof(relay->downstream_local_addr)); + } else { + snprintf(relay->downstream_local_addr, sizeof(relay->downstream_local_addr), "%s", listen_addr == NULL ? "" : listen_addr); + } + if (getsockname(fd_upstream, (struct sockaddr *) &upstream_local_ss, &upstream_local_len) == 0) { + omni_sockaddr_to_string((const struct sockaddr *) &upstream_local_ss, upstream_local_len, relay->upstream_local_addr, sizeof(relay->upstream_local_addr)); + } else { + snprintf(relay->upstream_local_addr, sizeof(relay->upstream_local_addr), "%s", listen_addr == NULL ? "" : listen_addr); + } + pthread_mutex_init(&relay->lock, NULL); + pthread_mutex_init(&relay->log_mu, NULL); + relay->packet_log_sample_every = udp_relay_packet_log_sample_every(); + atomic_init(&relay->packet_log_counter, 0U); + pthread_mutex_init(&relay->state_mu, NULL); + pthread_cond_init(&relay->state_cond, NULL); + return relay; +} + +int udp_relay_serve(udp_relay_t *relay) { + int thread_rc; + int rc; + int errnum; + + if (relay == NULL) { + errno = EINVAL; + return -1; + } + if (udp_relay_is_closed(relay)) { + errno = ECANCELED; + return -1; + } + + pthread_mutex_lock(&relay->state_mu); + relay->worker_done = 0; + relay->worker_rc = 0; + relay->worker_errno = 0; + pthread_mutex_unlock(&relay->state_mu); + + thread_rc = pthread_create(&relay->downstream_thread, NULL, udp_relay_forward_downstream_to_upstream, relay); + if (thread_rc != 0) { + errno = thread_rc; + return -1; + } + relay->downstream_thread_started = 1; + + thread_rc = pthread_create(&relay->upstream_thread, NULL, udp_relay_forward_upstream_to_downstream, relay); + if (thread_rc != 0) { + errno = thread_rc; + udp_relay_close(relay); + udp_relay_join_threads(relay); + return -1; + } + relay->upstream_thread_started = 1; + + pthread_mutex_lock(&relay->state_mu); + while (!relay->worker_done) { + pthread_cond_wait(&relay->state_cond, &relay->state_mu); + } + rc = relay->worker_rc; + errnum = relay->worker_errno; + pthread_mutex_unlock(&relay->state_mu); + + udp_relay_close(relay); + udp_relay_join_threads(relay); + + if (rc != 0 && errnum != 0) { + errno = errnum; + } + return rc; +} + +int udp_relay_close(udp_relay_t *relay) { + int downstream_fd; + int upstream_fd; + + if (relay == NULL) { + return 0; + } + + pthread_mutex_lock(&relay->state_mu); + if (relay->closed) { + pthread_mutex_unlock(&relay->state_mu); + return 0; + } + relay->closed = 1; + downstream_fd = relay->downstream_fd; + upstream_fd = relay->upstream_fd; + relay->downstream_fd = -1; + relay->upstream_fd = -1; + pthread_cond_broadcast(&relay->state_cond); + pthread_mutex_unlock(&relay->state_mu); + + if (downstream_fd >= 0) { + close(downstream_fd); + } + if (upstream_fd >= 0) { + close(upstream_fd); + } + return 0; +} + +void udp_relay_free(udp_relay_t *relay) { + if (relay == NULL) { + return; + } + udp_relay_close(relay); + udp_relay_join_threads(relay); + udp_relay_clear_routes(relay); + pthread_mutex_destroy(&relay->lock); + pthread_mutex_destroy(&relay->log_mu); + pthread_cond_destroy(&relay->state_cond); + pthread_mutex_destroy(&relay->state_mu); + free(relay); +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/src/transport_kcp.c b/robot/ros2/OmniSocketGo_robot_ros/src/transport_kcp.c new file mode 100644 index 0000000..fbb3a51 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/src/transport_kcp.c @@ -0,0 +1,2061 @@ +#include "transport_kcp.h" + +#include "ikcp.h" +#include "linux_timestamping.h" + +#include +#include +#include +#include + +#define KCP_RECV_CHUNK_SIZE (32U * 1024U) + +typedef struct kcp_packet_debug_pending { + struct kcp_packet_debug_pending *next; + uint32_t tx_id; + struct sockaddr_storage remote_addr; + socklen_t remote_addr_len; + int packet_bytes; + int has_conv; + uint32_t conv; + kcp_packet_debug_segment_t *segments; + size_t segment_count; + int saw_sched; + int saw_software; +} kcp_packet_debug_pending_t; + +typedef struct kcp_socket_debug_state { + int fd; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + kcp_packet_debug_logger_t *logger; + pthread_mutex_t write_mu; + pthread_mutex_t pending_mu; + pthread_t errqueue_thread; + int errqueue_thread_started; + uint32_t next_tx_id; + kcp_packet_debug_pending_t *pending_head; + atomic_int closed; + atomic_int last_send_errno; +} kcp_socket_debug_state_t; + +typedef struct kcp_session_entry kcp_session_entry_t; +typedef struct kcp_process_sampler kcp_process_sampler_t; + +struct kcp_conn { + ikcpcb *kcp; + int fd; + int is_client; + int owns_socket; + int socket_closed; + atomic_int closed; + struct sockaddr_storage remote_addr; + socklen_t remote_addr_len; + pthread_mutex_t kcp_mu; + pthread_mutex_t close_mu; + pthread_cond_t rx_cond; + pthread_t recv_thread; + int recv_thread_started; + pthread_t update_thread; + int update_thread_started; + pthread_t stats_thread; + 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; + uint64_t pending_out_pkts; + uint64_t pending_in_segs; + uint64_t pending_out_segs; + uint64_t pending_in_errs; + uint64_t pending_kcp_in_errs; + protocol_frame_decoder_t decoder; + int32_t min_srtt_ms; + uint32_t last_feedback_ms; + uint8_t scratch[KCP_RECV_CHUNK_SIZE]; + latency_logger_t *logger; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + kcp_session_stats_logger_t *stats_logger; + int stats_interval_ms; + kcp_process_sampler_t *process_sampler; + kcp_socket_debug_state_t *sock_state; + struct kcp_listener *listener; + struct kcp_conn *accept_next; + struct kcp_conn *process_next; +}; + +struct kcp_listener { + int fd; + int closed; + pthread_mutex_t lock; + pthread_mutex_t accept_mu; + pthread_cond_t accept_cond; + pthread_t recv_thread; + int recv_thread_started; + kcp_session_entry_t *sessions; + kcp_conn_t *accept_head; + kcp_conn_t *accept_tail; + kcp_socket_debug_state_t sock_state; +}; + +struct kcp_session_entry { + uint32_t conv; + kcp_conn_t *conn; + kcp_session_entry_t *next; +}; + +struct kcp_process_sampler { + kcp_process_sampler_t *next; + kcp_session_stats_logger_t *logger; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + int stats_interval_ms; + pthread_mutex_t lock; + pthread_cond_t cond; + pthread_t thread; + int thread_started; + int stopped; + int refcount; + int request_pending; + uint64_t pending_request_id; + uint64_t completed_request_id; + char pending_reason[32]; + kcp_conn_t *members; + uint64_t prev_bytes_sent; + uint64_t prev_bytes_received; + uint64_t prev_in_pkts; + uint64_t prev_out_pkts; + uint64_t prev_in_segs; + 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; + atomic_uint_fast64_t out_pkts; + atomic_uint_fast64_t in_segs; + atomic_uint_fast64_t out_segs; + atomic_uint_fast64_t in_errs; + atomic_uint_fast64_t kcp_in_errs; + atomic_uint_fast64_t curr_estab; +}; + +static pthread_mutex_t g_kcp_process_sampler_mu = PTHREAD_MUTEX_INITIALIZER; +static kcp_process_sampler_t *g_kcp_process_samplers = NULL; + +void kcp_conn_options_init(kcp_conn_options_t *options) { + if (options == NULL) { + return; + } + memset(options, 0, sizeof(*options)); + options->nodelay = KCP_DEFAULT_NODELAY; + options->interval_ms = KCP_DEFAULT_INTERVAL_MS; + options->resend = KCP_DEFAULT_RESEND; + options->nc = KCP_DEFAULT_NC; + options->sndwnd = KCP_DEFAULT_SND_WND; + options->rcvwnd = KCP_DEFAULT_RCV_WND; + options->mtu = KCP_DEFAULT_MTU; +} + +void kcp_conn_options_set_control_defaults(kcp_conn_options_t *options) { + if (options == NULL) { + return; + } + memset(options, 0, sizeof(*options)); + options->nodelay = KCP_CONTROL_NODELAY; + options->interval_ms = KCP_CONTROL_INTERVAL_MS; + options->resend = KCP_CONTROL_RESEND; + options->nc = KCP_CONTROL_NC; + options->sndwnd = KCP_CONTROL_SND_WND; + options->rcvwnd = KCP_CONTROL_RCV_WND; + options->mtu = KCP_CONTROL_MTU; +} + +void kcp_conn_options_set_video_defaults(kcp_conn_options_t *options) { + if (options == NULL) { + return; + } + memset(options, 0, sizeof(*options)); + options->nodelay = KCP_VIDEO_NODELAY; + options->interval_ms = KCP_VIDEO_INTERVAL_MS; + options->resend = KCP_VIDEO_RESEND; + options->nc = KCP_VIDEO_NC; + options->sndwnd = KCP_VIDEO_SND_WND; + options->rcvwnd = KCP_VIDEO_RCV_WND; + 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; + return -1; + } + if (options->interval_ms <= 0 || options->sndwnd <= 0 || options->rcvwnd <= 0 || options->mtu <= 0) { + errno = EINVAL; + return -1; + } + return 0; +} + +static int kcp_conn_apply_options_locked(kcp_conn_t *conn, const kcp_conn_options_t *options) { + if (conn == NULL || conn->kcp == NULL || kcp_conn_validate_options(options) != 0) { + return -1; + } + if (ikcp_wndsize(conn->kcp, options->sndwnd, options->rcvwnd) != 0) { + errno = EINVAL; + return -1; + } + if (ikcp_setmtu(conn->kcp, options->mtu) != 0) { + errno = EINVAL; + return -1; + } + if (ikcp_nodelay(conn->kcp, options->nodelay, options->interval_ms, options->resend, options->nc) != 0) { + errno = EINVAL; + return -1; + } + conn->kcp->stream = 1; + conn->options = *options; + conn->update_interval_ms = options->interval_ms; + return 0; +} + +static void kcp_parse_packet_segments(const uint8_t *packet, size_t len, uint32_t *conv, kcp_packet_debug_segment_t **segments, size_t *segment_count) { + size_t offset = 0; + size_t count = 0; + kcp_packet_debug_segment_t *items = NULL; + + if (conv != NULL) { + *conv = 0; + } + if (segments != NULL) { + *segments = NULL; + } + if (segment_count != NULL) { + *segment_count = 0; + } + if (len < 4) { + return; + } + if (conv != NULL) { + *conv = (uint32_t) ((unsigned char) packet[0] | + ((unsigned char) packet[1] << 8) | + ((unsigned char) packet[2] << 16) | + ((unsigned char) packet[3] << 24)); + } + while (offset + 24U <= len) { + uint32_t seg_len = (uint32_t) ((unsigned char) packet[offset + 20] | + ((unsigned char) packet[offset + 21] << 8) | + ((unsigned char) packet[offset + 22] << 16) | + ((unsigned char) packet[offset + 23] << 24)); + if (offset + 24U + seg_len > len) { + free(items); + return; + } + if (segments != NULL) { + kcp_packet_debug_segment_t *next = (kcp_packet_debug_segment_t *) realloc(items, (count + 1U) * sizeof(*items)); + if (next == NULL) { + free(items); + return; + } + items = next; + items[count].cmd = packet[offset + 4]; + items[count].frg = packet[offset + 5]; + items[count].wnd = (uint16_t) ((unsigned char) packet[offset + 6] | ((unsigned char) packet[offset + 7] << 8)); + items[count].sn = (uint32_t) ((unsigned char) packet[offset + 12] | + ((unsigned char) packet[offset + 13] << 8) | + ((unsigned char) packet[offset + 14] << 16) | + ((unsigned char) packet[offset + 15] << 24)); + items[count].una = (uint32_t) ((unsigned char) packet[offset + 16] | + ((unsigned char) packet[offset + 17] << 8) | + ((unsigned char) packet[offset + 18] << 16) | + ((unsigned char) packet[offset + 19] << 24)); + items[count].len = seg_len; + } + count++; + offset += 24U + seg_len; + } + if (segments != NULL) { + *segments = items; + } else { + free(items); + } + if (segment_count != NULL) { + *segment_count = count; + } +} + +static uint64_t kcp_counter_diff(uint64_t previous, uint64_t current) { + return current < previous ? 0 : current - previous; +} + +static void kcp_conn_update_min_srtt_locked(kcp_conn_t *conn) { + int32_t srtt_ms; + + if (conn == NULL || conn->kcp == NULL) { + return; + } + srtt_ms = conn->kcp->rx_srtt; + if (srtt_ms > 0 && (conn->min_srtt_ms <= 0 || srtt_ms < conn->min_srtt_ms)) { + conn->min_srtt_ms = srtt_ms; + } +} + +static void kcp_conn_note_feedback_locked(kcp_conn_t *conn) { + if (conn == NULL) { + return; + } + conn->last_feedback_ms = omni_now_millis32(); + kcp_conn_update_min_srtt_locked(conn); +} + +static int kcp_process_sampler_matches(const kcp_process_sampler_t *sampler, kcp_session_stats_logger_t *logger, const char *node_role, const char *node_id, int stats_interval_ms) { + if (sampler == NULL) { + return 0; + } + return sampler->logger == logger && + sampler->stats_interval_ms == stats_interval_ms && + strcmp(sampler->node_role, node_role == NULL ? "" : node_role) == 0 && + strcmp(sampler->node_id, node_id == NULL ? "" : node_id) == 0; +} + +static void kcp_process_sampler_record_send(kcp_process_sampler_t *sampler, int packet_bytes, size_t segments) { + if (sampler == NULL) { + return; + } + atomic_fetch_add_explicit(&sampler->bytes_sent, (uint64_t) packet_bytes, memory_order_relaxed); + atomic_fetch_add_explicit(&sampler->out_pkts, 1, memory_order_relaxed); + atomic_fetch_add_explicit(&sampler->out_segs, (uint64_t) segments, memory_order_relaxed); +} + +static void kcp_process_sampler_record_input(kcp_process_sampler_t *sampler, int packet_bytes, size_t segments) { + if (sampler == NULL) { + return; + } + atomic_fetch_add_explicit(&sampler->bytes_received, (uint64_t) packet_bytes, memory_order_relaxed); + atomic_fetch_add_explicit(&sampler->in_pkts, 1, memory_order_relaxed); + atomic_fetch_add_explicit(&sampler->in_segs, (uint64_t) segments, memory_order_relaxed); +} + +static void kcp_process_sampler_record_error(kcp_process_sampler_t *sampler) { + if (sampler == NULL) { + return; + } + atomic_fetch_add_explicit(&sampler->in_errs, 1, memory_order_relaxed); + atomic_fetch_add_explicit(&sampler->kcp_in_errs, 1, memory_order_relaxed); +} + +static void kcp_conn_record_send(kcp_conn_t *conn, int packet_bytes, size_t segments) { + 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; + } + conn->pending_bytes_sent += (uint64_t) packet_bytes; + conn->pending_out_pkts += 1; + conn->pending_out_segs += (uint64_t) segments; +} + +static void kcp_conn_record_input(kcp_conn_t *conn, int packet_bytes, size_t segments) { + if (conn == NULL) { + return; + } + if (conn->process_sampler != NULL) { + kcp_process_sampler_record_input(conn->process_sampler, packet_bytes, segments); + return; + } + conn->pending_bytes_received += (uint64_t) packet_bytes; + conn->pending_in_pkts += 1; + conn->pending_in_segs += (uint64_t) segments; +} + +static void kcp_conn_record_error(kcp_conn_t *conn) { + if (conn == NULL) { + return; + } + if (conn->process_sampler != NULL) { + kcp_process_sampler_record_error(conn->process_sampler); + return; + } + conn->pending_in_errs += 1; + conn->pending_kcp_in_errs += 1; +} + +static void kcp_process_sampler_curr_estab_inc(kcp_process_sampler_t *sampler) { + if (sampler == NULL) { + return; + } + atomic_fetch_add_explicit(&sampler->curr_estab, 1, memory_order_relaxed); +} + +static void kcp_process_sampler_curr_estab_dec(kcp_process_sampler_t *sampler) { + uint_fast64_t current; + + if (sampler == NULL) { + return; + } + current = atomic_load_explicit(&sampler->curr_estab, memory_order_relaxed); + while (current > 0) { + if (atomic_compare_exchange_weak_explicit(&sampler->curr_estab, ¤t, current - 1U, memory_order_relaxed, memory_order_relaxed)) { + return; + } + } +} + +static void kcp_process_sampler_add_conn(kcp_process_sampler_t *sampler, kcp_conn_t *conn) { + if (sampler == NULL || conn == NULL) { + return; + } + pthread_mutex_lock(&sampler->lock); + conn->process_next = sampler->members; + sampler->members = conn; + pthread_mutex_unlock(&sampler->lock); + kcp_process_sampler_curr_estab_inc(sampler); +} + +static void kcp_process_sampler_remove_conn(kcp_process_sampler_t *sampler, kcp_conn_t *conn) { + kcp_conn_t *prev = NULL; + kcp_conn_t *cur; + + if (sampler == NULL || conn == NULL) { + return; + } + pthread_mutex_lock(&sampler->lock); + for (cur = sampler->members; cur != NULL; cur = cur->process_next) { + if (cur == conn) { + if (prev == NULL) { + sampler->members = cur->process_next; + } else { + prev->process_next = cur->process_next; + } + conn->process_next = NULL; + break; + } + prev = cur; + } + pthread_mutex_unlock(&sampler->lock); + if (cur == conn) { + kcp_process_sampler_curr_estab_dec(sampler); + } +} + +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) { + *snd_queue = 0; + } + if (rcv_queue != NULL) { + *rcv_queue = 0; + } + 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; + } + + pthread_mutex_lock(&sampler->lock); + for (conn = sampler->members; conn != NULL; conn = conn->process_next) { + pthread_mutex_lock(&conn->kcp_mu); + if (conn->kcp != NULL) { + if (snd_queue != NULL) { + *snd_queue += conn->kcp->nsnd_que; + } + if (rcv_queue != NULL) { + *rcv_queue += conn->kcp->nrcv_que; + } + 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); + } + pthread_mutex_unlock(&sampler->lock); +} + +static void kcp_process_sampler_log_snapshot(kcp_process_sampler_t *sampler, const char *reason) { + kcp_session_stats_record_t record; + uint64_t bytes_sent; + uint64_t bytes_received; + uint64_t in_pkts; + uint64_t out_pkts; + uint64_t in_segs; + uint64_t out_segs; + uint64_t in_errs; + uint64_t kcp_in_errs; + 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; + } + + bytes_sent = atomic_load_explicit(&sampler->bytes_sent, memory_order_relaxed); + bytes_received = atomic_load_explicit(&sampler->bytes_received, memory_order_relaxed); + in_pkts = atomic_load_explicit(&sampler->in_pkts, memory_order_relaxed); + out_pkts = atomic_load_explicit(&sampler->out_pkts, memory_order_relaxed); + in_segs = atomic_load_explicit(&sampler->in_segs, memory_order_relaxed); + 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, + &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); + snprintf(record.node_role, sizeof(record.node_role), "%s", sampler->node_role); + snprintf(record.node_id, sizeof(record.node_id), "%s", sampler->node_id); + snprintf(record.sample_reason, sizeof(record.sample_reason), "%s", reason == NULL ? "" : reason); + record.ts_unix_nano = omni_now_unix_nano(); + + record.has_bytes_sent = 1; + record.bytes_sent = kcp_counter_diff(sampler->prev_bytes_sent, bytes_sent); + record.has_bytes_received = 1; + record.bytes_received = kcp_counter_diff(sampler->prev_bytes_received, bytes_received); + record.has_in_pkts = 1; + record.in_pkts = kcp_counter_diff(sampler->prev_in_pkts, in_pkts); + record.has_out_pkts = 1; + record.out_pkts = kcp_counter_diff(sampler->prev_out_pkts, out_pkts); + record.has_in_segs = 1; + 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; + record.kcp_in_errs = kcp_counter_diff(sampler->prev_kcp_in_errs, kcp_in_errs); + record.has_ring_buffer_snd_queue = 1; + record.ring_buffer_snd_queue = snd_queue; + record.has_ring_buffer_rcv_queue = 1; + record.ring_buffer_rcv_queue = rcv_queue; + record.has_ring_buffer_snd_buffer = 1; + record.ring_buffer_snd_buffer = snd_buffer; + record.has_curr_estab = 1; + record.curr_estab = atomic_load_explicit(&sampler->curr_estab, memory_order_relaxed); + + sampler->prev_bytes_sent = bytes_sent; + sampler->prev_bytes_received = bytes_received; + sampler->prev_in_pkts = in_pkts; + 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; + + (void) kcp_session_stats_log(sampler->logger, &record); +} + +static void *kcp_process_sampler_thread_main(void *arg) { + kcp_process_sampler_t *sampler = (kcp_process_sampler_t *) arg; + + for (;;) { + int has_request = 0; + uint64_t request_id = 0; + char reason[32]; + struct timespec deadline; + + clock_gettime(CLOCK_REALTIME, &deadline); + deadline.tv_sec += sampler->stats_interval_ms / 1000; + deadline.tv_nsec += (long) (sampler->stats_interval_ms % 1000) * 1000000L; + if (deadline.tv_nsec >= 1000000000L) { + deadline.tv_sec += 1; + deadline.tv_nsec -= 1000000000L; + } + + pthread_mutex_lock(&sampler->lock); + while (!sampler->stopped && !sampler->request_pending) { + int wait_rc = pthread_cond_timedwait(&sampler->cond, &sampler->lock, &deadline); + if (wait_rc == ETIMEDOUT) { + break; + } + } + if (sampler->stopped) { + pthread_mutex_unlock(&sampler->lock); + return NULL; + } + if (sampler->request_pending) { + has_request = 1; + request_id = sampler->pending_request_id; + snprintf(reason, sizeof(reason), "%s", sampler->pending_reason); + sampler->request_pending = 0; + } else { + snprintf(reason, sizeof(reason), "%s", "periodic"); + } + pthread_mutex_unlock(&sampler->lock); + + kcp_process_sampler_log_snapshot(sampler, reason); + + if (has_request) { + pthread_mutex_lock(&sampler->lock); + if (request_id > sampler->completed_request_id) { + sampler->completed_request_id = request_id; + } + pthread_cond_broadcast(&sampler->cond); + pthread_mutex_unlock(&sampler->lock); + } + } +} + +static kcp_process_sampler_t *kcp_process_sampler_acquire(kcp_session_stats_logger_t *logger, const char *node_role, const char *node_id, int stats_interval_ms) { + kcp_process_sampler_t *sampler; + + if (logger == NULL) { + return NULL; + } + + pthread_mutex_lock(&g_kcp_process_sampler_mu); + for (sampler = g_kcp_process_samplers; sampler != NULL; sampler = sampler->next) { + if (kcp_process_sampler_matches(sampler, logger, node_role, node_id, stats_interval_ms)) { + sampler->refcount++; + pthread_mutex_unlock(&g_kcp_process_sampler_mu); + return sampler; + } + } + + sampler = (kcp_process_sampler_t *) calloc(1, sizeof(*sampler)); + if (sampler == NULL) { + pthread_mutex_unlock(&g_kcp_process_sampler_mu); + return NULL; + } + + sampler->logger = logger; + sampler->stats_interval_ms = stats_interval_ms > 0 ? stats_interval_ms : KCP_DEFAULT_STATS_INTERVAL_MS; + sampler->refcount = 1; + snprintf(sampler->node_role, sizeof(sampler->node_role), "%s", node_role == NULL ? "" : node_role); + snprintf(sampler->node_id, sizeof(sampler->node_id), "%s", node_id == NULL ? "" : node_id); + pthread_mutex_init(&sampler->lock, NULL); + pthread_cond_init(&sampler->cond, NULL); + if (pthread_create(&sampler->thread, NULL, kcp_process_sampler_thread_main, sampler) != 0) { + pthread_cond_destroy(&sampler->cond); + pthread_mutex_destroy(&sampler->lock); + free(sampler); + pthread_mutex_unlock(&g_kcp_process_sampler_mu); + return NULL; + } + sampler->thread_started = 1; + sampler->next = g_kcp_process_samplers; + g_kcp_process_samplers = sampler; + pthread_mutex_unlock(&g_kcp_process_sampler_mu); + return sampler; +} + +static void kcp_process_sampler_release(kcp_process_sampler_t *sampler) { + kcp_process_sampler_t **cursor; + + if (sampler == NULL) { + return; + } + + pthread_mutex_lock(&g_kcp_process_sampler_mu); + sampler->refcount--; + if (sampler->refcount > 0) { + pthread_mutex_unlock(&g_kcp_process_sampler_mu); + return; + } + for (cursor = &g_kcp_process_samplers; *cursor != NULL; cursor = &(*cursor)->next) { + if (*cursor == sampler) { + *cursor = sampler->next; + break; + } + } + pthread_mutex_unlock(&g_kcp_process_sampler_mu); + + pthread_mutex_lock(&sampler->lock); + sampler->stopped = 1; + pthread_cond_broadcast(&sampler->cond); + pthread_mutex_unlock(&sampler->lock); + if (sampler->thread_started) { + pthread_join(sampler->thread, NULL); + } + pthread_cond_destroy(&sampler->cond); + pthread_mutex_destroy(&sampler->lock); + free(sampler); +} + +static void kcp_process_sampler_request_sample_and_wait(kcp_process_sampler_t *sampler, const char *reason) { + uint64_t request_id; + + if (sampler == NULL) { + return; + } + pthread_mutex_lock(&sampler->lock); + if (sampler->stopped) { + pthread_mutex_unlock(&sampler->lock); + return; + } + sampler->request_pending = 1; + request_id = ++sampler->pending_request_id; + snprintf(sampler->pending_reason, sizeof(sampler->pending_reason), "%s", reason == NULL ? "" : reason); + pthread_cond_broadcast(&sampler->cond); + while (!sampler->stopped && sampler->completed_request_id < request_id) { + pthread_cond_wait(&sampler->cond, &sampler->lock); + } + pthread_mutex_unlock(&sampler->lock); +} + +static int kcp_socket_debug_log_record(kcp_socket_debug_state_t *state, const char *event_name, const struct sockaddr_storage *remote_addr, socklen_t remote_addr_len, int packet_bytes, int has_tx_id, uint32_t tx_id, int has_conv, uint32_t conv, const kcp_packet_debug_segment_t *segments, size_t segment_count, int64_t ts_unix_nano) { + char local_addr_text[OMNI_MAX_ADDR_TEXT]; + char remote_addr_text[OMNI_MAX_ADDR_TEXT]; + struct sockaddr_storage local_addr; + socklen_t local_addr_len = sizeof(local_addr); + kcp_packet_debug_record_t record; + + if (state->logger == NULL) { + return 0; + } + memset(&record, 0, sizeof(record)); + getsockname(state->fd, (struct sockaddr *) &local_addr, &local_addr_len); + omni_sockaddr_to_string((struct sockaddr *) &local_addr, local_addr_len, local_addr_text, sizeof(local_addr_text)); + omni_sockaddr_to_string((const struct sockaddr *) remote_addr, remote_addr_len, remote_addr_text, sizeof(remote_addr_text)); + snprintf(record.event, sizeof(record.event), "%s", event_name); + snprintf(record.node_role, sizeof(record.node_role), "%s", state->node_role); + snprintf(record.node_id, sizeof(record.node_id), "%s", state->node_id); + snprintf(record.local_addr, sizeof(record.local_addr), "%s", local_addr_text); + snprintf(record.remote_addr, sizeof(record.remote_addr), "%s", remote_addr_text); + record.packet_bytes = packet_bytes; + record.has_udp_tx_id = has_tx_id; + record.udp_tx_id = tx_id; + record.has_kcp_conv = has_conv; + record.kcp_conv = conv; + record.ts_unix_nano = ts_unix_nano; + if (segment_count > 0) { + record.segments = (kcp_packet_debug_segment_t *) calloc(segment_count, sizeof(*record.segments)); + if (record.segments == NULL) { + return -1; + } + memcpy(record.segments, segments, segment_count * sizeof(*segments)); + record.segment_count = segment_count; + } + kcp_packet_debug_log(state->logger, &record); + kcp_packet_debug_record_clear(&record); + return 0; +} + +static void kcp_socket_debug_pending_free(kcp_packet_debug_pending_t *pending) { + while (pending != NULL) { + kcp_packet_debug_pending_t *next = pending->next; + free(pending->segments); + free(pending); + pending = next; + } +} + +static int kcp_socket_debug_reserve_tx(kcp_socket_debug_state_t *state, const struct sockaddr_storage *remote_addr, socklen_t remote_addr_len, const uint8_t *packet, size_t packet_len, uint32_t *out_tx_id) { + kcp_packet_debug_pending_t *pending; + if (state->logger == NULL) { + *out_tx_id = 0; + return 0; + } + pending = (kcp_packet_debug_pending_t *) calloc(1, sizeof(*pending)); + if (pending == NULL) { + return -1; + } + pending->tx_id = state->next_tx_id++; + pending->packet_bytes = (int) packet_len; + memcpy(&pending->remote_addr, remote_addr, sizeof(*remote_addr)); + pending->remote_addr_len = remote_addr_len; + kcp_parse_packet_segments(packet, packet_len, &pending->conv, &pending->segments, &pending->segment_count); + pending->has_conv = packet_len >= 4; + pthread_mutex_lock(&state->pending_mu); + pending->next = state->pending_head; + state->pending_head = pending; + pthread_mutex_unlock(&state->pending_mu); + *out_tx_id = pending->tx_id; + return 0; +} + +static void kcp_socket_debug_rollback_tx(kcp_socket_debug_state_t *state, uint32_t tx_id) { + kcp_packet_debug_pending_t *prev = NULL; + kcp_packet_debug_pending_t *cur; + pthread_mutex_lock(&state->pending_mu); + for (cur = state->pending_head; cur != NULL; cur = cur->next) { + if (cur->tx_id == tx_id) { + if (prev == NULL) { + state->pending_head = cur->next; + } else { + prev->next = cur->next; + } + free(cur->segments); + free(cur); + break; + } + prev = cur; + } + pthread_mutex_unlock(&state->pending_mu); +} + +static void *kcp_socket_debug_errqueue_thread(void *arg) { + kcp_socket_debug_state_t *state = (kcp_socket_debug_state_t *) arg; + uint8_t control[512]; + uint8_t dummy = 0; + struct iovec iov; + struct msghdr msg; + + while (!atomic_load(&state->closed)) { + ssize_t rc; + omni_tx_timestamp_event_t event; + kcp_packet_debug_pending_t *prev = NULL; + kcp_packet_debug_pending_t *cur = NULL; + + memset(&msg, 0, sizeof(msg)); + iov.iov_base = &dummy; + iov.iov_len = sizeof(dummy); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = control; + msg.msg_controllen = sizeof(control); + rc = recvmsg(state->fd, &msg, MSG_ERRQUEUE | MSG_DONTWAIT); + if (rc < 0) { + if (errno == EAGAIN || errno == EWOULDBLOCK || errno == EINTR) { + usleep(10000); + continue; + } + if (atomic_load(&state->closed)) { + return NULL; + } + usleep(10000); + continue; + } + if (linux_timestamping_parse_tx_timestamp(&msg, &event) != 0) { + continue; + } + pthread_mutex_lock(&state->pending_mu); + for (cur = state->pending_head; cur != NULL; cur = cur->next) { + if (cur->tx_id == event.ee_data) { + break; + } + prev = cur; + } + if (cur != NULL) { + if (strcmp(event.event_name, EVENT_A_TX_SCHED) == 0) { + cur->saw_sched = 1; + } else if (strcmp(event.event_name, EVENT_A_TX_SOFTWARE) == 0) { + cur->saw_software = 1; + } + kcp_socket_debug_log_record(state, event.event_name, &cur->remote_addr, cur->remote_addr_len, cur->packet_bytes, 1, cur->tx_id, cur->has_conv, cur->conv, cur->segments, cur->segment_count, event.ts_unix_nano); + if (cur->saw_sched && cur->saw_software) { + if (prev == NULL) { + state->pending_head = cur->next; + } else { + prev->next = cur->next; + } + free(cur->segments); + free(cur); + } + } + pthread_mutex_unlock(&state->pending_mu); + } + return NULL; +} + +static int kcp_socket_debug_init(kcp_socket_debug_state_t *state, int fd, kcp_packet_debug_logger_t *logger, const char *node_role, const char *node_id) { + int thread_rc; + memset(state, 0, sizeof(*state)); + state->fd = fd; + state->logger = logger; + snprintf(state->node_role, sizeof(state->node_role), "%s", node_role == NULL ? "" : node_role); + snprintf(state->node_id, sizeof(state->node_id), "%s", node_id == NULL ? "" : node_id); + pthread_mutex_init(&state->write_mu, NULL); + pthread_mutex_init(&state->pending_mu, NULL); + if (logger != NULL) { + if (linux_timestamping_enable_udp_socket(fd, 1) != 0) { + pthread_mutex_destroy(&state->write_mu); + pthread_mutex_destroy(&state->pending_mu); + return -1; + } + thread_rc = pthread_create(&state->errqueue_thread, NULL, kcp_socket_debug_errqueue_thread, state); + if (thread_rc != 0) { + errno = thread_rc; + pthread_mutex_destroy(&state->write_mu); + pthread_mutex_destroy(&state->pending_mu); + return -1; + } + state->errqueue_thread_started = 1; + } + return 0; +} + +static void kcp_socket_debug_destroy(kcp_socket_debug_state_t *state) { + atomic_store(&state->closed, 1); + if (state->errqueue_thread_started) { + pthread_join(state->errqueue_thread, NULL); + } + kcp_socket_debug_pending_free(state->pending_head); + pthread_mutex_destroy(&state->write_mu); + pthread_mutex_destroy(&state->pending_mu); +} + +static int kcp_socket_send_packet(kcp_socket_debug_state_t *state, const struct sockaddr_storage *remote_addr, socklen_t remote_addr_len, const uint8_t *packet, size_t packet_len) { + uint32_t tx_id = 0; + ssize_t rc; + if (state->logger != NULL && kcp_socket_debug_reserve_tx(state, remote_addr, remote_addr_len, packet, packet_len, &tx_id) != 0) { + atomic_store(&state->last_send_errno, errno != 0 ? errno : EIO); + return -1; + } + pthread_mutex_lock(&state->write_mu); + rc = sendto(state->fd, packet, packet_len, 0, (const struct sockaddr *) remote_addr, remote_addr_len); + pthread_mutex_unlock(&state->write_mu); + if (rc < 0 || (size_t) rc != packet_len) { + if (rc >= 0 && (size_t) rc != packet_len && errno == 0) { + errno = EIO; + } + atomic_store(&state->last_send_errno, errno != 0 ? errno : EIO); + if (state->logger != NULL) { + kcp_socket_debug_rollback_tx(state, tx_id); + } + return -1; + } + atomic_store(&state->last_send_errno, 0); + return 0; +} + +static int kcp_output_callback_impl(const char *buf, int len, struct IKCPCB *kcp, void *user) { + kcp_conn_t *conn = (kcp_conn_t *) user; + size_t segment_count = 0; + (void) kcp; + if (conn == NULL || atomic_load(&conn->closed)) { + return -1; + } + kcp_parse_packet_segments((const uint8_t *) buf, (size_t) len, NULL, NULL, &segment_count); + if (kcp_socket_send_packet(conn->sock_state, &conn->remote_addr, conn->remote_addr_len, (const uint8_t *) buf, (size_t) len) != 0) { + return -1; + } + kcp_conn_record_send(conn, len, segment_count); + return len; +} + +static int kcp_conn_attach_process_sampler(kcp_conn_t *conn) { + kcp_process_sampler_t *next_sampler; + kcp_process_sampler_t *previous_sampler; + uint64_t pending_bytes_sent = 0; + uint64_t pending_bytes_received = 0; + uint64_t pending_in_pkts = 0; + uint64_t pending_out_pkts = 0; + uint64_t pending_in_segs = 0; + uint64_t pending_out_segs = 0; + uint64_t pending_in_errs = 0; + uint64_t pending_kcp_in_errs = 0; + + if (conn == NULL) { + errno = EINVAL; + return -1; + } + + next_sampler = kcp_process_sampler_acquire(conn->stats_logger, conn->node_role, conn->node_id, conn->stats_interval_ms); + if (conn->stats_logger != NULL && next_sampler == NULL) { + return -1; + } + + previous_sampler = conn->process_sampler; + if (previous_sampler == next_sampler) { + return 0; + } + + if (next_sampler != NULL) { + kcp_process_sampler_add_conn(next_sampler, conn); + } + pthread_mutex_lock(&conn->kcp_mu); + previous_sampler = conn->process_sampler; + conn->process_sampler = next_sampler; + pending_bytes_sent = conn->pending_bytes_sent; + pending_bytes_received = conn->pending_bytes_received; + pending_in_pkts = conn->pending_in_pkts; + pending_out_pkts = conn->pending_out_pkts; + pending_in_segs = conn->pending_in_segs; + pending_out_segs = conn->pending_out_segs; + pending_in_errs = conn->pending_in_errs; + pending_kcp_in_errs = conn->pending_kcp_in_errs; + conn->pending_bytes_sent = 0; + conn->pending_bytes_received = 0; + conn->pending_in_pkts = 0; + conn->pending_out_pkts = 0; + conn->pending_in_segs = 0; + conn->pending_out_segs = 0; + conn->pending_in_errs = 0; + conn->pending_kcp_in_errs = 0; + pthread_mutex_unlock(&conn->kcp_mu); + if (next_sampler != NULL) { + atomic_fetch_add_explicit(&next_sampler->bytes_sent, pending_bytes_sent, memory_order_relaxed); + atomic_fetch_add_explicit(&next_sampler->bytes_received, pending_bytes_received, memory_order_relaxed); + atomic_fetch_add_explicit(&next_sampler->in_pkts, pending_in_pkts, memory_order_relaxed); + atomic_fetch_add_explicit(&next_sampler->out_pkts, pending_out_pkts, memory_order_relaxed); + atomic_fetch_add_explicit(&next_sampler->in_segs, pending_in_segs, memory_order_relaxed); + atomic_fetch_add_explicit(&next_sampler->out_segs, pending_out_segs, memory_order_relaxed); + atomic_fetch_add_explicit(&next_sampler->in_errs, pending_in_errs, memory_order_relaxed); + atomic_fetch_add_explicit(&next_sampler->kcp_in_errs, pending_kcp_in_errs, memory_order_relaxed); + } + if (previous_sampler != NULL) { + kcp_process_sampler_remove_conn(previous_sampler, conn); + kcp_process_sampler_release(previous_sampler); + } + return 0; +} + +static void kcp_conn_detach_process_sampler(kcp_conn_t *conn) { + kcp_process_sampler_t *sampler; + + if (conn == NULL || conn->process_sampler == NULL) { + return; + } + + sampler = conn->process_sampler; + conn->process_sampler = NULL; + kcp_process_sampler_remove_conn(sampler, conn); + kcp_process_sampler_release(sampler); +} + +static void kcp_log_session_snapshot(kcp_conn_t *conn, const char *reason) { + kcp_session_stats_record_t record; + struct sockaddr_storage local_addr; + 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; + } + memset(&record, 0, sizeof(record)); + snprintf(record.record_type, sizeof(record.record_type), "%s", KCP_SESSION_STATS_RECORD_SESSION_SAMPLE); + snprintf(record.node_role, sizeof(record.node_role), "%s", conn->node_role); + snprintf(record.node_id, sizeof(record.node_id), "%s", conn->node_id); + getsockname(conn->sock_state->fd, (struct sockaddr *) &local_addr, &local_len); + omni_sockaddr_to_string((struct sockaddr *) &local_addr, local_len, local_text, sizeof(local_text)); + omni_sockaddr_to_string((struct sockaddr *) &conn->remote_addr, conn->remote_addr_len, remote_text, sizeof(remote_text)); + snprintf(record.local_addr, sizeof(record.local_addr), "%s", local_text); + snprintf(record.remote_addr, sizeof(record.remote_addr), "%s", remote_text); + record.has_conv = 1; + record.conv = conn->kcp->conv; + record.ts_unix_nano = omni_now_unix_nano(); + snprintf(record.sample_reason, sizeof(record.sample_reason), "%s", reason); + pthread_mutex_lock(&conn->kcp_mu); + record.has_rto_ms = 1; + record.rto_ms = conn->kcp->rx_rto; + record.has_srtt_ms = 1; + record.srtt_ms = conn->kcp->rx_srtt; + kcp_conn_update_min_srtt_locked(conn); + record.has_min_srtt_ms = conn->min_srtt_ms > 0; + record.min_srtt_ms = conn->min_srtt_ms; + record.has_srttvar_ms = 1; + record.srttvar_ms = conn->kcp->rx_rttval; + record.has_last_feedback_age_ms = conn->last_feedback_ms != 0; + record.last_feedback_age_ms = conn->last_feedback_ms == 0 ? 0 : (omni_now_millis32() - conn->last_feedback_ms); + 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); +} + +static void *kcp_stats_thread_main(void *arg) { + kcp_conn_t *conn = (kcp_conn_t *) arg; + while (!atomic_load(&conn->closed)) { + usleep((useconds_t) conn->stats_interval_ms * 1000U); + if (!atomic_load(&conn->closed)) { + kcp_log_session_snapshot(conn, "periodic"); + } + } + return NULL; +} + +static int kcp_socket_open_bound(const char *listen_addr, const char *bind_device, struct sockaddr_storage *local_addr, socklen_t *local_len) { + int family; + int fd; + if (omni_parse_sockaddr(listen_addr, 1, local_addr, local_len, &family) != 0) { + return -1; + } + fd = socket(family, SOCK_DGRAM, 0); + if (fd < 0) { + return -1; + } + if (bind_device != NULL && bind_device[0] != '\0' && omni_bind_device(fd, bind_device) != 0) { + close(fd); + return -1; + } + if (bind(fd, (struct sockaddr *) local_addr, *local_len) != 0) { + close(fd); + return -1; + } + return fd; +} + +static int kcp_socket_open_dial(const char *server_addr, const char *bind_ip, const char *bind_device, struct sockaddr_storage *remote_addr, socklen_t *remote_len, int *family_out) { + int family; + struct sockaddr_storage local_addr; + socklen_t local_len; + int fd; + if (omni_parse_sockaddr(server_addr, 0, remote_addr, remote_len, &family) != 0) { + return -1; + } + fd = socket(family, SOCK_DGRAM, 0); + if (fd < 0) { + return -1; + } + if (bind_device != NULL && bind_device[0] != '\0' && omni_bind_device(fd, bind_device) != 0) { + close(fd); + return -1; + } + if (bind_ip != NULL && bind_ip[0] != '\0') { + struct addrinfo hints; + struct addrinfo *result = NULL; + memset(&hints, 0, sizeof(hints)); + hints.ai_family = family; + hints.ai_socktype = SOCK_DGRAM; + if (getaddrinfo(bind_ip, "0", &hints, &result) != 0 || result == NULL) { + close(fd); + errno = EINVAL; + return -1; + } + memcpy(&local_addr, result->ai_addr, result->ai_addrlen); + local_len = (socklen_t) result->ai_addrlen; + freeaddrinfo(result); + if (bind(fd, (struct sockaddr *) &local_addr, local_len) != 0) { + close(fd); + return -1; + } + } + if (family_out != NULL) { + *family_out = family; + } + return fd; +} + +static int kcp_sockaddr_equal(const struct sockaddr_storage *left, socklen_t left_len, const struct sockaddr_storage *right, socklen_t right_len) { + char left_text[OMNI_MAX_ADDR_TEXT]; + char right_text[OMNI_MAX_ADDR_TEXT]; + + if (left == NULL || right == NULL) { + return left == right; + } + return strcmp( + omni_sockaddr_to_string((const struct sockaddr *) left, left_len, left_text, sizeof(left_text)), + omni_sockaddr_to_string((const struct sockaddr *) right, right_len, right_text, sizeof(right_text)) + ) == 0; +} + +static void *kcp_client_recv_thread_main(void *arg) { + kcp_conn_t *conn = (kcp_conn_t *) arg; + uint8_t buffer[64 * 1024]; + uint8_t control[512]; + struct sockaddr_storage source; + struct iovec iov; + struct msghdr msg; + ssize_t n; + uint32_t conv = 0; + kcp_packet_debug_segment_t *segments = NULL; + size_t segment_count = 0; + int64_t rx_ts; + + while (!atomic_load(&conn->closed)) { + memset(&msg, 0, sizeof(msg)); + memset(&source, 0, sizeof(source)); + iov.iov_base = buffer; + iov.iov_len = sizeof(buffer); + msg.msg_name = &source; + msg.msg_namelen = sizeof(source); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + if (conn->sock_state->logger != NULL) { + msg.msg_control = control; + msg.msg_controllen = sizeof(control); + } + n = recvmsg(conn->fd, &msg, 0); + if (n < 0) { + if (errno == EINTR) { + continue; + } + if (atomic_load(&conn->closed)) { + return NULL; + } + return NULL; + } + kcp_parse_packet_segments(buffer, (size_t) n, &conv, &segments, &segment_count); + rx_ts = conn->sock_state->logger != NULL ? linux_timestamping_parse_rx_timestamp(&msg) : 0; + if (rx_ts > 0) { + kcp_socket_debug_log_record(conn->sock_state, EVENT_B_RX_SOFTWARE, &source, msg.msg_namelen, (int) n, 0, 0, 1, conv, segments, segment_count, rx_ts); + } + if (!kcp_sockaddr_equal(&source, msg.msg_namelen, &conn->remote_addr, conn->remote_addr_len)) { + free(segments); + segments = NULL; + segment_count = 0; + continue; + } + pthread_mutex_lock(&conn->kcp_mu); + conn->kcp->current = omni_now_millis32(); + if (ikcp_input(conn->kcp, (const char *) buffer, n) != 0) { + kcp_conn_record_error(conn); + } else { + kcp_conn_note_feedback_locked(conn); + kcp_conn_record_input(conn, (int) n, segment_count); + } + pthread_mutex_unlock(&conn->kcp_mu); + pthread_cond_broadcast(&conn->rx_cond); + free(segments); + segments = NULL; + segment_count = 0; + } + return NULL; +} + +static void *kcp_update_thread_main(void *arg) { + kcp_conn_t *conn = (kcp_conn_t *) arg; + while (!atomic_load(&conn->closed)) { + int interval_ms; + pthread_mutex_lock(&conn->kcp_mu); + ikcp_update(conn->kcp, omni_now_millis32()); + interval_ms = conn->update_interval_ms > 0 ? conn->update_interval_ms : KCP_DEFAULT_INTERVAL_MS; + pthread_mutex_unlock(&conn->kcp_mu); + usleep((useconds_t) interval_ms * 1000U); + } + return NULL; +} + +static int kcp_conn_start_stats_thread(kcp_conn_t *conn) { + int thread_rc; + if (conn == NULL || conn->stats_logger == NULL || conn->stats_thread_started) { + return 0; + } + thread_rc = pthread_create(&conn->stats_thread, NULL, kcp_stats_thread_main, conn); + if (thread_rc != 0) { + errno = thread_rc; + return -1; + } + conn->stats_thread_started = 1; + return 0; +} + +static kcp_conn_t *kcp_conn_alloc_common(int fd, const struct sockaddr_storage *remote_addr, socklen_t remote_addr_len, const kcp_conn_options_t *options, kcp_socket_debug_state_t *sock_state, 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 *conn = (kcp_conn_t *) calloc(1, sizeof(*conn)); + uint32_t conv; + int thread_rc; + kcp_conn_options_t effective_options; + + if (conn == NULL) { + errno = ENOMEM; + return NULL; + } + conn->fd = fd; + memcpy(&conn->remote_addr, remote_addr, sizeof(*remote_addr)); + conn->remote_addr_len = remote_addr_len; + pthread_mutex_init(&conn->kcp_mu, NULL); + pthread_mutex_init(&conn->close_mu, NULL); + pthread_cond_init(&conn->rx_cond, NULL); + protocol_frame_decoder_init(&conn->decoder); + conn->logger = logger; + snprintf(conn->node_role, sizeof(conn->node_role), "%s", node_role == NULL ? "" : node_role); + snprintf(conn->node_id, sizeof(conn->node_id), "%s", node_id == NULL ? "" : node_id); + conn->stats_logger = stats_logger; + conn->stats_interval_ms = stats_interval_ms > 0 ? stats_interval_ms : KCP_DEFAULT_STATS_INTERVAL_MS; + kcp_conn_options_init(&effective_options); + if (options != NULL) { + effective_options = *options; + } + conn->options = effective_options; + conn->update_interval_ms = effective_options.interval_ms; + conn->sock_state = sock_state; + if (omni_random_u32(&conv) != 0) { + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + return NULL; + } + conn->kcp = ikcp_create(conv, conn); + if (conn->kcp == NULL) { + errno = ENOMEM; + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + return NULL; + } + ikcp_setoutput(conn->kcp, kcp_output_callback_impl); + if (kcp_conn_apply_options_locked(conn, &effective_options) != 0) { + ikcp_release(conn->kcp); + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + return NULL; + } + if (kcp_conn_attach_process_sampler(conn) != 0) { + ikcp_release(conn->kcp); + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + return NULL; + } + if (kcp_conn_start_stats_thread(conn) != 0) { + kcp_conn_detach_process_sampler(conn); + ikcp_release(conn->kcp); + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + return NULL; + } + thread_rc = pthread_create(&conn->update_thread, NULL, kcp_update_thread_main, conn); + if (thread_rc != 0) { + errno = thread_rc; + if (conn->stats_thread_started) { + atomic_store(&conn->closed, 1); + pthread_join(conn->stats_thread, NULL); + } + kcp_conn_detach_process_sampler(conn); + ikcp_release(conn->kcp); + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + return NULL; + } + conn->update_thread_started = 1; + return conn; +} + +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) { + struct sockaddr_storage remote_addr; + socklen_t remote_len; + int family; + int fd = kcp_socket_open_dial(server_addr, bind_ip, bind_device, &remote_addr, &remote_len, &family); + kcp_conn_t *conn; + kcp_socket_debug_state_t *sock_state; + int thread_rc; + (void) family; + if (fd < 0) { + return NULL; + } + sock_state = (kcp_socket_debug_state_t *) calloc(1, sizeof(*sock_state)); + if (sock_state == NULL) { + errno = ENOMEM; + close(fd); + return NULL; + } + if (kcp_socket_debug_init(sock_state, fd, packet_logger, node_role, node_id) != 0) { + free(sock_state); + close(fd); + return NULL; + } + conn = kcp_conn_alloc_common(fd, &remote_addr, remote_len, options, sock_state, logger, node_role, node_id, stats_logger, stats_interval_ms); + if (conn == NULL) { + kcp_socket_debug_destroy(sock_state); + free(sock_state); + close(fd); + return NULL; + } + conn->is_client = 1; + conn->owns_socket = 1; + thread_rc = pthread_create(&conn->recv_thread, NULL, kcp_client_recv_thread_main, conn); + if (thread_rc != 0) { + errno = thread_rc; + kcp_conn_free(conn); + return NULL; + } + conn->recv_thread_started = 1; + return conn; +} + +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) { + return kcp_conn_dial_with_options(server_addr, bind_ip, bind_device, NULL, packet_logger, logger, node_role, node_id, stats_logger, stats_interval_ms); +} + +static void kcp_listener_enqueue_accept(kcp_listener_t *listener, kcp_conn_t *conn) { + pthread_mutex_lock(&listener->accept_mu); + if (listener->accept_tail == NULL) { + listener->accept_head = conn; + } else { + listener->accept_tail->accept_next = conn; + } + listener->accept_tail = conn; + conn->accept_next = NULL; + pthread_cond_signal(&listener->accept_cond); + pthread_mutex_unlock(&listener->accept_mu); +} + +static kcp_conn_t *kcp_listener_find_session(kcp_listener_t *listener, uint32_t conv) { + kcp_session_entry_t *entry; + for (entry = listener->sessions; entry != NULL; entry = entry->next) { + if (entry->conv == conv) { + return entry->conn; + } + } + return NULL; +} + +static int kcp_listener_add_session(kcp_listener_t *listener, uint32_t conv, kcp_conn_t *conn) { + kcp_session_entry_t *entry = (kcp_session_entry_t *) calloc(1, sizeof(*entry)); + if (entry == NULL) { + return -1; + } + entry->conv = conv; + entry->conn = conn; + entry->next = listener->sessions; + listener->sessions = entry; + return 0; +} + +static void kcp_listener_remove_session(kcp_listener_t *listener, kcp_conn_t *conn) { + kcp_session_entry_t *prev_entry = NULL; + kcp_session_entry_t *entry; + kcp_conn_t *prev_accept = NULL; + kcp_conn_t *accept; + + if (listener == NULL || conn == NULL) { + return; + } + + pthread_mutex_lock(&listener->lock); + for (entry = listener->sessions; entry != NULL; entry = entry->next) { + if (entry->conn == conn) { + if (prev_entry == NULL) { + listener->sessions = entry->next; + } else { + prev_entry->next = entry->next; + } + free(entry); + break; + } + prev_entry = entry; + } + pthread_mutex_unlock(&listener->lock); + + pthread_mutex_lock(&listener->accept_mu); + for (accept = listener->accept_head; accept != NULL; accept = accept->accept_next) { + if (accept == conn) { + if (prev_accept == NULL) { + listener->accept_head = accept->accept_next; + } else { + prev_accept->accept_next = accept->accept_next; + } + if (listener->accept_tail == conn) { + listener->accept_tail = prev_accept; + } + conn->accept_next = NULL; + break; + } + prev_accept = accept; + } + pthread_mutex_unlock(&listener->accept_mu); +} + +static void *kcp_listener_recv_thread_main(void *arg) { + kcp_listener_t *listener = (kcp_listener_t *) arg; + uint8_t buffer[64 * 1024]; + uint8_t control[512]; + struct sockaddr_storage source; + struct iovec iov; + struct msghdr msg; + ssize_t n; + uint32_t conv; + kcp_packet_debug_segment_t *segments = NULL; + size_t segment_count = 0; + int64_t rx_ts; + + while (!listener->closed) { + memset(&msg, 0, sizeof(msg)); + memset(&source, 0, sizeof(source)); + iov.iov_base = buffer; + iov.iov_len = sizeof(buffer); + msg.msg_name = &source; + msg.msg_namelen = sizeof(source); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + if (listener->sock_state.logger != NULL) { + msg.msg_control = control; + msg.msg_controllen = sizeof(control); + } + n = recvmsg(listener->fd, &msg, 0); + if (n < 0) { + if (errno == EINTR) { + continue; + } + if (listener->closed) { + return NULL; + } + return NULL; + } + kcp_parse_packet_segments(buffer, (size_t) n, &conv, &segments, &segment_count); + rx_ts = listener->sock_state.logger != NULL ? linux_timestamping_parse_rx_timestamp(&msg) : 0; + if (rx_ts > 0) { + kcp_socket_debug_log_record(&listener->sock_state, EVENT_B_RX_SOFTWARE, &source, msg.msg_namelen, (int) n, 0, 0, 1, conv, segments, segment_count, rx_ts); + } + pthread_mutex_lock(&listener->lock); + { + kcp_conn_t *conn = kcp_listener_find_session(listener, conv); + if (conn == NULL) { + conn = (kcp_conn_t *) calloc(1, sizeof(*conn)); + if (conn != NULL) { + kcp_conn_options_t accepted_options; + conn->fd = listener->fd; + memcpy(&conn->remote_addr, &source, sizeof(source)); + conn->remote_addr_len = msg.msg_namelen; + pthread_mutex_init(&conn->kcp_mu, NULL); + pthread_mutex_init(&conn->close_mu, NULL); + pthread_cond_init(&conn->rx_cond, NULL); + protocol_frame_decoder_init(&conn->decoder); + snprintf(conn->node_role, sizeof(conn->node_role), "%s", listener->sock_state.node_role); + snprintf(conn->node_id, sizeof(conn->node_id), "%s", listener->sock_state.node_id); + conn->stats_interval_ms = KCP_DEFAULT_STATS_INTERVAL_MS; + conn->sock_state = &listener->sock_state; + conn->listener = listener; + kcp_conn_options_init(&accepted_options); + conn->options = accepted_options; + conn->update_interval_ms = accepted_options.interval_ms; + conn->kcp = ikcp_create(conv, conn); + if (conn->kcp != NULL) { + int update_started = 0; + ikcp_setoutput(conn->kcp, kcp_output_callback_impl); + if (kcp_conn_apply_options_locked(conn, &accepted_options) == 0 && + pthread_create(&conn->update_thread, NULL, kcp_update_thread_main, conn) == 0) { + update_started = 1; + } + if (update_started && kcp_listener_add_session(listener, conv, conn) == 0) { + conn->update_thread_started = 1; + kcp_listener_enqueue_accept(listener, conn); + } else { + atomic_store(&conn->closed, 1); + if (update_started) { + pthread_join(conn->update_thread, NULL); + } + ikcp_release(conn->kcp); + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + conn = NULL; + } + } else { + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + conn = NULL; + } + } + } + if (conn != NULL && conn->kcp != NULL) { + pthread_mutex_lock(&conn->kcp_mu); + conn->kcp->current = omni_now_millis32(); + if (ikcp_input(conn->kcp, (const char *) buffer, n) != 0) { + kcp_conn_record_error(conn); + } else { + kcp_conn_note_feedback_locked(conn); + kcp_conn_record_input(conn, (int) n, segment_count); + } + pthread_mutex_unlock(&conn->kcp_mu); + pthread_cond_broadcast(&conn->rx_cond); + } + } + pthread_mutex_unlock(&listener->lock); + free(segments); + segments = NULL; + segment_count = 0; + } + return NULL; +} + +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) { + struct sockaddr_storage local_addr; + socklen_t local_len; + int fd = kcp_socket_open_bound(listen_addr, bind_device, &local_addr, &local_len); + kcp_listener_t *listener; + if (fd < 0) { + return NULL; + } + listener = (kcp_listener_t *) calloc(1, sizeof(*listener)); + if (listener == NULL) { + close(fd); + return NULL; + } + listener->fd = fd; + pthread_mutex_init(&listener->lock, NULL); + pthread_mutex_init(&listener->accept_mu, NULL); + pthread_cond_init(&listener->accept_cond, NULL); + if (kcp_socket_debug_init(&listener->sock_state, fd, packet_logger, node_role, node_id) != 0) { + kcp_listener_free(listener); + return NULL; + } + if (pthread_create(&listener->recv_thread, NULL, kcp_listener_recv_thread_main, listener) != 0) { + kcp_listener_free(listener); + return NULL; + } + listener->recv_thread_started = 1; + return listener; +} + +kcp_conn_t *kcp_listener_accept(kcp_listener_t *listener) { + kcp_conn_t *conn; + if (listener == NULL) { + errno = EINVAL; + return NULL; + } + pthread_mutex_lock(&listener->accept_mu); + while (!listener->closed && listener->accept_head == NULL) { + pthread_cond_wait(&listener->accept_cond, &listener->accept_mu); + } + if (listener->closed) { + pthread_mutex_unlock(&listener->accept_mu); + errno = ECANCELED; + return NULL; + } + conn = listener->accept_head; + listener->accept_head = conn->accept_next; + if (listener->accept_head == NULL) { + listener->accept_tail = NULL; + } + conn->accept_next = NULL; + pthread_mutex_unlock(&listener->accept_mu); + return conn; +} + +int kcp_conn_configure_runtime(kcp_conn_t *conn, latency_logger_t *logger, const char *node_role, const char *node_id, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms) { + if (conn == NULL) { + errno = EINVAL; + return -1; + } + pthread_mutex_lock(&conn->close_mu); + conn->logger = logger; + if (node_role != NULL) { + snprintf(conn->node_role, sizeof(conn->node_role), "%s", node_role); + } + if (node_id != NULL) { + snprintf(conn->node_id, sizeof(conn->node_id), "%s", node_id); + } + conn->stats_logger = stats_logger; + if (stats_interval_ms > 0) { + conn->stats_interval_ms = stats_interval_ms; + } else if (conn->stats_interval_ms <= 0) { + conn->stats_interval_ms = KCP_DEFAULT_STATS_INTERVAL_MS; + } + if (kcp_conn_attach_process_sampler(conn) != 0) { + pthread_mutex_unlock(&conn->close_mu); + return -1; + } + pthread_mutex_unlock(&conn->close_mu); + if (kcp_conn_start_stats_thread(conn) != 0) { + pthread_mutex_lock(&conn->close_mu); + kcp_conn_detach_process_sampler(conn); + pthread_mutex_unlock(&conn->close_mu); + return -1; + } + return 0; +} + +int kcp_conn_apply_options(kcp_conn_t *conn, const kcp_conn_options_t *options) { + int rc; + + if (conn == NULL || options == NULL) { + errno = EINVAL; + return -1; + } + pthread_mutex_lock(&conn->kcp_mu); + rc = kcp_conn_apply_options_locked(conn, options); + pthread_mutex_unlock(&conn->kcp_mu); + return rc; +} + +int kcp_conn_send(kcp_conn_t *conn, const message_t *msg) { + uint8_t *frame = NULL; + size_t frame_len = 0; + int send_errno = 0; + int kcp_send_rc = 0; + if (conn == NULL || msg == NULL) { + errno = EINVAL; + return -1; + } + if (protocol_encode_message_stream(msg, &frame, &frame_len) != 0) { + return -1; + } + latencylog_log_message_event(conn->logger, conn->node_role, conn->node_id, EVENT_SEND_HANDOFF_BEGIN, msg); + pthread_mutex_lock(&conn->kcp_mu); + atomic_store(&conn->sock_state->last_send_errno, 0); + conn->kcp->current = omni_now_millis32(); + kcp_send_rc = ikcp_send(conn->kcp, (const char *) frame, (int) frame_len); + if (kcp_send_rc < 0) { + pthread_mutex_unlock(&conn->kcp_mu); + errno = kcp_send_rc == -2 ? EMSGSIZE : EINVAL; + free(frame); + return -1; + } + ikcp_flush(conn->kcp); + send_errno = atomic_load(&conn->sock_state->last_send_errno); + pthread_mutex_unlock(&conn->kcp_mu); + if (send_errno != 0) { + errno = send_errno; + free(frame); + return -1; + } + latencylog_log_message_event(conn->logger, conn->node_role, conn->node_id, EVENT_SEND_HANDOFF_END, msg); + free(frame); + return 0; +} + +static void kcp_timespec_deadline_after_ms(struct timespec *deadline, int timeout_ms) { + clock_gettime(CLOCK_REALTIME, deadline); + deadline->tv_sec += timeout_ms / 1000; + deadline->tv_nsec += (long) (timeout_ms % 1000) * 1000000L; + if (deadline->tv_nsec >= 1000000000L) { + deadline->tv_sec += 1; + deadline->tv_nsec -= 1000000000L; + } +} + +int kcp_conn_receive_timed(kcp_conn_t *conn, message_t *out_msg, int timeout_ms) { + uint8_t *frame = NULL; + size_t frame_len = 0; + char err[128]; + int next_rc; + struct timespec deadline; + int use_deadline = timeout_ms > 0; + + if (conn == NULL || out_msg == NULL) { + errno = EINVAL; + return -1; + } + if (use_deadline) { + kcp_timespec_deadline_after_ms(&deadline, timeout_ms); + } + for (;;) { + next_rc = protocol_frame_decoder_next(&conn->decoder, &frame, &frame_len); + if (next_rc < 0) { + return -1; + } + if (next_rc == 1) { + if (protocol_decode_message_stream_payload(frame, frame_len, out_msg, err, sizeof(err)) != 0) { + free(frame); + errno = EPROTO; + return -1; + } + free(frame); + return 0; + } + pthread_mutex_lock(&conn->kcp_mu); + { + int n = ikcp_recv(conn->kcp, (char *) conn->scratch, (int) sizeof(conn->scratch)); + if (n > 0) { + if (protocol_frame_decoder_feed(&conn->decoder, conn->scratch, (size_t) n) != 0) { + pthread_mutex_unlock(&conn->kcp_mu); + return -1; + } + pthread_mutex_unlock(&conn->kcp_mu); + continue; + } + if (atomic_load(&conn->closed)) { + pthread_mutex_unlock(&conn->kcp_mu); + errno = ECANCELED; + return -1; + } + if (timeout_ms == 0) { + pthread_mutex_unlock(&conn->kcp_mu); + return 1; + } + if (timeout_ms < 0) { + pthread_cond_wait(&conn->rx_cond, &conn->kcp_mu); + } else { + int wait_rc = pthread_cond_timedwait(&conn->rx_cond, &conn->kcp_mu, &deadline); + if (wait_rc == ETIMEDOUT) { + pthread_mutex_unlock(&conn->kcp_mu); + return 1; + } + if (wait_rc != 0) { + pthread_mutex_unlock(&conn->kcp_mu); + errno = wait_rc; + return -1; + } + } + } + pthread_mutex_unlock(&conn->kcp_mu); + } +} + +int kcp_conn_receive(kcp_conn_t *conn, message_t *out_msg) { + return kcp_conn_receive_timed(conn, out_msg, -1); +} + +uint32_t kcp_conn_conv(const kcp_conn_t *conn) { + return conn == NULL || conn->kcp == NULL ? 0 : conn->kcp->conv; +} + +int kcp_conn_local_addr(const kcp_conn_t *conn, struct sockaddr_storage *addr, socklen_t *addr_len) { + socklen_t len = sizeof(*addr); + if (conn == NULL || addr == NULL || addr_len == NULL || conn->sock_state == NULL) { + errno = EINVAL; + return -1; + } + if (getsockname(conn->sock_state->fd, (struct sockaddr *) addr, &len) != 0) { + return -1; + } + *addr_len = len; + return 0; +} + +int kcp_conn_remote_addr(const kcp_conn_t *conn, struct sockaddr_storage *addr, socklen_t *addr_len) { + if (conn == NULL || addr == NULL || addr_len == NULL) { + errno = EINVAL; + return -1; + } + if (conn->remote_addr_len == 0) { + errno = ENOTCONN; + return -1; + } + return omni_clone_sockaddr((const struct sockaddr *) &conn->remote_addr, conn->remote_addr_len, addr, addr_len); +} + +void kcp_conn_runtime_stats_snapshot(kcp_conn_t *conn, kcp_runtime_stats_t *out_stats) { + if (out_stats == NULL) { + return; + } + + memset(out_stats, 0, sizeof(*out_stats)); + if (conn == NULL) { + return; + } + + out_stats->connected = atomic_load(&conn->closed) ? 0 : 1; + pthread_mutex_lock(&conn->kcp_mu); + if (conn->kcp != NULL) { + out_stats->conv = conn->kcp->conv; + out_stats->rto_ms = conn->kcp->rx_rto; + out_stats->srtt_ms = conn->kcp->rx_srtt; + kcp_conn_update_min_srtt_locked(conn); + out_stats->min_srtt_ms = conn->min_srtt_ms; + out_stats->srttvar_ms = conn->kcp->rx_rttval; + out_stats->last_feedback_age_ms = conn->last_feedback_ms == 0 ? 0 : (omni_now_millis32() - conn->last_feedback_ms); + out_stats->snd_wnd = conn->kcp->snd_wnd; + out_stats->rmt_wnd = conn->kcp->rmt_wnd; + out_stats->inflight = conn->kcp->snd_nxt - conn->kcp->snd_una; + out_stats->window_limit = conn->kcp->snd_wnd < conn->kcp->rmt_wnd ? conn->kcp->snd_wnd : conn->kcp->rmt_wnd; + out_stats->window_pressure_pct = out_stats->window_limit == 0 + ? 0.0 + : ((double) out_stats->inflight * 100.0) / (double) out_stats->window_limit; + out_stats->snd_queue = conn->kcp->nsnd_que; + out_stats->rcv_queue = conn->kcp->nrcv_que; + out_stats->snd_buffer = conn->kcp->nsnd_buf; + out_stats->out_segs_total = atomic_load_explicit(&conn->total_out_segs, memory_order_relaxed); + out_stats->fast_retrans_total = conn->kcp->fast_retrans_total; + 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_stats->connected = 0; + } + pthread_mutex_unlock(&conn->kcp_mu); +} + +int kcp_conn_close(kcp_conn_t *conn) { + if (conn == NULL) { + return 0; + } + pthread_mutex_lock(&conn->close_mu); + if (!atomic_load(&conn->closed)) { + kcp_log_session_snapshot(conn, "close"); + kcp_process_sampler_request_sample_and_wait(conn->process_sampler, "close"); + 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; + } + pthread_cond_broadcast(&conn->rx_cond); + pthread_mutex_unlock(&conn->kcp_mu); + } + pthread_mutex_unlock(&conn->close_mu); + return 0; +} + +void kcp_conn_free(kcp_conn_t *conn) { + if (conn == NULL) { + return; + } + kcp_conn_close(conn); + if (conn->recv_thread_started) { + pthread_join(conn->recv_thread, NULL); + } + if (conn->update_thread_started) { + pthread_join(conn->update_thread, NULL); + } + if (conn->stats_thread_started) { + pthread_join(conn->stats_thread, NULL); + } + if (conn->listener != NULL && !conn->listener->closed) { + kcp_listener_remove_session(conn->listener, conn); + } + kcp_conn_detach_process_sampler(conn); + if (conn->owns_socket && conn->sock_state != NULL) { + if (!conn->socket_closed) { + close(conn->fd); + conn->socket_closed = 1; + } + kcp_socket_debug_destroy(conn->sock_state); + free(conn->sock_state); + } + if (conn->kcp != NULL) { + ikcp_release(conn->kcp); + } + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); +} + +int kcp_listener_close(kcp_listener_t *listener) { + if (listener == NULL) { + return 0; + } + if (!listener->closed) { + listener->closed = 1; + close(listener->fd); + pthread_cond_broadcast(&listener->accept_cond); + } + return 0; +} + +void kcp_listener_free(kcp_listener_t *listener) { + kcp_session_entry_t *entry; + kcp_session_entry_t *next; + if (listener == NULL) { + return; + } + kcp_listener_close(listener); + if (listener->recv_thread_started) { + pthread_join(listener->recv_thread, NULL); + } + for (entry = listener->sessions; entry != NULL; entry = next) { + next = entry->next; + entry->conn->listener = NULL; + kcp_conn_free(entry->conn); + free(entry); + } + kcp_socket_debug_destroy(&listener->sock_state); + pthread_mutex_destroy(&listener->lock); + pthread_mutex_destroy(&listener->accept_mu); + pthread_cond_destroy(&listener->accept_cond); + free(listener); +} + +int kcp_session_stats_parse_interval_ms(const char *raw, int *out_ms) { + return omni_parse_duration_ms(raw, KCP_DEFAULT_STATS_INTERVAL_MS, out_ms); +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/src/transport_udp.c b/robot/ros2/OmniSocketGo_robot_ros/src/transport_udp.c new file mode 100644 index 0000000..49a0e18 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/src/transport_udp.c @@ -0,0 +1,486 @@ +#include "transport_udp.h" + +#include +#include +#include +#include +#include + +typedef struct udp_pending_tx { + struct udp_pending_tx *next; + uint32_t tx_id; + message_t msg; + int bytes_written; + int saw_sched; + int saw_software; +} udp_pending_tx_t; + +struct udp_conn { + int fd; + int connected; + int timestamping_enabled; + latency_logger_t *logger; + tx_timestamp_debug_logger_t *debug_logger; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + pthread_mutex_t write_mu; + pthread_mutex_t pending_mu; + pthread_t errqueue_thread; + int errqueue_thread_started; + uint32_t next_tx_id; + udp_pending_tx_t *pending_head; + uint8_t *recv_buffer; + size_t recv_buffer_cap; + int closed; +}; + +static int udp_open_socket_for_addr(const struct sockaddr *addr, socklen_t addr_len, int bind_device, const char *device) { + int fd; + int reuse = 1; + fd = socket(addr->sa_family, SOCK_DGRAM, 0); + if (fd < 0) { + return -1; + } + setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); + if (bind_device && omni_bind_device(fd, device) != 0) { + close(fd); + return -1; + } + (void) addr_len; + return fd; +} + +static int udp_resolve_ip_only(const char *ip, int family, struct sockaddr_storage *out, socklen_t *out_len) { + struct addrinfo hints; + struct addrinfo *result = NULL; + memset(&hints, 0, sizeof(hints)); + hints.ai_family = family; + hints.ai_socktype = SOCK_DGRAM; + if (getaddrinfo(ip, "0", &hints, &result) != 0 || result == NULL) { + errno = EINVAL; + return -1; + } + memcpy(out, result->ai_addr, result->ai_addrlen); + *out_len = (socklen_t) result->ai_addrlen; + freeaddrinfo(result); + return 0; +} + +static udp_conn_t *udp_conn_alloc(int fd, int connected, int enable_timestamping, latency_logger_t *logger, const char *node_role, const char *node_id, tx_timestamp_debug_logger_t *debug_logger) { + udp_conn_t *conn = (udp_conn_t *) calloc(1, sizeof(*conn)); + if (conn == NULL) { + return NULL; + } + conn->fd = fd; + conn->connected = connected; + conn->timestamping_enabled = enable_timestamping; + conn->logger = logger; + conn->debug_logger = debug_logger; + snprintf(conn->node_role, sizeof(conn->node_role), "%s", node_role == NULL ? "" : node_role); + snprintf(conn->node_id, sizeof(conn->node_id), "%s", node_id == NULL ? "" : node_id); + conn->recv_buffer = (uint8_t *) malloc(OMNI_MAX_FRAME_SIZE); + if (conn->recv_buffer == NULL) { + free(conn); + return NULL; + } + conn->recv_buffer_cap = OMNI_MAX_FRAME_SIZE; + pthread_mutex_init(&conn->write_mu, NULL); + pthread_mutex_init(&conn->pending_mu, NULL); + return conn; +} + +static void udp_pending_destroy(udp_pending_tx_t *pending) { + while (pending != NULL) { + udp_pending_tx_t *next = pending->next; + protocol_message_clear(&pending->msg); + free(pending); + pending = next; + } +} + +static int udp_debug_log_send_chunk(udp_conn_t *conn, const message_t *msg, int bytes_written, uint32_t tx_id) { + tx_timestamp_debug_record_t record; + if (conn->debug_logger == NULL) { + return 0; + } + memset(&record, 0, sizeof(record)); + snprintf(record.record_type, sizeof(record.record_type), "%s", TX_TIMESTAMP_DEBUG_RECORD_SEND_CHUNK); + snprintf(record.node_role, sizeof(record.node_role), "%s", conn->node_role); + snprintf(record.node_id, sizeof(record.node_id), "%s", conn->node_id); + record.message_type = msg->type; + record.message_id = msg->id; + snprintf(record.from, sizeof(record.from), "%s", msg->from); + snprintf(record.to, sizeof(record.to), "%s", msg->to); + snprintf(record.file_name, sizeof(record.file_name), "%s", msg->file_name); + record.body_size = (int) msg->body_len; + record.send_call_index = 0; + record.frame_offset_start = 0; + record.frame_offset_end = bytes_written > 0 ? bytes_written - 1 : 0; + record.bytes_written = bytes_written; + record.expected_tx_id = tx_id; + return tx_timestamp_debug_log(conn->debug_logger, &record); +} + +static void udp_debug_log_errqueue_event(udp_conn_t *conn, const message_t *msg, const omni_tx_timestamp_event_t *event, uint32_t tx_id, int selected) { + tx_timestamp_debug_record_t record; + if (conn->debug_logger == NULL) { + return; + } + memset(&record, 0, sizeof(record)); + snprintf(record.record_type, sizeof(record.record_type), "%s", TX_TIMESTAMP_DEBUG_RECORD_ERRQUEUE_EVENT); + snprintf(record.node_role, sizeof(record.node_role), "%s", conn->node_role); + snprintf(record.node_id, sizeof(record.node_id), "%s", conn->node_id); + record.message_type = msg->type; + record.message_id = msg->id; + snprintf(record.from, sizeof(record.from), "%s", msg->from); + snprintf(record.to, sizeof(record.to), "%s", msg->to); + snprintf(record.file_name, sizeof(record.file_name), "%s", msg->file_name); + record.body_size = (int) msg->body_len; + snprintf(record.phase, sizeof(record.phase), "%s", "background"); + record.read_index = 0; + snprintf(record.event_name, sizeof(record.event_name), "%s", event->event_name); + record.ts_unix_nano = event->ts_unix_nano; + record.ee_info = event->ee_info; + record.ee_data = event->ee_data; + record.expected_tx_id = tx_id; + record.selected_for_latency = selected; + tx_timestamp_debug_log(conn->debug_logger, &record); +} + +static void *udp_errqueue_thread_main(void *arg) { + udp_conn_t *conn = (udp_conn_t *) arg; + uint8_t control[512]; + struct msghdr msg; + struct iovec iov; + uint8_t dummy; + + while (!conn->closed) { + ssize_t rc; + omni_tx_timestamp_event_t event; + udp_pending_tx_t *prev = NULL; + udp_pending_tx_t *cur = NULL; + memset(&msg, 0, sizeof(msg)); + memset(control, 0, sizeof(control)); + dummy = 0; + iov.iov_base = &dummy; + iov.iov_len = sizeof(dummy); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = control; + msg.msg_controllen = sizeof(control); + rc = recvmsg(conn->fd, &msg, MSG_ERRQUEUE | MSG_DONTWAIT); + if (rc < 0) { + if (errno == EAGAIN || errno == EWOULDBLOCK || errno == EINTR) { + usleep(10000); + continue; + } + if (conn->closed) { + return NULL; + } + usleep(10000); + continue; + } + if (linux_timestamping_parse_tx_timestamp(&msg, &event) != 0) { + continue; + } + pthread_mutex_lock(&conn->pending_mu); + cur = NULL; + prev = NULL; + { + udp_pending_tx_t **head = &conn->pending_head; + udp_pending_tx_t *iter = *head; + while (iter != NULL) { + if (iter->tx_id == event.ee_data) { + cur = iter; + break; + } + prev = iter; + iter = iter->next; + } + if (cur != NULL) { + if (strcmp(event.event_name, EVENT_A_TX_SCHED) == 0 && !cur->saw_sched) { + cur->saw_sched = 1; + latencylog_log_message_event_at(conn->logger, conn->node_role, conn->node_id, EVENT_A_TX_SCHED, event.ts_unix_nano, &cur->msg); + } else if (strcmp(event.event_name, EVENT_A_TX_SOFTWARE) == 0 && !cur->saw_software) { + cur->saw_software = 1; + latencylog_log_message_event_at(conn->logger, conn->node_role, conn->node_id, EVENT_A_TX_SOFTWARE, event.ts_unix_nano, &cur->msg); + } + udp_debug_log_errqueue_event(conn, &cur->msg, &event, cur->tx_id, 1); + if (cur->saw_sched && cur->saw_software) { + if (prev == NULL) { + *head = cur->next; + } else { + prev->next = cur->next; + } + protocol_message_clear(&cur->msg); + free(cur); + } + } + } + pthread_mutex_unlock(&conn->pending_mu); + } + return NULL; +} + +static int udp_conn_start_errqueue(udp_conn_t *conn) { + if (!conn->timestamping_enabled) { + return 0; + } + if (pthread_create(&conn->errqueue_thread, NULL, udp_errqueue_thread_main, conn) != 0) { + return -1; + } + conn->errqueue_thread_started = 1; + return 0; +} + +udp_conn_t *udp_conn_dial(const char *server_addr, const char *bind_ip, const char *bind_device, int enable_timestamping, latency_logger_t *logger, const char *node_role, const char *node_id, tx_timestamp_debug_logger_t *debug_logger) { + struct sockaddr_storage remote_addr; + struct sockaddr_storage local_addr; + socklen_t remote_len; + socklen_t local_len; + int family; + int fd; + udp_conn_t *conn; + + if (omni_parse_sockaddr(server_addr, 0, &remote_addr, &remote_len, &family) != 0) { + return NULL; + } + fd = udp_open_socket_for_addr((struct sockaddr *) &remote_addr, remote_len, bind_device != NULL && bind_device[0] != '\0', bind_device); + if (fd < 0) { + return NULL; + } + if (bind_ip != NULL && bind_ip[0] != '\0') { + if (udp_resolve_ip_only(bind_ip, family, &local_addr, &local_len) != 0 || bind(fd, (struct sockaddr *) &local_addr, local_len) != 0) { + close(fd); + return NULL; + } + } + if (connect(fd, (struct sockaddr *) &remote_addr, remote_len) != 0) { + close(fd); + return NULL; + } + if (enable_timestamping && linux_timestamping_enable_udp_socket(fd, 1) != 0) { + close(fd); + return NULL; + } + conn = udp_conn_alloc(fd, 1, enable_timestamping, logger, node_role, node_id, debug_logger); + if (conn == NULL) { + close(fd); + return NULL; + } + if (udp_conn_start_errqueue(conn) != 0) { + udp_conn_free(conn); + return NULL; + } + return conn; +} + +udp_conn_t *udp_conn_bind(const char *listen_addr, const char *bind_device, int enable_timestamping, latency_logger_t *logger, const char *node_role, const char *node_id, tx_timestamp_debug_logger_t *debug_logger) { + struct sockaddr_storage local_addr; + socklen_t local_len; + int family; + int fd; + udp_conn_t *conn; + if (omni_parse_sockaddr(listen_addr, 1, &local_addr, &local_len, &family) != 0) { + return NULL; + } + fd = udp_open_socket_for_addr((struct sockaddr *) &local_addr, local_len, bind_device != NULL && bind_device[0] != '\0', bind_device); + if (fd < 0) { + return NULL; + } + if (bind(fd, (struct sockaddr *) &local_addr, local_len) != 0) { + close(fd); + return NULL; + } + if (enable_timestamping && linux_timestamping_enable_udp_socket(fd, 1) != 0) { + close(fd); + return NULL; + } + conn = udp_conn_alloc(fd, 0, enable_timestamping, logger, node_role, node_id, debug_logger); + if (conn == NULL) { + close(fd); + return NULL; + } + if (udp_conn_start_errqueue(conn) != 0) { + udp_conn_free(conn); + return NULL; + } + return conn; +} + +static int udp_conn_send_inner(udp_conn_t *conn, const message_t *msg, const struct sockaddr *addr, socklen_t addr_len) { + uint8_t *payload = NULL; + size_t payload_len = 0; + ssize_t rc; + udp_pending_tx_t *pending = NULL; + uint32_t tx_id; + + if (protocol_encode_message_datagram(msg, &payload, &payload_len) != 0) { + return -1; + } + pthread_mutex_lock(&conn->write_mu); + latencylog_log_message_event(conn->logger, conn->node_role, conn->node_id, EVENT_SEND_HANDOFF_BEGIN, msg); + tx_id = conn->next_tx_id++; + if (conn->timestamping_enabled) { + pending = (udp_pending_tx_t *) calloc(1, sizeof(*pending)); + if (pending == NULL || protocol_message_copy(&pending->msg, msg) != 0) { + free(payload); + pthread_mutex_unlock(&conn->write_mu); + free(pending); + return -1; + } + pending->tx_id = tx_id; + pending->bytes_written = (int) payload_len; + pthread_mutex_lock(&conn->pending_mu); + pending->next = conn->pending_head; + conn->pending_head = pending; + pthread_mutex_unlock(&conn->pending_mu); + udp_debug_log_send_chunk(conn, msg, (int) payload_len, tx_id); + } + if (addr != NULL) { + rc = sendto(conn->fd, payload, payload_len, 0, addr, addr_len); + } else { + rc = send(conn->fd, payload, payload_len, 0); + } + free(payload); + if (rc < 0 || (size_t) rc != payload_len) { + if (pending != NULL) { + udp_pending_tx_t *prev = NULL; + udp_pending_tx_t *cur; + pthread_mutex_lock(&conn->pending_mu); + for (cur = conn->pending_head; cur != NULL; cur = cur->next) { + if (cur == pending) { + if (prev == NULL) { + conn->pending_head = cur->next; + } else { + prev->next = cur->next; + } + break; + } + prev = cur; + } + pthread_mutex_unlock(&conn->pending_mu); + protocol_message_clear(&pending->msg); + free(pending); + } + pthread_mutex_unlock(&conn->write_mu); + return -1; + } + latencylog_log_message_event(conn->logger, conn->node_role, conn->node_id, EVENT_SEND_HANDOFF_END, msg); + pthread_mutex_unlock(&conn->write_mu); + return 0; +} + +int udp_conn_send(udp_conn_t *conn, const message_t *msg) { + if (conn == NULL || !conn->connected) { + errno = ENOTCONN; + return -1; + } + return udp_conn_send_inner(conn, msg, NULL, 0); +} + +int udp_conn_send_to(udp_conn_t *conn, const message_t *msg, const struct sockaddr *addr, socklen_t addr_len) { + if (conn == NULL || addr == NULL) { + errno = EINVAL; + return -1; + } + return udp_conn_send_inner(conn, msg, addr, addr_len); +} + +int udp_conn_receive(udp_conn_t *conn, message_t *out_msg, struct sockaddr_storage *addr, socklen_t *addr_len) { + uint8_t control[512]; + struct sockaddr_storage source; + struct iovec iov; + struct msghdr msg; + ssize_t n; + int64_t rx_ts = 0; + char err[128]; + + if (conn == NULL || out_msg == NULL) { + errno = EINVAL; + return -1; + } + memset(&msg, 0, sizeof(msg)); + memset(&source, 0, sizeof(source)); + iov.iov_base = conn->recv_buffer; + iov.iov_len = conn->recv_buffer_cap; + msg.msg_name = &source; + msg.msg_namelen = sizeof(source); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + if (conn->timestamping_enabled) { + msg.msg_control = control; + msg.msg_controllen = sizeof(control); + } + 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) { + rx_ts = linux_timestamping_parse_rx_timestamp(&msg); + } + if (protocol_decode_message_datagram(conn->recv_buffer, (size_t) n, out_msg, err, sizeof(err)) != 0) { + errno = EPROTO; + return -1; + } + if (addr != NULL && addr_len != NULL) { + omni_clone_sockaddr((struct sockaddr *) &source, msg.msg_namelen, addr, addr_len); + } + if (rx_ts > 0) { + latencylog_log_message_event_at(conn->logger, conn->node_role, conn->node_id, EVENT_B_RX_SOFTWARE, rx_ts, out_msg); + } + return 0; +} + +int udp_conn_fd(const udp_conn_t *conn) { + return conn == NULL ? -1 : conn->fd; +} + +int udp_conn_local_addr(const udp_conn_t *conn, struct sockaddr_storage *addr, socklen_t *addr_len) { + socklen_t len = sizeof(*addr); + if (conn == NULL || addr == NULL || addr_len == NULL) { + errno = EINVAL; + return -1; + } + if (getsockname(conn->fd, (struct sockaddr *) addr, &len) != 0) { + return -1; + } + *addr_len = len; + return 0; +} + +int udp_conn_close(udp_conn_t *conn) { + if (conn == NULL) { + return 0; + } + 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); + conn->errqueue_thread_started = 0; + } + } + return 0; +} + +void udp_conn_free(udp_conn_t *conn) { + if (conn == NULL) { + return; + } + udp_conn_close(conn); + udp_pending_destroy(conn->pending_head); + free(conn->recv_buffer); + pthread_mutex_destroy(&conn->write_mu); + pthread_mutex_destroy(&conn->pending_mu); + free(conn); +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/src/tx_timestamp_debug.c b/robot/ros2/OmniSocketGo_robot_ros/src/tx_timestamp_debug.c new file mode 100644 index 0000000..3cdf19c --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/src/tx_timestamp_debug.c @@ -0,0 +1,108 @@ +#include "tx_timestamp_debug.h" + +tx_timestamp_debug_logger_t *tx_timestamp_debug_open_jsonl(const char *path) { + tx_timestamp_debug_logger_t *logger; + FILE *file; + if (path == NULL || path[0] == '\0') { + return NULL; + } + if (omni_ensure_parent_dir(path) != 0) { + return NULL; + } + file = fopen(path, "ab"); + if (file == NULL) { + return NULL; + } + logger = (tx_timestamp_debug_logger_t *) calloc(1, sizeof(*logger)); + if (logger == NULL) { + fclose(file); + return NULL; + } + omni_file_logger_init_path(&logger->file_logger, file, path, 0); + logger->enabled = 1; + return logger; +} + +void tx_timestamp_debug_close(tx_timestamp_debug_logger_t *logger) { + if (logger == NULL) { + return; + } + if (logger->file_logger.file != NULL) { + fclose(logger->file_logger.file); + } + omni_file_logger_destroy(&logger->file_logger); + free(logger); +} + +int tx_timestamp_debug_log(tx_timestamp_debug_logger_t *logger, const tx_timestamp_debug_record_t *record) { + char *line; + char *node_role; + char *node_id; + char *from; + char *to; + char *file_name; + char *phase; + char *event_name; + + if (logger == NULL || record == NULL || !logger->enabled) { + return 0; + } + node_role = omni_json_escape(record->node_role); + node_id = omni_json_escape(record->node_id); + from = omni_json_escape(record->from); + to = omni_json_escape(record->to); + file_name = omni_json_escape(record->file_name); + phase = omni_json_escape(record->phase); + event_name = omni_json_escape(record->event_name); + if (node_role == NULL || node_id == NULL || from == NULL || to == NULL || file_name == NULL || phase == NULL || event_name == NULL) { + free(node_role); + free(node_id); + free(from); + free(to); + free(file_name); + free(phase); + free(event_name); + return -1; + } + line = omni_strdup_printf( + "{\"record_type\":\"%s\",\"node_role\":\"%s\",\"node_id\":\"%s\",\"message_type\":\"%s\",\"message_id\":%" PRIu64 ",\"from\":\"%s\",\"to\":\"%s\",\"file_name\":\"%s\",\"body_size\":%d,\"phase\":\"%s\",\"send_call_index\":%d,\"frame_offset_start\":%d,\"frame_offset_end\":%d,\"bytes_written\":%d,\"expected_tx_id\":%u,\"read_index\":%d,\"event_name\":\"%s\",\"ts_unix_nano\":%" PRId64 ",\"ee_info\":%u,\"ee_data\":%u,\"matched_send_call_index\":%d,\"selected_for_latency\":%d}", + record->record_type, + node_role, + node_id, + protocol_message_type_name(record->message_type), + record->message_id, + from, + to, + file_name, + record->body_size, + phase, + record->send_call_index, + record->frame_offset_start, + record->frame_offset_end, + record->bytes_written, + record->expected_tx_id, + record->read_index, + event_name, + record->ts_unix_nano, + record->ee_info, + record->ee_data, + record->matched_send_call_index, + record->selected_for_latency + ); + free(node_role); + free(node_id); + free(from); + free(to); + free(file_name); + free(phase); + free(event_name); + if (line == NULL) { + return -1; + } + if (omni_file_logger_write_line(&logger->file_logger, line) != 0) { + free(line); + return -1; + } + free(line); + return 0; +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/src/video_pipeline.c b/robot/ros2/OmniSocketGo_robot_ros/src/video_pipeline.c new file mode 100644 index 0000000..ab55d2d --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/src/video_pipeline.c @@ -0,0 +1,1710 @@ +#include "video_pipeline.h" + +#include "ros_image_shm.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#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_HEAD_CAMERA_DEVICE "/dev/video26" +#define VIDEO_DEFAULT_WAIST_CAMERA_DEVICE "/dev/video18" +#define VIDEO_DEFAULT_PEER_ID "peer-b-video" +#define VIDEO_DEFAULT_TARGET_PEER "peer-a-video" +#define VIDEO_SOFT_BACKPRESSURE_SEGMENTS_DEFAULT 64 +#define VIDEO_HARD_BACKPRESSURE_SEGMENTS_DEFAULT 192 +#define VIDEO_HARD_BACKPRESSURE_HOLD_MS_DEFAULT 1000 +#define VIDEO_DEFAULT_FRAME_STALL_RECONNECT_MS 3000 +#define VIDEO_SOFT_BACKPRESSURE_WINDOW_PRESSURE_PCT 90.0 +#define VIDEO_HARD_BACKPRESSURE_WINDOW_PRESSURE_PCT 98.0 +#define VIDEO_SESSION_POLL_INTERVAL_MS 250 +#define VIDEO_ROS2_HEAD_SHM_DEFAULT "/dev/shm/omnisocket-rgb-head" +#define VIDEO_ROS2_WAIST_SHM_DEFAULT "/dev/shm/omnisocket-rgb-waist" + +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; + uint64_t next_frame_seq; +} 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 int env_int_or_default(const char *name, int fallback) { + const char *value = getenv(name); + int parsed; + + if (value == NULL || value[0] == '\0') { + return fallback; + } + parsed = atoi(value); + if (parsed <= 0) { + return fallback; + } + return parsed; +} + +static size_t env_size_or_default(const char *name, size_t fallback) { + const char *value = getenv(name); + unsigned long long parsed; + + if (value == NULL || value[0] == '\0') { + return fallback; + } + parsed = strtoull(value, NULL, 10); + if (parsed == 0ULL) { + return fallback; + } + return (size_t) parsed; +} + +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); +} + +static void video_pipeline_report_progress(const video_pipeline_config_t *config) { + if (config == NULL || config->progress_callback == NULL) { + return; + } + config->progress_callback(config->progress_context); +} + +void video_pipeline_config_init(video_pipeline_config_t *config) { + if (config == NULL) { + return; + } + memset(config, 0, sizeof(*config)); + config->input_mode = VIDEO_INPUT_ROS2; + config->camera_device = VIDEO_DEFAULT_CAMERA_DEVICE; + config->camera_head_device = VIDEO_DEFAULT_HEAD_CAMERA_DEVICE; + config->camera_waist_device = VIDEO_DEFAULT_WAIST_CAMERA_DEVICE; + config->ros_head_shm = VIDEO_ROS2_HEAD_SHM_DEFAULT; + config->ros_waist_shm = VIDEO_ROS2_WAIST_SHM_DEFAULT; + config->ros_max_frame_bytes = ROS_IMAGE_SHM_DEFAULT_MAX_FRAME_BYTES; + config->active_camera = NULL; + 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; + config->soft_backpressure_segments = VIDEO_SOFT_BACKPRESSURE_SEGMENTS_DEFAULT; + config->hard_backpressure_segments = VIDEO_HARD_BACKPRESSURE_SEGMENTS_DEFAULT; + config->hard_backpressure_hold_ms = VIDEO_HARD_BACKPRESSURE_HOLD_MS_DEFAULT; + config->frame_stall_reconnect_ms = VIDEO_DEFAULT_FRAME_STALL_RECONNECT_MS; + config->stats_logger = NULL; + config->stage_logger = NULL; + config->stats_interval_ms = 1000; +} + +void video_pipeline_config_load_env(video_pipeline_config_t *config) { + if (config == NULL) { + return; + } + { + const char *input_mode = getenv("OMNI_CAMERA_SOURCE"); + config->input_mode = input_mode != NULL && strcmp(input_mode, "v4l2") == 0 + ? VIDEO_INPUT_V4L2 + : VIDEO_INPUT_ROS2; + } + config->camera_device = env_or_default("OMNI_CAMERA_DEVICE", config->camera_device); + config->camera_head_device = env_or_default("OMNI_CAMERA_HEAD_DEVICE", config->camera_head_device); + config->camera_waist_device = env_or_default("OMNI_CAMERA_WAIST_DEVICE", config->camera_waist_device); + config->ros_head_shm = env_or_default("OMNI_ROS2_HEAD_SHM", config->ros_head_shm); + config->ros_waist_shm = env_or_default("OMNI_ROS2_WAIST_SHM", config->ros_waist_shm); + config->ros_max_frame_bytes = env_size_or_default( + "OMNI_ROS2_MAX_FRAME_BYTES", + config->ros_max_frame_bytes + ); + 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); + config->soft_backpressure_segments = env_int_or_default("OMNI_VIDEO_SOFT_BACKPRESSURE_SEGMENTS", config->soft_backpressure_segments); + config->hard_backpressure_segments = env_int_or_default("OMNI_VIDEO_HARD_BACKPRESSURE_SEGMENTS", config->hard_backpressure_segments); + config->hard_backpressure_hold_ms = env_int_or_default("OMNI_VIDEO_HARD_BACKPRESSURE_HOLD_MS", config->hard_backpressure_hold_ms); + config->frame_stall_reconnect_ms = env_int_or_default("OMNI_VIDEO_FRAME_STALL_RECONNECT_MS", config->frame_stall_reconnect_ms); + config->stats_interval_ms = env_int_or_default("BLITZ_KCP_STATS_INTERVAL_MS", config->stats_interval_ms); +} + +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->backpressure_drops = stats->backpressure_drops; + out_stats->backlog_resets = stats->backlog_resets; + out_stats->last_frame_bytes = stats->last_frame_bytes; + out_stats->last_backlog_segments = stats->last_backlog_segments; + out_stats->last_capture_to_send_ms = stats->last_capture_to_send_ms; + out_stats->avg_capture_to_send_ms = stats->avg_capture_to_send_ms; + out_stats->connected = stats->connected; + snprintf(out_stats->last_error, sizeof(out_stats->last_error), "%s", stats->last_error); + snprintf(out_stats->last_backlog_reason, sizeof(out_stats->last_backlog_reason), "%s", stats->last_backlog_reason); + 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, + config->stats_logger, + config->stats_interval_ms + ); + if (sender->client == NULL) { + return -1; + } + return 0; +} + +static int video_sender_drain_pending_messages(video_sender_t *sender) { + int drained = 0; + + if (sender == NULL || sender->client == NULL) { + errno = EINVAL; + return -1; + } + + for (;;) { + message_t msg; + int rc; + + protocol_message_init(&msg); + rc = kcp_client_receive_timed(sender->client, &msg, 1); + if (rc == 1) { + protocol_message_clear(&msg); + return 0; + } + if (rc != 0) { + protocol_message_clear(&msg); + return -1; + } + + // Drain unread server errors so an offline receiver cannot back up the reverse KCP stream. + protocol_message_clear(&msg); + drained += 1; + if (drained >= 8) { + return 0; + } + } +} + +static int video_sender_send_packet( + video_sender_t *sender, + const AVPacket *encoded_pkt, + const video_pipeline_packet_metadata_t *metadata, + uint64_t *out_frame_seq +) { + uint8_t *payload; + size_t payload_len; + uint64_t frame_seq; + int rc; + + if (sender == NULL || sender->client == NULL || encoded_pkt == NULL || metadata == NULL) { + errno = EINVAL; + return -1; + } + + frame_seq = sender->next_frame_seq + 1U; + payload_len = 8U + (size_t) encoded_pkt->size + sizeof(*metadata); + if (video_sender_ensure_buffer_capacity(sender, payload_len) != 0) { + return -1; + } + payload = sender->send_buffer; + + payload[0] = (uint8_t) (frame_seq >> 56); + payload[1] = (uint8_t) (frame_seq >> 48); + payload[2] = (uint8_t) (frame_seq >> 40); + payload[3] = (uint8_t) (frame_seq >> 32); + payload[4] = (uint8_t) (frame_seq >> 24); + payload[5] = (uint8_t) (frame_seq >> 16); + payload[6] = (uint8_t) (frame_seq >> 8); + payload[7] = (uint8_t) frame_seq; + memcpy(payload + 8U, encoded_pkt->data, (size_t) encoded_pkt->size); + memcpy(payload + 8U + (size_t) encoded_pkt->size, metadata, sizeof(*metadata)); + rc = kcp_client_send_binary(sender->client, sender->target_peer, payload, payload_len); + if (rc != 0) { + return rc; + } + sender->next_frame_seq = frame_seq; + if (out_frame_seq != NULL) { + *out_frame_seq = frame_seq; + } + rc = video_sender_drain_pending_messages(sender); + 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 uint32_t video_sender_backlog_segments(const kcp_runtime_stats_t *stats) { + if (stats == NULL) { + return 0; + } + return stats->snd_queue + stats->snd_buffer; +} + +static int video_sender_soft_backpressure_active(const video_pipeline_config_t *config, const kcp_runtime_stats_t *transport) { + if (config == NULL || transport == NULL) { + return 0; + } + return video_sender_backlog_segments(transport) >= (uint32_t) config->soft_backpressure_segments + || transport->window_pressure_pct >= VIDEO_SOFT_BACKPRESSURE_WINDOW_PRESSURE_PCT; +} + +static int video_sender_hard_backpressure_active(const video_pipeline_config_t *config, const kcp_runtime_stats_t *transport) { + if (config == NULL || transport == NULL) { + return 0; + } + return video_sender_backlog_segments(transport) >= (uint32_t) config->hard_backpressure_segments + || transport->window_pressure_pct >= VIDEO_HARD_BACKPRESSURE_WINDOW_PRESSURE_PCT; +} + +static void video_pipeline_note_backpressure( + video_pipeline_stats_t *stats, + const char *reason, + const kcp_runtime_stats_t *transport, + int increment_drop, + int increment_reset +) { + if (stats == NULL) { + return; + } + pthread_mutex_lock(&stats->mutex); + if (increment_drop) { + stats->backpressure_drops += 1; + } + if (increment_reset) { + stats->backlog_resets += 1; + } + if (transport != NULL) { + stats->last_backlog_segments = video_sender_backlog_segments(transport); + stats->transport = *transport; + } else { + stats->last_backlog_segments = 0; + } + snprintf(stats->last_backlog_reason, sizeof(stats->last_backlog_reason), "%s", reason == NULL ? "" : reason); + pthread_mutex_unlock(&stats->mutex); +} + +static void video_pipeline_note_capture_to_send(video_pipeline_stats_t *stats, uint32_t capture_to_send_ms) { + if (stats == NULL) { + return; + } + pthread_mutex_lock(&stats->mutex); + stats->last_capture_to_send_ms = capture_to_send_ms; + if (stats->avg_capture_to_send_ms <= 0.0) { + stats->avg_capture_to_send_ms = (double) capture_to_send_ms; + } else { + stats->avg_capture_to_send_ms = stats->avg_capture_to_send_ms * 0.9 + (double) capture_to_send_ms * 0.1; + } + pthread_mutex_unlock(&stats->mutex); +} + +static int video_stage_logger_should_log(const video_stage_logger_t *logger, uint64_t frame_seq) { + if (logger == NULL || !logger->enabled) { + return 0; + } + if (logger->sample_mod <= 1U) { + return 1; + } + return frame_seq % logger->sample_mod == 0U; +} + +static void video_stage_logger_log_frame( + video_stage_logger_t *logger, + uint64_t frame_seq, + double capture_ms, + double decode_ms, + double scale_ms, + double encode_ms, + double send_ms, + double pipeline_total_ms, + size_t jpeg_bytes, + uint64_t kcp_out_seg_delta, + uint32_t backlog_segments, + double window_pressure_pct, + int32_t video_srtt_ms +) { + char *line; + + if (!video_stage_logger_should_log(logger, frame_seq)) { + return; + } + line = omni_strdup_printf( + "{\"ts_unix_nano\":%" PRId64 ",\"frame_seq\":%" PRIu64 ",\"capture_ms\":%.3f,\"decode_ms\":%.3f,\"scale_ms\":%.3f,\"encode_ms\":%.3f,\"send_ms\":%.3f,\"pipeline_total_ms\":%.3f,\"jpeg_bytes\":%zu,\"kcp_out_seg_delta\":%" PRIu64 ",\"backlog_segments\":%u,\"window_pressure_pct\":%.3f,\"video_srtt_ms\":%d}", + omni_now_unix_nano(), + frame_seq, + capture_ms, + decode_ms, + scale_ms, + encode_ms, + send_ms, + pipeline_total_ms, + jpeg_bytes, + kcp_out_seg_delta, + backlog_segments, + window_pressure_pct, + video_srtt_ms + ); + if (line == NULL) { + return; + } + (void) omni_file_logger_write_line(&logger->file_logger, line); + free(line); +} + +video_stage_logger_t *video_stage_logger_open_jsonl(const char *path, uint64_t sample_mod) { + video_stage_logger_t *logger; + FILE *file; + + if (path == NULL || path[0] == '\0') { + return NULL; + } + if (omni_ensure_parent_dir(path) != 0) { + return NULL; + } + file = fopen(path, "ab"); + if (file == NULL) { + return NULL; + } + logger = (video_stage_logger_t *) calloc(1, sizeof(*logger)); + if (logger == NULL) { + fclose(file); + return NULL; + } + omni_file_logger_init_path(&logger->file_logger, file, path, 0); + logger->enabled = 1; + logger->sample_mod = sample_mod == 0U ? 1U : sample_mod; + return logger; +} + +void video_stage_logger_close(video_stage_logger_t *logger) { + if (logger == NULL) { + return; + } + if (logger->file_logger.file != NULL) { + fclose(logger->file_logger.file); + } + omni_file_logger_destroy(&logger->file_logger); + free(logger); +} + +static int video_server_error_requires_reconnect(const char *message) { + if (message == NULL || message[0] == '\0') { + return 0; + } + return strstr(message, "not registered") != NULL + || strstr(message, "first message must be register") != NULL + || strstr(message, "peer replaced") != NULL + || strstr(message, "timed out waiting for server_register_ok") != NULL + || strstr(message, "failed to acknowledge server heartbeat") != NULL; +} + +static void video_pipeline_update_connection_state( + video_pipeline_stats_t *stats, + const kcp_client_state_t *client_state, + const kcp_runtime_stats_t *transport +) { + if (stats == NULL) { + return; + } + + pthread_mutex_lock(&stats->mutex); + if (transport != NULL) { + stats->transport = *transport; + } + if (client_state != NULL) { + stats->connected = client_state->connected != 0 && client_state->registered != 0; + if (client_state->last_server_error[0] != '\0') { + snprintf(stats->last_error, sizeof(stats->last_error), "%s", client_state->last_server_error); + } + } + pthread_mutex_unlock(&stats->mutex); +} + +static int video_sender_check_session_stale( + video_sender_t *sender, + const video_pipeline_config_t *config, + video_pipeline_stats_t *stats, + kcp_runtime_stats_t *transport_stats, + char *reason, + size_t reason_len +) { + kcp_client_state_t client_state; + + if ( + sender == NULL || sender->client == NULL || config == NULL || stats == NULL || transport_stats == NULL + || reason == NULL || reason_len == 0 + ) { + errno = EINVAL; + return -1; + } + + reason[0] = '\0'; + memset(&client_state, 0, sizeof(client_state)); + kcp_client_runtime_stats_snapshot(sender->client, transport_stats); + kcp_client_state_snapshot(sender->client, &client_state); + video_pipeline_update_connection_state(stats, &client_state, transport_stats); + + if (!transport_stats->connected || !client_state.connected) { + snprintf(reason, reason_len, "video session stale: transport disconnected"); + return 1; + } + if (!client_state.registered) { + snprintf(reason, reason_len, "video session stale: server reported unregistered"); + return 1; + } + if (video_server_error_requires_reconnect(client_state.last_server_error)) { + snprintf(reason, reason_len, "video session stale: server error %.180s", client_state.last_server_error); + return 1; + } + return 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); +} + +typedef struct video_camera_source { + const char *name; + const char *device; + int backend; + int fd; + video_buffer_t *buffers; + int num_buffers; + int streaming; + ros_image_shm_source_t ros_shm; + uint8_t *ros_frame_buffer; + size_t ros_frame_buffer_bytes; +} video_camera_source_t; + +enum { + VIDEO_SOURCE_BACKEND_V4L2 = 0, + VIDEO_SOURCE_BACKEND_ROS2 = 1 +}; + +static void video_camera_source_cleanup(video_camera_source_t *source) { + enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + + if (source == NULL) { + return; + } + if (source->backend == VIDEO_SOURCE_BACKEND_V4L2 && source->fd >= 0 && source->streaming) { + (void) ioctl(source->fd, VIDIOC_STREAMOFF, &type); + } + if (source->backend == VIDEO_SOURCE_BACKEND_V4L2) { + video_pipeline_cleanup_buffers(source->buffers, source->num_buffers); + if (source->fd >= 0) { + close(source->fd); + } + } else { + ros_image_shm_close(&source->ros_shm); + free(source->ros_frame_buffer); + } + source->backend = VIDEO_SOURCE_BACKEND_V4L2; + source->fd = -1; + source->buffers = NULL; + source->num_buffers = 0; + source->streaming = 0; + source->ros_frame_buffer = NULL; + source->ros_frame_buffer_bytes = 0; +} + +static int video_camera_source_start(video_camera_source_t *source, int width, int height) { + enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + int i; + + source->backend = VIDEO_SOURCE_BACKEND_V4L2; + source->fd = open_v4l2_device(source->device); + if (source->fd < 0 || init_v4l2_device(source->fd, width, height) < 0 + || init_mmap(source->fd, &source->buffers, &source->num_buffers) < 0) { + return -1; + } + for (i = 0; i < source->num_buffers; 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(source->fd, VIDIOC_QBUF, &buf) < 0) { + return -1; + } + } + if (ioctl(source->fd, VIDIOC_STREAMON, &type) < 0) { + return -1; + } + source->streaming = 1; + fprintf(stderr, "[video_pipeline] camera %s ready on %s\n", source->name, source->device); + return 0; +} + +static int video_camera_source_start_ros2( + video_camera_source_t *source, + const char *shm_path, + size_t max_frame_bytes +) { + if (source == NULL || shm_path == NULL || shm_path[0] == '\0' || max_frame_bytes == 0) { + errno = EINVAL; + return -1; + } + source->backend = VIDEO_SOURCE_BACKEND_ROS2; + source->fd = -1; + memset(&source->ros_shm, 0, sizeof(source->ros_shm)); + source->ros_shm.fd = -1; + source->ros_frame_buffer = (uint8_t *) malloc(max_frame_bytes); + if (source->ros_frame_buffer == NULL) { + errno = ENOMEM; + return -1; + } + source->ros_frame_buffer_bytes = max_frame_bytes; + if (ros_image_shm_open(&source->ros_shm, shm_path, max_frame_bytes) != 0) { + free(source->ros_frame_buffer); + source->ros_frame_buffer = NULL; + source->ros_frame_buffer_bytes = 0; + return -1; + } + source->streaming = 1; + fprintf(stderr, "[video_pipeline] ROS2 RGB source %s ready on %s\n", source->name, shm_path); + return 0; +} + +static void video_camera_source_discard_ready(video_camera_source_t *source) { + struct v4l2_buffer buf; + + if (source == NULL || source->backend != VIDEO_SOURCE_BACKEND_V4L2 || source->fd < 0) { + return; + } + memset(&buf, 0, sizeof(buf)); + buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + buf.memory = V4L2_MEMORY_MMAP; + if (ioctl(source->fd, VIDIOC_DQBUF, &buf) == 0) { + (void) ioctl(source->fd, VIDIOC_QBUF, &buf); + } +} + +static void video_camera_source_requeue( + video_camera_source_t *source, + const struct v4l2_buffer *buffer +) { + struct v4l2_buffer mutable_buffer; + + if (source == NULL || buffer == NULL || source->backend != VIDEO_SOURCE_BACKEND_V4L2 || source->fd < 0) { + return; + } + mutable_buffer = *buffer; + (void) ioctl(source->fd, VIDIOC_QBUF, &mutable_buffer); +} + +static enum AVPixelFormat ros_encoding_to_av_pixel_format(uint32_t encoding) { + switch (encoding) { + case ROS_IMAGE_ENCODING_RGB8: + return AV_PIX_FMT_RGB24; + case ROS_IMAGE_ENCODING_BGR8: + return AV_PIX_FMT_BGR24; + case ROS_IMAGE_ENCODING_RGBA8: + return AV_PIX_FMT_RGBA; + case ROS_IMAGE_ENCODING_BGRA8: + return AV_PIX_FMT_BGRA; + case ROS_IMAGE_ENCODING_MONO8: + return AV_PIX_FMT_GRAY8; + default: + return AV_PIX_FMT_NONE; + } +} + +static int video_camera_source_read_ros2_frame( + video_camera_source_t *source, + AVFrame **frame, + uint64_t *timestamp_ns, + int timeout_ms +) { + ros_image_shm_header_t header; + enum AVPixelFormat pixel_format; + AVFrame *input_frame; + + if (source == NULL || source->backend != VIDEO_SOURCE_BACKEND_ROS2 + || frame == NULL || timestamp_ns == NULL) { + errno = EINVAL; + return -1; + } + *frame = NULL; + if (ros_image_shm_read_latest( + &source->ros_shm, + source->ros_frame_buffer, + source->ros_frame_buffer_bytes, + &header, + timeout_ms + ) != 1) { + return -1; + } + pixel_format = ros_encoding_to_av_pixel_format(header.encoding); + if (pixel_format == AV_PIX_FMT_NONE) { + errno = ENOTSUP; + return -1; + } + input_frame = av_frame_alloc(); + if (input_frame == NULL) { + return -1; + } + input_frame->width = (int) header.width; + input_frame->height = (int) header.height; + input_frame->format = pixel_format; + if (av_image_fill_arrays( + input_frame->data, + input_frame->linesize, + source->ros_frame_buffer, + pixel_format, + (int) header.width, + (int) header.height, + 1 + ) < 0) { + av_frame_free(&input_frame); + errno = EINVAL; + return -1; + } + input_frame->linesize[0] = (int) header.stride; + *timestamp_ns = header.timestamp_ns; + *frame = input_frame; + return 0; +} + +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_camera_source_t cameras[2] = { + {.name = "head", .fd = -1}, + {.name = "waist", .fd = -1} + }; + AVCodecContext *decoder = NULL; + AVCodecContext *encoder = NULL; + struct SwsContext *sws_ctx = NULL; + int frame_index = 0; + int rc = -1; + int sws_src_width = 0; + int sws_src_height = 0; + int sws_src_format = -1; + uint32_t hard_backpressure_since_ms = 0; + uint32_t last_soft_drop_log_ms = 0; + uint32_t last_session_poll_ms = 0; + uint32_t last_successful_send_ms = 0; + uint64_t soft_drops_since_last_send = 0; + int have_sent_frame = 0; + int use_ros2 = 0; + const char *gpsd_host = env_or_default("OMNI_GPSD_HOST", "127.0.0.1"); + int gps_buffer_started = 0; + + memset(&sender, 0, sizeof(sender)); + if (stats == NULL) { + errno = EINVAL; + return -1; + } + + video_pipeline_config_init(&defaults); + if (config == NULL) { + config = &defaults; + } + use_ros2 = config->input_mode == VIDEO_INPUT_ROS2; + +#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; + } + + cameras[VIDEO_CAMERA_HEAD].device = config->active_camera == NULL + ? config->camera_device + : config->camera_head_device; + cameras[VIDEO_CAMERA_WAIST].device = config->camera_waist_device; + if (use_ros2) { + if (video_camera_source_start_ros2( + &cameras[VIDEO_CAMERA_HEAD], + config->ros_head_shm, + config->ros_max_frame_bytes + ) < 0) { + video_pipeline_set_errno_error(stats, "failed to open ROS2 head RGB source"); + goto cleanup; + } + if (config->active_camera != NULL + && video_camera_source_start_ros2( + &cameras[VIDEO_CAMERA_WAIST], + config->ros_waist_shm, + config->ros_max_frame_bytes + ) < 0) { + video_pipeline_set_errno_error(stats, "failed to open ROS2 waist RGB source"); + goto cleanup; + } + } else { + if (video_camera_source_start(&cameras[VIDEO_CAMERA_HEAD], config->capture_width, config->capture_height) < 0) { + video_pipeline_set_errno_error(stats, "failed to start head camera"); + goto cleanup; + } + if (config->active_camera != NULL + && video_camera_source_start(&cameras[VIDEO_CAMERA_WAIST], config->capture_width, config->capture_height) < 0) { + video_pipeline_set_errno_error(stats, "failed to start waist camera"); + goto cleanup; + } + } + + if (!use_ros2) { + decoder = create_mjpeg_decoder(config->capture_width, config->capture_height); + } + encoder = create_mjpeg_encoder(config->output_width, config->output_height); + if ((use_ros2 ? encoder == NULL : 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; + } + if (gps_buffer_init(gpsd_host) != 0) { + fprintf(stderr, "[video_pipeline] failed to start GPS buffer using %s:2947\n", gpsd_host); + } else { + gps_buffer_started = 1; + } + + pthread_mutex_lock(&stats->mutex); + stats->connected = 1; + stats->last_error[0] = '\0'; + pthread_mutex_unlock(&stats->mutex); + + 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; + kcp_runtime_stats_t transport_stats; + kcp_runtime_stats_t transport_after_send; + int select_rc; + int should_log_stage = 0; + 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; + video_pipeline_packet_metadata_t packet_metadata; + char reconnect_reason[256]; + int frame_number = frame_index + 1; + uint64_t frame_seq = 0; + uint64_t out_segs_before_send = 0; + uint64_t out_segs_after_send = 0; + uint32_t capture_to_send_ms = 0; + uint64_t capture_timestamp_ns = 0; + int active_camera = config->active_camera == NULL + ? VIDEO_CAMERA_HEAD + : atomic_load(config->active_camera); + video_camera_source_t *active_source; + video_camera_source_t *standby_source; + + if (active_camera != VIDEO_CAMERA_WAIST) { + active_camera = VIDEO_CAMERA_HEAD; + } + active_source = &cameras[active_camera]; + standby_source = config->active_camera == NULL + ? NULL + : &cameras[active_camera == VIDEO_CAMERA_HEAD ? VIDEO_CAMERA_WAIST : VIDEO_CAMERA_HEAD]; + + memset(&transport_stats, 0, sizeof(transport_stats)); + memset(&transport_after_send, 0, sizeof(transport_after_send)); + memset(&packet_metadata, 0, sizeof(packet_metadata)); + reconnect_reason[0] = '\0'; + video_pipeline_report_progress(config); + + if (config->max_frames > 0 && frame_index >= config->max_frames) { + break; + } + memset(&buf, 0, sizeof(buf)); + total_start_ms = video_pipeline_now_ms(); + + if (use_ros2) { + capture_start_ms = video_pipeline_now_ms(); + if (video_camera_source_read_ros2_frame( + active_source, + &decoded_frame, + &capture_timestamp_ns, + 2000 + ) != 0) { + video_pipeline_set_errno_error(stats, "failed waiting for ROS2 RGB frame"); + goto cleanup; + } + capture_end_ms = video_pipeline_now_ms(); + decode_start_ms = capture_end_ms; + decode_end_ms = capture_end_ms; + } else { + FD_ZERO(&fds); + FD_SET(cameras[VIDEO_CAMERA_HEAD].fd, &fds); + if (cameras[VIDEO_CAMERA_WAIST].fd >= 0) { + FD_SET(cameras[VIDEO_CAMERA_WAIST].fd, &fds); + } + timeout.tv_sec = 2; + timeout.tv_usec = 0; + select_rc = select( + (cameras[VIDEO_CAMERA_HEAD].fd > cameras[VIDEO_CAMERA_WAIST].fd + ? cameras[VIDEO_CAMERA_HEAD].fd + : cameras[VIDEO_CAMERA_WAIST].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 (standby_source != NULL && FD_ISSET(standby_source->fd, &fds)) { + video_camera_source_discard_ready(standby_source); + } + if (!FD_ISSET(active_source->fd, &fds)) { + continue; + } + 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(active_source->fd, VIDIOC_DQBUF, &buf) < 0) { + video_pipeline_set_errno_error(stats, "failed to dequeue V4L2 buffer"); + goto cleanup; + } + capture_end_ms = video_pipeline_now_ms(); + decode_start_ms = capture_end_ms; + + if (decode_mjpeg_frame(decoder, (const uint8_t *) active_source->buffers[buf.index].start, (int) buf.bytesused, &decoded_frame) != 0) { + if (config->enable_timing_logs) { + video_pipeline_print_timing_failure(frame_number, "decode"); + } + video_camera_source_requeue(active_source, &buf); + continue; + } + 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); + video_camera_source_requeue(active_source, &buf); + continue; + } + 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); + video_camera_source_requeue(active_source, &buf); + continue; + } + encode_end_ms = video_pipeline_now_ms(); + send_start_ms = encode_end_ms; + + { + gps_video_sample_t gps_sample = get_latest_gps_for_video(); + + packet_metadata.timestamp_ms = capture_timestamp_ns != 0 + ? capture_timestamp_ns / 1000000U + : (uint64_t) get_realtime_ms(); + packet_metadata.latitude = gps_sample.latitude; + packet_metadata.longitude = gps_sample.longitude; + } + + if ( + last_session_poll_ms == 0 + || omni_now_millis32() - last_session_poll_ms >= VIDEO_SESSION_POLL_INTERVAL_MS + ) { + if (video_sender_drain_pending_messages(&sender) != 0) { + video_pipeline_set_errno_error(stats, "failed to poll video session"); + av_frame_free(&decoded_frame); + av_frame_free(&scaled_frame); + av_packet_free(&encoded_pkt); + video_camera_source_requeue(active_source, &buf); + rc = VIDEO_PIPELINE_RUN_RETRY_IMMEDIATE; + goto cleanup; + } + if ( + video_sender_check_session_stale( + &sender, + config, + stats, + &transport_stats, + reconnect_reason, + sizeof(reconnect_reason) + ) != 0 + ) { + if (reconnect_reason[0] == '\0') { + snprintf(reconnect_reason, sizeof(reconnect_reason), "video session stale: poll failed"); + } + video_pipeline_set_error(stats, reconnect_reason); + fprintf(stderr, "[video_pipeline] %s\n", reconnect_reason); + av_frame_free(&decoded_frame); + av_frame_free(&scaled_frame); + av_packet_free(&encoded_pkt); + video_camera_source_requeue(active_source, &buf); + rc = VIDEO_PIPELINE_RUN_RETRY_IMMEDIATE; + goto cleanup; + } + last_session_poll_ms = omni_now_millis32(); + } else { + kcp_client_runtime_stats_snapshot(sender.client, &transport_stats); + } + if (video_sender_hard_backpressure_active(config, &transport_stats)) { + uint32_t now_ms = omni_now_millis32(); + + if (hard_backpressure_since_ms == 0) { + hard_backpressure_since_ms = now_ms; + } + if (now_ms - hard_backpressure_since_ms >= (uint32_t) config->hard_backpressure_hold_ms) { + char reason[128]; + uint32_t backlog_segments = video_sender_backlog_segments(&transport_stats); + + snprintf( + reason, + sizeof(reason), + "hard_reset backlog=%u snd_queue=%u snd_buffer=%u window_pressure=%.1f%% hold_ms=%d", + backlog_segments, + transport_stats.snd_queue, + transport_stats.snd_buffer, + transport_stats.window_pressure_pct, + config->hard_backpressure_hold_ms + ); + video_pipeline_note_backpressure(stats, reason, &transport_stats, 0, 1); + video_pipeline_set_error(stats, reason); + fprintf( + stderr, + "[video_pipeline] backlog hard reset: backlog=%u snd_queue=%u snd_buffer=%u window_pressure=%.1f%% hold_ms=%d\n", + backlog_segments, + transport_stats.snd_queue, + transport_stats.snd_buffer, + transport_stats.window_pressure_pct, + config->hard_backpressure_hold_ms + ); + av_frame_free(&decoded_frame); + av_frame_free(&scaled_frame); + av_packet_free(&encoded_pkt); + video_camera_source_requeue(active_source, &buf); + rc = VIDEO_PIPELINE_RUN_RETRY_IMMEDIATE; + goto cleanup; + } + } else { + hard_backpressure_since_ms = 0; + } + + if (video_sender_soft_backpressure_active(config, &transport_stats)) { + uint32_t now_ms = omni_now_millis32(); + uint32_t backlog_segments = video_sender_backlog_segments(&transport_stats); + char reason[128]; + + snprintf( + reason, + sizeof(reason), + "soft_drop backlog=%u snd_queue=%u snd_buffer=%u window_pressure=%.1f%% threshold=%d", + backlog_segments, + transport_stats.snd_queue, + transport_stats.snd_buffer, + transport_stats.window_pressure_pct, + config->soft_backpressure_segments + ); + video_pipeline_note_backpressure(stats, reason, &transport_stats, 1, 0); + soft_drops_since_last_send += 1; + if (now_ms - last_soft_drop_log_ms >= 1000U) { + fprintf( + stderr, + "[video_pipeline] soft drop: backlog=%u snd_queue=%u snd_buffer=%u window_pressure=%.1f%% threshold=%d\n", + backlog_segments, + transport_stats.snd_queue, + transport_stats.snd_buffer, + transport_stats.window_pressure_pct, + config->soft_backpressure_segments + ); + last_soft_drop_log_ms = now_ms; + } + if ( + have_sent_frame + && config->frame_stall_reconnect_ms > 0 + && now_ms - last_successful_send_ms >= (uint32_t) config->frame_stall_reconnect_ms + ) { + char stall_reason[192]; + + snprintf( + stall_reason, + sizeof(stall_reason), + "video pipeline stalled: no frames sent for %u ms while soft dropping (%llu drops, backlog=%u, srtt=%d ms)", + now_ms - last_successful_send_ms, + (unsigned long long) soft_drops_since_last_send, + backlog_segments, + transport_stats.srtt_ms + ); + video_pipeline_set_error(stats, stall_reason); + fprintf(stderr, "[video_pipeline] %s\n", stall_reason); + av_frame_free(&decoded_frame); + av_frame_free(&scaled_frame); + av_packet_free(&encoded_pkt); + video_camera_source_requeue(active_source, &buf); + rc = VIDEO_PIPELINE_RUN_RETRY_IMMEDIATE; + goto cleanup; + } + av_frame_free(&decoded_frame); + av_frame_free(&scaled_frame); + av_packet_free(&encoded_pkt); + video_camera_source_requeue(active_source, &buf); + continue; + } + + capture_to_send_ms = send_start_ms <= capture_start_ms + ? 0U + : (uint32_t) (send_start_ms - capture_start_ms + 0.5); + packet_metadata.capture_to_send_ms = capture_to_send_ms; + out_segs_before_send = transport_stats.out_segs_total; + + if (video_sender_send_packet(&sender, encoded_pkt, &packet_metadata, &frame_seq) != 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); + video_camera_source_requeue(active_source, &buf); + goto cleanup; + } + send_end_ms = video_pipeline_now_ms(); + should_log_stage = video_stage_logger_should_log(config->stage_logger, frame_seq); + if (should_log_stage) { + kcp_client_runtime_stats_snapshot(sender.client, &transport_after_send); + out_segs_after_send = transport_after_send.out_segs_total; + } else { + transport_after_send = transport_stats; + out_segs_after_send = out_segs_before_send; + } + video_pipeline_note_capture_to_send(stats, capture_to_send_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; + stats->transport = transport_after_send; + pthread_mutex_unlock(&stats->mutex); + have_sent_frame = 1; + last_successful_send_ms = omni_now_millis32(); + soft_drops_since_last_send = 0; + if (should_log_stage) { + video_stage_logger_log_frame( + config->stage_logger, + frame_seq, + 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, + (size_t) encoded_pkt->size, + out_segs_after_send >= out_segs_before_send ? out_segs_after_send - out_segs_before_send : 0U, + video_sender_backlog_segments(&transport_after_send), + transport_after_send.window_pressure_pct, + transport_after_send.srtt_ms + ); + } + 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 (active_source->backend == VIDEO_SOURCE_BACKEND_V4L2 + && ioctl(active_source->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 (gps_buffer_started) { + gps_buffer_cleanup(); + } + video_sender_close(&sender); + if (encoder != NULL) { + avcodec_free_context(&encoder); + } + if (decoder != NULL) { + avcodec_free_context(&decoder); + } + sws_freeContext(sws_ctx); + video_camera_source_cleanup(&cameras[VIDEO_CAMERA_HEAD]); + video_camera_source_cleanup(&cameras[VIDEO_CAMERA_WAIST]); + return rc; +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/src/video_pipeline_gps.c b/robot/ros2/OmniSocketGo_robot_ros/src/video_pipeline_gps.c new file mode 100644 index 0000000..e60d6ef --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/src/video_pipeline_gps.c @@ -0,0 +1,925 @@ +#include "video_pipeline.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#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); +} + +static void video_pipeline_report_progress(const video_pipeline_config_t *config) { + if (config == NULL || config->progress_callback == NULL) { + return; + } + config->progress_callback(config->progress_context); +} + +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; + config->stats_logger = NULL; + config->stats_interval_ms = 1000; +} + +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); + config->stats_interval_ms = env_int_or_default("BLITZ_KCP_STATS_INTERVAL_MS", config->stats_interval_ms); +} + +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, + config->stats_logger, + config->stats_interval_ms + ); + if (sender->client == NULL) { + return -1; + } + return 0; +} + +static int video_sender_drain_pending_messages(video_sender_t *sender) { + if (sender == NULL || sender->client == NULL) { + errno = EINVAL; + return -1; + } + + for (;;) { + message_t msg; + int rc; + + protocol_message_init(&msg); + rc = kcp_client_receive_timed(sender->client, &msg, 1); + if (rc == 1) { + protocol_message_clear(&msg); + return 0; + } + if (rc != 0) { + protocol_message_clear(&msg); + return -1; + } + + // Drain unread server errors so an offline receiver cannot back up the reverse KCP stream. + protocol_message_clear(&msg); + } +} + +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, ×tamp, sizeof(timestamp)); + rc = kcp_client_send_binary(sender->client, sender->target_peer, payload, payload_len); + if (rc != 0) { + return rc; + } + rc = video_sender_drain_pending_messages(sender); + 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; + + video_pipeline_report_progress(config); + + 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; +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/start-robot-lan.sh b/robot/ros2/OmniSocketGo_robot_ros/start-robot-lan.sh new file mode 100644 index 0000000..5f32aae --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/start-robot-lan.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env bash +set -euo pipefail + +PROJECT_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +cd "${PROJECT_ROOT}" + +if [[ ! -x bin/b_side_omnid ]]; then + echo "[start-robot-lan] building bin/b_side_omnid" >&2 + make b_side_omnid +fi + +exec bash scripts/dev/start-b-side-omnid.sh diff --git a/robot/ros2/OmniSocketGo_robot_ros/third_party/cjson/cJSON.c b/robot/ros2/OmniSocketGo_robot_ros/third_party/cjson/cJSON.c new file mode 100644 index 0000000..702ea61 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/third_party/cjson/cJSON.c @@ -0,0 +1,3302 @@ +/* + Copyright (c) 2009-2017 Dave Gamble and cJSON contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +/* cJSON */ +/* JSON parser in C. */ + +/* disable warnings about old C89 functions in MSVC */ +#if !defined(_CRT_SECURE_NO_DEPRECATE) && defined(_MSC_VER) +#define _CRT_SECURE_NO_DEPRECATE +#endif + +#ifdef __GNUC__ +#pragma GCC visibility push(default) +#endif +#if defined(_MSC_VER) +#pragma warning(push) +/* disable warning about single line comments in system headers */ +#pragma warning(disable : 4001) +#endif + +#include +#include +#include +#include +#include +#include +#include + +#ifdef ENABLE_LOCALES +#include +#endif + +#if defined(_MSC_VER) +#pragma warning(pop) +#endif +#ifdef __GNUC__ +#pragma GCC visibility pop +#endif + +#include "cJSON.h" + +/* define our own boolean type */ +#ifdef true +#undef true +#endif +#define true ((cJSON_bool)1) + +#ifdef false +#undef false +#endif +#define false ((cJSON_bool)0) + +/* define isnan and isinf for ANSI C, if in C99 or above, isnan and isinf has been defined in math.h */ +#ifndef isinf +#define isinf(d) (isnan((d - d)) && !isnan(d)) +#endif +#ifndef isnan +#define isnan(d) (d != d) +#endif + +#ifndef NAN +#ifdef _WIN32 +#define NAN sqrt(-1.0) +#else +#define NAN 0.0 / 0.0 +#endif +#endif + +typedef struct +{ + const unsigned char *json; + size_t position; +} error; +static error global_error = {NULL, 0}; + +CJSON_PUBLIC(const char *) +cJSON_GetErrorPtr(void) +{ + return (const char *)(global_error.json + global_error.position); +} + +CJSON_PUBLIC(char *) +cJSON_GetStringValue(const cJSON *const item) +{ + if (!cJSON_IsString(item)) + { + return NULL; + } + + return item->valuestring; +} + +CJSON_PUBLIC(double) +cJSON_GetNumberValue(const cJSON *const item) +{ + if (!cJSON_IsNumber(item)) + { + return (double)NAN; + } + + return item->valuedouble; +} + +/* This is a safeguard to prevent copy-pasters from using incompatible C and header files */ +#if (CJSON_VERSION_MAJOR != 1) || (CJSON_VERSION_MINOR != 7) || (CJSON_VERSION_PATCH != 19) +#error cJSON.h and cJSON.c have different versions. Make sure that both have the same. +#endif + +CJSON_PUBLIC(const char *) +cJSON_Version(void) +{ + static char version[15]; + sprintf(version, "%i.%i.%i", CJSON_VERSION_MAJOR, CJSON_VERSION_MINOR, CJSON_VERSION_PATCH); + + return version; +} + +/* Case insensitive string comparison, doesn't consider two NULL pointers equal though */ +static int case_insensitive_strcmp(const unsigned char *string1, const unsigned char *string2) +{ + if ((string1 == NULL) || (string2 == NULL)) + { + return 1; + } + + if (string1 == string2) + { + return 0; + } + + for (; tolower(*string1) == tolower(*string2); (void)string1++, string2++) + { + if (*string1 == '\0') + { + return 0; + } + } + + return tolower(*string1) - tolower(*string2); +} + +typedef struct internal_hooks +{ + void *(CJSON_CDECL *allocate)(size_t size); + void(CJSON_CDECL *deallocate)(void *pointer); + void *(CJSON_CDECL *reallocate)(void *pointer, size_t size); +} internal_hooks; + +#if defined(_MSC_VER) +/* work around MSVC error C2322: '...' address of dllimport '...' is not static */ +static void *CJSON_CDECL internal_malloc(size_t size) +{ + return malloc(size); +} +static void CJSON_CDECL internal_free(void *pointer) +{ + free(pointer); +} +static void *CJSON_CDECL internal_realloc(void *pointer, size_t size) +{ + return realloc(pointer, size); +} +#else +#define internal_malloc malloc +#define internal_free free +#define internal_realloc realloc +#endif + +/* strlen of character literals resolved at compile time */ +#define static_strlen(string_literal) (sizeof(string_literal) - sizeof("")) + +static internal_hooks global_hooks = {internal_malloc, internal_free, internal_realloc}; + +static unsigned char *cJSON_strdup(const unsigned char *string, const internal_hooks *const hooks) +{ + size_t length = 0; + unsigned char *copy = NULL; + + if (string == NULL) + { + return NULL; + } + + length = strlen((const char *)string) + sizeof(""); + copy = (unsigned char *)hooks->allocate(length); + if (copy == NULL) + { + return NULL; + } + memcpy(copy, string, length); + + return copy; +} + +CJSON_PUBLIC(void) +cJSON_InitHooks(cJSON_Hooks *hooks) +{ + if (hooks == NULL) + { + /* Reset hooks */ + global_hooks.allocate = malloc; + global_hooks.deallocate = free; + global_hooks.reallocate = realloc; + return; + } + + global_hooks.allocate = malloc; + if (hooks->malloc_fn != NULL) + { + global_hooks.allocate = hooks->malloc_fn; + } + + global_hooks.deallocate = free; + if (hooks->free_fn != NULL) + { + global_hooks.deallocate = hooks->free_fn; + } + + /* use realloc only if both free and malloc are used */ + global_hooks.reallocate = NULL; + if ((global_hooks.allocate == malloc) && (global_hooks.deallocate == free)) + { + global_hooks.reallocate = realloc; + } +} + +/* Internal constructor. */ +static cJSON *cJSON_New_Item(const internal_hooks *const hooks) +{ + cJSON *node = (cJSON *)hooks->allocate(sizeof(cJSON)); + if (node) + { + memset(node, '\0', sizeof(cJSON)); + } + + return node; +} + +/* Delete a cJSON structure. */ +CJSON_PUBLIC(void) +cJSON_Delete(cJSON *item) +{ + cJSON *next = NULL; + while (item != NULL) + { + next = item->next; + if (!(item->type & cJSON_IsReference) && (item->child != NULL)) + { + cJSON_Delete(item->child); + } + if (!(item->type & cJSON_IsReference) && (item->valuestring != NULL)) + { + global_hooks.deallocate(item->valuestring); + item->valuestring = NULL; + } + if (!(item->type & cJSON_StringIsConst) && (item->string != NULL)) + { + global_hooks.deallocate(item->string); + item->string = NULL; + } + global_hooks.deallocate(item); + item = next; + } +} + +/* get the decimal point character of the current locale */ +static unsigned char get_decimal_point(void) +{ +#ifdef ENABLE_LOCALES + struct lconv *lconv = localeconv(); + return (unsigned char)lconv->decimal_point[0]; +#else + return '.'; +#endif +} + +typedef struct +{ + const unsigned char *content; + size_t length; + size_t offset; + size_t depth; /* How deeply nested (in arrays/objects) is the input at the current offset. */ + internal_hooks hooks; +} parse_buffer; + +/* check if the given size is left to read in a given parse buffer (starting with 1) */ +#define can_read(buffer, size) ((buffer != NULL) && (((buffer)->offset + size) <= (buffer)->length)) +/* check if the buffer can be accessed at the given index (starting with 0) */ +#define can_access_at_index(buffer, index) ((buffer != NULL) && (((buffer)->offset + index) < (buffer)->length)) +#define cannot_access_at_index(buffer, index) (!can_access_at_index(buffer, index)) +/* get a pointer to the buffer at the position */ +#define buffer_at_offset(buffer) ((buffer)->content + (buffer)->offset) + +/* Parse the input text to generate a number, and populate the result into item. */ +static cJSON_bool parse_number(cJSON *const item, parse_buffer *const input_buffer) +{ + double number = 0; + unsigned char *after_end = NULL; + unsigned char *number_c_string; + unsigned char decimal_point = get_decimal_point(); + size_t i = 0; + size_t number_string_length = 0; + cJSON_bool has_decimal_point = false; + + if ((input_buffer == NULL) || (input_buffer->content == NULL)) + { + return false; + } + + /* copy the number into a temporary buffer and replace '.' with the decimal point + * of the current locale (for strtod) + * This also takes care of '\0' not necessarily being available for marking the end of the input */ + for (i = 0; can_access_at_index(input_buffer, i); i++) + { + switch (buffer_at_offset(input_buffer)[i]) + { + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + case '+': + case '-': + case 'e': + case 'E': + number_string_length++; + break; + + case '.': + number_string_length++; + has_decimal_point = true; + break; + + default: + goto loop_end; + } + } +loop_end: + /* malloc for temporary buffer, add 1 for '\0' */ + number_c_string = (unsigned char *)input_buffer->hooks.allocate(number_string_length + 1); + if (number_c_string == NULL) + { + return false; /* allocation failure */ + } + + memcpy(number_c_string, buffer_at_offset(input_buffer), number_string_length); + number_c_string[number_string_length] = '\0'; + + if (has_decimal_point) + { + for (i = 0; i < number_string_length; i++) + { + if (number_c_string[i] == '.') + { + /* replace '.' with the decimal point of the current locale (for strtod) */ + number_c_string[i] = decimal_point; + } + } + } + + number = strtod((const char *)number_c_string, (char **)&after_end); + if (number_c_string == after_end) + { + /* free the temporary buffer */ + input_buffer->hooks.deallocate(number_c_string); + return false; /* parse_error */ + } + + item->valuedouble = number; + + /* use saturation in case of overflow */ + if (number >= INT_MAX) + { + item->valueint = INT_MAX; + } + else if (number <= (double)INT_MIN) + { + item->valueint = INT_MIN; + } + else + { + item->valueint = (int)number; + } + + item->type = cJSON_Number; + + input_buffer->offset += (size_t)(after_end - number_c_string); + /* free the temporary buffer */ + input_buffer->hooks.deallocate(number_c_string); + return true; +} + +/* don't ask me, but the original cJSON_SetNumberValue returns an integer or double */ +CJSON_PUBLIC(double) +cJSON_SetNumberHelper(cJSON *object, double number) +{ + if (object == NULL) + { + return (double)NAN; + } + + if (number >= INT_MAX) + { + object->valueint = INT_MAX; + } + else if (number <= (double)INT_MIN) + { + object->valueint = INT_MIN; + } + else + { + object->valueint = (int)number; + } + + return object->valuedouble = number; +} + +/* Note: when passing a NULL valuestring, cJSON_SetValuestring treats this as an error and return NULL */ +CJSON_PUBLIC(char *) +cJSON_SetValuestring(cJSON *object, const char *valuestring) +{ + char *copy = NULL; + size_t v1_len; + size_t v2_len; + /* if object's type is not cJSON_String or is cJSON_IsReference, it should not set valuestring */ + if ((object == NULL) || !(object->type & cJSON_String) || (object->type & cJSON_IsReference)) + { + return NULL; + } + /* return NULL if the object is corrupted or valuestring is NULL */ + if (object->valuestring == NULL || valuestring == NULL) + { + return NULL; + } + + v1_len = strlen(valuestring); + v2_len = strlen(object->valuestring); + + if (v1_len <= v2_len) + { + /* strcpy does not handle overlapping string: [X1, X2] [Y1, Y2] => X2 < Y1 or Y2 < X1 */ + if (!(valuestring + v1_len < object->valuestring || object->valuestring + v2_len < valuestring)) + { + return NULL; + } + strcpy(object->valuestring, valuestring); + return object->valuestring; + } + copy = (char *)cJSON_strdup((const unsigned char *)valuestring, &global_hooks); + if (copy == NULL) + { + return NULL; + } + if (object->valuestring != NULL) + { + cJSON_free(object->valuestring); + } + object->valuestring = copy; + + return copy; +} + +typedef struct +{ + unsigned char *buffer; + size_t length; + size_t offset; + size_t depth; /* current nesting depth (for formatted printing) */ + cJSON_bool noalloc; + cJSON_bool format; /* is this print a formatted print */ + internal_hooks hooks; +} printbuffer; + +/* realloc printbuffer if necessary to have at least "needed" bytes more */ +static unsigned char *ensure(printbuffer *const p, size_t needed) +{ + unsigned char *newbuffer = NULL; + size_t newsize = 0; + + if ((p == NULL) || (p->buffer == NULL)) + { + return NULL; + } + + if ((p->length > 0) && (p->offset >= p->length)) + { + /* make sure that offset is valid */ + return NULL; + } + + if (needed > INT_MAX) + { + /* sizes bigger than INT_MAX are currently not supported */ + return NULL; + } + + needed += p->offset + 1; + if (needed <= p->length) + { + return p->buffer + p->offset; + } + + if (p->noalloc) + { + return NULL; + } + + /* calculate new buffer size */ + if (needed > (INT_MAX / 2)) + { + /* overflow of int, use INT_MAX if possible */ + if (needed <= INT_MAX) + { + newsize = INT_MAX; + } + else + { + return NULL; + } + } + else + { + newsize = needed * 2; + } + + if (p->hooks.reallocate != NULL) + { + /* reallocate with realloc if available */ + newbuffer = (unsigned char *)p->hooks.reallocate(p->buffer, newsize); + if (newbuffer == NULL) + { + p->hooks.deallocate(p->buffer); + p->length = 0; + p->buffer = NULL; + + return NULL; + } + } + else + { + /* otherwise reallocate manually */ + newbuffer = (unsigned char *)p->hooks.allocate(newsize); + if (!newbuffer) + { + p->hooks.deallocate(p->buffer); + p->length = 0; + p->buffer = NULL; + + return NULL; + } + + memcpy(newbuffer, p->buffer, p->offset + 1); + p->hooks.deallocate(p->buffer); + } + p->length = newsize; + p->buffer = newbuffer; + + return newbuffer + p->offset; +} + +/* calculate the new length of the string in a printbuffer and update the offset */ +static void update_offset(printbuffer *const buffer) +{ + const unsigned char *buffer_pointer = NULL; + if ((buffer == NULL) || (buffer->buffer == NULL)) + { + return; + } + buffer_pointer = buffer->buffer + buffer->offset; + + buffer->offset += strlen((const char *)buffer_pointer); +} + +/* securely comparison of floating-point variables */ +static cJSON_bool compare_double(double a, double b) +{ + double maxVal = fabs(a) > fabs(b) ? fabs(a) : fabs(b); + return (fabs(a - b) <= maxVal * DBL_EPSILON); +} + +/* Render the number nicely from the given item into a string. */ +static cJSON_bool print_number(const cJSON *const item, printbuffer *const output_buffer) +{ + unsigned char *output_pointer = NULL; + double d = item->valuedouble; + int length = 0; + size_t i = 0; + unsigned char number_buffer[26] = {0}; /* temporary buffer to print the number into */ + unsigned char decimal_point = get_decimal_point(); + double test = 0.0; + + if (output_buffer == NULL) + { + return false; + } + + /* This checks for NaN and Infinity */ + if (isnan(d) || isinf(d)) + { + length = sprintf((char *)number_buffer, "null"); + } + else if (d == (double)item->valueint) + { + length = sprintf((char *)number_buffer, "%d", item->valueint); + } + else + { + /* Try 15 decimal places of precision to avoid nonsignificant nonzero digits */ + length = sprintf((char *)number_buffer, "%1.15g", d); + + /* Check whether the original double can be recovered */ + if ((sscanf((char *)number_buffer, "%lg", &test) != 1) || !compare_double((double)test, d)) + { + /* If not, print with 17 decimal places of precision */ + length = sprintf((char *)number_buffer, "%1.17g", d); + } + } + + /* sprintf failed or buffer overrun occurred */ + if ((length < 0) || (length > (int)(sizeof(number_buffer) - 1))) + { + return false; + } + + /* reserve appropriate space in the output */ + output_pointer = ensure(output_buffer, (size_t)length + sizeof("")); + if (output_pointer == NULL) + { + return false; + } + + /* copy the printed number to the output and replace locale + * dependent decimal point with '.' */ + for (i = 0; i < ((size_t)length); i++) + { + if (number_buffer[i] == decimal_point) + { + output_pointer[i] = '.'; + continue; + } + + output_pointer[i] = number_buffer[i]; + } + output_pointer[i] = '\0'; + + output_buffer->offset += (size_t)length; + + return true; +} + +/* parse 4 digit hexadecimal number */ +static unsigned parse_hex4(const unsigned char *const input) +{ + unsigned int h = 0; + size_t i = 0; + + for (i = 0; i < 4; i++) + { + /* parse digit */ + if ((input[i] >= '0') && (input[i] <= '9')) + { + h += (unsigned int)input[i] - '0'; + } + else if ((input[i] >= 'A') && (input[i] <= 'F')) + { + h += (unsigned int)10 + input[i] - 'A'; + } + else if ((input[i] >= 'a') && (input[i] <= 'f')) + { + h += (unsigned int)10 + input[i] - 'a'; + } + else /* invalid */ + { + return 0; + } + + if (i < 3) + { + /* shift left to make place for the next nibble */ + h = h << 4; + } + } + + return h; +} + +/* converts a UTF-16 literal to UTF-8 + * A literal can be one or two sequences of the form \uXXXX */ +static unsigned char utf16_literal_to_utf8(const unsigned char *const input_pointer, const unsigned char *const input_end, unsigned char **output_pointer) +{ + long unsigned int codepoint = 0; + unsigned int first_code = 0; + const unsigned char *first_sequence = input_pointer; + unsigned char utf8_length = 0; + unsigned char utf8_position = 0; + unsigned char sequence_length = 0; + unsigned char first_byte_mark = 0; + + if ((input_end - first_sequence) < 6) + { + /* input ends unexpectedly */ + goto fail; + } + + /* get the first utf16 sequence */ + first_code = parse_hex4(first_sequence + 2); + + /* check that the code is valid */ + if (((first_code >= 0xDC00) && (first_code <= 0xDFFF))) + { + goto fail; + } + + /* UTF16 surrogate pair */ + if ((first_code >= 0xD800) && (first_code <= 0xDBFF)) + { + const unsigned char *second_sequence = first_sequence + 6; + unsigned int second_code = 0; + sequence_length = 12; /* \uXXXX\uXXXX */ + + if ((input_end - second_sequence) < 6) + { + /* input ends unexpectedly */ + goto fail; + } + + if ((second_sequence[0] != '\\') || (second_sequence[1] != 'u')) + { + /* missing second half of the surrogate pair */ + goto fail; + } + + /* get the second utf16 sequence */ + second_code = parse_hex4(second_sequence + 2); + /* check that the code is valid */ + if ((second_code < 0xDC00) || (second_code > 0xDFFF)) + { + /* invalid second half of the surrogate pair */ + goto fail; + } + + /* calculate the unicode codepoint from the surrogate pair */ + codepoint = 0x10000 + (((first_code & 0x3FF) << 10) | (second_code & 0x3FF)); + } + else + { + sequence_length = 6; /* \uXXXX */ + codepoint = first_code; + } + + /* encode as UTF-8 + * takes at maximum 4 bytes to encode: + * 11110xxx 10xxxxxx 10xxxxxx 10xxxxxx */ + if (codepoint < 0x80) + { + /* normal ascii, encoding 0xxxxxxx */ + utf8_length = 1; + } + else if (codepoint < 0x800) + { + /* two bytes, encoding 110xxxxx 10xxxxxx */ + utf8_length = 2; + first_byte_mark = 0xC0; /* 11000000 */ + } + else if (codepoint < 0x10000) + { + /* three bytes, encoding 1110xxxx 10xxxxxx 10xxxxxx */ + utf8_length = 3; + first_byte_mark = 0xE0; /* 11100000 */ + } + else if (codepoint <= 0x10FFFF) + { + /* four bytes, encoding 1110xxxx 10xxxxxx 10xxxxxx 10xxxxxx */ + utf8_length = 4; + first_byte_mark = 0xF0; /* 11110000 */ + } + else + { + /* invalid unicode codepoint */ + goto fail; + } + + /* encode as utf8 */ + for (utf8_position = (unsigned char)(utf8_length - 1); utf8_position > 0; utf8_position--) + { + /* 10xxxxxx */ + (*output_pointer)[utf8_position] = (unsigned char)((codepoint | 0x80) & 0xBF); + codepoint >>= 6; + } + /* encode first byte */ + if (utf8_length > 1) + { + (*output_pointer)[0] = (unsigned char)((codepoint | first_byte_mark) & 0xFF); + } + else + { + (*output_pointer)[0] = (unsigned char)(codepoint & 0x7F); + } + + *output_pointer += utf8_length; + + return sequence_length; + +fail: + return 0; +} + +/* Parse the input text into an unescaped cinput, and populate item. */ +static cJSON_bool parse_string(cJSON *const item, parse_buffer *const input_buffer) +{ + const unsigned char *input_pointer = buffer_at_offset(input_buffer) + 1; + const unsigned char *input_end = buffer_at_offset(input_buffer) + 1; + unsigned char *output_pointer = NULL; + unsigned char *output = NULL; + + /* not a string */ + if (buffer_at_offset(input_buffer)[0] != '\"') + { + goto fail; + } + + { + /* calculate approximate size of the output (overestimate) */ + size_t allocation_length = 0; + size_t skipped_bytes = 0; + while (((size_t)(input_end - input_buffer->content) < input_buffer->length) && (*input_end != '\"')) + { + /* is escape sequence */ + if (input_end[0] == '\\') + { + if ((size_t)(input_end + 1 - input_buffer->content) >= input_buffer->length) + { + /* prevent buffer overflow when last input character is a backslash */ + goto fail; + } + skipped_bytes++; + input_end++; + } + input_end++; + } + if (((size_t)(input_end - input_buffer->content) >= input_buffer->length) || (*input_end != '\"')) + { + goto fail; /* string ended unexpectedly */ + } + + /* This is at most how much we need for the output */ + allocation_length = (size_t)(input_end - buffer_at_offset(input_buffer)) - skipped_bytes; + output = (unsigned char *)input_buffer->hooks.allocate(allocation_length + sizeof("")); + if (output == NULL) + { + goto fail; /* allocation failure */ + } + } + + output_pointer = output; + /* loop through the string literal */ + while (input_pointer < input_end) + { + if (*input_pointer != '\\') + { + *output_pointer++ = *input_pointer++; + } + /* escape sequence */ + else + { + unsigned char sequence_length = 2; + if ((input_end - input_pointer) < 1) + { + goto fail; + } + + switch (input_pointer[1]) + { + case 'b': + *output_pointer++ = '\b'; + break; + case 'f': + *output_pointer++ = '\f'; + break; + case 'n': + *output_pointer++ = '\n'; + break; + case 'r': + *output_pointer++ = '\r'; + break; + case 't': + *output_pointer++ = '\t'; + break; + case '\"': + case '\\': + case '/': + *output_pointer++ = input_pointer[1]; + break; + + /* UTF-16 literal */ + case 'u': + sequence_length = utf16_literal_to_utf8(input_pointer, input_end, &output_pointer); + if (sequence_length == 0) + { + /* failed to convert UTF16-literal to UTF-8 */ + goto fail; + } + break; + + default: + goto fail; + } + input_pointer += sequence_length; + } + } + + /* zero terminate the output */ + *output_pointer = '\0'; + + item->type = cJSON_String; + item->valuestring = (char *)output; + + input_buffer->offset = (size_t)(input_end - input_buffer->content); + input_buffer->offset++; + + return true; + +fail: + if (output != NULL) + { + input_buffer->hooks.deallocate(output); + output = NULL; + } + + if (input_pointer != NULL) + { + input_buffer->offset = (size_t)(input_pointer - input_buffer->content); + } + + return false; +} + +/* Render the cstring provided to an escaped version that can be printed. */ +static cJSON_bool print_string_ptr(const unsigned char *const input, printbuffer *const output_buffer) +{ + const unsigned char *input_pointer = NULL; + unsigned char *output = NULL; + unsigned char *output_pointer = NULL; + size_t output_length = 0; + /* numbers of additional characters needed for escaping */ + size_t escape_characters = 0; + + if (output_buffer == NULL) + { + return false; + } + + /* empty string */ + if (input == NULL) + { + output = ensure(output_buffer, sizeof("\"\"")); + if (output == NULL) + { + return false; + } + strcpy((char *)output, "\"\""); + + return true; + } + + /* set "flag" to 1 if something needs to be escaped */ + for (input_pointer = input; *input_pointer; input_pointer++) + { + switch (*input_pointer) + { + case '\"': + case '\\': + case '\b': + case '\f': + case '\n': + case '\r': + case '\t': + /* one character escape sequence */ + escape_characters++; + break; + default: + if (*input_pointer < 32) + { + /* UTF-16 escape sequence uXXXX */ + escape_characters += 5; + } + break; + } + } + output_length = (size_t)(input_pointer - input) + escape_characters; + + output = ensure(output_buffer, output_length + sizeof("\"\"")); + if (output == NULL) + { + return false; + } + + /* no characters have to be escaped */ + if (escape_characters == 0) + { + output[0] = '\"'; + memcpy(output + 1, input, output_length); + output[output_length + 1] = '\"'; + output[output_length + 2] = '\0'; + + return true; + } + + output[0] = '\"'; + output_pointer = output + 1; + /* copy the string */ + for (input_pointer = input; *input_pointer != '\0'; (void)input_pointer++, output_pointer++) + { + if ((*input_pointer > 31) && (*input_pointer != '\"') && (*input_pointer != '\\')) + { + /* normal character, copy */ + *output_pointer = *input_pointer; + } + else + { + /* character needs to be escaped */ + *output_pointer++ = '\\'; + switch (*input_pointer) + { + case '\\': + *output_pointer = '\\'; + break; + case '\"': + *output_pointer = '\"'; + break; + case '\b': + *output_pointer = 'b'; + break; + case '\f': + *output_pointer = 'f'; + break; + case '\n': + *output_pointer = 'n'; + break; + case '\r': + *output_pointer = 'r'; + break; + case '\t': + *output_pointer = 't'; + break; + default: + /* escape and print as unicode codepoint */ + sprintf((char *)output_pointer, "u%04x", *input_pointer); + output_pointer += 4; + break; + } + } + } + output[output_length + 1] = '\"'; + output[output_length + 2] = '\0'; + + return true; +} + +/* Invoke print_string_ptr (which is useful) on an item. */ +static cJSON_bool print_string(const cJSON *const item, printbuffer *const p) +{ + return print_string_ptr((unsigned char *)item->valuestring, p); +} + +/* Predeclare these prototypes. */ +static cJSON_bool parse_value(cJSON *const item, parse_buffer *const input_buffer); +static cJSON_bool print_value(const cJSON *const item, printbuffer *const output_buffer); +static cJSON_bool parse_array(cJSON *const item, parse_buffer *const input_buffer); +static cJSON_bool print_array(const cJSON *const item, printbuffer *const output_buffer); +static cJSON_bool parse_object(cJSON *const item, parse_buffer *const input_buffer); +static cJSON_bool print_object(const cJSON *const item, printbuffer *const output_buffer); + +/* Utility to jump whitespace and cr/lf */ +static parse_buffer *buffer_skip_whitespace(parse_buffer *const buffer) +{ + if ((buffer == NULL) || (buffer->content == NULL)) + { + return NULL; + } + + if (cannot_access_at_index(buffer, 0)) + { + return buffer; + } + + while (can_access_at_index(buffer, 0) && (buffer_at_offset(buffer)[0] <= 32)) + { + buffer->offset++; + } + + if (buffer->offset == buffer->length) + { + buffer->offset--; + } + + return buffer; +} + +/* skip the UTF-8 BOM (byte order mark) if it is at the beginning of a buffer */ +static parse_buffer *skip_utf8_bom(parse_buffer *const buffer) +{ + if ((buffer == NULL) || (buffer->content == NULL) || (buffer->offset != 0)) + { + return NULL; + } + + if (can_access_at_index(buffer, 4) && (strncmp((const char *)buffer_at_offset(buffer), "\xEF\xBB\xBF", 3) == 0)) + { + buffer->offset += 3; + } + + return buffer; +} + +CJSON_PUBLIC(cJSON *) +cJSON_ParseWithOpts(const char *value, const char **return_parse_end, cJSON_bool require_null_terminated) +{ + size_t buffer_length; + + if (NULL == value) + { + return NULL; + } + + /* Adding null character size due to require_null_terminated. */ + buffer_length = strlen(value) + sizeof(""); + + return cJSON_ParseWithLengthOpts(value, buffer_length, return_parse_end, require_null_terminated); +} + +/* Parse an object - create a new root, and populate. */ +CJSON_PUBLIC(cJSON *) +cJSON_ParseWithLengthOpts(const char *value, size_t buffer_length, const char **return_parse_end, cJSON_bool require_null_terminated) +{ + parse_buffer buffer = {0, 0, 0, 0, {0, 0, 0}}; + cJSON *item = NULL; + + /* reset error position */ + global_error.json = NULL; + global_error.position = 0; + + if (value == NULL || 0 == buffer_length) + { + goto fail; + } + + buffer.content = (const unsigned char *)value; + buffer.length = buffer_length; + buffer.offset = 0; + buffer.hooks = global_hooks; + + item = cJSON_New_Item(&global_hooks); + if (item == NULL) /* memory fail */ + { + goto fail; + } + + if (!parse_value(item, buffer_skip_whitespace(skip_utf8_bom(&buffer)))) + { + /* parse failure. ep is set. */ + goto fail; + } + + /* if we require null-terminated JSON without appended garbage, skip and then check for a null terminator */ + if (require_null_terminated) + { + buffer_skip_whitespace(&buffer); + if ((buffer.offset >= buffer.length) || buffer_at_offset(&buffer)[0] != '\0') + { + goto fail; + } + } + if (return_parse_end) + { + *return_parse_end = (const char *)buffer_at_offset(&buffer); + } + + return item; + +fail: + if (item != NULL) + { + cJSON_Delete(item); + } + + if (value != NULL) + { + error local_error; + local_error.json = (const unsigned char *)value; + local_error.position = 0; + + if (buffer.offset < buffer.length) + { + local_error.position = buffer.offset; + } + else if (buffer.length > 0) + { + local_error.position = buffer.length - 1; + } + + if (return_parse_end != NULL) + { + *return_parse_end = (const char *)local_error.json + local_error.position; + } + + global_error = local_error; + } + + return NULL; +} + +/* Default options for cJSON_Parse */ +CJSON_PUBLIC(cJSON *) +cJSON_Parse(const char *value) +{ + return cJSON_ParseWithOpts(value, 0, 0); +} + +CJSON_PUBLIC(cJSON *) +cJSON_ParseWithLength(const char *value, size_t buffer_length) +{ + return cJSON_ParseWithLengthOpts(value, buffer_length, 0, 0); +} + +#define cjson_min(a, b) (((a) < (b)) ? (a) : (b)) + +static unsigned char *print(const cJSON *const item, cJSON_bool format, const internal_hooks *const hooks) +{ + static const size_t default_buffer_size = 256; + printbuffer buffer[1]; + unsigned char *printed = NULL; + + memset(buffer, 0, sizeof(buffer)); + + /* create buffer */ + buffer->buffer = (unsigned char *)hooks->allocate(default_buffer_size); + buffer->length = default_buffer_size; + buffer->format = format; + buffer->hooks = *hooks; + if (buffer->buffer == NULL) + { + goto fail; + } + + /* print the value */ + if (!print_value(item, buffer)) + { + goto fail; + } + update_offset(buffer); + + /* check if reallocate is available */ + if (hooks->reallocate != NULL) + { + printed = (unsigned char *)hooks->reallocate(buffer->buffer, buffer->offset + 1); + if (printed == NULL) + { + goto fail; + } + buffer->buffer = NULL; + } + else /* otherwise copy the JSON over to a new buffer */ + { + printed = (unsigned char *)hooks->allocate(buffer->offset + 1); + if (printed == NULL) + { + goto fail; + } + memcpy(printed, buffer->buffer, cjson_min(buffer->length, buffer->offset + 1)); + printed[buffer->offset] = '\0'; /* just to be sure */ + + /* free the buffer */ + hooks->deallocate(buffer->buffer); + buffer->buffer = NULL; + } + + return printed; + +fail: + if (buffer->buffer != NULL) + { + hooks->deallocate(buffer->buffer); + buffer->buffer = NULL; + } + + if (printed != NULL) + { + hooks->deallocate(printed); + printed = NULL; + } + + return NULL; +} + +/* Render a cJSON item/entity/structure to text. */ +CJSON_PUBLIC(char *) +cJSON_Print(const cJSON *item) +{ + return (char *)print(item, true, &global_hooks); +} + +CJSON_PUBLIC(char *) +cJSON_PrintUnformatted(const cJSON *item) +{ + return (char *)print(item, false, &global_hooks); +} + +CJSON_PUBLIC(char *) +cJSON_PrintBuffered(const cJSON *item, int prebuffer, cJSON_bool fmt) +{ + printbuffer p = {0, 0, 0, 0, 0, 0, {0, 0, 0}}; + + if (prebuffer < 0) + { + return NULL; + } + + p.buffer = (unsigned char *)global_hooks.allocate((size_t)prebuffer); + if (!p.buffer) + { + return NULL; + } + + p.length = (size_t)prebuffer; + p.offset = 0; + p.noalloc = false; + p.format = fmt; + p.hooks = global_hooks; + + if (!print_value(item, &p)) + { + global_hooks.deallocate(p.buffer); + p.buffer = NULL; + return NULL; + } + + return (char *)p.buffer; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_PrintPreallocated(cJSON *item, char *buffer, const int length, const cJSON_bool format) +{ + printbuffer p = {0, 0, 0, 0, 0, 0, {0, 0, 0}}; + + if ((length < 0) || (buffer == NULL)) + { + return false; + } + + p.buffer = (unsigned char *)buffer; + p.length = (size_t)length; + p.offset = 0; + p.noalloc = true; + p.format = format; + p.hooks = global_hooks; + + return print_value(item, &p); +} + +/* Parser core - when encountering text, process appropriately. */ +static cJSON_bool parse_value(cJSON *const item, parse_buffer *const input_buffer) +{ + if ((input_buffer == NULL) || (input_buffer->content == NULL)) + { + return false; /* no input */ + } + + /* parse the different types of values */ + /* null */ + if (can_read(input_buffer, 4) && (strncmp((const char *)buffer_at_offset(input_buffer), "null", 4) == 0)) + { + item->type = cJSON_NULL; + input_buffer->offset += 4; + return true; + } + /* false */ + if (can_read(input_buffer, 5) && (strncmp((const char *)buffer_at_offset(input_buffer), "false", 5) == 0)) + { + item->type = cJSON_False; + input_buffer->offset += 5; + return true; + } + /* true */ + if (can_read(input_buffer, 4) && (strncmp((const char *)buffer_at_offset(input_buffer), "true", 4) == 0)) + { + item->type = cJSON_True; + item->valueint = 1; + input_buffer->offset += 4; + return true; + } + /* string */ + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '\"')) + { + return parse_string(item, input_buffer); + } + /* number */ + if (can_access_at_index(input_buffer, 0) && ((buffer_at_offset(input_buffer)[0] == '-') || ((buffer_at_offset(input_buffer)[0] >= '0') && (buffer_at_offset(input_buffer)[0] <= '9')))) + { + return parse_number(item, input_buffer); + } + /* array */ + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '[')) + { + return parse_array(item, input_buffer); + } + /* object */ + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '{')) + { + return parse_object(item, input_buffer); + } + + return false; +} + +/* Render a value to text. */ +static cJSON_bool print_value(const cJSON *const item, printbuffer *const output_buffer) +{ + unsigned char *output = NULL; + + if ((item == NULL) || (output_buffer == NULL)) + { + return false; + } + + switch ((item->type) & 0xFF) + { + case cJSON_NULL: + output = ensure(output_buffer, 5); + if (output == NULL) + { + return false; + } + strcpy((char *)output, "null"); + return true; + + case cJSON_False: + output = ensure(output_buffer, 6); + if (output == NULL) + { + return false; + } + strcpy((char *)output, "false"); + return true; + + case cJSON_True: + output = ensure(output_buffer, 5); + if (output == NULL) + { + return false; + } + strcpy((char *)output, "true"); + return true; + + case cJSON_Number: + return print_number(item, output_buffer); + + case cJSON_Raw: + { + size_t raw_length = 0; + if (item->valuestring == NULL) + { + return false; + } + + raw_length = strlen(item->valuestring) + sizeof(""); + output = ensure(output_buffer, raw_length); + if (output == NULL) + { + return false; + } + memcpy(output, item->valuestring, raw_length); + return true; + } + + case cJSON_String: + return print_string(item, output_buffer); + + case cJSON_Array: + return print_array(item, output_buffer); + + case cJSON_Object: + return print_object(item, output_buffer); + + default: + return false; + } +} + +/* Build an array from input text. */ +static cJSON_bool parse_array(cJSON *const item, parse_buffer *const input_buffer) +{ + cJSON *head = NULL; /* head of the linked list */ + cJSON *current_item = NULL; + + if (input_buffer->depth >= CJSON_NESTING_LIMIT) + { + return false; /* to deeply nested */ + } + input_buffer->depth++; + + if (buffer_at_offset(input_buffer)[0] != '[') + { + /* not an array */ + goto fail; + } + + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == ']')) + { + /* empty array */ + goto success; + } + + /* check if we skipped to the end of the buffer */ + if (cannot_access_at_index(input_buffer, 0)) + { + input_buffer->offset--; + goto fail; + } + + /* step back to character in front of the first element */ + input_buffer->offset--; + /* loop through the comma separated array elements */ + do + { + /* allocate next item */ + cJSON *new_item = cJSON_New_Item(&(input_buffer->hooks)); + if (new_item == NULL) + { + goto fail; /* allocation failure */ + } + + /* attach next item to list */ + if (head == NULL) + { + /* start the linked list */ + current_item = head = new_item; + } + else + { + /* add to the end and advance */ + current_item->next = new_item; + new_item->prev = current_item; + current_item = new_item; + } + + /* parse next value */ + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (!parse_value(current_item, input_buffer)) + { + goto fail; /* failed to parse value */ + } + buffer_skip_whitespace(input_buffer); + } while (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == ',')); + + if (cannot_access_at_index(input_buffer, 0) || buffer_at_offset(input_buffer)[0] != ']') + { + goto fail; /* expected end of array */ + } + +success: + input_buffer->depth--; + + if (head != NULL) + { + head->prev = current_item; + } + + item->type = cJSON_Array; + item->child = head; + + input_buffer->offset++; + + return true; + +fail: + if (head != NULL) + { + cJSON_Delete(head); + } + + return false; +} + +/* Render an array to text */ +static cJSON_bool print_array(const cJSON *const item, printbuffer *const output_buffer) +{ + unsigned char *output_pointer = NULL; + size_t length = 0; + cJSON *current_element = item->child; + + if (output_buffer == NULL) + { + return false; + } + + if (output_buffer->depth >= CJSON_NESTING_LIMIT) + { + return false; /* nesting is too deep */ + } + + /* Compose the output array. */ + /* opening square bracket */ + output_pointer = ensure(output_buffer, 1); + if (output_pointer == NULL) + { + return false; + } + + *output_pointer = '['; + output_buffer->offset++; + output_buffer->depth++; + + while (current_element != NULL) + { + if (!print_value(current_element, output_buffer)) + { + return false; + } + update_offset(output_buffer); + if (current_element->next) + { + length = (size_t)(output_buffer->format ? 2 : 1); + output_pointer = ensure(output_buffer, length + 1); + if (output_pointer == NULL) + { + return false; + } + *output_pointer++ = ','; + if (output_buffer->format) + { + *output_pointer++ = ' '; + } + *output_pointer = '\0'; + output_buffer->offset += length; + } + current_element = current_element->next; + } + + output_pointer = ensure(output_buffer, 2); + if (output_pointer == NULL) + { + return false; + } + *output_pointer++ = ']'; + *output_pointer = '\0'; + output_buffer->depth--; + + return true; +} + +/* Build an object from the text. */ +static cJSON_bool parse_object(cJSON *const item, parse_buffer *const input_buffer) +{ + cJSON *head = NULL; /* linked list head */ + cJSON *current_item = NULL; + + if (input_buffer->depth >= CJSON_NESTING_LIMIT) + { + return false; /* to deeply nested */ + } + input_buffer->depth++; + + if (cannot_access_at_index(input_buffer, 0) || (buffer_at_offset(input_buffer)[0] != '{')) + { + goto fail; /* not an object */ + } + + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '}')) + { + goto success; /* empty object */ + } + + /* check if we skipped to the end of the buffer */ + if (cannot_access_at_index(input_buffer, 0)) + { + input_buffer->offset--; + goto fail; + } + + /* step back to character in front of the first element */ + input_buffer->offset--; + /* loop through the comma separated array elements */ + do + { + /* allocate next item */ + cJSON *new_item = cJSON_New_Item(&(input_buffer->hooks)); + if (new_item == NULL) + { + goto fail; /* allocation failure */ + } + + /* attach next item to list */ + if (head == NULL) + { + /* start the linked list */ + current_item = head = new_item; + } + else + { + /* add to the end and advance */ + current_item->next = new_item; + new_item->prev = current_item; + current_item = new_item; + } + + if (cannot_access_at_index(input_buffer, 1)) + { + goto fail; /* nothing comes after the comma */ + } + + /* parse the name of the child */ + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (!parse_string(current_item, input_buffer)) + { + goto fail; /* failed to parse name */ + } + buffer_skip_whitespace(input_buffer); + + /* swap valuestring and string, because we parsed the name */ + current_item->string = current_item->valuestring; + current_item->valuestring = NULL; + + if (cannot_access_at_index(input_buffer, 0) || (buffer_at_offset(input_buffer)[0] != ':')) + { + goto fail; /* invalid object */ + } + + /* parse the value */ + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (!parse_value(current_item, input_buffer)) + { + goto fail; /* failed to parse value */ + } + buffer_skip_whitespace(input_buffer); + } while (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == ',')); + + if (cannot_access_at_index(input_buffer, 0) || (buffer_at_offset(input_buffer)[0] != '}')) + { + goto fail; /* expected end of object */ + } + +success: + input_buffer->depth--; + + if (head != NULL) + { + head->prev = current_item; + } + + item->type = cJSON_Object; + item->child = head; + + input_buffer->offset++; + return true; + +fail: + if (head != NULL) + { + cJSON_Delete(head); + } + + return false; +} + +/* Render an object to text. */ +static cJSON_bool print_object(const cJSON *const item, printbuffer *const output_buffer) +{ + unsigned char *output_pointer = NULL; + size_t length = 0; + cJSON *current_item = item->child; + + if (output_buffer == NULL) + { + return false; + } + + if (output_buffer->depth >= CJSON_NESTING_LIMIT) + { + return false; /* nesting is too deep */ + } + + /* Compose the output: */ + length = (size_t)(output_buffer->format ? 2 : 1); /* fmt: {\n */ + output_pointer = ensure(output_buffer, length + 1); + if (output_pointer == NULL) + { + return false; + } + + *output_pointer++ = '{'; + output_buffer->depth++; + if (output_buffer->format) + { + *output_pointer++ = '\n'; + } + output_buffer->offset += length; + + while (current_item) + { + if (output_buffer->format) + { + size_t i; + output_pointer = ensure(output_buffer, output_buffer->depth); + if (output_pointer == NULL) + { + return false; + } + for (i = 0; i < output_buffer->depth; i++) + { + *output_pointer++ = '\t'; + } + output_buffer->offset += output_buffer->depth; + } + + /* print key */ + if (!print_string_ptr((unsigned char *)current_item->string, output_buffer)) + { + return false; + } + update_offset(output_buffer); + + length = (size_t)(output_buffer->format ? 2 : 1); + output_pointer = ensure(output_buffer, length); + if (output_pointer == NULL) + { + return false; + } + *output_pointer++ = ':'; + if (output_buffer->format) + { + *output_pointer++ = '\t'; + } + output_buffer->offset += length; + + /* print value */ + if (!print_value(current_item, output_buffer)) + { + return false; + } + update_offset(output_buffer); + + /* print comma if not last */ + length = ((size_t)(output_buffer->format ? 1 : 0) + (size_t)(current_item->next ? 1 : 0)); + output_pointer = ensure(output_buffer, length + 1); + if (output_pointer == NULL) + { + return false; + } + if (current_item->next) + { + *output_pointer++ = ','; + } + + if (output_buffer->format) + { + *output_pointer++ = '\n'; + } + *output_pointer = '\0'; + output_buffer->offset += length; + + current_item = current_item->next; + } + + output_pointer = ensure(output_buffer, output_buffer->format ? (output_buffer->depth + 1) : 2); + if (output_pointer == NULL) + { + return false; + } + if (output_buffer->format) + { + size_t i; + for (i = 0; i < (output_buffer->depth - 1); i++) + { + *output_pointer++ = '\t'; + } + } + *output_pointer++ = '}'; + *output_pointer = '\0'; + output_buffer->depth--; + + return true; +} + +/* Get Array size/item / object item. */ +CJSON_PUBLIC(int) +cJSON_GetArraySize(const cJSON *array) +{ + cJSON *child = NULL; + size_t size = 0; + + if (array == NULL) + { + return 0; + } + + child = array->child; + + while (child != NULL) + { + size++; + child = child->next; + } + + /* FIXME: Can overflow here. Cannot be fixed without breaking the API */ + + return (int)size; +} + +static cJSON *get_array_item(const cJSON *array, size_t index) +{ + cJSON *current_child = NULL; + + if (array == NULL) + { + return NULL; + } + + current_child = array->child; + while ((current_child != NULL) && (index > 0)) + { + index--; + current_child = current_child->next; + } + + return current_child; +} + +CJSON_PUBLIC(cJSON *) +cJSON_GetArrayItem(const cJSON *array, int index) +{ + if (index < 0) + { + return NULL; + } + + return get_array_item(array, (size_t)index); +} + +static cJSON *get_object_item(const cJSON *const object, const char *const name, const cJSON_bool case_sensitive) +{ + cJSON *current_element = NULL; + + if ((object == NULL) || (name == NULL)) + { + return NULL; + } + + current_element = object->child; + if (case_sensitive) + { + while ((current_element != NULL) && (current_element->string != NULL) && (strcmp(name, current_element->string) != 0)) + { + current_element = current_element->next; + } + } + else + { + while ((current_element != NULL) && (case_insensitive_strcmp((const unsigned char *)name, (const unsigned char *)(current_element->string)) != 0)) + { + current_element = current_element->next; + } + } + + if ((current_element == NULL) || (current_element->string == NULL)) + { + return NULL; + } + + return current_element; +} + +CJSON_PUBLIC(cJSON *) +cJSON_GetObjectItem(const cJSON *const object, const char *const string) +{ + return get_object_item(object, string, false); +} + +CJSON_PUBLIC(cJSON *) +cJSON_GetObjectItemCaseSensitive(const cJSON *const object, const char *const string) +{ + return get_object_item(object, string, true); +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_HasObjectItem(const cJSON *object, const char *string) +{ + return cJSON_GetObjectItem(object, string) ? 1 : 0; +} + +/* Utility for array list handling. */ +static void suffix_object(cJSON *prev, cJSON *item) +{ + prev->next = item; + item->prev = prev; +} + +/* Utility for handling references. */ +static cJSON *create_reference(const cJSON *item, const internal_hooks *const hooks) +{ + cJSON *reference = NULL; + if (item == NULL) + { + return NULL; + } + + reference = cJSON_New_Item(hooks); + if (reference == NULL) + { + return NULL; + } + + memcpy(reference, item, sizeof(cJSON)); + reference->string = NULL; + reference->type |= cJSON_IsReference; + reference->next = reference->prev = NULL; + return reference; +} + +static cJSON_bool add_item_to_array(cJSON *array, cJSON *item) +{ + cJSON *child = NULL; + + if ((item == NULL) || (array == NULL) || (array == item)) + { + return false; + } + + child = array->child; + /* + * To find the last item in array quickly, we use prev in array + */ + if (child == NULL) + { + /* list is empty, start new one */ + array->child = item; + item->prev = item; + item->next = NULL; + } + else + { + /* append to the end */ + if (child->prev) + { + suffix_object(child->prev, item); + array->child->prev = item; + } + } + + return true; +} + +/* Add item to array/object. */ +CJSON_PUBLIC(cJSON_bool) +cJSON_AddItemToArray(cJSON *array, cJSON *item) +{ + return add_item_to_array(array, item); +} + +#if defined(__clang__) || (defined(__GNUC__) && ((__GNUC__ > 4) || ((__GNUC__ == 4) && (__GNUC_MINOR__ > 5)))) +#pragma GCC diagnostic push +#endif +#ifdef __GNUC__ +#pragma GCC diagnostic ignored "-Wcast-qual" +#endif +/* helper function to cast away const */ +static void *cast_away_const(const void *string) +{ + return (void *)string; +} +#if defined(__clang__) || (defined(__GNUC__) && ((__GNUC__ > 4) || ((__GNUC__ == 4) && (__GNUC_MINOR__ > 5)))) +#pragma GCC diagnostic pop +#endif + +static cJSON_bool add_item_to_object(cJSON *const object, const char *const string, cJSON *const item, const internal_hooks *const hooks, const cJSON_bool constant_key) +{ + char *new_key = NULL; + int new_type = cJSON_Invalid; + + if ((object == NULL) || (string == NULL) || (item == NULL) || (object == item)) + { + return false; + } + + if (constant_key) + { + new_key = (char *)cast_away_const(string); + new_type = item->type | cJSON_StringIsConst; + } + else + { + new_key = (char *)cJSON_strdup((const unsigned char *)string, hooks); + if (new_key == NULL) + { + return false; + } + + new_type = item->type & ~cJSON_StringIsConst; + } + + if (!(item->type & cJSON_StringIsConst) && (item->string != NULL)) + { + hooks->deallocate(item->string); + } + + item->string = new_key; + item->type = new_type; + + return add_item_to_array(object, item); +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_AddItemToObject(cJSON *object, const char *string, cJSON *item) +{ + return add_item_to_object(object, string, item, &global_hooks, false); +} + +/* Add an item to an object with constant string as key */ +CJSON_PUBLIC(cJSON_bool) +cJSON_AddItemToObjectCS(cJSON *object, const char *string, cJSON *item) +{ + return add_item_to_object(object, string, item, &global_hooks, true); +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_AddItemReferenceToArray(cJSON *array, cJSON *item) +{ + if (array == NULL) + { + return false; + } + + return add_item_to_array(array, create_reference(item, &global_hooks)); +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_AddItemReferenceToObject(cJSON *object, const char *string, cJSON *item) +{ + if ((object == NULL) || (string == NULL)) + { + return false; + } + + return add_item_to_object(object, string, create_reference(item, &global_hooks), &global_hooks, false); +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddNullToObject(cJSON *const object, const char *const name) +{ + cJSON *null = cJSON_CreateNull(); + if (add_item_to_object(object, name, null, &global_hooks, false)) + { + return null; + } + + cJSON_Delete(null); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddTrueToObject(cJSON *const object, const char *const name) +{ + cJSON *true_item = cJSON_CreateTrue(); + if (add_item_to_object(object, name, true_item, &global_hooks, false)) + { + return true_item; + } + + cJSON_Delete(true_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddFalseToObject(cJSON *const object, const char *const name) +{ + cJSON *false_item = cJSON_CreateFalse(); + if (add_item_to_object(object, name, false_item, &global_hooks, false)) + { + return false_item; + } + + cJSON_Delete(false_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddBoolToObject(cJSON *const object, const char *const name, const cJSON_bool boolean) +{ + cJSON *bool_item = cJSON_CreateBool(boolean); + if (add_item_to_object(object, name, bool_item, &global_hooks, false)) + { + return bool_item; + } + + cJSON_Delete(bool_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddNumberToObject(cJSON *const object, const char *const name, const double number) +{ + cJSON *number_item = cJSON_CreateNumber(number); + if (add_item_to_object(object, name, number_item, &global_hooks, false)) + { + return number_item; + } + + cJSON_Delete(number_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddStringToObject(cJSON *const object, const char *const name, const char *const string) +{ + cJSON *string_item = cJSON_CreateString(string); + if (add_item_to_object(object, name, string_item, &global_hooks, false)) + { + return string_item; + } + + cJSON_Delete(string_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddRawToObject(cJSON *const object, const char *const name, const char *const raw) +{ + cJSON *raw_item = cJSON_CreateRaw(raw); + if (add_item_to_object(object, name, raw_item, &global_hooks, false)) + { + return raw_item; + } + + cJSON_Delete(raw_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddObjectToObject(cJSON *const object, const char *const name) +{ + cJSON *object_item = cJSON_CreateObject(); + if (add_item_to_object(object, name, object_item, &global_hooks, false)) + { + return object_item; + } + + cJSON_Delete(object_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddArrayToObject(cJSON *const object, const char *const name) +{ + cJSON *array = cJSON_CreateArray(); + if (add_item_to_object(object, name, array, &global_hooks, false)) + { + return array; + } + + cJSON_Delete(array); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_DetachItemViaPointer(cJSON *parent, cJSON *const item) +{ + if ((parent == NULL) || (item == NULL) || (item != parent->child && item->prev == NULL)) + { + return NULL; + } + + if (item != parent->child) + { + /* not the first element */ + item->prev->next = item->next; + } + if (item->next != NULL) + { + /* not the last element */ + item->next->prev = item->prev; + } + + if (item == parent->child) + { + /* first element */ + parent->child = item->next; + } + else if (item->next == NULL) + { + /* last element */ + parent->child->prev = item->prev; + } + + /* make sure the detached item doesn't point anywhere anymore */ + item->prev = NULL; + item->next = NULL; + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_DetachItemFromArray(cJSON *array, int which) +{ + if (which < 0) + { + return NULL; + } + + return cJSON_DetachItemViaPointer(array, get_array_item(array, (size_t)which)); +} + +CJSON_PUBLIC(void) +cJSON_DeleteItemFromArray(cJSON *array, int which) +{ + cJSON_Delete(cJSON_DetachItemFromArray(array, which)); +} + +CJSON_PUBLIC(cJSON *) +cJSON_DetachItemFromObject(cJSON *object, const char *string) +{ + cJSON *to_detach = cJSON_GetObjectItem(object, string); + + return cJSON_DetachItemViaPointer(object, to_detach); +} + +CJSON_PUBLIC(cJSON *) +cJSON_DetachItemFromObjectCaseSensitive(cJSON *object, const char *string) +{ + cJSON *to_detach = cJSON_GetObjectItemCaseSensitive(object, string); + + return cJSON_DetachItemViaPointer(object, to_detach); +} + +CJSON_PUBLIC(void) +cJSON_DeleteItemFromObject(cJSON *object, const char *string) +{ + cJSON_Delete(cJSON_DetachItemFromObject(object, string)); +} + +CJSON_PUBLIC(void) +cJSON_DeleteItemFromObjectCaseSensitive(cJSON *object, const char *string) +{ + cJSON_Delete(cJSON_DetachItemFromObjectCaseSensitive(object, string)); +} + +/* Replace array/object items with new ones. */ +CJSON_PUBLIC(cJSON_bool) +cJSON_InsertItemInArray(cJSON *array, int which, cJSON *newitem) +{ + cJSON *after_inserted = NULL; + + if (which < 0 || newitem == NULL) + { + return false; + } + + after_inserted = get_array_item(array, (size_t)which); + if (after_inserted == NULL) + { + return add_item_to_array(array, newitem); + } + + if (after_inserted != array->child && after_inserted->prev == NULL) + { + /* return false if after_inserted is a corrupted array item */ + return false; + } + + newitem->next = after_inserted; + newitem->prev = after_inserted->prev; + after_inserted->prev = newitem; + if (after_inserted == array->child) + { + array->child = newitem; + } + else + { + newitem->prev->next = newitem; + } + return true; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_ReplaceItemViaPointer(cJSON *const parent, cJSON *const item, cJSON *replacement) +{ + if ((parent == NULL) || (parent->child == NULL) || (replacement == NULL) || (item == NULL)) + { + return false; + } + + if (replacement == item) + { + return true; + } + + replacement->next = item->next; + replacement->prev = item->prev; + + if (replacement->next != NULL) + { + replacement->next->prev = replacement; + } + if (parent->child == item) + { + if (parent->child->prev == parent->child) + { + replacement->prev = replacement; + } + parent->child = replacement; + } + else + { /* + * To find the last item in array quickly, we use prev in array. + * We can't modify the last item's next pointer where this item was the parent's child + */ + if (replacement->prev != NULL) + { + replacement->prev->next = replacement; + } + if (replacement->next == NULL) + { + parent->child->prev = replacement; + } + } + + item->next = NULL; + item->prev = NULL; + cJSON_Delete(item); + + return true; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_ReplaceItemInArray(cJSON *array, int which, cJSON *newitem) +{ + if (which < 0) + { + return false; + } + + return cJSON_ReplaceItemViaPointer(array, get_array_item(array, (size_t)which), newitem); +} + +static cJSON_bool replace_item_in_object(cJSON *object, const char *string, cJSON *replacement, cJSON_bool case_sensitive) +{ + if ((replacement == NULL) || (string == NULL)) + { + return false; + } + + /* replace the name in the replacement */ + if (!(replacement->type & cJSON_StringIsConst) && (replacement->string != NULL)) + { + cJSON_free(replacement->string); + } + replacement->string = (char *)cJSON_strdup((const unsigned char *)string, &global_hooks); + if (replacement->string == NULL) + { + return false; + } + + replacement->type &= ~cJSON_StringIsConst; + + return cJSON_ReplaceItemViaPointer(object, get_object_item(object, string, case_sensitive), replacement); +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_ReplaceItemInObject(cJSON *object, const char *string, cJSON *newitem) +{ + return replace_item_in_object(object, string, newitem, false); +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_ReplaceItemInObjectCaseSensitive(cJSON *object, const char *string, cJSON *newitem) +{ + return replace_item_in_object(object, string, newitem, true); +} + +/* Create basic types: */ +CJSON_PUBLIC(cJSON *) +cJSON_CreateNull(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_NULL; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateTrue(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_True; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateFalse(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_False; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateBool(cJSON_bool boolean) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = boolean ? cJSON_True : cJSON_False; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateNumber(double num) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_Number; + item->valuedouble = num; + + /* use saturation in case of overflow */ + if (num >= INT_MAX) + { + item->valueint = INT_MAX; + } + else if (num <= (double)INT_MIN) + { + item->valueint = INT_MIN; + } + else + { + item->valueint = (int)num; + } + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateString(const char *string) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_String; + item->valuestring = (char *)cJSON_strdup((const unsigned char *)string, &global_hooks); + if (!item->valuestring) + { + cJSON_Delete(item); + return NULL; + } + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateStringReference(const char *string) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item != NULL) + { + item->type = cJSON_String | cJSON_IsReference; + item->valuestring = (char *)cast_away_const(string); + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateObjectReference(const cJSON *child) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item != NULL) + { + item->type = cJSON_Object | cJSON_IsReference; + item->child = (cJSON *)cast_away_const(child); + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateArrayReference(const cJSON *child) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item != NULL) + { + item->type = cJSON_Array | cJSON_IsReference; + item->child = (cJSON *)cast_away_const(child); + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateRaw(const char *raw) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_Raw; + item->valuestring = (char *)cJSON_strdup((const unsigned char *)raw, &global_hooks); + if (!item->valuestring) + { + cJSON_Delete(item); + return NULL; + } + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateArray(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_Array; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateObject(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_Object; + } + + return item; +} + +/* Create Arrays: */ +CJSON_PUBLIC(cJSON *) +cJSON_CreateIntArray(const int *numbers, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (numbers == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for (i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateNumber(numbers[i]); + if (!n) + { + cJSON_Delete(a); + return NULL; + } + if (!i) + { + a->child = n; + } + else + { + suffix_object(p, n); + } + p = n; + } + + if (a && a->child) + { + a->child->prev = n; + } + + return a; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateFloatArray(const float *numbers, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (numbers == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for (i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateNumber((double)numbers[i]); + if (!n) + { + cJSON_Delete(a); + return NULL; + } + if (!i) + { + a->child = n; + } + else + { + suffix_object(p, n); + } + p = n; + } + + if (a && a->child) + { + a->child->prev = n; + } + + return a; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateDoubleArray(const double *numbers, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (numbers == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for (i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateNumber(numbers[i]); + if (!n) + { + cJSON_Delete(a); + return NULL; + } + if (!i) + { + a->child = n; + } + else + { + suffix_object(p, n); + } + p = n; + } + + if (a && a->child) + { + a->child->prev = n; + } + + return a; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateStringArray(const char *const *strings, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (strings == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for (i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateString(strings[i]); + if (!n) + { + cJSON_Delete(a); + return NULL; + } + if (!i) + { + a->child = n; + } + else + { + suffix_object(p, n); + } + p = n; + } + + if (a && a->child) + { + a->child->prev = n; + } + + return a; +} + +/* Duplication */ +cJSON *cJSON_Duplicate_rec(const cJSON *item, size_t depth, cJSON_bool recurse); + +CJSON_PUBLIC(cJSON *) +cJSON_Duplicate(const cJSON *item, cJSON_bool recurse) +{ + return cJSON_Duplicate_rec(item, 0, recurse); +} + +cJSON *cJSON_Duplicate_rec(const cJSON *item, size_t depth, cJSON_bool recurse) +{ + cJSON *newitem = NULL; + cJSON *child = NULL; + cJSON *next = NULL; + cJSON *newchild = NULL; + + /* Bail on bad ptr */ + if (!item) + { + goto fail; + } + /* Create new item */ + newitem = cJSON_New_Item(&global_hooks); + if (!newitem) + { + goto fail; + } + /* Copy over all vars */ + newitem->type = item->type & (~cJSON_IsReference); + newitem->valueint = item->valueint; + newitem->valuedouble = item->valuedouble; + if (item->valuestring) + { + newitem->valuestring = (char *)cJSON_strdup((unsigned char *)item->valuestring, &global_hooks); + if (!newitem->valuestring) + { + goto fail; + } + } + if (item->string) + { + newitem->string = (item->type & cJSON_StringIsConst) ? item->string : (char *)cJSON_strdup((unsigned char *)item->string, &global_hooks); + if (!newitem->string) + { + goto fail; + } + } + /* If non-recursive, then we're done! */ + if (!recurse) + { + return newitem; + } + /* Walk the ->next chain for the child. */ + child = item->child; + while (child != NULL) + { + if (depth >= CJSON_CIRCULAR_LIMIT) + { + goto fail; + } + newchild = cJSON_Duplicate_rec(child, depth + 1, true); /* Duplicate (with recurse) each item in the ->next chain */ + if (!newchild) + { + goto fail; + } + if (next != NULL) + { + /* If newitem->child already set, then crosswire ->prev and ->next and move on */ + next->next = newchild; + newchild->prev = next; + next = newchild; + } + else + { + /* Set newitem->child and move to it */ + newitem->child = newchild; + next = newchild; + } + child = child->next; + } + if (newitem && newitem->child) + { + newitem->child->prev = newchild; + } + + return newitem; + +fail: + if (newitem != NULL) + { + cJSON_Delete(newitem); + } + + return NULL; +} + +static void skip_oneline_comment(char **input) +{ + *input += static_strlen("//"); + + for (; (*input)[0] != '\0'; ++(*input)) + { + if ((*input)[0] == '\n') + { + *input += static_strlen("\n"); + return; + } + } +} + +static void skip_multiline_comment(char **input) +{ + *input += static_strlen("/*"); + + for (; (*input)[0] != '\0'; ++(*input)) + { + if (((*input)[0] == '*') && ((*input)[1] == '/')) + { + *input += static_strlen("*/"); + return; + } + } +} + +static void minify_string(char **input, char **output) +{ + (*output)[0] = (*input)[0]; + *input += static_strlen("\""); + *output += static_strlen("\""); + + for (; (*input)[0] != '\0'; (void)++(*input), ++(*output)) + { + (*output)[0] = (*input)[0]; + + if ((*input)[0] == '\"') + { + (*output)[0] = '\"'; + *input += static_strlen("\""); + *output += static_strlen("\""); + return; + } + else if (((*input)[0] == '\\') && ((*input)[1] == '\"')) + { + (*output)[1] = (*input)[1]; + *input += static_strlen("\""); + *output += static_strlen("\""); + } + } +} + +CJSON_PUBLIC(void) +cJSON_Minify(char *json) +{ + char *into = json; + + if (json == NULL) + { + return; + } + + while (json[0] != '\0') + { + switch (json[0]) + { + case ' ': + case '\t': + case '\r': + case '\n': + json++; + break; + + case '/': + if (json[1] == '/') + { + skip_oneline_comment(&json); + } + else if (json[1] == '*') + { + skip_multiline_comment(&json); + } + else + { + json++; + } + break; + + case '\"': + minify_string(&json, (char **)&into); + break; + + default: + into[0] = json[0]; + json++; + into++; + } + } + + /* and null-terminate. */ + *into = '\0'; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsInvalid(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Invalid; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsFalse(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_False; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsTrue(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xff) == cJSON_True; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsBool(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & (cJSON_True | cJSON_False)) != 0; +} +CJSON_PUBLIC(cJSON_bool) +cJSON_IsNull(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_NULL; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsNumber(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Number; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsString(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_String; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsArray(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Array; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsObject(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Object; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsRaw(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Raw; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_Compare(const cJSON *const a, const cJSON *const b, const cJSON_bool case_sensitive) +{ + if ((a == NULL) || (b == NULL) || ((a->type & 0xFF) != (b->type & 0xFF))) + { + return false; + } + + /* check if type is valid */ + switch (a->type & 0xFF) + { + case cJSON_False: + case cJSON_True: + case cJSON_NULL: + case cJSON_Number: + case cJSON_String: + case cJSON_Raw: + case cJSON_Array: + case cJSON_Object: + break; + + default: + return false; + } + + /* identical objects are equal */ + if (a == b) + { + return true; + } + + switch (a->type & 0xFF) + { + /* in these cases and equal type is enough */ + case cJSON_False: + case cJSON_True: + case cJSON_NULL: + return true; + + case cJSON_Number: + if (compare_double(a->valuedouble, b->valuedouble)) + { + return true; + } + return false; + + case cJSON_String: + case cJSON_Raw: + if ((a->valuestring == NULL) || (b->valuestring == NULL)) + { + return false; + } + if (strcmp(a->valuestring, b->valuestring) == 0) + { + return true; + } + + return false; + + case cJSON_Array: + { + cJSON *a_element = a->child; + cJSON *b_element = b->child; + + for (; (a_element != NULL) && (b_element != NULL);) + { + if (!cJSON_Compare(a_element, b_element, case_sensitive)) + { + return false; + } + + a_element = a_element->next; + b_element = b_element->next; + } + + /* one of the arrays is longer than the other */ + if (a_element != b_element) + { + return false; + } + + return true; + } + + case cJSON_Object: + { + cJSON *a_element = NULL; + cJSON *b_element = NULL; + cJSON_ArrayForEach(a_element, a) + { + /* TODO This has O(n^2) runtime, which is horrible! */ + b_element = get_object_item(b, a_element->string, case_sensitive); + if (b_element == NULL) + { + return false; + } + + if (!cJSON_Compare(a_element, b_element, case_sensitive)) + { + return false; + } + } + + /* doing this twice, once on a and b to prevent true comparison if a subset of b + * TODO: Do this the proper way, this is just a fix for now */ + cJSON_ArrayForEach(b_element, b) + { + a_element = get_object_item(a, b_element->string, case_sensitive); + if (a_element == NULL) + { + return false; + } + + if (!cJSON_Compare(b_element, a_element, case_sensitive)) + { + return false; + } + } + + return true; + } + + default: + return false; + } +} + +CJSON_PUBLIC(void *) +cJSON_malloc(size_t size) +{ + return global_hooks.allocate(size); +} + +CJSON_PUBLIC(void) +cJSON_free(void *object) +{ + global_hooks.deallocate(object); + object = NULL; +} \ No newline at end of file diff --git a/robot/ros2/OmniSocketGo_robot_ros/third_party/cjson/cJSON.h b/robot/ros2/OmniSocketGo_robot_ros/third_party/cjson/cJSON.h new file mode 100644 index 0000000..c760c95 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/third_party/cjson/cJSON.h @@ -0,0 +1,381 @@ +/* + Copyright (c) 2009-2017 Dave Gamble and cJSON contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#ifndef cJSON__h +#define cJSON__h + +#ifdef __cplusplus +extern "C" +{ +#endif + +#if !defined(__WINDOWS__) && (defined(WIN32) || defined(WIN64) || defined(_MSC_VER) || defined(_WIN32)) +#define __WINDOWS__ +#endif + +#ifdef __WINDOWS__ + + /* When compiling for windows, we specify a specific calling convention to avoid issues where we are being called from a project with a different default calling convention. For windows you have 3 define options: + + CJSON_HIDE_SYMBOLS - Define this in the case where you don't want to ever dllexport symbols + CJSON_EXPORT_SYMBOLS - Define this on library build when you want to dllexport symbols (default) + CJSON_IMPORT_SYMBOLS - Define this if you want to dllimport symbol + + For *nix builds that support visibility attribute, you can define similar behavior by + + setting default visibility to hidden by adding + -fvisibility=hidden (for gcc) + or + -xldscope=hidden (for sun cc) + to CFLAGS + + then using the CJSON_API_VISIBILITY flag to "export" the same symbols the way CJSON_EXPORT_SYMBOLS does + + */ + +#define CJSON_CDECL __cdecl +#define CJSON_STDCALL __stdcall + +/* export symbols by default, this is necessary for copy pasting the C and header file */ +#if !defined(CJSON_HIDE_SYMBOLS) && !defined(CJSON_IMPORT_SYMBOLS) && !defined(CJSON_EXPORT_SYMBOLS) +#define CJSON_EXPORT_SYMBOLS +#endif + +#if defined(CJSON_HIDE_SYMBOLS) +#define CJSON_PUBLIC(type) type CJSON_STDCALL +#elif defined(CJSON_EXPORT_SYMBOLS) +#define CJSON_PUBLIC(type) __declspec(dllexport) type CJSON_STDCALL +#elif defined(CJSON_IMPORT_SYMBOLS) +#define CJSON_PUBLIC(type) __declspec(dllimport) type CJSON_STDCALL +#endif +#else /* !__WINDOWS__ */ +#define CJSON_CDECL +#define CJSON_STDCALL + +#if (defined(__GNUC__) || defined(__SUNPRO_CC) || defined(__SUNPRO_C)) && defined(CJSON_API_VISIBILITY) +#define CJSON_PUBLIC(type) __attribute__((visibility("default"))) type +#else +#define CJSON_PUBLIC(type) type +#endif +#endif + +/* project version */ +#define CJSON_VERSION_MAJOR 1 +#define CJSON_VERSION_MINOR 7 +#define CJSON_VERSION_PATCH 19 + +#include + +/* cJSON Types: */ +#define cJSON_Invalid (0) +#define cJSON_False (1 << 0) +#define cJSON_True (1 << 1) +#define cJSON_NULL (1 << 2) +#define cJSON_Number (1 << 3) +#define cJSON_String (1 << 4) +#define cJSON_Array (1 << 5) +#define cJSON_Object (1 << 6) +#define cJSON_Raw (1 << 7) /* raw json */ + +#define cJSON_IsReference 256 +#define cJSON_StringIsConst 512 + + /* The cJSON structure: */ + typedef struct cJSON + { + /* next/prev allow you to walk array/object chains. Alternatively, use GetArraySize/GetArrayItem/GetObjectItem */ + struct cJSON *next; + struct cJSON *prev; + /* An array or object item will have a child pointer pointing to a chain of the items in the array/object. */ + struct cJSON *child; + + /* The type of the item, as above. */ + int type; + + /* The item's string, if type==cJSON_String and type == cJSON_Raw */ + char *valuestring; + /* writing to valueint is DEPRECATED, use cJSON_SetNumberValue instead */ + int valueint; + /* The item's number, if type==cJSON_Number */ + double valuedouble; + + /* The item's name string, if this item is the child of, or is in the list of subitems of an object. */ + char *string; + } cJSON; + + typedef struct cJSON_Hooks + { + /* malloc/free are CDECL on Windows regardless of the default calling convention of the compiler, so ensure the hooks allow passing those functions directly. */ + void *(CJSON_CDECL *malloc_fn)(size_t sz); + void(CJSON_CDECL *free_fn)(void *ptr); + } cJSON_Hooks; + + typedef int cJSON_bool; + +/* Limits how deeply nested arrays/objects can be before cJSON rejects to parse them. + * This is to prevent stack overflows. */ +#ifndef CJSON_NESTING_LIMIT +#define CJSON_NESTING_LIMIT 1000 +#endif + +/* Limits the length of circular references can be before cJSON rejects to parse them. + * This is to prevent stack overflows. */ +#ifndef CJSON_CIRCULAR_LIMIT +#define CJSON_CIRCULAR_LIMIT 10000 +#endif + + /* returns the version of cJSON as a string */ + CJSON_PUBLIC(const char *) + cJSON_Version(void); + + /* Supply malloc, realloc and free functions to cJSON */ + CJSON_PUBLIC(void) + cJSON_InitHooks(cJSON_Hooks *hooks); + + /* Memory Management: the caller is always responsible to free the results from all variants of cJSON_Parse (with cJSON_Delete) and cJSON_Print (with stdlib free, cJSON_Hooks.free_fn, or cJSON_free as appropriate). The exception is cJSON_PrintPreallocated, where the caller has full responsibility of the buffer. */ + /* Supply a block of JSON, and this returns a cJSON object you can interrogate. */ + CJSON_PUBLIC(cJSON *) + cJSON_Parse(const char *value); + CJSON_PUBLIC(cJSON *) + cJSON_ParseWithLength(const char *value, size_t buffer_length); + /* ParseWithOpts allows you to require (and check) that the JSON is null terminated, and to retrieve the pointer to the final byte parsed. */ + /* If you supply a ptr in return_parse_end and parsing fails, then return_parse_end will contain a pointer to the error so will match cJSON_GetErrorPtr(). */ + CJSON_PUBLIC(cJSON *) + cJSON_ParseWithOpts(const char *value, const char **return_parse_end, cJSON_bool require_null_terminated); + CJSON_PUBLIC(cJSON *) + cJSON_ParseWithLengthOpts(const char *value, size_t buffer_length, const char **return_parse_end, cJSON_bool require_null_terminated); + + /* Render a cJSON entity to text for transfer/storage. */ + CJSON_PUBLIC(char *) + cJSON_Print(const cJSON *item); + /* Render a cJSON entity to text for transfer/storage without any formatting. */ + CJSON_PUBLIC(char *) + cJSON_PrintUnformatted(const cJSON *item); + /* Render a cJSON entity to text using a buffered strategy. prebuffer is a guess at the final size. guessing well reduces reallocation. fmt=0 gives unformatted, =1 gives formatted */ + CJSON_PUBLIC(char *) + cJSON_PrintBuffered(const cJSON *item, int prebuffer, cJSON_bool fmt); + /* Render a cJSON entity to text using a buffer already allocated in memory with given length. Returns 1 on success and 0 on failure. */ + /* NOTE: cJSON is not always 100% accurate in estimating how much memory it will use, so to be safe allocate 5 bytes more than you actually need */ + CJSON_PUBLIC(cJSON_bool) + cJSON_PrintPreallocated(cJSON *item, char *buffer, const int length, const cJSON_bool format); + /* Delete a cJSON entity and all subentities. */ + CJSON_PUBLIC(void) + cJSON_Delete(cJSON *item); + + /* Returns the number of items in an array (or object). */ + CJSON_PUBLIC(int) + cJSON_GetArraySize(const cJSON *array); + /* Retrieve item number "index" from array "array". Returns NULL if unsuccessful. */ + CJSON_PUBLIC(cJSON *) + cJSON_GetArrayItem(const cJSON *array, int index); + /* Get item "string" from object. Case insensitive. */ + CJSON_PUBLIC(cJSON *) + cJSON_GetObjectItem(const cJSON *const object, const char *const string); + CJSON_PUBLIC(cJSON *) + cJSON_GetObjectItemCaseSensitive(const cJSON *const object, const char *const string); + CJSON_PUBLIC(cJSON_bool) + cJSON_HasObjectItem(const cJSON *object, const char *string); + /* For analysing failed parses. This returns a pointer to the parse error. You'll probably need to look a few chars back to make sense of it. Defined when cJSON_Parse() returns 0. 0 when cJSON_Parse() succeeds. */ + CJSON_PUBLIC(const char *) + cJSON_GetErrorPtr(void); + + /* Check item type and return its value */ + CJSON_PUBLIC(char *) + cJSON_GetStringValue(const cJSON *const item); + CJSON_PUBLIC(double) + cJSON_GetNumberValue(const cJSON *const item); + + /* These functions check the type of an item */ + CJSON_PUBLIC(cJSON_bool) + cJSON_IsInvalid(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsFalse(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsTrue(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsBool(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsNull(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsNumber(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsString(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsArray(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsObject(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsRaw(const cJSON *const item); + + /* These calls create a cJSON item of the appropriate type. */ + CJSON_PUBLIC(cJSON *) + cJSON_CreateNull(void); + CJSON_PUBLIC(cJSON *) + cJSON_CreateTrue(void); + CJSON_PUBLIC(cJSON *) + cJSON_CreateFalse(void); + CJSON_PUBLIC(cJSON *) + cJSON_CreateBool(cJSON_bool boolean); + CJSON_PUBLIC(cJSON *) + cJSON_CreateNumber(double num); + CJSON_PUBLIC(cJSON *) + cJSON_CreateString(const char *string); + /* raw json */ + CJSON_PUBLIC(cJSON *) + cJSON_CreateRaw(const char *raw); + CJSON_PUBLIC(cJSON *) + cJSON_CreateArray(void); + CJSON_PUBLIC(cJSON *) + cJSON_CreateObject(void); + + /* Create a string where valuestring references a string so + * it will not be freed by cJSON_Delete */ + CJSON_PUBLIC(cJSON *) + cJSON_CreateStringReference(const char *string); + /* Create an object/array that only references it's elements so + * they will not be freed by cJSON_Delete */ + CJSON_PUBLIC(cJSON *) + cJSON_CreateObjectReference(const cJSON *child); + CJSON_PUBLIC(cJSON *) + cJSON_CreateArrayReference(const cJSON *child); + + /* These utilities create an Array of count items. + * The parameter count cannot be greater than the number of elements in the number array, otherwise array access will be out of bounds.*/ + CJSON_PUBLIC(cJSON *) + cJSON_CreateIntArray(const int *numbers, int count); + CJSON_PUBLIC(cJSON *) + cJSON_CreateFloatArray(const float *numbers, int count); + CJSON_PUBLIC(cJSON *) + cJSON_CreateDoubleArray(const double *numbers, int count); + CJSON_PUBLIC(cJSON *) + cJSON_CreateStringArray(const char *const *strings, int count); + + /* Append item to the specified array/object. */ + CJSON_PUBLIC(cJSON_bool) + cJSON_AddItemToArray(cJSON *array, cJSON *item); + CJSON_PUBLIC(cJSON_bool) + cJSON_AddItemToObject(cJSON *object, const char *string, cJSON *item); + /* Use this when string is definitely const (i.e. a literal, or as good as), and will definitely survive the cJSON object. + * WARNING: When this function was used, make sure to always check that (item->type & cJSON_StringIsConst) is zero before + * writing to `item->string` */ + CJSON_PUBLIC(cJSON_bool) + cJSON_AddItemToObjectCS(cJSON *object, const char *string, cJSON *item); + /* Append reference to item to the specified array/object. Use this when you want to add an existing cJSON to a new cJSON, but don't want to corrupt your existing cJSON. */ + CJSON_PUBLIC(cJSON_bool) + cJSON_AddItemReferenceToArray(cJSON *array, cJSON *item); + CJSON_PUBLIC(cJSON_bool) + cJSON_AddItemReferenceToObject(cJSON *object, const char *string, cJSON *item); + + /* Remove/Detach items from Arrays/Objects. */ + CJSON_PUBLIC(cJSON *) + cJSON_DetachItemViaPointer(cJSON *parent, cJSON *const item); + CJSON_PUBLIC(cJSON *) + cJSON_DetachItemFromArray(cJSON *array, int which); + CJSON_PUBLIC(void) + cJSON_DeleteItemFromArray(cJSON *array, int which); + CJSON_PUBLIC(cJSON *) + cJSON_DetachItemFromObject(cJSON *object, const char *string); + CJSON_PUBLIC(cJSON *) + cJSON_DetachItemFromObjectCaseSensitive(cJSON *object, const char *string); + CJSON_PUBLIC(void) + cJSON_DeleteItemFromObject(cJSON *object, const char *string); + CJSON_PUBLIC(void) + cJSON_DeleteItemFromObjectCaseSensitive(cJSON *object, const char *string); + + /* Update array items. */ + CJSON_PUBLIC(cJSON_bool) + cJSON_InsertItemInArray(cJSON *array, int which, cJSON *newitem); /* Shifts pre-existing items to the right. */ + CJSON_PUBLIC(cJSON_bool) + cJSON_ReplaceItemViaPointer(cJSON *const parent, cJSON *const item, cJSON *replacement); + CJSON_PUBLIC(cJSON_bool) + cJSON_ReplaceItemInArray(cJSON *array, int which, cJSON *newitem); + CJSON_PUBLIC(cJSON_bool) + cJSON_ReplaceItemInObject(cJSON *object, const char *string, cJSON *newitem); + CJSON_PUBLIC(cJSON_bool) + cJSON_ReplaceItemInObjectCaseSensitive(cJSON *object, const char *string, cJSON *newitem); + + /* Duplicate a cJSON item */ + CJSON_PUBLIC(cJSON *) + cJSON_Duplicate(const cJSON *item, cJSON_bool recurse); + /* Duplicate will create a new, identical cJSON item to the one you pass, in new memory that will + * need to be released. With recurse!=0, it will duplicate any children connected to the item. + * The item->next and ->prev pointers are always zero on return from Duplicate. */ + /* Recursively compare two cJSON items for equality. If either a or b is NULL or invalid, they will be considered unequal. + * case_sensitive determines if object keys are treated case sensitive (1) or case insensitive (0) */ + CJSON_PUBLIC(cJSON_bool) + cJSON_Compare(const cJSON *const a, const cJSON *const b, const cJSON_bool case_sensitive); + + /* Minify a strings, remove blank characters(such as ' ', '\t', '\r', '\n') from strings. + * The input pointer json cannot point to a read-only address area, such as a string constant, + * but should point to a readable and writable address area. */ + CJSON_PUBLIC(void) + cJSON_Minify(char *json); + + /* Helper functions for creating and adding items to an object at the same time. + * They return the added item or NULL on failure. */ + CJSON_PUBLIC(cJSON *) + cJSON_AddNullToObject(cJSON *const object, const char *const name); + CJSON_PUBLIC(cJSON *) + cJSON_AddTrueToObject(cJSON *const object, const char *const name); + CJSON_PUBLIC(cJSON *) + cJSON_AddFalseToObject(cJSON *const object, const char *const name); + CJSON_PUBLIC(cJSON *) + cJSON_AddBoolToObject(cJSON *const object, const char *const name, const cJSON_bool boolean); + CJSON_PUBLIC(cJSON *) + cJSON_AddNumberToObject(cJSON *const object, const char *const name, const double number); + CJSON_PUBLIC(cJSON *) + cJSON_AddStringToObject(cJSON *const object, const char *const name, const char *const string); + CJSON_PUBLIC(cJSON *) + cJSON_AddRawToObject(cJSON *const object, const char *const name, const char *const raw); + CJSON_PUBLIC(cJSON *) + cJSON_AddObjectToObject(cJSON *const object, const char *const name); + CJSON_PUBLIC(cJSON *) + cJSON_AddArrayToObject(cJSON *const object, const char *const name); + +/* When assigning an integer value, it needs to be propagated to valuedouble too. */ +#define cJSON_SetIntValue(object, number) ((object) ? (object)->valueint = (object)->valuedouble = (number) : (number)) + /* helper for the cJSON_SetNumberValue macro */ + CJSON_PUBLIC(double) + cJSON_SetNumberHelper(cJSON *object, double number); +#define cJSON_SetNumberValue(object, number) ((object != NULL) ? cJSON_SetNumberHelper(object, (double)number) : (number)) + /* Change the valuestring of a cJSON_String object, only takes effect when type of object is cJSON_String */ + CJSON_PUBLIC(char *) + cJSON_SetValuestring(cJSON *object, const char *valuestring); + +/* If the object is not a boolean type this does nothing and returns cJSON_Invalid else it returns the new type*/ +#define cJSON_SetBoolValue(object, boolValue) ( \ + (object != NULL && ((object)->type & (cJSON_False | cJSON_True))) ? (object)->type = ((object)->type & (~(cJSON_False | cJSON_True))) | ((boolValue) ? cJSON_True : cJSON_False) : cJSON_Invalid) + +/* Macro for iterating over an array or object */ +#define cJSON_ArrayForEach(element, array) for (element = (array != NULL) ? (array)->child : NULL; element != NULL; element = element->next) + + /* malloc/free objects using the malloc/free functions that have been set with cJSON_InitHooks */ + CJSON_PUBLIC(void *) + cJSON_malloc(size_t size); + CJSON_PUBLIC(void) + cJSON_free(void *object); + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file diff --git a/robot/ros2/OmniSocketGo_robot_ros/third_party/kcp/ikcp.c b/robot/ros2/OmniSocketGo_robot_ros/third_party/kcp/ikcp.c new file mode 100644 index 0000000..593ae41 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/third_party/kcp/ikcp.c @@ -0,0 +1,1466 @@ +//===================================================================== +// +// KCP - A Better ARQ Protocol Implementation +// skywind3000 (at) gmail.com, 2010-2011 +// +// Features: +// + Average RTT reduce 30% - 40% vs traditional ARQ like tcp. +// + Maximum RTT reduce three times vs tcp. +// + Lightweight, distributed as a single source file. +// +//===================================================================== +#include "ikcp.h" + +#include +#include +#include +#include +#include + +#define IKCP_FASTACK_CONSERVE + +//===================================================================== +// KCP BASIC +//===================================================================== +const IUINT32 IKCP_RTO_NDL = 30; // no delay min rto +const IUINT32 IKCP_RTO_MIN = 100; // normal min rto +const IUINT32 IKCP_RTO_DEF = 200; +const IUINT32 IKCP_RTO_MAX = 60000; +const IUINT32 IKCP_CMD_PUSH = 81; // cmd: push data +const IUINT32 IKCP_CMD_ACK = 82; // cmd: ack +const IUINT32 IKCP_CMD_WASK = 83; // cmd: window probe (ask) +const IUINT32 IKCP_CMD_WINS = 84; // cmd: window size (tell) +const IUINT32 IKCP_ASK_SEND = 1; // need to send IKCP_CMD_WASK +const IUINT32 IKCP_ASK_TELL = 2; // need to send IKCP_CMD_WINS +const IUINT32 IKCP_WND_SND = 32; +const IUINT32 IKCP_WND_RCV = 128; // must >= max fragment size +const IUINT32 IKCP_MTU_DEF = 1400; +const IUINT32 IKCP_ACK_FAST = 3; +const IUINT32 IKCP_INTERVAL = 100; +const IUINT32 IKCP_OVERHEAD = 24; +const IUINT32 IKCP_DEADLINK = 20; +const IUINT32 IKCP_THRESH_INIT = 2; +const IUINT32 IKCP_THRESH_MIN = 2; +const IUINT32 IKCP_PROBE_INIT = 7000; // 7 secs to probe window size +const IUINT32 IKCP_PROBE_LIMIT = 120000; // up to 120 secs to probe window +const IUINT32 IKCP_FASTACK_LIMIT = 5; // max times to trigger fastack + +//--------------------------------------------------------------------- +// encode / decode +//--------------------------------------------------------------------- + +/* encode 8 bits unsigned int */ +static inline char *ikcp_encode8u(char *p, unsigned char c) +{ + *(unsigned char *)p++ = c; + return p; +} + +/* decode 8 bits unsigned int */ +static inline const char *ikcp_decode8u(const char *p, unsigned char *c) +{ + *c = *(unsigned char *)p++; + return p; +} + +/* encode 16 bits unsigned int (lsb) */ +static inline char *ikcp_encode16u(char *p, unsigned short w) +{ +#if IWORDS_BIG_ENDIAN || IWORDS_MUST_ALIGN + *(unsigned char *)(p + 0) = (w & 255); + *(unsigned char *)(p + 1) = (w >> 8); +#else + memcpy(p, &w, 2); +#endif + p += 2; + return p; +} + +/* decode 16 bits unsigned int (lsb) */ +static inline const char *ikcp_decode16u(const char *p, unsigned short *w) +{ +#if IWORDS_BIG_ENDIAN || IWORDS_MUST_ALIGN + *w = *(const unsigned char *)(p + 1); + *w = *(const unsigned char *)(p + 0) + (*w << 8); +#else + memcpy(w, p, 2); +#endif + p += 2; + return p; +} + +/* encode 32 bits unsigned int (lsb) */ +static inline char *ikcp_encode32u(char *p, IUINT32 l) +{ +#if IWORDS_BIG_ENDIAN || IWORDS_MUST_ALIGN + *(unsigned char *)(p + 0) = (unsigned char)((l >> 0) & 0xff); + *(unsigned char *)(p + 1) = (unsigned char)((l >> 8) & 0xff); + *(unsigned char *)(p + 2) = (unsigned char)((l >> 16) & 0xff); + *(unsigned char *)(p + 3) = (unsigned char)((l >> 24) & 0xff); +#else + memcpy(p, &l, 4); +#endif + p += 4; + return p; +} + +/* decode 32 bits unsigned int (lsb) */ +static inline const char *ikcp_decode32u(const char *p, IUINT32 *l) +{ +#if IWORDS_BIG_ENDIAN || IWORDS_MUST_ALIGN + *l = *(const unsigned char *)(p + 3); + *l = *(const unsigned char *)(p + 2) + (*l << 8); + *l = *(const unsigned char *)(p + 1) + (*l << 8); + *l = *(const unsigned char *)(p + 0) + (*l << 8); +#else + memcpy(l, p, 4); +#endif + p += 4; + return p; +} + +static inline IUINT32 _imin_(IUINT32 a, IUINT32 b) +{ + return a <= b ? a : b; +} + +static inline IUINT32 _imax_(IUINT32 a, IUINT32 b) +{ + return a >= b ? a : b; +} + +static inline IUINT32 _ibound_(IUINT32 lower, IUINT32 middle, IUINT32 upper) +{ + return _imin_(_imax_(lower, middle), upper); +} + +static inline long _itimediff(IUINT32 later, IUINT32 earlier) +{ + return ((IINT32)(later - earlier)); +} + +//--------------------------------------------------------------------- +// manage segment +//--------------------------------------------------------------------- +typedef struct IKCPSEG IKCPSEG; + +static void *(*ikcp_malloc_hook)(size_t) = NULL; +static void (*ikcp_free_hook)(void *) = NULL; + +// internal malloc +static void *ikcp_malloc(size_t size) +{ + if (ikcp_malloc_hook) + return ikcp_malloc_hook(size); + return malloc(size); +} + +// internal free +static void ikcp_free(void *ptr) +{ + if (ikcp_free_hook) + { + ikcp_free_hook(ptr); + } + else + { + free(ptr); + } +} + +// redefine allocator +void ikcp_allocator(void *(*new_malloc)(size_t), void (*new_free)(void *)) +{ + ikcp_malloc_hook = new_malloc; + ikcp_free_hook = new_free; +} + +// allocate a new kcp segment +static IKCPSEG *ikcp_segment_new(ikcpcb *kcp, int size) +{ + return (IKCPSEG *)ikcp_malloc(sizeof(IKCPSEG) + size); +} + +// delete a segment +static void ikcp_segment_delete(ikcpcb *kcp, IKCPSEG *seg) +{ + ikcp_free(seg); +} + +// write log +void ikcp_log(ikcpcb *kcp, int mask, const char *fmt, ...) +{ + char buffer[1024]; + va_list argptr; + if ((mask & kcp->logmask) == 0 || kcp->writelog == 0) + return; + va_start(argptr, fmt); + vsprintf(buffer, fmt, argptr); + va_end(argptr); + kcp->writelog(buffer, kcp, kcp->user); +} + +// check log mask +static int ikcp_canlog(const ikcpcb *kcp, int mask) +{ + if ((mask & kcp->logmask) == 0 || kcp->writelog == NULL) + return 0; + return 1; +} + +// output segment +static int ikcp_output(ikcpcb *kcp, const void *data, int size) +{ + assert(kcp); + assert(kcp->output); + if (ikcp_canlog(kcp, IKCP_LOG_OUTPUT)) + { + ikcp_log(kcp, IKCP_LOG_OUTPUT, "[RO] %ld bytes", (long)size); + } + if (size == 0) + return 0; + return kcp->output((const char *)data, size, kcp, kcp->user); +} + +// output queue +void ikcp_qprint(const char *name, const struct IQUEUEHEAD *head) +{ +#if 0 + const struct IQUEUEHEAD *p; + printf("<%s>: [", name); + for (p = head->next; p != head; p = p->next) { + const IKCPSEG *seg = iqueue_entry(p, const IKCPSEG, node); + printf("(%lu %d)", (unsigned long)seg->sn, (int)(seg->ts % 10000)); + if (p->next != head) printf(","); + } + printf("]\n"); +#endif +} + +//--------------------------------------------------------------------- +// create a new kcpcb +//--------------------------------------------------------------------- +ikcpcb *ikcp_create(IUINT32 conv, void *user) +{ + ikcpcb *kcp = (ikcpcb *)ikcp_malloc(sizeof(struct IKCPCB)); + if (kcp == NULL) + return NULL; + kcp->conv = conv; + kcp->user = user; + kcp->snd_una = 0; + kcp->snd_nxt = 0; + kcp->rcv_nxt = 0; + kcp->ts_recent = 0; + kcp->ts_lastack = 0; + kcp->ts_probe = 0; + kcp->probe_wait = 0; + kcp->snd_wnd = IKCP_WND_SND; + kcp->rcv_wnd = IKCP_WND_RCV; + kcp->rmt_wnd = IKCP_WND_RCV; + kcp->cwnd = 0; + kcp->incr = 0; + kcp->probe = 0; + kcp->mtu = IKCP_MTU_DEF; + kcp->mss = kcp->mtu - IKCP_OVERHEAD; + kcp->stream = 0; + + kcp->buffer = (char *)ikcp_malloc((kcp->mtu + IKCP_OVERHEAD) * 3); + if (kcp->buffer == NULL) + { + ikcp_free(kcp); + return NULL; + } + + iqueue_init(&kcp->snd_queue); + iqueue_init(&kcp->rcv_queue); + iqueue_init(&kcp->snd_buf); + iqueue_init(&kcp->rcv_buf); + kcp->nrcv_buf = 0; + kcp->nsnd_buf = 0; + kcp->nrcv_que = 0; + kcp->nsnd_que = 0; + kcp->state = 0; + kcp->acklist = NULL; + kcp->ackblock = 0; + kcp->ackcount = 0; + kcp->rx_srtt = 0; + kcp->rx_rttval = 0; + kcp->rx_rto = IKCP_RTO_DEF; + kcp->rx_minrto = IKCP_RTO_MIN; + kcp->current = 0; + kcp->interval = IKCP_INTERVAL; + kcp->ts_flush = IKCP_INTERVAL; + kcp->nodelay = 0; + kcp->updated = 0; + kcp->logmask = 0; + kcp->ssthresh = IKCP_THRESH_INIT; + kcp->fastresend = 0; + kcp->fastlimit = IKCP_FASTACK_LIMIT; + kcp->nocwnd = 0; + kcp->xmit = 0; + kcp->timeout_retrans_total = 0; + kcp->fast_retrans_total = 0; + kcp->duplicate_recv_total = 0; + kcp->dead_link = IKCP_DEADLINK; + kcp->output = NULL; + kcp->writelog = NULL; + + return kcp; +} + +//--------------------------------------------------------------------- +// release a new kcpcb +//--------------------------------------------------------------------- +void ikcp_release(ikcpcb *kcp) +{ + assert(kcp); + if (kcp) + { + IKCPSEG *seg; + while (!iqueue_is_empty(&kcp->snd_buf)) + { + seg = iqueue_entry(kcp->snd_buf.next, IKCPSEG, node); + iqueue_del(&seg->node); + ikcp_segment_delete(kcp, seg); + } + while (!iqueue_is_empty(&kcp->rcv_buf)) + { + seg = iqueue_entry(kcp->rcv_buf.next, IKCPSEG, node); + iqueue_del(&seg->node); + ikcp_segment_delete(kcp, seg); + } + while (!iqueue_is_empty(&kcp->snd_queue)) + { + seg = iqueue_entry(kcp->snd_queue.next, IKCPSEG, node); + iqueue_del(&seg->node); + ikcp_segment_delete(kcp, seg); + } + while (!iqueue_is_empty(&kcp->rcv_queue)) + { + seg = iqueue_entry(kcp->rcv_queue.next, IKCPSEG, node); + iqueue_del(&seg->node); + ikcp_segment_delete(kcp, seg); + } + if (kcp->buffer) + { + ikcp_free(kcp->buffer); + } + if (kcp->acklist) + { + ikcp_free(kcp->acklist); + } + + kcp->nrcv_buf = 0; + kcp->nsnd_buf = 0; + kcp->nrcv_que = 0; + kcp->nsnd_que = 0; + kcp->ackcount = 0; + kcp->buffer = NULL; + kcp->acklist = NULL; + ikcp_free(kcp); + } +} + +//--------------------------------------------------------------------- +// set output callback, which will be invoked by kcp +//--------------------------------------------------------------------- +void ikcp_setoutput(ikcpcb *kcp, int (*output)(const char *buf, int len, + ikcpcb *kcp, void *user)) +{ + kcp->output = output; +} + +//--------------------------------------------------------------------- +// user/upper level recv: returns size, returns below zero for EAGAIN +//--------------------------------------------------------------------- +int ikcp_recv(ikcpcb *kcp, char *buffer, int len) +{ + struct IQUEUEHEAD *p; + int ispeek = (len < 0) ? 1 : 0; + int peeksize; + int recover = 0; + IKCPSEG *seg; + assert(kcp); + + if (iqueue_is_empty(&kcp->rcv_queue)) + return -1; + + if (len < 0) + len = -len; + + peeksize = ikcp_peeksize(kcp); + + if (peeksize < 0) + return -2; + + if (peeksize > len) + return -3; + + if (kcp->nrcv_que >= kcp->rcv_wnd) + recover = 1; + + // merge fragment + for (len = 0, p = kcp->rcv_queue.next; p != &kcp->rcv_queue;) + { + int fragment; + seg = iqueue_entry(p, IKCPSEG, node); + p = p->next; + + if (buffer) + { + memcpy(buffer, seg->data, seg->len); + buffer += seg->len; + } + + len += seg->len; + fragment = seg->frg; + + if (ikcp_canlog(kcp, IKCP_LOG_RECV)) + { + ikcp_log(kcp, IKCP_LOG_RECV, "recv sn=%lu", (unsigned long)seg->sn); + } + + if (ispeek == 0) + { + iqueue_del(&seg->node); + ikcp_segment_delete(kcp, seg); + kcp->nrcv_que--; + } + + if (fragment == 0) + break; + } + + assert(len == peeksize); + + // move available data from rcv_buf -> rcv_queue + while (!iqueue_is_empty(&kcp->rcv_buf)) + { + seg = iqueue_entry(kcp->rcv_buf.next, IKCPSEG, node); + if (seg->sn == kcp->rcv_nxt && kcp->nrcv_que < kcp->rcv_wnd) + { + iqueue_del(&seg->node); + kcp->nrcv_buf--; + iqueue_add_tail(&seg->node, &kcp->rcv_queue); + kcp->nrcv_que++; + kcp->rcv_nxt++; + } + else + { + break; + } + } + + // fast recover + if (kcp->nrcv_que < kcp->rcv_wnd && recover) + { + // ready to send back IKCP_CMD_WINS in ikcp_flush + // tell remote my window size + kcp->probe |= IKCP_ASK_TELL; + } + + return len; +} + +//--------------------------------------------------------------------- +// peek data size +//--------------------------------------------------------------------- +int ikcp_peeksize(const ikcpcb *kcp) +{ + struct IQUEUEHEAD *p; + IKCPSEG *seg; + int length = 0; + + assert(kcp); + + if (iqueue_is_empty(&kcp->rcv_queue)) + return -1; + + seg = iqueue_entry(kcp->rcv_queue.next, IKCPSEG, node); + if (seg->frg == 0) + return seg->len; + + if (kcp->nrcv_que < seg->frg + 1) + return -1; + + for (p = kcp->rcv_queue.next; p != &kcp->rcv_queue; p = p->next) + { + seg = iqueue_entry(p, IKCPSEG, node); + length += seg->len; + if (seg->frg == 0) + break; + } + + return length; +} + +//--------------------------------------------------------------------- +// user/upper level send, returns below zero for error +//--------------------------------------------------------------------- +int ikcp_send(ikcpcb *kcp, const char *buffer, int len) +{ + IKCPSEG *seg; + int count, i; + int sent = 0; + + assert(kcp->mss > 0); + if (len < 0) + return -1; + + // append to previous segment in streaming mode (if possible) + if (kcp->stream != 0) + { + if (!iqueue_is_empty(&kcp->snd_queue)) + { + IKCPSEG *old = iqueue_entry(kcp->snd_queue.prev, IKCPSEG, node); + if (old->len < kcp->mss) + { + int capacity = kcp->mss - old->len; + int extend = (len < capacity) ? len : capacity; + seg = ikcp_segment_new(kcp, old->len + extend); + assert(seg); + if (seg == NULL) + { + return -2; + } + iqueue_add_tail(&seg->node, &kcp->snd_queue); + memcpy(seg->data, old->data, old->len); + if (buffer) + { + memcpy(seg->data + old->len, buffer, extend); + buffer += extend; + } + seg->len = old->len + extend; + seg->frg = 0; + len -= extend; + iqueue_del_init(&old->node); + ikcp_segment_delete(kcp, old); + sent = extend; + } + } + if (len <= 0) + { + return sent; + } + } + + if (len <= (int)kcp->mss) + count = 1; + else + count = (len + kcp->mss - 1) / kcp->mss; + + if (count >= (int)IKCP_WND_RCV) + { + if (kcp->stream != 0 && sent > 0) + return sent; + return -2; + } + + if (count == 0) + count = 1; + + // fragment + for (i = 0; i < count; i++) + { + int size = len > (int)kcp->mss ? (int)kcp->mss : len; + seg = ikcp_segment_new(kcp, size); + assert(seg); + if (seg == NULL) + { + return -2; + } + if (buffer && len > 0) + { + memcpy(seg->data, buffer, size); + } + seg->len = size; + seg->frg = (kcp->stream == 0) ? (count - i - 1) : 0; + iqueue_init(&seg->node); + iqueue_add_tail(&seg->node, &kcp->snd_queue); + kcp->nsnd_que++; + if (buffer) + { + buffer += size; + } + len -= size; + sent += size; + } + + return sent; +} + +//--------------------------------------------------------------------- +// parse ack +//--------------------------------------------------------------------- +static void ikcp_update_ack(ikcpcb *kcp, IINT32 rtt) +{ + IINT32 rto = 0; + if (kcp->rx_srtt == 0) + { + kcp->rx_srtt = rtt; + kcp->rx_rttval = rtt / 2; + } + else + { + long delta = rtt - kcp->rx_srtt; + if (delta < 0) + delta = -delta; + kcp->rx_rttval = (3 * kcp->rx_rttval + delta) / 4; + kcp->rx_srtt = (7 * kcp->rx_srtt + rtt) / 8; + if (kcp->rx_srtt < 1) + kcp->rx_srtt = 1; + } + rto = kcp->rx_srtt + _imax_(kcp->interval, 4 * kcp->rx_rttval); + kcp->rx_rto = _ibound_(kcp->rx_minrto, rto, IKCP_RTO_MAX); +} + +static void ikcp_shrink_buf(ikcpcb *kcp) +{ + struct IQUEUEHEAD *p = kcp->snd_buf.next; + if (p != &kcp->snd_buf) + { + IKCPSEG *seg = iqueue_entry(p, IKCPSEG, node); + kcp->snd_una = seg->sn; + } + else + { + kcp->snd_una = kcp->snd_nxt; + } +} + +static void ikcp_parse_ack(ikcpcb *kcp, IUINT32 sn) +{ + struct IQUEUEHEAD *p, *next; + + if (_itimediff(sn, kcp->snd_una) < 0 || _itimediff(sn, kcp->snd_nxt) >= 0) + return; + + for (p = kcp->snd_buf.next; p != &kcp->snd_buf; p = next) + { + IKCPSEG *seg = iqueue_entry(p, IKCPSEG, node); + next = p->next; + if (sn == seg->sn) + { + iqueue_del(p); + ikcp_segment_delete(kcp, seg); + kcp->nsnd_buf--; + break; + } + if (_itimediff(sn, seg->sn) < 0) + { + break; + } + } +} + +static void ikcp_parse_una(ikcpcb *kcp, IUINT32 una) +{ + struct IQUEUEHEAD *p, *next; + for (p = kcp->snd_buf.next; p != &kcp->snd_buf; p = next) + { + IKCPSEG *seg = iqueue_entry(p, IKCPSEG, node); + next = p->next; + if (_itimediff(una, seg->sn) > 0) + { + iqueue_del(p); + ikcp_segment_delete(kcp, seg); + kcp->nsnd_buf--; + } + else + { + break; + } + } +} + +static void ikcp_parse_fastack(ikcpcb *kcp, IUINT32 sn, IUINT32 ts) +{ + struct IQUEUEHEAD *p, *next; + + if (_itimediff(sn, kcp->snd_una) < 0 || _itimediff(sn, kcp->snd_nxt) >= 0) + return; + + for (p = kcp->snd_buf.next; p != &kcp->snd_buf; p = next) + { + IKCPSEG *seg = iqueue_entry(p, IKCPSEG, node); + next = p->next; + if (_itimediff(sn, seg->sn) < 0) + { + break; + } + else if (sn != seg->sn) + { +#ifndef IKCP_FASTACK_CONSERVE + seg->fastack++; +#else + if (_itimediff(ts, seg->ts) >= 0) + seg->fastack++; +#endif + } + } +} + +//--------------------------------------------------------------------- +// ack append +//--------------------------------------------------------------------- +static void ikcp_ack_push(ikcpcb *kcp, IUINT32 sn, IUINT32 ts) +{ + IUINT32 newsize = kcp->ackcount + 1; + IUINT32 *ptr; + + if (newsize > kcp->ackblock) + { + IUINT32 *acklist; + IUINT32 newblock; + + for (newblock = 8; newblock < newsize; newblock <<= 1) + ; + acklist = (IUINT32 *)ikcp_malloc(newblock * sizeof(IUINT32) * 2); + + if (acklist == NULL) + { + assert(acklist != NULL); + abort(); + } + + if (kcp->acklist != NULL) + { + IUINT32 x; + for (x = 0; x < kcp->ackcount; x++) + { + acklist[x * 2 + 0] = kcp->acklist[x * 2 + 0]; + acklist[x * 2 + 1] = kcp->acklist[x * 2 + 1]; + } + ikcp_free(kcp->acklist); + } + + kcp->acklist = acklist; + kcp->ackblock = newblock; + } + + ptr = &kcp->acklist[kcp->ackcount * 2]; + ptr[0] = sn; + ptr[1] = ts; + kcp->ackcount++; +} + +static void ikcp_ack_get(const ikcpcb *kcp, int p, IUINT32 *sn, IUINT32 *ts) +{ + if (sn) + sn[0] = kcp->acklist[p * 2 + 0]; + if (ts) + ts[0] = kcp->acklist[p * 2 + 1]; +} + +//--------------------------------------------------------------------- +// parse data +//--------------------------------------------------------------------- +void ikcp_parse_data(ikcpcb *kcp, IKCPSEG *newseg) +{ + struct IQUEUEHEAD *p, *prev; + IUINT32 sn = newseg->sn; + int repeat = 0; + + if (_itimediff(sn, kcp->rcv_nxt + kcp->rcv_wnd) >= 0 || + _itimediff(sn, kcp->rcv_nxt) < 0) + { + ikcp_segment_delete(kcp, newseg); + return; + } + + for (p = kcp->rcv_buf.prev; p != &kcp->rcv_buf; p = prev) + { + IKCPSEG *seg = iqueue_entry(p, IKCPSEG, node); + prev = p->prev; + if (seg->sn == sn) + { + repeat = 1; + break; + } + if (_itimediff(sn, seg->sn) > 0) + { + break; + } + } + + if (repeat == 0) + { + iqueue_init(&newseg->node); + iqueue_add(&newseg->node, p); + kcp->nrcv_buf++; + } + else + { + kcp->duplicate_recv_total++; + ikcp_segment_delete(kcp, newseg); + } + +#if 0 + ikcp_qprint("rcvbuf", &kcp->rcv_buf); + printf("rcv_nxt=%lu\n", kcp->rcv_nxt); +#endif + + // move available data from rcv_buf -> rcv_queue + while (!iqueue_is_empty(&kcp->rcv_buf)) + { + IKCPSEG *seg = iqueue_entry(kcp->rcv_buf.next, IKCPSEG, node); + if (seg->sn == kcp->rcv_nxt && kcp->nrcv_que < kcp->rcv_wnd) + { + iqueue_del(&seg->node); + kcp->nrcv_buf--; + iqueue_add_tail(&seg->node, &kcp->rcv_queue); + kcp->nrcv_que++; + kcp->rcv_nxt++; + } + else + { + break; + } + } + +#if 0 + ikcp_qprint("queue", &kcp->rcv_queue); + printf("rcv_nxt=%lu\n", kcp->rcv_nxt); +#endif + +#if 1 +// printf("snd(buf=%d, queue=%d)\n", kcp->nsnd_buf, kcp->nsnd_que); +// printf("rcv(buf=%d, queue=%d)\n", kcp->nrcv_buf, kcp->nrcv_que); +#endif +} + +//--------------------------------------------------------------------- +// input data +//--------------------------------------------------------------------- +int ikcp_input(ikcpcb *kcp, const char *data, long size) +{ + IUINT32 prev_una = kcp->snd_una; + IUINT32 maxack = 0, latest_ts = 0; + int flag = 0; + + if (ikcp_canlog(kcp, IKCP_LOG_INPUT)) + { + ikcp_log(kcp, IKCP_LOG_INPUT, "[RI] %d bytes", (int)size); + } + + if (data == NULL || (int)size < (int)IKCP_OVERHEAD) + return -1; + + while (1) + { + IUINT32 ts, sn, len, una, conv; + IUINT16 wnd; + IUINT8 cmd, frg; + IKCPSEG *seg; + + if (size < (int)IKCP_OVERHEAD) + break; + + data = ikcp_decode32u(data, &conv); + if (conv != kcp->conv) + return -1; + + data = ikcp_decode8u(data, &cmd); + data = ikcp_decode8u(data, &frg); + data = ikcp_decode16u(data, &wnd); + data = ikcp_decode32u(data, &ts); + data = ikcp_decode32u(data, &sn); + data = ikcp_decode32u(data, &una); + data = ikcp_decode32u(data, &len); + + size -= IKCP_OVERHEAD; + + if ((long)size < (long)len || (int)len < 0) + return -2; + + if (cmd != IKCP_CMD_PUSH && cmd != IKCP_CMD_ACK && + cmd != IKCP_CMD_WASK && cmd != IKCP_CMD_WINS) + return -3; + + kcp->rmt_wnd = wnd; + ikcp_parse_una(kcp, una); + ikcp_shrink_buf(kcp); + + if (cmd == IKCP_CMD_ACK) + { + if (_itimediff(kcp->current, ts) >= 0) + { + ikcp_update_ack(kcp, _itimediff(kcp->current, ts)); + } + ikcp_parse_ack(kcp, sn); + ikcp_shrink_buf(kcp); + if (flag == 0) + { + flag = 1; + maxack = sn; + latest_ts = ts; + } + else + { + if (_itimediff(sn, maxack) > 0) + { +#ifndef IKCP_FASTACK_CONSERVE + maxack = sn; + latest_ts = ts; +#else + if (_itimediff(ts, latest_ts) > 0) + { + maxack = sn; + latest_ts = ts; + } +#endif + } + } + if (ikcp_canlog(kcp, IKCP_LOG_IN_ACK)) + { + ikcp_log(kcp, IKCP_LOG_IN_ACK, + "input ack: sn=%lu rtt=%ld rto=%ld", (unsigned long)sn, + (long)_itimediff(kcp->current, ts), + (long)kcp->rx_rto); + } + } + else if (cmd == IKCP_CMD_PUSH) + { + if (ikcp_canlog(kcp, IKCP_LOG_IN_DATA)) + { + ikcp_log(kcp, IKCP_LOG_IN_DATA, + "input psh: sn=%lu ts=%lu", (unsigned long)sn, (unsigned long)ts); + } + if (_itimediff(sn, kcp->rcv_nxt + kcp->rcv_wnd) < 0) + { + ikcp_ack_push(kcp, sn, ts); + if (_itimediff(sn, kcp->rcv_nxt) >= 0) + { + seg = ikcp_segment_new(kcp, len); + seg->conv = conv; + seg->cmd = cmd; + seg->frg = frg; + seg->wnd = wnd; + seg->ts = ts; + seg->sn = sn; + seg->una = una; + seg->len = len; + + if (len > 0) + { + memcpy(seg->data, data, len); + } + + ikcp_parse_data(kcp, seg); + } + } + } + else if (cmd == IKCP_CMD_WASK) + { + // ready to send back IKCP_CMD_WINS in ikcp_flush + // tell remote my window size + kcp->probe |= IKCP_ASK_TELL; + if (ikcp_canlog(kcp, IKCP_LOG_IN_PROBE)) + { + ikcp_log(kcp, IKCP_LOG_IN_PROBE, "input probe"); + } + } + else if (cmd == IKCP_CMD_WINS) + { + // do nothing + if (ikcp_canlog(kcp, IKCP_LOG_IN_WINS)) + { + ikcp_log(kcp, IKCP_LOG_IN_WINS, + "input wins: %lu", (unsigned long)(wnd)); + } + } + else + { + return -3; + } + + data += len; + size -= len; + } + + if (flag != 0) + { + ikcp_parse_fastack(kcp, maxack, latest_ts); + } + + if (_itimediff(kcp->snd_una, prev_una) > 0) + { + if (kcp->cwnd < kcp->rmt_wnd) + { + IUINT32 mss = kcp->mss; + if (kcp->cwnd < kcp->ssthresh) + { + kcp->cwnd++; + kcp->incr += mss; + } + else + { + if (kcp->incr < mss) + kcp->incr = mss; + kcp->incr += (mss * mss) / kcp->incr + (mss / 16); + if ((kcp->cwnd + 1) * mss <= kcp->incr) + { +#if 1 + kcp->cwnd = (kcp->incr + mss - 1) / ((mss > 0) ? mss : 1); +#else + kcp->cwnd++; +#endif + } + } + if (kcp->cwnd > kcp->rmt_wnd) + { + kcp->cwnd = kcp->rmt_wnd; + kcp->incr = kcp->rmt_wnd * mss; + } + } + } + + return 0; +} + +//--------------------------------------------------------------------- +// ikcp_encode_seg +//--------------------------------------------------------------------- +static char *ikcp_encode_seg(char *ptr, const IKCPSEG *seg) +{ + ptr = ikcp_encode32u(ptr, seg->conv); + ptr = ikcp_encode8u(ptr, (IUINT8)seg->cmd); + ptr = ikcp_encode8u(ptr, (IUINT8)seg->frg); + ptr = ikcp_encode16u(ptr, (IUINT16)seg->wnd); + ptr = ikcp_encode32u(ptr, seg->ts); + ptr = ikcp_encode32u(ptr, seg->sn); + ptr = ikcp_encode32u(ptr, seg->una); + ptr = ikcp_encode32u(ptr, seg->len); + return ptr; +} + +static int ikcp_wnd_unused(const ikcpcb *kcp) +{ + if (kcp->nrcv_que < kcp->rcv_wnd) + { + return kcp->rcv_wnd - kcp->nrcv_que; + } + return 0; +} + +//--------------------------------------------------------------------- +// ikcp_flush +//--------------------------------------------------------------------- +void ikcp_flush(ikcpcb *kcp) +{ + IUINT32 current = kcp->current; + char *buffer = kcp->buffer; + char *ptr = buffer; + int count, size, i; + IUINT32 resent, cwnd; + IUINT32 rtomin; + struct IQUEUEHEAD *p; + int change = 0; + int lost = 0; + IKCPSEG seg; + + // 'ikcp_update' haven't been called. + if (kcp->updated == 0) + return; + + seg.conv = kcp->conv; + seg.cmd = IKCP_CMD_ACK; + seg.frg = 0; + seg.wnd = ikcp_wnd_unused(kcp); + seg.una = kcp->rcv_nxt; + seg.len = 0; + seg.sn = 0; + seg.ts = 0; + + // flush acknowledges + count = kcp->ackcount; + for (i = 0; i < count; i++) + { + size = (int)(ptr - buffer); + if (size + (int)IKCP_OVERHEAD > (int)kcp->mtu) + { + ikcp_output(kcp, buffer, size); + ptr = buffer; + } + ikcp_ack_get(kcp, i, &seg.sn, &seg.ts); + ptr = ikcp_encode_seg(ptr, &seg); + } + + kcp->ackcount = 0; + + // probe window size (if remote window size equals zero) + if (kcp->rmt_wnd == 0) + { + if (kcp->probe_wait == 0) + { + kcp->probe_wait = IKCP_PROBE_INIT; + kcp->ts_probe = kcp->current + kcp->probe_wait; + } + else + { + if (_itimediff(kcp->current, kcp->ts_probe) >= 0) + { + if (kcp->probe_wait < IKCP_PROBE_INIT) + kcp->probe_wait = IKCP_PROBE_INIT; + kcp->probe_wait += kcp->probe_wait / 2; + if (kcp->probe_wait > IKCP_PROBE_LIMIT) + kcp->probe_wait = IKCP_PROBE_LIMIT; + kcp->ts_probe = kcp->current + kcp->probe_wait; + kcp->probe |= IKCP_ASK_SEND; + } + } + } + else + { + kcp->ts_probe = 0; + kcp->probe_wait = 0; + } + + // flush window probing commands + if (kcp->probe & IKCP_ASK_SEND) + { + seg.cmd = IKCP_CMD_WASK; + size = (int)(ptr - buffer); + if (size + (int)IKCP_OVERHEAD > (int)kcp->mtu) + { + ikcp_output(kcp, buffer, size); + ptr = buffer; + } + ptr = ikcp_encode_seg(ptr, &seg); + } + + // flush window probing commands + if (kcp->probe & IKCP_ASK_TELL) + { + seg.cmd = IKCP_CMD_WINS; + size = (int)(ptr - buffer); + if (size + (int)IKCP_OVERHEAD > (int)kcp->mtu) + { + ikcp_output(kcp, buffer, size); + ptr = buffer; + } + ptr = ikcp_encode_seg(ptr, &seg); + } + + kcp->probe = 0; + + // calculate window size + cwnd = _imin_(kcp->snd_wnd, kcp->rmt_wnd); + if (kcp->nocwnd == 0) + cwnd = _imin_(kcp->cwnd, cwnd); + + // move data from snd_queue to snd_buf + while (_itimediff(kcp->snd_nxt, kcp->snd_una + cwnd) < 0) + { + IKCPSEG *newseg; + if (iqueue_is_empty(&kcp->snd_queue)) + break; + + newseg = iqueue_entry(kcp->snd_queue.next, IKCPSEG, node); + + iqueue_del(&newseg->node); + iqueue_add_tail(&newseg->node, &kcp->snd_buf); + kcp->nsnd_que--; + kcp->nsnd_buf++; + + newseg->conv = kcp->conv; + newseg->cmd = IKCP_CMD_PUSH; + newseg->wnd = seg.wnd; + newseg->ts = current; + newseg->sn = kcp->snd_nxt++; + newseg->una = kcp->rcv_nxt; + newseg->resendts = current; + newseg->rto = kcp->rx_rto; + newseg->fastack = 0; + newseg->xmit = 0; + } + + // calculate resent + resent = (kcp->fastresend > 0) ? (IUINT32)kcp->fastresend : 0xffffffff; + rtomin = (kcp->nodelay == 0) ? (kcp->rx_rto >> 3) : 0; + + // flush data segments + for (p = kcp->snd_buf.next; p != &kcp->snd_buf; p = p->next) + { + IKCPSEG *segment = iqueue_entry(p, IKCPSEG, node); + int needsend = 0; + if (segment->xmit == 0) + { + needsend = 1; + segment->xmit++; + segment->rto = kcp->rx_rto; + segment->resendts = current + segment->rto + rtomin; + } + else if (_itimediff(current, segment->resendts) >= 0) + { + needsend = 1; + segment->xmit++; + kcp->xmit++; + kcp->timeout_retrans_total++; + if (kcp->nodelay == 0) + { + segment->rto += _imax_(segment->rto, (IUINT32)kcp->rx_rto); + } + else + { + IINT32 step = (kcp->nodelay < 2) ? ((IINT32)(segment->rto)) : kcp->rx_rto; + segment->rto += step / 2; + } + segment->resendts = current + segment->rto; + lost = 1; + } + else if (segment->fastack >= resent) + { + if ((int)segment->xmit <= kcp->fastlimit || + kcp->fastlimit <= 0) + { + needsend = 1; + segment->xmit++; + kcp->fast_retrans_total++; + segment->fastack = 0; + segment->resendts = current + segment->rto; + change++; + } + } + + if (needsend) + { + int need; + segment->ts = current; + segment->wnd = seg.wnd; + segment->una = kcp->rcv_nxt; + + size = (int)(ptr - buffer); + need = IKCP_OVERHEAD + segment->len; + + if (size + need > (int)kcp->mtu) + { + ikcp_output(kcp, buffer, size); + ptr = buffer; + } + + ptr = ikcp_encode_seg(ptr, segment); + + if (segment->len > 0) + { + memcpy(ptr, segment->data, segment->len); + ptr += segment->len; + } + + if (segment->xmit >= kcp->dead_link) + { + kcp->state = (IUINT32)-1; + } + } + } + + // flash remain segments + size = (int)(ptr - buffer); + if (size > 0) + { + ikcp_output(kcp, buffer, size); + } + + // update ssthresh + if (change) + { + IUINT32 inflight = kcp->snd_nxt - kcp->snd_una; + kcp->ssthresh = inflight / 2; + if (kcp->ssthresh < IKCP_THRESH_MIN) + kcp->ssthresh = IKCP_THRESH_MIN; + kcp->cwnd = kcp->ssthresh + resent; + kcp->incr = kcp->cwnd * kcp->mss; + } + + if (lost) + { + kcp->ssthresh = cwnd / 2; + if (kcp->ssthresh < IKCP_THRESH_MIN) + kcp->ssthresh = IKCP_THRESH_MIN; + kcp->cwnd = 1; + kcp->incr = kcp->mss; + } + + if (kcp->cwnd < 1) + { + kcp->cwnd = 1; + kcp->incr = kcp->mss; + } +} + +//--------------------------------------------------------------------- +// update state (call it repeatedly, every 10ms-100ms), or you can ask +// ikcp_check when to call it again (without ikcp_input/_send calling). +// 'current' - current timestamp in millisec. +//--------------------------------------------------------------------- +void ikcp_update(ikcpcb *kcp, IUINT32 current) +{ + IINT32 slap; + + kcp->current = current; + + if (kcp->updated == 0) + { + kcp->updated = 1; + kcp->ts_flush = kcp->current; + } + + slap = _itimediff(kcp->current, kcp->ts_flush); + + if (slap >= 10000 || slap < -10000) + { + kcp->ts_flush = kcp->current; + slap = 0; + } + + if (slap >= 0) + { + kcp->ts_flush += kcp->interval; + if (_itimediff(kcp->current, kcp->ts_flush) >= 0) + { + kcp->ts_flush = kcp->current + kcp->interval; + } + ikcp_flush(kcp); + } +} + +//--------------------------------------------------------------------- +// Determine when should you invoke ikcp_update: +// returns when you should invoke ikcp_update in millisec, if there +// is no ikcp_input/_send calling. you can call ikcp_update in that +// time, instead of call update repeatly. +// Important to reduce unnacessary ikcp_update invoking. use it to +// schedule ikcp_update (eg. implementing an epoll-like mechanism, +// or optimize ikcp_update when handling massive kcp connections) +//--------------------------------------------------------------------- +IUINT32 ikcp_check(const ikcpcb *kcp, IUINT32 current) +{ + IUINT32 ts_flush = kcp->ts_flush; + IINT32 tm_flush = 0x7fffffff; + IINT32 tm_packet = 0x7fffffff; + IUINT32 minimal = 0; + struct IQUEUEHEAD *p; + + if (kcp->updated == 0) + { + return current; + } + + if (_itimediff(current, ts_flush) >= 10000 || + _itimediff(current, ts_flush) < -10000) + { + ts_flush = current; + } + + if (_itimediff(current, ts_flush) >= 0) + { + return current; + } + + tm_flush = _itimediff(ts_flush, current); + + for (p = kcp->snd_buf.next; p != &kcp->snd_buf; p = p->next) + { + const IKCPSEG *seg = iqueue_entry(p, const IKCPSEG, node); + IINT32 diff = _itimediff(seg->resendts, current); + if (diff <= 0) + { + return current; + } + if (diff < tm_packet) + tm_packet = diff; + } + + minimal = (IUINT32)(tm_packet < tm_flush ? tm_packet : tm_flush); + if (minimal >= kcp->interval) + minimal = kcp->interval; + + return current + minimal; +} + +int ikcp_setmtu(ikcpcb *kcp, int mtu) +{ + char *buffer; + if (mtu < 50 || mtu < (int)IKCP_OVERHEAD) + return -1; + buffer = (char *)ikcp_malloc((mtu + IKCP_OVERHEAD) * 3); + if (buffer == NULL) + return -2; + kcp->mtu = mtu; + kcp->mss = kcp->mtu - IKCP_OVERHEAD; + ikcp_free(kcp->buffer); + kcp->buffer = buffer; + return 0; +} + +int ikcp_interval(ikcpcb *kcp, int interval) +{ + if (interval > 5000) + interval = 5000; + else if (interval < 10) + interval = 10; + kcp->interval = interval; + return 0; +} + +int ikcp_nodelay(ikcpcb *kcp, int nodelay, int interval, int resend, int nc) +{ + if (nodelay >= 0) + { + kcp->nodelay = nodelay; + if (nodelay) + { + kcp->rx_minrto = IKCP_RTO_NDL; + } + else + { + kcp->rx_minrto = IKCP_RTO_MIN; + } + } + if (interval >= 0) + { + if (interval > 5000) + interval = 5000; + else if (interval < 10) + interval = 10; + kcp->interval = interval; + } + if (resend >= 0) + { + kcp->fastresend = resend; + } + if (nc >= 0) + { + kcp->nocwnd = nc; + } + return 0; +} + +int ikcp_wndsize(ikcpcb *kcp, int sndwnd, int rcvwnd) +{ + if (kcp) + { + if (sndwnd > 0) + { + kcp->snd_wnd = sndwnd; + } + if (rcvwnd > 0) + { // must >= max fragment size + kcp->rcv_wnd = _imax_(rcvwnd, IKCP_WND_RCV); + } + } + return 0; +} + +int ikcp_waitsnd(const ikcpcb *kcp) +{ + return kcp->nsnd_buf + kcp->nsnd_que; +} + +// read conv +IUINT32 ikcp_getconv(const void *ptr) +{ + IUINT32 conv; + ikcp_decode32u((const char *)ptr, &conv); + return conv; +} diff --git a/robot/ros2/OmniSocketGo_robot_ros/third_party/kcp/ikcp.h b/robot/ros2/OmniSocketGo_robot_ros/third_party/kcp/ikcp.h new file mode 100644 index 0000000..54106f2 --- /dev/null +++ b/robot/ros2/OmniSocketGo_robot_ros/third_party/kcp/ikcp.h @@ -0,0 +1,421 @@ +//===================================================================== +// +// KCP - A Better ARQ Protocol Implementation +// skywind3000 (at) gmail.com, 2010-2011 +// +// Features: +// + Average RTT reduce 30% - 40% vs traditional ARQ like tcp. +// + Maximum RTT reduce three times vs tcp. +// + Lightweight, distributed as a single source file. +// +//===================================================================== +#ifndef __IKCP_H__ +#define __IKCP_H__ + +#include +#include +#include + +//===================================================================== +// 32BIT INTEGER DEFINITION +//===================================================================== +#ifndef __INTEGER_32_BITS__ +#define __INTEGER_32_BITS__ +#if defined(_WIN64) || defined(WIN64) || defined(__amd64__) || \ + defined(__x86_64) || defined(__x86_64__) || defined(_M_IA64) || \ + defined(_M_AMD64) +typedef unsigned int ISTDUINT32; +typedef int ISTDINT32; +#elif defined(_WIN32) || defined(WIN32) || defined(__i386__) || \ + defined(__i386) || defined(_M_X86) +typedef unsigned long ISTDUINT32; +typedef long ISTDINT32; +#elif defined(__MACOS__) +typedef UInt32 ISTDUINT32; +typedef SInt32 ISTDINT32; +#elif defined(__APPLE__) && defined(__MACH__) +#include +typedef u_int32_t ISTDUINT32; +typedef int32_t ISTDINT32; +#elif defined(__BEOS__) +#include +typedef u_int32_t ISTDUINT32; +typedef int32_t ISTDINT32; +#elif (defined(_MSC_VER) || defined(__BORLANDC__)) && (!defined(__MSDOS__)) +typedef unsigned __int32 ISTDUINT32; +typedef __int32 ISTDINT32; +#elif defined(__GNUC__) +#include +typedef uint32_t ISTDUINT32; +typedef int32_t ISTDINT32; +#else +typedef unsigned long ISTDUINT32; +typedef long ISTDINT32; +#endif +#endif + +//===================================================================== +// Integer Definition +//===================================================================== +#ifndef __IINT8_DEFINED +#define __IINT8_DEFINED +typedef char IINT8; +#endif + +#ifndef __IUINT8_DEFINED +#define __IUINT8_DEFINED +typedef unsigned char IUINT8; +#endif + +#ifndef __IUINT16_DEFINED +#define __IUINT16_DEFINED +typedef unsigned short IUINT16; +#endif + +#ifndef __IINT16_DEFINED +#define __IINT16_DEFINED +typedef short IINT16; +#endif + +#ifndef __IINT32_DEFINED +#define __IINT32_DEFINED +typedef ISTDINT32 IINT32; +#endif + +#ifndef __IUINT32_DEFINED +#define __IUINT32_DEFINED +typedef ISTDUINT32 IUINT32; +#endif + +#ifndef __IINT64_DEFINED +#define __IINT64_DEFINED +#if defined(_MSC_VER) || defined(__BORLANDC__) +typedef __int64 IINT64; +#else +typedef long long IINT64; +#endif +#endif + +#ifndef __IUINT64_DEFINED +#define __IUINT64_DEFINED +#if defined(_MSC_VER) || defined(__BORLANDC__) +typedef unsigned __int64 IUINT64; +#else +typedef unsigned long long IUINT64; +#endif +#endif + +#ifndef INLINE +#if defined(__GNUC__) + +#if (__GNUC__ > 3) || ((__GNUC__ == 3) && (__GNUC_MINOR__ >= 1)) +#define INLINE __inline__ __attribute__((always_inline)) +#else +#define INLINE __inline__ +#endif + +#elif (defined(_MSC_VER) || defined(__BORLANDC__) || defined(__WATCOMC__)) +#define INLINE __inline +#else +#define INLINE +#endif +#endif + +#if (!defined(__cplusplus)) && (!defined(inline)) +#define inline INLINE +#endif + +//===================================================================== +// QUEUE DEFINITION +//===================================================================== +#ifndef __IQUEUE_DEF__ +#define __IQUEUE_DEF__ + +struct IQUEUEHEAD +{ + struct IQUEUEHEAD *next, *prev; +}; + +typedef struct IQUEUEHEAD iqueue_head; + +//--------------------------------------------------------------------- +// queue init +//--------------------------------------------------------------------- +#define IQUEUE_HEAD_INIT(name) {&(name), &(name)} +#define IQUEUE_HEAD(name) \ + struct IQUEUEHEAD name = IQUEUE_HEAD_INIT(name) + +#define IQUEUE_INIT(ptr) ( \ + (ptr)->next = (ptr), (ptr)->prev = (ptr)) + +#define IOFFSETOF(TYPE, MEMBER) ((size_t)&((TYPE *)0)->MEMBER) + +#define ICONTAINEROF(ptr, type, member) ( \ + (type *)(((char *)((type *)ptr)) - IOFFSETOF(type, member))) + +#define IQUEUE_ENTRY(ptr, type, member) ICONTAINEROF(ptr, type, member) + +//--------------------------------------------------------------------- +// queue operation +//--------------------------------------------------------------------- +#define IQUEUE_ADD(node, head) ( \ + (node)->prev = (head), (node)->next = (head)->next, \ + (head)->next->prev = (node), (head)->next = (node)) + +#define IQUEUE_ADD_TAIL(node, head) ( \ + (node)->prev = (head)->prev, (node)->next = (head), \ + (head)->prev->next = (node), (head)->prev = (node)) + +#define IQUEUE_DEL_BETWEEN(p, n) ((n)->prev = (p), (p)->next = (n)) + +#define IQUEUE_DEL(entry) ( \ + (entry)->next->prev = (entry)->prev, \ + (entry)->prev->next = (entry)->next, \ + (entry)->next = 0, (entry)->prev = 0) + +#define IQUEUE_DEL_INIT(entry) \ + do \ + { \ + IQUEUE_DEL(entry); \ + IQUEUE_INIT(entry); \ + } while (0) + +#define IQUEUE_IS_EMPTY(entry) ((entry) == (entry)->next) + +#define iqueue_init IQUEUE_INIT +#define iqueue_entry IQUEUE_ENTRY +#define iqueue_add IQUEUE_ADD +#define iqueue_add_tail IQUEUE_ADD_TAIL +#define iqueue_del IQUEUE_DEL +#define iqueue_del_init IQUEUE_DEL_INIT +#define iqueue_is_empty IQUEUE_IS_EMPTY + +#define IQUEUE_FOREACH(iterator, head, TYPE, MEMBER) \ + for ((iterator) = iqueue_entry((head)->next, TYPE, MEMBER); \ + &((iterator)->MEMBER) != (head); \ + (iterator) = iqueue_entry((iterator)->MEMBER.next, TYPE, MEMBER)) + +#define iqueue_foreach(iterator, head, TYPE, MEMBER) \ + IQUEUE_FOREACH(iterator, head, TYPE, MEMBER) + +#define iqueue_foreach_entry(pos, head) \ + for ((pos) = (head)->next; (pos) != (head); (pos) = (pos)->next) + +#define __iqueue_splice(list, head) \ + do \ + { \ + iqueue_head *first = (list)->next, *last = (list)->prev; \ + iqueue_head *at = (head)->next; \ + (first)->prev = (head), (head)->next = (first); \ + (last)->next = (at), (at)->prev = (last); \ + } while (0) + +#define iqueue_splice(list, head) \ + do \ + { \ + if (!iqueue_is_empty(list)) \ + __iqueue_splice(list, head); \ + } while (0) + +#define iqueue_splice_init(list, head) \ + do \ + { \ + iqueue_splice(list, head); \ + iqueue_init(list); \ + } while (0) + +#ifdef _MSC_VER +#pragma warning(disable : 4311) +#pragma warning(disable : 4312) +#pragma warning(disable : 4996) +#endif + +#endif + +//--------------------------------------------------------------------- +// BYTE ORDER & ALIGNMENT +//--------------------------------------------------------------------- +#ifndef IWORDS_BIG_ENDIAN +#ifdef _BIG_ENDIAN_ +#if _BIG_ENDIAN_ +#define IWORDS_BIG_ENDIAN 1 +#endif +#endif +#ifndef IWORDS_BIG_ENDIAN +#if defined(__hppa__) || \ + defined(__m68k__) || defined(mc68000) || defined(_M_M68K) || \ + (defined(__MIPS__) && defined(__MIPSEB__)) || \ + defined(__ppc__) || defined(__POWERPC__) || defined(_M_PPC) || \ + defined(__sparc__) || defined(__powerpc__) || \ + defined(__mc68000__) || defined(__s390x__) || defined(__s390__) +#define IWORDS_BIG_ENDIAN 1 +#endif +#endif +#ifndef IWORDS_BIG_ENDIAN +#define IWORDS_BIG_ENDIAN 0 +#endif +#endif + +#ifndef IWORDS_MUST_ALIGN +#if defined(__i386__) || defined(__i386) || defined(_i386_) +#define IWORDS_MUST_ALIGN 0 +#elif defined(_M_IX86) || defined(_X86_) || defined(__x86_64__) +#define IWORDS_MUST_ALIGN 0 +#elif defined(__amd64) || defined(__amd64__) +#define IWORDS_MUST_ALIGN 0 +#else +#define IWORDS_MUST_ALIGN 1 +#endif +#endif + +//===================================================================== +// SEGMENT +//===================================================================== +struct IKCPSEG +{ + struct IQUEUEHEAD node; + IUINT32 conv; + IUINT32 cmd; + IUINT32 frg; + IUINT32 wnd; + IUINT32 ts; + IUINT32 sn; + IUINT32 una; + IUINT32 len; + IUINT32 resendts; + IUINT32 rto; + IUINT32 fastack; + IUINT32 xmit; + char data[1]; +}; + +//--------------------------------------------------------------------- +// IKCPCB +//--------------------------------------------------------------------- +struct IKCPCB +{ + IUINT32 conv, mtu, mss, state; + IUINT32 snd_una, snd_nxt, rcv_nxt; + IUINT32 ts_recent, ts_lastack, ssthresh; + 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 nrcv_buf, nsnd_buf; + IUINT32 nrcv_que, nsnd_que; + IUINT32 nodelay, updated; + IUINT32 ts_probe, probe_wait; + IUINT32 dead_link, incr; + struct IQUEUEHEAD snd_queue; + struct IQUEUEHEAD rcv_queue; + struct IQUEUEHEAD snd_buf; + struct IQUEUEHEAD rcv_buf; + IUINT32 *acklist; + IUINT32 ackcount; + IUINT32 ackblock; + IUINT64 timeout_retrans_total; + IUINT64 fast_retrans_total; + IUINT64 duplicate_recv_total; + void *user; + char *buffer; + int fastresend; + int fastlimit; + int nocwnd, stream; + int logmask; + int (*output)(const char *buf, int len, struct IKCPCB *kcp, void *user); + void (*writelog)(const char *log, struct IKCPCB *kcp, void *user); +}; + +typedef struct IKCPCB ikcpcb; + +#define IKCP_LOG_OUTPUT 1 +#define IKCP_LOG_INPUT 2 +#define IKCP_LOG_SEND 4 +#define IKCP_LOG_RECV 8 +#define IKCP_LOG_IN_DATA 16 +#define IKCP_LOG_IN_ACK 32 +#define IKCP_LOG_IN_PROBE 64 +#define IKCP_LOG_IN_WINS 128 +#define IKCP_LOG_OUT_DATA 256 +#define IKCP_LOG_OUT_ACK 512 +#define IKCP_LOG_OUT_PROBE 1024 +#define IKCP_LOG_OUT_WINS 2048 + +#ifdef __cplusplus +extern "C" +{ +#endif + + //--------------------------------------------------------------------- + // interface + //--------------------------------------------------------------------- + + // create a new kcp control object, 'conv' must equal in two endpoint + // from the same connection. 'user' will be passed to the output callback + // output callback can be setup like this: 'kcp->output = my_udp_output' + ikcpcb *ikcp_create(IUINT32 conv, void *user); + + // release kcp control object + void ikcp_release(ikcpcb *kcp); + + // set output callback, which will be invoked by kcp + void ikcp_setoutput(ikcpcb *kcp, int (*output)(const char *buf, int len, + ikcpcb *kcp, void *user)); + + // user/upper level recv: returns size, returns below zero for EAGAIN + int ikcp_recv(ikcpcb *kcp, char *buffer, int len); + + // user/upper level send, returns below zero for error + int ikcp_send(ikcpcb *kcp, const char *buffer, int len); + + // update state (call it repeatedly, every 10ms-100ms), or you can ask + // ikcp_check when to call it again (without ikcp_input/_send calling). + // 'current' - current timestamp in millisec. + void ikcp_update(ikcpcb *kcp, IUINT32 current); + + // Determine when should you invoke ikcp_update: + // returns when you should invoke ikcp_update in millisec, if there + // is no ikcp_input/_send calling. you can call ikcp_update in that + // time, instead of call update repeatly. + // Important to reduce unnacessary ikcp_update invoking. use it to + // schedule ikcp_update (eg. implementing an epoll-like mechanism, + // or optimize ikcp_update when handling massive kcp connections) + IUINT32 ikcp_check(const ikcpcb *kcp, IUINT32 current); + + // when you received a low level packet (eg. UDP packet), call it + int ikcp_input(ikcpcb *kcp, const char *data, long size); + + // flush pending data + void ikcp_flush(ikcpcb *kcp); + + // check the size of next message in the recv queue + int ikcp_peeksize(const ikcpcb *kcp); + + // change MTU size, default is 1400 + int ikcp_setmtu(ikcpcb *kcp, int mtu); + + // set maximum window size: sndwnd=32, rcvwnd=32 by default + int ikcp_wndsize(ikcpcb *kcp, int sndwnd, int rcvwnd); + + // get how many packet is waiting to be sent + int ikcp_waitsnd(const ikcpcb *kcp); + + // fastest: ikcp_nodelay(kcp, 1, 20, 2, 1) + // nodelay: 0:disable(default), 1:enable + // interval: internal update timer interval in millisec, default is 100ms + // resend: 0:disable fast resend(default), 1:enable fast resend + // nc: 0:normal congestion control(default), 1:disable congestion control + int ikcp_nodelay(ikcpcb *kcp, int nodelay, int interval, int resend, int nc); + + void ikcp_log(ikcpcb *kcp, int mask, const char *fmt, ...); + + // setup allocator + void ikcp_allocator(void *(*new_malloc)(size_t), void (*new_free)(void *)); + + // read conv + IUINT32 ikcp_getconv(const void *ptr); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/ros2/README.md b/robot/ros2/README.md new file mode 100644 index 0000000..5b9ea7c --- /dev/null +++ b/robot/ros2/README.md @@ -0,0 +1,27 @@ +# Robot ROS 2 release + +This release is `OmniSocketGo_robot_ros`. It retains the C/KCP transport and +adds `ros2/omnisocket_camera_bridge`, which subscribes to the Orbbec RGB topics +and publishes latest-frame shared-memory slots for `b_side_omnid`. + +Default acquisition is ROS 2, so `b_side_omnid` does not open `/dev/video*`. +The bridge can keep RGB/depth/CameraInfo/metadata consumers in the ROS 2 graph +while the encoded RGB stream is sent over the existing KCP protocol. The +optional V4L2 mode is only a fallback; select it explicitly with the documented +environment variable and never run it alongside a second camera opener. + +On the robot: + +```bash +cd OmniSocketGo_robot_ros +make +``` + +Then follow: + +- `OmniSocketGo_robot_ros/docs/ROS2_CAMERA_FORWARDING.md` +- `OmniSocketGo_robot_ros/docs/ROS_CAMERA_INTERFACES.md` +- `OmniSocketGo_robot_ros/ros2/README.md` + +The ROS 2 build must be performed on the robot (or another compatible Linux +ARM64 environment) with the robot's ROS 2 Jazzy workspace sourced. diff --git a/robot/v4l2/OmniSocketGo_robot/.gitignore b/robot/v4l2/OmniSocketGo_robot/.gitignore new file mode 100644 index 0000000..162f4de --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/.gitignore @@ -0,0 +1,34 @@ +bin/* +inbox/* +*.jsonl +*.html +peer-b-latency.* + + +*.bin +.vscode/settings.json +*.log +root@117.78.11.244 + +c/bin + +*__pycache__* + +/python/build +/python/omnisocket.egg-info + +*.so* + +/.venv + +**/build/ + +ros-control-py/install +ros-control-py/log +scripts/boot/modem_network_info.json + +logs/ + +# Machine-specific runtime configuration. +/scripts/dev/robot-remote.env.local +/scripts/boot/robot-boot.env.local diff --git a/robot/v4l2/OmniSocketGo_robot/Makefile b/robot/v4l2/OmniSocketGo_robot/Makefile new file mode 100644 index 0000000..1fc8978 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/Makefile @@ -0,0 +1,112 @@ +CC ?= gcc +CFLAGS ?= -std=c11 -Wall -Wextra -O2 -pthread -D_GNU_SOURCE +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 + +COMMON_SRCS := \ + $(SRC_DIR)/omni_common.c \ + $(SRC_DIR)/protocol.c \ + $(SRC_DIR)/latencylog.c \ + $(SRC_DIR)/tx_timestamp_debug.c \ + $(SRC_DIR)/kcp_packet_debug.c \ + $(SRC_DIR)/kcp_session_stats.c \ + $(SRC_DIR)/linux_timestamping.c \ + $(SRC_DIR)/interactive.c \ + $(SRC_DIR)/transport_udp.c \ + $(SRC_DIR)/transport_kcp.c \ + $(SRC_DIR)/server_udp_relay.c \ + $(SRC_DIR)/server_udp_hub.c \ + $(SRC_DIR)/server_kcp_hub.c \ + $(SRC_DIR)/peer_udp_client.c \ + $(SRC_DIR)/peer_kcp_client.c \ + third_party/cjson/cJSON.c \ + third_party/kcp/ikcp.c + +TARGETS := \ + $(BIN_DIR)/udpserver \ + $(BIN_DIR)/udppeer \ + $(BIN_DIR)/udpping \ + $(BIN_DIR)/udprelay \ + $(BIN_DIR)/kcpserver \ + $(BIN_DIR)/kcppeer \ + $(BIN_DIR)/kcpping + +CAMERA_VIDEO_SENDER := $(BIN_DIR)/camera_video_sender +FFMPEG_PIPELINE_COMMON_SRCS := \ + $(SRC_DIR)/video_pipeline.c \ + $(SRC_DIR)/gps_buffer.c \ + $(SRC_DIR)/omni_common.c \ + $(SRC_DIR)/protocol.c \ + $(SRC_DIR)/latencylog.c \ + $(SRC_DIR)/kcp_packet_debug.c \ + $(SRC_DIR)/kcp_session_stats.c \ + $(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 + +CAMERA_VIDEO_SENDER_SRCS := \ + $(CMD_DIR)/v1_camera_pipeline_ifdef.c \ + $(FFMPEG_PIPELINE_COMMON_SRCS) + +B_SIDE_OMNID := $(BIN_DIR)/b_side_omnid +B_SIDE_OMNID_SRCS := \ + $(CMD_DIR)/b_side_omnid.c \ + $(FFMPEG_PIPELINE_COMMON_SRCS) + +all: $(TARGETS) + +$(BIN_DIR): + mkdir -p $(BIN_DIR) + +$(BIN_DIR)/udpserver: $(CMD_DIR)/udpserver.c $(COMMON_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) -o $@ $^ $(LDFLAGS) + +$(BIN_DIR)/udppeer: $(CMD_DIR)/udppeer.c $(COMMON_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) -o $@ $^ $(LDFLAGS) + +$(BIN_DIR)/udpping: $(CMD_DIR)/udpping.c $(COMMON_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) -o $@ $^ $(LDFLAGS) + +$(BIN_DIR)/udprelay: $(CMD_DIR)/udprelay.c $(COMMON_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) -o $@ $^ $(LDFLAGS) + +$(BIN_DIR)/kcpserver: $(CMD_DIR)/kcpserver.c $(COMMON_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) -o $@ $^ $(LDFLAGS) + +$(BIN_DIR)/kcppeer: $(CMD_DIR)/kcppeer.c $(COMMON_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) -o $@ $^ $(LDFLAGS) + +$(BIN_DIR)/kcpping: $(CMD_DIR)/kcpping.c $(COMMON_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) -o $@ $^ $(LDFLAGS) + +$(CAMERA_VIDEO_SENDER): $(CAMERA_VIDEO_SENDER_SRCS) | $(BIN_DIR) + $(CC) $(CFLAGS) $(CPPFLAGS) $$(pkg-config --cflags libavformat libavcodec libavutil libswscale) -o $@ $^ $(LDFLAGS) $$(pkg-config --libs libavformat libavcodec libavutil libswscale) -lm + +camera_video_sender: $(CAMERA_VIDEO_SENDER) + +$(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) -lm + +b_side_omnid: $(B_SIDE_OMNID) + +clean: + rm -rf $(BIN_DIR) + +python-ext: + cd python && $(PYTHON) setup.py build_ext --inplace + +python-install: + cd python && $(PYTHON) -m pip install -e . + +.PHONY: all clean python-ext python-install camera_video_sender b_side_omnid diff --git a/robot/v4l2/OmniSocketGo_robot/README.md b/robot/v4l2/OmniSocketGo_robot/README.md new file mode 100644 index 0000000..046c630 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/README.md @@ -0,0 +1,120 @@ +# OmniSocketC + +Linux-only C11 implementation of the UDP/KCP transport stack from `OmniSocketGo`. + +This subtree is intentionally standalone. The Go code stays in place as the behavior reference, while the C implementation builds its own binaries under `c/bin/`. + +## Build + +```bash +make -j$(nproc) +``` + +Build outputs: + +- `./bin/udpserver` +- `./bin/udppeer` +- `./bin/udpping` +- `./bin/udprelay` +- `./bin/kcpserver` +- `./bin/kcppeer` +- `./bin/kcpping` + +Python extension build: + +```bash +make python-ext +make python-install +``` + +## 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 \ + -telemetry-interval 1000ms \ + -kcp-session-stats-log logs/d-kcp-stats.jsonl \ + -kcp-session-stats-interval 1000ms +``` + +For multi-hour runs, keep `-latency-log` and `-kcp-ts-debug-log` off unless you are collecting a short repro trace. + +Relay `C` runs a raw UDP forwarder to `D`: + +```bash +./bin/kcpserver -mode=relay -listen 0.0.0.0:10909 -relay-remote 172.21.32.15:10909 +``` + +Peer `A` dials `D` through relay `C`: + +```bash +./bin/kcppeer -id peer-a -server 172.21.32.15:10909 -relay-via 106.55.173.235:10909 \ + -inbox-dir inbox/a \ + -latency-log logs/a-latency.jsonl \ + -kcp-ts-debug-log logs/a-kcp-ts.jsonl \ + -kcp-session-stats-log logs/a-kcp-stats.jsonl +``` + +Peer `B` dials `D` directly: + +```bash +./bin/kcppeer -id peer-b -server 81.70.156.140:10909 \ + -inbox-dir inbox/b \ + -latency-log logs/b-latency.jsonl \ + -kcp-ts-debug-log logs/b-kcp-ts.jsonl \ + -kcp-session-stats-log logs/b-kcp-stats.jsonl +``` + +Optional ping / echo tools: + +```bash +./bin/kcpping -id peer-a -server 106.55.173.235:10909 -echo +./bin/kcpping -id peer-b -server 81.70.156.140:10909 -to peer-a -count 20 -interval 100ms +./bin/udpserver -listen 0.0.0.0:9001 +./bin/udppeer -id peer-a -server 127.0.0.1:9001 +./bin/udpping -id pinger -server 127.0.0.1:9001 -to peer-a -count 20 +``` + +Python control/video demos use two KCP sessions: + +- `peer-a-ctrl <-> peer-b-ctrl` for small binary control packets +- `peer-b-video -> peer-a-video` for larger binary video frames + +Example demo entry points: + +- `udp_keyboard_sender.py` +- `udp_xbox_sender.py` +- `udp_fsm_controller.py` +- `omnisocket_video_sender.py` +- `omnisocket_video_receiver.py` +- `scripts/kcp_control_benchmark.py` + +Python `recv_into()` note: + +- The writable buffer must be large enough for the full incoming payload. +- If the buffer is too small, `recv_into()` reports the required size but the current frame has already been consumed and is lost. +- For the video demo, keep `video_receiver.buffer_bytes >= video_sender.frame_bytes`. + +## Interactive Commands + +`udppeer` and `kcppeer` support the same interactive shell: + +```text +help +text peer-b hello +text peer-a hi +file peer-a /tmp/test125.bin +quit +``` + +## Notes + +- The C project targets Linux only. +- It preserves the Go wire format for UDP datagrams and KCP stream frames. +- It now supports `binary` payload messages in addition to `text`, `file`, `register`, and `error`. +- Python `Session.recv_into()` is a zero-copy receive helper for already-sized buffers; it does not retain oversized frames for a retry. +- It keeps runtime JSONL logging, UDP TX timestamp debug, KCP packet debug, and KCP session stats. +- Offline `latencysummary` and HTML chart generation are intentionally not migrated. +- No automated C tests are included in this subtree; validation is expected to happen on Linux via `make` and manual smoke tests. diff --git a/robot/v4l2/OmniSocketGo_robot/ROBOT_LAN_README.md b/robot/v4l2/OmniSocketGo_robot/ROBOT_LAN_README.md new file mode 100644 index 0000000..6ac0c4f --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/ROBOT_LAN_README.md @@ -0,0 +1,103 @@ +# OmniSocketGo Robot LAN Package + +This package is preconfigured for a robot connected directly by Ethernet to +the operator computer at `192.168.41.144`. + +## Network topology + +```text +robot Ethernet (for example 192.168.41.145/24) + -> UDP 192.168.41.144:10909 + -> local KCP hub on the operator computer +``` + +All relay settings are empty. Video uses +`peer-b-video -> peer-a-video`; control uses `peer-a-ctrl -> peer-b-ctrl`. + +Both V4L2 cameras are opened and kept streaming: + +- head: `/dev/video26` +- waist: `/dev/video18` + +Only the camera selected by `OMNI_CAMERA_ACTIVE` is encoded and transmitted. +The default is `head`. + +## Robot preparation + +On Ubuntu, install the build/runtime dependencies if they are not already +available: + +```bash +sudo apt-get install build-essential pkg-config \ + libavformat-dev libavcodec-dev libavutil-dev libswscale-dev \ + v4l-utils psmisc +``` + +Configure the robot Ethernet interface in the same subnet as the computer, +for example `192.168.41.145/24`, and verify: + +```bash +ping -c 3 192.168.41.144 +``` + +Then build and check the package: + +```bash +make b_side_omnid +./check-robot-lan.sh +``` + +## Start + +If the robot boot watchdog is already managing an older daemon, stop it first +so it cannot reopen the cameras: + +```bash +sudo systemctl stop blitz-watchdog.service blitz-b-side-omnid.service +``` + +Start the LAN sender: + +```bash +./start-robot-lan.sh +``` + +The camera preflight may stop only the known `orbbec_head.service` and +`orbbec_waist.service` units to release the two devices. It refuses to stop the +whole `proc_manager.service`. + +Successful dual-camera initialization prints: + +```text +[video_pipeline] camera head ready on /dev/video26 +[video_pipeline] camera waist ready on /dev/video18 +``` + +The periodic daemon line should then report `video registered=1`, with +`frames` increasing. For a development start, inspect: + +```bash +python3 -m json.tool logs/runtime/b-side-omnid.status.json +``` + +Expected fields include `video_connected: true`, an increasing +`video_frames_sent`, an empty `video_last_error`, and +`video_active_camera: head`. + +## Select the waist camera at startup + +Edit `scripts/dev/robot-remote.env.local` and set: + +```bash +OMNI_CAMERA_ACTIVE="waist" +``` + +Then restart `start-robot-lan.sh`. Runtime text commands `camera:head` and +`camera:waist` sent from `peer-a-ctrl` also switch the active input without +reopening either camera. + +## Important + +For this direct-LAN validation, use `start-robot-lan.sh`. Do not install the +existing 5G-oriented `blitz-robot.target` boot chain until its modem policy has +been adapted for the target robot. diff --git a/robot/v4l2/OmniSocketGo_robot/check-robot-lan.sh b/robot/v4l2/OmniSocketGo_robot/check-robot-lan.sh new file mode 100644 index 0000000..ce08dca --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/check-robot-lan.sh @@ -0,0 +1,60 @@ +#!/usr/bin/env bash +set -euo pipefail + +PROJECT_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${PROJECT_ROOT}/scripts/dev/load-env.sh" + +failed=0 + +check_empty_relay() { + local name="$1" + local value="${!name:-}" + if [[ -n "${value}" ]]; then + echo "[FAIL] ${name} must be empty, got: ${value}" >&2 + failed=1 + else + echo "[ OK ] ${name}=" + fi +} + +check_camera() { + local name="$1" + local device="$2" + if [[ -e "${device}" ]]; then + echo "[ OK ] ${name} camera: ${device} -> $(readlink -f "${device}")" + else + echo "[FAIL] ${name} camera is missing: ${device}" >&2 + failed=1 + fi +} + +echo "Robot video target: ${OMNI_VIDEO_SERVER_ADDR}" +echo "Robot control target: ${OMNI_CONTROL_SERVER_ADDR}" +check_empty_relay ROBOT_SIDE_OMNISOCKET_RELAY_VIA +check_empty_relay OMNI_VIDEO_RELAY_VIA +check_empty_relay OMNI_CONTROL_RELAY_VIA +check_camera head "${OMNI_CAMERA_HEAD_DEVICE}" +check_camera waist "${OMNI_CAMERA_WAIST_DEVICE}" + +if pkg-config --exists libavformat libavcodec libavutil libswscale; then + echo "[ OK ] FFmpeg development libraries" +else + echo "[FAIL] FFmpeg development libraries are missing" >&2 + failed=1 +fi + +if [[ -x "${PROJECT_ROOT}/bin/b_side_omnid" ]]; then + echo "[ OK ] bin/b_side_omnid is built" +else + echo "[WARN] bin/b_side_omnid is not built; run: make b_side_omnid" +fi + +server_host="${OMNI_VIDEO_SERVER_ADDR%:*}" +if command -v ping >/dev/null 2>&1 && ping -c 1 -W 1 "${server_host}" >/dev/null 2>&1; then + echo "[ OK ] computer is reachable: ${server_host}" +else + echo "[WARN] cannot ping ${server_host}; verify the direct Ethernet addresses" +fi + +exit "${failed}" diff --git a/robot/v4l2/OmniSocketGo_robot/cmd/b_side_omnid.c b/robot/v4l2/OmniSocketGo_robot/cmd/b_side_omnid.c new file mode 100644 index 0000000..b67faf2 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/cmd/b_side_omnid.c @@ -0,0 +1,1338 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cJSON.h" +#include "control_protocol.h" +#include "latencylog.h" +#include "protocol.h" +#include "video_pipeline.h" + +#define CONTROL_DEFAULT_PEER_ID "peer-b-ctrl" +#define CONTROL_DEFAULT_EXPECTED_SENDER "peer-a-ctrl" +#define CONTROL_ACK_DEFAULT_PEER_ID "peer-b-ctrl-ack" +#define CONTROL_ACK_DEFAULT_TARGET_PEER "peer-a-ctrl-ack" +#define CONTROL_DEFAULT_UNIX_SOCKET "/tmp/omnisocket-b-side-cmd.sock" +#define CONTROL_DEFAULT_SERVER_IDLE_RECONNECT_MS 3000 +#define DEFAULT_RUNTIME_DIR "/run/blitz-robot" +#define DEFAULT_STATUS_FILE_NAME "b-side-omnid.status.json" +#define DEFAULT_VIDEO_THREAD_FAULT_FILE "fault-injection-bside-video-thread-stall" +#define DEFAULT_CONTROL_THREAD_FAULT_FILE "fault-injection-bside-control-thread-stall" +#define DEFAULT_THREAD_HEARTBEAT_TIMEOUT_SEC 15 +#define DEFAULT_KCP_STATS_INTERVAL_MS 1000 +#define DEFAULT_CONTROL_LATENCY_SAMPLE_MOD 100 +#define DEFAULT_CONTROL_ACK_SAMPLE_MOD 10 +#define EXIT_CODE_VIDEO_THREAD_STALLED 101 +#define EXIT_CODE_CONTROL_THREAD_STALLED 102 + +typedef struct unix_dgram_client { + int fd; + char bind_path[108]; + char dest_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; + uint64_t reconnect_count; + uint32_t server_idle_ms; + int ever_connected; + int registered; + char last_error[256]; + char last_reconnect_reason[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; + atomic_int active_camera; + 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_ack_peer_id; + const char *control_ack_target_peer; + const char *control_unix_socket; + int control_server_idle_reconnect_ms; + const char *runtime_dir; + int heartbeat_timeout_sec; + int stats_interval_ms; + uint64_t control_latency_sample_mod; + uint64_t control_ack_sample_mod; + char status_file_path[512]; + char video_thread_fault_file[512]; + char control_thread_fault_file[512]; + atomic_long video_thread_heartbeat_epoch_sec; + atomic_long control_thread_heartbeat_epoch_sec; + atomic_int control_ack_shutdown_requested; + kcp_session_stats_logger_t *stats_logger; + latency_logger_t *control_latency_logger; + video_stage_logger_t *video_stage_logger; + unix_dgram_client_t unix_client; + control_bridge_stats_t control_stats; + pthread_mutex_t control_ack_mutex; + pthread_t control_ack_thread; + kcp_client_t *control_ack_client; + int control_ack_thread_started; + int control_ack_connect_requested; + int control_ack_connect_inflight; +} daemon_state_t; + +static void control_message_body_to_cstr(const message_t *msg, char *buffer, size_t buffer_len); + +static const char *camera_name(int camera) { + return camera == VIDEO_CAMERA_WAIST ? "waist" : "head"; +} + +static int handle_camera_select_message(daemon_state_t *state, kcp_client_t *client, const message_t *msg) { + char body[64]; + char reply[96]; + int selected; + + if (state == NULL || msg == NULL || msg->type != MSG_TYPE_TEXT) { + return 0; + } + control_message_body_to_cstr(msg, body, sizeof(body)); + if (strcmp(body, "camera:head") == 0 || strcmp(body, "camera.select=head") == 0) { + selected = VIDEO_CAMERA_HEAD; + } else if (strcmp(body, "camera:waist") == 0 || strcmp(body, "camera.select=waist") == 0) { + selected = VIDEO_CAMERA_WAIST; + } else { + return 0; + } + atomic_store(&state->active_camera, selected); + fprintf(stderr, "[b_side_omnid] active camera switched to %s\n", camera_name(selected)); + if (client != NULL && msg->from[0] != '\0') { + snprintf( + reply, + sizeof(reply), + "{\"type\":\"camera.selected\",\"camera\":\"%s\"}", + camera_name(selected) + ); + if (kcp_client_send_text(client, msg->from, reply) != 0) { + fprintf(stderr, "[b_side_omnid] failed to acknowledge camera selection: %s\n", strerror(errno)); + } + } + return 1; +} + +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 env_int_or_default(const char *name, int fallback) { + const char *value = getenv(name); + int parsed; + + if (value == NULL || value[0] == '\0') { + return fallback; + } + parsed = atoi(value); + if (parsed <= 0) { + return fallback; + } + return parsed; +} + +static uint64_t env_u64_or_default(const char *name, uint64_t fallback) { + const char *value = getenv(name); + unsigned long long parsed = 0ULL; + char *endptr = NULL; + + if (value == NULL || value[0] == '\0') { + return fallback; + } + parsed = strtoull(value, &endptr, 10); + if (endptr == value || *endptr != '\0' || parsed == 0ULL) { + return fallback; + } + return (uint64_t) parsed; +} + +static int64_t realtime_epoch_ms(void) { + struct timespec ts; + + clock_gettime(CLOCK_REALTIME, &ts); + return (int64_t) ts.tv_sec * 1000 + ts.tv_nsec / 1000000; +} + +static long realtime_epoch_sec(void) { + return (long) time(NULL); +} + +static void update_thread_heartbeat(atomic_long *heartbeat) { + if (heartbeat == NULL) { + return; + } + atomic_store(heartbeat, realtime_epoch_sec()); +} + +static int should_log_control_latency(const daemon_state_t *state, const message_t *msg) { + uint64_t sample_mod; + + if (state == NULL || state->control_latency_logger == NULL || msg == NULL) { + return 0; + } + sample_mod = state->control_latency_sample_mod; + if (sample_mod <= 1U) { + return 1; + } + return msg->id % sample_mod == 0U; +} + +static int should_send_control_ack(const daemon_state_t *state, const message_t *msg) { + uint64_t sample_mod; + + if (state == NULL || msg == NULL) { + return 0; + } + sample_mod = state->control_ack_sample_mod; + if (sample_mod <= 1U) { + return 1; + } + return msg->id % sample_mod == 0U; +} + +static void video_pipeline_heartbeat_progress(void *context) { + update_thread_heartbeat((atomic_long *) context); +} + +static int ensure_runtime_dir(const char *runtime_dir) { + struct stat st; + + if (runtime_dir == NULL || runtime_dir[0] == '\0') { + errno = EINVAL; + return -1; + } + if (stat(runtime_dir, &st) == 0) { + if (S_ISDIR(st.st_mode)) { + return 0; + } + errno = ENOTDIR; + return -1; + } + if (errno != ENOENT) { + return -1; + } + if (mkdir(runtime_dir, 0775) != 0 && errno != EEXIST) { + return -1; + } + return 0; +} + +static int path_exists(const char *path) { + return path != NULL && path[0] != '\0' && access(path, F_OK) == 0; +} + +static int consume_fault_flag(const char *path) { + if (!path_exists(path)) { + return 0; + } + unlink(path); + return 1; +} + +static void maybe_inject_thread_stall(daemon_state_t *state, const char *fault_path, const char *thread_name) { + if (state == NULL || fault_path == NULL || thread_name == NULL) { + return; + } + if (!consume_fault_flag(fault_path)) { + return; + } + fprintf( + stderr, + "[b_side_omnid] fault injection requested for %s thread, sleeping past %d second heartbeat timeout\n", + thread_name, + state->heartbeat_timeout_sec + ); + sleep((unsigned int) state->heartbeat_timeout_sec + 2U); +} + +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 unix_dgram_client_close(unix_dgram_client_t *client); +static void control_bridge_stats_snapshot(control_bridge_stats_t *stats, control_bridge_stats_t *out_stats); +static void close_control_ack_client(kcp_client_t **client_ptr); + +static int control_ack_enabled(const daemon_state_t *state) { + return state != NULL + && state->control_ack_peer_id != NULL + && state->control_ack_peer_id[0] != '\0' + && state->control_ack_target_peer != NULL + && state->control_ack_target_peer[0] != '\0'; +} + +static int control_ack_manager_init(daemon_state_t *state) { + int rc; + + if (state == NULL) { + errno = EINVAL; + return -1; + } + rc = pthread_mutex_init(&state->control_ack_mutex, NULL); + if (rc != 0) { + errno = rc; + return -1; + } + atomic_init(&state->control_ack_shutdown_requested, 0); + state->control_ack_client = NULL; + state->control_ack_thread_started = 0; + state->control_ack_connect_requested = 0; + state->control_ack_connect_inflight = 0; + return 0; +} + +static void control_ack_manager_reset(daemon_state_t *state, int request_connect) { + kcp_client_t *client = NULL; + + if (state == NULL) { + return; + } + pthread_mutex_lock(&state->control_ack_mutex); + client = state->control_ack_client; + state->control_ack_client = NULL; + state->control_ack_connect_requested = request_connect && control_ack_enabled(state) && state->control_ack_thread_started; + pthread_mutex_unlock(&state->control_ack_mutex); + close_control_ack_client(&client); +} + +static void control_ack_manager_destroy(daemon_state_t *state) { + if (state == NULL) { + return; + } + atomic_store(&state->control_ack_shutdown_requested, 1); + if (state->control_ack_thread_started) { + pthread_join(state->control_ack_thread, NULL); + state->control_ack_thread_started = 0; + } + control_ack_manager_reset(state, 0); + pthread_mutex_destroy(&state->control_ack_mutex); +} + +static int write_status_json_atomic(const char *path, cJSON *root) { + char *json; + char temp_path[640]; + FILE *file; + size_t json_len; + + if (path == NULL || root == NULL) { + errno = EINVAL; + return -1; + } + + json = cJSON_PrintUnformatted(root); + if (json == NULL) { + errno = ENOMEM; + return -1; + } + + snprintf(temp_path, sizeof(temp_path), "%s.tmp.%ld", path, (long) getpid()); + file = fopen(temp_path, "wb"); + if (file == NULL) { + cJSON_free(json); + return -1; + } + + json_len = strlen(json); + if (fwrite(json, 1, json_len, file) != json_len || fflush(file) != 0) { + int saved_errno = errno; + + fclose(file); + unlink(temp_path); + cJSON_free(json); + errno = saved_errno; + return -1; + } + if (fclose(file) != 0) { + int saved_errno = errno; + + unlink(temp_path); + cJSON_free(json); + errno = saved_errno; + return -1; + } + if (rename(temp_path, path) != 0) { + int saved_errno = errno; + + unlink(temp_path); + cJSON_free(json); + errno = saved_errno; + return -1; + } + + cJSON_free(json); + return 0; +} + +static int write_daemon_status_file(daemon_state_t *state) { + cJSON *root; + video_pipeline_stats_t video_stats; + control_bridge_stats_t control_stats; + int rc; + + if (state == NULL) { + errno = EINVAL; + return -1; + } + if (ensure_runtime_dir(state->runtime_dir) != 0) { + return -1; + } + + 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); + + root = cJSON_CreateObject(); + if (root == NULL) { + errno = ENOMEM; + return -1; + } + + cJSON_AddNumberToObject(root, "updated_at_epoch_ms", (double) realtime_epoch_ms()); + cJSON_AddNumberToObject(root, "pid", (double) getpid()); + cJSON_AddNumberToObject(root, "video_thread_heartbeat_epoch_ms", (double) atomic_load(&state->video_thread_heartbeat_epoch_sec) * 1000.0); + cJSON_AddNumberToObject(root, "control_thread_heartbeat_epoch_ms", (double) atomic_load(&state->control_thread_heartbeat_epoch_sec) * 1000.0); + cJSON_AddBoolToObject(root, "video_connected", video_stats.connected != 0); + cJSON_AddNumberToObject(root, "video_frames_sent", (double) video_stats.frames_sent); + cJSON_AddNumberToObject(root, "video_send_errors", (double) video_stats.send_errors); + cJSON_AddNumberToObject(root, "video_backlog_resets", (double) video_stats.backlog_resets); + cJSON_AddNumberToObject(root, "video_last_capture_to_send_ms", (double) video_stats.last_capture_to_send_ms); + cJSON_AddNumberToObject(root, "video_avg_capture_to_send_ms", video_stats.avg_capture_to_send_ms); + cJSON_AddStringToObject(root, "video_active_camera", camera_name(atomic_load(&state->active_camera))); + cJSON_AddStringToObject(root, "video_last_error", video_stats.last_error); + cJSON_AddBoolToObject(root, "control_registered", control_stats.registered != 0); + cJSON_AddNumberToObject(root, "control_reconnect_count", (double) control_stats.reconnect_count); + cJSON_AddNumberToObject(root, "control_unix_send_errors", (double) control_stats.unix_send_errors); + cJSON_AddStringToObject(root, "control_last_error", control_stats.last_error); + + rc = write_status_json_atomic(state->status_file_path, root); + cJSON_Delete(root); + return rc; +} + +static int thread_heartbeat_expired(atomic_long *heartbeat, int timeout_sec, long now_sec) { + long heartbeat_sec; + + if (heartbeat == NULL || timeout_sec <= 0) { + return 0; + } + heartbeat_sec = atomic_load(heartbeat); + if (heartbeat_sec <= 0) { + return 0; + } + return now_sec - heartbeat_sec > timeout_sec; +} + +static void exit_if_thread_stalled(daemon_state_t *state) { + long now_sec; + + if (state == NULL || state->heartbeat_timeout_sec <= 0) { + return; + } + now_sec = realtime_epoch_sec(); + if (thread_heartbeat_expired(&state->video_thread_heartbeat_epoch_sec, state->heartbeat_timeout_sec, now_sec)) { + fprintf(stderr, "[b_side_omnid] video thread heartbeat stalled for more than %d seconds\n", state->heartbeat_timeout_sec); + fflush(stderr); + exit(EXIT_CODE_VIDEO_THREAD_STALLED); + } + if (thread_heartbeat_expired(&state->control_thread_heartbeat_epoch_sec, state->heartbeat_timeout_sec, now_sec)) { + fprintf(stderr, "[b_side_omnid] control thread heartbeat stalled for more than %d seconds\n", state->heartbeat_timeout_sec); + fflush(stderr); + exit(EXIT_CODE_CONTROL_THREAD_STALLED); + } +} + +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_reconnect_reason(control_bridge_stats_t *stats, const char *message) { + if (stats == NULL) { + return; + } + pthread_mutex_lock(&stats->mutex); + snprintf(stats->last_reconnect_reason, sizeof(stats->last_reconnect_reason), "%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->reconnect_count = stats->reconnect_count; + out_stats->server_idle_ms = stats->server_idle_ms; + out_stats->registered = stats->registered; + snprintf(out_stats->last_error, sizeof(out_stats->last_error), "%s", stats->last_error); + snprintf(out_stats->last_reconnect_reason, sizeof(out_stats->last_reconnect_reason), "%s", stats->last_reconnect_reason); + out_stats->transport = stats->transport; + pthread_mutex_unlock(&stats->mutex); +} + +static int control_server_error_requires_reconnect(const char *message) { + if (message == NULL || message[0] == '\0') { + return 0; + } + return strstr(message, "not registered") != NULL + || strstr(message, "first message must be register") != NULL + || strstr(message, "peer replaced") != NULL + || strstr(message, "timed out waiting for server_register_ok") != NULL; +} + +static void control_message_body_to_cstr(const message_t *msg, char *buffer, size_t buffer_len) { + size_t copy_len; + + if (buffer == NULL || buffer_len == 0) { + return; + } + buffer[0] = '\0'; + if (msg == NULL || msg->body == NULL || msg->body_len == 0) { + return; + } + copy_len = msg->body_len < (buffer_len - 1U) ? msg->body_len : (buffer_len - 1U); + memcpy(buffer, msg->body, copy_len); + buffer[copy_len] = '\0'; +} + +static kcp_client_t *connect_control_ack_client(const daemon_state_t *state) { + kcp_conn_options_t options; + + if (state == NULL || state->control_ack_peer_id == NULL || state->control_ack_peer_id[0] == '\0') { + errno = EINVAL; + return NULL; + } + kcp_conn_options_set_control_defaults(&options); + return kcp_client_dial_with_options( + state->control_server_addr, + state->control_relay_via, + state->control_ack_peer_id, + state->control_bind_ip, + state->control_bind_device, + &options, + NULL, + NULL, + state->stats_logger, + state->stats_interval_ms + ); +} + +static void close_control_ack_client(kcp_client_t **client_ptr) { + if (client_ptr == NULL || *client_ptr == NULL) { + return; + } + kcp_client_close(*client_ptr); + kcp_client_free(*client_ptr); + *client_ptr = NULL; +} + +static void control_ack_manager_request_connect(daemon_state_t *state) { + if (state == NULL || !control_ack_enabled(state) || !state->control_ack_thread_started) { + return; + } + pthread_mutex_lock(&state->control_ack_mutex); + if (state->control_ack_client == NULL) { + state->control_ack_connect_requested = 1; + } + pthread_mutex_unlock(&state->control_ack_mutex); +} + +static void *control_ack_thread_main(void *arg) { + daemon_state_t *state = (daemon_state_t *) arg; + + while (!atomic_load(&state->control_ack_shutdown_requested) && !*state->stop_requested) { + kcp_client_t *client = NULL; + int connect_failed = 0; + int should_connect = 0; + + pthread_mutex_lock(&state->control_ack_mutex); + if (state->control_ack_connect_requested && state->control_ack_client == NULL && !state->control_ack_connect_inflight) { + state->control_ack_connect_inflight = 1; + should_connect = 1; + } + pthread_mutex_unlock(&state->control_ack_mutex); + + if (!should_connect) { + usleep(200000); + continue; + } + + client = connect_control_ack_client(state); + connect_failed = client == NULL; + + pthread_mutex_lock(&state->control_ack_mutex); + state->control_ack_connect_inflight = 0; + if ( + client != NULL + && state->control_ack_connect_requested + && state->control_ack_client == NULL + && !atomic_load(&state->control_ack_shutdown_requested) + && !*state->stop_requested + ) { + state->control_ack_client = client; + state->control_ack_connect_requested = 0; + client = NULL; + } + pthread_mutex_unlock(&state->control_ack_mutex); + + if (client != NULL) { + close_control_ack_client(&client); + } + if (connect_failed && !atomic_load(&state->control_ack_shutdown_requested) && !*state->stop_requested) { + sleep(1); + } + } + return NULL; +} + +static void maybe_send_control_ack( + daemon_state_t *state, + const message_t *msg, + int64_t recv_unix_nano, + int64_t persist_end_unix_nano, + const char *sample_reason +) { + kcp_client_t *ack_client = NULL; + kcp_client_t *client_to_close = NULL; + char *payload = NULL; + int send_rc = -1; + + if ( + state == NULL || msg == NULL || recv_unix_nano <= 0 || persist_end_unix_nano <= recv_unix_nano + || !control_ack_enabled(state) || !state->control_ack_thread_started + ) { + return; + } + + payload = omni_strdup_printf( + "{\"message_id\":%" PRIu64 ",\"ack_phase\":\"persist_end\",\"b_recv_to_persist_us\":%" PRId64 ",\"unix_send_ok\":true,\"sample_reason\":\"%s\"}", + msg->id, + (persist_end_unix_nano - recv_unix_nano) / 1000, + sample_reason == NULL ? "sample_mod" : sample_reason + ); + if (payload == NULL) { + return; + } + + pthread_mutex_lock(&state->control_ack_mutex); + ack_client = state->control_ack_client; + if (ack_client == NULL) { + state->control_ack_connect_requested = 1; + pthread_mutex_unlock(&state->control_ack_mutex); + free(payload); + return; + } + send_rc = kcp_client_send_text(ack_client, state->control_ack_target_peer, payload); + if (send_rc != 0) { + client_to_close = state->control_ack_client; + state->control_ack_client = NULL; + state->control_ack_connect_requested = 1; + } + pthread_mutex_unlock(&state->control_ack_mutex); + + free(payload); + if (client_to_close != NULL) { + close_control_ack_client(&client_to_close); + } +} + +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_path, sizeof(client->dest_path), "%s", dest_path); + 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 int unix_dgram_client_reopen(unix_dgram_client_t *client) { + char dest_path[sizeof(client->dest_path)]; + + if (client == NULL || client->dest_path[0] == '\0') { + errno = EINVAL; + return -1; + } + snprintf(dest_path, sizeof(dest_path), "%s", client->dest_path); + unix_dgram_client_close(client); + return unix_dgram_client_init(client, dest_path); +} + +static int unix_dgram_client_should_reopen(int error_code) { + return error_code == ENOENT || error_code == ECONNREFUSED || error_code == EBADF || error_code == ENOTCONN; +} + +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) { + update_thread_heartbeat(&state->video_thread_heartbeat_epoch_sec); + maybe_inject_thread_stall(state, state->video_thread_fault_file, "video"); + int video_rc = video_pipeline_run(&state->video_config, &state->video_stats, state->stop_requested); + update_thread_heartbeat(&state->video_thread_heartbeat_epoch_sec); + + if (video_rc == 0) { + break; + } + if (video_rc == VIDEO_PIPELINE_RUN_RETRY_IMMEDIATE) { + continue; + } + 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; + int reconnect_immediately = 0; + + update_thread_heartbeat(&state->control_thread_heartbeat_epoch_sec); + maybe_inject_thread_stall(state, state->control_thread_fault_file, "control"); + 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, + state->stats_logger, + state->stats_interval_ms + ); + if (client == NULL) { + control_bridge_set_errno_error(&state->control_stats, "failed to connect control session"); + sleep(1); + continue; + } + + { + kcp_client_state_t client_state; + + memset(&client_state, 0, sizeof(client_state)); + kcp_client_state_snapshot(client, &client_state); + pthread_mutex_lock(&state->control_stats.mutex); + if (state->control_stats.ever_connected) { + state->control_stats.reconnect_count += 1; + } else { + state->control_stats.ever_connected = 1; + } + state->control_stats.registered = client_state.registered; + state->control_stats.server_idle_ms = client_state.server_idle_ms; + state->control_stats.last_reconnect_reason[0] = '\0'; + snprintf(state->control_stats.last_error, sizeof(state->control_stats.last_error), "%s", client_state.last_server_error); + kcp_client_runtime_stats_snapshot(client, &state->control_stats.transport); + pthread_mutex_unlock(&state->control_stats.mutex); + } + control_ack_manager_request_connect(state); + + while (!*state->stop_requested) { + message_t msg; + int rc; + kcp_client_state_t client_state; + int ack_sampled = 0; + int log_control_latency = 0; + int64_t recv_unix_nano = 0; + int64_t persist_begin_unix_nano = 0; + int64_t persist_end_unix_nano = 0; + + update_thread_heartbeat(&state->control_thread_heartbeat_epoch_sec); + protocol_message_init(&msg); + rc = kcp_client_receive_timed(client, &msg, 100); + update_thread_heartbeat(&state->control_thread_heartbeat_epoch_sec); + if (rc == 1) { + char reconnect_reason[256]; + + protocol_message_clear(&msg); + memset(&client_state, 0, sizeof(client_state)); + kcp_client_state_snapshot(client, &client_state); + pthread_mutex_lock(&state->control_stats.mutex); + state->control_stats.registered = client_state.registered; + state->control_stats.server_idle_ms = client_state.server_idle_ms; + snprintf(state->control_stats.last_error, sizeof(state->control_stats.last_error), "%s", client_state.last_server_error); + kcp_client_runtime_stats_snapshot(client, &state->control_stats.transport); + pthread_mutex_unlock(&state->control_stats.mutex); + if (!client_state.registered) { + snprintf(reconnect_reason, sizeof(reconnect_reason), "control session stale: server reported unregistered"); + } else if ( + state->control_server_idle_reconnect_ms > 0 + && client_state.server_idle_ms >= (uint32_t) state->control_server_idle_reconnect_ms + ) { + snprintf( + reconnect_reason, + sizeof(reconnect_reason), + "control session stale: server idle timeout (%u ms >= %d ms)", + client_state.server_idle_ms, + state->control_server_idle_reconnect_ms + ); + } else if (control_server_error_requires_reconnect(client_state.last_server_error)) { + snprintf( + reconnect_reason, + sizeof(reconnect_reason), + "control session stale: server error %.180s", + client_state.last_server_error + ); + } else { + reconnect_reason[0] = '\0'; + } + if (reconnect_reason[0] != '\0') { + control_bridge_set_error(&state->control_stats, reconnect_reason); + control_bridge_set_reconnect_reason(&state->control_stats, reconnect_reason); + fprintf(stderr, "[b_side_omnid] %s\n", reconnect_reason); + reconnect_immediately = 1; + break; + } + continue; + } + if (rc != 0) { + memset(&client_state, 0, sizeof(client_state)); + kcp_client_state_snapshot(client, &client_state); + pthread_mutex_lock(&state->control_stats.mutex); + state->control_stats.registered = client_state.registered; + state->control_stats.server_idle_ms = client_state.server_idle_ms; + kcp_client_runtime_stats_snapshot(client, &state->control_stats.transport); + pthread_mutex_unlock(&state->control_stats.mutex); + if (client_state.last_server_error[0] != '\0') { + control_bridge_set_error(&state->control_stats, client_state.last_server_error); + if (control_server_error_requires_reconnect(client_state.last_server_error)) { + control_bridge_set_reconnect_reason(&state->control_stats, client_state.last_server_error); + reconnect_immediately = 1; + } + } else { + control_bridge_set_errno_error(&state->control_stats, "control receive loop stopped"); + } + protocol_message_clear(&msg); + break; + } + + if (msg.type == MSG_TYPE_ERROR && strcmp(msg.from, SERVER_PEER_ID) == 0) { + char server_error[256]; + + control_message_body_to_cstr(&msg, server_error, sizeof(server_error)); + control_bridge_set_error(&state->control_stats, server_error); + if (control_server_error_requires_reconnect(server_error)) { + char reconnect_reason[256]; + + snprintf(reconnect_reason, sizeof(reconnect_reason), "control session stale: server error %.180s", server_error); + control_bridge_set_reconnect_reason(&state->control_stats, reconnect_reason); + fprintf(stderr, "[b_side_omnid] %s\n", reconnect_reason); + reconnect_immediately = 1; + protocol_message_clear(&msg); + break; + } + protocol_message_clear(&msg); + continue; + } + 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 (handle_camera_select_message(state, client, &msg)) { + pthread_mutex_lock(&state->control_stats.mutex); + state->control_stats.packets_forwarded += 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; + } + + ack_sampled = should_send_control_ack(state, &msg); + log_control_latency = ack_sampled || should_log_control_latency(state, &msg); + if (log_control_latency) { + recv_unix_nano = omni_now_unix_nano(); + persist_begin_unix_nano = recv_unix_nano; + latencylog_log_message_event_at( + state->control_latency_logger, + OMNI_NODE_ROLE_PEER, + state->control_peer_id, + EVENT_B_APP_RECV, + recv_unix_nano, + &msg + ); + latencylog_log_message_event_at( + state->control_latency_logger, + OMNI_NODE_ROLE_PEER, + state->control_peer_id, + EVENT_B_PERSIST_BEGIN, + persist_begin_unix_nano, + &msg + ); + } + + if (unix_dgram_client_send(&state->unix_client, msg.body, msg.body_len) != 0) { + int send_errno = errno; + int recovered = 0; + + if (unix_dgram_client_should_reopen(send_errno) && unix_dgram_client_reopen(&state->unix_client) == 0) { + recovered = unix_dgram_client_send(&state->unix_client, msg.body, msg.body_len) == 0; + } + if (recovered) { + memset(&client_state, 0, sizeof(client_state)); + kcp_client_state_snapshot(client, &client_state); + pthread_mutex_lock(&state->control_stats.mutex); + state->control_stats.packets_forwarded += 1; + state->control_stats.registered = client_state.registered; + state->control_stats.server_idle_ms = client_state.server_idle_ms; + kcp_client_runtime_stats_snapshot(client, &state->control_stats.transport); + pthread_mutex_unlock(&state->control_stats.mutex); + if (log_control_latency) { + persist_end_unix_nano = omni_now_unix_nano(); + latencylog_log_message_event_at( + state->control_latency_logger, + OMNI_NODE_ROLE_PEER, + state->control_peer_id, + EVENT_B_PERSIST_END, + persist_end_unix_nano, + &msg + ); + } + if (ack_sampled) { + maybe_send_control_ack(state, &msg, recv_unix_nano, persist_end_unix_nano, "sample_mod"); + } + protocol_message_clear(&msg); + continue; + } + errno = send_errno; + 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; + } + + memset(&client_state, 0, sizeof(client_state)); + kcp_client_state_snapshot(client, &client_state); + pthread_mutex_lock(&state->control_stats.mutex); + state->control_stats.packets_forwarded += 1; + state->control_stats.registered = client_state.registered; + state->control_stats.server_idle_ms = client_state.server_idle_ms; + kcp_client_runtime_stats_snapshot(client, &state->control_stats.transport); + pthread_mutex_unlock(&state->control_stats.mutex); + if (log_control_latency) { + persist_end_unix_nano = omni_now_unix_nano(); + latencylog_log_message_event_at( + state->control_latency_logger, + OMNI_NODE_ROLE_PEER, + state->control_peer_id, + EVENT_B_PERSIST_END, + persist_end_unix_nano, + &msg + ); + } + if (ack_sampled) { + maybe_send_control_ack(state, &msg, recv_unix_nano, persist_end_unix_nano, "sample_mod"); + } + protocol_message_clear(&msg); + } + + pthread_mutex_lock(&state->control_stats.mutex); + state->control_stats.registered = 0; + state->control_stats.server_idle_ms = 0; + pthread_mutex_unlock(&state->control_stats.mutex); + control_ack_manager_reset(state, 0); + kcp_client_close(client); + kcp_client_free(client); + if (!*state->stop_requested && !reconnect_immediately) { + 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 registered=%d frames=%llu bytes=%llu drops=%llu resets=%llu backlog=%u cap2send=%ums avg=%.1fms reason=%s srtt=%dms | control registered=%d idle=%ums reconnects=%llu forwarded=%llu invalid=%llu unix_err=%llu srtt=%dms last_reconnect=%s\n", + video_stats.connected, + (unsigned long long) video_stats.frames_sent, + (unsigned long long) video_stats.bytes_sent, + (unsigned long long) video_stats.backpressure_drops, + (unsigned long long) video_stats.backlog_resets, + video_stats.last_backlog_segments, + video_stats.last_capture_to_send_ms, + video_stats.avg_capture_to_send_ms, + video_stats.last_backlog_reason[0] == '\0' ? "-" : video_stats.last_backlog_reason, + video_stats.transport.srtt_ms, + control_stats.registered, + control_stats.server_idle_ms, + (unsigned long long) control_stats.reconnect_count, + (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, + control_stats.last_reconnect_reason[0] == '\0' ? "-" : control_stats.last_reconnect_reason + ); +} + +int main(void) { + daemon_state_t state; + pthread_t video_thread; + pthread_t control_thread; + long initial_heartbeat; + + 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); + atomic_init( + &state.active_camera, + strcmp(env_or_default("OMNI_CAMERA_ACTIVE", "head"), "waist") == 0 + ? VIDEO_CAMERA_WAIST + : VIDEO_CAMERA_HEAD + ); + state.video_config.active_camera = &state.active_camera; + 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_ack_peer_id = env_or_default("OMNI_CONTROL_ACK_PEER_ID", CONTROL_ACK_DEFAULT_PEER_ID); + state.control_ack_target_peer = env_or_default("OMNI_CONTROL_ACK_TARGET_PEER", CONTROL_ACK_DEFAULT_TARGET_PEER); + state.control_unix_socket = env_or_default("OMNI_CONTROL_UNIX_SOCKET_PATH", CONTROL_DEFAULT_UNIX_SOCKET); + state.runtime_dir = env_or_default("BLITZ_RUNTIME_DIR", DEFAULT_RUNTIME_DIR); + state.heartbeat_timeout_sec = env_int_or_default( + "BLITZ_OMNID_THREAD_HEARTBEAT_TIMEOUT_SEC", + DEFAULT_THREAD_HEARTBEAT_TIMEOUT_SEC + ); + state.stats_interval_ms = env_int_or_default("BLITZ_KCP_STATS_INTERVAL_MS", DEFAULT_KCP_STATS_INTERVAL_MS); + state.control_latency_sample_mod = env_u64_or_default("BLITZ_CONTROL_LATENCY_LOG_SAMPLE_MOD", DEFAULT_CONTROL_LATENCY_SAMPLE_MOD); + state.control_ack_sample_mod = env_u64_or_default("BLITZ_CONTROL_ACK_SAMPLE_MOD", DEFAULT_CONTROL_ACK_SAMPLE_MOD); + state.video_config.progress_callback = video_pipeline_heartbeat_progress; + state.video_config.progress_context = &state.video_thread_heartbeat_epoch_sec; + state.video_config.stats_logger = NULL; + state.video_config.stage_logger = NULL; + state.video_config.stats_interval_ms = state.stats_interval_ms; + state.control_server_idle_reconnect_ms = env_int_or_default( + "OMNI_CONTROL_SERVER_IDLE_RECONNECT_MS", + CONTROL_DEFAULT_SERVER_IDLE_RECONNECT_MS + ); + snprintf(state.status_file_path, sizeof(state.status_file_path), "%s/%s", state.runtime_dir, DEFAULT_STATUS_FILE_NAME); + snprintf( + state.video_thread_fault_file, + sizeof(state.video_thread_fault_file), + "%s/%s", + state.runtime_dir, + DEFAULT_VIDEO_THREAD_FAULT_FILE + ); + snprintf( + state.control_thread_fault_file, + sizeof(state.control_thread_fault_file), + "%s/%s", + state.runtime_dir, + DEFAULT_CONTROL_THREAD_FAULT_FILE + ); + initial_heartbeat = realtime_epoch_sec(); + atomic_init(&state.video_thread_heartbeat_epoch_sec, initial_heartbeat); + atomic_init(&state.control_thread_heartbeat_epoch_sec, initial_heartbeat); + + 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 (control_ack_manager_init(&state) != 0) { + perror("control_ack_manager_init"); + control_bridge_stats_destroy(&state.control_stats); + 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_ack_manager_destroy(&state); + 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_ack_manager_destroy(&state); + control_bridge_stats_destroy(&state.control_stats); + video_pipeline_stats_destroy(&state.video_stats); + return 1; + } + + { + const char *stats_log_path = getenv("BLITZ_KCP_STATS_LOG_PATH"); + const char *latency_log_path = getenv("BLITZ_CONTROL_LATENCY_LOG_PATH"); + const char *video_stage_log_path = getenv("BLITZ_VIDEO_STAGE_LOG_PATH"); + int latency_enabled = env_int_or_default("BLITZ_CONTROL_LATENCY_LOG_ENABLED", 1); + int video_stage_log_enabled = env_int_or_default("BLITZ_VIDEO_STAGE_LOG_ENABLED", 1); + uint64_t video_stage_log_sample_mod = env_u64_or_default("BLITZ_VIDEO_STAGE_LOG_SAMPLE_MOD", 10); + + if (stats_log_path != NULL && stats_log_path[0] != '\0') { + state.stats_logger = kcp_session_stats_open_jsonl(stats_log_path); + if (state.stats_logger == NULL) { + fprintf(stderr, "[b_side_omnid] warning: failed to open KCP stats log %s\n", stats_log_path); + } + } + if (latency_enabled && latency_log_path != NULL && latency_log_path[0] != '\0') { + state.control_latency_logger = latencylog_open_jsonl(latency_log_path); + if (state.control_latency_logger == NULL) { + fprintf(stderr, "[b_side_omnid] warning: failed to open control latency log %s\n", latency_log_path); + } + } + if (video_stage_log_enabled && video_stage_log_path != NULL && video_stage_log_path[0] != '\0') { + state.video_stage_logger = video_stage_logger_open_jsonl(video_stage_log_path, video_stage_log_sample_mod); + if (state.video_stage_logger == NULL) { + fprintf(stderr, "[b_side_omnid] warning: failed to open video stage log %s\n", video_stage_log_path); + } + } + state.video_config.stats_logger = state.stats_logger; + state.video_config.stage_logger = state.video_stage_logger; + state.video_config.stats_interval_ms = state.stats_interval_ms; + } + + if (control_ack_enabled(&state)) { + if (pthread_create(&state.control_ack_thread, NULL, control_ack_thread_main, &state) != 0) { + fprintf(stderr, "[b_side_omnid] warning: failed to start async control ACK manager, ACK sampling disabled\n"); + } else { + state.control_ack_thread_started = 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_ack_manager_destroy(&state); + control_bridge_stats_destroy(&state.control_stats); + video_pipeline_stats_destroy(&state.video_stats); + latencylog_close(state.control_latency_logger); + video_stage_logger_close(state.video_stage_logger); + kcp_session_stats_close(state.stats_logger); + 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_ack_manager_destroy(&state); + control_bridge_stats_destroy(&state.control_stats); + video_pipeline_stats_destroy(&state.video_stats); + latencylog_close(state.control_latency_logger); + video_stage_logger_close(state.video_stage_logger); + kcp_session_stats_close(state.stats_logger); + return 1; + } + + while (!g_stop_requested) { + sleep(1); + print_stats(&state); + if (write_daemon_status_file(&state) != 0) { + fprintf(stderr, "[b_side_omnid] failed to write status file %s: %s\n", state.status_file_path, strerror(errno)); + } + exit_if_thread_stalled(&state); + } + + pthread_join(video_thread, NULL); + pthread_join(control_thread, NULL); + unix_dgram_client_close(&state.unix_client); + control_ack_manager_destroy(&state); + control_bridge_stats_destroy(&state.control_stats); + video_pipeline_stats_destroy(&state.video_stats); + latencylog_close(state.control_latency_logger); + video_stage_logger_close(state.video_stage_logger); + kcp_session_stats_close(state.stats_logger); + return 0; +} diff --git a/robot/v4l2/OmniSocketGo_robot/cmd/kcppeer.c b/robot/v4l2/OmniSocketGo_robot/cmd/kcppeer.c new file mode 100644 index 0000000..e71981c --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/cmd/kcppeer.c @@ -0,0 +1,352 @@ +#include "cli_parse.h" +#include "interactive.h" +#include "peer_kcp_client.h" + +#include +#include + +typedef struct kcppeer_receive_ctx { + kcp_client_t *client; + const char *inbox_dir; + volatile int stop_requested; + int rc; +} kcppeer_receive_ctx_t; + +static void kcppeer_usage(FILE *out) { + fprintf(out, "usage: kcppeer [-id peer-a] [-server 127.0.0.1:9002] [-relay-via addr]\n"); + fprintf(out, " [-to peer] [-text msg | -file path] [-bind-ip ip] [-bind-device dev]\n"); + fprintf(out, " [-inbox-dir dir] [-latency-log path] [-kcp-ts-debug-log path]\n"); + fprintf(out, " [-kcp-session-stats-log path] [-kcp-session-stats-interval 100ms]\n"); + fprintf(out, " [-interactive[=true|false]]\n"); +} + +static void *kcppeer_receive_thread_main(void *arg) { + kcppeer_receive_ctx_t *ctx = (kcppeer_receive_ctx_t *) arg; + + for (;;) { + message_t msg; + char persisted_path[512]; + + protocol_message_init(&msg); + if (kcp_client_receive(ctx->client, &msg) != 0) { + protocol_message_clear(&msg); + ctx->rc = ctx->stop_requested ? 0 : -1; + return NULL; + } + + switch (msg.type) { + case MSG_TYPE_TEXT: + if (kcp_client_persist_message(ctx->client, &msg, ctx->inbox_dir, persisted_path, sizeof(persisted_path)) != 0) { + fprintf(stderr, "kcppeer: persist text from %s to %s failed\n", msg.from, msg.to); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + fprintf(stderr, "received text from %s to %s and persisted to %s\n", msg.from, msg.to, persisted_path); + break; + case MSG_TYPE_FILE: + if (kcp_client_persist_message(ctx->client, &msg, ctx->inbox_dir, persisted_path, sizeof(persisted_path)) != 0) { + fprintf(stderr, "kcppeer: persist file from %s to %s failed\n", msg.from, msg.to); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + fprintf(stderr, "received file from %s to %s: %s (%lu bytes) -> %s\n", msg.from, msg.to, msg.file_name, (unsigned long) msg.body_len, persisted_path); + break; + case MSG_TYPE_BINARY: + if (kcp_client_persist_message(ctx->client, &msg, ctx->inbox_dir, persisted_path, sizeof(persisted_path)) != 0) { + fprintf(stderr, "kcppeer: persist binary payload from %s to %s failed\n", msg.from, msg.to); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + fprintf(stderr, "received binary payload from %s to %s (%lu bytes) -> %s\n", msg.from, msg.to, (unsigned long) msg.body_len, persisted_path); + break; + case MSG_TYPE_ERROR: + fprintf(stderr, "received error from %s to %s: %.*s\n", msg.from, msg.to, (int) msg.body_len, msg.body == NULL ? "" : (const char *) msg.body); + break; + default: + fprintf(stderr, "received unexpected message type %s from %s\n", protocol_message_type_name(msg.type), msg.from); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + protocol_message_clear(&msg); + } +} + +int main(int argc, char **argv) { + const char *peer_id = "peer-a"; + const char *server_addr = "127.0.0.1:9002"; + const char *relay_via = ""; + const char *actual_dial_target; + const char *target_peer = ""; + const char *text = ""; + const char *file_path = ""; + const char *bind_ip = ""; + const char *bind_device = ""; + const char *inbox_dir = "inbox"; + const char *latency_log_path = ""; + const char *packet_log_path = ""; + const char *stats_log_path = ""; + const char *stats_interval_raw = ""; + int stats_interval_ms = KCP_DEFAULT_STATS_INTERVAL_MS; + int interactive = 1; + latency_logger_t *latency_logger = NULL; + kcp_packet_debug_logger_t *packet_logger = NULL; + kcp_session_stats_logger_t *stats_logger = NULL; + kcp_client_t *client = NULL; + kcppeer_receive_ctx_t receive_ctx; + pthread_t receive_thread; + int receive_thread_started = 0; + int i; + int rc = 1; + + memset(&receive_ctx, 0, sizeof(receive_ctx)); + + for (i = 1; i < argc; ++i) { + const char *value = NULL; + int handled; + + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-id", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -id requires a value\n"); + return 1; + } else if (handled) { + peer_id = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-server", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -server requires a value\n"); + return 1; + } else if (handled) { + server_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-relay-via", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -relay-via requires a value\n"); + return 1; + } else if (handled) { + relay_via = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-to", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -to requires a value\n"); + return 1; + } else if (handled) { + target_peer = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-text", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -text requires a value\n"); + return 1; + } else if (handled) { + text = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-file", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -file requires a value\n"); + return 1; + } else if (handled) { + file_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-bind-ip", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -bind-ip requires a value\n"); + return 1; + } else if (handled) { + bind_ip = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-bind-device", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -bind-device requires a value\n"); + return 1; + } else if (handled) { + bind_device = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-inbox-dir", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -inbox-dir requires a value\n"); + return 1; + } else if (handled) { + inbox_dir = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-latency-log", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -latency-log requires a value\n"); + return 1; + } else if (handled) { + latency_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-kcp-ts-debug-log", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -kcp-ts-debug-log requires a value\n"); + return 1; + } else if (handled) { + packet_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-kcp-session-stats-log", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -kcp-session-stats-log requires a value\n"); + return 1; + } else if (handled) { + stats_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-kcp-session-stats-interval", &value)) < 0) { + fprintf(stderr, "kcppeer: flag -kcp-session-stats-interval requires a value\n"); + return 1; + } else if (handled) { + stats_interval_raw = value; + continue; + } + if ((handled = cli_parse_bool_flag(argv[i], "-interactive", &interactive)) < 0) { + fprintf(stderr, "kcppeer: invalid -interactive value\n"); + return 1; + } else if (handled) { + continue; + } + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + kcppeer_usage(stdout); + return 0; + } + fprintf(stderr, "kcppeer: unknown argument %s\n", argv[i]); + kcppeer_usage(stderr); + return 1; + } + + if (text[0] != '\0' && file_path[0] != '\0') { + fprintf(stderr, "kcppeer: only one of -text or -file may be specified\n"); + return 1; + } + if ((text[0] != '\0' || file_path[0] != '\0') && target_peer[0] == '\0') { + fprintf(stderr, "kcppeer: flag -to is required when sending text or file\n"); + return 1; + } + if (kcp_session_stats_parse_interval_ms(stats_interval_raw, &stats_interval_ms) != 0) { + fprintf(stderr, "kcppeer: invalid -kcp-session-stats-interval value %s\n", stats_interval_raw); + return 1; + } + + if (latency_log_path[0] != '\0') { + latency_logger = latencylog_open_jsonl(latency_log_path); + if (latency_logger == NULL) { + fprintf(stderr, "kcppeer: open latency logger %s failed\n", latency_log_path); + goto cleanup; + } + } + if (packet_log_path[0] != '\0') { + packet_logger = kcp_packet_debug_open_jsonl(packet_log_path); + if (packet_logger == NULL) { + fprintf(stderr, "kcppeer: open kcp packet debug logger %s failed\n", packet_log_path); + goto cleanup; + } + } + if (stats_log_path[0] != '\0') { + stats_logger = kcp_session_stats_open_jsonl(stats_log_path); + if (stats_logger == NULL) { + fprintf(stderr, "kcppeer: open kcp session stats logger %s failed\n", stats_log_path); + goto cleanup; + } + } + + actual_dial_target = relay_via[0] != '\0' ? relay_via : server_addr; + client = kcp_client_dial(server_addr, relay_via, peer_id, bind_ip, bind_device, latency_logger, packet_logger, stats_logger, stats_interval_ms); + if (client == NULL) { + int saved_errno = errno; + const char *reason = saved_errno != 0 ? strerror(saved_errno) : "unknown error"; + if (relay_via[0] != '\0') { + fprintf(stderr, "kcppeer: dial target %s failed (logical server %s): %s (errno=%d)\n", actual_dial_target, server_addr, reason, saved_errno); + } else { + fprintf(stderr, "kcppeer: dial kcp server %s failed: %s (errno=%d)\n", server_addr, reason, saved_errno); + } + goto cleanup; + } + if (relay_via[0] != '\0') { + fprintf(stderr, "opened KCP session as %s; logical server=%s, actual dial target=%s via relay; registration confirmed\n", kcp_client_id(client), server_addr, actual_dial_target); + } else { + fprintf(stderr, "opened KCP session as %s; logical server=%s, actual dial target=%s; registration confirmed\n", kcp_client_id(client), server_addr, actual_dial_target); + } + + receive_ctx.client = client; + receive_ctx.inbox_dir = inbox_dir; + if (pthread_create(&receive_thread, NULL, kcppeer_receive_thread_main, &receive_ctx) != 0) { + fprintf(stderr, "kcppeer: create receive thread failed\n"); + goto cleanup; + } + receive_thread_started = 1; + + if (target_peer[0] != '\0' && text[0] != '\0') { + if (kcp_client_send_text(client, target_peer, text) != 0) { + fprintf(stderr, "kcppeer: send text to %s failed\n", target_peer); + goto cleanup; + } + fprintf(stderr, "sent text to %s\n", target_peer); + } + if (target_peer[0] != '\0' && file_path[0] != '\0') { + if (kcp_client_send_file_path(client, target_peer, file_path) != 0) { + fprintf(stderr, "kcppeer: send file %s to %s failed\n", file_path, target_peer); + goto cleanup; + } + fprintf(stderr, "sent file %s to %s\n", file_path, target_peer); + } + + if (interactive) { + char line[2048]; + char prompt[128]; + + snprintf(prompt, sizeof(prompt), "%s> ", kcp_client_id(client)); + interactive_print_help(stdout, "KCP"); + while (fputs(prompt, stdout) >= 0 && fflush(stdout) == 0 && fgets(line, sizeof(line), stdin) != NULL) { + interactive_command_t command; + char err[128]; + + omni_trim_newline(line); + if (interactive_parse_command(line, &command, err, sizeof(err)) != 0) { + if (strstr(err, "empty command") == NULL) { + fprintf(stderr, "%s\n", err); + } + continue; + } + if (command.type == INTERACTIVE_CMD_HELP) { + interactive_print_help(stdout, "KCP"); + continue; + } + if (command.type == INTERACTIVE_CMD_QUIT) { + break; + } + if (command.type == INTERACTIVE_CMD_TEXT) { + if (kcp_client_send_text(client, command.to, command.value) != 0) { + fprintf(stderr, "kcppeer: send text to %s failed\n", command.to); + continue; + } + fprintf(stderr, "sent text to %s\n", command.to); + continue; + } + if (command.type == INTERACTIVE_CMD_FILE) { + if (kcp_client_send_file_path(client, command.to, command.value) != 0) { + fprintf(stderr, "kcppeer: send file %s to %s failed\n", command.value, command.to); + continue; + } + fprintf(stderr, "sent file %s to %s\n", command.value, command.to); + continue; + } + } + } + + rc = 0; + +cleanup: + receive_ctx.stop_requested = 1; + kcp_client_close(client); + if (receive_thread_started) { + pthread_join(receive_thread, NULL); + if (rc == 0 && receive_ctx.rc != 0) { + rc = 1; + } + } + kcp_client_free(client); + kcp_session_stats_close(stats_logger); + kcp_packet_debug_close(packet_logger); + latencylog_close(latency_logger); + return rc; +} diff --git a/robot/v4l2/OmniSocketGo_robot/cmd/kcpping.c b/robot/v4l2/OmniSocketGo_robot/cmd/kcpping.c new file mode 100644 index 0000000..42442d8 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/cmd/kcpping.c @@ -0,0 +1,788 @@ +#include "cli_parse.h" +#include "peer_kcp_client.h" + +#include "cJSON.h" + +#include +#include + +typedef struct kcp_ping_message_node { + struct kcp_ping_message_node *next; + message_t msg; +} kcp_ping_message_node_t; + +typedef struct kcp_ping_receiver_ctx { + kcp_client_t *client; + pthread_mutex_t mu; + kcp_ping_message_node_t *head; + kcp_ping_message_node_t *tail; + volatile int stop_requested; + int closed; + int rc; +} kcp_ping_receiver_ctx_t; + +typedef struct kcp_pending_ping { + struct kcp_pending_ping *next; + uint64_t seq; + int64_t deadline_ns; +} kcp_pending_ping_t; + +typedef struct kcp_ping_tracker { + kcp_pending_ping_t *pending; + int pending_count; + int sent; + int duplicates; + uint64_t max_seq_sent; + int64_t *samples_ns; + size_t sample_count; + size_t sample_cap; +} kcp_ping_tracker_t; + +static volatile sig_atomic_t g_kcpping_stop = 0; + +static void kcpping_on_signal(int signo) { + (void) signo; + g_kcpping_stop = 1; +} + +static void kcpping_usage(FILE *out) { + fprintf(out, "usage: kcpping [-id pinger] [-server 127.0.0.1:9002] [-to peer] [-echo]\n"); + fprintf(out, " [-count 100] [-interval 100ms] [-size 64] [-timeout 3s]\n"); + fprintf(out, " [-bind-ip ip] [-bind-device dev] [-latency-log path]\n"); +} + +static int kcp_ping_compare_i64(const void *left, const void *right) { + const int64_t *a = (const int64_t *) left; + const int64_t *b = (const int64_t *) right; + if (*a < *b) { + return -1; + } + if (*a > *b) { + return 1; + } + return 0; +} + +static double kcp_ping_sqrt(double value) { + double x = value; + int i; + + if (value <= 0.0) { + return 0.0; + } + if (x < 1.0) { + x = 1.0; + } + for (i = 0; i < 16; ++i) { + x = 0.5 * (x + value / x); + } + return x; +} + +static int kcp_ping_build_payload(uint64_t seq, int64_t ts_ns, int size, char **out_body, size_t *out_len) { + cJSON *root = NULL; + char *json = NULL; + char *pad = NULL; + size_t base_len; + size_t pad_len; + + *out_body = NULL; + *out_len = 0; + + root = cJSON_CreateObject(); + if (root == NULL) { + return -1; + } + cJSON_AddNumberToObject(root, "seq", (double) seq); + cJSON_AddNumberToObject(root, "ts_ns", (double) ts_ns); + cJSON_AddStringToObject(root, "pad", ""); + json = cJSON_PrintUnformatted(root); + cJSON_Delete(root); + if (json == NULL) { + return -1; + } + base_len = strlen(json); + cJSON_free(json); + if ((int) base_len > size) { + errno = EMSGSIZE; + return -1; + } + + pad_len = (size_t) size - base_len; + pad = (char *) malloc(pad_len + 1U); + if (pad == NULL) { + return -1; + } + memset(pad, 'A', pad_len); + pad[pad_len] = '\0'; + + root = cJSON_CreateObject(); + if (root == NULL) { + free(pad); + return -1; + } + cJSON_AddNumberToObject(root, "seq", (double) seq); + cJSON_AddNumberToObject(root, "ts_ns", (double) ts_ns); + cJSON_AddStringToObject(root, "pad", pad); + free(pad); + json = cJSON_PrintUnformatted(root); + cJSON_Delete(root); + if (json == NULL) { + return -1; + } + if ((int) strlen(json) != size) { + cJSON_free(json); + errno = EINVAL; + return -1; + } + *out_body = json; + *out_len = (size_t) size; + return 0; +} + +static int kcp_ping_parse_payload(const uint8_t *body, size_t body_len, uint64_t *seq, int64_t *ts_ns) { + char *text; + cJSON *root; + const cJSON *seq_item; + const cJSON *ts_item; + + if (body == NULL || seq == NULL || ts_ns == NULL) { + errno = EINVAL; + return -1; + } + text = (char *) malloc(body_len + 1U); + if (text == NULL) { + return -1; + } + memcpy(text, body, body_len); + text[body_len] = '\0'; + root = cJSON_Parse(text); + free(text); + if (root == NULL) { + errno = EPROTO; + return -1; + } + seq_item = cJSON_GetObjectItemCaseSensitive(root, "seq"); + ts_item = cJSON_GetObjectItemCaseSensitive(root, "ts_ns"); + if (!cJSON_IsNumber(seq_item) || !cJSON_IsNumber(ts_item) || seq_item->valuedouble <= 0 || ts_item->valuedouble <= 0) { + cJSON_Delete(root); + errno = EPROTO; + return -1; + } + *seq = (uint64_t) seq_item->valuedouble; + *ts_ns = (int64_t) ts_item->valuedouble; + cJSON_Delete(root); + return 0; +} + +static void kcp_ping_receiver_ctx_init(kcp_ping_receiver_ctx_t *ctx, kcp_client_t *client) { + memset(ctx, 0, sizeof(*ctx)); + ctx->client = client; + pthread_mutex_init(&ctx->mu, NULL); +} + +static void kcp_ping_receiver_ctx_destroy(kcp_ping_receiver_ctx_t *ctx) { + kcp_ping_message_node_t *node; + kcp_ping_message_node_t *next; + + if (ctx == NULL) { + return; + } + for (node = ctx->head; node != NULL; node = next) { + next = node->next; + protocol_message_clear(&node->msg); + free(node); + } + pthread_mutex_destroy(&ctx->mu); +} + +static void *kcpping_receive_thread_main(void *arg) { + kcp_ping_receiver_ctx_t *ctx = (kcp_ping_receiver_ctx_t *) arg; + + for (;;) { + message_t msg; + kcp_ping_message_node_t *node; + + protocol_message_init(&msg); + if (kcp_client_receive(ctx->client, &msg) != 0) { + protocol_message_clear(&msg); + pthread_mutex_lock(&ctx->mu); + ctx->rc = ctx->stop_requested ? 0 : -1; + ctx->closed = 1; + pthread_mutex_unlock(&ctx->mu); + return NULL; + } + + node = (kcp_ping_message_node_t *) calloc(1, sizeof(*node)); + if (node == NULL) { + protocol_message_clear(&msg); + pthread_mutex_lock(&ctx->mu); + ctx->rc = -1; + ctx->closed = 1; + pthread_mutex_unlock(&ctx->mu); + return NULL; + } + node->msg = msg; + + pthread_mutex_lock(&ctx->mu); + if (ctx->tail == NULL) { + ctx->head = node; + } else { + ctx->tail->next = node; + } + ctx->tail = node; + pthread_mutex_unlock(&ctx->mu); + } +} + +static int kcp_ping_receiver_pop(kcp_ping_receiver_ctx_t *ctx, message_t *out_msg) { + kcp_ping_message_node_t *node; + + pthread_mutex_lock(&ctx->mu); + node = ctx->head; + if (node != NULL) { + ctx->head = node->next; + if (ctx->head == NULL) { + ctx->tail = NULL; + } + } + pthread_mutex_unlock(&ctx->mu); + + if (node == NULL) { + return 0; + } + *out_msg = node->msg; + free(node); + return 1; +} + +static int kcp_ping_receiver_status(kcp_ping_receiver_ctx_t *ctx, int *closed, int *rc) { + pthread_mutex_lock(&ctx->mu); + *closed = ctx->closed; + *rc = ctx->rc; + pthread_mutex_unlock(&ctx->mu); + return 0; +} + +static void kcp_ping_tracker_init(kcp_ping_tracker_t *tracker) { + memset(tracker, 0, sizeof(*tracker)); +} + +static void kcp_ping_tracker_destroy(kcp_ping_tracker_t *tracker) { + kcp_pending_ping_t *pending; + kcp_pending_ping_t *next; + + for (pending = tracker->pending; pending != NULL; pending = next) { + next = pending->next; + free(pending); + } + free(tracker->samples_ns); +} + +static int kcp_ping_tracker_mark_sent(kcp_ping_tracker_t *tracker, uint64_t seq, int64_t sent_at_ns, int64_t timeout_ns) { + kcp_pending_ping_t *pending = (kcp_pending_ping_t *) calloc(1, sizeof(*pending)); + if (pending == NULL) { + return -1; + } + pending->seq = seq; + pending->deadline_ns = sent_at_ns + timeout_ns; + pending->next = tracker->pending; + tracker->pending = pending; + tracker->pending_count++; + tracker->sent++; + tracker->max_seq_sent = seq; + return 0; +} + +static kcp_pending_ping_t *kcp_ping_tracker_find_pending(kcp_ping_tracker_t *tracker, uint64_t seq, kcp_pending_ping_t **out_prev) { + kcp_pending_ping_t *prev = NULL; + kcp_pending_ping_t *cur; + + for (cur = tracker->pending; cur != NULL; cur = cur->next) { + if (cur->seq == seq) { + if (out_prev != NULL) { + *out_prev = prev; + } + return cur; + } + prev = cur; + } + if (out_prev != NULL) { + *out_prev = NULL; + } + return NULL; +} + +static int kcp_ping_tracker_add_sample(kcp_ping_tracker_t *tracker, int64_t rtt_ns) { + int64_t *next_samples; + size_t next_cap; + + if (tracker->sample_count == tracker->sample_cap) { + next_cap = tracker->sample_cap == 0 ? 16U : tracker->sample_cap * 2U; + next_samples = (int64_t *) realloc(tracker->samples_ns, next_cap * sizeof(*next_samples)); + if (next_samples == NULL) { + return -1; + } + tracker->samples_ns = next_samples; + tracker->sample_cap = next_cap; + } + tracker->samples_ns[tracker->sample_count++] = rtt_ns; + return 0; +} + +static int kcp_ping_tracker_observe_reply(kcp_ping_tracker_t *tracker, uint64_t seq, int64_t sent_ts_ns, int64_t received_ts_ns, int *disposition, int64_t *rtt_ns) { + kcp_pending_ping_t *prev = NULL; + kcp_pending_ping_t *pending; + + if (seq == 0 || seq > tracker->max_seq_sent) { + *disposition = 2; + *rtt_ns = 0; + return 0; + } + pending = kcp_ping_tracker_find_pending(tracker, seq, &prev); + if (pending == NULL) { + tracker->duplicates++; + *disposition = 1; + *rtt_ns = 0; + return 0; + } + if (prev == NULL) { + tracker->pending = pending->next; + } else { + prev->next = pending->next; + } + tracker->pending_count--; + free(pending); + + *rtt_ns = received_ts_ns - sent_ts_ns; + if (*rtt_ns < 0) { + *rtt_ns = 0; + } + if (kcp_ping_tracker_add_sample(tracker, *rtt_ns) != 0) { + return -1; + } + *disposition = 0; + return 0; +} + +static void kcp_ping_tracker_expire(kcp_ping_tracker_t *tracker, int64_t now_ns, FILE *out) { + kcp_pending_ping_t *prev = NULL; + kcp_pending_ping_t *cur = tracker->pending; + + while (cur != NULL) { + if (cur->deadline_ns <= now_ns) { + kcp_pending_ping_t *next = cur->next; + fprintf(out, "seq=%" PRIu64 " timeout\n", cur->seq); + if (prev == NULL) { + tracker->pending = next; + } else { + prev->next = next; + } + free(cur); + tracker->pending_count--; + cur = next; + continue; + } + prev = cur; + cur = cur->next; + } +} + +static int64_t kcp_ping_percentile_ns(const int64_t *sorted, size_t count, double percentile) { + size_t index; + double raw_index; + + if (count == 0) { + return 0; + } + if (percentile <= 0.0) { + return sorted[0]; + } + if (percentile >= 1.0) { + return sorted[count - 1]; + } + raw_index = percentile * (double) count; + index = (size_t) raw_index; + if ((double) index < raw_index) { + index++; + } + if (index > 0) { + index--; + } + if (index >= count) { + index = count - 1; + } + return sorted[index]; +} + +static void kcp_ping_print_summary(FILE *out, const char *target, const kcp_ping_tracker_t *tracker) { + int received = (int) tracker->sample_count; + double loss_pct = tracker->sent == 0 ? 0.0 : ((double) (tracker->sent - received) * 100.0 / (double) tracker->sent); + + fprintf(out, "--- %s kcp ping statistics ---\n", target); + fprintf(out, "%d packets transmitted, %d received, %d duplicates, %.2f%% packet loss\n", tracker->sent, received, tracker->duplicates, loss_pct); + if (tracker->sample_count == 0) { + fprintf(out, "rtt min/avg/max/p50/p95/p99 = n/a/n/a/n/a/n/a/n/a/n/a, stddev=n/a\n"); + return; + } + + { + int64_t *sorted = (int64_t *) malloc(tracker->sample_count * sizeof(*sorted)); + size_t i; + double sum = 0.0; + double variance = 0.0; + double avg; + int64_t min_ns; + int64_t max_ns; + int64_t p50_ns; + int64_t p95_ns; + int64_t p99_ns; + + if (sorted == NULL) { + fprintf(out, "rtt summary unavailable: memory allocation failed\n"); + return; + } + memcpy(sorted, tracker->samples_ns, tracker->sample_count * sizeof(*sorted)); + qsort(sorted, tracker->sample_count, sizeof(*sorted), kcp_ping_compare_i64); + for (i = 0; i < tracker->sample_count; ++i) { + sum += (double) sorted[i]; + } + avg = sum / (double) tracker->sample_count; + for (i = 0; i < tracker->sample_count; ++i) { + double delta = (double) sorted[i] - avg; + variance += delta * delta; + } + variance /= (double) tracker->sample_count; + + min_ns = sorted[0]; + max_ns = sorted[tracker->sample_count - 1]; + p50_ns = kcp_ping_percentile_ns(sorted, tracker->sample_count, 0.50); + p95_ns = kcp_ping_percentile_ns(sorted, tracker->sample_count, 0.95); + p99_ns = kcp_ping_percentile_ns(sorted, tracker->sample_count, 0.99); + + fprintf( + out, + "rtt min/avg/max/p50/p95/p99 = %.2fms/%.2fms/%.2fms/%.2fms/%.2fms/%.2fms, stddev=%.2fms\n", + (double) min_ns / 1000000.0, + avg / 1000000.0, + (double) max_ns / 1000000.0, + (double) p50_ns / 1000000.0, + (double) p95_ns / 1000000.0, + (double) p99_ns / 1000000.0, + kcp_ping_sqrt(variance) / 1000000.0 + ); + free(sorted); + } +} + +static int kcp_ping_expiry_poll_ms(int timeout_ms) { + int interval = timeout_ms / 4; + if (interval < 10) { + return 10; + } + if (interval > 100) { + return 100; + } + return interval; +} + +int main(int argc, char **argv) { + const char *peer_id = "pinger"; + const char *server_addr = "127.0.0.1:9002"; + const char *target_peer = ""; + const char *bind_ip = ""; + const char *bind_device = ""; + const char *latency_log_path = ""; + int echo_mode = 0; + int count = 100; + int interval_ms = 100; + int size = 64; + int timeout_ms = 3000; + latency_logger_t *latency_logger = NULL; + kcp_client_t *client = NULL; + kcp_ping_receiver_ctx_t receiver_ctx; + pthread_t receiver_thread; + int receiver_ctx_initialized = 0; + int receiver_thread_started = 0; + kcp_ping_tracker_t tracker; + int i; + int rc = 1; + + kcp_ping_tracker_init(&tracker); + memset(&receiver_ctx, 0, sizeof(receiver_ctx)); + + for (i = 1; i < argc; ++i) { + const char *value = NULL; + int handled; + + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-id", &value)) < 0) { + fprintf(stderr, "kcpping: flag -id requires a value\n"); + return 1; + } else if (handled) { + peer_id = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-server", &value)) < 0) { + fprintf(stderr, "kcpping: flag -server requires a value\n"); + return 1; + } else if (handled) { + server_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-to", &value)) < 0) { + fprintf(stderr, "kcpping: flag -to requires a value\n"); + return 1; + } else if (handled) { + target_peer = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-count", &value)) < 0) { + fprintf(stderr, "kcpping: flag -count requires a value\n"); + return 1; + } else if (handled) { + count = atoi(value); + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-interval", &value)) < 0) { + fprintf(stderr, "kcpping: flag -interval requires a value\n"); + return 1; + } else if (handled) { + if (omni_parse_duration_ms(value, interval_ms, &interval_ms) != 0) { + fprintf(stderr, "kcpping: invalid -interval value %s\n", value); + return 1; + } + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-size", &value)) < 0) { + fprintf(stderr, "kcpping: flag -size requires a value\n"); + return 1; + } else if (handled) { + size = atoi(value); + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-timeout", &value)) < 0) { + fprintf(stderr, "kcpping: flag -timeout requires a value\n"); + return 1; + } else if (handled) { + if (omni_parse_duration_ms(value, timeout_ms, &timeout_ms) != 0) { + fprintf(stderr, "kcpping: invalid -timeout value %s\n", value); + return 1; + } + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-bind-ip", &value)) < 0) { + fprintf(stderr, "kcpping: flag -bind-ip requires a value\n"); + return 1; + } else if (handled) { + bind_ip = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-bind-device", &value)) < 0) { + fprintf(stderr, "kcpping: flag -bind-device requires a value\n"); + return 1; + } else if (handled) { + bind_device = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-latency-log", &value)) < 0) { + fprintf(stderr, "kcpping: flag -latency-log requires a value\n"); + return 1; + } else if (handled) { + latency_log_path = value; + continue; + } + if ((handled = cli_parse_bool_flag(argv[i], "-echo", &echo_mode)) < 0) { + fprintf(stderr, "kcpping: invalid -echo value\n"); + return 1; + } else if (handled) { + continue; + } + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + kcpping_usage(stdout); + return 0; + } + fprintf(stderr, "kcpping: unknown argument %s\n", argv[i]); + kcpping_usage(stderr); + return 1; + } + + if (peer_id[0] == '\0' || server_addr[0] == '\0') { + fprintf(stderr, "kcpping: flags -id and -server are required\n"); + return 1; + } + if (!echo_mode && target_peer[0] == '\0') { + fprintf(stderr, "kcpping: flag -to is required unless -echo is set\n"); + return 1; + } + if (count < 0 || interval_ms <= 0 || size <= 0 || timeout_ms <= 0) { + fprintf(stderr, "kcpping: invalid numeric flag value\n"); + return 1; + } + + signal(SIGINT, kcpping_on_signal); + + if (latency_log_path[0] != '\0') { + latency_logger = latencylog_open_jsonl(latency_log_path); + if (latency_logger == NULL) { + fprintf(stderr, "kcpping: open latency logger %s failed\n", latency_log_path); + goto cleanup; + } + } + client = kcp_client_dial(server_addr, NULL, peer_id, bind_ip, bind_device, latency_logger, NULL, NULL, KCP_DEFAULT_STATS_INTERVAL_MS); + if (client == NULL) { + fprintf(stderr, "kcpping: dial kcp server %s failed\n", server_addr); + goto cleanup; + } + + if (echo_mode) { + while (!g_kcpping_stop) { + message_t msg; + + protocol_message_init(&msg); + if (kcp_client_receive(client, &msg) != 0) { + protocol_message_clear(&msg); + if (g_kcpping_stop) { + break; + } + fprintf(stderr, "kcpping: receive failed in echo mode\n"); + goto cleanup; + } + if (msg.type == MSG_TYPE_TEXT) { + char *text = (char *) malloc(msg.body_len + 1U); + if (text == NULL) { + protocol_message_clear(&msg); + goto cleanup; + } + memcpy(text, msg.body, msg.body_len); + text[msg.body_len] = '\0'; + if (kcp_client_send_text(client, msg.from, text) != 0) { + free(text); + protocol_message_clear(&msg); + fprintf(stderr, "kcpping: echo send back to %s failed\n", msg.from); + goto cleanup; + } + free(text); + } else if (msg.type == MSG_TYPE_ERROR) { + fprintf(stderr, "server error: %.*s\n", (int) msg.body_len, msg.body == NULL ? "" : (const char *) msg.body); + } else { + fprintf(stderr, "unexpected message type %s from %s ignored\n", protocol_message_type_name(msg.type), msg.from); + } + protocol_message_clear(&msg); + } + rc = 0; + goto cleanup; + } + + fprintf(stdout, "KCP PING %s via %s (payload=%d bytes, KCP)\n", target_peer, server_addr, size); + kcp_ping_receiver_ctx_init(&receiver_ctx, client); + receiver_ctx_initialized = 1; + if (pthread_create(&receiver_thread, NULL, kcpping_receive_thread_main, &receiver_ctx) != 0) { + fprintf(stderr, "kcpping: create receive thread failed\n"); + goto cleanup; + } + receiver_thread_started = 1; + + { + uint64_t next_seq = 1; + int stop_sending = 0; + int64_t next_send_at_ns = omni_now_unix_nano(); + int poll_ms = kcp_ping_expiry_poll_ms(timeout_ms); + int64_t timeout_ns = (int64_t) timeout_ms * 1000000LL; + + while (!g_kcpping_stop || tracker.pending_count > 0 || !stop_sending) { + int64_t now_ns = omni_now_unix_nano(); + message_t msg; + int popped; + int receiver_closed; + int receiver_status_rc; + + if (!stop_sending && now_ns >= next_send_at_ns) { + char *payload = NULL; + size_t payload_len = 0; + + if (count > 0 && tracker.sent >= count) { + stop_sending = 1; + } else { + if (kcp_ping_build_payload(next_seq, now_ns, size, &payload, &payload_len) != 0) { + fprintf(stderr, "kcpping: build payload for seq=%" PRIu64 " failed\n", next_seq); + free(payload); + goto cleanup; + } + if (kcp_client_send_text(client, target_peer, payload) != 0) { + fprintf(stderr, "kcpping: send ping seq=%" PRIu64 " failed\n", next_seq); + free(payload); + goto cleanup; + } + free(payload); + if (kcp_ping_tracker_mark_sent(&tracker, next_seq, now_ns, timeout_ns) != 0) { + goto cleanup; + } + next_seq++; + next_send_at_ns = now_ns + (int64_t) interval_ms * 1000000LL; + if (count > 0 && tracker.sent >= count) { + stop_sending = 1; + } + } + } + + kcp_ping_tracker_expire(&tracker, now_ns, stdout); + + do { + popped = kcp_ping_receiver_pop(&receiver_ctx, &msg); + if (popped == 1) { + if (msg.type == MSG_TYPE_TEXT) { + uint64_t seq; + int64_t sent_ts_ns; + int disposition; + int64_t rtt_ns; + + if (kcp_ping_parse_payload(msg.body, msg.body_len, &seq, &sent_ts_ns) != 0) { + fprintf(stderr, "ignore non-ping text message from %s\n", msg.from); + } else if (kcp_ping_tracker_observe_reply(&tracker, seq, sent_ts_ns, omni_now_unix_nano(), &disposition, &rtt_ns) != 0) { + protocol_message_clear(&msg); + goto cleanup; + } else if (disposition == 0) { + fprintf(stdout, "seq=%" PRIu64 " rtt=%.2fms\n", seq, (double) rtt_ns / 1000000.0); + } else if (disposition == 1) { + fprintf(stderr, "seq=%" PRIu64 " duplicate or late reply ignored\n", seq); + } else { + fprintf(stderr, "seq=%" PRIu64 " unexpected reply ignored\n", seq); + } + } else if (msg.type == MSG_TYPE_ERROR) { + fprintf(stderr, "server error: %.*s\n", (int) msg.body_len, msg.body == NULL ? "" : (const char *) msg.body); + } else { + fprintf(stderr, "unexpected message type %s from %s ignored\n", protocol_message_type_name(msg.type), msg.from); + } + protocol_message_clear(&msg); + } + } while (popped == 1); + + kcp_ping_receiver_status(&receiver_ctx, &receiver_closed, &receiver_status_rc); + if (receiver_closed && receiver_status_rc != 0) { + fprintf(stderr, "kcpping: receive loop failed\n"); + goto cleanup; + } + if ((g_kcpping_stop || stop_sending) && tracker.pending_count == 0) { + break; + } + usleep((useconds_t) poll_ms * 1000U); + } + } + + kcp_ping_print_summary(stdout, target_peer, &tracker); + rc = 0; + +cleanup: + receiver_ctx.stop_requested = 1; + kcp_client_close(client); + if (receiver_thread_started) { + pthread_join(receiver_thread, NULL); + kcp_ping_receiver_ctx_destroy(&receiver_ctx); + } else if (receiver_ctx_initialized) { + kcp_ping_receiver_ctx_destroy(&receiver_ctx); + } + kcp_client_free(client); + latencylog_close(latency_logger); + kcp_ping_tracker_destroy(&tracker); + return rc; +} diff --git a/robot/v4l2/OmniSocketGo_robot/cmd/kcpserver.c b/robot/v4l2/OmniSocketGo_robot/cmd/kcpserver.c new file mode 100644 index 0000000..afcf8f4 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/cmd/kcpserver.c @@ -0,0 +1,253 @@ +#include "cli_parse.h" +#include "server_kcp_hub.h" +#include "server_udp_relay.h" + +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"); +} + +int main(int argc, char **argv) { + const char *mode = "hub"; + const char *listen_addr = ":9002"; + const char *bind_device = ""; + const char *latency_log_path = ""; + 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; + + latency_logger_t *latency_logger = NULL; + kcp_packet_debug_logger_t *packet_logger = NULL; + kcp_session_stats_logger_t *stats_logger = NULL; + kcp_listener_t *listener = NULL; + kcp_hub_t *hub = NULL; + udp_relay_t *relay = NULL; + + for (i = 1; i < argc; ++i) { + const char *value = NULL; + int handled; + + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-mode", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -mode requires a value\n"); + return 1; + } else if (handled) { + mode = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-listen", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -listen requires a value\n"); + return 1; + } else if (handled) { + listen_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-bind-device", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -bind-device requires a value\n"); + return 1; + } else if (handled) { + bind_device = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-latency-log", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -latency-log requires a value\n"); + return 1; + } else if (handled) { + latency_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-kcp-ts-debug-log", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -kcp-ts-debug-log requires a value\n"); + return 1; + } else if (handled) { + packet_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-kcp-session-stats-log", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -kcp-session-stats-log requires a value\n"); + return 1; + } else if (handled) { + stats_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-kcp-session-stats-interval", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -kcp-session-stats-interval requires a value\n"); + return 1; + } else if (handled) { + 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; + } else if (handled) { + relay_listen_alias = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-relay-remote", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -relay-remote requires a value\n"); + return 1; + } else if (handled) { + relay_remote_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-relay-peer", &value)) < 0) { + fprintf(stderr, "kcpserver: flag -relay-peer requires a value\n"); + return 1; + } else if (handled) { + relay_peer_alias = value; + continue; + } + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + kcpserver_usage(stdout); + return 0; + } + fprintf(stderr, "kcpserver: unknown argument %s\n", argv[i]); + kcpserver_usage(stderr); + return 1; + } + + if (kcp_session_stats_parse_interval_ms(stats_interval_raw, &stats_interval_ms) != 0) { + 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"); + return 1; + } + if (relay_remote_addr[0] == '\0' && relay_peer_alias[0] != '\0') { + relay_remote_addr = relay_peer_alias; + } + if (relay_peer_alias[0] != '\0') { + fprintf(stderr, "warning: flag -relay-peer is deprecated; use -relay-remote instead\n"); + } + if (relay_listen_alias[0] != '\0') { + if (strcmp(mode, "relay") != 0) { + fprintf(stderr, "kcpserver: flag -relay-listen may only be used in relay mode\n"); + return 1; + } + if (listen_addr[0] != '\0' && strcmp(listen_addr, ":9002") != 0 && strcmp(listen_addr, relay_listen_alias) != 0) { + fprintf(stderr, "kcpserver: flags -listen and -relay-listen must match when both are set in relay mode\n"); + return 1; + } + listen_addr = relay_listen_alias; + fprintf(stderr, "warning: flag -relay-listen is deprecated; use -listen with -mode=relay instead\n"); + } + + if (strcmp(mode, "hub") == 0) { + if (relay_remote_addr[0] != '\0') { + fprintf(stderr, "kcpserver: flag -relay-remote may only be used in relay mode\n"); + return 1; + } + if (latency_log_path[0] != '\0') { + latency_logger = latencylog_open_jsonl(latency_log_path); + if (latency_logger == NULL) { + fprintf(stderr, "kcpserver: open latency logger %s failed\n", latency_log_path); + goto cleanup; + } + } + if (packet_log_path[0] != '\0') { + packet_logger = kcp_packet_debug_open_jsonl(packet_log_path); + if (packet_logger == NULL) { + fprintf(stderr, "kcpserver: open packet debug logger %s failed\n", packet_log_path); + goto cleanup; + } + } + if (stats_log_path[0] != '\0') { + stats_logger = kcp_session_stats_open_jsonl(stats_log_path); + if (stats_logger == NULL) { + fprintf(stderr, "kcpserver: open session stats logger %s failed\n", stats_log_path); + goto cleanup; + } + } + listener = kcp_listener_listen(listen_addr, bind_device, packet_logger, OMNI_NODE_ROLE_SERVER, "hub"); + if (listener == NULL) { + fprintf(stderr, "kcpserver: listen on %s failed\n", listen_addr); + goto cleanup; + } + hub = kcp_hub_new(latency_logger, stats_logger, stats_interval_ms); + if (hub == NULL) { + 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"); + goto cleanup; + } + rc = 0; + goto cleanup; + } + + 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; + } + if (relay_remote_addr[0] == '\0') { + fprintf(stderr, "kcpserver: flag -relay-remote is required in relay mode\n"); + return 1; + } + relay = udp_relay_open(listen_addr, relay_remote_addr); + if (relay == NULL) { + fprintf(stderr, "kcpserver: open udp relay %s -> %s failed\n", listen_addr, relay_remote_addr); + goto cleanup; + } + fprintf(stderr, "udp relay listening on %s and forwarding to %s\n", listen_addr, relay_remote_addr); + if (udp_relay_serve(relay) != 0) { + fprintf(stderr, "kcpserver: udp relay stopped with error\n"); + goto cleanup; + } + rc = 0; + goto cleanup; + } + + fprintf(stderr, "kcpserver: unsupported -mode=%s; want hub or relay\n", mode); + +cleanup: + udp_relay_free(relay); + kcp_hub_free(hub); + kcp_listener_free(listener); + kcp_session_stats_close(stats_logger); + kcp_packet_debug_close(packet_logger); + latencylog_close(latency_logger); + return rc; +} diff --git a/robot/v4l2/OmniSocketGo_robot/cmd/udppeer.c b/robot/v4l2/OmniSocketGo_robot/cmd/udppeer.c new file mode 100644 index 0000000..e5656ae --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/cmd/udppeer.c @@ -0,0 +1,291 @@ +#include "cli_parse.h" +#include "interactive.h" +#include "peer_udp_client.h" + +#include + +typedef struct udppeer_receive_ctx { + udp_client_t *client; + const char *inbox_dir; + volatile int stop_requested; + int rc; +} udppeer_receive_ctx_t; + +static void udppeer_usage(FILE *out) { + fprintf(out, "usage: udppeer [-id peer-a] [-server 127.0.0.1:9001] [-to peer] [-text msg | -file path]\n"); + fprintf(out, " [-bind-ip ip] [-inbox-dir dir] [-latency-log path] [-tx-ts-debug-log path]\n"); + fprintf(out, " [-interactive[=true|false]]\n"); +} + +static void *udppeer_receive_thread_main(void *arg) { + udppeer_receive_ctx_t *ctx = (udppeer_receive_ctx_t *) arg; + + for (;;) { + message_t msg; + char persisted_path[512]; + + protocol_message_init(&msg); + if (udp_client_receive(ctx->client, &msg) != 0) { + protocol_message_clear(&msg); + ctx->rc = ctx->stop_requested ? 0 : -1; + return NULL; + } + + switch (msg.type) { + case MSG_TYPE_TEXT: + if (udp_client_persist_message(ctx->client, &msg, ctx->inbox_dir, persisted_path, sizeof(persisted_path)) != 0) { + fprintf(stderr, "udppeer: persist text from %s to %s failed\n", msg.from, msg.to); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + fprintf(stderr, "received text from %s to %s and persisted to %s\n", msg.from, msg.to, persisted_path); + break; + case MSG_TYPE_FILE: + if (udp_client_persist_message(ctx->client, &msg, ctx->inbox_dir, persisted_path, sizeof(persisted_path)) != 0) { + fprintf(stderr, "udppeer: persist file from %s to %s failed\n", msg.from, msg.to); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + fprintf(stderr, "received file from %s to %s: %s (%lu bytes) -> %s\n", msg.from, msg.to, msg.file_name, (unsigned long) msg.body_len, persisted_path); + break; + case MSG_TYPE_BINARY: + if (udp_client_persist_message(ctx->client, &msg, ctx->inbox_dir, persisted_path, sizeof(persisted_path)) != 0) { + fprintf(stderr, "udppeer: persist binary payload from %s to %s failed\n", msg.from, msg.to); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + fprintf(stderr, "received binary payload from %s to %s (%lu bytes) -> %s\n", msg.from, msg.to, (unsigned long) msg.body_len, persisted_path); + break; + case MSG_TYPE_ERROR: + fprintf(stderr, "received error from %s to %s: %.*s\n", msg.from, msg.to, (int) msg.body_len, msg.body == NULL ? "" : (const char *) msg.body); + break; + default: + fprintf(stderr, "received unexpected message type %s from %s\n", protocol_message_type_name(msg.type), msg.from); + protocol_message_clear(&msg); + ctx->rc = -1; + return NULL; + } + protocol_message_clear(&msg); + } +} + +int main(int argc, char **argv) { + const char *peer_id = "peer-a"; + const char *server_addr = "127.0.0.1:9001"; + const char *target_peer = ""; + const char *text = ""; + const char *file_path = ""; + const char *bind_ip = ""; + const char *inbox_dir = "inbox"; + const char *latency_log_path = ""; + const char *tx_debug_log_path = ""; + int interactive = 1; + latency_logger_t *latency_logger = NULL; + tx_timestamp_debug_logger_t *debug_logger = NULL; + udp_client_t *client = NULL; + udppeer_receive_ctx_t receive_ctx; + pthread_t receive_thread; + int receive_thread_started = 0; + int i; + int rc = 1; + + memset(&receive_ctx, 0, sizeof(receive_ctx)); + + for (i = 1; i < argc; ++i) { + const char *value = NULL; + int handled; + + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-id", &value)) < 0) { + fprintf(stderr, "udppeer: flag -id requires a value\n"); + return 1; + } else if (handled) { + peer_id = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-server", &value)) < 0) { + fprintf(stderr, "udppeer: flag -server requires a value\n"); + return 1; + } else if (handled) { + server_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-to", &value)) < 0) { + fprintf(stderr, "udppeer: flag -to requires a value\n"); + return 1; + } else if (handled) { + target_peer = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-text", &value)) < 0) { + fprintf(stderr, "udppeer: flag -text requires a value\n"); + return 1; + } else if (handled) { + text = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-file", &value)) < 0) { + fprintf(stderr, "udppeer: flag -file requires a value\n"); + return 1; + } else if (handled) { + file_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-bind-ip", &value)) < 0) { + fprintf(stderr, "udppeer: flag -bind-ip requires a value\n"); + return 1; + } else if (handled) { + bind_ip = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-inbox-dir", &value)) < 0) { + fprintf(stderr, "udppeer: flag -inbox-dir requires a value\n"); + return 1; + } else if (handled) { + inbox_dir = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-latency-log", &value)) < 0) { + fprintf(stderr, "udppeer: flag -latency-log requires a value\n"); + return 1; + } else if (handled) { + latency_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-tx-ts-debug-log", &value)) < 0) { + fprintf(stderr, "udppeer: flag -tx-ts-debug-log requires a value\n"); + return 1; + } else if (handled) { + tx_debug_log_path = value; + continue; + } + if ((handled = cli_parse_bool_flag(argv[i], "-interactive", &interactive)) < 0) { + fprintf(stderr, "udppeer: invalid -interactive value\n"); + return 1; + } else if (handled) { + continue; + } + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + udppeer_usage(stdout); + return 0; + } + fprintf(stderr, "udppeer: unknown argument %s\n", argv[i]); + udppeer_usage(stderr); + return 1; + } + + if (text[0] != '\0' && file_path[0] != '\0') { + fprintf(stderr, "udppeer: only one of -text or -file may be specified\n"); + return 1; + } + if ((text[0] != '\0' || file_path[0] != '\0') && target_peer[0] == '\0') { + fprintf(stderr, "udppeer: flag -to is required when sending text or file\n"); + return 1; + } + + if (latency_log_path[0] != '\0') { + latency_logger = latencylog_open_jsonl(latency_log_path); + if (latency_logger == NULL) { + fprintf(stderr, "udppeer: open latency logger %s failed\n", latency_log_path); + goto cleanup; + } + } + if (tx_debug_log_path[0] != '\0') { + debug_logger = tx_timestamp_debug_open_jsonl(tx_debug_log_path); + if (debug_logger == NULL) { + fprintf(stderr, "udppeer: open tx timestamp debug logger %s failed\n", tx_debug_log_path); + goto cleanup; + } + } + + client = udp_client_dial(server_addr, peer_id, bind_ip, latency_logger, debug_logger, tx_debug_log_path[0] != '\0'); + if (client == NULL) { + fprintf(stderr, "udppeer: dial udp server %s failed\n", server_addr); + goto cleanup; + } + fprintf(stderr, "connected to %s as %s (UDP)\n", server_addr, udp_client_id(client)); + + receive_ctx.client = client; + receive_ctx.inbox_dir = inbox_dir; + if (pthread_create(&receive_thread, NULL, udppeer_receive_thread_main, &receive_ctx) != 0) { + fprintf(stderr, "udppeer: create receive thread failed\n"); + goto cleanup; + } + receive_thread_started = 1; + + if (target_peer[0] != '\0' && text[0] != '\0') { + if (udp_client_send_text(client, target_peer, text) != 0) { + fprintf(stderr, "udppeer: send text to %s failed\n", target_peer); + goto cleanup; + } + fprintf(stderr, "sent text to %s\n", target_peer); + } + if (target_peer[0] != '\0' && file_path[0] != '\0') { + if (udp_client_send_file_path(client, target_peer, file_path) != 0) { + fprintf(stderr, "udppeer: send file %s to %s failed\n", file_path, target_peer); + goto cleanup; + } + fprintf(stderr, "sent file %s to %s\n", file_path, target_peer); + } + + if (interactive) { + char line[2048]; + char prompt[128]; + + snprintf(prompt, sizeof(prompt), "%s> ", udp_client_id(client)); + interactive_print_help(stdout, "UDP"); + while (fputs(prompt, stdout) >= 0 && fflush(stdout) == 0 && fgets(line, sizeof(line), stdin) != NULL) { + interactive_command_t command; + char err[128]; + + omni_trim_newline(line); + if (interactive_parse_command(line, &command, err, sizeof(err)) != 0) { + if (strstr(err, "empty command") == NULL) { + fprintf(stderr, "%s\n", err); + } + continue; + } + if (command.type == INTERACTIVE_CMD_HELP) { + interactive_print_help(stdout, "UDP"); + continue; + } + if (command.type == INTERACTIVE_CMD_QUIT) { + break; + } + if (command.type == INTERACTIVE_CMD_TEXT) { + if (udp_client_send_text(client, command.to, command.value) != 0) { + fprintf(stderr, "udppeer: send text to %s failed\n", command.to); + continue; + } + fprintf(stderr, "sent text to %s\n", command.to); + continue; + } + if (command.type == INTERACTIVE_CMD_FILE) { + if (udp_client_send_file_path(client, command.to, command.value) != 0) { + fprintf(stderr, "udppeer: send file %s to %s failed\n", command.value, command.to); + continue; + } + fprintf(stderr, "sent file %s to %s\n", command.value, command.to); + continue; + } + } + } + + rc = 0; + +cleanup: + receive_ctx.stop_requested = 1; + udp_client_close(client); + if (receive_thread_started) { + pthread_join(receive_thread, NULL); + if (rc == 0 && receive_ctx.rc != 0) { + rc = 1; + } + } + udp_client_free(client); + tx_timestamp_debug_close(debug_logger); + latencylog_close(latency_logger); + return rc; +} diff --git a/robot/v4l2/OmniSocketGo_robot/cmd/udpping.c b/robot/v4l2/OmniSocketGo_robot/cmd/udpping.c new file mode 100644 index 0000000..1b652da --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/cmd/udpping.c @@ -0,0 +1,780 @@ +#include "cli_parse.h" +#include "peer_udp_client.h" + +#include "cJSON.h" + +#include +#include + +typedef struct ping_message_node { + struct ping_message_node *next; + message_t msg; +} ping_message_node_t; + +typedef struct ping_receiver_ctx { + udp_client_t *client; + pthread_mutex_t mu; + ping_message_node_t *head; + ping_message_node_t *tail; + volatile int stop_requested; + int closed; + int rc; +} ping_receiver_ctx_t; + +typedef struct pending_ping { + struct pending_ping *next; + uint64_t seq; + int64_t deadline_ns; +} pending_ping_t; + +typedef struct ping_tracker { + pending_ping_t *pending; + int pending_count; + int sent; + int duplicates; + uint64_t max_seq_sent; + int64_t *samples_ns; + size_t sample_count; + size_t sample_cap; +} ping_tracker_t; + +static volatile sig_atomic_t g_udpping_stop = 0; + +static void udpping_on_signal(int signo) { + (void) signo; + g_udpping_stop = 1; +} + +static void udpping_usage(FILE *out) { + fprintf(out, "usage: udpping [-id pinger] [-server 127.0.0.1:9001] [-to peer] [-echo]\n"); + fprintf(out, " [-count 100] [-interval 100ms] [-size 64] [-timeout 3s]\n"); + fprintf(out, " [-bind-ip ip] [-latency-log path]\n"); +} + +static int ping_compare_i64(const void *left, const void *right) { + const int64_t *a = (const int64_t *) left; + const int64_t *b = (const int64_t *) right; + if (*a < *b) { + return -1; + } + if (*a > *b) { + return 1; + } + return 0; +} + +static double ping_sqrt(double value) { + double x = value; + int i; + + if (value <= 0.0) { + return 0.0; + } + if (x < 1.0) { + x = 1.0; + } + for (i = 0; i < 16; ++i) { + x = 0.5 * (x + value / x); + } + return x; +} + +static int ping_build_payload(uint64_t seq, int64_t ts_ns, int size, char **out_body, size_t *out_len) { + cJSON *root = NULL; + char *json = NULL; + char *pad = NULL; + size_t base_len; + size_t pad_len; + + *out_body = NULL; + *out_len = 0; + + root = cJSON_CreateObject(); + if (root == NULL) { + return -1; + } + cJSON_AddNumberToObject(root, "seq", (double) seq); + cJSON_AddNumberToObject(root, "ts_ns", (double) ts_ns); + cJSON_AddStringToObject(root, "pad", ""); + json = cJSON_PrintUnformatted(root); + cJSON_Delete(root); + if (json == NULL) { + return -1; + } + base_len = strlen(json); + cJSON_free(json); + if ((int) base_len > size) { + errno = EMSGSIZE; + return -1; + } + + pad_len = (size_t) size - base_len; + pad = (char *) malloc(pad_len + 1U); + if (pad == NULL) { + return -1; + } + memset(pad, 'A', pad_len); + pad[pad_len] = '\0'; + + root = cJSON_CreateObject(); + if (root == NULL) { + free(pad); + return -1; + } + cJSON_AddNumberToObject(root, "seq", (double) seq); + cJSON_AddNumberToObject(root, "ts_ns", (double) ts_ns); + cJSON_AddStringToObject(root, "pad", pad); + free(pad); + json = cJSON_PrintUnformatted(root); + cJSON_Delete(root); + if (json == NULL) { + return -1; + } + if ((int) strlen(json) != size) { + cJSON_free(json); + errno = EINVAL; + return -1; + } + *out_body = json; + *out_len = (size_t) size; + return 0; +} + +static int ping_parse_payload(const uint8_t *body, size_t body_len, uint64_t *seq, int64_t *ts_ns) { + char *text; + cJSON *root; + const cJSON *seq_item; + const cJSON *ts_item; + + if (body == NULL || seq == NULL || ts_ns == NULL) { + errno = EINVAL; + return -1; + } + text = (char *) malloc(body_len + 1U); + if (text == NULL) { + return -1; + } + memcpy(text, body, body_len); + text[body_len] = '\0'; + root = cJSON_Parse(text); + free(text); + if (root == NULL) { + errno = EPROTO; + return -1; + } + seq_item = cJSON_GetObjectItemCaseSensitive(root, "seq"); + ts_item = cJSON_GetObjectItemCaseSensitive(root, "ts_ns"); + if (!cJSON_IsNumber(seq_item) || !cJSON_IsNumber(ts_item) || seq_item->valuedouble <= 0 || ts_item->valuedouble <= 0) { + cJSON_Delete(root); + errno = EPROTO; + return -1; + } + *seq = (uint64_t) seq_item->valuedouble; + *ts_ns = (int64_t) ts_item->valuedouble; + cJSON_Delete(root); + return 0; +} + +static void ping_receiver_ctx_init(ping_receiver_ctx_t *ctx, udp_client_t *client) { + memset(ctx, 0, sizeof(*ctx)); + ctx->client = client; + pthread_mutex_init(&ctx->mu, NULL); +} + +static void ping_receiver_ctx_destroy(ping_receiver_ctx_t *ctx) { + ping_message_node_t *node; + ping_message_node_t *next; + + if (ctx == NULL) { + return; + } + for (node = ctx->head; node != NULL; node = next) { + next = node->next; + protocol_message_clear(&node->msg); + free(node); + } + pthread_mutex_destroy(&ctx->mu); +} + +static void *udpping_receive_thread_main(void *arg) { + ping_receiver_ctx_t *ctx = (ping_receiver_ctx_t *) arg; + + for (;;) { + message_t msg; + ping_message_node_t *node; + + protocol_message_init(&msg); + if (udp_client_receive(ctx->client, &msg) != 0) { + protocol_message_clear(&msg); + pthread_mutex_lock(&ctx->mu); + ctx->rc = ctx->stop_requested ? 0 : -1; + ctx->closed = 1; + pthread_mutex_unlock(&ctx->mu); + return NULL; + } + + node = (ping_message_node_t *) calloc(1, sizeof(*node)); + if (node == NULL) { + protocol_message_clear(&msg); + pthread_mutex_lock(&ctx->mu); + ctx->rc = -1; + ctx->closed = 1; + pthread_mutex_unlock(&ctx->mu); + return NULL; + } + node->msg = msg; + + pthread_mutex_lock(&ctx->mu); + if (ctx->tail == NULL) { + ctx->head = node; + } else { + ctx->tail->next = node; + } + ctx->tail = node; + pthread_mutex_unlock(&ctx->mu); + } +} + +static int ping_receiver_pop(ping_receiver_ctx_t *ctx, message_t *out_msg) { + ping_message_node_t *node; + + pthread_mutex_lock(&ctx->mu); + node = ctx->head; + if (node != NULL) { + ctx->head = node->next; + if (ctx->head == NULL) { + ctx->tail = NULL; + } + } + pthread_mutex_unlock(&ctx->mu); + + if (node == NULL) { + return 0; + } + *out_msg = node->msg; + free(node); + return 1; +} + +static int ping_receiver_status(ping_receiver_ctx_t *ctx, int *closed, int *rc) { + pthread_mutex_lock(&ctx->mu); + *closed = ctx->closed; + *rc = ctx->rc; + pthread_mutex_unlock(&ctx->mu); + return 0; +} + +static void ping_tracker_init(ping_tracker_t *tracker) { + memset(tracker, 0, sizeof(*tracker)); +} + +static void ping_tracker_destroy(ping_tracker_t *tracker) { + pending_ping_t *pending; + pending_ping_t *next; + + for (pending = tracker->pending; pending != NULL; pending = next) { + next = pending->next; + free(pending); + } + free(tracker->samples_ns); +} + +static int ping_tracker_mark_sent(ping_tracker_t *tracker, uint64_t seq, int64_t sent_at_ns, int64_t timeout_ns) { + pending_ping_t *pending = (pending_ping_t *) calloc(1, sizeof(*pending)); + if (pending == NULL) { + return -1; + } + pending->seq = seq; + pending->deadline_ns = sent_at_ns + timeout_ns; + pending->next = tracker->pending; + tracker->pending = pending; + tracker->pending_count++; + tracker->sent++; + tracker->max_seq_sent = seq; + return 0; +} + +static pending_ping_t *ping_tracker_find_pending(ping_tracker_t *tracker, uint64_t seq, pending_ping_t **out_prev) { + pending_ping_t *prev = NULL; + pending_ping_t *cur; + + for (cur = tracker->pending; cur != NULL; cur = cur->next) { + if (cur->seq == seq) { + if (out_prev != NULL) { + *out_prev = prev; + } + return cur; + } + prev = cur; + } + if (out_prev != NULL) { + *out_prev = NULL; + } + return NULL; +} + +static int ping_tracker_add_sample(ping_tracker_t *tracker, int64_t rtt_ns) { + int64_t *next_samples; + size_t next_cap; + + if (tracker->sample_count == tracker->sample_cap) { + next_cap = tracker->sample_cap == 0 ? 16U : tracker->sample_cap * 2U; + next_samples = (int64_t *) realloc(tracker->samples_ns, next_cap * sizeof(*next_samples)); + if (next_samples == NULL) { + return -1; + } + tracker->samples_ns = next_samples; + tracker->sample_cap = next_cap; + } + tracker->samples_ns[tracker->sample_count++] = rtt_ns; + return 0; +} + +static int ping_tracker_observe_reply(ping_tracker_t *tracker, uint64_t seq, int64_t sent_ts_ns, int64_t received_ts_ns, int *disposition, int64_t *rtt_ns) { + pending_ping_t *prev = NULL; + pending_ping_t *pending; + + if (seq == 0 || seq > tracker->max_seq_sent) { + *disposition = 2; + *rtt_ns = 0; + return 0; + } + pending = ping_tracker_find_pending(tracker, seq, &prev); + if (pending == NULL) { + tracker->duplicates++; + *disposition = 1; + *rtt_ns = 0; + return 0; + } + if (prev == NULL) { + tracker->pending = pending->next; + } else { + prev->next = pending->next; + } + tracker->pending_count--; + free(pending); + + *rtt_ns = received_ts_ns - sent_ts_ns; + if (*rtt_ns < 0) { + *rtt_ns = 0; + } + if (ping_tracker_add_sample(tracker, *rtt_ns) != 0) { + return -1; + } + *disposition = 0; + return 0; +} + +static void ping_tracker_expire(ping_tracker_t *tracker, int64_t now_ns, FILE *out) { + pending_ping_t *prev = NULL; + pending_ping_t *cur = tracker->pending; + + while (cur != NULL) { + if (cur->deadline_ns <= now_ns) { + pending_ping_t *next = cur->next; + fprintf(out, "seq=%" PRIu64 " timeout\n", cur->seq); + if (prev == NULL) { + tracker->pending = next; + } else { + prev->next = next; + } + free(cur); + tracker->pending_count--; + cur = next; + continue; + } + prev = cur; + cur = cur->next; + } +} + +static int64_t ping_percentile_ns(const int64_t *sorted, size_t count, double percentile) { + size_t index; + double raw_index; + + if (count == 0) { + return 0; + } + if (percentile <= 0.0) { + return sorted[0]; + } + if (percentile >= 1.0) { + return sorted[count - 1]; + } + raw_index = percentile * (double) count; + index = (size_t) raw_index; + if ((double) index < raw_index) { + index++; + } + if (index > 0) { + index--; + } + if (index >= count) { + index = count - 1; + } + return sorted[index]; +} + +static void ping_print_summary(FILE *out, const char *target, const ping_tracker_t *tracker) { + int received = (int) tracker->sample_count; + double loss_pct = tracker->sent == 0 ? 0.0 : ((double) (tracker->sent - received) * 100.0 / (double) tracker->sent); + + fprintf(out, "--- %s udp ping statistics ---\n", target); + fprintf(out, "%d packets transmitted, %d received, %d duplicates, %.2f%% packet loss\n", tracker->sent, received, tracker->duplicates, loss_pct); + if (tracker->sample_count == 0) { + fprintf(out, "rtt min/avg/max/p50/p95/p99 = n/a/n/a/n/a/n/a/n/a/n/a, stddev=n/a\n"); + return; + } + + { + int64_t *sorted = (int64_t *) malloc(tracker->sample_count * sizeof(*sorted)); + size_t i; + double sum = 0.0; + double variance = 0.0; + double avg; + int64_t min_ns; + int64_t max_ns; + int64_t p50_ns; + int64_t p95_ns; + int64_t p99_ns; + + if (sorted == NULL) { + fprintf(out, "rtt summary unavailable: memory allocation failed\n"); + return; + } + memcpy(sorted, tracker->samples_ns, tracker->sample_count * sizeof(*sorted)); + qsort(sorted, tracker->sample_count, sizeof(*sorted), ping_compare_i64); + for (i = 0; i < tracker->sample_count; ++i) { + sum += (double) sorted[i]; + } + avg = sum / (double) tracker->sample_count; + for (i = 0; i < tracker->sample_count; ++i) { + double delta = (double) sorted[i] - avg; + variance += delta * delta; + } + variance /= (double) tracker->sample_count; + + min_ns = sorted[0]; + max_ns = sorted[tracker->sample_count - 1]; + p50_ns = ping_percentile_ns(sorted, tracker->sample_count, 0.50); + p95_ns = ping_percentile_ns(sorted, tracker->sample_count, 0.95); + p99_ns = ping_percentile_ns(sorted, tracker->sample_count, 0.99); + + fprintf( + out, + "rtt min/avg/max/p50/p95/p99 = %.2fms/%.2fms/%.2fms/%.2fms/%.2fms/%.2fms, stddev=%.2fms\n", + (double) min_ns / 1000000.0, + avg / 1000000.0, + (double) max_ns / 1000000.0, + (double) p50_ns / 1000000.0, + (double) p95_ns / 1000000.0, + (double) p99_ns / 1000000.0, + ping_sqrt(variance) / 1000000.0 + ); + free(sorted); + } +} + +static int ping_expiry_poll_ms(int timeout_ms) { + int interval = timeout_ms / 4; + if (interval < 10) { + return 10; + } + if (interval > 100) { + return 100; + } + return interval; +} + +int main(int argc, char **argv) { + const char *peer_id = "pinger"; + const char *server_addr = "127.0.0.1:9001"; + const char *target_peer = ""; + const char *bind_ip = ""; + const char *latency_log_path = ""; + int echo_mode = 0; + int count = 100; + int interval_ms = 100; + int size = 64; + int timeout_ms = 3000; + latency_logger_t *latency_logger = NULL; + udp_client_t *client = NULL; + ping_receiver_ctx_t receiver_ctx; + pthread_t receiver_thread; + int receiver_ctx_initialized = 0; + int receiver_thread_started = 0; + ping_tracker_t tracker; + int i; + int rc = 1; + + ping_tracker_init(&tracker); + memset(&receiver_ctx, 0, sizeof(receiver_ctx)); + + for (i = 1; i < argc; ++i) { + const char *value = NULL; + int handled; + + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-id", &value)) < 0) { + fprintf(stderr, "udpping: flag -id requires a value\n"); + return 1; + } else if (handled) { + peer_id = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-server", &value)) < 0) { + fprintf(stderr, "udpping: flag -server requires a value\n"); + return 1; + } else if (handled) { + server_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-to", &value)) < 0) { + fprintf(stderr, "udpping: flag -to requires a value\n"); + return 1; + } else if (handled) { + target_peer = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-count", &value)) < 0) { + fprintf(stderr, "udpping: flag -count requires a value\n"); + return 1; + } else if (handled) { + count = atoi(value); + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-interval", &value)) < 0) { + fprintf(stderr, "udpping: flag -interval requires a value\n"); + return 1; + } else if (handled) { + if (omni_parse_duration_ms(value, interval_ms, &interval_ms) != 0) { + fprintf(stderr, "udpping: invalid -interval value %s\n", value); + return 1; + } + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-size", &value)) < 0) { + fprintf(stderr, "udpping: flag -size requires a value\n"); + return 1; + } else if (handled) { + size = atoi(value); + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-timeout", &value)) < 0) { + fprintf(stderr, "udpping: flag -timeout requires a value\n"); + return 1; + } else if (handled) { + if (omni_parse_duration_ms(value, timeout_ms, &timeout_ms) != 0) { + fprintf(stderr, "udpping: invalid -timeout value %s\n", value); + return 1; + } + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-bind-ip", &value)) < 0) { + fprintf(stderr, "udpping: flag -bind-ip requires a value\n"); + return 1; + } else if (handled) { + bind_ip = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-latency-log", &value)) < 0) { + fprintf(stderr, "udpping: flag -latency-log requires a value\n"); + return 1; + } else if (handled) { + latency_log_path = value; + continue; + } + if ((handled = cli_parse_bool_flag(argv[i], "-echo", &echo_mode)) < 0) { + fprintf(stderr, "udpping: invalid -echo value\n"); + return 1; + } else if (handled) { + continue; + } + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + udpping_usage(stdout); + return 0; + } + fprintf(stderr, "udpping: unknown argument %s\n", argv[i]); + udpping_usage(stderr); + return 1; + } + + if (peer_id[0] == '\0' || server_addr[0] == '\0') { + fprintf(stderr, "udpping: flags -id and -server are required\n"); + return 1; + } + if (!echo_mode && target_peer[0] == '\0') { + fprintf(stderr, "udpping: flag -to is required unless -echo is set\n"); + return 1; + } + if (count < 0 || interval_ms <= 0 || size <= 0 || timeout_ms <= 0) { + fprintf(stderr, "udpping: invalid numeric flag value\n"); + return 1; + } + + signal(SIGINT, udpping_on_signal); + + if (latency_log_path[0] != '\0') { + latency_logger = latencylog_open_jsonl(latency_log_path); + if (latency_logger == NULL) { + fprintf(stderr, "udpping: open latency logger %s failed\n", latency_log_path); + goto cleanup; + } + } + client = udp_client_dial(server_addr, peer_id, bind_ip, latency_logger, NULL, 0); + if (client == NULL) { + fprintf(stderr, "udpping: dial udp server %s failed\n", server_addr); + goto cleanup; + } + + if (echo_mode) { + while (!g_udpping_stop) { + message_t msg; + + protocol_message_init(&msg); + if (udp_client_receive(client, &msg) != 0) { + protocol_message_clear(&msg); + if (g_udpping_stop) { + break; + } + fprintf(stderr, "udpping: receive failed in echo mode\n"); + goto cleanup; + } + if (msg.type == MSG_TYPE_TEXT) { + char *text = (char *) malloc(msg.body_len + 1U); + if (text == NULL) { + protocol_message_clear(&msg); + goto cleanup; + } + memcpy(text, msg.body, msg.body_len); + text[msg.body_len] = '\0'; + if (udp_client_send_text(client, msg.from, text) != 0) { + free(text); + protocol_message_clear(&msg); + fprintf(stderr, "udpping: echo send back to %s failed\n", msg.from); + goto cleanup; + } + free(text); + } else if (msg.type == MSG_TYPE_ERROR) { + fprintf(stderr, "server error: %.*s\n", (int) msg.body_len, msg.body == NULL ? "" : (const char *) msg.body); + } else { + fprintf(stderr, "unexpected message type %s from %s ignored\n", protocol_message_type_name(msg.type), msg.from); + } + protocol_message_clear(&msg); + } + rc = 0; + goto cleanup; + } + + fprintf(stdout, "UDP PING %s via %s (payload=%d bytes, UDP)\n", target_peer, server_addr, size); + ping_receiver_ctx_init(&receiver_ctx, client); + receiver_ctx_initialized = 1; + if (pthread_create(&receiver_thread, NULL, udpping_receive_thread_main, &receiver_ctx) != 0) { + fprintf(stderr, "udpping: create receive thread failed\n"); + goto cleanup; + } + receiver_thread_started = 1; + + { + uint64_t next_seq = 1; + int stop_sending = 0; + int64_t next_send_at_ns = omni_now_unix_nano(); + int poll_ms = ping_expiry_poll_ms(timeout_ms); + int64_t timeout_ns = (int64_t) timeout_ms * 1000000LL; + + while (!g_udpping_stop || tracker.pending_count > 0 || !stop_sending) { + int64_t now_ns = omni_now_unix_nano(); + message_t msg; + int popped; + int receiver_closed; + int receiver_status_rc; + + if (!stop_sending && now_ns >= next_send_at_ns) { + char *payload = NULL; + size_t payload_len = 0; + + if (count > 0 && tracker.sent >= count) { + stop_sending = 1; + } else { + if (ping_build_payload(next_seq, now_ns, size, &payload, &payload_len) != 0) { + fprintf(stderr, "udpping: build payload for seq=%" PRIu64 " failed\n", next_seq); + free(payload); + goto cleanup; + } + if (udp_client_send_text(client, target_peer, payload) != 0) { + fprintf(stderr, "udpping: send ping seq=%" PRIu64 " failed\n", next_seq); + free(payload); + goto cleanup; + } + free(payload); + if (ping_tracker_mark_sent(&tracker, next_seq, now_ns, timeout_ns) != 0) { + goto cleanup; + } + next_seq++; + next_send_at_ns = now_ns + (int64_t) interval_ms * 1000000LL; + if (count > 0 && tracker.sent >= count) { + stop_sending = 1; + } + } + } + + ping_tracker_expire(&tracker, now_ns, stdout); + + do { + popped = ping_receiver_pop(&receiver_ctx, &msg); + if (popped == 1) { + if (msg.type == MSG_TYPE_TEXT) { + uint64_t seq; + int64_t sent_ts_ns; + int disposition; + int64_t rtt_ns; + + if (ping_parse_payload(msg.body, msg.body_len, &seq, &sent_ts_ns) != 0) { + fprintf(stderr, "ignore non-ping text message from %s\n", msg.from); + } else if (ping_tracker_observe_reply(&tracker, seq, sent_ts_ns, omni_now_unix_nano(), &disposition, &rtt_ns) != 0) { + protocol_message_clear(&msg); + goto cleanup; + } else if (disposition == 0) { + fprintf(stdout, "seq=%" PRIu64 " rtt=%.2fms\n", seq, (double) rtt_ns / 1000000.0); + } else if (disposition == 1) { + fprintf(stderr, "seq=%" PRIu64 " duplicate or late reply ignored\n", seq); + } else { + fprintf(stderr, "seq=%" PRIu64 " unexpected reply ignored\n", seq); + } + } else if (msg.type == MSG_TYPE_ERROR) { + fprintf(stderr, "server error: %.*s\n", (int) msg.body_len, msg.body == NULL ? "" : (const char *) msg.body); + } else { + fprintf(stderr, "unexpected message type %s from %s ignored\n", protocol_message_type_name(msg.type), msg.from); + } + protocol_message_clear(&msg); + } + } while (popped == 1); + + ping_receiver_status(&receiver_ctx, &receiver_closed, &receiver_status_rc); + if (receiver_closed && receiver_status_rc != 0) { + fprintf(stderr, "udpping: receive loop failed\n"); + goto cleanup; + } + if ((g_udpping_stop || stop_sending) && tracker.pending_count == 0) { + break; + } + usleep((useconds_t) poll_ms * 1000U); + } + } + + ping_print_summary(stdout, target_peer, &tracker); + rc = 0; + +cleanup: + receiver_ctx.stop_requested = 1; + udp_client_close(client); + if (receiver_thread_started) { + pthread_join(receiver_thread, NULL); + ping_receiver_ctx_destroy(&receiver_ctx); + } else if (receiver_ctx_initialized) { + ping_receiver_ctx_destroy(&receiver_ctx); + } + udp_client_free(client); + latencylog_close(latency_logger); + ping_tracker_destroy(&tracker); + return rc; +} diff --git a/robot/v4l2/OmniSocketGo_robot/cmd/udprelay.c b/robot/v4l2/OmniSocketGo_robot/cmd/udprelay.c new file mode 100644 index 0000000..57cf5b2 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/cmd/udprelay.c @@ -0,0 +1,59 @@ +#include "cli_parse.h" +#include "server_udp_relay.h" + +static void udprelay_usage(FILE *out) { + fprintf(out, "usage: udprelay [-listen addr] [-upstream addr]\n"); +} + +int main(int argc, char **argv) { + const char *listen_addr = ":9003"; + const char *upstream_addr = "127.0.0.1:9002"; + udp_relay_t *relay = NULL; + int i; + int rc = 1; + + for (i = 1; i < argc; ++i) { + const char *value = NULL; + int handled; + + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-listen", &value)) < 0) { + fprintf(stderr, "udprelay: flag -listen requires a value\n"); + return 1; + } else if (handled) { + listen_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-upstream", &value)) < 0) { + fprintf(stderr, "udprelay: flag -upstream requires a value\n"); + return 1; + } else if (handled) { + upstream_addr = value; + continue; + } + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + udprelay_usage(stdout); + return 0; + } + fprintf(stderr, "udprelay: unknown argument %s\n", argv[i]); + udprelay_usage(stderr); + return 1; + } + + relay = udp_relay_open(listen_addr, upstream_addr); + if (relay == NULL) { + fprintf(stderr, "udprelay: open relay %s -> %s failed\n", listen_addr, upstream_addr); + goto cleanup; + } + + fprintf(stderr, "udp relay listening on %s, upstream %s\n", listen_addr, upstream_addr); + if (udp_relay_serve(relay) != 0) { + fprintf(stderr, "udprelay: relay serve failed\n"); + goto cleanup; + } + + rc = 0; + +cleanup: + udp_relay_free(relay); + return rc; +} diff --git a/robot/v4l2/OmniSocketGo_robot/cmd/udpserver.c b/robot/v4l2/OmniSocketGo_robot/cmd/udpserver.c new file mode 100644 index 0000000..978fd36 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/cmd/udpserver.c @@ -0,0 +1,88 @@ +#include "cli_parse.h" +#include "server_udp_hub.h" + +static void udpserver_usage(FILE *out) { + fprintf(out, "usage: udpserver [-listen addr] [-latency-log path] [-tx-ts-debug-log path]\n"); +} + +int main(int argc, char **argv) { + const char *listen_addr = ":9001"; + const char *latency_log_path = ""; + const char *tx_debug_log_path = ""; + latency_logger_t *latency_logger = NULL; + tx_timestamp_debug_logger_t *debug_logger = NULL; + udp_hub_t *hub = NULL; + int enable_timestamping = 0; + int i; + int rc = 1; + + for (i = 1; i < argc; ++i) { + const char *value = NULL; + int handled; + + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-listen", &value)) < 0) { + fprintf(stderr, "udpserver: flag -listen requires a value\n"); + return 1; + } else if (handled) { + listen_addr = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-latency-log", &value)) < 0) { + fprintf(stderr, "udpserver: flag -latency-log requires a value\n"); + return 1; + } else if (handled) { + latency_log_path = value; + continue; + } + if ((handled = cli_parse_value_flag(argc, argv, &i, argv[i], "-tx-ts-debug-log", &value)) < 0) { + fprintf(stderr, "udpserver: flag -tx-ts-debug-log requires a value\n"); + return 1; + } else if (handled) { + tx_debug_log_path = value; + continue; + } + if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { + udpserver_usage(stdout); + return 0; + } + fprintf(stderr, "udpserver: unknown argument %s\n", argv[i]); + udpserver_usage(stderr); + return 1; + } + + if (latency_log_path[0] != '\0') { + latency_logger = latencylog_open_jsonl(latency_log_path); + if (latency_logger == NULL) { + fprintf(stderr, "udpserver: open latency logger %s failed\n", latency_log_path); + goto cleanup; + } + } + if (tx_debug_log_path[0] != '\0') { + debug_logger = tx_timestamp_debug_open_jsonl(tx_debug_log_path); + if (debug_logger == NULL) { + fprintf(stderr, "udpserver: open tx timestamp debug logger %s failed\n", tx_debug_log_path); + goto cleanup; + } + enable_timestamping = 1; + } + + hub = udp_hub_open(listen_addr, latency_logger, debug_logger, enable_timestamping); + if (hub == NULL) { + fprintf(stderr, "udpserver: listen on %s failed\n", listen_addr); + goto cleanup; + } + + fprintf(stderr, "udp server listening on %s\n", listen_addr); + if (udp_hub_serve(hub) != 0) { + fprintf(stderr, "udpserver: serve failed\n"); + goto cleanup; + } + + rc = 0; + +cleanup: + udp_hub_free(hub); + tx_timestamp_debug_close(debug_logger); + latencylog_close(latency_logger); + return rc; +} diff --git a/robot/v4l2/OmniSocketGo_robot/cmd/v1_camera_pipeline_ifdef.c b/robot/v4l2/OmniSocketGo_robot/cmd/v1_camera_pipeline_ifdef.c new file mode 100644 index 0000000..093ee6e --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/cmd/v1_camera_pipeline_ifdef.c @@ -0,0 +1,35 @@ +#include +#include + +#include "video_pipeline.h" + +int main(void) { + video_pipeline_config_t config; + video_pipeline_stats_t stats; + + video_pipeline_config_init(&config); + video_pipeline_config_load_env(&config); + if (getenv("OMNI_VIDEO_DEBUG_TIMING") == NULL) { + config.enable_timing_logs = 1; + } + if (video_pipeline_stats_init(&stats) != 0) { + perror("video_pipeline_stats_init"); + return 1; + } + + for (;;) { + int rc = video_pipeline_run(&config, &stats, NULL); + + if (rc == 0) { + break; + } + if (rc != VIDEO_PIPELINE_RUN_RETRY_IMMEDIATE) { + perror("video_pipeline_run"); + video_pipeline_stats_destroy(&stats); + return 1; + } + } + + video_pipeline_stats_destroy(&stats); + return 0; +} diff --git a/robot/v4l2/OmniSocketGo_robot/config/omnisocket_demo.yaml b/robot/v4l2/OmniSocketGo_robot/config/omnisocket_demo.yaml new file mode 100644 index 0000000..80542f0 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/config/omnisocket_demo.yaml @@ -0,0 +1,41 @@ +transport: + server_addr: "127.0.0.1:10909" + relay_via: "" + bind_ip: "" + bind_device: "" + +control_sender: + peer_id: "peer-a-ctrl" + target_peer: "peer-b-ctrl" + joy_topic: "/xbox_data" + deadzone: 0.10 + analog_epsilon: 0.01 + dpad_threshold: 0.50 + trigger_pressed_threshold: -0.50 + +control_receiver: + peer_id: "peer-b-ctrl" + +motion: + initial_lift: 0.89 + lift_step: 0.05 + max_surge: 1.0 + max_sway: 0.5 + max_spin: 0.5 + max_lift: 0.90 + min_lift: 0.65 + surge_step: 0.1 + sway_step: 0.1 + spin_step: 0.1 + +video_sender: + peer_id: "peer-b-video" + target_peer: "peer-a-video" + frame_bytes: 30720 + frame_interval_ms: 66 + +video_receiver: + peer_id: "peer-a-video" + # recv_into() requires a buffer large enough for the whole frame. + # If buffer_bytes is smaller than video_sender.frame_bytes, the oversize frame is dropped. + buffer_bytes: 65536 diff --git a/robot/v4l2/OmniSocketGo_robot/go/README.md b/robot/v4l2/OmniSocketGo_robot/go/README.md new file mode 100644 index 0000000..770f75f --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/go/README.md @@ -0,0 +1,94 @@ +# OmniSocketGo + +Linux only. Go 1.22. + +如果目标机器只运行 `server`，只需要编译并拷贝 `server` 二进制。 +如果目标机器只运行 `peer`，只需要编译并拷贝 `peer` 二进制。 + +`go build ./cmd/server` 和 `go build ./cmd/peer` 会把各自依赖到的功能一起编译进最终二进制，不需要再单独编译 `cmd/internal/...` 包。 + +- `server` 二进制会包含它依赖到的转发、协议、传输等代码 +- `peer` 二进制会包含它依赖到的注册、交互发送、接收落盘、协议、传输等代码 +- 只有没有被这个可执行程序引用的其他命令，才不在该二进制里，比如 `cmd/latencysummary` + +## Build + +按目标架构分别编译。 + mkdir -p bin + go build -o bin/server ./cmd/server + go build -o bin/peer ./cmd/peer + go build -o bin/latencysummary ./cmd/latencysummary + +### Linux amd64 + +```bash +CGO_ENABLED=0 GOOS=linux GOARCH=amd64 go build -o bin/server-linux-amd64 ./cmd/server +``` + +### Linux arm64 + +```bash +CGO_ENABLED=0 GOOS=linux GOARCH=arm64 go build -o bin/peer-linux-arm64 ./cmd/peer +``` + +### Linux armv7 + +```bash +CGO_ENABLED=0 GOOS=linux GOARCH=arm GOARM=7 go build -o bin/server-linux-armv7 ./cmd/server +CGO_ENABLED=0 GOOS=linux GOARCH=arm GOARM=7 go build -o bin/peer-linux-armv7 ./cmd/peer +``` + + + +## Run On Different Machines + +`server D` 所在机器监听 `0.0.0.0:10909`。 + +```bash +go run cmd/kcpserver/ -listen 0.0.0.0:10909 +-kcp-ts-debug-log logs/d-kcp-ts.jsonl -kcp-session-stats-log logs/d-kcp-stats.jsonl +``` + +`relay server C` 所在机器 + +```bash +go run ./cmd/kcpserver/ -mode=relay -listen 0.0.0.0:10909 -relay-remote 172.21.32.15:10909 + +2>&1 | tee logs/c.stdout.log +``` + +### peer-a (A) + +```bash +go run ./cmd/kcppeer/ -id peer-a -server 172.21.32.15:10909 -relay-via 106.55.173.235:10909 -inbox-dir inbox/a + +-latency-log logs/a-latency.jsonl -kcp-ts-debug-log logs/a-kcp-ts.jsonl -kcp-session-stats-log logs/a-kcp-stats.jsonl + +go run ./cmd/kcpping/ -id peer-a -server 106.55.173.235:10909 -echo +``` + +### peer-b (B) + +```bash +go run ./cmd/kcppeer/ -id peer-b -server 81.70.156.140:10909 -inbox-dir inbox/b + +-latency-log logs/b-latency.jsonl -kcp-ts-debug-log logs/b-kcp-ts.jsonl -kcp-session-stats-log logs/b-kcp-stats.jsonl + +go run ./cmd/kcpping -id peer-b -server 81.70.156.140:10909 -to peer-a -count 20 -interval 100ms +``` + +## Interactive Commands + +`peer` 启动后可以在终端里持续使用同一条长连接发送多次消息。 + +```text +help +text peer-b hello +text peer-a hi +file peer-a /tmp/test125.bin +file peer-a /tmp/test5.bin +quit +``` +### 自动化拉取更新汇总数据 +cd /home/limingjie/LMJ_Work/OmniSocketGo +./scripts/refresh-latency-summary.sh \ No newline at end of file diff --git a/robot/v4l2/OmniSocketGo_robot/go/change_to_c.md b/robot/v4l2/OmniSocketGo_robot/go/change_to_c.md new file mode 100644 index 0000000..675c8a4 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/go/change_to_c.md @@ -0,0 +1,465 @@ +OmniSocketGo -> OmniSocketC 转换计划 + + Context + + 将现有的 Go 语言实现的 UDP/KCP 传输层项目 (OmniSocketGo) 转换为纯 C 语言项目，运行在 Linux 系统上。 + + 原项目架构：A(Jetson) <-> C(relay cloud) <-> D(hub cloud) <-> B(host) + - B <-> D：KCP 链路 + - D <-> C：UDP relay 转发 + - C <-> A：KCP 链路（A 通过 relay C 连接到 hub D） + - 最终目的：B 和 A 之间双向传输数据 + + 转换要求： + - 只保留 UDP 和 KCP，不需要 TCP + - 不需要写测试 + - 完整实现协议层、传输层、日志事件系统 + - Linux only + + 项目位置 + + OmniSocketGo/c/ — 作为当前 Go 项目的子目录 + + 项目结构 + + c/ + ├── Makefile + ├── README.md + ├── include/ + │ ├── protocol.h # 协议消息定义 + 编解码 + │ ├── transport_kcp.h # KCP 连接封装 + │ ├── transport_udp.h # UDP 连接封装（含 Linux timestamping） + │ ├── linux_timestamping.h # Linux SO_TIMESTAMPING 底层实现 + │ ├── kcp_packet_debug.h # KCP packet-level kernel timestamp debug logger + │ ├── kcp_session_stats.h # KCP session stats (RTO/SRTT) logger + │ ├── tx_timestamp_debug.h # TX errqueue timestamp debug logger + │ ├── server_kcp_hub.h # KCP Hub (D 节点) + │ ├── server_udp_relay.h # UDP Relay (C 节点) + │ ├── peer_kcp_client.h # KCP Peer Client (A/B 节点) + │ ├── latencylog.h # 延迟日志事件系统 + │ ├── interactive.h # 交互式命令行 + │ └── cJSON.h # JSON 库 (第三方轻量级) + ├── src/ + │ ├── protocol.c + │ ├── transport_kcp.c + │ ├── transport_udp.c + │ ├── linux_timestamping.c + │ ├── kcp_packet_debug.c + │ ├── kcp_session_stats.c + │ ├── tx_timestamp_debug.c + │ ├── server_kcp_hub.c + │ ├── server_udp_relay.c + │ ├── peer_kcp_client.c + │ ├── latencylog.c + │ ├── interactive.c + │ └── cJSON.c + ├── cmd/ + │ ├── kcpserver.c # 主程序: KCP Hub 或 UDP Relay + │ ├── kcppeer.c # 主程序: KCP Peer (A/B) + │ └── kcpping.c # 主程序: KCP Ping 工具 + └── third_party/ + └── kcp/ + ├── ikcp.h # KCP 协议核心实现 (github.com/skywind3000/kcp) + └── ikcp.c + + 依赖说明 + + - KCP: 使用 skywind3000/kcp 的原始 C 实现 (ikcp.h/ikcp.c)，替代 Go 的 xtaci/kcp-go/v5 + - JSON: 使用 cJSON (DaveGamble/cJSON) 替代 Go 的 encoding/json + - 线程: 使用 pthread 替代 Go goroutine + - 同步: 使用 pthread_mutex/pthread_rwlock 替代 Go sync.Mutex/sync.RWMutex + + 模块实现计划 + + 1. 第三方库集成 + + - 下载 ikcp.h/ikcp.c (skywind3000/kcp) + - 下载 cJSON.h/cJSON.c (DaveGamble/cJSON) + + 2. protocol.h / protocol.c + + 对应 Go: cmd/internal/protocol/message.go + codec.go + + // 消息类型 + typedef enum { + MSG_TYPE_TEXT = 0, + MSG_TYPE_FILE = 1, + MSG_TYPE_REGISTER = 2, + MSG_TYPE_ERROR = 3, + } message_type_t; + + // 消息结构 + typedef struct { + message_type_t type; + uint64_t id; + char from[64]; + char to[64]; + char file_name[256]; + uint8_t *body; + int body_len; + } message_t; + + #define MAX_FRAME_SIZE (8 * 1024 * 1024) + #define SERVER_PEER_ID "server" + + 核心函数： + - int protocol_encode_message(const message_t *msg, uint8_t **out, int *out_len) — 编码消息为 [4B headerLen][header JSON][body] + - int protocol_decode_message(const uint8_t *data, int data_len, message_t *msg) — 解码 + - int protocol_write_frame(int fd, const uint8_t *payload, int payload_len) — 写带长度前缀的帧 (用于 KCP stream) + - int protocol_read_frame(int fd, uint8_t **payload, int *payload_len) — 读帧 + - int protocol_write_message(int fd, const message_t *msg) — 完整编码+写帧 + - int protocol_read_message(int fd, message_t *msg) — 读帧+解码 + - int protocol_validate_message(const message_t *msg) — 校验 + - void message_free(message_t *msg) — 释放 body 内存 + + 注意: KCP session 在 stream 模式下行为类似 TCP，需要 [4B frameLen] 前缀来分帧。 + + 3. latencylog.h / latencylog.c + + 对应 Go: cmd/internal/latencylog/logger.go + + // 事件名常量 + #define EVENT_A_APP_PREP_BEGIN "A_APP_PREP_BEGIN" + #define EVENT_SEND_HANDOFF_BEGIN "send_handoff_begin" + #define EVENT_SEND_HANDOFF_END "send_handoff_end" + #define EVENT_B_APP_RECV "B_APP_RECV" + #define EVENT_B_PERSIST_BEGIN "B_PERSIST_BEGIN" + #define EVENT_B_PERSIST_END "B_PERSIST_END" + // ... 其他事件 + + typedef struct { + int64_t ts_unix_nano; + char node_role[16]; + char node_id[64]; + char event[32]; + message_type_t message_type; + uint64_t message_id; + char from[64]; + char to[64]; + char file_name[256]; + int body_size; + } latency_event_t; + + typedef struct latency_logger latency_logger_t; + + 核心函数： + - latency_logger_t *latencylog_new_jsonl(const char *path) — 创建 JSONL 文件日志器 + - void latencylog_log_event(latency_logger_t *logger, const latency_event_t *event) — 写事件 + - void latencylog_log_message_event(latency_logger_t *logger, const char *node_role, const char *node_id, const char *event_name, const message_t + *msg) — 为业务消息记事件 + - void latencylog_close(latency_logger_t *logger) — 关闭 + - int latencylog_is_business_message(const message_t *msg) — 判断是否业务消息 + + 4. transport_kcp.h / transport_kcp.c + + 对应 Go: cmd/internal/transport/kcp.go + kcp_packet_conn.go + + KCP 连接封装，底层用 raw ikcp + UDP socket： + + typedef struct kcp_conn { + ikcpcb *kcp; + int udp_fd; + struct sockaddr_in remote_addr; + pthread_mutex_t write_mu; + pthread_t recv_thread; // 底层 UDP -> ikcp_input 的线程 + latency_logger_t *logger; + char node_role[16]; + char node_id[64]; + int closed; + } kcp_conn_t; + + 核心函数： + - kcp_conn_t *kcp_conn_dial(const char *server_addr, const char *bind_ip, const char *bind_device) — 客户端拨号 + - kcp_conn_t *kcp_conn_accept(int udp_fd, struct sockaddr_in *remote, uint32_t conv) — 服务端接受 + - int kcp_conn_send(kcp_conn_t *conn, const message_t *msg) — 发送消息 + - int kcp_conn_receive(kcp_conn_t *conn, message_t *msg) — 接收消息 + - void kcp_conn_close(kcp_conn_t *conn) — 关闭 + + KCP 配置参数（与 Go 版一致）： + #define KCP_NODELAY 1 + #define KCP_INTERVAL 10 + #define KCP_RESEND 2 + #define KCP_NC 1 + #define KCP_WND_SIZE 256 + #define KCP_MTU 1400 + + KCP 底层架构说明： + Go 版使用 kcp-go 库，该库内部维护了一个 Listener 来多路复用一个 UDP socket 上的多个 KCP session（通过 conv ID 区分）。在 C 中需要自行实现： + - 服务端：一个 UDP socket 监听，一个接收线程读取所有 UDP 包，根据 conv ID 分发到对应的 ikcpcb + - 客户端：一个 UDP socket，一个 ikcpcb，一个后台线程负责 UDP recv -> ikcp_input + + 5. transport_udp.h / transport_udp.c + + 对应 Go: cmd/internal/transport/udp.go + udp_linux.go + + typedef struct udp_conn { + int fd; + struct sockaddr_in peer_addr; + syscall_rawconn_t raw; // syscall.RawConn 等价 + int linux_timestamping_enabled; + latency_logger_t *logger; + tx_timestamp_debug_logger_t *tx_debug_logger; + uint32_t tx_packet_seq; + // pending TX records for errqueue correlation + struct udp_tx_pending *pending_tx; + char node_role[16]; + char node_id[64]; + pthread_mutex_t write_mu; + } udp_conn_t; + + 完整实现 Linux SO_TIMESTAMPING： + - TX: SOF_TIMESTAMPING_TX_SCHED + SOF_TIMESTAMPING_TX_SOFTWARE + OPT_ID + - RX: SOF_TIMESTAMPING_RX_SOFTWARE + - errqueue 采集: recvmsg(MSG_ERRQUEUE) 读取 SCM_TIMESTAMPING 控制消息 + - TX timestamp debug logger: 记录 send_chunk / errqueue_event 到 JSONL + - 对应 Go 文件: udp_linux.go, tx_timestamp_debug.go + + 同时为 KCP packet conn 实现类似的 timestamping： + - 对应 Go 文件: kcp_packet_conn_linux.go, kcp_packet_debug.go + - KCP 底层 UDP 包的 TX/RX kernel timestamp 记录 + + KCP session stats 完整实现： + - session-level: conv, RTO, SRTT, SRTTVar 周期采样 + - 对应 Go 文件: kcp_session_stats.go + + 6. server_kcp_hub.h / server_kcp_hub.c + + 对应 Go: cmd/internal/server/kcp_hub.go + + typedef struct { + pthread_rwlock_t lock; + // peer_id -> kcp_conn_t* 的哈希表 + struct peer_entry *peers; // 简单链表或哈希表 + int peer_count; + latency_logger_t *logger; + // relay 相关 + int relay_udp_fd; + struct sockaddr_in relay_peer_addr; + int relay_peer_known; + } kcp_hub_t; + + 核心函数： + - kcp_hub_t *kcp_hub_new(latency_logger_t *logger) — 创建 hub + - int kcp_hub_serve_session(kcp_hub_t *hub, kcp_conn_t *conn) — 处理新会话（注册 + 转发循环） + - void kcp_hub_set_relay(kcp_hub_t *hub, int udp_fd, struct sockaddr_in *peer_addr) — 配置 relay + - int kcp_hub_serve_relay(kcp_hub_t *hub) — relay 接收循环 + - void kcp_hub_free(kcp_hub_t *hub) — 释放 + + 服务端 KCP listener 实现： + - 主 UDP socket 监听 + - 收到新 conv ID 时创建新 ikcpcb + - 用 pthread 为每个 session 创建处理线程 + + 7. server_udp_relay.h / server_udp_relay.c + + 对应 Go: cmd/internal/server/udp_relay.go + + typedef struct { + int downstream_fd; // 监听端 + int upstream_fd; // 连接到 hub D 的 UDP + struct sockaddr_in upstream_addr; + struct sockaddr_in client_addr; + int client_known; + pthread_mutex_t lock; + } udp_relay_t; + + 核心函数： + - udp_relay_t *udp_relay_new(int listen_fd, struct sockaddr_in *upstream_addr) — 创建 + - int udp_relay_serve(udp_relay_t *relay) — 双向转发循环（两个线程） + - void udp_relay_close(udp_relay_t *relay) — 关闭 + + 8. peer_kcp_client.h / peer_kcp_client.c + + 对应 Go: cmd/internal/peer/kcp_client.go + persist.go + + typedef struct { + char id[64]; + kcp_conn_t *conn; + latency_logger_t *logger; + uint64_t next_msg_id; // atomic + pthread_mutex_t id_mu; + } kcp_client_t; + + 核心函数： + - kcp_client_t *kcp_client_dial(const char *server_addr, const char *peer_id, ...) — 连接并注册 + - int kcp_client_send_text(kcp_client_t *c, const char *to, const char *text) — 发文本 + - int kcp_client_send_file(kcp_client_t *c, const char *to, const char *path) — 发文件 + - int kcp_client_receive(kcp_client_t *c, message_t *msg) — 接收 + - int kcp_client_persist_message(kcp_client_t *c, const message_t *msg, const char *inbox_dir) — 持久化 + - void kcp_client_close(kcp_client_t *c) — 关闭 + + 9. interactive.h / interactive.c + + 对应 Go: cmd/kcppeer/interactive.go + + 交互式命令行 REPL： + - help / text / file / quit + - int run_interactive_shell(kcp_client_t *client) — 运行交互循环 + + 10. cmd/kcpserver.c + + 对应 Go: cmd/kcpserver/main.go + + 用法: + kcpserver -listen 0.0.0.0:10909 # hub 模式 + kcpserver -mode relay -listen 0.0.0.0:10909 -relay-remote 172.21.32.15:10909 # relay 模式 + + - 解析命令行参数 (getopt) + - hub 模式：创建 KCP listener -> 接受连接 -> kcp_hub_serve_session + - relay 模式：创建 UDP relay -> udp_relay_serve + + 11. cmd/kcppeer.c + + 对应 Go: cmd/kcppeer/main.go + + 用法: + kcppeer -id peer-a -server 172.21.32.15:10909 -relay-via 106.55.173.235:10909 -inbox-dir inbox/a + kcppeer -id peer-b -server 81.70.156.140:10909 -inbox-dir inbox/b + + - 连接到 KCP server + - 启动接收线程 + - 运行交互式 shell 或单次发送 + + 12. cmd/kcpping.c + + 对应 Go: cmd/kcpping/main.go + platform_linux.go + + KCP ping 工具： + - ping 模式: 发 JSON payload, 计算 RTT + - echo 模式: 回弹文本消息 + - 统计: min/avg/max/p50/p95/p99/stddev + + KCP session 多路复用实现（核心难点） + + Go 版的 kcp-go 库在一个 UDP socket 上透明地多路复用多个 KCP session。C 版需要手动实现： + + typedef struct kcp_listener { + int udp_fd; + pthread_t recv_thread; + pthread_mutex_t sessions_lock; + // conv -> kcp_session 的哈希表 + struct kcp_session_entry *sessions; + // 新会话通知队列 + kcp_conn_t **accept_queue; + int accept_queue_head, accept_queue_tail, accept_queue_cap; + pthread_mutex_t accept_lock; + pthread_cond_t accept_cond; + } kcp_listener_t; + + - kcp_listener_t *kcp_listen(const char *addr, const char *bind_device) — 创建 listener + - kcp_conn_t *kcp_accept(kcp_listener_t *listener) — 阻塞等待新会话 + - 内部 recv_thread 循环读 UDP 包，解析前 4 字节 conv ID，分发到对应 ikcpcb + - 未知 conv ID 时创建新 session 并放入 accept_queue + + 编译 + + CC = gcc + CFLAGS = -Wall -Wextra -O2 -pthread -D_GNU_SOURCE + LDFLAGS = -lpthread + + SRCS = src/protocol.c src/transport_kcp.c src/transport_udp.c \ + src/server_kcp_hub.c src/server_udp_relay.c \ + src/peer_kcp_client.c src/latencylog.c src/interactive.c \ + src/cJSON.c third_party/kcp/ikcp.c + + all: kcpserver kcppeer kcpping + + kcpserver: cmd/kcpserver.c $(SRCS) + $(CC) $(CFLAGS) -Iinclude -Ithird_party/kcp -o $@ $^ $(LDFLAGS + + kcppeer: cmd/kcppeer.c $(SRCS) + $(CC) $(CFLAGS) -Iinclude -Ithird_party/kcp -o $@ $^ $(LDFLAGS + + kcpping: cmd/kcpping.c $(SRCS) + $(CC) $(CFLAGS) -Iinclude -Ithird_party/kcp -o $@ $^ $(LDFLAGS + + 验证方法 + + 1. 编译: make all 无错误无警告 + 2. 单机测试: + - 启动 hub: ./kcpserver -listen 0.0.0.0:10909 + - 启动 peer-a: ./kcppeer -id peer-a -server 127.0.0.1:10909 -inbox-dir inbox/a + - 启动 peer-b: ./kcppeer -id peer-b -server 127.0.0.1:10909 -inbox-dir inbox/b + - peer-b shell 中: text peer-a hello + - 验证 peer-a 收到消息并落盘到 inbox/a/ + 3. 跨机器 relay 测试: + - D 机器: ./kcpserver -listen 0.0.0.0:10909 + - C 机器: ./kcpserver -mode relay -listen 0.0.0.0:10909 -relay-remote :10909 + - A 机器: ./kcppeer -id peer-a -server :10909 -relay-via :10909 -inbox-dir inbox/a + - B 机器: ./kcppeer -id peer-b -server :10909 -inbox-dir inbox/b + 4. kcpping 测试: + - echo 端: ./kcpping -id peer-a -server :10909 -echo + - ping 端: ./kcpping -id peer-b -server :10909 -to peer-a -count 20 -interval 100 + + 实现顺序 + + 1. 集成第三方库 (ikcp, cJSON) + 2. protocol 模块 (消息编解码) + 3. latencylog 模块 (日志事件) + 4. transport_kcp 模块 (KCP 连接 + listener 多路复用) + 5. transport_udp 模块 (UDP 连接，简化 timestamping) + 6. server_udp_relay 模块 (C 节点 relay) + 7. server_kcp_hub 模块 (D 节点 hub) + 8. peer_kcp_client 模块 (A/B 节点 peer + persist) + 9. interactive 模块 (交互 shell) + 10. cmd/kcpserver.c 主程序 + 11. cmd/kcppeer.c 主程序 + 12. cmd/kcpping.c 主程序 + 13. Makefile + README + 14. 编译测试 + + 简化决策 + + - 不实现 TCP 传输: 去除 transport/tcp.go, server/hub.go(TCP版), peer/client.go(TCP版) 等 TCP 相关代码 + - 不写测试: 去除所有 _test.go 对应的测试代码 + - 完整实现 Linux timestamping: 完整移植 SO_TIMESTAMPING 的 TX/RX timestamp 采集，包括 errqueue TX sched/software timestamp 和 RX software + timestamp，以及对应的 debug logger (KCPPacketDebugLogger, TXTimestampDebugLogger) + - 完整实现 KCP session stats: 包括 session-level RTO/SRTT 采样和 JSONL 记录 + - 不实现 latency summary/chart: 不实现 latencysummary 工具和 HTML chart 生成（这是离线分析工具，不属于核心传输功能） + - peer 哈希表: 使用简单链表实现，hub 连接数不多时性能足够 + + +# OmniSocketGo -> OmniSocketC 全量 UDP/KCP 迁移计划 + +## Summary +- 在仓库新增 `c/` 子项目，作为 Linux-only、C11、`make` 驱动的独立实现；现有 Go 项目保留不动，作为行为对照。 +- 迁移范围按“全量 Go 对齐，但去掉 TCP 和离线 summary/chart”执行：保留 UDP/KCP 协议、纯 UDP 程序族、KCP 程序族、运行时 JSONL 日志、Linux timestamping、KCP packet debug、KCP session stats、以及 KCP hub-to-hub 内部 relay 能力。 +- 你当前草案需要修正的关键点有 5 个：`protocol_*frame(int fd, ...)` 不适合 KCP；KCP 必须补齐 `ikcp_update/check` 调度与 conv 多路复用；纯 UDP 程序族不能省略；`latencysummary`/HTML chart 本次不迁移；Makefile 需要修正链接目标并统一输出到 `c/bin/`。 + +## Public Interfaces +- 新增二进制：`kcpserver`、`kcppeer`、`kcpping`、`udpserver`、`udppeer`、`udpping`、`udprelay`。 +- `kcpserver` 保留当前 Go 旗标语义：`-mode=hub|relay`、`-listen`、`-bind-device`、`-relay-remote`、deprecated relay aliases、`-latency-log`、`-kcp-ts-debug-log`、`-kcp-session-stats-log`、`-kcp-session-stats-interval`。 +- `kcppeer` 保留当前 Go 旗标语义：`-id`、`-server`、`-relay-via`、`-to`、`-text`、`-file`、`-bind-ip`、`-bind-device`、`-inbox-dir`、`-interactive`、`-latency-log`、`-kcp-ts-debug-log`、`-kcp-session-stats-log`、`-kcp-session-stats-interval`。 +- `kcpping`、`udpserver`、`udppeer`、`udpping`、`udprelay` 的参数与输出行为对齐当前 Go 入口；`udpserver` 默认不开 Linux timestamping，只有设置 `-tx-ts-debug-log` 时才启用。 +- 协议层改为内存接口，不再设计 fd 风格 API：`message_t`、datagram 编解码、stream frame 编解码、增量 frame feed。 +- 运行时日志层保留当前 JSON 字段和事件名；server/hub 继续作为 black-box relay，不新增端到端业务事件。 +- 内部网络 API 包括：`udp_conn_t`、`kcp_conn_t`、`kcp_listener_t`、`udp_hub_t`、`kcp_hub_t`、`udp_relay_t`、`udp_client_t`、`kcp_client_t`；KCP hub-to-hub relay 只做库级能力，不新增额外 CLI。 + +## Implementation Changes +- 目录固定为 `c/include`、`c/src`、`c/cmd`、`c/third_party/{ikcp,cjson}`、`c/bin`、`c/README.md`、`c/Makefile`。 +- 第三方依赖直接 vendoring 到仓库：`ikcp` 用于 KCP 核心，`cJSON` 同时用于协议头、ping payload、运行时日志。 +- 协议规则完全保留：`text/file/register/error`、`ServerPeerID`、`8 MiB` 限制、UTF-8 校验、`file_name` 约束、`register/error` 来源与目标约束。 +- 线上 wire format 完全保留：UDP datagram 为 `[4B headerLen][header JSON][body]`；KCP stream 为 `[4B frameLen][4B headerLen][header JSON][body]`。 +- inbox 持久化完全保留：文本追加写 `messages.log` JSONL；文件落盘为 `--`。 +- UDP 传输层实现 connected/unconnected 两种发送模式，保留 register/forward 消息收发、Linux SO_TIMESTAMPING、TX errqueue 关联、JSONL debug 记录。 +- KCP 客户端连接采用“一连接一 UDP socket + 一 `ikcpcb` + 一接收线程 + 一 update 线程 + 一阻塞接收缓冲区/条件变量”模型。 +- KCP 服务端监听采用“单 listener UDP socket + 单 listener RX 线程 + conv->session 表 + accept 队列”模型；每个 session 拥有自己的 `ikcpcb`、update 线程、接收缓冲区和关闭状态，发送通过 listener 共享 socket 和写锁完成。 +- `kcpserver` 的 relay 模式保持为原始 UDP 端口转发，不解码协议；`udprelay` 同样保持透明字节转发。 +- 纯 UDP hub、KCP hub、双 peer、双 ping 工具、两套 interactive shell 全部对齐现有 Go 行为。 +- KCP hub 保留“先本地投递，再尝试 relay”的策略；未知目标、重复注册、已注册 peer 再发 `register/error`、过大 relay 消息等错误路径全部保留。 +- Linux 观测能力完整迁移：业务事件 JSONL、UDP TX debug、KCP packet debug、KCP session/process stats；不迁移 `latencysummary` 与 HTML chart。 + +## Acceptance +- 在 Linux 上执行 `make` 能无缺失符号地构建 7 个二进制，并输出到 `c/bin/`。 +- 纯 UDP 冒烟通过：`udpserver` + 两个 `udppeer` 可双向收发文本和文件，`udpping` 的 echo/ping 正常。 +- 单 hub KCP 冒烟通过：`kcpserver` + 两个 `kcppeer` 可双向收发文本和文件，`kcpping` 的 echo/ping 正常。 +- README 目标拓扑通过：D 跑 `kcpserver -mode=hub`，C 跑 `kcpserver -mode=relay`，A 用 `-relay-via C` 连 D，B 直连 D，A/B 双向传输正常。 +- 全量 Go 对齐场景通过：两个 KCP hub 通过内部 raw UDP relay API 互通，跨 hub 文本、文件、错误回送行为与当前 Go 一致。 +- 负路径通过：重复注册被拒、未注册 UDP sender 被拒、未知目标返回 `error`、已注册 peer 发送 `register/error` 被拒、oversize relayed message 在实际 `WriteTo` 前被拒、`bind-ip`/`bind-device` 非法值在启动时失败。 +- 打开任一日志旗标后，生成的 JSONL 记录字段名、事件名、时间戳语义与现有运行时日志一致，并在 Linux 支持的情况下出现非零 kernel timestamps。 + +## Assumptions +- 默认编译器为 `gcc`/`clang`，编译参数基线为 `-std=c11 -Wall -Wextra -O2 -pthread -D_GNU_SOURCE`。 +- 本次不迁移任何 Go 测试文件，也不为 C 版编写自动化测试；验证仅靠 Linux 构建和手工场景回归。 +- 本次不迁移 TCP 入口，也不迁移 `latencysummary`/HTML chart。 +- hub-to-hub relay 在 C 版中实现为内部库能力，保持与当前 Go 仓库一致的范围，不额外扩展新的公共命令。 diff --git a/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/latencylog/logger.go b/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/latencylog/logger.go new file mode 100644 index 0000000..f1f1f31 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/latencylog/logger.go @@ -0,0 +1,166 @@ +package latencylog + +import ( + "encoding/json" + "os" + "path/filepath" + "sync" + "time" + + "omnisocketgo/cmd/internal/protocol" +) + +const ( + NodeRolePeer = "peer" //客户端节点 + NodeRoleServer = "server" //云端转发节点 +) + +// 记录的消息事件的类型常量。 +const ( + EventAAppPrepBegin = "A_APP_PREP_BEGIN" // A 端应用开始准备这条消息 + EventATXSched = "A_TX_SCHED" // A 端进入 Linux qdisc 之前 + EventATXSoftware = "A_TX_SOFTWARE" // A 端即将交给网卡驱动 + EventATXHardware = "A_TX_HARDWARE" // A 端网卡真正发出到物理介质 + EventBRXHardware = "B_RX_HARDWARE" // B 端网卡真正从物理介质收到 + EventBRXSoftware = "B_RX_SOFTWARE" // B 端驱动把数据交给 Linux 接收栈 + EventBAppRecv = "B_APP_RECV" // B 端应用真正读到完整消息 + EventBPersistBegin = "B_PERSIST_BEGIN" // B 端开始写盘 + EventBPersistEnd = "B_PERSIST_END" // B 端写盘完成 + + EventSendHandoffBegin = "send_handoff_begin" // 调试事件：应用把消息交给传输层开始 + EventSendHandoffEnd = "send_handoff_end" // 调试事件：应用把消息交给传输层结束 +) + +// Event 是一条时延时间戳日志记录。 +type Event struct { + TsUnixNano int64 `json:"ts_unix_nano"` + NodeRole string `json:"node_role"` + NodeID string `json:"node_id"` + Event string `json:"event"` + MessageType protocol.MessageType `json:"message_type"` + MessageID uint64 `json:"message_id"` + From string `json:"from"` + To string `json:"to"` + FileName string `json:"file_name,omitempty"` + BodySize int `json:"body_size"` +} + +// Logger 负责接收事件并将其写入外部介质。 +type Logger interface { + LogEvent(Event) error +} + +// NoopLogger 是默认的空实现。 +type NoopLogger struct{} + +// LogEvent 对空日志实现始终返回 nil。 +func (NoopLogger) LogEvent(Event) error { + return nil +} + +// JSONLLogger 以 JSONL 形式追加写日志文件。 +type JSONLLogger struct { + mu sync.Mutex + closeOnce sync.Once + closeErr error + file *os.File +} + +// NewJSONLLogger 创建一个线程安全的 JSONL 文件日志器。 +func NewJSONLLogger(path string) (*JSONLLogger, error) { + dir := filepath.Dir(path) + if err := os.MkdirAll(dir, 0o755); err != nil { + return nil, err + } + + file, err := os.OpenFile(path, os.O_CREATE|os.O_WRONLY|os.O_APPEND, 0o644) + if err != nil { + return nil, err + } + + return &JSONLLogger{file: file}, nil +} + +// LogEvent 以单行 JSON 的形式追加一条事件。 +func (l *JSONLLogger) LogEvent(event Event) error { + line, err := json.Marshal(event) + if err != nil { + return err + } + + l.mu.Lock() + defer l.mu.Unlock() + + if _, err := l.file.Write(append(line, '\n')); err != nil { + return err + } + + return nil +} + +// Close 关闭底层文件；重复调用是安全的。 +func (l *JSONLLogger) Close() error { + l.closeOnce.Do(func() { + l.closeErr = l.file.Close() + }) + + return l.closeErr +} + +// IsBusinessMessage 判断消息是否属于要参与 A-C-B 时延分析的业务消息。 +func IsBusinessMessage(msg protocol.Message) bool { + switch msg.Type { + case protocol.MessageTypeText, protocol.MessageTypeFile: + return true + default: + return false + } +} + +// NewMessageEvent 用当前 UTC 时间为一条业务消息构造事件。 +func NewMessageEvent(nodeRole, nodeID, eventName string, msg protocol.Message) Event { + return NewMessageEventAt(time.Now().UTC().UnixNano(), nodeRole, nodeID, eventName, msg) +} + +// NewMessageEventAt 用指定的 UnixNano 时间为一条业务消息构造事件。 +func NewMessageEventAt(tsUnixNano int64, nodeRole, nodeID, eventName string, msg protocol.Message) Event { + return Event{ + TsUnixNano: tsUnixNano, + NodeRole: nodeRole, + NodeID: nodeID, + Event: eventName, + MessageType: msg.Type, + MessageID: msg.ID, + From: msg.From, + To: msg.To, + FileName: msg.FileName, + BodySize: len(msg.Body), + } +} + +// LogBestEffort 写一条事件，失败时静默忽略，避免打断主收发流程。 +func LogBestEffort(logger Logger, event Event) { + if logger == nil { + return + } + + _ = logger.LogEvent(event) +} + +// LogMessageEvent 为业务消息构造并写入一条事件。 +func LogMessageEvent(logger Logger, nodeRole, nodeID, eventName string, msg protocol.Message) { + if !IsBusinessMessage(msg) { + return + } + + LogBestEffort(logger, NewMessageEvent(nodeRole, nodeID, eventName, msg)) +} + +// LogMessageEventAt 为业务消息写入一条指定时间戳的事件。 +func LogMessageEventAt(logger Logger, nodeRole, nodeID, eventName string, tsUnixNano int64, msg protocol.Message) { + if !IsBusinessMessage(msg) { + return + } + + LogBestEffort(logger, NewMessageEventAt(tsUnixNano, nodeRole, nodeID, eventName, msg)) +} diff --git a/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/latencylog/logger_test.go b/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/latencylog/logger_test.go new file mode 100644 index 0000000..d1850fe --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/latencylog/logger_test.go @@ -0,0 +1,131 @@ +package latencylog + +import ( + "bufio" + "encoding/json" + "os" + "path/filepath" + "sync" + "testing" + + "omnisocketgo/cmd/internal/protocol" +) + +func TestJSONLLoggerWritesOneEventPerLine(t *testing.T) { + path := filepath.Join(t.TempDir(), "latency.jsonl") + + logger, err := NewJSONLLogger(path) + if err != nil { + t.Fatalf("NewJSONLLogger() error = %v", err) + } + t.Cleanup(func() { + _ = logger.Close() + }) + + event := Event{ + TsUnixNano: 123, + NodeRole: NodeRolePeer, + NodeID: "peer-a", + Event: EventAAppPrepBegin, + MessageType: protocol.MessageTypeText, + MessageID: 1, + From: "peer-a", + To: "peer-b", + BodySize: 5, + } + if err := logger.LogEvent(event); err != nil { + t.Fatalf("LogEvent() error = %v", err) + } + + file, err := os.Open(path) + if err != nil { + t.Fatalf("os.Open() error = %v", err) + } + defer file.Close() + + scanner := bufio.NewScanner(file) + if !scanner.Scan() { + t.Fatal("expected one JSONL line, got none") + } + + var got Event + if err := json.Unmarshal(scanner.Bytes(), &got); err != nil { + t.Fatalf("json.Unmarshal() error = %v", err) + } + if got != event { + t.Fatalf("event mismatch: got %+v want %+v", got, event) + } + if scanner.Scan() { + t.Fatal("expected exactly one JSONL line") + } + if err := scanner.Err(); err != nil { + t.Fatalf("scanner.Err() = %v", err) + } +} + +func TestJSONLLoggerHandlesConcurrentWrites(t *testing.T) { + path := filepath.Join(t.TempDir(), "latency.jsonl") + + logger, err := NewJSONLLogger(path) + if err != nil { + t.Fatalf("NewJSONLLogger() error = %v", err) + } + t.Cleanup(func() { + _ = logger.Close() + }) + + const total = 32 + + var wg sync.WaitGroup + for i := 0; i < total; i++ { + i := i + wg.Add(1) + go func() { + defer wg.Done() + + err := logger.LogEvent(Event{ + TsUnixNano: int64(i + 1), + NodeRole: NodeRoleServer, + NodeID: protocol.ServerPeerID, + Event: EventBAppRecv, + MessageType: protocol.MessageTypeFile, + MessageID: uint64(i + 1), + From: "peer-a", + To: "peer-b", + FileName: "payload.bin", + BodySize: 3, + }) + if err != nil { + t.Errorf("LogEvent() error = %v", err) + } + }() + } + wg.Wait() + + file, err := os.Open(path) + if err != nil { + t.Fatalf("os.Open() error = %v", err) + } + defer file.Close() + + scanner := bufio.NewScanner(file) + var count int + seen := make(map[uint64]bool, total) + for scanner.Scan() { + var event Event + if err := json.Unmarshal(scanner.Bytes(), &event); err != nil { + t.Fatalf("json.Unmarshal() error = %v", err) + } + count++ + seen[event.MessageID] = true + } + if err := scanner.Err(); err != nil { + t.Fatalf("scanner.Err() = %v", err) + } + if count != total { + t.Fatalf("line count = %d, want %d", count, total) + } + if len(seen) != total { + t.Fatalf("unique message count = %d, want %d", len(seen), total) + } +} diff --git a/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/latencylog/summary.go b/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/latencylog/summary.go new file mode 100644 index 0000000..dd825d1 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/latencylog/summary.go @@ -0,0 +1,457 @@ +package latencylog + +import ( + "bufio" + "encoding/json" + "fmt" + "os" + "path/filepath" + "sort" + + "omnisocketgo/cmd/internal/protocol" +) + +// Summary 是针对单条消息的时延的规则列表。 +var requiredTimestampNames = []string{ + EventAAppPrepBegin, // A 端应用开始准备这条消息 + EventATXSched, // A 端进入 Linux qdisc 之前 + EventATXSoftware, // A 端即将交给网卡驱动 + EventBRXSoftware, // B 端网卡驱动把数据交给 Linux 接收栈 + EventBAppRecv, // B 端应用真正读到完整消息 + EventBPersistEnd, // B 端写盘完成 +} + +// Summary 是针对单条消息的时延整理结果。 +type Summary struct { + MessageType protocol.MessageType `json:"message_type"` //消息类型 + MessageID uint64 `json:"message_id"` //消息ID + From string `json:"from"` //发送方 + To string `json:"to"` //接收方 + FileName string `json:"file_name,omitempty"` //文件名（仅文件消息） + BodySize int `json:"body_size"` //消息体大小（字节数） + Timestamps map[string]int64 `json:"timestamps"` //事件时间戳，key 是事件名称，value 是 UnixNano 时间戳 + + AProcessingLatencyNS *int64 `json:"a_processing_latency_ns,omitempty"` // A 处理时延：A_TX_SCHED - A_APP_PREP_BEGIN + AQueueLatencyNS *int64 `json:"a_queue_latency_ns,omitempty"` // A 排队时延：A_TX_SOFTWARE - A_TX_SCHED + ABTransportPropagationNS *int64 `json:"a_b_transport_propagation_ns,omitempty"` // A-B 传输+传播时延近似：B_APP_RECV - A_TX_SOFTWARE + BKernelReceivePathLatencyNS *int64 `json:"b_kernel_receive_path_latency_ns,omitempty"` // B 内核接收路径近似：B_APP_RECV - B_RX_SOFTWARE + BProcessingLatencyNS *int64 `json:"b_processing_latency_ns,omitempty"` // B 处理时延：B_PERSIST_END - B_APP_RECV + EndToEndLatencyNS *int64 `json:"end_to_end_latency_ns,omitempty"` // 端到端时延：B_PERSIST_END - A_APP_PREP_BEGIN + AProcessingBitrateBPS *float64 `json:"a_processing_bitrate_bps,omitempty"` // A 处理阶段近似比特率：(BodySize * 8) / A 处理时延（秒） + ABTransportPropagationBitrateBPS *float64 `json:"a_b_transport_propagation_bitrate_bps,omitempty"` // A-B 传输+传播阶段近似比特率：(BodySize * 8) / A-B 传输+传播时延（秒） + EndToEndBitrateBPS *float64 `json:"end_to_end_bitrate_bps,omitempty"` // 端到端近似比特率：(BodySize * 8) / 端到端时延（秒） + ApproxRTTNS *int64 `json:"approx_rtt_ns,omitempty"` // 近似 RTT：首条反向应答的 B_APP_RECV - 当前请求的 A_TX_SOFTWARE + MissingTimestamps []string `json:"missing_timestamps,omitempty"` // 缺失的时间戳列表，包含 requiredTimestampNames 中但在原始事件中没有的事件名称 +} + +// LoadEventsFromFiles 从JSONL 原始日志文件中加载事件。 +type messageKey struct { + MessageType protocol.MessageType //消息类型 + MessageID uint64 //消息ID + From string //发送方 + To string //接收方 +} + +// LoadEventsFromFiles 从多个 JSONL 原始日志文件中加载事件。 +func LoadEventsFromFiles(paths []string) ([]Event, error) { + var events []Event + for _, path := range paths { + fileEvents, err := LoadEventsFromFile(path) + if err != nil { + return nil, err + } + events = append(events, fileEvents...) + } + + return events, nil +} + +// LoadEventsFromFilesWithSharedMaxOffset 从多个 JSONL 原始日志文件中加载事件， +// 并按每个输入文件的最大 message_id 计算共享截断点。 +func LoadEventsFromFilesWithSharedMaxOffset(paths []string, sharedMaxOffset uint64) ([]Event, *uint64, error) { + eventsByFile := make([][]Event, 0, len(paths)) + var minMaxMessageID uint64 + hasSharedMax := false + + for _, path := range paths { + fileEvents, err := LoadEventsFromFile(path) + if err != nil { + return nil, nil, err + } + + eventsByFile = append(eventsByFile, fileEvents) + + fileMaxMessageID, ok := maxBusinessMessageID(fileEvents) + if !ok { + return nil, nil, nil + } + if !hasSharedMax || fileMaxMessageID < minMaxMessageID { + minMaxMessageID = fileMaxMessageID + hasSharedMax = true + } + } + + if !hasSharedMax { + return nil, nil, nil + } + + cutoff, ok := subtractUint64(minMaxMessageID, sharedMaxOffset) + if !ok { + return []Event{}, nil, nil + } + + var events []Event + for _, fileEvents := range eventsByFile { + events = append(events, filterEventsByMaxMessageID(fileEvents, cutoff)...) + } + + return events, &cutoff, nil +} + +// LoadEventsFromFile 从单个 JSONL 原始日志文件中加载事件。 +func LoadEventsFromFile(path string) ([]Event, error) { + file, err := os.Open(path) + if err != nil { + return nil, fmt.Errorf("latencylog: open raw log %s: %w", path, err) + } + defer file.Close() + + var events []Event + scanner := bufio.NewScanner(file) + for scanner.Scan() { + if len(scanner.Bytes()) == 0 { + continue + } + + var event Event + if err := json.Unmarshal(scanner.Bytes(), &event); err != nil { //解析 JSONL 行失败，返回错误 + return nil, fmt.Errorf("latencylog: decode event from %s: %w", path, err) + } + events = append(events, event) + } + if err := scanner.Err(); err != nil { + return nil, fmt.Errorf("latencylog: scan raw log %s: %w", path, err) + } + + return events, nil +} + +// SummarizeEvents 将原始事件整理成按消息分组的时延结果。 +func SummarizeEvents(events []Event) []Summary { + grouped := make(map[messageKey]*Summary) + + for _, event := range events { + if !IsBusinessEvent(event) { + continue + } + + key := messageKey{ + MessageType: event.MessageType, + MessageID: event.MessageID, + From: event.From, + To: event.To, + } + + summary, ok := grouped[key] + if !ok { + summary = &Summary{ + MessageType: event.MessageType, + MessageID: event.MessageID, + From: event.From, + To: event.To, + FileName: event.FileName, + BodySize: event.BodySize, + Timestamps: make(map[string]int64), + } + grouped[key] = summary + } + + if summary.FileName == "" { + summary.FileName = event.FileName + } + if event.BodySize > 0 { + summary.BodySize = event.BodySize + } + + if existing, exists := summary.Timestamps[event.Event]; !exists || event.TsUnixNano < existing { + summary.Timestamps[event.Event] = event.TsUnixNano + } + } + + summaryPointers := make([]*Summary, 0, len(grouped)) + for _, summary := range grouped { + completeSummary(summary) //补全时延指标和缺失时间戳信息 + summaryPointers = append(summaryPointers, summary) + } + assignApproxRTTs(summaryPointers) + + summaries := make([]Summary, 0, len(summaryPointers)) + for _, summary := range summaryPointers { + summaries = append(summaries, *summary) + } + //对整理结果进行排序，先按发送方、再按接收方、再按消息 ID、最后按消息类型排序，保证输出的稳定性和可读性。 + sort.Slice(summaries, func(i, j int) bool { + if summaries[i].From != summaries[j].From { + return summaries[i].From < summaries[j].From + } + if summaries[i].To != summaries[j].To { + return summaries[i].To < summaries[j].To + } + if summaries[i].MessageID != summaries[j].MessageID { + return summaries[i].MessageID < summaries[j].MessageID + } + return summaries[i].MessageType < summaries[j].MessageType + }) + + return summaries +} + +// WriteSummariesJSONL 将整理结果写成 JSONL 汇总文件。 +func WriteSummariesJSONL(path string, summaries []Summary) error { + if err := os.MkdirAll(filepath.Dir(path), 0o755); err != nil { + return fmt.Errorf("latencylog: create summary dir for %s: %w", path, err) + } + + file, err := os.OpenFile(path, os.O_CREATE|os.O_WRONLY|os.O_TRUNC, 0o644) + if err != nil { + return fmt.Errorf("latencylog: open summary file %s: %w", path, err) + } + defer file.Close() + + writer := bufio.NewWriter(file) + for _, summary := range summaries { //将每条整理结果编码成 JSONL 行并写入文件 + line, err := json.Marshal(summary) + if err != nil { + return fmt.Errorf("latencylog: encode summary for message %d: %w", summary.MessageID, err) + } + if _, err := writer.Write(append(line, '\n')); err != nil { + return fmt.Errorf("latencylog: write summary file %s: %w", path, err) + } + } + + if err := writer.Flush(); err != nil { //将缓冲区内容写入文件 + return fmt.Errorf("latencylog: flush summary file %s: %w", path, err) + } + + return nil +} + +// completeSummary 根据事件时间戳计算时延指标，并找出缺失的时间戳。 +func completeSummary(summary *Summary) { + summary.MissingTimestamps = missingTimestampNames(summary.Timestamps) + + if value := subtractIfPresent(summary.Timestamps, EventATXSched, EventAAppPrepBegin); value != nil { + summary.AProcessingLatencyNS = value + } + if value := subtractIfPresent(summary.Timestamps, EventATXSoftware, EventATXSched); value != nil { + summary.AQueueLatencyNS = value + } + if value := subtractIfPresent(summary.Timestamps, EventBAppRecv, EventATXSoftware); value != nil { + summary.ABTransportPropagationNS = value + } + if value := subtractIfPresent(summary.Timestamps, EventBAppRecv, EventBRXSoftware); value != nil { + summary.BKernelReceivePathLatencyNS = value + } + if value := subtractIfPresent(summary.Timestamps, EventBPersistEnd, EventBAppRecv); value != nil { + summary.BProcessingLatencyNS = value + } + if value := subtractIfPresent(summary.Timestamps, EventBPersistEnd, EventAAppPrepBegin); value != nil { + summary.EndToEndLatencyNS = value + } + + summary.AProcessingBitrateBPS = calculateBitrateBPS(summary.BodySize, summary.AProcessingLatencyNS) + summary.ABTransportPropagationBitrateBPS = calculateBitrateBPS(summary.BodySize, summary.ABTransportPropagationNS) + summary.EndToEndBitrateBPS = calculateBitrateBPS(summary.BodySize, summary.EndToEndLatencyNS) +} + +type routeKey struct { + From string + To string +} + +func assignApproxRTTs(summaries []*Summary) { + grouped := make(map[routeKey][]*Summary) + for _, summary := range summaries { + grouped[routeKey{From: summary.From, To: summary.To}] = append(grouped[routeKey{From: summary.From, To: summary.To}], summary) + } + + for key, requests := range grouped { + replies := grouped[routeKey{From: key.To, To: key.From}] + if len(replies) == 0 { + continue + } + + assignApproxRTTsForRoute( + sortSummariesByTimestamp(requests, EventBAppRecv), + sortSummariesByTimestamp(replies, EventATXSoftware), + ) + } +} + +func assignApproxRTTsForRoute(requests, replies []*Summary) { + replyIndex := 0 + for _, request := range requests { + requestReceivedAtResponder, ok := request.Timestamps[EventBAppRecv] + if !ok { + continue + } + + for replyIndex < len(replies) { + reply := replies[replyIndex] + replySentAtResponder, ok := reply.Timestamps[EventATXSoftware] + if !ok { + replyIndex++ + continue + } + if replySentAtResponder < requestReceivedAtResponder { + replyIndex++ + continue + } + + if value := subtractSummaryTimestamps(reply, EventBAppRecv, request, EventATXSoftware); value != nil { + request.ApproxRTTNS = value + } + replyIndex++ + break + } + } +} + +func sortSummariesByTimestamp(summaries []*Summary, eventName string) []*Summary { + sorted := append([]*Summary(nil), summaries...) + sort.SliceStable(sorted, func(i, j int) bool { + leftTS, leftOK := sorted[i].Timestamps[eventName] + rightTS, rightOK := sorted[j].Timestamps[eventName] + switch { + case leftOK && rightOK: + if leftTS != rightTS { + return leftTS < rightTS + } + case leftOK: + return true + case rightOK: + return false + } + + if sorted[i].MessageID != sorted[j].MessageID { + return sorted[i].MessageID < sorted[j].MessageID + } + if sorted[i].From != sorted[j].From { + return sorted[i].From < sorted[j].From + } + if sorted[i].To != sorted[j].To { + return sorted[i].To < sorted[j].To + } + + return sorted[i].MessageType < sorted[j].MessageType + }) + return sorted +} + +// 返回 requiredTimestampNames 中哪些在给定的 timestamps 中缺失。 +func missingTimestampNames(timestamps map[string]int64) []string { + var missing []string + for _, name := range requiredTimestampNames { + if _, ok := timestamps[name]; !ok { + missing = append(missing, name) + } + } + + return missing +} + +// 如果 timestamps 中同时存在 endName 和 beginName，则返回它们的差值；否则返回 nil。 +func subtractIfPresent(timestamps map[string]int64, endName, beginName string) *int64 { + end, ok := timestamps[endName] + if !ok { + return nil + } + begin, ok := timestamps[beginName] + if !ok { + return nil + } + + value := end - begin + return &value +} + +func subtractSummaryTimestamps(endSummary *Summary, endName string, beginSummary *Summary, beginName string) *int64 { + end, ok := endSummary.Timestamps[endName] + if !ok { + return nil + } + begin, ok := beginSummary.Timestamps[beginName] + if !ok { + return nil + } + + value := end - begin + return &value +} + +// 除法函数，如果 bodySize <= 0 或 latencyNS 不存在或 <= 0，则返回 nil；否则返回 bodySize / latencyNS 的结果。 +func calculateBitrateBPS(bodySize int, latencyNS *int64) *float64 { + if bodySize <= 0 || latencyNS == nil || *latencyNS <= 0 { + return nil + } + + value := float64(bodySize) * 8 * 1_000_000_000 / float64(*latencyNS) + return &value +} + +// 最大 message_id 计算函数 +func maxBusinessMessageID(events []Event) (uint64, bool) { + var maxMessageID uint64 + hasBusinessMessage := false + + for _, event := range events { + if !IsBusinessEvent(event) { + continue + } + if !hasBusinessMessage || event.MessageID > maxMessageID { + maxMessageID = event.MessageID + hasBusinessMessage = true + } + } + + return maxMessageID, hasBusinessMessage +} + +// 根据 message_id 截断事件列表的函数 +func filterEventsByMaxMessageID(events []Event, maxMessageID uint64) []Event { + filtered := make([]Event, 0, len(events)) + for _, event := range events { + if event.MessageID > maxMessageID { + continue + } + filtered = append(filtered, event) + } + + return filtered +} + +func subtractUint64(value, offset uint64) (uint64, bool) { + if offset > value { + return 0, false + } + + return value - offset, true +} + +// 判断事件是否是业务相关的时延事件（其中一项） +func IsBusinessEvent(event Event) bool { + switch event.Event { + case EventAAppPrepBegin, + EventATXSched, + EventATXSoftware, + EventATXHardware, + EventBRXHardware, + EventBRXSoftware, + EventBAppRecv, + EventBPersistBegin, + EventBPersistEnd: + return true + default: + return false + } +} diff --git a/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/latencylog/summary_chart.go b/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/latencylog/summary_chart.go new file mode 100644 index 0000000..a37a4c2 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/latencylog/summary_chart.go @@ -0,0 +1,498 @@ +package latencylog + +import ( + "bufio" + "fmt" + "html/template" + "os" + "path/filepath" + "strings" + + "omnisocketgo/cmd/internal/protocol" +) + +const summaryChartHTMLTemplate = ` + + + + + Latency Summary Chart + + + +
+

Latency Summary

+

A simple per-message end-to-end latency chart generated from summarized JSONL records.

+ +
+
+
Messages
+
{{.TotalMessages}}
+
+
+
With End-To-End
+
{{.MessagesWithEndToEnd}}
+
+
+
Average End-To-End
+
{{.AverageEndToEnd}}
+
+
+
Max End-To-End
+
{{.MaxEndToEnd}}
+
+
+ +
+ {{range .Legend}} + + + {{.Label}} + + {{end}} +
+ + {{if .Rows}} +
+ {{range .Rows}} +
+
+

{{.Title}}

+
{{.EndToEnd}}
+
+
{{.Subtitle}}
+
{{.ApproxRTT}}
+ {{if .RatioMetrics}} +
+ {{range .RatioMetrics}} + {{.Label}} {{.Value}} + {{end}} +
+ {{end}} +
+ {{range .Segments}} +
+ {{end}} +
+ {{if .Segments}} +
+ {{range .Segments}} + + + {{.Label}} {{.Value}} + + {{end}} +
+ {{end}} + {{if .MissingTimestamps}} +
Missing timestamps: {{.MissingTimestamps}}
+ {{end}} +
+ {{end}} +
+ {{else}} +
No summarized messages were available for chart rendering.
+ {{end}} +
+ + +` + +type summaryChartPage struct { + TotalMessages int + MessagesWithEndToEnd int + AverageEndToEnd string + MaxEndToEnd string + Legend []summaryChartLegendItem + Rows []summaryChartRow +} + +type summaryChartLegendItem struct { + Label string + Color string +} + +type summaryChartRow struct { + Title string + Subtitle string + EndToEnd string + ApproxRTT string + MissingTimestamps string + RatioMetrics []summaryChartValue + Segments []summaryChartSegment +} + +type summaryChartSegment struct { + Label string + Value string + Color string + WidthPercent float64 +} + +type summaryChartValue struct { + Label string + Value string +} + +type summaryChartSegmentMetric struct { + label string + value *int64 + color string +} + +// WriteSummariesHTMLChart 将整理结果写成一个可直接在浏览器中打开的简单 HTML 图表。 +func WriteSummariesHTMLChart(path string, summaries []Summary) error { + if err := os.MkdirAll(filepath.Dir(path), 0o755); err != nil { + return fmt.Errorf("latencylog: create chart dir for %s: %w", path, err) + } + + file, err := os.OpenFile(path, os.O_CREATE|os.O_WRONLY|os.O_TRUNC, 0o644) + if err != nil { + return fmt.Errorf("latencylog: open chart file %s: %w", path, err) + } + defer file.Close() + + page := buildSummaryChartPage(summaries) + tmpl, err := template.New("summary-chart").Parse(summaryChartHTMLTemplate) + if err != nil { + return fmt.Errorf("latencylog: parse chart template: %w", err) + } + + writer := bufio.NewWriter(file) + if err := tmpl.Execute(writer, page); err != nil { + return fmt.Errorf("latencylog: render chart %s: %w", path, err) + } + if err := writer.Flush(); err != nil { + return fmt.Errorf("latencylog: flush chart %s: %w", path, err) + } + + return nil +} + +func buildSummaryChartPage(summaries []Summary) summaryChartPage { + page := summaryChartPage{ + TotalMessages: len(summaries), + Legend: []summaryChartLegendItem{ + {Label: "A processing", Color: "var(--a-proc)"}, + {Label: "A queue", Color: "var(--a-queue)"}, + {Label: "A-B transport + propagation", Color: "var(--transport)"}, + {Label: "B processing", Color: "var(--b-proc)"}, + {Label: "Unknown / missing", Color: "var(--unknown)"}, + }, + Rows: make([]summaryChartRow, 0, len(summaries)), + } + + var ( + endToEndValues []int64 + totalEndToEnd int64 + maxEndToEnd int64 + ) + + for _, summary := range summaries { + page.Rows = append(page.Rows, buildSummaryChartRow(summary)) + + if summary.EndToEndLatencyNS == nil { + continue + } + endToEnd := *summary.EndToEndLatencyNS + endToEndValues = append(endToEndValues, endToEnd) + totalEndToEnd += endToEnd + if endToEnd > maxEndToEnd { + maxEndToEnd = endToEnd + } + } + + page.MessagesWithEndToEnd = len(endToEndValues) + page.AverageEndToEnd = "n/a" + page.MaxEndToEnd = "n/a" + if len(endToEndValues) > 0 { + page.AverageEndToEnd = formatLatencyNS(totalEndToEnd / int64(len(endToEndValues))) + page.MaxEndToEnd = formatLatencyNS(maxEndToEnd) + } + + return page +} + +func buildSummaryChartRow(summary Summary) summaryChartRow { + row := summaryChartRow{ + Title: buildSummaryChartTitle(summary), + Subtitle: buildSummaryChartSubtitle(summary), + EndToEnd: "End-to-end: n/a", + ApproxRTT: "Approx RTT: n/a", + MissingTimestamps: strings.Join(summary.MissingTimestamps, ", "), + } + if summary.ApproxRTTNS != nil && *summary.ApproxRTTNS > 0 { + row.ApproxRTT = fmt.Sprintf("Approx RTT: %s", formatLatencyNS(*summary.ApproxRTTNS)) + } + + ratioMetrics := []struct { + label string + value *float64 + }{ + {label: "A processing bitrate", value: summary.AProcessingBitrateBPS}, + {label: "A-B transport + propagation bitrate", value: summary.ABTransportPropagationBitrateBPS}, + {label: "End-to-end bitrate", value: summary.EndToEndBitrateBPS}, + } + for _, metric := range ratioMetrics { + if metric.value == nil || *metric.value <= 0 { + continue + } + row.RatioMetrics = append(row.RatioMetrics, summaryChartValue{ + Label: metric.label, + Value: formatBitrateBPS(*metric.value), + }) + } + + if summary.EndToEndLatencyNS == nil || *summary.EndToEndLatencyNS <= 0 { + return row + } + + total := *summary.EndToEndLatencyNS + row.EndToEnd = fmt.Sprintf("End-to-end: %s", formatLatencyNS(total)) + + metrics := []summaryChartSegmentMetric{ + {label: "A processing", value: summary.AProcessingLatencyNS, color: "var(--a-proc)"}, + {label: "A queue", value: summary.AQueueLatencyNS, color: "var(--a-queue)"}, + {label: "A-B transport + propagation", value: summary.ABTransportPropagationNS, color: "var(--transport)"}, + {label: "B processing", value: summary.BProcessingLatencyNS, color: "var(--b-proc)"}, + } + + var knownTotal int64 + for _, metric := range metrics { + if metric.value == nil || *metric.value <= 0 { + continue + } + knownTotal += *metric.value + } + + scaleTotal := total + if knownTotal > scaleTotal { + scaleTotal = knownTotal + } + if scaleTotal <= 0 { + return row + } + + for _, metric := range metrics { + if metric.value == nil || *metric.value <= 0 { + continue + } + row.Segments = append(row.Segments, summaryChartSegment{ + Label: metric.label, + Value: formatLatencyNS(*metric.value), + Color: metric.color, + WidthPercent: float64(*metric.value) * 100 / float64(scaleTotal), + }) + } + + if remaining := total - knownTotal; remaining > 0 { + row.Segments = append(row.Segments, summaryChartSegment{ + Label: "Unknown / missing", + Value: formatLatencyNS(remaining), + Color: "var(--unknown)", + WidthPercent: float64(remaining) * 100 / float64(scaleTotal), + }) + } + + return row +} + +func buildSummaryChartTitle(summary Summary) string { + if summary.MessageType == protocol.MessageTypeFile && summary.FileName != "" { + return fmt.Sprintf("%s #%d (%s)", summary.MessageType, summary.MessageID, summary.FileName) + } + + return fmt.Sprintf("%s #%d", summary.MessageType, summary.MessageID) +} + +func buildSummaryChartSubtitle(summary Summary) string { + parts := []string{ + fmt.Sprintf("%s -> %s", summary.From, summary.To), + fmt.Sprintf("%d bytes", summary.BodySize), + } + + if summary.MessageType == protocol.MessageTypeFile && summary.FileName != "" { + parts = append(parts, fmt.Sprintf("file: %s", summary.FileName)) + } + + return strings.Join(parts, " | ") +} + +func formatLatencyNS(ns int64) string { + return fmt.Sprintf("%.3f ms", float64(ns)/1_000_000) +} + +func formatBitrateBPS(bitsPerSecond float64) string { + return fmt.Sprintf("%.3f Mb/s", bitsPerSecond/1_000_000) +} diff --git a/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/latencylog/summary_chart_test.go b/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/latencylog/summary_chart_test.go new file mode 100644 index 0000000..d9f41ec --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/latencylog/summary_chart_test.go @@ -0,0 +1,79 @@ +package latencylog + +import ( + "os" + "path/filepath" + "strings" + "testing" + + "omnisocketgo/cmd/internal/protocol" +) + +func TestWriteSummariesHTMLChart(t *testing.T) { + aProcessing := int64(20_000_000) + aQueue := int64(10_000_000) + transport := int64(40_000_000) + bProcessing := int64(30_000_000) + endToEnd := int64(100_000_000) + aProcessingBitrate := float64(5) * 8 * 1_000_000_000 / float64(aProcessing) + transportBitrate := float64(5) * 8 * 1_000_000_000 / float64(transport) + endToEndBitrate := float64(5) * 8 * 1_000_000_000 / float64(endToEnd) + + summaries := []Summary{ + { + MessageType: protocol.MessageTypeText, + MessageID: 7, + From: "peer-a", + To: "peer-b", + BodySize: 5, + AProcessingLatencyNS: &aProcessing, + AQueueLatencyNS: &aQueue, + ABTransportPropagationNS: &transport, + BProcessingLatencyNS: &bProcessing, + EndToEndLatencyNS: &endToEnd, + AProcessingBitrateBPS: &aProcessingBitrate, + ABTransportPropagationBitrateBPS: &transportBitrate, + EndToEndBitrateBPS: &endToEndBitrate, + ApproxRTTNS: &endToEnd, + }, + { + MessageType: protocol.MessageTypeFile, + MessageID: 8, + From: "peer-b", + To: "peer-a", + FileName: "payload.bin", + BodySize: 128, + MissingTimestamps: []string{EventBRXSoftware}, + }, + } + + path := filepath.Join(t.TempDir(), "charts", "latency-summary.html") + if err := WriteSummariesHTMLChart(path, summaries); err != nil { + t.Fatalf("WriteSummariesHTMLChart() error = %v", err) + } + + data, err := os.ReadFile(path) + if err != nil { + t.Fatalf("os.ReadFile() error = %v", err) + } + + content := string(data) + for _, want := range []string{ + "Latency Summary", + "text #7", + "peer-a -> peer-b | 5 bytes", + "End-to-end: 100.000 ms", + "Approx RTT: 100.000 ms", + "A processing bitrate 0.002 Mb/s", + "A-B transport + propagation bitrate 0.001 Mb/s", + "End-to-end bitrate 0.000 Mb/s", + "A processing 20.000 ms", + "A-B transport + propagation 40.000 ms", + "file #8 (payload.bin)", + "Missing timestamps: B_RX_SOFTWARE", + } { + if !strings.Contains(content, want) { + t.Fatalf("chart content missing %q\n%s", want, content) + } + } +} diff --git a/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/latencylog/summary_test.go b/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/latencylog/summary_test.go new file mode 100644 index 0000000..f1bb8da --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/latencylog/summary_test.go @@ -0,0 +1,399 @@ +package latencylog + +import ( + "bufio" + "encoding/json" + "os" + "path/filepath" + "reflect" + "sort" + "testing" + + "omnisocketgo/cmd/internal/protocol" +) + +func TestSummarizeEventsComputesLatencyMetrics(t *testing.T) { + events := []Event{ + {TsUnixNano: 100, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 120, Event: EventATXSched, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 140, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 180, Event: EventBRXSoftware, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 220, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 230, Event: EventBPersistBegin, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 260, Event: EventBPersistEnd, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + } + + summaries := SummarizeEvents(events) + if len(summaries) != 1 { + t.Fatalf("summary count = %d, want 1", len(summaries)) + } + + summary := summaries[0] + if got := ptrValue(summary.AProcessingLatencyNS); got != 20 { + t.Fatalf("AProcessingLatencyNS = %d, want 20", got) + } + if got := ptrValue(summary.AQueueLatencyNS); got != 20 { + t.Fatalf("AQueueLatencyNS = %d, want 20", got) + } + if got := ptrValue(summary.ABTransportPropagationNS); got != 80 { + t.Fatalf("ABTransportPropagationNS = %d, want 80", got) + } + if got := ptrValue(summary.BKernelReceivePathLatencyNS); got != 40 { + t.Fatalf("BKernelReceivePathLatencyNS = %d, want 40", got) + } + if got := ptrValue(summary.BProcessingLatencyNS); got != 40 { + t.Fatalf("BProcessingLatencyNS = %d, want 40", got) + } + if got := ptrValue(summary.EndToEndLatencyNS); got != 160 { + t.Fatalf("EndToEndLatencyNS = %d, want 160", got) + } + if got := ptrValueFloat(summary.AProcessingBitrateBPS); got != 128_000_000_000 { + t.Fatalf("AProcessingBitrateBPS = %v, want 128000000000", got) + } + if got := ptrValueFloat(summary.ABTransportPropagationBitrateBPS); got != 32_000_000_000 { + t.Fatalf("ABTransportPropagationBitrateBPS = %v, want 32000000000", got) + } + if got := ptrValueFloat(summary.EndToEndBitrateBPS); got != 16_000_000_000 { + t.Fatalf("EndToEndBitrateBPS = %v, want 16000000000", got) + } + if got := summary.Timestamps[EventBRXSoftware]; got != 180 { + t.Fatalf("timestamps[%q] = %d, want 180", EventBRXSoftware, got) + } + if len(summary.MissingTimestamps) != 0 { + t.Fatalf("MissingTimestamps = %v, want empty", summary.MissingTimestamps) + } +} + +func TestSummarizeEventsComputesApproxRTTByPairingReverseMessages(t *testing.T) { + events := []Event{ + {TsUnixNano: 100, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b"}, + {TsUnixNano: 110, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b"}, + {TsUnixNano: 180, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b"}, + {TsUnixNano: 120, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b"}, + {TsUnixNano: 140, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b"}, + {TsUnixNano: 190, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b"}, + {TsUnixNano: 200, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 11, From: "peer-b", To: "peer-a"}, + {TsUnixNano: 210, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 11, From: "peer-b", To: "peer-a"}, + {TsUnixNano: 260, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 11, From: "peer-b", To: "peer-a"}, + {TsUnixNano: 220, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 12, From: "peer-b", To: "peer-a"}, + {TsUnixNano: 230, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 12, From: "peer-b", To: "peer-a"}, + {TsUnixNano: 310, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 12, From: "peer-b", To: "peer-a"}, + } + + summaries := SummarizeEvents(events) + if len(summaries) != 4 { + t.Fatalf("summary count = %d, want 4", len(summaries)) + } + + gotByMessageID := make(map[uint64]Summary, len(summaries)) + for _, summary := range summaries { + gotByMessageID[summary.MessageID] = summary + } + + if got := ptrValue(gotByMessageID[1].ApproxRTTNS); got != 150 { + t.Fatalf("message 1 ApproxRTTNS = %d, want 150", got) + } + if got := ptrValue(gotByMessageID[2].ApproxRTTNS); got != 170 { + t.Fatalf("message 2 ApproxRTTNS = %d, want 170", got) + } + if gotByMessageID[11].ApproxRTTNS != nil { + t.Fatalf("message 11 ApproxRTTNS = %d, want nil", ptrValue(gotByMessageID[11].ApproxRTTNS)) + } + if gotByMessageID[12].ApproxRTTNS != nil { + t.Fatalf("message 12 ApproxRTTNS = %d, want nil", ptrValue(gotByMessageID[12].ApproxRTTNS)) + } +} + +func TestSummarizeEventsReportsMissingTimestamps(t *testing.T) { + events := []Event{ + {TsUnixNano: 100, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b"}, + {TsUnixNano: 240, Event: EventBPersistEnd, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b"}, + } + + summaries := SummarizeEvents(events) + if len(summaries) != 1 { + t.Fatalf("summary count = %d, want 1", len(summaries)) + } + + wantMissing := []string{EventATXSched, EventATXSoftware, EventBRXSoftware, EventBAppRecv} + if !reflect.DeepEqual(summaries[0].MissingTimestamps, wantMissing) { + t.Fatalf("MissingTimestamps = %v, want %v", summaries[0].MissingTimestamps, wantMissing) + } + if summaries[0].AProcessingLatencyNS != nil { + t.Fatalf("AProcessingLatencyNS = %v, want nil", ptrValue(summaries[0].AProcessingLatencyNS)) + } + if summaries[0].EndToEndLatencyNS == nil { + t.Fatal("EndToEndLatencyNS = nil, want non-nil because endpoints are present") + } +} + +func TestLoadAndWriteSummaryFiles(t *testing.T) { + rawPath := filepath.Join(t.TempDir(), "raw.jsonl") + rawLogger, err := NewJSONLLogger(rawPath) + if err != nil { + t.Fatalf("NewJSONLLogger() error = %v", err) + } + t.Cleanup(func() { + _ = rawLogger.Close() + }) + + for _, event := range []Event{ + {TsUnixNano: 100, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 120, Event: EventATXSched, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 140, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 180, Event: EventBRXSoftware, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 220, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 260, Event: EventBPersistEnd, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 320}, + } { + if err := rawLogger.LogEvent(event); err != nil { + t.Fatalf("LogEvent() error = %v", err) + } + } + + events, err := LoadEventsFromFile(rawPath) + if err != nil { + t.Fatalf("LoadEventsFromFile() error = %v", err) + } + + summaryPath := filepath.Join(t.TempDir(), "summary.jsonl") + if err := WriteSummariesJSONL(summaryPath, SummarizeEvents(events)); err != nil { + t.Fatalf("WriteSummariesJSONL() error = %v", err) + } + + file, err := os.Open(summaryPath) + if err != nil { + t.Fatalf("os.Open() error = %v", err) + } + defer file.Close() + + scanner := bufio.NewScanner(file) + if !scanner.Scan() { + t.Fatal("expected one summary line, got none") + } + + var summary Summary + if err := json.Unmarshal(scanner.Bytes(), &summary); err != nil { + t.Fatalf("json.Unmarshal() error = %v", err) + } + if summary.MessageID != 3 { + t.Fatalf("MessageID = %d, want 3", summary.MessageID) + } + if got := ptrValue(summary.BKernelReceivePathLatencyNS); got != 40 { + t.Fatalf("BKernelReceivePathLatencyNS = %d, want 40", got) + } + if got := ptrValue(summary.EndToEndLatencyNS); got != 160 { + t.Fatalf("EndToEndLatencyNS = %d, want 160", got) + } + if got := ptrValueFloat(summary.EndToEndBitrateBPS); got != 16_000_000_000 { + t.Fatalf("EndToEndBitrateBPS = %v, want 16000000000", got) + } +} + +func TestLoadEventsFromFilesWithSharedMaxOffsetFiltersToSharedCutoff(t *testing.T) { + t.Parallel() + + testCases := []struct { + name string + firstMessageIDs []uint64 + secondMessageIDs []uint64 + offset uint64 + wantCutoff *uint64 + wantMessageIDs []uint64 + }{ + { + name: "same max message id rolls back one", + firstMessageIDs: []uint64{1, 2, 3, 4, 5, 6, 7}, + secondMessageIDs: []uint64{1, 2, 3, 4, 5, 6, 7}, + offset: 1, + wantCutoff: uint64Ptr(6), + wantMessageIDs: []uint64{1, 2, 3, 4, 5, 6}, + }, + { + name: "smaller input max wins before rollback", + firstMessageIDs: []uint64{1, 2, 3, 4, 5, 6, 7, 8, 9}, + secondMessageIDs: []uint64{1, 2, 3, 4, 5, 6, 7}, + offset: 1, + wantCutoff: uint64Ptr(6), + wantMessageIDs: []uint64{1, 2, 3, 4, 5, 6}, + }, + { + name: "not enough shared messages yields empty result", + firstMessageIDs: []uint64{1}, + secondMessageIDs: []uint64{1}, + offset: 1, + wantCutoff: uint64Ptr(0), + wantMessageIDs: nil, + }, + } + + for _, tt := range testCases { + tt := tt + t.Run(tt.name, func(t *testing.T) { + t.Parallel() + + tempDir := t.TempDir() + firstPath := filepath.Join(tempDir, "first.jsonl") + secondPath := filepath.Join(tempDir, "second.jsonl") + writeEventsJSONL(t, firstPath, testEventsForMessageIDs(tt.firstMessageIDs, "peer-a", "peer-b")) + writeEventsJSONL(t, secondPath, testEventsForMessageIDs(tt.secondMessageIDs, "peer-b", "peer-a")) + + events, cutoff, err := LoadEventsFromFilesWithSharedMaxOffset([]string{firstPath, secondPath}, tt.offset) + if err != nil { + t.Fatalf("LoadEventsFromFilesWithSharedMaxOffset() error = %v", err) + } + if !reflect.DeepEqual(cutoff, tt.wantCutoff) { + t.Fatalf("cutoff = %v, want %v", cutoff, tt.wantCutoff) + } + + if got := businessMessageIDs(events); !reflect.DeepEqual(got, tt.wantMessageIDs) { + t.Fatalf("message IDs = %v, want %v", got, tt.wantMessageIDs) + } + }) + } +} + +func TestLoadEventsFromFilesWithSharedMaxOffsetPreservesEarlierSummaries(t *testing.T) { + tempDir := t.TempDir() + firstPath := filepath.Join(tempDir, "first.jsonl") + secondPath := filepath.Join(tempDir, "second.jsonl") + + writeEventsJSONL(t, firstPath, []Event{ + {TsUnixNano: 100, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 120, Event: EventATXSched, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 140, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 180, Event: EventBRXSoftware, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 220, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 260, Event: EventBPersistEnd, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-a", To: "peer-b", BodySize: 320}, + {TsUnixNano: 300, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b", BodySize: 160}, + {TsUnixNano: 330, Event: EventATXSched, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b", BodySize: 160}, + {TsUnixNano: 360, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b", BodySize: 160}, + {TsUnixNano: 390, Event: EventBRXSoftware, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b", BodySize: 160}, + {TsUnixNano: 420, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b", BodySize: 160}, + {TsUnixNano: 470, Event: EventBPersistEnd, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-a", To: "peer-b", BodySize: 160}, + {TsUnixNano: 500, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 80}, + {TsUnixNano: 520, Event: EventATXSched, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 80}, + {TsUnixNano: 540, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 80}, + {TsUnixNano: 560, Event: EventBRXSoftware, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 80}, + {TsUnixNano: 580, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 80}, + {TsUnixNano: 600, Event: EventBPersistEnd, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-a", To: "peer-b", BodySize: 80}, + {TsUnixNano: 700, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 4, From: "peer-a", To: "peer-b", BodySize: 40}, + }) + writeEventsJSONL(t, secondPath, []Event{ + {TsUnixNano: 90, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 95, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 150, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 1, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 290, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 295, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 350, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 2, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 490, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 495, Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 550, Event: EventBAppRecv, MessageType: protocol.MessageTypeText, MessageID: 3, From: "peer-b", To: "peer-a", BodySize: 20}, + {TsUnixNano: 690, Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: 4, From: "peer-b", To: "peer-a", BodySize: 20}, + }) + + events, cutoff, err := LoadEventsFromFilesWithSharedMaxOffset([]string{firstPath, secondPath}, 1) + if err != nil { + t.Fatalf("LoadEventsFromFilesWithSharedMaxOffset() error = %v", err) + } + if !reflect.DeepEqual(cutoff, uint64Ptr(3)) { + t.Fatalf("cutoff = %v, want %v", cutoff, uint64Ptr(3)) + } + + summaries := SummarizeEvents(events) + if got := len(summaries); got != 6 { + t.Fatalf("summary count = %d, want 6", got) + } + + for _, summary := range summaries { + if summary.MessageID == 4 { + t.Fatalf("message 4 should have been truncated from summaries: %+v", summary) + } + } + + var forwardMessageTwo Summary + found := false + for _, summary := range summaries { + if summary.From == "peer-a" && summary.To == "peer-b" && summary.MessageID == 2 { + forwardMessageTwo = summary + found = true + break + } + } + if !found { + t.Fatal("summary for message 2 peer-a -> peer-b not found") + } + if got := ptrValue(forwardMessageTwo.EndToEndLatencyNS); got != 170 { + t.Fatalf("message 2 EndToEndLatencyNS = %d, want 170", got) + } + if got := ptrValue(forwardMessageTwo.ApproxRTTNS); got != 190 { + t.Fatalf("message 2 ApproxRTTNS = %d, want 190", got) + } +} + +func ptrValue(value *int64) int64 { + if value == nil { + return 0 + } + return *value +} + +func ptrValueFloat(value *float64) float64 { + if value == nil { + return 0 + } + return *value +} + +func uint64Ptr(value uint64) *uint64 { + return &value +} + +func businessMessageIDs(events []Event) []uint64 { + seen := make(map[uint64]struct{}) + var ids []uint64 + + for _, event := range events { + if !IsBusinessEvent(event) { + continue + } + if _, ok := seen[event.MessageID]; ok { + continue + } + seen[event.MessageID] = struct{}{} + ids = append(ids, event.MessageID) + } + + sort.Slice(ids, func(i, j int) bool { + return ids[i] < ids[j] + }) + return ids +} + +func testEventsForMessageIDs(messageIDs []uint64, from, to string) []Event { + events := make([]Event, 0, len(messageIDs)*2) + for _, messageID := range messageIDs { + events = append(events, + Event{TsUnixNano: int64(messageID*100 + 10), Event: EventAAppPrepBegin, MessageType: protocol.MessageTypeText, MessageID: messageID, From: from, To: to, BodySize: 32}, + Event{TsUnixNano: int64(messageID*100 + 20), Event: EventATXSoftware, MessageType: protocol.MessageTypeText, MessageID: messageID, From: from, To: to, BodySize: 32}, + ) + } + + return events +} + +func writeEventsJSONL(t *testing.T, path string, events []Event) { + t.Helper() + + file, err := os.OpenFile(path, os.O_CREATE|os.O_WRONLY|os.O_TRUNC, 0o644) + if err != nil { + t.Fatalf("os.OpenFile(%s) error = %v", path, err) + } + defer file.Close() + + encoder := json.NewEncoder(file) + for _, event := range events { + if err := encoder.Encode(event); err != nil { + t.Fatalf("encoder.Encode(%s) error = %v", path, err) + } + } +} diff --git a/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/protocol/codec.go b/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/protocol/codec.go new file mode 100644 index 0000000..fef658b --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/protocol/codec.go @@ -0,0 +1,279 @@ +package protocol + +import ( + "encoding/binary" + "encoding/json" + "errors" + "fmt" + "io" + "unicode/utf8" +) + +// MaxFrameSize 用于限制单个帧的最大长度， +// 避免异常对端通过伪造超大长度值导致接收方无上限分配内存。 +const MaxFrameSize = 8 * 1024 * 1024 // 先临时设置传输的视频帧不超过8MB + +var ( + ErrInvalidFrameLength = errors.New("protocol: invalid frame length") // 表示帧长度非法，例如长度为 0。 + ErrFrameTooLarge = errors.New("protocol: frame too large") // 表示帧长度超过允许的上限。 + ErrInvalidMessageType = errors.New("protocol: invalid message type") // 表示消息类型不是当前协议支持的类型。 + ErrMissingFrom = errors.New("protocol: missing from") // 表示消息缺少发送方标识。 + ErrMissingTo = errors.New("protocol: missing to") // 表示消息缺少接收方标识。 + ErrMissingFileName = errors.New("protocol: missing file name") // 表示 file 消息缺少文件名。 + ErrUnexpectedFileName = errors.New("protocol: unexpected file name") // 表示 text 消息错误地携带了文件名。 + ErrInvalidTextBody = errors.New("protocol: invalid text body") // 表示 text 消息正文不是合法 UTF-8。 + ErrUnexpectedBody = errors.New("protocol: unexpected body") // 表示某些控制消息不允许携带正文。 + ErrInvalidRegisterTarget = errors.New("protocol: invalid register target") // 表示 register 消息没有发往 server。 + ErrInvalidErrorSource = errors.New("protocol: invalid error source") // 表示 error 消息不是由 server 发出。 + ErrInvalidHeaderLength = errors.New("protocol: invalid header length") // 表示 header 长度字段为 0、越界或无法完整切分。 + ErrInvalidHeaderJSON = errors.New("protocol: invalid header json") // 表示 header JSON 无法解析，可能是格式错误或缺少必要字段。 + ErrInvalidContentLength = errors.New("protocol: invalid content length") // 表示头部记录的正文长度与实际正文不一致。 +) + +// 应用层消息：[4字节 frameLength][4字节 headerLen][header JSON（下面自定义的Message头）][body bytes] +// 写了 tag：JSON 字段名是你指定的 type；不写 tag：JSON 字段名默认是 Go 字段名 Type +type messageHeader struct { + Type MessageType `json:"type"` + ID uint64 `json:"id"` + From string `json:"from"` + To string `json:"to"` + FileName string `json:"file_name,omitempty"` + ContentLength int `json:"content_length"` +} + +// EncodeMessage 将逻辑消息编码为帧内字节格式： +// 1. 4 字节大端序 header 长度 +// 2. header JSON +// 3. 原始 body 字节 +func EncodeMessage(msg Message) ([]byte, error) { + if err := validateMessage(msg); err != nil { + return nil, err + } + + header := messageHeader{ + Type: msg.Type, + ID: msg.ID, + From: msg.From, + To: msg.To, + FileName: msg.FileName, + ContentLength: len(msg.Body), + } + + headerPayload, err := json.Marshal(header) + if err != nil { + return nil, fmt.Errorf("protocol: encode header: %w", err) + } + // 创建一个新的字节切片来存储完整的帧内容，避免直接在 headerPayload 上修改导致数据混乱。 + payload := make([]byte, 4+len(headerPayload)+len(msg.Body)) + // 在 payload 前 4 字节写入 header 长度，后续内容依次是 header JSON（第五个字节开始） 和 body。 + binary.BigEndian.PutUint32(payload[:4], uint32(len(headerPayload))) + copy(payload[4:], headerPayload) + copy(payload[4+len(headerPayload):], msg.Body) + + //检查整个帧长度是否合法，避免上层调用者构造的消息过大导致发送失败。 + if len(payload) > MaxFrameSize { + return nil, ErrFrameTooLarge + } + + return payload, nil +} + +// DecodeMessage 将帧内字节格式还原为 Message。 +func DecodeMessage(data []byte) (Message, error) { + if len(data) > MaxFrameSize { + return Message{}, ErrFrameTooLarge + } + if len(data) < 4 { + return Message{}, ErrInvalidHeaderLength + } + + headerLen := int(binary.BigEndian.Uint32(data[:4])) + if headerLen == 0 || headerLen > len(data)-4 { + return Message{}, ErrInvalidHeaderLength + } + + headerPayload := data[4 : 4+headerLen] + body := data[4+headerLen:] + + var header messageHeader + if err := json.Unmarshal(headerPayload, &header); err != nil { + return Message{}, fmt.Errorf("protocol: decode header: %w", errors.Join(ErrInvalidHeaderJSON, err)) + } + + if header.ContentLength < 0 || header.ContentLength != len(body) { + return Message{}, ErrInvalidContentLength + } + + bodyCopy := make([]byte, len(body)) + copy(bodyCopy, body) + + msg := Message{ + Type: header.Type, + ID: header.ID, + From: header.From, + To: header.To, + FileName: header.FileName, + Body: bodyCopy, + } + + if err := validateMessage(msg); err != nil { + return Message{}, err + } + + return msg, nil +} + +// WriteFrame 向流中写入一个带长度前缀的帧。 +// TCP帧格式如下： +// 1. 4 字节大端序长度 +// 2. 后续 payload 内容 +// +// TCP 是字节流协议，没有天然的消息边界。 +// 增加显式长度前缀后，接收方就知道一条完整消息应该读取多少字节， +// 从而解决粘包和拆包问题。 +func WriteFrame(w io.Writer, payload []byte) error { + size := len(payload) + //空帧 + if size == 0 { + return ErrInvalidFrameLength + } + //帧过大 + if size > MaxFrameSize { + return ErrFrameTooLarge + } + + var header [4]byte + binary.BigEndian.PutUint32(header[:], uint32(size)) + + // 先写长度头，接收方才能根据长度一次性读取完整消息体。 + if err := writeFull(w, header[:]); err != nil { + return err + } + + return writeFull(w, payload) +} + +// ReadFrame 从流中读取一个完整的长度前缀帧。 +// 它会先读取固定 4 字节长度头，校验长度是否合法， +// 再使用 io.ReadFull 按长度读取完整消息体， +// 这样即使底层 TCP 发生分段读取，也不会把半条消息暴露给上层。 +func ReadFrame(r io.Reader) ([]byte, error) { + var header [4]byte + if _, err := io.ReadFull(r, header[:]); err != nil { + return nil, err + } + + size := binary.BigEndian.Uint32(header[:]) + // 长度为 0 的帧被认为是非法输入，而不是合法的空消息。 + if size == 0 { + return nil, ErrInvalidFrameLength + } + // 长度超过上限的帧会被拒绝，避免接收方无上限分配内存。 + if size > MaxFrameSize { + return nil, ErrFrameTooLarge + } + + payload := make([]byte, int(size)) + if _, err := io.ReadFull(r, payload); err != nil { + return nil, err + } + + return payload, nil +} + +// WriteMessage 是给上层直接使用的完整发送路径： +// 把一条结构化消息完整编码并发送出去”的总入口。 +// Message -> header+body -> 长度前缀帧 -> io.Writer。 +func WriteMessage(w io.Writer, msg Message) error { + payload, err := EncodeMessage(msg) + if err != nil { + return fmt.Errorf("protocol: encode message: %w", err) + } + + if err := WriteFrame(w, payload); err != nil { + return fmt.Errorf("protocol: write frame: %w", err) + } + + return nil +} + +// ReadMessage 是给上层直接使用的完整接收路径： +// io.Reader -> 长度前缀帧 -> header+body -> Message。 +func ReadMessage(r io.Reader) (Message, error) { + payload, err := ReadFrame(r) + if err != nil { + return Message{}, fmt.Errorf("protocol: read frame: %w", err) + } + + msg, err := DecodeMessage(payload) + if err != nil { + return Message{}, fmt.Errorf("protocol: decode message: %w", err) + } + + return msg, nil +} + +// validateMessage 检查 Message 传输的类型（只接受 text 和 file ）。 +func validateMessage(msg Message) error { + if msg.From == "" { + return ErrMissingFrom + } + if msg.To == "" { + return ErrMissingTo + } + + switch msg.Type { + case MessageTypeText: + if msg.FileName != "" { + return ErrUnexpectedFileName + } + if !utf8.Valid(msg.Body) { + return ErrInvalidTextBody + } + case MessageTypeFile: + if msg.FileName == "" { + return ErrMissingFileName + } + case MessageTypeRegister: + if msg.To != ServerPeerID { + return ErrInvalidRegisterTarget + } + if msg.FileName != "" { + return ErrUnexpectedFileName + } + if len(msg.Body) != 0 { + return ErrUnexpectedBody + } + case MessageTypeError: + if msg.From != ServerPeerID { + return ErrInvalidErrorSource + } + if msg.FileName != "" { + return ErrUnexpectedFileName + } + if !utf8.Valid(msg.Body) { + return ErrInvalidTextBody + } + default: + return ErrInvalidMessageType + } + + return nil +} + +// writeFull 会持续写入，直到所有字节都写完或者底层返回错误。 +// 这样可以避免某些 Writer 发生部分写入时破坏帧格式。 +func writeFull(w io.Writer, data []byte) error { + for len(data) > 0 { + n, err := w.Write(data) + if err != nil { + return err + } + if n == 0 { + return io.ErrShortWrite + } + data = data[n:] + } + + return nil +} diff --git a/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/protocol/codec_test.go b/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/protocol/codec_test.go new file mode 100644 index 0000000..b229b36 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/protocol/codec_test.go @@ -0,0 +1,507 @@ +package protocol + +import ( + "bytes" + "encoding/binary" + "encoding/json" + "errors" + "reflect" + "strings" + "testing" +) + +// TestEncodeDecodeMessageTextASCII 验证 ASCII 文本可以按 text 消息往返编解码。 +func TestEncodeDecodeMessageTextASCII(t *testing.T) { + original := Message{ + Type: MessageTypeText, + ID: 42, + From: "peer-a", + To: "peer-b", + Body: []byte("hello"), + } + + data, err := EncodeMessage(original) + if err != nil { + t.Fatalf("EncodeMessage() error = %v", err) + } + + decoded, err := DecodeMessage(data) + if err != nil { + t.Fatalf("DecodeMessage() error = %v", err) + } + + if !reflect.DeepEqual(decoded, original) { + t.Fatalf("round trip mismatch: got %+v want %+v", decoded, original) + } +} + +// TestEncodeDecodeMessageTextUTF8 验证 text 消息允许合法 UTF-8， +// 从而天然兼容 ASCII 之外的普通文本。 +func TestEncodeDecodeMessageTextUTF8(t *testing.T) { + original := Message{ + Type: MessageTypeText, + ID: 43, + From: "peer-a", + To: "peer-b", + Body: []byte("你好, world"), + } + + data, err := EncodeMessage(original) + if err != nil { + t.Fatalf("EncodeMessage() error = %v", err) + } + + decoded, err := DecodeMessage(data) + if err != nil { + t.Fatalf("DecodeMessage() error = %v", err) + } + + if !reflect.DeepEqual(decoded, original) { + t.Fatalf("round trip mismatch: got %+v want %+v", decoded, original) + } +} + +// TestEncodeDecodeMessageFile 验证 file 消息会保留文件名和原始二进制正文。 +func TestEncodeDecodeMessageFile(t *testing.T) { + original := Message{ + Type: MessageTypeFile, + ID: 44, + From: "peer-a", + To: "peer-b", + FileName: "data.bin", + Body: []byte{0x00, 0xff, 0x10, 0x7f}, + } + + data, err := EncodeMessage(original) + if err != nil { + t.Fatalf("EncodeMessage() error = %v", err) + } + + decoded, err := DecodeMessage(data) + if err != nil { + t.Fatalf("DecodeMessage() error = %v", err) + } + + if !reflect.DeepEqual(decoded, original) { + t.Fatalf("round trip mismatch: got %+v want %+v", decoded, original) + } +} + +// TestEncodeDecodeMessageRegister 验证 register 控制消息也能正常编解码。 +func TestEncodeDecodeMessageRegister(t *testing.T) { + original := Message{ + Type: MessageTypeRegister, + ID: 45, + From: "peer-a", + To: ServerPeerID, + Body: []byte{}, + } + + data, err := EncodeMessage(original) + if err != nil { + t.Fatalf("EncodeMessage() error = %v", err) + } + + decoded, err := DecodeMessage(data) + if err != nil { + t.Fatalf("DecodeMessage() error = %v", err) + } + + if !reflect.DeepEqual(decoded, original) { + t.Fatalf("round trip mismatch: got %+v want %+v", decoded, original) + } +} + +// TestEncodeDecodeMessageError 验证 error 控制消息会保留 UTF-8 错误文本。 +func TestEncodeDecodeMessageError(t *testing.T) { + original := Message{ + Type: MessageTypeError, + ID: 46, + From: ServerPeerID, + To: "peer-a", + Body: []byte("unknown target"), + } + + data, err := EncodeMessage(original) + if err != nil { + t.Fatalf("EncodeMessage() error = %v", err) + } + + decoded, err := DecodeMessage(data) + if err != nil { + t.Fatalf("DecodeMessage() error = %v", err) + } + + if !reflect.DeepEqual(decoded, original) { + t.Fatalf("round trip mismatch: got %+v want %+v", decoded, original) + } +} + +// TestWriteReadFrame 单独验证最底层的长度前缀帧逻辑， +// 不依赖 Message 结构，方便确认 TCP 粘包拆包问题是否被正确处理。 +func TestWriteReadFrame(t *testing.T) { + var buf bytes.Buffer + payload := []byte("header+body") + + if err := WriteFrame(&buf, payload); err != nil { + t.Fatalf("WriteFrame() error = %v", err) + } + + got, err := ReadFrame(&buf) + if err != nil { + t.Fatalf("ReadFrame() error = %v", err) + } + + if !bytes.Equal(got, payload) { + t.Fatalf("payload mismatch: got %q want %q", got, payload) + } +} + +// TestWriteReadMessageAllowsEmptyBody 验证空文本和空文件都可以正常通过协议层， +// 因为外层帧非空的前提下，空正文是合法业务内容。 +func TestWriteReadMessageAllowsEmptyBody(t *testing.T) { + tests := []struct { + name string + message Message + }{ + { + name: "empty text", + message: Message{ + Type: MessageTypeText, + ID: 1, + From: "peer-a", + To: "peer-b", + Body: []byte(""), + }, + }, + { + name: "empty file", + message: Message{ + Type: MessageTypeFile, + ID: 2, + From: "peer-a", + To: "peer-b", + FileName: "empty.txt", + Body: []byte{}, + }, + }, + } + + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + var buf bytes.Buffer + + if err := WriteMessage(&buf, tt.message); err != nil { + t.Fatalf("WriteMessage() error = %v", err) + } + + got, err := ReadMessage(&buf) + if err != nil { + t.Fatalf("ReadMessage() error = %v", err) + } + + if !reflect.DeepEqual(got, tt.message) { + t.Fatalf("round trip mismatch: got %+v want %+v", got, tt.message) + } + }) + } +} + +// TestWriteReadMessageRejectsInvalidMessages 验证协议层会在编码前拦住明显非法的消息。 +func TestWriteReadMessageRejectsInvalidMessages(t *testing.T) { + tests := []struct { + name string + message Message + wantErr error + }{ + { + name: "invalid type", + message: Message{ + Type: MessageType("unknown"), + ID: 1, + From: "peer-a", + To: "peer-b", + }, + wantErr: ErrInvalidMessageType, + }, + { + name: "missing from", + message: Message{ + Type: MessageTypeText, + ID: 2, + To: "peer-b", + }, + wantErr: ErrMissingFrom, + }, + { + name: "missing to", + message: Message{ + Type: MessageTypeText, + ID: 3, + From: "peer-a", + }, + wantErr: ErrMissingTo, + }, + { + name: "text with file name", + message: Message{ + Type: MessageTypeText, + ID: 4, + From: "peer-a", + To: "peer-b", + FileName: "bad.txt", + Body: []byte("hello"), + }, + wantErr: ErrUnexpectedFileName, + }, + { + name: "text with invalid utf8", + message: Message{ + Type: MessageTypeText, + ID: 5, + From: "peer-a", + To: "peer-b", + Body: []byte{0xff, 0xfe}, + }, + wantErr: ErrInvalidTextBody, + }, + { + name: "file without file name", + message: Message{ + Type: MessageTypeFile, + ID: 6, + From: "peer-a", + To: "peer-b", + Body: []byte{0x01}, + }, + wantErr: ErrMissingFileName, + }, + { + name: "register with wrong target", + message: Message{ + Type: MessageTypeRegister, + ID: 7, + From: "peer-a", + To: "peer-b", + }, + wantErr: ErrInvalidRegisterTarget, + }, + { + name: "register with body", + message: Message{ + Type: MessageTypeRegister, + ID: 8, + From: "peer-a", + To: ServerPeerID, + Body: []byte("unexpected"), + }, + wantErr: ErrUnexpectedBody, + }, + { + name: "error with wrong source", + message: Message{ + Type: MessageTypeError, + ID: 9, + From: "peer-a", + To: "peer-b", + Body: []byte("bad"), + }, + wantErr: ErrInvalidErrorSource, + }, + { + name: "error with file name", + message: Message{ + Type: MessageTypeError, + ID: 10, + From: ServerPeerID, + To: "peer-a", + FileName: "bad.txt", + Body: []byte("bad"), + }, + wantErr: ErrUnexpectedFileName, + }, + } + + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + _, err := EncodeMessage(tt.message) + if !errors.Is(err, tt.wantErr) { + t.Fatalf("EncodeMessage() error = %v, want %v", err, tt.wantErr) + } + }) + } +} + +// TestReadFrameRejectsInvalidLength 验证长度为 0 的帧会被当成非法输入， +// 而不是被当成一条合法的空消息。 +func TestReadFrameRejectsInvalidLength(t *testing.T) { + var buf bytes.Buffer + + if err := binary.Write(&buf, binary.BigEndian, uint32(0)); err != nil { + t.Fatalf("binary.Write() error = %v", err) + } + + _, err := ReadFrame(&buf) + if !errors.Is(err, ErrInvalidFrameLength) { + t.Fatalf("ReadFrame() error = %v, want %v", err, ErrInvalidFrameLength) + } +} + +// TestReadFrameRejectsTooLargeFrame 验证超大帧会在分配消息体前被拒绝， +// 从而保证最大长度限制真正生效。 +func TestReadFrameRejectsTooLargeFrame(t *testing.T) { + var buf bytes.Buffer + + if err := binary.Write(&buf, binary.BigEndian, uint32(MaxFrameSize+1)); err != nil { + t.Fatalf("binary.Write() error = %v", err) + } + + _, err := ReadFrame(&buf) + if !errors.Is(err, ErrFrameTooLarge) { + t.Fatalf("ReadFrame() error = %v, want %v", err, ErrFrameTooLarge) + } +} + +// TestWriteFrameRejectsEmptyPayload 验证写入端和读取端的约束保持一致： +// 既然读取端不接受 0 长度帧，写入端也不应该产生这种帧。 +func TestWriteFrameRejectsEmptyPayload(t *testing.T) { + var buf bytes.Buffer + + err := WriteFrame(&buf, nil) + if !errors.Is(err, ErrInvalidFrameLength) { + t.Fatalf("WriteFrame() error = %v, want %v", err, ErrInvalidFrameLength) + } +} + +// TestDecodeMessageRejectsInvalidHeaderLength 验证无法切出完整头部时会被立即拒绝。 +func TestDecodeMessageRejectsInvalidHeaderLength(t *testing.T) { + tests := []struct { + name string + data []byte + }{ + { + name: "too short for header len", + data: []byte{0x00, 0x00, 0x00}, + }, + { + name: "zero header len", + data: []byte{0x00, 0x00, 0x00, 0x00}, + }, + { + name: "header len exceeds payload", + data: []byte{0x00, 0x00, 0x00, 0x10, '{', '}'}, + }, + } + + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + _, err := DecodeMessage(tt.data) + if !errors.Is(err, ErrInvalidHeaderLength) { + t.Fatalf("DecodeMessage() error = %v, want %v", err, ErrInvalidHeaderLength) + } + }) + } +} + +// TestDecodeMessageRejectsInvalidHeaderJSON 验证头部 JSON 非法时能返回明确错误。 +func TestDecodeMessageRejectsInvalidHeaderJSON(t *testing.T) { + data := append([]byte{0x00, 0x00, 0x00, 0x09}, []byte("{invalid}")...) + + _, err := DecodeMessage(data) + if !errors.Is(err, ErrInvalidHeaderJSON) { + t.Fatalf("DecodeMessage() error = %v, want %v", err, ErrInvalidHeaderJSON) + } +} + +// TestDecodeMessageRejectsContentLengthMismatch 验证头部声明长度和实际正文不一致时会失败。 +func TestDecodeMessageRejectsContentLengthMismatch(t *testing.T) { + headerPayload, err := json.Marshal(messageHeader{ + Type: MessageTypeText, + ID: 7, + From: "peer-a", + To: "peer-b", + ContentLength: 10, + }) + if err != nil { + t.Fatalf("json.Marshal() error = %v", err) + } + + var data bytes.Buffer + if err := binary.Write(&data, binary.BigEndian, uint32(len(headerPayload))); err != nil { + t.Fatalf("binary.Write() error = %v", err) + } + if _, err := data.Write(headerPayload); err != nil { + t.Fatalf("data.Write(headerPayload) error = %v", err) + } + if _, err := data.Write([]byte("hello")); err != nil { + t.Fatalf("data.Write(body) error = %v", err) + } + + _, err = DecodeMessage(data.Bytes()) + if !errors.Is(err, ErrInvalidContentLength) { + t.Fatalf("DecodeMessage() error = %v, want %v", err, ErrInvalidContentLength) + } +} + +// TestReadMultipleMessages 模拟同一条流中连续写入 text 和 file， +// 验证读取端每次都能严格停在当前帧边界，不会串包。 +func TestReadMultipleMessages(t *testing.T) { + var buf bytes.Buffer + + first := Message{ + Type: MessageTypeText, + ID: 1, + From: "peer-a", + To: "peer-b", + Body: []byte("hello"), + } + + second := Message{ + Type: MessageTypeFile, + ID: 2, + From: "peer-b", + To: "peer-a", + FileName: "payload.bin", + Body: []byte{0x01, 0x02, 0x03}, + } + + if err := WriteMessage(&buf, first); err != nil { + t.Fatalf("WriteMessage(first) error = %v", err) + } + if err := WriteMessage(&buf, second); err != nil { + t.Fatalf("WriteMessage(second) error = %v", err) + } + + gotFirst, err := ReadMessage(&buf) + if err != nil { + t.Fatalf("ReadMessage(first) error = %v", err) + } + gotSecond, err := ReadMessage(&buf) + if err != nil { + t.Fatalf("ReadMessage(second) error = %v", err) + } + + if !reflect.DeepEqual(gotFirst, first) { + t.Fatalf("first message mismatch: got %+v want %+v", gotFirst, first) + } + if !reflect.DeepEqual(gotSecond, second) { + t.Fatalf("second message mismatch: got %+v want %+v", gotSecond, second) + } +} + +// TestReadMessageWrapsDecodeError 验证 ReadMessage 在返回错误时会保留解码阶段上下文。 +func TestReadMessageWrapsDecodeError(t *testing.T) { + var buf bytes.Buffer + + if err := WriteFrame(&buf, append([]byte{0x00, 0x00, 0x00, 0x09}, []byte("{invalid}")...)); err != nil { + t.Fatalf("WriteFrame() error = %v", err) + } + + _, err := ReadMessage(&buf) + if err == nil { + t.Fatal("ReadMessage() error = nil, want non-nil") + } + if !strings.Contains(err.Error(), "decode message") { + t.Fatalf("ReadMessage() error = %v, want wrapped decode error", err) + } +} diff --git a/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/protocol/message.go b/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/protocol/message.go new file mode 100644 index 0000000..5f5d28b --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/go/cmd/internal/protocol/message.go @@ -0,0 +1,33 @@ +package protocol + +// MessageType 表示一条消息的传输类型。 +// v1 只区分普通文本和文件两类负载。 +type MessageType string + +const ( + // MessageTypeText 表示正文按 UTF-8 文本解释，天然兼容 ASCII。 + MessageTypeText MessageType = "text" + // MessageTypeFile 表示正文是原始文件字节。 + MessageTypeFile MessageType = "file" + // MessageTypeRegister 表示 peer 向 server 显式注册自己的身份。 + MessageTypeRegister MessageType = "register" + // MessageTypeError 表示 server 向 peer 返回错误信息。 + MessageTypeError MessageType = "error" +) + +// ServerPeerID 是协议中约定的 server 端固定标识。 +const ServerPeerID = "server" + +// Message 是 peer 和 server 共用的传输消息结构。 +// 头部元信息会被编码为 JSON，Body 则作为原始字节拼接在头部之后。 +type Message struct { + Type MessageType `json:"type"` // 消息类型，只允许 text 或 file。 + ID uint64 `json:"id"` // 由发送方生成，用于追踪消息。 + From string `json:"from"` // 发送方标识。 + To string `json:"to"` // 接收方标识。 + + // FileName 仅在 Type 为 file 时使用。 + FileName string `json:"file_name,omitempty"` + // Body 是真正传输的正文内容，不进入头部 JSON。 + Body []byte `json:"-"` +} diff --git a/robot/v4l2/OmniSocketGo_robot/go/cmd/latencysummary/main.go b/robot/v4l2/OmniSocketGo_robot/go/cmd/latencysummary/main.go new file mode 100644 index 0000000..1e5eac4 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/go/cmd/latencysummary/main.go @@ -0,0 +1,67 @@ +package main + +import ( + "flag" + "log" + "path/filepath" + "strings" + + "omnisocketgo/cmd/internal/latencylog" +) + +type stringListFlag []string + +func (f *stringListFlag) String() string { + return "" +} + +func (f *stringListFlag) Set(value string) error { + *f = append(*f, value) + return nil +} + +func main() { + var inputPaths stringListFlag + outputPath := flag.String("output", "latency-summary.jsonl", "output JSONL file for summarized latency metrics") + // shared-max-offset 是一个可选参数，用于在对齐输入文件的 per-file max message_id 后，排除掉最新的共享 message_id 以外的记录。它指定了要排除的共享 message_id 的数量。 + sharedMaxOffset := flag.Uint64("shared-max-offset", 1, "number of newest shared message IDs to exclude after aligning inputs by per-file max message_id") + flag.Var(&inputPaths, "input", "raw latency JSONL file path; can be provided multiple times") + flag.Parse() + + if len(inputPaths) == 0 { + log.Fatal("at least one -input raw latency log file is required") + } + + events, sharedMaxMessageID, err := latencylog.LoadEventsFromFilesWithSharedMaxOffset(inputPaths, *sharedMaxOffset) + if err != nil { + log.Fatalf("load raw latency logs: %v", err) + } + // sharedMaxMessageID 可能为 nil，表示没有可用的共享 message_id 截止值（例如因为输入文件中没有共享消息）。在这种情况下，我们将继续处理所有事件，但会记录一个警告。 + if sharedMaxMessageID != nil { + log.Printf("using shared message_id cutoff <= %d (shared-max-offset=%d)", *sharedMaxMessageID, *sharedMaxOffset) + } else { + log.Printf("no shared message_id cutoff available after applying shared-max-offset=%d", *sharedMaxOffset) + } + + summaries := latencylog.SummarizeEvents(events) + if err := latencylog.WriteSummariesJSONL(*outputPath, summaries); err != nil { + log.Fatalf("write latency summary: %v", err) + } + + chartPath := replaceFileExt(*outputPath, ".html") + if err := latencylog.WriteSummariesHTMLChart(chartPath, summaries); err != nil { + log.Fatalf("write latency chart: %v", err) + } + + log.Printf("wrote %d summarized message records to %s", len(summaries), *outputPath) + log.Printf("wrote simple latency chart to %s", chartPath) +} + +func replaceFileExt(path, ext string) string { + currentExt := filepath.Ext(path) + if currentExt == "" { + return path + ext + } + + return strings.TrimSuffix(path, currentExt) + ext +} diff --git a/robot/v4l2/OmniSocketGo_robot/go/go.mod b/robot/v4l2/OmniSocketGo_robot/go/go.mod new file mode 100644 index 0000000..8a2d2c0 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/go/go.mod @@ -0,0 +1,16 @@ +module omnisocketgo + +go 1.24.0 + +require github.com/xtaci/kcp-go/v5 v5.6.70 + +require ( + github.com/klauspost/cpuid/v2 v2.2.6 // indirect + github.com/klauspost/reedsolomon v1.12.0 // indirect + github.com/pkg/errors v0.9.1 // indirect + github.com/tjfoc/gmsm v1.4.1 // indirect + golang.org/x/crypto v0.45.0 // indirect + golang.org/x/net v0.47.0 // indirect + golang.org/x/sys v0.38.0 // indirect + golang.org/x/time v0.14.0 // indirect +) diff --git a/robot/v4l2/OmniSocketGo_robot/go/go.sum b/robot/v4l2/OmniSocketGo_robot/go/go.sum new file mode 100644 index 0000000..1876ec0 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/go/go.sum @@ -0,0 +1,98 @@ +cloud.google.com/go v0.26.0/go.mod h1:aQUYkXzVsufM+DwF1aE+0xfcU+56JwCaLick0ClmMTw= +github.com/BurntSushi/toml v0.3.1/go.mod h1:xHWCNGjB5oqiDr8zfno3MHue2Ht5sIBksp03qcyfWMU= +github.com/census-instrumentation/opencensus-proto v0.2.1/go.mod h1:f6KPmirojxKA12rnyqOA5BBL4O983OfeGPqjHWSTneU= +github.com/client9/misspell v0.3.4/go.mod h1:qj6jICC3Q7zFZvVWo7KLAzC3yx5G7kyvSDkc90ppPyw= +github.com/cncf/udpa/go v0.0.0-20191209042840-269d4d468f6f/go.mod h1:M8M6+tZqaGXZJjfX53e64911xZQV5JYwmTeXPW+k8Sc= +github.com/davecgh/go-spew v1.1.0 h1:ZDRjVQ15GmhC3fiQ8ni8+OwkZQO4DARzQgrnXU1Liz8= +github.com/davecgh/go-spew v1.1.0/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38= +github.com/envoyproxy/go-control-plane v0.9.0/go.mod h1:YTl/9mNaCwkRvm6d1a2C3ymFceY/DCBVvsKhRF0iEA4= +github.com/envoyproxy/go-control-plane v0.9.4/go.mod h1:6rpuAdCZL397s3pYoYcLgu1mIlRU8Am5FuJP05cCM98= +github.com/envoyproxy/protoc-gen-validate v0.1.0/go.mod h1:iSmxcyjqTsJpI2R4NaDN7+kN2VEUnK/pcBlmesArF7c= +github.com/golang/glog v0.0.0-20160126235308-23def4e6c14b/go.mod h1:SBH7ygxi8pfUlaOkMMuAQtPIUF8ecWP5IEl/CR7VP2Q= +github.com/golang/mock v1.1.1/go.mod h1:oTYuIxOrZwtPieC+H1uAHpcLFnEyAGVDL/k47Jfbm0A= +github.com/golang/protobuf v1.2.0/go.mod h1:6lQm79b+lXiMfvg/cZm0SGofjICqVBUtrP5yJMmIC1U= +github.com/golang/protobuf v1.3.2/go.mod h1:6lQm79b+lXiMfvg/cZm0SGofjICqVBUtrP5yJMmIC1U= +github.com/golang/protobuf v1.3.3/go.mod h1:vzj43D7+SQXF/4pzW/hwtAqwc6iTitCiVSaWz5lYuqw= +github.com/golang/protobuf v1.4.0-rc.1/go.mod h1:ceaxUfeHdC40wWswd/P6IGgMaK3YpKi5j83Wpe3EHw8= +github.com/golang/protobuf v1.4.0-rc.1.0.20200221234624-67d41d38c208/go.mod h1:xKAWHe0F5eneWXFV3EuXVDTCmh+JuBKY0li0aMyXATA= +github.com/golang/protobuf v1.4.0-rc.2/go.mod h1:LlEzMj4AhA7rCAGe4KMBDvJI+AwstrUpVNzEA03Pprs= +github.com/golang/protobuf v1.4.0-rc.4.0.20200313231945-b860323f09d0/go.mod h1:WU3c8KckQ9AFe+yFwt9sWVRKCVIyN9cPHBJSNnbL67w= +github.com/golang/protobuf v1.4.0/go.mod h1:jodUvKwWbYaEsadDk5Fwe5c77LiNKVO9IDvqG2KuDX0= +github.com/golang/protobuf v1.4.2/go.mod h1:oDoupMAO8OvCJWAcko0GGGIgR6R6ocIYbsSw735rRwI= +github.com/google/go-cmp v0.2.0/go.mod h1:oXzfMopK8JAjlY9xF4vHSVASa0yLyX7SntLO5aqRK0M= +github.com/google/go-cmp v0.3.0/go.mod h1:8QqcDgzrUqlUb/G2PQTWiueGozuR1884gddMywk6iLU= +github.com/google/go-cmp v0.3.1/go.mod h1:8QqcDgzrUqlUb/G2PQTWiueGozuR1884gddMywk6iLU= +github.com/google/go-cmp v0.4.0/go.mod h1:v8dTdLbMG2kIc/vJvl+f65V22dbkXbowE6jgT/gNBxE= +github.com/klauspost/cpuid/v2 v2.2.6 h1:ndNyv040zDGIDh8thGkXYjnFtiN02M1PVVF+JE/48xc= +github.com/klauspost/cpuid/v2 v2.2.6/go.mod h1:Lcz8mBdAVJIBVzewtcLocK12l3Y+JytZYpaMropDUws= +github.com/klauspost/reedsolomon v1.12.0 h1:I5FEp3xSwVCcEh3F5A7dofEfhXdF/bWhQWPH+XwBFno= +github.com/klauspost/reedsolomon v1.12.0/go.mod h1:EPLZJeh4l27pUGC3aXOjheaoh1I9yut7xTURiW3LQ9Y= +github.com/pkg/errors v0.9.1 h1:FEBLx1zS214owpjy7qsBeixbURkuhQAwrK5UwLGTwt4= +github.com/pkg/errors v0.9.1/go.mod h1:bwawxfHBFNV+L2hUp1rHADufV3IMtnDRdf1r5NINEl0= +github.com/pmezard/go-difflib v1.0.0 h1:4DBwDE0NGyQoBHbLQYPwSUPoCMWR5BEzIk/f1lZbAQM= +github.com/pmezard/go-difflib v1.0.0/go.mod h1:iKH77koFhYxTK1pcRnkKkqfTogsbg7gZNVY4sRDYZ/4= +github.com/prometheus/client_model v0.0.0-20190812154241-14fe0d1b01d4/go.mod h1:xMI15A0UPsDsEKsMN9yxemIoYk6Tm2C1GtYGdfGttqA= +github.com/stretchr/testify v1.6.1 h1:hDPOHmpOpP40lSULcqw7IrRb/u7w6RpDC9399XyoNd0= +github.com/stretchr/testify v1.6.1/go.mod h1:6Fq8oRcR53rry900zMqJjRRixrwX3KX962/h/Wwjteg= +github.com/tjfoc/gmsm v1.4.1 h1:aMe1GlZb+0bLjn+cKTPEvvn9oUEBlJitaZiiBwsbgho= +github.com/tjfoc/gmsm v1.4.1/go.mod h1:j4INPkHWMrhJb38G+J6W4Tw0AbuN8Thu3PbdVYhVcTE= +github.com/xtaci/kcp-go/v5 v5.6.70 h1:AYX0QZl6PqmNj2IdYGZGuBfZuDUkUfl+eHYNijCqaO0= +github.com/xtaci/kcp-go/v5 v5.6.70/go.mod h1:9O3D8WR+cyyUjGiTILYfg17vn72otWuXK2AFfqIe6CM= +github.com/xtaci/lossyconn v0.0.0-20190602105132-8df528c0c9ae h1:J0GxkO96kL4WF+AIT3M4mfUVinOCPgf2uUWYFUzN0sM= +github.com/xtaci/lossyconn v0.0.0-20190602105132-8df528c0c9ae/go.mod h1:gXtu8J62kEgmN++bm9BVICuT/e8yiLI2KFobd/TRFsE= +golang.org/x/crypto v0.0.0-20190308221718-c2843e01d9a2/go.mod h1:djNgcEr1/C05ACkg1iLfiJU5Ep61QUkGW8qpdssI0+w= +golang.org/x/crypto v0.0.0-20200622213623-75b288015ac9/go.mod h1:LzIPMQfyMNhhGPhUkYOs5KpL4U8rLKemX1yGLhDgUto= +golang.org/x/crypto v0.0.0-20201012173705-84dcc777aaee/go.mod h1:LzIPMQfyMNhhGPhUkYOs5KpL4U8rLKemX1yGLhDgUto= +golang.org/x/crypto v0.45.0 h1:jMBrvKuj23MTlT0bQEOBcAE0mjg8mK9RXFhRH6nyF3Q= +golang.org/x/crypto v0.45.0/go.mod h1:XTGrrkGJve7CYK7J8PEww4aY7gM3qMCElcJQ8n8JdX4= +golang.org/x/exp v0.0.0-20190121172915-509febef88a4/go.mod h1:CJ0aWSM057203Lf6IL+f9T1iT9GByDxfZKAQTCR3kQA= +golang.org/x/lint v0.0.0-20181026193005-c67002cb31c3/go.mod h1:UVdnD1Gm6xHRNCYTkRU2/jEulfH38KcIWyp/GAMgvoE= +golang.org/x/lint v0.0.0-20190227174305-5b3e6a55c961/go.mod h1:wehouNa3lNwaWXcvxsM5YxQ5yQlVC4a0KAMCusXpPoU= +golang.org/x/lint v0.0.0-20190313153728-d0100b6bd8b3/go.mod h1:6SW0HCj/g11FgYtHlgUYUwCkIfeOF89ocIRzGO/8vkc= +golang.org/x/net v0.0.0-20180724234803-3673e40ba225/go.mod h1:mL1N/T3taQHkDXs73rZJwtUhF3w3ftmwwsq0BUmARs4= +golang.org/x/net v0.0.0-20180826012351-8a410e7b638d/go.mod h1:mL1N/T3taQHkDXs73rZJwtUhF3w3ftmwwsq0BUmARs4= +golang.org/x/net v0.0.0-20190213061140-3a22650c66bd/go.mod h1:mL1N/T3taQHkDXs73rZJwtUhF3w3ftmwwsq0BUmARs4= +golang.org/x/net v0.0.0-20190311183353-d8887717615a/go.mod h1:t9HGtf8HONx5eT2rtn7q6eTqICYqUVnKs3thJo3Qplg= +golang.org/x/net v0.0.0-20190404232315-eb5bcb51f2a3/go.mod h1:t9HGtf8HONx5eT2rtn7q6eTqICYqUVnKs3thJo3Qplg= +golang.org/x/net v0.0.0-20201010224723-4f7140c49acb/go.mod h1:sp8m0HH+o8qH0wwXwYZr8TS3Oi6o0r6Gce1SSxlDquU= +golang.org/x/net v0.47.0 h1:Mx+4dIFzqraBXUugkia1OOvlD6LemFo1ALMHjrXDOhY= +golang.org/x/net v0.47.0/go.mod h1:/jNxtkgq5yWUGYkaZGqo27cfGZ1c5Nen03aYrrKpVRU= +golang.org/x/oauth2 v0.0.0-20180821212333-d2e6202438be/go.mod h1:N/0e6XlmueqKjAGxoOufVs8QHGRruUQn6yWY3a++T0U= +golang.org/x/sync v0.0.0-20180314180146-1d60e4601c6f/go.mod h1:RxMgew5VJxzue5/jJTE5uejpjVlOe/izrB70Jof72aM= +golang.org/x/sync v0.0.0-20181108010431-42b317875d0f/go.mod h1:RxMgew5VJxzue5/jJTE5uejpjVlOe/izrB70Jof72aM= +golang.org/x/sync v0.0.0-20190423024810-112230192c58/go.mod h1:RxMgew5VJxzue5/jJTE5uejpjVlOe/izrB70Jof72aM= +golang.org/x/sys v0.0.0-20180830151530-49385e6e1522/go.mod h1:STP8DvDyc/dI5b8T5hshtkjS+E42TnysNCUPdjciGhY= +golang.org/x/sys v0.0.0-20190215142949-d0b11bdaac8a/go.mod h1:STP8DvDyc/dI5b8T5hshtkjS+E42TnysNCUPdjciGhY= +golang.org/x/sys v0.0.0-20190412213103-97732733099d/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs= +golang.org/x/sys v0.0.0-20200930185726-fdedc70b468f/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs= +golang.org/x/sys v0.5.0/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= +golang.org/x/sys v0.38.0 h1:3yZWxaJjBmCWXqhN1qh02AkOnCQ1poK6oF+a7xWL6Gc= +golang.org/x/sys v0.38.0/go.mod h1:OgkHotnGiDImocRcuBABYBEXf8A9a87e/uXjp9XT3ks= +golang.org/x/text v0.3.0/go.mod h1:NqM8EUOU14njkJ3fqMW+pc6Ldnwhi/IjpwHt7yyuwOQ= +golang.org/x/text v0.3.3/go.mod h1:5Zoc/QRtKVWzQhOtBMvqHzDpF6irO9z98xDceosuGiQ= +golang.org/x/time v0.14.0 h1:MRx4UaLrDotUKUdCIqzPC48t1Y9hANFKIRpNx+Te8PI= +golang.org/x/time v0.14.0/go.mod h1:eL/Oa2bBBK0TkX57Fyni+NgnyQQN4LitPmob2Hjnqw4= +golang.org/x/tools v0.0.0-20180917221912-90fa682c2a6e/go.mod h1:n7NCudcB/nEzxVGmLbDWY5pfWTLqBcC2KZ6jyYvM4mQ= +golang.org/x/tools v0.0.0-20190114222345-bf090417da8b/go.mod h1:n7NCudcB/nEzxVGmLbDWY5pfWTLqBcC2KZ6jyYvM4mQ= +golang.org/x/tools v0.0.0-20190226205152-f727befe758c/go.mod h1:9Yl7xja0Znq3iFh3HoIrodX9oNMXvdceNzlUR8zjMvY= +golang.org/x/tools v0.0.0-20190311212946-11955173bddd/go.mod h1:LCzVGOaR6xXOjkQ3onu1FJEFr0SW1gC7cKk1uF8kGRs= +golang.org/x/tools v0.0.0-20190524140312-2c0ae7006135/go.mod h1:RgjU9mgBXZiqYHBnxXauZ1Gv1EHHAz9KjViQ78xBX0Q= +golang.org/x/xerrors v0.0.0-20191204190536-9bdfabe68543/go.mod h1:I/5z698sn9Ka8TeJc9MKroUUfqBBauWjQqLJ2OPfmY0= +google.golang.org/appengine v1.1.0/go.mod h1:EbEs0AVv82hx2wNQdGPgUI5lhzA/G0D9YwlJXL52JkM= +google.golang.org/appengine v1.4.0/go.mod h1:xpcJRLb0r/rnEns0DIKYYv+WjYCduHsrkT7/EB5XEv4= +google.golang.org/genproto v0.0.0-20180817151627-c66870c02cf8/go.mod h1:JiN7NxoALGmiZfu7CAH4rXhgtRTLTxftemlI0sWmxmc= +google.golang.org/genproto v0.0.0-20190819201941-24fa4b261c55/go.mod h1:DMBHOl98Agz4BDEuKkezgsaosCRResVns1a3J2ZsMNc= +google.golang.org/grpc v1.19.0/go.mod h1:mqu4LbDTu4XGKhr4mRzUsmM4RtVoemTSY81AxZiDr8c= +google.golang.org/grpc v1.23.0/go.mod h1:Y5yQAOtifL1yxbo5wqy6BxZv8vAUGQwXBOALyacEbxg= +google.golang.org/grpc v1.25.1/go.mod h1:c3i+UQWmh7LiEpx4sFZnkU36qjEYZ0imhYfXVyQciAY= +google.golang.org/grpc v1.31.0/go.mod h1:N36X2cJ7JwdamYAgDz+s+rVMFjt3numwzf/HckM8pak= +google.golang.org/protobuf v0.0.0-20200109180630-ec00e32a8dfd/go.mod h1:DFci5gLYBciE7Vtevhsrf46CRTquxDuWsQurQQe4oz8= +google.golang.org/protobuf v0.0.0-20200221191635-4d8936d0db64/go.mod h1:kwYJMbMJ01Woi6D6+Kah6886xMZcty6N08ah7+eCXa0= +google.golang.org/protobuf v0.0.0-20200228230310-ab0ca4ff8a60/go.mod h1:cfTl7dwQJ+fmap5saPgwCLgHXTUD7jkjRqWcaiX5VyM= +google.golang.org/protobuf v1.20.1-0.20200309200217-e05f789c0967/go.mod h1:A+miEFZTKqfCUM6K7xSMQL9OKL/b6hQv+e19PK+JZNE= +google.golang.org/protobuf v1.21.0/go.mod h1:47Nbq4nVaFHyn7ilMalzfO3qCViNmqZ2kzikPIcrTAo= +google.golang.org/protobuf v1.23.0/go.mod h1:EGpADcykh3NcUnDUJcl1+ZksZNG86OlYog2l/sGQquU= +gopkg.in/yaml.v3 v3.0.1 h1:fxVm/GzAzEWqLHuvctI91KS9hhNmmWOoWu0XTYJS7CA= +gopkg.in/yaml.v3 v3.0.1/go.mod h1:K4uyk7z7BCEPqu6E+C64Yfv1cQ7kz7rIZviUmN+EgEM= +honnef.co/go/tools v0.0.0-20190102054323-c2f93a96b099/go.mod h1:rf3lG4BRIbNafJWhAfAdb/ePZxsR/4RtNHQocxwk9r4= +honnef.co/go/tools v0.0.0-20190523083050-ea95bdfd59fc/go.mod h1:rf3lG4BRIbNafJWhAfAdb/ePZxsR/4RtNHQocxwk9r4= diff --git a/robot/v4l2/OmniSocketGo_robot/include/cli_parse.h b/robot/v4l2/OmniSocketGo_robot/include/cli_parse.h new file mode 100644 index 0000000..11bbf78 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/include/cli_parse.h @@ -0,0 +1,57 @@ +#ifndef OMNI_CLI_PARSE_H +#define OMNI_CLI_PARSE_H + +#include "omni_common.h" + +static int cli_parse_bool_text(const char *raw, int *out_value) { + if (raw == NULL || out_value == NULL) { + errno = EINVAL; + return -1; + } + if (strcmp(raw, "1") == 0 || strcmp(raw, "true") == 0 || strcmp(raw, "yes") == 0 || strcmp(raw, "on") == 0) { + *out_value = 1; + return 0; + } + if (strcmp(raw, "0") == 0 || strcmp(raw, "false") == 0 || strcmp(raw, "no") == 0 || strcmp(raw, "off") == 0) { + *out_value = 0; + return 0; + } + errno = EINVAL; + return -1; +} + +static int cli_parse_value_flag(int argc, char **argv, int *index, const char *arg, const char *flag, const char **out_value) { + size_t flag_len = strlen(flag); + + if (strcmp(arg, flag) == 0) { + if (*index + 1 >= argc) { + errno = EINVAL; + return -1; + } + *out_value = argv[++(*index)]; + return 1; + } + if (strncmp(arg, flag, flag_len) == 0 && arg[flag_len] == '=') { + *out_value = arg + flag_len + 1; + return 1; + } + return 0; +} + +static int cli_parse_bool_flag(const char *arg, const char *flag, int *out_value) { + size_t flag_len = strlen(flag); + + if (strcmp(arg, flag) == 0) { + *out_value = 1; + return 1; + } + if (strncmp(arg, flag, flag_len) == 0 && arg[flag_len] == '=') { + if (cli_parse_bool_text(arg + flag_len + 1, out_value) != 0) { + return -1; + } + return 1; + } + return 0; +} + +#endif diff --git a/robot/v4l2/OmniSocketGo_robot/include/control_protocol.h b/robot/v4l2/OmniSocketGo_robot/include/control_protocol.h new file mode 100644 index 0000000..c589f1e --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/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 diff --git a/robot/v4l2/OmniSocketGo_robot/include/gps_buffer.h b/robot/v4l2/OmniSocketGo_robot/include/gps_buffer.h new file mode 100644 index 0000000..f38db88 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/include/gps_buffer.h @@ -0,0 +1,16 @@ +#ifndef GPS_BUFFER_H +#define GPS_BUFFER_H + +#include + +typedef struct gps_video_sample { + double latitude; + double longitude; +} gps_video_sample_t; + +gps_video_sample_t get_latest_gps_for_video(void); + + +int gps_buffer_init(const char* host); +void gps_buffer_cleanup(void); +#endif diff --git a/robot/v4l2/OmniSocketGo_robot/include/interactive.h b/robot/v4l2/OmniSocketGo_robot/include/interactive.h new file mode 100644 index 0000000..775f456 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/include/interactive.h @@ -0,0 +1,30 @@ +#ifndef OMNI_INTERACTIVE_H +#define OMNI_INTERACTIVE_H + +#include "omni_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef enum interactive_command_type { + INTERACTIVE_CMD_HELP = 0, + INTERACTIVE_CMD_QUIT = 1, + INTERACTIVE_CMD_TEXT = 2, + INTERACTIVE_CMD_FILE = 3 +} interactive_command_type_t; + +typedef struct interactive_command { + interactive_command_type_t type; + char to[OMNI_MAX_PEER_ID]; + char value[1024]; +} interactive_command_t; + +int interactive_parse_command(const char *line, interactive_command_t *command, char *err, size_t err_len); +void interactive_print_help(FILE *out, const char *transport_name); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/v4l2/OmniSocketGo_robot/include/kcp_packet_debug.h b/robot/v4l2/OmniSocketGo_robot/include/kcp_packet_debug.h new file mode 100644 index 0000000..45632ee --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/include/kcp_packet_debug.h @@ -0,0 +1,49 @@ +#ifndef OMNI_KCP_PACKET_DEBUG_H +#define OMNI_KCP_PACKET_DEBUG_H + +#include "omni_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct kcp_packet_debug_segment { + uint8_t cmd; + uint32_t sn; + uint32_t una; + uint8_t frg; + uint16_t wnd; + uint32_t len; +} kcp_packet_debug_segment_t; + +typedef struct kcp_packet_debug_record { + char event[OMNI_MAX_EVENT_NAME]; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + char local_addr[OMNI_MAX_ADDR_TEXT]; + char remote_addr[OMNI_MAX_ADDR_TEXT]; + int packet_bytes; + int has_udp_tx_id; + uint32_t udp_tx_id; + int has_kcp_conv; + uint32_t kcp_conv; + int64_t ts_unix_nano; + kcp_packet_debug_segment_t *segments; + size_t segment_count; +} kcp_packet_debug_record_t; + +typedef struct kcp_packet_debug_logger { + omni_file_logger_t file_logger; + int enabled; +} kcp_packet_debug_logger_t; + +kcp_packet_debug_logger_t *kcp_packet_debug_open_jsonl(const char *path); +void kcp_packet_debug_close(kcp_packet_debug_logger_t *logger); +int kcp_packet_debug_log(kcp_packet_debug_logger_t *logger, const kcp_packet_debug_record_t *record); +void kcp_packet_debug_record_clear(kcp_packet_debug_record_t *record); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/v4l2/OmniSocketGo_robot/include/kcp_session_stats.h b/robot/v4l2/OmniSocketGo_robot/include/kcp_session_stats.h new file mode 100644 index 0000000..166f237 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/include/kcp_session_stats.h @@ -0,0 +1,92 @@ +#ifndef OMNI_KCP_SESSION_STATS_H +#define OMNI_KCP_SESSION_STATS_H + +#include "omni_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define KCP_SESSION_STATS_RECORD_SESSION_SAMPLE "session_sample" +#define KCP_SESSION_STATS_RECORD_PROCESS_SAMPLE "process_snmp_sample" + +typedef struct kcp_session_stats_record { + char record_type[32]; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + char local_addr[OMNI_MAX_ADDR_TEXT]; + char remote_addr[OMNI_MAX_ADDR_TEXT]; + int has_conv; + uint32_t conv; + int64_t ts_unix_nano; + char sample_reason[32]; + int has_rto_ms; + uint32_t rto_ms; + int has_srtt_ms; + int32_t srtt_ms; + int has_min_srtt_ms; + int32_t min_srtt_ms; + int has_srttvar_ms; + int32_t srttvar_ms; + int has_last_feedback_age_ms; + uint32_t last_feedback_age_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; + uint64_t bytes_received; + int has_in_pkts; + uint64_t in_pkts; + int has_out_pkts; + uint64_t out_pkts; + int has_in_segs; + uint64_t in_segs; + int has_out_segs; + uint64_t out_segs; + int has_retrans_segs; + uint64_t retrans_segs; + int has_fast_retrans_segs; + uint64_t fast_retrans_segs; + int has_early_retrans_segs; + uint64_t early_retrans_segs; + int has_lost_segs; + uint64_t lost_segs; + int has_repeat_segs; + uint64_t repeat_segs; + int has_in_errs; + uint64_t in_errs; + int has_kcp_in_errs; + uint64_t kcp_in_errs; + int has_ring_buffer_snd_queue; + uint64_t ring_buffer_snd_queue; + int has_ring_buffer_rcv_queue; + uint64_t ring_buffer_rcv_queue; + int has_ring_buffer_snd_buffer; + uint64_t ring_buffer_snd_buffer; + int has_curr_estab; + uint64_t curr_estab; +} kcp_session_stats_record_t; + +typedef struct kcp_session_stats_logger { + omni_file_logger_t file_logger; + int enabled; +} kcp_session_stats_logger_t; + +kcp_session_stats_logger_t *kcp_session_stats_open_jsonl(const char *path); +void kcp_session_stats_close(kcp_session_stats_logger_t *logger); +int kcp_session_stats_log(kcp_session_stats_logger_t *logger, const kcp_session_stats_record_t *record); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/v4l2/OmniSocketGo_robot/include/latencylog.h b/robot/v4l2/OmniSocketGo_robot/include/latencylog.h new file mode 100644 index 0000000..809f515 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/include/latencylog.h @@ -0,0 +1,51 @@ +#ifndef OMNI_LATENCYLOG_H +#define OMNI_LATENCYLOG_H + +#include "protocol.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define EVENT_A_APP_PREP_BEGIN "A_APP_PREP_BEGIN" +#define EVENT_A_TX_SCHED "A_TX_SCHED" +#define EVENT_A_TX_SOFTWARE "A_TX_SOFTWARE" +#define EVENT_A_TX_HARDWARE "A_TX_HARDWARE" +#define EVENT_B_RX_HARDWARE "B_RX_HARDWARE" +#define EVENT_B_RX_SOFTWARE "B_RX_SOFTWARE" +#define EVENT_B_APP_RECV "B_APP_RECV" +#define EVENT_B_PERSIST_BEGIN "B_PERSIST_BEGIN" +#define EVENT_B_PERSIST_END "B_PERSIST_END" +#define EVENT_SEND_HANDOFF_BEGIN "send_handoff_begin" +#define EVENT_SEND_HANDOFF_END "send_handoff_end" + +typedef struct latency_event { + int64_t ts_unix_nano; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + char event[OMNI_MAX_EVENT_NAME]; + message_type_t message_type; + uint64_t message_id; + char from[OMNI_MAX_PEER_ID]; + char to[OMNI_MAX_PEER_ID]; + char file_name[OMNI_MAX_FILE_NAME]; + int body_size; +} latency_event_t; + +typedef struct latency_logger { + omni_file_logger_t file_logger; + int enabled; +} latency_logger_t; + +latency_logger_t *latencylog_open_jsonl(const char *path); +void latencylog_close(latency_logger_t *logger); +int latencylog_log_event(latency_logger_t *logger, const latency_event_t *event); +int latencylog_is_business_message(const message_t *msg); +void latencylog_log_message_event(latency_logger_t *logger, const char *node_role, const char *node_id, const char *event_name, const message_t *msg); +void latencylog_log_message_event_at(latency_logger_t *logger, const char *node_role, const char *node_id, const char *event_name, int64_t ts_unix_nano, const message_t *msg); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/v4l2/OmniSocketGo_robot/include/linux_timestamping.h b/robot/v4l2/OmniSocketGo_robot/include/linux_timestamping.h new file mode 100644 index 0000000..0cd2572 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/include/linux_timestamping.h @@ -0,0 +1,25 @@ +#ifndef OMNI_LINUX_TIMESTAMPING_H +#define OMNI_LINUX_TIMESTAMPING_H + +#include "omni_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct omni_tx_timestamp_event { + char event_name[OMNI_MAX_EVENT_NAME]; + int64_t ts_unix_nano; + uint32_t ee_info; + uint32_t ee_data; +} omni_tx_timestamp_event_t; + +int linux_timestamping_enable_udp_socket(int fd, int enable_rx); +int64_t linux_timestamping_parse_rx_timestamp(const struct msghdr *msg); +int linux_timestamping_parse_tx_timestamp(const struct msghdr *msg, omni_tx_timestamp_event_t *out_event); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/v4l2/OmniSocketGo_robot/include/omni_common.h b/robot/v4l2/OmniSocketGo_robot/include/omni_common.h new file mode 100644 index 0000000..09b8432 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/include/omni_common.h @@ -0,0 +1,78 @@ +#ifndef OMNI_COMMON_H +#define OMNI_COMMON_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define OMNI_NODE_ROLE_PEER "peer" +#define OMNI_NODE_ROLE_SERVER "server" + +#define OMNI_MAX_PEER_ID 64 +#define OMNI_MAX_NODE_ROLE 16 +#define OMNI_MAX_EVENT_NAME 64 +#define OMNI_MAX_FILE_NAME 256 +#define OMNI_MAX_ADDR_TEXT 128 +#define OMNI_MAX_FRAME_SIZE (8U * 1024U * 1024U) + +#define OMNI_ARRAY_LEN(x) (sizeof(x) / sizeof((x)[0])) + +typedef struct omni_file_logger { + FILE *file; + pthread_mutex_t mutex; + char path[PATH_MAX]; + size_t current_bytes; + size_t buffered_bytes; + size_t flush_bytes; + size_t max_bytes; + int flush_interval_ms; + int max_files; + int immediate_flush; + uint64_t last_flush_monotonic_ms; +} omni_file_logger_t; + +int64_t omni_now_unix_nano(void); +uint32_t omni_now_millis32(void); + +int omni_set_nonblocking(int fd, int enabled); +int omni_parse_sockaddr(const char *raw, int passive, struct sockaddr_storage *addr, socklen_t *addr_len, int *family_out); +int omni_clone_sockaddr(const struct sockaddr *src, socklen_t src_len, struct sockaddr_storage *dst, socklen_t *dst_len); +const char *omni_sockaddr_to_string(const struct sockaddr *addr, socklen_t addr_len, char *buffer, size_t buffer_len); + +int omni_bind_device(int fd, const char *device); +int omni_ensure_dir(const char *path); +int omni_ensure_parent_dir(const char *path); +int omni_read_file(const char *path, uint8_t **out, size_t *out_len); +int omni_write_full_fd(int fd, const uint8_t *data, size_t len); +int omni_append_file(const char *path, const uint8_t *data, size_t len); +int omni_write_file(const char *path, const uint8_t *data, size_t len); +int omni_random_u32(uint32_t *out); + +char *omni_strdup(const char *src); +char *omni_strdup_printf(const char *fmt, ...); +char *omni_json_escape(const char *src); +char *omni_json_escape_bytes(const uint8_t *src, size_t len); +int omni_utf8_valid(const uint8_t *data, size_t len); +void omni_trim_newline(char *line); +int omni_parse_duration_ms(const char *raw, int default_ms, int *out_ms); +double omni_duration_ms_to_ns(double ms); +const char *omni_path_base_name(const char *path); + +void omni_file_logger_init(omni_file_logger_t *logger, FILE *file); +void omni_file_logger_init_path(omni_file_logger_t *logger, FILE *file, const char *path, int immediate_flush); +void omni_file_logger_destroy(omni_file_logger_t *logger); +int omni_file_logger_write_line(omni_file_logger_t *logger, const char *line); + +#endif diff --git a/robot/v4l2/OmniSocketGo_robot/include/peer_kcp_client.h b/robot/v4l2/OmniSocketGo_robot/include/peer_kcp_client.h new file mode 100644 index 0000000..426ec3d --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/include/peer_kcp_client.h @@ -0,0 +1,46 @@ +#ifndef OMNI_PEER_KCP_CLIENT_H +#define OMNI_PEER_KCP_CLIENT_H + +#include "transport_kcp.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct kcp_client kcp_client_t; +typedef struct kcp_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; +} kcp_client_recv_meta_t; +typedef struct kcp_client_state { + int connected; + int registered; + uint32_t server_idle_ms; + char last_server_error[256]; +} kcp_client_state_t; + +kcp_client_t *kcp_client_dial_with_options(const char *server_addr, const char *dial_addr, const char *peer_id, const char *bind_ip, const char *bind_device, const kcp_conn_options_t *options, latency_logger_t *logger, kcp_packet_debug_logger_t *packet_logger, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms); +kcp_client_t *kcp_client_dial(const char *server_addr, const char *dial_addr, const char *peer_id, const char *bind_ip, const char *bind_device, latency_logger_t *logger, kcp_packet_debug_logger_t *packet_logger, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms); +const char *kcp_client_id(const kcp_client_t *client); +int kcp_client_send_text(kcp_client_t *client, const char *to, const char *text); +int kcp_client_send_binary(kcp_client_t *client, const char *to, const void *data, size_t data_len); +int kcp_client_send_binary_with_id(kcp_client_t *client, const char *to, const void *data, size_t data_len, uint64_t *out_id); +int kcp_client_send_file_path(kcp_client_t *client, const char *to, const char *path); +int kcp_client_receive_timed(kcp_client_t *client, message_t *out_msg, int timeout_ms); +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); +void kcp_client_state_snapshot(kcp_client_t *client, kcp_client_state_t *out_state); +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); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/v4l2/OmniSocketGo_robot/include/peer_udp_client.h b/robot/v4l2/OmniSocketGo_robot/include/peer_udp_client.h new file mode 100644 index 0000000..937e49e --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/include/peer_udp_client.h @@ -0,0 +1,37 @@ +#ifndef OMNI_PEER_UDP_CLIENT_H +#define OMNI_PEER_UDP_CLIENT_H + +#include "transport_udp.h" + +#ifdef __cplusplus +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); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/v4l2/OmniSocketGo_robot/include/protocol.h b/robot/v4l2/OmniSocketGo_robot/include/protocol.h new file mode 100644 index 0000000..a6c64ad --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/include/protocol.h @@ -0,0 +1,62 @@ +#ifndef OMNI_PROTOCOL_H +#define OMNI_PROTOCOL_H + +#include "omni_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef enum message_type { + MSG_TYPE_TEXT = 0, + MSG_TYPE_FILE = 1, + MSG_TYPE_REGISTER = 2, + MSG_TYPE_ERROR = 3, + MSG_TYPE_BINARY = 4, + MSG_TYPE_INVALID = 255 +} message_type_t; + +#define SERVER_PEER_ID "server" + +typedef struct message { + 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]; + uint8_t *body; + size_t body_len; +} message_t; + +typedef struct protocol_frame_decoder { + uint8_t *buffer; + size_t len; + size_t cap; +} protocol_frame_decoder_t; + +const char *protocol_message_type_name(message_type_t type); +int protocol_message_type_from_name(const char *raw, message_type_t *out); + +void protocol_message_init(message_t *msg); +void protocol_message_clear(message_t *msg); +int protocol_message_copy(message_t *dst, const message_t *src); + +int protocol_validate_message(const message_t *msg, char *err, size_t err_len); + +int protocol_encode_message_datagram(const message_t *msg, uint8_t **out, size_t *out_len); +int protocol_decode_message_datagram(const uint8_t *data, size_t data_len, message_t *out_msg, char *err, size_t err_len); + +int protocol_encode_message_stream(const message_t *msg, uint8_t **out, size_t *out_len); +int protocol_decode_message_stream_payload(const uint8_t *payload, size_t payload_len, message_t *out_msg, char *err, size_t err_len); + +void protocol_frame_decoder_init(protocol_frame_decoder_t *decoder); +void protocol_frame_decoder_reset(protocol_frame_decoder_t *decoder); +void protocol_frame_decoder_destroy(protocol_frame_decoder_t *decoder); +int protocol_frame_decoder_feed(protocol_frame_decoder_t *decoder, const uint8_t *data, size_t data_len); +int protocol_frame_decoder_next(protocol_frame_decoder_t *decoder, uint8_t **payload, size_t *payload_len); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/v4l2/OmniSocketGo_robot/include/server_kcp_hub.h b/robot/v4l2/OmniSocketGo_robot/include/server_kcp_hub.h new file mode 100644 index 0000000..37140df --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/include/server_kcp_hub.h @@ -0,0 +1,27 @@ +#ifndef OMNI_SERVER_KCP_HUB_H +#define OMNI_SERVER_KCP_HUB_H + +#include "transport_kcp.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct kcp_hub kcp_hub_t; + +kcp_hub_t *kcp_hub_new(latency_logger_t *logger, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms); +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); +void kcp_hub_free(kcp_hub_t *hub); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/v4l2/OmniSocketGo_robot/include/server_udp_hub.h b/robot/v4l2/OmniSocketGo_robot/include/server_udp_hub.h new file mode 100644 index 0000000..7baed3c --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/include/server_udp_hub.h @@ -0,0 +1,21 @@ +#ifndef OMNI_SERVER_UDP_HUB_H +#define OMNI_SERVER_UDP_HUB_H + +#include "transport_udp.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct udp_hub udp_hub_t; + +udp_hub_t *udp_hub_open(const char *listen_addr, latency_logger_t *logger, tx_timestamp_debug_logger_t *debug_logger, int enable_timestamping); +int udp_hub_serve(udp_hub_t *hub); +int udp_hub_close(udp_hub_t *hub); +void udp_hub_free(udp_hub_t *hub); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/v4l2/OmniSocketGo_robot/include/server_udp_relay.h b/robot/v4l2/OmniSocketGo_robot/include/server_udp_relay.h new file mode 100644 index 0000000..1c7728e --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/include/server_udp_relay.h @@ -0,0 +1,21 @@ +#ifndef OMNI_SERVER_UDP_RELAY_H +#define OMNI_SERVER_UDP_RELAY_H + +#include "omni_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct udp_relay udp_relay_t; + +udp_relay_t *udp_relay_open(const char *listen_addr, const char *upstream_addr); +int udp_relay_serve(udp_relay_t *relay); +int udp_relay_close(udp_relay_t *relay); +void udp_relay_free(udp_relay_t *relay); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/v4l2/OmniSocketGo_robot/include/transport_kcp.h b/robot/v4l2/OmniSocketGo_robot/include/transport_kcp.h new file mode 100644 index 0000000..f9e1bbc --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/include/transport_kcp.h @@ -0,0 +1,117 @@ +#ifndef OMNI_TRANSPORT_KCP_H +#define OMNI_TRANSPORT_KCP_H + +#include "kcp_packet_debug.h" +#include "kcp_session_stats.h" +#include "latencylog.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define KCP_DEFAULT_NODELAY 1 +#define KCP_DEFAULT_INTERVAL_MS 10 +#define KCP_DEFAULT_RESEND 2 +#define KCP_DEFAULT_NC 1 +#define KCP_DEFAULT_SND_WND 256 +#define KCP_DEFAULT_RCV_WND 256 +#define KCP_DEFAULT_MTU 1400 +#define KCP_DEFAULT_STATS_INTERVAL_MS 100 + +#define KCP_CONTROL_NODELAY 1 +#define KCP_CONTROL_INTERVAL_MS 5 +#define KCP_CONTROL_RESEND 2 +#define KCP_CONTROL_NC 1 +#define KCP_CONTROL_SND_WND 32 +#define KCP_CONTROL_RCV_WND 32 +#define KCP_CONTROL_MTU 1400 + +#define KCP_VIDEO_NODELAY 1 +#define KCP_VIDEO_INTERVAL_MS 10 +#define KCP_VIDEO_RESEND 2 +#define KCP_VIDEO_NC 1 +#define KCP_VIDEO_SND_WND 256 +#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 +#define KCP_NC KCP_DEFAULT_NC +#define KCP_WND_SIZE KCP_DEFAULT_SND_WND +#define KCP_MTU KCP_DEFAULT_MTU + +typedef struct kcp_conn kcp_conn_t; +typedef struct kcp_listener kcp_listener_t; +typedef struct kcp_runtime_stats { + int connected; + uint32_t conv; + uint32_t rto_ms; + int32_t srtt_ms; + int32_t min_srtt_ms; + int32_t srttvar_ms; + uint32_t last_feedback_age_ms; + uint32_t snd_wnd; + uint32_t rmt_wnd; + uint32_t inflight; + uint32_t window_limit; + double window_pressure_pct; + uint32_t snd_queue; + uint32_t rcv_queue; + uint32_t snd_buffer; + uint64_t out_segs_total; + uint64_t retrans_total; + uint64_t fast_retrans_total; + uint64_t lost_total; + uint64_t repeat_total; + uint32_t xmit_total; +} kcp_runtime_stats_t; +typedef struct kcp_conn_options { + int nodelay; + int interval_ms; + int resend; + int nc; + int sndwnd; + int rcvwnd; + int mtu; +} kcp_conn_options_t; + +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); +int kcp_conn_configure_runtime(kcp_conn_t *conn, latency_logger_t *logger, const char *node_role, const char *node_id, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms); +int kcp_conn_apply_options(kcp_conn_t *conn, const kcp_conn_options_t *options); +int kcp_conn_send(kcp_conn_t *conn, const message_t *msg); +int kcp_conn_receive_timed(kcp_conn_t *conn, message_t *out_msg, int timeout_ms); +int kcp_conn_receive(kcp_conn_t *conn, message_t *out_msg); +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_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); +int kcp_listener_close(kcp_listener_t *listener); +void kcp_listener_free(kcp_listener_t *listener); + +int kcp_session_stats_parse_interval_ms(const char *raw, int *out_ms); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/v4l2/OmniSocketGo_robot/include/transport_udp.h b/robot/v4l2/OmniSocketGo_robot/include/transport_udp.h new file mode 100644 index 0000000..54e7155 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/include/transport_udp.h @@ -0,0 +1,30 @@ +#ifndef OMNI_TRANSPORT_UDP_H +#define OMNI_TRANSPORT_UDP_H + +#include "latencylog.h" +#include "linux_timestamping.h" +#include "tx_timestamp_debug.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct udp_conn udp_conn_t; + +udp_conn_t *udp_conn_dial(const char *server_addr, const char *bind_ip, const char *bind_device, int enable_timestamping, latency_logger_t *logger, const char *node_role, const char *node_id, tx_timestamp_debug_logger_t *debug_logger); +udp_conn_t *udp_conn_bind(const char *listen_addr, const char *bind_device, int enable_timestamping, latency_logger_t *logger, const char *node_role, const char *node_id, tx_timestamp_debug_logger_t *debug_logger); + +int udp_conn_send(udp_conn_t *conn, const message_t *msg); +int udp_conn_send_to(udp_conn_t *conn, const message_t *msg, const struct sockaddr *addr, socklen_t addr_len); +int udp_conn_receive(udp_conn_t *conn, message_t *out_msg, struct sockaddr_storage *addr, socklen_t *addr_len); + +int udp_conn_fd(const udp_conn_t *conn); +int udp_conn_local_addr(const udp_conn_t *conn, struct sockaddr_storage *addr, socklen_t *addr_len); +int udp_conn_close(udp_conn_t *conn); +void udp_conn_free(udp_conn_t *conn); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/v4l2/OmniSocketGo_robot/include/tx_timestamp_debug.h b/robot/v4l2/OmniSocketGo_robot/include/tx_timestamp_debug.h new file mode 100644 index 0000000..c5795ca --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/include/tx_timestamp_debug.h @@ -0,0 +1,51 @@ +#ifndef OMNI_TX_TIMESTAMP_DEBUG_H +#define OMNI_TX_TIMESTAMP_DEBUG_H + +#include "protocol.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define TX_TIMESTAMP_DEBUG_RECORD_SEND_CHUNK "send_chunk" +#define TX_TIMESTAMP_DEBUG_RECORD_ERRQUEUE_EVENT "errqueue_event" + +typedef struct tx_timestamp_debug_record { + char record_type[32]; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + message_type_t message_type; + uint64_t message_id; + char from[OMNI_MAX_PEER_ID]; + char to[OMNI_MAX_PEER_ID]; + char file_name[OMNI_MAX_FILE_NAME]; + int body_size; + char phase[32]; + int send_call_index; + int frame_offset_start; + int frame_offset_end; + int bytes_written; + uint32_t expected_tx_id; + int read_index; + char event_name[OMNI_MAX_EVENT_NAME]; + int64_t ts_unix_nano; + uint32_t ee_info; + uint32_t ee_data; + int matched_send_call_index; + int selected_for_latency; +} tx_timestamp_debug_record_t; + +typedef struct tx_timestamp_debug_logger { + omni_file_logger_t file_logger; + int enabled; +} tx_timestamp_debug_logger_t; + +tx_timestamp_debug_logger_t *tx_timestamp_debug_open_jsonl(const char *path); +void tx_timestamp_debug_close(tx_timestamp_debug_logger_t *logger); +int tx_timestamp_debug_log(tx_timestamp_debug_logger_t *logger, const tx_timestamp_debug_record_t *record); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/v4l2/OmniSocketGo_robot/include/video_pipeline.h b/robot/v4l2/OmniSocketGo_robot/include/video_pipeline.h new file mode 100644 index 0000000..b63e9db --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/include/video_pipeline.h @@ -0,0 +1,105 @@ +#ifndef OMNI_VIDEO_PIPELINE_H +#define OMNI_VIDEO_PIPELINE_H + +#include +#include +#include +#include + +#include "gps_buffer.h" +#include "omni_common.h" +#include "peer_kcp_client.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined(__GNUC__) +typedef struct __attribute__((packed)) video_pipeline_packet_metadata { +#else +typedef struct video_pipeline_packet_metadata { +#endif + uint64_t timestamp_ms; + double latitude; + double longitude; + uint32_t capture_to_send_ms; +} video_pipeline_packet_metadata_t; + +typedef struct video_stage_logger { + omni_file_logger_t file_logger; + int enabled; + uint64_t sample_mod; +} video_stage_logger_t; + +typedef void (*video_pipeline_progress_fn)(void *context); + +#if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L +_Static_assert(sizeof(video_pipeline_packet_metadata_t) == 28, "video trailer metadata must be 28 bytes"); +#endif + +typedef struct video_pipeline_config { + const char *camera_device; + const char *camera_head_device; + const char *camera_waist_device; + atomic_int *active_camera; + 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; + int soft_backpressure_segments; + int hard_backpressure_segments; + int hard_backpressure_hold_ms; + int frame_stall_reconnect_ms; + kcp_session_stats_logger_t *stats_logger; + video_stage_logger_t *stage_logger; + int stats_interval_ms; + video_pipeline_progress_fn progress_callback; + void *progress_context; +} video_pipeline_config_t; + +enum { + VIDEO_CAMERA_HEAD = 0, + VIDEO_CAMERA_WAIST = 1 +}; + +typedef struct video_pipeline_stats { + pthread_mutex_t mutex; + uint64_t frames_sent; + uint64_t bytes_sent; + uint64_t send_errors; + uint64_t backpressure_drops; + uint64_t backlog_resets; + uint64_t last_frame_bytes; + uint32_t last_backlog_segments; + uint32_t last_capture_to_send_ms; + double avg_capture_to_send_ms; + int connected; + char last_error[256]; + char last_backlog_reason[128]; + kcp_runtime_stats_t transport; +} video_pipeline_stats_t; + +#define VIDEO_PIPELINE_RUN_RETRY_IMMEDIATE 2 + +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); +video_stage_logger_t *video_stage_logger_open_jsonl(const char *path, uint64_t sample_mod); +void video_stage_logger_close(video_stage_logger_t *logger); +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 diff --git a/robot/v4l2/OmniSocketGo_robot/python/omnisocket/__init__.py b/robot/v4l2/OmniSocketGo_robot/python/omnisocket/__init__.py new file mode 100644 index 0000000..2b23277 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/python/omnisocket/__init__.py @@ -0,0 +1,57 @@ +try: + from ._omnisocket import ( + MSG_TYPE_BINARY, + MSG_TYPE_ERROR, + MSG_TYPE_FILE, + MSG_TYPE_REGISTER, + MSG_TYPE_TEXT, + Session, + UdpSession, + ) +except ImportError as exc: + raise ImportError( + "omnisocket extension is not built; run `make python-ext` on a Linux host first" + ) from exc + +CONTROL_DEFAULTS = { + "nodelay": 1, + "interval_ms": 5, + "resend": 2, + "nc": 1, + "sndwnd": 32, + "rcvwnd": 32, + "mtu": 1400, +} + +VIDEO_DEFAULTS = { + "nodelay": 1, + "interval_ms": 10, + "resend": 2, + "nc": 1, + "sndwnd": 256, + "rcvwnd": 256, + "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", + "MSG_TYPE_FILE", + "MSG_TYPE_REGISTER", + "MSG_TYPE_TEXT", + "Session", + "UdpSession", +] diff --git a/robot/v4l2/OmniSocketGo_robot/python/omnisocket/_omnisocket.c b/robot/v4l2/OmniSocketGo_robot/python/omnisocket/_omnisocket.c new file mode 100644 index 0000000..5c59f70 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/python/omnisocket/_omnisocket.c @@ -0,0 +1,705 @@ +#define PY_SSIZE_T_CLEAN +#include + +#include "omnisocket_client.h" + +typedef struct PyOmniSession { + PyObject_HEAD + 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" +); + +PyDoc_STRVAR( + PyOmniSession_recv_into_doc, + "recv_into(buffer, timeout_ms=-1) -> dict | None\n" + "\n" + "The writable buffer must be large enough for the full message body.\n" + "If it is too small, BufferError reports the required size but the\n" + "current frame has already been consumed and is lost." +); + +static PyObject *build_recv_result(const message_t *msg) { + PyObject *body = NULL; + PyObject *result = NULL; + + 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,s:i,s:s}", + "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, + "registered", + stats->registered, + "last_server_error", + stats->last_server_error + ); +} + +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("min_srtt_ms", PyLong_FromLong(stats->min_srtt_ms)); + SET_KCP_STAT("srttvar_ms", PyLong_FromLong(stats->srttvar_ms)); + SET_KCP_STAT("last_feedback_age_ms", PyLong_FromUnsignedLong(stats->last_feedback_age_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; + (void) args; + (void) kwargs; + + self = (PyOmniSession *) type->tp_alloc(type, 0); + if (self == NULL) { + return NULL; + } + if (omnisocket_session_init(&self->session) != 0) { + type->tp_free((PyObject *) self); + return PyErr_SetFromErrno(PyExc_OSError); + } + return (PyObject *) self; +} + +static void PyOmniSession_dealloc(PyOmniSession *self) { + omnisocket_session_destroy(&self->session); + Py_TYPE(self)->tp_free((PyObject *) self); +} + +static PyObject *PyOmniSession_connect(PyOmniSession *self, PyObject *args, PyObject *kwargs) { + const char *server_addr; + const char *peer_id; + const char *relay_via = ""; + const char *bind_ip = ""; + const char *bind_device = ""; + int nodelay = KCP_DEFAULT_NODELAY; + int interval_ms = KCP_DEFAULT_INTERVAL_MS; + int resend = KCP_DEFAULT_RESEND; + int nc = KCP_DEFAULT_NC; + int sndwnd = KCP_DEFAULT_SND_WND; + int rcvwnd = KCP_DEFAULT_RCV_WND; + int mtu = KCP_DEFAULT_MTU; + int stats_interval_ms = KCP_DEFAULT_STATS_INTERVAL_MS; + kcp_conn_options_t options; + int rc; + + static char *kwlist[] = { + "server_addr", + "peer_id", + "relay_via", + "bind_ip", + "bind_device", + "nodelay", + "interval_ms", + "resend", + "nc", + "sndwnd", + "rcvwnd", + "mtu", + "stats_interval_ms", + NULL + }; + + if (!PyArg_ParseTupleAndKeywords( + args, + kwargs, + "ss|sssiiiiiiii", + kwlist, + &server_addr, + &peer_id, + &relay_via, + &bind_ip, + &bind_device, + &nodelay, + &interval_ms, + &resend, + &nc, + &sndwnd, + &rcvwnd, + &mtu, + &stats_interval_ms)) { + return NULL; + } + + kcp_conn_options_init(&options); + options.nodelay = nodelay; + options.interval_ms = interval_ms; + options.resend = resend; + options.nc = nc; + options.sndwnd = sndwnd; + options.rcvwnd = rcvwnd; + options.mtu = mtu; + + Py_BEGIN_ALLOW_THREADS + rc = omnisocket_session_connect( + &self->session, + server_addr, + relay_via, + peer_id, + bind_ip, + bind_device, + &options, + stats_interval_ms + ); + Py_END_ALLOW_THREADS + + if (rc != 0) { + return PyErr_SetFromErrno(PyExc_OSError); + } + Py_RETURN_NONE; +} + +static PyObject *PyOmniSession_close(PyOmniSession *self, PyObject *Py_UNUSED(ignored)) { + int rc; + + Py_BEGIN_ALLOW_THREADS + rc = omnisocket_session_close(&self->session); + Py_END_ALLOW_THREADS + + if (rc != 0) { + return PyErr_SetFromErrno(PyExc_OSError); + } + Py_RETURN_NONE; +} + +static PyObject *PyOmniSession_send(PyOmniSession *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_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 *PyOmniSession_send_text(PyOmniSession *self, PyObject *args, PyObject *kwargs) { + const char *to; + const char *text; + int rc; + static char *kwlist[] = {"to", "text", NULL}; + + if (!PyArg_ParseTupleAndKeywords(args, kwargs, "ss", kwlist, &to, &text)) { + return NULL; + } + + Py_BEGIN_ALLOW_THREADS + rc = omnisocket_session_send_text(&self->session, to, text); + Py_END_ALLOW_THREADS + + if (rc != 0) { + return PyErr_SetFromErrno(PyExc_OSError); + } + Py_RETURN_NONE; +} + +static PyObject *PyOmniSession_send_with_id(PyOmniSession *self, PyObject *args, PyObject *kwargs) { + const char *to; + Py_buffer payload; + int rc; + uint64_t message_id = 0; + 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_session_send_with_id(&self->session, to, payload.buf, (size_t) payload.len, &message_id); + Py_END_ALLOW_THREADS + + PyBuffer_Release(&payload); + if (rc != 0) { + return PyErr_SetFromErrno(PyExc_OSError); + } + return PyLong_FromUnsignedLongLong((unsigned long long) message_id); +} + +static PyObject *PyOmniSession_recv(PyOmniSession *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_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 *PyOmniSession_recv_into(PyOmniSession *self, PyObject *args, PyObject *kwargs) { + PyObject *buffer_obj; + Py_buffer view; + int timeout_ms = -1; + int rc; + kcp_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_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 *PyOmniSession_stats(PyOmniSession *self, PyObject *Py_UNUSED(ignored)) { + omnisocket_session_stats_t stats; + + memset(&stats, 0, sizeof(stats)); + omnisocket_session_stats_snapshot(&self->session, &stats); + return build_stats_dict(&stats); +} + +static PyObject *PyOmniSession_kcp_stats(PyOmniSession *self, PyObject *Py_UNUSED(ignored)) { + omnisocket_session_kcp_stats_t stats; + + memset(&stats, 0, sizeof(stats)); + omnisocket_session_kcp_stats_snapshot(&self->session, &stats); + return build_kcp_stats_dict(&stats); +} + +static PyMethodDef PyOmniSession_methods[] = { + {"connect", (PyCFunction) PyOmniSession_connect, METH_VARARGS | METH_KEYWORDS, NULL}, + {"close", (PyCFunction) PyOmniSession_close, METH_NOARGS, NULL}, + {"send", (PyCFunction) PyOmniSession_send, METH_VARARGS | METH_KEYWORDS, NULL}, + {"send_text", (PyCFunction) PyOmniSession_send_text, METH_VARARGS | METH_KEYWORDS, NULL}, + {"send_with_id", (PyCFunction) PyOmniSession_send_with_id, METH_VARARGS | METH_KEYWORDS, NULL}, + {"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_stats", (PyCFunction) PyOmniSession_kcp_stats, METH_NOARGS, NULL}, + {NULL, NULL, 0, NULL} +}; + +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", + .m_size = -1, +}; + +PyMODINIT_FUNC PyInit__omnisocket(void) { + PyObject *module; + + PyOmniSessionType.tp_name = "omnisocket.Session"; + PyOmniSessionType.tp_basicsize = sizeof(PyOmniSession); + PyOmniSessionType.tp_flags = Py_TPFLAGS_DEFAULT; + PyOmniSessionType.tp_new = PyOmniSession_new; + PyOmniSessionType.tp_dealloc = (destructor) PyOmniSession_dealloc; + PyOmniSessionType.tp_methods = PyOmniSession_methods; + + if (PyType_Ready(&PyOmniSessionType) < 0) { + 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; + } + + Py_INCREF(&PyOmniSessionType); + if (PyModule_AddObject(module, "Session", (PyObject *) &PyOmniSessionType) != 0) { + Py_DECREF(&PyOmniSessionType); + Py_DECREF(module); + 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 || + PyModule_AddIntConstant(module, "MSG_TYPE_ERROR", MSG_TYPE_ERROR) != 0 || + PyModule_AddIntConstant(module, "MSG_TYPE_BINARY", MSG_TYPE_BINARY) != 0) { + Py_DECREF(module); + return NULL; + } + + return module; +} diff --git a/robot/v4l2/OmniSocketGo_robot/python/omnisocket/omnisocket_client.c b/robot/v4l2/OmniSocketGo_robot/python/omnisocket/omnisocket_client.c new file mode 100644 index 0000000..c68bfe2 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/python/omnisocket/omnisocket_client.c @@ -0,0 +1,603 @@ +#include "omnisocket_client.h" + +static void omnisocket_session_sync_client_state_locked(omnisocket_session_t *session, kcp_client_t *client) { + kcp_client_state_t client_state; + + if (session == NULL) { + return; + } + memset(&client_state, 0, sizeof(client_state)); + if (client != NULL) { + kcp_client_state_snapshot(client, &client_state); + } + session->stats.connected = client_state.connected; + session->stats.registered = client_state.registered; + snprintf( + session->stats.last_server_error, + sizeof(session->stats.last_server_error), + "%s", + client_state.last_server_error + ); +} + +static void omnisocket_session_mark_disconnected_locked(omnisocket_session_t *session) { + if (session == NULL) { + return; + } + session->stats.connected = 0; + session->stats.registered = 0; +} + +int omnisocket_session_init(omnisocket_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_session_destroy(omnisocket_session_t *session) { + if (session == NULL) { + return; + } + (void) omnisocket_session_close(session); + pthread_cond_destroy(&session->idle_cond); + pthread_mutex_destroy(&session->mutex); +} + +static int omnisocket_session_begin_client_op(omnisocket_session_t *session, kcp_client_t **out_client) { + if (session == NULL || out_client == NULL) { + errno = EINVAL; + return -1; + } + + 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_session_connect( + omnisocket_session_t *session, + const char *server_addr, + const char *relay_via, + const char *peer_id, + const char *bind_ip, + const char *bind_device, + const kcp_conn_options_t *options, + int stats_interval_ms +) { + kcp_client_t *client; + + if (session == NULL || server_addr == NULL || peer_id == NULL) { + errno = EINVAL; + return -1; + } + + pthread_mutex_lock(&session->mutex); + while (session->closing) { + pthread_cond_wait(&session->idle_cond, &session->mutex); + } + if (session->client != NULL) { + pthread_mutex_unlock(&session->mutex); + errno = EISCONN; + return -1; + } + client = kcp_client_dial_with_options( + server_addr, + relay_via, + peer_id, + bind_ip, + bind_device, + options, + NULL, + NULL, + NULL, + stats_interval_ms + ); + if (client == NULL) { + pthread_mutex_unlock(&session->mutex); + return -1; + } + session->client = client; + omnisocket_session_sync_client_state_locked(session, client); + pthread_mutex_unlock(&session->mutex); + return 0; +} + +int omnisocket_session_close(omnisocket_session_t *session) { + kcp_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; + } + omnisocket_session_mark_disconnected_locked(session); + pthread_mutex_unlock(&session->mutex); + + if (client != NULL) { + kcp_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); + kcp_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_session_send(omnisocket_session_t *session, const char *to, const void *data, size_t data_len) { + return omnisocket_session_send_with_id(session, to, data, data_len, NULL); +} + +int omnisocket_session_send_text(omnisocket_session_t *session, const char *to, const char *text) { + kcp_client_t *client; + int rc; + + if (session == NULL || to == NULL || text == NULL) { + errno = EINVAL; + return -1; + } + + if (omnisocket_session_begin_client_op(session, &client) != 0) { + return -1; + } + rc = kcp_client_send_text(client, to, text); + pthread_mutex_lock(&session->mutex); + if (rc == 0) { + session->stats.send_calls += 1; + session->stats.send_bytes += (uint64_t) strlen(text); + } else { + session->stats.send_errors += 1; + } + omnisocket_session_sync_client_state_locked(session, client); + 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; +} + +int omnisocket_session_send_with_id( + omnisocket_session_t *session, + const char *to, + const void *data, + size_t data_len, + uint64_t *out_message_id +) { + kcp_client_t *client; + int rc; + + if (session == NULL || to == NULL || (data == NULL && data_len > 0)) { + errno = EINVAL; + return -1; + } + + if (omnisocket_session_begin_client_op(session, &client) != 0) { + return -1; + } + rc = kcp_client_send_binary_with_id(client, to, data, data_len, out_message_id); + 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; + } + omnisocket_session_sync_client_state_locked(session, client); + 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; +} + +int omnisocket_session_recv(omnisocket_session_t *session, message_t *out_msg, int timeout_ms) { + kcp_client_t *client; + int rc; + + if (session == NULL || out_msg == NULL) { + errno = EINVAL; + return -1; + } + + if (omnisocket_session_begin_client_op(session, &client) != 0) { + return -1; + } + rc = kcp_client_receive_timed(client, out_msg, timeout_ms); + pthread_mutex_lock(&session->mutex); + if (rc == 0) { + session->stats.recv_calls += 1; + session->stats.recv_bytes += (uint64_t) out_msg->body_len; + } else if (rc == 1) { + session->stats.recv_timeouts += 1; + } else { + session->stats.recv_errors += 1; + } + omnisocket_session_sync_client_state_locked(session, client); + 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; +} + +int omnisocket_session_recv_into( + omnisocket_session_t *session, + void *buffer, + size_t buffer_len, + kcp_client_recv_meta_t *out_meta, + int timeout_ms +) { + kcp_client_t *client; + int rc; + + if (session == NULL || out_meta == NULL || (buffer == NULL && buffer_len > 0)) { + errno = EINVAL; + return -1; + } + + if (omnisocket_session_begin_client_op(session, &client) != 0) { + return -1; + } + rc = kcp_client_receive_binary_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; + } + omnisocket_session_sync_client_state_locked(session, client); + 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_session_stats_snapshot(omnisocket_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); +} + +void omnisocket_session_kcp_stats_snapshot(omnisocket_session_t *session, omnisocket_session_kcp_stats_t *out_stats) { + kcp_runtime_stats_t runtime_stats; + + 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->min_srtt_ms = runtime_stats.min_srtt_ms; + out_stats->srttvar_ms = runtime_stats.srttvar_ms; + out_stats->last_feedback_age_ms = runtime_stats.last_feedback_age_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; + } + + 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); + while (session->closing) { + pthread_cond_wait(&session->idle_cond, &session->mutex); + } + 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; + session->stats.registered = 1; + session->stats.last_server_error[0] = '\0'; + 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; + session->stats.registered = 0; + pthread_mutex_unlock(&session->mutex); + + if (client != NULL) { + 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; + } + + if (omnisocket_udp_session_begin_client_op(session, &client) != 0) { + 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; + } + if (session->closing && session->active_ops == 0) { + pthread_cond_broadcast(&session->idle_cond); + } + pthread_mutex_unlock(&session->mutex); + return rc; +} + +int omnisocket_udp_session_recv(omnisocket_udp_session_t *session, message_t *out_msg, int timeout_ms) { + udp_client_t *client; + int rc; + + if (session == NULL || out_msg == NULL) { + errno = EINVAL; + return -1; + } + + if (omnisocket_udp_session_begin_client_op(session, &client) != 0) { + return -1; + } + rc = udp_client_receive_timed(client, out_msg, timeout_ms); + pthread_mutex_lock(&session->mutex); + if (rc == 0) { + session->stats.recv_calls += 1; + session->stats.recv_bytes += (uint64_t) out_msg->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; +} + +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 +) { + 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); +} diff --git a/robot/v4l2/OmniSocketGo_robot/python/omnisocket/omnisocket_client.h b/robot/v4l2/OmniSocketGo_robot/python/omnisocket/omnisocket_client.h new file mode 100644 index 0000000..7313b7a --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/python/omnisocket/omnisocket_client.h @@ -0,0 +1,119 @@ +#ifndef OMNISOCKET_PY_CLIENT_H +#define OMNISOCKET_PY_CLIENT_H + +#include "peer_kcp_client.h" +#include "peer_udp_client.h" + +typedef struct omnisocket_session_stats { + uint64_t send_calls; + uint64_t send_bytes; + uint64_t send_errors; + uint64_t recv_calls; + uint64_t recv_bytes; + uint64_t recv_timeouts; + uint64_t recv_errors; + int connected; + int registered; + char last_server_error[256]; +} omnisocket_session_stats_t; + +typedef struct omnisocket_session_kcp_stats { + int connected; + uint32_t conv; + uint32_t rto_ms; + int32_t srtt_ms; + int32_t min_srtt_ms; + int32_t srttvar_ms; + uint32_t last_feedback_age_ms; + uint32_t snd_wnd; + uint32_t rmt_wnd; + uint32_t inflight; + uint32_t window_limit; + double window_pressure_pct; + uint32_t snd_queue; + uint32_t rcv_queue; + uint32_t snd_buffer; + uint64_t out_segs_total; + uint64_t retrans_total; + uint64_t fast_retrans_total; + uint64_t lost_total; + uint64_t repeat_total; + uint32_t xmit_total; +} omnisocket_session_kcp_stats_t; + +typedef struct omnisocket_session { + pthread_mutex_t mutex; + pthread_cond_t idle_cond; + kcp_client_t *client; + size_t active_ops; + int closing; + 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); + +int omnisocket_session_connect( + omnisocket_session_t *session, + const char *server_addr, + const char *relay_via, + const char *peer_id, + const char *bind_ip, + const char *bind_device, + const kcp_conn_options_t *options, + int stats_interval_ms +); +int omnisocket_session_close(omnisocket_session_t *session); +int omnisocket_session_send(omnisocket_session_t *session, const char *to, const void *data, size_t data_len); +int omnisocket_session_send_text(omnisocket_session_t *session, const char *to, const char *text); +int omnisocket_session_send_with_id( + omnisocket_session_t *session, + const char *to, + const void *data, + size_t data_len, + uint64_t *out_message_id +); +int omnisocket_session_recv(omnisocket_session_t *session, message_t *out_msg, int timeout_ms); +int omnisocket_session_recv_into( + omnisocket_session_t *session, + void *buffer, + size_t buffer_len, + kcp_client_recv_meta_t *out_meta, + int timeout_ms +); +void omnisocket_session_stats_snapshot(omnisocket_session_t *session, omnisocket_session_stats_t *out_stats); +void omnisocket_session_kcp_stats_snapshot(omnisocket_session_t *session, omnisocket_session_kcp_stats_t *out_stats); + +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 diff --git a/robot/v4l2/OmniSocketGo_robot/python/setup.py b/robot/v4l2/OmniSocketGo_robot/python/setup.py new file mode 100644 index 0000000..f302f32 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/python/setup.py @@ -0,0 +1,58 @@ +from pathlib import Path +import sys + +from setuptools import Extension, setup + + +ROOT = Path(__file__).resolve().parent.parent +PY_ROOT = Path(__file__).resolve().parent + +if sys.platform != "linux": + raise RuntimeError("omnisocket Python extension can only be built on Linux") + + +COMMON_SOURCES = [ + ROOT / "src" / "omni_common.c", + ROOT / "src" / "protocol.c", + ROOT / "src" / "latencylog.c", + ROOT / "src" / "tx_timestamp_debug.c", + ROOT / "src" / "kcp_packet_debug.c", + ROOT / "src" / "kcp_session_stats.c", + ROOT / "src" / "linux_timestamping.c", + ROOT / "src" / "interactive.c", + ROOT / "src" / "transport_udp.c", + ROOT / "src" / "transport_kcp.c", + ROOT / "src" / "server_udp_relay.c", + ROOT / "src" / "server_udp_hub.c", + ROOT / "src" / "server_kcp_hub.c", + ROOT / "src" / "peer_udp_client.c", + ROOT / "src" / "peer_kcp_client.c", + ROOT / "third_party" / "cjson" / "cJSON.c", + ROOT / "third_party" / "kcp" / "ikcp.c", +] + + +setup( + name="omnisocket", + version="0.1.0", + packages=["omnisocket"], + ext_modules=[ + Extension( + "omnisocket._omnisocket", + sources=[ + str(PY_ROOT / "omnisocket" / "_omnisocket.c"), + str(PY_ROOT / "omnisocket" / "omnisocket_client.c"), + *[str(path) for path in COMMON_SOURCES], + ], + include_dirs=[ + str(ROOT / "include"), + str(ROOT / "third_party" / "cjson"), + str(ROOT / "third_party" / "kcp"), + str(PY_ROOT / "omnisocket"), + ], + define_macros=[("_GNU_SOURCE", None)], + extra_compile_args=["-std=c11", "-O2", "-pthread"], + extra_link_args=["-pthread"], + ) + ], +) diff --git a/robot/v4l2/OmniSocketGo_robot/python/tests/test_sessions.py b/robot/v4l2/OmniSocketGo_robot/python/tests/test_sessions.py new file mode 100644 index 0000000..bc7db39 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/python/tests/test_sessions.py @@ -0,0 +1,325 @@ +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 +MSG_TYPE_TEXT = omnisocket.MSG_TYPE_TEXT +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) + + +@contextmanager +def _run_relay(listen_addr: str, remote_addr: str): + binary = ROOT / 'bin' / 'kcpserver' + if not binary.exists(): + pytest.skip(f'{binary} is not built') + + process = subprocess.Popen( + [str(binary), '-mode', 'relay', '-listen', listen_addr, '-relay-remote', remote_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, relay_via: 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) + if relay_via: + kwargs['relay_via'] = relay_via + 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['registered'] == 1 + assert receiver_stats['registered'] == 1 + assert sender_stats['send_calls'] >= 2 + assert receiver_stats['recv_calls'] >= 2 + if transport == 'kcp': + sender.send_text(to=receiver_id, text='camera:waist') + from_peer, msg_type, recv_payload = receiver.recv(timeout_ms=1000) + assert from_peer == sender_id + assert msg_type == MSG_TYPE_TEXT + assert recv_payload == b'camera:waist' + + 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_kcp_duplicate_peer_new_instance_wins() -> None: + port = _reserve_port() + listen_addr = f'127.0.0.1:{port}' + shared_peer_id = 'pytest-kcp-shared-peer' + sender_id = 'pytest-kcp-unique-sender' + + with _run_server('kcpserver', listen_addr): + original = _connect_with_retry(Session, transport='kcp', server_addr=listen_addr, peer_id=shared_peer_id) + sender = _connect_with_retry(Session, transport='kcp', server_addr=listen_addr, peer_id=sender_id) + replacement = None + + try: + replacement = _connect_with_retry(Session, transport='kcp', server_addr=listen_addr, peer_id=shared_peer_id) + replacement_stats = replacement.stats() + assert replacement_stats['connected'] == 1 + assert replacement_stats['registered'] == 1 + + with pytest.raises(OSError): + original.recv(timeout_ms=1000) + + payload = b'registered-replacement' + sender.send(to=shared_peer_id, data=payload) + from_peer, msg_type, recv_payload = replacement.recv(timeout_ms=1000) + assert from_peer == sender_id + assert msg_type == MSG_TYPE_BINARY + assert recv_payload == payload + finally: + original.close() + sender.close() + if replacement is not None: + replacement.close() + + +def test_kcp_idle_video_peers_survive_without_receive_loop() -> None: + port = _reserve_port() + listen_addr = f'127.0.0.1:{port}' + sender_id = 'peer-b-video' + receiver_id = 'pytest-kcp-video-idle-receiver' + + with _run_server('kcpserver', listen_addr): + sender = _connect_with_retry(Session, transport='kcp', server_addr=listen_addr, peer_id=sender_id) + receiver = _connect_with_retry(Session, transport='kcp', server_addr=listen_addr, peer_id=receiver_id) + + try: + time.sleep(5.0) + + payload = b'idle-video-session-still-alive' + 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 + finally: + sender.close() + receiver.close() + + +def test_kcp_peer_a_video_stale_receiver_is_evicted() -> None: + port = _reserve_port() + listen_addr = f'127.0.0.1:{port}' + receiver_id = 'peer-a-video' + + with _run_server('kcpserver', listen_addr): + receiver = _connect_with_retry(Session, transport='kcp', server_addr=listen_addr, peer_id=receiver_id) + + try: + time.sleep(5.0) + with pytest.raises(OSError): + receiver.recv(timeout_ms=1000) + finally: + receiver.close() + + +def test_kcp_relay_routes_multiple_sessions_by_conv() -> None: + hub_port = _reserve_port() + relay_port = _reserve_port() + hub_addr = f'127.0.0.1:{hub_port}' + relay_addr = f'127.0.0.1:{relay_port}' + + with _run_server('kcpserver', hub_addr): + with _run_relay(relay_addr, hub_addr): + sender = _connect_with_retry(Session, transport='kcp', server_addr=hub_addr, peer_id='pytest-relay-sender', relay_via=relay_addr) + receiver = _connect_with_retry(Session, transport='kcp', server_addr=hub_addr, peer_id='pytest-relay-receiver', relay_via=relay_addr) + chatter = _connect_with_retry(Session, transport='kcp', server_addr=hub_addr, peer_id='pytest-relay-chatter', relay_via=relay_addr) + + try: + chatter.send(to='pytest-relay-sender', data=b'chatter-primes-last-client') + from_peer, msg_type, recv_payload = sender.recv(timeout_ms=1000) + assert from_peer == 'pytest-relay-chatter' + assert msg_type == MSG_TYPE_BINARY + assert recv_payload == b'chatter-primes-last-client' + + sender.send(to='pytest-relay-receiver', data=b'relay-video-frame') + from_peer, msg_type, recv_payload = receiver.recv(timeout_ms=1000) + assert from_peer == 'pytest-relay-sender' + assert msg_type == MSG_TYPE_BINARY + assert recv_payload == b'relay-video-frame' + finally: + sender.close() + receiver.close() + chatter.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) diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-c/Makefile b/robot/v4l2/OmniSocketGo_robot/ros-control-c/Makefile new file mode 100644 index 0000000..26b692e --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/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) diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-c/README.md b/robot/v4l2/OmniSocketGo_robot/ros-control-c/README.md new file mode 100644 index 0000000..42603d0 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/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 ` 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 +``` diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-c/Robot Remote Control via UDP - Implementation Plan.md b/robot/v4l2/OmniSocketGo_robot/ros-control-c/Robot Remote Control via UDP - Implementation Plan.md new file mode 100644 index 0000000..350852b --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/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 , -p , -l , -a + + --- + 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 , -p , -d , -l , -a , -z + + --- + 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 \ No newline at end of file diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-c/common/protocol.h b/robot/v4l2/OmniSocketGo_robot/ros-control-c/common/protocol.h new file mode 100644 index 0000000..91adf5b --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/ros-control-c/common/protocol.h @@ -0,0 +1,26 @@ +#ifndef PROTOCOL_H +#define PROTOCOL_H + +#include + +#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 */ diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-c/common/teleop_transport.c b/robot/v4l2/OmniSocketGo_robot/ros-control-c/common/teleop_transport.c new file mode 100644 index 0000000..c5d875d --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/ros-control-c/common/teleop_transport.c @@ -0,0 +1,300 @@ +#include "teleop_transport.h" + +#include +#include +#include +#include +#include +#include + +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; + } +} diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-c/common/teleop_transport.h b/robot/v4l2/OmniSocketGo_robot/ros-control-c/common/teleop_transport.h new file mode 100644 index 0000000..6061aff --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/ros-control-c/common/teleop_transport.h @@ -0,0 +1,59 @@ +#ifndef TELEOP_TRANSPORT_H +#define TELEOP_TRANSPORT_H + +#include +#include + +#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 */ diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-c/remote/gamepad_controller.c b/robot/v4l2/OmniSocketGo_robot/ros-control-c/remote/gamepad_controller.c new file mode 100644 index 0000000..db96133 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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; +} diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-c/remote/keyboard_controller.c b/robot/v4l2/OmniSocketGo_robot/ros-control-c/remote/keyboard_controller.c new file mode 100644 index 0000000..239dad8 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/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 +#include +#include +#include +#include +#include +#include +#include +#include + +#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; +} diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-c/robot/udp_ros_bridge.py b/robot/v4l2/OmniSocketGo_robot/ros-control-c/robot/udp_ros_bridge.py new file mode 100644 index 0000000..1b435d8 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/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 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() diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-py/README.md b/robot/v4l2/OmniSocketGo_robot/ros-control-py/README.md new file mode 100644 index 0000000..be5c392 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/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 消息、错误长度消息都会被丢弃并记录日志 diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-py/ROS2 Teleop over UDP.md b/robot/v4l2/OmniSocketGo_robot/ros-control-py/ROS2 Teleop over UDP.md new file mode 100644 index 0000000..976d719 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/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 模式有效 diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/config/xbox_twist_joy.yaml b/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/config/xbox_twist_joy.yaml new file mode 100644 index 0000000..c48735e --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/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 diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/launch/keyboard_sender.launch.py b/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/launch/keyboard_sender.launch.py new file mode 100644 index 0000000..49d7667 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/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), + }], + ), + ]) diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/launch/robot_udp_receiver.launch.py b/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/launch/robot_udp_receiver.launch.py new file mode 100644 index 0000000..4a9d1e5 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/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), + }], + ), + ]) diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/launch/xbox_to_udp.launch.py b/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/launch/xbox_to_udp.launch.py new file mode 100644 index 0000000..d9038d0 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/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), + }], + ), + ]) diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/package.xml b/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/package.xml new file mode 100644 index 0000000..fc70b79 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/package.xml @@ -0,0 +1,25 @@ + + + udp_teleop_bridge + 0.1.0 + ROS 2 OmniSocket UDP/KCP bridge for teleop TwistStamped commands. + + Codex + MIT + + ament_python + + ament_index_python + geometry_msgs + joy + launch + launch_ros + rclpy + rosidl_runtime_py + teleop_twist_joy + teleop_twist_keyboard + + + ament_python + + diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/resource/udp_teleop_bridge b/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/resource/udp_teleop_bridge new file mode 100644 index 0000000..9cc185f --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/resource/udp_teleop_bridge @@ -0,0 +1 @@ +udp_teleop_bridge diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/setup.cfg b/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/setup.cfg new file mode 100644 index 0000000..8f79a94 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/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 diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/setup.py b/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/setup.py new file mode 100644 index 0000000..ae42c8d --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/setup.py @@ -0,0 +1,34 @@ +from setuptools import find_packages, setup + + +package_name = 'udp_teleop_bridge' + + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', [f'resource/{package_name}']), + (f'share/{package_name}', ['package.xml']), + (f'share/{package_name}/launch', [ + 'launch/keyboard_sender.launch.py', + 'launch/robot_udp_receiver.launch.py', + 'launch/xbox_to_udp.launch.py', + ]), + (f'share/{package_name}/config', ['config/xbox_twist_joy.yaml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Codex', + maintainer_email='codex@example.com', + description='ROS 2 OmniSocket UDP/KCP bridge for teleop TwistStamped commands.', + license='MIT', + 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', + ], + }, +) diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/test/test_protocol.py b/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/test/test_protocol.py new file mode 100644 index 0000000..87cfbe1 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/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') diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/__init__.py b/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/__init__.py new file mode 100644 index 0000000..094feaf --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/__init__.py @@ -0,0 +1 @@ +"""OmniSocket teleop bridge package.""" diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/cmd_vel_udp_sender.py b/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/cmd_vel_udp_sender.py new file mode 100644 index 0000000..34a5ce1 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/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() diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/omni_transport.py b/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/omni_transport.py new file mode 100644 index 0000000..3978ac3 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/omni_transport.py @@ -0,0 +1,101 @@ +"""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 send_with_id(self, *, to: str, data: bytes) -> int: + if not hasattr(self._session, 'send_with_id'): + self._session.send(to=to, data=data) + raise RuntimeError('send_with_id is not available on this omnisocket build') + return int(self._session.send_with_id(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', +] diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/protocol.py b/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/protocol.py new file mode 100644 index 0000000..44f4641 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/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)) diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/topic_status_reader.py b/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/topic_status_reader.py new file mode 100644 index 0000000..8d0f8d1 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/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() diff --git a/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/udp_cmd_vel_receiver.py b/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/udp_cmd_vel_receiver.py new file mode 100644 index 0000000..8eac4fb --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/ros-control-py/udp_teleop_bridge/udp_teleop_bridge/udp_cmd_vel_receiver.py @@ -0,0 +1,480 @@ +"""ROS 2 node that receives OmniSocket teleop packets and republishes TwistStamped.""" + +from __future__ import annotations + +import json +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 = self._create_transport() + + 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._runtime_dir = os.getenv('BLITZ_RUNTIME_DIR', '/run/blitz-robot').strip() or '/run/blitz-robot' + self._status_path = os.path.join(self._runtime_dir, 'ros-receiver.status.json') + self._transport_reconnect_count = 0 + self._recv_thread_heartbeat_epoch_ms = self._now_epoch_ms() + self._runtime_last_error = '' + + self.create_timer(1.0 / self._publish_rate_hz, self._publish_tick) + self.create_timer(1.0, self._write_status_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 _close_unix_socket(self) -> None: + if self._unix_socket is not None: + try: + self._unix_socket.close() + except OSError: + pass + self._unix_socket = None + + def _create_transport(self): + from .omni_transport import OmniTransport + + return OmniTransport( + transport=self._transport_name, + server_addr=self._server_addr, + relay_via=self._relay_via, + peer_id=self._peer_id, + ) + + def _reconnect_transport(self) -> bool: + while not self._closing.is_set() and rclpy.ok(): + current_transport = self._transport + if current_transport is not None: + try: + current_transport.close() + except OSError: + pass + try: + self._transport = self._create_transport() + self._transport_reconnect_count += 1 + self._set_runtime_last_error('') + if self._should_log('transport_reconnected', 1.0): + self.get_logger().info( + 'Reconnected OmniSocket transport %s://%s as %s' + % (self._transport_name, self._server_addr, self._peer_id) + ) + return True + except OSError as exc: + self._transport = None + self._set_runtime_last_error(str(exc)) + if self._should_log('transport_reconnect_error', 2.0): + self.get_logger().error(f'Failed to reconnect OmniSocket transport: {exc}') + time.sleep(0.5) + return False + + def _rebind_unix_socket(self) -> bool: + while not self._closing.is_set() and rclpy.ok(): + self._close_unix_socket() + try: + self._setup_unix_socket() + self._transport_reconnect_count += 1 + self._set_runtime_last_error('') + if self._should_log('unix_rebound', 1.0): + self.get_logger().info(f'Rebound unix datagram socket at {self._local_socket_path}') + return True + except OSError as exc: + self._set_runtime_last_error(str(exc)) + if self._should_log('unix_rebind_error', 2.0): + self.get_logger().error(f'Failed to rebind unix datagram socket: {exc}') + time.sleep(0.5) + return False + + 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 _now_epoch_ms(self) -> int: + return time.time_ns() // 1_000_000 + + def _update_recv_heartbeat(self) -> None: + with self._lock: + self._recv_thread_heartbeat_epoch_ms = self._now_epoch_ms() + + def _last_packet_age_ms(self) -> int | None: + with self._lock: + last_packet_monotonic = self._last_packet_monotonic + if last_packet_monotonic is None: + return None + return max(0, int((time.monotonic() - last_packet_monotonic) * 1000.0)) + + def _socket_bound(self) -> bool: + if self._transport_name == 'unix_dgram': + return self._unix_socket is not None and os.path.exists(self._local_socket_path) + return self._transport is not None + + def _set_runtime_last_error(self, message: str) -> None: + self._runtime_last_error = message + + def _status_payload(self) -> dict[str, object]: + with self._lock: + recv_thread_heartbeat_epoch_ms = self._recv_thread_heartbeat_epoch_ms + return { + 'updated_at_epoch_ms': self._now_epoch_ms(), + 'pid': os.getpid(), + 'recv_thread_heartbeat_epoch_ms': recv_thread_heartbeat_epoch_ms, + 'transport': self._transport_name, + 'local_socket_path': self._local_socket_path, + 'socket_bound': self._socket_bound(), + 'transport_reconnect_count': self._transport_reconnect_count, + 'last_packet_age_ms': self._last_packet_age_ms(), + 'last_error': self._runtime_last_error, + } + + def _write_status_tick(self) -> None: + payload = self._status_payload() + if self._transport_name == 'unix_dgram': + if self._unix_socket is None: + payload['last_error'] = self._runtime_last_error or 'unix datagram socket is not bound' + else: + if self._transport is None: + payload['last_error'] = self._runtime_last_error or 'OmniSocket transport is not connected' + try: + os.makedirs(self._runtime_dir, exist_ok=True) + temp_path = f'{self._status_path}.tmp.{os.getpid()}' + with open(temp_path, 'w', encoding='utf-8') as handle: + json.dump(payload, handle, ensure_ascii=True, separators=(',', ':')) + os.replace(temp_path, self._status_path) + except OSError as exc: + if self._should_log('status_write_error', 5.0): + self.get_logger().warning(f'Failed to write receiver status file: {exc}') + + 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(): + self._update_recv_heartbeat() + try: + assert self._transport is not None + meta = self._transport.recv_into(buffer=self._recv_buffer, timeout_ms=100) + except BufferError as exc: + self._set_runtime_last_error(str(exc)) + if self._should_log('buffer_error', 2.0): + self.get_logger().warning(f'Dropped oversized OmniSocket frame: {exc}') + continue + except OSError as exc: + self._set_runtime_last_error(str(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}') + if not self._reconnect_transport(): + return + continue + + self._update_recv_heartbeat() + if meta is None: + continue + self._set_runtime_last_error('') + + 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._set_runtime_last_error(f'server error message from {from_peer}') + self._handle_error_message(from_peer, body_len) + continue + + if self._expected_sender and from_peer != self._expected_sender: + self._set_runtime_last_error(f'unexpected sender {from_peer}') + 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: + self._set_runtime_last_error(f'unexpected message type {msg_type}') + 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: + self._set_runtime_last_error(f'invalid payload size {body_len}') + 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: + self._set_runtime_last_error(str(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() + self._set_runtime_last_error('') + + def _recv_loop_unix_dgram(self) -> None: + assert self._unix_socket is not None + + while not self._closing.is_set() and rclpy.ok(): + self._update_recv_heartbeat() + try: + payload = self._unix_socket.recv(DEFAULT_RECV_BUFFER_BYTES) + except socket.timeout: + if not os.path.exists(self._local_socket_path): + self._set_runtime_last_error('unix datagram socket path disappeared') + if self._should_log('unix_socket_missing', 2.0): + self.get_logger().warning( + f'Unix datagram socket path disappeared, rebinding {self._local_socket_path}' + ) + if not self._rebind_unix_socket(): + return + continue + except OSError as exc: + self._set_runtime_last_error(str(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}') + if not self._rebind_unix_socket(): + return + continue + + self._update_recv_heartbeat() + if len(payload) != PACKET_SIZE: + self._set_runtime_last_error(f'invalid unix datagram payload size {len(payload)}') + 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: + self._set_runtime_last_error(str(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() + self._set_runtime_last_error('') + + 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._close_unix_socket() + except OSError as exc: + if self._should_log('unix_close_error', 2.0): + self.get_logger().warning(f'Closing unix socket failed: {exc}') + 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() diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/BACDauto_test.sh b/robot/v4l2/OmniSocketGo_robot/scripts/BACDauto_test.sh new file mode 100644 index 0000000..70f7300 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/BACDauto_test.sh @@ -0,0 +1,296 @@ +#!/bin/bash + +LOCAL_REPO_DIR="/home/limingjie/LMJ_Work/RobotCompetition/OmniSocketGo" +KCP_PEER_BIN="./bin/kcppeer" +OUTPUT_ROOT="/home/limingjie/LMJ_Work/RobotCompetition/KCPData/BDAClogs" +PEERB_POLL_INTERVAL_SEC=5 +PEERB_MAX_POLLS=180 +PEER_A_EXIT_WAIT_SEC=5 + +require_local_binary() { + if [ ! -x "$1" ]; then + echo "ERROR: 缺少可执行文件 $1" + exit 1 + fi +} + +cleanup_remote_peerb() { + ssh omni-peer bash -s <<'EOF' +pids=$(ps -eo pid=,args= | awk '/[b]in\/kcppeer -id peer-b/ {print $1}') +if [ -n "$pids" ]; then + kill $pids 2>/dev/null || true +fi +pids=$(ps -eo pid=,args= | awk '/\/tmp\/peerb_batch\.sh/ {print $1}') +if [ -n "$pids" ]; then + kill $pids 2>/dev/null || true +fi +rm -f /tmp/peerb_batch_done /tmp/peerb_batch.sh /tmp/peerb_commands +EOF +} + +echo "=== 开始自动化测试 ===" + +cd "$LOCAL_REPO_DIR" +require_local_binary "$KCP_PEER_BIN" + +echo ">>> 0. 清理上次残留进程..." +pkill -f 'bin/kcppeer -id peer-a' 2>/dev/null || true +cleanup_remote_peerb || exit 1 + +rm -rf logs +rm -rf inbox/a +mkdir -p logs inbox/a + +# 1. 清理残留 & 启动 Server D 和 Relay C +echo ">>> 1. 启动 Server D 和 Relay C..." + +ssh bj-txy bash -s <<'EOF' +pkill -f kcpserver 2>/dev/null || true +pkill -f 'bin/kcpserver' 2>/dev/null || true +sleep 1 +cd /home/ubuntu/OmniSocketGo +rm -rf logs +mkdir -p logs +if [ ! -x ./bin/kcpserver ]; then + echo "ERROR: 缺少 ./bin/kcpserver" + 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 || true +pkill -f 'bin/kcpserver' 2>/dev/null || true +sleep 1 +cd /home/ubuntu/OmniSocketGo +rm -rf logs +mkdir -p logs +if [ ! -x ./bin/kcpserver ]; then + echo "ERROR: 缺少 ./bin/kcpserver" + exit 1 +fi +setsid ./bin/kcpserver -mode=relay -listen 0.0.0.0:10909 -relay-remote 172.21.32.15:10909 > relay_console.log 2>&1 /dev/null; then + echo " Server D 就绪 (${i}s)" + break + fi + if [ "$i" -eq 60 ]; then + echo " ERROR: Server D 60s 内未就绪，退出" + exit 1 + fi + sleep 1 +done + +# 等待 relay C 端口就绪 +echo " 等待 Relay C 端口就绪..." +for i in $(seq 1 60); do + if ssh sz-txy "ss -ulnp | grep -q 10909" 2>/dev/null; then + echo " Relay C 就绪 (${i}s)" + break + fi + if [ "$i" -eq 60 ]; then + echo " ERROR: Relay C 60s 内未就绪，退出" + exit 1 + fi + sleep 1 +done + +# 2. 启动本地 Peer-A +echo ">>> 2. 启动本地 Peer-A..." +PEER_A_CMD_FIFO="/tmp/peera_commands_$$" +rm -f "$PEER_A_CMD_FIFO" +mkfifo "$PEER_A_CMD_FIFO" +nohup "$KCP_PEER_BIN" \ + -id peer-a \ + -server 172.21.32.15:10909 \ + -relay-via 106.55.173.235:10909 \ + -inbox-dir inbox/a \ + -latency-log logs/a-latency.jsonl \ + -kcp-ts-debug-log logs/a-kcp-ts.jsonl \ + -kcp-session-stats-log logs/a-kcp-stats.jsonl \ + < "$PEER_A_CMD_FIFO" > logs/peera_console.log 2>&1 & +PEER_A_PID=$! +exec 4>"$PEER_A_CMD_FIFO" + +# 等待 peer-a 注册成功 +echo " 等待 Peer-A 注册..." +for i in $(seq 1 30); do + if grep -Eq "opened KCP session as peer-a|connected to .* as peer-a( \\(KCP\\))?" logs/peera_console.log 2>/dev/null; then + echo " Peer-A 就绪 (${i}s)" + break + fi + if [ "$i" -eq 30 ]; then + echo " WARNING: Peer-A 30s 内未就绪" + fi + sleep 1 +done + +# 3. 在远端后台启动 peer-b 整个发送流程，不依赖长 SSH 连接 +echo ">>> 3. 启动远端 Peer-B 并执行 50 轮打流测试..." +ssh omni-peer "cd /home/boll/LMJWork/OmniSocketGo && rm -rf logs inbox/b && mkdir -p logs inbox/b" + +PEERB_DONE_FLAG="/tmp/peerb_batch_done" +PEERB_BATCH_SCRIPT="/tmp/peerb_batch.sh" + +# 把整个发送脚本写到远端，setsid 后台执行 +ssh omni-peer bash -s <<'DEPLOY_SCRIPT' +DONE_FLAG="/tmp/peerb_batch_done" +BATCH_SCRIPT="/tmp/peerb_batch.sh" +rm -f "$DONE_FLAG" + +cat > "$BATCH_SCRIPT" <<'INNER_EOF' +#!/bin/bash +cd /home/boll/LMJWork/OmniSocketGo + +CMD_FIFO=/tmp/peerb_commands +DONE_FLAG="/tmp/peerb_batch_done" +STATUS="error" +rm -f "$CMD_FIFO" "$DONE_FLAG" +mkfifo "$CMD_FIFO" + +finish() { + local status_to_write="$STATUS" + rm -f "$CMD_FIFO" + printf '%s\n' "$status_to_write" > "$DONE_FLAG" +} + +trap finish EXIT + +if [ ! -x ./bin/kcppeer ]; then + echo "ERROR: 缺少 ./bin/kcppeer" > logs/peerb_console.log + exit 1 +fi + +# 启动 peer-b +./bin/kcppeer \ + -id peer-b \ + -server 81.70.156.140:10909 \ + -inbox-dir inbox/b \ + -latency-log logs/b-latency.jsonl \ + -kcp-ts-debug-log logs/b-kcp-ts.jsonl \ + -kcp-session-stats-log logs/b-kcp-stats.jsonl \ + < "$CMD_FIFO" > logs/peerb_console.log 2>&1 & +PEER_B_PID=$! + +exec 3>"$CMD_FIFO" + +# 等 peer-b 就绪 +for i in $(seq 1 60); do + if grep -Eq "opened KCP session as peer-b|connected to .* as peer-b( \\(KCP\\))?" logs/peerb_console.log 2>/dev/null; then + break + fi + sleep 1 +done + +# 50 轮发送 +for i in $(seq 1 50); do + echo "file peer-a /home/boll/test30.bin" >&3 + sleep 1 + echo "file peer-a /home/boll/test5.bin" >&3 + sleep 1 +done + +sleep 5 +echo "quit" >&3 || true +exec 3>&- + +peer_b_exited=0 +for i in $(seq 1 15); do + if ! kill -0 $PEER_B_PID 2>/dev/null; then + peer_b_exited=1 + break + fi + sleep 1 +done + +if [ "$peer_b_exited" -eq 0 ]; then + kill $PEER_B_PID 2>/dev/null || true + sleep 1 +fi + +if kill -0 $PEER_B_PID 2>/dev/null; then + kill -9 $PEER_B_PID 2>/dev/null || true +fi + +wait $PEER_B_PID 2>/dev/null || true + +# 写完成标记 +STATUS="done" +INNER_EOF + +chmod +x "$BATCH_SCRIPT" +setsid bash "$BATCH_SCRIPT" /dev/null 2>&1 & +echo "peer-b batch launched in background" +DEPLOY_SCRIPT + +# 本地轮询等待远端完成（短 SSH 连接，不怕断开） +echo " 等待 peer-b 发送完成（预计 ~110 秒）..." +for i in $(seq 1 "$PEERB_MAX_POLLS"); do + PEERB_STATUS=$(ssh omni-peer "cat /tmp/peerb_batch_done 2>/dev/null || true") + if [ "$PEERB_STATUS" = "done" ]; then + ELAPSED_SEC=$(( (i - 1) * PEERB_POLL_INTERVAL_SEC )) + echo " peer-b 发送完成（约 ${ELAPSED_SEC}s）" + break + fi + if [ "$PEERB_STATUS" = "error" ]; then + echo " ERROR: peer-b 后台任务启动失败，请检查远端 logs/peerb_console.log" + exit 1 + fi + if [ "$i" -eq "$PEERB_MAX_POLLS" ]; then + echo " ERROR: peer-b $((PEERB_MAX_POLLS * PEERB_POLL_INTERVAL_SEC))s 内未完成" + exit 1 + fi + # 每 5 秒查一次，减少 SSH 连接频率 + sleep "$PEERB_POLL_INTERVAL_SEC" +done + +# 4. 清理 +echo ">>> 4. 清理所有进程..." +sleep 2 +echo "quit" >&4 || true +exec 4>&- +for i in $(seq 1 "$PEER_A_EXIT_WAIT_SEC"); do + if ! kill -0 "$PEER_A_PID" 2>/dev/null; then + break + fi + sleep 1 +done +if kill -0 "$PEER_A_PID" 2>/dev/null; then + kill "$PEER_A_PID" 2>/dev/null || true +fi +wait "$PEER_A_PID" 2>/dev/null || true +rm -f "$PEER_A_CMD_FIFO" +ssh bj-txy "pkill -f kcpserver || true; pkill -f 'bin/kcpserver' || true" +ssh sz-txy "pkill -f kcpserver || true; pkill -f 'bin/kcpserver' || true" + +# 获取当前时间戳，格式为 YYYYMMDD_HHMMSS +TIMESTAMP=$(date +"%Y%m%d_%H%M%S") +OUTPUT_DIR="$OUTPUT_ROOT/$TIMESTAMP" +# 5. 拉取数据 & 生成报告 +echo ">>> 5. 拉取数据并生成汇总报告..." +mkdir -p "$OUTPUT_DIR" +scp -o ServerAliveInterval=15 -P 10022 boll@175.178.116.187:/home/boll/LMJWork/OmniSocketGo/logs/b-latency.jsonl "$LOCAL_REPO_DIR/logs/b-latency.jsonl" || exit 1 + +(cd "$LOCAL_REPO_DIR/go" && go run ./cmd/latencysummary \ + -input /home/limingjie/LMJ_Work/RobotCompetition/OmniSocketGo/logs/a-latency.jsonl \ + -input /home/limingjie/LMJ_Work/RobotCompetition/OmniSocketGo/logs/b-latency.jsonl \ + -output /home/limingjie/LMJ_Work/RobotCompetition/OmniSocketGo/logs/latency-summary.jsonl) || exit 1 +cd "$LOCAL_REPO_DIR/.." || exit 1 +mv "$LOCAL_REPO_DIR/logs/a-latency.jsonl" "$OUTPUT_DIR/a-latency.jsonl" || exit 1 +mv "$LOCAL_REPO_DIR/logs/b-latency.jsonl" "$OUTPUT_DIR/b-latency.jsonl" || exit 1 +mv "$LOCAL_REPO_DIR/logs/latency-summary.jsonl" "$OUTPUT_DIR/latency-summary.jsonl" || exit 1 +if [ -f "$LOCAL_REPO_DIR/logs/latency-summary.html" ]; then + mv "$LOCAL_REPO_DIR/logs/latency-summary.html" "$OUTPUT_DIR/latency-summary.html" || exit 1 +fi + +echo "=== 测试完成！===" diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/BDAanto_test.sh b/robot/v4l2/OmniSocketGo_robot/scripts/BDAanto_test.sh new file mode 100644 index 0000000..dab1169 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/BDAanto_test.sh @@ -0,0 +1,259 @@ +#!/bin/bash + +LOCAL_REPO_DIR="/home/limingjie/LMJ_Work/RobotCompetition/OmniSocketGo" +KCP_PEER_BIN="./bin/kcppeer" +OUTPUT_ROOT="/home/limingjie/LMJ_Work/RobotCompetition/KCPData/BCAlogs/" +PEERB_POLL_INTERVAL_SEC=5 +PEERB_MAX_POLLS=180 +PEER_A_EXIT_WAIT_SEC=5 + +require_local_binary() { + if [ ! -x "$1" ]; then + echo "ERROR: 缺少可执行文件 $1" + exit 1 + fi +} + +cleanup_remote_peerb() { + ssh omni-peer bash -s <<'EOF' +pids=$(ps -eo pid=,args= | awk '/[b]in\/kcppeer -id peer-b/ {print $1}') +if [ -n "$pids" ]; then + kill $pids 2>/dev/null || true +fi +pids=$(ps -eo pid=,args= | awk '/\/tmp\/peerb_batch\.sh/ {print $1}') +if [ -n "$pids" ]; then + kill $pids 2>/dev/null || true +fi +rm -f /tmp/peerb_batch_done /tmp/peerb_batch.sh /tmp/peerb_commands +EOF +} + +echo "=== 开始自动化测试 ===" + +cd "$LOCAL_REPO_DIR" +require_local_binary "$KCP_PEER_BIN" + +echo ">>> 0. 清理上次残留进程..." +pkill -f 'bin/kcppeer -id peer-a' 2>/dev/null || true +cleanup_remote_peerb || exit 1 + +rm -rf logs +rm -rf inbox/a +mkdir -p logs inbox/a + +# 1. 清理残留 & 启动 Server D +echo ">>> 1. 启动 Server D..." + +ssh bj-txy bash -s <<'EOF' +pkill -f kcpserver 2>/dev/null || true +pkill -f 'bin/kcpserver' 2>/dev/null || true +sleep 1 +cd /home/ubuntu/OmniSocketGo +rm -rf logs +mkdir -p logs +if [ ! -x ./bin/kcpserver ]; then + echo "ERROR: 缺少 ./bin/kcpserver" + 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; then + echo " Server D 就绪 (${i}s)" + break + fi + if [ "$i" -eq 60 ]; then + echo " ERROR: Server D 60s 内未就绪，退出" + exit 1 + fi + sleep 1 +done + +# 2. 启动本地 Peer-A +echo ">>> 2. 启动本地 Peer-A..." +PEER_A_CMD_FIFO="/tmp/peera_commands_$$" +rm -f "$PEER_A_CMD_FIFO" +mkfifo "$PEER_A_CMD_FIFO" +nohup "$KCP_PEER_BIN" \ + -id peer-a \ + -server 81.70.156.140:10909 \ + -inbox-dir inbox/a \ + -latency-log logs/a-latency.jsonl \ + -kcp-ts-debug-log logs/a-kcp-ts.jsonl \ + -kcp-session-stats-log logs/a-kcp-stats.jsonl \ + < "$PEER_A_CMD_FIFO" > logs/peera_console.log 2>&1 & +PEER_A_PID=$! +exec 4>"$PEER_A_CMD_FIFO" + +# 等待 peer-a 注册成功 +echo " 等待 Peer-A 注册..." +for i in $(seq 1 30); do + if grep -Eq "opened KCP session as peer-a|connected to .* as peer-a( \\(KCP\\))?" logs/peera_console.log 2>/dev/null; then + echo " Peer-A 就绪 (${i}s)" + break + fi + if [ "$i" -eq 30 ]; then + echo " WARNING: Peer-A 30s 内未就绪" + fi + sleep 1 +done + +# 3. 在远端后台启动 peer-b 整个发送流程，不依赖长 SSH 连接 +echo ">>> 3. 启动远端 Peer-B 并执行 50 轮打流测试..." +ssh omni-peer "cd /home/boll/LMJWork/OmniSocketGo && rm -rf logs inbox/b && mkdir -p logs inbox/b" + +PEERB_DONE_FLAG="/tmp/peerb_batch_done" +PEERB_BATCH_SCRIPT="/tmp/peerb_batch.sh" + +# 把整个发送脚本写到远端，setsid 后台执行 +ssh omni-peer bash -s <<'DEPLOY_SCRIPT' +DONE_FLAG="/tmp/peerb_batch_done" +BATCH_SCRIPT="/tmp/peerb_batch.sh" +rm -f "$DONE_FLAG" + +cat > "$BATCH_SCRIPT" <<'INNER_EOF' +#!/bin/bash +cd /home/boll/LMJWork/OmniSocketGo + +CMD_FIFO=/tmp/peerb_commands +DONE_FLAG="/tmp/peerb_batch_done" +rm -f "$CMD_FIFO" "$DONE_FLAG" +mkfifo "$CMD_FIFO" + +if [ ! -x ./bin/kcppeer ]; then + echo "ERROR: 缺少 ./bin/kcppeer" > logs/peerb_console.log + echo "error" > "$DONE_FLAG" + exit 1 +fi + +# 启动 peer-b +./bin/kcppeer \ + -id peer-b \ + -server 81.70.156.140:10909 \ + -inbox-dir inbox/b \ + -latency-log logs/b-latency.jsonl \ + -kcp-ts-debug-log logs/b-kcp-ts.jsonl \ + -kcp-session-stats-log logs/b-kcp-stats.jsonl \ + < "$CMD_FIFO" > logs/peerb_console.log 2>&1 & +PEER_B_PID=$! + +exec 3>"$CMD_FIFO" + +# 等 peer-b 就绪 +for i in $(seq 1 60); do + if grep -Eq "opened KCP session as peer-b|connected to .* as peer-b( \\(KCP\\))?" logs/peerb_console.log 2>/dev/null; then + break + fi + sleep 1 +done + +# 50 轮发送 +for i in $(seq 1 50); do + echo "file peer-a /home/boll/test30.bin" >&3 + sleep 1 + echo "file peer-a /home/boll/test5.bin" >&3 + sleep 1 +done + +sleep 5 +echo "quit" >&3 +exec 3>&- +rm -f "$CMD_FIFO" + +peer_b_exited=0 +for i in $(seq 1 15); do + if ! kill -0 $PEER_B_PID 2>/dev/null; then + peer_b_exited=1 + break + fi + sleep 1 +done + +if [ "$peer_b_exited" -eq 0 ]; then + kill $PEER_B_PID 2>/dev/null || true + sleep 1 +fi + +if kill -0 $PEER_B_PID 2>/dev/null; then + kill -9 $PEER_B_PID 2>/dev/null || true +fi + +wait $PEER_B_PID 2>/dev/null || true + +# 写完成标记 +echo "done" > "$DONE_FLAG" +INNER_EOF + +chmod +x "$BATCH_SCRIPT" +setsid bash "$BATCH_SCRIPT" /dev/null 2>&1 & +echo "peer-b batch launched in background" +DEPLOY_SCRIPT + +# 本地轮询等待远端完成（短 SSH 连接，不怕断开） +echo " 等待 peer-b 发送完成（预计 ~110 秒）..." +for i in $(seq 1 "$PEERB_MAX_POLLS"); do + PEERB_STATUS=$(ssh omni-peer "cat /tmp/peerb_batch_done 2>/dev/null || true") + if [ "$PEERB_STATUS" = "done" ]; then + ELAPSED_SEC=$(( (i - 1) * PEERB_POLL_INTERVAL_SEC )) + echo " peer-b 发送完成（约 ${ELAPSED_SEC}s）" + break + fi + if [ "$PEERB_STATUS" = "error" ]; then + echo " ERROR: peer-b 后台任务启动失败，请检查远端 logs/peerb_console.log" + exit 1 + fi + if [ "$i" -eq "$PEERB_MAX_POLLS" ]; then + echo " ERROR: peer-b $((PEERB_MAX_POLLS * PEERB_POLL_INTERVAL_SEC))s 内未完成" + exit 1 + fi + sleep "$PEERB_POLL_INTERVAL_SEC" +done + +# 4. 清理 +echo ">>> 4. 清理所有进程..." +sleep 2 +echo "quit" >&4 || true +exec 4>&- +for i in $(seq 1 "$PEER_A_EXIT_WAIT_SEC"); do + if ! kill -0 "$PEER_A_PID" 2>/dev/null; then + break + fi + sleep 1 +done +if kill -0 "$PEER_A_PID" 2>/dev/null; then + kill "$PEER_A_PID" 2>/dev/null || true +fi +wait "$PEER_A_PID" 2>/dev/null || true +rm -f "$PEER_A_CMD_FIFO" +ssh bj-txy "pkill -f kcpserver || true; pkill -f 'bin/kcpserver' || true" + +# 获取当前时间戳，格式为 YYYYMMDD_HHMMSS +TIMESTAMP=$(date +"%Y%m%d_%H%M%S") +OUTPUT_DIR="$OUTPUT_ROOT/$TIMESTAMP" + +# 5. 拉取数据 & 生成报告 +echo ">>> 5. 拉取数据并生成汇总报告..." +mkdir -p "$OUTPUT_DIR" +scp -o ServerAliveInterval=15 -P 10022 boll@175.178.116.187:/home/boll/LMJWork/OmniSocketGo/logs/b-latency.jsonl "$LOCAL_REPO_DIR/logs/b-latency.jsonl" || exit 1 + +(cd "$LOCAL_REPO_DIR/go" && go run ./cmd/latencysummary \ + -input "$LOCAL_REPO_DIR/logs/a-latency.jsonl" \ + -input "$LOCAL_REPO_DIR/logs/b-latency.jsonl" \ + -output "$LOCAL_REPO_DIR/logs/latency-summary.jsonl") || exit 1 + +cd "$LOCAL_REPO_DIR/.." || exit 1 +mv "$LOCAL_REPO_DIR/logs/a-latency.jsonl" "$OUTPUT_DIR/a-latency.jsonl" || exit 1 +mv "$LOCAL_REPO_DIR/logs/b-latency.jsonl" "$OUTPUT_DIR/b-latency.jsonl" || exit 1 +mv "$LOCAL_REPO_DIR/logs/latency-summary.jsonl" "$OUTPUT_DIR/latency-summary.jsonl" || exit 1 +if [ -f "$LOCAL_REPO_DIR/logs/latency-summary.html" ]; then + mv "$LOCAL_REPO_DIR/logs/latency-summary.html" "$OUTPUT_DIR/latency-summary.html" || exit 1 +fi + +echo "=== 测试完成！===" diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/5g-dial.sh b/robot/v4l2/OmniSocketGo_robot/scripts/boot/5g-dial.sh new file mode 100644 index 0000000..e2c07c7 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/5g-dial.sh @@ -0,0 +1,180 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="5g-dial" + +append_route_targets() { + local raw_list="$1" + local target + + if [[ -z "${raw_list}" ]]; then + return 0 + fi + + for target in ${raw_list//,/ }; do + if [[ -z "${target}" ]]; then + continue + fi + dial_cmd+=(--route-target "${target}") + done +} + +read_detected_interface() { + local info_json="$1" + + if [[ ! -f "${info_json}" ]]; then + return 1 + fi + + python3 -c 'import json, sys; print((json.load(open(sys.argv[1], encoding="utf-8")).get("interface") or "").strip())' "${info_json}" +} + +disable_interfaces() { + local raw_list="$1" + local iface + local nmcli_available=0 + + if [[ -z "${raw_list}" ]]; then + return 0 + fi + if command -v nmcli >/dev/null 2>&1; then + nmcli_available=1 + fi + + for iface in ${raw_list//,/ }; do + if [[ -z "${iface}" ]]; then + continue + fi + blitz_log "${STEP}" "disable-interface" "start" "iface=${iface}" 0 + if [[ "${nmcli_available}" -eq 1 ]]; then + nmcli device disconnect "${iface}" >/dev/null 2>&1 || true + fi + if ip link show dev "${iface}" >/dev/null 2>&1; then + if ip link set dev "${iface}" down; then + blitz_log "${STEP}" "disable-interface" "success" "iface=${iface}" 0 + else + rc=$? + blitz_log "${STEP}" "disable-interface" "failure" "iface=${iface}" "${rc}" + return "${rc}" + fi + else + blitz_log "${STEP}" "disable-interface" "success" "iface=${iface} not present, skipping" 0 + fi + done +} + +wait_for_serial() { + local serial_port="$1" + local timeout_sec="$2" + local waited=0 + + while (( waited < timeout_sec )); do + if [[ -e "${serial_port}" ]]; then + blitz_log "${STEP}" "wait-serial" "success" "serial_port=${serial_port} waited_sec=${waited}" 0 + return 0 + fi + if (( waited == 0 || waited % 5 == 0 )); then + blitz_log "${STEP}" "wait-serial" "waiting" "serial_port=${serial_port} waited_sec=${waited}" 0 + fi + sleep 1 + waited=$(( waited + 1 )) + done + + blitz_log "${STEP}" "wait-serial" "failure" "serial_port=${serial_port} timeout_sec=${timeout_sec}" 1 + return 1 +} + +wait_for_route() { + local target_ip="$1" + local timeout_sec="$2" + local expected_interface="${3:-}" + local waited=0 + local route_output + + while (( waited < timeout_sec )); do + route_output="$(blitz_route_ready "${target_ip}" "${expected_interface}" || true)" + if [[ -n "${route_output}" ]]; then + blitz_log "${STEP}" "route-check" "success" "target_ip=${target_ip} interface=${expected_interface:-auto} route=${route_output}" 0 + return 0 + fi + if (( waited == 0 || waited % 5 == 0 )); then + blitz_log "${STEP}" "route-check" "waiting" "target_ip=${target_ip} interface=${expected_interface:-auto} waited_sec=${waited}" 0 + fi + sleep 1 + waited=$(( waited + 1 )) + done + + blitz_log "${STEP}" "route-check" "failure" "target_ip=${target_ip} interface=${expected_interface:-auto} timeout_sec=${timeout_sec}" 1 + return 1 +} + +blitz_load_boot_env +blitz_require_root "${STEP}" +blitz_require_command ip "${STEP}" +blitz_require_command python3 "${STEP}" +blitz_require_file "${BLITZ_5G_DIAL_DIR}/rndis_dial.py" "${STEP}" + +if [[ -z "${BLITZ_TIME_SERVER_IP}" ]]; then + blitz_log "${STEP}" "precheck" "failure" "BLITZ_TIME_SERVER_IP is empty and no fallback could be derived" 1 + exit 1 +fi + +disable_interfaces "${BLITZ_5G_DISABLE_INTERFACES:-}" + +if [[ -n "${BLITZ_5G_INTERFACE:-}" ]]; then + route_output="$(blitz_route_ready "${BLITZ_TIME_SERVER_IP}" "${BLITZ_5G_INTERFACE}" || true)" + if [[ -n "${route_output}" ]]; then + blitz_log "${STEP}" "dial" "already_up" "target_ip=${BLITZ_TIME_SERVER_IP} interface=${BLITZ_5G_INTERFACE} route=${route_output}" 0 + exit 0 + fi +else + blitz_log "${STEP}" "route-check" "info" "BLITZ_5G_INTERFACE is empty, skipping pre-dial route shortcut and using auto-detect mode" 0 +fi + +wait_for_serial "${BLITZ_5G_SERIAL_PORT}" "${BLITZ_5G_SERIAL_WAIT_SEC}" + +dial_cmd=( + python3 + rndis_dial.py + --serial-port "${BLITZ_5G_SERIAL_PORT}" + --modem-subnet "${BLITZ_5G_MODEM_SUBNET}" +) +if [[ -n "${BLITZ_5G_INTERFACE:-}" ]]; then + dial_cmd+=(--interface "${BLITZ_5G_INTERFACE}") +fi +case "${BLITZ_5G_SKIP_DHCP:-0}" in + 1|true|TRUE|yes|YES) + dial_cmd+=(--skip-dhcp) + ;; +esac +case "${BLITZ_5G_REMOVE_DEFAULT_ROUTE:-1}" in + 1|true|TRUE|yes|YES) + dial_cmd+=(--remove-default-route --gateway "${BLITZ_5G_GATEWAY}" --route-target "${BLITZ_TIME_SERVER_IP}") + append_route_targets "${BLITZ_5G_ROUTE_TARGETS:-}" + ;; +esac + +pushd "${BLITZ_5G_DIAL_DIR}" >/dev/null +blitz_run "${STEP}" "dial" "${dial_cmd[@]}" +popd >/dev/null + +resolved_interface="${BLITZ_5G_INTERFACE:-}" +if [[ -z "${resolved_interface}" ]]; then + resolved_interface="$(read_detected_interface "${BLITZ_5G_INFO_JSON}" || true)" + if [[ -n "${resolved_interface}" ]]; then + blitz_log "${STEP}" "resolve-interface" "success" "resolved interface from ${BLITZ_5G_INFO_JSON}: ${resolved_interface}" 0 + else + blitz_log "${STEP}" "resolve-interface" "failure" "failed to read detected interface from ${BLITZ_5G_INFO_JSON}" 1 + fi +fi + +if [[ -n "${resolved_interface}" ]]; then + wait_for_route "${BLITZ_TIME_SERVER_IP}" "${BLITZ_5G_ROUTE_WAIT_SEC}" "${resolved_interface}" + blitz_log "${STEP}" "complete" "success" "5G dial completed and route is ready on ${resolved_interface}" 0 +else + blitz_log "${STEP}" "complete" "success" "5G dial completed but route wait was skipped because no interface could be resolved; refer to rndis_dial.py logs" 0 +fi diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/README.md b/robot/v4l2/OmniSocketGo_robot/scripts/boot/README.md new file mode 100644 index 0000000..ab75d33 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/README.md @@ -0,0 +1,219 @@ +# Robot B-Side Boot Chain + +This directory contains the robot-side boot and recovery scripts. + +Normal usage is: + +```bash +sudo bash scripts/boot/install-systemd.sh +sudo systemctl start blitz-robot.target +``` + +After installation, `blitz-robot.target` is enabled and will start automatically on reboot. + +To stop the chain now and disable boot-time autostart for future reboots: + +```bash +sudo bash scripts/boot/disable-systemd.sh +``` + +## Current Startup Order + +The current cold-start chain is: + +1. `blitz-boot-gate.service` +2. `blitz-5g-dial.service` +3. `blitz-ros-receiver.service` +4. `blitz-b-side-omnid.service` +5. `blitz-watchdog.service` + +There is no longer any automatic time-sync step in the boot chain. + +## What Each Script Does + +- `robot-boot.env`: default boot configuration +- `robot-boot.env.local`: machine-local overrides +- `common.sh`: shared env loading, logging, and helper functions +- `boot-gate.sh`: fixed startup delay gate +- `5g-dial.sh`: brings up the 5G modem path and verifies routing +- `start-ros-receiver-service.sh`: boot wrapper for ROS receiver +- `wait-for-unix-socket.sh`: waits for the ROS receiver unix socket +- `start-b-side-omnid-service.sh`: boot wrapper for `b_side_omnid` +- `blitz-watchdog.sh`: runtime health watchdog and recovery orchestrator +- `blitz-fault-inject.sh`: fault injection entrypoint +- `install-systemd.sh`: installs systemd units into `/etc/systemd/system` +- `disable-systemd.sh`: stops the boot chain and disables autostart + +## Important Configuration + +Most machine-specific overrides should go into: + +```text +scripts/boot/robot-boot.env.local +``` + +Typical settings: + +```bash +BLITZ_BOOT_DELAY_SEC="30" +BLITZ_LOG_FILE="/var/log/blitz-robot/startup.log" +BLITZ_RUNTIME_DIR="/run/blitz-robot" + +BLITZ_5G_DIAL_DIR="${OMNISOCKETGO_ROOT}/scripts/boot" +BLITZ_5G_SERIAL_PORT="/dev/ttyUSB2" +BLITZ_5G_INTERFACE="" +BLITZ_5G_MODEM_SUBNET="192.168.224.0/22" +BLITZ_5G_GATEWAY="192.168.225.1" +BLITZ_5G_REMOVE_DEFAULT_ROUTE="1" +BLITZ_5G_ROUTE_TARGETS="106.55.173.235" +BLITZ_5G_INFO_JSON="${OMNISOCKETGO_ROOT}/scripts/boot/modem_network_info.json" + +BLITZ_TIME_SERVER_IP="81.70.156.140" + +BLITZ_ROS_USER="nvidia" +BLITZ_ROS_SOCKET_WAIT_SEC="20" +BLITZ_WATCHDOG_INTERVAL_SEC="5" +BLITZ_HEALTH_STALE_SEC="15" +BLITZ_OMNID_THREAD_HEARTBEAT_TIMEOUT_SEC="15" +BLITZ_NETWORK_FAIL_THRESHOLD="3" +BLITZ_NETWORK_RECOVERY_COOLDOWN_SEC="30" +BLITZ_GPS_MONITOR_ENABLED="1" +BLITZ_GPS_DEVICE_GLOB="/dev/ttyCH341USB*" +BLITZ_GPS_CHECK_INTERVAL_SEC="10" +BLITZ_GPS_RESTART_UNITS="gpsd.socket gpsd.service" +BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION="0" +``` + +`BLITZ_TIME_SERVER_IP` is still used, but only as the 5G route/ping health-check target. It is no longer used for automatic clock synchronization. + +If `BLITZ_TIME_SERVER_IP` is left empty, the scripts fall back to the host part of `ROBOT_SIDE_OMNISOCKET_SERVER_ADDR`. + +## Install Or Upgrade + +Run: + +```bash +sudo bash scripts/boot/install-systemd.sh +sudo systemctl daemon-reload +sudo systemctl restart blitz-robot.target +``` + +`install-systemd.sh` will also remove any old `blitz-time-sync.service` unit left over from earlier versions. + +## Disable Autostart + +To stop the currently running services and disable autostart for future reboots: + +```bash +sudo bash scripts/boot/disable-systemd.sh +``` + +To re-enable later: + +```bash +sudo bash scripts/boot/install-systemd.sh +sudo systemctl start blitz-robot.target +``` + +## Logs + +All boot-chain and watchdog logs are appended to: + +```text +/var/log/blitz-robot/startup.log +``` + +Follow the log live: + +```bash +sudo tail -f /var/log/blitz-robot/startup.log +``` + +Check service state: + +```bash +sudo systemctl status blitz-robot.target +sudo systemctl status blitz-5g-dial.service +sudo systemctl status blitz-ros-receiver.service +sudo systemctl status blitz-b-side-omnid.service +sudo systemctl status blitz-watchdog.service +``` + +Check systemd journal: + +```bash +sudo journalctl -u blitz-robot.target -u blitz-5g-dial.service \ + -u blitz-ros-receiver.service -u blitz-b-side-omnid.service \ + -u blitz-watchdog.service -f +``` + +## Runtime Status Files + +The runtime status directory is: + +```text +/run/blitz-robot +``` + +Key files: + +- `b-side-omnid.status.json` +- `ros-receiver.status.json` +- `watchdog.status.json` + +`watchdog.status.json` now also records `gps_ok` and `gps_device_present` so you can quickly tell whether the GPS USB serial node is currently visible and whether the last `gpsd` reconnect attempt succeeded. + +Pretty-print them: + +```bash +sudo python3 -m json.tool /run/blitz-robot/watchdog.status.json +sudo python3 -m json.tool /run/blitz-robot/b-side-omnid.status.json +sudo python3 -m json.tool /run/blitz-robot/ros-receiver.status.json +``` + +## Fault Injection + +Available test commands: + +```bash +sudo bash scripts/boot/blitz-fault-inject.sh bside-crash +sudo bash scripts/boot/blitz-fault-inject.sh bside-process-freeze +sudo bash scripts/boot/blitz-fault-inject.sh bside-video-thread-stall +sudo bash scripts/boot/blitz-fault-inject.sh bside-control-thread-stall +sudo bash scripts/boot/blitz-fault-inject.sh ros-crash +sudo bash scripts/boot/blitz-fault-inject.sh ros-freeze +``` + +For synthetic network fault injection, first enable it in `robot-boot.env.local`: + +```bash +BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION="1" +``` + +Then restart watchdog and inject: + +```bash +sudo systemctl restart blitz-watchdog.service +sudo bash scripts/boot/blitz-fault-inject.sh network-down on +sudo bash scripts/boot/blitz-fault-inject.sh network-down off +``` + +## Recovery Behavior Summary + +- If `b_side_omnid` dies or its status file goes stale, watchdog first tries a targeted `b_side` restart. +- If ROS receiver dies, loses its socket, or its heartbeat goes stale, watchdog performs an ordered full restart: + - stop `b_side` + - restart ROS receiver + - wait for unix socket + - start `b_side` +- If network checks fail repeatedly, watchdog stops `b_side`, runs `5g-dial.sh`, waits for route recovery, and then restores services. +- While 5G is healthy, watchdog keeps every host route listed by `BLITZ_TIME_SERVER_IP` and `BLITZ_5G_ROUTE_TARGETS` pinned to the resolved 5G interface. When 5G becomes unhealthy, watchdog deletes those host routes so traffic can fall back to the remaining default network path. If that fallback path is still reachable, watchdog keeps `b_side_omnid` running instead of treating it as a full network outage. +- Whenever watchdog changes or restores those host routes, it logs `route-path` lines for each target so you can see which interface Linux currently chooses for `81.70.156.140`, `106.55.173.235`, and any other configured 5G-pinned target. +- If GPS monitoring is enabled, watchdog checks `BLITZ_GPS_DEVICE_GLOB` every `BLITZ_GPS_CHECK_INTERVAL_SEC` seconds. When the GPS serial device disappears and later reappears, watchdog restarts the units in `BLITZ_GPS_RESTART_UNITS` so `gpsd` can bind to the new device node again. +- Camera disappearance is logged as degraded state. Reappearance triggers a `b_side` restart after the device is stable. + +## Notes + +- `time-sync.sh` and `blitz-time-sync.service` are intentionally removed from the automatic boot path. +- `b_side_omnid` must already be built before boot-time startup. +- `bin/b_side_omnid` missing, ROS env missing, or modem script missing will all show up in `startup.log`. diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/blitz-5g-link-logger.sh b/robot/v4l2/OmniSocketGo_robot/scripts/boot/blitz-5g-link-logger.sh new file mode 100644 index 0000000..bfcdfcd --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/blitz-5g-link-logger.sh @@ -0,0 +1,137 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="5g-link-logger" + +resolve_target_ip() { + if [[ -n "${BLITZ_TIME_SERVER_IP:-}" ]]; then + printf '%s\n' "${BLITZ_TIME_SERVER_IP}" + return 0 + fi + + for candidate in ${BLITZ_5G_ROUTE_TARGETS//,/ }; do + if [[ -n "${candidate}" ]]; then + printf '%s\n' "${candidate}" + return 0 + fi + done + return 1 +} + +emit_sample_json() { + local interface_name="${1:-}" + local target_ip="${2:-}" + + python3 - "${interface_name}" "${target_ip}" <<'PY' +import json +import subprocess +import sys +import time + +interface_name = sys.argv[1] +target_ip = sys.argv[2] + +payload = { + "ts_unix_ms": time.time_ns() // 1_000_000, + "interface": interface_name, + "target_ip": target_ip, + "link_present": False, + "route_output": "", + "route_ok": False, + "probe_ok": False, + "ping_rtt_ms": None, + "rx_bytes": 0, + "tx_bytes": 0, + "rx_packets": 0, + "tx_packets": 0, + "rx_errors": 0, + "tx_errors": 0, + "rx_drops": 0, + "tx_drops": 0, +} + +if interface_name: + try: + output = subprocess.check_output( + ["ip", "-j", "-s", "link", "show", "dev", interface_name], + text=True, + stderr=subprocess.DEVNULL, + ) + stats = json.loads(output) + if stats: + item = stats[0] + payload["link_present"] = True + rx = item.get("stats64", {}).get("rx", {}) + tx = item.get("stats64", {}).get("tx", {}) + if not rx and not tx: + rx = item.get("stats", {}).get("rx", {}) + tx = item.get("stats", {}).get("tx", {}) + payload["rx_bytes"] = int(rx.get("bytes") or 0) + payload["tx_bytes"] = int(tx.get("bytes") or 0) + payload["rx_packets"] = int(rx.get("packets") or 0) + payload["tx_packets"] = int(tx.get("packets") or 0) + payload["rx_errors"] = int(rx.get("errors") or 0) + payload["tx_errors"] = int(tx.get("errors") or 0) + payload["rx_drops"] = int(rx.get("dropped") or 0) + payload["tx_drops"] = int(tx.get("dropped") or 0) + except Exception: + pass + +if target_ip: + try: + route = subprocess.check_output( + ["ip", "route", "get", target_ip], + text=True, + stderr=subprocess.STDOUT, + ).strip() + payload["route_output"] = route.splitlines()[0] if route else "" + payload["route_ok"] = bool(payload["route_output"]) and ( + not interface_name or f" dev {interface_name}" in payload["route_output"] + ) + except Exception as exc: + payload["route_output"] = str(exc) + + ping_cmd = ["ping", "-c", "1", "-W", "2", target_ip] + if interface_name: + ping_cmd[1:1] = ["-I", interface_name] + ping = subprocess.run(ping_cmd, capture_output=True, text=True) + payload["probe_ok"] = ping.returncode == 0 + output = (ping.stdout or "") + "\n" + (ping.stderr or "") + for token in output.replace("\n", " ").split(): + if token.startswith("time="): + value = token.split("=", 1)[1].rstrip("ms") + try: + payload["ping_rtt_ms"] = float(value) + except ValueError: + pass + break + +print(json.dumps(payload, separators=(",", ":"), ensure_ascii=False)) +PY +} + +if [[ "${OMNI_BOOT_MODE:-0}" == "1" ]]; then + blitz_load_boot_env + blitz_require_run_context +fi + +if [[ -z "${BLITZ_RUN_DIR:-}" && -f "${BLITZ_RUN_CONTEXT_FILE:-}" ]]; then + blitz_load_run_context_env || true +fi +blitz_ensure_instance_id + +export BLITZ_5G_LINK_LOG_PATH="${BLITZ_5G_LINK_LOG_PATH:-${BLITZ_RUN_DIR}/b-5g-link-quality.${BLITZ_INSTANCE_ID}.jsonl}" +target_ip="$(resolve_target_ip || true)" + +blitz_log "${STEP}" "start" "start" "path=${BLITZ_5G_LINK_LOG_PATH} interval_sec=${BLITZ_5G_LINK_LOG_INTERVAL_SEC}" 0 + +while true; do + interface_name="$(blitz_resolve_5g_interface || true)" + line="$(emit_sample_json "${interface_name}" "${target_ip}")" + blitz_jsonl_append_line "${BLITZ_5G_LINK_LOG_PATH}" "${line}" + sleep "${BLITZ_5G_LINK_LOG_INTERVAL_SEC}" +done diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/blitz-fault-inject.sh b/robot/v4l2/OmniSocketGo_robot/scripts/boot/blitz-fault-inject.sh new file mode 100644 index 0000000..8ec1b2f --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/blitz-fault-inject.sh @@ -0,0 +1,139 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="fault-inject" +B_SIDE_SERVICE="blitz-b-side-omnid.service" +ROS_SERVICE="blitz-ros-receiver.service" + +main_pid_for_service() { + local service_name="$1" + systemctl show --property MainPID --value "${service_name}" +} + +wait_for_service_pid_change() { + local service_name="$1" + local previous_pid="$2" + local timeout_sec="${3:-10}" + local waited=0 + local current_pid="" + + while (( waited < timeout_sec )); do + current_pid="$(main_pid_for_service "${service_name}")" + if [[ -n "${current_pid}" && "${current_pid}" != "0" && "${current_pid}" != "${previous_pid}" ]]; then + printf '%s\n' "${current_pid}" + return 0 + fi + sleep 1 + waited=$(( waited + 1 )) + done + + return 1 +} + +require_running_pid() { + local service_name="$1" + local pid + + pid="$(main_pid_for_service "${service_name}")" + if [[ -z "${pid}" || "${pid}" == "0" ]]; then + blitz_log "${STEP}" "lookup-pid" "failure" "service=${service_name}" 1 + exit 1 + fi + printf '%s\n' "${pid}" +} + +write_fault_flag() { + local flag_name="$1" + local flag_path="${BLITZ_RUNTIME_DIR}/${flag_name}" + printf '%s\n' "$(date +%s)" > "${flag_path}" + blitz_log "${STEP}" "flag-on" "success" "path=${flag_path}" 0 +} + +clear_fault_flag() { + local flag_name="$1" + local flag_path="${BLITZ_RUNTIME_DIR}/${flag_name}" + rm -f "${flag_path}" + blitz_log "${STEP}" "flag-off" "success" "path=${flag_path}" 0 +} + +blitz_load_boot_env +blitz_require_root "${STEP}" +blitz_prepare_runtime_dir + +case "${1:-}" in + bside-crash) + target_pid="$(require_running_pid "${B_SIDE_SERVICE}")" + blitz_log "${STEP}" "bside-crash" "start" "service=${B_SIDE_SERVICE} pid=${target_pid}" 0 + kill -9 "${target_pid}" + if restarted_pid="$(wait_for_service_pid_change "${B_SIDE_SERVICE}" "${target_pid}")"; then + blitz_log "${STEP}" "bside-crash" "success" "old_pid=${target_pid} new_pid=${restarted_pid}" 0 + else + blitz_log "${STEP}" "bside-crash" "failure" "old_pid=${target_pid} restart_not_observed_within=10s" 1 + exit 1 + fi + ;; + bside-process-freeze) + target_pid="$(require_running_pid "${B_SIDE_SERVICE}")" + blitz_log "${STEP}" "bside-process-freeze" "start" "service=${B_SIDE_SERVICE} pid=${target_pid}" 0 + kill -STOP "${target_pid}" + blitz_log "${STEP}" "bside-process-freeze" "success" "service=${B_SIDE_SERVICE} pid=${target_pid}" 0 + ;; + bside-video-thread-stall) + write_fault_flag "fault-injection-bside-video-thread-stall" + ;; + bside-control-thread-stall) + write_fault_flag "fault-injection-bside-control-thread-stall" + ;; + ros-crash) + target_pid="$(require_running_pid "${ROS_SERVICE}")" + blitz_log "${STEP}" "ros-crash" "start" "service=${ROS_SERVICE} pid=${target_pid}" 0 + kill -9 "${target_pid}" + if restarted_pid="$(wait_for_service_pid_change "${ROS_SERVICE}" "${target_pid}")"; then + blitz_log "${STEP}" "ros-crash" "success" "old_pid=${target_pid} new_pid=${restarted_pid}" 0 + else + blitz_log "${STEP}" "ros-crash" "failure" "old_pid=${target_pid} restart_not_observed_within=10s" 1 + exit 1 + fi + ;; + ros-freeze) + target_pid="$(require_running_pid "${ROS_SERVICE}")" + blitz_log "${STEP}" "ros-freeze" "start" "service=${ROS_SERVICE} pid=${target_pid}" 0 + kill -STOP "${target_pid}" + blitz_log "${STEP}" "ros-freeze" "success" "service=${ROS_SERVICE} pid=${target_pid}" 0 + ;; + network-down) + if [[ "${BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION}" != "1" ]]; then + blitz_log "${STEP}" "network-down" "failure" "set BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION=1 first" 1 + exit 1 + fi + case "${2:-}" in + on) + write_fault_flag "fault-injection-network-down" + ;; + off) + clear_fault_flag "fault-injection-network-down" + ;; + *) + echo "usage: $0 network-down on|off" >&2 + exit 2 + ;; + esac + ;; + *) + cat <<'EOF' +usage: + blitz-fault-inject.sh bside-crash + blitz-fault-inject.sh bside-process-freeze + blitz-fault-inject.sh bside-video-thread-stall + blitz-fault-inject.sh bside-control-thread-stall + blitz-fault-inject.sh ros-crash + blitz-fault-inject.sh ros-freeze + blitz-fault-inject.sh network-down on|off +EOF + exit 2 + ;; +esac diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/blitz-incident-capture-launch.sh b/robot/v4l2/OmniSocketGo_robot/scripts/boot/blitz-incident-capture-launch.sh new file mode 100644 index 0000000..0bd788b --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/blitz-incident-capture-launch.sh @@ -0,0 +1,50 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="incident-launch" +incident_id="" +args=() +timeout_bin="" + +while (($# > 0)); do + case "$1" in + --incident-id) + incident_id="${2:-}" + shift 2 + ;; + *) + args+=("$1") + shift + ;; + esac +done + +blitz_load_boot_env +blitz_require_root "${STEP}" +blitz_require_command systemd-run "${STEP}" +blitz_require_command timeout "${STEP}" +timeout_bin="$(command -v timeout)" + +if [[ -z "${incident_id}" ]]; then + incident_id="$(blitz_new_incident_id)" +fi + +unit_name="blitz-incident-${incident_id//[^A-Za-z0-9_.-]/-}" + +systemd-run \ + --quiet \ + --collect \ + --unit "${unit_name}" \ + --property=Type=oneshot \ + --property="StandardOutput=append:${BLITZ_LOG_FILE}" \ + --property="StandardError=append:${BLITZ_LOG_FILE}" \ + "${timeout_bin}" "${BLITZ_INCIDENT_TOTAL_TIMEOUT_SEC}s" \ + /bin/bash "${SCRIPT_DIR}/blitz-incident-capture.sh" \ + --incident-id "${incident_id}" \ + "${args[@]}" + +printf '%s\n' "${incident_id}" diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/blitz-incident-capture.sh b/robot/v4l2/OmniSocketGo_robot/scripts/boot/blitz-incident-capture.sh new file mode 100644 index 0000000..c6bcdfd --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/blitz-incident-capture.sh @@ -0,0 +1,131 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="incident-capture" +incident_id="" +incident_source="" +incident_reason="" +incident_unit="" +incident_result="" +incident_exit_status="" + +run_capture() { + local output_path="$1" + shift + + if command -v timeout >/dev/null 2>&1; then + timeout "${BLITZ_INCIDENT_COMMAND_TIMEOUT_SEC}s" "$@" > "${output_path}" 2>&1 || true + else + "$@" > "${output_path}" 2>&1 || true + fi +} + +while (($# > 0)); do + case "$1" in + --incident-id) + incident_id="${2:-}" + shift 2 + ;; + --source) + incident_source="${2:-}" + shift 2 + ;; + --reason) + incident_reason="${2:-}" + shift 2 + ;; + --unit) + incident_unit="${2:-}" + shift 2 + ;; + --result) + incident_result="${2:-}" + shift 2 + ;; + --exit-status) + incident_exit_status="${2:-}" + shift 2 + ;; + *) + blitz_log "${STEP}" "parse-arg" "failure" "unknown argument: $1" 2 + exit 2 + ;; + esac +done + +if [[ -n "${incident_result}" && "${incident_result}" == "success" ]]; then + exit 0 +fi + +blitz_load_boot_env +blitz_load_run_context_env || true +blitz_prepare_runtime_dir +blitz_prepare_run_root + +if [[ -z "${incident_id}" ]]; then + incident_id="$(blitz_new_incident_id)" +fi + +incident_dir="${BLITZ_RUN_ROOT}/incidents/${incident_id}" +mkdir -p "${incident_dir}" + +python3 - "${incident_dir}/incident.json" "${incident_id}" "${BLITZ_RUN_ID:-}" "${incident_source}" "${incident_reason}" "${incident_unit}" "${incident_result}" "${incident_exit_status}" "${BLITZ_RUN_DIR:-}" "${HOSTNAME:-$(hostname)}" <<'PY' +import json +import sys +import time + +path, incident_id, run_id, source, reason, unit, result, exit_status, run_dir, hostname = sys.argv[1:10] +payload = { + "incident_id": incident_id, + "run_id": run_id, + "source": source, + "fault_reason": reason, + "unit": unit, + "service_result": result, + "exit_status": exit_status, + "run_dir": run_dir, + "hostname": hostname, + "captured_at": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()), +} +with open(path, "w", encoding="utf-8") as handle: + json.dump(payload, handle, ensure_ascii=False, indent=2, sort_keys=True) +PY + +for status_file in \ + "${BLITZ_RUNTIME_DIR}/watchdog.status.json" \ + "${BLITZ_RUNTIME_DIR}/b-side-omnid.status.json" \ + "${BLITZ_RUNTIME_DIR}/ros-receiver.status.json" +do + if [[ -f "${status_file}" ]]; then + cp -f "${status_file}" "${incident_dir}/$(basename "${status_file}")" + fi +done + +if [[ -f "${BLITZ_LOG_FILE}" ]]; then + tail -n 400 "${BLITZ_LOG_FILE}" > "${incident_dir}/startup.log.tail" +fi + +run_capture "${incident_dir}/systemctl-status.txt" \ + systemctl status blitz-robot.target blitz-run-context.service blitz-5g-dial.service blitz-5g-link-logger.service blitz-ros-receiver.service blitz-b-side-omnid.service blitz-watchdog.service +run_capture "${incident_dir}/journal.txt" \ + journalctl --no-pager --since "5 minutes ago" -u blitz-run-context.service -u blitz-5g-dial.service -u blitz-5g-link-logger.service -u blitz-ros-receiver.service -u blitz-b-side-omnid.service -u blitz-watchdog.service +run_capture "${incident_dir}/ip-addr.txt" ip addr +run_capture "${incident_dir}/ip-route.txt" ip route +run_capture "${incident_dir}/ss-uapn.txt" ss -uapn +run_capture "${incident_dir}/ss-xlp.txt" ss -xlp + +if [[ -f "${BLITZ_5G_INFO_JSON:-}" ]]; then + cp -f "${BLITZ_5G_INFO_JSON}" "${incident_dir}/$(basename "${BLITZ_5G_INFO_JSON}")" +fi + +if [[ -n "${BLITZ_RUN_DIR:-}" && -d "${BLITZ_RUN_DIR}" ]]; then + while IFS= read -r -d '' jsonl; do + tail -n 200 "${jsonl}" > "${incident_dir}/tail-$(basename "${jsonl}")" + done < <(find "${BLITZ_RUN_DIR}" -maxdepth 1 -type f -name '*.jsonl' -print0 2>/dev/null) +fi + +blitz_log "${STEP}" "complete" "success" "incident_id=${incident_id} path=${incident_dir}" 0 diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/blitz-run-context.sh b/robot/v4l2/OmniSocketGo_robot/scripts/boot/blitz-run-context.sh new file mode 100644 index 0000000..b159722 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/blitz-run-context.sh @@ -0,0 +1,22 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="run-context" + +on_error() { + local rc="$?" + blitz_log "${STEP}" "error" "failure" "line=${1:-unknown} cmd=${BASH_COMMAND:-unknown}" "${rc}" + exit "${rc}" +} + +trap 'on_error "${LINENO}"' ERR + +blitz_load_boot_env +blitz_require_root "${STEP}" +blitz_require_command python3 "${STEP}" +blitz_init_run_context +blitz_log "${STEP}" "complete" "success" "run_id=${BLITZ_RUN_ID} run_dir=${BLITZ_RUN_DIR}" 0 diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/blitz-watchdog.sh b/robot/v4l2/OmniSocketGo_robot/scripts/boot/blitz-watchdog.sh new file mode 100644 index 0000000..758d521 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/blitz-watchdog.sh @@ -0,0 +1,971 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="watchdog" +B_SIDE_SERVICE="blitz-b-side-omnid.service" +ROS_SERVICE="blitz-ros-receiver.service" +B_SIDE_STATUS_FILE="" +ROS_STATUS_FILE="" +WATCHDOG_STATUS_FILE="" +NETWORK_FAULT_FILE="" +WATCHDOG_EVENT_LOG="" +WATCHDOG_SAMPLE_LOG="" +WATCHDOG_EVENT_LOG_FAILURE_REPORTED=0 +WATCHDOG_SAMPLE_LOG_FAILURE_REPORTED=0 +CAMERA_MISSING_PREV=0 +CAMERA_RECOVERY_STABLE_COUNT=0 +NETWORK_FAIL_COUNT=0 +NETWORK_COOLDOWN_UNTIL=0 +BACKOFF_UNTIL=0 +LAST_ACTION="none" +LAST_ACTION_EPOCH_MS=0 +FULL_RESTART_WINDOW_START=0 +FULL_RESTART_WINDOW_COUNT=0 +NETWORK_LAST_INTERFACE="" +NETWORK_ROUTE_INTERFACE_LAST_KNOWN="" +NETWORK_PRIMARY_LAST_RETRY_SEC=0 +GPS_LAST_CHECK_SEC=0 +GPS_DEVICE_PRESENT_PREV=-1 +GPS_DEVICE_PRESENT_STATE=1 +GPS_STACK_ACTIVE_STATE=1 +LAST_REPORTED_FAULT_REASON="" +LAST_REPORTED_RECOVERY_STATE="" +declare -A TARGETED_RESTART_WINDOW_START=() +declare -A TARGETED_RESTART_WINDOW_COUNT=() + +now_epoch_sec() { + date +%s +} + +now_epoch_ms() { + date +%s%3N +} + +service_is_active() { + systemctl is-active --quiet "$1" +} + +gps_monitor_enabled() { + [[ "${BLITZ_GPS_MONITOR_ENABLED:-0}" == "1" ]] +} + +gps_stack_active() { + local units=() + local unit + + read -r -a units <<< "${BLITZ_GPS_RESTART_UNITS:-}" + if (( ${#units[@]} == 0 )); then + return 1 + fi + + for unit in "${units[@]}"; do + if service_is_active "${unit}"; then + return 0 + fi + done + return 1 +} + +restart_gps_stack() { + local reason="$1" + local devices="$2" + local units=() + local rc + + read -r -a units <<< "${BLITZ_GPS_RESTART_UNITS:-}" + if (( ${#units[@]} == 0 )); then + GPS_STACK_ACTIVE_STATE=0 + blitz_log "${STEP}" "gps-reconnect" "failure" "reason=${reason} devices=${devices} units=empty" 1 + return 1 + fi + + set_last_action "gps-reconnect" + blitz_log "${STEP}" "gps-reconnect" "start" "reason=${reason} devices=${devices} units=${BLITZ_GPS_RESTART_UNITS}" 0 + if systemctl restart "${units[@]}"; then + GPS_STACK_ACTIVE_STATE=1 + blitz_log "${STEP}" "gps-reconnect" "success" "reason=${reason} devices=${devices} units=${BLITZ_GPS_RESTART_UNITS}" 0 + return 0 + fi + + rc=$? + GPS_STACK_ACTIVE_STATE=0 + blitz_log "${STEP}" "gps-reconnect" "failure" "reason=${reason} devices=${devices} units=${BLITZ_GPS_RESTART_UNITS}" "${rc}" + return "${rc}" +} + +check_gps_health() { + local now_sec="$1" + local check_interval_sec="${BLITZ_GPS_CHECK_INTERVAL_SEC:-10}" + local device_glob="${BLITZ_GPS_DEVICE_GLOB:-}" + local previous_present="${GPS_DEVICE_PRESENT_PREV}" + local recovery_reason="" + local device_summary="" + local -a devices=() + + if ! gps_monitor_enabled; then + GPS_DEVICE_PRESENT_STATE=1 + GPS_STACK_ACTIVE_STATE=1 + return 0 + fi + + if (( check_interval_sec < 1 )); then + check_interval_sec=1 + fi + if (( GPS_LAST_CHECK_SEC != 0 && now_sec - GPS_LAST_CHECK_SEC < check_interval_sec )); then + if (( GPS_DEVICE_PRESENT_STATE == 1 && GPS_STACK_ACTIVE_STATE == 1 )); then + return 0 + fi + return 1 + fi + GPS_LAST_CHECK_SEC="${now_sec}" + + mapfile -t devices < <(compgen -G "${device_glob}" || true) + if (( ${#devices[@]} == 0 )); then + GPS_DEVICE_PRESENT_STATE=0 + GPS_STACK_ACTIVE_STATE=0 + if (( previous_present != 0 )); then + blitz_log "${STEP}" "gps-device-check" "failure" "state=missing glob=${device_glob}" 1 + fi + GPS_DEVICE_PRESENT_PREV=0 + return 1 + fi + + device_summary="$(IFS=,; printf '%s' "${devices[*]}")" + GPS_DEVICE_PRESENT_STATE=1 + GPS_DEVICE_PRESENT_PREV=1 + + if (( previous_present == 0 )); then + blitz_log "${STEP}" "gps-device-check" "success" "state=reappeared devices=${device_summary}" 0 + recovery_reason="device-reappeared" + elif ! gps_stack_active; then + recovery_reason="gpsd-inactive" + fi + + if [[ -n "${recovery_reason}" ]]; then + if restart_gps_stack "${recovery_reason}" "${device_summary}"; then + return 0 + fi + return 1 + fi + + GPS_STACK_ACTIVE_STATE=1 + return 0 +} + +status_file_fresh() { + local path="$1" + local max_age_sec="$2" + local now_sec + local mtime_sec + + if [[ ! -f "${path}" ]]; then + return 1 + fi + now_sec="$(now_epoch_sec)" + mtime_sec="$(stat -c %Y "${path}" 2>/dev/null || echo 0)" + (( now_sec - mtime_sec <= max_age_sec )) +} + +ros_receiver_status_fresh() { + local path="$1" + local max_age_sec="$2" + local now_epoch_ms_value + + now_epoch_ms_value="$(now_epoch_ms)" + python3 - "${path}" "${now_epoch_ms_value}" "${max_age_sec}" <<'PY' +import json +import sys + +path = sys.argv[1] +now_epoch_ms = int(sys.argv[2]) +max_age_ms = int(sys.argv[3]) * 1000 + +try: + with open(path, "r", encoding="utf-8") as handle: + payload = json.load(handle) +except Exception: + raise SystemExit(1) + +heartbeat_ms = int(payload.get("recv_thread_heartbeat_epoch_ms") or 0) +socket_bound = bool(payload.get("socket_bound")) + +if heartbeat_ms <= 0 or not socket_bound: + raise SystemExit(1) + +raise SystemExit(0 if now_epoch_ms - heartbeat_ms <= max_age_ms else 1) +PY +} + +ros_receiver_healthy() { + local max_age_sec="$1" + + service_is_active "${ROS_SERVICE}" \ + && [[ -S "${ROBOT_RECEIVER_LOCAL_SOCKET_PATH}" ]] \ + && status_file_fresh "${ROS_STATUS_FILE}" "${max_age_sec}" \ + && ros_receiver_status_fresh "${ROS_STATUS_FILE}" "${max_age_sec}" +} + +write_watchdog_status() { + local fault_reason="$1" + local recovery_state="$2" + local network_ok="$3" + local camera_ok="$4" + local ros_ok="$5" + local bside_ok="$6" + local gps_ok="$7" + local gps_device_present="$8" + local tmp_file + + tmp_file="${WATCHDOG_STATUS_FILE}.tmp.$$" + cat > "${tmp_file}" <&1)"; then + if (( WATCHDOG_EVENT_LOG_FAILURE_REPORTED == 0 )); then + blitz_log "${STEP}" "watchdog-event-log" "failure" "path=${WATCHDOG_EVENT_LOG} detail=${line}" 0 || true + WATCHDOG_EVENT_LOG_FAILURE_REPORTED=1 + fi + return 0 + fi + if ! blitz_jsonl_append_line "${WATCHDOG_EVENT_LOG}" "${line}"; then + if (( WATCHDOG_EVENT_LOG_FAILURE_REPORTED == 0 )); then + blitz_log "${STEP}" "watchdog-event-log" "failure" "path=${WATCHDOG_EVENT_LOG} detail=append-failed" 0 || true + WATCHDOG_EVENT_LOG_FAILURE_REPORTED=1 + fi + return 0 + fi + WATCHDOG_EVENT_LOG_FAILURE_REPORTED=0 +} + +watchdog_append_sample() { + local line="" + + [[ -n "${WATCHDOG_SAMPLE_LOG}" ]] || return 0 + if ! line="$(watchdog_emit_json "$@" 2>&1)"; then + if (( WATCHDOG_SAMPLE_LOG_FAILURE_REPORTED == 0 )); then + blitz_log "${STEP}" "watchdog-sample-log" "failure" "path=${WATCHDOG_SAMPLE_LOG} detail=${line}" 0 || true + WATCHDOG_SAMPLE_LOG_FAILURE_REPORTED=1 + fi + return 0 + fi + if ! blitz_jsonl_append_line "${WATCHDOG_SAMPLE_LOG}" "${line}"; then + if (( WATCHDOG_SAMPLE_LOG_FAILURE_REPORTED == 0 )); then + blitz_log "${STEP}" "watchdog-sample-log" "failure" "path=${WATCHDOG_SAMPLE_LOG} detail=append-failed" 0 || true + WATCHDOG_SAMPLE_LOG_FAILURE_REPORTED=1 + fi + return 0 + fi + WATCHDOG_SAMPLE_LOG_FAILURE_REPORTED=0 +} + +watchdog_record_state_transition() { + local fault_reason="$1" + local recovery_state="$2" + + if [[ "${fault_reason}" == "${LAST_REPORTED_FAULT_REASON}" && "${recovery_state}" == "${LAST_REPORTED_RECOVERY_STATE}" ]]; then + return 0 + fi + watchdog_append_event "event" "state-transition" "${fault_reason}" "${recovery_state}" "" "" + LAST_REPORTED_FAULT_REASON="${fault_reason}" + LAST_REPORTED_RECOVERY_STATE="${recovery_state}" +} + +watchdog_launch_incident() { + local reason="$1" + local unit_name="$2" + + blitz_launch_incident_capture \ + --source watchdog \ + --reason "${reason}" \ + --unit "${unit_name}" \ + --result failure \ + --exit-status 1 2>/dev/null || true +} + +set_last_action() { + LAST_ACTION="$1" + LAST_ACTION_EPOCH_MS="$(now_epoch_ms)" +} + +targeted_restart_total() { + local total=0 + local key + + for key in "${!TARGETED_RESTART_WINDOW_COUNT[@]}"; do + total=$(( total + TARGETED_RESTART_WINDOW_COUNT["${key}"] )) + done + printf '%s\n' "${total}" +} + +register_targeted_restart() { + local fault_key="$1" + local now_sec + local window_start + local count + + now_sec="$(now_epoch_sec)" + window_start="${TARGETED_RESTART_WINDOW_START["${fault_key}"]:-0}" + count="${TARGETED_RESTART_WINDOW_COUNT["${fault_key}"]:-0}" + if (( window_start == 0 || now_sec - window_start > 60 )); then + window_start="${now_sec}" + count=1 + else + count=$(( count + 1 )) + fi + TARGETED_RESTART_WINDOW_START["${fault_key}"]="${window_start}" + TARGETED_RESTART_WINDOW_COUNT["${fault_key}"]="${count}" + (( count >= 2 )) +} + +record_full_restart() { + local now_sec + + now_sec="$(now_epoch_sec)" + if (( FULL_RESTART_WINDOW_START == 0 || now_sec - FULL_RESTART_WINDOW_START > 600 )); then + FULL_RESTART_WINDOW_START="${now_sec}" + FULL_RESTART_WINDOW_COUNT=1 + else + FULL_RESTART_WINDOW_COUNT=$(( FULL_RESTART_WINDOW_COUNT + 1 )) + fi + if (( FULL_RESTART_WINDOW_COUNT >= 3 )); then + BACKOFF_UNTIL=$(( now_sec + 60 )) + watchdog_append_event "event" "backoff-enter" "backoff" "backoff" "full_restart_count=${FULL_RESTART_WINDOW_COUNT}" "" + fi +} + +restart_bside_targeted() { + local fault_key="$1" + local reason="$2" + local rc + local incident_id="" + + if register_targeted_restart "${fault_key}"; then + blitz_log "${STEP}" "escalate-full-restart" "start" "reason=${reason}" 0 + watchdog_append_event "event" "escalate-full-restart" "${reason}-escalated" "recovering" "fault_key=${fault_key}" "" + full_restart_stack "${reason}-escalated" + return 0 + fi + + incident_id="$(watchdog_launch_incident "${reason}" "${B_SIDE_SERVICE}")" + set_last_action "restart-bside" + RECOVERY_ACTION_TAKEN=1 + blitz_log "${STEP}" "restart-bside" "start" "reason=${reason}" 0 + watchdog_append_event "event" "restart-bside-start" "${reason}" "recovering" "fault_key=${fault_key}" "${incident_id}" + if systemctl restart "${B_SIDE_SERVICE}"; then + blitz_log "${STEP}" "restart-bside" "success" "reason=${reason}" 0 + watchdog_append_event "event" "restart-bside-success" "${reason}" "recovering" "fault_key=${fault_key}" "${incident_id}" + return 0 + fi + + rc=$? + blitz_log "${STEP}" "restart-bside" "failure" "reason=${reason}" "${rc}" + watchdog_append_event "event" "restart-bside-failure" "${reason}" "recovering" "fault_key=${fault_key} rc=${rc}" "${incident_id}" + return "${rc}" +} + +full_restart_stack() { + local reason="$1" + local rc + local incident_id="" + + incident_id="$(watchdog_launch_incident "${reason}" "blitz-robot.target")" + set_last_action "full-restart" + RECOVERY_ACTION_TAKEN=1 + recovery_state="recovering" + fault_reason="${reason}" + + blitz_log "${STEP}" "full-restart-stop-bside" "start" "reason=${reason}" 0 + watchdog_append_event "event" "full-restart-start" "${reason}" "recovering" "" "${incident_id}" + systemctl stop "${B_SIDE_SERVICE}" || true + + if systemctl restart "${ROS_SERVICE}"; then + blitz_log "${STEP}" "full-restart-restart-ros" "success" "reason=${reason}" 0 + else + rc=$? + blitz_log "${STEP}" "full-restart-restart-ros" "failure" "reason=${reason}" "${rc}" + record_full_restart + return "${rc}" + fi + + if bash "${BOOT_SCRIPT_DIR}/wait-for-unix-socket.sh" --step "${STEP}" --timeout "${BLITZ_ROS_SOCKET_WAIT_SEC}"; then + : + else + rc=$? + blitz_log "${STEP}" "full-restart-wait-socket" "failure" "reason=${reason}" "${rc}" + record_full_restart + return "${rc}" + fi + + if systemctl start "${B_SIDE_SERVICE}"; then + blitz_log "${STEP}" "full-restart-start-bside" "success" "reason=${reason}" 0 + else + rc=$? + blitz_log "${STEP}" "full-restart-start-bside" "failure" "reason=${reason}" "${rc}" + watchdog_append_event "event" "full-restart-failure" "${reason}" "recovering" "stage=start-bside rc=${rc}" "${incident_id}" + record_full_restart + return "${rc}" + fi + watchdog_append_event "event" "full-restart-success" "${reason}" "recovering" "" "${incident_id}" + record_full_restart +} + +network_fault_injected() { + [[ "${BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION}" == "1" && -f "${NETWORK_FAULT_FILE}" ]] +} + +resolve_network_interface() { + NETWORK_LAST_INTERFACE="$(blitz_resolve_5g_interface || true)" + if [[ -n "${NETWORK_LAST_INTERFACE}" ]]; then + NETWORK_ROUTE_INTERFACE_LAST_KNOWN="${NETWORK_LAST_INTERFACE}" + return 0 + fi + return 1 +} + +network_route_targets() { + local target + + if [[ -n "${BLITZ_TIME_SERVER_IP:-}" ]]; then + printf '%s\n' "${BLITZ_TIME_SERVER_IP}" + fi + for target in ${BLITZ_5G_ROUTE_TARGETS//,/ }; do + if [[ -n "${target}" && "${target}" != "${BLITZ_TIME_SERVER_IP:-}" ]]; then + printf '%s\n' "${target}" + fi + done +} + +log_target_route_paths() { + local action="$1" + local target + local route_output + + while IFS= read -r target; do + [[ -n "${target}" ]] || continue + route_output="$(ip route get "${target}" 2>&1 | head -n 1 || true)" + if [[ -z "${route_output}" ]]; then + route_output="unresolved" + fi + blitz_log "${STEP}" "route-path" "info" "action=${action} target=${target} route=${route_output}" 0 + done < <(network_route_targets) +} + +route_output_uses_interface() { + local route_output="$1" + local interface_name="$2" + + [[ -n "${interface_name}" ]] || return 1 + [[ "${route_output}" == *" dev ${interface_name} "* || "${route_output}" == *" dev ${interface_name}" ]] +} + +route_output_uses_gateway() { + local route_output="$1" + local gateway="$2" + + [[ -n "${gateway}" ]] || return 1 + [[ "${route_output}" == *"via ${gateway}"* ]] +} + +route_is_desired_target_route() { + local route_output="$1" + local interface_name="$2" + local gateway="$3" + + route_output_uses_interface "${route_output}" "${interface_name}" \ + && route_output_uses_gateway "${route_output}" "${gateway}" +} + +route_is_managed_5g_route() { + local route_output="$1" + local interface_name="${2:-}" + local gateway="${3:-}" + + if route_output_uses_interface "${route_output}" "${interface_name}"; then + return 0 + fi + if route_output_uses_gateway "${route_output}" "${gateway}"; then + return 0 + fi + if route_output_uses_gateway "${route_output}" "${BLITZ_5G_GATEWAY:-}"; then + return 0 + fi + return 1 +} + +resolve_route_cleanup_interface() { + local interface_name="" + local info_json="${BLITZ_5G_INFO_JSON:-}" + + if [[ -n "${NETWORK_LAST_INTERFACE}" ]]; then + printf '%s\n' "${NETWORK_LAST_INTERFACE}" + return 0 + fi + if [[ -n "${NETWORK_ROUTE_INTERFACE_LAST_KNOWN}" ]]; then + printf '%s\n' "${NETWORK_ROUTE_INTERFACE_LAST_KNOWN}" + return 0 + fi + + interface_name="$(blitz_read_5g_info_interface "${info_json}" || true)" + if [[ -n "${interface_name}" ]]; then + printf '%s\n' "${interface_name}" + return 0 + fi + return 1 +} + +resolve_network_gateway() { + local interface_name="$1" + local default_route + local gateway="" + local tokens=() + local index + + default_route="$(ip -o route show default dev "${interface_name}" 2>/dev/null | head -n 1 || true)" + if [[ -n "${default_route}" ]]; then + read -r -a tokens <<< "${default_route}" + for (( index=0; index<${#tokens[@]}-1; index++ )); do + if [[ "${tokens[index]}" == "via" ]]; then + gateway="${tokens[index + 1]}" + break + fi + done + fi + + if [[ -n "${gateway}" ]]; then + printf '%s\n' "${gateway}" + return 0 + fi + if [[ -n "${BLITZ_5G_GATEWAY:-}" ]]; then + printf '%s\n' "${BLITZ_5G_GATEWAY}" + return 0 + fi + return 1 +} + +sync_target_routes_to_5g() { + local interface_name="$1" + local gateway="${2:-}" + local route_output="" + local updated=0 + local target + local rc + + if [[ -z "${interface_name}" ]]; then + return 1 + fi + + if [[ -z "${gateway}" ]]; then + gateway="$(resolve_network_gateway "${interface_name}" || true)" + fi + if [[ -z "${gateway}" ]]; then + blitz_log "${STEP}" "route-sync-gateway" "failure" "interface=${interface_name}" 1 + return 1 + fi + + while IFS= read -r target; do + [[ -n "${target}" ]] || continue + route_output="$(ip route show "${target}/32" 2>/dev/null | head -n 1 || true)" + if [[ -n "${route_output}" ]] && route_is_desired_target_route "${route_output}" "${interface_name}" "${gateway}"; then + continue + fi + if ip route replace "${target}/32" via "${gateway}" dev "${interface_name}"; then + updated=1 + blitz_log "${STEP}" "route-sync-target" "success" "target=${target} interface=${interface_name} gateway=${gateway}" 0 + else + rc=$? + blitz_log "${STEP}" "route-sync-target" "failure" "target=${target} interface=${interface_name} gateway=${gateway}" "${rc}" + return "${rc}" + fi + done < <(network_route_targets) + + if (( updated == 1 )); then + NETWORK_ROUTE_INTERFACE_LAST_KNOWN="${interface_name}" + log_target_route_paths "sync-to-5g" + fi + return 0 +} + +clear_target_routes_from_5g() { + local interface_name="${1:-}" + local gateway="${2:-}" + local route_output="" + local target + local removed_any=0 + local rc + + if [[ -z "${interface_name}" ]]; then + interface_name="$(resolve_route_cleanup_interface || true)" + fi + if [[ -z "${gateway}" && -n "${interface_name}" ]]; then + gateway="$(resolve_network_gateway "${interface_name}" || true)" + fi + if [[ -z "${gateway}" ]]; then + gateway="${BLITZ_5G_GATEWAY:-}" + fi + + while IFS= read -r target; do + [[ -n "${target}" ]] || continue + route_output="$(ip route show "${target}/32" 2>/dev/null | head -n 1 || true)" + if [[ -z "${route_output}" ]] || ! route_is_managed_5g_route "${route_output}" "${interface_name}" "${gateway}"; then + continue + fi + if ip route del "${target}/32"; then + removed_any=1 + blitz_log "${STEP}" "route-clear-target" "success" "target=${target} interface=${interface_name:-unknown} gateway=${gateway:-unknown}" 0 + else + rc=$? + blitz_log "${STEP}" "route-clear-target" "failure" "target=${target} interface=${interface_name:-unknown} gateway=${gateway:-unknown}" "${rc}" + return "${rc}" + fi + done < <(network_route_targets) + + if (( removed_any == 1 )); then + blitz_log "${STEP}" "route-clear" "success" "interface=${interface_name:-unknown} gateway=${gateway:-unknown}" 0 + log_target_route_paths "clear-from-5g" + fi + return 0 +} + +repair_network_routes() { + local interface_name="$1" + local gateway="" + local route_output + + if [[ -z "${interface_name}" ]]; then + return 1 + fi + + gateway="$(resolve_network_gateway "${interface_name}" || true)" + if [[ -z "${gateway}" ]]; then + blitz_log "${STEP}" "route-repair-gateway" "failure" "interface=${interface_name}" 1 + return 1 + fi + + if ! sync_target_routes_to_5g "${interface_name}" "${gateway}"; then + clear_target_routes_from_5g "${interface_name}" "${gateway}" || true + return 1 + fi + + route_output="$(blitz_route_ready "${BLITZ_TIME_SERVER_IP}" "${interface_name}" || true)" + if [[ -z "${route_output}" ]]; then + clear_target_routes_from_5g "${interface_name}" "${gateway}" || true + blitz_log "${STEP}" "route-repair-postcheck" "failure" "interface=${interface_name} gateway=${gateway}" 1 + return 1 + fi + + if ! ping -I "${interface_name}" -c 1 -W 2 "${BLITZ_TIME_SERVER_IP}" >/dev/null 2>&1; then + clear_target_routes_from_5g "${interface_name}" "${gateway}" || true + blitz_log "${STEP}" "route-repair-probe" "failure" "interface=${interface_name} target=${BLITZ_TIME_SERVER_IP}" 1 + return 1 + fi + + blitz_log "${STEP}" "route-repair-postcheck" "success" "interface=${interface_name} gateway=${gateway} route=${route_output}" 0 + return 0 +} + +network_is_healthy() { + local route_output + + NETWORK_LAST_INTERFACE="" + if network_fault_injected; then + return 1 + fi + if ! resolve_network_interface; then + return 1 + fi + route_output="$(blitz_route_ready "${BLITZ_TIME_SERVER_IP}" "${NETWORK_LAST_INTERFACE}" || true)" + if [[ -z "${route_output}" ]]; then + return 1 + fi + ping -I "${NETWORK_LAST_INTERFACE}" -c 1 -W 2 "${BLITZ_TIME_SERVER_IP}" >/dev/null 2>&1 +} + +fallback_network_is_healthy() { + local route_output + + if [[ -z "${BLITZ_TIME_SERVER_IP:-}" ]]; then + return 1 + fi + + route_output="$(blitz_route_ready "${BLITZ_TIME_SERVER_IP}" || true)" + if [[ -z "${route_output}" ]]; then + return 1 + fi + + ping -c 1 -W 2 "${BLITZ_TIME_SERVER_IP}" >/dev/null 2>&1 +} + +wait_for_network_recovery() { + local timeout_sec="$1" + local waited=0 + + while (( waited < timeout_sec )); do + if network_is_healthy; then + blitz_log "${STEP}" "network-postcheck" "success" "interface=${NETWORK_LAST_INTERFACE} waited_sec=${waited}" 0 + return 0 + fi + if (( waited == 0 || waited % 5 == 0 )); then + blitz_log "${STEP}" "network-postcheck" "waiting" "interface=${NETWORK_LAST_INTERFACE:-unresolved} waited_sec=${waited}" 0 + fi + sleep 1 + waited=$(( waited + 1 )) + done + + blitz_log "${STEP}" "network-postcheck" "failure" "interface=${NETWORK_LAST_INTERFACE:-unresolved} timeout_sec=${timeout_sec}" 1 + return 1 +} + +perform_network_recovery() { + local rc=0 + local incident_id="" + + if resolve_network_interface && repair_network_routes "${NETWORK_LAST_INTERFACE}"; then + set_last_action "route-repair" + RECOVERY_ACTION_TAKEN=1 + NETWORK_COOLDOWN_UNTIL=$(( $(now_epoch_sec) + BLITZ_NETWORK_RECOVERY_COOLDOWN_SEC )) + NETWORK_FAIL_COUNT=0 + blitz_log "${STEP}" "network-recovery" "success" "mode=route-repair interface=${NETWORK_LAST_INTERFACE}" 0 + watchdog_append_event "event" "route-repair-success" "network_or_robot_unreachable" "recovering" "interface=${NETWORK_LAST_INTERFACE}" "" + return 0 + fi + + incident_id="$(watchdog_launch_incident "network-recovery" "blitz-5g-dial.service")" + set_last_action "network-recovery" + RECOVERY_ACTION_TAKEN=1 + blitz_log "${STEP}" "network-recovery" "start" "fail_count=${NETWORK_FAIL_COUNT}" 0 + watchdog_append_event "event" "network-recovery-start" "network_or_robot_unreachable" "recovering" "fail_count=${NETWORK_FAIL_COUNT}" "${incident_id}" + systemctl stop "${B_SIDE_SERVICE}" || true + + if bash "${BOOT_SCRIPT_DIR}/5g-dial.sh"; then + : + else + rc=$? + blitz_log "${STEP}" "network-redial" "failure" "fail_count=${NETWORK_FAIL_COUNT} script=${BOOT_SCRIPT_DIR}/5g-dial.sh" "${rc}" + watchdog_append_event "event" "network-recovery-failure" "network_or_robot_unreachable" "recovering" "stage=redial rc=${rc}" "${incident_id}" + return "${rc}" + fi + + if wait_for_network_recovery "${BLITZ_5G_ROUTE_WAIT_SEC}"; then + : + else + rc=$? + blitz_log "${STEP}" "network-recovery" "failure" "fail_count=${NETWORK_FAIL_COUNT} interface=${NETWORK_LAST_INTERFACE:-unresolved}" "${rc}" + watchdog_append_event "event" "network-recovery-failure" "network_or_robot_unreachable" "recovering" "stage=postcheck rc=${rc}" "${incident_id}" + return "${rc}" + fi + + NETWORK_COOLDOWN_UNTIL=$(( $(now_epoch_sec) + BLITZ_NETWORK_RECOVERY_COOLDOWN_SEC )) + NETWORK_FAIL_COUNT=0 + watchdog_append_event "event" "network-recovery-success" "network_or_robot_unreachable" "recovering" "interface=${NETWORK_LAST_INTERFACE:-unresolved}" "${incident_id}" + if ros_receiver_healthy "${BLITZ_HEALTH_STALE_SEC}"; then + restart_bside_targeted "network" "network-recovered" + return 0 + fi + full_restart_stack "network-recovered-ros-unhealthy" + return 0 +} + +blitz_load_boot_env +blitz_require_root "${STEP}" +blitz_require_command systemctl "${STEP}" +blitz_require_command stat "${STEP}" +blitz_require_command ping "${STEP}" +blitz_require_command python3 "${STEP}" +blitz_prepare_runtime_dir +blitz_require_run_context + +B_SIDE_STATUS_FILE="${BLITZ_RUNTIME_DIR}/b-side-omnid.status.json" +ROS_STATUS_FILE="${BLITZ_RUNTIME_DIR}/ros-receiver.status.json" +WATCHDOG_STATUS_FILE="${BLITZ_RUNTIME_DIR}/watchdog.status.json" +NETWORK_FAULT_FILE="${BLITZ_RUNTIME_DIR}/fault-injection-network-down" +WATCHDOG_EVENT_LOG="${BLITZ_RUN_DIR}/watchdog-events.jsonl" +WATCHDOG_SAMPLE_LOG="${BLITZ_RUN_DIR}/watchdog-samples.jsonl" + +while true; do + fault_reason="none" + recovery_state="ok" + network_ok=1 + camera_ok=1 + ros_ok=1 + bside_ok=1 + gps_ok=1 + gps_device_present=1 + RECOVERY_ACTION_TAKEN=0 + now_sec="$(now_epoch_sec)" + + if gps_monitor_enabled; then + gps_device_present="${GPS_DEVICE_PRESENT_STATE}" + if (( GPS_DEVICE_PRESENT_STATE == 0 || GPS_STACK_ACTIVE_STATE == 0 )); then + gps_ok=0 + fi + fi + + if (( BACKOFF_UNTIL > now_sec )); then + fault_reason="backoff" + recovery_state="backoff" + watchdog_record_state_transition "${fault_reason}" "${recovery_state}" + write_watchdog_status "${fault_reason}" "${recovery_state}" 0 0 0 0 "${gps_ok}" "${gps_device_present}" + watchdog_append_sample "sample" "loop" "${fault_reason}" "${recovery_state}" "" "" 0 0 0 0 "${gps_ok}" "${gps_device_present}" + sleep "${BLITZ_WATCHDOG_INTERVAL_SEC}" + continue + fi + + if (( NETWORK_COOLDOWN_UNTIL > now_sec )); then + recovery_state="recovering" + elif ! network_is_healthy; then + clear_target_routes_from_5g || true + if fallback_network_is_healthy; then + NETWORK_FAIL_COUNT=0 + fault_reason="network_fallback_active" + recovery_state="degraded" + blitz_log "${STEP}" "network-check" "fallback" "interface=${NETWORK_LAST_INTERFACE:-unresolved} target=${BLITZ_TIME_SERVER_IP}" 0 + if (( NETWORK_PRIMARY_LAST_RETRY_SEC == 0 || now_sec - NETWORK_PRIMARY_LAST_RETRY_SEC >= 10 )); then + NETWORK_PRIMARY_LAST_RETRY_SEC="${now_sec}" + if resolve_network_interface && repair_network_routes "${NETWORK_LAST_INTERFACE}"; then + NETWORK_PRIMARY_LAST_RETRY_SEC=0 + fault_reason="none" + recovery_state="ok" + blitz_log "${STEP}" "network-check" "primary-restored" "interface=${NETWORK_LAST_INTERFACE} target=${BLITZ_TIME_SERVER_IP}" 0 + log_target_route_paths "primary-restored" + fi + fi + else + network_ok=0 + NETWORK_FAIL_COUNT=$(( NETWORK_FAIL_COUNT + 1 )) + fault_reason="network_or_robot_unreachable" + recovery_state="recovering" + blitz_log "${STEP}" "network-check" "failure" "count=${NETWORK_FAIL_COUNT} interface=${NETWORK_LAST_INTERFACE:-unresolved}" 1 + if (( NETWORK_FAIL_COUNT >= BLITZ_NETWORK_FAIL_THRESHOLD )); then + perform_network_recovery || true + fi + fi + else + NETWORK_PRIMARY_LAST_RETRY_SEC=0 + NETWORK_FAIL_COUNT=0 + sync_target_routes_to_5g "${NETWORK_LAST_INTERFACE}" || true + fi + + if check_gps_health "${now_sec}"; then + gps_ok=1 + else + gps_ok=0 + gps_device_present="${GPS_DEVICE_PRESENT_STATE}" + if [[ "${fault_reason}" == "none" ]]; then + if (( GPS_DEVICE_PRESENT_STATE == 0 )); then + fault_reason="gps_device_missing" + else + fault_reason="gps_reconnect_failed" + fi + recovery_state="degraded" + fi + fi + gps_device_present="${GPS_DEVICE_PRESENT_STATE}" + + if [[ ! -e "${OMNI_CAMERA_DEVICE}" ]]; then + camera_ok=0 + fault_reason="camera_missing" + recovery_state="degraded" + CAMERA_MISSING_PREV=1 + CAMERA_RECOVERY_STABLE_COUNT=0 + elif (( RECOVERY_ACTION_TAKEN == 0 && CAMERA_MISSING_PREV == 1 )); then + CAMERA_RECOVERY_STABLE_COUNT=$(( CAMERA_RECOVERY_STABLE_COUNT + 1 )) + recovery_state="recovering" + fault_reason="camera_recovered" + if (( CAMERA_RECOVERY_STABLE_COUNT >= 2 )); then + restart_bside_targeted "camera" "camera-reappeared" || true + CAMERA_MISSING_PREV=0 + CAMERA_RECOVERY_STABLE_COUNT=0 + fi + else + CAMERA_RECOVERY_STABLE_COUNT=0 + fi + + if (( RECOVERY_ACTION_TAKEN == 0 )) && { ! service_is_active "${B_SIDE_SERVICE}" || ! status_file_fresh "${B_SIDE_STATUS_FILE}" "${BLITZ_HEALTH_STALE_SEC}"; }; then + bside_ok=0 + fault_reason="bside_status_stale" + recovery_state="recovering" + restart_bside_targeted "bside" "bside-unhealthy" || true + fi + + if (( RECOVERY_ACTION_TAKEN == 0 )) && ! ros_receiver_healthy "${BLITZ_HEALTH_STALE_SEC}"; then + ros_ok=0 + fault_reason="ros_receiver_unhealthy" + recovery_state="recovering" + full_restart_stack "ros-unhealthy" || true + fi + + watchdog_record_state_transition "${fault_reason}" "${recovery_state}" + write_watchdog_status "${fault_reason}" "${recovery_state}" "${network_ok}" "${camera_ok}" "${ros_ok}" "${bside_ok}" "${gps_ok}" "${gps_device_present}" + watchdog_append_sample "sample" "loop" "${fault_reason}" "${recovery_state}" "" "" "${network_ok}" "${camera_ok}" "${ros_ok}" "${bside_ok}" "${gps_ok}" "${gps_device_present}" + sleep "${BLITZ_WATCHDOG_INTERVAL_SEC}" +done diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/boot-gate.sh b/robot/v4l2/OmniSocketGo_robot/scripts/boot/boot-gate.sh new file mode 100644 index 0000000..ef22e0f --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/boot-gate.sh @@ -0,0 +1,15 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="boot-gate" + +blitz_load_boot_env + +blitz_log "${STEP}" "start" "start" "delay_sec=${BLITZ_BOOT_DELAY_SEC}" 0 +blitz_log "${STEP}" "delay" "start" "sleep ${BLITZ_BOOT_DELAY_SEC}s before starting Blitz services" 0 +sleep "${BLITZ_BOOT_DELAY_SEC}" +blitz_log "${STEP}" "delay" "success" "boot gate released after ${BLITZ_BOOT_DELAY_SEC}s" 0 diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/common.sh b/robot/v4l2/OmniSocketGo_robot/scripts/boot/common.sh new file mode 100644 index 0000000..61a2205 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/common.sh @@ -0,0 +1,661 @@ +#!/usr/bin/env bash +set -euo pipefail + +BOOT_SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +DEV_SCRIPT_DIR="$(cd "${BOOT_SCRIPT_DIR}/../dev" && pwd)" + +source_with_nounset_off() { + set +u + # shellcheck disable=SC1090 + source "$1" + set -u +} + +blitz_host_from_addr() { + local value="${1:-}" + + if [[ -z "${value}" ]]; then + return 1 + fi + if [[ "${value}" == \[*\]:* ]]; then + value="${value#\[}" + printf '%s\n' "${value%%]:*}" + return 0 + fi + printf '%s\n' "${value%%:*}" +} + +blitz_load_boot_env() { + local env_file + local default_time_server + local dev_run_root + local dev_runtime_dir + + if [[ "${BLITZ_BOOT_ENV_LOADED:-0}" == "1" ]]; then + return 0 + fi + + export BLITZ_BOOT_LOADING_ENV="1" + # shellcheck disable=SC1091 + source "${DEV_SCRIPT_DIR}/load-env.sh" + unset BLITZ_BOOT_LOADING_ENV + + for env_file in \ + "${BOOT_SCRIPT_DIR}/robot-boot.env" \ + "${BOOT_SCRIPT_DIR}/robot-boot.env.local" + do + if [[ -f "${env_file}" ]]; then + set -a + # shellcheck disable=SC1090 + source "${env_file}" + set +a + fi + done + + if declare -F normalize_loaded_env_vars >/dev/null 2>&1; then + normalize_loaded_env_vars + fi + + dev_run_root="${OMNISOCKETGO_ROOT}/logs" + dev_runtime_dir="${dev_run_root}/runtime" + + if [[ -z "${BLITZ_RUN_ROOT:-}" || "${BLITZ_RUN_ROOT}" == "${dev_run_root}" ]]; then + export BLITZ_RUN_ROOT="/var/log/blitz-robot" + fi + if [[ -z "${BLITZ_RUNTIME_DIR:-}" || "${BLITZ_RUNTIME_DIR}" == "${dev_runtime_dir}" ]]; then + export BLITZ_RUNTIME_DIR="/run/blitz-robot" + fi + if [[ -z "${BLITZ_RUN_CONTEXT_FILE:-}" || "${BLITZ_RUN_CONTEXT_FILE}" == "${dev_runtime_dir}/run-context.env" ]]; then + export BLITZ_RUN_CONTEXT_FILE="${BLITZ_RUNTIME_DIR}/run-context.env" + fi + if [[ -z "${BLITZ_RUN_ID_FILE:-}" || "${BLITZ_RUN_ID_FILE}" == "${dev_runtime_dir}/run-id" ]]; then + export BLITZ_RUN_ID_FILE="${BLITZ_RUNTIME_DIR}/run-id" + fi + if [[ -z "${BLITZ_CURRENT_RUN_LINK:-}" || "${BLITZ_CURRENT_RUN_LINK}" == "${dev_run_root}/current" ]]; then + export BLITZ_CURRENT_RUN_LINK="${BLITZ_RUN_ROOT}/current" + fi + + default_time_server="$(blitz_host_from_addr "${ROBOT_SIDE_OMNISOCKET_SERVER_ADDR:-}" || true)" + + export BLITZ_BOOT_DELAY_SEC="${BLITZ_BOOT_DELAY_SEC:-30}" + export BLITZ_RUN_ROOT="${BLITZ_RUN_ROOT:-/var/log/blitz-robot}" + export BLITZ_LOG_FILE="${BLITZ_LOG_FILE:-/var/log/blitz-robot/startup.log}" + export BLITZ_RUNTIME_DIR="${BLITZ_RUNTIME_DIR:-/run/blitz-robot}" + export BLITZ_RUN_CONTEXT_FILE="${BLITZ_RUN_CONTEXT_FILE:-${BLITZ_RUNTIME_DIR}/run-context.env}" + export BLITZ_RUN_ID_FILE="${BLITZ_RUN_ID_FILE:-${BLITZ_RUNTIME_DIR}/run-id}" + export BLITZ_CURRENT_RUN_LINK="${BLITZ_CURRENT_RUN_LINK:-${BLITZ_RUN_ROOT}/current}" + export BLITZ_5G_DIAL_DIR="${BLITZ_5G_DIAL_DIR:-${BOOT_SCRIPT_DIR}}" + export BLITZ_5G_SERIAL_PORT="${BLITZ_5G_SERIAL_PORT:-/dev/ttyUSB7}" + export BLITZ_5G_INTERFACE="${BLITZ_5G_INTERFACE:-}" + export BLITZ_5G_MODEM_SUBNET="${BLITZ_5G_MODEM_SUBNET:-192.168.224.0/22}" + export BLITZ_5G_GATEWAY="${BLITZ_5G_GATEWAY:-192.168.225.1}" + export BLITZ_5G_SKIP_DHCP="${BLITZ_5G_SKIP_DHCP:-0}" + export BLITZ_5G_REMOVE_DEFAULT_ROUTE="${BLITZ_5G_REMOVE_DEFAULT_ROUTE:-1}" + export BLITZ_5G_ROUTE_TARGETS="${BLITZ_5G_ROUTE_TARGETS:-106.55.173.235}" + export BLITZ_5G_INFO_JSON="${BLITZ_5G_INFO_JSON:-${BLITZ_5G_DIAL_DIR}/modem_network_info.json}" + export BLITZ_5G_DISABLE_INTERFACES="${BLITZ_5G_DISABLE_INTERFACES:-}" + export BLITZ_5G_SERIAL_WAIT_SEC="${BLITZ_5G_SERIAL_WAIT_SEC:-60}" + export BLITZ_5G_ROUTE_WAIT_SEC="${BLITZ_5G_ROUTE_WAIT_SEC:-30}" + export BLITZ_TIME_SERVER_IP="${BLITZ_TIME_SERVER_IP:-${default_time_server}}" + export BLITZ_ROS_USER="${BLITZ_ROS_USER:-nvidia}" + export BLITZ_ROS_SOCKET_WAIT_SEC="${BLITZ_ROS_SOCKET_WAIT_SEC:-20}" + export BLITZ_WATCHDOG_INTERVAL_SEC="${BLITZ_WATCHDOG_INTERVAL_SEC:-5}" + export BLITZ_HEALTH_STALE_SEC="${BLITZ_HEALTH_STALE_SEC:-15}" + export BLITZ_OMNID_THREAD_HEARTBEAT_TIMEOUT_SEC="${BLITZ_OMNID_THREAD_HEARTBEAT_TIMEOUT_SEC:-15}" + export BLITZ_KCP_STATS_INTERVAL_MS="${BLITZ_KCP_STATS_INTERVAL_MS:-1000}" + export BLITZ_CONTROL_LATENCY_LOG_ENABLED="${BLITZ_CONTROL_LATENCY_LOG_ENABLED:-1}" + export BLITZ_CONTROL_LATENCY_LOG_SAMPLE_MOD="${BLITZ_CONTROL_LATENCY_LOG_SAMPLE_MOD:-100}" + export BLITZ_5G_LINK_LOG_INTERVAL_SEC="${BLITZ_5G_LINK_LOG_INTERVAL_SEC:-5}" + export BLITZ_JSONL_FLUSH_INTERVAL_MS="${BLITZ_JSONL_FLUSH_INTERVAL_MS:-1000}" + export BLITZ_JSONL_FLUSH_BYTES="${BLITZ_JSONL_FLUSH_BYTES:-262144}" + export BLITZ_JSONL_ROTATE_BYTES="${BLITZ_JSONL_ROTATE_BYTES:-134217728}" + export BLITZ_JSONL_ROTATE_FILES="${BLITZ_JSONL_ROTATE_FILES:-8}" + export BLITZ_INCIDENT_COMMAND_TIMEOUT_SEC="${BLITZ_INCIDENT_COMMAND_TIMEOUT_SEC:-5}" + export BLITZ_INCIDENT_TOTAL_TIMEOUT_SEC="${BLITZ_INCIDENT_TOTAL_TIMEOUT_SEC:-30}" + export BLITZ_NETWORK_FAIL_THRESHOLD="${BLITZ_NETWORK_FAIL_THRESHOLD:-3}" + export BLITZ_NETWORK_RECOVERY_COOLDOWN_SEC="${BLITZ_NETWORK_RECOVERY_COOLDOWN_SEC:-30}" + export BLITZ_GPS_MONITOR_ENABLED="${BLITZ_GPS_MONITOR_ENABLED:-1}" + export BLITZ_GPS_DEVICE_GLOB="${BLITZ_GPS_DEVICE_GLOB:-/dev/ttyCH341USB*}" + export BLITZ_GPS_CHECK_INTERVAL_SEC="${BLITZ_GPS_CHECK_INTERVAL_SEC:-10}" + export BLITZ_GPS_RESTART_UNITS="${BLITZ_GPS_RESTART_UNITS:-gpsd.socket gpsd.service}" + export BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION="${BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION:-0}" + export BLITZ_BOOT_ENV_LOADED="1" +} + +blitz_timestamp() { + date '+%Y-%m-%d %H:%M:%S%z' +} + +blitz_sanitize_detail() { + local detail="${1:-}" + + detail="${detail//$'\n'/ ; }" + detail="${detail//$'\r'/ }" + printf '%s' "${detail}" +} + +blitz_log() { + local step="${1:-unknown-step}" + local action="${2:-unknown-action}" + local result="${3:-info}" + local details="${4:-}" + local exit_code="${5:-0}" + + printf '%s | %s | %s | %s | %s | %s\n' \ + "$(blitz_timestamp)" \ + "${step}" \ + "${action}" \ + "${result}" \ + "$(blitz_sanitize_detail "${details}")" \ + "${exit_code}" +} + +blitz_join_cmd() { + local cmd=() + local arg + + for arg in "$@"; do + cmd+=("$(printf '%q' "${arg}")") + done + printf '%s' "${cmd[*]}" +} + +blitz_require_command() { + local command_name="$1" + local step="${2:-precheck}" + + if command -v "${command_name}" >/dev/null 2>&1; then + blitz_log "${step}" "require-command" "success" "command=${command_name}" 0 + return 0 + fi + + blitz_log "${step}" "require-command" "failure" "missing command: ${command_name}" 127 + return 127 +} + +blitz_require_file() { + local path="$1" + local step="${2:-precheck}" + + if [[ -f "${path}" ]]; then + blitz_log "${step}" "require-file" "success" "path=${path}" 0 + return 0 + fi + + blitz_log "${step}" "require-file" "failure" "missing file: ${path}" 1 + return 1 +} + +blitz_require_executable() { + local path="$1" + local step="${2:-precheck}" + + if [[ -x "${path}" ]]; then + blitz_log "${step}" "require-executable" "success" "path=${path}" 0 + return 0 + fi + + blitz_log "${step}" "require-executable" "failure" "missing executable: ${path}" 1 + return 1 +} + +blitz_require_root() { + local step="${1:-precheck}" + + if [[ "${EUID}" -eq 0 ]]; then + blitz_log "${step}" "require-root" "success" "uid=${EUID}" 0 + return 0 + fi + + blitz_log "${step}" "require-root" "failure" "root privileges are required" 1 + return 1 +} + +blitz_run() { + local step="$1" + local action="$2" + local rc + shift 2 + + blitz_log "${step}" "${action}" "start" "$(blitz_join_cmd "$@")" 0 + if "$@"; then + blitz_log "${step}" "${action}" "success" "$(blitz_join_cmd "$@")" 0 + return 0 + else + rc=$? + fi + + blitz_log "${step}" "${action}" "failure" "$(blitz_join_cmd "$@")" "${rc}" + return "${rc}" +} + +blitz_route_ready() { + local target_ip="$1" + local expected_interface="${2:-}" + local route_output + + route_output="$(ip route get "${target_ip}" 2>&1 || true)" + if [[ -z "${route_output}" ]]; then + return 1 + fi + if [[ "${route_output}" == *"unreachable"* || "${route_output}" == *"prohibit"* ]]; then + return 1 + fi + if [[ -n "${expected_interface}" && "${route_output}" != *" dev ${expected_interface} "* && "${route_output}" != *" dev ${expected_interface}" ]]; then + return 1 + fi + + printf '%s\n' "${route_output}" + return 0 +} + +blitz_interface_exists() { + local interface_name="${1:-}" + + if [[ -z "${interface_name}" ]]; then + return 1 + fi + ip link show dev "${interface_name}" >/dev/null 2>&1 +} + +blitz_read_5g_info_interface() { + local info_json="$1" + + if [[ -z "${info_json}" || ! -f "${info_json}" ]]; then + return 1 + fi + + python3 - "${info_json}" <<'PY' +import json +import sys + +path = sys.argv[1] + +try: + with open(path, "r", encoding="utf-8") as handle: + payload = json.load(handle) +except Exception: + raise SystemExit(1) + +interface = str(payload.get("interface") or "").strip() +if not interface: + raise SystemExit(1) + +print(interface) +PY +} + +blitz_detect_5g_interface_from_subnet() { + local modem_subnet="${1:-${BLITZ_5G_MODEM_SUBNET:-}}" + + if [[ -z "${modem_subnet}" ]]; then + return 1 + fi + + python3 - "${modem_subnet}" <<'PY' +import ipaddress +import json +import subprocess +import sys + +subnet = ipaddress.ip_network(sys.argv[1], strict=False) +skip = {"lo", "docker0", "l4tbr0"} + +def priority(name: str) -> tuple[int, str]: + if name.startswith("enx"): + return (0, name) + if name.startswith("wwan"): + return (1, name) + if name.startswith("usb"): + return (2, name) + if name.startswith("eth"): + return (3, name) + return (9, name) + +try: + output = subprocess.check_output(["ip", "-j", "-4", "addr", "show"], text=True) + payload = json.loads(output) +except Exception: + raise SystemExit(1) + +candidates = [] +for item in payload: + ifname = str(item.get("ifname") or "").strip() + if not ifname or ifname in skip: + continue + for addr in item.get("addr_info") or []: + if addr.get("family") != "inet": + continue + local = addr.get("local") + prefixlen = addr.get("prefixlen") + if not local or prefixlen is None: + continue + try: + iface = ipaddress.ip_interface(f"{local}/{prefixlen}") + except ValueError: + continue + if iface.ip in subnet: + candidates.append((priority(ifname), ifname)) + break + +if not candidates: + raise SystemExit(1) + +candidates.sort(key=lambda item: item[0]) +print(candidates[0][1]) +PY +} + +blitz_refresh_5g_info_json() { + local interface_name="$1" + local info_json="${2:-${BLITZ_5G_INFO_JSON:-}}" + + if [[ -z "${interface_name}" || -z "${info_json}" ]]; then + return 1 + fi + + python3 - "${interface_name}" "${info_json}" <<'PY' +import json +import os +import subprocess +import sys + +interface_name = sys.argv[1] +path = sys.argv[2] + +try: + output = subprocess.check_output(["ip", "-j", "addr", "show", "dev", interface_name], text=True) + payload = json.loads(output) +except Exception: + raise SystemExit(1) + +if not payload: + raise SystemExit(1) + +item = payload[0] +ipv4 = [] +ipv6 = [] +for addr in item.get("addr_info") or []: + local = addr.get("local") + prefixlen = addr.get("prefixlen") + family = addr.get("family") + if not local or prefixlen is None: + continue + entry = f"{local}/{prefixlen}" + if family == "inet": + ipv4.append(entry) + elif family == "inet6": + ipv6.append(entry) + +data = { + "interface": interface_name, + "ipv4": ipv4, + "ipv6": ipv6, +} + +parent = os.path.dirname(path) +if parent: + os.makedirs(parent, exist_ok=True) +temp_path = f"{path}.tmp.{os.getpid()}" +with open(temp_path, "w", encoding="utf-8") as handle: + json.dump(data, handle, ensure_ascii=False, indent=2) +os.replace(temp_path, path) +PY +} + +blitz_resolve_5g_interface() { + local explicit_interface="${BLITZ_5G_INTERFACE:-}" + local info_json="${BLITZ_5G_INFO_JSON:-}" + local recorded_interface="" + local detected_interface="" + + if [[ -n "${explicit_interface}" ]]; then + if blitz_interface_exists "${explicit_interface}"; then + printf '%s\n' "${explicit_interface}" + return 0 + fi + return 1 + fi + + recorded_interface="$(blitz_read_5g_info_interface "${info_json}" || true)" + if [[ -n "${recorded_interface}" ]] && blitz_interface_exists "${recorded_interface}"; then + printf '%s\n' "${recorded_interface}" + return 0 + fi + + detected_interface="$(blitz_detect_5g_interface_from_subnet || true)" + if [[ -n "${detected_interface}" ]]; then + if [[ "${detected_interface}" != "${recorded_interface}" ]]; then + blitz_refresh_5g_info_json "${detected_interface}" "${info_json}" >/dev/null 2>&1 || true + fi + printf '%s\n' "${detected_interface}" + return 0 + fi + + return 1 +} + +blitz_prepare_runtime_dir() { + local runtime_dir + + blitz_load_boot_env + runtime_dir="${BLITZ_RUNTIME_DIR}" + + mkdir -p "${runtime_dir}" + if [[ "${EUID}" -eq 0 ]]; then + chown "root:${BLITZ_ROS_USER}" "${runtime_dir}" + chmod 0775 "${runtime_dir}" + else + chmod 0775 "${runtime_dir}" 2>/dev/null || true + fi + blitz_log "runtime-dir" "prepare" "success" "path=${runtime_dir}" 0 +} + +blitz_prepare_run_root() { + local run_root + local run_dir + local incidents_dir + + blitz_load_boot_env + run_root="${BLITZ_RUN_ROOT}" + run_dir="${run_root}/runs" + incidents_dir="${run_root}/incidents" + + mkdir -p "${run_dir}" "${incidents_dir}" + if [[ "${EUID}" -eq 0 ]]; then + chown -R "root:${BLITZ_ROS_USER}" "${run_root}" 2>/dev/null || true + chmod 0775 "${run_root}" "${run_dir}" "${incidents_dir}" 2>/dev/null || true + fi +} + +blitz_load_run_context_env() { + local context_file="${1:-${BLITZ_RUN_CONTEXT_FILE:-}}" + + if [[ -z "${context_file}" || ! -f "${context_file}" ]]; then + return 1 + fi + + set -a + # shellcheck disable=SC1090 + source "${context_file}" + set +a + return 0 +} + +blitz_read_run_id() { + local run_id_file="${BLITZ_RUN_ID_FILE:-}" + + if [[ -z "${run_id_file}" || ! -f "${run_id_file}" ]]; then + return 1 + fi + tr -d '\r\n' < "${run_id_file}" +} + +blitz_utc_compact_timestamp() { + date -u '+%Y%m%dT%H%M%SZ' +} + +blitz_new_run_id() { + printf '%s\n' "$(blitz_utc_compact_timestamp)" +} + +blitz_new_incident_id() { + local prefix="${1:-incident}" + printf '%s-%s-%d\n' "${prefix}" "$(blitz_utc_compact_timestamp)" "$$" +} + +blitz_new_instance_id() { + printf '%s-%d\n' "$(blitz_utc_compact_timestamp)" "$$" +} + +blitz_git_commit() { + git -C "${OMNISOCKETGO_ROOT}" rev-parse HEAD 2>/dev/null || true +} + +blitz_git_dirty_flag() { + if git -C "${OMNISOCKETGO_ROOT}" diff --quiet --ignore-submodules=dirty >/dev/null 2>&1; then + printf '0\n' + return 0 + fi + printf '1\n' +} + +blitz_write_run_context() { + local run_id="$1" + local run_dir="$2" + local boot_id="$3" + local context_file="${BLITZ_RUN_CONTEXT_FILE}" + local id_file="${BLITZ_RUN_ID_FILE}" + local temp_context + local temp_info + local commit_hash + local dirty_flag + local started_at + + commit_hash="$(blitz_git_commit)" + dirty_flag="$(blitz_git_dirty_flag)" + started_at="$(date -u '+%Y-%m-%dT%H:%M:%SZ')" + temp_context="${context_file}.tmp.$$" + temp_info="${run_dir}/run-info.json.tmp.$$" + + mkdir -p "${run_dir}" + printf '%s\n' "${run_id}" > "${id_file}" + + cat > "${temp_context}" </dev/null || echo 0)" + if (( size < max_bytes )); then + return 0 + fi + + for (( index=max_files; index>=1; index-- )); do + if [[ "${index}" -eq "${max_files}" ]]; then + rm -f "${path}.${index}" + fi + if [[ -f "${path}.${index}" ]]; then + mv -f "${path}.${index}" "${path}.$(( index + 1 ))" + fi + done + mv -f "${path}" "${path}.1" +} + +blitz_jsonl_append_line() { + local path="$1" + local line="$2" + + mkdir -p "$(dirname "${path}")" + blitz_jsonl_rotate_if_needed "${path}" + printf '%s\n' "${line}" >> "${path}" +} + +blitz_launch_incident_capture() { + local launch_script="${BOOT_SCRIPT_DIR}/blitz-incident-capture-launch.sh" + + if [[ ! -f "${launch_script}" ]]; then + return 1 + fi + /bin/bash "${launch_script}" "$@" >/dev/null 2>&1 || return 1 +} diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/disable-systemd.sh b/robot/v4l2/OmniSocketGo_robot/scripts/boot/disable-systemd.sh new file mode 100644 index 0000000..e2f6601 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/disable-systemd.sh @@ -0,0 +1,53 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="disable" +SYSTEMD_DEST_DIR="/etc/systemd/system" +UNITS=( + "blitz-watchdog.service" + "blitz-5g-link-logger.service" + "blitz-b-side-omnid.service" + "blitz-ros-receiver.service" + "blitz-5g-dial.service" + "blitz-run-context.service" + "blitz-boot-gate.service" + "blitz-robot.target" +) + +stop_unit_if_present() { + local unit_name="$1" + local unit_path="${SYSTEMD_DEST_DIR}/${unit_name}" + + if [[ ! -f "${unit_path}" ]]; then + return 0 + fi + blitz_run "${STEP}" "stop-unit" systemctl stop "${unit_name}" || true +} + +disable_unit_if_present() { + local unit_name="$1" + local unit_path="${SYSTEMD_DEST_DIR}/${unit_name}" + + if [[ ! -f "${unit_path}" ]]; then + return 0 + fi + blitz_run "${STEP}" "disable-unit" systemctl disable "${unit_name}" || true +} + +blitz_load_boot_env +blitz_require_root "${STEP}" +blitz_require_command systemctl "${STEP}" + +for unit_name in "${UNITS[@]}"; do + stop_unit_if_present "${unit_name}" +done + +for unit_name in "${UNITS[@]}"; do + disable_unit_if_present "${unit_name}" +done + +blitz_log "${STEP}" "complete" "success" "boot chain stopped and disabled; next reboot will not auto-start blitz services" 0 diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/install-systemd.sh b/robot/v4l2/OmniSocketGo_robot/scripts/boot/install-systemd.sh new file mode 100644 index 0000000..00145a7 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/install-systemd.sh @@ -0,0 +1,71 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +SYSTEMD_TEMPLATE_DIR="${SCRIPT_DIR}/systemd" +SYSTEMD_DEST_DIR="/etc/systemd/system" + +render_template() { + local template_path="$1" + local output_path="$2" + + sed \ + -e "s|@OMNISOCKETGO_ROOT@|${OMNISOCKETGO_ROOT}|g" \ + -e "s|@BLITZ_LOG_FILE@|${BLITZ_LOG_FILE}|g" \ + -e "s|@BLITZ_ROS_USER@|${BLITZ_ROS_USER}|g" \ + "${template_path}" > "${output_path}" +} + +install_unit() { + local template_name="$1" + local temp_output + + temp_output="$(mktemp)" + render_template "${SYSTEMD_TEMPLATE_DIR}/${template_name}" "${temp_output}" + install -m 0644 "${temp_output}" "${SYSTEMD_DEST_DIR}/${template_name%.in}" + rm -f "${temp_output}" + blitz_log "install" "install-unit" "success" "unit=${SYSTEMD_DEST_DIR}/${template_name%.in}" 0 +} + +remove_unit_if_present() { + local unit_name="$1" + local unit_path="${SYSTEMD_DEST_DIR}/${unit_name}" + + if [[ ! -f "${unit_path}" ]]; then + return 0 + fi + + systemctl disable --now "${unit_name}" >/dev/null 2>&1 || true + rm -f "${unit_path}" + blitz_log "install" "remove-unit" "success" "unit=${unit_path}" 0 +} + +blitz_load_boot_env +blitz_require_root "install" +blitz_require_command install "install" +blitz_require_command systemctl "install" + +mkdir -p "${SYSTEMD_DEST_DIR}" +install -d -m 0755 "$(dirname "${BLITZ_LOG_FILE}")" +touch "${BLITZ_LOG_FILE}" +chmod 0644 "${BLITZ_LOG_FILE}" +blitz_log "install" "prepare-log-file" "success" "log_file=${BLITZ_LOG_FILE}" 0 +blitz_prepare_runtime_dir +blitz_prepare_run_root + +install_unit "blitz-boot-gate.service.in" +install_unit "blitz-run-context.service.in" +install_unit "blitz-5g-dial.service.in" +install_unit "blitz-5g-link-logger.service.in" +install_unit "blitz-ros-receiver.service.in" +install_unit "blitz-b-side-omnid.service.in" +install_unit "blitz-watchdog.service.in" +install_unit "blitz-robot.target.in" +remove_unit_if_present "blitz-time-sync.service" + +blitz_run "install" "daemon-reload" systemctl daemon-reload +blitz_run "install" "enable-target" systemctl enable blitz-robot.target +blitz_log "install" "complete" "success" "run systemctl start blitz-robot.target to launch immediately" 0 diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/prepare-runtime-dir.sh b/robot/v4l2/OmniSocketGo_robot/scripts/boot/prepare-runtime-dir.sh new file mode 100644 index 0000000..c2b954a --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/prepare-runtime-dir.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="runtime-dir" + +blitz_load_boot_env +blitz_prepare_runtime_dir +blitz_log "${STEP}" "complete" "success" "runtime_dir=${BLITZ_RUNTIME_DIR}" 0 diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/rndis_dial.py b/robot/v4l2/OmniSocketGo_robot/scripts/boot/rndis_dial.py new file mode 100644 index 0000000..956b871 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/rndis_dial.py @@ -0,0 +1,854 @@ +#!/usr/bin/env python3 +"""RM520N-GL RNDIS 自动拨号脚本。 + +流程: +1. 检测 USB 设备是否存在 +2. 打开 AT 口并检查 SIM 状态 +3. 配置 RNDIS 模式: AT+QCFG="usbnet",3 +4. 重启模块: AT+CFUN=1,1 +5. 等待模块重新枚举并识别 5G 网卡 +6. 如果网卡还没有 IPv4, 自动尝试 DHCP + +用法: + sudo python3 rndis_dial.py + sudo python3 rndis_dial.py --serial-port /dev/ttyUSB7 + sudo python3 rndis_dial.py --interface eth0 #指定网口 +""" + +from __future__ import annotations + +import argparse +import errno +import ipaddress +import json +import os +import select +import shlex +import shutil +import subprocess +import sys +import termios +import time +import tty + +USB_ID = "2c7c:0801" +DEFAULT_SERIAL_PORT = "/dev/ttyUSB7" #串口设备节点 +DEFAULT_BAUD_RATE = 115200 +CHECK_INTERVAL = 2 +SERIAL_READ_TIMEOUT = 0.2 +SERIAL_POLL_INTERVAL = 0.1 +SERIAL_SETTLE_DELAY = 0.3 +AT_SYNC_RETRIES = 3 +AT_SYNC_TIMEOUT = 2.5 +# 示例地址 192.168.225.38/22 所在网段。 +# 拨号成功后会用这个网段来最终确认哪个接口是 5G 模组。 +DEFAULT_MODEM_SUBNET = "192.168.224.0/22" +DEFAULT_MODEM_GATEWAY = "192.168.225.1" +DEFAULT_PUBLIC_TARGETS = ("81.70.156.140", "106.55.173.235") +DEFAULT_INFO_JSON = "modem_network_info.json" +SKIP_INTERFACES = {"lo", "docker0", "l4tbr0"} +BAUD_RATE_MAP = { + 9600: termios.B9600, + 19200: termios.B19200, + 38400: termios.B38400, + 57600: termios.B57600, + 115200: termios.B115200, +} + + +def run_cmd(cmd, timeout=30, check=False): + print(f"[CMD] {format_shell_cmd(cmd)}") + result = subprocess.run( + cmd, + capture_output=True, + text=True, + timeout=timeout, + check=False, + ) + output = (result.stdout or "") + (result.stderr or "") + if check and result.returncode != 0: + raise RuntimeError(f"命令执行失败: {' '.join(cmd)}\n{output.strip()}") + return result.returncode, output.strip() + + +def format_shell_cmd(cmd): + """把命令参数格式化成可直接阅读的 shell 形式。""" + return " ".join(shlex.quote(part) for part in cmd) + + +def parse_ipv4_address(value): + try: + return str(ipaddress.IPv4Address(value)) + except ipaddress.AddressValueError as exc: + raise argparse.ArgumentTypeError(f"无效的 IPv4 地址: {value}") from exc + + +def dedupe_keep_order(values): + seen = set() + result = [] + for value in values: + if value in seen: + continue + seen.add(value) + result.append(value) + return result + + +def require_root(): + if os.geteuid() != 0: + print("[FAIL] 请使用 sudo 运行此脚本") + sys.exit(1) + + +def require_commands(): + missing = [cmd for cmd in ("lsusb", "ip") if shutil.which(cmd) is None] + if missing: + print(f"[FAIL] 缺少系统命令: {', '.join(missing)}") + sys.exit(1) + + +def usb_device_present(): + # 1. 第一次检测 lsusb，确认模块已经被系统识别。 + """通过 lsusb 检查模块是否已经被系统识别。""" + code, output = run_cmd(["lsusb"], timeout=10) + if code != 0: + return False, output + + for line in output.splitlines(): + if USB_ID in line: + return True, line.strip() + return False, output + + +def wait_for_usb_device(expected_present, timeout): + """等待模块 USB 设备下线或重新上线。""" + deadline = time.time() + timeout + last_seen = "" + while time.time() < deadline: + present, detail = usb_device_present() + last_seen = detail + if present == expected_present: + return True, detail + time.sleep(CHECK_INTERVAL) + return False, last_seen + + +def wait_for_path(path, timeout): + """等待串口节点或其他路径重新出现。""" + deadline = time.time() + timeout + while time.time() < deadline: + if os.path.exists(path): + return True + time.sleep(1) + return False + + +def normalize_serial_output(text): + """整理串口原始输出，便于后续匹配关键字。""" + cleaned = text.replace("\r", "\n") + return "\n".join(line for line in cleaned.splitlines() if line.strip()).strip() + + +def serial_response_complete(text): + if not text: + return False + + for line in reversed(text.splitlines()): + stripped = line.strip() + if stripped == "OK": + return True + if "ERROR" in stripped: + return True + return False + + +class RawSerialSession: + """使用 Python 标准库直接控制 Linux 串口，尽量贴近 stty/raw 行为。""" + + def __init__(self, port, baudrate): + if baudrate not in BAUD_RATE_MAP: + raise RuntimeError(f"不支持的波特率: {baudrate}") + + self.port = port + self.fd = None + self._original_attrs = None + + try: + self.fd = os.open(port, os.O_RDWR | os.O_NOCTTY | os.O_NONBLOCK) + self._original_attrs = termios.tcgetattr(self.fd) + tty.setraw(self.fd, when=termios.TCSANOW) + + attrs = termios.tcgetattr(self.fd) + attrs[0] = 0 + attrs[1] = 0 + attrs[2] &= ~(termios.PARENB | termios.CSTOPB | termios.CSIZE) + attrs[2] |= termios.CS8 | termios.CLOCAL | termios.CREAD + attrs[3] = 0 + attrs[4] = BAUD_RATE_MAP[baudrate] + attrs[5] = BAUD_RATE_MAP[baudrate] + attrs[6][termios.VMIN] = 0 + attrs[6][termios.VTIME] = 0 + termios.tcsetattr(self.fd, termios.TCSANOW, attrs) + termios.tcflush(self.fd, termios.TCIOFLUSH) + except OSError as exc: + self.close() + raise RuntimeError(f"无法打开串口 {port}: {exc}") from exc + + @property + def is_open(self): + return self.fd is not None + + def reset_input_buffer(self): + if self.fd is not None: + termios.tcflush(self.fd, termios.TCIFLUSH) + + def reset_output_buffer(self): + if self.fd is not None: + termios.tcflush(self.fd, termios.TCOFLUSH) + + def write(self, data): + if self.fd is None: + raise OSError("串口未打开") + + sent = 0 + while sent < len(data): + try: + written = os.write(self.fd, data[sent:]) + except BlockingIOError: + time.sleep(SERIAL_POLL_INTERVAL) + continue + if written <= 0: + raise OSError("串口写入返回 0 字节") + sent += written + + def flush(self): + if self.fd is not None: + termios.tcdrain(self.fd) + + def read_chunk(self, timeout, size=4096): + if self.fd is None: + return b"" + + ready, _, _ = select.select([self.fd], [], [], timeout) + if not ready: + return b"" + + try: + return os.read(self.fd, size) + except BlockingIOError: + return b"" + + def close(self): + if self.fd is None: + return + + fd = self.fd + self.fd = None + + if self._original_attrs is not None: + try: + termios.tcsetattr(fd, termios.TCSANOW, self._original_attrs) + except termios.error: + pass + os.close(fd) + + +def read_serial_output(session, timeout, allow_disconnect=False): + """在给定时间窗口内读取 AT 响应，直到出现结束标记或超时。""" + deadline = time.time() + timeout + chunks = [] + saw_terminal_line = False + last_data_time = None + + while time.time() < deadline: + try: + chunk = session.read_chunk(timeout=min(SERIAL_READ_TIMEOUT, max(deadline - time.time(), 0))) + except OSError as exc: + if allow_disconnect and exc.errno in (errno.EIO, errno.ENODEV, errno.EBADF): + break + raise RuntimeError(f"读取串口响应失败: {exc}") from exc + + if chunk: + chunks.append(chunk.decode(errors="ignore")) + last_data_time = time.time() + current_text = normalize_serial_output("".join(chunks)) + if serial_response_complete(current_text): + saw_terminal_line = True + continue + + if saw_terminal_line and last_data_time is not None and time.time() - last_data_time >= SERIAL_SETTLE_DELAY: + break + + time.sleep(SERIAL_POLL_INTERVAL) + + return normalize_serial_output("".join(chunks)) + + +def open_serial_session(port): + """打开 AT 串口会话，后续在同一连接里顺序发送多条命令。""" + ser = RawSerialSession(port=port, baudrate=DEFAULT_BAUD_RATE) + time.sleep(0.2) + ser.reset_input_buffer() + ser.reset_output_buffer() + return ser + + +def execute_serial_step(ser, command, expect=None, timeout=3, allow_disconnect=False): + """在当前串口会话里发送一条 AT 命令并校验响应。""" + print(f"[AT] {command}") + try: + ser.reset_input_buffer() + ser.write((command + "\r").encode()) + ser.flush() + except OSError as exc: + raise RuntimeError(f"AT 命令 `{command}` 发送失败: {exc}") from exc + + response = read_serial_output(ser, timeout=timeout, allow_disconnect=allow_disconnect) + + if response: + print(response) + else: + print("(无响应)") + + if "ERROR" in response: + raise RuntimeError(f"AT 命令 `{command}` 执行失败: {response}") + if expect and expect not in response and not allow_disconnect: + raise RuntimeError(f"AT 命令 `{command}` 响应异常: {response or '空响应'}") + return response + + +def synchronize_at_channel(ser): + """某些模组 AT 口在刚打开时需要先用 AT 做一次预热。""" + last_error = None + + for attempt in range(1, AT_SYNC_RETRIES + 1): + try: + print(f"[INFO] 预热 AT 通道，第 {attempt} 次") + response = execute_serial_step(ser, "AT", expect="OK", timeout=AT_SYNC_TIMEOUT) + if "OK" in response: + return + except RuntimeError as exc: + last_error = exc + time.sleep(0.5) + + if last_error is not None: + raise RuntimeError( + "AT 通道预热失败，请确认串口是否是 AT 命令口，例如 /dev/ttyUSB2" + ) from last_error + raise RuntimeError("AT 通道预热失败") + + +def run_serial_steps(port, steps): + """在同一个串口会话里顺序执行多条 AT 命令。""" + ser = None + + try: + ser = open_serial_session(port) + synchronize_at_channel(ser) + for step in steps: + execute_serial_step( + ser, + step["command"], + expect=step.get("expect"), + timeout=step.get("timeout", 3), + allow_disconnect=step.get("allow_disconnect", False), + ) + finally: + if ser is not None and ser.is_open: + ser.close() + +def configure_rndis(port): + # 2. 用 Python 串口库在同一会话里顺序执行拨号相关 AT 命令。 + """切换到 RNDIS 模式并触发模块重启。""" + if not wait_for_path(port, timeout=30): + raise RuntimeError(f"串口不存在: {port}") + + print(f"[OK] 串口已打开: {port}") + run_serial_steps( + port, + [ + {"command": "AT+CPIN?", "expect": "READY", "timeout": 4}, + {"command": 'AT+QCFG="usbnet",3', "expect": "OK", "timeout": 5}, + {"command": "AT+CFUN=1,1", "timeout": 4, "allow_disconnect": True}, + ], + ) + + +def get_interfaces(): + """列出当前系统中的接口，过滤明显无关的本地接口。""" + interfaces = [] + try: + for name in os.listdir("/sys/class/net"): + if name in SKIP_INTERFACES or is_usb_gadget(name): + continue + interfaces.append(name) + except FileNotFoundError: + return [] + return sorted(interfaces) + + +def is_usb_gadget(iface): + """过滤 Jetson 自己暴露出去的 gadget 网卡。""" + sysfs_path = f"/sys/class/net/{iface}" + if not os.path.exists(sysfs_path): + return False + return "/gadget/" in os.path.realpath(sysfs_path) + + +def is_usb_network_interface(iface): + """判断接口是否来自 USB 设备。""" + device_path = f"/sys/class/net/{iface}/device" + if not os.path.exists(device_path): + return False + real_path = os.path.realpath(device_path) + return "/usb" in real_path + + +def get_ipv4_addrs(): + """返回所有接口的 IPv4/CIDR 信息。""" + code, output = run_cmd(["ip", "-o", "-4", "addr", "show"], timeout=10) + if code != 0: + return {} + + ipv4_addrs = {} + for line in output.splitlines(): + parts = line.split() + if len(parts) >= 4: + iface = parts[1] + ipv4_addrs.setdefault(iface, []).append(parts[3]) + return ipv4_addrs + + +def get_ipv6_addrs(): + """返回所有接口的 IPv6/CIDR 信息。""" + code, output = run_cmd(["ip", "-o", "-6", "addr", "show"], timeout=10) + if code != 0: + return {} + + ipv6_addrs = {} + for line in output.splitlines(): + parts = line.split() + if len(parts) >= 4: + iface = parts[1] + ipv6_addrs.setdefault(iface, []).append(parts[3]) + return ipv6_addrs + + +def interface_priority(iface): + if iface.startswith("wwan"): + return 0 + if iface.startswith("enx"): + return 1 + if iface.startswith("usb"): + return 2 + return 10 + + +def list_usb_network_candidates(explicit_iface=None): + """列出拨号前可尝试的 USB 网卡候选项。 + + 这里不靠固定网口名确认 5G 模组，只是在还没有 IP 的时候先缩小范围。 + 真正确认模组接口，会在 DHCP 之后根据 IP 网段判断。 + """ + candidates = [] + + for iface in get_interfaces(): + if explicit_iface and iface != explicit_iface: + continue + if not is_usb_network_interface(iface): + continue + candidates.append((interface_priority(iface), iface)) + + if not candidates: + return [] + + candidates.sort() + return [iface for _, iface in candidates] + + +def ip_in_subnet(ip_cidr, subnet): + """判断接口地址是否落在指定网段内。""" + try: + return ipaddress.ip_interface(ip_cidr).ip in ipaddress.ip_network(subnet, strict=False) + except ValueError: + return False + + +def find_interface_by_subnet(modem_subnet, explicit_iface=None): + """拨号成功后，通过 IP 网段确认 5G 模组网卡。""" + candidates = [] + for iface, addrs in get_ipv4_addrs().items(): + if iface in SKIP_INTERFACES or is_usb_gadget(iface): + continue + if not is_usb_network_interface(iface): + continue + if explicit_iface and iface != explicit_iface: + continue + + matched_addrs = [addr for addr in addrs if ip_in_subnet(addr, modem_subnet)] + if matched_addrs: + candidates.append((interface_priority(iface), iface, matched_addrs)) + + if not candidates: + return None, [] + + candidates.sort() + _, iface, matched_addrs = candidates[0] + return iface, matched_addrs + + +def wait_for_usb_candidates(explicit_iface=None, timeout=90): + """等待模块枚举出 USB 网卡候选项。""" + deadline = time.time() + timeout + while time.time() < deadline: + candidates = list_usb_network_candidates(explicit_iface=explicit_iface) + if candidates: + return candidates + time.sleep(CHECK_INTERVAL) + return [] + + +def bring_interface_up(iface): + code, output = run_cmd(["ip", "link", "set", "dev", iface, "up"], timeout=10) + if code != 0: + raise RuntimeError(f"拉起网卡失败: {iface}\n{output}") + + +def renew_dhcp(iface): + dhclient = shutil.which("dhclient") + udhcpc = shutil.which("udhcpc") + + if dhclient: + print(f"[INFO] 使用 dhclient 为 {iface} 获取 IP") + code, output = run_cmd(["dhclient", "-1", "-v", iface], timeout=45) + return code == 0, output + + if udhcpc: + print(f"[INFO] 使用 udhcpc 为 {iface} 获取 IP") + code, output = run_cmd(["udhcpc", "-n", "-q", "-i", iface], timeout=45) + return code == 0, output + + return False, "系统中未找到 dhclient 或 udhcpc" + + +def get_default_routes(iface): + code, output = run_cmd(["ip", "-o", "route", "show", "default", "dev", iface], timeout=10) + if code != 0: + return [] + return [line.strip() for line in output.splitlines() if line.strip()] + + +def resolve_gateway(iface, fallback_gateway): + for route in get_default_routes(iface): + tokens = route.split() + for index, token in enumerate(tokens[:-1]): + if token == "via": + gateway = tokens[index + 1] + print(f"[INFO] 从默认路由检测到 {iface} 网关: {gateway}") + return gateway + + print(f"[INFO] 未从默认路由检测到 {iface} 网关，回退到 {fallback_gateway}") + return fallback_gateway + + +def delete_default_routes(iface): + removed = 0 + + while True: + routes = get_default_routes(iface) + if not routes: + return removed + + deleted_this_round = False + for route in routes: + cmd = ["ip", "route", "del", *route.split()] + code, output = run_cmd(cmd, timeout=10) + if code != 0: + code, output = run_cmd(["ip", "route", "del", "default", "dev", iface], timeout=10) + if code != 0: + raise RuntimeError(f"删除默认路由失败: {iface}\n{output}") + removed += 1 + deleted_this_round = True + + if not deleted_this_round: + raise RuntimeError(f"未能删除 {iface} 的默认路由") + + +def install_host_routes(iface, gateway, targets): + for target in dedupe_keep_order(targets): + cmd = ["ip", "route", "replace", f"{target}/32", "via", gateway, "dev", iface] + code, output = run_cmd(cmd, timeout=10) + if code != 0: + raise RuntimeError(f"添加主机路由失败: {target} via {gateway} dev {iface}\n{output}") + + print(f"[OK] 已添加主机路由: {target}/32 via {gateway} dev {iface}") + + +def enforce_route_policy(iface, fallback_gateway, route_targets): + gateway = resolve_gateway(iface, fallback_gateway) + removed = delete_default_routes(iface) + print(f"[OK] 已删除 {iface} 上的 {removed} 条默认路由") + + if route_targets: + install_host_routes(iface, gateway, route_targets) + else: + print(f"[WARN] {iface} 未配置任何主机路由目标，5G 将不再承载公网流量") + + +def ensure_ipv4(iface): + """为指定接口申请 IPv4 地址。""" + ipv4_addrs = get_ipv4_addrs().get(iface, []) + if ipv4_addrs: + return ipv4_addrs + + bring_interface_up(iface) + ok, output = renew_dhcp(iface) + if output: + print(output) + if not ok: + return [] + + return get_ipv4_addrs().get(iface, []) + + +def acquire_modem_interface(modem_subnet, explicit_iface=None): + """通过 DHCP + IP 网段识别真正的模组接口。""" + iface, matched_addrs = find_interface_by_subnet( + modem_subnet, + explicit_iface=explicit_iface, + ) + if iface: + return iface, matched_addrs + + candidates = list_usb_network_candidates(explicit_iface=explicit_iface) + if not candidates: + raise RuntimeError("未找到可尝试 DHCP 的 USB 网卡候选项") + + print(f"[INFO] 当前 USB 网卡候选项: {', '.join(candidates)}") + + for iface in candidates: + print(f"[INFO] 尝试为 {iface} 获取 IPv4") + ensure_ipv4(iface) + + matched_iface, matched_addrs = find_interface_by_subnet( + modem_subnet, + explicit_iface=explicit_iface, + ) + if matched_iface: + return matched_iface, matched_addrs + + return None, [] + + +def print_interface_status(iface): + # 3. 拨号成功后，打印 ip/ifconfig，确认模组网口和地址。 + print(f"[OK] 检测到 5G 网卡: {iface}") + + code, output = run_cmd(["ip", "-4", "addr", "show", "dev", iface], timeout=10) + if code == 0 and output: + print(output) + + if shutil.which("ifconfig"): + code, ifconfig_output = run_cmd(["ifconfig", iface], timeout=10) + if code == 0 and ifconfig_output: + print("\n===== ifconfig =====") + print(ifconfig_output) + + +def save_interface_info(iface, output_file=DEFAULT_INFO_JSON): + """把网口名称、IPv4、IPv6 保存到 JSON 文件。""" + data = { + "interface": iface, + "ipv4": get_ipv4_addrs().get(iface, []), + "ipv6": get_ipv6_addrs().get(iface, []), + } + + with open(output_file, "w", encoding="utf-8") as json_file: + json.dump(data, json_file, ensure_ascii=False, indent=2) + + print(f"[OK] 网口信息已保存到 {output_file}") + + +def ping_target(iface, target, count=3, timeout=15): + """通过指定网口 ping 一个目标。""" + code, output = run_cmd( + ["ping", "-I", iface, "-c", str(count), "-W", "3", target], + timeout=timeout, + ) + return code == 0, output + + +def print_ping_summary(output): + """只打印 ping 的关键结果。""" + for line in output.splitlines(): + if "packets transmitted" in line or "rtt " in line or "Destination " in line: + print(line) + + +def verify_connectivity(iface, gateway=DEFAULT_MODEM_GATEWAY, targets=DEFAULT_PUBLIC_TARGETS, retry_interval=3, max_wait=45): + # 4. 最后先 ping 模组网关，再重试公网连通性。 + """先测模组网关，再轮询公网目标地址。""" + ok, output = ping_target(iface, gateway, count=3, timeout=15) + if ok: + print(f"[OK] {iface} 可到达模组网关 {gateway}") + print_ping_summary(output) + else: + print(f"[WARN] {iface} 无法到达模组网关 {gateway}") + if output: + print(output) + return False + + deadline = time.time() + max_wait + attempt = 1 + while True: + for target in targets: + ok, output = ping_target(iface, target, count=3, timeout=15) + if ok: + print(f"[OK] {iface} 可通过 {target}") + print_ping_summary(output) + return True + + print(f"[WARN] 第 {attempt} 次 Ping {target} 失败") + if output: + print_ping_summary(output) + + if time.time() >= deadline: + print(f"[WARN] {iface} 在 {max_wait} 秒内仍无法连通 {', '.join(targets)}") + return False + + attempt += 1 + time.sleep(retry_interval) + + +def ping_via_interface(iface, targets=DEFAULT_PUBLIC_TARGETS): + """保留原调用点，内部走完整连通性检查。""" + return verify_connectivity(iface, targets=targets) + + +def parse_args(): + parser = argparse.ArgumentParser(description="RM520N-GL RNDIS 自动拨号脚本") + parser.add_argument( + "--serial-port", + default=DEFAULT_SERIAL_PORT, + help=f"AT 串口路径，默认 {DEFAULT_SERIAL_PORT}", + ) + parser.add_argument( + "--interface", + help="指定期望的 5G 网卡名，例如 eth0", + ) + parser.add_argument( + "--modem-subnet", + default=DEFAULT_MODEM_SUBNET, + help=f"拨号成功后用于识别模组接口的 IPv4 网段，默认 {DEFAULT_MODEM_SUBNET}", + ) + parser.add_argument( + "--gateway", + type=parse_ipv4_address, + default=DEFAULT_MODEM_GATEWAY, + help=f"5G 模组网关地址，默认 {DEFAULT_MODEM_GATEWAY}", + ) + parser.add_argument( + "--skip-dhcp", + action="store_true", + help="只等待 USB 网卡出现，不主动申请 IPv4", + ) + parser.add_argument( + "--remove-default-route", + action="store_true", + help="拨号成功后删除 5G 接口上的默认路由，只保留显式主机路由", + ) + parser.add_argument( + "--route-target", + action="append", + default=[], + type=parse_ipv4_address, + help="拨号完成后通过 5G 接口保留的 IPv4 主机路由目标，可重复传入", + ) + return parser.parse_args() + + +def main(): + args = parse_args() + require_root() + require_commands() + + print("===== RM520N-GL RNDIS 自动拨号 =====") + print(f"[INFO] 目标模组网段: {args.modem_subnet}") + + #1.检测 lsusb，确认是否识别到模块 + present, detail = usb_device_present() + if not present: + print(f"[FAIL] 未检测到模块 USB 设备 {USB_ID}") + if detail: + print(detail) + sys.exit(1) + + print(f"[OK] 检测到 USB 设备: {detail}") + print(f"[INFO] 使用 AT 口: {args.serial_port}") + + #2.进行 Python 串口拨号 + try: + configure_rndis(args.serial_port) + + print("[INFO] 已发送 AT+CFUN=1,1，等待模块重启") + disappeared, _ = wait_for_usb_device(expected_present=False, timeout=25) + if disappeared: + print("[OK] 模块已下线，继续等待重新枚举") + else: + print("[WARN] 未观察到模块下线，继续等待重新枚举") + + reappeared, detail = wait_for_usb_device(expected_present=True, timeout=90) + if not reappeared: + print(f"[FAIL] 模块重启后未重新枚举: {USB_ID}") + sys.exit(1) + + print(f"[OK] 模块已重新枚举: {detail}") + + candidates = wait_for_usb_candidates(explicit_iface=args.interface, timeout=90) + if not candidates: + print("[FAIL] 未检测到 5G 模组枚举出的 USB 网卡") + sys.exit(1) + + if args.skip_dhcp: + print(f"[INFO] 当前 USB 网卡候选项: {', '.join(candidates)}") + iface, ipv4_addrs = find_interface_by_subnet( + args.modem_subnet, + explicit_iface=args.interface, + ) + if not iface: + print(f"[WARN] 当前还没有接口拿到目标网段 {args.modem_subnet} 的地址") + sys.exit(1) + else: + iface, ipv4_addrs = acquire_modem_interface( + args.modem_subnet, + explicit_iface=args.interface, + ) + if not iface: + print(f"[FAIL] 未找到落在目标网段 {args.modem_subnet} 内的模组接口") + sys.exit(1) + + print_interface_status(iface) + + if ipv4_addrs: + for addr in ipv4_addrs: + print(f"[OK] {iface} 已获取 IPv4: {addr}") + save_interface_info(iface) + route_targets = dedupe_keep_order(args.route_target) + if args.remove_default_route: + enforce_route_policy(iface, args.gateway, route_targets) + + connectivity_targets = route_targets or list(DEFAULT_PUBLIC_TARGETS) + ping_via_interface(iface, targets=connectivity_targets) + print(f"[DONE] RNDIS 拨号完成，可执行: sudo python3 speed_test.py {iface}") + return + + print(f"[WARN] {iface} 已出现，但还没有 IPv4 地址") + print(f"[INFO] 可手动检查: ip addr show {iface}") + sys.exit(1) + except (RuntimeError, subprocess.TimeoutExpired) as exc: + print(f"[FAIL] {exc}") + sys.exit(1) + + +if __name__ == "__main__": + main() diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/robot-boot.env b/robot/v4l2/OmniSocketGo_robot/scripts/boot/robot-boot.env new file mode 100644 index 0000000..152a737 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/robot-boot.env @@ -0,0 +1,60 @@ +# Boot-time settings for the robot-side autostart chain. +# Override machine-specific values in robot-boot.env.local. + +BLITZ_BOOT_DELAY_SEC="30" +BLITZ_RUN_ROOT="/var/log/blitz-robot" +BLITZ_LOG_FILE="/var/log/blitz-robot/startup.log" +BLITZ_RUNTIME_DIR="/run/blitz-robot" +BLITZ_RUN_CONTEXT_FILE="${BLITZ_RUNTIME_DIR}/run-context.env" +BLITZ_RUN_ID_FILE="${BLITZ_RUNTIME_DIR}/run-id" +BLITZ_CURRENT_RUN_LINK="${BLITZ_RUN_ROOT}/current" + +BLITZ_5G_DIAL_DIR="${OMNISOCKETGO_ROOT}/scripts/boot" +BLITZ_5G_SERIAL_PORT="/dev/ttyUSB2" +BLITZ_5G_INTERFACE="" +BLITZ_5G_MODEM_SUBNET="192.168.224.0/22" +BLITZ_5G_GATEWAY="192.168.225.1" +BLITZ_5G_SKIP_DHCP="0" +BLITZ_5G_REMOVE_DEFAULT_ROUTE="1" +BLITZ_5G_ROUTE_TARGETS="106.55.173.235" +BLITZ_5G_INFO_JSON="${OMNISOCKETGO_ROOT}/scripts/boot/modem_network_info.json" +BLITZ_5G_SERIAL_WAIT_SEC="60" +BLITZ_5G_ROUTE_WAIT_SEC="30" + +# Leave empty to fall back to the host part of ROBOT_SIDE_OMNISOCKET_SERVER_ADDR. +BLITZ_TIME_SERVER_IP="81.70.156.140" + +BLITZ_ROS_USER="nvidia" +BLITZ_ROS_SOCKET_WAIT_SEC="20" +BLITZ_WATCHDOG_INTERVAL_SEC="5" +BLITZ_HEALTH_STALE_SEC="15" +BLITZ_OMNID_THREAD_HEARTBEAT_TIMEOUT_SEC="15" +BLITZ_KCP_STATS_INTERVAL_MS="1000" +BLITZ_CONTROL_LATENCY_LOG_ENABLED="1" +BLITZ_CONTROL_LATENCY_LOG_SAMPLE_MOD="100" +BLITZ_CONTROL_ACK_SAMPLE_MOD="10" +BLITZ_VIDEO_STAGE_LOG_ENABLED="1" +BLITZ_VIDEO_STAGE_LOG_SAMPLE_MOD="10" +BLITZ_5G_LINK_LOG_INTERVAL_SEC="5" +BLITZ_JSONL_FLUSH_INTERVAL_MS="1000" +BLITZ_JSONL_FLUSH_BYTES="262144" +BLITZ_JSONL_ROTATE_BYTES="134217728" +BLITZ_JSONL_ROTATE_FILES="8" +# Log one normal relay packet out of every N packets. Drop events still log immediately. +OMNI_RELAY_PACKET_LOG_SAMPLE_EVERY="200" +BLITZ_INCIDENT_COMMAND_TIMEOUT_SEC="5" +BLITZ_INCIDENT_TOTAL_TIMEOUT_SEC="30" +BLITZ_NETWORK_FAIL_THRESHOLD="3" +BLITZ_NETWORK_RECOVERY_COOLDOWN_SEC="30" +BLITZ_GPS_MONITOR_ENABLED="1" +BLITZ_GPS_DEVICE_GLOB="/dev/ttyCH341USB*" +BLITZ_GPS_CHECK_INTERVAL_SEC="10" +BLITZ_GPS_RESTART_UNITS="gpsd.socket gpsd.service" +BLITZ_WATCHDOG_ALLOW_FAULT_INJECTION="0" + +OMNI_CAMERA_DEVICE="/dev/v4l/by-path/platform-a80aa10000.usb-usb-0:3.2:1.4-video-index0" + +# Boot units run b_side_omnid as root directly, so nested sudo must stay off. +B_SIDE_OMNID_USE_SUDO="0" +OMNI_CONTROL_ACK_PEER_ID="peer-b-ctrl-ack" +OMNI_CONTROL_ACK_TARGET_PEER="peer-a-ctrl-ack" diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/start-5g-link-logger-service.sh b/robot/v4l2/OmniSocketGo_robot/scripts/boot/start-5g-link-logger-service.sh new file mode 100644 index 0000000..ea2c051 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/start-5g-link-logger-service.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}/common.sh" + +STEP="5g-link-logger-service" + +blitz_load_boot_env +blitz_require_run_context + +export OMNI_BOOT_MODE="1" +export BLITZ_INSTANCE_ID="${BLITZ_INSTANCE_ID:-$(blitz_new_instance_id)}" +export BLITZ_5G_LINK_LOG_PATH="${BLITZ_5G_LINK_LOG_PATH:-${BLITZ_RUN_DIR}/b-5g-link-quality.${BLITZ_INSTANCE_ID}.jsonl}" + +blitz_log "${STEP}" "start" "start" "exec bash ${OMNISOCKETGO_ROOT}/scripts/boot/blitz-5g-link-logger.sh" 0 +exec bash "${OMNISOCKETGO_ROOT}/scripts/boot/blitz-5g-link-logger.sh" diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/start-b-side-omnid-service.sh b/robot/v4l2/OmniSocketGo_robot/scripts/boot/start-b-side-omnid-service.sh new file mode 100644 index 0000000..53eea06 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/start-b-side-omnid-service.sh @@ -0,0 +1,19 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="b-side-omnid" + +blitz_load_boot_env +blitz_require_run_context + +blitz_require_executable "${OMNISOCKETGO_ROOT}/bin/b_side_omnid" "${STEP}" + +export OMNI_BOOT_MODE="1" +export B_SIDE_OMNID_USE_SUDO="0" + +blitz_log "${STEP}" "start" "start" "exec bash ${OMNISOCKETGO_ROOT}/scripts/dev/start-b-side-omnid.sh" 0 +exec bash "${OMNISOCKETGO_ROOT}/scripts/dev/start-b-side-omnid.sh" diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/start-ros-receiver-service.sh b/robot/v4l2/OmniSocketGo_robot/scripts/boot/start-ros-receiver-service.sh new file mode 100644 index 0000000..8bea80a --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/start-ros-receiver-service.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}/common.sh" + +STEP="ros-receiver" + +blitz_load_boot_env +blitz_require_run_context + +blitz_require_file "/opt/ros/${ROS_DISTRO}/setup.bash" "${STEP}" +blitz_require_file "${ROS_CONTROL_PY_DIR}/install/setup.bash" "${STEP}" + +export OMNI_BOOT_MODE="1" +blitz_log "${STEP}" "start" "start" "exec bash ${OMNISOCKETGO_ROOT}/scripts/dev/start-ros-receiver.sh" 0 +exec bash "${OMNISOCKETGO_ROOT}/scripts/dev/start-ros-receiver.sh" diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-5g-dial.service.in b/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-5g-dial.service.in new file mode 100644 index 0000000..02a5c64 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-5g-dial.service.in @@ -0,0 +1,15 @@ +[Unit] +Description=Blitz robot 5G dial +PartOf=blitz-robot.target +After=blitz-run-context.service +Requires=blitz-run-context.service + +[Service] +Type=oneshot +RemainAfterExit=yes +ExecStart=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/5g-dial.sh +StandardOutput=append:@BLITZ_LOG_FILE@ +StandardError=append:@BLITZ_LOG_FILE@ + +[Install] +WantedBy=blitz-robot.target diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-5g-link-logger.service.in b/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-5g-link-logger.service.in new file mode 100644 index 0000000..81b810b --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-5g-link-logger.service.in @@ -0,0 +1,19 @@ +[Unit] +Description=Blitz robot 5G link logger +PartOf=blitz-robot.target +After=blitz-run-context.service blitz-5g-dial.service +Requires=blitz-run-context.service +Wants=blitz-run-context.service blitz-5g-dial.service + +[Service] +Type=simple +EnvironmentFile=-/run/blitz-robot/run-context.env +ExecStartPre=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/prepare-runtime-dir.sh +ExecStart=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/start-5g-link-logger-service.sh +Restart=always +RestartSec=5 +StandardOutput=append:@BLITZ_LOG_FILE@ +StandardError=append:@BLITZ_LOG_FILE@ + +[Install] +WantedBy=blitz-robot.target diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-b-side-omnid.service.in b/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-b-side-omnid.service.in new file mode 100644 index 0000000..bce9b11 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-b-side-omnid.service.in @@ -0,0 +1,20 @@ +[Unit] +Description=Blitz robot b-side omnid +PartOf=blitz-robot.target +After=blitz-run-context.service blitz-5g-dial.service blitz-ros-receiver.service +Requires=blitz-run-context.service +Wants=blitz-run-context.service blitz-5g-dial.service blitz-ros-receiver.service + +[Service] +Type=simple +EnvironmentFile=-/run/blitz-robot/run-context.env +ExecStartPre=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/prepare-runtime-dir.sh +ExecStart=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/start-b-side-omnid-service.sh +ExecStopPost=/bin/bash -lc 'if [[ "${SERVICE_RESULT:-success}" != "success" ]]; then exec /bin/bash "@OMNISOCKETGO_ROOT@/scripts/boot/blitz-incident-capture-launch.sh" --source exec-stop-post --unit "%n" --result "${SERVICE_RESULT:-}" --exit-status "${EXIT_STATUS:-}" --reason b-side-service-exit; fi' +Restart=always +RestartSec=2 +StandardOutput=append:@BLITZ_LOG_FILE@ +StandardError=append:@BLITZ_LOG_FILE@ + +[Install] +WantedBy=blitz-robot.target diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-boot-gate.service.in b/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-boot-gate.service.in new file mode 100644 index 0000000..5f918ef --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-boot-gate.service.in @@ -0,0 +1,15 @@ +[Unit] +Description=Blitz robot boot gate +PartOf=blitz-robot.target +After=multi-user.target network-online.target +Wants=network-online.target + +[Service] +Type=oneshot +RemainAfterExit=yes +ExecStart=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/boot-gate.sh +StandardOutput=append:@BLITZ_LOG_FILE@ +StandardError=append:@BLITZ_LOG_FILE@ + +[Install] +WantedBy=blitz-robot.target diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-robot.target.in b/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-robot.target.in new file mode 100644 index 0000000..7590c67 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-robot.target.in @@ -0,0 +1,13 @@ +[Unit] +Description=Blitz robot boot chain +Wants=blitz-boot-gate.service +Wants=blitz-run-context.service +Wants=blitz-5g-dial.service +Wants=blitz-5g-link-logger.service +Wants=blitz-ros-receiver.service +Wants=blitz-b-side-omnid.service +Wants=blitz-watchdog.service +After=multi-user.target + +[Install] +WantedBy=multi-user.target diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-ros-receiver.service.in b/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-ros-receiver.service.in new file mode 100644 index 0000000..634b19b --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-ros-receiver.service.in @@ -0,0 +1,23 @@ +[Unit] +Description=Blitz robot ROS receiver +PartOf=blitz-robot.target +After=blitz-run-context.service blitz-5g-dial.service +Requires=blitz-run-context.service +Wants=blitz-run-context.service blitz-5g-dial.service + +[Service] +Type=simple +User=@BLITZ_ROS_USER@ +PermissionsStartOnly=true +EnvironmentFile=-/run/blitz-robot/run-context.env +ExecStartPre=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/prepare-runtime-dir.sh +ExecStart=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/start-ros-receiver-service.sh +ExecStartPost=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/wait-for-unix-socket.sh --step ros-receiver +ExecStopPost=/bin/bash -lc 'if [[ "${SERVICE_RESULT:-success}" != "success" ]]; then exec /bin/bash "@OMNISOCKETGO_ROOT@/scripts/boot/blitz-incident-capture-launch.sh" --source exec-stop-post --unit "%n" --result "${SERVICE_RESULT:-}" --exit-status "${EXIT_STATUS:-}" --reason ros-service-exit; fi' +Restart=always +RestartSec=2 +StandardOutput=append:@BLITZ_LOG_FILE@ +StandardError=append:@BLITZ_LOG_FILE@ + +[Install] +WantedBy=blitz-robot.target diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-run-context.service.in b/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-run-context.service.in new file mode 100644 index 0000000..2ace077 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-run-context.service.in @@ -0,0 +1,15 @@ +[Unit] +Description=Blitz robot run context +PartOf=blitz-robot.target +After=blitz-boot-gate.service +Requires=blitz-boot-gate.service + +[Service] +Type=oneshot +RemainAfterExit=yes +ExecStart=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/blitz-run-context.sh +StandardOutput=append:@BLITZ_LOG_FILE@ +StandardError=append:@BLITZ_LOG_FILE@ + +[Install] +WantedBy=blitz-robot.target diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-watchdog.service.in b/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-watchdog.service.in new file mode 100644 index 0000000..882d5b7 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/systemd/blitz-watchdog.service.in @@ -0,0 +1,19 @@ +[Unit] +Description=Blitz robot health watchdog +PartOf=blitz-robot.target +After=blitz-run-context.service blitz-b-side-omnid.service blitz-ros-receiver.service +Requires=blitz-run-context.service +Wants=blitz-run-context.service blitz-b-side-omnid.service blitz-ros-receiver.service + +[Service] +Type=simple +EnvironmentFile=-/run/blitz-robot/run-context.env +ExecStartPre=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/prepare-runtime-dir.sh +ExecStart=/bin/bash @OMNISOCKETGO_ROOT@/scripts/boot/blitz-watchdog.sh +Restart=always +RestartSec=5 +StandardOutput=append:@BLITZ_LOG_FILE@ +StandardError=append:@BLITZ_LOG_FILE@ + +[Install] +WantedBy=blitz-robot.target diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/boot/wait-for-unix-socket.sh b/robot/v4l2/OmniSocketGo_robot/scripts/boot/wait-for-unix-socket.sh new file mode 100644 index 0000000..2d4d411 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/boot/wait-for-unix-socket.sh @@ -0,0 +1,49 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/common.sh" + +STEP="ros-receiver" +SOCKET_PATH="" +TIMEOUT_SEC="" + +while [[ $# -gt 0 ]]; do + case "$1" in + --path) + SOCKET_PATH="$2" + shift 2 + ;; + --timeout) + TIMEOUT_SEC="$2" + shift 2 + ;; + --step) + STEP="$2" + shift 2 + ;; + *) + blitz_log "${STEP}" "wait-socket-arg" "failure" "unknown argument: $1" 2 + exit 2 + ;; + esac +done + +blitz_load_boot_env + +SOCKET_PATH="${SOCKET_PATH:-${ROBOT_RECEIVER_LOCAL_SOCKET_PATH}}" +TIMEOUT_SEC="${TIMEOUT_SEC:-${BLITZ_ROS_SOCKET_WAIT_SEC}}" + +blitz_log "${STEP}" "wait-socket" "start" "path=${SOCKET_PATH} timeout_sec=${TIMEOUT_SEC}" 0 + +for (( waited=0; waited< TIMEOUT_SEC; waited++ )); do + if [[ -S "${SOCKET_PATH}" ]]; then + blitz_log "${STEP}" "wait-socket" "success" "path=${SOCKET_PATH} waited_sec=${waited}" 0 + exit 0 + fi + sleep 1 +done + +blitz_log "${STEP}" "wait-socket" "failure" "path=${SOCKET_PATH} timeout_sec=${TIMEOUT_SEC}" 1 +exit 1 diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/dev/README.md b/robot/v4l2/OmniSocketGo_robot/scripts/dev/README.md new file mode 100644 index 0000000..70d18f2 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/dev/README.md @@ -0,0 +1,188 @@ +# 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`. +`start-backend.sh` and `start-frontend.sh` need that repo; `start-ros-receiver.sh` and `start-b-side-omnid.sh` do not. + +## 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 +- `prepare-camera-device.sh`: reports V4L2 owners and optionally stops one explicitly configured camera service +- `resolve-camera-device.sh`: resolves an RGB capture node by USB serial plus MJPEG resolution support +- `apply-camera-controls.sh`: applies the camera preset before `b_side_omnid` starts +- `start-backend.sh`: starts Django ASGI with `uvicorn` +- `log-network-summary.py`: polls the backend `network/latest` API and appends compact JSONL snapshots +- `start-frontend.sh`: starts the Vite dev server +- `start-ros-receiver.sh`: starts the ROS2 `udp_teleop_bridge` receiver +- `start-b-side-omnid.sh`: applies camera controls, then 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 +``` + +When you launch via `start-*.sh`, you do not need to manually `export` the variables from +`robot-remote.env` or `robot-remote.env.local`. `load-env.sh` loads those files with `set -a`, +so the variables are exported automatically for the child process. Manual `export` is only needed +if you bypass these scripts and start binaries directly from a clean shell. + +## 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_CAMERA_AUTO_DISCOVER=1` resolves both camera nodes on every start instead of trusting unstable `/dev/video*` numbers +- `OMNI_CAMERA_HEAD_SERIAL` and `OMNI_CAMERA_WAIST_SERIAL` permanently map the physical cameras to the head/waist roles +- `OMNI_CAMERA_HEAD_DEVICE` and `OMNI_CAMERA_WAIST_DEVICE` are fallback nodes when automatic discovery is disabled +- `OMNI_CAMERA_DISCOVERY_WIDTH`, `OMNI_CAMERA_DISCOVERY_HEIGHT`, and `OMNI_CAMERA_DISCOVERY_TIMEOUT_SEC` tune capability matching and retry time +- `OMNI_CAMERA_ACTIVE=head|waist` selects the camera sent at startup + +`b_side_omnid` keeps both configured cameras streaming and only decodes/encodes/sends the selected input. Send a text control message with body `camera:head` or `camera:waist` to `peer-b-ctrl` to switch without reopening either camera. After applying the selection, the robot replies to the sender with `{"type":"camera.selected","camera":"head|waist"}` so callers can confirm the actual state. The normal fixed-size binary robot control packets are unchanged. +- `OMNI_CAMERA_OCCUPANCY_POLICY` +- `OMNI_CAMERA_RELEASE_SERVICE` +- `OMNI_CAMERA_PROFILE` +- `OMNI_CAMERA_BRIGHTNESS` +- `OMNI_CAMERA_CUSTOM_CTRL` +- `OMNI_CAMERA_VERIFY` +- `OMNI_VIDEO_PEER_ID` +- `OMNI_CONTROL_PEER_ID` +- `OMNI_VIDEO_SOFT_BACKPRESSURE_SEGMENTS` +- `OMNI_VIDEO_HARD_BACKPRESSURE_SEGMENTS` +- `OMNI_VIDEO_HARD_BACKPRESSURE_HOLD_MS` +- `OMNI_CONTROL_SERVER_IDLE_RECONNECT_MS` +- `OMNI_VIDEO_MAX_FRAME_AGE_MS` +- `OMNISOCKET_TELEMETRY_PEER_ID` +- `OMNISOCKET_TELEMETRY_INTERVAL_MS` +- `OMNISOCKET_TELEMETRY_STALE_AFTER_MS` +- `OMNI_NETWORK_SUMMARY_LOG_ENABLED` +- `OMNI_NETWORK_SUMMARY_LOG_PATH` +- `OMNI_NETWORK_SUMMARY_LOG_INTERVAL_MS` + +Camera discovery uses `udevadm` and `v4l2-ctl` on the robot side. A candidate must have the configured serial number and advertise MJPEG at the configured capture resolution; depth, IR, Bayer, and metadata nodes are rejected. With `OMNI_CAMERA_OCCUPANCY_POLICY=release-known`, the start script stops the configured head/waist Orbbec services before discovery so libusb-owned interfaces can reattach to `uvcvideo`. + +Role mapping: + +- `start-backend.sh` uses the `CONTROL_SIDE_*` address pair +- `start-b-side-omnid.sh` uses the `ROBOT_SIDE_*` address pair +- `start-b-side-omnid.sh` also applies the `OMNI_CAMERA_*` preset before the daemon opens the camera +- `start-b-side-omnid.sh` runs the camera occupancy preflight before applying camera controls +- `start-ros-receiver.sh` defaults to the robot-side address pair, but with `transport=unix_dgram` it usually does not need the server address + +New repair knobs: + +- `OMNI_VIDEO_SOFT_BACKPRESSURE_SEGMENTS`, `OMNI_VIDEO_HARD_BACKPRESSURE_SEGMENTS`, and `OMNI_VIDEO_HARD_BACKPRESSURE_HOLD_MS` are used by `b_side_omnid` +- `OMNI_CONTROL_SERVER_IDLE_RECONNECT_MS` is used by `b_side_omnid` +- `OMNI_VIDEO_MAX_FRAME_AGE_MS` is used by `start-backend.sh` on the A-side backend, not by `b_side_omnid` +- `OMNISOCKET_TELEMETRY_INTERVAL_MS` and `OMNISOCKET_TELEMETRY_STALE_AFTER_MS` tune the backend's D-side telemetry freshness window +- `OMNI_NETWORK_SUMMARY_LOG_*` controls the A-side JSONL summary logger that polls `GET /api/network/latest/` + +Default long-run network logging: + +- A-side starts a compact JSONL logger by default at `${OMNISOCKETGO_ROOT}/logs/a-network-summary.jsonl` +- The default A-side polling interval is `2000 ms` +- For D-side long runs, prefer: + +```bash +./bin/kcpserver -listen 0.0.0.0:10909 \ + -telemetry-peer peer-a-telemetry \ + -telemetry-interval 1000ms \ + -kcp-session-stats-log logs/d-kcp-stats.jsonl \ + -kcp-session-stats-interval 1000ms +``` + +- Keep `-latency-log` and `-kcp-ts-debug-log` off by default for multi-hour runs +- Do not continuously redirect relay `C` stderr to a file unless you are reproducing a short issue window + +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" +OMNI_NETWORK_SUMMARY_LOG_INTERVAL_MS="5000" +``` + +Camera occupancy handling is deliberately narrow. `check` only reports owners and fails if the +device is busy. `release-known` stops exactly `OMNI_CAMERA_RELEASE_SERVICE`, then checks the device +again. It never kills an arbitrary PID and refuses to stop `proc_manager.service` automatically: + +```bash +OMNI_CAMERA_DEVICE="/dev/video18" +OMNI_CAMERA_OCCUPANCY_POLICY="release-known" +OMNI_CAMERA_RELEASE_SERVICE="orbbec_waist.service" +``` + +Run the preflight without starting the daemon: + +```bash +bash scripts/dev/prepare-camera-device.sh +``` + +If a remaining owner belongs to `proc_manager.service`, inspect it with `ros2 component list` and +unload only the camera component. Stopping the complete process manager can interrupt unrelated +robot functions. + +Default camera behavior is the `night` preset: + +```bash +OMNI_CAMERA_PROFILE="night" +# Optional per-machine tweak: +OMNI_CAMERA_BRIGHTNESS="8" +``` + +To switch to a daytime preset with brightness only: + +```bash +OMNI_CAMERA_PROFILE="day" +OMNI_CAMERA_BRIGHTNESS="8" +``` + +To send the raw `v4l2-ctl --set-ctrl=...` payload yourself: + +```bash +OMNI_CAMERA_PROFILE="custom" +OMNI_CAMERA_CUSTOM_CTRL="brightness=8,auto_exposure=1,exposure_time_absolute=800,gain=64" +OMNI_CAMERA_VERIFY="1" +``` diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/dev/aggregate-latency-estimates.py b/robot/v4l2/OmniSocketGo_robot/scripts/dev/aggregate-latency-estimates.py new file mode 100644 index 0000000..e490d3c --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/dev/aggregate-latency-estimates.py @@ -0,0 +1,292 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import argparse +from datetime import datetime, timezone +import html +import json +from pathlib import Path +from typing import Any + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="Aggregate run logs into control/video latency estimate outputs.") + parser.add_argument("--run-dir", required=True, help="Run directory containing JSONL logs.") + parser.add_argument("--output-dir", help="Output directory. Defaults to --run-dir.") + return parser.parse_args() + + +def iter_jsonl(path: Path) -> list[dict[str, Any]]: + records: list[dict[str, Any]] = [] + if not path.exists(): + return records + with path.open("r", encoding="utf-8") as handle: + for raw_line in handle: + line = raw_line.strip() + if not line: + continue + try: + payload = json.loads(line) + except json.JSONDecodeError: + continue + if isinstance(payload, dict): + records.append(payload) + return records + + +def load_glob_jsonl(run_dir: Path, pattern: str) -> list[dict[str, Any]]: + records: list[dict[str, Any]] = [] + for path in sorted(run_dir.glob(pattern)): + records.extend(iter_jsonl(path)) + return records + + +def write_jsonl(path: Path, records: list[dict[str, Any]]) -> None: + with path.open("w", encoding="utf-8") as handle: + for record in records: + handle.write(json.dumps(record, ensure_ascii=False, separators=(",", ":"))) + handle.write("\n") + + +def parse_unix_ms(value: Any) -> int | None: + if value is None: + return None + if isinstance(value, (int, float)): + return int(value) + text = str(value).strip() + if not text: + return None + if text.endswith("Z"): + text = f"{text[:-1]}+00:00" + try: + return int(datetime.fromisoformat(text).astimezone(timezone.utc).timestamp() * 1000) + except ValueError: + return None + + +def flatten_net_epoch(samples: list[dict[str, Any]]) -> list[dict[str, Any]]: + flattened: list[dict[str, Any]] = [] + for sample in samples: + links = sample.get("links") or {} + a_to_d = (links.get("a_to_d") or {}).get("sessions") or {} + d_to_b = (links.get("d_to_b") or {}).get("sessions") or {} + a_control = (a_to_d.get("control") or {}).get("kcp") or {} + d_control = (d_to_b.get("control") or {}).get("kcp") or {} + a_video = (a_to_d.get("video") or {}).get("kcp") or {} + d_video = (d_to_b.get("video") or {}).get("kcp") or {} + flattened.append( + { + "updated_at": sample.get("updated_at"), + "a_to_d_control_srtt_ms": a_control.get("srtt_ms"), + "a_to_d_control_min_srtt_ms": a_control.get("min_srtt_ms"), + "d_to_b_control_srtt_ms": d_control.get("srtt_ms"), + "d_to_b_control_min_srtt_ms": d_control.get("min_srtt_ms"), + "a_to_d_video_srtt_ms": a_video.get("srtt_ms"), + "a_to_d_video_min_srtt_ms": a_video.get("min_srtt_ms"), + "d_to_b_video_srtt_ms": d_video.get("srtt_ms"), + "d_to_b_video_min_srtt_ms": d_video.get("min_srtt_ms"), + "a_to_d_control_feedback_age_ms": a_control.get("last_feedback_age_ms"), + "d_to_b_control_feedback_age_ms": d_control.get("last_feedback_age_ms"), + "a_to_d_video_feedback_age_ms": a_video.get("last_feedback_age_ms"), + "d_to_b_video_feedback_age_ms": d_video.get("last_feedback_age_ms"), + "a_to_d_control_retrans_delta": ((a_to_d.get("control") or {}).get("trend") or {}).get("retrans_delta"), + "d_to_b_control_retrans_delta": ((d_to_b.get("control") or {}).get("trend") or {}).get("retrans_delta"), + "a_to_d_video_retrans_delta": ((a_to_d.get("video") or {}).get("trend") or {}).get("retrans_delta"), + "d_to_b_video_retrans_delta": ((d_to_b.get("video") or {}).get("trend") or {}).get("retrans_delta"), + "a_to_d_video_window_pressure_pct": a_video.get("window_pressure_pct"), + "d_to_b_video_window_pressure_pct": d_video.get("window_pressure_pct"), + "robot_health": sample.get("robot_health"), + } + ) + return flattened + + +def aggregate_control_estimates( + network_samples: list[dict[str, Any]], + control_events: list[dict[str, Any]], + control_acks: list[dict[str, Any]], +) -> list[dict[str, Any]]: + if control_acks: + return control_acks + + fallback: list[dict[str, Any]] = [] + for sample in network_samples: + estimate = sample.get("latency_estimate") or {} + fallback.append( + { + "updated_at": sample.get("updated_at"), + "estimate_method": "srtt_fallback", + "control_loop_rtt_ms": estimate.get("control_loop_rtt_ms"), + "control_to_persist_est_ms": estimate.get("control_to_persist_est_ms"), + "control_oneway_srtt_est_ms": estimate.get("control_oneway_srtt_est_ms"), + "control_oneway_bestcase_est_ms": estimate.get("control_oneway_bestcase_est_ms"), + "source_event_count": len(control_events), + } + ) + return fallback + + +def aggregate_video_estimates( + network_samples: list[dict[str, Any]], + frame_recv_records: list[dict[str, Any]], + display_probe_records: list[dict[str, Any]], +) -> list[dict[str, Any]]: + network_timeline = sorted( + ( + (updated_at_ms, sample.get("latency_estimate") or {}) + for sample in network_samples + for updated_at_ms in [parse_unix_ms(sample.get("updated_at"))] + if updated_at_ms is not None + ), + key=lambda item: item[0], + ) + probes_by_seq = { + int(record["frame_seq"]): record + for record in display_probe_records + if record.get("frame_seq") is not None + } + estimates: list[dict[str, Any]] = [] + timeline_index = 0 + + for record in frame_recv_records: + frame_seq = record.get("frame_seq") + if frame_seq is None: + continue + probe = probes_by_seq.get(int(frame_seq)) + backend_received_unix_ns = record.get("backend_received_unix_ns") + backend_received_unix_ms = None + try: + if backend_received_unix_ns is not None: + backend_received_unix_ms = int(int(backend_received_unix_ns) / 1_000_000) + except (TypeError, ValueError): + backend_received_unix_ms = None + + latency_estimate: dict[str, Any] = {} + if backend_received_unix_ms is not None and network_timeline: + while timeline_index + 1 < len(network_timeline) and network_timeline[timeline_index + 1][0] <= backend_received_unix_ms: + timeline_index += 1 + if network_timeline[timeline_index][0] <= backend_received_unix_ms: + latency_estimate = network_timeline[timeline_index][1] + + network_oneway = latency_estimate.get("video_network_oneway_est_ms") + capture_to_send = record.get("b_side_capture_to_send_ms") + partial_est = None + if capture_to_send is not None or network_oneway is not None: + partial_est = round(float(capture_to_send or 0.0) + float(network_oneway or 0.0), 3) + request_to_paint_ms = None + if probe is not None and probe.get("request_to_paint_ms") is not None: + request_to_paint_ms = round(float(probe["request_to_paint_ms"]), 3) + elif probe is not None and probe.get("request_started_unix_ms") is not None and probe.get("paint_unix_ms") is not None: + request_to_paint_ms = round(float(probe["paint_unix_ms"]) - float(probe["request_started_unix_ms"]), 3) + video_e2e_est_ms = round(partial_est + request_to_paint_ms, 3) if partial_est is not None and request_to_paint_ms is not None else None + estimates.append( + { + "frame_seq": frame_seq, + "backend_received_unix_ns": record.get("backend_received_unix_ns"), + "frame_hash": record.get("frame_hash"), + "estimate_method": "capture_to_send+srtt/2+request_to_paint" if video_e2e_est_ms is not None else "capture_to_send+srtt/2", + "video_network_oneway_est_ms": network_oneway, + "b_side_capture_to_send_ms": capture_to_send, + "request_to_paint_ms": request_to_paint_ms, + "response_to_paint_ms": probe.get("response_to_paint_ms") if probe is not None else None, + "backend_to_request_ms": probe.get("backend_to_request_ms") if probe is not None else None, + "backend_to_request_ms_raw": probe.get("backend_to_request_ms_raw") if probe is not None else None, + "backend_to_paint_ms": probe.get("backend_to_paint_ms") if probe is not None else None, + "backend_to_paint_ms_raw": probe.get("backend_to_paint_ms_raw") if probe is not None else None, + "browser_backend_clock_offset_ms": probe.get("browser_backend_clock_offset_ms") if probe is not None else None, + "browser_backend_clock_rtt_ms": probe.get("browser_backend_clock_rtt_ms") if probe is not None else None, + "video_partial_est_ms": partial_est, + "video_e2e_est_ms": video_e2e_est_ms, + "sequence_gap": record.get("sequence_gap"), + "repeat_flag": record.get("repeat_flag"), + "sender_clock_delta_ms_raw": record.get("sender_clock_delta_ms_raw"), + } + ) + return estimates + + +def write_html_summary( + path: Path, + *, + net_epochs: list[dict[str, Any]], + control_estimates: list[dict[str, Any]], + video_estimates: list[dict[str, Any]], +) -> None: + latest_control = control_estimates[-1] if control_estimates else {} + latest_video = video_estimates[-1] if video_estimates else {} + latest_net = net_epochs[-1] if net_epochs else {} + html_text = f""" + + + + Latency Estimates + + + +

Latency Estimates

+
+
+

Control

+

loop RTT: {html.escape(str(latest_control.get("control_loop_rtt_ms")))}

+

to persist: {html.escape(str(latest_control.get("control_to_persist_est_ms")))}

+

method: {html.escape(str(latest_control.get("estimate_method")))}

+

samples: {len(control_estimates)}

+
+
+

Video

+

network one-way: {html.escape(str(latest_video.get("video_network_oneway_est_ms")))}

+

partial: {html.escape(str(latest_video.get("video_partial_est_ms")))}

+

end-to-end: {html.escape(str(latest_video.get("video_e2e_est_ms")))}

+

samples: {len(video_estimates)}

+
+
+

Net Epoch

+

a→d control srtt: {html.escape(str(latest_net.get("a_to_d_control_srtt_ms")))}

+

d→b control srtt: {html.escape(str(latest_net.get("d_to_b_control_srtt_ms")))}

+

a→d video srtt: {html.escape(str(latest_net.get("a_to_d_video_srtt_ms")))}

+

d→b video srtt: {html.escape(str(latest_net.get("d_to_b_video_srtt_ms")))}

+
+
+ + +""" + path.write_text(html_text, encoding="utf-8") + + +def main() -> int: + args = parse_args() + run_dir = Path(args.run_dir).resolve() + output_dir = Path(args.output_dir).resolve() if args.output_dir else run_dir + output_dir.mkdir(parents=True, exist_ok=True) + + network_samples = load_glob_jsonl(run_dir, "a-network-summary.*.jsonl") + control_events = load_glob_jsonl(run_dir, "a-control-events.*.jsonl") + control_acks = load_glob_jsonl(run_dir, "a-control-acks.*.jsonl") + frame_recv_records = load_glob_jsonl(run_dir, "a-video-frame-recv.*.jsonl") + display_probe_records = load_glob_jsonl(run_dir, "a-video-display-probe.*.jsonl") + + net_epochs = flatten_net_epoch(network_samples) + control_estimates = aggregate_control_estimates(network_samples, control_events, control_acks) + video_estimates = aggregate_video_estimates(network_samples, frame_recv_records, display_probe_records) + + write_jsonl(output_dir / "net-epoch-summary.jsonl", net_epochs) + write_jsonl(output_dir / "control-latency-estimates.jsonl", control_estimates) + write_jsonl(output_dir / "video-latency-estimates.jsonl", video_estimates) + write_html_summary( + output_dir / "latency-estimates.html", + net_epochs=net_epochs, + control_estimates=control_estimates, + video_estimates=video_estimates, + ) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/dev/apply-camera-controls.sh b/robot/v4l2/OmniSocketGo_robot/scripts/dev/apply-camera-controls.sh new file mode 100644 index 0000000..6b32c81 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/dev/apply-camera-controls.sh @@ -0,0 +1,111 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" + +camera_device="${OMNI_CAMERA_DEVICE}" +camera_profile="${OMNI_CAMERA_PROFILE}" +camera_brightness="${OMNI_CAMERA_BRIGHTNESS}" +camera_custom_ctrl="${OMNI_CAMERA_CUSTOM_CTRL}" +camera_verify="${OMNI_CAMERA_VERIFY}" + +is_truthy() { + case "${1:-0}" in + 1|true|TRUE|yes|YES|on|ON) + return 0 + ;; + *) + return 1 + ;; + esac +} + +require_v4l2_ctl() { + if command -v v4l2-ctl >/dev/null 2>&1; then + return 0 + fi + + echo "Missing required command: v4l2-ctl. Install v4l-utils on the robot side before starting b_side_omnid." >&2 + exit 1 +} + +run_v4l2_ctl() { + v4l2-ctl -d "${camera_device}" "$@" +} + +set_ctrl() { + local ctrl="$1" + + echo "[camera-controls] set ${camera_device} ${ctrl}" + run_v4l2_ctl "--set-ctrl=${ctrl}" +} + +verify_ctrl() { + local ctrl="$1" + + echo "[camera-controls] verify ${camera_device} ${ctrl}" + run_v4l2_ctl "--get-ctrl=${ctrl}" +} + +needs_v4l2_ctl=0 + +case "${camera_profile}" in + night) + needs_v4l2_ctl=1 + ;; + day) + if [[ -n "${camera_brightness}" ]]; then + needs_v4l2_ctl=1 + fi + ;; + custom) + if [[ -z "${camera_custom_ctrl}" ]]; then + echo "OMNI_CAMERA_CUSTOM_CTRL must be non-empty when OMNI_CAMERA_PROFILE=custom." >&2 + exit 1 + fi + needs_v4l2_ctl=1 + ;; + *) + echo "Unsupported OMNI_CAMERA_PROFILE: ${camera_profile}. Expected one of: night, day, custom." >&2 + exit 1 + ;; +esac + +if is_truthy "${camera_verify}"; then + needs_v4l2_ctl=1 +fi + +if [[ "${needs_v4l2_ctl}" == "0" ]]; then + echo "[camera-controls] profile=${camera_profile}; no camera controls requested" + exit 0 +fi + +require_v4l2_ctl + +case "${camera_profile}" in + night) + set_ctrl "auto_exposure=1" + set_ctrl "exposure_time_absolute=800" + set_ctrl "gain=64" + if [[ -n "${camera_brightness}" ]]; then + set_ctrl "brightness=${camera_brightness}" + fi + ;; + day) + if [[ -n "${camera_brightness}" ]]; then + set_ctrl "brightness=${camera_brightness}" + fi + ;; + custom) + set_ctrl "${camera_custom_ctrl}" + ;; +esac + +if is_truthy "${camera_verify}"; then + verify_ctrl "auto_exposure" + verify_ctrl "exposure_time_absolute" + verify_ctrl "gain" + verify_ctrl "brightness" +fi diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/dev/load-env.sh b/robot/v4l2/OmniSocketGo_robot/scripts/dev/load-env.sh new file mode 100644 index 0000000..42b44c0 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/dev/load-env.sh @@ -0,0 +1,335 @@ +#!/usr/bin/env bash +set -euo pipefail + +LOAD_ENV_SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +DEFAULT_OMNISOCKETGO_ROOT="$(cd "${LOAD_ENV_SCRIPT_DIR}/../.." && pwd)" + +die() { + echo "$*" >&2 + return 1 2>/dev/null || exit 1 +} + +normalize_loaded_env_vars() { + local var_name + local value + + for var_name in $(compgen -A variable); do + case "${var_name}" in + BACKEND_*|BLITZ_*|B_SIDE_*|CONTROL_*|FRONTEND_*|OMNI_*|PYTHON3_BIN|PYTHON_VENV_PATH|ROBOT_*|ROS_DISTRO|VITE_*) + value="${!var_name}" + if [[ "${value}" == *$'\r' ]]; then + printf -v "${var_name}" '%s' "${value%$'\r'}" + export "${var_name}" + fi + ;; + esac + done +} + +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" ]] +} + +require_robot_command_center_root() { + 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 ${LOAD_ENV_SCRIPT_DIR}/robot-remote.env.local if needed." + fi +} + +export OMNISOCKETGO_ROOT="${OMNISOCKETGO_ROOT:-${DEFAULT_OMNISOCKETGO_ROOT}}" + +omni_camera_device_was_set=0 +omni_camera_profile_was_set=0 +omni_camera_brightness_was_set=0 +omni_camera_custom_ctrl_was_set=0 +omni_camera_verify_was_set=0 + +if [[ "${OMNI_CAMERA_DEVICE+x}" == "x" ]]; then + omni_camera_device_was_set=1 + preserved_omni_camera_device="${OMNI_CAMERA_DEVICE}" +fi +if [[ "${OMNI_CAMERA_PROFILE+x}" == "x" ]]; then + omni_camera_profile_was_set=1 + preserved_omni_camera_profile="${OMNI_CAMERA_PROFILE}" +fi +if [[ "${OMNI_CAMERA_BRIGHTNESS+x}" == "x" ]]; then + omni_camera_brightness_was_set=1 + preserved_omni_camera_brightness="${OMNI_CAMERA_BRIGHTNESS}" +fi +if [[ "${OMNI_CAMERA_CUSTOM_CTRL+x}" == "x" ]]; then + omni_camera_custom_ctrl_was_set=1 + preserved_omni_camera_custom_ctrl="${OMNI_CAMERA_CUSTOM_CTRL}" +fi +if [[ "${OMNI_CAMERA_VERIFY+x}" == "x" ]]; then + omni_camera_verify_was_set=1 + preserved_omni_camera_verify="${OMNI_CAMERA_VERIFY}" +fi + +ENV_FILES=( + "${LOAD_ENV_SCRIPT_DIR}/robot-remote.env" + "${LOAD_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 + +normalize_loaded_env_vars + +if [[ "${omni_camera_device_was_set}" == "1" ]]; then + export OMNI_CAMERA_DEVICE="${preserved_omni_camera_device}" +fi +if [[ "${omni_camera_profile_was_set}" == "1" ]]; then + export OMNI_CAMERA_PROFILE="${preserved_omni_camera_profile}" +fi +if [[ "${omni_camera_brightness_was_set}" == "1" ]]; then + export OMNI_CAMERA_BRIGHTNESS="${preserved_omni_camera_brightness}" +fi +if [[ "${omni_camera_custom_ctrl_was_set}" == "1" ]]; then + export OMNI_CAMERA_CUSTOM_CTRL="${preserved_omni_camera_custom_ctrl}" +fi +if [[ "${omni_camera_verify_was_set}" == "1" ]]; then + export OMNI_CAMERA_VERIFY="${preserved_omni_camera_verify}" +fi + +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 + +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 OMNISOCKET_TELEMETRY_PEER_ID="${OMNISOCKET_TELEMETRY_PEER_ID:-peer-a-telemetry}" +export OMNISOCKET_TELEMETRY_INTERVAL_MS="${OMNISOCKET_TELEMETRY_INTERVAL_MS:-1000}" +export OMNISOCKET_TELEMETRY_STALE_AFTER_MS="${OMNISOCKET_TELEMETRY_STALE_AFTER_MS:-3000}" +export OMNI_NETWORK_SUMMARY_LOG_ENABLED="${OMNI_NETWORK_SUMMARY_LOG_ENABLED:-1}" +export OMNI_NETWORK_SUMMARY_LOG_PATH="${OMNI_NETWORK_SUMMARY_LOG_PATH:-${OMNISOCKETGO_ROOT}/logs/a-network-summary.jsonl}" +export OMNI_NETWORK_SUMMARY_LOG_INTERVAL_MS="${OMNI_NETWORK_SUMMARY_LOG_INTERVAL_MS:-1000}" +export OMNI_NETWORK_SUMMARY_LOG_REQUEST_TIMEOUT_SEC="${OMNI_NETWORK_SUMMARY_LOG_REQUEST_TIMEOUT_SEC:-3}" +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_CAMERA_DEVICE="${OMNI_CAMERA_DEVICE:-/dev/video0}" +export OMNI_CAMERA_HEAD_DEVICE="${OMNI_CAMERA_HEAD_DEVICE:-/dev/video26}" +export OMNI_CAMERA_WAIST_DEVICE="${OMNI_CAMERA_WAIST_DEVICE:-/dev/video18}" +export OMNI_CAMERA_AUTO_DISCOVER="${OMNI_CAMERA_AUTO_DISCOVER:-0}" +export OMNI_CAMERA_HEAD_SERIAL="${OMNI_CAMERA_HEAD_SERIAL:-}" +export OMNI_CAMERA_WAIST_SERIAL="${OMNI_CAMERA_WAIST_SERIAL:-}" +export OMNI_CAMERA_DISCOVERY_WIDTH="${OMNI_CAMERA_DISCOVERY_WIDTH:-1280}" +export OMNI_CAMERA_DISCOVERY_HEIGHT="${OMNI_CAMERA_DISCOVERY_HEIGHT:-720}" +export OMNI_CAMERA_DISCOVERY_TIMEOUT_SEC="${OMNI_CAMERA_DISCOVERY_TIMEOUT_SEC:-10}" +export OMNI_CAMERA_HEAD_RELEASE_SERVICE="${OMNI_CAMERA_HEAD_RELEASE_SERVICE:-orbbec_head.service}" +export OMNI_CAMERA_WAIST_RELEASE_SERVICE="${OMNI_CAMERA_WAIST_RELEASE_SERVICE:-orbbec_waist.service}" +export OMNI_CAMERA_ACTIVE="${OMNI_CAMERA_ACTIVE:-head}" +export OMNI_CAMERA_PROFILE="${OMNI_CAMERA_PROFILE:-night}" +export OMNI_CAMERA_BRIGHTNESS="${OMNI_CAMERA_BRIGHTNESS:-}" +export OMNI_CAMERA_CUSTOM_CTRL="${OMNI_CAMERA_CUSTOM_CTRL:-}" +export OMNI_CAMERA_VERIFY="${OMNI_CAMERA_VERIFY:-0}" +export OMNI_GPSD_HOST="${OMNI_GPSD_HOST:-127.0.0.1}" +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 OMNI_CONTROL_ACK_PEER_ID="${OMNI_CONTROL_ACK_PEER_ID:-peer-b-ctrl-ack}" +export OMNI_CONTROL_ACK_TARGET_PEER="${OMNI_CONTROL_ACK_TARGET_PEER:-peer-a-ctrl-ack}" +export B_SIDE_OMNID_USE_SUDO="${B_SIDE_OMNID_USE_SUDO:-1}" +export BLITZ_RUNTIME_DIR="${BLITZ_RUNTIME_DIR:-${OMNISOCKETGO_ROOT}/logs/runtime}" +export BLITZ_RUN_ROOT="${BLITZ_RUN_ROOT:-${OMNISOCKETGO_ROOT}/logs}" +export BLITZ_RUN_CONTEXT_FILE="${BLITZ_RUN_CONTEXT_FILE:-${BLITZ_RUNTIME_DIR}/run-context.env}" +export BLITZ_RUN_ID_FILE="${BLITZ_RUN_ID_FILE:-${BLITZ_RUNTIME_DIR}/run-id}" +export BLITZ_CURRENT_RUN_LINK="${BLITZ_CURRENT_RUN_LINK:-${BLITZ_RUN_ROOT}/current}" +export BLITZ_5G_INTERFACE="${BLITZ_5G_INTERFACE:-}" +export BLITZ_5G_MODEM_SUBNET="${BLITZ_5G_MODEM_SUBNET:-192.168.224.0/22}" +export BLITZ_5G_GATEWAY="${BLITZ_5G_GATEWAY:-192.168.225.1}" +export BLITZ_5G_ROUTE_TARGETS="${BLITZ_5G_ROUTE_TARGETS:-106.55.173.235}" +export BLITZ_5G_INFO_JSON="${BLITZ_5G_INFO_JSON:-${OMNISOCKETGO_ROOT}/scripts/boot/modem_network_info.json}" +export BLITZ_TIME_SERVER_IP="${BLITZ_TIME_SERVER_IP:-}" +export BLITZ_KCP_STATS_INTERVAL_MS="${BLITZ_KCP_STATS_INTERVAL_MS:-1000}" +export BLITZ_CONTROL_LATENCY_LOG_ENABLED="${BLITZ_CONTROL_LATENCY_LOG_ENABLED:-1}" +export BLITZ_CONTROL_LATENCY_LOG_SAMPLE_MOD="${BLITZ_CONTROL_LATENCY_LOG_SAMPLE_MOD:-100}" +export BLITZ_CONTROL_ACK_SAMPLE_MOD="${BLITZ_CONTROL_ACK_SAMPLE_MOD:-10}" +export BLITZ_VIDEO_STAGE_LOG_ENABLED="${BLITZ_VIDEO_STAGE_LOG_ENABLED:-1}" +export BLITZ_VIDEO_STAGE_LOG_SAMPLE_MOD="${BLITZ_VIDEO_STAGE_LOG_SAMPLE_MOD:-10}" +export BLITZ_5G_LINK_LOG_INTERVAL_SEC="${BLITZ_5G_LINK_LOG_INTERVAL_SEC:-5}" +export BLITZ_JSONL_FLUSH_INTERVAL_MS="${BLITZ_JSONL_FLUSH_INTERVAL_MS:-1000}" +export BLITZ_JSONL_FLUSH_BYTES="${BLITZ_JSONL_FLUSH_BYTES:-262144}" +export BLITZ_JSONL_ROTATE_BYTES="${BLITZ_JSONL_ROTATE_BYTES:-134217728}" +export BLITZ_JSONL_ROTATE_FILES="${BLITZ_JSONL_ROTATE_FILES:-8}" + +blitz_dev_utc_compact_timestamp() { + date -u '+%Y%m%dT%H%M%SZ' +} + +blitz_dev_git_commit() { + git -C "${OMNISOCKETGO_ROOT}" rev-parse HEAD 2>/dev/null || true +} + +blitz_dev_git_dirty_flag() { + if git -C "${OMNISOCKETGO_ROOT}" diff --quiet --ignore-submodules=dirty >/dev/null 2>&1; then + printf '0\n' + return 0 + fi + printf '1\n' +} + +blitz_dev_prepare_dirs() { + mkdir -p "${BLITZ_RUNTIME_DIR}" "${BLITZ_RUN_ROOT}/runs" "${BLITZ_RUN_ROOT}/incidents" +} + +blitz_dev_write_run_info() { + local run_dir="$1" + local run_id="$2" + local boot_id="$3" + local tmp_info="${run_dir}/run-info.json.tmp.$$" + local started_at + local commit_hash + local dirty_flag + + started_at="$(date -u '+%Y-%m-%dT%H:%M:%SZ')" + commit_hash="$(blitz_dev_git_commit)" + dirty_flag="$(blitz_dev_git_dirty_flag)" + + python3 - "${tmp_info}" "${run_id}" "${run_dir}" "${boot_id}" "${started_at}" "${commit_hash}" "${dirty_flag}" "${HOSTNAME:-$(hostname)}" <<'PY' +import json +import os +import sys + +path, run_id, run_dir, boot_id, started_at, commit_hash, dirty_flag, hostname = sys.argv[1:9] +payload = { + "run_id": run_id, + "run_dir": run_dir, + "boot_id": boot_id, + "started_at": started_at, + "hostname": hostname, + "git_commit": commit_hash, + "git_dirty": dirty_flag == "1", + "env": { + key: os.environ.get(key, "") + for key in sorted(os.environ) + if key.startswith(("BLITZ_", "OMNI_", "ROBOT_RECEIVER_")) + }, +} +with open(path, "w", encoding="utf-8") as handle: + json.dump(payload, handle, ensure_ascii=False, indent=2, sort_keys=True) +PY + mv -f "${tmp_info}" "${run_dir}/run-info.json" +} + +blitz_dev_init_run_context() { + local run_id="${1:-$(blitz_dev_utc_compact_timestamp)}" + local boot_id="dev-$(blitz_dev_utc_compact_timestamp)" + local run_dir="${BLITZ_RUN_ROOT}/runs/${run_id}" + local tmp_context="${BLITZ_RUN_CONTEXT_FILE}.tmp.$$" + + blitz_dev_prepare_dirs + mkdir -p "${run_dir}" + export BLITZ_RUN_ID="${run_id}" + export BLITZ_RUN_DIR="${run_dir}" + export BLITZ_BOOT_ID="${boot_id}" + printf '%s\n' "${run_id}" > "${BLITZ_RUN_ID_FILE}" + cat > "${tmp_context}" < None: + del signum, frame + global STOP_REQUESTED + STOP_REQUESTED = True + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="Poll /api/network/latest/ and append JSONL snapshots.") + parser.add_argument("--url", required=True, help="HTTP endpoint that returns the network summary JSON.") + parser.add_argument("--output", required=True, help="Output JSONL path.") + parser.add_argument( + "--interval-ms", + type=int, + default=2000, + help="Polling interval in milliseconds. Default: 2000.", + ) + parser.add_argument( + "--request-timeout-sec", + type=float, + default=3.0, + help="Single request timeout in seconds. Default: 3.0.", + ) + return parser.parse_args() + + +def sleep_with_stop(seconds: float) -> None: + deadline = time.monotonic() + max(0.0, seconds) + while not STOP_REQUESTED: + remaining = deadline - time.monotonic() + if remaining <= 0.0: + return + time.sleep(min(remaining, 0.2)) + + +def fetch_json(url: str, timeout_sec: float) -> str: + request = urllib.request.Request( + url, + headers={ + "Accept": "application/json", + "Cache-Control": "no-cache", + }, + method="GET", + ) + # This logger always polls the local backend. Ignore HTTP_PROXY/HTTPS_PROXY + # so a developer proxy cannot turn a 127.0.0.1 request into a 502. + with LOCAL_HTTP_OPENER.open(request, timeout=timeout_sec) as response: + charset = response.headers.get_content_charset("utf-8") + payload = response.read().decode(charset) + parsed = json.loads(payload) + return json.dumps(parsed, separators=(",", ":"), ensure_ascii=False) + + +def main() -> int: + args = parse_args() + interval_sec = max(args.interval_ms, 200) / 1000.0 + output_path = Path(args.output) + last_error_log_monotonic = 0.0 + + signal.signal(signal.SIGINT, handle_signal) + signal.signal(signal.SIGTERM, handle_signal) + + output_path.parent.mkdir(parents=True, exist_ok=True) + + with output_path.open("a", encoding="utf-8") as output_file: + while not STOP_REQUESTED: + started = time.monotonic() + try: + line = fetch_json(args.url, args.request_timeout_sec) + except (TimeoutError, urllib.error.URLError, urllib.error.HTTPError, json.JSONDecodeError) as error: + now = time.monotonic() + if now - last_error_log_monotonic >= 10.0: + print(f"[network-summary] poll failed: {error}", file=sys.stderr) + last_error_log_monotonic = now + else: + output_file.write(line) + output_file.write("\n") + output_file.flush() + + elapsed = time.monotonic() - started + sleep_with_stop(max(0.0, interval_sec - elapsed)) + + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/dev/prepare-camera-device.sh b/robot/v4l2/OmniSocketGo_robot/scripts/dev/prepare-camera-device.sh new file mode 100644 index 0000000..b847c6f --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/dev/prepare-camera-device.sh @@ -0,0 +1,108 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" + +camera_device="${OMNI_CAMERA_DEVICE}" +occupancy_policy="${OMNI_CAMERA_OCCUPANCY_POLICY:-check}" +release_service="${OMNI_CAMERA_RELEASE_SERVICE:-}" + +die() { + echo "[camera-preflight] $*" >&2 + exit 1 +} + +camera_pids() { + fuser "${camera_device}" 2>/dev/null \ + | tr ' ' '\n' \ + | grep -E '^[0-9]+$' \ + | sort -nu \ + || true +} + +pid_service() { + local pid="$1" + local service + + service="$(sed -nE 's#.*[/:]([^/:]+\.service)$#\1#p' "/proc/${pid}/cgroup" 2>/dev/null | head -1)" + printf '%s' "${service:-unknown}" +} + +report_owners() { + local pid + local comm + local service + local found=0 + + while read -r pid; do + [[ -n "${pid}" ]] || continue + found=1 + comm="$(cat "/proc/${pid}/comm" 2>/dev/null || printf 'unknown')" + service="$(pid_service "${pid}")" + echo "[camera-preflight] owner pid=${pid} command=${comm} service=${service}" >&2 + done < <(camera_pids) + + if [[ "${found}" == "1" ]]; then + return 0 + fi + return 1 +} + +has_proc_manager_owner() { + local pid + + while read -r pid; do + [[ -n "${pid}" ]] || continue + if [[ "$(pid_service "${pid}")" == "proc_manager.service" ]]; then + return 0 + fi + done < <(camera_pids) + + return 1 +} + +if [[ ! -e "${camera_device}" ]]; then + die "camera device does not exist: ${camera_device}" +fi + +if ! command -v fuser >/dev/null 2>&1; then + die "missing required command: fuser (install the psmisc package)" +fi + +resolved_device="$(readlink -f "${camera_device}" 2>/dev/null || printf '%s' "${camera_device}")" +echo "[camera-preflight] checking ${camera_device} (${resolved_device}) policy=${occupancy_policy}" >&2 + +if ! report_owners; then + echo "[camera-preflight] ${camera_device} is free" >&2 + exit 0 +fi + +case "${occupancy_policy}" in + check) + die "${camera_device} is busy; no process was stopped" + ;; + release-known) + if [[ -z "${release_service}" ]]; then + die "OMNI_CAMERA_RELEASE_SERVICE is required when policy=release-known" + fi + + echo "[camera-preflight] stopping known camera service ${release_service}" >&2 + systemctl stop "${release_service}" + + if ! report_owners; then + echo "[camera-preflight] ${camera_device} was released by ${release_service}" >&2 + exit 0 + fi + + if has_proc_manager_owner; then + die "${camera_device} is still owned by proc_manager.service; refusing to stop the whole process manager. Unload the owning ROS component explicitly." + fi + + die "${camera_device} remains busy after stopping ${release_service}" + ;; + *) + die "unsupported OMNI_CAMERA_OCCUPANCY_POLICY=${occupancy_policy}; expected check or release-known" + ;; +esac diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/dev/reset-run-context.sh b/robot/v4l2/OmniSocketGo_robot/scripts/dev/reset-run-context.sh new file mode 100644 index 0000000..4b9233d --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/dev/reset-run-context.sh @@ -0,0 +1,10 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +export BLITZ_SKIP_DEV_RUN_CONTEXT_INIT="1" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" + +blitz_dev_reset_run_context +printf 'run_id=%s\nrun_dir=%s\n' "${BLITZ_RUN_ID}" "${BLITZ_RUN_DIR}" diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/dev/resolve-camera-device.sh b/robot/v4l2/OmniSocketGo_robot/scripts/dev/resolve-camera-device.sh new file mode 100644 index 0000000..42074c1 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/dev/resolve-camera-device.sh @@ -0,0 +1,59 @@ +#!/usr/bin/env bash +set -euo pipefail + +camera_serial="${1:-}" +camera_label="${2:-camera}" +capture_width="${OMNI_CAMERA_DISCOVERY_WIDTH:-1280}" +capture_height="${OMNI_CAMERA_DISCOVERY_HEIGHT:-720}" +timeout_sec="${OMNI_CAMERA_DISCOVERY_TIMEOUT_SEC:-10}" + +die() { + echo "[camera-discovery] ${camera_label}: $*" >&2 + exit 1 +} + +[[ -n "${camera_serial}" ]] || die "camera serial is required" +[[ "${timeout_sec}" =~ ^[0-9]+$ ]] || die "OMNI_CAMERA_DISCOVERY_TIMEOUT_SEC must be a non-negative integer" + +command -v udevadm >/dev/null 2>&1 || die "missing required command: udevadm" +command -v v4l2-ctl >/dev/null 2>&1 || die "missing required command: v4l2-ctl" + +shopt -s nullglob +deadline=$((SECONDS + timeout_sec)) + +while true; do + matches=() + serial_nodes=() + + for device in /dev/video*; do + [[ -c "${device}" ]] || continue + device_serial="$( + udevadm info --query=property --name="${device}" 2>/dev/null \ + | sed -n 's/^ID_SERIAL_SHORT=//p' \ + | head -1 + )" + [[ "${device_serial}" == "${camera_serial}" ]] || continue + serial_nodes+=("${device}") + + formats="$(v4l2-ctl -d "${device}" --list-formats-ext 2>/dev/null || true)" + grep -q "'MJPG'" <<<"${formats}" || continue + grep -q "Size: Discrete ${capture_width}x${capture_height}" <<<"${formats}" || continue + matches+=("${device}") + done + + if (( ${#matches[@]} == 1 )); then + echo "[camera-discovery] ${camera_label}: serial=${camera_serial} -> ${matches[0]} (MJPG ${capture_width}x${capture_height})" >&2 + printf '%s\n' "${matches[0]}" + exit 0 + fi + if (( ${#matches[@]} > 1 )); then + die "serial=${camera_serial} matched multiple MJPG nodes: ${matches[*]}" + fi + if (( SECONDS >= deadline )); then + if (( ${#serial_nodes[@]} == 0 )); then + die "serial=${camera_serial} was not found under /dev/video*" + fi + die "serial=${camera_serial} has no MJPG ${capture_width}x${capture_height} node; serial nodes: ${serial_nodes[*]}" + fi + sleep 0.2 +done diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/dev/robot-remote.env b/robot/v4l2/OmniSocketGo_robot/scripts/dev/robot-remote.env new file mode 100644 index 0000000..bb63cb3 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/dev/robot-remote.env @@ -0,0 +1,87 @@ +# 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="192.168.41.144:10909" # Local LAN hub +CONTROL_SIDE_OMNISOCKET_RELAY_VIA="" # No relay + +ROBOT_SIDE_OMNISOCKET_SERVER_ADDR="192.168.41.144:10909" # Local LAN hub +ROBOT_SIDE_OMNISOCKET_RELAY_VIA="" # Direct LAN +# Log one normal relay packet out of every N packets. Drop events still log immediately. +OMNI_RELAY_PACKET_LOG_SAMPLE_EVERY="200" + +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" +OMNISOCKET_TELEMETRY_PEER_ID="peer-a-telemetry" +OMNISOCKET_TELEMETRY_INTERVAL_MS="1000" +OMNISOCKET_TELEMETRY_STALE_AFTER_MS="3000" +OMNI_NETWORK_SUMMARY_LOG_ENABLED="1" +OMNI_NETWORK_SUMMARY_LOG_PATH="${OMNISOCKETGO_ROOT}/logs/a-network-summary.jsonl" +OMNI_NETWORK_SUMMARY_LOG_INTERVAL_MS="1000" +OMNI_NETWORK_SUMMARY_LOG_REQUEST_TIMEOUT_SEC="3" + +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_GPSD_HOST="127.0.0.1" +OMNI_CAMERA_HEAD_DEVICE="/dev/video26" +OMNI_CAMERA_WAIST_DEVICE="/dev/video18" +OMNI_CAMERA_AUTO_DISCOVER="1" +OMNI_CAMERA_HEAD_SERIAL="CP9E163000H3" +OMNI_CAMERA_WAIST_SERIAL="CPCK8530005N" +OMNI_CAMERA_DISCOVERY_WIDTH="1280" +OMNI_CAMERA_DISCOVERY_HEIGHT="720" +OMNI_CAMERA_DISCOVERY_TIMEOUT_SEC="10" +OMNI_CAMERA_HEAD_RELEASE_SERVICE="orbbec_head.service" +OMNI_CAMERA_WAIST_RELEASE_SERVICE="orbbec_waist.service" +OMNI_CAMERA_ACTIVE="head" +OMNI_CAMERA_OCCUPANCY_POLICY="release-known" +OMNI_CAMERA_PROFILE="day" +OMNI_CAMERA_BRIGHTNESS="" +OMNI_CAMERA_CUSTOM_CTRL="" +OMNI_CAMERA_VERIFY="0" +OMNI_VIDEO_SERVER_ADDR="${ROBOT_SIDE_OMNISOCKET_SERVER_ADDR}" +OMNI_VIDEO_RELAY_VIA="${ROBOT_SIDE_OMNISOCKET_RELAY_VIA}" +OMNI_VIDEO_SOFT_BACKPRESSURE_SEGMENTS="256" +OMNI_VIDEO_HARD_BACKPRESSURE_SEGMENTS="1024" +OMNI_VIDEO_HARD_BACKPRESSURE_HOLD_MS="5000" +OMNI_VIDEO_FRAME_STALL_RECONNECT_MS="30000" +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}" +OMNI_CONTROL_ACK_PEER_ID="peer-b-ctrl-ack" +OMNI_CONTROL_ACK_TARGET_PEER="peer-a-ctrl-ack" +BLITZ_CONTROL_ACK_SAMPLE_MOD="10" +BLITZ_VIDEO_STAGE_LOG_ENABLED="1" +BLITZ_VIDEO_STAGE_LOG_SAMPLE_MOD="10" +OMNI_CONTROL_SERVER_IDLE_RECONNECT_MS="30000" + +# A-side backend video freshness guard. Used by scripts/dev/start-backend.sh. +OMNI_VIDEO_MAX_FRAME_AGE_MS="1000" + +B_SIDE_OMNID_USE_SUDO="1" diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/dev/start-5g-link-logger.sh b/robot/v4l2/OmniSocketGo_robot/scripts/dev/start-5g-link-logger.sh new file mode 100644 index 0000000..093928f --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/dev/start-5g-link-logger.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" + +blitz_dev_prepare_5g_logging_env +exec bash "${OMNISOCKETGO_ROOT}/scripts/boot/blitz-5g-link-logger.sh" diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/dev/start-b-side-omnid.sh b/robot/v4l2/OmniSocketGo_robot/scripts/dev/start-b-side-omnid.sh new file mode 100644 index 0000000..cbf6eb4 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/dev/start-b-side-omnid.sh @@ -0,0 +1,100 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" +blitz_dev_prepare_bside_logging_env + +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}" + +logger_pid="" + +cleanup() { + if [[ -n "${logger_pid}" ]]; then + kill "${logger_pid}" 2>/dev/null || true + wait "${logger_pid}" 2>/dev/null || true + fi +} + +start_5g_link_logger_if_needed() { + if [[ "${OMNI_5G_LINK_LOG_ENABLED:-1}" != "1" ]]; then + echo "[start-b-side-omnid] 5G link logger disabled" >&2 + return 0 + fi + if [[ "${OMNI_BOOT_MODE:-0}" == "1" ]]; then + return 0 + fi + bash "${SCRIPT_DIR}/start-5g-link-logger.sh" & + logger_pid=$! + echo "[start-b-side-omnid] 5G link logger -> ${BLITZ_5G_LINK_LOG_PATH:-unset}" >&2 +} + +release_known_camera_service() { + local service="$1" + + if [[ "${OMNI_CAMERA_OCCUPANCY_POLICY:-check}" != "release-known" || -z "${service}" ]]; then + return 0 + fi + if systemctl is-active --quiet "${service}"; then + echo "[start-b-side-omnid] stopping known camera service ${service} before discovery" >&2 + systemctl stop "${service}" + fi +} + +resolve_camera_devices() { + if [[ "${OMNI_CAMERA_AUTO_DISCOVER:-0}" != "1" ]]; then + return 0 + fi + + OMNI_CAMERA_HEAD_DEVICE="$( + bash "${SCRIPT_DIR}/resolve-camera-device.sh" "${OMNI_CAMERA_HEAD_SERIAL}" head + )" + OMNI_CAMERA_WAIST_DEVICE="$( + bash "${SCRIPT_DIR}/resolve-camera-device.sh" "${OMNI_CAMERA_WAIST_SERIAL}" waist + )" + if [[ "${OMNI_CAMERA_HEAD_DEVICE}" == "${OMNI_CAMERA_WAIST_DEVICE}" ]]; then + echo "[start-b-side-omnid] head and waist resolved to the same device: ${OMNI_CAMERA_HEAD_DEVICE}" >&2 + return 1 + fi + export OMNI_CAMERA_HEAD_DEVICE OMNI_CAMERA_WAIST_DEVICE + echo "[start-b-side-omnid] resolved head=${OMNI_CAMERA_HEAD_DEVICE} waist=${OMNI_CAMERA_WAIST_DEVICE}" >&2 +} + +if [[ ! -x "./bin/b_side_omnid" ]]; then + if [[ "${OMNI_BOOT_MODE:-0}" == "1" ]]; then + echo "Missing ./bin/b_side_omnid in boot mode; build it before enabling the autostart service." >&2 + exit 1 + fi + make b_side_omnid +fi + +launch_b_side_omnid() { + trap cleanup EXIT INT TERM + start_5g_link_logger_if_needed + release_known_camera_service "${OMNI_CAMERA_HEAD_RELEASE_SERVICE:-orbbec_head.service}" + release_known_camera_service "${OMNI_CAMERA_WAIST_RELEASE_SERVICE:-orbbec_waist.service}" + resolve_camera_devices + OMNI_CAMERA_DEVICE="${OMNI_CAMERA_HEAD_DEVICE}" \ + OMNI_CAMERA_RELEASE_SERVICE="${OMNI_CAMERA_HEAD_RELEASE_SERVICE:-orbbec_head.service}" \ + bash "${SCRIPT_DIR}/prepare-camera-device.sh" + OMNI_CAMERA_DEVICE="${OMNI_CAMERA_WAIST_DEVICE}" \ + OMNI_CAMERA_RELEASE_SERVICE="${OMNI_CAMERA_WAIST_RELEASE_SERVICE:-orbbec_waist.service}" \ + bash "${SCRIPT_DIR}/prepare-camera-device.sh" + OMNI_CAMERA_DEVICE="${OMNI_CAMERA_HEAD_DEVICE}" bash "${SCRIPT_DIR}/apply-camera-controls.sh" + OMNI_CAMERA_DEVICE="${OMNI_CAMERA_WAIST_DEVICE}" bash "${SCRIPT_DIR}/apply-camera-controls.sh" + ./bin/b_side_omnid +} + +if [[ "${B_SIDE_OMNID_USE_SUDO}" == "1" && "${EUID}" -ne 0 ]]; then + exec sudo -E bash -lc 'cd "$1" && export B_SIDE_OMNID_USE_SUDO=0 && exec bash "$2"' _ "${OMNISOCKETGO_ROOT}" "${SCRIPT_DIR}/start-b-side-omnid.sh" +fi + +launch_b_side_omnid diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/dev/start-backend.sh b/robot/v4l2/OmniSocketGo_robot/scripts/dev/start-backend.sh new file mode 100644 index 0000000..bc8e4d4 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/dev/start-backend.sh @@ -0,0 +1,60 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" +require_robot_command_center_root +blitz_dev_prepare_backend_logging_env + +if [[ ! -x "${PYTHON_VENV_PATH}/bin/python" ]]; then + echo "[start-backend] creating or repairing virtualenv at ${PYTHON_VENV_PATH}" >&2 + "${PYTHON3_BIN}" -m venv "${PYTHON_VENV_PATH}" +fi + +if [[ ! -x "${PYTHON_VENV_PATH}/bin/python" ]]; then + echo "[start-backend] virtualenv is incomplete: missing ${PYTHON_VENV_PATH}/bin/python" >&2 + exit 1 +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}" + +logger_pid="" + +cleanup() { + if [[ -n "${logger_pid}" ]]; then + kill "${logger_pid}" 2>/dev/null || true + wait "${logger_pid}" 2>/dev/null || true + fi +} + +start_network_summary_logger() { + local logger_url + local logger_dir + + if [[ "${OMNI_NETWORK_SUMMARY_LOG_ENABLED}" != "1" ]]; then + return + fi + + logger_url="http://127.0.0.1:${BACKEND_PORT}/api/network/latest/" + logger_dir="$(dirname "${OMNI_NETWORK_SUMMARY_LOG_PATH}")" + mkdir -p "${logger_dir}" + + python "${SCRIPT_DIR}/log-network-summary.py" \ + --url "${logger_url}" \ + --output "${OMNI_NETWORK_SUMMARY_LOG_PATH}" \ + --interval-ms "${OMNI_NETWORK_SUMMARY_LOG_INTERVAL_MS}" \ + --request-timeout-sec "${OMNI_NETWORK_SUMMARY_LOG_REQUEST_TIMEOUT_SEC}" & + logger_pid=$! + echo "[start-backend] network summary logger -> ${OMNI_NETWORK_SUMMARY_LOG_PATH} (${OMNI_NETWORK_SUMMARY_LOG_INTERVAL_MS} ms)" >&2 +} + +trap cleanup EXIT INT TERM + +start_network_summary_logger +python -m uvicorn config.asgi:application --host "${BACKEND_HOST}" --port "${BACKEND_PORT}" diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/dev/start-dev-tmux.sh b/robot/v4l2/OmniSocketGo_robot/scripts/dev/start-dev-tmux.sh new file mode 100644 index 0000000..e9c2fe2 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/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}" diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/dev/start-frontend.sh b/robot/v4l2/OmniSocketGo_robot/scripts/dev/start-frontend.sh new file mode 100644 index 0000000..b33a87a --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/dev/start-frontend.sh @@ -0,0 +1,10 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" +require_robot_command_center_root + +cd "${FRONTEND_DIR}" +exec npm run dev -- --host "${FRONTEND_HOST}" --port "${FRONTEND_PORT}" diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/dev/start-local-hub.sh b/robot/v4l2/OmniSocketGo_robot/scripts/dev/start-local-hub.sh new file mode 100644 index 0000000..3ca0c0f --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/dev/start-local-hub.sh @@ -0,0 +1,22 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" + +hub_binary="${OMNISOCKETGO_ROOT}/bin/kcpserver" +hub_listen_addr="${LOCAL_HUB_LISTEN_ADDR:-0.0.0.0:10909}" +telemetry_peer_id="${LOCAL_HUB_TELEMETRY_PEER_ID:-peer-a-telemetry}" +telemetry_interval="${LOCAL_HUB_TELEMETRY_INTERVAL:-1000ms}" + +if [[ ! -x "${hub_binary}" ]]; then + echo "[start-local-hub] missing executable ${hub_binary}; run: make bin/kcpserver" >&2 + exit 1 +fi + +echo "[start-local-hub] listen=${hub_listen_addr} relay=disabled telemetry_peer=${telemetry_peer_id}" >&2 +exec "${hub_binary}" \ + -listen "${hub_listen_addr}" \ + -telemetry-peer "${telemetry_peer_id}" \ + -telemetry-interval "${telemetry_interval}" diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/dev/start-ros-receiver.sh b/robot/v4l2/OmniSocketGo_robot/scripts/dev/start-ros-receiver.sh new file mode 100644 index 0000000..6c90630 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/dev/start-ros-receiver.sh @@ -0,0 +1,50 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" + +source_with_nounset_off() { + set +u + # shellcheck disable=SC1090 + source "$1" + set -u +} + +# shellcheck disable=SC1091 +source "${SCRIPT_DIR}/load-env.sh" +if [[ ! -f "/opt/ros/${ROS_DISTRO}/setup.bash" ]]; then + echo "Missing ROS distro setup: /opt/ros/${ROS_DISTRO}/setup.bash" >&2 + exit 1 +fi +source_with_nounset_off "/opt/ros/${ROS_DISTRO}/setup.bash" + +cd "${ROS_CONTROL_PY_DIR}" +if [[ ! -f "install/setup.bash" ]]; then + echo "Missing ROS workspace setup: ${ROS_CONTROL_PY_DIR}/install/setup.bash" >&2 + exit 1 +fi +source_with_nounset_off "install/setup.bash" + +launch_args=( + "transport:=${ROBOT_RECEIVER_TRANSPORT}" + "peer_id:=${ROBOT_RECEIVER_PEER_ID}" + "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}" +) + +if [[ -n "${ROBOT_RECEIVER_SERVER_ADDR}" ]]; then + launch_args+=("server_addr:=${ROBOT_RECEIVER_SERVER_ADDR}") +fi + +if [[ -n "${ROBOT_RECEIVER_RELAY_VIA}" ]]; then + launch_args+=("relay_via:=${ROBOT_RECEIVER_RELAY_VIA}") +fi + +if [[ -n "${ROBOT_RECEIVER_EXPECTED_SENDER}" ]]; then + launch_args+=("expected_sender:=${ROBOT_RECEIVER_EXPECTED_SENDER}") +fi + +exec ros2 launch udp_teleop_bridge robot_udp_receiver.launch.py "${launch_args[@]}" diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/kcp_control_benchmark.py b/robot/v4l2/OmniSocketGo_robot/scripts/kcp_control_benchmark.py new file mode 100644 index 0000000..90c5b23 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/kcp_control_benchmark.py @@ -0,0 +1,76 @@ +"""Send high-rate control packets to benchmark the KCP control session.""" + +from __future__ import annotations + +import argparse +from pathlib import Path +import sys +import time + +ROOT = Path(__file__).resolve().parents[1] +sys.path.insert(0, str(ROOT)) + +import yaml + +from omnisocket_control import make_control_packet + +try: + from omnisocket import CONTROL_DEFAULTS, Session +except ImportError: + sys.path.insert(0, str(ROOT / "python")) + from omnisocket import CONTROL_DEFAULTS, Session + + +def load_config() -> dict: + config_path = ROOT / "config" / "omnisocket_demo.yaml" + if not config_path.exists(): + return {} + with config_path.open("r", encoding="utf-8") as file: + return yaml.safe_load(file) or {} + + +def main() -> None: + parser = argparse.ArgumentParser() + parser.add_argument("--rate", type=float, default=200.0, help="send rate in Hz") + parser.add_argument("--count", type=int, default=1000, help="packets to send") + args = parser.parse_args() + + config = load_config() + transport_cfg = config.get("transport", {}) + sender_cfg = config.get("control_sender", {}) + + session = Session() + session.connect( + server_addr=str(transport_cfg.get("server_addr", "127.0.0.1:10909")), + peer_id=str(sender_cfg.get("peer_id", "peer-a-ctrl")), + relay_via=str(transport_cfg.get("relay_via", "")), + bind_ip=str(transport_cfg.get("bind_ip", "")), + bind_device=str(transport_cfg.get("bind_device", "")), + **CONTROL_DEFAULTS, + ) + + target_peer = str(sender_cfg.get("target_peer", "peer-b-ctrl")) + spacing = 1.0 / args.rate if args.rate > 0 else 0.0 + start = time.perf_counter() + + try: + for seq_id in range(args.count): + packet = make_control_packet(seq_id, "set_surge", drive_value=0.25) + session.send(to=target_peer, data=packet.encode()) + if spacing > 0: + target = start + (seq_id + 1) * spacing + remaining = target - time.perf_counter() + if remaining > 0: + time.sleep(remaining) + finally: + elapsed = time.perf_counter() - start + print( + f"sent {args.count} control packets in {elapsed:.3f}s " + f"({(args.count / elapsed) if elapsed > 0 else 0.0:.1f} pkt/s)" + ) + print(f"stats={session.stats()}") + session.close() + + +if __name__ == "__main__": + main() diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/refresh-latency-summary.sh b/robot/v4l2/OmniSocketGo_robot/scripts/refresh-latency-summary.sh new file mode 100644 index 0000000..6734e65 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/refresh-latency-summary.sh @@ -0,0 +1,136 @@ +#!/usr/bin/env bash + +set -u +set -o pipefail + +repo_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" +remote_source="boll@175.178.116.187:/home/boll/LMJWork/OmniSocketGo/peer-b-latency.jsonl" +local_peer_a="$repo_dir/peer-a-latency.jsonl" +local_peer_b="$repo_dir/peer-b-latency.jsonl" +summary_output="$repo_dir/latency-summary.jsonl" +chart_output="$repo_dir/latency-summary.html" +latency_binary="$repo_dir/bin/latencysummary" +go_cache_dir="${GOCACHE:-/tmp/omnisocketgo-go-build}" +poll_interval_seconds=1 + +remote_tmp="" +summary_tmp="" +chart_tmp="" + +log() { + printf '[%s] %s\n' "$(date '+%Y-%m-%d %H:%M:%S')" "$*" +} + +cleanup_temp_file() { + local path="$1" + if [[ -n "$path" && -e "$path" ]]; then + rm -f "$path" + fi +} + +cleanup() { + cleanup_temp_file "$remote_tmp" + cleanup_temp_file "$summary_tmp" + cleanup_temp_file "$chart_tmp" +} + +handle_interrupt() { + log "received interrupt signal, stopping refresh loop" + cleanup + exit 130 +} + +handle_terminate() { + log "received terminate signal, stopping refresh loop" + cleanup + exit 143 +} + +trap cleanup EXIT +trap handle_interrupt INT +trap handle_terminate TERM + +cd "$repo_dir" || exit 1 + +mkdir -p "$repo_dir/bin" +mkdir -p "$go_cache_dir" +if ! GOCACHE="$go_cache_dir" go build -o "$latency_binary" ./cmd/latencysummary; then + log "build failed; exiting" + exit 1 +fi + +log "starting 1-second refresh loop" + +while true; do + remote_tmp="$(mktemp "$repo_dir/peer-b-latency.jsonl.tmp.XXXXXX")" || exit 1 + if scp -P 10022 "$remote_source" "$remote_tmp"; then + if mv -f "$remote_tmp" "$local_peer_b"; then + remote_tmp="" + else + status=$? + log "failed to replace $(basename "$local_peer_b") after scp (exit $status)" + cleanup_temp_file "$remote_tmp" + remote_tmp="" + sleep "$poll_interval_seconds" + continue + fi + else + status=$? + log "scp refresh failed (exit $status)" + cleanup_temp_file "$remote_tmp" + remote_tmp="" + sleep "$poll_interval_seconds" + continue + fi + + summary_tmp="$(mktemp "$repo_dir/latency-summary.tmp.XXXXXX.jsonl")" || exit 1 + chart_tmp="${summary_tmp%.jsonl}.html" + if "$latency_binary" \ + -input "$local_peer_a" \ + -input "$local_peer_b" \ + -shared-max-offset 1 \ + -output "$summary_tmp"; then + if [[ ! -f "$summary_tmp" || ! -f "$chart_tmp" ]]; then + log "summary succeeded but temporary outputs are incomplete" + cleanup_temp_file "$summary_tmp" + cleanup_temp_file "$chart_tmp" + summary_tmp="" + chart_tmp="" + sleep "$poll_interval_seconds" + continue + fi + + if ! mv -f "$summary_tmp" "$summary_output"; then + status=$? + log "failed to replace $(basename "$summary_output") (exit $status)" + cleanup_temp_file "$summary_tmp" + cleanup_temp_file "$chart_tmp" + summary_tmp="" + chart_tmp="" + sleep "$poll_interval_seconds" + continue + fi + summary_tmp="" + + if ! mv -f "$chart_tmp" "$chart_output"; then + status=$? + log "failed to replace $(basename "$chart_output") (exit $status)" + cleanup_temp_file "$chart_tmp" + chart_tmp="" + sleep "$poll_interval_seconds" + continue + fi + chart_tmp="" + else + status=$? + log "latency summary refresh failed (exit $status)" + cleanup_temp_file "$summary_tmp" + cleanup_temp_file "$chart_tmp" + summary_tmp="" + chart_tmp="" + sleep "$poll_interval_seconds" + continue + fi + + sleep "$poll_interval_seconds" +done diff --git a/robot/v4l2/OmniSocketGo_robot/scripts/run-kcp-batch-test.sh b/robot/v4l2/OmniSocketGo_robot/scripts/run-kcp-batch-test.sh new file mode 100644 index 0000000..29c29d7 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/scripts/run-kcp-batch-test.sh @@ -0,0 +1,1202 @@ +#!/usr/bin/env bash + +set -euo pipefail + +script_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +repo_dir="$(cd "$script_dir/.." && pwd)" +script_name="$(basename "$0")" + +run_mode="direct" +server_ssh="" +peerb_ssh="" +relay_ssh="" +server_addr="" +relay_addr="" +relay_remote="" +log_prefix="" +listen_addr="0.0.0.0:10909" +relay_listen_addr="0.0.0.0:10909" +server_workdir="$repo_dir" +peerb_workdir="$repo_dir" +relay_workdir="$repo_dir" +local_workdir="$repo_dir" +ready_timeout=60 +send_interval=1 +drain_wait=5 +repeat_count=1 + +declare -a peerb_files=() + +server_started=0 +relay_started=0 +peer_b_started=0 +peer_a_pid="" + +usage() { + printf 'Usage:\n' + printf ' %s --mode --server-ssh --peerb-ssh \\\n' "$script_name" + printf ' --server-addr --log-prefix --file [options]\n' + printf '\n' + printf 'Modes:\n' + printf ' direct peer-a -> hub(server) <- peer-b (default)\n' + printf ' relay peer-a -> relay(C) -> hub(D) <- peer-b\n' + printf '\n' + printf 'Required arguments:\n' + printf ' --server-ssh SSH target for the hub server machine\n' + printf ' --peerb-ssh SSH target for the peer-b machine\n' + printf ' --server-addr Hub server IP (combined with listen port for peers)\n' + printf ' --log-prefix Log directory prefix; logs go under logs/\n' + printf ' --file Existing file path on peer-b; repeat for multiple files\n' + printf '\n' + printf 'Relay mode arguments (required when --mode=relay):\n' + printf ' --relay-ssh SSH target for the relay server machine\n' + printf ' --relay-addr Relay server IP (combined with relay listen port for peer-a)\n' + printf ' --relay-remote Hub address from relay perspective (relay -relay-remote)\n' + printf '\n' + printf 'Options:\n' + printf ' --mode Run mode (default: %s)\n' "$run_mode" + printf ' --listen-addr Hub server listen address (default: %s)\n' "$listen_addr" + printf ' --relay-listen-addr Relay server listen address (default: %s)\n' "$relay_listen_addr" + printf ' --server-workdir Hub server-side workdir (default: %s)\n' "$server_workdir" + printf ' --relay-workdir Relay server-side workdir (default: %s)\n' "$relay_workdir" + printf ' --peerb-workdir Peer-b-side workdir (default: %s)\n' "$peerb_workdir" + printf ' --local-workdir Local peer-a workdir (default: %s)\n' "$local_workdir" + printf ' --ready-timeout Startup wait timeout (default: %s)\n' "$ready_timeout" + printf ' --repeat Repeat the full --file list this many rounds (default: %s)\n' "$repeat_count" + printf ' --send-interval Delay between file commands (default: %s)\n' "$send_interval" + printf ' --drain-wait Wait after the last file before quit (default: %s)\n' "$drain_wait" + printf ' -h, --help Show this help\n' + printf '\n' + printf 'Example (direct mode):\n' + printf ' %s \\\n' "$script_name" + printf ' --mode direct \\\n' + printf ' --server-ssh root@server-host \\\n' + printf ' --peerb-ssh root@peer-b-host \\\n' + printf ' --server-addr 203.0.113.10 \\\n' + printf ' --log-prefix case01- \\\n' + printf ' --repeat 30 \\\n' + printf ' --file /tmp/test125.bin\n' + printf '\n' + printf 'Example (relay mode):\n' + printf ' %s \\\n' "$script_name" + printf ' --mode relay \\\n' + printf ' --server-ssh root@hub-host \\\n' + printf ' --relay-ssh root@relay-host \\\n' + printf ' --peerb-ssh root@peer-b-host \\\n' + printf ' --server-addr 152.136.164.246 \\\n' + printf ' --relay-addr 139.199.57.110 \\\n' + printf ' --relay-remote 172.21.0.13:10909 \\\n' + printf ' --log-prefix case01- \\\n' + printf ' --repeat 30 \\\n' + printf ' --file /tmp/test125.bin\n' +} + +log() { + printf '[%s] %s\n' "$(date '+%Y-%m-%d %H:%M:%S')" "$*" +} + +die() { + printf >&2 '[%s] error: %s\n' "$(date '+%Y-%m-%d %H:%M:%S')" "$*" + exit 1 +} + +join_path() { + local base="${1%/}" + printf '%s/%s' "$base" "$2" +} + +build_quoted_command() { + local out_var="$1" + shift + + local command="" + local part="" + local quoted="" + for part in "$@"; do + printf -v quoted '%q' "$part" + if [[ -n "$command" ]]; then + command+=" " + fi + command+="$quoted" + done + + printf -v "$out_var" '%s' "$command" +} + +run_remote_script() { + local target="$1" + local script="$2" + shift 2 + + local parts=("env") + local assignment="" + for assignment in "$@"; do + parts+=("$assignment") + done + parts+=("bash" "-s" "--") + + local remote_cmd="" + build_quoted_command remote_cmd "${parts[@]}" + ssh -T "$target" "$remote_cmd" <<<"$script" +} + +validate_positive_integer() { + local name="$1" + local value="$2" + + if [[ ! "$value" =~ ^[1-9][0-9]*$ ]]; then + die "$name must be a positive integer, got: $value" + fi +} + +validate_sleep_value() { + local name="$1" + local value="$2" + + if [[ ! "$value" =~ ^([0-9]+([.][0-9]+)?|[.][0-9]+)$ ]]; then + die "$name must be a non-negative number understood by sleep, got: $value" + fi +} + +dump_local_log_head() { + local path="$1" + + if [[ -f "$path" ]]; then + sed -n '1,120p' "$path" >&2 || true + fi +} + +dump_remote_log_head() { + local target="$1" + local log_file="$2" + local label="$3" + local script="" + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ -f "$LOG_FILE" ]]; then + sed -n '1,120p' "$LOG_FILE" +fi +EOF +)" + + log "showing $label log head from $target" + run_remote_script "$target" "$script" "LOG_FILE=$log_file" || true +} + +check_local_dependencies() { + command -v ssh >/dev/null 2>&1 || die "ssh is required" + command -v scp >/dev/null 2>&1 || die "scp is required" + command -v go >/dev/null 2>&1 || die "go is required for local peer-a" +} + +copy_remote_file_to_local() { + local remote_source="$1" + local local_dest="$2" + local local_dir="" + local local_tmp="" + + local_dir="$(dirname "$local_dest")" + mkdir -p "$local_dir" + local_tmp="$(mktemp "$local_dir/.copy.tmp.XXXXXX")" + + if scp "$remote_source" "$local_tmp"; then + mv -f "$local_tmp" "$local_dest" + else + local status=$? + rm -f "$local_tmp" + return "$status" + fi +} + +remove_local_log_dir() { + if [[ -e "$local_log_dir" ]]; then + log "removing local log dir: $local_log_dir" + rm -rf "$local_log_dir" + fi +} + +remove_remote_log_dir() { + local target="$1" + local log_dir="$2" + local label="$3" + local pid_file="${4:-}" + local script="" + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ -n "${PID_FILE:-}" && -f "$PID_FILE" ]]; then + existing_pid="$(<"$PID_FILE")" + if [[ -n "$existing_pid" ]] && kill -0 "$existing_pid" 2>/dev/null; then + printf >&2 'refusing to remove log dir while process %s is still running\n' "$existing_pid" + exit 1 + fi +fi + +rm -rf "$LOG_DIR" +EOF +)" + + log "removing $label log dir on $target: $log_dir" + run_remote_script "$target" "$script" \ + "LOG_DIR=$log_dir" \ + "PID_FILE=$pid_file" +} + +clean_log_directories() { + remove_local_log_dir + remove_remote_log_dir "$server_ssh" "$server_log_dir" "server" "$server_pid_file" + remove_remote_log_dir "$peerb_ssh" "$peerb_log_dir" "peer-b" + if [[ "$run_mode" == "relay" ]]; then + remove_remote_log_dir "$relay_ssh" "$relay_log_dir" "relay" "$relay_pid_file" + fi +} + +truncate_local_file() { + local path="$1" + local dir="" + + dir="$(dirname "$path")" + mkdir -p "$dir" + : > "$path" +} + +truncate_remote_file() { + local target="$1" + local path="$2" + local script="" + + script="$(cat <<'EOF' +set -euo pipefail + +mkdir -p "$(dirname "$FILE_PATH")" +: > "$FILE_PATH" +EOF +)" + + run_remote_script "$target" "$script" "FILE_PATH=$path" +} + +reset_logs_after_probe() { + log "resetting peer logs after connectivity probe" + + rm -f "$local_peer_a_messages_log" + truncate_local_file "$local_peer_a_stdout_log" + truncate_local_file "$local_peer_a_latency_log" + truncate_local_file "$local_peer_a_ts_debug_log" + truncate_local_file "$local_peer_a_session_stats_log" + + truncate_remote_file "$peerb_ssh" "$peerb_stdout_log" + truncate_remote_file "$peerb_ssh" "$peerb_latency_log" + truncate_remote_file "$peerb_ssh" "$peerb_ts_debug_log" + truncate_remote_file "$peerb_ssh" "$peerb_session_stats_log" +} + +fetch_remote_peer_b_logs() { + log "copying peer-b latency log from $peerb_ssh:$peerb_latency_log to $local_peer_b_latency_log" + copy_remote_file_to_local "$peerb_ssh:$peerb_latency_log" "$local_peer_b_latency_log" +} + +run_local_latency_summary() { + [[ -f "$local_peer_a_latency_log" ]] || die "local peer-a latency log not found: $local_peer_a_latency_log" + [[ -f "$local_peer_b_latency_log" ]] || die "local peer-b latency log not found: $local_peer_b_latency_log" + + log "generating local latency summary: $local_kcp_latency_summary_log" + ( + cd "$repo_dir" + exec go run ./cmd/latencysummary \ + -input "$local_peer_a_latency_log" \ + -input "$local_peer_b_latency_log" \ + -output "$local_kcp_latency_summary_log" + ) +} + +check_remote_peerb_files() { + local script="" + local file="" + + script="$(cat <<'EOF' +set -euo pipefail + +cd "$PEERB_WORKDIR" +if [[ ! -f "$FILE_PATH" ]]; then + printf >&2 'peer-b file not found: %s\n' "$FILE_PATH" + exit 1 +fi +EOF +)" + + for file in "${peerb_files[@]}"; do + log "checking peer-b file exists: $file" + run_remote_script "$peerb_ssh" "$script" \ + "PEERB_WORKDIR=$peerb_workdir" \ + "FILE_PATH=$file" + done +} + +start_remote_server() { + local script="" + + script="$(cat <<'EOF' +export PATH="$PATH:/usr/local/go/bin:$HOME/go/bin" +set -euo pipefail + +cd "$SERVER_WORKDIR" +mkdir -p "$LOG_DIR" + +if [[ -f "$PID_FILE" ]]; then + existing_pid="$(<"$PID_FILE")" + if [[ -n "$existing_pid" ]] && kill -0 "$existing_pid" 2>/dev/null; then + printf >&2 'server already running with pid %s\n' "$existing_pid" + exit 1 + fi +fi + +: > "$STDOUT_LOG" +setsid go run ./cmd/kcpserver/ \ + -listen "$LISTEN_ADDR" \ + >>"$STDOUT_LOG" 2>&1 "$PID_FILE" +EOF +)" + + log "starting remote kcpserver (hub) on $server_ssh" + run_remote_script "$server_ssh" "$script" \ + "SERVER_WORKDIR=$server_workdir" \ + "LOG_DIR=$server_log_dir" \ + "PID_FILE=$server_pid_file" \ + "STDOUT_LOG=$server_stdout_log" \ + "LISTEN_ADDR=$listen_addr" + + server_started=1 +} + +wait_for_remote_server_ready() { + local pattern="kcp hub listening" + local script="" + local start_time="$SECONDS" + local status=0 + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ -f "$LOG_FILE" ]] && grep -Fq -- "$READY_PATTERN" "$LOG_FILE"; then + exit 0 +fi + +if [[ -f "$PID_FILE" ]]; then + pid="$(<"$PID_FILE")" + if [[ -n "$pid" ]] && kill -0 "$pid" 2>/dev/null; then + exit 10 + fi +fi + +exit 20 +EOF +)" + + while (( SECONDS - start_time < ready_timeout )); do + status=0 + run_remote_script "$server_ssh" "$script" \ + "LOG_FILE=$server_stdout_log" \ + "READY_PATTERN=$pattern" \ + "PID_FILE=$server_pid_file" || status=$? + + case "$status" in + 0) + log "remote server is ready" + return 0 + ;; + 10) + sleep 1 + ;; + 20) + log "remote server exited before readiness" + dump_remote_log_head "$server_ssh" "$server_stdout_log" "server" + return 1 + ;; + *) + log "remote server readiness check failed with status $status" + dump_remote_log_head "$server_ssh" "$server_stdout_log" "server" + return 1 + ;; + esac + done + + log "timed out waiting for remote server readiness after ${ready_timeout}s" + dump_remote_log_head "$server_ssh" "$server_stdout_log" "server" + return 1 +} + +stop_remote_server() { + local script="" + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ ! -f "$PID_FILE" ]]; then + exit 0 +fi + +pid="$(<"$PID_FILE")" +if [[ -z "$pid" ]]; then + rm -f "$PID_FILE" + exit 0 +fi + +# Kill the entire process group (setsid creates a new group with pid == pgid). +kill -- -"$pid" 2>/dev/null || kill "$pid" 2>/dev/null || true +for _ in 1 2 3 4 5; do + if ! kill -0 "$pid" 2>/dev/null; then + rm -f "$PID_FILE" + exit 0 + fi + sleep 1 +done + +kill -9 -- -"$pid" 2>/dev/null || kill -9 "$pid" 2>/dev/null || true +rm -f "$PID_FILE" +EOF +)" + + run_remote_script "$server_ssh" "$script" "PID_FILE=$server_pid_file" +} + +start_remote_relay() { + local script="" + + script="$(cat <<'EOF' +export PATH="$PATH:/usr/local/go/bin:$HOME/go/bin" +set -euo pipefail + +cd "$RELAY_WORKDIR" +mkdir -p "$LOG_DIR" + +if [[ -f "$PID_FILE" ]]; then + existing_pid="$(<"$PID_FILE")" + if [[ -n "$existing_pid" ]] && kill -0 "$existing_pid" 2>/dev/null; then + printf >&2 'relay already running with pid %s\n' "$existing_pid" + exit 1 + fi +fi + +: > "$STDOUT_LOG" +setsid go run ./cmd/kcpserver/ \ + -mode=relay \ + -listen "$LISTEN_ADDR" \ + -relay-remote "$RELAY_REMOTE" \ + >>"$STDOUT_LOG" 2>&1 "$PID_FILE" +EOF +)" + + log "starting remote relay on $relay_ssh" + run_remote_script "$relay_ssh" "$script" \ + "RELAY_WORKDIR=$relay_workdir" \ + "LOG_DIR=$relay_log_dir" \ + "PID_FILE=$relay_pid_file" \ + "STDOUT_LOG=$relay_stdout_log" \ + "LISTEN_ADDR=$relay_listen_addr" \ + "RELAY_REMOTE=$relay_remote" + + relay_started=1 +} + +wait_for_remote_relay_ready() { + local pattern="udp relay listening" + local script="" + local start_time="$SECONDS" + local status=0 + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ -f "$LOG_FILE" ]] && grep -Fq -- "$READY_PATTERN" "$LOG_FILE"; then + exit 0 +fi + +if [[ -f "$PID_FILE" ]]; then + pid="$(<"$PID_FILE")" + if [[ -n "$pid" ]] && kill -0 "$pid" 2>/dev/null; then + exit 10 + fi +fi + +exit 20 +EOF +)" + + while (( SECONDS - start_time < ready_timeout )); do + status=0 + run_remote_script "$relay_ssh" "$script" \ + "LOG_FILE=$relay_stdout_log" \ + "READY_PATTERN=$pattern" \ + "PID_FILE=$relay_pid_file" || status=$? + + case "$status" in + 0) + log "remote relay is ready" + return 0 + ;; + 10) + sleep 1 + ;; + 20) + log "remote relay exited before readiness" + dump_remote_log_head "$relay_ssh" "$relay_stdout_log" "relay" + return 1 + ;; + *) + log "remote relay readiness check failed with status $status" + dump_remote_log_head "$relay_ssh" "$relay_stdout_log" "relay" + return 1 + ;; + esac + done + + log "timed out waiting for remote relay readiness after ${ready_timeout}s" + dump_remote_log_head "$relay_ssh" "$relay_stdout_log" "relay" + return 1 +} + +stop_remote_relay() { + local script="" + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ ! -f "$PID_FILE" ]]; then + exit 0 +fi + +pid="$(<"$PID_FILE")" +if [[ -z "$pid" ]]; then + rm -f "$PID_FILE" + exit 0 +fi + +kill -- -"$pid" 2>/dev/null || kill "$pid" 2>/dev/null || true +for _ in 1 2 3 4 5; do + if ! kill -0 "$pid" 2>/dev/null; then + rm -f "$PID_FILE" + exit 0 + fi + sleep 1 +done + +kill -9 -- -"$pid" 2>/dev/null || kill -9 "$pid" 2>/dev/null || true +rm -f "$PID_FILE" +EOF +)" + + run_remote_script "$relay_ssh" "$script" "PID_FILE=$relay_pid_file" +} + +start_local_peer_a() { + log "starting local peer-a" + mkdir -p "$local_log_dir" "$local_peer_a_inbox" + : > "$local_peer_a_stdout_log" + + local peer_a_args=( + -id peer-a + -server "$server_connect_addr" + -inbox-dir "$local_peer_a_inbox" + -latency-log "$local_peer_a_latency_log" + -kcp-ts-debug-log "$local_peer_a_ts_debug_log" + -kcp-session-stats-log "$local_peer_a_session_stats_log" + -interactive=false + ) + + if [[ "$run_mode" == "relay" ]]; then + peer_a_args+=(-relay-via "$relay_connect_addr") + fi + + ( + cd "$local_workdir" + exec go run ./cmd/kcppeer "${peer_a_args[@]}" \ + >>"$local_peer_a_stdout_log" 2>&1 + ) & + + peer_a_pid="$!" +} + +wait_for_local_peer_a_ready() { + local pattern="opened KCP session as peer-a" + local start_time="$SECONDS" + + while (( SECONDS - start_time < ready_timeout )); do + if [[ -f "$local_peer_a_stdout_log" ]] && grep -Fq -- "$pattern" "$local_peer_a_stdout_log"; then + log "local peer-a is ready" + return 0 + fi + + if [[ -n "$peer_a_pid" ]] && ! kill -0 "$peer_a_pid" 2>/dev/null; then + log "local peer-a exited before readiness" + dump_local_log_head "$local_peer_a_stdout_log" + return 1 + fi + + sleep 1 + done + + log "timed out waiting for local peer-a readiness after ${ready_timeout}s" + dump_local_log_head "$local_peer_a_stdout_log" + return 1 +} + +stop_local_peer_a() { + if [[ -z "$peer_a_pid" ]]; then + return 0 + fi + + if kill -0 "$peer_a_pid" 2>/dev/null; then + kill "$peer_a_pid" 2>/dev/null || true + wait "$peer_a_pid" 2>/dev/null || true + else + wait "$peer_a_pid" 2>/dev/null || true + fi + + peer_a_pid="" +} + +start_remote_peer_b() { + local script="" + + script="$(cat <<'EOF' +export PATH="$PATH:/usr/local/go/bin:$HOME/go/bin" +set -euo pipefail + +cd "$PEERB_WORKDIR" +mkdir -p "$LOG_DIR" "$INBOX_DIR" + +if [[ -f "$PID_FILE" ]]; then + existing_pid="$(<"$PID_FILE")" + if [[ -n "$existing_pid" ]] && kill -0 "$existing_pid" 2>/dev/null; then + printf >&2 'peer-b already running with pid %s\n' "$existing_pid" + exit 1 + fi +fi + +: > "$STDOUT_LOG" +: > "$COMMAND_FILE" + + peer_b_cmd="$(cat <<'INNER' + tail -n +1 -f "$COMMAND_FILE" | exec go run ./cmd/kcppeer/ \ + -id peer-b \ + -server "$SERVER_ADDR" \ + -inbox-dir "$INBOX_DIR" \ + -latency-log "$LATENCY_LOG" \ + -kcp-ts-debug-log "$TS_DEBUG_LOG" \ + -kcp-session-stats-log "$SESSION_STATS_LOG" +INNER +)" + +nohup setsid bash -lc "$peer_b_cmd" >>"$STDOUT_LOG" 2>&1 "$PID_FILE" +EOF +)" + + log "starting remote peer-b on $peerb_ssh" + run_remote_script "$peerb_ssh" "$script" \ + "PEERB_WORKDIR=$peerb_workdir" \ + "LOG_DIR=$peerb_log_dir" \ + "INBOX_DIR=$peerb_inbox_dir" \ + "STDOUT_LOG=$peerb_stdout_log" \ + "COMMAND_FILE=$peerb_command_file" \ + "PID_FILE=$peerb_pid_file" \ + "SERVER_ADDR=$server_connect_addr" \ + "LATENCY_LOG=$peerb_latency_log" \ + "TS_DEBUG_LOG=$peerb_ts_debug_log" \ + "SESSION_STATS_LOG=$peerb_session_stats_log" + + peer_b_started=1 +} + +wait_for_remote_peer_b_ready() { + local pattern="opened KCP session as peer-b" + local script="" + local start_time="$SECONDS" + local status=0 + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ -f "$LOG_FILE" ]] && grep -Fq -- "$READY_PATTERN" "$LOG_FILE"; then + exit 0 +fi + +if [[ -f "$PID_FILE" ]]; then + pid="$(<"$PID_FILE")" + if [[ -n "$pid" ]] && kill -0 "$pid" 2>/dev/null; then + exit 10 + fi +fi + +exit 20 +EOF +)" + + while (( SECONDS - start_time < ready_timeout )); do + status=0 + run_remote_script "$peerb_ssh" "$script" \ + "LOG_FILE=$peerb_stdout_log" \ + "READY_PATTERN=$pattern" \ + "PID_FILE=$peerb_pid_file" || status=$? + + case "$status" in + 0) + log "remote peer-b is ready" + return 0 + ;; + 10) + sleep 1 + ;; + 20) + log "remote peer-b exited before readiness" + dump_remote_log_head "$peerb_ssh" "$peerb_stdout_log" "peer-b" + return 1 + ;; + *) + log "remote peer-b readiness check failed with status $status" + dump_remote_log_head "$peerb_ssh" "$peerb_stdout_log" "peer-b" + return 1 + ;; + esac + done + + log "timed out waiting for remote peer-b readiness after ${ready_timeout}s" + dump_remote_log_head "$peerb_ssh" "$peerb_stdout_log" "peer-b" + return 1 +} + +probe_peer_b_to_local_peer_a() { + local marker="" + local command_line="" + local quoted_command="" + local script="" + local start_time="$SECONDS" + + marker="probe-$(date +%s)-$$" + printf -v command_line 'text peer-a %s' "$marker" + printf -v quoted_command '%q' "$command_line" + + script="$(cat <> "\$COMMAND_FILE" +EOF +)" + + log "probing peer-b -> peer-a message delivery before batch" + run_remote_script "$peerb_ssh" "$script" "COMMAND_FILE=$peerb_command_file" + + while (( SECONDS - start_time < ready_timeout )); do + if [[ -f "$local_peer_a_messages_log" ]] && grep -Fq -- "$marker" "$local_peer_a_messages_log"; then + log "peer-b -> peer-a probe succeeded" + reset_logs_after_probe + return 0 + fi + + if [[ -n "$peer_a_pid" ]] && ! kill -0 "$peer_a_pid" 2>/dev/null; then + log "local peer-a exited during connectivity probe" + dump_local_log_head "$local_peer_a_stdout_log" + return 1 + fi + + sleep 1 + done + + log "timed out waiting for peer-b -> peer-a probe delivery after ${ready_timeout}s" + dump_local_log_head "$local_peer_a_stdout_log" + dump_remote_log_head "$peerb_ssh" "$peerb_stdout_log" "peer-b" + return 1 +} + +run_remote_peer_b_batch() { + local script="" + local batch_commands="" + local round=0 + local i=0 + local send_index=0 + local total_sends=$(( ${#peerb_files[@]} * repeat_count )) + local file="" + local command_line="" + local quoted_command="" + local quoted_sleep="" + + for (( round = 1; round <= repeat_count; round++ )); do + for (( i = 0; i < ${#peerb_files[@]}; i++ )); do + file="${peerb_files[$i]}" + send_index=$(( send_index + 1 )) + log "queueing peer-b -> peer-a file (round $round/$repeat_count, send $send_index/$total_sends): $file" + printf -v command_line 'file peer-a %s' "$file" + printf -v quoted_command '%q' "$command_line" + batch_commands+="printf '%s\n' ${quoted_command} >> \"\$COMMAND_FILE\""$'\n' + if (( send_index < total_sends )); then + printf -v quoted_sleep '%q' "$send_interval" + batch_commands+="sleep ${quoted_sleep}"$'\n' + fi + done + done + printf -v quoted_sleep '%q' "$drain_wait" + batch_commands+="sleep ${quoted_sleep}"$'\n' + batch_commands+="printf '%s\n' quit >> \"\$COMMAND_FILE\""$'\n' + + script="$(cat <&2 'peer-b pid file not found: %s\n' "\$PID_FILE" + exit 1 +fi + +pid="\$(<"\$PID_FILE")" +if [[ -z "\$pid" ]] || ! kill -0 "\$pid" 2>/dev/null; then + printf >&2 'peer-b is not running\n' + exit 1 +fi + +$batch_commands + +for (( i = 0; i < READY_TIMEOUT; i++ )); do + if ! kill -0 "\$pid" 2>/dev/null; then + rm -f "\$PID_FILE" "\$COMMAND_FILE" + exit 0 + fi + sleep 1 +done + +printf >&2 'peer-b did not exit after quit within %s seconds\n' "\$READY_TIMEOUT" +exit 1 +EOF +)" + + log "sending ${#peerb_files[@]} files across $repeat_count rounds ($total_sends sends total) from peer-b" + run_remote_script "$peerb_ssh" "$script" \ + "PID_FILE=$peerb_pid_file" \ + "COMMAND_FILE=$peerb_command_file" \ + "READY_TIMEOUT=$ready_timeout" + + peer_b_started=0 +} + +stop_remote_peer_b() { + local script="" + + script="$(cat <<'EOF' +set -euo pipefail + +if [[ ! -f "$PID_FILE" ]]; then + rm -f "$COMMAND_FILE" + exit 0 +fi + +pid="$(<"$PID_FILE")" +if [[ -z "$pid" ]]; then + rm -f "$PID_FILE" "$COMMAND_FILE" + exit 0 +fi + +if kill -0 "$pid" 2>/dev/null; then + printf 'quit\n' >> "$COMMAND_FILE" 2>/dev/null || true + for _ in 1 2 3 4 5; do + if ! kill -0 "$pid" 2>/dev/null; then + rm -f "$PID_FILE" "$COMMAND_FILE" + exit 0 + fi + sleep 1 + done + kill -- -"$pid" 2>/dev/null || kill "$pid" 2>/dev/null || true + for _ in 1 2 3 4 5; do + if ! kill -0 "$pid" 2>/dev/null; then + rm -f "$PID_FILE" "$COMMAND_FILE" + exit 0 + fi + sleep 1 + done + kill -9 -- -"$pid" 2>/dev/null || kill -9 "$pid" 2>/dev/null || true +fi + +rm -f "$PID_FILE" "$COMMAND_FILE" +EOF +)" + + run_remote_script "$peerb_ssh" "$script" \ + "PID_FILE=$peerb_pid_file" \ + "COMMAND_FILE=$peerb_command_file" +} + +cleanup() { + local exit_code="$?" + + trap - EXIT INT TERM + + if [[ -n "$peer_a_pid" ]]; then + log "stopping local peer-a" + stop_local_peer_a + fi + + if (( peer_b_started == 1 )); then + log "stopping remote peer-b on $peerb_ssh" + stop_remote_peer_b || true + fi + + if (( relay_started == 1 )); then + log "stopping remote relay on $relay_ssh" + stop_remote_relay || true + fi + + if (( server_started == 1 )); then + log "stopping remote server on $server_ssh" + stop_remote_server || true + fi + + exit "$exit_code" +} + +handle_interrupt() { + log "received interrupt signal" + exit 130 +} + +handle_terminate() { + log "received terminate signal" + exit 143 +} + +while [[ $# -gt 0 ]]; do + case "$1" in + --mode) + [[ $# -ge 2 ]] || die "--mode requires a value" + run_mode="$2" + shift 2 + ;; + --server-ssh) + [[ $# -ge 2 ]] || die "--server-ssh requires a value" + server_ssh="$2" + shift 2 + ;; + --peerb-ssh) + [[ $# -ge 2 ]] || die "--peerb-ssh requires a value" + peerb_ssh="$2" + shift 2 + ;; + --relay-ssh) + [[ $# -ge 2 ]] || die "--relay-ssh requires a value" + relay_ssh="$2" + shift 2 + ;; + --server-addr) + [[ $# -ge 2 ]] || die "--server-addr requires a value" + server_addr="$2" + shift 2 + ;; + --relay-addr) + [[ $# -ge 2 ]] || die "--relay-addr requires a value" + relay_addr="$2" + shift 2 + ;; + --relay-remote) + [[ $# -ge 2 ]] || die "--relay-remote requires a value" + relay_remote="$2" + shift 2 + ;; + --log-prefix) + [[ $# -ge 2 ]] || die "--log-prefix requires a value" + log_prefix="$2" + shift 2 + ;; + --listen-addr) + [[ $# -ge 2 ]] || die "--listen-addr requires a value" + listen_addr="$2" + shift 2 + ;; + --relay-listen-addr) + [[ $# -ge 2 ]] || die "--relay-listen-addr requires a value" + relay_listen_addr="$2" + shift 2 + ;; + --server-workdir) + [[ $# -ge 2 ]] || die "--server-workdir requires a value" + server_workdir="$2" + shift 2 + ;; + --relay-workdir) + [[ $# -ge 2 ]] || die "--relay-workdir requires a value" + relay_workdir="$2" + shift 2 + ;; + --peerb-workdir) + [[ $# -ge 2 ]] || die "--peerb-workdir requires a value" + peerb_workdir="$2" + shift 2 + ;; + --local-workdir) + [[ $# -ge 2 ]] || die "--local-workdir requires a value" + local_workdir="$2" + shift 2 + ;; + --ready-timeout) + [[ $# -ge 2 ]] || die "--ready-timeout requires a value" + ready_timeout="$2" + shift 2 + ;; + --repeat) + [[ $# -ge 2 ]] || die "--repeat requires a value" + repeat_count="$2" + shift 2 + ;; + --send-interval) + [[ $# -ge 2 ]] || die "--send-interval requires a value" + send_interval="$2" + shift 2 + ;; + --drain-wait) + [[ $# -ge 2 ]] || die "--drain-wait requires a value" + drain_wait="$2" + shift 2 + ;; + --file) + [[ $# -ge 2 ]] || die "--file requires a value" + peerb_files+=("$2") + shift 2 + ;; + -h|--help) + usage + exit 0 + ;; + *) + die "unknown argument: $1" + ;; + esac +done + +[[ "$run_mode" == "direct" || "$run_mode" == "relay" ]] || die "--mode must be 'direct' or 'relay', got: $run_mode" +[[ -n "$server_ssh" ]] || die "--server-ssh is required" +[[ -n "$peerb_ssh" ]] || die "--peerb-ssh is required" +[[ -n "$server_addr" ]] || die "--server-addr is required" +[[ -n "$log_prefix" ]] || die "--log-prefix is required" +(( ${#peerb_files[@]} > 0 )) || die "at least one --file is required" + +if [[ "$run_mode" == "relay" ]]; then + [[ -n "$relay_ssh" ]] || die "--relay-ssh is required in relay mode" + [[ -n "$relay_addr" ]] || die "--relay-addr is required in relay mode" + [[ -n "$relay_remote" ]] || die "--relay-remote is required in relay mode" +fi + +validate_positive_integer "--ready-timeout" "$ready_timeout" +validate_positive_integer "--repeat" "$repeat_count" +validate_sleep_value "--send-interval" "$send_interval" +validate_sleep_value "--drain-wait" "$drain_wait" + +check_local_dependencies + +# Extract ports and build peer connection addresses. +server_port="${listen_addr##*:}" +server_connect_addr="${server_addr}:${server_port}" + +relay_connect_addr="" +if [[ "$run_mode" == "relay" ]]; then + relay_port="${relay_listen_addr##*:}" + relay_connect_addr="${relay_addr}:${relay_port}" +fi + +log_dir_name="${log_prefix}logs" +inbox_dir_name="${log_prefix}inbox" + +local_log_dir="$(join_path "$local_workdir" "$log_dir_name")" +local_peer_a_inbox="$(join_path "$local_workdir" "$inbox_dir_name/peer-a")" +local_peer_a_messages_log="$(join_path "$local_peer_a_inbox" "messages.log")" +local_peer_a_stdout_log="$(join_path "$local_log_dir" "peer-a.stdout.log")" +local_peer_a_latency_log="$(join_path "$local_log_dir" "peer-a-kcp-latency.jsonl")" +local_peer_a_ts_debug_log="$(join_path "$local_log_dir" "peer-a-kcp-packet-debug.jsonl")" +local_peer_a_session_stats_log="$(join_path "$local_log_dir" "peer-a-kcp-session-stats.jsonl")" +local_peer_b_stdout_log="$(join_path "$local_log_dir" "peer-b.stdout.log")" +local_peer_b_latency_log="$(join_path "$local_log_dir" "peer-b-kcp-latency.jsonl")" +local_peer_b_ts_debug_log="$(join_path "$local_log_dir" "peer-b-kcp-packet-debug.jsonl")" +local_peer_b_session_stats_log="$(join_path "$local_log_dir" "peer-b-kcp-session-stats.jsonl")" +local_kcp_latency_summary_log="$(join_path "$local_log_dir" "kcp-latency-summary.jsonl")" + +server_log_dir="$(join_path "$server_workdir" "$log_dir_name")" +server_pid_file="$(join_path "$server_log_dir" "server.pid")" +server_stdout_log="$(join_path "$server_log_dir" "server.stdout.log")" + +relay_log_dir="" +relay_pid_file="" +relay_stdout_log="" +if [[ "$run_mode" == "relay" ]]; then + relay_log_dir="$(join_path "$relay_workdir" "$log_dir_name")" + relay_pid_file="$(join_path "$relay_log_dir" "relay.pid")" + relay_stdout_log="$(join_path "$relay_log_dir" "relay.stdout.log")" +fi + +peerb_log_dir="$(join_path "$peerb_workdir" "$log_dir_name")" +peerb_inbox_dir="$(join_path "$peerb_workdir" "$inbox_dir_name/peer-b")" +peerb_stdout_log="$(join_path "$peerb_log_dir" "peer-b.stdout.log")" +peerb_latency_log="$(join_path "$peerb_log_dir" "peer-b-kcp-latency.jsonl")" +peerb_ts_debug_log="$(join_path "$peerb_log_dir" "peer-b-kcp-packet-debug.jsonl")" +peerb_session_stats_log="$(join_path "$peerb_log_dir" "peer-b-kcp-session-stats.jsonl")" +peerb_pid_file="$(join_path "$peerb_log_dir" "peer-b.pid")" +peerb_command_file="$(join_path "$peerb_log_dir" "peer-b.commands")" + +trap cleanup EXIT +trap handle_interrupt INT +trap handle_terminate TERM + +clean_log_directories + +mkdir -p "$local_log_dir" "$local_peer_a_inbox" + +log "run mode: $run_mode" +log "local peer-a logs: $local_log_dir" +log "remote server logs: $server_log_dir" +if [[ "$run_mode" == "relay" ]]; then + log "remote relay logs: $relay_log_dir" +fi +log "remote peer-b logs: $peerb_log_dir" + +check_remote_peerb_files +start_remote_server +wait_for_remote_server_ready + +if [[ "$run_mode" == "relay" ]]; then + start_remote_relay + wait_for_remote_relay_ready +fi + +start_local_peer_a +start_remote_peer_b +wait_for_local_peer_a_ready +wait_for_remote_peer_b_ready +probe_peer_b_to_local_peer_a +run_remote_peer_b_batch + +log "batch send completed" + +if [[ -n "$peer_a_pid" ]]; then + log "stopping local peer-a after batch" + stop_local_peer_a +fi + +if (( relay_started == 1 )); then + log "stopping remote relay on $relay_ssh after batch" + if stop_remote_relay; then + relay_started=0 + else + log "failed to stop remote relay cleanly; cleanup will retry" + fi +fi + +if (( server_started == 1 )); then + log "stopping remote server on $server_ssh after batch" + if stop_remote_server; then + server_started=0 + else + log "failed to stop remote server cleanly; cleanup will retry" + fi +fi + +fetch_remote_peer_b_logs +run_local_latency_summary diff --git a/robot/v4l2/OmniSocketGo_robot/src/gps_buffer.c b/robot/v4l2/OmniSocketGo_robot/src/gps_buffer.c new file mode 100644 index 0000000..9d65b91 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/src/gps_buffer.c @@ -0,0 +1,333 @@ +#include "gps_buffer.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include // 确保包含 errno + +// 全局共享变量 +static gps_video_sample_t g_current_gps_data = {0.0, 0.0}; +static volatile int g_running = 0; +static pthread_t g_gps_thread; +static pthread_mutex_t g_gps_mutex = PTHREAD_MUTEX_INITIALIZER; + +static double normalize_coordinate(double coordinate) { + return round(coordinate * 1000000.0) / 1000000.0; +} + +static void store_gps(double latitude, double longitude) { + pthread_mutex_lock(&g_gps_mutex); + g_current_gps_data.latitude = normalize_coordinate(latitude); + g_current_gps_data.longitude = normalize_coordinate(longitude); + pthread_mutex_unlock(&g_gps_mutex); +} + +static void clear_gps(void) { + pthread_mutex_lock(&g_gps_mutex); + g_current_gps_data.latitude = 0.0; + g_current_gps_data.longitude = 0.0; + pthread_mutex_unlock(&g_gps_mutex); +} + +static gps_video_sample_t load_gps(void) { + gps_video_sample_t sample; + + pthread_mutex_lock(&g_gps_mutex); + sample = g_current_gps_data; + pthread_mutex_unlock(&g_gps_mutex); + return sample; +} + +static void gps_sleep_before_retry(void) { + int retry_ms = 1000; + int step_ms = 100; + int elapsed_ms = 0; + + while (g_running && elapsed_ms < retry_ms) { + usleep((useconds_t) step_ms * 1000U); + elapsed_ms += step_ms; + } +} + +// 将经纬度规范化为 double，保留 6 位小数。 +static int normalize_gps(double latitude, double longitude, gps_video_sample_t* sample) { + if (!isfinite(latitude) || !isfinite(longitude)) { + return -1; + } + // 过滤掉 0,0 这种无效坐标 + if (fabs(latitude) < 1e-6 && fabs(longitude) < 1e-6) { + return -1; + } + + if (sample == NULL) { + return -1; + } + + sample->latitude = normalize_coordinate(latitude); + sample->longitude = normalize_coordinate(longitude); + return 0; +} + +// ================================================================= +// 以下是借鉴 gps_parse.c 实现的底层解析函数 +// ================================================================= + +// 1. 辅助函数：在 JSON 字符串中查找键对应的值的起始位置 +static const char* find_json_value(const char* json, const char* key) { + char pattern[64]; + int written; + const char* position; + + if (json == NULL || key == NULL) return NULL; + + // 构建搜索模式: "key": + written = snprintf(pattern, sizeof(pattern), "\"%s\":", key); + if (written < 0 || (size_t)written >= sizeof(pattern)) { + return NULL; + } + + position = strstr(json, pattern); + if (position == NULL) { + return NULL; + } + + // 跳过 "key": + position += written; + + // 跳过可能存在的空格 + while (*position == ' ' || *position == '\t') { + position++; + } + + return position; +} + +// 2. 解析函数：从 JSON 字符串中提取 Double 类型的值 +static int json_extract_double(const char* json, const char* key, double* value) { + const char* position; + char* endptr = NULL; + double parsed; + + position = find_json_value(json, key); + if (position == NULL) { + return 0; // 键不存在 + } + + // 确保当前位置是数字或负号 + if (*position != '-' && !(*position >= '0' && *position <= '9')) { + return 0; + } + + // 重置 errno 以检测错误 + errno = 0; + parsed = strtod(position, &endptr); + + // 检查转换是否成功 + if (errno != 0 || endptr == position || !isfinite(parsed)) { + return 0; + } + + *value = parsed; + return 1; +} + +// 3. 解析函数：从 JSON 字符串中提取 Int 类型的值 +static int json_extract_int(const char* json, const char* key, int* value) { + double dval; + if (json_extract_double(json, key, &dval)) { + *value = (int)dval; + return 1; + } + return 0; +} + +// 4. 检查是否为 TPV (定位数据) 包 +static int is_tpv_class(const char* json) { + char class_buf[32] = {0}; + const char* pos = find_json_value(json, "class"); + if (pos == NULL || *pos != '"') return 0; + + // 简单提取 class 的值 (TPV/SKY/DEVICES) + sscanf(pos, "\"%31[^\"]\"", class_buf); + return (strcmp(class_buf, "TPV") == 0); +} + +// ================================================================= +// 后台线程函数：负责连接 gpsd 并更新全局变量 +// ================================================================= +void* gps_update_thread(void* arg) { + const char* host = (const char*)arg; + const char* gpsd_host = (host != NULL && host[0] != '\0') ? host : "127.0.0.1"; + + while (g_running) { + int sockfd = -1; + struct addrinfo hints; + struct addrinfo *res = NULL; + struct addrinfo *rp = NULL; + int s; + char buffer[4096]; + size_t offset = 0; + + // 1. 解析地址并连接 gpsd (默认端口 2947) + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_UNSPEC; // 兼容 IPv4/IPv6 + hints.ai_socktype = SOCK_STREAM; + + s = getaddrinfo(gpsd_host, "2947", &hints, &res); + if (s != 0) { + fprintf(stderr, "GPS线程: 解析 gpsd 地址失败 %s:2947: %s\n", gpsd_host, gai_strerror(s)); + gps_sleep_before_retry(); + continue; + } + + // 尝试连接每一个解析出来的地址 + for (rp = res; rp != NULL; rp = rp->ai_next) { + sockfd = socket(rp->ai_family, rp->ai_socktype, rp->ai_protocol); + if (sockfd == -1) { + continue; + } + + if (connect(sockfd, rp->ai_addr, rp->ai_addrlen) != -1) { + break; + } + close(sockfd); + sockfd = -1; + } + + freeaddrinfo(res); + + if (sockfd < 0) { + fprintf(stderr, "GPS线程: 无法连接到 %s:2947，1 秒后重试\n", gpsd_host); + gps_sleep_before_retry(); + continue; + } + + printf("GPS线程: 已连接到 gpsd %s\n", gpsd_host); + + // 2. 发送 WATCH 命令，开启 JSON 流 + { + const char* watch_cmd = "?WATCH={\"enable\":true,\"json\":true};\n"; + + if (send(sockfd, watch_cmd, strlen(watch_cmd), 0) < 0) { + perror("GPS线程: 发送 WATCH 命令失败"); + close(sockfd); + gps_sleep_before_retry(); + continue; + } + } + + // 3. 主循环：读取并解析数据流 + // 注意：gpsd 数据是以 \n 结尾的，不能直接用固定长度 recv + while (g_running) { + ssize_t len = recv(sockfd, buffer + offset, sizeof(buffer) - 1 - offset, 0); + + if (len <= 0) { + break; + } + + offset += (size_t) len; + buffer[offset] = '\0'; // 确保字符串结束 + + // 查找换行符 \n，因为一条完整的 JSON 消息以 \n 结尾 + char* start = buffer; + char* end; + + while ((end = memchr(start, '\n', (buffer + offset) - start)) != NULL) { + *end = '\0'; // 临时截断，形成独立字符串 + + // --- 核心解析逻辑 --- + // 1. 检查是否为 TPV 数据包 + if (is_tpv_class(start)) { + double lat = 0.0; + double lon = 0.0; + int mode = 0; + int has_fix = 0; + + // 2. 提取定位模式 (mode: 1=无定位, 2=2D, 3=3D) + if (json_extract_int(start, "mode", &mode)) { + has_fix = (mode >= 2); + } + + // 3. 如果有定位，提取经纬度 + if (has_fix) { + int got_lat = json_extract_double(start, "lat", &lat); + int got_lon = json_extract_double(start, "lon", &lon); + + if (got_lat && got_lon) { + gps_video_sample_t sample; + + // 4. 更新全局共享变量，使用 double 直接携带经纬度。 + if (normalize_gps(lat, lon, &sample) == 0) { + store_gps(sample.latitude, sample.longitude); + } + // 调试：取消注释可查看实时经纬度 + // printf("更新GPS: lat=%.6f, lon=%.6f\n", lat, lon); + } + } + // 如果无定位，这里不操作，保持上一次的有效值 + } + // --- 解析结束 --- + + // 移动指针到下一条消息 + start = end + 1; + } + + // 处理完所有完整消息后，将剩余未处理的数据移到缓冲区头部 + if (start < buffer + offset) { + size_t remaining = (size_t) ((buffer + offset) - start); + memmove(buffer, start, remaining); + offset = remaining; + } else { + offset = 0; // 缓冲区已清空 + } + } + + close(sockfd); + if (g_running) { + fprintf(stderr, "GPS线程: 连接断开，1 秒后重连...\n"); + gps_sleep_before_retry(); + } + } + + return NULL; +} + +// ================================================================= +// 接口函数实现 +// ================================================================= +gps_video_sample_t get_latest_gps_for_video(void) { + return load_gps(); +} + +int gps_buffer_init(const char* host) { + if (g_running) return 0; + + g_running = 1; + clear_gps(); + pthread_attr_t attr; + pthread_attr_init(&attr); + pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED); + // 创建后台线程 + if (pthread_create(&g_gps_thread, &attr, gps_update_thread, (void*)host) != 0) { + g_running = 0; + pthread_attr_destroy(&attr); // 清理属性 + perror("无法创建 GPS 线程"); + return -1; + } + pthread_attr_destroy(&attr); // 清理属性 + return 0; +} + +void gps_buffer_cleanup(void) { + g_running = 0; + // 等待线程结束 + + usleep(10000); // 等待 100ms 让后台线程有机会处理退出标志 +} + + +//gcc main.c video_pipeline_run.c gps_buffer.c -lpthread -lm -o my_app 请确保在编译命令中链接 pthread 和 m (math) 库 diff --git a/robot/v4l2/OmniSocketGo_robot/src/interactive.c b/robot/v4l2/OmniSocketGo_robot/src/interactive.c new file mode 100644 index 0000000..14f5a89 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/src/interactive.c @@ -0,0 +1,77 @@ +#include "interactive.h" + +#include + +static void interactive_skip_spaces(const char **cursor) { + while (**cursor != '\0' && isspace((unsigned char) **cursor)) { + (*cursor)++; + } +} + +int interactive_parse_command(const char *line, interactive_command_t *command, char *err, size_t err_len) { + const char *cursor = line; + char action[16]; + size_t action_len = 0; + size_t to_len = 0; + size_t value_len; + + if (line == NULL || command == NULL) { + snprintf(err, err_len, "interactive: invalid command"); + return -1; + } + memset(command, 0, sizeof(*command)); + interactive_skip_spaces(&cursor); + while (*cursor != '\0' && !isspace((unsigned char) *cursor) && action_len + 1 < sizeof(action)) { + action[action_len++] = *cursor++; + } + action[action_len] = '\0'; + if (action_len == 0) { + snprintf(err, err_len, "interactive: empty command"); + return -1; + } + if (strcmp(action, "help") == 0) { + command->type = INTERACTIVE_CMD_HELP; + return 0; + } + if (strcmp(action, "quit") == 0) { + command->type = INTERACTIVE_CMD_QUIT; + return 0; + } + + interactive_skip_spaces(&cursor); + while (*cursor != '\0' && !isspace((unsigned char) *cursor) && to_len + 1 < sizeof(command->to)) { + command->to[to_len++] = *cursor++; + } + command->to[to_len] = '\0'; + interactive_skip_spaces(&cursor); + if (command->to[0] == '\0' || *cursor == '\0') { + snprintf(err, err_len, "interactive: missing target or value"); + return -1; + } + + value_len = strlen(cursor); + if (value_len >= sizeof(command->value)) { + snprintf(err, err_len, "interactive: value too long"); + return -1; + } + snprintf(command->value, sizeof(command->value), "%s", cursor); + + if (strcmp(action, "text") == 0) { + command->type = INTERACTIVE_CMD_TEXT; + return 0; + } + if (strcmp(action, "file") == 0) { + command->type = INTERACTIVE_CMD_FILE; + return 0; + } + snprintf(err, err_len, "interactive: unknown command %s", action); + return -1; +} + +void interactive_print_help(FILE *out, const char *transport_name) { + fprintf(out, "interactive mode commands (%s):\n", transport_name); + fprintf(out, " help show this help\n"); + fprintf(out, " text send one text message\n"); + fprintf(out, " file send one file\n"); + fprintf(out, " quit exit this process\n"); +} diff --git a/robot/v4l2/OmniSocketGo_robot/src/kcp_packet_debug.c b/robot/v4l2/OmniSocketGo_robot/src/kcp_packet_debug.c new file mode 100644 index 0000000..87540f8 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/src/kcp_packet_debug.c @@ -0,0 +1,166 @@ +#include "kcp_packet_debug.h" + +kcp_packet_debug_logger_t *kcp_packet_debug_open_jsonl(const char *path) { + kcp_packet_debug_logger_t *logger; + FILE *file; + if (path == NULL || path[0] == '\0') { + return NULL; + } + if (omni_ensure_parent_dir(path) != 0) { + return NULL; + } + file = fopen(path, "ab"); + if (file == NULL) { + return NULL; + } + logger = (kcp_packet_debug_logger_t *) calloc(1, sizeof(*logger)); + if (logger == NULL) { + fclose(file); + return NULL; + } + omni_file_logger_init_path(&logger->file_logger, file, path, 0); + logger->enabled = 1; + return logger; +} + +void kcp_packet_debug_close(kcp_packet_debug_logger_t *logger) { + if (logger == NULL) { + return; + } + if (logger->file_logger.file != NULL) { + fclose(logger->file_logger.file); + } + omni_file_logger_destroy(&logger->file_logger); + free(logger); +} + +void kcp_packet_debug_record_clear(kcp_packet_debug_record_t *record) { + if (record == NULL) { + return; + } + free(record->segments); + memset(record, 0, sizeof(*record)); +} + +int kcp_packet_debug_log(kcp_packet_debug_logger_t *logger, const kcp_packet_debug_record_t *record) { + char *event = NULL; + char *node_role = NULL; + char *node_id = NULL; + char *local_addr = NULL; + char *remote_addr = NULL; + char *segments_json = NULL; + char *tx_id_text = NULL; + char *conv_text = NULL; + char *line = NULL; + size_t i; + size_t cap = 128U; + size_t len = 0U; + + if (logger == NULL || record == NULL || !logger->enabled) { + return 0; + } + + event = omni_json_escape(record->event); + node_role = omni_json_escape(record->node_role); + node_id = omni_json_escape(record->node_id); + local_addr = omni_json_escape(record->local_addr); + remote_addr = omni_json_escape(record->remote_addr); + if (event == NULL || node_role == NULL || node_id == NULL || local_addr == NULL || remote_addr == NULL) { + free(event); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + return -1; + } + + segments_json = (char *) malloc(cap); + if (segments_json == NULL) { + free(event); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + return -1; + } + segments_json[len++] = '['; + for (i = 0; i < record->segment_count; ++i) { + int written; + while (len + 96U > cap) { + char *next = (char *) realloc(segments_json, cap * 2U); + if (next == NULL) { + free(event); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + free(segments_json); + return -1; + } + segments_json = next; + cap *= 2U; + } + written = snprintf( + segments_json + len, + cap - len, + "%s{\"cmd\":%u,\"sn\":%u,\"una\":%u,\"frg\":%u,\"wnd\":%u,\"len\":%u}", + i == 0 ? "" : ",", + record->segments[i].cmd, + record->segments[i].sn, + record->segments[i].una, + record->segments[i].frg, + record->segments[i].wnd, + record->segments[i].len + ); + len += (size_t) written; + } + segments_json[len++] = ']'; + segments_json[len] = '\0'; + + tx_id_text = record->has_udp_tx_id ? omni_strdup_printf("%u", record->udp_tx_id) : omni_strdup("null"); + conv_text = record->has_kcp_conv ? omni_strdup_printf("%u", record->kcp_conv) : omni_strdup("null"); + if (tx_id_text == NULL || conv_text == NULL) { + free(event); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + free(segments_json); + free(tx_id_text); + free(conv_text); + return -1; + } + + line = omni_strdup_printf( + "{\"event\":\"%s\",\"node_role\":\"%s\",\"node_id\":\"%s\",\"local_addr\":\"%s\",\"remote_addr\":\"%s\",\"packet_bytes\":%d,\"udp_tx_id\":%s,\"kcp_conv\":%s,\"segments\":%s,\"ts_unix_nano\":%" PRId64 "}", + event, + node_role, + node_id, + local_addr, + remote_addr, + record->packet_bytes, + tx_id_text, + conv_text, + segments_json, + record->ts_unix_nano + ); + + free(event); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + free(segments_json); + free(tx_id_text); + free(conv_text); + + if (line == NULL) { + return -1; + } + if (omni_file_logger_write_line(&logger->file_logger, line) != 0) { + free(line); + return -1; + } + free(line); + return 0; +} diff --git a/robot/v4l2/OmniSocketGo_robot/src/kcp_session_stats.c b/robot/v4l2/OmniSocketGo_robot/src/kcp_session_stats.c new file mode 100644 index 0000000..c0b8ef8 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/src/kcp_session_stats.c @@ -0,0 +1,286 @@ +#include "kcp_session_stats.h" + +static int kcp_session_stats_append(char **line, size_t *len, const char *suffix) { + size_t suffix_len; + char *next; + + if (line == NULL || len == NULL || suffix == NULL) { + errno = EINVAL; + return -1; + } + suffix_len = strlen(suffix); + next = (char *) realloc(*line, *len + suffix_len + 1U); + if (next == NULL) { + return -1; + } + memcpy(next + *len, suffix, suffix_len + 1U); + *line = next; + *len += suffix_len; + return 0; +} + +static int kcp_session_stats_appendf(char **line, size_t *len, const char *fmt, ...) { + va_list args; + va_list copy; + int needed; + char *buffer; + + if (line == NULL || len == NULL || fmt == NULL) { + errno = EINVAL; + return -1; + } + + va_start(args, fmt); + va_copy(copy, args); + needed = vsnprintf(NULL, 0, fmt, copy); + va_end(copy); + if (needed < 0) { + va_end(args); + return -1; + } + + buffer = (char *) malloc((size_t) needed + 1U); + if (buffer == NULL) { + va_end(args); + return -1; + } + vsnprintf(buffer, (size_t) needed + 1U, fmt, args); + va_end(args); + + if (kcp_session_stats_append(line, len, buffer) != 0) { + free(buffer); + return -1; + } + free(buffer); + return 0; +} + +kcp_session_stats_logger_t *kcp_session_stats_open_jsonl(const char *path) { + kcp_session_stats_logger_t *logger; + FILE *file; + if (path == NULL || path[0] == '\0') { + return NULL; + } + if (omni_ensure_parent_dir(path) != 0) { + return NULL; + } + file = fopen(path, "ab"); + if (file == NULL) { + return NULL; + } + logger = (kcp_session_stats_logger_t *) calloc(1, sizeof(*logger)); + if (logger == NULL) { + fclose(file); + return NULL; + } + omni_file_logger_init_path(&logger->file_logger, file, path, 0); + logger->enabled = 1; + return logger; +} + +void kcp_session_stats_close(kcp_session_stats_logger_t *logger) { + if (logger == NULL) { + return; + } + if (logger->file_logger.file != NULL) { + fclose(logger->file_logger.file); + } + omni_file_logger_destroy(&logger->file_logger); + free(logger); +} + +int kcp_session_stats_log(kcp_session_stats_logger_t *logger, const kcp_session_stats_record_t *record) { + char *record_type = NULL; + char *node_role = NULL; + char *node_id = NULL; + char *local_addr = NULL; + char *remote_addr = NULL; + char *sample_reason = NULL; + char *line = NULL; + size_t line_len = 0; + + if (logger == NULL || record == NULL || !logger->enabled) { + return 0; + } + record_type = omni_json_escape(record->record_type); + node_role = omni_json_escape(record->node_role); + node_id = omni_json_escape(record->node_id); + local_addr = omni_json_escape(record->local_addr); + remote_addr = omni_json_escape(record->remote_addr); + sample_reason = omni_json_escape(record->sample_reason); + if (record_type == NULL || node_role == NULL || node_id == NULL || local_addr == NULL || remote_addr == NULL || sample_reason == NULL) { + free(record_type); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + free(sample_reason); + return -1; + } + line = omni_strdup(""); + if (line == NULL) { + free(record_type); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + free(sample_reason); + return -1; + } + + if (kcp_session_stats_appendf(&line, &line_len, "{\"record_type\":\"%s\",\"node_role\":\"%s\",\"node_id\":\"%s\",\"ts_unix_nano\":%" PRId64 ",\"sample_reason\":\"%s\"", + record_type, + node_role, + node_id, + record->ts_unix_nano, + sample_reason) != 0) { + goto cleanup; + } + if (record->local_addr[0] != '\0' && + kcp_session_stats_appendf(&line, &line_len, ",\"local_addr\":\"%s\"", local_addr) != 0) { + goto cleanup; + } + if (record->remote_addr[0] != '\0' && + kcp_session_stats_appendf(&line, &line_len, ",\"remote_addr\":\"%s\"", remote_addr) != 0) { + goto cleanup; + } + if (record->has_conv && + kcp_session_stats_appendf(&line, &line_len, ",\"conv\":%u", record->conv) != 0) { + goto cleanup; + } + if (record->has_rto_ms && + kcp_session_stats_appendf(&line, &line_len, ",\"rto_ms\":%u", record->rto_ms) != 0) { + goto cleanup; + } + if (record->has_srtt_ms && + kcp_session_stats_appendf(&line, &line_len, ",\"srtt_ms\":%d", record->srtt_ms) != 0) { + goto cleanup; + } + if (record->has_min_srtt_ms && + kcp_session_stats_appendf(&line, &line_len, ",\"min_srtt_ms\":%d", record->min_srtt_ms) != 0) { + goto cleanup; + } + if (record->has_srttvar_ms && + kcp_session_stats_appendf(&line, &line_len, ",\"srttvar_ms\":%d", record->srttvar_ms) != 0) { + goto cleanup; + } + if (record->has_last_feedback_age_ms && + kcp_session_stats_appendf(&line, &line_len, ",\"last_feedback_age_ms\":%u", record->last_feedback_age_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; + } + if (record->has_bytes_received && + kcp_session_stats_appendf(&line, &line_len, ",\"bytes_received\":%" PRIu64, record->bytes_received) != 0) { + goto cleanup; + } + if (record->has_in_pkts && + kcp_session_stats_appendf(&line, &line_len, ",\"in_pkts\":%" PRIu64, record->in_pkts) != 0) { + goto cleanup; + } + if (record->has_out_pkts && + kcp_session_stats_appendf(&line, &line_len, ",\"out_pkts\":%" PRIu64, record->out_pkts) != 0) { + goto cleanup; + } + if (record->has_in_segs && + kcp_session_stats_appendf(&line, &line_len, ",\"in_segs\":%" PRIu64, record->in_segs) != 0) { + goto cleanup; + } + if (record->has_out_segs && + kcp_session_stats_appendf(&line, &line_len, ",\"out_segs\":%" PRIu64, record->out_segs) != 0) { + goto cleanup; + } + if (record->has_retrans_segs && + kcp_session_stats_appendf(&line, &line_len, ",\"retrans_segs\":%" PRIu64, record->retrans_segs) != 0) { + goto cleanup; + } + if (record->has_fast_retrans_segs && + kcp_session_stats_appendf(&line, &line_len, ",\"fast_retrans_segs\":%" PRIu64, record->fast_retrans_segs) != 0) { + goto cleanup; + } + if (record->has_early_retrans_segs && + kcp_session_stats_appendf(&line, &line_len, ",\"early_retrans_segs\":%" PRIu64, record->early_retrans_segs) != 0) { + goto cleanup; + } + if (record->has_lost_segs && + kcp_session_stats_appendf(&line, &line_len, ",\"lost_segs\":%" PRIu64, record->lost_segs) != 0) { + goto cleanup; + } + if (record->has_repeat_segs && + kcp_session_stats_appendf(&line, &line_len, ",\"repeat_segs\":%" PRIu64, record->repeat_segs) != 0) { + goto cleanup; + } + if (record->has_in_errs && + kcp_session_stats_appendf(&line, &line_len, ",\"in_errs\":%" PRIu64, record->in_errs) != 0) { + goto cleanup; + } + if (record->has_kcp_in_errs && + kcp_session_stats_appendf(&line, &line_len, ",\"kcp_in_errs\":%" PRIu64, record->kcp_in_errs) != 0) { + goto cleanup; + } + if (record->has_ring_buffer_snd_queue && + kcp_session_stats_appendf(&line, &line_len, ",\"ring_buffer_snd_queue\":%" PRIu64, record->ring_buffer_snd_queue) != 0) { + goto cleanup; + } + if (record->has_ring_buffer_rcv_queue && + kcp_session_stats_appendf(&line, &line_len, ",\"ring_buffer_rcv_queue\":%" PRIu64, record->ring_buffer_rcv_queue) != 0) { + goto cleanup; + } + if (record->has_ring_buffer_snd_buffer && + kcp_session_stats_appendf(&line, &line_len, ",\"ring_buffer_snd_buffer\":%" PRIu64, record->ring_buffer_snd_buffer) != 0) { + goto cleanup; + } + if (record->has_curr_estab && + kcp_session_stats_appendf(&line, &line_len, ",\"curr_estab\":%" PRIu64, record->curr_estab) != 0) { + goto cleanup; + } + if (kcp_session_stats_append(&line, &line_len, "}") != 0) { + goto cleanup; + } + + free(record_type); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + free(sample_reason); + + if (omni_file_logger_write_line(&logger->file_logger, line) != 0) { + free(line); + return -1; + } + free(line); + return 0; + +cleanup: + free(record_type); + free(node_role); + free(node_id); + free(local_addr); + free(remote_addr); + free(sample_reason); + free(line); + return -1; +} diff --git a/robot/v4l2/OmniSocketGo_robot/src/latencylog.c b/robot/v4l2/OmniSocketGo_robot/src/latencylog.c new file mode 100644 index 0000000..3a3314e --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/src/latencylog.c @@ -0,0 +1,130 @@ +#include "latencylog.h" + +static void latencylog_fill_event(latency_event_t *event, const char *node_role, const char *node_id, const char *event_name, int64_t ts_unix_nano, const message_t *msg) { + memset(event, 0, sizeof(*event)); + event->ts_unix_nano = ts_unix_nano; + snprintf(event->node_role, sizeof(event->node_role), "%s", node_role == NULL ? "" : node_role); + snprintf(event->node_id, sizeof(event->node_id), "%s", node_id == NULL ? "" : node_id); + snprintf(event->event, sizeof(event->event), "%s", event_name == NULL ? "" : event_name); + event->message_type = msg->type; + event->message_id = msg->id; + snprintf(event->from, sizeof(event->from), "%s", msg->from); + snprintf(event->to, sizeof(event->to), "%s", msg->to); + snprintf(event->file_name, sizeof(event->file_name), "%s", msg->file_name); + event->body_size = (int) msg->body_len; +} + +latency_logger_t *latencylog_open_jsonl(const char *path) { + latency_logger_t *logger; + FILE *file; + if (path == NULL || path[0] == '\0') { + return NULL; + } + if (omni_ensure_parent_dir(path) != 0) { + return NULL; + } + file = fopen(path, "ab"); + if (file == NULL) { + return NULL; + } + logger = (latency_logger_t *) calloc(1, sizeof(*logger)); + if (logger == NULL) { + fclose(file); + return NULL; + } + omni_file_logger_init_path(&logger->file_logger, file, path, 0); + logger->enabled = 1; + return logger; +} + +void latencylog_close(latency_logger_t *logger) { + if (logger == NULL) { + return; + } + if (logger->file_logger.file != NULL) { + fclose(logger->file_logger.file); + } + omni_file_logger_destroy(&logger->file_logger); + free(logger); +} + +int latencylog_log_event(latency_logger_t *logger, const latency_event_t *event) { + char *node_role = NULL; + char *node_id = NULL; + char *event_name = NULL; + char *from = NULL; + char *to = NULL; + char *file_name = NULL; + char *line = NULL; + + if (logger == NULL || event == NULL || !logger->enabled) { + return 0; + } + + node_role = omni_json_escape(event->node_role); + node_id = omni_json_escape(event->node_id); + event_name = omni_json_escape(event->event); + from = omni_json_escape(event->from); + to = omni_json_escape(event->to); + file_name = omni_json_escape(event->file_name); + if (node_role == NULL || node_id == NULL || event_name == NULL || from == NULL || to == NULL || file_name == NULL) { + free(node_role); + free(node_id); + free(event_name); + free(from); + free(to); + free(file_name); + return -1; + } + + line = omni_strdup_printf( + "{\"ts_unix_nano\":%" PRId64 ",\"node_role\":\"%s\",\"node_id\":\"%s\",\"event\":\"%s\",\"message_type\":\"%s\",\"message_id\":%" PRIu64 ",\"from\":\"%s\",\"to\":\"%s\",\"file_name\":\"%s\",\"body_size\":%d}", + event->ts_unix_nano, + node_role, + node_id, + event_name, + protocol_message_type_name(event->message_type), + event->message_id, + from, + to, + file_name, + event->body_size + ); + + free(node_role); + free(node_id); + free(event_name); + free(from); + free(to); + free(file_name); + + if (line == NULL) { + return -1; + } + if (omni_file_logger_write_line(&logger->file_logger, line) != 0) { + free(line); + return -1; + } + free(line); + return 0; +} + +int latencylog_is_business_message(const message_t *msg) { + if (msg == NULL) { + return 0; + } + return msg->type == MSG_TYPE_TEXT || msg->type == MSG_TYPE_FILE || msg->type == MSG_TYPE_BINARY; +} + +void latencylog_log_message_event(latency_logger_t *logger, const char *node_role, const char *node_id, const char *event_name, const message_t *msg) { + latencylog_log_message_event_at(logger, node_role, node_id, event_name, omni_now_unix_nano(), msg); +} + +void latencylog_log_message_event_at(latency_logger_t *logger, const char *node_role, const char *node_id, const char *event_name, int64_t ts_unix_nano, const message_t *msg) { + latency_event_t event; + if (!latencylog_is_business_message(msg)) { + return; + } + latencylog_fill_event(&event, node_role, node_id, event_name, ts_unix_nano, msg); + (void) latencylog_log_event(logger, &event); +} diff --git a/robot/v4l2/OmniSocketGo_robot/src/linux_timestamping.c b/robot/v4l2/OmniSocketGo_robot/src/linux_timestamping.c new file mode 100644 index 0000000..0a5c910 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/src/linux_timestamping.c @@ -0,0 +1,103 @@ +#include "linux_timestamping.h" +#include "latencylog.h" + +#ifdef __linux__ +#include +#include +#include +#include + +static int64_t linux_timespec_to_ns(const struct timespec *ts) { + if (ts == NULL) { + return 0; + } + return (int64_t) ts->tv_sec * 1000000000LL + ts->tv_nsec; +} + +int linux_timestamping_enable_udp_socket(int fd, int enable_rx) { + int flags = SOF_TIMESTAMPING_SOFTWARE | SOF_TIMESTAMPING_TX_SCHED | SOF_TIMESTAMPING_TX_SOFTWARE | SOF_TIMESTAMPING_OPT_ID | SOF_TIMESTAMPING_OPT_TSONLY; + if (enable_rx) { + flags |= SOF_TIMESTAMPING_RX_SOFTWARE; + } + return setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING, &flags, sizeof(flags)); +} + +int64_t linux_timestamping_parse_rx_timestamp(const struct msghdr *msg) { + struct cmsghdr *cmsg; + const struct scm_timestamping *timestamps; + if (msg == NULL) { + return 0; + } + for (cmsg = CMSG_FIRSTHDR((struct msghdr *) msg); cmsg != NULL; cmsg = CMSG_NXTHDR((struct msghdr *) msg, cmsg)) { + if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_TIMESTAMPING) { + timestamps = (const struct scm_timestamping *) CMSG_DATA(cmsg); + if (timestamps->ts[0].tv_sec != 0 || timestamps->ts[0].tv_nsec != 0) { + return linux_timespec_to_ns(×tamps->ts[0]); + } + } + } + return 0; +} + +int linux_timestamping_parse_tx_timestamp(const struct msghdr *msg, omni_tx_timestamp_event_t *out_event) { + struct cmsghdr *cmsg; + const struct scm_timestamping *timestamps = NULL; + const struct sock_extended_err *sock_err = NULL; + int64_t timestamp_ns = 0; + + if (msg == NULL || out_event == NULL) { + errno = EINVAL; + return -1; + } + memset(out_event, 0, sizeof(*out_event)); + + for (cmsg = CMSG_FIRSTHDR((struct msghdr *) msg); cmsg != NULL; cmsg = CMSG_NXTHDR((struct msghdr *) msg, cmsg)) { + if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_TIMESTAMPING) { + timestamps = (const struct scm_timestamping *) CMSG_DATA(cmsg); + } else if ((cmsg->cmsg_level == SOL_IP && cmsg->cmsg_type == IP_RECVERR) || + (cmsg->cmsg_level == SOL_IPV6 && cmsg->cmsg_type == IPV6_RECVERR)) { + sock_err = (const struct sock_extended_err *) CMSG_DATA(cmsg); + } + } + if (timestamps == NULL || sock_err == NULL) { + errno = EAGAIN; + return -1; + } + if (timestamps->ts[0].tv_sec != 0 || timestamps->ts[0].tv_nsec != 0) { + timestamp_ns = linux_timespec_to_ns(×tamps->ts[0]); + snprintf(out_event->event_name, sizeof(out_event->event_name), "%s", EVENT_A_TX_SOFTWARE); + } else if (timestamps->ts[1].tv_sec != 0 || timestamps->ts[1].tv_nsec != 0) { + timestamp_ns = linux_timespec_to_ns(×tamps->ts[1]); + snprintf(out_event->event_name, sizeof(out_event->event_name), "%s", EVENT_A_TX_SCHED); + } else { + errno = EAGAIN; + return -1; + } + out_event->ts_unix_nano = timestamp_ns; + out_event->ee_info = sock_err->ee_info; + out_event->ee_data = sock_err->ee_data; + return 0; +} + +#else + +int linux_timestamping_enable_udp_socket(int fd, int enable_rx) { + (void) fd; + (void) enable_rx; + errno = ENOTSUP; + return -1; +} + +int64_t linux_timestamping_parse_rx_timestamp(const struct msghdr *msg) { + (void) msg; + return 0; +} + +int linux_timestamping_parse_tx_timestamp(const struct msghdr *msg, omni_tx_timestamp_event_t *out_event) { + (void) msg; + (void) out_event; + errno = ENOTSUP; + return -1; +} + +#endif diff --git a/robot/v4l2/OmniSocketGo_robot/src/omni_common.c b/robot/v4l2/OmniSocketGo_robot/src/omni_common.c new file mode 100644 index 0000000..fe2b1a8 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/src/omni_common.c @@ -0,0 +1,795 @@ +#include "omni_common.h" + +#include +#include +#include +#include +#include + +int64_t omni_now_unix_nano(void) { + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + return (int64_t) ts.tv_sec * 1000000000LL + ts.tv_nsec; +} + +uint32_t omni_now_millis32(void) { + struct timespec ts; + uint64_t ms; + clock_gettime(CLOCK_MONOTONIC, &ts); + ms = (uint64_t) ts.tv_sec * 1000ULL + (uint64_t) (ts.tv_nsec / 1000000L); + return (uint32_t) (ms & 0xffffffffu); +} + +int omni_set_nonblocking(int fd, int enabled) { + int flags = fcntl(fd, F_GETFL, 0); + if (flags < 0) { + return -1; + } + if (enabled) { + flags |= O_NONBLOCK; + } else { + flags &= ~O_NONBLOCK; + } + return fcntl(fd, F_SETFL, flags); +} + +int omni_parse_sockaddr(const char *raw, int passive, struct sockaddr_storage *addr, socklen_t *addr_len, int *family_out) { + struct addrinfo hints; + struct addrinfo *result = NULL; + char host_copy[OMNI_MAX_ADDR_TEXT]; + char port_copy[32]; + const char *host = NULL; + const char *service = NULL; + const char *last_colon; + size_t host_len; + + if (raw == NULL || addr == NULL || addr_len == NULL) { + errno = EINVAL; + return -1; + } + + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_DGRAM; + hints.ai_flags = passive ? AI_PASSIVE : 0; + + last_colon = strrchr(raw, ':'); + if (last_colon == NULL) { + host = passive ? NULL : raw; + service = passive ? raw : "0"; + } else { + host_len = (size_t) (last_colon - raw); + if (host_len >= sizeof(host_copy)) { + errno = ENAMETOOLONG; + return -1; + } + memcpy(host_copy, raw, host_len); + host_copy[host_len] = '\0'; + snprintf(port_copy, sizeof(port_copy), "%s", last_colon + 1); + host = host_len == 0 ? NULL : host_copy; + service = port_copy; + } + + if (getaddrinfo(host, service, &hints, &result) != 0 || result == NULL) { + errno = EINVAL; + return -1; + } + memcpy(addr, result->ai_addr, result->ai_addrlen); + *addr_len = (socklen_t) result->ai_addrlen; + if (family_out != NULL) { + *family_out = result->ai_family; + } + freeaddrinfo(result); + return 0; +} + +int omni_clone_sockaddr(const struct sockaddr *src, socklen_t src_len, struct sockaddr_storage *dst, socklen_t *dst_len) { + if (src == NULL || dst == NULL || dst_len == NULL || src_len > sizeof(*dst)) { + errno = EINVAL; + return -1; + } + memset(dst, 0, sizeof(*dst)); + memcpy(dst, src, src_len); + *dst_len = src_len; + return 0; +} + +const char *omni_sockaddr_to_string(const struct sockaddr *addr, socklen_t addr_len, char *buffer, size_t buffer_len) { + char host[NI_MAXHOST]; + char service[NI_MAXSERV]; + + if (buffer == NULL || buffer_len == 0) { + return ""; + } + if (addr == NULL) { + snprintf(buffer, buffer_len, ""); + return buffer; + } + if (getnameinfo(addr, addr_len, host, sizeof(host), service, sizeof(service), NI_NUMERICHOST | NI_NUMERICSERV) != 0) { + snprintf(buffer, buffer_len, ""); + return buffer; + } + if (addr->sa_family == AF_INET6) { + snprintf(buffer, buffer_len, "[%s]:%s", host, service); + } else { + snprintf(buffer, buffer_len, "%s:%s", host, service); + } + return buffer; +} + +int omni_bind_device(int fd, const char *device) { +#ifdef __linux__ + if (device == NULL || device[0] == '\0') { + return 0; + } + return setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE, device, (socklen_t) strlen(device)); +#else + (void) fd; + (void) device; + errno = ENOTSUP; + return -1; +#endif +} + +static int omni_mkdir_single(const char *path) { + if (mkdir(path, 0755) == 0 || errno == EEXIST) { + return 0; + } + return -1; +} + +int omni_ensure_dir(const char *path) { + char tmp[PATH_MAX]; + size_t i; + + if (path == NULL || path[0] == '\0') { + return 0; + } + if (strlen(path) >= sizeof(tmp)) { + errno = ENAMETOOLONG; + return -1; + } + snprintf(tmp, sizeof(tmp), "%s", path); + for (i = 1; tmp[i] != '\0'; ++i) { + if (tmp[i] == '/') { + tmp[i] = '\0'; + if (tmp[0] != '\0' && omni_mkdir_single(tmp) != 0) { + return -1; + } + tmp[i] = '/'; + } + } + return omni_mkdir_single(tmp); +} + +int omni_ensure_parent_dir(const char *path) { + char tmp[PATH_MAX]; + char *slash; + + if (path == NULL || path[0] == '\0') { + return 0; + } + if (strlen(path) >= sizeof(tmp)) { + errno = ENAMETOOLONG; + return -1; + } + snprintf(tmp, sizeof(tmp), "%s", path); + slash = strrchr(tmp, '/'); + if (slash == NULL) { + return 0; + } + if (slash == tmp) { + return omni_mkdir_single("/"); + } + *slash = '\0'; + return omni_ensure_dir(tmp); +} + +int omni_read_file(const char *path, uint8_t **out, size_t *out_len) { + FILE *file; + long size; + uint8_t *buffer; + if (out == NULL || out_len == NULL) { + errno = EINVAL; + return -1; + } + *out = NULL; + *out_len = 0; + file = fopen(path, "rb"); + if (file == NULL) { + return -1; + } + if (fseek(file, 0, SEEK_END) != 0) { + fclose(file); + return -1; + } + size = ftell(file); + if (size < 0) { + fclose(file); + return -1; + } + if (fseek(file, 0, SEEK_SET) != 0) { + fclose(file); + return -1; + } + buffer = (uint8_t *) malloc((size_t) size); + if (size > 0 && buffer == NULL) { + fclose(file); + errno = ENOMEM; + return -1; + } + if ((size_t) size > 0 && fread(buffer, 1, (size_t) size, file) != (size_t) size) { + free(buffer); + fclose(file); + errno = EIO; + return -1; + } + fclose(file); + *out = buffer; + *out_len = (size_t) size; + return 0; +} + +int omni_write_full_fd(int fd, const uint8_t *data, size_t len) { + ssize_t written; + while (len > 0) { + written = write(fd, data, len); + if (written < 0) { + if (errno == EINTR) { + continue; + } + return -1; + } + if (written == 0) { + errno = EIO; + return -1; + } + data += written; + len -= (size_t) written; + } + return 0; +} + +static int omni_write_file_internal(const char *path, const uint8_t *data, size_t len, const char *mode) { + FILE *file; + if (omni_ensure_parent_dir(path) != 0) { + return -1; + } + file = fopen(path, mode); + if (file == NULL) { + return -1; + } + if (len > 0 && fwrite(data, 1, len, file) != len) { + fclose(file); + errno = EIO; + return -1; + } + if (fclose(file) != 0) { + return -1; + } + return 0; +} + +int omni_append_file(const char *path, const uint8_t *data, size_t len) { + return omni_write_file_internal(path, data, len, "ab"); +} + +int omni_write_file(const char *path, const uint8_t *data, size_t len) { + return omni_write_file_internal(path, data, len, "wb"); +} + +int omni_random_u32(uint32_t *out) { + uint8_t *cursor; + size_t remaining; + int fd; + + if (out == NULL) { + errno = EINVAL; + return -1; + } + + fd = open("/dev/urandom", O_RDONLY); + if (fd < 0) { + return -1; + } + + cursor = (uint8_t *) out; + remaining = sizeof(*out); + while (remaining > 0) { + ssize_t n = read(fd, cursor, remaining); + if (n < 0) { + if (errno == EINTR) { + continue; + } + close(fd); + return -1; + } + if (n == 0) { + close(fd); + errno = EIO; + return -1; + } + cursor += n; + remaining -= (size_t) n; + } + close(fd); + + if (*out == 0) { + *out = 1; + } + return 0; +} + +char *omni_strdup(const char *src) { + size_t len; + char *dst; + if (src == NULL) { + return NULL; + } + len = strlen(src); + dst = (char *) malloc(len + 1U); + if (dst == NULL) { + return NULL; + } + memcpy(dst, src, len + 1U); + return dst; +} + +char *omni_strdup_printf(const char *fmt, ...) { + va_list args; + va_list copy; + int needed; + char *buffer; + va_start(args, fmt); + va_copy(copy, args); + needed = vsnprintf(NULL, 0, fmt, copy); + va_end(copy); + if (needed < 0) { + va_end(args); + return NULL; + } + buffer = (char *) malloc((size_t) needed + 1U); + if (buffer == NULL) { + va_end(args); + return NULL; + } + vsnprintf(buffer, (size_t) needed + 1U, fmt, args); + va_end(args); + return buffer; +} + +char *omni_json_escape_bytes(const uint8_t *src, size_t len) { + size_t i; + size_t out_len = 0; + char *out; + char *cursor; + + if (src == NULL) { + if (len == 0) { + return omni_strdup(""); + } + errno = EINVAL; + return NULL; + } + + for (i = 0; i < len; ++i) { + switch (src[i]) { + case '\\': + case '"': + case '\b': + case '\f': + case '\n': + case '\r': + case '\t': + out_len += 2; + break; + default: + out_len += src[i] < 0x20 ? 6U : 1U; + break; + } + } + out = (char *) malloc(out_len + 1U); + if (out == NULL) { + return NULL; + } + cursor = out; + for (i = 0; i < len; ++i) { + switch (src[i]) { + case '\\': + *cursor++ = '\\'; + *cursor++ = '\\'; + break; + case '"': + *cursor++ = '\\'; + *cursor++ = '"'; + break; + case '\b': + *cursor++ = '\\'; + *cursor++ = 'b'; + break; + case '\f': + *cursor++ = '\\'; + *cursor++ = 'f'; + break; + case '\n': + *cursor++ = '\\'; + *cursor++ = 'n'; + break; + case '\r': + *cursor++ = '\\'; + *cursor++ = 'r'; + break; + case '\t': + *cursor++ = '\\'; + *cursor++ = 't'; + break; + default: + if (src[i] < 0x20) { + snprintf(cursor, 7, "\\u%04x", src[i]); + cursor += 6; + } else { + *cursor++ = (char) src[i]; + } + break; + } + } + *cursor = '\0'; + return out; +} + +char *omni_json_escape(const char *src) { + if (src == NULL) { + return omni_strdup(""); + } + return omni_json_escape_bytes((const uint8_t *) src, strlen(src)); +} + +int omni_utf8_valid(const uint8_t *data, size_t len) { + size_t i = 0; + uint8_t c; + while (i < len) { + c = data[i]; + if (c <= 0x7f) { + i++; + continue; + } + if ((c & 0xe0) == 0xc0) { + if (i + 1 >= len || (data[i + 1] & 0xc0) != 0x80 || c < 0xc2) { + return 0; + } + i += 2; + continue; + } + if ((c & 0xf0) == 0xe0) { + if (i + 2 >= len || (data[i + 1] & 0xc0) != 0x80 || (data[i + 2] & 0xc0) != 0x80) { + return 0; + } + if (c == 0xe0 && data[i + 1] < 0xa0) { + return 0; + } + if (c == 0xed && data[i + 1] >= 0xa0) { + return 0; + } + i += 3; + continue; + } + if ((c & 0xf8) == 0xf0) { + if (i + 3 >= len || (data[i + 1] & 0xc0) != 0x80 || (data[i + 2] & 0xc0) != 0x80 || (data[i + 3] & 0xc0) != 0x80) { + return 0; + } + if (c == 0xf0 && data[i + 1] < 0x90) { + return 0; + } + if (c > 0xf4 || (c == 0xf4 && data[i + 1] >= 0x90)) { + return 0; + } + i += 4; + continue; + } + return 0; + } + return 1; +} + +void omni_trim_newline(char *line) { + size_t len; + if (line == NULL) { + return; + } + len = strlen(line); + while (len > 0 && (line[len - 1] == '\n' || line[len - 1] == '\r')) { + line[--len] = '\0'; + } +} + +int omni_parse_duration_ms(const char *raw, int default_ms, int *out_ms) { + char *endptr; + long value; + if (out_ms == NULL) { + errno = EINVAL; + return -1; + } + if (raw == NULL || raw[0] == '\0') { + *out_ms = default_ms; + return 0; + } + value = strtol(raw, &endptr, 10); + if (endptr == raw || value <= 0) { + errno = EINVAL; + return -1; + } + if (*endptr == '\0' || strcmp(endptr, "ms") == 0) { + *out_ms = (int) value; + return 0; + } + if (strcmp(endptr, "s") == 0) { + *out_ms = (int) (value * 1000L); + return 0; + } + errno = EINVAL; + return -1; +} + +double omni_duration_ms_to_ns(double ms) { + return ms * 1000000.0; +} + +const char *omni_path_base_name(const char *path) { + const char *slash; + + if (path == NULL) { + return ""; + } + slash = strrchr(path, '/'); + return slash == NULL ? path : slash + 1; +} + +static uint64_t omni_now_monotonic_ms64(void) { + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + return (uint64_t) ts.tv_sec * 1000ULL + (uint64_t) (ts.tv_nsec / 1000000L); +} + +static int omni_positive_int_env(const char *name, int default_value) { + const char *raw = getenv(name); + long parsed; + char *endptr = NULL; + + if (raw == NULL || raw[0] == '\0') { + return default_value; + } + parsed = strtol(raw, &endptr, 10); + if (endptr == raw || *endptr != '\0' || parsed <= 0) { + return default_value; + } + return (int) parsed; +} + +static size_t omni_positive_size_env(const char *name, size_t default_value) { + const char *raw = getenv(name); + unsigned long long parsed; + char *endptr = NULL; + + if (raw == NULL || raw[0] == '\0') { + return default_value; + } + parsed = strtoull(raw, &endptr, 10); + if (endptr == raw || *endptr != '\0' || parsed == 0ULL) { + return default_value; + } + return (size_t) parsed; +} + +static int omni_file_logger_flush_locked(omni_file_logger_t *logger, uint64_t now_ms) { + if (logger == NULL || logger->file == NULL) { + errno = EINVAL; + return -1; + } + if (fflush(logger->file) != 0) { + return -1; + } + logger->buffered_bytes = 0U; + logger->last_flush_monotonic_ms = now_ms; + return 0; +} + +static int omni_build_rotated_path(char *buffer, size_t buffer_len, const char *path, int suffix) { + size_t path_len; + int written; + + if (buffer == NULL || buffer_len == 0U || path == NULL || path[0] == '\0') { + errno = EINVAL; + return -1; + } + path_len = strlen(path); + if (path_len + 16U >= buffer_len) { + errno = ENAMETOOLONG; + return -1; + } + memcpy(buffer, path, path_len); + written = snprintf(buffer + path_len, buffer_len - path_len, ".%d", suffix); + if (written < 0 || (size_t) written >= buffer_len - path_len) { + errno = ENAMETOOLONG; + return -1; + } + return 0; +} + +static int omni_file_logger_reopen_append_locked(omni_file_logger_t *logger) { + struct stat st; + FILE *file; + + if (logger == NULL || logger->path[0] == '\0') { + errno = EINVAL; + return -1; + } + + file = fopen(logger->path, "ab"); + if (file == NULL) { + return -1; + } + + logger->file = file; + logger->current_bytes = 0U; + if (stat(logger->path, &st) == 0) { + logger->current_bytes = (size_t) st.st_size; + } + logger->buffered_bytes = 0U; + logger->last_flush_monotonic_ms = omni_now_monotonic_ms64(); + return 0; +} + +static int omni_file_logger_recover_after_rotate_locked(omni_file_logger_t *logger, const char *rotated_current_path) { + int reopen_errno; + + if (omni_file_logger_reopen_append_locked(logger) == 0) { + return 0; + } + + reopen_errno = errno; + if (rotated_current_path != NULL && rotated_current_path[0] != '\0') { + if (rename(rotated_current_path, logger->path) == 0) { + if (omni_file_logger_reopen_append_locked(logger) == 0) { + return 0; + } + } + } + + errno = reopen_errno; + return -1; +} + +static int omni_file_logger_rotate_locked(omni_file_logger_t *logger) { + int index; + int saved_errno = 0; + int should_recover = 0; + char rotated_current_path[PATH_MAX]; + char from_path[PATH_MAX]; + char to_path[PATH_MAX]; + + if (logger == NULL || logger->path[0] == '\0' || logger->max_bytes == 0U || logger->max_files <= 0) { + return 0; + } + rotated_current_path[0] = '\0'; + if (logger->file != NULL) { + if (omni_file_logger_flush_locked(logger, omni_now_monotonic_ms64()) != 0) { + return -1; + } + should_recover = 1; + if (fclose(logger->file) != 0) { + logger->file = NULL; + saved_errno = errno; + goto recover; + } + logger->file = NULL; + } + + if (omni_build_rotated_path(from_path, sizeof(from_path), logger->path, logger->max_files) != 0) { + saved_errno = errno; + goto recover; + } + unlink(from_path); + for (index = logger->max_files - 1; index >= 1; --index) { + if (omni_build_rotated_path(from_path, sizeof(from_path), logger->path, index) != 0 || + omni_build_rotated_path(to_path, sizeof(to_path), logger->path, index + 1) != 0) { + saved_errno = errno; + goto recover; + } + if (rename(from_path, to_path) != 0 && errno != ENOENT) { + saved_errno = errno; + goto recover; + } + } + if (omni_build_rotated_path(to_path, sizeof(to_path), logger->path, 1) != 0) { + saved_errno = errno; + goto recover; + } + if (rename(logger->path, to_path) != 0 && errno != ENOENT) { + saved_errno = errno; + goto recover; + } + snprintf(rotated_current_path, sizeof(rotated_current_path), "%s", to_path); + + if (omni_file_logger_reopen_append_locked(logger) != 0) { + saved_errno = errno; + goto recover; + } + return 0; + +recover: + if (should_recover) { + int recover_errno = saved_errno != 0 ? saved_errno : errno; + if (omni_file_logger_recover_after_rotate_locked(logger, rotated_current_path) == 0) { + errno = recover_errno; + } else if (saved_errno != 0) { + errno = saved_errno; + } + } else if (saved_errno != 0) { + errno = saved_errno; + } + return -1; +} + +void omni_file_logger_init(omni_file_logger_t *logger, FILE *file) { + memset(logger, 0, sizeof(*logger)); + logger->file = file; + pthread_mutex_init(&logger->mutex, NULL); + logger->flush_bytes = 1U; + logger->flush_interval_ms = 0; + logger->immediate_flush = 1; + logger->last_flush_monotonic_ms = omni_now_monotonic_ms64(); +} + +void omni_file_logger_init_path(omni_file_logger_t *logger, FILE *file, const char *path, int immediate_flush) { + struct stat st; + + omni_file_logger_init(logger, file); + if (path != NULL && path[0] != '\0') { + snprintf(logger->path, sizeof(logger->path), "%s", path); + if (stat(path, &st) == 0) { + logger->current_bytes = (size_t) st.st_size; + } + } + logger->flush_bytes = omni_positive_size_env("BLITZ_JSONL_FLUSH_BYTES", 262144U); + logger->flush_interval_ms = omni_positive_int_env("BLITZ_JSONL_FLUSH_INTERVAL_MS", 1000); + logger->max_bytes = omni_positive_size_env("BLITZ_JSONL_ROTATE_BYTES", 134217728U); + logger->max_files = omni_positive_int_env("BLITZ_JSONL_ROTATE_FILES", 8); + logger->immediate_flush = immediate_flush != 0; +} + +void omni_file_logger_destroy(omni_file_logger_t *logger) { + pthread_mutex_destroy(&logger->mutex); +} + +int omni_file_logger_write_line(omni_file_logger_t *logger, const char *line) { + int rc = 0; + size_t line_len; + uint64_t now_ms; + if (logger == NULL || logger->file == NULL || line == NULL) { + errno = EINVAL; + return -1; + } + line_len = strlen(line) + 1U; + now_ms = omni_now_monotonic_ms64(); + pthread_mutex_lock(&logger->mutex); + if (fputs(line, logger->file) == EOF || fputc('\n', logger->file) == EOF) { + rc = -1; + } else { + logger->current_bytes += line_len; + logger->buffered_bytes += line_len; + if (logger->immediate_flush || + logger->buffered_bytes >= logger->flush_bytes || + (logger->flush_interval_ms > 0 && now_ms - logger->last_flush_monotonic_ms >= (uint64_t) logger->flush_interval_ms)) { + if (omni_file_logger_flush_locked(logger, now_ms) != 0) { + rc = -1; + } + } + if (rc == 0 && logger->max_bytes > 0U && logger->current_bytes >= logger->max_bytes) { + if (omni_file_logger_rotate_locked(logger) != 0) { + rc = -1; + } + } + } + pthread_mutex_unlock(&logger->mutex); + return rc; +} diff --git a/robot/v4l2/OmniSocketGo_robot/src/peer_kcp_client.c b/robot/v4l2/OmniSocketGo_robot/src/peer_kcp_client.c new file mode 100644 index 0000000..733f076 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/src/peer_kcp_client.c @@ -0,0 +1,675 @@ +#include "peer_kcp_client.h" + +#include +#include +#include +#include + +#define KCP_CLIENT_REGISTER_TIMEOUT_MS 3000 +#define KCP_CLIENT_CTRL_REGISTER_OK "{\"type\":\"server_register_ok\"}" +#define KCP_CLIENT_CTRL_PEER_REPLACED "{\"type\":\"server_peer_replaced\",\"reason\":\"new_instance_wins\"}" +#define KCP_CLIENT_CTRL_HEARTBEAT "{\"type\":\"server_heartbeat\"}" +#define KCP_CLIENT_CTRL_HEARTBEAT_ACK "{\"type\":\"server_heartbeat_ack\"}" + +struct kcp_client { + char id[OMNI_MAX_PEER_ID]; + char server_addr[OMNI_MAX_ADDR_TEXT]; + kcp_conn_t *conn; + latency_logger_t *logger; + pthread_mutex_t state_mu; + uint64_t next_message_id; + int registered; + uint32_t last_server_activity_ms; + char last_server_error[256]; +}; + +static int kcp_client_next_message_id(kcp_client_t *client, uint64_t *out_id) { + if (client == NULL || out_id == NULL) { + errno = EINVAL; + return -1; + } + pthread_mutex_lock(&client->state_mu); + *out_id = ++client->next_message_id; + pthread_mutex_unlock(&client->state_mu); + return 0; +} + +static void kcp_client_set_registered(kcp_client_t *client, int registered) { + if (client == NULL) { + return; + } + pthread_mutex_lock(&client->state_mu); + client->registered = registered != 0; + pthread_mutex_unlock(&client->state_mu); +} + +static void kcp_client_touch_server_activity(kcp_client_t *client) { + if (client == NULL) { + return; + } + pthread_mutex_lock(&client->state_mu); + client->last_server_activity_ms = omni_now_millis32(); + pthread_mutex_unlock(&client->state_mu); +} + +static void kcp_client_set_last_server_error(kcp_client_t *client, const char *message) { + if (client == NULL) { + return; + } + pthread_mutex_lock(&client->state_mu); + snprintf(client->last_server_error, sizeof(client->last_server_error), "%s", message == NULL ? "" : message); + pthread_mutex_unlock(&client->state_mu); +} + +static void kcp_client_clear_last_server_error(kcp_client_t *client) { + kcp_client_set_last_server_error(client, ""); +} + +static int kcp_client_server_error_invalidates_registration(const char *message) { + if (message == NULL || message[0] == '\0') { + return 0; + } + return strstr(message, "not registered") != NULL + || strstr(message, "first message must be register") != NULL + || strstr(message, "peer replaced") != NULL + || strstr(message, "timed out waiting for server_register_ok") != NULL; +} + +static int kcp_client_is_registered(kcp_client_t *client) { + int registered; + + if (client == NULL) { + return 0; + } + pthread_mutex_lock(&client->state_mu); + registered = client->registered; + pthread_mutex_unlock(&client->state_mu); + return registered; +} + +static int kcp_client_text_body_equals(const message_t *msg, const char *payload) { + size_t expected_len; + + if (msg == NULL || payload == NULL || msg->body == NULL) { + return 0; + } + expected_len = strlen(payload); + return msg->body_len == expected_len && memcmp(msg->body, payload, expected_len) == 0; +} + +static void kcp_client_copy_server_error_body(const message_t *msg, char *buffer, size_t buffer_len) { + size_t copy_len; + + if (buffer == NULL || buffer_len == 0) { + return; + } + buffer[0] = '\0'; + if (msg == NULL || msg->body == NULL || msg->body_len == 0) { + return; + } + copy_len = msg->body_len < (buffer_len - 1U) ? msg->body_len : (buffer_len - 1U); + memcpy(buffer, msg->body, copy_len); + buffer[copy_len] = '\0'; +} + +static int kcp_client_registration_errno_from_message(const char *message) { + if (message == NULL || message[0] == '\0') { + return ECONNREFUSED; + } + if (strstr(message, "duplicate peer id") != NULL) { + return EEXIST; + } + if (strstr(message, "first message must be register") != NULL) { + return EPROTO; + } + return ECONNREFUSED; +} + +static int kcp_client_send_text_internal(kcp_client_t *client, const char *to, const char *text, int log_business_event) { + message_t msg; + uint64_t id; + + if (client == NULL || to == NULL || text == NULL || client->conn == NULL) { + errno = EINVAL; + return -1; + } + + protocol_message_init(&msg); + if (kcp_client_next_message_id(client, &id) != 0) { + return -1; + } + msg.type = MSG_TYPE_TEXT; + msg.id = id; + snprintf(msg.from, sizeof(msg.from), "%s", client->id); + snprintf(msg.to, sizeof(msg.to), "%s", to); + msg.body = (uint8_t *) omni_strdup(text); + if (msg.body == NULL) { + return -1; + } + msg.body_len = strlen((const char *) msg.body); + if (log_business_event) { + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_A_APP_PREP_BEGIN, &msg); + } + if (kcp_conn_send(client->conn, &msg) != 0) { + protocol_message_clear(&msg); + return -1; + } + protocol_message_clear(&msg); + return 0; +} + +static int kcp_client_send_business_preflight(kcp_client_t *client) { + if (client == NULL || client->conn == NULL) { + errno = ENOTCONN; + return -1; + } + if (!kcp_client_is_registered(client)) { + errno = ENOTCONN; + return -1; + } + return 0; +} + +static int kcp_client_handle_reserved_server_message(kcp_client_t *client, const message_t *msg) { + if (client == NULL || msg == NULL) { + errno = EINVAL; + return -1; + } + if (msg->type != MSG_TYPE_TEXT || strcmp(msg->from, SERVER_PEER_ID) != 0) { + return 0; + } + kcp_client_touch_server_activity(client); + if (kcp_client_text_body_equals(msg, KCP_CLIENT_CTRL_REGISTER_OK)) { + kcp_client_set_registered(client, 1); + kcp_client_clear_last_server_error(client); + return 1; + } + if (kcp_client_text_body_equals(msg, KCP_CLIENT_CTRL_HEARTBEAT)) { + if (kcp_client_send_text_internal(client, SERVER_PEER_ID, KCP_CLIENT_CTRL_HEARTBEAT_ACK, 0) != 0) { + kcp_client_set_registered(client, 0); + kcp_client_set_last_server_error(client, "failed to acknowledge server heartbeat"); + (void) kcp_conn_close(client->conn); + return -1; + } + return 1; + } + if (kcp_client_text_body_equals(msg, KCP_CLIENT_CTRL_HEARTBEAT_ACK)) { + return 1; + } + if (kcp_client_text_body_equals(msg, KCP_CLIENT_CTRL_PEER_REPLACED)) { + kcp_client_set_registered(client, 0); + kcp_client_set_last_server_error(client, "server peer replaced this session"); + (void) kcp_conn_close(client->conn); + errno = ECONNRESET; + return -1; + } + return 0; +} + +static int kcp_client_remaining_timeout_ms(int original_timeout_ms, uint32_t start_ms) { + uint32_t elapsed_ms; + + if (original_timeout_ms < 0) { + return -1; + } + elapsed_ms = omni_now_millis32() - start_ms; + if (elapsed_ms >= (uint32_t) original_timeout_ms) { + return 0; + } + return original_timeout_ms - (int) elapsed_ms; +} + +static int kcp_client_wait_for_register_ok(kcp_client_t *client) { + uint32_t start_ms; + + if (client == NULL || client->conn == NULL) { + errno = EINVAL; + return -1; + } + + start_ms = omni_now_millis32(); + for (;;) { + message_t msg; + int rc; + int remaining_timeout_ms = kcp_client_remaining_timeout_ms(KCP_CLIENT_REGISTER_TIMEOUT_MS, start_ms); + + if (remaining_timeout_ms <= 0) { + kcp_client_set_registered(client, 0); + kcp_client_set_last_server_error(client, "timed out waiting for server_register_ok"); + (void) kcp_conn_close(client->conn); + errno = ETIMEDOUT; + return -1; + } + + protocol_message_init(&msg); + rc = kcp_conn_receive_timed(client->conn, &msg, remaining_timeout_ms); + if (rc == 1) { + protocol_message_clear(&msg); + kcp_client_set_registered(client, 0); + kcp_client_set_last_server_error(client, "timed out waiting for server_register_ok"); + (void) kcp_conn_close(client->conn); + errno = ETIMEDOUT; + return -1; + } + if (rc != 0) { + protocol_message_clear(&msg); + kcp_client_set_registered(client, 0); + return -1; + } + if (msg.type == MSG_TYPE_ERROR && strcmp(msg.from, SERVER_PEER_ID) == 0) { + char error_text[256]; + + kcp_client_copy_server_error_body(&msg, error_text, sizeof(error_text)); + kcp_client_touch_server_activity(client); + kcp_client_set_registered(client, 0); + kcp_client_set_last_server_error(client, error_text); + protocol_message_clear(&msg); + (void) kcp_conn_close(client->conn); + errno = kcp_client_registration_errno_from_message(error_text); + return -1; + } + rc = kcp_client_handle_reserved_server_message(client, &msg); + protocol_message_clear(&msg); + if (rc < 0) { + return -1; + } + if (rc > 0 && kcp_client_is_registered(client)) { + return 0; + } + + kcp_client_set_registered(client, 0); + kcp_client_set_last_server_error(client, "unexpected message while waiting for server_register_ok"); + (void) kcp_conn_close(client->conn); + errno = EPROTO; + return -1; + } +} + +static int kcp_client_receive_business_timed(kcp_client_t *client, message_t *out_msg, int timeout_ms) { + uint32_t start_ms; + + if (client == NULL || out_msg == NULL || client->conn == NULL) { + errno = EINVAL; + return -1; + } + + start_ms = omni_now_millis32(); + protocol_message_init(out_msg); + for (;;) { + int rc; + int reserved_rc; + int effective_timeout_ms = timeout_ms < 0 ? -1 : kcp_client_remaining_timeout_ms(timeout_ms, start_ms); + + if (timeout_ms >= 0 && effective_timeout_ms <= 0) { + return 1; + } + protocol_message_clear(out_msg); + rc = kcp_conn_receive_timed(client->conn, out_msg, effective_timeout_ms); + if (rc != 0) { + if (rc != 1) { + kcp_client_set_registered(client, 0); + } + return rc; + } + + if (strcmp(out_msg->from, SERVER_PEER_ID) == 0) { + kcp_client_touch_server_activity(client); + } + reserved_rc = kcp_client_handle_reserved_server_message(client, out_msg); + if (reserved_rc < 0) { + protocol_message_clear(out_msg); + return -1; + } + if (reserved_rc > 0) { + protocol_message_clear(out_msg); + continue; + } + if (out_msg->type == MSG_TYPE_ERROR && strcmp(out_msg->from, SERVER_PEER_ID) == 0) { + char error_text[256]; + + kcp_client_copy_server_error_body(out_msg, error_text, sizeof(error_text)); + kcp_client_set_last_server_error(client, error_text); + if (kcp_client_server_error_invalidates_registration(error_text)) { + kcp_client_set_registered(client, 0); + } + } + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_B_APP_RECV, out_msg); + return 0; + } +} + +static int kcp_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) { + return -1; + } + if (msg->type == MSG_TYPE_TEXT) { + char *body = omni_json_escape_bytes(msg->body, msg->body_len); + char *from = omni_json_escape(msg->from); + char *to = omni_json_escape(msg->to); + char *line; + + if (body == NULL || from == NULL || to == NULL) { + free(body); + free(from); + free(to); + return -1; + } + snprintf(path, sizeof(path), "%s/messages.log", inbox_dir); + line = omni_strdup_printf( + "{\"message_type\":\"%s\",\"message_id\":%" PRIu64 ",\"from\":\"%s\",\"to\":\"%s\",\"body\":\"%s\"}\n", + protocol_message_type_name(msg->type), + msg->id, + from, + to, + body + ); + free(body); + free(from); + free(to); + if (line == NULL) { + return -1; + } + if (omni_append_file(path, (const uint8_t *) line, strlen(line)) != 0) { + free(line); + return -1; + } + free(line); + } else if (msg->type == MSG_TYPE_FILE) { + const char *file_name = omni_path_base_name(msg->file_name); + if (file_name[0] == '\0') { + file_name = "unnamed"; + } + snprintf(path, sizeof(path), "%s/%s-%" PRIu64 "-%s", inbox_dir, msg->from, msg->id, file_name); + if (omni_write_file(path, msg->body, msg->body_len) != 0) { + return -1; + } + } else if (msg->type == MSG_TYPE_BINARY) { + snprintf(path, sizeof(path), "%s/%s-%" PRIu64 ".bin", inbox_dir, msg->from, msg->id); + if (omni_write_file(path, msg->body, msg->body_len) != 0) { + return -1; + } + } else { + errno = EINVAL; + return -1; + } + + if (out_path != NULL && out_path_len > 0) { + snprintf(out_path, out_path_len, "%s", path); + } + return 0; +} + +static void kcp_client_fill_recv_meta(kcp_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); +} + +kcp_client_t *kcp_client_dial_with_options(const char *server_addr, const char *dial_addr, const char *peer_id, const char *bind_ip, const char *bind_device, const kcp_conn_options_t *options, latency_logger_t *logger, kcp_packet_debug_logger_t *packet_logger, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms) { + kcp_client_t *client; + const char *actual_dial_addr = (dial_addr != NULL && dial_addr[0] != '\0') ? dial_addr : server_addr; + message_t register_msg; + int saved_errno = 0; + + client = (kcp_client_t *) calloc(1, sizeof(*client)); + if (client == NULL) { + return NULL; + } + snprintf(client->id, sizeof(client->id), "%s", peer_id); + snprintf(client->server_addr, sizeof(client->server_addr), "%s", server_addr == NULL ? "" : server_addr); + pthread_mutex_init(&client->state_mu, NULL); + client->last_server_activity_ms = omni_now_millis32(); + client->logger = logger; + client->conn = kcp_conn_dial_with_options(actual_dial_addr, bind_ip, bind_device, options, packet_logger, logger, OMNI_NODE_ROLE_PEER, peer_id, stats_logger, stats_interval_ms); + if (client->conn == NULL) { + saved_errno = errno; + kcp_client_free(client); + errno = saved_errno; + return NULL; + } + + protocol_message_init(®ister_msg); + register_msg.type = MSG_TYPE_REGISTER; + register_msg.id = 0; + snprintf(register_msg.from, sizeof(register_msg.from), "%s", peer_id); + snprintf(register_msg.to, sizeof(register_msg.to), "%s", SERVER_PEER_ID); + if (kcp_conn_send(client->conn, ®ister_msg) != 0) { + saved_errno = errno; + kcp_client_free(client); + errno = saved_errno; + return NULL; + } + if (kcp_client_wait_for_register_ok(client) != 0) { + saved_errno = errno; + kcp_client_free(client); + errno = saved_errno; + return NULL; + } + return client; +} + +kcp_client_t *kcp_client_dial(const char *server_addr, const char *dial_addr, const char *peer_id, const char *bind_ip, const char *bind_device, latency_logger_t *logger, kcp_packet_debug_logger_t *packet_logger, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms) { + return kcp_client_dial_with_options(server_addr, dial_addr, peer_id, bind_ip, bind_device, NULL, logger, packet_logger, stats_logger, stats_interval_ms); +} + +const char *kcp_client_id(const kcp_client_t *client) { + return client == NULL ? "" : client->id; +} + +int kcp_client_send_text(kcp_client_t *client, const char *to, const char *text) { + if (client == NULL || to == NULL || text == NULL) { + errno = EINVAL; + return -1; + } + if (kcp_client_send_business_preflight(client) != 0) { + return -1; + } + return kcp_client_send_text_internal(client, to, text, 1); +} + +int kcp_client_send_binary(kcp_client_t *client, const char *to, const void *data, size_t data_len) { + return kcp_client_send_binary_with_id(client, to, data, data_len, NULL); +} + +int kcp_client_send_binary_with_id( + kcp_client_t *client, + const char *to, + const void *data, + size_t data_len, + uint64_t *out_id +) { + message_t msg; + uint64_t id; + + if (client == NULL || to == NULL || (data == NULL && data_len > 0)) { + errno = EINVAL; + return -1; + } + if (kcp_client_send_business_preflight(client) != 0) { + return -1; + } + protocol_message_init(&msg); + if (kcp_client_next_message_id(client, &id) != 0) { + return -1; + } + 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 (kcp_conn_send(client->conn, &msg) != 0) { + protocol_message_clear(&msg); + return -1; + } + if (out_id != NULL) { + *out_id = id; + } + protocol_message_clear(&msg); + return 0; +} + +int kcp_client_send_file_path(kcp_client_t *client, const char *to, const char *path) { + message_t msg; + uint64_t id; + uint8_t *body = NULL; + size_t body_len = 0; + const char *base_name = strrchr(path, '/'); + + if (client == NULL || to == NULL || path == NULL) { + errno = EINVAL; + return -1; + } + if (kcp_client_send_business_preflight(client) != 0) { + return -1; + } + if (omni_read_file(path, &body, &body_len) != 0) { + return -1; + } + protocol_message_init(&msg); + if (kcp_client_next_message_id(client, &id) != 0) { + free(body); + return -1; + } + msg.type = MSG_TYPE_FILE; + msg.id = id; + snprintf(msg.from, sizeof(msg.from), "%s", client->id); + snprintf(msg.to, sizeof(msg.to), "%s", to); + snprintf(msg.file_name, sizeof(msg.file_name), "%s", base_name == NULL ? path : base_name + 1); + msg.body = body; + msg.body_len = body_len; + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_A_APP_PREP_BEGIN, &msg); + if (kcp_conn_send(client->conn, &msg) != 0) { + protocol_message_clear(&msg); + return -1; + } + protocol_message_clear(&msg); + return 0; +} + +int kcp_client_receive_timed(kcp_client_t *client, message_t *out_msg, int timeout_ms) { + if (client == NULL || out_msg == NULL) { + errno = EINVAL; + return -1; + } + return kcp_client_receive_business_timed(client, out_msg, timeout_ms); +} + +int kcp_client_receive(kcp_client_t *client, message_t *out_msg) { + if (kcp_client_receive_timed(client, out_msg, -1) != 0) { + return -1; + } + return 0; +} + +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) { + 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 = kcp_client_receive_timed(client, &msg, timeout_ms); + if (rc != 0) { + protocol_message_clear(&msg); + return rc; + } + + kcp_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 kcp_client_persist_message(kcp_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; + return -1; + } + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_B_PERSIST_BEGIN, msg); + if (kcp_client_persist_message_to_disk(msg, inbox_dir, out_path, out_path_len) != 0) { + return -1; + } + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_B_PERSIST_END, msg); + return 0; +} + +void kcp_client_state_snapshot(kcp_client_t *client, kcp_client_state_t *out_state) { + kcp_runtime_stats_t runtime_stats; + + if (out_state == NULL) { + return; + } + memset(out_state, 0, sizeof(*out_state)); + if (client == NULL) { + return; + } + memset(&runtime_stats, 0, sizeof(runtime_stats)); + if (client->conn != NULL) { + kcp_conn_runtime_stats_snapshot(client->conn, &runtime_stats); + out_state->connected = runtime_stats.connected; + } + pthread_mutex_lock(&client->state_mu); + out_state->registered = client->registered; + out_state->server_idle_ms = client->last_server_activity_ms == 0 + ? 0 + : (omni_now_millis32() - client->last_server_activity_ms); + snprintf(out_state->last_server_error, sizeof(out_state->last_server_error), "%s", client->last_server_error); + pthread_mutex_unlock(&client->state_mu); +} + +void kcp_client_runtime_stats_snapshot(kcp_client_t *client, kcp_runtime_stats_t *out_stats) { + if (out_stats == NULL) { + return; + } + + 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) { + if (client == NULL) { + return 0; + } + kcp_client_set_registered(client, 0); + return kcp_conn_close(client->conn); +} + +void kcp_client_free(kcp_client_t *client) { + if (client == NULL) { + return; + } + kcp_conn_free(client->conn); + pthread_mutex_destroy(&client->state_mu); + free(client); +} diff --git a/robot/v4l2/OmniSocketGo_robot/src/peer_udp_client.c b/robot/v4l2/OmniSocketGo_robot/src/peer_udp_client.c new file mode 100644 index 0000000..9e617ab --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/src/peer_udp_client.c @@ -0,0 +1,297 @@ +#include "peer_udp_client.h" + +#include +#include +#include + +struct udp_client { + char id[OMNI_MAX_PEER_ID]; + udp_conn_t *conn; + latency_logger_t *logger; + pthread_mutex_t id_mu; + uint64_t next_message_id; +}; + +static int client_next_message_id(udp_client_t *client, uint64_t *out_id) { + pthread_mutex_lock(&client->id_mu); + *out_id = ++client->next_message_id; + pthread_mutex_unlock(&client->id_mu); + 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) { + return -1; + } + if (msg->type == MSG_TYPE_TEXT) { + char *body = omni_json_escape_bytes(msg->body, msg->body_len); + char *from = omni_json_escape(msg->from); + char *to = omni_json_escape(msg->to); + char *line; + if (body == NULL || from == NULL || to == NULL) { + free(body); + free(from); + free(to); + return -1; + } + snprintf(path, sizeof(path), "%s/messages.log", inbox_dir); + line = omni_strdup_printf("{\"message_type\":\"%s\",\"message_id\":%" PRIu64 ",\"from\":\"%s\",\"to\":\"%s\",\"body\":\"%s\"}\n", protocol_message_type_name(msg->type), msg->id, from, to, body); + free(body); + free(from); + free(to); + if (line == NULL) { + return -1; + } + if (omni_append_file(path, (const uint8_t *) line, strlen(line)) != 0) { + free(line); + return -1; + } + free(line); + } else if (msg->type == MSG_TYPE_FILE) { + const char *file_name = omni_path_base_name(msg->file_name); + if (file_name[0] == '\0') { + file_name = "unnamed"; + } + snprintf(path, sizeof(path), "%s/%s-%" PRIu64 "-%s", inbox_dir, msg->from, msg->id, file_name); + if (omni_write_file(path, msg->body, msg->body_len) != 0) { + return -1; + } + } else if (msg->type == MSG_TYPE_BINARY) { + snprintf(path, sizeof(path), "%s/%s-%" PRIu64 ".bin", inbox_dir, msg->from, msg->id); + if (omni_write_file(path, msg->body, msg->body_len) != 0) { + return -1; + } + } else { + errno = EINVAL; + return -1; + } + if (out_path != NULL && out_path_len > 0) { + snprintf(out_path, out_path_len, "%s", path); + } + return 0; +} + +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)); + if (client == NULL) { + return NULL; + } + 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, bind_device, enable_timestamping, logger, OMNI_NODE_ROLE_PEER, peer_id, debug_logger); + if (client->conn == NULL) { + udp_client_free(client); + return NULL; + } + protocol_message_init(®ister_msg); + register_msg.type = MSG_TYPE_REGISTER; + register_msg.id = 0; + snprintf(register_msg.from, sizeof(register_msg.from), "%s", peer_id); + snprintf(register_msg.to, sizeof(register_msg.to), "%s", SERVER_PEER_ID); + if (udp_conn_send(client->conn, ®ister_msg) != 0) { + udp_client_free(client); + return NULL; + } + 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; +} + +int udp_client_send_text(udp_client_t *client, const char *to, const char *text) { + message_t msg; + uint64_t id; + protocol_message_init(&msg); + client_next_message_id(client, &id); + msg.type = MSG_TYPE_TEXT; + msg.id = id; + snprintf(msg.from, sizeof(msg.from), "%s", client->id); + snprintf(msg.to, sizeof(msg.to), "%s", to); + msg.body = (uint8_t *) omni_strdup(text); + if (msg.body == NULL) { + return -1; + } + msg.body_len = strlen((const char *) msg.body); + 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_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; + uint8_t *body = NULL; + size_t body_len = 0; + const char *base_name = strrchr(path, '/'); + if (omni_read_file(path, &body, &body_len) != 0) { + return -1; + } + protocol_message_init(&msg); + client_next_message_id(client, &id); + msg.type = MSG_TYPE_FILE; + msg.id = id; + snprintf(msg.from, sizeof(msg.from), "%s", client->id); + snprintf(msg.to, sizeof(msg.to), "%s", to); + snprintf(msg.file_name, sizeof(msg.file_name), "%s", base_name == NULL ? path : base_name + 1); + msg.body = body; + msg.body_len = body_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_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; + } + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_B_APP_RECV, 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; + return -1; + } + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_B_PERSIST_BEGIN, msg); + if (client_persist_message_to_disk(msg, inbox_dir, out_path, out_path_len) != 0) { + return -1; + } + latencylog_log_message_event(client->logger, OMNI_NODE_ROLE_PEER, client->id, EVENT_B_PERSIST_END, msg); + return 0; +} + +int udp_client_close(udp_client_t *client) { + return client == NULL ? 0 : udp_conn_close(client->conn); +} + +void udp_client_free(udp_client_t *client) { + if (client == NULL) { + return; + } + udp_conn_free(client->conn); + pthread_mutex_destroy(&client->id_mu); + free(client); +} diff --git a/robot/v4l2/OmniSocketGo_robot/src/protocol.c b/robot/v4l2/OmniSocketGo_robot/src/protocol.c new file mode 100644 index 0000000..d82d045 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/src/protocol.c @@ -0,0 +1,415 @@ +#include "protocol.h" + +#include "cJSON.h" + +#include + +static const char *protocol_message_type_table[] = { + "text", + "file", + "register", + "error", + "binary" +}; + +const char *protocol_message_type_name(message_type_t type) { + if ((int) type < 0 || (size_t) type >= OMNI_ARRAY_LEN(protocol_message_type_table)) { + return "invalid"; + } + return protocol_message_type_table[type]; +} + +int protocol_message_type_from_name(const char *raw, message_type_t *out) { + size_t i; + if (raw == NULL || out == NULL) { + return -1; + } + for (i = 0; i < OMNI_ARRAY_LEN(protocol_message_type_table); ++i) { + if (strcmp(raw, protocol_message_type_table[i]) == 0) { + *out = (message_type_t) i; + return 0; + } + } + return -1; +} + +void protocol_message_init(message_t *msg) { + if (msg == NULL) { + return; + } + memset(msg, 0, sizeof(*msg)); + msg->type = MSG_TYPE_INVALID; +} + +void protocol_message_clear(message_t *msg) { + if (msg == NULL) { + return; + } + free(msg->body); + protocol_message_init(msg); +} + +int protocol_message_copy(message_t *dst, const message_t *src) { + if (dst == NULL || src == NULL) { + errno = EINVAL; + return -1; + } + protocol_message_clear(dst); + memcpy(dst, src, sizeof(*dst)); + dst->body = NULL; + if (src->body_len > 0) { + dst->body = (uint8_t *) malloc(src->body_len); + if (dst->body == NULL) { + protocol_message_init(dst); + errno = ENOMEM; + return -1; + } + memcpy(dst->body, src->body, src->body_len); + } + return 0; +} + +static int protocol_set_err(char *err, size_t err_len, const char *fmt, ...) { + va_list args; + if (err != NULL && err_len > 0) { + va_start(args, fmt); + vsnprintf(err, err_len, fmt, args); + va_end(args); + } + return -1; +} + +int protocol_validate_message(const message_t *msg, char *err, size_t err_len) { + if (msg == NULL) { + return protocol_set_err(err, err_len, "protocol: nil message"); + } + if (msg->from[0] == '\0') { + return protocol_set_err(err, err_len, "protocol: missing from"); + } + if (msg->to[0] == '\0') { + return protocol_set_err(err, err_len, "protocol: missing to"); + } + switch (msg->type) { + case MSG_TYPE_TEXT: + if (msg->file_name[0] != '\0') { + return protocol_set_err(err, err_len, "protocol: unexpected file name"); + } + if (!omni_utf8_valid(msg->body, msg->body_len)) { + return protocol_set_err(err, err_len, "protocol: invalid text body"); + } + break; + case MSG_TYPE_FILE: + if (msg->file_name[0] == '\0') { + return protocol_set_err(err, err_len, "protocol: missing file name"); + } + break; + case MSG_TYPE_BINARY: + if (msg->file_name[0] != '\0') { + return protocol_set_err(err, err_len, "protocol: unexpected file name"); + } + break; + case MSG_TYPE_REGISTER: + if (strcmp(msg->to, SERVER_PEER_ID) != 0) { + return protocol_set_err(err, err_len, "protocol: invalid register target"); + } + if (msg->file_name[0] != '\0') { + return protocol_set_err(err, err_len, "protocol: unexpected file name"); + } + if (msg->body_len != 0) { + return protocol_set_err(err, err_len, "protocol: unexpected body"); + } + break; + case MSG_TYPE_ERROR: + if (strcmp(msg->from, SERVER_PEER_ID) != 0) { + return protocol_set_err(err, err_len, "protocol: invalid error source"); + } + if (msg->file_name[0] != '\0') { + return protocol_set_err(err, err_len, "protocol: unexpected file name"); + } + if (!omni_utf8_valid(msg->body, msg->body_len)) { + return protocol_set_err(err, err_len, "protocol: invalid text body"); + } + break; + default: + return protocol_set_err(err, err_len, "protocol: invalid message type"); + } + return 0; +} + +static int protocol_build_header_json(const message_t *msg, char **out_json, size_t *out_len) { + cJSON *root; + char *json; + + root = cJSON_CreateObject(); + if (root == NULL) { + errno = ENOMEM; + return -1; + } + cJSON_AddStringToObject(root, "type", protocol_message_type_name(msg->type)); + cJSON_AddNumberToObject(root, "id", (double) msg->id); + cJSON_AddStringToObject(root, "from", msg->from); + cJSON_AddStringToObject(root, "to", msg->to); + if (msg->file_name[0] != '\0') { + cJSON_AddStringToObject(root, "file_name", msg->file_name); + } + cJSON_AddNumberToObject(root, "content_length", (double) msg->body_len); + json = cJSON_PrintUnformatted(root); + cJSON_Delete(root); + if (json == NULL) { + errno = ENOMEM; + return -1; + } + *out_len = strlen(json); + *out_json = json; + return 0; +} + +int protocol_encode_message_datagram(const message_t *msg, uint8_t **out, size_t *out_len) { + uint8_t *buffer; + char *header_json; + size_t header_len; + uint32_t net_header_len; + char err[128]; + + if (out == NULL || out_len == NULL) { + errno = EINVAL; + return -1; + } + *out = NULL; + *out_len = 0; + if (protocol_validate_message(msg, err, sizeof(err)) != 0) { + errno = EINVAL; + return -1; + } + if (protocol_build_header_json(msg, &header_json, &header_len) != 0) { + return -1; + } + if (4U + header_len + msg->body_len > OMNI_MAX_FRAME_SIZE) { + cJSON_free(header_json); + errno = EMSGSIZE; + return -1; + } + buffer = (uint8_t *) malloc(4U + header_len + msg->body_len); + if (buffer == NULL) { + cJSON_free(header_json); + errno = ENOMEM; + return -1; + } + net_header_len = htonl((uint32_t) header_len); + memcpy(buffer, &net_header_len, 4); + memcpy(buffer + 4, header_json, header_len); + if (msg->body_len > 0) { + memcpy(buffer + 4 + header_len, msg->body, msg->body_len); + } + cJSON_free(header_json); + *out = buffer; + *out_len = 4U + header_len + msg->body_len; + return 0; +} + +static int protocol_copy_string_field(char *dst, size_t dst_len, const cJSON *object, const char *field, int required, char *err, size_t err_len) { + const cJSON *item = cJSON_GetObjectItemCaseSensitive(object, field); + if (item == NULL) { + if (required) { + return protocol_set_err(err, err_len, "protocol: missing %s", field); + } + dst[0] = '\0'; + return 0; + } + if (!cJSON_IsString(item) || item->valuestring == NULL) { + return protocol_set_err(err, err_len, "protocol: invalid %s", field); + } + snprintf(dst, dst_len, "%s", item->valuestring); + return 0; +} + +static int protocol_copy_u64_field(uint64_t *dst, const cJSON *object, const char *field, int required, char *err, size_t err_len) { + const cJSON *item = cJSON_GetObjectItemCaseSensitive(object, field); + if (item == NULL) { + if (required) { + return protocol_set_err(err, err_len, "protocol: missing %s", field); + } + *dst = 0; + return 0; + } + if (!cJSON_IsNumber(item)) { + return protocol_set_err(err, err_len, "protocol: invalid %s", field); + } + *dst = (uint64_t) item->valuedouble; + return 0; +} + +int protocol_decode_message_datagram(const uint8_t *data, size_t data_len, message_t *out_msg, char *err, size_t err_len) { + uint32_t net_header_len; + uint32_t header_len; + char *header_text = NULL; + cJSON *header = NULL; + const cJSON *type_item; + uint64_t content_length = 0; + + if (data == NULL || out_msg == NULL || data_len < 4U) { + return protocol_set_err(err, err_len, "protocol: invalid datagram"); + } + if (data_len > OMNI_MAX_FRAME_SIZE) { + return protocol_set_err(err, err_len, "protocol: frame too large"); + } + + protocol_message_clear(out_msg); + + memcpy(&net_header_len, data, 4); + header_len = ntohl(net_header_len); + if (header_len == 0 || (size_t) header_len > data_len - 4U) { + return protocol_set_err(err, err_len, "protocol: invalid header length"); + } + header_text = (char *) malloc((size_t) header_len + 1U); + if (header_text == NULL) { + errno = ENOMEM; + return -1; + } + memcpy(header_text, data + 4, header_len); + header_text[header_len] = '\0'; + header = cJSON_Parse(header_text); + free(header_text); + if (header == NULL || !cJSON_IsObject(header)) { + if (header != NULL) { + cJSON_Delete(header); + } + return protocol_set_err(err, err_len, "protocol: invalid header json"); + } + type_item = cJSON_GetObjectItemCaseSensitive(header, "type"); + if (type_item == NULL || !cJSON_IsString(type_item) || protocol_message_type_from_name(type_item->valuestring, &out_msg->type) != 0) { + cJSON_Delete(header); + return protocol_set_err(err, err_len, "protocol: invalid message type"); + } + if (protocol_copy_u64_field(&out_msg->id, header, "id", 1, err, err_len) != 0 || + protocol_copy_string_field(out_msg->from, sizeof(out_msg->from), header, "from", 1, err, err_len) != 0 || + protocol_copy_string_field(out_msg->to, sizeof(out_msg->to), header, "to", 1, err, err_len) != 0 || + protocol_copy_string_field(out_msg->file_name, sizeof(out_msg->file_name), header, "file_name", 0, err, err_len) != 0 || + protocol_copy_u64_field(&content_length, header, "content_length", 1, err, err_len) != 0) { + cJSON_Delete(header); + protocol_message_clear(out_msg); + return -1; + } + cJSON_Delete(header); + if ((size_t) content_length != data_len - 4U - (size_t) header_len) { + protocol_message_clear(out_msg); + return protocol_set_err(err, err_len, "protocol: invalid content length"); + } + out_msg->body_len = (size_t) content_length; + if (out_msg->body_len > 0) { + out_msg->body = (uint8_t *) malloc(out_msg->body_len); + if (out_msg->body == NULL) { + protocol_message_clear(out_msg); + errno = ENOMEM; + return -1; + } + memcpy(out_msg->body, data + 4U + header_len, out_msg->body_len); + } + if (protocol_validate_message(out_msg, err, err_len) != 0) { + protocol_message_clear(out_msg); + return -1; + } + return 0; +} + +int protocol_encode_message_stream(const message_t *msg, uint8_t **out, size_t *out_len) { + uint8_t *payload; + uint8_t *buffer; + size_t payload_len; + uint32_t net_len; + + if (protocol_encode_message_datagram(msg, &payload, &payload_len) != 0) { + return -1; + } + buffer = (uint8_t *) malloc(payload_len + 4U); + if (buffer == NULL) { + free(payload); + errno = ENOMEM; + return -1; + } + net_len = htonl((uint32_t) payload_len); + memcpy(buffer, &net_len, 4); + memcpy(buffer + 4, payload, payload_len); + free(payload); + *out = buffer; + *out_len = payload_len + 4U; + return 0; +} + +int protocol_decode_message_stream_payload(const uint8_t *payload, size_t payload_len, message_t *out_msg, char *err, size_t err_len) { + return protocol_decode_message_datagram(payload, payload_len, out_msg, err, err_len); +} + +void protocol_frame_decoder_init(protocol_frame_decoder_t *decoder) { + memset(decoder, 0, sizeof(*decoder)); +} + +void protocol_frame_decoder_reset(protocol_frame_decoder_t *decoder) { + decoder->len = 0; +} + +void protocol_frame_decoder_destroy(protocol_frame_decoder_t *decoder) { + free(decoder->buffer); + memset(decoder, 0, sizeof(*decoder)); +} + +int protocol_frame_decoder_feed(protocol_frame_decoder_t *decoder, const uint8_t *data, size_t data_len) { + uint8_t *next_buffer; + size_t next_cap; + if (decoder->len + data_len > OMNI_MAX_FRAME_SIZE * 2U) { + errno = EMSGSIZE; + return -1; + } + if (decoder->len + data_len > decoder->cap) { + next_cap = decoder->cap == 0 ? 4096U : decoder->cap; + while (next_cap < decoder->len + data_len) { + next_cap *= 2U; + } + next_buffer = (uint8_t *) realloc(decoder->buffer, next_cap); + if (next_buffer == NULL) { + errno = ENOMEM; + return -1; + } + decoder->buffer = next_buffer; + decoder->cap = next_cap; + } + memcpy(decoder->buffer + decoder->len, data, data_len); + decoder->len += data_len; + return 0; +} + +int protocol_frame_decoder_next(protocol_frame_decoder_t *decoder, uint8_t **payload, size_t *payload_len) { + uint32_t net_len; + uint32_t frame_len; + uint8_t *frame; + + if (payload == NULL || payload_len == NULL) { + errno = EINVAL; + return -1; + } + *payload = NULL; + *payload_len = 0; + if (decoder->len < 4U) { + return 0; + } + memcpy(&net_len, decoder->buffer, 4); + frame_len = ntohl(net_len); + if (frame_len == 0 || frame_len > OMNI_MAX_FRAME_SIZE) { + errno = EMSGSIZE; + return -1; + } + if (decoder->len < 4U + frame_len) { + return 0; + } + frame = (uint8_t *) malloc(frame_len); + if (frame == NULL) { + errno = ENOMEM; + return -1; + } + memcpy(frame, decoder->buffer + 4, frame_len); + memmove(decoder->buffer, decoder->buffer + 4U + frame_len, decoder->len - 4U - frame_len); + decoder->len -= 4U + frame_len; + *payload = frame; + *payload_len = frame_len; + return 1; +} diff --git a/robot/v4l2/OmniSocketGo_robot/src/server_kcp_hub.c b/robot/v4l2/OmniSocketGo_robot/src/server_kcp_hub.c new file mode 100644 index 0000000..cdcf2f3 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/src/server_kcp_hub.c @@ -0,0 +1,1136 @@ +#include "server_kcp_hub.h" + +#include "cJSON.h" + +#include +#include +#include +#include +#include + +#define KCP_RELAY_MAX_DATAGRAM_SIZE (60 * 1024) +#define KCP_HUB_MAINTENANCE_INTERVAL_MS 250 +#define KCP_HUB_DEFAULT_TELEMETRY_INTERVAL_MS 500 +#define KCP_HUB_DEFAULT_HEARTBEAT_INTERVAL_MS 1000 +#define KCP_HUB_DEFAULT_LEASE_TIMEOUT_MS 4000 +#define KCP_HUB_TELEMETRY_NODE_ID "hub-telemetry" +#define KCP_HUB_DEFAULT_NODE_ID "hub" +#define KCP_HUB_CTRL_REGISTER_OK "{\"type\":\"server_register_ok\"}" +#define KCP_HUB_CTRL_PEER_REPLACED "{\"type\":\"server_peer_replaced\",\"reason\":\"new_instance_wins\"}" +#define KCP_HUB_CTRL_HEARTBEAT "{\"type\":\"server_heartbeat\"}" +#define KCP_HUB_CTRL_HEARTBEAT_ACK "{\"type\":\"server_heartbeat_ack\"}" + +typedef struct kcp_peer_entry { + struct kcp_peer_entry *next; + char peer_id[OMNI_MAX_PEER_ID]; + kcp_conn_t *conn; + uint32_t last_seen_ms; + uint32_t last_heartbeat_sent_ms; +} kcp_peer_entry_t; + +typedef struct kcp_session_thread_ctx { + kcp_hub_t *hub; + kcp_conn_t *conn; +} kcp_session_thread_ctx_t; + +typedef struct kcp_hub_pending_action { + struct kcp_hub_pending_action *next; + char peer_id[OMNI_MAX_PEER_ID]; + kcp_conn_t *conn; +} kcp_hub_pending_action_t; + +struct kcp_hub { + pthread_rwlock_t lock; + kcp_peer_entry_t *peers; + 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; + int heartbeat_interval_ms; + int lease_timeout_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; + 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_send_server_text(kcp_conn_t *conn, const char *to, const char *payload); +static void kcp_hub_touch_peer(kcp_hub_t *hub, const char *peer_id, kcp_conn_t *conn); +static void kcp_hub_run_maintenance(kcp_hub_t *hub); + +static uint32_t kcp_hub_now_ms(void) { + return omni_now_millis32(); +} + +static uint32_t kcp_hub_elapsed_ms(uint32_t now_ms, uint32_t then_ms) { + return now_ms - then_ms; +} + +static int kcp_hub_text_body_equals(const message_t *msg, const char *payload) { + size_t expected_len; + + if (msg == NULL || payload == NULL) { + return 0; + } + expected_len = strlen(payload); + return msg->body_len == expected_len && msg->body != NULL && memcmp(msg->body, payload, expected_len) == 0; +} + +static int kcp_hub_append_pending_action(kcp_hub_pending_action_t **head, const char *peer_id, kcp_conn_t *conn) { + kcp_hub_pending_action_t *action; + + if (head == NULL || peer_id == NULL || conn == NULL) { + errno = EINVAL; + return -1; + } + action = (kcp_hub_pending_action_t *) calloc(1, sizeof(*action)); + if (action == NULL) { + return -1; + } + snprintf(action->peer_id, sizeof(action->peer_id), "%s", peer_id); + action->conn = conn; + action->next = *head; + *head = action; + return 0; +} + +static void kcp_hub_free_pending_actions(kcp_hub_pending_action_t *head) { + while (head != NULL) { + kcp_hub_pending_action_t *next = head->next; + free(head); + head = next; + } +} + +static int kcp_hub_peer_is_telemetry(const char *peer_id) { + return kcp_hub_peer_id_has_suffix(peer_id, "-telemetry"); +} + +static int kcp_hub_peer_is_video_receiver(const char *peer_id) { + return peer_id != NULL && strcmp(peer_id, "peer-a-video") == 0; +} + +static int kcp_hub_peer_uses_server_lease(const char *peer_id) { + if (peer_id == NULL || peer_id[0] == '\0') { + return 0; + } + return kcp_hub_peer_id_has_suffix(peer_id, "-ctrl") + || kcp_hub_peer_is_telemetry(peer_id) + || kcp_hub_peer_is_video_receiver(peer_id); +} + +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; + + if (hub == NULL || peer_id == NULL || peer_id[0] == '\0') { + return; + } + + pthread_rwlock_wrlock(&hub->lock); + for (entry = hub->peers; entry != NULL; entry = entry->next) { + if (strcmp(entry->peer_id, peer_id) == 0 && entry->conn == conn) { + if (prev == NULL) { + hub->peers = entry->next; + } else { + prev->next = entry->next; + } + free(entry); + break; + } + prev = entry; + } + pthread_rwlock_unlock(&hub->lock); +} + +static kcp_peer_entry_t *kcp_hub_find_peer(kcp_hub_t *hub, const char *peer_id) { + kcp_peer_entry_t *entry; + for (entry = hub->peers; entry != NULL; entry = entry->next) { + if (strcmp(entry->peer_id, peer_id) == 0) { + return entry; + } + } + return NULL; +} + +static int kcp_hub_peer_id_has_suffix(const char *peer_id, const char *suffix) { + size_t peer_len; + size_t suffix_len; + + if (peer_id == NULL || suffix == NULL) { + return 0; + } + peer_len = strlen(peer_id); + suffix_len = strlen(suffix); + return peer_len >= suffix_len && strcmp(peer_id + peer_len - suffix_len, suffix) == 0; +} + +static int kcp_hub_configure_peer_transport(kcp_conn_t *conn, const char *peer_id) { + kcp_conn_options_t options; + + if (conn == NULL || peer_id == NULL) { + errno = EINVAL; + return -1; + } + if (kcp_hub_peer_id_has_suffix(peer_id, "-ctrl")) { + kcp_conn_options_set_control_defaults(&options); + return kcp_conn_apply_options(conn, &options); + } + if (kcp_hub_peer_id_has_suffix(peer_id, "-video")) { + 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 void kcp_hub_touch_peer_locked(kcp_hub_t *hub, const char *peer_id, kcp_conn_t *conn) { + kcp_peer_entry_t *entry; + + if (hub == NULL || peer_id == NULL || peer_id[0] == '\0') { + return; + } + entry = kcp_hub_find_peer(hub, peer_id); + if (entry != NULL && (conn == NULL || entry->conn == conn)) { + entry->last_seen_ms = kcp_hub_now_ms(); + } +} + +static void kcp_hub_touch_peer(kcp_hub_t *hub, const char *peer_id, kcp_conn_t *conn) { + if (hub == NULL || peer_id == NULL || peer_id[0] == '\0') { + return; + } + pthread_rwlock_wrlock(&hub->lock); + kcp_hub_touch_peer_locked(hub, peer_id, conn); + pthread_rwlock_unlock(&hub->lock); +} + +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, "min_srtt_ms", (double) stats->min_srtt_ms) == NULL || + cJSON_AddNumberToObject(object, "srttvar_ms", (double) stats->srttvar_ms) == NULL || + cJSON_AddNumberToObject(object, "last_feedback_age_ms", (double) stats->last_feedback_age_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; + uint32_t last_telemetry_push_ms = 0; + + while (!atomic_load(&hub->closed)) { + int interval_ms = KCP_HUB_DEFAULT_TELEMETRY_INTERVAL_MS; + uint32_t now_ms = kcp_hub_now_ms(); + int telemetry_enabled = 0; + + pthread_rwlock_rdlock(&hub->lock); + telemetry_enabled = hub->telemetry_peer_id[0] != '\0'; + if (telemetry_enabled && hub->telemetry_interval_ms > 0) { + interval_ms = hub->telemetry_interval_ms; + } + pthread_rwlock_unlock(&hub->lock); + + if (telemetry_enabled && (last_telemetry_push_ms == 0 || kcp_hub_elapsed_ms(now_ms, last_telemetry_push_ms) >= (uint32_t) interval_ms)) { + (void) kcp_hub_push_telemetry_snapshot(hub); + last_telemetry_push_ms = now_ms; + } + kcp_hub_run_maintenance(hub); + if (atomic_load(&hub->closed)) { + break; + } + usleep((useconds_t) KCP_HUB_MAINTENANCE_INTERVAL_MS * 1000U); + } + return NULL; +} + +static int kcp_hub_send_server_text(kcp_conn_t *conn, const char *to, const char *payload) { + message_t msg; + + 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", (to == NULL || to[0] == '\0') ? "unknown" : to); + msg.body = (uint8_t *) omni_strdup(payload == NULL ? "" : payload); + if (msg.body == NULL) { + return -1; + } + msg.body_len = strlen((const char *) msg.body); + if (kcp_conn_send(conn, &msg) != 0) { + protocol_message_clear(&msg); + return -1; + } + protocol_message_clear(&msg); + return 0; +} + +static int kcp_hub_send_server_error(kcp_conn_t *conn, const char *to, const char *message) { + message_t msg; + protocol_message_init(&msg); + msg.type = MSG_TYPE_ERROR; + snprintf(msg.from, sizeof(msg.from), "%s", SERVER_PEER_ID); + snprintf(msg.to, sizeof(msg.to), "%s", (to == NULL || to[0] == '\0') ? "unknown" : to); + msg.body = (uint8_t *) omni_strdup(message == NULL ? "" : message); + if (msg.body == NULL) { + return -1; + } + msg.body_len = strlen((const char *) msg.body); + if (kcp_conn_send(conn, &msg) != 0) { + protocol_message_clear(&msg); + return -1; + } + protocol_message_clear(&msg); + return 0; +} + +static int kcp_hub_sockaddr_equal(const struct sockaddr *left, socklen_t left_len, const struct sockaddr *right, socklen_t right_len) { + char left_text[OMNI_MAX_ADDR_TEXT]; + char right_text[OMNI_MAX_ADDR_TEXT]; + + if (left == NULL || right == NULL) { + return left == right; + } + return strcmp( + omni_sockaddr_to_string(left, left_len, left_text, sizeof(left_text)), + omni_sockaddr_to_string(right, right_len, right_text, sizeof(right_text)) + ) == 0; +} + +static int kcp_hub_accept_relay_peer(kcp_hub_t *hub, const struct sockaddr *addr, socklen_t addr_len) { + int accepted = 0; + + pthread_rwlock_wrlock(&hub->lock); + if (hub->relay_peer_addr_len == 0 && hub->relay_learn_peer) { + omni_clone_sockaddr(addr, addr_len, &hub->relay_peer_addr, &hub->relay_peer_addr_len); + accepted = 1; + } else if (hub->relay_peer_addr_len == 0) { + accepted = 1; + } else { + accepted = kcp_hub_sockaddr_equal((const struct sockaddr *) &hub->relay_peer_addr, hub->relay_peer_addr_len, addr, addr_len); + } + pthread_rwlock_unlock(&hub->lock); + return accepted; +} + +static int kcp_hub_forward_to_relay(kcp_hub_t *hub, const message_t *msg, int *relay_status) { + uint8_t *payload = NULL; + size_t payload_len = 0; + struct sockaddr_storage relay_addr; + socklen_t relay_addr_len = 0; + int relay_fd = -1; + int relay_configured = 0; + + if (relay_status != NULL) { + *relay_status = 0; + } + if (protocol_encode_message_datagram(msg, &payload, &payload_len) != 0) { + return -1; + } + if (payload_len > KCP_RELAY_MAX_DATAGRAM_SIZE) { + free(payload); + errno = EMSGSIZE; + if (relay_status != NULL) { + *relay_status = 3; + } + return -1; + } + + pthread_rwlock_rdlock(&hub->lock); + relay_fd = hub->relay_fd; + relay_configured = hub->relay_configured; + if (hub->relay_peer_addr_len > 0) { + omni_clone_sockaddr((const struct sockaddr *) &hub->relay_peer_addr, hub->relay_peer_addr_len, &relay_addr, &relay_addr_len); + } + pthread_rwlock_unlock(&hub->lock); + + if (!relay_configured || relay_fd < 0) { + free(payload); + errno = ENOTCONN; + if (relay_status != NULL) { + *relay_status = 1; + } + return -1; + } + if (relay_addr_len == 0) { + free(payload); + errno = EDESTADDRREQ; + if (relay_status != NULL) { + *relay_status = 2; + } + return -1; + } + if (sendto(relay_fd, payload, payload_len, 0, (struct sockaddr *) &relay_addr, relay_addr_len) < 0) { + free(payload); + return -1; + } + free(payload); + return 0; +} + +static int kcp_hub_forward_relay_server_error(kcp_hub_t *hub, const char *to, const char *message) { + message_t msg; + int rc; + + protocol_message_init(&msg); + msg.type = MSG_TYPE_ERROR; + snprintf(msg.from, sizeof(msg.from), "%s", SERVER_PEER_ID); + snprintf(msg.to, sizeof(msg.to), "%s", (to == NULL || to[0] == '\0') ? "unknown" : to); + msg.body = (uint8_t *) omni_strdup(message == NULL ? "" : message); + if (msg.body == NULL) { + return -1; + } + msg.body_len = strlen((const char *) msg.body); + rc = kcp_hub_forward_to_relay(hub, &msg, NULL); + protocol_message_clear(&msg); + return rc; +} + +static int kcp_hub_deliver_to_local_peer(kcp_hub_t *hub, const message_t *msg) { + kcp_conn_t *target_conn = NULL; + int rc; + + pthread_rwlock_rdlock(&hub->lock); + { + kcp_peer_entry_t *entry = kcp_hub_find_peer(hub, msg->to); + if (entry != NULL) { + target_conn = entry->conn; + } + } + pthread_rwlock_unlock(&hub->lock); + + if (target_conn == NULL) { + errno = ENOENT; + return -1; + } + rc = kcp_conn_send(target_conn, msg); + if (rc != 0) { + kcp_hub_unregister(hub, msg->to, target_conn); + kcp_conn_close(target_conn); + return -1; + } + return 0; +} + +static int kcp_hub_deliver_relayed_message(kcp_hub_t *hub, const message_t *msg) { + char *error_text; + + if (kcp_hub_deliver_to_local_peer(hub, msg) == 0) { + return 0; + } + if (errno != ENOENT) { + if (msg->type == MSG_TYPE_ERROR) { + return 0; + } + error_text = omni_strdup_printf("failed to forward to %s", msg->to); + if (error_text == NULL) { + return -1; + } + if (kcp_hub_forward_relay_server_error(hub, msg->from, error_text) != 0) { + free(error_text); + return -1; + } + free(error_text); + return 0; + } + + if (msg->type == MSG_TYPE_ERROR) { + return 0; + } + + error_text = omni_strdup_printf("unknown target: %s", msg->to); + if (error_text == NULL) { + return -1; + } + if (kcp_hub_forward_relay_server_error(hub, msg->from, error_text) != 0) { + free(error_text); + return -1; + } + free(error_text); + return 0; +} + +static int kcp_hub_handle_peer_message(kcp_hub_t *hub, const char *peer_id, kcp_conn_t *conn, message_t *msg) { + char *error_text = NULL; + int relay_status = 0; + + kcp_hub_touch_peer(hub, peer_id, conn); + switch (msg->type) { + case MSG_TYPE_TEXT: + if (strcmp(msg->to, SERVER_PEER_ID) == 0) { + if (kcp_hub_text_body_equals(msg, KCP_HUB_CTRL_HEARTBEAT_ACK)) { + return 0; + } + if (kcp_hub_send_server_error(conn, peer_id, "unsupported server control message") != 0) { + return -1; + } + errno = EPROTO; + return -1; + } + snprintf(msg->from, sizeof(msg->from), "%s", peer_id); + if (kcp_hub_deliver_to_local_peer(hub, msg) == 0) { + return 0; + } + if (errno != ENOENT) { + error_text = omni_strdup_printf("failed to forward to %s", msg->to); + if (error_text == NULL) { + return -1; + } + if (kcp_hub_send_server_error(conn, peer_id, error_text) != 0) { + free(error_text); + return -1; + } + free(error_text); + return 0; + } + if (kcp_hub_forward_to_relay(hub, msg, &relay_status) == 0) { + return 0; + } + if (relay_status == 1) { + error_text = omni_strdup_printf("unknown target: %s", msg->to); + } else if (relay_status == 2) { + error_text = omni_strdup("failed to relay to remote peer"); + } else if (relay_status == 3) { + error_text = omni_strdup("message too large for relay udp"); + } else { + error_text = omni_strdup("failed to relay to remote peer"); + } + if (error_text == NULL) { + return -1; + } + if (kcp_hub_send_server_error(conn, peer_id, error_text) != 0) { + free(error_text); + return -1; + } + free(error_text); + return 0; + case MSG_TYPE_FILE: + case MSG_TYPE_BINARY: + snprintf(msg->from, sizeof(msg->from), "%s", peer_id); + if (kcp_hub_deliver_to_local_peer(hub, msg) == 0) { + return 0; + } + if (errno != ENOENT) { + error_text = omni_strdup_printf("failed to forward to %s", msg->to); + if (error_text == NULL) { + return -1; + } + if (kcp_hub_send_server_error(conn, peer_id, error_text) != 0) { + free(error_text); + return -1; + } + free(error_text); + return 0; + } + if (kcp_hub_forward_to_relay(hub, msg, &relay_status) == 0) { + return 0; + } + if (relay_status == 1) { + error_text = omni_strdup_printf("unknown target: %s", msg->to); + } else if (relay_status == 2) { + error_text = omni_strdup("failed to relay to remote peer"); + } else if (relay_status == 3) { + error_text = omni_strdup("message too large for relay udp"); + } else { + error_text = omni_strdup("failed to relay to remote peer"); + } + if (error_text == NULL) { + return -1; + } + if (kcp_hub_send_server_error(conn, peer_id, error_text) != 0) { + free(error_text); + return -1; + } + free(error_text); + return 0; + case MSG_TYPE_REGISTER: + case MSG_TYPE_ERROR: + if (kcp_hub_send_server_error(conn, peer_id, "registered peers can only send text, file, or binary messages") != 0) { + return -1; + } + errno = EPROTO; + return -1; + default: + error_text = omni_strdup_printf("unsupported message type: %s", protocol_message_type_name(msg->type)); + if (error_text == NULL) { + return -1; + } + if (kcp_hub_send_server_error(conn, peer_id, error_text) != 0) { + free(error_text); + return -1; + } + free(error_text); + errno = EPROTO; + return -1; + } +} + +static int kcp_hub_commit_registered_conn( + kcp_hub_t *hub, + const char *peer_id, + kcp_conn_t *conn, + uint32_t now_ms, + kcp_conn_t **out_old_conn +) { + kcp_peer_entry_t *entry; + + if (hub == NULL || peer_id == NULL || peer_id[0] == '\0' || conn == NULL) { + errno = EINVAL; + return -1; + } + if (out_old_conn != NULL) { + *out_old_conn = NULL; + } + + pthread_rwlock_wrlock(&hub->lock); + entry = kcp_hub_find_peer(hub, peer_id); + if (entry != NULL) { + if (out_old_conn != NULL) { + *out_old_conn = entry->conn; + } + entry->conn = conn; + entry->last_seen_ms = now_ms; + entry->last_heartbeat_sent_ms = 0; + pthread_rwlock_unlock(&hub->lock); + return 0; + } + + entry = (kcp_peer_entry_t *) calloc(1, sizeof(*entry)); + if (entry == NULL) { + pthread_rwlock_unlock(&hub->lock); + return -1; + } + snprintf(entry->peer_id, sizeof(entry->peer_id), "%s", peer_id); + entry->conn = conn; + entry->last_seen_ms = now_ms; + entry->last_heartbeat_sent_ms = 0; + entry->next = hub->peers; + hub->peers = entry; + pthread_rwlock_unlock(&hub->lock); + return 0; +} + +static int kcp_hub_register_conn(kcp_hub_t *hub, kcp_conn_t *conn, char *peer_id, size_t peer_id_len) { + message_t msg; + kcp_conn_t *old_conn = NULL; + uint32_t now_ms; + + protocol_message_init(&msg); + if (kcp_conn_receive(conn, &msg) != 0) { + protocol_message_clear(&msg); + return -1; + } + if (msg.type != MSG_TYPE_REGISTER) { + kcp_hub_send_server_error(conn, msg.from, "first message must be register"); + protocol_message_clear(&msg); + errno = EPROTO; + return -1; + } + + snprintf(peer_id, peer_id_len, "%s", msg.from); + if (kcp_hub_send_server_text(conn, msg.from, KCP_HUB_CTRL_REGISTER_OK) != 0) { + protocol_message_clear(&msg); + return -1; + } + + now_ms = kcp_hub_now_ms(); + if (kcp_hub_commit_registered_conn(hub, msg.from, conn, now_ms, &old_conn) != 0) { + protocol_message_clear(&msg); + return -1; + } + + if (old_conn != NULL && old_conn != conn) { + (void) kcp_hub_send_server_text(old_conn, msg.from, KCP_HUB_CTRL_PEER_REPLACED); + kcp_conn_close(old_conn); + } + protocol_message_clear(&msg); + return 0; +} + +static void *kcp_hub_session_thread_main(void *arg) { + kcp_session_thread_ctx_t *ctx = (kcp_session_thread_ctx_t *) arg; + kcp_hub_serve_session(ctx->hub, ctx->conn); + free(ctx); + return NULL; +} + +kcp_hub_t *kcp_hub_new(latency_logger_t *logger, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms) { + kcp_hub_t *hub = (kcp_hub_t *) calloc(1, sizeof(*hub)); + if (hub == NULL) { + return NULL; + } + pthread_rwlock_init(&hub->lock, NULL); + 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->heartbeat_interval_ms = KCP_HUB_DEFAULT_HEARTBEAT_INTERVAL_MS; + hub->lease_timeout_ms = KCP_HUB_DEFAULT_LEASE_TIMEOUT_MS; + hub->relay_fd = -1; + atomic_init(&hub->closed, 0); + if (pthread_create(&hub->telemetry_thread, NULL, kcp_hub_telemetry_thread_main, hub) != 0) { + pthread_rwlock_destroy(&hub->lock); + free(hub); + return NULL; + } + hub->telemetry_thread_started = 1; + return hub; +} + +int kcp_hub_serve_listener(kcp_hub_t *hub, kcp_listener_t *listener) { + if (hub == NULL || listener == NULL) { + errno = EINVAL; + return -1; + } + 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 (atomic_load(&hub->closed)) { + return 0; + } + return -1; + } + ctx = (kcp_session_thread_ctx_t *) calloc(1, sizeof(*ctx)); + if (ctx == NULL) { + kcp_conn_close(conn); + kcp_conn_free(conn); + return -1; + } + ctx->hub = hub; + ctx->conn = conn; + if (pthread_create(&thread, NULL, kcp_hub_session_thread_main, ctx) != 0) { + free(ctx); + kcp_conn_close(conn); + kcp_conn_free(conn); + return -1; + } + pthread_detach(thread); + } + return 0; +} + +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) { + errno = EINVAL; + return -1; + } + peer_id[0] = '\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_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; + } + + for (;;) { + message_t msg; + protocol_message_init(&msg); + if (kcp_conn_receive(conn, &msg) != 0) { + protocol_message_clear(&msg); + rc = -1; + break; + } + if (kcp_hub_handle_peer_message(hub, peer_id, conn, &msg) != 0) { + protocol_message_clear(&msg); + rc = -1; + break; + } + protocol_message_clear(&msg); + } + + kcp_hub_unregister(hub, peer_id, conn); + kcp_conn_close(conn); + kcp_conn_free(conn); + return rc; +} + +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) { + if (hub == NULL || relay_fd < 0) { + errno = EINVAL; + return -1; + } + pthread_rwlock_wrlock(&hub->lock); + hub->relay_fd = relay_fd; + hub->relay_configured = 1; + hub->relay_learn_peer = learn_peer; + hub->relay_peer_addr_len = 0; + if (peer_addr != NULL && peer_addr_len > 0) { + omni_clone_sockaddr(peer_addr, peer_addr_len, &hub->relay_peer_addr, &hub->relay_peer_addr_len); + } + pthread_rwlock_unlock(&hub->lock); + return 0; +} + +int kcp_hub_set_telemetry(kcp_hub_t *hub, const char *peer_id, int interval_ms) { + 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; + pthread_rwlock_unlock(&hub->lock); + return 0; +} + +static void kcp_hub_run_maintenance(kcp_hub_t *hub) { + kcp_hub_pending_action_t *heartbeat_actions = NULL; + kcp_hub_pending_action_t *close_actions = NULL; + uint32_t now_ms; + int heartbeat_interval_ms; + int lease_timeout_ms; + + if (hub == NULL) { + return; + } + + now_ms = kcp_hub_now_ms(); + heartbeat_interval_ms = KCP_HUB_DEFAULT_HEARTBEAT_INTERVAL_MS; + lease_timeout_ms = KCP_HUB_DEFAULT_LEASE_TIMEOUT_MS; + + pthread_rwlock_wrlock(&hub->lock); + if (hub->heartbeat_interval_ms > 0) { + heartbeat_interval_ms = hub->heartbeat_interval_ms; + } + if (hub->lease_timeout_ms > 0) { + lease_timeout_ms = hub->lease_timeout_ms; + } + { + kcp_peer_entry_t *prev = NULL; + kcp_peer_entry_t *entry = hub->peers; + + while (entry != NULL) { + kcp_peer_entry_t *next = entry->next; + uint32_t idle_ms = kcp_hub_elapsed_ms(now_ms, entry->last_seen_ms); + int uses_server_lease = kcp_hub_peer_uses_server_lease(entry->peer_id); + + if (entry->conn == NULL || entry->peer_id[0] == '\0') { + prev = entry; + entry = next; + continue; + } + if (uses_server_lease && lease_timeout_ms > 0 && idle_ms >= (uint32_t) lease_timeout_ms) { + if (prev == NULL) { + hub->peers = next; + } else { + prev->next = next; + } + (void) kcp_hub_append_pending_action(&close_actions, entry->peer_id, entry->conn); + free(entry); + entry = next; + continue; + } + if ( + uses_server_lease + && + heartbeat_interval_ms > 0 + && idle_ms >= (uint32_t) heartbeat_interval_ms + && (entry->last_heartbeat_sent_ms == 0 || kcp_hub_elapsed_ms(now_ms, entry->last_heartbeat_sent_ms) >= (uint32_t) heartbeat_interval_ms) + ) { + entry->last_heartbeat_sent_ms = now_ms; + (void) kcp_hub_append_pending_action(&heartbeat_actions, entry->peer_id, entry->conn); + } + prev = entry; + entry = next; + } + } + pthread_rwlock_unlock(&hub->lock); + + while (heartbeat_actions != NULL) { + kcp_hub_pending_action_t *next = heartbeat_actions->next; + if (kcp_hub_send_server_text(heartbeat_actions->conn, heartbeat_actions->peer_id, KCP_HUB_CTRL_HEARTBEAT) != 0) { + kcp_hub_unregister(hub, heartbeat_actions->peer_id, heartbeat_actions->conn); + kcp_conn_close(heartbeat_actions->conn); + } + free(heartbeat_actions); + heartbeat_actions = next; + } + + while (close_actions != NULL) { + kcp_hub_pending_action_t *next = close_actions->next; + kcp_conn_close(close_actions->conn); + free(close_actions); + close_actions = next; + } + + kcp_hub_free_pending_actions(heartbeat_actions); + kcp_hub_free_pending_actions(close_actions); +} + +int kcp_hub_serve_relay(kcp_hub_t *hub) { + uint8_t buffer[KCP_RELAY_MAX_DATAGRAM_SIZE]; + + if (hub == NULL) { + errno = EINVAL; + return -1; + } + while (!atomic_load(&hub->closed)) { + struct sockaddr_storage source; + socklen_t source_len = sizeof(source); + ssize_t n; + message_t msg; + char err[128]; + int relay_fd; + + pthread_rwlock_rdlock(&hub->lock); + relay_fd = hub->relay_fd; + pthread_rwlock_unlock(&hub->lock); + if (relay_fd < 0) { + errno = ENOTCONN; + return -1; + } + + n = recvfrom(relay_fd, buffer, sizeof(buffer), 0, (struct sockaddr *) &source, &source_len); + if (n < 0) { + if (atomic_load(&hub->closed)) { + return 0; + } + if (errno == EINTR) { + continue; + } + return -1; + } + if (!kcp_hub_accept_relay_peer(hub, (struct sockaddr *) &source, source_len)) { + continue; + } + + protocol_message_init(&msg); + if (protocol_decode_message_datagram(buffer, (size_t) n, &msg, err, sizeof(err)) != 0) { + protocol_message_clear(&msg); + continue; + } + if (msg.type != MSG_TYPE_TEXT && msg.type != MSG_TYPE_FILE && msg.type != MSG_TYPE_BINARY && msg.type != MSG_TYPE_ERROR) { + protocol_message_clear(&msg); + continue; + } + (void) kcp_hub_deliver_relayed_message(hub, &msg); + protocol_message_clear(&msg); + } + return 0; +} + +int kcp_hub_close(kcp_hub_t *hub) { + if (hub == NULL) { + return 0; + } + if (!atomic_exchange(&hub->closed, 1)) { + if (hub->relay_fd >= 0) { + close(hub->relay_fd); + hub->relay_fd = -1; + } + } + return 0; +} + +void kcp_hub_free(kcp_hub_t *hub) { + kcp_peer_entry_t *entry; + kcp_peer_entry_t *next; + + if (hub == NULL) { + 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) { + kcp_conn_close(entry->conn); + } + free(entry); + } + pthread_rwlock_destroy(&hub->lock); + free(hub); +} diff --git a/robot/v4l2/OmniSocketGo_robot/src/server_udp_hub.c b/robot/v4l2/OmniSocketGo_robot/src/server_udp_hub.c new file mode 100644 index 0000000..faf67ed --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/src/server_udp_hub.c @@ -0,0 +1,181 @@ +#include "server_udp_hub.h" + +#include + +typedef struct udp_peer_entry { + struct udp_peer_entry *next; + char peer_id[OMNI_MAX_PEER_ID]; + struct sockaddr_storage addr; + socklen_t addr_len; +} udp_peer_entry_t; + +struct udp_hub { + udp_conn_t *conn; + pthread_rwlock_t lock; + udp_peer_entry_t *peers; +}; + +static int udp_addr_equal(const struct sockaddr_storage *a, socklen_t a_len, const struct sockaddr_storage *b, socklen_t b_len) { + return a_len == b_len && memcmp(a, b, a_len) == 0; +} + +static udp_peer_entry_t *udp_hub_find_by_id(udp_hub_t *hub, const char *peer_id) { + udp_peer_entry_t *entry; + for (entry = hub->peers; entry != NULL; entry = entry->next) { + if (strcmp(entry->peer_id, peer_id) == 0) { + return entry; + } + } + return NULL; +} + +static udp_peer_entry_t *udp_hub_find_by_addr(udp_hub_t *hub, const struct sockaddr_storage *addr, socklen_t addr_len) { + udp_peer_entry_t *entry; + for (entry = hub->peers; entry != NULL; entry = entry->next) { + if (udp_addr_equal(&entry->addr, entry->addr_len, addr, addr_len)) { + return entry; + } + } + return NULL; +} + +static int udp_hub_send_error(udp_hub_t *hub, const struct sockaddr_storage *addr, socklen_t addr_len, const char *to, const char *message) { + message_t msg; + protocol_message_init(&msg); + msg.type = MSG_TYPE_ERROR; + msg.id = 0; + snprintf(msg.from, sizeof(msg.from), "%s", SERVER_PEER_ID); + snprintf(msg.to, sizeof(msg.to), "%s", to == NULL || to[0] == '\0' ? "unknown" : to); + msg.body = (uint8_t *) omni_strdup(message); + msg.body_len = msg.body == NULL ? 0 : strlen((const char *) msg.body); + if (msg.body == NULL) { + return -1; + } + if (udp_conn_send_to(hub->conn, &msg, (const struct sockaddr *) addr, addr_len) != 0) { + protocol_message_clear(&msg); + return -1; + } + protocol_message_clear(&msg); + return 0; +} + +udp_hub_t *udp_hub_open(const char *listen_addr, latency_logger_t *logger, tx_timestamp_debug_logger_t *debug_logger, int enable_timestamping) { + udp_hub_t *hub = (udp_hub_t *) calloc(1, sizeof(*hub)); + if (hub == NULL) { + return NULL; + } + hub->conn = udp_conn_bind(listen_addr, NULL, enable_timestamping, logger, OMNI_NODE_ROLE_SERVER, "hub", debug_logger); + if (hub->conn == NULL) { + free(hub); + return NULL; + } + pthread_rwlock_init(&hub->lock, NULL); + return hub; +} + +int udp_hub_serve(udp_hub_t *hub) { + message_t msg; + struct sockaddr_storage addr; + socklen_t addr_len; + udp_peer_entry_t *sender; + udp_peer_entry_t *target; + udp_peer_entry_t *entry; + + if (hub == NULL) { + errno = EINVAL; + return -1; + } + + protocol_message_init(&msg); + for (;;) { + protocol_message_clear(&msg); + if (udp_conn_receive(hub->conn, &msg, &addr, &addr_len) != 0) { + return -1; + } + + if (msg.type == MSG_TYPE_REGISTER) { + pthread_rwlock_wrlock(&hub->lock); + entry = udp_hub_find_by_id(hub, msg.from); + if (entry == NULL) { + entry = (udp_peer_entry_t *) calloc(1, sizeof(*entry)); + if (entry == NULL) { + pthread_rwlock_unlock(&hub->lock); + protocol_message_clear(&msg); + return -1; + } + snprintf(entry->peer_id, sizeof(entry->peer_id), "%s", msg.from); + entry->next = hub->peers; + hub->peers = entry; + } + memcpy(&entry->addr, &addr, sizeof(addr)); + entry->addr_len = addr_len; + pthread_rwlock_unlock(&hub->lock); + continue; + } + if (msg.type != MSG_TYPE_TEXT && msg.type != MSG_TYPE_FILE && msg.type != MSG_TYPE_BINARY) { + if (msg.type == MSG_TYPE_ERROR) { + udp_hub_send_error(hub, &addr, addr_len, msg.from, "peers cannot send error messages"); + } else { + char *error_text = omni_strdup_printf("unsupported message type: %s", protocol_message_type_name(msg.type)); + if (error_text != NULL) { + udp_hub_send_error(hub, &addr, addr_len, msg.from, error_text); + free(error_text); + } + } + continue; + } + + pthread_rwlock_rdlock(&hub->lock); + sender = udp_hub_find_by_addr(hub, &addr, addr_len); + if (sender == NULL) { + pthread_rwlock_unlock(&hub->lock); + udp_hub_send_error(hub, &addr, addr_len, msg.from, "not registered; send register first"); + continue; + } + snprintf(msg.from, sizeof(msg.from), "%s", sender->peer_id); + target = udp_hub_find_by_id(hub, msg.to); + if (target == NULL) { + char *error_text; + pthread_rwlock_unlock(&hub->lock); + error_text = omni_strdup_printf("unknown target: %s", msg.to); + if (error_text != NULL) { + udp_hub_send_error(hub, &addr, addr_len, sender->peer_id, error_text); + free(error_text); + } + continue; + } + if (udp_conn_send_to(hub->conn, &msg, (const struct sockaddr *) &target->addr, target->addr_len) != 0) { + char *error_text; + pthread_rwlock_unlock(&hub->lock); + error_text = omni_strdup_printf("failed to forward to %s", msg.to); + if (error_text != NULL) { + udp_hub_send_error(hub, &addr, addr_len, sender->peer_id, error_text); + free(error_text); + } + continue; + } + pthread_rwlock_unlock(&hub->lock); + } +} + +int udp_hub_close(udp_hub_t *hub) { + if (hub == NULL) { + return 0; + } + return udp_conn_close(hub->conn); +} + +void udp_hub_free(udp_hub_t *hub) { + udp_peer_entry_t *entry; + udp_peer_entry_t *next; + if (hub == NULL) { + return; + } + udp_conn_free(hub->conn); + for (entry = hub->peers; entry != NULL; entry = next) { + next = entry->next; + free(entry); + } + pthread_rwlock_destroy(&hub->lock); + free(hub); +} diff --git a/robot/v4l2/OmniSocketGo_robot/src/server_udp_relay.c b/robot/v4l2/OmniSocketGo_robot/src/server_udp_relay.c new file mode 100644 index 0000000..c562df1 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/src/server_udp_relay.c @@ -0,0 +1,613 @@ +#include "server_udp_relay.h" + +#include +#include +#include +#include +#include + +#define UDP_RELAY_BUF_SIZE (64U * 1024U) +#define UDP_RELAY_ROUTE_TIMEOUT_MS 30000U +#define UDP_RELAY_DEFAULT_PACKET_LOG_SAMPLE_EVERY 200U + +struct udp_relay { + int downstream_fd; + int upstream_fd; + struct sockaddr_storage upstream_addr; + socklen_t upstream_addr_len; + char downstream_local_addr[OMNI_MAX_ADDR_TEXT]; + char upstream_local_addr[OMNI_MAX_ADDR_TEXT]; + struct sockaddr_storage client_addr; + socklen_t client_addr_len; + int has_client; + uint32_t client_last_seen_ms; + struct udp_relay_route *routes; + pthread_mutex_t lock; + pthread_mutex_t log_mu; + unsigned int packet_log_sample_every; + atomic_ullong packet_log_counter; + pthread_mutex_t state_mu; + pthread_cond_t state_cond; + pthread_t downstream_thread; + int downstream_thread_started; + pthread_t upstream_thread; + int upstream_thread_started; + int worker_done; + int worker_rc; + int worker_errno; + int closed; +}; + +typedef struct udp_relay_route { + struct udp_relay_route *next; + uint32_t conv; + struct sockaddr_storage client_addr; + socklen_t client_addr_len; + uint32_t last_seen_ms; +} udp_relay_route_t; + +static uint32_t udp_relay_now_ms(void) { + return omni_now_millis32(); +} + +static uint32_t udp_relay_elapsed_ms(uint32_t now_ms, uint32_t then_ms) { + return now_ms - then_ms; +} + +static unsigned int udp_relay_packet_log_sample_every(void) { + const char *raw = getenv("OMNI_RELAY_PACKET_LOG_SAMPLE_EVERY"); + unsigned long parsed; + char *endptr = NULL; + + if (raw == NULL || raw[0] == '\0') { + return UDP_RELAY_DEFAULT_PACKET_LOG_SAMPLE_EVERY; + } + parsed = strtoul(raw, &endptr, 10); + if (endptr == raw || *endptr != '\0') { + return UDP_RELAY_DEFAULT_PACKET_LOG_SAMPLE_EVERY; + } + return (unsigned int) parsed; +} + +static int udp_relay_event_should_always_log(const char *event_name) { + return event_name != NULL && strstr(event_name, "_drop_") != NULL; +} + +static int udp_relay_should_log_packet(udp_relay_t *relay, const char *event_name) { + unsigned long long seq; + + if (relay == NULL) { + return 0; + } + if (udp_relay_event_should_always_log(event_name)) { + return 1; + } + if (relay->packet_log_sample_every == 0U) { + return 0; + } + if (relay->packet_log_sample_every == 1U) { + return 1; + } + seq = atomic_fetch_add_explicit(&relay->packet_log_counter, 1U, memory_order_relaxed) + 1U; + return (seq % (unsigned long long) relay->packet_log_sample_every) == 0U; +} + +static void udp_relay_parse_kcp_summary(const uint8_t *packet, size_t len, int *has_conv, uint32_t *conv, size_t *segment_count) { + size_t offset = 0; + size_t count = 0; + + if (has_conv != NULL) { + *has_conv = 0; + } + if (conv != NULL) { + *conv = 0; + } + if (segment_count != NULL) { + *segment_count = 0; + } + if (packet == NULL || len < 4U) { + return; + } + if (has_conv != NULL) { + *has_conv = 1; + } + if (conv != NULL) { + *conv = (uint32_t) ((unsigned char) packet[0] | + ((unsigned char) packet[1] << 8) | + ((unsigned char) packet[2] << 16) | + ((unsigned char) packet[3] << 24)); + } + while (offset + 24U <= len) { + uint32_t seg_len = (uint32_t) ((unsigned char) packet[offset + 20] | + ((unsigned char) packet[offset + 21] << 8) | + ((unsigned char) packet[offset + 22] << 16) | + ((unsigned char) packet[offset + 23] << 24)); + if (offset + 24U + seg_len > len) { + return; + } + count++; + offset += 24U + seg_len; + } + if (segment_count != NULL) { + *segment_count = count; + } +} + +static void udp_relay_print_packet(udp_relay_t *relay, const char *event_name, const char *local_addr, const struct sockaddr_storage *remote_addr, socklen_t remote_addr_len, const uint8_t *packet, size_t packet_len) { + char remote_addr_text[OMNI_MAX_ADDR_TEXT]; + int64_t ts_unix_nano; + int has_conv = 0; + uint32_t conv = 0; + size_t segment_count = 0; + + if (relay == NULL) { + return; + } + if (!udp_relay_should_log_packet(relay, event_name)) { + return; + } + + if (remote_addr != NULL && remote_addr_len > 0) { + omni_sockaddr_to_string((const struct sockaddr *) remote_addr, remote_addr_len, remote_addr_text, sizeof(remote_addr_text)); + } else { + remote_addr_text[0] = '\0'; + } + ts_unix_nano = omni_now_unix_nano(); + udp_relay_parse_kcp_summary(packet, packet_len, &has_conv, &conv, &segment_count); + + pthread_mutex_lock(&relay->log_mu); + if (has_conv) { + fprintf(stderr, "[relay] ts=%" PRId64 " event=%s local=%s remote=%s bytes=%zu conv=%" PRIu32 " segs=%zu\n", + ts_unix_nano, + event_name == NULL ? "" : event_name, + local_addr == NULL ? "" : local_addr, + remote_addr_text, + packet_len, + conv, + segment_count); + } else { + fprintf(stderr, "[relay] ts=%" PRId64 " event=%s local=%s remote=%s bytes=%zu\n", + ts_unix_nano, + event_name == NULL ? "" : event_name, + local_addr == NULL ? "" : local_addr, + remote_addr_text, + packet_len); + } + fflush(stderr); + pthread_mutex_unlock(&relay->log_mu); +} + +static int udp_relay_is_closed(udp_relay_t *relay) { + int closed; + + pthread_mutex_lock(&relay->state_mu); + closed = relay->closed; + pthread_mutex_unlock(&relay->state_mu); + return closed; +} + +static void udp_relay_note_result(udp_relay_t *relay, int rc, int errnum) { + pthread_mutex_lock(&relay->state_mu); + if (!relay->worker_done) { + relay->worker_done = 1; + relay->worker_rc = rc; + relay->worker_errno = errnum; + pthread_cond_signal(&relay->state_cond); + } + pthread_mutex_unlock(&relay->state_mu); +} + +static void udp_relay_record_client(udp_relay_t *relay, const struct sockaddr_storage *addr, socklen_t addr_len) { + pthread_mutex_lock(&relay->lock); + memcpy(&relay->client_addr, addr, sizeof(*addr)); + relay->client_addr_len = addr_len; + relay->has_client = 1; + relay->client_last_seen_ms = udp_relay_now_ms(); + pthread_mutex_unlock(&relay->lock); +} + +static void udp_relay_prune_routes_locked(udp_relay_t *relay, uint32_t now_ms) { + udp_relay_route_t *prev = NULL; + udp_relay_route_t *route; + + if (relay == NULL) { + return; + } + + route = relay->routes; + while (route != NULL) { + udp_relay_route_t *next = route->next; + + if (udp_relay_elapsed_ms(now_ms, route->last_seen_ms) >= UDP_RELAY_ROUTE_TIMEOUT_MS) { + if (prev == NULL) { + relay->routes = next; + } else { + prev->next = next; + } + free(route); + route = next; + continue; + } + + prev = route; + route = next; + } + + if (relay->has_client && udp_relay_elapsed_ms(now_ms, relay->client_last_seen_ms) >= UDP_RELAY_ROUTE_TIMEOUT_MS) { + relay->has_client = 0; + relay->client_addr_len = 0; + memset(&relay->client_addr, 0, sizeof(relay->client_addr)); + } +} + +static int udp_relay_record_route(udp_relay_t *relay, uint32_t conv, const struct sockaddr_storage *addr, socklen_t addr_len) { + udp_relay_route_t *route; + uint32_t now_ms; + + if (relay == NULL || addr == NULL || addr_len == 0) { + errno = EINVAL; + return -1; + } + + now_ms = udp_relay_now_ms(); + pthread_mutex_lock(&relay->lock); + udp_relay_prune_routes_locked(relay, now_ms); + for (route = relay->routes; route != NULL; route = route->next) { + if (route->conv == conv) { + memcpy(&route->client_addr, addr, sizeof(*addr)); + route->client_addr_len = addr_len; + route->last_seen_ms = now_ms; + pthread_mutex_unlock(&relay->lock); + return 0; + } + } + + route = (udp_relay_route_t *) calloc(1, sizeof(*route)); + if (route == NULL) { + pthread_mutex_unlock(&relay->lock); + return -1; + } + route->conv = conv; + memcpy(&route->client_addr, addr, sizeof(*addr)); + route->client_addr_len = addr_len; + route->last_seen_ms = now_ms; + route->next = relay->routes; + relay->routes = route; + pthread_mutex_unlock(&relay->lock); + return 0; +} + +static int udp_relay_copy_client(udp_relay_t *relay, struct sockaddr_storage *addr, socklen_t *addr_len) { + int has_client; + uint32_t now_ms; + + now_ms = udp_relay_now_ms(); + pthread_mutex_lock(&relay->lock); + udp_relay_prune_routes_locked(relay, now_ms); + has_client = relay->has_client; + if (has_client) { + memcpy(addr, &relay->client_addr, sizeof(*addr)); + *addr_len = relay->client_addr_len; + } + pthread_mutex_unlock(&relay->lock); + return has_client; +} + +static int udp_relay_copy_route(udp_relay_t *relay, uint32_t conv, struct sockaddr_storage *addr, socklen_t *addr_len) { + udp_relay_route_t *route; + uint32_t now_ms; + + now_ms = udp_relay_now_ms(); + pthread_mutex_lock(&relay->lock); + udp_relay_prune_routes_locked(relay, now_ms); + for (route = relay->routes; route != NULL; route = route->next) { + if (route->conv == conv) { + memcpy(addr, &route->client_addr, sizeof(*addr)); + *addr_len = route->client_addr_len; + pthread_mutex_unlock(&relay->lock); + return 1; + } + } + pthread_mutex_unlock(&relay->lock); + return 0; +} + +static void udp_relay_clear_routes(udp_relay_t *relay) { + udp_relay_route_t *route; + udp_relay_route_t *next; + + if (relay == NULL) { + return; + } + + pthread_mutex_lock(&relay->lock); + route = relay->routes; + relay->routes = NULL; + pthread_mutex_unlock(&relay->lock); + + while (route != NULL) { + next = route->next; + free(route); + route = next; + } +} + +static void *udp_relay_forward_downstream_to_upstream(void *arg) { + udp_relay_t *relay = (udp_relay_t *) arg; + uint8_t buffer[UDP_RELAY_BUF_SIZE]; + + for (;;) { + struct sockaddr_storage source; + socklen_t source_len = sizeof(source); + ssize_t n = recvfrom(relay->downstream_fd, buffer, sizeof(buffer), 0, (struct sockaddr *) &source, &source_len); + int has_conv = 0; + uint32_t conv = 0; + + if (n < 0) { + int errnum = errno; + if (errnum == EINTR) { + continue; + } + if (udp_relay_is_closed(relay)) { + udp_relay_note_result(relay, 0, 0); + } else { + udp_relay_note_result(relay, -1, errnum); + } + return NULL; + } + + udp_relay_record_client(relay, &source, source_len); + udp_relay_parse_kcp_summary(buffer, (size_t) n, &has_conv, &conv, NULL); + if (has_conv) { + (void) udp_relay_record_route(relay, conv, &source, source_len); + } + udp_relay_print_packet(relay, "relay_downstream_rx", relay->downstream_local_addr, &source, source_len, buffer, (size_t) n); + for (;;) { + if (send(relay->upstream_fd, buffer, (size_t) n, 0) >= 0) { + udp_relay_print_packet(relay, "relay_upstream_tx", relay->upstream_local_addr, &relay->upstream_addr, relay->upstream_addr_len, buffer, (size_t) n); + break; + } + { + int errnum = errno; + if (errnum == EINTR) { + continue; + } + if (udp_relay_is_closed(relay)) { + udp_relay_note_result(relay, 0, 0); + } else { + udp_relay_note_result(relay, -1, errnum); + } + return NULL; + } + } + } +} + +static void *udp_relay_forward_upstream_to_downstream(void *arg) { + udp_relay_t *relay = (udp_relay_t *) arg; + uint8_t buffer[UDP_RELAY_BUF_SIZE]; + + for (;;) { + struct sockaddr_storage client_addr; + socklen_t client_addr_len = 0; + ssize_t n = recv(relay->upstream_fd, buffer, sizeof(buffer), 0); + int has_conv = 0; + uint32_t conv = 0; + + if (n < 0) { + int errnum = errno; + if (errnum == EINTR) { + continue; + } + if (udp_relay_is_closed(relay)) { + udp_relay_note_result(relay, 0, 0); + } else { + udp_relay_note_result(relay, -1, errnum); + } + return NULL; + } + + udp_relay_print_packet(relay, "relay_upstream_rx", relay->upstream_local_addr, &relay->upstream_addr, relay->upstream_addr_len, buffer, (size_t) n); + udp_relay_parse_kcp_summary(buffer, (size_t) n, &has_conv, &conv, NULL); + if (has_conv && !udp_relay_copy_route(relay, conv, &client_addr, &client_addr_len)) { + udp_relay_print_packet(relay, "relay_upstream_drop_unknown_conv", relay->upstream_local_addr, &relay->upstream_addr, relay->upstream_addr_len, buffer, (size_t) n); + continue; + } + if (!has_conv && !udp_relay_copy_client(relay, &client_addr, &client_addr_len)) { + udp_relay_print_packet(relay, "relay_upstream_drop_no_client", relay->upstream_local_addr, &relay->upstream_addr, relay->upstream_addr_len, buffer, (size_t) n); + continue; + } + + for (;;) { + if (sendto(relay->downstream_fd, buffer, (size_t) n, 0, (struct sockaddr *) &client_addr, client_addr_len) >= 0) { + udp_relay_print_packet(relay, "relay_downstream_tx", relay->downstream_local_addr, &client_addr, client_addr_len, buffer, (size_t) n); + break; + } + { + int errnum = errno; + if (errnum == EINTR) { + continue; + } + if (udp_relay_is_closed(relay)) { + udp_relay_note_result(relay, 0, 0); + } else { + udp_relay_note_result(relay, -1, errnum); + } + return NULL; + } + } + } +} + +static void udp_relay_join_threads(udp_relay_t *relay) { + if (relay->downstream_thread_started) { + pthread_join(relay->downstream_thread, NULL); + relay->downstream_thread_started = 0; + } + if (relay->upstream_thread_started) { + pthread_join(relay->upstream_thread, NULL); + relay->upstream_thread_started = 0; + } +} + +udp_relay_t *udp_relay_open(const char *listen_addr, const char *upstream_addr) { + struct sockaddr_storage listen_ss; + struct sockaddr_storage upstream_ss; + struct sockaddr_storage downstream_local_ss; + struct sockaddr_storage upstream_local_ss; + socklen_t listen_len; + socklen_t upstream_len; + socklen_t downstream_local_len = sizeof(downstream_local_ss); + socklen_t upstream_local_len = sizeof(upstream_local_ss); + int family; + int fd_listen = -1; + int fd_upstream = -1; + udp_relay_t *relay = NULL; + + if (omni_parse_sockaddr(listen_addr, 1, &listen_ss, &listen_len, &family) != 0 || + omni_parse_sockaddr(upstream_addr, 0, &upstream_ss, &upstream_len, &family) != 0) { + return NULL; + } + fd_listen = socket(listen_ss.ss_family, SOCK_DGRAM, 0); + if (fd_listen < 0) { + return NULL; + } + if (bind(fd_listen, (struct sockaddr *) &listen_ss, listen_len) != 0) { + close(fd_listen); + return NULL; + } + fd_upstream = socket(upstream_ss.ss_family, SOCK_DGRAM, 0); + if (fd_upstream < 0) { + close(fd_listen); + return NULL; + } + if (connect(fd_upstream, (struct sockaddr *) &upstream_ss, upstream_len) != 0) { + close(fd_upstream); + close(fd_listen); + return NULL; + } + relay = (udp_relay_t *) calloc(1, sizeof(*relay)); + if (relay == NULL) { + close(fd_upstream); + close(fd_listen); + return NULL; + } + relay->downstream_fd = fd_listen; + relay->upstream_fd = fd_upstream; + memcpy(&relay->upstream_addr, &upstream_ss, sizeof(upstream_ss)); + relay->upstream_addr_len = upstream_len; + if (getsockname(fd_listen, (struct sockaddr *) &downstream_local_ss, &downstream_local_len) == 0) { + omni_sockaddr_to_string((const struct sockaddr *) &downstream_local_ss, downstream_local_len, relay->downstream_local_addr, sizeof(relay->downstream_local_addr)); + } else { + snprintf(relay->downstream_local_addr, sizeof(relay->downstream_local_addr), "%s", listen_addr == NULL ? "" : listen_addr); + } + if (getsockname(fd_upstream, (struct sockaddr *) &upstream_local_ss, &upstream_local_len) == 0) { + omni_sockaddr_to_string((const struct sockaddr *) &upstream_local_ss, upstream_local_len, relay->upstream_local_addr, sizeof(relay->upstream_local_addr)); + } else { + snprintf(relay->upstream_local_addr, sizeof(relay->upstream_local_addr), "%s", listen_addr == NULL ? "" : listen_addr); + } + pthread_mutex_init(&relay->lock, NULL); + pthread_mutex_init(&relay->log_mu, NULL); + relay->packet_log_sample_every = udp_relay_packet_log_sample_every(); + atomic_init(&relay->packet_log_counter, 0U); + pthread_mutex_init(&relay->state_mu, NULL); + pthread_cond_init(&relay->state_cond, NULL); + return relay; +} + +int udp_relay_serve(udp_relay_t *relay) { + int thread_rc; + int rc; + int errnum; + + if (relay == NULL) { + errno = EINVAL; + return -1; + } + if (udp_relay_is_closed(relay)) { + errno = ECANCELED; + return -1; + } + + pthread_mutex_lock(&relay->state_mu); + relay->worker_done = 0; + relay->worker_rc = 0; + relay->worker_errno = 0; + pthread_mutex_unlock(&relay->state_mu); + + thread_rc = pthread_create(&relay->downstream_thread, NULL, udp_relay_forward_downstream_to_upstream, relay); + if (thread_rc != 0) { + errno = thread_rc; + return -1; + } + relay->downstream_thread_started = 1; + + thread_rc = pthread_create(&relay->upstream_thread, NULL, udp_relay_forward_upstream_to_downstream, relay); + if (thread_rc != 0) { + errno = thread_rc; + udp_relay_close(relay); + udp_relay_join_threads(relay); + return -1; + } + relay->upstream_thread_started = 1; + + pthread_mutex_lock(&relay->state_mu); + while (!relay->worker_done) { + pthread_cond_wait(&relay->state_cond, &relay->state_mu); + } + rc = relay->worker_rc; + errnum = relay->worker_errno; + pthread_mutex_unlock(&relay->state_mu); + + udp_relay_close(relay); + udp_relay_join_threads(relay); + + if (rc != 0 && errnum != 0) { + errno = errnum; + } + return rc; +} + +int udp_relay_close(udp_relay_t *relay) { + int downstream_fd; + int upstream_fd; + + if (relay == NULL) { + return 0; + } + + pthread_mutex_lock(&relay->state_mu); + if (relay->closed) { + pthread_mutex_unlock(&relay->state_mu); + return 0; + } + relay->closed = 1; + downstream_fd = relay->downstream_fd; + upstream_fd = relay->upstream_fd; + relay->downstream_fd = -1; + relay->upstream_fd = -1; + pthread_cond_broadcast(&relay->state_cond); + pthread_mutex_unlock(&relay->state_mu); + + if (downstream_fd >= 0) { + close(downstream_fd); + } + if (upstream_fd >= 0) { + close(upstream_fd); + } + return 0; +} + +void udp_relay_free(udp_relay_t *relay) { + if (relay == NULL) { + return; + } + udp_relay_close(relay); + udp_relay_join_threads(relay); + udp_relay_clear_routes(relay); + pthread_mutex_destroy(&relay->lock); + pthread_mutex_destroy(&relay->log_mu); + pthread_cond_destroy(&relay->state_cond); + pthread_mutex_destroy(&relay->state_mu); + free(relay); +} diff --git a/robot/v4l2/OmniSocketGo_robot/src/transport_kcp.c b/robot/v4l2/OmniSocketGo_robot/src/transport_kcp.c new file mode 100644 index 0000000..fbb3a51 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/src/transport_kcp.c @@ -0,0 +1,2061 @@ +#include "transport_kcp.h" + +#include "ikcp.h" +#include "linux_timestamping.h" + +#include +#include +#include +#include + +#define KCP_RECV_CHUNK_SIZE (32U * 1024U) + +typedef struct kcp_packet_debug_pending { + struct kcp_packet_debug_pending *next; + uint32_t tx_id; + struct sockaddr_storage remote_addr; + socklen_t remote_addr_len; + int packet_bytes; + int has_conv; + uint32_t conv; + kcp_packet_debug_segment_t *segments; + size_t segment_count; + int saw_sched; + int saw_software; +} kcp_packet_debug_pending_t; + +typedef struct kcp_socket_debug_state { + int fd; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + kcp_packet_debug_logger_t *logger; + pthread_mutex_t write_mu; + pthread_mutex_t pending_mu; + pthread_t errqueue_thread; + int errqueue_thread_started; + uint32_t next_tx_id; + kcp_packet_debug_pending_t *pending_head; + atomic_int closed; + atomic_int last_send_errno; +} kcp_socket_debug_state_t; + +typedef struct kcp_session_entry kcp_session_entry_t; +typedef struct kcp_process_sampler kcp_process_sampler_t; + +struct kcp_conn { + ikcpcb *kcp; + int fd; + int is_client; + int owns_socket; + int socket_closed; + atomic_int closed; + struct sockaddr_storage remote_addr; + socklen_t remote_addr_len; + pthread_mutex_t kcp_mu; + pthread_mutex_t close_mu; + pthread_cond_t rx_cond; + pthread_t recv_thread; + int recv_thread_started; + pthread_t update_thread; + int update_thread_started; + pthread_t stats_thread; + 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; + uint64_t pending_out_pkts; + uint64_t pending_in_segs; + uint64_t pending_out_segs; + uint64_t pending_in_errs; + uint64_t pending_kcp_in_errs; + protocol_frame_decoder_t decoder; + int32_t min_srtt_ms; + uint32_t last_feedback_ms; + uint8_t scratch[KCP_RECV_CHUNK_SIZE]; + latency_logger_t *logger; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + kcp_session_stats_logger_t *stats_logger; + int stats_interval_ms; + kcp_process_sampler_t *process_sampler; + kcp_socket_debug_state_t *sock_state; + struct kcp_listener *listener; + struct kcp_conn *accept_next; + struct kcp_conn *process_next; +}; + +struct kcp_listener { + int fd; + int closed; + pthread_mutex_t lock; + pthread_mutex_t accept_mu; + pthread_cond_t accept_cond; + pthread_t recv_thread; + int recv_thread_started; + kcp_session_entry_t *sessions; + kcp_conn_t *accept_head; + kcp_conn_t *accept_tail; + kcp_socket_debug_state_t sock_state; +}; + +struct kcp_session_entry { + uint32_t conv; + kcp_conn_t *conn; + kcp_session_entry_t *next; +}; + +struct kcp_process_sampler { + kcp_process_sampler_t *next; + kcp_session_stats_logger_t *logger; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + int stats_interval_ms; + pthread_mutex_t lock; + pthread_cond_t cond; + pthread_t thread; + int thread_started; + int stopped; + int refcount; + int request_pending; + uint64_t pending_request_id; + uint64_t completed_request_id; + char pending_reason[32]; + kcp_conn_t *members; + uint64_t prev_bytes_sent; + uint64_t prev_bytes_received; + uint64_t prev_in_pkts; + uint64_t prev_out_pkts; + uint64_t prev_in_segs; + 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; + atomic_uint_fast64_t out_pkts; + atomic_uint_fast64_t in_segs; + atomic_uint_fast64_t out_segs; + atomic_uint_fast64_t in_errs; + atomic_uint_fast64_t kcp_in_errs; + atomic_uint_fast64_t curr_estab; +}; + +static pthread_mutex_t g_kcp_process_sampler_mu = PTHREAD_MUTEX_INITIALIZER; +static kcp_process_sampler_t *g_kcp_process_samplers = NULL; + +void kcp_conn_options_init(kcp_conn_options_t *options) { + if (options == NULL) { + return; + } + memset(options, 0, sizeof(*options)); + options->nodelay = KCP_DEFAULT_NODELAY; + options->interval_ms = KCP_DEFAULT_INTERVAL_MS; + options->resend = KCP_DEFAULT_RESEND; + options->nc = KCP_DEFAULT_NC; + options->sndwnd = KCP_DEFAULT_SND_WND; + options->rcvwnd = KCP_DEFAULT_RCV_WND; + options->mtu = KCP_DEFAULT_MTU; +} + +void kcp_conn_options_set_control_defaults(kcp_conn_options_t *options) { + if (options == NULL) { + return; + } + memset(options, 0, sizeof(*options)); + options->nodelay = KCP_CONTROL_NODELAY; + options->interval_ms = KCP_CONTROL_INTERVAL_MS; + options->resend = KCP_CONTROL_RESEND; + options->nc = KCP_CONTROL_NC; + options->sndwnd = KCP_CONTROL_SND_WND; + options->rcvwnd = KCP_CONTROL_RCV_WND; + options->mtu = KCP_CONTROL_MTU; +} + +void kcp_conn_options_set_video_defaults(kcp_conn_options_t *options) { + if (options == NULL) { + return; + } + memset(options, 0, sizeof(*options)); + options->nodelay = KCP_VIDEO_NODELAY; + options->interval_ms = KCP_VIDEO_INTERVAL_MS; + options->resend = KCP_VIDEO_RESEND; + options->nc = KCP_VIDEO_NC; + options->sndwnd = KCP_VIDEO_SND_WND; + options->rcvwnd = KCP_VIDEO_RCV_WND; + 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; + return -1; + } + if (options->interval_ms <= 0 || options->sndwnd <= 0 || options->rcvwnd <= 0 || options->mtu <= 0) { + errno = EINVAL; + return -1; + } + return 0; +} + +static int kcp_conn_apply_options_locked(kcp_conn_t *conn, const kcp_conn_options_t *options) { + if (conn == NULL || conn->kcp == NULL || kcp_conn_validate_options(options) != 0) { + return -1; + } + if (ikcp_wndsize(conn->kcp, options->sndwnd, options->rcvwnd) != 0) { + errno = EINVAL; + return -1; + } + if (ikcp_setmtu(conn->kcp, options->mtu) != 0) { + errno = EINVAL; + return -1; + } + if (ikcp_nodelay(conn->kcp, options->nodelay, options->interval_ms, options->resend, options->nc) != 0) { + errno = EINVAL; + return -1; + } + conn->kcp->stream = 1; + conn->options = *options; + conn->update_interval_ms = options->interval_ms; + return 0; +} + +static void kcp_parse_packet_segments(const uint8_t *packet, size_t len, uint32_t *conv, kcp_packet_debug_segment_t **segments, size_t *segment_count) { + size_t offset = 0; + size_t count = 0; + kcp_packet_debug_segment_t *items = NULL; + + if (conv != NULL) { + *conv = 0; + } + if (segments != NULL) { + *segments = NULL; + } + if (segment_count != NULL) { + *segment_count = 0; + } + if (len < 4) { + return; + } + if (conv != NULL) { + *conv = (uint32_t) ((unsigned char) packet[0] | + ((unsigned char) packet[1] << 8) | + ((unsigned char) packet[2] << 16) | + ((unsigned char) packet[3] << 24)); + } + while (offset + 24U <= len) { + uint32_t seg_len = (uint32_t) ((unsigned char) packet[offset + 20] | + ((unsigned char) packet[offset + 21] << 8) | + ((unsigned char) packet[offset + 22] << 16) | + ((unsigned char) packet[offset + 23] << 24)); + if (offset + 24U + seg_len > len) { + free(items); + return; + } + if (segments != NULL) { + kcp_packet_debug_segment_t *next = (kcp_packet_debug_segment_t *) realloc(items, (count + 1U) * sizeof(*items)); + if (next == NULL) { + free(items); + return; + } + items = next; + items[count].cmd = packet[offset + 4]; + items[count].frg = packet[offset + 5]; + items[count].wnd = (uint16_t) ((unsigned char) packet[offset + 6] | ((unsigned char) packet[offset + 7] << 8)); + items[count].sn = (uint32_t) ((unsigned char) packet[offset + 12] | + ((unsigned char) packet[offset + 13] << 8) | + ((unsigned char) packet[offset + 14] << 16) | + ((unsigned char) packet[offset + 15] << 24)); + items[count].una = (uint32_t) ((unsigned char) packet[offset + 16] | + ((unsigned char) packet[offset + 17] << 8) | + ((unsigned char) packet[offset + 18] << 16) | + ((unsigned char) packet[offset + 19] << 24)); + items[count].len = seg_len; + } + count++; + offset += 24U + seg_len; + } + if (segments != NULL) { + *segments = items; + } else { + free(items); + } + if (segment_count != NULL) { + *segment_count = count; + } +} + +static uint64_t kcp_counter_diff(uint64_t previous, uint64_t current) { + return current < previous ? 0 : current - previous; +} + +static void kcp_conn_update_min_srtt_locked(kcp_conn_t *conn) { + int32_t srtt_ms; + + if (conn == NULL || conn->kcp == NULL) { + return; + } + srtt_ms = conn->kcp->rx_srtt; + if (srtt_ms > 0 && (conn->min_srtt_ms <= 0 || srtt_ms < conn->min_srtt_ms)) { + conn->min_srtt_ms = srtt_ms; + } +} + +static void kcp_conn_note_feedback_locked(kcp_conn_t *conn) { + if (conn == NULL) { + return; + } + conn->last_feedback_ms = omni_now_millis32(); + kcp_conn_update_min_srtt_locked(conn); +} + +static int kcp_process_sampler_matches(const kcp_process_sampler_t *sampler, kcp_session_stats_logger_t *logger, const char *node_role, const char *node_id, int stats_interval_ms) { + if (sampler == NULL) { + return 0; + } + return sampler->logger == logger && + sampler->stats_interval_ms == stats_interval_ms && + strcmp(sampler->node_role, node_role == NULL ? "" : node_role) == 0 && + strcmp(sampler->node_id, node_id == NULL ? "" : node_id) == 0; +} + +static void kcp_process_sampler_record_send(kcp_process_sampler_t *sampler, int packet_bytes, size_t segments) { + if (sampler == NULL) { + return; + } + atomic_fetch_add_explicit(&sampler->bytes_sent, (uint64_t) packet_bytes, memory_order_relaxed); + atomic_fetch_add_explicit(&sampler->out_pkts, 1, memory_order_relaxed); + atomic_fetch_add_explicit(&sampler->out_segs, (uint64_t) segments, memory_order_relaxed); +} + +static void kcp_process_sampler_record_input(kcp_process_sampler_t *sampler, int packet_bytes, size_t segments) { + if (sampler == NULL) { + return; + } + atomic_fetch_add_explicit(&sampler->bytes_received, (uint64_t) packet_bytes, memory_order_relaxed); + atomic_fetch_add_explicit(&sampler->in_pkts, 1, memory_order_relaxed); + atomic_fetch_add_explicit(&sampler->in_segs, (uint64_t) segments, memory_order_relaxed); +} + +static void kcp_process_sampler_record_error(kcp_process_sampler_t *sampler) { + if (sampler == NULL) { + return; + } + atomic_fetch_add_explicit(&sampler->in_errs, 1, memory_order_relaxed); + atomic_fetch_add_explicit(&sampler->kcp_in_errs, 1, memory_order_relaxed); +} + +static void kcp_conn_record_send(kcp_conn_t *conn, int packet_bytes, size_t segments) { + 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; + } + conn->pending_bytes_sent += (uint64_t) packet_bytes; + conn->pending_out_pkts += 1; + conn->pending_out_segs += (uint64_t) segments; +} + +static void kcp_conn_record_input(kcp_conn_t *conn, int packet_bytes, size_t segments) { + if (conn == NULL) { + return; + } + if (conn->process_sampler != NULL) { + kcp_process_sampler_record_input(conn->process_sampler, packet_bytes, segments); + return; + } + conn->pending_bytes_received += (uint64_t) packet_bytes; + conn->pending_in_pkts += 1; + conn->pending_in_segs += (uint64_t) segments; +} + +static void kcp_conn_record_error(kcp_conn_t *conn) { + if (conn == NULL) { + return; + } + if (conn->process_sampler != NULL) { + kcp_process_sampler_record_error(conn->process_sampler); + return; + } + conn->pending_in_errs += 1; + conn->pending_kcp_in_errs += 1; +} + +static void kcp_process_sampler_curr_estab_inc(kcp_process_sampler_t *sampler) { + if (sampler == NULL) { + return; + } + atomic_fetch_add_explicit(&sampler->curr_estab, 1, memory_order_relaxed); +} + +static void kcp_process_sampler_curr_estab_dec(kcp_process_sampler_t *sampler) { + uint_fast64_t current; + + if (sampler == NULL) { + return; + } + current = atomic_load_explicit(&sampler->curr_estab, memory_order_relaxed); + while (current > 0) { + if (atomic_compare_exchange_weak_explicit(&sampler->curr_estab, ¤t, current - 1U, memory_order_relaxed, memory_order_relaxed)) { + return; + } + } +} + +static void kcp_process_sampler_add_conn(kcp_process_sampler_t *sampler, kcp_conn_t *conn) { + if (sampler == NULL || conn == NULL) { + return; + } + pthread_mutex_lock(&sampler->lock); + conn->process_next = sampler->members; + sampler->members = conn; + pthread_mutex_unlock(&sampler->lock); + kcp_process_sampler_curr_estab_inc(sampler); +} + +static void kcp_process_sampler_remove_conn(kcp_process_sampler_t *sampler, kcp_conn_t *conn) { + kcp_conn_t *prev = NULL; + kcp_conn_t *cur; + + if (sampler == NULL || conn == NULL) { + return; + } + pthread_mutex_lock(&sampler->lock); + for (cur = sampler->members; cur != NULL; cur = cur->process_next) { + if (cur == conn) { + if (prev == NULL) { + sampler->members = cur->process_next; + } else { + prev->process_next = cur->process_next; + } + conn->process_next = NULL; + break; + } + prev = cur; + } + pthread_mutex_unlock(&sampler->lock); + if (cur == conn) { + kcp_process_sampler_curr_estab_dec(sampler); + } +} + +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) { + *snd_queue = 0; + } + if (rcv_queue != NULL) { + *rcv_queue = 0; + } + 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; + } + + pthread_mutex_lock(&sampler->lock); + for (conn = sampler->members; conn != NULL; conn = conn->process_next) { + pthread_mutex_lock(&conn->kcp_mu); + if (conn->kcp != NULL) { + if (snd_queue != NULL) { + *snd_queue += conn->kcp->nsnd_que; + } + if (rcv_queue != NULL) { + *rcv_queue += conn->kcp->nrcv_que; + } + 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); + } + pthread_mutex_unlock(&sampler->lock); +} + +static void kcp_process_sampler_log_snapshot(kcp_process_sampler_t *sampler, const char *reason) { + kcp_session_stats_record_t record; + uint64_t bytes_sent; + uint64_t bytes_received; + uint64_t in_pkts; + uint64_t out_pkts; + uint64_t in_segs; + uint64_t out_segs; + uint64_t in_errs; + uint64_t kcp_in_errs; + 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; + } + + bytes_sent = atomic_load_explicit(&sampler->bytes_sent, memory_order_relaxed); + bytes_received = atomic_load_explicit(&sampler->bytes_received, memory_order_relaxed); + in_pkts = atomic_load_explicit(&sampler->in_pkts, memory_order_relaxed); + out_pkts = atomic_load_explicit(&sampler->out_pkts, memory_order_relaxed); + in_segs = atomic_load_explicit(&sampler->in_segs, memory_order_relaxed); + 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, + &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); + snprintf(record.node_role, sizeof(record.node_role), "%s", sampler->node_role); + snprintf(record.node_id, sizeof(record.node_id), "%s", sampler->node_id); + snprintf(record.sample_reason, sizeof(record.sample_reason), "%s", reason == NULL ? "" : reason); + record.ts_unix_nano = omni_now_unix_nano(); + + record.has_bytes_sent = 1; + record.bytes_sent = kcp_counter_diff(sampler->prev_bytes_sent, bytes_sent); + record.has_bytes_received = 1; + record.bytes_received = kcp_counter_diff(sampler->prev_bytes_received, bytes_received); + record.has_in_pkts = 1; + record.in_pkts = kcp_counter_diff(sampler->prev_in_pkts, in_pkts); + record.has_out_pkts = 1; + record.out_pkts = kcp_counter_diff(sampler->prev_out_pkts, out_pkts); + record.has_in_segs = 1; + 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; + record.kcp_in_errs = kcp_counter_diff(sampler->prev_kcp_in_errs, kcp_in_errs); + record.has_ring_buffer_snd_queue = 1; + record.ring_buffer_snd_queue = snd_queue; + record.has_ring_buffer_rcv_queue = 1; + record.ring_buffer_rcv_queue = rcv_queue; + record.has_ring_buffer_snd_buffer = 1; + record.ring_buffer_snd_buffer = snd_buffer; + record.has_curr_estab = 1; + record.curr_estab = atomic_load_explicit(&sampler->curr_estab, memory_order_relaxed); + + sampler->prev_bytes_sent = bytes_sent; + sampler->prev_bytes_received = bytes_received; + sampler->prev_in_pkts = in_pkts; + 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; + + (void) kcp_session_stats_log(sampler->logger, &record); +} + +static void *kcp_process_sampler_thread_main(void *arg) { + kcp_process_sampler_t *sampler = (kcp_process_sampler_t *) arg; + + for (;;) { + int has_request = 0; + uint64_t request_id = 0; + char reason[32]; + struct timespec deadline; + + clock_gettime(CLOCK_REALTIME, &deadline); + deadline.tv_sec += sampler->stats_interval_ms / 1000; + deadline.tv_nsec += (long) (sampler->stats_interval_ms % 1000) * 1000000L; + if (deadline.tv_nsec >= 1000000000L) { + deadline.tv_sec += 1; + deadline.tv_nsec -= 1000000000L; + } + + pthread_mutex_lock(&sampler->lock); + while (!sampler->stopped && !sampler->request_pending) { + int wait_rc = pthread_cond_timedwait(&sampler->cond, &sampler->lock, &deadline); + if (wait_rc == ETIMEDOUT) { + break; + } + } + if (sampler->stopped) { + pthread_mutex_unlock(&sampler->lock); + return NULL; + } + if (sampler->request_pending) { + has_request = 1; + request_id = sampler->pending_request_id; + snprintf(reason, sizeof(reason), "%s", sampler->pending_reason); + sampler->request_pending = 0; + } else { + snprintf(reason, sizeof(reason), "%s", "periodic"); + } + pthread_mutex_unlock(&sampler->lock); + + kcp_process_sampler_log_snapshot(sampler, reason); + + if (has_request) { + pthread_mutex_lock(&sampler->lock); + if (request_id > sampler->completed_request_id) { + sampler->completed_request_id = request_id; + } + pthread_cond_broadcast(&sampler->cond); + pthread_mutex_unlock(&sampler->lock); + } + } +} + +static kcp_process_sampler_t *kcp_process_sampler_acquire(kcp_session_stats_logger_t *logger, const char *node_role, const char *node_id, int stats_interval_ms) { + kcp_process_sampler_t *sampler; + + if (logger == NULL) { + return NULL; + } + + pthread_mutex_lock(&g_kcp_process_sampler_mu); + for (sampler = g_kcp_process_samplers; sampler != NULL; sampler = sampler->next) { + if (kcp_process_sampler_matches(sampler, logger, node_role, node_id, stats_interval_ms)) { + sampler->refcount++; + pthread_mutex_unlock(&g_kcp_process_sampler_mu); + return sampler; + } + } + + sampler = (kcp_process_sampler_t *) calloc(1, sizeof(*sampler)); + if (sampler == NULL) { + pthread_mutex_unlock(&g_kcp_process_sampler_mu); + return NULL; + } + + sampler->logger = logger; + sampler->stats_interval_ms = stats_interval_ms > 0 ? stats_interval_ms : KCP_DEFAULT_STATS_INTERVAL_MS; + sampler->refcount = 1; + snprintf(sampler->node_role, sizeof(sampler->node_role), "%s", node_role == NULL ? "" : node_role); + snprintf(sampler->node_id, sizeof(sampler->node_id), "%s", node_id == NULL ? "" : node_id); + pthread_mutex_init(&sampler->lock, NULL); + pthread_cond_init(&sampler->cond, NULL); + if (pthread_create(&sampler->thread, NULL, kcp_process_sampler_thread_main, sampler) != 0) { + pthread_cond_destroy(&sampler->cond); + pthread_mutex_destroy(&sampler->lock); + free(sampler); + pthread_mutex_unlock(&g_kcp_process_sampler_mu); + return NULL; + } + sampler->thread_started = 1; + sampler->next = g_kcp_process_samplers; + g_kcp_process_samplers = sampler; + pthread_mutex_unlock(&g_kcp_process_sampler_mu); + return sampler; +} + +static void kcp_process_sampler_release(kcp_process_sampler_t *sampler) { + kcp_process_sampler_t **cursor; + + if (sampler == NULL) { + return; + } + + pthread_mutex_lock(&g_kcp_process_sampler_mu); + sampler->refcount--; + if (sampler->refcount > 0) { + pthread_mutex_unlock(&g_kcp_process_sampler_mu); + return; + } + for (cursor = &g_kcp_process_samplers; *cursor != NULL; cursor = &(*cursor)->next) { + if (*cursor == sampler) { + *cursor = sampler->next; + break; + } + } + pthread_mutex_unlock(&g_kcp_process_sampler_mu); + + pthread_mutex_lock(&sampler->lock); + sampler->stopped = 1; + pthread_cond_broadcast(&sampler->cond); + pthread_mutex_unlock(&sampler->lock); + if (sampler->thread_started) { + pthread_join(sampler->thread, NULL); + } + pthread_cond_destroy(&sampler->cond); + pthread_mutex_destroy(&sampler->lock); + free(sampler); +} + +static void kcp_process_sampler_request_sample_and_wait(kcp_process_sampler_t *sampler, const char *reason) { + uint64_t request_id; + + if (sampler == NULL) { + return; + } + pthread_mutex_lock(&sampler->lock); + if (sampler->stopped) { + pthread_mutex_unlock(&sampler->lock); + return; + } + sampler->request_pending = 1; + request_id = ++sampler->pending_request_id; + snprintf(sampler->pending_reason, sizeof(sampler->pending_reason), "%s", reason == NULL ? "" : reason); + pthread_cond_broadcast(&sampler->cond); + while (!sampler->stopped && sampler->completed_request_id < request_id) { + pthread_cond_wait(&sampler->cond, &sampler->lock); + } + pthread_mutex_unlock(&sampler->lock); +} + +static int kcp_socket_debug_log_record(kcp_socket_debug_state_t *state, const char *event_name, const struct sockaddr_storage *remote_addr, socklen_t remote_addr_len, int packet_bytes, int has_tx_id, uint32_t tx_id, int has_conv, uint32_t conv, const kcp_packet_debug_segment_t *segments, size_t segment_count, int64_t ts_unix_nano) { + char local_addr_text[OMNI_MAX_ADDR_TEXT]; + char remote_addr_text[OMNI_MAX_ADDR_TEXT]; + struct sockaddr_storage local_addr; + socklen_t local_addr_len = sizeof(local_addr); + kcp_packet_debug_record_t record; + + if (state->logger == NULL) { + return 0; + } + memset(&record, 0, sizeof(record)); + getsockname(state->fd, (struct sockaddr *) &local_addr, &local_addr_len); + omni_sockaddr_to_string((struct sockaddr *) &local_addr, local_addr_len, local_addr_text, sizeof(local_addr_text)); + omni_sockaddr_to_string((const struct sockaddr *) remote_addr, remote_addr_len, remote_addr_text, sizeof(remote_addr_text)); + snprintf(record.event, sizeof(record.event), "%s", event_name); + snprintf(record.node_role, sizeof(record.node_role), "%s", state->node_role); + snprintf(record.node_id, sizeof(record.node_id), "%s", state->node_id); + snprintf(record.local_addr, sizeof(record.local_addr), "%s", local_addr_text); + snprintf(record.remote_addr, sizeof(record.remote_addr), "%s", remote_addr_text); + record.packet_bytes = packet_bytes; + record.has_udp_tx_id = has_tx_id; + record.udp_tx_id = tx_id; + record.has_kcp_conv = has_conv; + record.kcp_conv = conv; + record.ts_unix_nano = ts_unix_nano; + if (segment_count > 0) { + record.segments = (kcp_packet_debug_segment_t *) calloc(segment_count, sizeof(*record.segments)); + if (record.segments == NULL) { + return -1; + } + memcpy(record.segments, segments, segment_count * sizeof(*segments)); + record.segment_count = segment_count; + } + kcp_packet_debug_log(state->logger, &record); + kcp_packet_debug_record_clear(&record); + return 0; +} + +static void kcp_socket_debug_pending_free(kcp_packet_debug_pending_t *pending) { + while (pending != NULL) { + kcp_packet_debug_pending_t *next = pending->next; + free(pending->segments); + free(pending); + pending = next; + } +} + +static int kcp_socket_debug_reserve_tx(kcp_socket_debug_state_t *state, const struct sockaddr_storage *remote_addr, socklen_t remote_addr_len, const uint8_t *packet, size_t packet_len, uint32_t *out_tx_id) { + kcp_packet_debug_pending_t *pending; + if (state->logger == NULL) { + *out_tx_id = 0; + return 0; + } + pending = (kcp_packet_debug_pending_t *) calloc(1, sizeof(*pending)); + if (pending == NULL) { + return -1; + } + pending->tx_id = state->next_tx_id++; + pending->packet_bytes = (int) packet_len; + memcpy(&pending->remote_addr, remote_addr, sizeof(*remote_addr)); + pending->remote_addr_len = remote_addr_len; + kcp_parse_packet_segments(packet, packet_len, &pending->conv, &pending->segments, &pending->segment_count); + pending->has_conv = packet_len >= 4; + pthread_mutex_lock(&state->pending_mu); + pending->next = state->pending_head; + state->pending_head = pending; + pthread_mutex_unlock(&state->pending_mu); + *out_tx_id = pending->tx_id; + return 0; +} + +static void kcp_socket_debug_rollback_tx(kcp_socket_debug_state_t *state, uint32_t tx_id) { + kcp_packet_debug_pending_t *prev = NULL; + kcp_packet_debug_pending_t *cur; + pthread_mutex_lock(&state->pending_mu); + for (cur = state->pending_head; cur != NULL; cur = cur->next) { + if (cur->tx_id == tx_id) { + if (prev == NULL) { + state->pending_head = cur->next; + } else { + prev->next = cur->next; + } + free(cur->segments); + free(cur); + break; + } + prev = cur; + } + pthread_mutex_unlock(&state->pending_mu); +} + +static void *kcp_socket_debug_errqueue_thread(void *arg) { + kcp_socket_debug_state_t *state = (kcp_socket_debug_state_t *) arg; + uint8_t control[512]; + uint8_t dummy = 0; + struct iovec iov; + struct msghdr msg; + + while (!atomic_load(&state->closed)) { + ssize_t rc; + omni_tx_timestamp_event_t event; + kcp_packet_debug_pending_t *prev = NULL; + kcp_packet_debug_pending_t *cur = NULL; + + memset(&msg, 0, sizeof(msg)); + iov.iov_base = &dummy; + iov.iov_len = sizeof(dummy); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = control; + msg.msg_controllen = sizeof(control); + rc = recvmsg(state->fd, &msg, MSG_ERRQUEUE | MSG_DONTWAIT); + if (rc < 0) { + if (errno == EAGAIN || errno == EWOULDBLOCK || errno == EINTR) { + usleep(10000); + continue; + } + if (atomic_load(&state->closed)) { + return NULL; + } + usleep(10000); + continue; + } + if (linux_timestamping_parse_tx_timestamp(&msg, &event) != 0) { + continue; + } + pthread_mutex_lock(&state->pending_mu); + for (cur = state->pending_head; cur != NULL; cur = cur->next) { + if (cur->tx_id == event.ee_data) { + break; + } + prev = cur; + } + if (cur != NULL) { + if (strcmp(event.event_name, EVENT_A_TX_SCHED) == 0) { + cur->saw_sched = 1; + } else if (strcmp(event.event_name, EVENT_A_TX_SOFTWARE) == 0) { + cur->saw_software = 1; + } + kcp_socket_debug_log_record(state, event.event_name, &cur->remote_addr, cur->remote_addr_len, cur->packet_bytes, 1, cur->tx_id, cur->has_conv, cur->conv, cur->segments, cur->segment_count, event.ts_unix_nano); + if (cur->saw_sched && cur->saw_software) { + if (prev == NULL) { + state->pending_head = cur->next; + } else { + prev->next = cur->next; + } + free(cur->segments); + free(cur); + } + } + pthread_mutex_unlock(&state->pending_mu); + } + return NULL; +} + +static int kcp_socket_debug_init(kcp_socket_debug_state_t *state, int fd, kcp_packet_debug_logger_t *logger, const char *node_role, const char *node_id) { + int thread_rc; + memset(state, 0, sizeof(*state)); + state->fd = fd; + state->logger = logger; + snprintf(state->node_role, sizeof(state->node_role), "%s", node_role == NULL ? "" : node_role); + snprintf(state->node_id, sizeof(state->node_id), "%s", node_id == NULL ? "" : node_id); + pthread_mutex_init(&state->write_mu, NULL); + pthread_mutex_init(&state->pending_mu, NULL); + if (logger != NULL) { + if (linux_timestamping_enable_udp_socket(fd, 1) != 0) { + pthread_mutex_destroy(&state->write_mu); + pthread_mutex_destroy(&state->pending_mu); + return -1; + } + thread_rc = pthread_create(&state->errqueue_thread, NULL, kcp_socket_debug_errqueue_thread, state); + if (thread_rc != 0) { + errno = thread_rc; + pthread_mutex_destroy(&state->write_mu); + pthread_mutex_destroy(&state->pending_mu); + return -1; + } + state->errqueue_thread_started = 1; + } + return 0; +} + +static void kcp_socket_debug_destroy(kcp_socket_debug_state_t *state) { + atomic_store(&state->closed, 1); + if (state->errqueue_thread_started) { + pthread_join(state->errqueue_thread, NULL); + } + kcp_socket_debug_pending_free(state->pending_head); + pthread_mutex_destroy(&state->write_mu); + pthread_mutex_destroy(&state->pending_mu); +} + +static int kcp_socket_send_packet(kcp_socket_debug_state_t *state, const struct sockaddr_storage *remote_addr, socklen_t remote_addr_len, const uint8_t *packet, size_t packet_len) { + uint32_t tx_id = 0; + ssize_t rc; + if (state->logger != NULL && kcp_socket_debug_reserve_tx(state, remote_addr, remote_addr_len, packet, packet_len, &tx_id) != 0) { + atomic_store(&state->last_send_errno, errno != 0 ? errno : EIO); + return -1; + } + pthread_mutex_lock(&state->write_mu); + rc = sendto(state->fd, packet, packet_len, 0, (const struct sockaddr *) remote_addr, remote_addr_len); + pthread_mutex_unlock(&state->write_mu); + if (rc < 0 || (size_t) rc != packet_len) { + if (rc >= 0 && (size_t) rc != packet_len && errno == 0) { + errno = EIO; + } + atomic_store(&state->last_send_errno, errno != 0 ? errno : EIO); + if (state->logger != NULL) { + kcp_socket_debug_rollback_tx(state, tx_id); + } + return -1; + } + atomic_store(&state->last_send_errno, 0); + return 0; +} + +static int kcp_output_callback_impl(const char *buf, int len, struct IKCPCB *kcp, void *user) { + kcp_conn_t *conn = (kcp_conn_t *) user; + size_t segment_count = 0; + (void) kcp; + if (conn == NULL || atomic_load(&conn->closed)) { + return -1; + } + kcp_parse_packet_segments((const uint8_t *) buf, (size_t) len, NULL, NULL, &segment_count); + if (kcp_socket_send_packet(conn->sock_state, &conn->remote_addr, conn->remote_addr_len, (const uint8_t *) buf, (size_t) len) != 0) { + return -1; + } + kcp_conn_record_send(conn, len, segment_count); + return len; +} + +static int kcp_conn_attach_process_sampler(kcp_conn_t *conn) { + kcp_process_sampler_t *next_sampler; + kcp_process_sampler_t *previous_sampler; + uint64_t pending_bytes_sent = 0; + uint64_t pending_bytes_received = 0; + uint64_t pending_in_pkts = 0; + uint64_t pending_out_pkts = 0; + uint64_t pending_in_segs = 0; + uint64_t pending_out_segs = 0; + uint64_t pending_in_errs = 0; + uint64_t pending_kcp_in_errs = 0; + + if (conn == NULL) { + errno = EINVAL; + return -1; + } + + next_sampler = kcp_process_sampler_acquire(conn->stats_logger, conn->node_role, conn->node_id, conn->stats_interval_ms); + if (conn->stats_logger != NULL && next_sampler == NULL) { + return -1; + } + + previous_sampler = conn->process_sampler; + if (previous_sampler == next_sampler) { + return 0; + } + + if (next_sampler != NULL) { + kcp_process_sampler_add_conn(next_sampler, conn); + } + pthread_mutex_lock(&conn->kcp_mu); + previous_sampler = conn->process_sampler; + conn->process_sampler = next_sampler; + pending_bytes_sent = conn->pending_bytes_sent; + pending_bytes_received = conn->pending_bytes_received; + pending_in_pkts = conn->pending_in_pkts; + pending_out_pkts = conn->pending_out_pkts; + pending_in_segs = conn->pending_in_segs; + pending_out_segs = conn->pending_out_segs; + pending_in_errs = conn->pending_in_errs; + pending_kcp_in_errs = conn->pending_kcp_in_errs; + conn->pending_bytes_sent = 0; + conn->pending_bytes_received = 0; + conn->pending_in_pkts = 0; + conn->pending_out_pkts = 0; + conn->pending_in_segs = 0; + conn->pending_out_segs = 0; + conn->pending_in_errs = 0; + conn->pending_kcp_in_errs = 0; + pthread_mutex_unlock(&conn->kcp_mu); + if (next_sampler != NULL) { + atomic_fetch_add_explicit(&next_sampler->bytes_sent, pending_bytes_sent, memory_order_relaxed); + atomic_fetch_add_explicit(&next_sampler->bytes_received, pending_bytes_received, memory_order_relaxed); + atomic_fetch_add_explicit(&next_sampler->in_pkts, pending_in_pkts, memory_order_relaxed); + atomic_fetch_add_explicit(&next_sampler->out_pkts, pending_out_pkts, memory_order_relaxed); + atomic_fetch_add_explicit(&next_sampler->in_segs, pending_in_segs, memory_order_relaxed); + atomic_fetch_add_explicit(&next_sampler->out_segs, pending_out_segs, memory_order_relaxed); + atomic_fetch_add_explicit(&next_sampler->in_errs, pending_in_errs, memory_order_relaxed); + atomic_fetch_add_explicit(&next_sampler->kcp_in_errs, pending_kcp_in_errs, memory_order_relaxed); + } + if (previous_sampler != NULL) { + kcp_process_sampler_remove_conn(previous_sampler, conn); + kcp_process_sampler_release(previous_sampler); + } + return 0; +} + +static void kcp_conn_detach_process_sampler(kcp_conn_t *conn) { + kcp_process_sampler_t *sampler; + + if (conn == NULL || conn->process_sampler == NULL) { + return; + } + + sampler = conn->process_sampler; + conn->process_sampler = NULL; + kcp_process_sampler_remove_conn(sampler, conn); + kcp_process_sampler_release(sampler); +} + +static void kcp_log_session_snapshot(kcp_conn_t *conn, const char *reason) { + kcp_session_stats_record_t record; + struct sockaddr_storage local_addr; + 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; + } + memset(&record, 0, sizeof(record)); + snprintf(record.record_type, sizeof(record.record_type), "%s", KCP_SESSION_STATS_RECORD_SESSION_SAMPLE); + snprintf(record.node_role, sizeof(record.node_role), "%s", conn->node_role); + snprintf(record.node_id, sizeof(record.node_id), "%s", conn->node_id); + getsockname(conn->sock_state->fd, (struct sockaddr *) &local_addr, &local_len); + omni_sockaddr_to_string((struct sockaddr *) &local_addr, local_len, local_text, sizeof(local_text)); + omni_sockaddr_to_string((struct sockaddr *) &conn->remote_addr, conn->remote_addr_len, remote_text, sizeof(remote_text)); + snprintf(record.local_addr, sizeof(record.local_addr), "%s", local_text); + snprintf(record.remote_addr, sizeof(record.remote_addr), "%s", remote_text); + record.has_conv = 1; + record.conv = conn->kcp->conv; + record.ts_unix_nano = omni_now_unix_nano(); + snprintf(record.sample_reason, sizeof(record.sample_reason), "%s", reason); + pthread_mutex_lock(&conn->kcp_mu); + record.has_rto_ms = 1; + record.rto_ms = conn->kcp->rx_rto; + record.has_srtt_ms = 1; + record.srtt_ms = conn->kcp->rx_srtt; + kcp_conn_update_min_srtt_locked(conn); + record.has_min_srtt_ms = conn->min_srtt_ms > 0; + record.min_srtt_ms = conn->min_srtt_ms; + record.has_srttvar_ms = 1; + record.srttvar_ms = conn->kcp->rx_rttval; + record.has_last_feedback_age_ms = conn->last_feedback_ms != 0; + record.last_feedback_age_ms = conn->last_feedback_ms == 0 ? 0 : (omni_now_millis32() - conn->last_feedback_ms); + 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); +} + +static void *kcp_stats_thread_main(void *arg) { + kcp_conn_t *conn = (kcp_conn_t *) arg; + while (!atomic_load(&conn->closed)) { + usleep((useconds_t) conn->stats_interval_ms * 1000U); + if (!atomic_load(&conn->closed)) { + kcp_log_session_snapshot(conn, "periodic"); + } + } + return NULL; +} + +static int kcp_socket_open_bound(const char *listen_addr, const char *bind_device, struct sockaddr_storage *local_addr, socklen_t *local_len) { + int family; + int fd; + if (omni_parse_sockaddr(listen_addr, 1, local_addr, local_len, &family) != 0) { + return -1; + } + fd = socket(family, SOCK_DGRAM, 0); + if (fd < 0) { + return -1; + } + if (bind_device != NULL && bind_device[0] != '\0' && omni_bind_device(fd, bind_device) != 0) { + close(fd); + return -1; + } + if (bind(fd, (struct sockaddr *) local_addr, *local_len) != 0) { + close(fd); + return -1; + } + return fd; +} + +static int kcp_socket_open_dial(const char *server_addr, const char *bind_ip, const char *bind_device, struct sockaddr_storage *remote_addr, socklen_t *remote_len, int *family_out) { + int family; + struct sockaddr_storage local_addr; + socklen_t local_len; + int fd; + if (omni_parse_sockaddr(server_addr, 0, remote_addr, remote_len, &family) != 0) { + return -1; + } + fd = socket(family, SOCK_DGRAM, 0); + if (fd < 0) { + return -1; + } + if (bind_device != NULL && bind_device[0] != '\0' && omni_bind_device(fd, bind_device) != 0) { + close(fd); + return -1; + } + if (bind_ip != NULL && bind_ip[0] != '\0') { + struct addrinfo hints; + struct addrinfo *result = NULL; + memset(&hints, 0, sizeof(hints)); + hints.ai_family = family; + hints.ai_socktype = SOCK_DGRAM; + if (getaddrinfo(bind_ip, "0", &hints, &result) != 0 || result == NULL) { + close(fd); + errno = EINVAL; + return -1; + } + memcpy(&local_addr, result->ai_addr, result->ai_addrlen); + local_len = (socklen_t) result->ai_addrlen; + freeaddrinfo(result); + if (bind(fd, (struct sockaddr *) &local_addr, local_len) != 0) { + close(fd); + return -1; + } + } + if (family_out != NULL) { + *family_out = family; + } + return fd; +} + +static int kcp_sockaddr_equal(const struct sockaddr_storage *left, socklen_t left_len, const struct sockaddr_storage *right, socklen_t right_len) { + char left_text[OMNI_MAX_ADDR_TEXT]; + char right_text[OMNI_MAX_ADDR_TEXT]; + + if (left == NULL || right == NULL) { + return left == right; + } + return strcmp( + omni_sockaddr_to_string((const struct sockaddr *) left, left_len, left_text, sizeof(left_text)), + omni_sockaddr_to_string((const struct sockaddr *) right, right_len, right_text, sizeof(right_text)) + ) == 0; +} + +static void *kcp_client_recv_thread_main(void *arg) { + kcp_conn_t *conn = (kcp_conn_t *) arg; + uint8_t buffer[64 * 1024]; + uint8_t control[512]; + struct sockaddr_storage source; + struct iovec iov; + struct msghdr msg; + ssize_t n; + uint32_t conv = 0; + kcp_packet_debug_segment_t *segments = NULL; + size_t segment_count = 0; + int64_t rx_ts; + + while (!atomic_load(&conn->closed)) { + memset(&msg, 0, sizeof(msg)); + memset(&source, 0, sizeof(source)); + iov.iov_base = buffer; + iov.iov_len = sizeof(buffer); + msg.msg_name = &source; + msg.msg_namelen = sizeof(source); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + if (conn->sock_state->logger != NULL) { + msg.msg_control = control; + msg.msg_controllen = sizeof(control); + } + n = recvmsg(conn->fd, &msg, 0); + if (n < 0) { + if (errno == EINTR) { + continue; + } + if (atomic_load(&conn->closed)) { + return NULL; + } + return NULL; + } + kcp_parse_packet_segments(buffer, (size_t) n, &conv, &segments, &segment_count); + rx_ts = conn->sock_state->logger != NULL ? linux_timestamping_parse_rx_timestamp(&msg) : 0; + if (rx_ts > 0) { + kcp_socket_debug_log_record(conn->sock_state, EVENT_B_RX_SOFTWARE, &source, msg.msg_namelen, (int) n, 0, 0, 1, conv, segments, segment_count, rx_ts); + } + if (!kcp_sockaddr_equal(&source, msg.msg_namelen, &conn->remote_addr, conn->remote_addr_len)) { + free(segments); + segments = NULL; + segment_count = 0; + continue; + } + pthread_mutex_lock(&conn->kcp_mu); + conn->kcp->current = omni_now_millis32(); + if (ikcp_input(conn->kcp, (const char *) buffer, n) != 0) { + kcp_conn_record_error(conn); + } else { + kcp_conn_note_feedback_locked(conn); + kcp_conn_record_input(conn, (int) n, segment_count); + } + pthread_mutex_unlock(&conn->kcp_mu); + pthread_cond_broadcast(&conn->rx_cond); + free(segments); + segments = NULL; + segment_count = 0; + } + return NULL; +} + +static void *kcp_update_thread_main(void *arg) { + kcp_conn_t *conn = (kcp_conn_t *) arg; + while (!atomic_load(&conn->closed)) { + int interval_ms; + pthread_mutex_lock(&conn->kcp_mu); + ikcp_update(conn->kcp, omni_now_millis32()); + interval_ms = conn->update_interval_ms > 0 ? conn->update_interval_ms : KCP_DEFAULT_INTERVAL_MS; + pthread_mutex_unlock(&conn->kcp_mu); + usleep((useconds_t) interval_ms * 1000U); + } + return NULL; +} + +static int kcp_conn_start_stats_thread(kcp_conn_t *conn) { + int thread_rc; + if (conn == NULL || conn->stats_logger == NULL || conn->stats_thread_started) { + return 0; + } + thread_rc = pthread_create(&conn->stats_thread, NULL, kcp_stats_thread_main, conn); + if (thread_rc != 0) { + errno = thread_rc; + return -1; + } + conn->stats_thread_started = 1; + return 0; +} + +static kcp_conn_t *kcp_conn_alloc_common(int fd, const struct sockaddr_storage *remote_addr, socklen_t remote_addr_len, const kcp_conn_options_t *options, kcp_socket_debug_state_t *sock_state, 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 *conn = (kcp_conn_t *) calloc(1, sizeof(*conn)); + uint32_t conv; + int thread_rc; + kcp_conn_options_t effective_options; + + if (conn == NULL) { + errno = ENOMEM; + return NULL; + } + conn->fd = fd; + memcpy(&conn->remote_addr, remote_addr, sizeof(*remote_addr)); + conn->remote_addr_len = remote_addr_len; + pthread_mutex_init(&conn->kcp_mu, NULL); + pthread_mutex_init(&conn->close_mu, NULL); + pthread_cond_init(&conn->rx_cond, NULL); + protocol_frame_decoder_init(&conn->decoder); + conn->logger = logger; + snprintf(conn->node_role, sizeof(conn->node_role), "%s", node_role == NULL ? "" : node_role); + snprintf(conn->node_id, sizeof(conn->node_id), "%s", node_id == NULL ? "" : node_id); + conn->stats_logger = stats_logger; + conn->stats_interval_ms = stats_interval_ms > 0 ? stats_interval_ms : KCP_DEFAULT_STATS_INTERVAL_MS; + kcp_conn_options_init(&effective_options); + if (options != NULL) { + effective_options = *options; + } + conn->options = effective_options; + conn->update_interval_ms = effective_options.interval_ms; + conn->sock_state = sock_state; + if (omni_random_u32(&conv) != 0) { + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + return NULL; + } + conn->kcp = ikcp_create(conv, conn); + if (conn->kcp == NULL) { + errno = ENOMEM; + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + return NULL; + } + ikcp_setoutput(conn->kcp, kcp_output_callback_impl); + if (kcp_conn_apply_options_locked(conn, &effective_options) != 0) { + ikcp_release(conn->kcp); + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + return NULL; + } + if (kcp_conn_attach_process_sampler(conn) != 0) { + ikcp_release(conn->kcp); + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + return NULL; + } + if (kcp_conn_start_stats_thread(conn) != 0) { + kcp_conn_detach_process_sampler(conn); + ikcp_release(conn->kcp); + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + return NULL; + } + thread_rc = pthread_create(&conn->update_thread, NULL, kcp_update_thread_main, conn); + if (thread_rc != 0) { + errno = thread_rc; + if (conn->stats_thread_started) { + atomic_store(&conn->closed, 1); + pthread_join(conn->stats_thread, NULL); + } + kcp_conn_detach_process_sampler(conn); + ikcp_release(conn->kcp); + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + return NULL; + } + conn->update_thread_started = 1; + return conn; +} + +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) { + struct sockaddr_storage remote_addr; + socklen_t remote_len; + int family; + int fd = kcp_socket_open_dial(server_addr, bind_ip, bind_device, &remote_addr, &remote_len, &family); + kcp_conn_t *conn; + kcp_socket_debug_state_t *sock_state; + int thread_rc; + (void) family; + if (fd < 0) { + return NULL; + } + sock_state = (kcp_socket_debug_state_t *) calloc(1, sizeof(*sock_state)); + if (sock_state == NULL) { + errno = ENOMEM; + close(fd); + return NULL; + } + if (kcp_socket_debug_init(sock_state, fd, packet_logger, node_role, node_id) != 0) { + free(sock_state); + close(fd); + return NULL; + } + conn = kcp_conn_alloc_common(fd, &remote_addr, remote_len, options, sock_state, logger, node_role, node_id, stats_logger, stats_interval_ms); + if (conn == NULL) { + kcp_socket_debug_destroy(sock_state); + free(sock_state); + close(fd); + return NULL; + } + conn->is_client = 1; + conn->owns_socket = 1; + thread_rc = pthread_create(&conn->recv_thread, NULL, kcp_client_recv_thread_main, conn); + if (thread_rc != 0) { + errno = thread_rc; + kcp_conn_free(conn); + return NULL; + } + conn->recv_thread_started = 1; + return conn; +} + +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) { + return kcp_conn_dial_with_options(server_addr, bind_ip, bind_device, NULL, packet_logger, logger, node_role, node_id, stats_logger, stats_interval_ms); +} + +static void kcp_listener_enqueue_accept(kcp_listener_t *listener, kcp_conn_t *conn) { + pthread_mutex_lock(&listener->accept_mu); + if (listener->accept_tail == NULL) { + listener->accept_head = conn; + } else { + listener->accept_tail->accept_next = conn; + } + listener->accept_tail = conn; + conn->accept_next = NULL; + pthread_cond_signal(&listener->accept_cond); + pthread_mutex_unlock(&listener->accept_mu); +} + +static kcp_conn_t *kcp_listener_find_session(kcp_listener_t *listener, uint32_t conv) { + kcp_session_entry_t *entry; + for (entry = listener->sessions; entry != NULL; entry = entry->next) { + if (entry->conv == conv) { + return entry->conn; + } + } + return NULL; +} + +static int kcp_listener_add_session(kcp_listener_t *listener, uint32_t conv, kcp_conn_t *conn) { + kcp_session_entry_t *entry = (kcp_session_entry_t *) calloc(1, sizeof(*entry)); + if (entry == NULL) { + return -1; + } + entry->conv = conv; + entry->conn = conn; + entry->next = listener->sessions; + listener->sessions = entry; + return 0; +} + +static void kcp_listener_remove_session(kcp_listener_t *listener, kcp_conn_t *conn) { + kcp_session_entry_t *prev_entry = NULL; + kcp_session_entry_t *entry; + kcp_conn_t *prev_accept = NULL; + kcp_conn_t *accept; + + if (listener == NULL || conn == NULL) { + return; + } + + pthread_mutex_lock(&listener->lock); + for (entry = listener->sessions; entry != NULL; entry = entry->next) { + if (entry->conn == conn) { + if (prev_entry == NULL) { + listener->sessions = entry->next; + } else { + prev_entry->next = entry->next; + } + free(entry); + break; + } + prev_entry = entry; + } + pthread_mutex_unlock(&listener->lock); + + pthread_mutex_lock(&listener->accept_mu); + for (accept = listener->accept_head; accept != NULL; accept = accept->accept_next) { + if (accept == conn) { + if (prev_accept == NULL) { + listener->accept_head = accept->accept_next; + } else { + prev_accept->accept_next = accept->accept_next; + } + if (listener->accept_tail == conn) { + listener->accept_tail = prev_accept; + } + conn->accept_next = NULL; + break; + } + prev_accept = accept; + } + pthread_mutex_unlock(&listener->accept_mu); +} + +static void *kcp_listener_recv_thread_main(void *arg) { + kcp_listener_t *listener = (kcp_listener_t *) arg; + uint8_t buffer[64 * 1024]; + uint8_t control[512]; + struct sockaddr_storage source; + struct iovec iov; + struct msghdr msg; + ssize_t n; + uint32_t conv; + kcp_packet_debug_segment_t *segments = NULL; + size_t segment_count = 0; + int64_t rx_ts; + + while (!listener->closed) { + memset(&msg, 0, sizeof(msg)); + memset(&source, 0, sizeof(source)); + iov.iov_base = buffer; + iov.iov_len = sizeof(buffer); + msg.msg_name = &source; + msg.msg_namelen = sizeof(source); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + if (listener->sock_state.logger != NULL) { + msg.msg_control = control; + msg.msg_controllen = sizeof(control); + } + n = recvmsg(listener->fd, &msg, 0); + if (n < 0) { + if (errno == EINTR) { + continue; + } + if (listener->closed) { + return NULL; + } + return NULL; + } + kcp_parse_packet_segments(buffer, (size_t) n, &conv, &segments, &segment_count); + rx_ts = listener->sock_state.logger != NULL ? linux_timestamping_parse_rx_timestamp(&msg) : 0; + if (rx_ts > 0) { + kcp_socket_debug_log_record(&listener->sock_state, EVENT_B_RX_SOFTWARE, &source, msg.msg_namelen, (int) n, 0, 0, 1, conv, segments, segment_count, rx_ts); + } + pthread_mutex_lock(&listener->lock); + { + kcp_conn_t *conn = kcp_listener_find_session(listener, conv); + if (conn == NULL) { + conn = (kcp_conn_t *) calloc(1, sizeof(*conn)); + if (conn != NULL) { + kcp_conn_options_t accepted_options; + conn->fd = listener->fd; + memcpy(&conn->remote_addr, &source, sizeof(source)); + conn->remote_addr_len = msg.msg_namelen; + pthread_mutex_init(&conn->kcp_mu, NULL); + pthread_mutex_init(&conn->close_mu, NULL); + pthread_cond_init(&conn->rx_cond, NULL); + protocol_frame_decoder_init(&conn->decoder); + snprintf(conn->node_role, sizeof(conn->node_role), "%s", listener->sock_state.node_role); + snprintf(conn->node_id, sizeof(conn->node_id), "%s", listener->sock_state.node_id); + conn->stats_interval_ms = KCP_DEFAULT_STATS_INTERVAL_MS; + conn->sock_state = &listener->sock_state; + conn->listener = listener; + kcp_conn_options_init(&accepted_options); + conn->options = accepted_options; + conn->update_interval_ms = accepted_options.interval_ms; + conn->kcp = ikcp_create(conv, conn); + if (conn->kcp != NULL) { + int update_started = 0; + ikcp_setoutput(conn->kcp, kcp_output_callback_impl); + if (kcp_conn_apply_options_locked(conn, &accepted_options) == 0 && + pthread_create(&conn->update_thread, NULL, kcp_update_thread_main, conn) == 0) { + update_started = 1; + } + if (update_started && kcp_listener_add_session(listener, conv, conn) == 0) { + conn->update_thread_started = 1; + kcp_listener_enqueue_accept(listener, conn); + } else { + atomic_store(&conn->closed, 1); + if (update_started) { + pthread_join(conn->update_thread, NULL); + } + ikcp_release(conn->kcp); + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + conn = NULL; + } + } else { + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); + conn = NULL; + } + } + } + if (conn != NULL && conn->kcp != NULL) { + pthread_mutex_lock(&conn->kcp_mu); + conn->kcp->current = omni_now_millis32(); + if (ikcp_input(conn->kcp, (const char *) buffer, n) != 0) { + kcp_conn_record_error(conn); + } else { + kcp_conn_note_feedback_locked(conn); + kcp_conn_record_input(conn, (int) n, segment_count); + } + pthread_mutex_unlock(&conn->kcp_mu); + pthread_cond_broadcast(&conn->rx_cond); + } + } + pthread_mutex_unlock(&listener->lock); + free(segments); + segments = NULL; + segment_count = 0; + } + return NULL; +} + +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) { + struct sockaddr_storage local_addr; + socklen_t local_len; + int fd = kcp_socket_open_bound(listen_addr, bind_device, &local_addr, &local_len); + kcp_listener_t *listener; + if (fd < 0) { + return NULL; + } + listener = (kcp_listener_t *) calloc(1, sizeof(*listener)); + if (listener == NULL) { + close(fd); + return NULL; + } + listener->fd = fd; + pthread_mutex_init(&listener->lock, NULL); + pthread_mutex_init(&listener->accept_mu, NULL); + pthread_cond_init(&listener->accept_cond, NULL); + if (kcp_socket_debug_init(&listener->sock_state, fd, packet_logger, node_role, node_id) != 0) { + kcp_listener_free(listener); + return NULL; + } + if (pthread_create(&listener->recv_thread, NULL, kcp_listener_recv_thread_main, listener) != 0) { + kcp_listener_free(listener); + return NULL; + } + listener->recv_thread_started = 1; + return listener; +} + +kcp_conn_t *kcp_listener_accept(kcp_listener_t *listener) { + kcp_conn_t *conn; + if (listener == NULL) { + errno = EINVAL; + return NULL; + } + pthread_mutex_lock(&listener->accept_mu); + while (!listener->closed && listener->accept_head == NULL) { + pthread_cond_wait(&listener->accept_cond, &listener->accept_mu); + } + if (listener->closed) { + pthread_mutex_unlock(&listener->accept_mu); + errno = ECANCELED; + return NULL; + } + conn = listener->accept_head; + listener->accept_head = conn->accept_next; + if (listener->accept_head == NULL) { + listener->accept_tail = NULL; + } + conn->accept_next = NULL; + pthread_mutex_unlock(&listener->accept_mu); + return conn; +} + +int kcp_conn_configure_runtime(kcp_conn_t *conn, latency_logger_t *logger, const char *node_role, const char *node_id, kcp_session_stats_logger_t *stats_logger, int stats_interval_ms) { + if (conn == NULL) { + errno = EINVAL; + return -1; + } + pthread_mutex_lock(&conn->close_mu); + conn->logger = logger; + if (node_role != NULL) { + snprintf(conn->node_role, sizeof(conn->node_role), "%s", node_role); + } + if (node_id != NULL) { + snprintf(conn->node_id, sizeof(conn->node_id), "%s", node_id); + } + conn->stats_logger = stats_logger; + if (stats_interval_ms > 0) { + conn->stats_interval_ms = stats_interval_ms; + } else if (conn->stats_interval_ms <= 0) { + conn->stats_interval_ms = KCP_DEFAULT_STATS_INTERVAL_MS; + } + if (kcp_conn_attach_process_sampler(conn) != 0) { + pthread_mutex_unlock(&conn->close_mu); + return -1; + } + pthread_mutex_unlock(&conn->close_mu); + if (kcp_conn_start_stats_thread(conn) != 0) { + pthread_mutex_lock(&conn->close_mu); + kcp_conn_detach_process_sampler(conn); + pthread_mutex_unlock(&conn->close_mu); + return -1; + } + return 0; +} + +int kcp_conn_apply_options(kcp_conn_t *conn, const kcp_conn_options_t *options) { + int rc; + + if (conn == NULL || options == NULL) { + errno = EINVAL; + return -1; + } + pthread_mutex_lock(&conn->kcp_mu); + rc = kcp_conn_apply_options_locked(conn, options); + pthread_mutex_unlock(&conn->kcp_mu); + return rc; +} + +int kcp_conn_send(kcp_conn_t *conn, const message_t *msg) { + uint8_t *frame = NULL; + size_t frame_len = 0; + int send_errno = 0; + int kcp_send_rc = 0; + if (conn == NULL || msg == NULL) { + errno = EINVAL; + return -1; + } + if (protocol_encode_message_stream(msg, &frame, &frame_len) != 0) { + return -1; + } + latencylog_log_message_event(conn->logger, conn->node_role, conn->node_id, EVENT_SEND_HANDOFF_BEGIN, msg); + pthread_mutex_lock(&conn->kcp_mu); + atomic_store(&conn->sock_state->last_send_errno, 0); + conn->kcp->current = omni_now_millis32(); + kcp_send_rc = ikcp_send(conn->kcp, (const char *) frame, (int) frame_len); + if (kcp_send_rc < 0) { + pthread_mutex_unlock(&conn->kcp_mu); + errno = kcp_send_rc == -2 ? EMSGSIZE : EINVAL; + free(frame); + return -1; + } + ikcp_flush(conn->kcp); + send_errno = atomic_load(&conn->sock_state->last_send_errno); + pthread_mutex_unlock(&conn->kcp_mu); + if (send_errno != 0) { + errno = send_errno; + free(frame); + return -1; + } + latencylog_log_message_event(conn->logger, conn->node_role, conn->node_id, EVENT_SEND_HANDOFF_END, msg); + free(frame); + return 0; +} + +static void kcp_timespec_deadline_after_ms(struct timespec *deadline, int timeout_ms) { + clock_gettime(CLOCK_REALTIME, deadline); + deadline->tv_sec += timeout_ms / 1000; + deadline->tv_nsec += (long) (timeout_ms % 1000) * 1000000L; + if (deadline->tv_nsec >= 1000000000L) { + deadline->tv_sec += 1; + deadline->tv_nsec -= 1000000000L; + } +} + +int kcp_conn_receive_timed(kcp_conn_t *conn, message_t *out_msg, int timeout_ms) { + uint8_t *frame = NULL; + size_t frame_len = 0; + char err[128]; + int next_rc; + struct timespec deadline; + int use_deadline = timeout_ms > 0; + + if (conn == NULL || out_msg == NULL) { + errno = EINVAL; + return -1; + } + if (use_deadline) { + kcp_timespec_deadline_after_ms(&deadline, timeout_ms); + } + for (;;) { + next_rc = protocol_frame_decoder_next(&conn->decoder, &frame, &frame_len); + if (next_rc < 0) { + return -1; + } + if (next_rc == 1) { + if (protocol_decode_message_stream_payload(frame, frame_len, out_msg, err, sizeof(err)) != 0) { + free(frame); + errno = EPROTO; + return -1; + } + free(frame); + return 0; + } + pthread_mutex_lock(&conn->kcp_mu); + { + int n = ikcp_recv(conn->kcp, (char *) conn->scratch, (int) sizeof(conn->scratch)); + if (n > 0) { + if (protocol_frame_decoder_feed(&conn->decoder, conn->scratch, (size_t) n) != 0) { + pthread_mutex_unlock(&conn->kcp_mu); + return -1; + } + pthread_mutex_unlock(&conn->kcp_mu); + continue; + } + if (atomic_load(&conn->closed)) { + pthread_mutex_unlock(&conn->kcp_mu); + errno = ECANCELED; + return -1; + } + if (timeout_ms == 0) { + pthread_mutex_unlock(&conn->kcp_mu); + return 1; + } + if (timeout_ms < 0) { + pthread_cond_wait(&conn->rx_cond, &conn->kcp_mu); + } else { + int wait_rc = pthread_cond_timedwait(&conn->rx_cond, &conn->kcp_mu, &deadline); + if (wait_rc == ETIMEDOUT) { + pthread_mutex_unlock(&conn->kcp_mu); + return 1; + } + if (wait_rc != 0) { + pthread_mutex_unlock(&conn->kcp_mu); + errno = wait_rc; + return -1; + } + } + } + pthread_mutex_unlock(&conn->kcp_mu); + } +} + +int kcp_conn_receive(kcp_conn_t *conn, message_t *out_msg) { + return kcp_conn_receive_timed(conn, out_msg, -1); +} + +uint32_t kcp_conn_conv(const kcp_conn_t *conn) { + return conn == NULL || conn->kcp == NULL ? 0 : conn->kcp->conv; +} + +int kcp_conn_local_addr(const kcp_conn_t *conn, struct sockaddr_storage *addr, socklen_t *addr_len) { + socklen_t len = sizeof(*addr); + if (conn == NULL || addr == NULL || addr_len == NULL || conn->sock_state == NULL) { + errno = EINVAL; + return -1; + } + if (getsockname(conn->sock_state->fd, (struct sockaddr *) addr, &len) != 0) { + return -1; + } + *addr_len = len; + return 0; +} + +int kcp_conn_remote_addr(const kcp_conn_t *conn, struct sockaddr_storage *addr, socklen_t *addr_len) { + if (conn == NULL || addr == NULL || addr_len == NULL) { + errno = EINVAL; + return -1; + } + if (conn->remote_addr_len == 0) { + errno = ENOTCONN; + return -1; + } + return omni_clone_sockaddr((const struct sockaddr *) &conn->remote_addr, conn->remote_addr_len, addr, addr_len); +} + +void kcp_conn_runtime_stats_snapshot(kcp_conn_t *conn, kcp_runtime_stats_t *out_stats) { + if (out_stats == NULL) { + return; + } + + memset(out_stats, 0, sizeof(*out_stats)); + if (conn == NULL) { + return; + } + + out_stats->connected = atomic_load(&conn->closed) ? 0 : 1; + pthread_mutex_lock(&conn->kcp_mu); + if (conn->kcp != NULL) { + out_stats->conv = conn->kcp->conv; + out_stats->rto_ms = conn->kcp->rx_rto; + out_stats->srtt_ms = conn->kcp->rx_srtt; + kcp_conn_update_min_srtt_locked(conn); + out_stats->min_srtt_ms = conn->min_srtt_ms; + out_stats->srttvar_ms = conn->kcp->rx_rttval; + out_stats->last_feedback_age_ms = conn->last_feedback_ms == 0 ? 0 : (omni_now_millis32() - conn->last_feedback_ms); + out_stats->snd_wnd = conn->kcp->snd_wnd; + out_stats->rmt_wnd = conn->kcp->rmt_wnd; + out_stats->inflight = conn->kcp->snd_nxt - conn->kcp->snd_una; + out_stats->window_limit = conn->kcp->snd_wnd < conn->kcp->rmt_wnd ? conn->kcp->snd_wnd : conn->kcp->rmt_wnd; + out_stats->window_pressure_pct = out_stats->window_limit == 0 + ? 0.0 + : ((double) out_stats->inflight * 100.0) / (double) out_stats->window_limit; + out_stats->snd_queue = conn->kcp->nsnd_que; + out_stats->rcv_queue = conn->kcp->nrcv_que; + out_stats->snd_buffer = conn->kcp->nsnd_buf; + out_stats->out_segs_total = atomic_load_explicit(&conn->total_out_segs, memory_order_relaxed); + out_stats->fast_retrans_total = conn->kcp->fast_retrans_total; + 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_stats->connected = 0; + } + pthread_mutex_unlock(&conn->kcp_mu); +} + +int kcp_conn_close(kcp_conn_t *conn) { + if (conn == NULL) { + return 0; + } + pthread_mutex_lock(&conn->close_mu); + if (!atomic_load(&conn->closed)) { + kcp_log_session_snapshot(conn, "close"); + kcp_process_sampler_request_sample_and_wait(conn->process_sampler, "close"); + 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; + } + pthread_cond_broadcast(&conn->rx_cond); + pthread_mutex_unlock(&conn->kcp_mu); + } + pthread_mutex_unlock(&conn->close_mu); + return 0; +} + +void kcp_conn_free(kcp_conn_t *conn) { + if (conn == NULL) { + return; + } + kcp_conn_close(conn); + if (conn->recv_thread_started) { + pthread_join(conn->recv_thread, NULL); + } + if (conn->update_thread_started) { + pthread_join(conn->update_thread, NULL); + } + if (conn->stats_thread_started) { + pthread_join(conn->stats_thread, NULL); + } + if (conn->listener != NULL && !conn->listener->closed) { + kcp_listener_remove_session(conn->listener, conn); + } + kcp_conn_detach_process_sampler(conn); + if (conn->owns_socket && conn->sock_state != NULL) { + if (!conn->socket_closed) { + close(conn->fd); + conn->socket_closed = 1; + } + kcp_socket_debug_destroy(conn->sock_state); + free(conn->sock_state); + } + if (conn->kcp != NULL) { + ikcp_release(conn->kcp); + } + protocol_frame_decoder_destroy(&conn->decoder); + pthread_cond_destroy(&conn->rx_cond); + pthread_mutex_destroy(&conn->kcp_mu); + pthread_mutex_destroy(&conn->close_mu); + free(conn); +} + +int kcp_listener_close(kcp_listener_t *listener) { + if (listener == NULL) { + return 0; + } + if (!listener->closed) { + listener->closed = 1; + close(listener->fd); + pthread_cond_broadcast(&listener->accept_cond); + } + return 0; +} + +void kcp_listener_free(kcp_listener_t *listener) { + kcp_session_entry_t *entry; + kcp_session_entry_t *next; + if (listener == NULL) { + return; + } + kcp_listener_close(listener); + if (listener->recv_thread_started) { + pthread_join(listener->recv_thread, NULL); + } + for (entry = listener->sessions; entry != NULL; entry = next) { + next = entry->next; + entry->conn->listener = NULL; + kcp_conn_free(entry->conn); + free(entry); + } + kcp_socket_debug_destroy(&listener->sock_state); + pthread_mutex_destroy(&listener->lock); + pthread_mutex_destroy(&listener->accept_mu); + pthread_cond_destroy(&listener->accept_cond); + free(listener); +} + +int kcp_session_stats_parse_interval_ms(const char *raw, int *out_ms) { + return omni_parse_duration_ms(raw, KCP_DEFAULT_STATS_INTERVAL_MS, out_ms); +} diff --git a/robot/v4l2/OmniSocketGo_robot/src/transport_udp.c b/robot/v4l2/OmniSocketGo_robot/src/transport_udp.c new file mode 100644 index 0000000..49a0e18 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/src/transport_udp.c @@ -0,0 +1,486 @@ +#include "transport_udp.h" + +#include +#include +#include +#include +#include + +typedef struct udp_pending_tx { + struct udp_pending_tx *next; + uint32_t tx_id; + message_t msg; + int bytes_written; + int saw_sched; + int saw_software; +} udp_pending_tx_t; + +struct udp_conn { + int fd; + int connected; + int timestamping_enabled; + latency_logger_t *logger; + tx_timestamp_debug_logger_t *debug_logger; + char node_role[OMNI_MAX_NODE_ROLE]; + char node_id[OMNI_MAX_PEER_ID]; + pthread_mutex_t write_mu; + pthread_mutex_t pending_mu; + pthread_t errqueue_thread; + int errqueue_thread_started; + uint32_t next_tx_id; + udp_pending_tx_t *pending_head; + uint8_t *recv_buffer; + size_t recv_buffer_cap; + int closed; +}; + +static int udp_open_socket_for_addr(const struct sockaddr *addr, socklen_t addr_len, int bind_device, const char *device) { + int fd; + int reuse = 1; + fd = socket(addr->sa_family, SOCK_DGRAM, 0); + if (fd < 0) { + return -1; + } + setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); + if (bind_device && omni_bind_device(fd, device) != 0) { + close(fd); + return -1; + } + (void) addr_len; + return fd; +} + +static int udp_resolve_ip_only(const char *ip, int family, struct sockaddr_storage *out, socklen_t *out_len) { + struct addrinfo hints; + struct addrinfo *result = NULL; + memset(&hints, 0, sizeof(hints)); + hints.ai_family = family; + hints.ai_socktype = SOCK_DGRAM; + if (getaddrinfo(ip, "0", &hints, &result) != 0 || result == NULL) { + errno = EINVAL; + return -1; + } + memcpy(out, result->ai_addr, result->ai_addrlen); + *out_len = (socklen_t) result->ai_addrlen; + freeaddrinfo(result); + return 0; +} + +static udp_conn_t *udp_conn_alloc(int fd, int connected, int enable_timestamping, latency_logger_t *logger, const char *node_role, const char *node_id, tx_timestamp_debug_logger_t *debug_logger) { + udp_conn_t *conn = (udp_conn_t *) calloc(1, sizeof(*conn)); + if (conn == NULL) { + return NULL; + } + conn->fd = fd; + conn->connected = connected; + conn->timestamping_enabled = enable_timestamping; + conn->logger = logger; + conn->debug_logger = debug_logger; + snprintf(conn->node_role, sizeof(conn->node_role), "%s", node_role == NULL ? "" : node_role); + snprintf(conn->node_id, sizeof(conn->node_id), "%s", node_id == NULL ? "" : node_id); + conn->recv_buffer = (uint8_t *) malloc(OMNI_MAX_FRAME_SIZE); + if (conn->recv_buffer == NULL) { + free(conn); + return NULL; + } + conn->recv_buffer_cap = OMNI_MAX_FRAME_SIZE; + pthread_mutex_init(&conn->write_mu, NULL); + pthread_mutex_init(&conn->pending_mu, NULL); + return conn; +} + +static void udp_pending_destroy(udp_pending_tx_t *pending) { + while (pending != NULL) { + udp_pending_tx_t *next = pending->next; + protocol_message_clear(&pending->msg); + free(pending); + pending = next; + } +} + +static int udp_debug_log_send_chunk(udp_conn_t *conn, const message_t *msg, int bytes_written, uint32_t tx_id) { + tx_timestamp_debug_record_t record; + if (conn->debug_logger == NULL) { + return 0; + } + memset(&record, 0, sizeof(record)); + snprintf(record.record_type, sizeof(record.record_type), "%s", TX_TIMESTAMP_DEBUG_RECORD_SEND_CHUNK); + snprintf(record.node_role, sizeof(record.node_role), "%s", conn->node_role); + snprintf(record.node_id, sizeof(record.node_id), "%s", conn->node_id); + record.message_type = msg->type; + record.message_id = msg->id; + snprintf(record.from, sizeof(record.from), "%s", msg->from); + snprintf(record.to, sizeof(record.to), "%s", msg->to); + snprintf(record.file_name, sizeof(record.file_name), "%s", msg->file_name); + record.body_size = (int) msg->body_len; + record.send_call_index = 0; + record.frame_offset_start = 0; + record.frame_offset_end = bytes_written > 0 ? bytes_written - 1 : 0; + record.bytes_written = bytes_written; + record.expected_tx_id = tx_id; + return tx_timestamp_debug_log(conn->debug_logger, &record); +} + +static void udp_debug_log_errqueue_event(udp_conn_t *conn, const message_t *msg, const omni_tx_timestamp_event_t *event, uint32_t tx_id, int selected) { + tx_timestamp_debug_record_t record; + if (conn->debug_logger == NULL) { + return; + } + memset(&record, 0, sizeof(record)); + snprintf(record.record_type, sizeof(record.record_type), "%s", TX_TIMESTAMP_DEBUG_RECORD_ERRQUEUE_EVENT); + snprintf(record.node_role, sizeof(record.node_role), "%s", conn->node_role); + snprintf(record.node_id, sizeof(record.node_id), "%s", conn->node_id); + record.message_type = msg->type; + record.message_id = msg->id; + snprintf(record.from, sizeof(record.from), "%s", msg->from); + snprintf(record.to, sizeof(record.to), "%s", msg->to); + snprintf(record.file_name, sizeof(record.file_name), "%s", msg->file_name); + record.body_size = (int) msg->body_len; + snprintf(record.phase, sizeof(record.phase), "%s", "background"); + record.read_index = 0; + snprintf(record.event_name, sizeof(record.event_name), "%s", event->event_name); + record.ts_unix_nano = event->ts_unix_nano; + record.ee_info = event->ee_info; + record.ee_data = event->ee_data; + record.expected_tx_id = tx_id; + record.selected_for_latency = selected; + tx_timestamp_debug_log(conn->debug_logger, &record); +} + +static void *udp_errqueue_thread_main(void *arg) { + udp_conn_t *conn = (udp_conn_t *) arg; + uint8_t control[512]; + struct msghdr msg; + struct iovec iov; + uint8_t dummy; + + while (!conn->closed) { + ssize_t rc; + omni_tx_timestamp_event_t event; + udp_pending_tx_t *prev = NULL; + udp_pending_tx_t *cur = NULL; + memset(&msg, 0, sizeof(msg)); + memset(control, 0, sizeof(control)); + dummy = 0; + iov.iov_base = &dummy; + iov.iov_len = sizeof(dummy); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = control; + msg.msg_controllen = sizeof(control); + rc = recvmsg(conn->fd, &msg, MSG_ERRQUEUE | MSG_DONTWAIT); + if (rc < 0) { + if (errno == EAGAIN || errno == EWOULDBLOCK || errno == EINTR) { + usleep(10000); + continue; + } + if (conn->closed) { + return NULL; + } + usleep(10000); + continue; + } + if (linux_timestamping_parse_tx_timestamp(&msg, &event) != 0) { + continue; + } + pthread_mutex_lock(&conn->pending_mu); + cur = NULL; + prev = NULL; + { + udp_pending_tx_t **head = &conn->pending_head; + udp_pending_tx_t *iter = *head; + while (iter != NULL) { + if (iter->tx_id == event.ee_data) { + cur = iter; + break; + } + prev = iter; + iter = iter->next; + } + if (cur != NULL) { + if (strcmp(event.event_name, EVENT_A_TX_SCHED) == 0 && !cur->saw_sched) { + cur->saw_sched = 1; + latencylog_log_message_event_at(conn->logger, conn->node_role, conn->node_id, EVENT_A_TX_SCHED, event.ts_unix_nano, &cur->msg); + } else if (strcmp(event.event_name, EVENT_A_TX_SOFTWARE) == 0 && !cur->saw_software) { + cur->saw_software = 1; + latencylog_log_message_event_at(conn->logger, conn->node_role, conn->node_id, EVENT_A_TX_SOFTWARE, event.ts_unix_nano, &cur->msg); + } + udp_debug_log_errqueue_event(conn, &cur->msg, &event, cur->tx_id, 1); + if (cur->saw_sched && cur->saw_software) { + if (prev == NULL) { + *head = cur->next; + } else { + prev->next = cur->next; + } + protocol_message_clear(&cur->msg); + free(cur); + } + } + } + pthread_mutex_unlock(&conn->pending_mu); + } + return NULL; +} + +static int udp_conn_start_errqueue(udp_conn_t *conn) { + if (!conn->timestamping_enabled) { + return 0; + } + if (pthread_create(&conn->errqueue_thread, NULL, udp_errqueue_thread_main, conn) != 0) { + return -1; + } + conn->errqueue_thread_started = 1; + return 0; +} + +udp_conn_t *udp_conn_dial(const char *server_addr, const char *bind_ip, const char *bind_device, int enable_timestamping, latency_logger_t *logger, const char *node_role, const char *node_id, tx_timestamp_debug_logger_t *debug_logger) { + struct sockaddr_storage remote_addr; + struct sockaddr_storage local_addr; + socklen_t remote_len; + socklen_t local_len; + int family; + int fd; + udp_conn_t *conn; + + if (omni_parse_sockaddr(server_addr, 0, &remote_addr, &remote_len, &family) != 0) { + return NULL; + } + fd = udp_open_socket_for_addr((struct sockaddr *) &remote_addr, remote_len, bind_device != NULL && bind_device[0] != '\0', bind_device); + if (fd < 0) { + return NULL; + } + if (bind_ip != NULL && bind_ip[0] != '\0') { + if (udp_resolve_ip_only(bind_ip, family, &local_addr, &local_len) != 0 || bind(fd, (struct sockaddr *) &local_addr, local_len) != 0) { + close(fd); + return NULL; + } + } + if (connect(fd, (struct sockaddr *) &remote_addr, remote_len) != 0) { + close(fd); + return NULL; + } + if (enable_timestamping && linux_timestamping_enable_udp_socket(fd, 1) != 0) { + close(fd); + return NULL; + } + conn = udp_conn_alloc(fd, 1, enable_timestamping, logger, node_role, node_id, debug_logger); + if (conn == NULL) { + close(fd); + return NULL; + } + if (udp_conn_start_errqueue(conn) != 0) { + udp_conn_free(conn); + return NULL; + } + return conn; +} + +udp_conn_t *udp_conn_bind(const char *listen_addr, const char *bind_device, int enable_timestamping, latency_logger_t *logger, const char *node_role, const char *node_id, tx_timestamp_debug_logger_t *debug_logger) { + struct sockaddr_storage local_addr; + socklen_t local_len; + int family; + int fd; + udp_conn_t *conn; + if (omni_parse_sockaddr(listen_addr, 1, &local_addr, &local_len, &family) != 0) { + return NULL; + } + fd = udp_open_socket_for_addr((struct sockaddr *) &local_addr, local_len, bind_device != NULL && bind_device[0] != '\0', bind_device); + if (fd < 0) { + return NULL; + } + if (bind(fd, (struct sockaddr *) &local_addr, local_len) != 0) { + close(fd); + return NULL; + } + if (enable_timestamping && linux_timestamping_enable_udp_socket(fd, 1) != 0) { + close(fd); + return NULL; + } + conn = udp_conn_alloc(fd, 0, enable_timestamping, logger, node_role, node_id, debug_logger); + if (conn == NULL) { + close(fd); + return NULL; + } + if (udp_conn_start_errqueue(conn) != 0) { + udp_conn_free(conn); + return NULL; + } + return conn; +} + +static int udp_conn_send_inner(udp_conn_t *conn, const message_t *msg, const struct sockaddr *addr, socklen_t addr_len) { + uint8_t *payload = NULL; + size_t payload_len = 0; + ssize_t rc; + udp_pending_tx_t *pending = NULL; + uint32_t tx_id; + + if (protocol_encode_message_datagram(msg, &payload, &payload_len) != 0) { + return -1; + } + pthread_mutex_lock(&conn->write_mu); + latencylog_log_message_event(conn->logger, conn->node_role, conn->node_id, EVENT_SEND_HANDOFF_BEGIN, msg); + tx_id = conn->next_tx_id++; + if (conn->timestamping_enabled) { + pending = (udp_pending_tx_t *) calloc(1, sizeof(*pending)); + if (pending == NULL || protocol_message_copy(&pending->msg, msg) != 0) { + free(payload); + pthread_mutex_unlock(&conn->write_mu); + free(pending); + return -1; + } + pending->tx_id = tx_id; + pending->bytes_written = (int) payload_len; + pthread_mutex_lock(&conn->pending_mu); + pending->next = conn->pending_head; + conn->pending_head = pending; + pthread_mutex_unlock(&conn->pending_mu); + udp_debug_log_send_chunk(conn, msg, (int) payload_len, tx_id); + } + if (addr != NULL) { + rc = sendto(conn->fd, payload, payload_len, 0, addr, addr_len); + } else { + rc = send(conn->fd, payload, payload_len, 0); + } + free(payload); + if (rc < 0 || (size_t) rc != payload_len) { + if (pending != NULL) { + udp_pending_tx_t *prev = NULL; + udp_pending_tx_t *cur; + pthread_mutex_lock(&conn->pending_mu); + for (cur = conn->pending_head; cur != NULL; cur = cur->next) { + if (cur == pending) { + if (prev == NULL) { + conn->pending_head = cur->next; + } else { + prev->next = cur->next; + } + break; + } + prev = cur; + } + pthread_mutex_unlock(&conn->pending_mu); + protocol_message_clear(&pending->msg); + free(pending); + } + pthread_mutex_unlock(&conn->write_mu); + return -1; + } + latencylog_log_message_event(conn->logger, conn->node_role, conn->node_id, EVENT_SEND_HANDOFF_END, msg); + pthread_mutex_unlock(&conn->write_mu); + return 0; +} + +int udp_conn_send(udp_conn_t *conn, const message_t *msg) { + if (conn == NULL || !conn->connected) { + errno = ENOTCONN; + return -1; + } + return udp_conn_send_inner(conn, msg, NULL, 0); +} + +int udp_conn_send_to(udp_conn_t *conn, const message_t *msg, const struct sockaddr *addr, socklen_t addr_len) { + if (conn == NULL || addr == NULL) { + errno = EINVAL; + return -1; + } + return udp_conn_send_inner(conn, msg, addr, addr_len); +} + +int udp_conn_receive(udp_conn_t *conn, message_t *out_msg, struct sockaddr_storage *addr, socklen_t *addr_len) { + uint8_t control[512]; + struct sockaddr_storage source; + struct iovec iov; + struct msghdr msg; + ssize_t n; + int64_t rx_ts = 0; + char err[128]; + + if (conn == NULL || out_msg == NULL) { + errno = EINVAL; + return -1; + } + memset(&msg, 0, sizeof(msg)); + memset(&source, 0, sizeof(source)); + iov.iov_base = conn->recv_buffer; + iov.iov_len = conn->recv_buffer_cap; + msg.msg_name = &source; + msg.msg_namelen = sizeof(source); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + if (conn->timestamping_enabled) { + msg.msg_control = control; + msg.msg_controllen = sizeof(control); + } + 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) { + rx_ts = linux_timestamping_parse_rx_timestamp(&msg); + } + if (protocol_decode_message_datagram(conn->recv_buffer, (size_t) n, out_msg, err, sizeof(err)) != 0) { + errno = EPROTO; + return -1; + } + if (addr != NULL && addr_len != NULL) { + omni_clone_sockaddr((struct sockaddr *) &source, msg.msg_namelen, addr, addr_len); + } + if (rx_ts > 0) { + latencylog_log_message_event_at(conn->logger, conn->node_role, conn->node_id, EVENT_B_RX_SOFTWARE, rx_ts, out_msg); + } + return 0; +} + +int udp_conn_fd(const udp_conn_t *conn) { + return conn == NULL ? -1 : conn->fd; +} + +int udp_conn_local_addr(const udp_conn_t *conn, struct sockaddr_storage *addr, socklen_t *addr_len) { + socklen_t len = sizeof(*addr); + if (conn == NULL || addr == NULL || addr_len == NULL) { + errno = EINVAL; + return -1; + } + if (getsockname(conn->fd, (struct sockaddr *) addr, &len) != 0) { + return -1; + } + *addr_len = len; + return 0; +} + +int udp_conn_close(udp_conn_t *conn) { + if (conn == NULL) { + return 0; + } + 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); + conn->errqueue_thread_started = 0; + } + } + return 0; +} + +void udp_conn_free(udp_conn_t *conn) { + if (conn == NULL) { + return; + } + udp_conn_close(conn); + udp_pending_destroy(conn->pending_head); + free(conn->recv_buffer); + pthread_mutex_destroy(&conn->write_mu); + pthread_mutex_destroy(&conn->pending_mu); + free(conn); +} diff --git a/robot/v4l2/OmniSocketGo_robot/src/tx_timestamp_debug.c b/robot/v4l2/OmniSocketGo_robot/src/tx_timestamp_debug.c new file mode 100644 index 0000000..3cdf19c --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/src/tx_timestamp_debug.c @@ -0,0 +1,108 @@ +#include "tx_timestamp_debug.h" + +tx_timestamp_debug_logger_t *tx_timestamp_debug_open_jsonl(const char *path) { + tx_timestamp_debug_logger_t *logger; + FILE *file; + if (path == NULL || path[0] == '\0') { + return NULL; + } + if (omni_ensure_parent_dir(path) != 0) { + return NULL; + } + file = fopen(path, "ab"); + if (file == NULL) { + return NULL; + } + logger = (tx_timestamp_debug_logger_t *) calloc(1, sizeof(*logger)); + if (logger == NULL) { + fclose(file); + return NULL; + } + omni_file_logger_init_path(&logger->file_logger, file, path, 0); + logger->enabled = 1; + return logger; +} + +void tx_timestamp_debug_close(tx_timestamp_debug_logger_t *logger) { + if (logger == NULL) { + return; + } + if (logger->file_logger.file != NULL) { + fclose(logger->file_logger.file); + } + omni_file_logger_destroy(&logger->file_logger); + free(logger); +} + +int tx_timestamp_debug_log(tx_timestamp_debug_logger_t *logger, const tx_timestamp_debug_record_t *record) { + char *line; + char *node_role; + char *node_id; + char *from; + char *to; + char *file_name; + char *phase; + char *event_name; + + if (logger == NULL || record == NULL || !logger->enabled) { + return 0; + } + node_role = omni_json_escape(record->node_role); + node_id = omni_json_escape(record->node_id); + from = omni_json_escape(record->from); + to = omni_json_escape(record->to); + file_name = omni_json_escape(record->file_name); + phase = omni_json_escape(record->phase); + event_name = omni_json_escape(record->event_name); + if (node_role == NULL || node_id == NULL || from == NULL || to == NULL || file_name == NULL || phase == NULL || event_name == NULL) { + free(node_role); + free(node_id); + free(from); + free(to); + free(file_name); + free(phase); + free(event_name); + return -1; + } + line = omni_strdup_printf( + "{\"record_type\":\"%s\",\"node_role\":\"%s\",\"node_id\":\"%s\",\"message_type\":\"%s\",\"message_id\":%" PRIu64 ",\"from\":\"%s\",\"to\":\"%s\",\"file_name\":\"%s\",\"body_size\":%d,\"phase\":\"%s\",\"send_call_index\":%d,\"frame_offset_start\":%d,\"frame_offset_end\":%d,\"bytes_written\":%d,\"expected_tx_id\":%u,\"read_index\":%d,\"event_name\":\"%s\",\"ts_unix_nano\":%" PRId64 ",\"ee_info\":%u,\"ee_data\":%u,\"matched_send_call_index\":%d,\"selected_for_latency\":%d}", + record->record_type, + node_role, + node_id, + protocol_message_type_name(record->message_type), + record->message_id, + from, + to, + file_name, + record->body_size, + phase, + record->send_call_index, + record->frame_offset_start, + record->frame_offset_end, + record->bytes_written, + record->expected_tx_id, + record->read_index, + event_name, + record->ts_unix_nano, + record->ee_info, + record->ee_data, + record->matched_send_call_index, + record->selected_for_latency + ); + free(node_role); + free(node_id); + free(from); + free(to); + free(file_name); + free(phase); + free(event_name); + if (line == NULL) { + return -1; + } + if (omni_file_logger_write_line(&logger->file_logger, line) != 0) { + free(line); + return -1; + } + free(line); + return 0; +} diff --git a/robot/v4l2/OmniSocketGo_robot/src/video_pipeline.c b/robot/v4l2/OmniSocketGo_robot/src/video_pipeline.c new file mode 100644 index 0000000..026ed3c --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/src/video_pipeline.c @@ -0,0 +1,1497 @@ +#include "video_pipeline.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#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_HEAD_CAMERA_DEVICE "/dev/video26" +#define VIDEO_DEFAULT_WAIST_CAMERA_DEVICE "/dev/video18" +#define VIDEO_DEFAULT_PEER_ID "peer-b-video" +#define VIDEO_DEFAULT_TARGET_PEER "peer-a-video" +#define VIDEO_SOFT_BACKPRESSURE_SEGMENTS_DEFAULT 64 +#define VIDEO_HARD_BACKPRESSURE_SEGMENTS_DEFAULT 192 +#define VIDEO_HARD_BACKPRESSURE_HOLD_MS_DEFAULT 1000 +#define VIDEO_DEFAULT_FRAME_STALL_RECONNECT_MS 3000 +#define VIDEO_SOFT_BACKPRESSURE_WINDOW_PRESSURE_PCT 90.0 +#define VIDEO_HARD_BACKPRESSURE_WINDOW_PRESSURE_PCT 98.0 +#define VIDEO_SESSION_POLL_INTERVAL_MS 250 + +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; + uint64_t next_frame_seq; +} 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 int env_int_or_default(const char *name, int fallback) { + const char *value = getenv(name); + int parsed; + + if (value == NULL || value[0] == '\0') { + return fallback; + } + parsed = atoi(value); + if (parsed <= 0) { + return fallback; + } + return parsed; +} + +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); +} + +static void video_pipeline_report_progress(const video_pipeline_config_t *config) { + if (config == NULL || config->progress_callback == NULL) { + return; + } + config->progress_callback(config->progress_context); +} + +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->camera_head_device = VIDEO_DEFAULT_HEAD_CAMERA_DEVICE; + config->camera_waist_device = VIDEO_DEFAULT_WAIST_CAMERA_DEVICE; + config->active_camera = NULL; + 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; + config->soft_backpressure_segments = VIDEO_SOFT_BACKPRESSURE_SEGMENTS_DEFAULT; + config->hard_backpressure_segments = VIDEO_HARD_BACKPRESSURE_SEGMENTS_DEFAULT; + config->hard_backpressure_hold_ms = VIDEO_HARD_BACKPRESSURE_HOLD_MS_DEFAULT; + config->frame_stall_reconnect_ms = VIDEO_DEFAULT_FRAME_STALL_RECONNECT_MS; + config->stats_logger = NULL; + config->stage_logger = NULL; + config->stats_interval_ms = 1000; +} + +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->camera_head_device = env_or_default("OMNI_CAMERA_HEAD_DEVICE", config->camera_head_device); + config->camera_waist_device = env_or_default("OMNI_CAMERA_WAIST_DEVICE", config->camera_waist_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); + config->soft_backpressure_segments = env_int_or_default("OMNI_VIDEO_SOFT_BACKPRESSURE_SEGMENTS", config->soft_backpressure_segments); + config->hard_backpressure_segments = env_int_or_default("OMNI_VIDEO_HARD_BACKPRESSURE_SEGMENTS", config->hard_backpressure_segments); + config->hard_backpressure_hold_ms = env_int_or_default("OMNI_VIDEO_HARD_BACKPRESSURE_HOLD_MS", config->hard_backpressure_hold_ms); + config->frame_stall_reconnect_ms = env_int_or_default("OMNI_VIDEO_FRAME_STALL_RECONNECT_MS", config->frame_stall_reconnect_ms); + config->stats_interval_ms = env_int_or_default("BLITZ_KCP_STATS_INTERVAL_MS", config->stats_interval_ms); +} + +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->backpressure_drops = stats->backpressure_drops; + out_stats->backlog_resets = stats->backlog_resets; + out_stats->last_frame_bytes = stats->last_frame_bytes; + out_stats->last_backlog_segments = stats->last_backlog_segments; + out_stats->last_capture_to_send_ms = stats->last_capture_to_send_ms; + out_stats->avg_capture_to_send_ms = stats->avg_capture_to_send_ms; + out_stats->connected = stats->connected; + snprintf(out_stats->last_error, sizeof(out_stats->last_error), "%s", stats->last_error); + snprintf(out_stats->last_backlog_reason, sizeof(out_stats->last_backlog_reason), "%s", stats->last_backlog_reason); + 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, + config->stats_logger, + config->stats_interval_ms + ); + if (sender->client == NULL) { + return -1; + } + return 0; +} + +static int video_sender_drain_pending_messages(video_sender_t *sender) { + int drained = 0; + + if (sender == NULL || sender->client == NULL) { + errno = EINVAL; + return -1; + } + + for (;;) { + message_t msg; + int rc; + + protocol_message_init(&msg); + rc = kcp_client_receive_timed(sender->client, &msg, 1); + if (rc == 1) { + protocol_message_clear(&msg); + return 0; + } + if (rc != 0) { + protocol_message_clear(&msg); + return -1; + } + + // Drain unread server errors so an offline receiver cannot back up the reverse KCP stream. + protocol_message_clear(&msg); + drained += 1; + if (drained >= 8) { + return 0; + } + } +} + +static int video_sender_send_packet( + video_sender_t *sender, + const AVPacket *encoded_pkt, + const video_pipeline_packet_metadata_t *metadata, + uint64_t *out_frame_seq +) { + uint8_t *payload; + size_t payload_len; + uint64_t frame_seq; + int rc; + + if (sender == NULL || sender->client == NULL || encoded_pkt == NULL || metadata == NULL) { + errno = EINVAL; + return -1; + } + + frame_seq = sender->next_frame_seq + 1U; + payload_len = 8U + (size_t) encoded_pkt->size + sizeof(*metadata); + if (video_sender_ensure_buffer_capacity(sender, payload_len) != 0) { + return -1; + } + payload = sender->send_buffer; + + payload[0] = (uint8_t) (frame_seq >> 56); + payload[1] = (uint8_t) (frame_seq >> 48); + payload[2] = (uint8_t) (frame_seq >> 40); + payload[3] = (uint8_t) (frame_seq >> 32); + payload[4] = (uint8_t) (frame_seq >> 24); + payload[5] = (uint8_t) (frame_seq >> 16); + payload[6] = (uint8_t) (frame_seq >> 8); + payload[7] = (uint8_t) frame_seq; + memcpy(payload + 8U, encoded_pkt->data, (size_t) encoded_pkt->size); + memcpy(payload + 8U + (size_t) encoded_pkt->size, metadata, sizeof(*metadata)); + rc = kcp_client_send_binary(sender->client, sender->target_peer, payload, payload_len); + if (rc != 0) { + return rc; + } + sender->next_frame_seq = frame_seq; + if (out_frame_seq != NULL) { + *out_frame_seq = frame_seq; + } + rc = video_sender_drain_pending_messages(sender); + 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 uint32_t video_sender_backlog_segments(const kcp_runtime_stats_t *stats) { + if (stats == NULL) { + return 0; + } + return stats->snd_queue + stats->snd_buffer; +} + +static int video_sender_soft_backpressure_active(const video_pipeline_config_t *config, const kcp_runtime_stats_t *transport) { + if (config == NULL || transport == NULL) { + return 0; + } + return video_sender_backlog_segments(transport) >= (uint32_t) config->soft_backpressure_segments + || transport->window_pressure_pct >= VIDEO_SOFT_BACKPRESSURE_WINDOW_PRESSURE_PCT; +} + +static int video_sender_hard_backpressure_active(const video_pipeline_config_t *config, const kcp_runtime_stats_t *transport) { + if (config == NULL || transport == NULL) { + return 0; + } + return video_sender_backlog_segments(transport) >= (uint32_t) config->hard_backpressure_segments + || transport->window_pressure_pct >= VIDEO_HARD_BACKPRESSURE_WINDOW_PRESSURE_PCT; +} + +static void video_pipeline_note_backpressure( + video_pipeline_stats_t *stats, + const char *reason, + const kcp_runtime_stats_t *transport, + int increment_drop, + int increment_reset +) { + if (stats == NULL) { + return; + } + pthread_mutex_lock(&stats->mutex); + if (increment_drop) { + stats->backpressure_drops += 1; + } + if (increment_reset) { + stats->backlog_resets += 1; + } + if (transport != NULL) { + stats->last_backlog_segments = video_sender_backlog_segments(transport); + stats->transport = *transport; + } else { + stats->last_backlog_segments = 0; + } + snprintf(stats->last_backlog_reason, sizeof(stats->last_backlog_reason), "%s", reason == NULL ? "" : reason); + pthread_mutex_unlock(&stats->mutex); +} + +static void video_pipeline_note_capture_to_send(video_pipeline_stats_t *stats, uint32_t capture_to_send_ms) { + if (stats == NULL) { + return; + } + pthread_mutex_lock(&stats->mutex); + stats->last_capture_to_send_ms = capture_to_send_ms; + if (stats->avg_capture_to_send_ms <= 0.0) { + stats->avg_capture_to_send_ms = (double) capture_to_send_ms; + } else { + stats->avg_capture_to_send_ms = stats->avg_capture_to_send_ms * 0.9 + (double) capture_to_send_ms * 0.1; + } + pthread_mutex_unlock(&stats->mutex); +} + +static int video_stage_logger_should_log(const video_stage_logger_t *logger, uint64_t frame_seq) { + if (logger == NULL || !logger->enabled) { + return 0; + } + if (logger->sample_mod <= 1U) { + return 1; + } + return frame_seq % logger->sample_mod == 0U; +} + +static void video_stage_logger_log_frame( + video_stage_logger_t *logger, + uint64_t frame_seq, + double capture_ms, + double decode_ms, + double scale_ms, + double encode_ms, + double send_ms, + double pipeline_total_ms, + size_t jpeg_bytes, + uint64_t kcp_out_seg_delta, + uint32_t backlog_segments, + double window_pressure_pct, + int32_t video_srtt_ms +) { + char *line; + + if (!video_stage_logger_should_log(logger, frame_seq)) { + return; + } + line = omni_strdup_printf( + "{\"ts_unix_nano\":%" PRId64 ",\"frame_seq\":%" PRIu64 ",\"capture_ms\":%.3f,\"decode_ms\":%.3f,\"scale_ms\":%.3f,\"encode_ms\":%.3f,\"send_ms\":%.3f,\"pipeline_total_ms\":%.3f,\"jpeg_bytes\":%zu,\"kcp_out_seg_delta\":%" PRIu64 ",\"backlog_segments\":%u,\"window_pressure_pct\":%.3f,\"video_srtt_ms\":%d}", + omni_now_unix_nano(), + frame_seq, + capture_ms, + decode_ms, + scale_ms, + encode_ms, + send_ms, + pipeline_total_ms, + jpeg_bytes, + kcp_out_seg_delta, + backlog_segments, + window_pressure_pct, + video_srtt_ms + ); + if (line == NULL) { + return; + } + (void) omni_file_logger_write_line(&logger->file_logger, line); + free(line); +} + +video_stage_logger_t *video_stage_logger_open_jsonl(const char *path, uint64_t sample_mod) { + video_stage_logger_t *logger; + FILE *file; + + if (path == NULL || path[0] == '\0') { + return NULL; + } + if (omni_ensure_parent_dir(path) != 0) { + return NULL; + } + file = fopen(path, "ab"); + if (file == NULL) { + return NULL; + } + logger = (video_stage_logger_t *) calloc(1, sizeof(*logger)); + if (logger == NULL) { + fclose(file); + return NULL; + } + omni_file_logger_init_path(&logger->file_logger, file, path, 0); + logger->enabled = 1; + logger->sample_mod = sample_mod == 0U ? 1U : sample_mod; + return logger; +} + +void video_stage_logger_close(video_stage_logger_t *logger) { + if (logger == NULL) { + return; + } + if (logger->file_logger.file != NULL) { + fclose(logger->file_logger.file); + } + omni_file_logger_destroy(&logger->file_logger); + free(logger); +} + +static int video_server_error_requires_reconnect(const char *message) { + if (message == NULL || message[0] == '\0') { + return 0; + } + return strstr(message, "not registered") != NULL + || strstr(message, "first message must be register") != NULL + || strstr(message, "peer replaced") != NULL + || strstr(message, "timed out waiting for server_register_ok") != NULL + || strstr(message, "failed to acknowledge server heartbeat") != NULL; +} + +static void video_pipeline_update_connection_state( + video_pipeline_stats_t *stats, + const kcp_client_state_t *client_state, + const kcp_runtime_stats_t *transport +) { + if (stats == NULL) { + return; + } + + pthread_mutex_lock(&stats->mutex); + if (transport != NULL) { + stats->transport = *transport; + } + if (client_state != NULL) { + stats->connected = client_state->connected != 0 && client_state->registered != 0; + if (client_state->last_server_error[0] != '\0') { + snprintf(stats->last_error, sizeof(stats->last_error), "%s", client_state->last_server_error); + } + } + pthread_mutex_unlock(&stats->mutex); +} + +static int video_sender_check_session_stale( + video_sender_t *sender, + const video_pipeline_config_t *config, + video_pipeline_stats_t *stats, + kcp_runtime_stats_t *transport_stats, + char *reason, + size_t reason_len +) { + kcp_client_state_t client_state; + + if ( + sender == NULL || sender->client == NULL || config == NULL || stats == NULL || transport_stats == NULL + || reason == NULL || reason_len == 0 + ) { + errno = EINVAL; + return -1; + } + + reason[0] = '\0'; + memset(&client_state, 0, sizeof(client_state)); + kcp_client_runtime_stats_snapshot(sender->client, transport_stats); + kcp_client_state_snapshot(sender->client, &client_state); + video_pipeline_update_connection_state(stats, &client_state, transport_stats); + + if (!transport_stats->connected || !client_state.connected) { + snprintf(reason, reason_len, "video session stale: transport disconnected"); + return 1; + } + if (!client_state.registered) { + snprintf(reason, reason_len, "video session stale: server reported unregistered"); + return 1; + } + if (video_server_error_requires_reconnect(client_state.last_server_error)) { + snprintf(reason, reason_len, "video session stale: server error %.180s", client_state.last_server_error); + return 1; + } + return 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); +} + +typedef struct video_camera_source { + const char *name; + const char *device; + int fd; + video_buffer_t *buffers; + int num_buffers; + int streaming; +} video_camera_source_t; + +static void video_camera_source_cleanup(video_camera_source_t *source) { + enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + + if (source == NULL) { + return; + } + if (source->fd >= 0 && source->streaming) { + (void) ioctl(source->fd, VIDIOC_STREAMOFF, &type); + } + video_pipeline_cleanup_buffers(source->buffers, source->num_buffers); + if (source->fd >= 0) { + close(source->fd); + } + source->fd = -1; + source->buffers = NULL; + source->num_buffers = 0; + source->streaming = 0; +} + +static int video_camera_source_start(video_camera_source_t *source, int width, int height) { + enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + int i; + + source->fd = open_v4l2_device(source->device); + if (source->fd < 0 || init_v4l2_device(source->fd, width, height) < 0 + || init_mmap(source->fd, &source->buffers, &source->num_buffers) < 0) { + return -1; + } + for (i = 0; i < source->num_buffers; 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(source->fd, VIDIOC_QBUF, &buf) < 0) { + return -1; + } + } + if (ioctl(source->fd, VIDIOC_STREAMON, &type) < 0) { + return -1; + } + source->streaming = 1; + fprintf(stderr, "[video_pipeline] camera %s ready on %s\n", source->name, source->device); + return 0; +} + +static void video_camera_source_discard_ready(video_camera_source_t *source) { + struct v4l2_buffer buf; + + if (source == NULL || source->fd < 0) { + return; + } + memset(&buf, 0, sizeof(buf)); + buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + buf.memory = V4L2_MEMORY_MMAP; + if (ioctl(source->fd, VIDIOC_DQBUF, &buf) == 0) { + (void) ioctl(source->fd, VIDIOC_QBUF, &buf); + } +} + +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_camera_source_t cameras[2] = { + {.name = "head", .fd = -1}, + {.name = "waist", .fd = -1} + }; + AVCodecContext *decoder = NULL; + AVCodecContext *encoder = NULL; + struct SwsContext *sws_ctx = NULL; + int frame_index = 0; + int rc = -1; + int sws_src_width = 0; + int sws_src_height = 0; + int sws_src_format = -1; + uint32_t hard_backpressure_since_ms = 0; + uint32_t last_soft_drop_log_ms = 0; + uint32_t last_session_poll_ms = 0; + uint32_t last_successful_send_ms = 0; + uint64_t soft_drops_since_last_send = 0; + int have_sent_frame = 0; + const char *gpsd_host = env_or_default("OMNI_GPSD_HOST", "127.0.0.1"); + int gps_buffer_started = 0; + + 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; + } + + cameras[VIDEO_CAMERA_HEAD].device = config->active_camera == NULL + ? config->camera_device + : config->camera_head_device; + cameras[VIDEO_CAMERA_WAIST].device = config->camera_waist_device; + if (video_camera_source_start(&cameras[VIDEO_CAMERA_HEAD], config->capture_width, config->capture_height) < 0) { + video_pipeline_set_errno_error(stats, "failed to start head camera"); + goto cleanup; + } + if (config->active_camera != NULL + && video_camera_source_start(&cameras[VIDEO_CAMERA_WAIST], config->capture_width, config->capture_height) < 0) { + video_pipeline_set_errno_error(stats, "failed to start waist camera"); + 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; + } + if (gps_buffer_init(gpsd_host) != 0) { + fprintf(stderr, "[video_pipeline] failed to start GPS buffer using %s:2947\n", gpsd_host); + } else { + gps_buffer_started = 1; + } + + pthread_mutex_lock(&stats->mutex); + stats->connected = 1; + stats->last_error[0] = '\0'; + pthread_mutex_unlock(&stats->mutex); + + 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; + kcp_runtime_stats_t transport_stats; + kcp_runtime_stats_t transport_after_send; + int select_rc; + int should_log_stage = 0; + 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; + video_pipeline_packet_metadata_t packet_metadata; + char reconnect_reason[256]; + int frame_number = frame_index + 1; + uint64_t frame_seq = 0; + uint64_t out_segs_before_send = 0; + uint64_t out_segs_after_send = 0; + uint32_t capture_to_send_ms = 0; + int active_camera = config->active_camera == NULL + ? VIDEO_CAMERA_HEAD + : atomic_load(config->active_camera); + video_camera_source_t *active_source; + video_camera_source_t *standby_source; + + if (active_camera != VIDEO_CAMERA_WAIST) { + active_camera = VIDEO_CAMERA_HEAD; + } + active_source = &cameras[active_camera]; + standby_source = config->active_camera == NULL + ? NULL + : &cameras[active_camera == VIDEO_CAMERA_HEAD ? VIDEO_CAMERA_WAIST : VIDEO_CAMERA_HEAD]; + + memset(&transport_stats, 0, sizeof(transport_stats)); + memset(&transport_after_send, 0, sizeof(transport_after_send)); + memset(&packet_metadata, 0, sizeof(packet_metadata)); + reconnect_reason[0] = '\0'; + video_pipeline_report_progress(config); + + if (config->max_frames > 0 && frame_index >= config->max_frames) { + break; + } + total_start_ms = video_pipeline_now_ms(); + + FD_ZERO(&fds); + FD_SET(cameras[VIDEO_CAMERA_HEAD].fd, &fds); + if (cameras[VIDEO_CAMERA_WAIST].fd >= 0) { + FD_SET(cameras[VIDEO_CAMERA_WAIST].fd, &fds); + } + timeout.tv_sec = 2; + timeout.tv_usec = 0; + select_rc = select( + (cameras[VIDEO_CAMERA_HEAD].fd > cameras[VIDEO_CAMERA_WAIST].fd + ? cameras[VIDEO_CAMERA_HEAD].fd + : cameras[VIDEO_CAMERA_WAIST].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 (standby_source != NULL && FD_ISSET(standby_source->fd, &fds)) { + video_camera_source_discard_ready(standby_source); + } + if (!FD_ISSET(active_source->fd, &fds)) { + continue; + } + 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(active_source->fd, VIDIOC_DQBUF, &buf) < 0) { + video_pipeline_set_errno_error(stats, "failed to dequeue V4L2 buffer"); + goto cleanup; + } + capture_end_ms = video_pipeline_now_ms(); + decode_start_ms = capture_end_ms; + + if (decode_mjpeg_frame(decoder, (const uint8_t *) active_source->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(active_source->fd, VIDIOC_QBUF, &buf); + continue; + } + 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(active_source->fd, VIDIOC_QBUF, &buf); + continue; + } + 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(active_source->fd, VIDIOC_QBUF, &buf); + continue; + } + encode_end_ms = video_pipeline_now_ms(); + send_start_ms = encode_end_ms; + + { + gps_video_sample_t gps_sample = get_latest_gps_for_video(); + + packet_metadata.timestamp_ms = (uint64_t) get_realtime_ms(); + packet_metadata.latitude = gps_sample.latitude; + packet_metadata.longitude = gps_sample.longitude; + } + + if ( + last_session_poll_ms == 0 + || omni_now_millis32() - last_session_poll_ms >= VIDEO_SESSION_POLL_INTERVAL_MS + ) { + if (video_sender_drain_pending_messages(&sender) != 0) { + video_pipeline_set_errno_error(stats, "failed to poll video session"); + av_frame_free(&decoded_frame); + av_frame_free(&scaled_frame); + av_packet_free(&encoded_pkt); + (void) ioctl(active_source->fd, VIDIOC_QBUF, &buf); + rc = VIDEO_PIPELINE_RUN_RETRY_IMMEDIATE; + goto cleanup; + } + if ( + video_sender_check_session_stale( + &sender, + config, + stats, + &transport_stats, + reconnect_reason, + sizeof(reconnect_reason) + ) != 0 + ) { + if (reconnect_reason[0] == '\0') { + snprintf(reconnect_reason, sizeof(reconnect_reason), "video session stale: poll failed"); + } + video_pipeline_set_error(stats, reconnect_reason); + fprintf(stderr, "[video_pipeline] %s\n", reconnect_reason); + av_frame_free(&decoded_frame); + av_frame_free(&scaled_frame); + av_packet_free(&encoded_pkt); + (void) ioctl(active_source->fd, VIDIOC_QBUF, &buf); + rc = VIDEO_PIPELINE_RUN_RETRY_IMMEDIATE; + goto cleanup; + } + last_session_poll_ms = omni_now_millis32(); + } else { + kcp_client_runtime_stats_snapshot(sender.client, &transport_stats); + } + if (video_sender_hard_backpressure_active(config, &transport_stats)) { + uint32_t now_ms = omni_now_millis32(); + + if (hard_backpressure_since_ms == 0) { + hard_backpressure_since_ms = now_ms; + } + if (now_ms - hard_backpressure_since_ms >= (uint32_t) config->hard_backpressure_hold_ms) { + char reason[128]; + uint32_t backlog_segments = video_sender_backlog_segments(&transport_stats); + + snprintf( + reason, + sizeof(reason), + "hard_reset backlog=%u snd_queue=%u snd_buffer=%u window_pressure=%.1f%% hold_ms=%d", + backlog_segments, + transport_stats.snd_queue, + transport_stats.snd_buffer, + transport_stats.window_pressure_pct, + config->hard_backpressure_hold_ms + ); + video_pipeline_note_backpressure(stats, reason, &transport_stats, 0, 1); + video_pipeline_set_error(stats, reason); + fprintf( + stderr, + "[video_pipeline] backlog hard reset: backlog=%u snd_queue=%u snd_buffer=%u window_pressure=%.1f%% hold_ms=%d\n", + backlog_segments, + transport_stats.snd_queue, + transport_stats.snd_buffer, + transport_stats.window_pressure_pct, + config->hard_backpressure_hold_ms + ); + av_frame_free(&decoded_frame); + av_frame_free(&scaled_frame); + av_packet_free(&encoded_pkt); + (void) ioctl(active_source->fd, VIDIOC_QBUF, &buf); + rc = VIDEO_PIPELINE_RUN_RETRY_IMMEDIATE; + goto cleanup; + } + } else { + hard_backpressure_since_ms = 0; + } + + if (video_sender_soft_backpressure_active(config, &transport_stats)) { + uint32_t now_ms = omni_now_millis32(); + uint32_t backlog_segments = video_sender_backlog_segments(&transport_stats); + char reason[128]; + + snprintf( + reason, + sizeof(reason), + "soft_drop backlog=%u snd_queue=%u snd_buffer=%u window_pressure=%.1f%% threshold=%d", + backlog_segments, + transport_stats.snd_queue, + transport_stats.snd_buffer, + transport_stats.window_pressure_pct, + config->soft_backpressure_segments + ); + video_pipeline_note_backpressure(stats, reason, &transport_stats, 1, 0); + soft_drops_since_last_send += 1; + if (now_ms - last_soft_drop_log_ms >= 1000U) { + fprintf( + stderr, + "[video_pipeline] soft drop: backlog=%u snd_queue=%u snd_buffer=%u window_pressure=%.1f%% threshold=%d\n", + backlog_segments, + transport_stats.snd_queue, + transport_stats.snd_buffer, + transport_stats.window_pressure_pct, + config->soft_backpressure_segments + ); + last_soft_drop_log_ms = now_ms; + } + if ( + have_sent_frame + && config->frame_stall_reconnect_ms > 0 + && now_ms - last_successful_send_ms >= (uint32_t) config->frame_stall_reconnect_ms + ) { + char stall_reason[192]; + + snprintf( + stall_reason, + sizeof(stall_reason), + "video pipeline stalled: no frames sent for %u ms while soft dropping (%llu drops, backlog=%u, srtt=%d ms)", + now_ms - last_successful_send_ms, + (unsigned long long) soft_drops_since_last_send, + backlog_segments, + transport_stats.srtt_ms + ); + video_pipeline_set_error(stats, stall_reason); + fprintf(stderr, "[video_pipeline] %s\n", stall_reason); + av_frame_free(&decoded_frame); + av_frame_free(&scaled_frame); + av_packet_free(&encoded_pkt); + (void) ioctl(active_source->fd, VIDIOC_QBUF, &buf); + rc = VIDEO_PIPELINE_RUN_RETRY_IMMEDIATE; + goto cleanup; + } + av_frame_free(&decoded_frame); + av_frame_free(&scaled_frame); + av_packet_free(&encoded_pkt); + (void) ioctl(active_source->fd, VIDIOC_QBUF, &buf); + continue; + } + + capture_to_send_ms = send_start_ms <= capture_start_ms + ? 0U + : (uint32_t) (send_start_ms - capture_start_ms + 0.5); + packet_metadata.capture_to_send_ms = capture_to_send_ms; + out_segs_before_send = transport_stats.out_segs_total; + + if (video_sender_send_packet(&sender, encoded_pkt, &packet_metadata, &frame_seq) != 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(active_source->fd, VIDIOC_QBUF, &buf); + goto cleanup; + } + send_end_ms = video_pipeline_now_ms(); + should_log_stage = video_stage_logger_should_log(config->stage_logger, frame_seq); + if (should_log_stage) { + kcp_client_runtime_stats_snapshot(sender.client, &transport_after_send); + out_segs_after_send = transport_after_send.out_segs_total; + } else { + transport_after_send = transport_stats; + out_segs_after_send = out_segs_before_send; + } + video_pipeline_note_capture_to_send(stats, capture_to_send_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; + stats->transport = transport_after_send; + pthread_mutex_unlock(&stats->mutex); + have_sent_frame = 1; + last_successful_send_ms = omni_now_millis32(); + soft_drops_since_last_send = 0; + if (should_log_stage) { + video_stage_logger_log_frame( + config->stage_logger, + frame_seq, + 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, + (size_t) encoded_pkt->size, + out_segs_after_send >= out_segs_before_send ? out_segs_after_send - out_segs_before_send : 0U, + video_sender_backlog_segments(&transport_after_send), + transport_after_send.window_pressure_pct, + transport_after_send.srtt_ms + ); + } + 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(active_source->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 (gps_buffer_started) { + gps_buffer_cleanup(); + } + video_sender_close(&sender); + if (encoder != NULL) { + avcodec_free_context(&encoder); + } + if (decoder != NULL) { + avcodec_free_context(&decoder); + } + sws_freeContext(sws_ctx); + video_camera_source_cleanup(&cameras[VIDEO_CAMERA_HEAD]); + video_camera_source_cleanup(&cameras[VIDEO_CAMERA_WAIST]); + return rc; +} diff --git a/robot/v4l2/OmniSocketGo_robot/src/video_pipeline_gps.c b/robot/v4l2/OmniSocketGo_robot/src/video_pipeline_gps.c new file mode 100644 index 0000000..e60d6ef --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/src/video_pipeline_gps.c @@ -0,0 +1,925 @@ +#include "video_pipeline.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#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); +} + +static void video_pipeline_report_progress(const video_pipeline_config_t *config) { + if (config == NULL || config->progress_callback == NULL) { + return; + } + config->progress_callback(config->progress_context); +} + +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; + config->stats_logger = NULL; + config->stats_interval_ms = 1000; +} + +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); + config->stats_interval_ms = env_int_or_default("BLITZ_KCP_STATS_INTERVAL_MS", config->stats_interval_ms); +} + +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, + config->stats_logger, + config->stats_interval_ms + ); + if (sender->client == NULL) { + return -1; + } + return 0; +} + +static int video_sender_drain_pending_messages(video_sender_t *sender) { + if (sender == NULL || sender->client == NULL) { + errno = EINVAL; + return -1; + } + + for (;;) { + message_t msg; + int rc; + + protocol_message_init(&msg); + rc = kcp_client_receive_timed(sender->client, &msg, 1); + if (rc == 1) { + protocol_message_clear(&msg); + return 0; + } + if (rc != 0) { + protocol_message_clear(&msg); + return -1; + } + + // Drain unread server errors so an offline receiver cannot back up the reverse KCP stream. + protocol_message_clear(&msg); + } +} + +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, ×tamp, sizeof(timestamp)); + rc = kcp_client_send_binary(sender->client, sender->target_peer, payload, payload_len); + if (rc != 0) { + return rc; + } + rc = video_sender_drain_pending_messages(sender); + 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; + + video_pipeline_report_progress(config); + + 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; +} diff --git a/robot/v4l2/OmniSocketGo_robot/start-robot-lan.sh b/robot/v4l2/OmniSocketGo_robot/start-robot-lan.sh new file mode 100644 index 0000000..5f32aae --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/start-robot-lan.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env bash +set -euo pipefail + +PROJECT_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +cd "${PROJECT_ROOT}" + +if [[ ! -x bin/b_side_omnid ]]; then + echo "[start-robot-lan] building bin/b_side_omnid" >&2 + make b_side_omnid +fi + +exec bash scripts/dev/start-b-side-omnid.sh diff --git a/robot/v4l2/OmniSocketGo_robot/third_party/cjson/cJSON.c b/robot/v4l2/OmniSocketGo_robot/third_party/cjson/cJSON.c new file mode 100644 index 0000000..702ea61 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/third_party/cjson/cJSON.c @@ -0,0 +1,3302 @@ +/* + Copyright (c) 2009-2017 Dave Gamble and cJSON contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +/* cJSON */ +/* JSON parser in C. */ + +/* disable warnings about old C89 functions in MSVC */ +#if !defined(_CRT_SECURE_NO_DEPRECATE) && defined(_MSC_VER) +#define _CRT_SECURE_NO_DEPRECATE +#endif + +#ifdef __GNUC__ +#pragma GCC visibility push(default) +#endif +#if defined(_MSC_VER) +#pragma warning(push) +/* disable warning about single line comments in system headers */ +#pragma warning(disable : 4001) +#endif + +#include +#include +#include +#include +#include +#include +#include + +#ifdef ENABLE_LOCALES +#include +#endif + +#if defined(_MSC_VER) +#pragma warning(pop) +#endif +#ifdef __GNUC__ +#pragma GCC visibility pop +#endif + +#include "cJSON.h" + +/* define our own boolean type */ +#ifdef true +#undef true +#endif +#define true ((cJSON_bool)1) + +#ifdef false +#undef false +#endif +#define false ((cJSON_bool)0) + +/* define isnan and isinf for ANSI C, if in C99 or above, isnan and isinf has been defined in math.h */ +#ifndef isinf +#define isinf(d) (isnan((d - d)) && !isnan(d)) +#endif +#ifndef isnan +#define isnan(d) (d != d) +#endif + +#ifndef NAN +#ifdef _WIN32 +#define NAN sqrt(-1.0) +#else +#define NAN 0.0 / 0.0 +#endif +#endif + +typedef struct +{ + const unsigned char *json; + size_t position; +} error; +static error global_error = {NULL, 0}; + +CJSON_PUBLIC(const char *) +cJSON_GetErrorPtr(void) +{ + return (const char *)(global_error.json + global_error.position); +} + +CJSON_PUBLIC(char *) +cJSON_GetStringValue(const cJSON *const item) +{ + if (!cJSON_IsString(item)) + { + return NULL; + } + + return item->valuestring; +} + +CJSON_PUBLIC(double) +cJSON_GetNumberValue(const cJSON *const item) +{ + if (!cJSON_IsNumber(item)) + { + return (double)NAN; + } + + return item->valuedouble; +} + +/* This is a safeguard to prevent copy-pasters from using incompatible C and header files */ +#if (CJSON_VERSION_MAJOR != 1) || (CJSON_VERSION_MINOR != 7) || (CJSON_VERSION_PATCH != 19) +#error cJSON.h and cJSON.c have different versions. Make sure that both have the same. +#endif + +CJSON_PUBLIC(const char *) +cJSON_Version(void) +{ + static char version[15]; + sprintf(version, "%i.%i.%i", CJSON_VERSION_MAJOR, CJSON_VERSION_MINOR, CJSON_VERSION_PATCH); + + return version; +} + +/* Case insensitive string comparison, doesn't consider two NULL pointers equal though */ +static int case_insensitive_strcmp(const unsigned char *string1, const unsigned char *string2) +{ + if ((string1 == NULL) || (string2 == NULL)) + { + return 1; + } + + if (string1 == string2) + { + return 0; + } + + for (; tolower(*string1) == tolower(*string2); (void)string1++, string2++) + { + if (*string1 == '\0') + { + return 0; + } + } + + return tolower(*string1) - tolower(*string2); +} + +typedef struct internal_hooks +{ + void *(CJSON_CDECL *allocate)(size_t size); + void(CJSON_CDECL *deallocate)(void *pointer); + void *(CJSON_CDECL *reallocate)(void *pointer, size_t size); +} internal_hooks; + +#if defined(_MSC_VER) +/* work around MSVC error C2322: '...' address of dllimport '...' is not static */ +static void *CJSON_CDECL internal_malloc(size_t size) +{ + return malloc(size); +} +static void CJSON_CDECL internal_free(void *pointer) +{ + free(pointer); +} +static void *CJSON_CDECL internal_realloc(void *pointer, size_t size) +{ + return realloc(pointer, size); +} +#else +#define internal_malloc malloc +#define internal_free free +#define internal_realloc realloc +#endif + +/* strlen of character literals resolved at compile time */ +#define static_strlen(string_literal) (sizeof(string_literal) - sizeof("")) + +static internal_hooks global_hooks = {internal_malloc, internal_free, internal_realloc}; + +static unsigned char *cJSON_strdup(const unsigned char *string, const internal_hooks *const hooks) +{ + size_t length = 0; + unsigned char *copy = NULL; + + if (string == NULL) + { + return NULL; + } + + length = strlen((const char *)string) + sizeof(""); + copy = (unsigned char *)hooks->allocate(length); + if (copy == NULL) + { + return NULL; + } + memcpy(copy, string, length); + + return copy; +} + +CJSON_PUBLIC(void) +cJSON_InitHooks(cJSON_Hooks *hooks) +{ + if (hooks == NULL) + { + /* Reset hooks */ + global_hooks.allocate = malloc; + global_hooks.deallocate = free; + global_hooks.reallocate = realloc; + return; + } + + global_hooks.allocate = malloc; + if (hooks->malloc_fn != NULL) + { + global_hooks.allocate = hooks->malloc_fn; + } + + global_hooks.deallocate = free; + if (hooks->free_fn != NULL) + { + global_hooks.deallocate = hooks->free_fn; + } + + /* use realloc only if both free and malloc are used */ + global_hooks.reallocate = NULL; + if ((global_hooks.allocate == malloc) && (global_hooks.deallocate == free)) + { + global_hooks.reallocate = realloc; + } +} + +/* Internal constructor. */ +static cJSON *cJSON_New_Item(const internal_hooks *const hooks) +{ + cJSON *node = (cJSON *)hooks->allocate(sizeof(cJSON)); + if (node) + { + memset(node, '\0', sizeof(cJSON)); + } + + return node; +} + +/* Delete a cJSON structure. */ +CJSON_PUBLIC(void) +cJSON_Delete(cJSON *item) +{ + cJSON *next = NULL; + while (item != NULL) + { + next = item->next; + if (!(item->type & cJSON_IsReference) && (item->child != NULL)) + { + cJSON_Delete(item->child); + } + if (!(item->type & cJSON_IsReference) && (item->valuestring != NULL)) + { + global_hooks.deallocate(item->valuestring); + item->valuestring = NULL; + } + if (!(item->type & cJSON_StringIsConst) && (item->string != NULL)) + { + global_hooks.deallocate(item->string); + item->string = NULL; + } + global_hooks.deallocate(item); + item = next; + } +} + +/* get the decimal point character of the current locale */ +static unsigned char get_decimal_point(void) +{ +#ifdef ENABLE_LOCALES + struct lconv *lconv = localeconv(); + return (unsigned char)lconv->decimal_point[0]; +#else + return '.'; +#endif +} + +typedef struct +{ + const unsigned char *content; + size_t length; + size_t offset; + size_t depth; /* How deeply nested (in arrays/objects) is the input at the current offset. */ + internal_hooks hooks; +} parse_buffer; + +/* check if the given size is left to read in a given parse buffer (starting with 1) */ +#define can_read(buffer, size) ((buffer != NULL) && (((buffer)->offset + size) <= (buffer)->length)) +/* check if the buffer can be accessed at the given index (starting with 0) */ +#define can_access_at_index(buffer, index) ((buffer != NULL) && (((buffer)->offset + index) < (buffer)->length)) +#define cannot_access_at_index(buffer, index) (!can_access_at_index(buffer, index)) +/* get a pointer to the buffer at the position */ +#define buffer_at_offset(buffer) ((buffer)->content + (buffer)->offset) + +/* Parse the input text to generate a number, and populate the result into item. */ +static cJSON_bool parse_number(cJSON *const item, parse_buffer *const input_buffer) +{ + double number = 0; + unsigned char *after_end = NULL; + unsigned char *number_c_string; + unsigned char decimal_point = get_decimal_point(); + size_t i = 0; + size_t number_string_length = 0; + cJSON_bool has_decimal_point = false; + + if ((input_buffer == NULL) || (input_buffer->content == NULL)) + { + return false; + } + + /* copy the number into a temporary buffer and replace '.' with the decimal point + * of the current locale (for strtod) + * This also takes care of '\0' not necessarily being available for marking the end of the input */ + for (i = 0; can_access_at_index(input_buffer, i); i++) + { + switch (buffer_at_offset(input_buffer)[i]) + { + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + case '+': + case '-': + case 'e': + case 'E': + number_string_length++; + break; + + case '.': + number_string_length++; + has_decimal_point = true; + break; + + default: + goto loop_end; + } + } +loop_end: + /* malloc for temporary buffer, add 1 for '\0' */ + number_c_string = (unsigned char *)input_buffer->hooks.allocate(number_string_length + 1); + if (number_c_string == NULL) + { + return false; /* allocation failure */ + } + + memcpy(number_c_string, buffer_at_offset(input_buffer), number_string_length); + number_c_string[number_string_length] = '\0'; + + if (has_decimal_point) + { + for (i = 0; i < number_string_length; i++) + { + if (number_c_string[i] == '.') + { + /* replace '.' with the decimal point of the current locale (for strtod) */ + number_c_string[i] = decimal_point; + } + } + } + + number = strtod((const char *)number_c_string, (char **)&after_end); + if (number_c_string == after_end) + { + /* free the temporary buffer */ + input_buffer->hooks.deallocate(number_c_string); + return false; /* parse_error */ + } + + item->valuedouble = number; + + /* use saturation in case of overflow */ + if (number >= INT_MAX) + { + item->valueint = INT_MAX; + } + else if (number <= (double)INT_MIN) + { + item->valueint = INT_MIN; + } + else + { + item->valueint = (int)number; + } + + item->type = cJSON_Number; + + input_buffer->offset += (size_t)(after_end - number_c_string); + /* free the temporary buffer */ + input_buffer->hooks.deallocate(number_c_string); + return true; +} + +/* don't ask me, but the original cJSON_SetNumberValue returns an integer or double */ +CJSON_PUBLIC(double) +cJSON_SetNumberHelper(cJSON *object, double number) +{ + if (object == NULL) + { + return (double)NAN; + } + + if (number >= INT_MAX) + { + object->valueint = INT_MAX; + } + else if (number <= (double)INT_MIN) + { + object->valueint = INT_MIN; + } + else + { + object->valueint = (int)number; + } + + return object->valuedouble = number; +} + +/* Note: when passing a NULL valuestring, cJSON_SetValuestring treats this as an error and return NULL */ +CJSON_PUBLIC(char *) +cJSON_SetValuestring(cJSON *object, const char *valuestring) +{ + char *copy = NULL; + size_t v1_len; + size_t v2_len; + /* if object's type is not cJSON_String or is cJSON_IsReference, it should not set valuestring */ + if ((object == NULL) || !(object->type & cJSON_String) || (object->type & cJSON_IsReference)) + { + return NULL; + } + /* return NULL if the object is corrupted or valuestring is NULL */ + if (object->valuestring == NULL || valuestring == NULL) + { + return NULL; + } + + v1_len = strlen(valuestring); + v2_len = strlen(object->valuestring); + + if (v1_len <= v2_len) + { + /* strcpy does not handle overlapping string: [X1, X2] [Y1, Y2] => X2 < Y1 or Y2 < X1 */ + if (!(valuestring + v1_len < object->valuestring || object->valuestring + v2_len < valuestring)) + { + return NULL; + } + strcpy(object->valuestring, valuestring); + return object->valuestring; + } + copy = (char *)cJSON_strdup((const unsigned char *)valuestring, &global_hooks); + if (copy == NULL) + { + return NULL; + } + if (object->valuestring != NULL) + { + cJSON_free(object->valuestring); + } + object->valuestring = copy; + + return copy; +} + +typedef struct +{ + unsigned char *buffer; + size_t length; + size_t offset; + size_t depth; /* current nesting depth (for formatted printing) */ + cJSON_bool noalloc; + cJSON_bool format; /* is this print a formatted print */ + internal_hooks hooks; +} printbuffer; + +/* realloc printbuffer if necessary to have at least "needed" bytes more */ +static unsigned char *ensure(printbuffer *const p, size_t needed) +{ + unsigned char *newbuffer = NULL; + size_t newsize = 0; + + if ((p == NULL) || (p->buffer == NULL)) + { + return NULL; + } + + if ((p->length > 0) && (p->offset >= p->length)) + { + /* make sure that offset is valid */ + return NULL; + } + + if (needed > INT_MAX) + { + /* sizes bigger than INT_MAX are currently not supported */ + return NULL; + } + + needed += p->offset + 1; + if (needed <= p->length) + { + return p->buffer + p->offset; + } + + if (p->noalloc) + { + return NULL; + } + + /* calculate new buffer size */ + if (needed > (INT_MAX / 2)) + { + /* overflow of int, use INT_MAX if possible */ + if (needed <= INT_MAX) + { + newsize = INT_MAX; + } + else + { + return NULL; + } + } + else + { + newsize = needed * 2; + } + + if (p->hooks.reallocate != NULL) + { + /* reallocate with realloc if available */ + newbuffer = (unsigned char *)p->hooks.reallocate(p->buffer, newsize); + if (newbuffer == NULL) + { + p->hooks.deallocate(p->buffer); + p->length = 0; + p->buffer = NULL; + + return NULL; + } + } + else + { + /* otherwise reallocate manually */ + newbuffer = (unsigned char *)p->hooks.allocate(newsize); + if (!newbuffer) + { + p->hooks.deallocate(p->buffer); + p->length = 0; + p->buffer = NULL; + + return NULL; + } + + memcpy(newbuffer, p->buffer, p->offset + 1); + p->hooks.deallocate(p->buffer); + } + p->length = newsize; + p->buffer = newbuffer; + + return newbuffer + p->offset; +} + +/* calculate the new length of the string in a printbuffer and update the offset */ +static void update_offset(printbuffer *const buffer) +{ + const unsigned char *buffer_pointer = NULL; + if ((buffer == NULL) || (buffer->buffer == NULL)) + { + return; + } + buffer_pointer = buffer->buffer + buffer->offset; + + buffer->offset += strlen((const char *)buffer_pointer); +} + +/* securely comparison of floating-point variables */ +static cJSON_bool compare_double(double a, double b) +{ + double maxVal = fabs(a) > fabs(b) ? fabs(a) : fabs(b); + return (fabs(a - b) <= maxVal * DBL_EPSILON); +} + +/* Render the number nicely from the given item into a string. */ +static cJSON_bool print_number(const cJSON *const item, printbuffer *const output_buffer) +{ + unsigned char *output_pointer = NULL; + double d = item->valuedouble; + int length = 0; + size_t i = 0; + unsigned char number_buffer[26] = {0}; /* temporary buffer to print the number into */ + unsigned char decimal_point = get_decimal_point(); + double test = 0.0; + + if (output_buffer == NULL) + { + return false; + } + + /* This checks for NaN and Infinity */ + if (isnan(d) || isinf(d)) + { + length = sprintf((char *)number_buffer, "null"); + } + else if (d == (double)item->valueint) + { + length = sprintf((char *)number_buffer, "%d", item->valueint); + } + else + { + /* Try 15 decimal places of precision to avoid nonsignificant nonzero digits */ + length = sprintf((char *)number_buffer, "%1.15g", d); + + /* Check whether the original double can be recovered */ + if ((sscanf((char *)number_buffer, "%lg", &test) != 1) || !compare_double((double)test, d)) + { + /* If not, print with 17 decimal places of precision */ + length = sprintf((char *)number_buffer, "%1.17g", d); + } + } + + /* sprintf failed or buffer overrun occurred */ + if ((length < 0) || (length > (int)(sizeof(number_buffer) - 1))) + { + return false; + } + + /* reserve appropriate space in the output */ + output_pointer = ensure(output_buffer, (size_t)length + sizeof("")); + if (output_pointer == NULL) + { + return false; + } + + /* copy the printed number to the output and replace locale + * dependent decimal point with '.' */ + for (i = 0; i < ((size_t)length); i++) + { + if (number_buffer[i] == decimal_point) + { + output_pointer[i] = '.'; + continue; + } + + output_pointer[i] = number_buffer[i]; + } + output_pointer[i] = '\0'; + + output_buffer->offset += (size_t)length; + + return true; +} + +/* parse 4 digit hexadecimal number */ +static unsigned parse_hex4(const unsigned char *const input) +{ + unsigned int h = 0; + size_t i = 0; + + for (i = 0; i < 4; i++) + { + /* parse digit */ + if ((input[i] >= '0') && (input[i] <= '9')) + { + h += (unsigned int)input[i] - '0'; + } + else if ((input[i] >= 'A') && (input[i] <= 'F')) + { + h += (unsigned int)10 + input[i] - 'A'; + } + else if ((input[i] >= 'a') && (input[i] <= 'f')) + { + h += (unsigned int)10 + input[i] - 'a'; + } + else /* invalid */ + { + return 0; + } + + if (i < 3) + { + /* shift left to make place for the next nibble */ + h = h << 4; + } + } + + return h; +} + +/* converts a UTF-16 literal to UTF-8 + * A literal can be one or two sequences of the form \uXXXX */ +static unsigned char utf16_literal_to_utf8(const unsigned char *const input_pointer, const unsigned char *const input_end, unsigned char **output_pointer) +{ + long unsigned int codepoint = 0; + unsigned int first_code = 0; + const unsigned char *first_sequence = input_pointer; + unsigned char utf8_length = 0; + unsigned char utf8_position = 0; + unsigned char sequence_length = 0; + unsigned char first_byte_mark = 0; + + if ((input_end - first_sequence) < 6) + { + /* input ends unexpectedly */ + goto fail; + } + + /* get the first utf16 sequence */ + first_code = parse_hex4(first_sequence + 2); + + /* check that the code is valid */ + if (((first_code >= 0xDC00) && (first_code <= 0xDFFF))) + { + goto fail; + } + + /* UTF16 surrogate pair */ + if ((first_code >= 0xD800) && (first_code <= 0xDBFF)) + { + const unsigned char *second_sequence = first_sequence + 6; + unsigned int second_code = 0; + sequence_length = 12; /* \uXXXX\uXXXX */ + + if ((input_end - second_sequence) < 6) + { + /* input ends unexpectedly */ + goto fail; + } + + if ((second_sequence[0] != '\\') || (second_sequence[1] != 'u')) + { + /* missing second half of the surrogate pair */ + goto fail; + } + + /* get the second utf16 sequence */ + second_code = parse_hex4(second_sequence + 2); + /* check that the code is valid */ + if ((second_code < 0xDC00) || (second_code > 0xDFFF)) + { + /* invalid second half of the surrogate pair */ + goto fail; + } + + /* calculate the unicode codepoint from the surrogate pair */ + codepoint = 0x10000 + (((first_code & 0x3FF) << 10) | (second_code & 0x3FF)); + } + else + { + sequence_length = 6; /* \uXXXX */ + codepoint = first_code; + } + + /* encode as UTF-8 + * takes at maximum 4 bytes to encode: + * 11110xxx 10xxxxxx 10xxxxxx 10xxxxxx */ + if (codepoint < 0x80) + { + /* normal ascii, encoding 0xxxxxxx */ + utf8_length = 1; + } + else if (codepoint < 0x800) + { + /* two bytes, encoding 110xxxxx 10xxxxxx */ + utf8_length = 2; + first_byte_mark = 0xC0; /* 11000000 */ + } + else if (codepoint < 0x10000) + { + /* three bytes, encoding 1110xxxx 10xxxxxx 10xxxxxx */ + utf8_length = 3; + first_byte_mark = 0xE0; /* 11100000 */ + } + else if (codepoint <= 0x10FFFF) + { + /* four bytes, encoding 1110xxxx 10xxxxxx 10xxxxxx 10xxxxxx */ + utf8_length = 4; + first_byte_mark = 0xF0; /* 11110000 */ + } + else + { + /* invalid unicode codepoint */ + goto fail; + } + + /* encode as utf8 */ + for (utf8_position = (unsigned char)(utf8_length - 1); utf8_position > 0; utf8_position--) + { + /* 10xxxxxx */ + (*output_pointer)[utf8_position] = (unsigned char)((codepoint | 0x80) & 0xBF); + codepoint >>= 6; + } + /* encode first byte */ + if (utf8_length > 1) + { + (*output_pointer)[0] = (unsigned char)((codepoint | first_byte_mark) & 0xFF); + } + else + { + (*output_pointer)[0] = (unsigned char)(codepoint & 0x7F); + } + + *output_pointer += utf8_length; + + return sequence_length; + +fail: + return 0; +} + +/* Parse the input text into an unescaped cinput, and populate item. */ +static cJSON_bool parse_string(cJSON *const item, parse_buffer *const input_buffer) +{ + const unsigned char *input_pointer = buffer_at_offset(input_buffer) + 1; + const unsigned char *input_end = buffer_at_offset(input_buffer) + 1; + unsigned char *output_pointer = NULL; + unsigned char *output = NULL; + + /* not a string */ + if (buffer_at_offset(input_buffer)[0] != '\"') + { + goto fail; + } + + { + /* calculate approximate size of the output (overestimate) */ + size_t allocation_length = 0; + size_t skipped_bytes = 0; + while (((size_t)(input_end - input_buffer->content) < input_buffer->length) && (*input_end != '\"')) + { + /* is escape sequence */ + if (input_end[0] == '\\') + { + if ((size_t)(input_end + 1 - input_buffer->content) >= input_buffer->length) + { + /* prevent buffer overflow when last input character is a backslash */ + goto fail; + } + skipped_bytes++; + input_end++; + } + input_end++; + } + if (((size_t)(input_end - input_buffer->content) >= input_buffer->length) || (*input_end != '\"')) + { + goto fail; /* string ended unexpectedly */ + } + + /* This is at most how much we need for the output */ + allocation_length = (size_t)(input_end - buffer_at_offset(input_buffer)) - skipped_bytes; + output = (unsigned char *)input_buffer->hooks.allocate(allocation_length + sizeof("")); + if (output == NULL) + { + goto fail; /* allocation failure */ + } + } + + output_pointer = output; + /* loop through the string literal */ + while (input_pointer < input_end) + { + if (*input_pointer != '\\') + { + *output_pointer++ = *input_pointer++; + } + /* escape sequence */ + else + { + unsigned char sequence_length = 2; + if ((input_end - input_pointer) < 1) + { + goto fail; + } + + switch (input_pointer[1]) + { + case 'b': + *output_pointer++ = '\b'; + break; + case 'f': + *output_pointer++ = '\f'; + break; + case 'n': + *output_pointer++ = '\n'; + break; + case 'r': + *output_pointer++ = '\r'; + break; + case 't': + *output_pointer++ = '\t'; + break; + case '\"': + case '\\': + case '/': + *output_pointer++ = input_pointer[1]; + break; + + /* UTF-16 literal */ + case 'u': + sequence_length = utf16_literal_to_utf8(input_pointer, input_end, &output_pointer); + if (sequence_length == 0) + { + /* failed to convert UTF16-literal to UTF-8 */ + goto fail; + } + break; + + default: + goto fail; + } + input_pointer += sequence_length; + } + } + + /* zero terminate the output */ + *output_pointer = '\0'; + + item->type = cJSON_String; + item->valuestring = (char *)output; + + input_buffer->offset = (size_t)(input_end - input_buffer->content); + input_buffer->offset++; + + return true; + +fail: + if (output != NULL) + { + input_buffer->hooks.deallocate(output); + output = NULL; + } + + if (input_pointer != NULL) + { + input_buffer->offset = (size_t)(input_pointer - input_buffer->content); + } + + return false; +} + +/* Render the cstring provided to an escaped version that can be printed. */ +static cJSON_bool print_string_ptr(const unsigned char *const input, printbuffer *const output_buffer) +{ + const unsigned char *input_pointer = NULL; + unsigned char *output = NULL; + unsigned char *output_pointer = NULL; + size_t output_length = 0; + /* numbers of additional characters needed for escaping */ + size_t escape_characters = 0; + + if (output_buffer == NULL) + { + return false; + } + + /* empty string */ + if (input == NULL) + { + output = ensure(output_buffer, sizeof("\"\"")); + if (output == NULL) + { + return false; + } + strcpy((char *)output, "\"\""); + + return true; + } + + /* set "flag" to 1 if something needs to be escaped */ + for (input_pointer = input; *input_pointer; input_pointer++) + { + switch (*input_pointer) + { + case '\"': + case '\\': + case '\b': + case '\f': + case '\n': + case '\r': + case '\t': + /* one character escape sequence */ + escape_characters++; + break; + default: + if (*input_pointer < 32) + { + /* UTF-16 escape sequence uXXXX */ + escape_characters += 5; + } + break; + } + } + output_length = (size_t)(input_pointer - input) + escape_characters; + + output = ensure(output_buffer, output_length + sizeof("\"\"")); + if (output == NULL) + { + return false; + } + + /* no characters have to be escaped */ + if (escape_characters == 0) + { + output[0] = '\"'; + memcpy(output + 1, input, output_length); + output[output_length + 1] = '\"'; + output[output_length + 2] = '\0'; + + return true; + } + + output[0] = '\"'; + output_pointer = output + 1; + /* copy the string */ + for (input_pointer = input; *input_pointer != '\0'; (void)input_pointer++, output_pointer++) + { + if ((*input_pointer > 31) && (*input_pointer != '\"') && (*input_pointer != '\\')) + { + /* normal character, copy */ + *output_pointer = *input_pointer; + } + else + { + /* character needs to be escaped */ + *output_pointer++ = '\\'; + switch (*input_pointer) + { + case '\\': + *output_pointer = '\\'; + break; + case '\"': + *output_pointer = '\"'; + break; + case '\b': + *output_pointer = 'b'; + break; + case '\f': + *output_pointer = 'f'; + break; + case '\n': + *output_pointer = 'n'; + break; + case '\r': + *output_pointer = 'r'; + break; + case '\t': + *output_pointer = 't'; + break; + default: + /* escape and print as unicode codepoint */ + sprintf((char *)output_pointer, "u%04x", *input_pointer); + output_pointer += 4; + break; + } + } + } + output[output_length + 1] = '\"'; + output[output_length + 2] = '\0'; + + return true; +} + +/* Invoke print_string_ptr (which is useful) on an item. */ +static cJSON_bool print_string(const cJSON *const item, printbuffer *const p) +{ + return print_string_ptr((unsigned char *)item->valuestring, p); +} + +/* Predeclare these prototypes. */ +static cJSON_bool parse_value(cJSON *const item, parse_buffer *const input_buffer); +static cJSON_bool print_value(const cJSON *const item, printbuffer *const output_buffer); +static cJSON_bool parse_array(cJSON *const item, parse_buffer *const input_buffer); +static cJSON_bool print_array(const cJSON *const item, printbuffer *const output_buffer); +static cJSON_bool parse_object(cJSON *const item, parse_buffer *const input_buffer); +static cJSON_bool print_object(const cJSON *const item, printbuffer *const output_buffer); + +/* Utility to jump whitespace and cr/lf */ +static parse_buffer *buffer_skip_whitespace(parse_buffer *const buffer) +{ + if ((buffer == NULL) || (buffer->content == NULL)) + { + return NULL; + } + + if (cannot_access_at_index(buffer, 0)) + { + return buffer; + } + + while (can_access_at_index(buffer, 0) && (buffer_at_offset(buffer)[0] <= 32)) + { + buffer->offset++; + } + + if (buffer->offset == buffer->length) + { + buffer->offset--; + } + + return buffer; +} + +/* skip the UTF-8 BOM (byte order mark) if it is at the beginning of a buffer */ +static parse_buffer *skip_utf8_bom(parse_buffer *const buffer) +{ + if ((buffer == NULL) || (buffer->content == NULL) || (buffer->offset != 0)) + { + return NULL; + } + + if (can_access_at_index(buffer, 4) && (strncmp((const char *)buffer_at_offset(buffer), "\xEF\xBB\xBF", 3) == 0)) + { + buffer->offset += 3; + } + + return buffer; +} + +CJSON_PUBLIC(cJSON *) +cJSON_ParseWithOpts(const char *value, const char **return_parse_end, cJSON_bool require_null_terminated) +{ + size_t buffer_length; + + if (NULL == value) + { + return NULL; + } + + /* Adding null character size due to require_null_terminated. */ + buffer_length = strlen(value) + sizeof(""); + + return cJSON_ParseWithLengthOpts(value, buffer_length, return_parse_end, require_null_terminated); +} + +/* Parse an object - create a new root, and populate. */ +CJSON_PUBLIC(cJSON *) +cJSON_ParseWithLengthOpts(const char *value, size_t buffer_length, const char **return_parse_end, cJSON_bool require_null_terminated) +{ + parse_buffer buffer = {0, 0, 0, 0, {0, 0, 0}}; + cJSON *item = NULL; + + /* reset error position */ + global_error.json = NULL; + global_error.position = 0; + + if (value == NULL || 0 == buffer_length) + { + goto fail; + } + + buffer.content = (const unsigned char *)value; + buffer.length = buffer_length; + buffer.offset = 0; + buffer.hooks = global_hooks; + + item = cJSON_New_Item(&global_hooks); + if (item == NULL) /* memory fail */ + { + goto fail; + } + + if (!parse_value(item, buffer_skip_whitespace(skip_utf8_bom(&buffer)))) + { + /* parse failure. ep is set. */ + goto fail; + } + + /* if we require null-terminated JSON without appended garbage, skip and then check for a null terminator */ + if (require_null_terminated) + { + buffer_skip_whitespace(&buffer); + if ((buffer.offset >= buffer.length) || buffer_at_offset(&buffer)[0] != '\0') + { + goto fail; + } + } + if (return_parse_end) + { + *return_parse_end = (const char *)buffer_at_offset(&buffer); + } + + return item; + +fail: + if (item != NULL) + { + cJSON_Delete(item); + } + + if (value != NULL) + { + error local_error; + local_error.json = (const unsigned char *)value; + local_error.position = 0; + + if (buffer.offset < buffer.length) + { + local_error.position = buffer.offset; + } + else if (buffer.length > 0) + { + local_error.position = buffer.length - 1; + } + + if (return_parse_end != NULL) + { + *return_parse_end = (const char *)local_error.json + local_error.position; + } + + global_error = local_error; + } + + return NULL; +} + +/* Default options for cJSON_Parse */ +CJSON_PUBLIC(cJSON *) +cJSON_Parse(const char *value) +{ + return cJSON_ParseWithOpts(value, 0, 0); +} + +CJSON_PUBLIC(cJSON *) +cJSON_ParseWithLength(const char *value, size_t buffer_length) +{ + return cJSON_ParseWithLengthOpts(value, buffer_length, 0, 0); +} + +#define cjson_min(a, b) (((a) < (b)) ? (a) : (b)) + +static unsigned char *print(const cJSON *const item, cJSON_bool format, const internal_hooks *const hooks) +{ + static const size_t default_buffer_size = 256; + printbuffer buffer[1]; + unsigned char *printed = NULL; + + memset(buffer, 0, sizeof(buffer)); + + /* create buffer */ + buffer->buffer = (unsigned char *)hooks->allocate(default_buffer_size); + buffer->length = default_buffer_size; + buffer->format = format; + buffer->hooks = *hooks; + if (buffer->buffer == NULL) + { + goto fail; + } + + /* print the value */ + if (!print_value(item, buffer)) + { + goto fail; + } + update_offset(buffer); + + /* check if reallocate is available */ + if (hooks->reallocate != NULL) + { + printed = (unsigned char *)hooks->reallocate(buffer->buffer, buffer->offset + 1); + if (printed == NULL) + { + goto fail; + } + buffer->buffer = NULL; + } + else /* otherwise copy the JSON over to a new buffer */ + { + printed = (unsigned char *)hooks->allocate(buffer->offset + 1); + if (printed == NULL) + { + goto fail; + } + memcpy(printed, buffer->buffer, cjson_min(buffer->length, buffer->offset + 1)); + printed[buffer->offset] = '\0'; /* just to be sure */ + + /* free the buffer */ + hooks->deallocate(buffer->buffer); + buffer->buffer = NULL; + } + + return printed; + +fail: + if (buffer->buffer != NULL) + { + hooks->deallocate(buffer->buffer); + buffer->buffer = NULL; + } + + if (printed != NULL) + { + hooks->deallocate(printed); + printed = NULL; + } + + return NULL; +} + +/* Render a cJSON item/entity/structure to text. */ +CJSON_PUBLIC(char *) +cJSON_Print(const cJSON *item) +{ + return (char *)print(item, true, &global_hooks); +} + +CJSON_PUBLIC(char *) +cJSON_PrintUnformatted(const cJSON *item) +{ + return (char *)print(item, false, &global_hooks); +} + +CJSON_PUBLIC(char *) +cJSON_PrintBuffered(const cJSON *item, int prebuffer, cJSON_bool fmt) +{ + printbuffer p = {0, 0, 0, 0, 0, 0, {0, 0, 0}}; + + if (prebuffer < 0) + { + return NULL; + } + + p.buffer = (unsigned char *)global_hooks.allocate((size_t)prebuffer); + if (!p.buffer) + { + return NULL; + } + + p.length = (size_t)prebuffer; + p.offset = 0; + p.noalloc = false; + p.format = fmt; + p.hooks = global_hooks; + + if (!print_value(item, &p)) + { + global_hooks.deallocate(p.buffer); + p.buffer = NULL; + return NULL; + } + + return (char *)p.buffer; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_PrintPreallocated(cJSON *item, char *buffer, const int length, const cJSON_bool format) +{ + printbuffer p = {0, 0, 0, 0, 0, 0, {0, 0, 0}}; + + if ((length < 0) || (buffer == NULL)) + { + return false; + } + + p.buffer = (unsigned char *)buffer; + p.length = (size_t)length; + p.offset = 0; + p.noalloc = true; + p.format = format; + p.hooks = global_hooks; + + return print_value(item, &p); +} + +/* Parser core - when encountering text, process appropriately. */ +static cJSON_bool parse_value(cJSON *const item, parse_buffer *const input_buffer) +{ + if ((input_buffer == NULL) || (input_buffer->content == NULL)) + { + return false; /* no input */ + } + + /* parse the different types of values */ + /* null */ + if (can_read(input_buffer, 4) && (strncmp((const char *)buffer_at_offset(input_buffer), "null", 4) == 0)) + { + item->type = cJSON_NULL; + input_buffer->offset += 4; + return true; + } + /* false */ + if (can_read(input_buffer, 5) && (strncmp((const char *)buffer_at_offset(input_buffer), "false", 5) == 0)) + { + item->type = cJSON_False; + input_buffer->offset += 5; + return true; + } + /* true */ + if (can_read(input_buffer, 4) && (strncmp((const char *)buffer_at_offset(input_buffer), "true", 4) == 0)) + { + item->type = cJSON_True; + item->valueint = 1; + input_buffer->offset += 4; + return true; + } + /* string */ + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '\"')) + { + return parse_string(item, input_buffer); + } + /* number */ + if (can_access_at_index(input_buffer, 0) && ((buffer_at_offset(input_buffer)[0] == '-') || ((buffer_at_offset(input_buffer)[0] >= '0') && (buffer_at_offset(input_buffer)[0] <= '9')))) + { + return parse_number(item, input_buffer); + } + /* array */ + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '[')) + { + return parse_array(item, input_buffer); + } + /* object */ + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '{')) + { + return parse_object(item, input_buffer); + } + + return false; +} + +/* Render a value to text. */ +static cJSON_bool print_value(const cJSON *const item, printbuffer *const output_buffer) +{ + unsigned char *output = NULL; + + if ((item == NULL) || (output_buffer == NULL)) + { + return false; + } + + switch ((item->type) & 0xFF) + { + case cJSON_NULL: + output = ensure(output_buffer, 5); + if (output == NULL) + { + return false; + } + strcpy((char *)output, "null"); + return true; + + case cJSON_False: + output = ensure(output_buffer, 6); + if (output == NULL) + { + return false; + } + strcpy((char *)output, "false"); + return true; + + case cJSON_True: + output = ensure(output_buffer, 5); + if (output == NULL) + { + return false; + } + strcpy((char *)output, "true"); + return true; + + case cJSON_Number: + return print_number(item, output_buffer); + + case cJSON_Raw: + { + size_t raw_length = 0; + if (item->valuestring == NULL) + { + return false; + } + + raw_length = strlen(item->valuestring) + sizeof(""); + output = ensure(output_buffer, raw_length); + if (output == NULL) + { + return false; + } + memcpy(output, item->valuestring, raw_length); + return true; + } + + case cJSON_String: + return print_string(item, output_buffer); + + case cJSON_Array: + return print_array(item, output_buffer); + + case cJSON_Object: + return print_object(item, output_buffer); + + default: + return false; + } +} + +/* Build an array from input text. */ +static cJSON_bool parse_array(cJSON *const item, parse_buffer *const input_buffer) +{ + cJSON *head = NULL; /* head of the linked list */ + cJSON *current_item = NULL; + + if (input_buffer->depth >= CJSON_NESTING_LIMIT) + { + return false; /* to deeply nested */ + } + input_buffer->depth++; + + if (buffer_at_offset(input_buffer)[0] != '[') + { + /* not an array */ + goto fail; + } + + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == ']')) + { + /* empty array */ + goto success; + } + + /* check if we skipped to the end of the buffer */ + if (cannot_access_at_index(input_buffer, 0)) + { + input_buffer->offset--; + goto fail; + } + + /* step back to character in front of the first element */ + input_buffer->offset--; + /* loop through the comma separated array elements */ + do + { + /* allocate next item */ + cJSON *new_item = cJSON_New_Item(&(input_buffer->hooks)); + if (new_item == NULL) + { + goto fail; /* allocation failure */ + } + + /* attach next item to list */ + if (head == NULL) + { + /* start the linked list */ + current_item = head = new_item; + } + else + { + /* add to the end and advance */ + current_item->next = new_item; + new_item->prev = current_item; + current_item = new_item; + } + + /* parse next value */ + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (!parse_value(current_item, input_buffer)) + { + goto fail; /* failed to parse value */ + } + buffer_skip_whitespace(input_buffer); + } while (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == ',')); + + if (cannot_access_at_index(input_buffer, 0) || buffer_at_offset(input_buffer)[0] != ']') + { + goto fail; /* expected end of array */ + } + +success: + input_buffer->depth--; + + if (head != NULL) + { + head->prev = current_item; + } + + item->type = cJSON_Array; + item->child = head; + + input_buffer->offset++; + + return true; + +fail: + if (head != NULL) + { + cJSON_Delete(head); + } + + return false; +} + +/* Render an array to text */ +static cJSON_bool print_array(const cJSON *const item, printbuffer *const output_buffer) +{ + unsigned char *output_pointer = NULL; + size_t length = 0; + cJSON *current_element = item->child; + + if (output_buffer == NULL) + { + return false; + } + + if (output_buffer->depth >= CJSON_NESTING_LIMIT) + { + return false; /* nesting is too deep */ + } + + /* Compose the output array. */ + /* opening square bracket */ + output_pointer = ensure(output_buffer, 1); + if (output_pointer == NULL) + { + return false; + } + + *output_pointer = '['; + output_buffer->offset++; + output_buffer->depth++; + + while (current_element != NULL) + { + if (!print_value(current_element, output_buffer)) + { + return false; + } + update_offset(output_buffer); + if (current_element->next) + { + length = (size_t)(output_buffer->format ? 2 : 1); + output_pointer = ensure(output_buffer, length + 1); + if (output_pointer == NULL) + { + return false; + } + *output_pointer++ = ','; + if (output_buffer->format) + { + *output_pointer++ = ' '; + } + *output_pointer = '\0'; + output_buffer->offset += length; + } + current_element = current_element->next; + } + + output_pointer = ensure(output_buffer, 2); + if (output_pointer == NULL) + { + return false; + } + *output_pointer++ = ']'; + *output_pointer = '\0'; + output_buffer->depth--; + + return true; +} + +/* Build an object from the text. */ +static cJSON_bool parse_object(cJSON *const item, parse_buffer *const input_buffer) +{ + cJSON *head = NULL; /* linked list head */ + cJSON *current_item = NULL; + + if (input_buffer->depth >= CJSON_NESTING_LIMIT) + { + return false; /* to deeply nested */ + } + input_buffer->depth++; + + if (cannot_access_at_index(input_buffer, 0) || (buffer_at_offset(input_buffer)[0] != '{')) + { + goto fail; /* not an object */ + } + + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == '}')) + { + goto success; /* empty object */ + } + + /* check if we skipped to the end of the buffer */ + if (cannot_access_at_index(input_buffer, 0)) + { + input_buffer->offset--; + goto fail; + } + + /* step back to character in front of the first element */ + input_buffer->offset--; + /* loop through the comma separated array elements */ + do + { + /* allocate next item */ + cJSON *new_item = cJSON_New_Item(&(input_buffer->hooks)); + if (new_item == NULL) + { + goto fail; /* allocation failure */ + } + + /* attach next item to list */ + if (head == NULL) + { + /* start the linked list */ + current_item = head = new_item; + } + else + { + /* add to the end and advance */ + current_item->next = new_item; + new_item->prev = current_item; + current_item = new_item; + } + + if (cannot_access_at_index(input_buffer, 1)) + { + goto fail; /* nothing comes after the comma */ + } + + /* parse the name of the child */ + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (!parse_string(current_item, input_buffer)) + { + goto fail; /* failed to parse name */ + } + buffer_skip_whitespace(input_buffer); + + /* swap valuestring and string, because we parsed the name */ + current_item->string = current_item->valuestring; + current_item->valuestring = NULL; + + if (cannot_access_at_index(input_buffer, 0) || (buffer_at_offset(input_buffer)[0] != ':')) + { + goto fail; /* invalid object */ + } + + /* parse the value */ + input_buffer->offset++; + buffer_skip_whitespace(input_buffer); + if (!parse_value(current_item, input_buffer)) + { + goto fail; /* failed to parse value */ + } + buffer_skip_whitespace(input_buffer); + } while (can_access_at_index(input_buffer, 0) && (buffer_at_offset(input_buffer)[0] == ',')); + + if (cannot_access_at_index(input_buffer, 0) || (buffer_at_offset(input_buffer)[0] != '}')) + { + goto fail; /* expected end of object */ + } + +success: + input_buffer->depth--; + + if (head != NULL) + { + head->prev = current_item; + } + + item->type = cJSON_Object; + item->child = head; + + input_buffer->offset++; + return true; + +fail: + if (head != NULL) + { + cJSON_Delete(head); + } + + return false; +} + +/* Render an object to text. */ +static cJSON_bool print_object(const cJSON *const item, printbuffer *const output_buffer) +{ + unsigned char *output_pointer = NULL; + size_t length = 0; + cJSON *current_item = item->child; + + if (output_buffer == NULL) + { + return false; + } + + if (output_buffer->depth >= CJSON_NESTING_LIMIT) + { + return false; /* nesting is too deep */ + } + + /* Compose the output: */ + length = (size_t)(output_buffer->format ? 2 : 1); /* fmt: {\n */ + output_pointer = ensure(output_buffer, length + 1); + if (output_pointer == NULL) + { + return false; + } + + *output_pointer++ = '{'; + output_buffer->depth++; + if (output_buffer->format) + { + *output_pointer++ = '\n'; + } + output_buffer->offset += length; + + while (current_item) + { + if (output_buffer->format) + { + size_t i; + output_pointer = ensure(output_buffer, output_buffer->depth); + if (output_pointer == NULL) + { + return false; + } + for (i = 0; i < output_buffer->depth; i++) + { + *output_pointer++ = '\t'; + } + output_buffer->offset += output_buffer->depth; + } + + /* print key */ + if (!print_string_ptr((unsigned char *)current_item->string, output_buffer)) + { + return false; + } + update_offset(output_buffer); + + length = (size_t)(output_buffer->format ? 2 : 1); + output_pointer = ensure(output_buffer, length); + if (output_pointer == NULL) + { + return false; + } + *output_pointer++ = ':'; + if (output_buffer->format) + { + *output_pointer++ = '\t'; + } + output_buffer->offset += length; + + /* print value */ + if (!print_value(current_item, output_buffer)) + { + return false; + } + update_offset(output_buffer); + + /* print comma if not last */ + length = ((size_t)(output_buffer->format ? 1 : 0) + (size_t)(current_item->next ? 1 : 0)); + output_pointer = ensure(output_buffer, length + 1); + if (output_pointer == NULL) + { + return false; + } + if (current_item->next) + { + *output_pointer++ = ','; + } + + if (output_buffer->format) + { + *output_pointer++ = '\n'; + } + *output_pointer = '\0'; + output_buffer->offset += length; + + current_item = current_item->next; + } + + output_pointer = ensure(output_buffer, output_buffer->format ? (output_buffer->depth + 1) : 2); + if (output_pointer == NULL) + { + return false; + } + if (output_buffer->format) + { + size_t i; + for (i = 0; i < (output_buffer->depth - 1); i++) + { + *output_pointer++ = '\t'; + } + } + *output_pointer++ = '}'; + *output_pointer = '\0'; + output_buffer->depth--; + + return true; +} + +/* Get Array size/item / object item. */ +CJSON_PUBLIC(int) +cJSON_GetArraySize(const cJSON *array) +{ + cJSON *child = NULL; + size_t size = 0; + + if (array == NULL) + { + return 0; + } + + child = array->child; + + while (child != NULL) + { + size++; + child = child->next; + } + + /* FIXME: Can overflow here. Cannot be fixed without breaking the API */ + + return (int)size; +} + +static cJSON *get_array_item(const cJSON *array, size_t index) +{ + cJSON *current_child = NULL; + + if (array == NULL) + { + return NULL; + } + + current_child = array->child; + while ((current_child != NULL) && (index > 0)) + { + index--; + current_child = current_child->next; + } + + return current_child; +} + +CJSON_PUBLIC(cJSON *) +cJSON_GetArrayItem(const cJSON *array, int index) +{ + if (index < 0) + { + return NULL; + } + + return get_array_item(array, (size_t)index); +} + +static cJSON *get_object_item(const cJSON *const object, const char *const name, const cJSON_bool case_sensitive) +{ + cJSON *current_element = NULL; + + if ((object == NULL) || (name == NULL)) + { + return NULL; + } + + current_element = object->child; + if (case_sensitive) + { + while ((current_element != NULL) && (current_element->string != NULL) && (strcmp(name, current_element->string) != 0)) + { + current_element = current_element->next; + } + } + else + { + while ((current_element != NULL) && (case_insensitive_strcmp((const unsigned char *)name, (const unsigned char *)(current_element->string)) != 0)) + { + current_element = current_element->next; + } + } + + if ((current_element == NULL) || (current_element->string == NULL)) + { + return NULL; + } + + return current_element; +} + +CJSON_PUBLIC(cJSON *) +cJSON_GetObjectItem(const cJSON *const object, const char *const string) +{ + return get_object_item(object, string, false); +} + +CJSON_PUBLIC(cJSON *) +cJSON_GetObjectItemCaseSensitive(const cJSON *const object, const char *const string) +{ + return get_object_item(object, string, true); +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_HasObjectItem(const cJSON *object, const char *string) +{ + return cJSON_GetObjectItem(object, string) ? 1 : 0; +} + +/* Utility for array list handling. */ +static void suffix_object(cJSON *prev, cJSON *item) +{ + prev->next = item; + item->prev = prev; +} + +/* Utility for handling references. */ +static cJSON *create_reference(const cJSON *item, const internal_hooks *const hooks) +{ + cJSON *reference = NULL; + if (item == NULL) + { + return NULL; + } + + reference = cJSON_New_Item(hooks); + if (reference == NULL) + { + return NULL; + } + + memcpy(reference, item, sizeof(cJSON)); + reference->string = NULL; + reference->type |= cJSON_IsReference; + reference->next = reference->prev = NULL; + return reference; +} + +static cJSON_bool add_item_to_array(cJSON *array, cJSON *item) +{ + cJSON *child = NULL; + + if ((item == NULL) || (array == NULL) || (array == item)) + { + return false; + } + + child = array->child; + /* + * To find the last item in array quickly, we use prev in array + */ + if (child == NULL) + { + /* list is empty, start new one */ + array->child = item; + item->prev = item; + item->next = NULL; + } + else + { + /* append to the end */ + if (child->prev) + { + suffix_object(child->prev, item); + array->child->prev = item; + } + } + + return true; +} + +/* Add item to array/object. */ +CJSON_PUBLIC(cJSON_bool) +cJSON_AddItemToArray(cJSON *array, cJSON *item) +{ + return add_item_to_array(array, item); +} + +#if defined(__clang__) || (defined(__GNUC__) && ((__GNUC__ > 4) || ((__GNUC__ == 4) && (__GNUC_MINOR__ > 5)))) +#pragma GCC diagnostic push +#endif +#ifdef __GNUC__ +#pragma GCC diagnostic ignored "-Wcast-qual" +#endif +/* helper function to cast away const */ +static void *cast_away_const(const void *string) +{ + return (void *)string; +} +#if defined(__clang__) || (defined(__GNUC__) && ((__GNUC__ > 4) || ((__GNUC__ == 4) && (__GNUC_MINOR__ > 5)))) +#pragma GCC diagnostic pop +#endif + +static cJSON_bool add_item_to_object(cJSON *const object, const char *const string, cJSON *const item, const internal_hooks *const hooks, const cJSON_bool constant_key) +{ + char *new_key = NULL; + int new_type = cJSON_Invalid; + + if ((object == NULL) || (string == NULL) || (item == NULL) || (object == item)) + { + return false; + } + + if (constant_key) + { + new_key = (char *)cast_away_const(string); + new_type = item->type | cJSON_StringIsConst; + } + else + { + new_key = (char *)cJSON_strdup((const unsigned char *)string, hooks); + if (new_key == NULL) + { + return false; + } + + new_type = item->type & ~cJSON_StringIsConst; + } + + if (!(item->type & cJSON_StringIsConst) && (item->string != NULL)) + { + hooks->deallocate(item->string); + } + + item->string = new_key; + item->type = new_type; + + return add_item_to_array(object, item); +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_AddItemToObject(cJSON *object, const char *string, cJSON *item) +{ + return add_item_to_object(object, string, item, &global_hooks, false); +} + +/* Add an item to an object with constant string as key */ +CJSON_PUBLIC(cJSON_bool) +cJSON_AddItemToObjectCS(cJSON *object, const char *string, cJSON *item) +{ + return add_item_to_object(object, string, item, &global_hooks, true); +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_AddItemReferenceToArray(cJSON *array, cJSON *item) +{ + if (array == NULL) + { + return false; + } + + return add_item_to_array(array, create_reference(item, &global_hooks)); +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_AddItemReferenceToObject(cJSON *object, const char *string, cJSON *item) +{ + if ((object == NULL) || (string == NULL)) + { + return false; + } + + return add_item_to_object(object, string, create_reference(item, &global_hooks), &global_hooks, false); +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddNullToObject(cJSON *const object, const char *const name) +{ + cJSON *null = cJSON_CreateNull(); + if (add_item_to_object(object, name, null, &global_hooks, false)) + { + return null; + } + + cJSON_Delete(null); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddTrueToObject(cJSON *const object, const char *const name) +{ + cJSON *true_item = cJSON_CreateTrue(); + if (add_item_to_object(object, name, true_item, &global_hooks, false)) + { + return true_item; + } + + cJSON_Delete(true_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddFalseToObject(cJSON *const object, const char *const name) +{ + cJSON *false_item = cJSON_CreateFalse(); + if (add_item_to_object(object, name, false_item, &global_hooks, false)) + { + return false_item; + } + + cJSON_Delete(false_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddBoolToObject(cJSON *const object, const char *const name, const cJSON_bool boolean) +{ + cJSON *bool_item = cJSON_CreateBool(boolean); + if (add_item_to_object(object, name, bool_item, &global_hooks, false)) + { + return bool_item; + } + + cJSON_Delete(bool_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddNumberToObject(cJSON *const object, const char *const name, const double number) +{ + cJSON *number_item = cJSON_CreateNumber(number); + if (add_item_to_object(object, name, number_item, &global_hooks, false)) + { + return number_item; + } + + cJSON_Delete(number_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddStringToObject(cJSON *const object, const char *const name, const char *const string) +{ + cJSON *string_item = cJSON_CreateString(string); + if (add_item_to_object(object, name, string_item, &global_hooks, false)) + { + return string_item; + } + + cJSON_Delete(string_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddRawToObject(cJSON *const object, const char *const name, const char *const raw) +{ + cJSON *raw_item = cJSON_CreateRaw(raw); + if (add_item_to_object(object, name, raw_item, &global_hooks, false)) + { + return raw_item; + } + + cJSON_Delete(raw_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddObjectToObject(cJSON *const object, const char *const name) +{ + cJSON *object_item = cJSON_CreateObject(); + if (add_item_to_object(object, name, object_item, &global_hooks, false)) + { + return object_item; + } + + cJSON_Delete(object_item); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_AddArrayToObject(cJSON *const object, const char *const name) +{ + cJSON *array = cJSON_CreateArray(); + if (add_item_to_object(object, name, array, &global_hooks, false)) + { + return array; + } + + cJSON_Delete(array); + return NULL; +} + +CJSON_PUBLIC(cJSON *) +cJSON_DetachItemViaPointer(cJSON *parent, cJSON *const item) +{ + if ((parent == NULL) || (item == NULL) || (item != parent->child && item->prev == NULL)) + { + return NULL; + } + + if (item != parent->child) + { + /* not the first element */ + item->prev->next = item->next; + } + if (item->next != NULL) + { + /* not the last element */ + item->next->prev = item->prev; + } + + if (item == parent->child) + { + /* first element */ + parent->child = item->next; + } + else if (item->next == NULL) + { + /* last element */ + parent->child->prev = item->prev; + } + + /* make sure the detached item doesn't point anywhere anymore */ + item->prev = NULL; + item->next = NULL; + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_DetachItemFromArray(cJSON *array, int which) +{ + if (which < 0) + { + return NULL; + } + + return cJSON_DetachItemViaPointer(array, get_array_item(array, (size_t)which)); +} + +CJSON_PUBLIC(void) +cJSON_DeleteItemFromArray(cJSON *array, int which) +{ + cJSON_Delete(cJSON_DetachItemFromArray(array, which)); +} + +CJSON_PUBLIC(cJSON *) +cJSON_DetachItemFromObject(cJSON *object, const char *string) +{ + cJSON *to_detach = cJSON_GetObjectItem(object, string); + + return cJSON_DetachItemViaPointer(object, to_detach); +} + +CJSON_PUBLIC(cJSON *) +cJSON_DetachItemFromObjectCaseSensitive(cJSON *object, const char *string) +{ + cJSON *to_detach = cJSON_GetObjectItemCaseSensitive(object, string); + + return cJSON_DetachItemViaPointer(object, to_detach); +} + +CJSON_PUBLIC(void) +cJSON_DeleteItemFromObject(cJSON *object, const char *string) +{ + cJSON_Delete(cJSON_DetachItemFromObject(object, string)); +} + +CJSON_PUBLIC(void) +cJSON_DeleteItemFromObjectCaseSensitive(cJSON *object, const char *string) +{ + cJSON_Delete(cJSON_DetachItemFromObjectCaseSensitive(object, string)); +} + +/* Replace array/object items with new ones. */ +CJSON_PUBLIC(cJSON_bool) +cJSON_InsertItemInArray(cJSON *array, int which, cJSON *newitem) +{ + cJSON *after_inserted = NULL; + + if (which < 0 || newitem == NULL) + { + return false; + } + + after_inserted = get_array_item(array, (size_t)which); + if (after_inserted == NULL) + { + return add_item_to_array(array, newitem); + } + + if (after_inserted != array->child && after_inserted->prev == NULL) + { + /* return false if after_inserted is a corrupted array item */ + return false; + } + + newitem->next = after_inserted; + newitem->prev = after_inserted->prev; + after_inserted->prev = newitem; + if (after_inserted == array->child) + { + array->child = newitem; + } + else + { + newitem->prev->next = newitem; + } + return true; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_ReplaceItemViaPointer(cJSON *const parent, cJSON *const item, cJSON *replacement) +{ + if ((parent == NULL) || (parent->child == NULL) || (replacement == NULL) || (item == NULL)) + { + return false; + } + + if (replacement == item) + { + return true; + } + + replacement->next = item->next; + replacement->prev = item->prev; + + if (replacement->next != NULL) + { + replacement->next->prev = replacement; + } + if (parent->child == item) + { + if (parent->child->prev == parent->child) + { + replacement->prev = replacement; + } + parent->child = replacement; + } + else + { /* + * To find the last item in array quickly, we use prev in array. + * We can't modify the last item's next pointer where this item was the parent's child + */ + if (replacement->prev != NULL) + { + replacement->prev->next = replacement; + } + if (replacement->next == NULL) + { + parent->child->prev = replacement; + } + } + + item->next = NULL; + item->prev = NULL; + cJSON_Delete(item); + + return true; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_ReplaceItemInArray(cJSON *array, int which, cJSON *newitem) +{ + if (which < 0) + { + return false; + } + + return cJSON_ReplaceItemViaPointer(array, get_array_item(array, (size_t)which), newitem); +} + +static cJSON_bool replace_item_in_object(cJSON *object, const char *string, cJSON *replacement, cJSON_bool case_sensitive) +{ + if ((replacement == NULL) || (string == NULL)) + { + return false; + } + + /* replace the name in the replacement */ + if (!(replacement->type & cJSON_StringIsConst) && (replacement->string != NULL)) + { + cJSON_free(replacement->string); + } + replacement->string = (char *)cJSON_strdup((const unsigned char *)string, &global_hooks); + if (replacement->string == NULL) + { + return false; + } + + replacement->type &= ~cJSON_StringIsConst; + + return cJSON_ReplaceItemViaPointer(object, get_object_item(object, string, case_sensitive), replacement); +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_ReplaceItemInObject(cJSON *object, const char *string, cJSON *newitem) +{ + return replace_item_in_object(object, string, newitem, false); +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_ReplaceItemInObjectCaseSensitive(cJSON *object, const char *string, cJSON *newitem) +{ + return replace_item_in_object(object, string, newitem, true); +} + +/* Create basic types: */ +CJSON_PUBLIC(cJSON *) +cJSON_CreateNull(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_NULL; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateTrue(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_True; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateFalse(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_False; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateBool(cJSON_bool boolean) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = boolean ? cJSON_True : cJSON_False; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateNumber(double num) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_Number; + item->valuedouble = num; + + /* use saturation in case of overflow */ + if (num >= INT_MAX) + { + item->valueint = INT_MAX; + } + else if (num <= (double)INT_MIN) + { + item->valueint = INT_MIN; + } + else + { + item->valueint = (int)num; + } + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateString(const char *string) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_String; + item->valuestring = (char *)cJSON_strdup((const unsigned char *)string, &global_hooks); + if (!item->valuestring) + { + cJSON_Delete(item); + return NULL; + } + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateStringReference(const char *string) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item != NULL) + { + item->type = cJSON_String | cJSON_IsReference; + item->valuestring = (char *)cast_away_const(string); + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateObjectReference(const cJSON *child) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item != NULL) + { + item->type = cJSON_Object | cJSON_IsReference; + item->child = (cJSON *)cast_away_const(child); + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateArrayReference(const cJSON *child) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item != NULL) + { + item->type = cJSON_Array | cJSON_IsReference; + item->child = (cJSON *)cast_away_const(child); + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateRaw(const char *raw) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_Raw; + item->valuestring = (char *)cJSON_strdup((const unsigned char *)raw, &global_hooks); + if (!item->valuestring) + { + cJSON_Delete(item); + return NULL; + } + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateArray(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_Array; + } + + return item; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateObject(void) +{ + cJSON *item = cJSON_New_Item(&global_hooks); + if (item) + { + item->type = cJSON_Object; + } + + return item; +} + +/* Create Arrays: */ +CJSON_PUBLIC(cJSON *) +cJSON_CreateIntArray(const int *numbers, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (numbers == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for (i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateNumber(numbers[i]); + if (!n) + { + cJSON_Delete(a); + return NULL; + } + if (!i) + { + a->child = n; + } + else + { + suffix_object(p, n); + } + p = n; + } + + if (a && a->child) + { + a->child->prev = n; + } + + return a; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateFloatArray(const float *numbers, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (numbers == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for (i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateNumber((double)numbers[i]); + if (!n) + { + cJSON_Delete(a); + return NULL; + } + if (!i) + { + a->child = n; + } + else + { + suffix_object(p, n); + } + p = n; + } + + if (a && a->child) + { + a->child->prev = n; + } + + return a; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateDoubleArray(const double *numbers, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (numbers == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for (i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateNumber(numbers[i]); + if (!n) + { + cJSON_Delete(a); + return NULL; + } + if (!i) + { + a->child = n; + } + else + { + suffix_object(p, n); + } + p = n; + } + + if (a && a->child) + { + a->child->prev = n; + } + + return a; +} + +CJSON_PUBLIC(cJSON *) +cJSON_CreateStringArray(const char *const *strings, int count) +{ + size_t i = 0; + cJSON *n = NULL; + cJSON *p = NULL; + cJSON *a = NULL; + + if ((count < 0) || (strings == NULL)) + { + return NULL; + } + + a = cJSON_CreateArray(); + + for (i = 0; a && (i < (size_t)count); i++) + { + n = cJSON_CreateString(strings[i]); + if (!n) + { + cJSON_Delete(a); + return NULL; + } + if (!i) + { + a->child = n; + } + else + { + suffix_object(p, n); + } + p = n; + } + + if (a && a->child) + { + a->child->prev = n; + } + + return a; +} + +/* Duplication */ +cJSON *cJSON_Duplicate_rec(const cJSON *item, size_t depth, cJSON_bool recurse); + +CJSON_PUBLIC(cJSON *) +cJSON_Duplicate(const cJSON *item, cJSON_bool recurse) +{ + return cJSON_Duplicate_rec(item, 0, recurse); +} + +cJSON *cJSON_Duplicate_rec(const cJSON *item, size_t depth, cJSON_bool recurse) +{ + cJSON *newitem = NULL; + cJSON *child = NULL; + cJSON *next = NULL; + cJSON *newchild = NULL; + + /* Bail on bad ptr */ + if (!item) + { + goto fail; + } + /* Create new item */ + newitem = cJSON_New_Item(&global_hooks); + if (!newitem) + { + goto fail; + } + /* Copy over all vars */ + newitem->type = item->type & (~cJSON_IsReference); + newitem->valueint = item->valueint; + newitem->valuedouble = item->valuedouble; + if (item->valuestring) + { + newitem->valuestring = (char *)cJSON_strdup((unsigned char *)item->valuestring, &global_hooks); + if (!newitem->valuestring) + { + goto fail; + } + } + if (item->string) + { + newitem->string = (item->type & cJSON_StringIsConst) ? item->string : (char *)cJSON_strdup((unsigned char *)item->string, &global_hooks); + if (!newitem->string) + { + goto fail; + } + } + /* If non-recursive, then we're done! */ + if (!recurse) + { + return newitem; + } + /* Walk the ->next chain for the child. */ + child = item->child; + while (child != NULL) + { + if (depth >= CJSON_CIRCULAR_LIMIT) + { + goto fail; + } + newchild = cJSON_Duplicate_rec(child, depth + 1, true); /* Duplicate (with recurse) each item in the ->next chain */ + if (!newchild) + { + goto fail; + } + if (next != NULL) + { + /* If newitem->child already set, then crosswire ->prev and ->next and move on */ + next->next = newchild; + newchild->prev = next; + next = newchild; + } + else + { + /* Set newitem->child and move to it */ + newitem->child = newchild; + next = newchild; + } + child = child->next; + } + if (newitem && newitem->child) + { + newitem->child->prev = newchild; + } + + return newitem; + +fail: + if (newitem != NULL) + { + cJSON_Delete(newitem); + } + + return NULL; +} + +static void skip_oneline_comment(char **input) +{ + *input += static_strlen("//"); + + for (; (*input)[0] != '\0'; ++(*input)) + { + if ((*input)[0] == '\n') + { + *input += static_strlen("\n"); + return; + } + } +} + +static void skip_multiline_comment(char **input) +{ + *input += static_strlen("/*"); + + for (; (*input)[0] != '\0'; ++(*input)) + { + if (((*input)[0] == '*') && ((*input)[1] == '/')) + { + *input += static_strlen("*/"); + return; + } + } +} + +static void minify_string(char **input, char **output) +{ + (*output)[0] = (*input)[0]; + *input += static_strlen("\""); + *output += static_strlen("\""); + + for (; (*input)[0] != '\0'; (void)++(*input), ++(*output)) + { + (*output)[0] = (*input)[0]; + + if ((*input)[0] == '\"') + { + (*output)[0] = '\"'; + *input += static_strlen("\""); + *output += static_strlen("\""); + return; + } + else if (((*input)[0] == '\\') && ((*input)[1] == '\"')) + { + (*output)[1] = (*input)[1]; + *input += static_strlen("\""); + *output += static_strlen("\""); + } + } +} + +CJSON_PUBLIC(void) +cJSON_Minify(char *json) +{ + char *into = json; + + if (json == NULL) + { + return; + } + + while (json[0] != '\0') + { + switch (json[0]) + { + case ' ': + case '\t': + case '\r': + case '\n': + json++; + break; + + case '/': + if (json[1] == '/') + { + skip_oneline_comment(&json); + } + else if (json[1] == '*') + { + skip_multiline_comment(&json); + } + else + { + json++; + } + break; + + case '\"': + minify_string(&json, (char **)&into); + break; + + default: + into[0] = json[0]; + json++; + into++; + } + } + + /* and null-terminate. */ + *into = '\0'; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsInvalid(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Invalid; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsFalse(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_False; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsTrue(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xff) == cJSON_True; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsBool(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & (cJSON_True | cJSON_False)) != 0; +} +CJSON_PUBLIC(cJSON_bool) +cJSON_IsNull(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_NULL; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsNumber(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Number; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsString(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_String; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsArray(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Array; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsObject(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Object; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_IsRaw(const cJSON *const item) +{ + if (item == NULL) + { + return false; + } + + return (item->type & 0xFF) == cJSON_Raw; +} + +CJSON_PUBLIC(cJSON_bool) +cJSON_Compare(const cJSON *const a, const cJSON *const b, const cJSON_bool case_sensitive) +{ + if ((a == NULL) || (b == NULL) || ((a->type & 0xFF) != (b->type & 0xFF))) + { + return false; + } + + /* check if type is valid */ + switch (a->type & 0xFF) + { + case cJSON_False: + case cJSON_True: + case cJSON_NULL: + case cJSON_Number: + case cJSON_String: + case cJSON_Raw: + case cJSON_Array: + case cJSON_Object: + break; + + default: + return false; + } + + /* identical objects are equal */ + if (a == b) + { + return true; + } + + switch (a->type & 0xFF) + { + /* in these cases and equal type is enough */ + case cJSON_False: + case cJSON_True: + case cJSON_NULL: + return true; + + case cJSON_Number: + if (compare_double(a->valuedouble, b->valuedouble)) + { + return true; + } + return false; + + case cJSON_String: + case cJSON_Raw: + if ((a->valuestring == NULL) || (b->valuestring == NULL)) + { + return false; + } + if (strcmp(a->valuestring, b->valuestring) == 0) + { + return true; + } + + return false; + + case cJSON_Array: + { + cJSON *a_element = a->child; + cJSON *b_element = b->child; + + for (; (a_element != NULL) && (b_element != NULL);) + { + if (!cJSON_Compare(a_element, b_element, case_sensitive)) + { + return false; + } + + a_element = a_element->next; + b_element = b_element->next; + } + + /* one of the arrays is longer than the other */ + if (a_element != b_element) + { + return false; + } + + return true; + } + + case cJSON_Object: + { + cJSON *a_element = NULL; + cJSON *b_element = NULL; + cJSON_ArrayForEach(a_element, a) + { + /* TODO This has O(n^2) runtime, which is horrible! */ + b_element = get_object_item(b, a_element->string, case_sensitive); + if (b_element == NULL) + { + return false; + } + + if (!cJSON_Compare(a_element, b_element, case_sensitive)) + { + return false; + } + } + + /* doing this twice, once on a and b to prevent true comparison if a subset of b + * TODO: Do this the proper way, this is just a fix for now */ + cJSON_ArrayForEach(b_element, b) + { + a_element = get_object_item(a, b_element->string, case_sensitive); + if (a_element == NULL) + { + return false; + } + + if (!cJSON_Compare(b_element, a_element, case_sensitive)) + { + return false; + } + } + + return true; + } + + default: + return false; + } +} + +CJSON_PUBLIC(void *) +cJSON_malloc(size_t size) +{ + return global_hooks.allocate(size); +} + +CJSON_PUBLIC(void) +cJSON_free(void *object) +{ + global_hooks.deallocate(object); + object = NULL; +} \ No newline at end of file diff --git a/robot/v4l2/OmniSocketGo_robot/third_party/cjson/cJSON.h b/robot/v4l2/OmniSocketGo_robot/third_party/cjson/cJSON.h new file mode 100644 index 0000000..c760c95 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/third_party/cjson/cJSON.h @@ -0,0 +1,381 @@ +/* + Copyright (c) 2009-2017 Dave Gamble and cJSON contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#ifndef cJSON__h +#define cJSON__h + +#ifdef __cplusplus +extern "C" +{ +#endif + +#if !defined(__WINDOWS__) && (defined(WIN32) || defined(WIN64) || defined(_MSC_VER) || defined(_WIN32)) +#define __WINDOWS__ +#endif + +#ifdef __WINDOWS__ + + /* When compiling for windows, we specify a specific calling convention to avoid issues where we are being called from a project with a different default calling convention. For windows you have 3 define options: + + CJSON_HIDE_SYMBOLS - Define this in the case where you don't want to ever dllexport symbols + CJSON_EXPORT_SYMBOLS - Define this on library build when you want to dllexport symbols (default) + CJSON_IMPORT_SYMBOLS - Define this if you want to dllimport symbol + + For *nix builds that support visibility attribute, you can define similar behavior by + + setting default visibility to hidden by adding + -fvisibility=hidden (for gcc) + or + -xldscope=hidden (for sun cc) + to CFLAGS + + then using the CJSON_API_VISIBILITY flag to "export" the same symbols the way CJSON_EXPORT_SYMBOLS does + + */ + +#define CJSON_CDECL __cdecl +#define CJSON_STDCALL __stdcall + +/* export symbols by default, this is necessary for copy pasting the C and header file */ +#if !defined(CJSON_HIDE_SYMBOLS) && !defined(CJSON_IMPORT_SYMBOLS) && !defined(CJSON_EXPORT_SYMBOLS) +#define CJSON_EXPORT_SYMBOLS +#endif + +#if defined(CJSON_HIDE_SYMBOLS) +#define CJSON_PUBLIC(type) type CJSON_STDCALL +#elif defined(CJSON_EXPORT_SYMBOLS) +#define CJSON_PUBLIC(type) __declspec(dllexport) type CJSON_STDCALL +#elif defined(CJSON_IMPORT_SYMBOLS) +#define CJSON_PUBLIC(type) __declspec(dllimport) type CJSON_STDCALL +#endif +#else /* !__WINDOWS__ */ +#define CJSON_CDECL +#define CJSON_STDCALL + +#if (defined(__GNUC__) || defined(__SUNPRO_CC) || defined(__SUNPRO_C)) && defined(CJSON_API_VISIBILITY) +#define CJSON_PUBLIC(type) __attribute__((visibility("default"))) type +#else +#define CJSON_PUBLIC(type) type +#endif +#endif + +/* project version */ +#define CJSON_VERSION_MAJOR 1 +#define CJSON_VERSION_MINOR 7 +#define CJSON_VERSION_PATCH 19 + +#include + +/* cJSON Types: */ +#define cJSON_Invalid (0) +#define cJSON_False (1 << 0) +#define cJSON_True (1 << 1) +#define cJSON_NULL (1 << 2) +#define cJSON_Number (1 << 3) +#define cJSON_String (1 << 4) +#define cJSON_Array (1 << 5) +#define cJSON_Object (1 << 6) +#define cJSON_Raw (1 << 7) /* raw json */ + +#define cJSON_IsReference 256 +#define cJSON_StringIsConst 512 + + /* The cJSON structure: */ + typedef struct cJSON + { + /* next/prev allow you to walk array/object chains. Alternatively, use GetArraySize/GetArrayItem/GetObjectItem */ + struct cJSON *next; + struct cJSON *prev; + /* An array or object item will have a child pointer pointing to a chain of the items in the array/object. */ + struct cJSON *child; + + /* The type of the item, as above. */ + int type; + + /* The item's string, if type==cJSON_String and type == cJSON_Raw */ + char *valuestring; + /* writing to valueint is DEPRECATED, use cJSON_SetNumberValue instead */ + int valueint; + /* The item's number, if type==cJSON_Number */ + double valuedouble; + + /* The item's name string, if this item is the child of, or is in the list of subitems of an object. */ + char *string; + } cJSON; + + typedef struct cJSON_Hooks + { + /* malloc/free are CDECL on Windows regardless of the default calling convention of the compiler, so ensure the hooks allow passing those functions directly. */ + void *(CJSON_CDECL *malloc_fn)(size_t sz); + void(CJSON_CDECL *free_fn)(void *ptr); + } cJSON_Hooks; + + typedef int cJSON_bool; + +/* Limits how deeply nested arrays/objects can be before cJSON rejects to parse them. + * This is to prevent stack overflows. */ +#ifndef CJSON_NESTING_LIMIT +#define CJSON_NESTING_LIMIT 1000 +#endif + +/* Limits the length of circular references can be before cJSON rejects to parse them. + * This is to prevent stack overflows. */ +#ifndef CJSON_CIRCULAR_LIMIT +#define CJSON_CIRCULAR_LIMIT 10000 +#endif + + /* returns the version of cJSON as a string */ + CJSON_PUBLIC(const char *) + cJSON_Version(void); + + /* Supply malloc, realloc and free functions to cJSON */ + CJSON_PUBLIC(void) + cJSON_InitHooks(cJSON_Hooks *hooks); + + /* Memory Management: the caller is always responsible to free the results from all variants of cJSON_Parse (with cJSON_Delete) and cJSON_Print (with stdlib free, cJSON_Hooks.free_fn, or cJSON_free as appropriate). The exception is cJSON_PrintPreallocated, where the caller has full responsibility of the buffer. */ + /* Supply a block of JSON, and this returns a cJSON object you can interrogate. */ + CJSON_PUBLIC(cJSON *) + cJSON_Parse(const char *value); + CJSON_PUBLIC(cJSON *) + cJSON_ParseWithLength(const char *value, size_t buffer_length); + /* ParseWithOpts allows you to require (and check) that the JSON is null terminated, and to retrieve the pointer to the final byte parsed. */ + /* If you supply a ptr in return_parse_end and parsing fails, then return_parse_end will contain a pointer to the error so will match cJSON_GetErrorPtr(). */ + CJSON_PUBLIC(cJSON *) + cJSON_ParseWithOpts(const char *value, const char **return_parse_end, cJSON_bool require_null_terminated); + CJSON_PUBLIC(cJSON *) + cJSON_ParseWithLengthOpts(const char *value, size_t buffer_length, const char **return_parse_end, cJSON_bool require_null_terminated); + + /* Render a cJSON entity to text for transfer/storage. */ + CJSON_PUBLIC(char *) + cJSON_Print(const cJSON *item); + /* Render a cJSON entity to text for transfer/storage without any formatting. */ + CJSON_PUBLIC(char *) + cJSON_PrintUnformatted(const cJSON *item); + /* Render a cJSON entity to text using a buffered strategy. prebuffer is a guess at the final size. guessing well reduces reallocation. fmt=0 gives unformatted, =1 gives formatted */ + CJSON_PUBLIC(char *) + cJSON_PrintBuffered(const cJSON *item, int prebuffer, cJSON_bool fmt); + /* Render a cJSON entity to text using a buffer already allocated in memory with given length. Returns 1 on success and 0 on failure. */ + /* NOTE: cJSON is not always 100% accurate in estimating how much memory it will use, so to be safe allocate 5 bytes more than you actually need */ + CJSON_PUBLIC(cJSON_bool) + cJSON_PrintPreallocated(cJSON *item, char *buffer, const int length, const cJSON_bool format); + /* Delete a cJSON entity and all subentities. */ + CJSON_PUBLIC(void) + cJSON_Delete(cJSON *item); + + /* Returns the number of items in an array (or object). */ + CJSON_PUBLIC(int) + cJSON_GetArraySize(const cJSON *array); + /* Retrieve item number "index" from array "array". Returns NULL if unsuccessful. */ + CJSON_PUBLIC(cJSON *) + cJSON_GetArrayItem(const cJSON *array, int index); + /* Get item "string" from object. Case insensitive. */ + CJSON_PUBLIC(cJSON *) + cJSON_GetObjectItem(const cJSON *const object, const char *const string); + CJSON_PUBLIC(cJSON *) + cJSON_GetObjectItemCaseSensitive(const cJSON *const object, const char *const string); + CJSON_PUBLIC(cJSON_bool) + cJSON_HasObjectItem(const cJSON *object, const char *string); + /* For analysing failed parses. This returns a pointer to the parse error. You'll probably need to look a few chars back to make sense of it. Defined when cJSON_Parse() returns 0. 0 when cJSON_Parse() succeeds. */ + CJSON_PUBLIC(const char *) + cJSON_GetErrorPtr(void); + + /* Check item type and return its value */ + CJSON_PUBLIC(char *) + cJSON_GetStringValue(const cJSON *const item); + CJSON_PUBLIC(double) + cJSON_GetNumberValue(const cJSON *const item); + + /* These functions check the type of an item */ + CJSON_PUBLIC(cJSON_bool) + cJSON_IsInvalid(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsFalse(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsTrue(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsBool(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsNull(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsNumber(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsString(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsArray(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsObject(const cJSON *const item); + CJSON_PUBLIC(cJSON_bool) + cJSON_IsRaw(const cJSON *const item); + + /* These calls create a cJSON item of the appropriate type. */ + CJSON_PUBLIC(cJSON *) + cJSON_CreateNull(void); + CJSON_PUBLIC(cJSON *) + cJSON_CreateTrue(void); + CJSON_PUBLIC(cJSON *) + cJSON_CreateFalse(void); + CJSON_PUBLIC(cJSON *) + cJSON_CreateBool(cJSON_bool boolean); + CJSON_PUBLIC(cJSON *) + cJSON_CreateNumber(double num); + CJSON_PUBLIC(cJSON *) + cJSON_CreateString(const char *string); + /* raw json */ + CJSON_PUBLIC(cJSON *) + cJSON_CreateRaw(const char *raw); + CJSON_PUBLIC(cJSON *) + cJSON_CreateArray(void); + CJSON_PUBLIC(cJSON *) + cJSON_CreateObject(void); + + /* Create a string where valuestring references a string so + * it will not be freed by cJSON_Delete */ + CJSON_PUBLIC(cJSON *) + cJSON_CreateStringReference(const char *string); + /* Create an object/array that only references it's elements so + * they will not be freed by cJSON_Delete */ + CJSON_PUBLIC(cJSON *) + cJSON_CreateObjectReference(const cJSON *child); + CJSON_PUBLIC(cJSON *) + cJSON_CreateArrayReference(const cJSON *child); + + /* These utilities create an Array of count items. + * The parameter count cannot be greater than the number of elements in the number array, otherwise array access will be out of bounds.*/ + CJSON_PUBLIC(cJSON *) + cJSON_CreateIntArray(const int *numbers, int count); + CJSON_PUBLIC(cJSON *) + cJSON_CreateFloatArray(const float *numbers, int count); + CJSON_PUBLIC(cJSON *) + cJSON_CreateDoubleArray(const double *numbers, int count); + CJSON_PUBLIC(cJSON *) + cJSON_CreateStringArray(const char *const *strings, int count); + + /* Append item to the specified array/object. */ + CJSON_PUBLIC(cJSON_bool) + cJSON_AddItemToArray(cJSON *array, cJSON *item); + CJSON_PUBLIC(cJSON_bool) + cJSON_AddItemToObject(cJSON *object, const char *string, cJSON *item); + /* Use this when string is definitely const (i.e. a literal, or as good as), and will definitely survive the cJSON object. + * WARNING: When this function was used, make sure to always check that (item->type & cJSON_StringIsConst) is zero before + * writing to `item->string` */ + CJSON_PUBLIC(cJSON_bool) + cJSON_AddItemToObjectCS(cJSON *object, const char *string, cJSON *item); + /* Append reference to item to the specified array/object. Use this when you want to add an existing cJSON to a new cJSON, but don't want to corrupt your existing cJSON. */ + CJSON_PUBLIC(cJSON_bool) + cJSON_AddItemReferenceToArray(cJSON *array, cJSON *item); + CJSON_PUBLIC(cJSON_bool) + cJSON_AddItemReferenceToObject(cJSON *object, const char *string, cJSON *item); + + /* Remove/Detach items from Arrays/Objects. */ + CJSON_PUBLIC(cJSON *) + cJSON_DetachItemViaPointer(cJSON *parent, cJSON *const item); + CJSON_PUBLIC(cJSON *) + cJSON_DetachItemFromArray(cJSON *array, int which); + CJSON_PUBLIC(void) + cJSON_DeleteItemFromArray(cJSON *array, int which); + CJSON_PUBLIC(cJSON *) + cJSON_DetachItemFromObject(cJSON *object, const char *string); + CJSON_PUBLIC(cJSON *) + cJSON_DetachItemFromObjectCaseSensitive(cJSON *object, const char *string); + CJSON_PUBLIC(void) + cJSON_DeleteItemFromObject(cJSON *object, const char *string); + CJSON_PUBLIC(void) + cJSON_DeleteItemFromObjectCaseSensitive(cJSON *object, const char *string); + + /* Update array items. */ + CJSON_PUBLIC(cJSON_bool) + cJSON_InsertItemInArray(cJSON *array, int which, cJSON *newitem); /* Shifts pre-existing items to the right. */ + CJSON_PUBLIC(cJSON_bool) + cJSON_ReplaceItemViaPointer(cJSON *const parent, cJSON *const item, cJSON *replacement); + CJSON_PUBLIC(cJSON_bool) + cJSON_ReplaceItemInArray(cJSON *array, int which, cJSON *newitem); + CJSON_PUBLIC(cJSON_bool) + cJSON_ReplaceItemInObject(cJSON *object, const char *string, cJSON *newitem); + CJSON_PUBLIC(cJSON_bool) + cJSON_ReplaceItemInObjectCaseSensitive(cJSON *object, const char *string, cJSON *newitem); + + /* Duplicate a cJSON item */ + CJSON_PUBLIC(cJSON *) + cJSON_Duplicate(const cJSON *item, cJSON_bool recurse); + /* Duplicate will create a new, identical cJSON item to the one you pass, in new memory that will + * need to be released. With recurse!=0, it will duplicate any children connected to the item. + * The item->next and ->prev pointers are always zero on return from Duplicate. */ + /* Recursively compare two cJSON items for equality. If either a or b is NULL or invalid, they will be considered unequal. + * case_sensitive determines if object keys are treated case sensitive (1) or case insensitive (0) */ + CJSON_PUBLIC(cJSON_bool) + cJSON_Compare(const cJSON *const a, const cJSON *const b, const cJSON_bool case_sensitive); + + /* Minify a strings, remove blank characters(such as ' ', '\t', '\r', '\n') from strings. + * The input pointer json cannot point to a read-only address area, such as a string constant, + * but should point to a readable and writable address area. */ + CJSON_PUBLIC(void) + cJSON_Minify(char *json); + + /* Helper functions for creating and adding items to an object at the same time. + * They return the added item or NULL on failure. */ + CJSON_PUBLIC(cJSON *) + cJSON_AddNullToObject(cJSON *const object, const char *const name); + CJSON_PUBLIC(cJSON *) + cJSON_AddTrueToObject(cJSON *const object, const char *const name); + CJSON_PUBLIC(cJSON *) + cJSON_AddFalseToObject(cJSON *const object, const char *const name); + CJSON_PUBLIC(cJSON *) + cJSON_AddBoolToObject(cJSON *const object, const char *const name, const cJSON_bool boolean); + CJSON_PUBLIC(cJSON *) + cJSON_AddNumberToObject(cJSON *const object, const char *const name, const double number); + CJSON_PUBLIC(cJSON *) + cJSON_AddStringToObject(cJSON *const object, const char *const name, const char *const string); + CJSON_PUBLIC(cJSON *) + cJSON_AddRawToObject(cJSON *const object, const char *const name, const char *const raw); + CJSON_PUBLIC(cJSON *) + cJSON_AddObjectToObject(cJSON *const object, const char *const name); + CJSON_PUBLIC(cJSON *) + cJSON_AddArrayToObject(cJSON *const object, const char *const name); + +/* When assigning an integer value, it needs to be propagated to valuedouble too. */ +#define cJSON_SetIntValue(object, number) ((object) ? (object)->valueint = (object)->valuedouble = (number) : (number)) + /* helper for the cJSON_SetNumberValue macro */ + CJSON_PUBLIC(double) + cJSON_SetNumberHelper(cJSON *object, double number); +#define cJSON_SetNumberValue(object, number) ((object != NULL) ? cJSON_SetNumberHelper(object, (double)number) : (number)) + /* Change the valuestring of a cJSON_String object, only takes effect when type of object is cJSON_String */ + CJSON_PUBLIC(char *) + cJSON_SetValuestring(cJSON *object, const char *valuestring); + +/* If the object is not a boolean type this does nothing and returns cJSON_Invalid else it returns the new type*/ +#define cJSON_SetBoolValue(object, boolValue) ( \ + (object != NULL && ((object)->type & (cJSON_False | cJSON_True))) ? (object)->type = ((object)->type & (~(cJSON_False | cJSON_True))) | ((boolValue) ? cJSON_True : cJSON_False) : cJSON_Invalid) + +/* Macro for iterating over an array or object */ +#define cJSON_ArrayForEach(element, array) for (element = (array != NULL) ? (array)->child : NULL; element != NULL; element = element->next) + + /* malloc/free objects using the malloc/free functions that have been set with cJSON_InitHooks */ + CJSON_PUBLIC(void *) + cJSON_malloc(size_t size); + CJSON_PUBLIC(void) + cJSON_free(void *object); + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file diff --git a/robot/v4l2/OmniSocketGo_robot/third_party/kcp/ikcp.c b/robot/v4l2/OmniSocketGo_robot/third_party/kcp/ikcp.c new file mode 100644 index 0000000..593ae41 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/third_party/kcp/ikcp.c @@ -0,0 +1,1466 @@ +//===================================================================== +// +// KCP - A Better ARQ Protocol Implementation +// skywind3000 (at) gmail.com, 2010-2011 +// +// Features: +// + Average RTT reduce 30% - 40% vs traditional ARQ like tcp. +// + Maximum RTT reduce three times vs tcp. +// + Lightweight, distributed as a single source file. +// +//===================================================================== +#include "ikcp.h" + +#include +#include +#include +#include +#include + +#define IKCP_FASTACK_CONSERVE + +//===================================================================== +// KCP BASIC +//===================================================================== +const IUINT32 IKCP_RTO_NDL = 30; // no delay min rto +const IUINT32 IKCP_RTO_MIN = 100; // normal min rto +const IUINT32 IKCP_RTO_DEF = 200; +const IUINT32 IKCP_RTO_MAX = 60000; +const IUINT32 IKCP_CMD_PUSH = 81; // cmd: push data +const IUINT32 IKCP_CMD_ACK = 82; // cmd: ack +const IUINT32 IKCP_CMD_WASK = 83; // cmd: window probe (ask) +const IUINT32 IKCP_CMD_WINS = 84; // cmd: window size (tell) +const IUINT32 IKCP_ASK_SEND = 1; // need to send IKCP_CMD_WASK +const IUINT32 IKCP_ASK_TELL = 2; // need to send IKCP_CMD_WINS +const IUINT32 IKCP_WND_SND = 32; +const IUINT32 IKCP_WND_RCV = 128; // must >= max fragment size +const IUINT32 IKCP_MTU_DEF = 1400; +const IUINT32 IKCP_ACK_FAST = 3; +const IUINT32 IKCP_INTERVAL = 100; +const IUINT32 IKCP_OVERHEAD = 24; +const IUINT32 IKCP_DEADLINK = 20; +const IUINT32 IKCP_THRESH_INIT = 2; +const IUINT32 IKCP_THRESH_MIN = 2; +const IUINT32 IKCP_PROBE_INIT = 7000; // 7 secs to probe window size +const IUINT32 IKCP_PROBE_LIMIT = 120000; // up to 120 secs to probe window +const IUINT32 IKCP_FASTACK_LIMIT = 5; // max times to trigger fastack + +//--------------------------------------------------------------------- +// encode / decode +//--------------------------------------------------------------------- + +/* encode 8 bits unsigned int */ +static inline char *ikcp_encode8u(char *p, unsigned char c) +{ + *(unsigned char *)p++ = c; + return p; +} + +/* decode 8 bits unsigned int */ +static inline const char *ikcp_decode8u(const char *p, unsigned char *c) +{ + *c = *(unsigned char *)p++; + return p; +} + +/* encode 16 bits unsigned int (lsb) */ +static inline char *ikcp_encode16u(char *p, unsigned short w) +{ +#if IWORDS_BIG_ENDIAN || IWORDS_MUST_ALIGN + *(unsigned char *)(p + 0) = (w & 255); + *(unsigned char *)(p + 1) = (w >> 8); +#else + memcpy(p, &w, 2); +#endif + p += 2; + return p; +} + +/* decode 16 bits unsigned int (lsb) */ +static inline const char *ikcp_decode16u(const char *p, unsigned short *w) +{ +#if IWORDS_BIG_ENDIAN || IWORDS_MUST_ALIGN + *w = *(const unsigned char *)(p + 1); + *w = *(const unsigned char *)(p + 0) + (*w << 8); +#else + memcpy(w, p, 2); +#endif + p += 2; + return p; +} + +/* encode 32 bits unsigned int (lsb) */ +static inline char *ikcp_encode32u(char *p, IUINT32 l) +{ +#if IWORDS_BIG_ENDIAN || IWORDS_MUST_ALIGN + *(unsigned char *)(p + 0) = (unsigned char)((l >> 0) & 0xff); + *(unsigned char *)(p + 1) = (unsigned char)((l >> 8) & 0xff); + *(unsigned char *)(p + 2) = (unsigned char)((l >> 16) & 0xff); + *(unsigned char *)(p + 3) = (unsigned char)((l >> 24) & 0xff); +#else + memcpy(p, &l, 4); +#endif + p += 4; + return p; +} + +/* decode 32 bits unsigned int (lsb) */ +static inline const char *ikcp_decode32u(const char *p, IUINT32 *l) +{ +#if IWORDS_BIG_ENDIAN || IWORDS_MUST_ALIGN + *l = *(const unsigned char *)(p + 3); + *l = *(const unsigned char *)(p + 2) + (*l << 8); + *l = *(const unsigned char *)(p + 1) + (*l << 8); + *l = *(const unsigned char *)(p + 0) + (*l << 8); +#else + memcpy(l, p, 4); +#endif + p += 4; + return p; +} + +static inline IUINT32 _imin_(IUINT32 a, IUINT32 b) +{ + return a <= b ? a : b; +} + +static inline IUINT32 _imax_(IUINT32 a, IUINT32 b) +{ + return a >= b ? a : b; +} + +static inline IUINT32 _ibound_(IUINT32 lower, IUINT32 middle, IUINT32 upper) +{ + return _imin_(_imax_(lower, middle), upper); +} + +static inline long _itimediff(IUINT32 later, IUINT32 earlier) +{ + return ((IINT32)(later - earlier)); +} + +//--------------------------------------------------------------------- +// manage segment +//--------------------------------------------------------------------- +typedef struct IKCPSEG IKCPSEG; + +static void *(*ikcp_malloc_hook)(size_t) = NULL; +static void (*ikcp_free_hook)(void *) = NULL; + +// internal malloc +static void *ikcp_malloc(size_t size) +{ + if (ikcp_malloc_hook) + return ikcp_malloc_hook(size); + return malloc(size); +} + +// internal free +static void ikcp_free(void *ptr) +{ + if (ikcp_free_hook) + { + ikcp_free_hook(ptr); + } + else + { + free(ptr); + } +} + +// redefine allocator +void ikcp_allocator(void *(*new_malloc)(size_t), void (*new_free)(void *)) +{ + ikcp_malloc_hook = new_malloc; + ikcp_free_hook = new_free; +} + +// allocate a new kcp segment +static IKCPSEG *ikcp_segment_new(ikcpcb *kcp, int size) +{ + return (IKCPSEG *)ikcp_malloc(sizeof(IKCPSEG) + size); +} + +// delete a segment +static void ikcp_segment_delete(ikcpcb *kcp, IKCPSEG *seg) +{ + ikcp_free(seg); +} + +// write log +void ikcp_log(ikcpcb *kcp, int mask, const char *fmt, ...) +{ + char buffer[1024]; + va_list argptr; + if ((mask & kcp->logmask) == 0 || kcp->writelog == 0) + return; + va_start(argptr, fmt); + vsprintf(buffer, fmt, argptr); + va_end(argptr); + kcp->writelog(buffer, kcp, kcp->user); +} + +// check log mask +static int ikcp_canlog(const ikcpcb *kcp, int mask) +{ + if ((mask & kcp->logmask) == 0 || kcp->writelog == NULL) + return 0; + return 1; +} + +// output segment +static int ikcp_output(ikcpcb *kcp, const void *data, int size) +{ + assert(kcp); + assert(kcp->output); + if (ikcp_canlog(kcp, IKCP_LOG_OUTPUT)) + { + ikcp_log(kcp, IKCP_LOG_OUTPUT, "[RO] %ld bytes", (long)size); + } + if (size == 0) + return 0; + return kcp->output((const char *)data, size, kcp, kcp->user); +} + +// output queue +void ikcp_qprint(const char *name, const struct IQUEUEHEAD *head) +{ +#if 0 + const struct IQUEUEHEAD *p; + printf("<%s>: [", name); + for (p = head->next; p != head; p = p->next) { + const IKCPSEG *seg = iqueue_entry(p, const IKCPSEG, node); + printf("(%lu %d)", (unsigned long)seg->sn, (int)(seg->ts % 10000)); + if (p->next != head) printf(","); + } + printf("]\n"); +#endif +} + +//--------------------------------------------------------------------- +// create a new kcpcb +//--------------------------------------------------------------------- +ikcpcb *ikcp_create(IUINT32 conv, void *user) +{ + ikcpcb *kcp = (ikcpcb *)ikcp_malloc(sizeof(struct IKCPCB)); + if (kcp == NULL) + return NULL; + kcp->conv = conv; + kcp->user = user; + kcp->snd_una = 0; + kcp->snd_nxt = 0; + kcp->rcv_nxt = 0; + kcp->ts_recent = 0; + kcp->ts_lastack = 0; + kcp->ts_probe = 0; + kcp->probe_wait = 0; + kcp->snd_wnd = IKCP_WND_SND; + kcp->rcv_wnd = IKCP_WND_RCV; + kcp->rmt_wnd = IKCP_WND_RCV; + kcp->cwnd = 0; + kcp->incr = 0; + kcp->probe = 0; + kcp->mtu = IKCP_MTU_DEF; + kcp->mss = kcp->mtu - IKCP_OVERHEAD; + kcp->stream = 0; + + kcp->buffer = (char *)ikcp_malloc((kcp->mtu + IKCP_OVERHEAD) * 3); + if (kcp->buffer == NULL) + { + ikcp_free(kcp); + return NULL; + } + + iqueue_init(&kcp->snd_queue); + iqueue_init(&kcp->rcv_queue); + iqueue_init(&kcp->snd_buf); + iqueue_init(&kcp->rcv_buf); + kcp->nrcv_buf = 0; + kcp->nsnd_buf = 0; + kcp->nrcv_que = 0; + kcp->nsnd_que = 0; + kcp->state = 0; + kcp->acklist = NULL; + kcp->ackblock = 0; + kcp->ackcount = 0; + kcp->rx_srtt = 0; + kcp->rx_rttval = 0; + kcp->rx_rto = IKCP_RTO_DEF; + kcp->rx_minrto = IKCP_RTO_MIN; + kcp->current = 0; + kcp->interval = IKCP_INTERVAL; + kcp->ts_flush = IKCP_INTERVAL; + kcp->nodelay = 0; + kcp->updated = 0; + kcp->logmask = 0; + kcp->ssthresh = IKCP_THRESH_INIT; + kcp->fastresend = 0; + kcp->fastlimit = IKCP_FASTACK_LIMIT; + kcp->nocwnd = 0; + kcp->xmit = 0; + kcp->timeout_retrans_total = 0; + kcp->fast_retrans_total = 0; + kcp->duplicate_recv_total = 0; + kcp->dead_link = IKCP_DEADLINK; + kcp->output = NULL; + kcp->writelog = NULL; + + return kcp; +} + +//--------------------------------------------------------------------- +// release a new kcpcb +//--------------------------------------------------------------------- +void ikcp_release(ikcpcb *kcp) +{ + assert(kcp); + if (kcp) + { + IKCPSEG *seg; + while (!iqueue_is_empty(&kcp->snd_buf)) + { + seg = iqueue_entry(kcp->snd_buf.next, IKCPSEG, node); + iqueue_del(&seg->node); + ikcp_segment_delete(kcp, seg); + } + while (!iqueue_is_empty(&kcp->rcv_buf)) + { + seg = iqueue_entry(kcp->rcv_buf.next, IKCPSEG, node); + iqueue_del(&seg->node); + ikcp_segment_delete(kcp, seg); + } + while (!iqueue_is_empty(&kcp->snd_queue)) + { + seg = iqueue_entry(kcp->snd_queue.next, IKCPSEG, node); + iqueue_del(&seg->node); + ikcp_segment_delete(kcp, seg); + } + while (!iqueue_is_empty(&kcp->rcv_queue)) + { + seg = iqueue_entry(kcp->rcv_queue.next, IKCPSEG, node); + iqueue_del(&seg->node); + ikcp_segment_delete(kcp, seg); + } + if (kcp->buffer) + { + ikcp_free(kcp->buffer); + } + if (kcp->acklist) + { + ikcp_free(kcp->acklist); + } + + kcp->nrcv_buf = 0; + kcp->nsnd_buf = 0; + kcp->nrcv_que = 0; + kcp->nsnd_que = 0; + kcp->ackcount = 0; + kcp->buffer = NULL; + kcp->acklist = NULL; + ikcp_free(kcp); + } +} + +//--------------------------------------------------------------------- +// set output callback, which will be invoked by kcp +//--------------------------------------------------------------------- +void ikcp_setoutput(ikcpcb *kcp, int (*output)(const char *buf, int len, + ikcpcb *kcp, void *user)) +{ + kcp->output = output; +} + +//--------------------------------------------------------------------- +// user/upper level recv: returns size, returns below zero for EAGAIN +//--------------------------------------------------------------------- +int ikcp_recv(ikcpcb *kcp, char *buffer, int len) +{ + struct IQUEUEHEAD *p; + int ispeek = (len < 0) ? 1 : 0; + int peeksize; + int recover = 0; + IKCPSEG *seg; + assert(kcp); + + if (iqueue_is_empty(&kcp->rcv_queue)) + return -1; + + if (len < 0) + len = -len; + + peeksize = ikcp_peeksize(kcp); + + if (peeksize < 0) + return -2; + + if (peeksize > len) + return -3; + + if (kcp->nrcv_que >= kcp->rcv_wnd) + recover = 1; + + // merge fragment + for (len = 0, p = kcp->rcv_queue.next; p != &kcp->rcv_queue;) + { + int fragment; + seg = iqueue_entry(p, IKCPSEG, node); + p = p->next; + + if (buffer) + { + memcpy(buffer, seg->data, seg->len); + buffer += seg->len; + } + + len += seg->len; + fragment = seg->frg; + + if (ikcp_canlog(kcp, IKCP_LOG_RECV)) + { + ikcp_log(kcp, IKCP_LOG_RECV, "recv sn=%lu", (unsigned long)seg->sn); + } + + if (ispeek == 0) + { + iqueue_del(&seg->node); + ikcp_segment_delete(kcp, seg); + kcp->nrcv_que--; + } + + if (fragment == 0) + break; + } + + assert(len == peeksize); + + // move available data from rcv_buf -> rcv_queue + while (!iqueue_is_empty(&kcp->rcv_buf)) + { + seg = iqueue_entry(kcp->rcv_buf.next, IKCPSEG, node); + if (seg->sn == kcp->rcv_nxt && kcp->nrcv_que < kcp->rcv_wnd) + { + iqueue_del(&seg->node); + kcp->nrcv_buf--; + iqueue_add_tail(&seg->node, &kcp->rcv_queue); + kcp->nrcv_que++; + kcp->rcv_nxt++; + } + else + { + break; + } + } + + // fast recover + if (kcp->nrcv_que < kcp->rcv_wnd && recover) + { + // ready to send back IKCP_CMD_WINS in ikcp_flush + // tell remote my window size + kcp->probe |= IKCP_ASK_TELL; + } + + return len; +} + +//--------------------------------------------------------------------- +// peek data size +//--------------------------------------------------------------------- +int ikcp_peeksize(const ikcpcb *kcp) +{ + struct IQUEUEHEAD *p; + IKCPSEG *seg; + int length = 0; + + assert(kcp); + + if (iqueue_is_empty(&kcp->rcv_queue)) + return -1; + + seg = iqueue_entry(kcp->rcv_queue.next, IKCPSEG, node); + if (seg->frg == 0) + return seg->len; + + if (kcp->nrcv_que < seg->frg + 1) + return -1; + + for (p = kcp->rcv_queue.next; p != &kcp->rcv_queue; p = p->next) + { + seg = iqueue_entry(p, IKCPSEG, node); + length += seg->len; + if (seg->frg == 0) + break; + } + + return length; +} + +//--------------------------------------------------------------------- +// user/upper level send, returns below zero for error +//--------------------------------------------------------------------- +int ikcp_send(ikcpcb *kcp, const char *buffer, int len) +{ + IKCPSEG *seg; + int count, i; + int sent = 0; + + assert(kcp->mss > 0); + if (len < 0) + return -1; + + // append to previous segment in streaming mode (if possible) + if (kcp->stream != 0) + { + if (!iqueue_is_empty(&kcp->snd_queue)) + { + IKCPSEG *old = iqueue_entry(kcp->snd_queue.prev, IKCPSEG, node); + if (old->len < kcp->mss) + { + int capacity = kcp->mss - old->len; + int extend = (len < capacity) ? len : capacity; + seg = ikcp_segment_new(kcp, old->len + extend); + assert(seg); + if (seg == NULL) + { + return -2; + } + iqueue_add_tail(&seg->node, &kcp->snd_queue); + memcpy(seg->data, old->data, old->len); + if (buffer) + { + memcpy(seg->data + old->len, buffer, extend); + buffer += extend; + } + seg->len = old->len + extend; + seg->frg = 0; + len -= extend; + iqueue_del_init(&old->node); + ikcp_segment_delete(kcp, old); + sent = extend; + } + } + if (len <= 0) + { + return sent; + } + } + + if (len <= (int)kcp->mss) + count = 1; + else + count = (len + kcp->mss - 1) / kcp->mss; + + if (count >= (int)IKCP_WND_RCV) + { + if (kcp->stream != 0 && sent > 0) + return sent; + return -2; + } + + if (count == 0) + count = 1; + + // fragment + for (i = 0; i < count; i++) + { + int size = len > (int)kcp->mss ? (int)kcp->mss : len; + seg = ikcp_segment_new(kcp, size); + assert(seg); + if (seg == NULL) + { + return -2; + } + if (buffer && len > 0) + { + memcpy(seg->data, buffer, size); + } + seg->len = size; + seg->frg = (kcp->stream == 0) ? (count - i - 1) : 0; + iqueue_init(&seg->node); + iqueue_add_tail(&seg->node, &kcp->snd_queue); + kcp->nsnd_que++; + if (buffer) + { + buffer += size; + } + len -= size; + sent += size; + } + + return sent; +} + +//--------------------------------------------------------------------- +// parse ack +//--------------------------------------------------------------------- +static void ikcp_update_ack(ikcpcb *kcp, IINT32 rtt) +{ + IINT32 rto = 0; + if (kcp->rx_srtt == 0) + { + kcp->rx_srtt = rtt; + kcp->rx_rttval = rtt / 2; + } + else + { + long delta = rtt - kcp->rx_srtt; + if (delta < 0) + delta = -delta; + kcp->rx_rttval = (3 * kcp->rx_rttval + delta) / 4; + kcp->rx_srtt = (7 * kcp->rx_srtt + rtt) / 8; + if (kcp->rx_srtt < 1) + kcp->rx_srtt = 1; + } + rto = kcp->rx_srtt + _imax_(kcp->interval, 4 * kcp->rx_rttval); + kcp->rx_rto = _ibound_(kcp->rx_minrto, rto, IKCP_RTO_MAX); +} + +static void ikcp_shrink_buf(ikcpcb *kcp) +{ + struct IQUEUEHEAD *p = kcp->snd_buf.next; + if (p != &kcp->snd_buf) + { + IKCPSEG *seg = iqueue_entry(p, IKCPSEG, node); + kcp->snd_una = seg->sn; + } + else + { + kcp->snd_una = kcp->snd_nxt; + } +} + +static void ikcp_parse_ack(ikcpcb *kcp, IUINT32 sn) +{ + struct IQUEUEHEAD *p, *next; + + if (_itimediff(sn, kcp->snd_una) < 0 || _itimediff(sn, kcp->snd_nxt) >= 0) + return; + + for (p = kcp->snd_buf.next; p != &kcp->snd_buf; p = next) + { + IKCPSEG *seg = iqueue_entry(p, IKCPSEG, node); + next = p->next; + if (sn == seg->sn) + { + iqueue_del(p); + ikcp_segment_delete(kcp, seg); + kcp->nsnd_buf--; + break; + } + if (_itimediff(sn, seg->sn) < 0) + { + break; + } + } +} + +static void ikcp_parse_una(ikcpcb *kcp, IUINT32 una) +{ + struct IQUEUEHEAD *p, *next; + for (p = kcp->snd_buf.next; p != &kcp->snd_buf; p = next) + { + IKCPSEG *seg = iqueue_entry(p, IKCPSEG, node); + next = p->next; + if (_itimediff(una, seg->sn) > 0) + { + iqueue_del(p); + ikcp_segment_delete(kcp, seg); + kcp->nsnd_buf--; + } + else + { + break; + } + } +} + +static void ikcp_parse_fastack(ikcpcb *kcp, IUINT32 sn, IUINT32 ts) +{ + struct IQUEUEHEAD *p, *next; + + if (_itimediff(sn, kcp->snd_una) < 0 || _itimediff(sn, kcp->snd_nxt) >= 0) + return; + + for (p = kcp->snd_buf.next; p != &kcp->snd_buf; p = next) + { + IKCPSEG *seg = iqueue_entry(p, IKCPSEG, node); + next = p->next; + if (_itimediff(sn, seg->sn) < 0) + { + break; + } + else if (sn != seg->sn) + { +#ifndef IKCP_FASTACK_CONSERVE + seg->fastack++; +#else + if (_itimediff(ts, seg->ts) >= 0) + seg->fastack++; +#endif + } + } +} + +//--------------------------------------------------------------------- +// ack append +//--------------------------------------------------------------------- +static void ikcp_ack_push(ikcpcb *kcp, IUINT32 sn, IUINT32 ts) +{ + IUINT32 newsize = kcp->ackcount + 1; + IUINT32 *ptr; + + if (newsize > kcp->ackblock) + { + IUINT32 *acklist; + IUINT32 newblock; + + for (newblock = 8; newblock < newsize; newblock <<= 1) + ; + acklist = (IUINT32 *)ikcp_malloc(newblock * sizeof(IUINT32) * 2); + + if (acklist == NULL) + { + assert(acklist != NULL); + abort(); + } + + if (kcp->acklist != NULL) + { + IUINT32 x; + for (x = 0; x < kcp->ackcount; x++) + { + acklist[x * 2 + 0] = kcp->acklist[x * 2 + 0]; + acklist[x * 2 + 1] = kcp->acklist[x * 2 + 1]; + } + ikcp_free(kcp->acklist); + } + + kcp->acklist = acklist; + kcp->ackblock = newblock; + } + + ptr = &kcp->acklist[kcp->ackcount * 2]; + ptr[0] = sn; + ptr[1] = ts; + kcp->ackcount++; +} + +static void ikcp_ack_get(const ikcpcb *kcp, int p, IUINT32 *sn, IUINT32 *ts) +{ + if (sn) + sn[0] = kcp->acklist[p * 2 + 0]; + if (ts) + ts[0] = kcp->acklist[p * 2 + 1]; +} + +//--------------------------------------------------------------------- +// parse data +//--------------------------------------------------------------------- +void ikcp_parse_data(ikcpcb *kcp, IKCPSEG *newseg) +{ + struct IQUEUEHEAD *p, *prev; + IUINT32 sn = newseg->sn; + int repeat = 0; + + if (_itimediff(sn, kcp->rcv_nxt + kcp->rcv_wnd) >= 0 || + _itimediff(sn, kcp->rcv_nxt) < 0) + { + ikcp_segment_delete(kcp, newseg); + return; + } + + for (p = kcp->rcv_buf.prev; p != &kcp->rcv_buf; p = prev) + { + IKCPSEG *seg = iqueue_entry(p, IKCPSEG, node); + prev = p->prev; + if (seg->sn == sn) + { + repeat = 1; + break; + } + if (_itimediff(sn, seg->sn) > 0) + { + break; + } + } + + if (repeat == 0) + { + iqueue_init(&newseg->node); + iqueue_add(&newseg->node, p); + kcp->nrcv_buf++; + } + else + { + kcp->duplicate_recv_total++; + ikcp_segment_delete(kcp, newseg); + } + +#if 0 + ikcp_qprint("rcvbuf", &kcp->rcv_buf); + printf("rcv_nxt=%lu\n", kcp->rcv_nxt); +#endif + + // move available data from rcv_buf -> rcv_queue + while (!iqueue_is_empty(&kcp->rcv_buf)) + { + IKCPSEG *seg = iqueue_entry(kcp->rcv_buf.next, IKCPSEG, node); + if (seg->sn == kcp->rcv_nxt && kcp->nrcv_que < kcp->rcv_wnd) + { + iqueue_del(&seg->node); + kcp->nrcv_buf--; + iqueue_add_tail(&seg->node, &kcp->rcv_queue); + kcp->nrcv_que++; + kcp->rcv_nxt++; + } + else + { + break; + } + } + +#if 0 + ikcp_qprint("queue", &kcp->rcv_queue); + printf("rcv_nxt=%lu\n", kcp->rcv_nxt); +#endif + +#if 1 +// printf("snd(buf=%d, queue=%d)\n", kcp->nsnd_buf, kcp->nsnd_que); +// printf("rcv(buf=%d, queue=%d)\n", kcp->nrcv_buf, kcp->nrcv_que); +#endif +} + +//--------------------------------------------------------------------- +// input data +//--------------------------------------------------------------------- +int ikcp_input(ikcpcb *kcp, const char *data, long size) +{ + IUINT32 prev_una = kcp->snd_una; + IUINT32 maxack = 0, latest_ts = 0; + int flag = 0; + + if (ikcp_canlog(kcp, IKCP_LOG_INPUT)) + { + ikcp_log(kcp, IKCP_LOG_INPUT, "[RI] %d bytes", (int)size); + } + + if (data == NULL || (int)size < (int)IKCP_OVERHEAD) + return -1; + + while (1) + { + IUINT32 ts, sn, len, una, conv; + IUINT16 wnd; + IUINT8 cmd, frg; + IKCPSEG *seg; + + if (size < (int)IKCP_OVERHEAD) + break; + + data = ikcp_decode32u(data, &conv); + if (conv != kcp->conv) + return -1; + + data = ikcp_decode8u(data, &cmd); + data = ikcp_decode8u(data, &frg); + data = ikcp_decode16u(data, &wnd); + data = ikcp_decode32u(data, &ts); + data = ikcp_decode32u(data, &sn); + data = ikcp_decode32u(data, &una); + data = ikcp_decode32u(data, &len); + + size -= IKCP_OVERHEAD; + + if ((long)size < (long)len || (int)len < 0) + return -2; + + if (cmd != IKCP_CMD_PUSH && cmd != IKCP_CMD_ACK && + cmd != IKCP_CMD_WASK && cmd != IKCP_CMD_WINS) + return -3; + + kcp->rmt_wnd = wnd; + ikcp_parse_una(kcp, una); + ikcp_shrink_buf(kcp); + + if (cmd == IKCP_CMD_ACK) + { + if (_itimediff(kcp->current, ts) >= 0) + { + ikcp_update_ack(kcp, _itimediff(kcp->current, ts)); + } + ikcp_parse_ack(kcp, sn); + ikcp_shrink_buf(kcp); + if (flag == 0) + { + flag = 1; + maxack = sn; + latest_ts = ts; + } + else + { + if (_itimediff(sn, maxack) > 0) + { +#ifndef IKCP_FASTACK_CONSERVE + maxack = sn; + latest_ts = ts; +#else + if (_itimediff(ts, latest_ts) > 0) + { + maxack = sn; + latest_ts = ts; + } +#endif + } + } + if (ikcp_canlog(kcp, IKCP_LOG_IN_ACK)) + { + ikcp_log(kcp, IKCP_LOG_IN_ACK, + "input ack: sn=%lu rtt=%ld rto=%ld", (unsigned long)sn, + (long)_itimediff(kcp->current, ts), + (long)kcp->rx_rto); + } + } + else if (cmd == IKCP_CMD_PUSH) + { + if (ikcp_canlog(kcp, IKCP_LOG_IN_DATA)) + { + ikcp_log(kcp, IKCP_LOG_IN_DATA, + "input psh: sn=%lu ts=%lu", (unsigned long)sn, (unsigned long)ts); + } + if (_itimediff(sn, kcp->rcv_nxt + kcp->rcv_wnd) < 0) + { + ikcp_ack_push(kcp, sn, ts); + if (_itimediff(sn, kcp->rcv_nxt) >= 0) + { + seg = ikcp_segment_new(kcp, len); + seg->conv = conv; + seg->cmd = cmd; + seg->frg = frg; + seg->wnd = wnd; + seg->ts = ts; + seg->sn = sn; + seg->una = una; + seg->len = len; + + if (len > 0) + { + memcpy(seg->data, data, len); + } + + ikcp_parse_data(kcp, seg); + } + } + } + else if (cmd == IKCP_CMD_WASK) + { + // ready to send back IKCP_CMD_WINS in ikcp_flush + // tell remote my window size + kcp->probe |= IKCP_ASK_TELL; + if (ikcp_canlog(kcp, IKCP_LOG_IN_PROBE)) + { + ikcp_log(kcp, IKCP_LOG_IN_PROBE, "input probe"); + } + } + else if (cmd == IKCP_CMD_WINS) + { + // do nothing + if (ikcp_canlog(kcp, IKCP_LOG_IN_WINS)) + { + ikcp_log(kcp, IKCP_LOG_IN_WINS, + "input wins: %lu", (unsigned long)(wnd)); + } + } + else + { + return -3; + } + + data += len; + size -= len; + } + + if (flag != 0) + { + ikcp_parse_fastack(kcp, maxack, latest_ts); + } + + if (_itimediff(kcp->snd_una, prev_una) > 0) + { + if (kcp->cwnd < kcp->rmt_wnd) + { + IUINT32 mss = kcp->mss; + if (kcp->cwnd < kcp->ssthresh) + { + kcp->cwnd++; + kcp->incr += mss; + } + else + { + if (kcp->incr < mss) + kcp->incr = mss; + kcp->incr += (mss * mss) / kcp->incr + (mss / 16); + if ((kcp->cwnd + 1) * mss <= kcp->incr) + { +#if 1 + kcp->cwnd = (kcp->incr + mss - 1) / ((mss > 0) ? mss : 1); +#else + kcp->cwnd++; +#endif + } + } + if (kcp->cwnd > kcp->rmt_wnd) + { + kcp->cwnd = kcp->rmt_wnd; + kcp->incr = kcp->rmt_wnd * mss; + } + } + } + + return 0; +} + +//--------------------------------------------------------------------- +// ikcp_encode_seg +//--------------------------------------------------------------------- +static char *ikcp_encode_seg(char *ptr, const IKCPSEG *seg) +{ + ptr = ikcp_encode32u(ptr, seg->conv); + ptr = ikcp_encode8u(ptr, (IUINT8)seg->cmd); + ptr = ikcp_encode8u(ptr, (IUINT8)seg->frg); + ptr = ikcp_encode16u(ptr, (IUINT16)seg->wnd); + ptr = ikcp_encode32u(ptr, seg->ts); + ptr = ikcp_encode32u(ptr, seg->sn); + ptr = ikcp_encode32u(ptr, seg->una); + ptr = ikcp_encode32u(ptr, seg->len); + return ptr; +} + +static int ikcp_wnd_unused(const ikcpcb *kcp) +{ + if (kcp->nrcv_que < kcp->rcv_wnd) + { + return kcp->rcv_wnd - kcp->nrcv_que; + } + return 0; +} + +//--------------------------------------------------------------------- +// ikcp_flush +//--------------------------------------------------------------------- +void ikcp_flush(ikcpcb *kcp) +{ + IUINT32 current = kcp->current; + char *buffer = kcp->buffer; + char *ptr = buffer; + int count, size, i; + IUINT32 resent, cwnd; + IUINT32 rtomin; + struct IQUEUEHEAD *p; + int change = 0; + int lost = 0; + IKCPSEG seg; + + // 'ikcp_update' haven't been called. + if (kcp->updated == 0) + return; + + seg.conv = kcp->conv; + seg.cmd = IKCP_CMD_ACK; + seg.frg = 0; + seg.wnd = ikcp_wnd_unused(kcp); + seg.una = kcp->rcv_nxt; + seg.len = 0; + seg.sn = 0; + seg.ts = 0; + + // flush acknowledges + count = kcp->ackcount; + for (i = 0; i < count; i++) + { + size = (int)(ptr - buffer); + if (size + (int)IKCP_OVERHEAD > (int)kcp->mtu) + { + ikcp_output(kcp, buffer, size); + ptr = buffer; + } + ikcp_ack_get(kcp, i, &seg.sn, &seg.ts); + ptr = ikcp_encode_seg(ptr, &seg); + } + + kcp->ackcount = 0; + + // probe window size (if remote window size equals zero) + if (kcp->rmt_wnd == 0) + { + if (kcp->probe_wait == 0) + { + kcp->probe_wait = IKCP_PROBE_INIT; + kcp->ts_probe = kcp->current + kcp->probe_wait; + } + else + { + if (_itimediff(kcp->current, kcp->ts_probe) >= 0) + { + if (kcp->probe_wait < IKCP_PROBE_INIT) + kcp->probe_wait = IKCP_PROBE_INIT; + kcp->probe_wait += kcp->probe_wait / 2; + if (kcp->probe_wait > IKCP_PROBE_LIMIT) + kcp->probe_wait = IKCP_PROBE_LIMIT; + kcp->ts_probe = kcp->current + kcp->probe_wait; + kcp->probe |= IKCP_ASK_SEND; + } + } + } + else + { + kcp->ts_probe = 0; + kcp->probe_wait = 0; + } + + // flush window probing commands + if (kcp->probe & IKCP_ASK_SEND) + { + seg.cmd = IKCP_CMD_WASK; + size = (int)(ptr - buffer); + if (size + (int)IKCP_OVERHEAD > (int)kcp->mtu) + { + ikcp_output(kcp, buffer, size); + ptr = buffer; + } + ptr = ikcp_encode_seg(ptr, &seg); + } + + // flush window probing commands + if (kcp->probe & IKCP_ASK_TELL) + { + seg.cmd = IKCP_CMD_WINS; + size = (int)(ptr - buffer); + if (size + (int)IKCP_OVERHEAD > (int)kcp->mtu) + { + ikcp_output(kcp, buffer, size); + ptr = buffer; + } + ptr = ikcp_encode_seg(ptr, &seg); + } + + kcp->probe = 0; + + // calculate window size + cwnd = _imin_(kcp->snd_wnd, kcp->rmt_wnd); + if (kcp->nocwnd == 0) + cwnd = _imin_(kcp->cwnd, cwnd); + + // move data from snd_queue to snd_buf + while (_itimediff(kcp->snd_nxt, kcp->snd_una + cwnd) < 0) + { + IKCPSEG *newseg; + if (iqueue_is_empty(&kcp->snd_queue)) + break; + + newseg = iqueue_entry(kcp->snd_queue.next, IKCPSEG, node); + + iqueue_del(&newseg->node); + iqueue_add_tail(&newseg->node, &kcp->snd_buf); + kcp->nsnd_que--; + kcp->nsnd_buf++; + + newseg->conv = kcp->conv; + newseg->cmd = IKCP_CMD_PUSH; + newseg->wnd = seg.wnd; + newseg->ts = current; + newseg->sn = kcp->snd_nxt++; + newseg->una = kcp->rcv_nxt; + newseg->resendts = current; + newseg->rto = kcp->rx_rto; + newseg->fastack = 0; + newseg->xmit = 0; + } + + // calculate resent + resent = (kcp->fastresend > 0) ? (IUINT32)kcp->fastresend : 0xffffffff; + rtomin = (kcp->nodelay == 0) ? (kcp->rx_rto >> 3) : 0; + + // flush data segments + for (p = kcp->snd_buf.next; p != &kcp->snd_buf; p = p->next) + { + IKCPSEG *segment = iqueue_entry(p, IKCPSEG, node); + int needsend = 0; + if (segment->xmit == 0) + { + needsend = 1; + segment->xmit++; + segment->rto = kcp->rx_rto; + segment->resendts = current + segment->rto + rtomin; + } + else if (_itimediff(current, segment->resendts) >= 0) + { + needsend = 1; + segment->xmit++; + kcp->xmit++; + kcp->timeout_retrans_total++; + if (kcp->nodelay == 0) + { + segment->rto += _imax_(segment->rto, (IUINT32)kcp->rx_rto); + } + else + { + IINT32 step = (kcp->nodelay < 2) ? ((IINT32)(segment->rto)) : kcp->rx_rto; + segment->rto += step / 2; + } + segment->resendts = current + segment->rto; + lost = 1; + } + else if (segment->fastack >= resent) + { + if ((int)segment->xmit <= kcp->fastlimit || + kcp->fastlimit <= 0) + { + needsend = 1; + segment->xmit++; + kcp->fast_retrans_total++; + segment->fastack = 0; + segment->resendts = current + segment->rto; + change++; + } + } + + if (needsend) + { + int need; + segment->ts = current; + segment->wnd = seg.wnd; + segment->una = kcp->rcv_nxt; + + size = (int)(ptr - buffer); + need = IKCP_OVERHEAD + segment->len; + + if (size + need > (int)kcp->mtu) + { + ikcp_output(kcp, buffer, size); + ptr = buffer; + } + + ptr = ikcp_encode_seg(ptr, segment); + + if (segment->len > 0) + { + memcpy(ptr, segment->data, segment->len); + ptr += segment->len; + } + + if (segment->xmit >= kcp->dead_link) + { + kcp->state = (IUINT32)-1; + } + } + } + + // flash remain segments + size = (int)(ptr - buffer); + if (size > 0) + { + ikcp_output(kcp, buffer, size); + } + + // update ssthresh + if (change) + { + IUINT32 inflight = kcp->snd_nxt - kcp->snd_una; + kcp->ssthresh = inflight / 2; + if (kcp->ssthresh < IKCP_THRESH_MIN) + kcp->ssthresh = IKCP_THRESH_MIN; + kcp->cwnd = kcp->ssthresh + resent; + kcp->incr = kcp->cwnd * kcp->mss; + } + + if (lost) + { + kcp->ssthresh = cwnd / 2; + if (kcp->ssthresh < IKCP_THRESH_MIN) + kcp->ssthresh = IKCP_THRESH_MIN; + kcp->cwnd = 1; + kcp->incr = kcp->mss; + } + + if (kcp->cwnd < 1) + { + kcp->cwnd = 1; + kcp->incr = kcp->mss; + } +} + +//--------------------------------------------------------------------- +// update state (call it repeatedly, every 10ms-100ms), or you can ask +// ikcp_check when to call it again (without ikcp_input/_send calling). +// 'current' - current timestamp in millisec. +//--------------------------------------------------------------------- +void ikcp_update(ikcpcb *kcp, IUINT32 current) +{ + IINT32 slap; + + kcp->current = current; + + if (kcp->updated == 0) + { + kcp->updated = 1; + kcp->ts_flush = kcp->current; + } + + slap = _itimediff(kcp->current, kcp->ts_flush); + + if (slap >= 10000 || slap < -10000) + { + kcp->ts_flush = kcp->current; + slap = 0; + } + + if (slap >= 0) + { + kcp->ts_flush += kcp->interval; + if (_itimediff(kcp->current, kcp->ts_flush) >= 0) + { + kcp->ts_flush = kcp->current + kcp->interval; + } + ikcp_flush(kcp); + } +} + +//--------------------------------------------------------------------- +// Determine when should you invoke ikcp_update: +// returns when you should invoke ikcp_update in millisec, if there +// is no ikcp_input/_send calling. you can call ikcp_update in that +// time, instead of call update repeatly. +// Important to reduce unnacessary ikcp_update invoking. use it to +// schedule ikcp_update (eg. implementing an epoll-like mechanism, +// or optimize ikcp_update when handling massive kcp connections) +//--------------------------------------------------------------------- +IUINT32 ikcp_check(const ikcpcb *kcp, IUINT32 current) +{ + IUINT32 ts_flush = kcp->ts_flush; + IINT32 tm_flush = 0x7fffffff; + IINT32 tm_packet = 0x7fffffff; + IUINT32 minimal = 0; + struct IQUEUEHEAD *p; + + if (kcp->updated == 0) + { + return current; + } + + if (_itimediff(current, ts_flush) >= 10000 || + _itimediff(current, ts_flush) < -10000) + { + ts_flush = current; + } + + if (_itimediff(current, ts_flush) >= 0) + { + return current; + } + + tm_flush = _itimediff(ts_flush, current); + + for (p = kcp->snd_buf.next; p != &kcp->snd_buf; p = p->next) + { + const IKCPSEG *seg = iqueue_entry(p, const IKCPSEG, node); + IINT32 diff = _itimediff(seg->resendts, current); + if (diff <= 0) + { + return current; + } + if (diff < tm_packet) + tm_packet = diff; + } + + minimal = (IUINT32)(tm_packet < tm_flush ? tm_packet : tm_flush); + if (minimal >= kcp->interval) + minimal = kcp->interval; + + return current + minimal; +} + +int ikcp_setmtu(ikcpcb *kcp, int mtu) +{ + char *buffer; + if (mtu < 50 || mtu < (int)IKCP_OVERHEAD) + return -1; + buffer = (char *)ikcp_malloc((mtu + IKCP_OVERHEAD) * 3); + if (buffer == NULL) + return -2; + kcp->mtu = mtu; + kcp->mss = kcp->mtu - IKCP_OVERHEAD; + ikcp_free(kcp->buffer); + kcp->buffer = buffer; + return 0; +} + +int ikcp_interval(ikcpcb *kcp, int interval) +{ + if (interval > 5000) + interval = 5000; + else if (interval < 10) + interval = 10; + kcp->interval = interval; + return 0; +} + +int ikcp_nodelay(ikcpcb *kcp, int nodelay, int interval, int resend, int nc) +{ + if (nodelay >= 0) + { + kcp->nodelay = nodelay; + if (nodelay) + { + kcp->rx_minrto = IKCP_RTO_NDL; + } + else + { + kcp->rx_minrto = IKCP_RTO_MIN; + } + } + if (interval >= 0) + { + if (interval > 5000) + interval = 5000; + else if (interval < 10) + interval = 10; + kcp->interval = interval; + } + if (resend >= 0) + { + kcp->fastresend = resend; + } + if (nc >= 0) + { + kcp->nocwnd = nc; + } + return 0; +} + +int ikcp_wndsize(ikcpcb *kcp, int sndwnd, int rcvwnd) +{ + if (kcp) + { + if (sndwnd > 0) + { + kcp->snd_wnd = sndwnd; + } + if (rcvwnd > 0) + { // must >= max fragment size + kcp->rcv_wnd = _imax_(rcvwnd, IKCP_WND_RCV); + } + } + return 0; +} + +int ikcp_waitsnd(const ikcpcb *kcp) +{ + return kcp->nsnd_buf + kcp->nsnd_que; +} + +// read conv +IUINT32 ikcp_getconv(const void *ptr) +{ + IUINT32 conv; + ikcp_decode32u((const char *)ptr, &conv); + return conv; +} diff --git a/robot/v4l2/OmniSocketGo_robot/third_party/kcp/ikcp.h b/robot/v4l2/OmniSocketGo_robot/third_party/kcp/ikcp.h new file mode 100644 index 0000000..54106f2 --- /dev/null +++ b/robot/v4l2/OmniSocketGo_robot/third_party/kcp/ikcp.h @@ -0,0 +1,421 @@ +//===================================================================== +// +// KCP - A Better ARQ Protocol Implementation +// skywind3000 (at) gmail.com, 2010-2011 +// +// Features: +// + Average RTT reduce 30% - 40% vs traditional ARQ like tcp. +// + Maximum RTT reduce three times vs tcp. +// + Lightweight, distributed as a single source file. +// +//===================================================================== +#ifndef __IKCP_H__ +#define __IKCP_H__ + +#include +#include +#include + +//===================================================================== +// 32BIT INTEGER DEFINITION +//===================================================================== +#ifndef __INTEGER_32_BITS__ +#define __INTEGER_32_BITS__ +#if defined(_WIN64) || defined(WIN64) || defined(__amd64__) || \ + defined(__x86_64) || defined(__x86_64__) || defined(_M_IA64) || \ + defined(_M_AMD64) +typedef unsigned int ISTDUINT32; +typedef int ISTDINT32; +#elif defined(_WIN32) || defined(WIN32) || defined(__i386__) || \ + defined(__i386) || defined(_M_X86) +typedef unsigned long ISTDUINT32; +typedef long ISTDINT32; +#elif defined(__MACOS__) +typedef UInt32 ISTDUINT32; +typedef SInt32 ISTDINT32; +#elif defined(__APPLE__) && defined(__MACH__) +#include +typedef u_int32_t ISTDUINT32; +typedef int32_t ISTDINT32; +#elif defined(__BEOS__) +#include +typedef u_int32_t ISTDUINT32; +typedef int32_t ISTDINT32; +#elif (defined(_MSC_VER) || defined(__BORLANDC__)) && (!defined(__MSDOS__)) +typedef unsigned __int32 ISTDUINT32; +typedef __int32 ISTDINT32; +#elif defined(__GNUC__) +#include +typedef uint32_t ISTDUINT32; +typedef int32_t ISTDINT32; +#else +typedef unsigned long ISTDUINT32; +typedef long ISTDINT32; +#endif +#endif + +//===================================================================== +// Integer Definition +//===================================================================== +#ifndef __IINT8_DEFINED +#define __IINT8_DEFINED +typedef char IINT8; +#endif + +#ifndef __IUINT8_DEFINED +#define __IUINT8_DEFINED +typedef unsigned char IUINT8; +#endif + +#ifndef __IUINT16_DEFINED +#define __IUINT16_DEFINED +typedef unsigned short IUINT16; +#endif + +#ifndef __IINT16_DEFINED +#define __IINT16_DEFINED +typedef short IINT16; +#endif + +#ifndef __IINT32_DEFINED +#define __IINT32_DEFINED +typedef ISTDINT32 IINT32; +#endif + +#ifndef __IUINT32_DEFINED +#define __IUINT32_DEFINED +typedef ISTDUINT32 IUINT32; +#endif + +#ifndef __IINT64_DEFINED +#define __IINT64_DEFINED +#if defined(_MSC_VER) || defined(__BORLANDC__) +typedef __int64 IINT64; +#else +typedef long long IINT64; +#endif +#endif + +#ifndef __IUINT64_DEFINED +#define __IUINT64_DEFINED +#if defined(_MSC_VER) || defined(__BORLANDC__) +typedef unsigned __int64 IUINT64; +#else +typedef unsigned long long IUINT64; +#endif +#endif + +#ifndef INLINE +#if defined(__GNUC__) + +#if (__GNUC__ > 3) || ((__GNUC__ == 3) && (__GNUC_MINOR__ >= 1)) +#define INLINE __inline__ __attribute__((always_inline)) +#else +#define INLINE __inline__ +#endif + +#elif (defined(_MSC_VER) || defined(__BORLANDC__) || defined(__WATCOMC__)) +#define INLINE __inline +#else +#define INLINE +#endif +#endif + +#if (!defined(__cplusplus)) && (!defined(inline)) +#define inline INLINE +#endif + +//===================================================================== +// QUEUE DEFINITION +//===================================================================== +#ifndef __IQUEUE_DEF__ +#define __IQUEUE_DEF__ + +struct IQUEUEHEAD +{ + struct IQUEUEHEAD *next, *prev; +}; + +typedef struct IQUEUEHEAD iqueue_head; + +//--------------------------------------------------------------------- +// queue init +//--------------------------------------------------------------------- +#define IQUEUE_HEAD_INIT(name) {&(name), &(name)} +#define IQUEUE_HEAD(name) \ + struct IQUEUEHEAD name = IQUEUE_HEAD_INIT(name) + +#define IQUEUE_INIT(ptr) ( \ + (ptr)->next = (ptr), (ptr)->prev = (ptr)) + +#define IOFFSETOF(TYPE, MEMBER) ((size_t)&((TYPE *)0)->MEMBER) + +#define ICONTAINEROF(ptr, type, member) ( \ + (type *)(((char *)((type *)ptr)) - IOFFSETOF(type, member))) + +#define IQUEUE_ENTRY(ptr, type, member) ICONTAINEROF(ptr, type, member) + +//--------------------------------------------------------------------- +// queue operation +//--------------------------------------------------------------------- +#define IQUEUE_ADD(node, head) ( \ + (node)->prev = (head), (node)->next = (head)->next, \ + (head)->next->prev = (node), (head)->next = (node)) + +#define IQUEUE_ADD_TAIL(node, head) ( \ + (node)->prev = (head)->prev, (node)->next = (head), \ + (head)->prev->next = (node), (head)->prev = (node)) + +#define IQUEUE_DEL_BETWEEN(p, n) ((n)->prev = (p), (p)->next = (n)) + +#define IQUEUE_DEL(entry) ( \ + (entry)->next->prev = (entry)->prev, \ + (entry)->prev->next = (entry)->next, \ + (entry)->next = 0, (entry)->prev = 0) + +#define IQUEUE_DEL_INIT(entry) \ + do \ + { \ + IQUEUE_DEL(entry); \ + IQUEUE_INIT(entry); \ + } while (0) + +#define IQUEUE_IS_EMPTY(entry) ((entry) == (entry)->next) + +#define iqueue_init IQUEUE_INIT +#define iqueue_entry IQUEUE_ENTRY +#define iqueue_add IQUEUE_ADD +#define iqueue_add_tail IQUEUE_ADD_TAIL +#define iqueue_del IQUEUE_DEL +#define iqueue_del_init IQUEUE_DEL_INIT +#define iqueue_is_empty IQUEUE_IS_EMPTY + +#define IQUEUE_FOREACH(iterator, head, TYPE, MEMBER) \ + for ((iterator) = iqueue_entry((head)->next, TYPE, MEMBER); \ + &((iterator)->MEMBER) != (head); \ + (iterator) = iqueue_entry((iterator)->MEMBER.next, TYPE, MEMBER)) + +#define iqueue_foreach(iterator, head, TYPE, MEMBER) \ + IQUEUE_FOREACH(iterator, head, TYPE, MEMBER) + +#define iqueue_foreach_entry(pos, head) \ + for ((pos) = (head)->next; (pos) != (head); (pos) = (pos)->next) + +#define __iqueue_splice(list, head) \ + do \ + { \ + iqueue_head *first = (list)->next, *last = (list)->prev; \ + iqueue_head *at = (head)->next; \ + (first)->prev = (head), (head)->next = (first); \ + (last)->next = (at), (at)->prev = (last); \ + } while (0) + +#define iqueue_splice(list, head) \ + do \ + { \ + if (!iqueue_is_empty(list)) \ + __iqueue_splice(list, head); \ + } while (0) + +#define iqueue_splice_init(list, head) \ + do \ + { \ + iqueue_splice(list, head); \ + iqueue_init(list); \ + } while (0) + +#ifdef _MSC_VER +#pragma warning(disable : 4311) +#pragma warning(disable : 4312) +#pragma warning(disable : 4996) +#endif + +#endif + +//--------------------------------------------------------------------- +// BYTE ORDER & ALIGNMENT +//--------------------------------------------------------------------- +#ifndef IWORDS_BIG_ENDIAN +#ifdef _BIG_ENDIAN_ +#if _BIG_ENDIAN_ +#define IWORDS_BIG_ENDIAN 1 +#endif +#endif +#ifndef IWORDS_BIG_ENDIAN +#if defined(__hppa__) || \ + defined(__m68k__) || defined(mc68000) || defined(_M_M68K) || \ + (defined(__MIPS__) && defined(__MIPSEB__)) || \ + defined(__ppc__) || defined(__POWERPC__) || defined(_M_PPC) || \ + defined(__sparc__) || defined(__powerpc__) || \ + defined(__mc68000__) || defined(__s390x__) || defined(__s390__) +#define IWORDS_BIG_ENDIAN 1 +#endif +#endif +#ifndef IWORDS_BIG_ENDIAN +#define IWORDS_BIG_ENDIAN 0 +#endif +#endif + +#ifndef IWORDS_MUST_ALIGN +#if defined(__i386__) || defined(__i386) || defined(_i386_) +#define IWORDS_MUST_ALIGN 0 +#elif defined(_M_IX86) || defined(_X86_) || defined(__x86_64__) +#define IWORDS_MUST_ALIGN 0 +#elif defined(__amd64) || defined(__amd64__) +#define IWORDS_MUST_ALIGN 0 +#else +#define IWORDS_MUST_ALIGN 1 +#endif +#endif + +//===================================================================== +// SEGMENT +//===================================================================== +struct IKCPSEG +{ + struct IQUEUEHEAD node; + IUINT32 conv; + IUINT32 cmd; + IUINT32 frg; + IUINT32 wnd; + IUINT32 ts; + IUINT32 sn; + IUINT32 una; + IUINT32 len; + IUINT32 resendts; + IUINT32 rto; + IUINT32 fastack; + IUINT32 xmit; + char data[1]; +}; + +//--------------------------------------------------------------------- +// IKCPCB +//--------------------------------------------------------------------- +struct IKCPCB +{ + IUINT32 conv, mtu, mss, state; + IUINT32 snd_una, snd_nxt, rcv_nxt; + IUINT32 ts_recent, ts_lastack, ssthresh; + 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 nrcv_buf, nsnd_buf; + IUINT32 nrcv_que, nsnd_que; + IUINT32 nodelay, updated; + IUINT32 ts_probe, probe_wait; + IUINT32 dead_link, incr; + struct IQUEUEHEAD snd_queue; + struct IQUEUEHEAD rcv_queue; + struct IQUEUEHEAD snd_buf; + struct IQUEUEHEAD rcv_buf; + IUINT32 *acklist; + IUINT32 ackcount; + IUINT32 ackblock; + IUINT64 timeout_retrans_total; + IUINT64 fast_retrans_total; + IUINT64 duplicate_recv_total; + void *user; + char *buffer; + int fastresend; + int fastlimit; + int nocwnd, stream; + int logmask; + int (*output)(const char *buf, int len, struct IKCPCB *kcp, void *user); + void (*writelog)(const char *log, struct IKCPCB *kcp, void *user); +}; + +typedef struct IKCPCB ikcpcb; + +#define IKCP_LOG_OUTPUT 1 +#define IKCP_LOG_INPUT 2 +#define IKCP_LOG_SEND 4 +#define IKCP_LOG_RECV 8 +#define IKCP_LOG_IN_DATA 16 +#define IKCP_LOG_IN_ACK 32 +#define IKCP_LOG_IN_PROBE 64 +#define IKCP_LOG_IN_WINS 128 +#define IKCP_LOG_OUT_DATA 256 +#define IKCP_LOG_OUT_ACK 512 +#define IKCP_LOG_OUT_PROBE 1024 +#define IKCP_LOG_OUT_WINS 2048 + +#ifdef __cplusplus +extern "C" +{ +#endif + + //--------------------------------------------------------------------- + // interface + //--------------------------------------------------------------------- + + // create a new kcp control object, 'conv' must equal in two endpoint + // from the same connection. 'user' will be passed to the output callback + // output callback can be setup like this: 'kcp->output = my_udp_output' + ikcpcb *ikcp_create(IUINT32 conv, void *user); + + // release kcp control object + void ikcp_release(ikcpcb *kcp); + + // set output callback, which will be invoked by kcp + void ikcp_setoutput(ikcpcb *kcp, int (*output)(const char *buf, int len, + ikcpcb *kcp, void *user)); + + // user/upper level recv: returns size, returns below zero for EAGAIN + int ikcp_recv(ikcpcb *kcp, char *buffer, int len); + + // user/upper level send, returns below zero for error + int ikcp_send(ikcpcb *kcp, const char *buffer, int len); + + // update state (call it repeatedly, every 10ms-100ms), or you can ask + // ikcp_check when to call it again (without ikcp_input/_send calling). + // 'current' - current timestamp in millisec. + void ikcp_update(ikcpcb *kcp, IUINT32 current); + + // Determine when should you invoke ikcp_update: + // returns when you should invoke ikcp_update in millisec, if there + // is no ikcp_input/_send calling. you can call ikcp_update in that + // time, instead of call update repeatly. + // Important to reduce unnacessary ikcp_update invoking. use it to + // schedule ikcp_update (eg. implementing an epoll-like mechanism, + // or optimize ikcp_update when handling massive kcp connections) + IUINT32 ikcp_check(const ikcpcb *kcp, IUINT32 current); + + // when you received a low level packet (eg. UDP packet), call it + int ikcp_input(ikcpcb *kcp, const char *data, long size); + + // flush pending data + void ikcp_flush(ikcpcb *kcp); + + // check the size of next message in the recv queue + int ikcp_peeksize(const ikcpcb *kcp); + + // change MTU size, default is 1400 + int ikcp_setmtu(ikcpcb *kcp, int mtu); + + // set maximum window size: sndwnd=32, rcvwnd=32 by default + int ikcp_wndsize(ikcpcb *kcp, int sndwnd, int rcvwnd); + + // get how many packet is waiting to be sent + int ikcp_waitsnd(const ikcpcb *kcp); + + // fastest: ikcp_nodelay(kcp, 1, 20, 2, 1) + // nodelay: 0:disable(default), 1:enable + // interval: internal update timer interval in millisec, default is 100ms + // resend: 0:disable fast resend(default), 1:enable fast resend + // nc: 0:normal congestion control(default), 1:disable congestion control + int ikcp_nodelay(ikcpcb *kcp, int nodelay, int interval, int resend, int nc); + + void ikcp_log(ikcpcb *kcp, int mask, const char *fmt, ...); + + // setup allocator + void ikcp_allocator(void *(*new_malloc)(size_t), void (*new_free)(void *)); + + // read conv + IUINT32 ikcp_getconv(const void *ptr); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/robot/v4l2/README.md b/robot/v4l2/README.md new file mode 100644 index 0000000..76ce51c --- /dev/null +++ b/robot/v4l2/README.md @@ -0,0 +1,23 @@ +# Robot V4L2 release + +This directory is the extracted `OmniSocketGo_robot-20260808` package. The +runtime path opens the selected `/dev/video*` node directly and performs the +existing MJPEG/KCP transport pipeline. + +Before starting: + +```bash +cd OmniSocketGo_robot +./check-robot-lan.sh +``` + +If a camera is reported busy, inspect `lsof`/`fuser` and stop the specific +camera service or process first. Start this release with: + +```bash +./start-robot-lan.sh +``` + +Build on the ARM64 robot with the included `Makefile`; do not reuse host +binaries. The `scripts/dev/*.env` file is a template. Keep machine-specific +values in an ignored `robot-remote.env.local`. diff --git a/robot/v4l2/README_PACKAGE.md b/robot/v4l2/README_PACKAGE.md new file mode 100644 index 0000000..7d6c1bd --- /dev/null +++ b/robot/v4l2/README_PACKAGE.md @@ -0,0 +1,65 @@ +# OmniSocketGo robot package + +This archive is the robot-side source package. It contains the camera discovery, +occupancy handling, dual-camera switching, KCP video sender and robot control +daemon. It does not contain the web frontend or the control-host backend. + +## Install + +```bash +cd OmniSocketGo_robot + +sudo apt-get update +sudo apt-get install -y \ + build-essential pkg-config \ + libavformat-dev libavcodec-dev libavutil-dev libswscale-dev \ + v4l-utils psmisc udev systemd \ + python3 iproute2 iputils-ping curl + +make b_side_omnid +``` + +The package intentionally excludes binaries built on the x86_64 control host. +Always build `bin/b_side_omnid` on the robot itself. + +## Verify the included camera mapping logic + +```bash +test -f scripts/dev/resolve-camera-device.sh +grep -n "resolve_camera_devices" scripts/dev/start-b-side-omnid.sh +grep -n "OMNI_CAMERA_AUTO_DISCOVER" scripts/dev/robot-remote.env.local +``` + +The current mapping is: + +```text +head serial: CP9E163000H3 +waist serial: CPCK8530005N +capture mode: MJPG 1280x720 +``` + +Review `scripts/dev/robot-remote.env.local` before starting, especially the +KCP server address, camera serial numbers and service names. + +## Start direct-LAN mode + +```bash +sudo systemctl stop blitz-watchdog.service blitz-b-side-omnid.service || true +./start-robot-lan.sh +``` + +Expected startup messages include: + +```text +stopping known camera service ... +[camera-discovery] head: serial=... -> /dev/video... +[camera-discovery] waist: serial=... -> /dev/video... +[start-b-side-omnid] resolved head=... waist=... +[video_pipeline] camera head ready ... +[video_pipeline] camera waist ready ... +video registered=1 +``` + +`check-robot-lan.sh` still reports the configured fallback `/dev/video*` nodes. +The real startup path performs dynamic serial-number discovery after releasing +the known camera services.