|
|
@@ -3,13 +3,12 @@ from __future__ import annotations
|
|
|
import asyncio
|
|
|
import json
|
|
|
import time
|
|
|
-import uuid
|
|
|
from dataclasses import dataclass
|
|
|
from typing import Any, Dict, List, Optional, Set, Tuple
|
|
|
|
|
|
from config.logger import setup_logging
|
|
|
-from core.providers.tts.dto.dto import ContentType
|
|
|
from core.providers.tools.device_mcp.mcp_handler import call_mcp_tool
|
|
|
+from core.providers.tts.dto.dto import ContentType
|
|
|
|
|
|
from .geo import haversine_m, nearest_point_index
|
|
|
from .storage import Hazard, NavDB, RoutePoint
|
|
|
@@ -18,10 +17,13 @@ from .storage import Hazard, NavDB, RoutePoint
|
|
|
TAG = __name__
|
|
|
logger = setup_logging()
|
|
|
|
|
|
+COUNTER_MAX_VALUE = 256
|
|
|
+COUNTER_WRAP_THRESHOLD = 128
|
|
|
+
|
|
|
|
|
|
_HAZARD_LABELS: Dict[str, str] = {
|
|
|
"sharp_turn": "急转弯",
|
|
|
- "bump": "大坑/起伏",
|
|
|
+ "bump": "大坑",
|
|
|
"jump": "跳台",
|
|
|
"slope": "陡坡",
|
|
|
"water": "涉水",
|
|
|
@@ -32,17 +34,15 @@ _HAZARD_LABELS: Dict[str, str] = {
|
|
|
_HAZARD_SYNONYMS: Dict[str, str] = {
|
|
|
"急转弯": "sharp_turn",
|
|
|
"急弯": "sharp_turn",
|
|
|
- "弯": "sharp_turn",
|
|
|
"大坑": "bump",
|
|
|
"坑": "bump",
|
|
|
- "颠簸": "bump",
|
|
|
"跳台": "jump",
|
|
|
"陡坡": "slope",
|
|
|
"涉水": "water",
|
|
|
- "水": "water",
|
|
|
"障碍": "obstacle",
|
|
|
}
|
|
|
|
|
|
+
|
|
|
@dataclass
|
|
|
class GnssFix:
|
|
|
ok: bool
|
|
|
@@ -53,64 +53,92 @@ class GnssFix:
|
|
|
t: Optional[int] = None
|
|
|
|
|
|
|
|
|
-def _parse_fix(raw: Any) -> GnssFix:
|
|
|
+@dataclass
|
|
|
+class CounterState:
|
|
|
+ ok: bool
|
|
|
+ value: int
|
|
|
+ absolute: int
|
|
|
+ cycle: int
|
|
|
+ t: Optional[int] = None
|
|
|
+
|
|
|
+
|
|
|
+@dataclass(frozen=True)
|
|
|
+class HazardMarkResult:
|
|
|
+ hazard_id: int
|
|
|
+ route_id: int
|
|
|
+ mode: str
|
|
|
+ counter_value: Optional[int] = None
|
|
|
+ counter_absolute: Optional[int] = None
|
|
|
+
|
|
|
+
|
|
|
+@dataclass(frozen=True)
|
|
|
+class StopRecordingResult:
|
|
|
+ route_id: int
|
|
|
+ mode: str
|
|
|
+ hazard_count: int
|
|
|
+ monitor_started: bool
|
|
|
+
|
|
|
+
|
|
|
+def _parse_jsonish(raw: Any) -> Optional[dict]:
|
|
|
if raw is None:
|
|
|
- return GnssFix(False, 0.0, 0.0, 0.0)
|
|
|
+ return None
|
|
|
if isinstance(raw, str):
|
|
|
raw = raw.strip()
|
|
|
if not raw:
|
|
|
- return GnssFix(False, 0.0, 0.0, 0.0)
|
|
|
+ return None
|
|
|
try:
|
|
|
raw = json.loads(raw)
|
|
|
except Exception:
|
|
|
- return GnssFix(False, 0.0, 0.0, 0.0)
|
|
|
+ return None
|
|
|
if not isinstance(raw, dict):
|
|
|
+ return None
|
|
|
+ return raw
|
|
|
+
|
|
|
+
|
|
|
+def _parse_fix(raw: Any) -> GnssFix:
|
|
|
+ payload = _parse_jsonish(raw)
|
|
|
+ if payload is None:
|
|
|
return GnssFix(False, 0.0, 0.0, 0.0)
|
|
|
|
|
|
- # Device firmware may wrap fields in {"gnss": {...}}
|
|
|
- if "gnss" in raw and isinstance(raw.get("gnss"), dict):
|
|
|
- raw = raw["gnss"]
|
|
|
+ if "gnss" in payload and isinstance(payload.get("gnss"), dict):
|
|
|
+ payload = payload["gnss"]
|
|
|
|
|
|
- ok = bool(raw.get("ok", True))
|
|
|
- has_fix = raw.get("has_fix")
|
|
|
+ ok = bool(payload.get("ok", True))
|
|
|
+ has_fix = payload.get("has_fix")
|
|
|
if has_fix is not None:
|
|
|
ok = ok and bool(has_fix)
|
|
|
- lat = raw.get("lat")
|
|
|
- lon = raw.get("lon")
|
|
|
- if lat is None or lon is None:
|
|
|
- lat = raw.get("latitude")
|
|
|
- lon = raw.get("longitude")
|
|
|
+
|
|
|
+ lat = payload.get("lat", payload.get("latitude"))
|
|
|
+ lon = payload.get("lon", payload.get("longitude"))
|
|
|
try:
|
|
|
lat_f = float(lat)
|
|
|
lon_f = float(lon)
|
|
|
except Exception:
|
|
|
return GnssFix(False, 0.0, 0.0, 0.0)
|
|
|
|
|
|
- speed_mps = raw.get("speed_mps")
|
|
|
+ speed_mps = payload.get("speed_mps")
|
|
|
if speed_mps is None:
|
|
|
- speed_knots = raw.get("speed_knots")
|
|
|
+ speed_knots = payload.get("speed_knots")
|
|
|
if speed_knots is not None:
|
|
|
try:
|
|
|
speed_mps = float(speed_knots) * 0.514444
|
|
|
except Exception:
|
|
|
speed_mps = 0.0
|
|
|
else:
|
|
|
- speed_kmh = raw.get("speed_kmh")
|
|
|
+ speed_kmh = payload.get("speed_kmh")
|
|
|
if speed_kmh is not None:
|
|
|
try:
|
|
|
speed_mps = float(speed_kmh) / 3.6
|
|
|
except Exception:
|
|
|
speed_mps = 0.0
|
|
|
else:
|
|
|
- speed_mps = raw.get("speed") or 0.0
|
|
|
+ speed_mps = payload.get("speed") or 0.0
|
|
|
try:
|
|
|
speed_mps_f = float(speed_mps)
|
|
|
except Exception:
|
|
|
speed_mps_f = 0.0
|
|
|
|
|
|
- heading = raw.get("heading_deg")
|
|
|
- if heading is None:
|
|
|
- heading = raw.get("course_deg")
|
|
|
+ heading = payload.get("heading_deg", payload.get("course_deg"))
|
|
|
heading_f = None
|
|
|
if heading is not None:
|
|
|
try:
|
|
|
@@ -118,7 +146,7 @@ def _parse_fix(raw: Any) -> GnssFix:
|
|
|
except Exception:
|
|
|
heading_f = None
|
|
|
|
|
|
- t = raw.get("t") or raw.get("timestamp") or raw.get("time")
|
|
|
+ t = payload.get("t") or payload.get("timestamp") or payload.get("time")
|
|
|
t_i = None
|
|
|
if t is not None:
|
|
|
try:
|
|
|
@@ -126,8 +154,7 @@ def _parse_fix(raw: Any) -> GnssFix:
|
|
|
except Exception:
|
|
|
t_i = None
|
|
|
|
|
|
- # Optional: fix quality
|
|
|
- fix = raw.get("fix") or raw.get("fix_quality") or raw.get("quality")
|
|
|
+ fix = payload.get("fix") or payload.get("fix_quality") or payload.get("quality")
|
|
|
if fix is not None:
|
|
|
try:
|
|
|
ok = ok and int(fix) > 0
|
|
|
@@ -137,20 +164,72 @@ def _parse_fix(raw: Any) -> GnssFix:
|
|
|
return GnssFix(ok, lat_f, lon_f, max(0.0, speed_mps_f), heading_f, t_i)
|
|
|
|
|
|
|
|
|
-def _pick_gnss_tool_name(conn) -> Optional[str]:
|
|
|
+def _parse_counter_state(raw: Any) -> CounterState:
|
|
|
+ payload = _parse_jsonish(raw)
|
|
|
+ if payload is None:
|
|
|
+ return CounterState(False, 0, 0, 0)
|
|
|
+
|
|
|
+ if "counter" in payload and isinstance(payload.get("counter"), dict):
|
|
|
+ payload = payload["counter"]
|
|
|
+
|
|
|
+ ok = bool(payload.get("ok", True))
|
|
|
+ has_value = payload.get("has_value")
|
|
|
+ if has_value is not None:
|
|
|
+ ok = ok and bool(has_value)
|
|
|
+
|
|
|
+ value = (
|
|
|
+ payload.get("value")
|
|
|
+ or payload.get("counter_value")
|
|
|
+ or payload.get("counter")
|
|
|
+ or payload.get("raw")
|
|
|
+ )
|
|
|
+ absolute = payload.get("absolute") or payload.get("counter_absolute")
|
|
|
+ cycle = payload.get("cycle", 0)
|
|
|
+ t = payload.get("t") or payload.get("timestamp") or payload.get("time")
|
|
|
+
|
|
|
+ try:
|
|
|
+ value_i = int(value)
|
|
|
+ except Exception:
|
|
|
+ return CounterState(False, 0, 0, 0)
|
|
|
+ if value_i < 1 or value_i > COUNTER_MAX_VALUE:
|
|
|
+ return CounterState(False, 0, 0, 0)
|
|
|
+
|
|
|
+ try:
|
|
|
+ absolute_i = int(absolute) if absolute is not None else value_i
|
|
|
+ except Exception:
|
|
|
+ absolute_i = value_i
|
|
|
+ try:
|
|
|
+ cycle_i = int(cycle)
|
|
|
+ except Exception:
|
|
|
+ cycle_i = 0
|
|
|
+ try:
|
|
|
+ t_i = int(t) if t is not None else None
|
|
|
+ except Exception:
|
|
|
+ t_i = None
|
|
|
+
|
|
|
+ return CounterState(ok, value_i, max(value_i, absolute_i), max(0, cycle_i), t_i)
|
|
|
+
|
|
|
+
|
|
|
+def _pick_tool_name(conn, preferred: str, keywords: Tuple[str, ...]) -> Optional[str]:
|
|
|
if not hasattr(conn, "mcp_client") or not conn.mcp_client:
|
|
|
return None
|
|
|
- # Prefer sanitized name (server side sanitizes tool names).
|
|
|
- preferred = "self_gnss_get_fix"
|
|
|
if conn.mcp_client.has_tool(preferred):
|
|
|
return preferred
|
|
|
- # Fallback heuristic
|
|
|
for name in conn.mcp_client.tools.keys():
|
|
|
- if "gnss" in name.lower() and "fix" in name.lower():
|
|
|
+ lowered = name.lower()
|
|
|
+ if all(keyword in lowered for keyword in keywords):
|
|
|
return name
|
|
|
return None
|
|
|
|
|
|
|
|
|
+def _pick_gnss_tool_name(conn) -> Optional[str]:
|
|
|
+ return _pick_tool_name(conn, "self_gnss_get_fix", ("gnss", "fix"))
|
|
|
+
|
|
|
+
|
|
|
+def _pick_counter_tool_name(conn) -> Optional[str]:
|
|
|
+ return _pick_tool_name(conn, "self_nav_counter_get_state", ("counter", "state"))
|
|
|
+
|
|
|
+
|
|
|
class NavCopilotService:
|
|
|
def __init__(self, conn, db: NavDB):
|
|
|
self.conn = conn
|
|
|
@@ -158,80 +237,277 @@ class NavCopilotService:
|
|
|
|
|
|
self.record_task: Optional[asyncio.Task] = None
|
|
|
self.record_route_id: Optional[int] = None
|
|
|
+ self.record_mode: Optional[str] = None
|
|
|
self._record_seq = 0
|
|
|
self._record_cum_m = 0.0
|
|
|
self._record_last_latlon: Optional[Tuple[float, float]] = None
|
|
|
self._record_last_t = 0
|
|
|
- self._record_interval_s = 1.0
|
|
|
+ self._record_interval_s = 0.1
|
|
|
+ self._record_counter_start_absolute: Optional[int] = None
|
|
|
+ self._record_counter_last_absolute: Optional[int] = None
|
|
|
+ self._record_counter_hazards: List[Hazard] = []
|
|
|
+ self._record_counter_announced: Set[Tuple[int, int, str]] = set()
|
|
|
+ self._record_last_announce_at = 0.0
|
|
|
|
|
|
self.run_task: Optional[asyncio.Task] = None
|
|
|
self.run_route_id: Optional[int] = None
|
|
|
- self._run_interval_s = 0.2
|
|
|
+ self.run_mode: Optional[str] = None
|
|
|
+ self._run_interval_s = 0.1
|
|
|
self._run_points: List[RoutePoint] = []
|
|
|
self._run_hazards: List[Hazard] = []
|
|
|
self._run_last_idx: Optional[int] = None
|
|
|
self._run_announced: Set[int] = set()
|
|
|
+ self._run_counter_announced: Set[Tuple[int, int, str]] = set()
|
|
|
self._run_last_announce_at = 0.0
|
|
|
|
|
|
self.announce_at_m = 50.0
|
|
|
self.announce_window_m = 18.0
|
|
|
self.announce_cooldown_s = 5.0
|
|
|
+ self.counter_announce_ahead = 6
|
|
|
+ self.counter_announce_cooldown_s = 1.5
|
|
|
+
|
|
|
+ def _counter_input_hint(self) -> str:
|
|
|
+ return (
|
|
|
+ "请确认 STM32 已接到 RXD1/GND，波特率 115200，"
|
|
|
+ "并且按带分隔符的文本格式发送，例如 1\\n 2\\n 3\\n ... 256\\n。"
|
|
|
+ )
|
|
|
+
|
|
|
+ def _speak(self, text: str) -> None:
|
|
|
+ try:
|
|
|
+ self.conn.tts.tts_one_sentence(
|
|
|
+ self.conn,
|
|
|
+ ContentType.TEXT,
|
|
|
+ content_detail=text,
|
|
|
+ )
|
|
|
+ except Exception as exc:
|
|
|
+ logger.bind(tag=TAG).warning(f"TTS speak failed: {exc}")
|
|
|
+
|
|
|
+ def _normalize_mode(self, mode: str | None) -> str:
|
|
|
+ normalized = (mode or "auto").strip().lower()
|
|
|
+ if normalized not in {"auto", "gps", "counter"}:
|
|
|
+ return "auto"
|
|
|
+ return normalized
|
|
|
+
|
|
|
+ def _resolve_mode(self, requested_mode: str | None, route_id: int | None = None) -> str:
|
|
|
+ mode = self._normalize_mode(requested_mode)
|
|
|
+ if route_id:
|
|
|
+ stored_mode = self.db.get_route_mode(route_id)
|
|
|
+ if stored_mode in {"gps", "counter"}:
|
|
|
+ return stored_mode
|
|
|
+ if mode == "gps":
|
|
|
+ if not _pick_gnss_tool_name(self.conn):
|
|
|
+ raise RuntimeError("设备端没有可用的 GNSS 定位工具")
|
|
|
+ return "gps"
|
|
|
+ if mode == "counter":
|
|
|
+ if not _pick_counter_tool_name(self.conn):
|
|
|
+ raise RuntimeError("设备端没有可用的串口计数工具")
|
|
|
+ return "counter"
|
|
|
+ if _pick_counter_tool_name(self.conn):
|
|
|
+ return "counter"
|
|
|
+ if _pick_gnss_tool_name(self.conn):
|
|
|
+ return "gps"
|
|
|
+ raise RuntimeError("设备端既没有 GNSS 工具，也没有串口计数工具")
|
|
|
|
|
|
async def _get_fix(self) -> GnssFix:
|
|
|
tool_name = _pick_gnss_tool_name(self.conn)
|
|
|
if not tool_name:
|
|
|
return GnssFix(False, 0.0, 0.0, 0.0)
|
|
|
try:
|
|
|
- raw = await call_mcp_tool(self.conn, self.conn.mcp_client, tool_name, "{}")
|
|
|
+ raw = await call_mcp_tool(
|
|
|
+ self.conn,
|
|
|
+ self.conn.mcp_client,
|
|
|
+ tool_name,
|
|
|
+ "{}",
|
|
|
+ timeout=5,
|
|
|
+ )
|
|
|
return _parse_fix(raw)
|
|
|
- except Exception as e:
|
|
|
- logger.bind(tag=TAG).warning(f"GNSS fix failed: {e}")
|
|
|
+ except Exception as exc:
|
|
|
+ logger.bind(tag=TAG).warning(f"GNSS fix failed: {exc}")
|
|
|
return GnssFix(False, 0.0, 0.0, 0.0)
|
|
|
|
|
|
- def _speak(self, text: str) -> None:
|
|
|
+ async def _get_counter_state(self) -> CounterState:
|
|
|
+ tool_name = _pick_counter_tool_name(self.conn)
|
|
|
+ if not tool_name:
|
|
|
+ return CounterState(False, 0, 0, 0)
|
|
|
try:
|
|
|
- self.conn.tts.tts_one_sentence(self.conn, ContentType.TEXT, content_detail=text)
|
|
|
- except Exception as e:
|
|
|
- logger.bind(tag=TAG).warning(f"TTS speak failed: {e}")
|
|
|
-
|
|
|
- async def start_recording(self, route_name: Optional[str] = None, sample_hz: int = 1) -> int:
|
|
|
- if self.record_task and not self.record_task.done():
|
|
|
- raise RuntimeError("已在录制中")
|
|
|
- if self.run_task and not self.run_task.done():
|
|
|
- raise RuntimeError("正在领航中，不能开始录制")
|
|
|
- if not _pick_gnss_tool_name(self.conn):
|
|
|
- raise RuntimeError("车载端暂未提供定位数据接口，请先在设备端启用定位数据上报。")
|
|
|
+ raw = await call_mcp_tool(
|
|
|
+ self.conn,
|
|
|
+ self.conn.mcp_client,
|
|
|
+ tool_name,
|
|
|
+ "{}",
|
|
|
+ timeout=5,
|
|
|
+ )
|
|
|
+ return _parse_counter_state(raw)
|
|
|
+ except Exception as exc:
|
|
|
+ logger.bind(tag=TAG).warning(f"Counter state failed: {exc}")
|
|
|
+ return CounterState(False, 0, 0, 0)
|
|
|
+
|
|
|
+ def _counter_distance(self, current_value: int, target_value: int) -> int:
|
|
|
+ delta = target_value - current_value
|
|
|
+ if delta < 0:
|
|
|
+ delta += COUNTER_MAX_VALUE
|
|
|
+ return delta
|
|
|
+
|
|
|
+ def _prune_counter_announced(
|
|
|
+ self,
|
|
|
+ announced: Set[Tuple[int, int, str]],
|
|
|
+ current_cycle: int,
|
|
|
+ ) -> None:
|
|
|
+ stale = {
|
|
|
+ item for item in announced if item[1] < max(0, current_cycle - 1)
|
|
|
+ }
|
|
|
+ announced.difference_update(stale)
|
|
|
+
|
|
|
+ def _maybe_announce_counter_hazard(
|
|
|
+ self,
|
|
|
+ sample: CounterState,
|
|
|
+ hazards: List[Hazard],
|
|
|
+ announced: Set[Tuple[int, int, str]],
|
|
|
+ *,
|
|
|
+ last_announce_at: float,
|
|
|
+ ) -> float:
|
|
|
+ if not sample.ok or not hazards:
|
|
|
+ return last_announce_at
|
|
|
+
|
|
|
+ self._prune_counter_announced(announced, sample.cycle)
|
|
|
+
|
|
|
+ for hazard in hazards:
|
|
|
+ if hazard.counter_value <= 0:
|
|
|
+ continue
|
|
|
+ arrive_key = (hazard.id, sample.cycle, "arrived")
|
|
|
+ if sample.value != hazard.counter_value or arrive_key in announced:
|
|
|
+ continue
|
|
|
+
|
|
|
+ label = _HAZARD_LABELS.get(hazard.type, _HAZARD_LABELS["other"])
|
|
|
+ text = f"当前{label}"
|
|
|
+ if hazard.note:
|
|
|
+ text = f"{text}，{hazard.note}"
|
|
|
+ self._speak(text)
|
|
|
+ announced.add(arrive_key)
|
|
|
+ return time.time()
|
|
|
+
|
|
|
+ next_hazard: Optional[Hazard] = None
|
|
|
+ next_distance: Optional[int] = None
|
|
|
+ for hazard in hazards:
|
|
|
+ if hazard.counter_value <= 0:
|
|
|
+ continue
|
|
|
+ distance = self._counter_distance(sample.value, hazard.counter_value)
|
|
|
+ if distance == 0:
|
|
|
+ continue
|
|
|
+ ahead_key = (hazard.id, sample.cycle, "ahead")
|
|
|
+ if ahead_key in announced:
|
|
|
+ continue
|
|
|
+ if next_distance is None or distance < next_distance:
|
|
|
+ next_hazard = hazard
|
|
|
+ next_distance = distance
|
|
|
+
|
|
|
+ if next_hazard is None or next_distance is None:
|
|
|
+ return last_announce_at
|
|
|
+ if not (1 <= next_distance <= self.counter_announce_ahead):
|
|
|
+ return last_announce_at
|
|
|
+
|
|
|
+ now = time.time()
|
|
|
+ if now - last_announce_at < self.counter_announce_cooldown_s:
|
|
|
+ return last_announce_at
|
|
|
+
|
|
|
+ label = _HAZARD_LABELS.get(next_hazard.type, _HAZARD_LABELS["other"])
|
|
|
+ text = f"注意{label}"
|
|
|
+ if next_hazard.counter_value > 0:
|
|
|
+ text = f"{text}，目标点 {next_hazard.counter_value}"
|
|
|
+ if next_hazard.note:
|
|
|
+ text = f"{text}，{next_hazard.note}"
|
|
|
+ self._speak(text)
|
|
|
+ announced.add((next_hazard.id, sample.cycle, "ahead"))
|
|
|
+ return now
|
|
|
+
|
|
|
+ def _pick_counter_point(self, points: List[RoutePoint], sample: CounterState) -> RoutePoint:
|
|
|
+ if not points:
|
|
|
+ raise RuntimeError("路线里还没有计数点")
|
|
|
+ return min(
|
|
|
+ points,
|
|
|
+ key=lambda point: min(
|
|
|
+ self._counter_distance(sample.value, point.counter_value or 1),
|
|
|
+ self._counter_distance(point.counter_value or 1, sample.value),
|
|
|
+ ),
|
|
|
+ )
|
|
|
|
|
|
+ async def start_recording(
|
|
|
+ self,
|
|
|
+ route_name: Optional[str] = None,
|
|
|
+ sample_hz: int = 10,
|
|
|
+ mode: str | None = None,
|
|
|
+ ) -> int:
|
|
|
+ if self.record_route_id is not None:
|
|
|
+ raise RuntimeError("当前已经在记录路线")
|
|
|
+ if self.run_route_id is not None:
|
|
|
+ raise RuntimeError("当前正在预警，不能同时开始记录")
|
|
|
+
|
|
|
+ route_mode = self._resolve_mode(mode)
|
|
|
name = (route_name or "").strip() or f"路线 {time.strftime('%Y-%m-%d %H:%M:%S')}"
|
|
|
- route_id = self.db.create_route(name)
|
|
|
+ route_id = self.db.create_route(name, route_mode)
|
|
|
+
|
|
|
self.record_route_id = route_id
|
|
|
+ self.record_mode = route_mode
|
|
|
self._record_seq = 0
|
|
|
self._record_cum_m = 0.0
|
|
|
self._record_last_latlon = None
|
|
|
self._record_last_t = 0
|
|
|
- self._record_interval_s = 1.0 / max(1, int(sample_hz))
|
|
|
-
|
|
|
- self.record_task = asyncio.create_task(self._record_loop(), name=f"NavRecord:{route_id}")
|
|
|
+ self._record_counter_start_absolute = None
|
|
|
+ self._record_counter_last_absolute = None
|
|
|
+ self._record_counter_hazards = []
|
|
|
+ self._record_counter_announced = set()
|
|
|
+ self._record_last_announce_at = 0.0
|
|
|
+ hz = max(1, int(sample_hz or 10))
|
|
|
+ if route_mode == "counter":
|
|
|
+ hz = max(5, hz)
|
|
|
+ self._record_interval_s = 1.0 / hz
|
|
|
+
|
|
|
+ if route_mode == "counter":
|
|
|
+ self.record_task = None
|
|
|
+ else:
|
|
|
+ self.record_task = asyncio.create_task(
|
|
|
+ self._record_gps_loop(),
|
|
|
+ name=f"NavRecordGps:{route_id}",
|
|
|
+ )
|
|
|
return route_id
|
|
|
|
|
|
- async def stop_recording(self) -> Optional[int]:
|
|
|
- rid = self.record_route_id
|
|
|
+ async def stop_recording(self) -> Optional[StopRecordingResult]:
|
|
|
+ route_id = self.record_route_id
|
|
|
+ if route_id is None:
|
|
|
+ return None
|
|
|
+
|
|
|
+ route_mode = self.record_mode or self.db.get_route_mode(route_id)
|
|
|
if self.record_task and not self.record_task.done():
|
|
|
self.record_task.cancel()
|
|
|
try:
|
|
|
await self.record_task
|
|
|
except BaseException:
|
|
|
pass
|
|
|
+
|
|
|
self.record_task = None
|
|
|
self.record_route_id = None
|
|
|
- if rid is not None:
|
|
|
- self.db.finish_route(rid)
|
|
|
- return rid
|
|
|
+ self.record_mode = None
|
|
|
+ self._record_counter_hazards = []
|
|
|
+ self._record_counter_announced = set()
|
|
|
+ self.db.finish_route(route_id)
|
|
|
+
|
|
|
+ hazard_count = len(self.db.load_hazards(route_id))
|
|
|
+ monitor_started = False
|
|
|
+ if route_mode == "counter" and hazard_count > 0:
|
|
|
+ await self.start_run(route_id=route_id, tick_hz=10, mode="counter")
|
|
|
+ monitor_started = True
|
|
|
+
|
|
|
+ return StopRecordingResult(
|
|
|
+ route_id=route_id,
|
|
|
+ mode=route_mode,
|
|
|
+ hazard_count=hazard_count,
|
|
|
+ monitor_started=monitor_started,
|
|
|
+ )
|
|
|
|
|
|
- async def _record_loop(self) -> None:
|
|
|
+ async def _record_gps_loop(self) -> None:
|
|
|
assert self.record_route_id is not None
|
|
|
route_id = self.record_route_id
|
|
|
- logger.bind(tag=TAG).info(f"Nav record loop started, route_id={route_id}")
|
|
|
+ logger.bind(tag=TAG).info(f"GPS record loop started, route_id={route_id}")
|
|
|
try:
|
|
|
while not self.conn.stop_event.is_set():
|
|
|
fix = await self._get_fix()
|
|
|
@@ -283,9 +559,65 @@ class NavCopilotService:
|
|
|
except Exception:
|
|
|
pass
|
|
|
self.record_route_id = None
|
|
|
+ self.record_mode = None
|
|
|
self.record_task = None
|
|
|
|
|
|
- async def mark_hazard(self, hazard_type: str, note: str = "") -> int:
|
|
|
+ async def _record_counter_loop(self) -> None:
|
|
|
+ assert self.record_route_id is not None
|
|
|
+ route_id = self.record_route_id
|
|
|
+ logger.bind(tag=TAG).info(f"Counter record loop started, route_id={route_id}")
|
|
|
+ try:
|
|
|
+ while not self.conn.stop_event.is_set():
|
|
|
+ sample = await self._get_counter_state()
|
|
|
+ now_t = int(sample.t or time.time())
|
|
|
+ if sample.ok:
|
|
|
+ if self._record_counter_start_absolute is None:
|
|
|
+ self._record_counter_start_absolute = sample.absolute
|
|
|
+
|
|
|
+ should_add = (
|
|
|
+ self._record_counter_last_absolute is None
|
|
|
+ or sample.absolute != self._record_counter_last_absolute
|
|
|
+ )
|
|
|
+ if should_add:
|
|
|
+ self._record_seq += 1
|
|
|
+ self._record_cum_m = float(
|
|
|
+ max(0, sample.absolute - self._record_counter_start_absolute)
|
|
|
+ )
|
|
|
+ self._record_counter_last_absolute = sample.absolute
|
|
|
+ self._record_last_t = now_t
|
|
|
+ self.db.add_point(
|
|
|
+ route_id=route_id,
|
|
|
+ seq=self._record_seq,
|
|
|
+ t=now_t,
|
|
|
+ lat=0.0,
|
|
|
+ lon=0.0,
|
|
|
+ speed_mps=0.0,
|
|
|
+ cum_dist_m=self._record_cum_m,
|
|
|
+ counter_value=sample.value,
|
|
|
+ counter_absolute=sample.absolute,
|
|
|
+ )
|
|
|
+
|
|
|
+ self._record_last_announce_at = self._maybe_announce_counter_hazard(
|
|
|
+ sample,
|
|
|
+ self._record_counter_hazards,
|
|
|
+ self._record_counter_announced,
|
|
|
+ last_announce_at=self._record_last_announce_at,
|
|
|
+ )
|
|
|
+
|
|
|
+ await asyncio.sleep(self._record_interval_s)
|
|
|
+ finally:
|
|
|
+ if self.record_route_id == route_id:
|
|
|
+ try:
|
|
|
+ self.db.finish_route(route_id)
|
|
|
+ except Exception:
|
|
|
+ pass
|
|
|
+ self.record_route_id = None
|
|
|
+ self.record_mode = None
|
|
|
+ self.record_task = None
|
|
|
+ self._record_counter_hazards = []
|
|
|
+ self._record_counter_announced = set()
|
|
|
+
|
|
|
+ async def mark_hazard(self, hazard_type: str, note: str = "") -> HazardMarkResult:
|
|
|
hazard_type = (hazard_type or "").strip() or "other"
|
|
|
if hazard_type in _HAZARD_SYNONYMS:
|
|
|
hazard_type = _HAZARD_SYNONYMS[hazard_type]
|
|
|
@@ -294,32 +626,71 @@ class NavCopilotService:
|
|
|
|
|
|
route_id = self.record_route_id or self.run_route_id or self.db.get_active_route_id()
|
|
|
if not route_id:
|
|
|
- raise RuntimeError("没有可用路线：先说“开始录制路线”")
|
|
|
- if not _pick_gnss_tool_name(self.conn):
|
|
|
- raise RuntimeError("车载端暂未提供定位数据接口，无法读取当前位置。")
|
|
|
+ raise RuntimeError("没有可用路线，请先开始记录")
|
|
|
+ route_mode = self.db.get_route_mode(route_id)
|
|
|
+ now_t = int(time.time())
|
|
|
+
|
|
|
+ if route_mode == "counter":
|
|
|
+ sample = await self._get_counter_state()
|
|
|
+ if not sample.ok:
|
|
|
+ raise RuntimeError(
|
|
|
+ "当前没有可用的串口计数值，无法标记风险点。"
|
|
|
+ + self._counter_input_hint()
|
|
|
+ )
|
|
|
+
|
|
|
+ points = self.db.load_route_points(route_id)
|
|
|
+ if points:
|
|
|
+ point = self._pick_counter_point(points, sample)
|
|
|
+ seq = point.seq
|
|
|
+ cum = point.cum_dist_m
|
|
|
+ else:
|
|
|
+ seq = len(self.db.load_hazards(route_id)) + 1
|
|
|
+ cum = float(max(0, seq - 1))
|
|
|
+
|
|
|
+ hazard_id = self.db.add_hazard(
|
|
|
+ route_id=route_id,
|
|
|
+ seq=seq,
|
|
|
+ t=int(sample.t or now_t),
|
|
|
+ lat=0.0,
|
|
|
+ lon=0.0,
|
|
|
+ cum_dist_m=cum,
|
|
|
+ hazard_type=hazard_type,
|
|
|
+ note=note or "",
|
|
|
+ counter_value=sample.value,
|
|
|
+ counter_absolute=sample.absolute,
|
|
|
+ )
|
|
|
+
|
|
|
+ if self.record_route_id == route_id and self.record_mode == "counter":
|
|
|
+ self._record_counter_hazards = self.db.load_hazards(route_id)
|
|
|
+ if self.run_route_id == route_id and self.run_mode == "counter":
|
|
|
+ self._run_hazards = self.db.load_hazards(route_id)
|
|
|
+ return HazardMarkResult(
|
|
|
+ hazard_id=hazard_id,
|
|
|
+ route_id=route_id,
|
|
|
+ mode="counter",
|
|
|
+ counter_value=sample.value,
|
|
|
+ counter_absolute=sample.absolute,
|
|
|
+ )
|
|
|
|
|
|
fix = await self._get_fix()
|
|
|
if not fix.ok:
|
|
|
- raise RuntimeError("当前没有有效GPS定位，标记失败")
|
|
|
-
|
|
|
- now_t = int(fix.t or time.time())
|
|
|
+ raise RuntimeError("当前没有有效 GPS 定位，无法标记风险点")
|
|
|
|
|
|
- # Prefer current recording cum if we are recording
|
|
|
if self.record_route_id == route_id and self._record_last_latlon is not None:
|
|
|
seq = self._record_seq
|
|
|
cum = self._record_cum_m
|
|
|
else:
|
|
|
points = self.db.load_route_points(route_id)
|
|
|
if not points:
|
|
|
- raise RuntimeError("路线还没有轨迹点")
|
|
|
- idx, _ = nearest_point_index([(p.lat, p.lon) for p in points], fix.lat, fix.lon)
|
|
|
+ raise RuntimeError("路线里还没有轨迹点")
|
|
|
+ idx, _ = nearest_point_index([(point.lat, point.lon) for point in points], fix.lat, fix.lon)
|
|
|
seq = points[idx].seq
|
|
|
cum = points[idx].cum_dist_m
|
|
|
|
|
|
- hid = self.db.add_hazard(
|
|
|
+ hazard_id = self.db.add_hazard(
|
|
|
route_id=route_id,
|
|
|
seq=seq,
|
|
|
- t=now_t,
|
|
|
+ t=int(fix.t or now_t),
|
|
|
lat=fix.lat,
|
|
|
lon=fix.lon,
|
|
|
cum_dist_m=cum,
|
|
|
@@ -327,42 +698,64 @@ class NavCopilotService:
|
|
|
note=note or "",
|
|
|
)
|
|
|
|
|
|
- # If we're running on this route, refresh hazards in memory
|
|
|
- if self.run_route_id == route_id and self.run_task and not self.run_task.done():
|
|
|
+ if self.run_route_id == route_id and self.run_mode == "gps":
|
|
|
self._run_hazards = self.db.load_hazards(route_id)
|
|
|
+ return HazardMarkResult(
|
|
|
+ hazard_id=hazard_id,
|
|
|
+ route_id=route_id,
|
|
|
+ mode="gps",
|
|
|
+ )
|
|
|
|
|
|
- return hid
|
|
|
-
|
|
|
- async def start_run(self, route_id: Optional[int] = None, tick_hz: int = 5) -> int:
|
|
|
- if self.run_task and not self.run_task.done():
|
|
|
- raise RuntimeError("已在领航中")
|
|
|
- if self.record_task and not self.record_task.done():
|
|
|
- raise RuntimeError("正在录制中，不能开始领航")
|
|
|
- if not _pick_gnss_tool_name(self.conn):
|
|
|
- raise RuntimeError("车载端暂未提供定位数据接口，请先在设备端启用定位数据上报。")
|
|
|
-
|
|
|
- rid = route_id or self.db.get_active_route_id()
|
|
|
- if not rid:
|
|
|
- raise RuntimeError("没有可用路线：先说“开始录制路线”并跑一遍")
|
|
|
-
|
|
|
- points = self.db.load_route_points(rid)
|
|
|
- hazards = self.db.load_hazards(rid)
|
|
|
- if len(points) < 2:
|
|
|
- raise RuntimeError("路线点太少，无法领航")
|
|
|
-
|
|
|
- self.run_route_id = rid
|
|
|
+ async def start_run(
|
|
|
+ self,
|
|
|
+ route_id: Optional[int] = None,
|
|
|
+ tick_hz: int = 10,
|
|
|
+ mode: str | None = None,
|
|
|
+ ) -> int:
|
|
|
+ if self.run_route_id is not None:
|
|
|
+ raise RuntimeError("当前已经在预警")
|
|
|
+ if self.record_route_id is not None:
|
|
|
+ raise RuntimeError("当前正在记录路线，不能同时开始预警")
|
|
|
+
|
|
|
+ route_id = route_id or self.db.get_active_route_id()
|
|
|
+ if not route_id:
|
|
|
+ raise RuntimeError("没有可用路线，请先开始记录路线")
|
|
|
+
|
|
|
+ route_mode = self._resolve_mode(mode, route_id=route_id)
|
|
|
+ points = self.db.load_route_points(route_id)
|
|
|
+ hazards = self.db.load_hazards(route_id)
|
|
|
+ if route_mode == "gps" and len(points) < 2:
|
|
|
+ raise RuntimeError("GPS 路线点太少，无法开始预警")
|
|
|
+ if route_mode == "counter" and not hazards:
|
|
|
+ raise RuntimeError("这条计数路线还没有任何标记点")
|
|
|
+
|
|
|
+ self.run_route_id = route_id
|
|
|
+ self.run_mode = route_mode
|
|
|
self._run_points = points
|
|
|
self._run_hazards = hazards
|
|
|
self._run_last_idx = None
|
|
|
self._run_announced = set()
|
|
|
+ self._run_counter_announced = set()
|
|
|
self._run_last_announce_at = 0.0
|
|
|
- self._run_interval_s = 1.0 / max(1, int(tick_hz))
|
|
|
-
|
|
|
- self.run_task = asyncio.create_task(self._run_loop(), name=f"NavRun:{rid}")
|
|
|
- return rid
|
|
|
+ hz = max(1, int(tick_hz or 10))
|
|
|
+ if route_mode == "counter":
|
|
|
+ hz = max(5, hz)
|
|
|
+ self._run_interval_s = 1.0 / hz
|
|
|
+
|
|
|
+ if route_mode == "counter":
|
|
|
+ self.run_task = asyncio.create_task(
|
|
|
+ self._run_counter_loop(),
|
|
|
+ name=f"NavRunCounter:{route_id}",
|
|
|
+ )
|
|
|
+ else:
|
|
|
+ self.run_task = asyncio.create_task(
|
|
|
+ self._run_gps_loop(),
|
|
|
+ name=f"NavRunGps:{route_id}",
|
|
|
+ )
|
|
|
+ return route_id
|
|
|
|
|
|
async def stop_run(self) -> Optional[int]:
|
|
|
- rid = self.run_route_id
|
|
|
+ route_id = self.run_route_id
|
|
|
if self.run_task and not self.run_task.done():
|
|
|
self.run_task.cancel()
|
|
|
try:
|
|
|
@@ -371,19 +764,21 @@ class NavCopilotService:
|
|
|
pass
|
|
|
self.run_task = None
|
|
|
self.run_route_id = None
|
|
|
+ self.run_mode = None
|
|
|
self._run_points = []
|
|
|
self._run_hazards = []
|
|
|
self._run_last_idx = None
|
|
|
self._run_announced = set()
|
|
|
- return rid
|
|
|
+ self._run_counter_announced = set()
|
|
|
+ return route_id
|
|
|
|
|
|
- async def _run_loop(self) -> None:
|
|
|
+ async def _run_gps_loop(self) -> None:
|
|
|
assert self.run_route_id is not None
|
|
|
- rid = self.run_route_id
|
|
|
- logger.bind(tag=TAG).info(f"Nav run loop started, route_id={rid}")
|
|
|
+ route_id = self.run_route_id
|
|
|
+ logger.bind(tag=TAG).info(f"GPS run loop started, route_id={route_id}")
|
|
|
|
|
|
points = self._run_points
|
|
|
- latlon_points = [(p.lat, p.lon) for p in points]
|
|
|
+ latlon_points = [(point.lat, point.lon) for point in points]
|
|
|
try:
|
|
|
while not self.conn.stop_event.is_set():
|
|
|
fix = await self._get_fix()
|
|
|
@@ -391,7 +786,7 @@ class NavCopilotService:
|
|
|
await asyncio.sleep(1.0)
|
|
|
continue
|
|
|
|
|
|
- idx, _d = nearest_point_index(
|
|
|
+ idx, _ = nearest_point_index(
|
|
|
latlon_points,
|
|
|
fix.lat,
|
|
|
fix.lon,
|
|
|
@@ -401,64 +796,109 @@ class NavCopilotService:
|
|
|
self._run_last_idx = idx
|
|
|
cur_cum = points[idx].cum_dist_m
|
|
|
|
|
|
- # Find next hazard ahead
|
|
|
- next_h: Optional[Hazard] = None
|
|
|
- next_dist = None
|
|
|
- for h in self._run_hazards:
|
|
|
- if h.id in self._run_announced:
|
|
|
+ next_hazard: Optional[Hazard] = None
|
|
|
+ next_distance: Optional[float] = None
|
|
|
+ for hazard in self._run_hazards:
|
|
|
+ if hazard.id in self._run_announced:
|
|
|
continue
|
|
|
- d = h.cum_dist_m - cur_cum
|
|
|
- if d < -15:
|
|
|
- # already passed
|
|
|
- self._run_announced.add(h.id)
|
|
|
+ distance = hazard.cum_dist_m - cur_cum
|
|
|
+ if distance < -15:
|
|
|
+ self._run_announced.add(hazard.id)
|
|
|
continue
|
|
|
- if d < 0:
|
|
|
+ if distance < 0:
|
|
|
continue
|
|
|
- if next_dist is None or d < next_dist:
|
|
|
- next_h = h
|
|
|
- next_dist = d
|
|
|
+ if next_distance is None or distance < next_distance:
|
|
|
+ next_hazard = hazard
|
|
|
+ next_distance = distance
|
|
|
|
|
|
- if next_h and next_dist is not None:
|
|
|
+ if next_hazard and next_distance is not None:
|
|
|
now = time.time()
|
|
|
if now - self._run_last_announce_at >= self.announce_cooldown_s:
|
|
|
- if (self.announce_at_m - self.announce_window_m) <= next_dist <= self.announce_at_m:
|
|
|
+ if (
|
|
|
+ self.announce_at_m - self.announce_window_m
|
|
|
+ <= next_distance
|
|
|
+ <= self.announce_at_m
|
|
|
+ ):
|
|
|
speed_mps = max(0.5, float(fix.speed_mps or 0.0))
|
|
|
- speed_kmh = speed_mps * 3.6
|
|
|
- eta_s = int(round(next_dist / speed_mps))
|
|
|
- label = _HAZARD_LABELS.get(next_h.type, _HAZARD_LABELS["other"])
|
|
|
- dist_i = int(round(next_dist))
|
|
|
- speed_i = int(round(speed_kmh))
|
|
|
- text = f"注意前方{dist_i}米{label}，当前{speed_i}公里每小时，预计{eta_s}秒抵达。"
|
|
|
- if next_h.note:
|
|
|
- text = f"{text}{next_h.note}"
|
|
|
+ label = _HAZARD_LABELS.get(
|
|
|
+ next_hazard.type,
|
|
|
+ _HAZARD_LABELS["other"],
|
|
|
+ )
|
|
|
+ eta_s = int(round(next_distance / speed_mps))
|
|
|
+ dist_i = int(round(next_distance))
|
|
|
+ text = f"注意前方 {dist_i} 米{label}，预计 {eta_s} 秒到达"
|
|
|
+ if next_hazard.note:
|
|
|
+ text = f"{text}，{next_hazard.note}"
|
|
|
self._speak(text)
|
|
|
self._run_last_announce_at = now
|
|
|
- self._run_announced.add(next_h.id)
|
|
|
+ self._run_announced.add(next_hazard.id)
|
|
|
+
|
|
|
+ await asyncio.sleep(self._run_interval_s)
|
|
|
+ finally:
|
|
|
+ if self.run_route_id == route_id:
|
|
|
+ self.run_route_id = None
|
|
|
+ self.run_mode = None
|
|
|
+ self.run_task = None
|
|
|
+ self._run_points = []
|
|
|
+ self._run_hazards = []
|
|
|
+ self._run_last_idx = None
|
|
|
+ self._run_announced = set()
|
|
|
+ self._run_counter_announced = set()
|
|
|
|
|
|
+ async def _run_counter_loop(self) -> None:
|
|
|
+ assert self.run_route_id is not None
|
|
|
+ route_id = self.run_route_id
|
|
|
+ logger.bind(tag=TAG).info(f"Counter run loop started, route_id={route_id}")
|
|
|
+ try:
|
|
|
+ while not self.conn.stop_event.is_set():
|
|
|
+ sample = await self._get_counter_state()
|
|
|
+ if sample.ok:
|
|
|
+ self._run_last_announce_at = self._maybe_announce_counter_hazard(
|
|
|
+ sample,
|
|
|
+ self._run_hazards,
|
|
|
+ self._run_counter_announced,
|
|
|
+ last_announce_at=self._run_last_announce_at,
|
|
|
+ )
|
|
|
await asyncio.sleep(self._run_interval_s)
|
|
|
finally:
|
|
|
- if self.run_route_id == rid:
|
|
|
+ if self.run_route_id == route_id:
|
|
|
self.run_route_id = None
|
|
|
+ self.run_mode = None
|
|
|
self.run_task = None
|
|
|
self._run_points = []
|
|
|
self._run_hazards = []
|
|
|
self._run_last_idx = None
|
|
|
self._run_announced = set()
|
|
|
+ self._run_counter_announced = set()
|
|
|
|
|
|
def status_text(self) -> str:
|
|
|
- parts = []
|
|
|
+ parts: List[str] = []
|
|
|
if self.record_route_id:
|
|
|
- parts.append(f"正在录制路线ID={self.record_route_id}，点数={self._record_seq}")
|
|
|
+ if self.record_mode == "counter":
|
|
|
+ parts.append(
|
|
|
+ f"正在准备录制计数路线 ID={self.record_route_id}，已标记风险点={len(self.db.load_hazards(self.record_route_id))}"
|
|
|
+ )
|
|
|
+ else:
|
|
|
+ parts.append(
|
|
|
+ f"正在记录路线 ID={self.record_route_id}，模式={self.record_mode or 'unknown'}，点数={self._record_seq}"
|
|
|
+ )
|
|
|
if self.run_route_id:
|
|
|
- parts.append(
|
|
|
- f"正在领航路线ID={self.run_route_id}，障碍点={len(self._run_hazards)}"
|
|
|
- )
|
|
|
+ if self.run_mode == "counter":
|
|
|
+ parts.append(
|
|
|
+ f"正在监听预警路线 ID={self.run_route_id}，风险点={len(self._run_hazards)}"
|
|
|
+ )
|
|
|
+ else:
|
|
|
+ parts.append(
|
|
|
+ f"正在预警路线 ID={self.run_route_id}，模式={self.run_mode or 'unknown'}，风险点={len(self._run_hazards)}"
|
|
|
+ )
|
|
|
if not parts:
|
|
|
- rid = self.db.get_active_route_id()
|
|
|
- if rid:
|
|
|
- parts.append(f"当前激活路线ID={rid}")
|
|
|
+ route_id = self.db.get_active_route_id()
|
|
|
+ if route_id:
|
|
|
+ parts.append(
|
|
|
+ f"当前激活路线 ID={route_id}，模式={self.db.get_route_mode(route_id)}"
|
|
|
+ )
|
|
|
else:
|
|
|
- parts.append("暂无路线，先说“开始录制路线”")
|
|
|
+ parts.append("当前还没有路线，请先开始记录")
|
|
|
return "；".join(parts)
|
|
|
|
|
|
|
