编辑c3_client.py

2026-05-27 12:59:50 +08:00
parent c8d03d03da
commit d0193a7d08
1 changed files with 234 additions and 44 deletions
@@ -7,15 +7,20 @@ C3 Client — 部署到 C3 设备上

 通过 openpilot 的 PythonProcess 自动启动：
  PythonProcess("c3-client", "selfdrive.c3_client", always_run),
+
+零外部依赖（内嵌纯 Python 版 WebSocket 客户端，纯标准库实现）
 """

 import asyncio
-import importlib
+import base64
 import json
 import os
+import random
+import struct
 import subprocess
 import sys
 import traceback
+import urllib.parse
 from datetime import datetime

 # openpilot 基础模块
@@ -23,17 +28,236 @@ from openpilot.system.hardware import HARDWARE, PC
 from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog

+
+# ================= 内嵌 WebSocket 客户端（纯标准库） =================
+
+class _WebSocketError(Exception):
+  pass
+
+
+class _WebSocketClient:
+  """异步 WebSocket 客户端（RFC 6455），仅依赖 Python 标准库"""
+
+  def __init__(self, url, ping_interval=20, ping_timeout=15):
+    parsed = urllib.parse.urlparse(url)
+    if parsed.scheme not in ("ws",):
+      raise ValueError(f"不支持的协议: {parsed.scheme}")
+    self.host = parsed.hostname
+    self.port = parsed.port or 80
+    self.path = parsed.path or "/"
+    if parsed.query:
+      self.path += "?" + parsed.query
+
+    self.ping_interval = ping_interval
+    self.ping_timeout = ping_timeout
+    self._reader = None
+    self._writer = None
+    self._closed = False
+    self._recv_queue = asyncio.Queue()
+    self._pong_event = asyncio.Event()
+
+  async def connect(self):
+    """建立 TCP + WebSocket 握手"""
+    self._reader, self._writer = await asyncio.wait_for(
+      asyncio.open_connection(self.host, self.port), timeout=10
+    )
+
+    # 生成 Sec-WebSocket-Key
+    rand_bytes = bytes(random.randint(0, 255) for _ in range(16))
+    ws_key = base64.b64encode(rand_bytes).decode()
+
+    request = (
+      f"GET {self.path} HTTP/1.1\r\n"
+      f"Host: {self.host}:{self.port}\r\n"
+      f"Upgrade: websocket\r\n"
+      f"Connection: Upgrade\r\n"
+      f"Sec-WebSocket-Key: {ws_key}\r\n"
+      f"Sec-WebSocket-Version: 13\r\n"
+      f"\r\n"
+    )
+    self._writer.write(request.encode())
+    await self._writer.drain()
+
+    # 读取响应头
+    response = b""
+    while b"\r\n\r\n" not in response:
+      chunk = await asyncio.wait_for(self._reader.read(4096), timeout=10)
+      if not chunk:
+        raise _WebSocketError("连接关闭")
+      response += chunk
+
+    header_text = response.decode("utf-8", errors="replace")
+    status_line = header_text.split("\r\n")[0]
+    if "101" not in status_line:
+      raise _WebSocketError(f"握手失败: {status_line}")
+
+    self._recv_task = asyncio.create_task(self._recv_loop())
+    self._ping_task = asyncio.create_task(self._ping_loop())
+
+  async def _recv_loop(self):
+    """持续读取 WebSocket 帧"""
+    try:
+      while not self._closed:
+        frame = await self._read_frame()
+        if frame is None:
+          break
+
+        opcode = frame[0] & 0x0F
+        payload = frame[1]
+
+        if opcode in (0x0, 0x1):  # 继续帧 / 文本帧
+          await self._recv_queue.put(("text", payload.decode("utf-8", errors="replace")))
+        elif opcode == 0x8:  # 关闭帧
+          await self._send_close()
+          self._closed = True
+          break
+        elif opcode == 0x9:  # Ping → 自动 Pong
+          await self._send_frame(0xA, payload)
+        elif opcode == 0xA:  # Pong
+          self._pong_event.set()
+    except (asyncio.CancelledError, ConnectionError):
+      pass
+    except Exception as e:
+      print(f"[C3] WebSocket 接收异常: {e}")
+    finally:
+      self._closed = True
+
+  async def _read_frame(self):
+    """读取一个 WebSocket 帧"""
+    header = await self._read_exact(2)
+    if not header:
+      return None
+
+    first_byte = header[0]
+    second_byte = header[1]
+    length = second_byte & 0x7F
+
+    if length == 126:
+      ext = await self._read_exact(2)
+      length = struct.unpack("!H", ext)[0]
+    elif length == 127:
+      ext = await self._read_exact(8)
+      length = struct.unpack("!Q", ext)[0]
+
+    mask_key = await self._read_exact(4) if (second_byte & 0x80) else None
+    payload = await self._read_exact(length)
+
+    if mask_key:
+      payload = bytes(b ^ mask_key[i % 4] for i, b in enumerate(payload))
+
+    return (first_byte, payload)
+
+  async def _read_exact(self, n):
+    """精确读取 n 字节"""
+    data = b""
+    while len(data) < n:
+      chunk = await self._reader.read(n - len(data))
+      if not chunk:
+        raise _WebSocketError("连接断开")
+      data += chunk
+    return data
+
+  async def send(self, text):
+    """发送文本消息"""
+    payload = text.encode("utf-8") if isinstance(text, str) else text
+    await self._send_frame(0x1, payload)
+
+  async def _send_frame(self, opcode, payload):
+    """发送 WebSocket 帧（客户端必须 mask）"""
+    header = bytearray()
+    header.append(0x80 | opcode)  # FIN + opcode
+
+    length = len(payload)
+    if length < 126:
+      header.append(0x80 | length)
+    elif length < 65536:
+      header.append(0x80 | 126)
+      header.extend(struct.pack("!H", length))
+    else:
+      header.append(0x80 | 127)
+      header.extend(struct.pack("!Q", length))
+
+    mask_key = bytes(random.randint(0, 255) for _ in range(4))
+    header.extend(mask_key)
+    header.extend(bytes(b ^ mask_key[i % 4] for i, b in enumerate(payload)))
+
+    self._writer.write(bytes(header))
+    await self._writer.drain()
+
+  async def _send_close(self):
+    try:
+      self._writer.write(bytes([0x88, 0x00]))
+      await self._writer.drain()
+    except Exception:
+      pass
+
+  async def _ping_loop(self):
+    try:
+      while not self._closed:
+        await asyncio.sleep(self.ping_interval)
+        self._pong_event.clear()
+        await self._send_frame(0x9, b"")
+        try:
+          await asyncio.wait_for(self._pong_event.wait(), timeout=self.ping_timeout)
+        except asyncio.TimeoutError:
+          print("[C3] Ping 超时")
+          break
+    except asyncio.CancelledError:
+      pass
+    except Exception as e:
+      print(f"[C3] Ping 循环异常: {e}")
+    finally:
+      self._closed = True
+
+  async def recv(self):
+    """接收一条消息"""
+    while not self._closed:
+      msg_type, text = await self._recv_queue.get()
+      if msg_type == "text":
+        return text
+    raise ConnectionError("连接已关闭")
+
+  def __aiter__(self):
+    return self._async_iterator()
+
+  async def _async_iterator(self):
+    while not self._closed:
+      try:
+        yield await self.recv()
+      except ConnectionError:
+        break
+
+  async def close(self):
+    self._closed = True
+    if self._writer:
+      await self._send_close()
+      try:
+        self._writer.close()
+        await self._writer.wait_closed()
+      except Exception:
+        pass
+    for attr in ("_recv_task", "_ping_task"):
+      if hasattr(self, attr):
+        getattr(self, attr).cancel()
+
+  async def __aenter__(self):
+    await self.connect()
+    return self
+
+  async def __aexit__(self, *args):
+    await self.close()
+
+
 # ================= 配置 =================
 SERVER_URL = "ws://1.15.136.221:8500"
-HEARTBEAT_INTERVAL = 15  # 心跳间隔秒数
-RECONNECT_DELAY = 5      # 断线重连等待秒数
+HEARTBEAT_INTERVAL = 15
+RECONNECT_DELAY = 5

 # ================= 设备标识 =================
 _params = Params()


 def get_serial():
-  """通过 HARDWARE API 获取设备序列号"""
  try:
    return HARDWARE.get_serial()
  except Exception:
@@ -41,7 +265,6 @@ def get_serial():


 def get_dongle_id():
-  """通过 Params 获取 dongle_id"""
  try:
    dongle = _params.get("DongleId")
    return dongle if dongle else ""
@@ -50,7 +273,6 @@ def get_dongle_id():


 def get_git_branch():
-  """获取当前 openpilot 的分支名"""
  try:
    result = subprocess.run(
      ["git", "rev-parse", "--abbrev-ref", "HEAD"],
@@ -64,7 +286,6 @@ def get_git_branch():

 # ================= 指令执行 =================
 async def execute_cmd(command, timeout=30):
-  """执行 shell 命令，返回 stdout+stderr"""
  try:
    result = await asyncio.wait_for(
      asyncio.create_subprocess_shell(
@@ -87,13 +308,6 @@ async def execute_cmd(command, timeout=30):


 async def execute_tmux():
-  """执行 tmux a 获取当前 tmux 输出（故障诊断）"""
-  commands = [
-    "tmux capture-pane -t $(tmux list-sessions -F '#{session_name}' 2>/dev/null | head -1) -p -S -200 2>/dev/null",
-    "tmux list-sessions 2>/dev/null && tmux send-keys -t $(tmux list-sessions -F '#{session_name}' 2>/dev/null | head -1) '' Enter && sleep 0.5 && tmux capture-pane -t $(tmux list-sessions -F '#{session_name}' 2>/dev/null | head -1) -p -S -200 2>/dev/null",
-    "tmux has-session 2>/dev/null && tmux send-keys -t $(tmux list-sessions -F '#{session_name}' 2>/dev/null | head -1) '' && sleep 0.5 && tmux capture-pane -t $(tmux list-sessions -F '#{session_name}' 2>/dev/null | head -1) -p -S -200 2>/dev/null || echo 'TMUX_ERROR: no session found'",
-  ]
-
  tmux_check = await execute_cmd("ps aux | grep tmux | grep -v grep", timeout=3)
  if not tmux_check.get("output", "").strip():
    return {"status": "ok", "output": "⚠️ 设备上未检测到 tmux 进程运行"}
@@ -102,7 +316,10 @@ async def execute_tmux():
  if "error" in sessions_result.get("output", ""):
    return {"status": "ok", "output": f"⚠️ 无法获取 tmux 会话\n{sessions_result['output']}"}

-  result = await execute_cmd(commands[0], timeout=5)
+  result = await execute_cmd(
+    "tmux capture-pane -t $(tmux list-sessions -F '#{session_name}' 2>/dev/null | head -1) -p -S -200 2>/dev/null",
+    timeout=5
+  )
  if result["status"] == "ok" and result.get("output", "").strip():
    session_info = await execute_cmd("tmux list-sessions 2>&1", timeout=3)
    output = f"--- tmux sessions ---\n{session_info.get('output', '')}\n\n--- capture output ---\n{result['output']}"
@@ -113,7 +330,6 @@ async def execute_tmux():

 # ================= 消息处理 =================
 async def handle_message(data, ws):
-  """处理服务端发来的消息"""
  msg_type = data.get("type")
  msg_id = data.get("id")
  content = data.get("content", "")
@@ -141,28 +357,7 @@ async def handle_message(data, ws):


 # ================= 主循环 =================
-def _ensure_websockets():
-  """确保 websockets 可用，未安装则尝试自动安装"""
-  try:
-    return importlib.import_module("websockets")
-  except ImportError:
-    print("[C3] 🔧 未检测到 websockets 模块，尝试自动安装...")
-    try:
-      subprocess.check_call(
-        [sys.executable, "-m", "pip", "install", "websockets", "--break-system-packages"],
-        stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, timeout=30
-      )
-      print("[C3] ✅ websockets 安装成功")
-      return importlib.import_module("websockets")
-    except Exception:
-      print("[C3] ❌ 自动安装失败！请手动执行: pip install websockets")
-      sys.exit(1)
-
-
 async def run():
-  """异步主循环（连接、注册、心跳、消息处理、自动重连）"""
-  websockets = _ensure_websockets()
-
  serial = get_serial()
  dongle_id = get_dongle_id()
  git_branch = get_git_branch()
@@ -171,14 +366,13 @@ async def run():

  while True:
    try:
-      async with websockets.connect(
+      async with _WebSocketClient(
        SERVER_URL,
        ping_interval=20,
        ping_timeout=15
      ) as ws:
        print(f"[C3] ✅ 已连接服务器")

-        # 注册（带上分支信息）
        register_msg = json.dumps({
          "type": "register",
          "serial": serial,
@@ -209,7 +403,7 @@ async def run():

        await asyncio.gather(heartbeat(), receiver())

-    except websockets.ConnectionClosed:
+    except (ConnectionError, OSError, _WebSocketError):
      print(f"[C3] 连接断开")
    except ConnectionRefusedError:
      print(f"[C3] 服务器拒绝连接")
@@ -227,10 +421,6 @@ async def run():

 # ================= 入口 =================
 def main():
-  """
-  main() 函数 - 供 openpilot PythonProcess 调用
-  用法: PythonProcess("c3-client", "selfdrive.c3_client", always_run)
-  """
  try:
    asyncio.run(run())
  except KeyboardInterrupt: