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

15
backend/monitoring/common.py
View File

@@ -10,18 +10,18 @@ from typing import Any
PROJECT_ROOT = Path(__file__).resolve().parents[2] PROJECT_ROOT = Path(__file__).resolve().parents[2]
WORKSPACE_ROOT = PROJECT_ROOT.parent WORKSPACE_ROOT = PROJECT_ROOT.parent
JPEG_FRAME_DIR = WORKSPACE_ROOT / "RobotDataShow" / "jpeg-frames" JPEG_FRAME_DIR = WORKSPACE_ROOT / "RobotDataShow" / "jpeg-frames"
GEOSTREAM_JSON_PATH = WORKSPACE_ROOT / "GeoStream" / "gps_latest.json"
GEOSTREAM_STALE_SECONDS = 15
OMNISOCKET_CONFIG_PATH = PROJECT_ROOT / "config" / "omnisocket_demo.yaml" OMNISOCKET_CONFIG_PATH = PROJECT_ROOT / "config" / "omnisocket_demo.yaml"
VIDEO_SOURCE_MODE = os.getenv("VIDEO_SOURCE_MODE", "auto").strip().lower() VIDEO_SOURCE_MODE = os.getenv("VIDEO_SOURCE_MODE", "auto").strip().lower()
OMNISOCKET_FRAME_FRESH_SECONDS = 2.0 OMNISOCKET_FRAME_FRESH_SECONDS = 2.0
VIDEO_TIMESTAMP_SAMPLE_SIZE = 10 VIDEO_TIMESTAMP_SAMPLE_SIZE = 10
VIDEO_TIMESTAMP_TRAILER_BYTES = 8 VIDEO_TRAILER_ENDIANNESS = "little"
VIDEO_TIMESTAMP_ENDIANNESS = "little" VIDEO_TRAILER_TIMESTAMP_UNIT = "ms"
VIDEO_TIMESTAMP_UNIT = "ms" VIDEO_TRAILER_TIMESTAMP_MULTIPLIER_NS = 1_000_000
VIDEO_TIMESTAMP_MULTIPLIER_NS = 1_000_000 VIDEO_TRAILER_TIMESTAMP_MAX_SKEW_NS = 7 * 24 * 60 * 60 * 1_000_000_000
VIDEO_TIMESTAMP_MAX_SKEW_NS = 7 * 24 * 60 * 60 * 1_000_000_000 VIDEO_TRAILER_COORDINATE_FORMAT = "float32 little-endian"
VIDEO_TRAILER_STRUCT = struct.Struct("<Qff")
VIDEO_TRAILER_BYTES = VIDEO_TRAILER_STRUCT.size
CONTROL_PACKET = struct.Struct("<6f") CONTROL_PACKET = struct.Struct("<6f")
CONTROL_PACKET_SIZE = CONTROL_PACKET.size CONTROL_PACKET_SIZE = CONTROL_PACKET.size
@@ -203,4 +203,3 @@ def parse_host_port(bind_addr: str) -> tuple[str, int]:
if port <= 0 or port > 65535: if port <= 0 or port > 65535:
raise ValueError(f"invalid port in bind address: {bind_addr}") raise ValueError(f"invalid port in bind address: {bind_addr}")
return host, port return host, port

3
backend/monitoring/services.py
View File

@@ -15,7 +15,7 @@ control_arbiter = ControlArbiter(_control_sender)
native_control_ingress = NativeUdpControlIngress(control_arbiter) native_control_ingress = NativeUdpControlIngress(control_arbiter)
video_service = VideoFrameService(_video_receiver) video_service = VideoFrameService(_video_receiver)
gps_service = GpsDataService() gps_service = GpsDataService(_video_receiver)
network_service = NetworkTelemetryService( network_service = NetworkTelemetryService(
_video_receiver, _video_receiver,
_control_sender, _control_sender,
@@ -41,4 +41,3 @@ def shutdown_monitoring_services() -> None:
atexit.register(shutdown_monitoring_services) atexit.register(shutdown_monitoring_services)

73
backend/monitoring/telemetry.py
View File

@@ -2,7 +2,6 @@ from __future__ import annotations
from collections import deque from collections import deque
import json import json
import math
import sys import sys
import threading import threading
import time import time
@@ -10,14 +9,13 @@ from datetime import datetime, timezone
from typing import Any from typing import Any
from .common import ( from .common import (
GEOSTREAM_JSON_PATH, VIDEO_TRAILER_COORDINATE_FORMAT,
GEOSTREAM_STALE_SECONDS,
WORKSPACE_ROOT, WORKSPACE_ROOT,
load_omnisocket_config, load_omnisocket_config,
utc_iso_now, utc_iso_now,
) )
from .control import ControlArbiter, NativeUdpControlIngress, OmniSocketControlSender from .control import ControlArbiter, NativeUdpControlIngress, OmniSocketControlSender
from .video import OmniSocketVideoReceiver from .video import FrameTrailerMetadata, OmniSocketVideoReceiver
LOCAL_SAMPLE_INTERVAL_MS = 500 LOCAL_SAMPLE_INTERVAL_MS = 500
@@ -50,46 +48,45 @@ def _coerce_float(value: Any, default: float = 0.0) -> float:
class GpsDataService: class GpsDataService:
def __init__(self, receiver: OmniSocketVideoReceiver) -> None:
self._receiver = receiver
def get_latest(self) -> dict[str, Any]: def get_latest(self) -> dict[str, Any]:
payload = self._read_geostream_payload() metadata = self._receiver.get_latest_frame_metadata()
if payload is not None: if metadata is None:
payload["source_mode"] = "geostream-json" return self._build_waiting_payload()
payload["updated_at"] = utc_iso_now() return self._build_payload_from_metadata(metadata)
return payload
return self._build_simulated_payload() def _build_waiting_payload(self) -> dict[str, Any]:
return {
def _read_geostream_payload(self) -> dict[str, Any] | None: "has_fix": False,
if not GEOSTREAM_JSON_PATH.exists(): "utc_time": "--:--:--",
return None "latitude": None,
"longitude": None,
age_seconds = time.time() - GEOSTREAM_JSON_PATH.stat().st_mtime "satellites": None,
if age_seconds > GEOSTREAM_STALE_SECONDS: "altitude_m": None,
return None "coordinate_system": "WGS84",
"source_sentence": "VIDEO_TRAILER",
try: "raw_coordinate_format": VIDEO_TRAILER_COORDINATE_FORMAT,
with GEOSTREAM_JSON_PATH.open("r", encoding="utf-8") as file: "source_mode": "video-frame-trailer-waiting",
return json.load(file) "updated_at": "",
except (OSError, json.JSONDecodeError): }
return None
def _build_simulated_payload(self) -> dict[str, Any]:
tick = time.time() / 12.0
latitude = 31.2304 + math.sin(tick) * 0.0014
longitude = 121.4737 + math.cos(tick) * 0.0018
def _build_payload_from_metadata(self, metadata: FrameTrailerMetadata) -> dict[str, Any]:
timestamp_seconds = metadata.timestamp_ns / 1_000_000_000
updated_at = _utc_from_epoch(metadata.received_at) or ""
return { return {
"has_fix": True, "has_fix": True,
"utc_time": datetime.now(timezone.utc).strftime("%H:%M:%S"), "utc_time": datetime.fromtimestamp(timestamp_seconds, timezone.utc).strftime("%H:%M:%S"),
"latitude": round(latitude, 6), "latitude": round(metadata.latitude, 6),
"longitude": round(longitude, 6), "longitude": round(metadata.longitude, 6),
"satellites": 14 + int((math.sin(tick * 0.7) + 1.0) * 2), "satellites": None,
"altitude_m": round(6.5 + math.cos(tick * 0.5) * 1.2, 2), "altitude_m": None,
"coordinate_system": "WGS84", "coordinate_system": "WGS84",
"source_sentence": "SIMULATED", "source_sentence": "VIDEO_TRAILER",
"raw_coordinate_format": "decimal degrees", "raw_coordinate_format": VIDEO_TRAILER_COORDINATE_FORMAT,
"source_mode": "simulated", "source_mode": "video-frame-trailer",
"updated_at": utc_iso_now(), "updated_at": updated_at,
} }

91
backend/monitoring/video.py
View File

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