diff --git a/src/net/pose_server.gd b/src/net/pose_server.gd new file mode 100644 index 0000000..fec3b36 --- /dev/null +++ b/src/net/pose_server.gd @@ -0,0 +1,324 @@ +class_name PoseServer +extends Node + +## T-WIN-003:WebSocket 关键点接收服务 +## +## 契约 contracts/websocket-keypoints.md §1: +## 端点 ws://:8765/pose +## 子协议 opendance.pose.v1 +## 二进制帧 = 239 字节关键点(见 KeypointFrame) +## 文本帧 = JSON 控制消息(hello / welcome / config / stats / bye) +## +## 本端为**服务端**,Android 辅助 App 主动连入。 +## +## 说明:Godot 的 WebSocketPeer 不暴露握手请求路径,因此无法校验客户端 +## 连接的 path 是否为 /pose;本实现接受任意 path,仅校验子协议。 + +signal listening(port: int) +signal listen_failed(port: int, reason: String) +signal client_connected(client_id: int, device: String) +signal client_disconnected(client_id: int, reason: String) +signal frame_received(frame: KeypointFrame.PoseFrame) +signal control_received(client_id: int, message: Dictionary) +signal stats_updated(stats: Dictionary) + +const KeypointFrame = preload("res://src/net/keypoint_frame.gd") + +const DEFAULT_PORT := 8765 +const DEFAULT_SUBPROTOCOL := "opendance.pose.v1" + +## 超过该时长(毫秒)没有收到任何帧,判定为超时断开 +const CLIENT_TIMEOUT_MS := 5000 +## 每秒向客户端推送一次 config 的间隔(0 表示不主动推送) +const CONFIG_PUSH_INTERVAL_MS := 2000 + +var subprotocol: String = DEFAULT_SUBPROTOCOL +var expected_fps: int = 30 +var mirror: bool = false +var upper_body_only: bool = false +var resolution: String = "640x480" + +var _tcp := TCPServer.new() +var _clients: Array = [] +var _next_client_id: int = 1 +var _port: int = 0 +var _running: bool = false +var _last_config_push_ms: int = 0 + +## 累计统计 +var frames_total: int = 0 +var frames_dropped: int = 0 +var malformed_frames: int = 0 +var last_frame: KeypointFrame.PoseFrame = null +var last_frame_at_ms: int = 0 + + +func is_running() -> bool: + return _running + + +func port() -> int: + return _port + + +func client_count() -> int: + return _clients.size() + + +## 已握手完成的连接数 +func connected_count() -> int: + var n := 0 + for entry in _clients: + if entry["handshaken"]: + n += 1 + return n + + +func listen(host: String = "0.0.0.0", listen_port: int = DEFAULT_PORT) -> Error: + stop() + + var bind_host := host + if bind_host == "" or bind_host == "0.0.0.0": + bind_host = "*" + + var err := _tcp.listen(listen_port, bind_host) + if err != OK: + listen_failed.emit(listen_port, error_string(err)) + push_error("[PoseServer] 监听 %s:%d 失败:%s" % [host, listen_port, error_string(err)]) + return err + + _port = listen_port + _running = true + set_process(true) + listening.emit(listen_port) + return OK + + +func stop() -> void: + _running = false + for entry in _clients: + var peer: WebSocketPeer = entry["peer"] + if peer.get_ready_state() == WebSocketPeer.STATE_OPEN: + peer.close(1001, "server shutdown") + _tcp.stop() + _clients.clear() + set_process(false) + + +func _ready() -> void: + set_process(false) + + +func _process(_delta: float) -> void: + if not _running: + return + + _accept_new_connections() + _poll_clients() + _maybe_push_config() + + +func _accept_new_connections() -> void: + while _tcp.is_connection_available(): + var conn := _tcp.take_connection() + if conn == null: + continue + var peer := WebSocketPeer.new() + peer.supported_protocols = PackedStringArray([subprotocol]) + var err := peer.accept_stream(conn) + if err != OK: + push_warning("[PoseServer] accept_stream 失败:%s" % error_string(err)) + continue + _clients.append({ + "id": _next_client_id, + "peer": peer, + "handshaken": false, + "device": "", + "last_seen_ms": Time.get_ticks_msec(), + "last_frame_id": -1, + "frames": 0, + "dropped": 0, + "fps_window_start_ms": Time.get_ticks_msec(), + "fps_window_frames": 0, + "fps": 0.0, + }) + _next_client_id += 1 + + +func _poll_clients() -> void: + var now := Time.get_ticks_msec() + var survivors: Array = [] + + for entry in _clients: + var peer: WebSocketPeer = entry["peer"] + peer.poll() + + var state := peer.get_ready_state() + if state == WebSocketPeer.STATE_CLOSED: + client_disconnected.emit(entry["id"], "连接已关闭") + continue + + if state == WebSocketPeer.STATE_OPEN and not entry["handshaken"]: + entry["handshaken"] = true + var selected := peer.get_selected_protocol() + if selected != "" and selected != subprotocol: + push_warning("[PoseServer] 客户端选择子协议 %s,与期望 %s 不符" % [selected, subprotocol]) + client_connected.emit(entry["id"], entry["device"]) + + while peer.get_available_packet_count() > 0: + _handle_packet(entry, peer) + entry["last_seen_ms"] = now + + # 超时剔除 + if entry["handshaken"] and now - int(entry["last_seen_ms"]) > CLIENT_TIMEOUT_MS: + client_disconnected.emit(entry["id"], "超时未收到数据") + peer.close(1001, "timeout") + continue + + survivors.append(entry) + + _clients = survivors + + +func _handle_packet(entry: Dictionary, peer: WebSocketPeer) -> void: + var packet := peer.get_packet() + var is_text := peer.was_string_packet() + + if is_text: + _handle_control(entry, packet.get_string_from_utf8()) + return + + # 二进制帧:契约要求恰好 239 字节 + var problem := KeypointFrame.validate(packet) + if problem != "": + malformed_frames += 1 + push_warning("[PoseServer] 客户端 %d 发来非法帧:%s" % [entry["id"], problem]) + return + + var frame := KeypointFrame.decode(packet) + if frame == null: + malformed_frames += 1 + return + + _update_frame_stats(entry, frame) + frames_total += 1 + last_frame = frame + last_frame_at_ms = Time.get_ticks_msec() + frame_received.emit(frame) + + +func _update_frame_stats(entry: Dictionary, frame: KeypointFrame.PoseFrame) -> void: + var previous := int(entry["last_frame_id"]) + if previous >= 0 and frame.frame_id > previous + 1: + # u32 回绕保护:差距过大视为计数器重置而非丢包 + var gap := frame.frame_id - previous - 1 + if gap < 100000: + entry["dropped"] = int(entry["dropped"]) + gap + frames_dropped += gap + entry["last_frame_id"] = frame.frame_id + entry["frames"] = int(entry["frames"]) + 1 + + entry["fps_window_frames"] = int(entry["fps_window_frames"]) + 1 + var now := Time.get_ticks_msec() + var elapsed := now - int(entry["fps_window_start_ms"]) + if elapsed >= 1000: + entry["fps"] = float(entry["fps_window_frames"]) * 1000.0 / float(elapsed) + entry["fps_window_frames"] = 0 + entry["fps_window_start_ms"] = now + + var stats := { + "client_id": entry["id"], + "fps": entry["fps"], + "frames": entry["frames"], + "dropped": entry["dropped"], + "device": entry["device"], + "total_frames": frames_total, + "total_dropped": frames_dropped, + "malformed": malformed_frames, + } + stats_updated.emit(stats) + + +func _handle_control(entry: Dictionary, text: String) -> void: + var parsed: Variant = JSON.parse_string(text) + if typeof(parsed) != TYPE_DICTIONARY: + push_warning("[PoseServer] 客户端 %d 发来非法控制帧" % entry["id"]) + return + + var message: Dictionary = parsed + var type := str(message.get("type", "")) + control_received.emit(entry["id"], message) + + match type: + "hello": + entry["device"] = str(message.get("device", "")) + var protocol := str(message.get("protocol", subprotocol)) + if protocol != subprotocol: + push_warning("[PoseServer] 协议版本不匹配:客户端 %s,本端 %s" % [protocol, subprotocol]) + _send_json(entry, { + "type": "bye", + "reason": "protocol_mismatch: 本端要求 %s" % subprotocol, + }) + var peer: WebSocketPeer = entry["peer"] + peer.close(1002, "protocol mismatch") + return + if message.has("targetFps"): + expected_fps = int(message["targetFps"]) + _send_json(entry, { + "type": "welcome", + "protocol": subprotocol, + "sessionId": _session_id(), + "expectedFps": expected_fps, + }) + "bye": + client_disconnected.emit(entry["id"], str(message.get("reason", "客户端主动断开"))) + var peer2: WebSocketPeer = entry["peer"] + peer2.close(1000, "bye") + _: + pass + + +func _maybe_push_config() -> void: + if CONFIG_PUSH_INTERVAL_MS <= 0: + return + var now := Time.get_ticks_msec() + if now - _last_config_push_ms < CONFIG_PUSH_INTERVAL_MS: + return + _last_config_push_ms = now + broadcast_config() + + +func broadcast_config() -> void: + var payload := { + "type": "config", + "mirror": mirror, + "targetFps": expected_fps, + "upperBodyOnly": upper_body_only, + "resolution": resolution, + } + for entry in _clients: + if entry["handshaken"]: + _send_json(entry, payload) + + +func _send_json(entry: Dictionary, payload: Dictionary) -> void: + var peer: WebSocketPeer = entry["peer"] + if peer.get_ready_state() != WebSocketPeer.STATE_OPEN: + return + peer.send_text(JSON.stringify(payload)) + + +func _session_id() -> String: + return "%d-%d" % [Time.get_unix_time_from_system(), _port] + + +func snapshot() -> Dictionary: + return { + "running": _running, + "port": _port, + "clients": _clients.size(), + "connected": connected_count(), + "frames_total": frames_total, + "frames_dropped": frames_dropped, + "malformed_frames": malformed_frames, + } diff --git a/tests/test_pose_server.gd b/tests/test_pose_server.gd new file mode 100644 index 0000000..426be72 --- /dev/null +++ b/tests/test_pose_server.gd @@ -0,0 +1,295 @@ +extends TestCase + +## T-WIN-003:WebSocket 关键点接收服务 +## +## 这是真实的 socket 端到端测试:在本机起 PoseServer, +## 用 WebSocketPeer 作为客户端连入,走完整的握手 → 二进制帧 → 控制帧流程。 + +const PoseServer = preload("res://src/net/pose_server.gd") +const KeypointFrame = preload("res://src/net/keypoint_frame.gd") + +const TEST_PORT := 18765 +const SUBPROTOCOL := "opendance.pose.v1" + +var _server: PoseServer = null +var _client: WebSocketPeer = null +var _received: Array = [] +var _controls: Array = [] +var _connected := 0 + + +func _tree() -> SceneTree: + return Engine.get_main_loop() as SceneTree + + +## 推进若干帧,让服务端与客户端都有机会 poll +func _pump(frames: int = 1) -> void: + for i in frames: + await _tree().process_frame + + +func _setup() -> void: + _received.clear() + _controls.clear() + _connected = 0 + + _server = PoseServer.new() + _server.subprotocol = SUBPROTOCOL + _tree().root.add_child(_server) + _server.frame_received.connect(func(frame): _received.append(frame)) + _server.control_received.connect(func(_id, msg): _controls.append(msg)) + _server.client_connected.connect(func(_id, _device): _connected += 1) + _server.listen("127.0.0.1", TEST_PORT) + + _client = WebSocketPeer.new() + _client.supported_protocols = PackedStringArray([SUBPROTOCOL]) + _client.connect_to_url("ws://127.0.0.1:%d/pose" % TEST_PORT) + + # 等待握手完成 + var guard := 0 + while _client.get_ready_state() == WebSocketPeer.STATE_CONNECTING and guard < 240: + _client.poll() + await _pump() + guard += 1 + + +func _teardown() -> void: + if _client != null: + _client.close() + _client = null + if _server != null: + _server.stop() + _server.queue_free() + _server = null + await _pump(2) + + +func _sample_points(offset: float = 0.0) -> PackedVector3Array: + var points := PackedVector3Array() + points.resize(33) + for i in 33: + points[i] = Vector3( + clampf(0.5 + offset + float(i % 9) * 0.01, 0.0, 1.0), + clampf(0.5 + float(i % 7) * 0.02, 0.0, 1.0), + float((i % 5) - 2) * 0.1 + ) + return points + + +func _sample_confidence() -> PackedFloat32Array: + var conf := PackedFloat32Array() + conf.resize(33) + conf.fill(1.0) + return conf + + +## 把客户端收到的包取出来 +func _drain_client() -> Array: + var out: Array = [] + while _client.get_available_packet_count() > 0: + var packet := _client.get_packet() + out.append({ + "text": packet.get_string_from_utf8() if _client.was_string_packet() else "", + "binary": packet if not _client.was_string_packet() else PackedByteArray(), + "is_text": _client.was_string_packet(), + }) + return out + + +func test_listen_and_handshake() -> void: + await _setup() + is_true(_server.is_running(), "服务端进入监听状态") + eq(_server.port(), TEST_PORT, "监听端口正确") + eq(_client.get_ready_state(), WebSocketPeer.STATE_OPEN, "客户端握手成功(STATE_OPEN)") + + await _pump(3) + eq(_connected, 1, "服务端发出 client_connected 信号") + eq(_server.connected_count(), 1, "已握手连接数为 1") + + await _teardown() + + +func test_subprotocol_is_negotiated() -> void: + await _setup() + eq(_client.get_selected_protocol(), SUBPROTOCOL, "子协议协商为 opendance.pose.v1") + await _teardown() + + +func test_hello_gets_welcome() -> void: + await _setup() + _client.send_text(JSON.stringify({ + "type": "hello", + "protocol": SUBPROTOCOL, + "device": "Pixel 8 (测试)", + "appVersion": "0.1.0", + "cameraFacing": "front", + "targetFps": 30, + "resolution": "640x480", + })) + + var got_welcome := false + var welcome: Dictionary = {} + for i in 60: + _client.poll() + await _pump() + for packet in _drain_client(): + if packet["is_text"]: + var parsed: Variant = JSON.parse_string(packet["text"]) + if typeof(parsed) == TYPE_DICTIONARY and str((parsed as Dictionary).get("type", "")) == "welcome": + welcome = parsed + got_welcome = true + if got_welcome: + break + + is_true(got_welcome, "收到 welcome 控制帧") + eq(str(welcome.get("protocol", "")), SUBPROTOCOL, "welcome 回显协议版本") + ok(welcome.has("sessionId"), "welcome 含 sessionId") + ok(_controls.size() > 0, "服务端发出 control_received 信号") + eq(str(_controls[0].get("device", "")), "Pixel 8 (测试)", "hello 中的设备名被记录") + + await _teardown() + + +func test_binary_frame_is_received_and_decoded() -> void: + await _setup() + var points := _sample_points() + var conf := _sample_confidence() + _client.send(KeypointFrame.encode(1, points, conf, 0)) + + var got := false + for i in 60: + _client.poll() + await _pump() + if _received.size() > 0: + got = true + break + + is_true(got, "服务端收到并解析出关键点帧") + if got: + var frame: KeypointFrame.PoseFrame = _received[0] + eq(frame.frame_id, 1, "frame_id 解析正确") + eq(frame.points.size(), 33, "33 个关键点") + var max_delta := 0.0 + for i in 33: + max_delta = maxf(max_delta, absf(frame.points[i].x - points[i].x)) + ok(max_delta < 1e-4, "坐标经网络往返后仍在定点精度内", "最大误差 %.8f" % max_delta) + eq(_server.frames_total, 1, "累计帧计数为 1") + eq(_server.malformed_frames, 0, "无畸形帧") + + await _teardown() + + +func test_malformed_frame_is_rejected_without_crashing() -> void: + await _setup() + + # 长度错误 + _client.send(PackedByteArray([1, 2, 3])) + # magic 错误但长度正确 + var bad := PackedByteArray() + bad.resize(239) + _client.send(bad) + # 正常帧,用于确认连接未被破坏 + _client.send(KeypointFrame.encode(1, _sample_points(), _sample_confidence(), 0)) + + var good_arrived := false + for i in 90: + _client.poll() + await _pump() + if _received.size() > 0: + good_arrived = true + break + + eq(_server.malformed_frames, 2, "两个畸形帧被拒绝并计数") + is_true(good_arrived, "畸形帧之后连接仍可正常收帧") + eq(_server.frames_total, 1, "仅有效帧计入累计") + + await _teardown() + + +func test_frame_id_gap_is_counted_as_dropped() -> void: + await _setup() + var conf := _sample_confidence() + _client.send(KeypointFrame.encode(1, _sample_points(), conf)) + _client.send(KeypointFrame.encode(2, _sample_points(0.01), conf)) + # 跳过 3、4、5 + _client.send(KeypointFrame.encode(6, _sample_points(0.02), conf)) + + var guard := 0 + while _received.size() < 3 and guard < 120: + _client.poll() + await _pump() + guard += 1 + + eq(_received.size(), 3, "收到 3 帧") + eq(_server.frames_dropped, 3, "frame_id 跳号被计为丢包(3 帧)") + eq(_server.frames_total, 3, "累计收到 3 帧") + + await _teardown() + + +func test_upper_body_and_mirror_flags_survive_transport() -> void: + await _setup() + var flags := KeypointFrame.FLAG_MIRRORED | KeypointFrame.FLAG_UPPER_BODY_ONLY + _client.send(KeypointFrame.encode(7, _sample_points(), _sample_confidence(), flags)) + + var guard := 0 + while _received.is_empty() and guard < 90: + _client.poll() + await _pump() + guard += 1 + + if _received.is_empty(): + ok(false, "未收到帧") + else: + var frame: KeypointFrame.PoseFrame = _received[0] + is_true(frame.is_mirrored(), "镜像标志经网络传输保留") + is_true(frame.is_upper_body_only(), "上半身模式标志经网络传输保留") + + await _teardown() + + +func test_config_broadcast_reaches_client() -> void: + await _setup() + await _pump(3) + + _server.mirror = true + _server.expected_fps = 24 + _server.broadcast_config() + + var config: Dictionary = {} + for i in 60: + _client.poll() + await _pump() + for packet in _drain_client(): + if packet["is_text"]: + var parsed: Variant = JSON.parse_string(packet["text"]) + if typeof(parsed) == TYPE_DICTIONARY and str((parsed as Dictionary).get("type", "")) == "config": + config = parsed + if not config.is_empty(): + break + + is_false(config.is_empty(), "客户端收到 config 控制帧") + eq(bool(config.get("mirror", false)), true, "config.mirror 正确") + eq(int(config.get("targetFps", 0)), 24, "config.targetFps 正确") + ok(config.has("upperBodyOnly"), "config 含 upperBodyOnly") + ok(config.has("resolution"), "config 含 resolution") + + await _teardown() + + +func test_snapshot_reports_state() -> void: + await _setup() + await _pump(3) + var snap := _server.snapshot() + is_true(bool(snap["running"]), "snapshot 报告运行中") + eq(int(snap["port"]), TEST_PORT, "snapshot 报告端口") + eq(int(snap["connected"]), 1, "snapshot 报告已连接 1 个客户端") + await _teardown() + + +func test_stop_closes_listener() -> void: + await _setup() + _server.stop() + is_false(_server.is_running(), "stop() 后不再监听") + eq(_server.connected_count(), 0, "stop() 后连接清空") + await _teardown()