first commit

2026-09-02 15:13:13 +08:00
commit cb00322a50
107 changed files with 10918 additions and 0 deletions
@@ -0,0 +1,278 @@
+"""视觉跟踪控制律配置（TrackingConfig + load_from_json）。
+
+对应 C++ 参考实现 reference-c++/visual_track_control/src/vtc_config.cpp
+（规格：docs/物理追踪思路.md §8：全部阈值集中配置，NFR-007）。
+
+启动期一次性解析：缺字段 / 类型错误 / 非法取值抛 ValueError 并携带完整
+字段名（如 "heading.gain"）；解析结果与 JSON 脱钩，热路径不做热更新。
+"""
+
+import json
+from dataclasses import dataclass, field
+from enum import Enum
+from typing import Any
+
+from .types import ControlMode  # noqa: F401  (re-export 便利性)
+
+
+class HeadingMode(Enum):
+    """航向通道模式（§4.2）。默认速率式：yaw_rate 天然是相对量，无坐标系歧义。"""
+
+    RATE = "rate"          # β → 偏航角速度，输出 yaw_rate_dps
+    POSITION = "position"  # β → 修正量，与当前航向合成绝对航向输出 yaw_deg
+
+
+class NoDistanceAction(Enum):
+    """雷达距离不可用（未关联/超龄）时的距离通道降级动作（§6.3）。"""
+
+    HOLD = "hold"                  # 保持：前向速度指令清零
+    SLOW_APPROACH = "slow_approach"  # 限速接近：按 no_distance_approach_limit_mps 缓速
+    EXIT = "exit"                  # 退出近距操作：输出降级保持
+
+
+@dataclass
+class ControlGroupConfig:
+    frequency_hz: float = 20.0
+    visual_stale_ms: int = 200
+    radar_stale_ms: int = 500
+    attitude_stale_ms: int = 300
+
+
+@dataclass
+class CameraGroupConfig:
+    image_width: int = 1920
+    image_height: int = 1080
+    fov_h_deg: float = 60.0
+    fov_v_deg: float = 35.0
+
+
+@dataclass
+class HeadingGroupConfig:
+    mode: HeadingMode = HeadingMode.RATE
+    gain: float = 1.0
+    yaw_rate_limit_dps: float = 30.0
+    yaw_slew_limit_dps2: float = 60.0
+
+
+@dataclass
+class VerticalGroupConfig:
+    vz_gain_mps_per_deg: float = 0.1
+    vz_limit_mps: float = 2.0
+
+
+@dataclass
+class DistanceGroupConfig:
+    d_exp_m: float = 10.0
+    kp: float = 0.0
+    ki: float = 0.0
+    kd: float = 0.0
+    integral_limit: float = 0.0
+    derivative_filter_coef: float = 0.0
+    approach_velocity_limit_mps: float = 3.0
+    retreat_velocity_limit_mps: float = 1.0
+    no_distance_action: NoDistanceAction = NoDistanceAction.HOLD
+    no_distance_approach_limit_mps: float = 0.5
+
+
+@dataclass
+class AccelLimitConfig:
+    ax_mps2: float = 2.0
+    ay_mps2: float = 2.0
+    az_mps2: float = 1.5
+
+
+@dataclass
+class TrackingConfig:
+    control: ControlGroupConfig = field(default_factory=ControlGroupConfig)
+    camera: CameraGroupConfig = field(default_factory=CameraGroupConfig)
+    heading: HeadingGroupConfig = field(default_factory=HeadingGroupConfig)
+    vertical: VerticalGroupConfig = field(default_factory=VerticalGroupConfig)
+    distance: DistanceGroupConfig = field(default_factory=DistanceGroupConfig)
+    accel_limit: AccelLimitConfig = field(default_factory=AccelLimitConfig)
+
+
+# ---------------------------------------------------------------------------
+# JSON 解析与校验（错误消息携带完整字段名，与 C++ 版文本一致）
+# ---------------------------------------------------------------------------
+
+def _require_section(root: dict, section: str) -> dict:
+    value = root.get(section)
+    if not isinstance(value, dict):
+        raise ValueError(f"配置缺失分组或类型错误: {section}")
+    return value
+
+
+def _is_number(value: Any) -> bool:
+    return isinstance(value, (int, float)) and not isinstance(value, bool)
+
+
+def _is_integer(value: Any) -> bool:
+    return isinstance(value, int) and not isinstance(value, bool)
+
+
+def _require_number(section: dict, section_name: str, key: str) -> float:
+    value = section.get(key)
+    if not _is_number(value):
+        raise ValueError(f"配置缺失字段或类型错误: {section_name}.{key}")
+    return float(value)
+
+
+def _require_integer(section: dict, section_name: str, key: str) -> int:
+    value = section.get(key)
+    if not _is_integer(value):
+        raise ValueError(f"配置缺失字段或类型错误(需整数): {section_name}.{key}")
+    return int(value)
+
+
+def _require_string(section: dict, section_name: str, key: str) -> str:
+    value = section.get(key)
+    if not isinstance(value, str):
+        raise ValueError(f"配置缺失字段或类型错误(需字符串): {section_name}.{key}")
+    return value
+
+
+def _require_positive(value: float, field_name: str) -> float:
+    if not (value > 0.0):
+        raise ValueError(f"配置非法(必须为正): {field_name}")
+    return value
+
+
+def _require_non_negative(value: float, field_name: str) -> float:
+    if value < 0.0:
+        raise ValueError(f"配置非法(不得为负): {field_name}")
+    return value
+
+
+def _require_range(value: float, lo: float, hi: float, field_name: str) -> float:
+    if not (lo < value < hi):
+        raise ValueError(f"配置非法(超出范围): {field_name}")
+    return value
+
+
+def _load_control(root: dict) -> ControlGroupConfig:
+    s = _require_section(root, "control")
+    c = ControlGroupConfig()
+    c.frequency_hz = _require_positive(
+        _require_number(s, "control", "frequency_hz"), "control.frequency_hz")
+    c.visual_stale_ms = _require_integer(s, "control", "visual_stale_ms")
+    c.radar_stale_ms = _require_integer(s, "control", "radar_stale_ms")
+    c.attitude_stale_ms = _require_integer(s, "control", "attitude_stale_ms")
+    if c.visual_stale_ms <= 0 or c.radar_stale_ms <= 0 or c.attitude_stale_ms <= 0:
+        raise ValueError("配置非法: control.*_stale_ms 必须为正")
+    return c
+
+
+def _load_camera(root: dict) -> CameraGroupConfig:
+    s = _require_section(root, "camera")
+    c = CameraGroupConfig()
+    c.image_width = int(_require_positive(
+        float(_require_integer(s, "camera", "image_width")), "camera.image_width"))
+    c.image_height = int(_require_positive(
+        float(_require_integer(s, "camera", "image_height")), "camera.image_height"))
+    c.fov_h_deg = _require_range(_require_number(s, "camera", "fov_h_deg"),
+                                 0.0, 180.0, "camera.fov_h_deg")
+    c.fov_v_deg = _require_range(_require_number(s, "camera", "fov_v_deg"),
+                                 0.0, 180.0, "camera.fov_v_deg")
+    return c
+
+
+def _load_heading(root: dict) -> HeadingGroupConfig:
+    s = _require_section(root, "heading")
+    h = HeadingGroupConfig()
+    mode = _require_string(s, "heading", "mode")
+    if mode == "position":
+        h.mode = HeadingMode.POSITION
+    elif mode == "rate":
+        h.mode = HeadingMode.RATE
+    else:
+        raise ValueError("配置非法: heading.mode 仅支持 position / rate")
+    h.gain = _require_non_negative(_require_number(s, "heading", "gain"), "heading.gain")
+    h.yaw_rate_limit_dps = _require_positive(
+        _require_number(s, "heading", "yaw_rate_limit_dps"), "heading.yaw_rate_limit_dps")
+    h.yaw_slew_limit_dps2 = _require_positive(
+        _require_number(s, "heading", "yaw_slew_limit_dps2"), "heading.yaw_slew_limit_dps2")
+    return h
+
+
+def _load_vertical(root: dict) -> VerticalGroupConfig:
+    s = _require_section(root, "vertical")
+    v = VerticalGroupConfig()
+    v.vz_gain_mps_per_deg = _require_non_negative(
+        _require_number(s, "vertical", "vz_gain_mps_per_deg"),
+        "vertical.vz_gain_mps_per_deg")
+    v.vz_limit_mps = _require_positive(
+        _require_number(s, "vertical", "vz_limit_mps"), "vertical.vz_limit_mps")
+    return v
+
+
+def _load_distance(root: dict) -> DistanceGroupConfig:
+    s = _require_section(root, "distance")
+    d = DistanceGroupConfig()
+    d.d_exp_m = _require_positive(_require_number(s, "distance", "d_exp_m"),
+                                  "distance.d_exp_m")
+    d.kp = _require_non_negative(_require_number(s, "distance", "kp"), "distance.kp")
+    d.ki = _require_non_negative(_require_number(s, "distance", "ki"), "distance.ki")
+    d.kd = _require_non_negative(_require_number(s, "distance", "kd"), "distance.kd")
+    d.integral_limit = _require_non_negative(
+        _require_number(s, "distance", "integral_limit"), "distance.integral_limit")
+    # 容许 -0.0：范围下界略低于 0，读入后归一化为 0
+    d.derivative_filter_coef = _require_range(
+        _require_number(s, "distance", "derivative_filter_coef"),
+        -0.000001, 1.0, "distance.derivative_filter_coef")
+    if d.derivative_filter_coef < 0.0:
+        d.derivative_filter_coef = 0.0
+    d.approach_velocity_limit_mps = _require_positive(
+        _require_number(s, "distance", "approach_velocity_limit_mps"),
+        "distance.approach_velocity_limit_mps")
+    d.retreat_velocity_limit_mps = _require_positive(
+        _require_number(s, "distance", "retreat_velocity_limit_mps"),
+        "distance.retreat_velocity_limit_mps")
+    action = _require_string(s, "distance", "no_distance_action")
+    if action == "hold":
+        d.no_distance_action = NoDistanceAction.HOLD
+    elif action == "slow_approach":
+        d.no_distance_action = NoDistanceAction.SLOW_APPROACH
+    elif action == "exit":
+        d.no_distance_action = NoDistanceAction.EXIT
+    else:
+        raise ValueError(
+            "配置非法: distance.no_distance_action 仅支持 hold / slow_approach / exit")
+    d.no_distance_approach_limit_mps = _require_non_negative(
+        _require_number(s, "distance", "no_distance_approach_limit_mps"),
+        "distance.no_distance_approach_limit_mps")
+    return d
+
+
+def _load_accel_limit(root: dict) -> AccelLimitConfig:
+    s = _require_section(root, "accel_limit")
+    a = AccelLimitConfig()
+    a.ax_mps2 = _require_positive(_require_number(s, "accel_limit", "ax_mps2"),
+                                  "accel_limit.ax_mps2")
+    a.ay_mps2 = _require_positive(_require_number(s, "accel_limit", "ay_mps2"),
+                                  "accel_limit.ay_mps2")
+    a.az_mps2 = _require_positive(_require_number(s, "accel_limit", "az_mps2"),
+                                  "accel_limit.az_mps2")
+    return a
+
+
+def load_from_json(path: str) -> TrackingConfig:
+    """从 JSON 文件一次性加载配置（启动期调用，调参重启生效）。"""
+    try:
+        with open(path, "r", encoding="utf-8") as fp:
+            root = json.load(fp)
+    except OSError:
+        raise ValueError(f"配置文件打开失败: {path}") from None
+    except json.JSONDecodeError as exc:
+        raise ValueError(f"配置文件解析失败: {path} ({exc})") from None
+
+    if not isinstance(root, dict):
+        raise ValueError(f"配置文件解析失败: {path} (根节点必须为对象)")
+
+    cfg = TrackingConfig()
+    cfg.control = _load_control(root)
+    cfg.camera = _load_camera(root)
+    cfg.heading = _load_heading(root)
+    cfg.vertical = _load_vertical(root)
+    cfg.distance = _load_distance(root)
+    cfg.accel_limit = _load_accel_limit(root)
+    return cfg