287 lines
11 KiB
Python
287 lines
11 KiB
Python
"""视觉跟踪控制律配置（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" # 退出近距操作：持续无距离达 no_distance_exit_grace_ms
|
|
# 才降级保持；grace 期间按 SLOW_APPROACH 行为
|
|
|
|
|
|
@dataclass
|
|
class ControlGroupConfig:
|
|
frequency_hz: float = 20.0
|
|
visual_stale_ms: int = 200
|
|
distance_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
|
|
# 默认定速接近（D4 远场策略）：双目有效域外无距离是常态，退出仅留给
|
|
# 持续无距离（传感器级故障）场景
|
|
no_distance_action: NoDistanceAction = NoDistanceAction.SLOW_APPROACH
|
|
no_distance_approach_limit_mps: float = 0.5
|
|
no_distance_exit_grace_ms: int = 3000 # EXIT 门限：持续无距离达此时长才退出（毫秒，占位）
|
|
|
|
|
|
@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.distance_stale_ms = _require_integer(s, "control", "distance_stale_ms")
|
|
c.attitude_stale_ms = _require_integer(s, "control", "attitude_stale_ms")
|
|
if c.visual_stale_ms <= 0 or c.distance_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")
|
|
# 0 容许：等效旧版"首个无效拍即退出"行为
|
|
d.no_distance_exit_grace_ms = _require_non_negative(
|
|
_require_integer(s, "distance", "no_distance_exit_grace_ms"),
|
|
"distance.no_distance_exit_grace_ms")
|
|
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
|