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