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 @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")) 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_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, "{}", 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 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_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 = 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 定位，无法标记风险点") 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([(point.lat, point.lon) for point in points], fix.lat, fix.lon) seq = points[idx].seq cum = points[idx].cum_dist_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", ) 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 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._run_points = [] self._run_hazards = [] self._run_last_idx = None self._run_announced = set() 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}") points = self._run_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() if not fix.ok: await asyncio.sleep(1.0) continue idx, _ = nearest_point_index( latlon_points, fix.lat, fix.lon, start_idx=self._run_last_idx, window=120, ) self._run_last_idx = idx cur_cum = points[idx].cum_dist_m next_hazard: Optional[Hazard] = None next_distance: Optional[float] = None for hazard in self._run_hazards: if hazard.id in self._run_announced: continue distance = hazard.cum_dist_m - cur_cum if distance < -15: self._run_announced.add(hazard.id) continue if distance < 0: continue if next_distance is None or distance < next_distance: next_hazard = hazard next_distance = distance 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_distance <= self.announce_at_m ): speed_mps = max(0.5, float(fix.speed_mps or 0.0)) 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_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 == 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: 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_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