|
|
@@ -1,991 +0,0 @@
|
|
|
-from __future__ import annotations
|
|
|
-
|
|
|
-import asyncio
|
|
|
-import json
|
|
|
-import time
|
|
|
-from dataclasses import dataclass
|
|
|
-from typing import Any, Dict, List, Optional, Set, Tuple
|
|
|
-
|
|
|
-from config.logger import setup_logging
|
|
|
-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
|
|
|
-
|
|
|
-
|
|
|
-TAG = __name__
|
|
|
-logger = setup_logging()
|
|
|
-
|
|
|
-COUNTER_MAX_VALUE = 256
|
|
|
-COUNTER_WRAP_THRESHOLD = 128
|
|
|
-
|
|
|
-
|
|
|
-_HAZARD_LABELS: Dict[str, str] = {
|
|
|
- "sharp_turn": "急转弯",
|
|
|
- "bump": "大坑",
|
|
|
- "jump": "跳台",
|
|
|
- "slope": "陡坡",
|
|
|
- "water": "涉水",
|
|
|
- "obstacle": "障碍",
|
|
|
- "other": "风险点",
|
|
|
-}
|
|
|
-
|
|
|
-_HAZARD_SYNONYMS: Dict[str, str] = {
|
|
|
- "急转弯": "sharp_turn",
|
|
|
- "急弯": "sharp_turn",
|
|
|
- "大坑": "bump",
|
|
|
- "坑": "bump",
|
|
|
- "跳台": "jump",
|
|
|
- "陡坡": "slope",
|
|
|
- "涉水": "water",
|
|
|
- "障碍": "obstacle",
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-@dataclass
|
|
|
-class GnssFix:
|
|
|
- ok: bool
|
|
|
- lat: float
|
|
|
- lon: float
|
|
|
- speed_mps: float
|
|
|
- heading_deg: Optional[float] = None
|
|
|
- t: Optional[int] = None
|
|
|
-
|
|
|
-
|
|
|
-@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
|
|
|
- lat: Optional[float] = None
|
|
|
- lon: Optional[float] = 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 None
|
|
|
- if isinstance(raw, str):
|
|
|
- raw = raw.strip()
|
|
|
- if not raw:
|
|
|
- return None
|
|
|
- try:
|
|
|
- raw = json.loads(raw)
|
|
|
- except Exception:
|
|
|
- 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)
|
|
|
-
|
|
|
- if "gnss" in payload and isinstance(payload.get("gnss"), dict):
|
|
|
- payload = payload["gnss"]
|
|
|
-
|
|
|
- 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 = 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 = payload.get("speed_mps")
|
|
|
- if speed_mps is None:
|
|
|
- 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 = 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 = payload.get("speed") or 0.0
|
|
|
- try:
|
|
|
- speed_mps_f = float(speed_mps)
|
|
|
- except Exception:
|
|
|
- speed_mps_f = 0.0
|
|
|
-
|
|
|
- heading = payload.get("heading_deg", payload.get("course_deg"))
|
|
|
- heading_f = None
|
|
|
- if heading is not None:
|
|
|
- try:
|
|
|
- heading_f = float(heading)
|
|
|
- except Exception:
|
|
|
- heading_f = None
|
|
|
-
|
|
|
- t = payload.get("t") or payload.get("timestamp") or payload.get("time")
|
|
|
- t_i = None
|
|
|
- if t is not None:
|
|
|
- try:
|
|
|
- t_i = int(t)
|
|
|
- except Exception:
|
|
|
- t_i = None
|
|
|
-
|
|
|
- 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
|
|
|
- except Exception:
|
|
|
- pass
|
|
|
-
|
|
|
- return GnssFix(ok, lat_f, lon_f, max(0.0, speed_mps_f), heading_f, t_i)
|
|
|
-
|
|
|
-
|
|
|
-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
|
|
|
- if conn.mcp_client.has_tool(preferred):
|
|
|
- return preferred
|
|
|
- for name in conn.mcp_client.tools.keys():
|
|
|
- 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"))
|
|
|
-
|
|
|
-
|
|
|
-def _pick_nav_alert_tool_name(conn) -> Optional[str]:
|
|
|
- return _pick_tool_name(conn, "self_nav_alert_play", ("nav", "alert", "play"))
|
|
|
-
|
|
|
-
|
|
|
-class NavCopilotService:
|
|
|
- def __init__(self, conn, db: NavDB):
|
|
|
- self.conn = conn
|
|
|
- self.db = db
|
|
|
-
|
|
|
- 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 = 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_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_gps_last_announce_at: Dict[int, float] = {}
|
|
|
- self._run_counter_announced: Set[Tuple[int, int, str]] = set()
|
|
|
- self._run_last_announce_at = 0.0
|
|
|
-
|
|
|
- self.gps_alert_radius_m = 3.0
|
|
|
- self.gps_alert_repeat_cooldown_s = 2.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
|
|
|
-
|
|
|
- async def recover_monitoring_if_needed(self) -> Optional[int]:
|
|
|
- if self.run_route_id is not None:
|
|
|
- return self.run_route_id
|
|
|
- if not _pick_gnss_tool_name(self.conn):
|
|
|
- return None
|
|
|
-
|
|
|
- route_id = self.db.get_monitoring_route_id()
|
|
|
- if not route_id:
|
|
|
- return None
|
|
|
-
|
|
|
- hazards = self.db.load_hazards(route_id)
|
|
|
- if not hazards:
|
|
|
- logger.bind(tag=TAG).warning(
|
|
|
- f"Route {route_id} was marked as monitoring but has no hazards; clearing monitoring state"
|
|
|
- )
|
|
|
- self.db.set_monitoring_route(None)
|
|
|
- return None
|
|
|
-
|
|
|
- route_mode = self.db.get_route_mode(route_id)
|
|
|
- if route_mode != "gps":
|
|
|
- logger.bind(tag=TAG).warning(
|
|
|
- f"Route {route_id} monitoring recovery only supports gps mode, current mode={route_mode}; clearing monitoring state"
|
|
|
- )
|
|
|
- self.db.set_monitoring_route(None)
|
|
|
- return None
|
|
|
-
|
|
|
- try:
|
|
|
- await self.start_run(route_id=route_id, tick_hz=10, mode=route_mode)
|
|
|
- except RuntimeError as exc:
|
|
|
- logger.bind(tag=TAG).warning(
|
|
|
- f"Failed to recover monitoring route {route_id}: {exc}"
|
|
|
- )
|
|
|
- return None
|
|
|
-
|
|
|
- logger.bind(tag=TAG).info(
|
|
|
- f"Recovered GPS monitoring after reconnect, route_id={route_id}, hazards={len(hazards)}"
|
|
|
- )
|
|
|
- return route_id
|
|
|
-
|
|
|
- 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}")
|
|
|
-
|
|
|
- async def _play_local_hazard_alert(self, hazard: Hazard) -> bool:
|
|
|
- tool_name = _pick_nav_alert_tool_name(self.conn)
|
|
|
- if not tool_name:
|
|
|
- return False
|
|
|
- try:
|
|
|
- await call_mcp_tool(
|
|
|
- self.conn,
|
|
|
- self.conn.mcp_client,
|
|
|
- tool_name,
|
|
|
- json.dumps(
|
|
|
- {
|
|
|
- "hazard_type": hazard.type,
|
|
|
- "note": hazard.note or "",
|
|
|
- },
|
|
|
- ensure_ascii=False,
|
|
|
- ),
|
|
|
- timeout=5,
|
|
|
- )
|
|
|
- return True
|
|
|
- except Exception as exc:
|
|
|
- logger.bind(tag=TAG).warning(f"Local hazard alert failed: {exc}")
|
|
|
- return False
|
|
|
-
|
|
|
- async def _announce_gps_hazard(self, hazard: Hazard) -> None:
|
|
|
- if await self._play_local_hazard_alert(hazard):
|
|
|
- return
|
|
|
- label = _HAZARD_LABELS.get(hazard.type, _HAZARD_LABELS["other"])
|
|
|
- text = f"注意{label}"
|
|
|
- if hazard.note:
|
|
|
- text = f"{text},{hazard.note}"
|
|
|
- self._speak(text)
|
|
|
-
|
|
|
- def _normalize_mode(self, mode: str | None) -> str:
|
|
|
- normalized = (mode or "gps").strip().lower()
|
|
|
- if normalized not in {"auto", "gps", "counter"}:
|
|
|
- return "gps"
|
|
|
- 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 _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,
|
|
|
- "{}",
|
|
|
- timeout=5,
|
|
|
- )
|
|
|
- return _parse_fix(raw)
|
|
|
- except Exception as exc:
|
|
|
- logger.bind(tag=TAG).warning(f"GNSS fix failed: {exc}")
|
|
|
- return GnssFix(False, 0.0, 0.0, 0.0)
|
|
|
-
|
|
|
- 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:
|
|
|
- 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_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_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[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
|
|
|
- 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
|
|
|
- logger.bind(tag=TAG).info(
|
|
|
- f"Stop recording route_id={route_id}, mode={route_mode}, hazards={hazard_count}"
|
|
|
- )
|
|
|
- if hazard_count > 0:
|
|
|
- await self.start_run(route_id=route_id, tick_hz=10, mode=route_mode)
|
|
|
- monitor_started = True
|
|
|
-
|
|
|
- return StopRecordingResult(
|
|
|
- route_id=route_id,
|
|
|
- mode=route_mode,
|
|
|
- hazard_count=hazard_count,
|
|
|
- monitor_started=monitor_started,
|
|
|
- )
|
|
|
-
|
|
|
- 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"GPS record loop started, route_id={route_id}")
|
|
|
- try:
|
|
|
- while not self.conn.stop_event.is_set():
|
|
|
- fix = await self._get_fix()
|
|
|
- now_t = int(fix.t or time.time())
|
|
|
- if fix.ok:
|
|
|
- latlon = (fix.lat, fix.lon)
|
|
|
- should_add = False
|
|
|
- if self._record_last_latlon is None:
|
|
|
- should_add = True
|
|
|
- else:
|
|
|
- moved_m = haversine_m(
|
|
|
- self._record_last_latlon[0],
|
|
|
- self._record_last_latlon[1],
|
|
|
- latlon[0],
|
|
|
- latlon[1],
|
|
|
- )
|
|
|
- if moved_m >= 1.0:
|
|
|
- should_add = True
|
|
|
- if (now_t - self._record_last_t) >= 2:
|
|
|
- should_add = True
|
|
|
-
|
|
|
- if should_add:
|
|
|
- if self._record_last_latlon is not None:
|
|
|
- self._record_cum_m += haversine_m(
|
|
|
- self._record_last_latlon[0],
|
|
|
- self._record_last_latlon[1],
|
|
|
- latlon[0],
|
|
|
- latlon[1],
|
|
|
- )
|
|
|
- self._record_last_latlon = latlon
|
|
|
- self._record_last_t = now_t
|
|
|
- self._record_seq += 1
|
|
|
-
|
|
|
- self.db.add_point(
|
|
|
- route_id=route_id,
|
|
|
- seq=self._record_seq,
|
|
|
- t=now_t,
|
|
|
- lat=latlon[0],
|
|
|
- lon=latlon[1],
|
|
|
- speed_mps=fix.speed_mps,
|
|
|
- cum_dist_m=self._record_cum_m,
|
|
|
- )
|
|
|
-
|
|
|
- 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
|
|
|
-
|
|
|
- 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]
|
|
|
- if hazard_type not in _HAZARD_LABELS:
|
|
|
- hazard_type = "other"
|
|
|
-
|
|
|
- route_id = self.record_route_id or self.run_route_id or self.db.get_active_route_id()
|
|
|
- if not route_id:
|
|
|
- raise RuntimeError("没有可用路线,请先开始记录")
|
|
|
- route_mode = "gps"
|
|
|
- now_t = int(time.time())
|
|
|
-
|
|
|
- if False and route_mode == "counter":
|
|
|
- sample = await self._get_counter_state()
|
|
|
- if not sample.ok:
|
|
|
- fix = await self._get_fix()
|
|
|
- if fix.ok:
|
|
|
- logger.bind(tag=TAG).warning(
|
|
|
- f"Counter route {route_id} has no live counter sample but GPS is available; switching route to gps mode"
|
|
|
- )
|
|
|
- self.db.update_route_mode(route_id, "gps")
|
|
|
- route_mode = "gps"
|
|
|
- if self.record_route_id == route_id:
|
|
|
- self.record_mode = "gps"
|
|
|
- if self.record_task is None or self.record_task.done():
|
|
|
- self._record_interval_s = 0.1
|
|
|
- self.record_task = asyncio.create_task(
|
|
|
- self._record_gps_loop(),
|
|
|
- name=f"NavRecordGps:{route_id}",
|
|
|
- )
|
|
|
- if self.run_route_id == route_id:
|
|
|
- self.run_mode = "gps"
|
|
|
- if route_mode == "counter" and not sample.ok:
|
|
|
- raise RuntimeError(
|
|
|
- "当前没有可用的串口计数值,无法标记风险点。"
|
|
|
- + self._counter_input_hint()
|
|
|
- )
|
|
|
-
|
|
|
- if route_mode == "counter":
|
|
|
- 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 定位,无法标记风险点")
|
|
|
-
|
|
|
- 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 points:
|
|
|
- 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
|
|
|
- else:
|
|
|
- seq = self._record_seq if self._record_seq > 0 else len(self.db.load_hazards(route_id)) + 1
|
|
|
- cum = self._record_cum_m
|
|
|
-
|
|
|
- hazard_id = self.db.add_hazard(
|
|
|
- route_id=route_id,
|
|
|
- seq=seq,
|
|
|
- t=int(fix.t or now_t),
|
|
|
- lat=fix.lat,
|
|
|
- lon=fix.lon,
|
|
|
- cum_dist_m=cum,
|
|
|
- hazard_type=hazard_type,
|
|
|
- note=note or "",
|
|
|
- )
|
|
|
-
|
|
|
- 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",
|
|
|
- lat=fix.lat,
|
|
|
- lon=fix.lon,
|
|
|
- )
|
|
|
-
|
|
|
- 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 not hazards:
|
|
|
- raise RuntimeError("这条 GPS 路线还没有任何标记点")
|
|
|
- if route_mode == "counter" and not hazards:
|
|
|
- raise RuntimeError("这条计数路线还没有任何标记点")
|
|
|
-
|
|
|
- logger.bind(tag=TAG).info(
|
|
|
- f"Start run requested, route_id={route_id}, mode={route_mode}, hazards={len(hazards)}, points={len(points)}"
|
|
|
- )
|
|
|
- self.run_route_id = route_id
|
|
|
- self.run_mode = route_mode
|
|
|
- self.db.set_active_route(route_id)
|
|
|
- self.db.set_monitoring_route(route_id)
|
|
|
- self._run_points = points
|
|
|
- self._run_hazards = hazards
|
|
|
- self._run_last_idx = None
|
|
|
- self._run_announced = set()
|
|
|
- self._run_gps_last_announce_at = {}
|
|
|
- self._run_counter_announced = set()
|
|
|
- self._run_last_announce_at = 0.0
|
|
|
- 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]:
|
|
|
- route_id = self.run_route_id
|
|
|
- if self.run_task and not self.run_task.done():
|
|
|
- self.run_task.cancel()
|
|
|
- try:
|
|
|
- await self.run_task
|
|
|
- except BaseException:
|
|
|
- pass
|
|
|
- self.run_task = None
|
|
|
- self.run_route_id = None
|
|
|
- self.run_mode = None
|
|
|
- self.db.set_monitoring_route(None)
|
|
|
- self._run_points = []
|
|
|
- self._run_hazards = []
|
|
|
- self._run_last_idx = None
|
|
|
- self._run_announced = set()
|
|
|
- self._run_gps_last_announce_at = {}
|
|
|
- self._run_counter_announced = set()
|
|
|
- return route_id
|
|
|
-
|
|
|
- async def _run_gps_loop(self) -> None:
|
|
|
- assert self.run_route_id is not None
|
|
|
- route_id = self.run_route_id
|
|
|
- logger.bind(tag=TAG).info(
|
|
|
- f"GPS run loop started, route_id={route_id}, hazards={len(self._run_hazards)}, radius_m={self.gps_alert_radius_m}, repeat_cooldown_s={self.gps_alert_repeat_cooldown_s}"
|
|
|
- )
|
|
|
-
|
|
|
- try:
|
|
|
- while not self.conn.stop_event.is_set():
|
|
|
- fix = await self._get_fix()
|
|
|
- if not fix.ok:
|
|
|
- await asyncio.sleep(1.0)
|
|
|
- continue
|
|
|
-
|
|
|
- for hazard in self._run_hazards:
|
|
|
- if hazard.lat == 0.0 and hazard.lon == 0.0:
|
|
|
- continue
|
|
|
- distance = haversine_m(fix.lat, fix.lon, hazard.lat, hazard.lon)
|
|
|
- if distance <= self.gps_alert_radius_m:
|
|
|
- now = time.monotonic()
|
|
|
- last_announce_at = self._run_gps_last_announce_at.get(hazard.id, 0.0)
|
|
|
- if now - last_announce_at < self.gps_alert_repeat_cooldown_s:
|
|
|
- continue
|
|
|
- logger.bind(tag=TAG).info(
|
|
|
- f"GPS hazard triggered: route_id={route_id}, hazard_id={hazard.id}, distance_m={distance:.3f}, fix=({fix.lat:.6f},{fix.lon:.6f}), hazard=({hazard.lat:.6f},{hazard.lon:.6f})"
|
|
|
- )
|
|
|
- await self._announce_gps_hazard(hazard)
|
|
|
- self._run_gps_last_announce_at[hazard.id] = now
|
|
|
-
|
|
|
- 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_gps_last_announce_at = {}
|
|
|
- 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 == 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_gps_last_announce_at = {}
|
|
|
- self._run_counter_announced = set()
|
|
|
-
|
|
|
- def status_text(self) -> str:
|
|
|
- parts: List[str] = []
|
|
|
- if self.record_route_id:
|
|
|
- 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:
|
|
|
- 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:
|
|
|
- route_id = self.db.get_monitoring_route_id()
|
|
|
- if route_id:
|
|
|
- parts.append(
|
|
|
- f"数据库显示正在预警路线 ID={route_id},模式={self.db.get_route_mode(route_id)},等待本次连接恢复执行"
|
|
|
- )
|
|
|
- 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("当前还没有路线,请先开始记录")
|
|
|
- return ";".join(parts)
|
|
|
-
|
|
|
-
|
|
|
-def get_service(conn) -> NavCopilotService:
|
|
|
- if not hasattr(conn, "nav_copilot_service") or conn.nav_copilot_service is None:
|
|
|
- conn.nav_copilot_service = NavCopilotService(conn, NavDB.default())
|
|
|
- return conn.nav_copilot_service
|