code-test-drone/utils/task_runner/log.py

"""log.py —— 任务日志模块：结构化记录任务全流程（事件 + 周期遥测）。

预定流程 B 阶段：让任意一次 SITL / 实飞都能回溯出完整阶段轨迹与异常。

分层：
  * 事件日志（JSONL）：状态机转移、飞行编排事件（解锁/起飞/返航）、异常/紧急。
    低频、带语义、结构化，便于 grep / jq 定位「什么时候发生了什么」。
  * 周期遥测日志（CSV）：每控制周期的位姿 / GPS / 目标 / 控制输出，便于回放与画图。

时间戳：单调毫秒（now_ms，与运行时代码同源）+ 相对时间 t_s（自首个 now_ms 起）。
文件 IO 集中在 open / close，热路径只追加一行，不做 flush 决策。
"""

import csv
import json
import time
from dataclasses import dataclass
from pathlib import Path
from typing import Optional

_TELEMETRY_FIELDS = (
    "t_s", "state",
    "pos_valid", "fix_type", "num_satellites",
    "latitude_deg", "longitude_deg", "relative_altitude_m", "yaw_deg",
    "target_injected", "distance_to_target_m", "target_locked",
    "off_target_x_deg", "off_target_y_deg",
    "mode", "yaw_rate_dps", "vx_body_mps", "vz_mps",
)


@dataclass
class TelemetryRecord:
    """单周期遥测记录（字段顺序即 CSV 列序）。"""

    t_s: float = 0.0
    state: str = ""
    pos_valid: bool = False
    fix_type: int = 0
    num_satellites: int = 0
    latitude_deg: float = 0.0
    longitude_deg: float = 0.0
    relative_altitude_m: float = 0.0
    yaw_deg: float = 0.0
    target_injected: bool = False
    distance_to_target_m: float = float("inf")
    target_locked: bool = False
    off_target_x_deg: float = 0.0
    off_target_y_deg: float = 0.0
    mode: str = ""
    yaw_rate_dps: float = 0.0
    vx_body_mps: float = 0.0
    vz_mps: float = 0.0


@dataclass
class TaskLogConfig:
    """任务日志配置（全外置）。"""

    enabled: bool = True
    dir: str = "logs"                       # 输出目录（相对工作目录）
    event_file: str = "task_events.jsonl"  # 事件日志
    telemetry_file: str = "task_telemetry.csv"  # 周期遥测日志
    telemetry_every_n: int = 1             # 每 N 周期记一次遥测（1 = 每周期）


def _monotonic_ms() -> int:
    return int(time.monotonic() * 1000)


class TaskLogger:
    """结构化任务日志。用法：

        logger = TaskLogger(cfg, now_ms_fn)
        logger.open()                    # 建目录 + 写 CSV 表头
        logger.log_event(now_ms, "state", from_state="wait", ...)
        logger.log_telemetry(now_ms, record)
        logger.close()

    enabled=False 或未 open 时，所有 log 方法为空操作（不抛异常、不写文件）。
    """

    def __init__(self, cfg: TaskLogConfig = TaskLogConfig(),
                 now_ms_fn=_monotonic_ms) -> None:
        self._cfg = cfg
        self._now_ms = now_ms_fn
        self._event_fp = None
        self._tel_fp = None
        self._tel_writer = None
        self._t0_ms: Optional[int] = None
        self._tick = 0

    # --- 生命周期 -----------------------------------------------------------
    def open(self) -> None:
        """创建输出目录并打开两个日志文件；disabled 时跳过。"""
        if not self._cfg.enabled:
            return
        out_dir = Path(self._cfg.dir)
        out_dir.mkdir(parents=True, exist_ok=True)
        self._event_fp = (out_dir / self._cfg.event_file).open(
            "a", encoding="utf-8")
        self._tel_fp = (out_dir / self._cfg.telemetry_file).open(
            "a", encoding="utf-8", newline="")
        self._tel_writer = csv.writer(self._tel_fp)
        if (out_dir / self._cfg.telemetry_file).stat().st_size == 0:
            self._tel_writer.writerow(_TELEMETRY_FIELDS)
        self._event_fp.flush()
        self._tel_fp.flush()

    def close(self) -> None:
        """flush 并关闭两个日志文件。"""
        if self._event_fp is not None:
            self._event_fp.flush()
            self._event_fp.close()
            self._event_fp = None
        if self._tel_fp is not None:
            self._tel_fp.flush()
            self._tel_fp.close()
            self._tel_fp = None
            self._tel_writer = None

    # --- 记录 ---------------------------------------------------------------
    def log_event(self, now_ms: int, event_type: str, **fields) -> None:
        """追加一条 JSONL 事件。"""
        if self._event_fp is None:
            return
        record = {"type": event_type, "ts_ms": now_ms,
                  "t_s": self._rel_s(now_ms)}
        record.update(fields)
        self._event_fp.write(json.dumps(record, ensure_ascii=False) + "\n")

    def log_telemetry(self, now_ms: int, rec: TelemetryRecord) -> None:
        """追加一行遥测（按 telemetry_every_n 节流）。"""
        if self._tel_writer is None:
            return
        self._tick += 1
        if self._tick % max(1, self._cfg.telemetry_every_n) != 0:
            return
        rec.t_s = self._rel_s(now_ms)
        self._tel_writer.writerow([getattr(rec, f) for f in _TELEMETRY_FIELDS])

    # --- 内部 ---------------------------------------------------------------
    def _rel_s(self, now_ms: int) -> float:
        if self._t0_ms is None:
            self._t0_ms = now_ms
        return (now_ms - self._t0_ms) / 1000.0

    @property
    def fieldnames(self) -> tuple:
        return _TELEMETRY_FIELDS