limingjie/robot-command-center
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: 对接GPS数据

Browse Source
This commit is contained in:
nnbcccscdscdsc
2026-04-11 12:15:59 +08:00
parent adb43efb12
commit f02295daea
4 changed files with 106 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files

91
backend/monitoring/video.py
View File

@@ -1,6 +1,9 @@
from __future__ import annotations
from collections import deque
from dataclasses import dataclass
import math
import struct
import sys
import threading
import time
@@ -10,14 +13,14 @@ from .common import (
JPEG_FRAME_DIR,
OMNISOCKET_CONFIG_PATH,
OMNISOCKET_FRAME_FRESH_SECONDS,
PROJECT_ROOT,
VIDEO_SOURCE_MODE,
VIDEO_TIMESTAMP_ENDIANNESS,
VIDEO_TIMESTAMP_MAX_SKEW_NS,
VIDEO_TIMESTAMP_MULTIPLIER_NS,
VIDEO_TIMESTAMP_SAMPLE_SIZE,
VIDEO_TIMESTAMP_TRAILER_BYTES,
VIDEO_TIMESTAMP_UNIT,
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,
)
@@ -32,6 +35,14 @@ def safe_kcp_stats(session: Any) -> dict[str, Any]:
return {}
@dataclass(frozen=True)
class FrameTrailerMetadata:
timestamp_ns: int
latitude: float
longitude: float
received_at: float
class OmniSocketVideoReceiver:
def __init__(self) -> None:
self._lock = threading.Lock()
@@ -44,6 +55,7 @@ class OmniSocketVideoReceiver:
self._latest_frame: bytes | None = None
self._latest_received_at = 0.0
self._latest_sequence: int | None = None
self._latest_metadata: FrameTrailerMetadata | None = None
self._latest_latency_ms: float | None = None
self._latest_timestamp_unit: str | None = None
self._latest_timestamp_endianness: str | None = None
@@ -123,10 +135,10 @@ class OmniSocketVideoReceiver:
return jpeg_payload, b""
if (
len(jpeg_payload) >= VIDEO_TIMESTAMP_TRAILER_BYTES + 2
and jpeg_payload[-(VIDEO_TIMESTAMP_TRAILER_BYTES + 2) : -VIDEO_TIMESTAMP_TRAILER_BYTES] == b"\xff\xd9"
len(jpeg_payload) >= VIDEO_TRAILER_BYTES + 2
and jpeg_payload[-(VIDEO_TRAILER_BYTES + 2) : -VIDEO_TRAILER_BYTES] == b"\xff\xd9"
):
return jpeg_payload[:-VIDEO_TIMESTAMP_TRAILER_BYTES], jpeg_payload[-VIDEO_TIMESTAMP_TRAILER_BYTES:]
return jpeg_payload[:-VIDEO_TRAILER_BYTES], jpeg_payload[-VIDEO_TRAILER_BYTES:]
eoi_index = jpeg_payload.rfind(b"\xff\xd9")
if eoi_index < 0:
@@ -154,20 +166,38 @@ class OmniSocketVideoReceiver:
_, trailer = split_payload
return trailer
def _extract_frame_timestamp(self, frame: bytes) -> tuple[int, str, str] | None:
def _extract_frame_metadata(self, frame: bytes, received_at: float | None = None) -> FrameTrailerMetadata | None:
trailer = self._extract_frame_tail(frame)
if len(trailer) != VIDEO_TIMESTAMP_TRAILER_BYTES:
if len(trailer) != VIDEO_TRAILER_BYTES:
return None
value = int.from_bytes(trailer, VIDEO_TIMESTAMP_ENDIANNESS, signed=False)
if value <= 0:
try:
timestamp_ms, latitude, longitude = VIDEO_TRAILER_STRUCT.unpack(trailer)
except struct.error:
return None
timestamp_ns = value * VIDEO_TIMESTAMP_MULTIPLIER_NS
if abs(time.time_ns() - timestamp_ns) > VIDEO_TIMESTAMP_MAX_SKEW_NS:
if timestamp_ms <= 0:
return None
if not math.isfinite(latitude) or not math.isfinite(longitude):
return None
if not (-90.0 <= latitude <= 90.0) or not (-180.0 <= longitude <= 180.0):
return None
return timestamp_ns, VIDEO_TIMESTAMP_UNIT, VIDEO_TIMESTAMP_ENDIANNESS
timestamp_ns = timestamp_ms * VIDEO_TRAILER_TIMESTAMP_MULTIPLIER_NS
if abs(time.time_ns() - timestamp_ns) > VIDEO_TRAILER_TIMESTAMP_MAX_SKEW_NS:
return None
return FrameTrailerMetadata(
timestamp_ns=timestamp_ns,
latitude=latitude,
longitude=longitude,
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 _run(self) -> None:
while not self._closing.is_set():
@@ -195,19 +225,22 @@ class OmniSocketVideoReceiver:
self._last_error = "received non-JPEG binary frame"
continue
timestamp_meta = self._extract_frame_timestamp(frame)
received_at = time.time()
frame_metadata = self._extract_frame_metadata(frame, received_at=received_at)
latency_ms = None
if timestamp_meta is not None:
timestamp_ns, unit, endianness = timestamp_meta
latency_ms = round((time.time_ns() - timestamp_ns) / 1_000_000, 3)
if frame_metadata is not None:
latency_ms = round((time.time_ns() - frame_metadata.timestamp_ns) / 1_000_000, 3)
unit = VIDEO_TRAILER_TIMESTAMP_UNIT
endianness = VIDEO_TRAILER_ENDIANNESS
else:
unit = None
endianness = None
with self._lock:
self._latest_frame = jpeg_frame
self._latest_received_at = time.time()
self._latest_received_at = received_at
self._latest_sequence = self._extract_sequence(frame)
self._latest_metadata = frame_metadata
self._latest_latency_ms = latency_ms
self._latest_timestamp_unit = unit
self._latest_timestamp_endianness = endianness
@@ -238,12 +271,17 @@ class OmniSocketVideoReceiver:
def get_latest_frame(self) -> bytes | None:
self.ensure_started()
with self._lock:
if self._latest_frame is None:
return None
if time.time() - self._latest_received_at > OMNISOCKET_FRAME_FRESH_SECONDS:
if not self._has_fresh_frame_locked():
return None
return self._latest_frame
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:
@@ -268,9 +306,7 @@ class OmniSocketVideoReceiver:
video_cfg = config.get("video_receiver", {})
session_stats = self.session_stats()
with self._lock:
has_recent_frame = self._latest_frame is not None and (
time.time() - self._latest_received_at <= OMNISOCKET_FRAME_FRESH_SECONDS
)
has_recent_frame = self._has_fresh_frame_locked()
if has_recent_frame and self._latest_latency_ms is not None:
timing_status = {
"available": True,
@@ -375,4 +411,3 @@ class VideoFrameService:
)
yield header + frame + b"\r\n"
time.sleep(frame_interval)