diff --git a/README.md b/README.md index cef810b..e71cb62 100644 --- a/README.md +++ b/README.md @@ -16,11 +16,12 @@ src/mcm26b/ ├── protocol/ # P0/P1 字段表、计时模型、状态机、串行幂等客户端（已完工） ├── geometry/ # P2 角域交会、直径、MEC、问题二安全候选盘证书（已完工） ├── sim/ # 自建模拟器、故障注入、演练日志回放（测试专用，已完工） -├── belief/ # P3 逐频道候选集合 K_c（骨架） -├── coverage/ # P4 49点覆盖账本（骨架 + 网格生成） -├── planner/ # P3 选点 / P4 全向闭环 / P5 定向扩展（骨架） -└── evaluation/ # 日志复算与统计（复算已完工） -tests/ # pytest（兼容 unittest 风格），53 项 +├── belief/ # P3 逐频道候选集合 K_c：保守外包多边形 + 半平面裁剪（已完工） +├── coverage/ # P4 49点覆盖账本（网格生成 + 逐频道记录） +├── planner/ # P3 保证接收盘选点器 / P4 全向闭环 / P5 混合源定向闭环（已完工） +├── evaluation/ # 日志复算与统计（已完工） +└── sim/scenes.py # 自检场景生成器：全向与混合源（测试端专用） +tests/ # pytest（兼容 unittest 风格），95 项 scripts/ # 采集、基准、可视化与校核脚本 output/ examples/ # 演练证据、算例与生成产物 ``` @@ -33,6 +34,9 @@ Python ≥ 3.10，依赖仅 `requests`（测试另需 `pytest`）。 uv sync # 安装（可编辑挂载本包） uv run pytest # 全部测试 uv run mcm26b sim-check # P0 自检：复现附件计时示例 105/111/194/199 +uv run mcm26b run --robot-id 2026001 --self-test # 问题3自检：随机全向场景闭环（不连官方） +uv run mcm26b run --robot-id 2026001 --self-test --problem 4 # 问题4自检：混合定向源场景 +uv run mcm26b run --robot-id <队号> --log output/run/session.jsonl # 对接模拟器正式/演练 uv run mcm26b q1 examples/q1/normal.json # 问题一求解（等价 python -m mcm26b.geometry ...） uv run python main.py examples/q1/triangle.json --rounding-margin-deg 0.005 ``` @@ -79,9 +83,9 @@ client.exit() | P0 规则固定（规则表/状态机/字段表，示例逐步 105/111/194/199） | ✅ 完成 | | P1 协议基线（客户端 + 自建回放/故障注入测试） | ✅ 完成 | | P2 几何内核（角域交、直径、MEC、证书） | ✅ 完成（迁入 `mcm26b.geometry`） | -| P3 问题二选点（K1、候选区域 Q、采样评估） | 🦴 骨架（证书已完工） | -| P4 问题3全向闭环（49点覆盖、定位队列、清除、停止） | 🦴 骨架 | -| P5 问题4定向扩展（联合约束、后备清除、预算） | 🦴 骨架 | +| P3 问题二选点（K1、候选区域 Q、采样评估） | ✅ 完成（belief + 选点器，测试固化） | +| P4 问题3全向闭环（49点覆盖、定位队列、清除、停止） | ✅ 自建完成（官方演练待执行） | +| P5 问题4定向扩展（联合约束、后备清除、预算） | ✅ 自建完成（官方演练待执行） | | P6 加速与消融 | 🦴 未开始 | ## 采集与正式测试约束 diff --git a/docs/B题建模分析与实施计划.md b/docs/B题建模分析与实施计划.md index 5645ea2..85766de 100644 --- a/docs/B题建模分析与实施计划.md +++ b/docs/B题建模分析与实施计划.md @@ -187,13 +187,13 @@ AI 的适合用法是把这些模块逐一变成可测试程序、生成边界 | 阶段 | 交付与执行任务 | 验收门槛 | 当前状态 | | --- | --- | --- | --- | -| P0 规则固定 | 规则表、状态机、四类API字段与成本 | 示例逐步105/111/194/199秒；clear不切换频道 | 已核对规则，完整状态机待工程化 | -| P1 协议基线 | enter/measure/clear/exit客户端；串行、幂等、截止时间 | accepted=false和超时重试不污染状态；主动退出；故障注入回放 | 已有少量测向采集器及真实计时回放；完整客户端待实现 | +| P0 规则固定 | 规则表、状态机、四类API字段与成本 | 示例逐步105/111/194/199秒；clear不切换频道 | 已完成：`protocol/rules+timing+state` 工程化，`sim-check` 与 `tests/test_protocol_timing.py` 固化 | +| P1 协议基线 | enter/measure/clear/exit客户端；串行、幂等、截止时间 | accepted=false和超时重试不污染状态；主动退出；故障注入回放 | 已完成：`protocol/client`，`ScriptedFaultTransport`/`PlaybackTransport` 回放，`tests/test_protocol_client.py` 全过 | | P2 几何内核 | 问题一解答、区域分类、直径圆证据、MEC、清除余量 | 跨界、退化、无界、解析反例、独立算法对照 | 已实现并优化，20项自动化测试通过 | -| P3 问题二选点 | $K_1$ 先验、安全候选盘、后验半径采样与 `near` 分支 | 距离证书有效；采样加密；不把探索当接收保证 | 解析非空证明及证书已实现；选点器与实验待实现 | -| P4 全向闭环 | 49点覆盖记录、定位队列、清除、停止、日志统计 | 10/16源、边界源和错误响应案例；官方演练全清并正常退出 | 待实现 | -| P5 定向扩展 | no_signal联合约束、覆盖发现、有限清除后备与预算 | 朝外边界源不漏；真源不误删；格数与退出预算检查 | 已有证明和设计；闭环待实现 | -| P6 加速消融 | 共享测点、自适应覆盖、目标排序、提前清除 | 配对案例全清不退化，时间/请求数改善可复算 | 可删减；复杂学习与朝向搜索后置 | +| P3 问题二选点 | $K_1$ 先验、安全候选盘、后验半径采样与 `near` 分支 | 距离证书有效；采样加密；不把探索当接收保证 | 已实现：`belief` 保守候选集 + `planner` 保证接收盘选点器（worst-posterior 采样、1/4 间距加密复核）；策略对比实验待做 | +| P4 全向闭环 | 49点覆盖记录、定位队列、清除、停止、日志统计 | 10/16源、边界源和错误响应案例；官方演练全清并正常退出 | 自建闭环已完成：`planner.OmnidirectionalLoop`（10/16/0 源、近边界源、预算耗尽、故障注入测试通过）；官方演练全清待团队执行 | +| P5 定向扩展 | no_signal联合约束、覆盖发现、有限清除后备与预算 | 朝外边界源不漏；真源不误删；格数与退出预算检查 | 已实现：`planner.DirectionalExtension`（49点覆盖对定向源成立、保证接收盘内 no_signal 记为观测到定向、对跖+环探梯子、`fallback_clear_cells` 有限圆盘覆盖后备清除带格数/预算双硬顶）；`tests/test_loop_q4.py` 全过；官方混合源演练待执行 | +| P6 加速消融 | 共享测点、自适应覆盖、目标排序、提前清除 | 配对案例全清不退化，时间/请求数改善可复算 | 可删减；闭环基线数据已可用于消融对照 | | P7 正式与论文 | 固定版本、论文表1、日志原名、上传核查 | 问题3/4各3份正式记录与程序版本对应 | 团队后续安排；本任务不执行正式测试 | 按2026年9月10日起的剩余时间，建议10日补齐P1并衔接P3，11日完成P4自建边界案例与全向演练，12日完成P5及混合源演练；未达验收就删减P6并优先修正闭环。演练不限3次，正式机会为问题3和4各3次。团队在版本稳定后自行安排正式记录，留出失败恢复和论文整理时间。附件建议9月13日15:30前完成正式测试，17:30后不能启动新的演练或正式测试；已进入上传队列的日志仍会继续上传。此处引用所给附件安排，运行前核对模拟器界面公告；这份排期不改变当前禁止使用正式测试的限制。 @@ -210,4 +210,17 @@ AI 的适合用法是把这些模块逐一变成可测试程序、生成边界 可复现报告见 `output/q1/optimization_report.json`，问题一证明、输入输出及数值限制见 `docs/问题一解答.md`。局部MEC基准只反映本地几何计算，不能据此宣称请求延迟、完整策略或官方成绩得到同样提升。 -下一步优先补齐P1完整协议与故障回放，再实现P3选点器并接入P4“发现—定位—清除—证明完成—退出”闭环。P2已经具备可复用输入输出；不要重复开发几何内核或提前堆叠学习算法。以全向演练的实际请求、日志和耗时决定P5后备规模与P6加速投入。 +P0–P5（自建部分）与 P2 均已交付：协议层（串行幂等客户端、状态机、预算截止、 +故障回放）、几何内核、belief 保守候选集、保证接收盘选点器、`OmnidirectionalLoop` +全向闭环与 `DirectionalExtension` 混合源定向闭环（49 点覆盖对定向源同样成立、 +no_signal 联合约束基线不删候选、对跖+环探遮挡梯子、`fallback_clear_cells` +有限圆盘覆盖后备清除，格数与现实预算双硬顶下不盲扫）。自建验收： +`uv run pytest`（95 项）与 `uv run mcm26b sim-check`；随机场景自检： +`uv run mcm26b run --robot-id <队号> --self-test`（问题4加 `--problem 4`）。 + +下一步优先把两个闭环接入官方演练（问题3用全向、问题4用混合，先实测单请求 +延迟与总请求数，核对 1200s 现实预算与 2MB 日志体积），再做 P6 消融对照。 +基线量级供参照：10 源场景约 550–630 次检测、虚拟总时 1.4–1.7 万秒；16 源 +场景约 200–320 次检测即触发数量上界停止。该数字说明共享测点与跳点策略 +（P6）有明确改进空间。不要重复开发几何内核或提前堆叠学习算法；以官方演练 +的实际请求、日志和耗时决定 P6 加速投入。 diff --git a/docs/协议规则表与状态机.md b/docs/协议规则表与状态机.md index 68cbbb2..b278148 100644 --- a/docs/协议规则表与状态机.md +++ b/docs/协议规则表与状态机.md @@ -121,3 +121,74 @@ remaining_real_duration_s 为准，不得假定恒为 1200）；测试窗口 25 进程崩溃不丢已发记录，回放测试与 evaluation 复算共用同一格式； - **本地预校验**：channel 范围、坐标有限性与 2e6 上限、robot_id/request_id 格式 在发送前拒绝（与服务器 400 语义一致，省一次往返）。 + +## 6. 策略层如何使用本协议（P3/P4 交付） + +第 5 节的客户端是策略层唯一的事实来源；`belief`（候选集）、`planner`（选点与闭环） +不绕开它直接发请求。衔接约定： + +- **串行不变**：闭环每个决策步只发一个动作，`measure`/`clear` 的返回值 + （`MeasureOutcome`/`ClearOutcome`）是 belief 更新的唯一输入； +- **状态同步**：机器人位置/测向频道/虚拟时刻一律读 `client.state`， + 频道规则（§3 状态机表）由状态机自动维护，策略层不自行推算； +- **预算驱动退出**：`DeadlineExceeded` 只在 `/exit` 之外的动作前抛出； + 闭环捕获后立即 `/exit`（豁免检查），并把该局记为"未证明完成"， + 不与全清成功混记； +- **选点的接收保证**：选点器只在保证接收盘 B(mec, 1000-ρ) 内取点， + 盘内任一点到候选集每点距离 < 1000 ≤ 最小接收半径，因此定位阶段 + 不会浪费 `no_signal`（ρ>1000 的一般候选集退化为探索 MEC 圆心，允许 + no_signal，理由字段中如实标注）； +- **清除证书**：候选集外包多边形半径（含近似误差与工程余量） + ≤ 20m 才发起 `/clear`；证书有效但清除失败视为异常，先补测再重试， + 不在同一证书上重复消耗 3 秒/次的失败清除； +- **停止条件**（全部显式实现）：清除数达 16（源数上界证明）、 + 每频道均"已清除或覆盖证明不存在"、现实预算耗尽。发现若干源、 + 连续 no_signal、候选采样为空都不是停止条件； +- **验收**：`tests/test_loop_q3.py` 在自建模拟器上覆盖 10/16/0 源、 + 近边界源、预算耗尽与故障注入；`uv run mcm26b run --robot-id <队号> + --self-test` 可离线复跑随机场景，正式模式去掉 `--self-test` 即对接 + 模拟器（日志用 `--log` 落盘，格式同 §5 JSONL）。 + +## 7. 定向扩展如何使用本协议（P5 交付） + +问题4的定向源在**协议字段上与全向源完全不可区分**：`/measure` 仍只返回 +no_signal/near/direction（direction 附 svd_deg），`/clear` 仍只看 20m 半径。 +"该源是定向的"无法从报文读出，只能由观测推理。因此 §5/§6 的协议用法全部 +照用，定向性只改变策略层的解释与决策： + +- **发现阶段零改动**：49 点覆盖保证对定向源同样成立——源位于某网格单元 + 凸包内时，任意发射半平面（朝向 ±90°，含边界）必含至少一个顶点，且该 + 顶点距离 ≤ 700√2 ≈ 989.95 < 1000 ≤ 最小接收半径，覆盖阶段无需知道 + 源类型； +- **no_signal 联合约束基线**：定向源的 no_signal 约束的是"位置×半径×朝向" + 联合假设，单独维护空间候选会被误删，基线**不据此删任何候选** + （`belief.update_measure` 对 no_signal 无操作）；缺席证明仍只由完整覆盖 + 给出。唯一启用的推理：若 no_signal 发生在保证接收盘 B(mec, 1000-ρ) 内 + （该盘内任一全向源必然可收），则只能由朝向遮挡解释 [观测推理]——该频道 + 记为"观测到定向"，进入探点升级梯子：对 MEC 圆心对跖复核一次，再绕 + MEC、半径 = ρ 的圆环按 45° 步进探点（环包围任意候选源，发射半平面在 + 环上截出 ≥180° 弧）。被遮挡探点不消耗定位步数，单轮由 + `max_blocked_probes`（4 [工程]）限流；梯子不构成接收保证，失败由后备 + 清除兜底； +- **后备清除的协议用法**：定位停滞（候选集有界但未达 §6 清除证书）时， + 对候选外包多边形构造 20m 有限圆盘覆盖（`fallback_clear_cells`：全局 + 半开网格索引、相交判定带正容差，格内任一点距某格心 ≤ 10√2 < 20m）， + 按"距机器人最近优先"逐格发 `/clear`（同频道、不同位置）——清除与朝向 + 无关 [题面]，是定向源不依赖可见性的完备后备。两条预算硬门槛（任一 + 不满足即跳过并留痕，绝不盲扫）： + - 格数上界：N ≤ `fallback_max_cells`（400 [工程]）；外包框枚举先按 + (⌈w/20⌉+2)(⌈h/20⌉+2) 做规模检查，宽候选集（如仅一次读数的 750m + 截断角域，数千格）必须继续定位而非硬扫； + - 现实预算：N × `real_rtt_p95_s` + `reserve_exit_s` ≤ 剩余现实时间。 + 虚拟耗时上界 ≈ 移动 L/5 + 3N（全失败）或 3(N-1)+5（末次成功）； +- **真源不误删**：no_signal、失败的 `/clear`（含后备逐格全失败）均不删 + 空间候选；后备全失败按守恒性破坏记异常（格心距格内任一点 < 20m， + 理论上必命中）； +- **停止条件与预算语义不变**：仍是清除数达 16、每频道均"已清除或覆盖 + 证明不存在"、现实预算耗尽（记未证明完成）三种； +- **验收**：`tests/test_loop_q4.py`（15 项：覆盖格无空洞与格线两侧保留、 + 退化多边形、格数上界公式；混合 10/16 源全清、朝向背离圆心的近边界源 + 不漏、批量种子回归；no_signal 不删候选；格数硬顶与预算跳过；后备 + 正路径命中与全失败异常留痕）；`uv run mcm26b run --robot-id <队号> + --problem 4 --self-test` 可离线复跑混合场景（`--directional` 指定 + 定向源数），正式模式同 §6。 diff --git a/src/mcm26b/belief/__init__.py b/src/mcm26b/belief/__init__.py index 552f655..6ce25c6 100644 --- a/src/mcm26b/belief/__init__.py +++ b/src/mcm26b/belief/__init__.py @@ -1,24 +1,38 @@ """P3 逐频道信念状态：候选集合 K_c 维护与频道处理状态。 -现状：骨架（P3 起填充）。接口与不变式先行固定，防止后续实现漂移。 - 核心不变式（验收标准）： -- 保守性：真源永远属于维护中的候选集合；任何更新不得因误差、 - 圆弧离散或浮点容差把真点排除（自建真值案例中真源始终保留）； -- direction 读数：K_c ⊆ Ω ∩ W(s, θ±δ) ∩ B(s, 1500)（首次测向构造见 - geometry.planning.first_bearing_candidate_disk 的解析证书）； -- no_signal 读数（P4，全向源）：仅可排除 B(s, 1000)；用于定向源时 - 是"位置×朝向"联合约束，单独维护空间候选会误删真源（P5 才启用）； +- 保守性：真源永远属于维护中的候选集合。候选集用"圆盘外切多边形 ∩ + 测向角域半平面"的凸多边形表示——外切多边形严格包含圆盘，半平面裁剪 + 带正容差，因此每步更新只会放大而不会缩小真实候选集； +- direction 读数：K_c ⊆ Ω ∩ W(s, θ±δ_eff) ∩ B(s, 1500)，后续读数再交 + 同样的约束；δ_eff = 误差半角 + 示向度舍入裕量（服务器保留两位小数， + 舍入本身引入 ≤0.005° 偏差）； +- no_signal 读数（P4，全向源）：基线不据此删空间候选——对定向源它是 + "位置×朝向"联合约束，单独维护空间候选会误删真源（P5 才启用）； + "该频道不存在源"的证明由覆盖账本（coverage.CoverageLedger）给出； +- near 读数：源在 5m 内，候选集收缩为 B(s, 5)，可直接产生清除证书； - /clear 成功：状态转 CLEARED；失败只说明"该频道 20m 内无可清除源"， - 不得直接删除超出 20m 的候选。 + 不得直接删除超出 20m 的候选，必须先补测再重试。 """ from __future__ import annotations +import math from dataclasses import dataclass, field from enum import Enum -from ..config import ProblemConfig +from ..config import ProblemConfig, Vec2 +from ..geometry.localization import smallest_enclosing_circle +from ..protocol.rules import DIRECTION, NEAR, NO_SIGNAL + +# 半平面裁剪的正容差（米）。坐标量级 ~2100，浮点误差远小于它； +# 容差只向"保留更多候选"的方向倾斜，不破坏保守性。 +_CLIP_TOL_M = 1e-6 +_DEDUPE_TOL_M = 1e-7 + + +class InconsistentBelief(RuntimeError): + """读数把候选集剪空等违背保守性的情形；须停机核查而非继续跑。""" class ChannelStatus(Enum): @@ -29,11 +43,185 @@ class ChannelStatus(Enum): ABSENT = "absent" # 覆盖证明不存在（P4） +def circumscribed_polygon(center: Vec2, radius_m: float, sides: int) -> list[Vec2]: + """圆的外切正多边形顶点（外接半径 r/cos(π/n)），严格包含圆盘。 + + 近似误差（外接圆超出圆盘的部分）为 r(1/cos(π/n) - 1) ≤ r(1-cos(π/n))·sec²， + 调用方用 (1-cos(π/n)) 口径记账即可（略保守偏大）。 + """ + if sides < 8: + raise ValueError("外切多边形边数过少，近似误差不可控") + circumradius = radius_m / math.cos(math.pi / sides) + step = 2 * math.pi / sides + return [(center[0] + circumradius * math.cos(step * i + step / 2), + center[1] + circumradius * math.sin(step * i + step / 2)) + for i in range(sides)] + + +def polygon_halfplanes(polygon: list[Vec2]) -> list[tuple[float, float, float]]: + """CCW 多边形 -> 外法线半平面列表 (a, b, c)，含义 a·x + b·y <= c。""" + planes = [] + count = len(polygon) + for index in range(count): + p, q = polygon[index], polygon[(index + 1) % count] + dx, dy = q[0] - p[0], q[1] - p[1] + norm = math.hypot(dx, dy) + if norm <= 1e-12: + continue + a, b = dy / norm, -dx / norm # CCW 的右法线朝外 + planes.append((a, b, a * p[0] + b * p[1])) + return planes + + +def clip_halfplane(polygon: list[Vec2], a: float, b: float, c: float, + tol: float = _CLIP_TOL_M) -> list[Vec2]: + """Sutherland–Hodgman 凸多边形半平面裁剪，保留 a·x + b·y <= c + tol。 + + 边界交点取在无容差直线上；进入判定带正容差，保证"贴边"的真点不丢。 + """ + if not polygon: + return [] + output: list[Vec2] = [] + count = len(polygon) + for index in range(count): + p, q = polygon[index], polygon[(index + 1) % count] + fp = a * p[0] + b * p[1] - c + fq = a * q[0] + b * q[1] - c + p_in, q_in = fp <= tol, fq <= tol + if p_in: + output.append(p) + if p_in != q_in: + ratio = fp / (fp - fq) + output.append((p[0] + ratio * (q[0] - p[0]), + p[1] + ratio * (q[1] - p[1]))) + return _dedupe(output) + + +def _dedupe(points: list[Vec2], tol: float = _DEDUPE_TOL_M) -> list[Vec2]: + unique: list[Vec2] = [] + for point in points: + if not unique or math.dist(point, unique[-1]) > tol: + unique.append(point) + if len(unique) > 1 and math.dist(unique[0], unique[-1]) <= tol: + unique.pop() + return unique + + +class CandidateRegion: + """某频道候选集合的保守凸外包多边形（顶点按 CCW；允许退化为 1-2 点）。""" + + def __init__(self, polygon: list[Vec2], slack_m: float = 0.0): + self.polygon = list(polygon) + self.slack_m = float(slack_m) # 外包近似引入的半径向误差记账 + self._mec: tuple[Vec2, float] | None = None + + # ------------------------------------------------------------ 构造 + @classmethod + def disk(cls, center: Vec2, radius_m: float, sides: int, + slack_m: float | None = None) -> "CandidateRegion": + if slack_m is None: + slack_m = radius_m * (1 - math.cos(math.pi / sides)) + return cls(circumscribed_polygon(center, radius_m, sides), slack_m) + + @classmethod + def initial(cls, station: Vec2, config: ProblemConfig) -> "CandidateRegion": + """首个 direction 后的基础集：Ω ∩ B(station, r_max) 的保守外包。""" + field, sides = config.field, config.search.polygon_sides + slack = field.omega_radius_m * (1 - math.cos(math.pi / sides)) + region = cls.disk((0.0, 0.0), field.omega_radius_m, sides, slack) + region.clip_disk(station, field.r_eff_max_m, sides) + return region + + # ------------------------------------------------------------ 收缩 + def _clip(self, a: float, b: float, c: float) -> None: + polygon = clip_halfplane(self.polygon, a, b, c) + if not polygon: + raise InconsistentBelief( + f"候选区域被半平面 ({a:.6f},{b:.6f},{c:.6f}) 剪空：读数与保守集矛盾") + self.polygon = polygon + self._mec = None + + def clip_disk(self, center: Vec2, radius_m: float, sides: int) -> None: + """与圆盘 B(center, radius) 的外切多边形求交（近似误差入账）。""" + for a, b, c in polygon_halfplanes(circumscribed_polygon(center, radius_m, sides)): + self._clip(a, b, c) + self.slack_m += radius_m * (1 - math.cos(math.pi / sides)) + + def clip_wedge(self, station: Vec2, bearing_deg: float, + half_angle_deg: float) -> None: + """与测向角域 W(station, bearing±half_angle) 求交（半角须 <90°）。 + + 两个边界半平面与 localization.halfplanes 同口径，0°/360° 跨界由 + sin/cos 直接处理，无需分支。 + """ + if not 0 < half_angle_deg < 90: + raise ValueError("角域半角须在 (0, 90) 度") + lower = math.radians(bearing_deg - half_angle_deg) + upper = math.radians(bearing_deg + half_angle_deg) + sx, sy = float(station[0]), float(station[1]) + self._clip(math.sin(lower), -math.cos(lower), + math.sin(lower) * sx - math.cos(lower) * sy) + self._clip(-math.sin(upper), math.cos(upper), + -math.sin(upper) * sx + math.cos(upper) * sy) + + # ------------------------------------------------------------ 查询 + def mec(self) -> tuple[Vec2, float]: + """最小包围圆（圆心, 半径）。半径是真实候选集 ρ 的上界。""" + if self._mec is None: + if not self.polygon: + raise InconsistentBelief("候选区域为空") + self._mec = smallest_enclosing_circle(self.polygon) + return self._mec + + def witnesses(self, spacing_m: float) -> list[Vec2]: + """顶点 + 边界按间距细分的采样点（最坏后验评估用）。""" + if len(self.polygon) < 3: + return list(self.polygon) + points = list(self.polygon) + count = len(self.polygon) + for index in range(count): + p, q = self.polygon[index], self.polygon[(index + 1) % count] + length = math.dist(p, q) + steps = int(length // spacing_m) + for k in range(1, steps + 1): + ratio = k / (steps + 1) + points.append((p[0] + ratio * (q[0] - p[0]), + p[1] + ratio * (q[1] - p[1]))) + return _dedupe(points, tol=1e-9) + + def contains(self, point: Vec2, tol_m: float = 0.05) -> bool: + """点是否在候选集内（带容差；用于真源保留性检验，不做删点依据）。""" + if not self.polygon: + return False + if len(self.polygon) == 1: + return math.dist(point, self.polygon[0]) <= tol_m + self.slack_m + if len(self.polygon) == 2: + p, q = self.polygon + return _point_segment_distance(point, p, q) <= tol_m + self.slack_m + for index in range(len(self.polygon)): + p = self.polygon[index] + q = self.polygon[(index + 1) % len(self.polygon)] + if (q[0] - p[0]) * (point[1] - p[1]) - (q[1] - p[1]) * (point[0] - p[0]) \ + < -tol_m - self.slack_m: + return False + return True + + +def _point_segment_distance(point: Vec2, p: Vec2, q: Vec2) -> float: + vx, vy = q[0] - p[0], q[1] - p[1] + length_sq = vx * vx + vy * vy + if length_sq <= 1e-18: + return math.dist(point, p) + t = max(0.0, min(1.0, ((point[0] - p[0]) * vx + (point[1] - p[1]) * vy) / length_sq)) + return math.dist(point, (p[0] + t * vx, p[1] + t * vy)) + + @dataclass class ChannelBelief: channel: int status: ChannelStatus = ChannelStatus.UNKNOWN - measurements: list = field(default_factory=list) # (位置, 结果, svd_deg) 记录 + region: CandidateRegion | None = None + measurements: list = field(default_factory=list) # 逐步观测记录（审计用） class BeliefState: @@ -45,14 +233,53 @@ class BeliefState: for channel in range(config.field.channel_min, config.field.channel_max + 1)} - def update_measure(self, channel: int, position, result: str, - svd_deg: float | None) -> None: - """按读数收敛候选集合。未实现：需要几何模块的在线圆盘角域交。""" - raise NotImplementedError("P3：接入 geometry 的圆盘约束角域交后实现") + @property + def effective_half_angle_deg(self) -> float: + """示向度误差半角 + 舍入裕量：服务器保留两位小数引入 ≤0.005° 偏差。""" + return (self.config.field.bearing_error_half_deg + + self.config.search.bearing_rounding_margin_deg) - def candidate_set(self, channel: int): - """返回该频道当前候选集合（保守外包形式）。未实现。""" - raise NotImplementedError("P3") + def update_measure(self, channel: int, position: Vec2, result: str, + svd_deg: float | None) -> None: + """按读数收敛候选集合；真源保留性由保守表示保证。""" + belief = self.channels[channel] + belief.measurements.append({"position": tuple(position), "result": result, + "svd_deg": svd_deg}) + if result == DIRECTION: + if svd_deg is None: + raise ValueError("direction 读数必须携带 svd_deg") + sides = self.config.search.polygon_sides + if belief.region is None: + belief.region = CandidateRegion.initial(position, self.config) + else: + # 后续 direction 同样隐含距离 <= r_max [题面] + belief.region.clip_disk(position, self.config.field.r_eff_max_m, sides) + belief.region.clip_wedge(position, svd_deg, self.effective_half_angle_deg) + belief.status = ChannelStatus.DETECTED + elif result == NEAR: + sides = self.config.search.polygon_sides + if belief.region is None: + belief.region = CandidateRegion.disk( + tuple(position), self.config.field.near_radius_m, sides) + else: + belief.region.clip_disk(position, self.config.field.near_radius_m, sides) + belief.status = ChannelStatus.DETECTED + elif result == NO_SIGNAL: + # P3/P4 基线：不据此删空间候选（缺席证明由覆盖账本给出）。 + pass + else: + raise ValueError(f"未知测向结果: {result!r}") + + def candidate_set(self, channel: int) -> CandidateRegion | None: + """该频道当前候选集合（保守外包形式）；尚无 direction/near 时为 None。""" + return self.channels[channel].region def mark_cleared(self, channel: int) -> None: self.channels[channel].status = ChannelStatus.CLEARED + + def mark_absent(self, channel: int) -> None: + self.channels[channel].status = ChannelStatus.ABSENT + + def channels_by_status(self, *statuses: ChannelStatus) -> list[int]: + return sorted(channel for channel, belief in self.channels.items() + if belief.status in statuses) diff --git a/src/mcm26b/cli.py b/src/mcm26b/cli.py index a4b6a80..3caf11a 100644 --- a/src/mcm26b/cli.py +++ b/src/mcm26b/cli.py @@ -2,7 +2,8 @@ - q1 问题一角域交会求解（P2 已完工） - sim-check 用自建模拟器复现附件计时示例（P0 自检） -- run 全向闭环（P4 骨架，未实现） +- run 闭环：发现—定位—清除—退出（--problem 3 全向 / 4 混合源； + --self-test 走本地模拟器） """ from __future__ import annotations @@ -10,12 +11,17 @@ from __future__ import annotations import argparse import json import sys +from dataclasses import replace +from pathlib import Path from .config import ProblemConfig from .geometry.__main__ import main as q1_main +from .planner import DirectionalExtension, OmnidirectionalLoop from .protocol.client import SimulatorClient +from .protocol.log import SessionLog from .protocol.rules import EXIT_USER from .sim import LocalSimTransport, LocalSimulator, Source +from .sim.scenes import mixed_scene, random_omni_scene def _sim_check(args) -> int: @@ -40,8 +46,36 @@ def _sim_check(args) -> int: def _run(args) -> int: - raise NotImplementedError( - "P4 全向闭环尚未实现；当前可用子命令：q1（问题一求解）、sim-check（协议自检）") + """问题3/4闭环。--self-test 时真值只存在于测试端入口，策略端仅见响应。""" + config = ProblemConfig() + session = replace(config.session, + base_url=args.base_url or config.session.base_url) + config = replace(config, + session=session, + search=replace(config.search, + bearing_rounding_margin_deg=args.bearing_margin_deg)) + truth = None + transport = None + if args.self_test: + if args.problem == 4: + sources = mixed_scene(seed=args.seed, count=args.sources, + directional=args.directional) + else: + sources = random_omni_scene(seed=args.seed, count=args.sources) + truth = sorted(source.channel for source in sources) + transport = LocalSimTransport(LocalSimulator(sources, robot_id=args.robot_id, + config=config)) + log = SessionLog(path=Path(args.log)) if args.log else SessionLog() + client = SimulatorClient(args.robot_id, config=config, transport=transport, log=log) + loop_cls = DirectionalExtension if args.problem == 4 else OmnidirectionalLoop + summary = loop_cls(client, config).run() + summary["robot_id"] = args.robot_id + if truth is not None: + # 仅测试端核对：清除集合是否与真值一致。 + summary["truth"] = {"source_channels": truth, + "all_cleared": sorted(summary["cleared_channels"]) == truth} + print(json.dumps(summary, ensure_ascii=False, indent=2)) + return 0 if summary["completed"] and (truth is None or summary["truth"]["all_cleared"]) else 1 def main(argv=None) -> int: @@ -62,8 +96,21 @@ def main(argv=None) -> int: check.add_argument("--robot-id", default="2026001") check.set_defaults(func=_sim_check) - run = subparsers.add_parser("run", help="P4 全向闭环（骨架）") - run.add_argument("--robot-id", required=True) + run = subparsers.add_parser("run", help="闭环：发现—定位—清除—退出（问题3/4）") + run.add_argument("--robot-id", required=True, help="登录参赛队号，逐字节一致") + run.add_argument("--base-url", default=None, + help="模拟器接口地址（默认取配置 http://127.0.0.1:2026）") + run.add_argument("--log", default=None, help="会话日志 JSONL 输出路径") + run.add_argument("--problem", type=int, choices=(3, 4), default=3, + help="问题3=全向源，问题4=混合定向源（默认 3）") + run.add_argument("--bearing-margin-deg", type=float, default=0.005, + help="示向度舍入裕量（服务器保留两位小数；默认 0.005°）") + run.add_argument("--self-test", action="store_true", + help="走本地模拟器与随机场景（测试端自检，不连官方）") + run.add_argument("--seed", type=int, default=20260910, help="自检场景种子") + run.add_argument("--sources", type=int, default=10, help="自检场景源数量") + run.add_argument("--directional", type=int, default=3, + help="问题4自检场景中定向源数量（--problem 4 生效）") run.set_defaults(func=_run) args = parser.parse_args(argv) diff --git a/src/mcm26b/config.py b/src/mcm26b/config.py index 70eb864..3743a85 100644 --- a/src/mcm26b/config.py +++ b/src/mcm26b/config.py @@ -73,6 +73,17 @@ class SearchConfig: grid_half_cells: int = 3 # i,j ∈ [-3,3]，49 点覆盖 [-2100,2100]^2 ⊇ Ω fallback_clear_grid_m: float = 20.0 # P5 后备清除网格；10*sqrt(2)<=20 bearing_rounding_margin_deg: float = 0.0 # 默认不加舍入裕量；敏感性分析时显式设 0.005 + # ---- P3/P4 在线定位与闭环参数 [工程] ---- + polygon_sides: int = 192 # 圆盘外切多边形边数；Ω 近似误差 1800(1-cos(π/192))≈0.24m + witness_spacing_m: float = 60.0 # 最坏后验评估的边界采样间距；选定后再按 1/4 加密复核 + candidate_rings: tuple = (0.0, 0.4, 0.75, 1.0) # 安全接收盘内候选点的半径比例 + candidate_angles: int = 12 # 每环候选方向数 + clear_margin_m: float = 0.5 # 清除证书的额外工程余量（多边形近似误差之外） + max_localize_steps: int = 8 # 单频道定位阶段允许的追加测量上限 + max_anomalies_per_channel: int = 3 # 单频道异常记录上限，防止异常刷屏 + # ---- P5 定向扩展参数 [工程] ---- + max_blocked_probes: int = 4 # 定位单轮内被朝向遮挡的探点预算（对跖复核+环探） + fallback_max_cells: int = 400 # 后备清除格数硬顶；超过则不盲扫（宽候选集继续定位） @dataclass(frozen=True) diff --git a/src/mcm26b/planner/__init__.py b/src/mcm26b/planner/__init__.py index 29cb3d9..eae31b6 100644 --- a/src/mcm26b/planner/__init__.py +++ b/src/mcm26b/planner/__init__.py @@ -1,80 +1,539 @@ """P3/P4/P5 策略层：主动选点、全向闭环与定向扩展。 -现状：骨架。各入口先固定签名与验收标准，逐阶段填充实现。 - P3 第二检测点（验收：保证收到信号；Q 空时退化；near 分支）： - K1 = Ω ∩ W(s1, θ1±δ) ∩ B(s1, 1500)；安全候选盘证书见 geometry.planning.first_bearing_candidate_disk（δ<=1.005° 时非空）； -- 在证书圆盘内围绕测向轴两侧采样候选点，以"最坏后验包围圆半径" - 为主指标、移动+检测代价为次指标； +- 在保证接收盘 B(mec_center, 1000-ρ) 内环形采样候选点，以 + "最坏后验包围圆半径"为主指标、移动代价为次指标； +- 采样是启发式（目标边界点 × 读数区间端点），不是严格最坏情形 + 最优解；选定后按 1/4 间距加密复核并如实上报数值； - 返回 near 时：候选源在 5m 内，直接对当前频道尝试 /clear。 P4 全向闭环（验收：10/16 源正确退出；边界朝外不漏；停止条件正确）： 发现（coverage.CoverageLedger）→ 定位（belief 收敛）→ 清除 → 停止判定 （清除数达 16，或每频道均有清除成功/不存在证明）→ /exit。 -发现 10 源、连续无信号、候选采样为空都不是停止条件。 +发现 10 源、连续无信号、候选采样为空都不是停止条件； +现实预算耗尽退出记为未证明完成（completed=false），不与成功混记。 -P5 定向扩展（验收：朝外边界源能发现；失去信号能清除；不误删真源）： -no_signal 是"位置×朝向"联合约束，基线不据此删空间候选； -失去信号后对有界候选做 <=20m 有限圆盘覆盖逐点 /clear， -并按预算上界（格数×请求耗时）决定是否执行。 +P5 定向扩展（问题4混合源闭环）： +- 49 点覆盖的发现保证对定向源同样成立（源在单元凸包内，任意发射半平面 + 含至少一个顶点），覆盖阶段不区分源类型； +- 保证接收盘内的 no_signal 只能由朝向遮挡解释 [观测推理]：频道记为 + "观测到定向"，对跖探点复核一次；联合约束基线不删空间候选（真源不误删）； +- 定位停滞时对有界候选集构造 <=20m 有限圆盘覆盖（fallback_clear_cells）， + 预算核对后逐格 /clear（清除与朝向无关）；格数超硬顶或预算不足即跳过， + 绝不盲扫。 """ from __future__ import annotations +import math from dataclasses import dataclass -from ..config import ProblemConfig -from ..protocol.client import SimulatorClient +from ..belief import (BeliefState, CandidateRegion, ChannelStatus, + clip_halfplane) +from ..config import ProblemConfig, Vec2 +from ..coverage import CoverageLedger +from ..evaluation import recompute_virtual_time, summarize +from ..protocol.client import DeadlineExceeded, SimulatorClient +from ..protocol.rules import CLEAR_SUCCESS, DIRECTION, NEAR, NO_SIGNAL +from ..protocol.timing import move_time -@dataclass +def bearing_deg(a: Vec2, b: Vec2) -> float: + """从 a 指向 b 的方位角（0-360，0 为 x 正方向，逆时针）。""" + return math.degrees(math.atan2(b[1] - a[1], b[0] - a[0])) % 360.0 + + +@dataclass(frozen=True) class SecondPointPlan: - """第二检测点的候选与依据（P3 输出形态预览）。""" + """下一检测点的候选与依据（P3 输出）。""" - position: tuple[float, float] + position: Vec2 worst_posterior_radius_m: float move_cost_s: float reason: str -def select_second_point(client: SimulatorClient, - first_position, first_bearing_deg: float, - config: ProblemConfig = ProblemConfig()): - """在安全候选盘内选择第二检测点。未实现：P3。""" - raise NotImplementedError("P3：先实现 belief 候选集，再实现采样评估") +@dataclass(frozen=True) +class ClearCertificate: + """清除证书：候选集外包半径（含近似误差）不超过 20m 阈值。""" + + channel: int + center: Vec2 + radius_m: float + limit_m: float + certified: bool + + +def clear_certificate(region: CandidateRegion, channel: int, + config: ProblemConfig = ProblemConfig()) -> ClearCertificate: + """ρ_poly <= 20 - 外包近似误差 - 工程余量 才允许发起清除。""" + center, radius = region.mec() + limit = (config.field.clear_radius_m - region.slack_m + - config.search.clear_margin_m) + return ClearCertificate(channel, center, radius, limit, radius <= limit) + + +def candidate_points(center: Vec2, safe_radius_m: float, rings, angles: int) -> list[Vec2]: + """保证接收盘内的候选点：盘心 + 各比例半径 × 均匀方向。""" + points = [tuple(center)] + for fraction in rings: + radius = fraction * safe_radius_m + if radius <= 1e-9: + continue + step = 2 * math.pi / angles + for k in range(angles): + angle = step * k + step / 2 + points.append((center[0] + radius * math.cos(angle), + center[1] + radius * math.sin(angle))) + return points + + +def _worst_posterior_radius(region: CandidateRegion, q: Vec2, + witnesses: list[Vec2], half_angle_deg: float) -> float: + """对候选点 q，遍历"目标边界点 × 读数区间端点"的最坏后验包围半径。 + + 读数 θ' 与目标 g 一致当且仅当 |θ' - bearing(q→g)| <= δ，因此端点 + φ(g)±δ 是每个 g 的最不利读数。这是启发式近似，不构成最优性证明 [工程]。 + """ + posterior_radii = [] + for g in witnesses: + phi = bearing_deg(q, g) + for reading in (phi - half_angle_deg, phi + half_angle_deg): + probe = CandidateRegion(list(region.polygon), region.slack_m) + probe.clip_wedge(q, reading, half_angle_deg) + if probe.polygon: + posterior_radii.append(probe.mec()[1]) + if not posterior_radii: + return region.mec()[1] # 无有效采样点时退回当前半径 + return max(posterior_radii) + + +def safe_disk_radius(region: CandidateRegion, + config: ProblemConfig = ProblemConfig()) -> float: + """保证接收盘 B(mec, safe) 的半径；<0 表示无保证（ρ 超过最小接收半径）。 + + 盘内任意点到候选集每点距离 <= safe + ρ + 1e-3 < 1000 <= 最小接收半径， + 故必然收到 direction/near。1mm 数值余量避免浮点贴边被 r_eff=1000 的源 + 判为 no_signal。 + """ + return config.field.r_eff_min_m - region.mec()[1] - 1e-3 + + +def select_measurement_point(region: CandidateRegion, from_position: Vec2, + config: ProblemConfig = ProblemConfig()) -> SecondPointPlan: + """在保证接收盘内选择下一检测点（P3 核心；对任意候选集通用）。 + + 保证接收盘 B(mec_center, 1000-ρ)：盘内任意点到候选集每点距离 + <= (1000-ρ) + ρ = 1000 <= 最小接收半径，故必然收到 direction/near， + 不会浪费一次 no_signal。ρ>1000 时退化为探索 MEC 圆心（可能 no_signal）。 + """ + field, search = config.field, config.search + center, radius = region.mec() + half_angle = field.bearing_error_half_deg + search.bearing_rounding_margin_deg + + def move_cost(point: Vec2) -> float: + return move_time(from_position, point, config.cost) + + # 1mm 数值余量在 safe_disk_radius 内记账。 + safe_radius = safe_disk_radius(region, config) + if safe_radius < 0: + return SecondPointPlan(tuple(center), radius, move_cost(center), + "无保证接收盘（ρ>1000）：探索 MEC 圆心，可能 no_signal") + coarse = region.witnesses(search.witness_spacing_m) + samples = candidate_points(center, safe_radius, + search.candidate_rings, search.candidate_angles) + best_key, best_point, best_worst = None, None, None + for point in samples: + worst = _worst_posterior_radius(region, point, coarse, half_angle) + key = (round(worst, 6), round(move_cost(point), 6)) + if best_key is None or key < best_key: + best_key, best_point, best_worst = key, point, worst + dense = _worst_posterior_radius(region, best_point, + region.witnesses(search.witness_spacing_m / 4), + half_angle) + reason = (f"安全盘 B(centre,{safe_radius:.0f}m) 内 {len(samples)} 候选；" + f"粗采样最坏后验 {best_worst:.1f}m，1/4 间距加密复核 {dense:.1f}m") + return SecondPointPlan(tuple(best_point), dense, move_cost(best_point), reason) + + +def select_second_point(client: SimulatorClient, first_position: Vec2, + first_bearing_deg: float, + config: ProblemConfig = ProblemConfig()) -> SecondPointPlan: + """P3 入口：由首次 direction 构造 K1，返回保证接收的第二检测点方案。""" + region = CandidateRegion.initial(first_position, config) + half_angle = config.field.bearing_error_half_deg \ + + config.search.bearing_rounding_margin_deg + region.clip_wedge(first_position, first_bearing_deg, half_angle) + return select_measurement_point(region, client.state.position, config) class OmnidirectionalLoop: - """问题3全向"发现—定位—清除—停止"闭环。未实现：P4。""" + """问题3全向"发现—定位—清除—停止"闭环（P4 基线）。 - def __init__(self, client: SimulatorClient, - config: ProblemConfig = ProblemConfig()): - self.client = client - self.config = config - - def run(self) -> dict: - raise NotImplementedError("P4：依赖 coverage + belief + planner 就位") - - -class DirectionalExtension: - """问题4定向源扩展：朝向不确定下的覆盖与后备清除。未实现：P5。""" - - def __init__(self, client: SimulatorClient, - config: ProblemConfig = ProblemConfig()): - self.client = client - self.config = config - - def run(self) -> dict: - raise NotImplementedError("P5：依赖 P4 闭环稳定后扩展") - - -def fallback_clear_cells(center, width_m: float, height_m: float, - config: ProblemConfig = ProblemConfig()): - """失去信号后的后备清除格心列表（<=20m 圆盘覆盖）。 - - 半开区间统一索引，源在格线上时至少归入一格；相交判定数值 - 不确定时保留格子（宁可多测）。格数即 /clear 请求预算上界。 - 未实现：P5（先由演练实测请求预算决定启用规模）。 + 串行驱动 SimulatorClient：覆盖阶段在 49 点网格上对每个 UNKNOWN 频道 + 检测；发现 direction 即进入该频道的定位子循环（选点→测量→收敛→ + 证书清除），near 直接清除。停止条件只有三种：清除数达 16（数量上界 + 证明）、每频道均 CLEARED/ABSENT、现实预算耗尽（记为未完成）。 """ - raise NotImplementedError("P5") + + def __init__(self, client: SimulatorClient, + config: ProblemConfig = ProblemConfig()): + self.client = client + self.config = config + self.belief = BeliefState(config) + self.ledger = CoverageLedger(config) + self.cleared: list[int] = [] + self.anomalies: list[dict] = [] + self.stop_reason: str | None = None + # 证书有效但 /clear 失败后必须先补测再重试，避免在同一证书上 + # 反复消耗 3 秒/次的失败清除。 + self._may_clear: set[int] = set() + + # ------------------------------------------------------------ 主流程 + def run(self) -> dict: + self.client.enter() + try: + self._coverage_phase() + if self.stop_reason is None: + self._mark_absent_after_coverage() + self.stop_reason = self._resolve_detected() + except DeadlineExceeded: + self.stop_reason = "real_budget_exhausted" + info = self.client.exit() + return self._summary(info) + + # ------------------------------------------------------------ 覆盖阶段 + def _pending_channels_at(self, index: int) -> list[int]: + return [channel for channel, belief in self.belief.channels.items() + if belief.status is ChannelStatus.UNKNOWN + and not self.ledger.measured_at(channel, index)] + + def _next_point_index(self) -> int | None: + """尚有 UNKNOWN 频道待测的最近网格点（贪心最近邻 [工程]）。""" + position = self.client.state.position + best_index, best_distance = None, 0.0 + for index, point in enumerate(self.ledger.points): + if not self._pending_channels_at(index): + continue + distance = math.dist(position, point) + if best_index is None or distance < best_distance: + best_index, best_distance = index, distance + return best_index + + def _coverage_phase(self) -> None: + while True: + if self._count_limit_reached(): + return + index = self._next_point_index() + if index is None: + return + point = self.ledger.points[index] + for channel in self._pending_channels_at(index): + outcome = self.client.measure(point, channel) + self.ledger.mark_measured(channel, index) + self.belief.update_measure(channel, point, outcome.result, + outcome.svd_deg) + if outcome.result == DIRECTION: + self._localize_and_clear(channel) + elif outcome.result == NEAR: + region = self.belief.candidate_set(channel) + cert = clear_certificate(region, channel, self.config) + if cert.certified: + self._attempt_clear(channel, cert.center, cert) + else: # 理论上不发生（near 候选半径 5m）；保守记录 + self._anomaly("near_without_certificate", channel, + position=point) + if self._count_limit_reached(): + return + + def _count_limit_reached(self) -> bool: + if len(self.cleared) >= self.config.field.source_count_max: + self.stop_reason = "sixteen_cleared" + return True + return False + + # ------------------------------------------------------------ 定位与清除 + def _localize_and_clear(self, channel: int) -> bool: + """定位子循环：选点测量收敛候选集，拿到清除证书后清除。 + + 返回是否已清除；步数耗尽仍未收敛则保持 DETECTED 留待二轮。 + 进入本函数前该频道刚有一次 direction 读数，允许立即清除。 + """ + search = self.config.search + self._may_clear.add(channel) + for _ in range(search.max_localize_steps): + region = self.belief.candidate_set(channel) + if region is None: + return False + cert = clear_certificate(region, channel, self.config) + if cert.certified and channel in self._may_clear: + if self._attempt_clear(channel, cert.center, cert): + return True + self._may_clear.discard(channel) + continue + plan = select_measurement_point(region, self.client.state.position, + self.config) + outcome = self.client.measure(plan.position, channel) + self.belief.update_measure(channel, plan.position, outcome.result, + outcome.svd_deg) + self._may_clear.add(channel) + if outcome.result == NO_SIGNAL: + self._anomaly("no_signal_in_safe_disk", channel, + position=plan.position, detail=plan.reason) + return self.belief.channels[channel].status is ChannelStatus.CLEARED + + def _attempt_clear(self, channel: int, target: Vec2, + cert: ClearCertificate) -> bool: + outcome = self.client.clear(target, channel) + if outcome.result == CLEAR_SUCCESS: + self.belief.mark_cleared(channel) + self.cleared.append(channel) + return True + if cert.certified: + self._anomaly("clear_failed_with_certificate", channel, position=target, + radius_m=round(cert.radius_m, 3), limit_m=cert.limit_m) + return False + + def _anomaly(self, kind: str, channel: int, **detail) -> None: + """异常只在守恒性被打破或证书失效时出现；每频道限量记录防刷屏。""" + recorded = sum(1 for item in self.anomalies if item["channel"] == channel) + if recorded >= self.config.search.max_anomalies_per_channel: + return + self.anomalies.append({"kind": kind, "channel": channel, **detail}) + + # ------------------------------------------------------------ 收尾 + def _mark_absent_after_coverage(self) -> None: + """全向源缺席证明：全网格无非 no_signal 读数的频道不存在源 [题面]。""" + for channel, belief in self.belief.channels.items(): + if belief.status is ChannelStatus.UNKNOWN \ + and self.ledger.coverage_complete(channel): + self.belief.mark_absent(channel) + + def _resolve_detected(self) -> str: + """覆盖完成后，对仍未清除的 DETECTED 频道做第二轮定位。""" + for channel in self.belief.channels_by_status(ChannelStatus.DETECTED): + self._localize_and_clear(channel) + unresolved = [channel for channel, belief in self.belief.channels.items() + if belief.status not in (ChannelStatus.CLEARED, + ChannelStatus.ABSENT)] + return "all_resolved" if not unresolved else "unresolved_channels" + + def _summary(self, info) -> dict: + events = self.client.log.action_events() + stats = summarize(events) + report = recompute_virtual_time(events) + cleared = self.belief.channels_by_status(ChannelStatus.CLEARED) + absent = self.belief.channels_by_status(ChannelStatus.ABSENT) + resolved = set(cleared) | set(absent) + unresolved = [channel for channel in sorted(self.belief.channels) + if channel not in resolved] + completed = (self.stop_reason == "sixteen_cleared" + or (self.stop_reason == "all_resolved" and not unresolved)) + return { + "completed": completed, + "stop_reason": self.stop_reason, + "exit_reason": info.reason, + "virtual_time_s": round(info.virtual_time_s, 3), + "cleared_channels": cleared, + "absent_channels": absent, + "unresolved_channels": unresolved, + "measures": stats["measures"], + "clears": stats["clears"], + "clear_success": stats["cleared"], + "move_distance_m": round(stats["distance_m"], 1), + "anomalies": self.anomalies, + "virtual_recompute_mismatches": len(report["mismatches"]), + } + + +class DirectionalExtension(OmnidirectionalLoop): + """问题4混合源闭环（P5）：全向基线 + 定向观测处理 + 后备清除。 + + 与 P4 的三点差异（协议衔接见 docs/协议规则表与状态机.md §7）： + - 49 点覆盖的发现保证对定向源同样成立，覆盖阶段不区分源类型； + - 保证接收盘内的 no_signal 只能由朝向遮挡解释 [观测推理]：该频道 + 记为"观测到定向"，随后对 MEC 圆心取对跖探点复核；联合约束基线 + 不据此删除空间候选（真源不误删）； + - 定位停滞（候选集有界但未达清除证书）时构造 <=20m 有限圆盘覆盖， + 预算核对后逐格 /clear：清除与朝向无关 [题面]，是定向源不依赖 + 可见性的完备后备。格数超硬顶或现实预算不足则跳过并留痕，绝不盲扫。 + """ + + def __init__(self, client: SimulatorClient, + config: ProblemConfig = ProblemConfig()): + super().__init__(client, config) + self.directional_observed: list[int] = [] # 观测到定向性的频道 + self.blocked_probes: dict[int, Vec2] = {} # channel -> 被遮挡的探点 + self.fallback_log: list[dict] = [] # 后备清除逐次留痕 + + # ------------------------------------------------------------ 定位与清除 + def _localize_and_clear(self, channel: int) -> bool: + """定位子循环：选点测量收敛候选集，拿到清除证书后清除。 + + 与 P4 的差异：保证接收盘内的 no_signal 说明探点被朝向遮挡 + [观测推理]，走"对跖复核 -> 环探"升级梯子（不计入定位步数， + 由 max_blocked_probes 限流），之后仍未收敛再转后备清除。 + """ + search = self.config.search + self._may_clear.add(channel) + steps = 0 + while steps < search.max_localize_steps: + region = self.belief.candidate_set(channel) + if region is None: + return False + cert = clear_certificate(region, channel, self.config) + if cert.certified and channel in self._may_clear: + if self._attempt_clear(channel, cert.center, cert): + return True + self._may_clear.discard(channel) # 证书失败：补测后才可重试 + continue + if len(self.blocked_probes.get(channel, [])) > search.max_blocked_probes: + break # 梯子用尽仍被遮挡：定位停滞，转后备清除 + point = self._next_probe(region, channel) + outcome = self.client.measure(point, channel) + self.belief.update_measure(channel, point, outcome.result, + outcome.svd_deg) + self._may_clear.add(channel) + if outcome.result == NO_SIGNAL \ + and safe_disk_radius(region, self.config) >= 0: + if channel not in self.directional_observed: + self.directional_observed.append(channel) + self.blocked_probes.setdefault(channel, []).append(point) + continue # 被遮挡探点不消耗定位步数，由遮挡预算限流 + if outcome.result == NO_SIGNAL: + self._anomaly("no_signal_outside_safe_disk", channel, + position=point) + else: + self.blocked_probes.pop(channel, None) + steps += 1 + if self.belief.channels[channel].status is ChannelStatus.CLEARED: + return True + region = self.belief.candidate_set(channel) + if region is None: + return False + cert = clear_certificate(region, channel, self.config) + if cert.certified and channel in self._may_clear: + if self._attempt_clear(channel, cert.center, cert): + return True + self._may_clear.discard(channel) + return self._fallback_clear(channel, region) + + def _next_probe(self, region: CandidateRegion, channel: int) -> Vec2: + """定向遮挡下的探点升级梯子 [工程]。 + + - 未被遮挡过：worst-posterior 选点器（保证接收盘内）； + - 第 1 次被遮挡：对 MEC 圆心取对跖点，仍在保证接收盘内； + - 之后：绕 MEC 圆心、半径 = ρ 的环探，方位自首个被遮挡点起 + 每次 +45°。环包围任意候选源（|g-c| <= ρ），发射半平面在环上 + 截出 >=180° 弧，等距采样大概率落入；环点距离不构成接收保证， + 失败由后备清除兜底。 + """ + blocked = self.blocked_probes.get(channel, []) + center, radius = region.mec() + if not blocked: + return select_measurement_point(region, self.client.state.position, + self.config).position + safe = safe_disk_radius(region, self.config) + first = blocked[0] + if len(blocked) == 1 and safe >= 0: + dx, dy = center[0] - first[0], center[1] - first[1] + norm = math.hypot(dx, dy) + if norm <= 1e-9: + probe = min(safe, 1.0) + return (center[0] + probe, center[1]) + scale = min(1.0, safe / norm) + return (center[0] + dx / norm * scale, center[1] + dy / norm * scale) + base = math.atan2(first[1] - center[1], first[0] - center[0]) + azimuth = base + math.radians(45.0 * (len(blocked) - 1)) + return (center[0] + radius * math.cos(azimuth), + center[1] + radius * math.sin(azimuth)) + + def _fallback_clear(self, channel: int, region: CandidateRegion) -> bool: + """定位停滞的后备：候选集有限圆盘覆盖逐格清除（格数即请求上界）。""" + search, session = self.config.search, self.config.session + width = max(p[0] for p in region.polygon) - min(p[0] for p in region.polygon) + height = max(p[1] for p in region.polygon) - min(p[1] for p in region.polygon) + cells = fallback_clear_cells(region.polygon, self.config) + entry = {"channel": channel, "cells": len(cells), + "bound": fallback_cell_bound(width, height, self.config), + "posterior_radius_m": round(region.mec()[1], 3)} + self.fallback_log.append(entry) + if len(cells) > search.fallback_max_cells: + # 宽候选集盲扫违背预算纪律：留待定位收敛，不得硬扫 [计划§4]。 + entry["outcome"] = "skipped_cell_cap" + self._anomaly("fallback_cell_cap_exceeded", channel, + cells=len(cells), cap=search.fallback_max_cells) + return False + # 现实预算核对：N 次请求 × 单请求高分位估计 + 退出余量，不足不启动。 + remaining = self.client.real_remaining_s + if remaining is None or len(cells) * session.real_rtt_p95_s \ + + session.reserve_exit_s > remaining: + entry["outcome"] = "skipped_budget" + return False + position = self.client.state.position + attempts = 0 + for cell in sorted(cells, key=lambda c: math.dist(position, c)): + attempts += 1 + outcome = self.client.clear(cell, channel) + if outcome.result == CLEAR_SUCCESS: + self.belief.mark_cleared(channel) + self.cleared.append(channel) + entry["outcome"] = "cleared" + entry["attempts"] = attempts + return True + # 候选集保守包含真源，格心距格内任一点 <= 10*sqrt(2) < 20m：全部 + # 失败说明守恒性被打破，按异常留痕，不得据此删除候选。 + entry["outcome"] = "exhausted" + entry["attempts"] = attempts + self._anomaly("fallback_exhausted_without_success", channel, + cells=len(cells)) + return False + + # ------------------------------------------------------------ 收尾 + def _summary(self, info) -> dict: + report = super()._summary(info) + report["directional_observed_channels"] = list(self.directional_observed) + report["fallback_clears"] = self.fallback_log + return report + + +def fallback_cell_bound(width_m: float, height_m: float, + config: ProblemConfig = ProblemConfig()) -> int: + """外包框枚举的格数上界 (⌈w/cell⌉+2)(⌈h/cell⌉+2)（计划§4 规模检查）。""" + cell = config.search.fallback_clear_grid_m + return (math.ceil(width_m / cell) + 2) * (math.ceil(height_m / cell) + 2) + + +def fallback_clear_cells(polygon: list[Vec2], + config: ProblemConfig = ProblemConfig()) -> list[Vec2]: + """候选外包多边形的 <=20m 有限圆盘覆盖格心列表（P5 后备清除）。 + + 全局半开 20m 网格索引；多边形与格子的相交判定带正容差（宁可多测）， + 多边形边界恰在格线上时两侧格子都保留，因此格内任意点距某格心 + <= 10*sqrt(2) < 20m，逐格 /clear 至少一次命中。列表长度即 /clear + 请求预算上界；先核对 fallback_cell_bound，超预算不得逐格盲扫。 + """ + if not polygon: + return [] + cell = config.search.fallback_clear_grid_m + xs = [p[0] for p in polygon] + ys = [p[1] for p in polygon] + i0, i1 = math.floor(min(xs) / cell), math.floor(max(xs) / cell) + j0, j1 = math.floor(min(ys) / cell), math.floor(max(ys) / cell) + eps = 1e-9 + centers: list[Vec2] = [] + for i in range(i0, i1 + 1): + for j in range(j0, j1 + 1): + clipped = list(polygon) + for a, b, c in ((1.0, 0.0, (i + 1) * cell + eps), + (-1.0, 0.0, -(i * cell - eps)), + (0.0, 1.0, (j + 1) * cell + eps), + (0.0, -1.0, -(j * cell - eps))): + clipped = clip_halfplane(clipped, a, b, c, tol=eps) + if not clipped: + break + if clipped: + centers.append(((i + 0.5) * cell, (j + 0.5) * cell)) + return centers diff --git a/src/mcm26b/sim/scenes.py b/src/mcm26b/sim/scenes.py new file mode 100644 index 0000000..703b454 --- /dev/null +++ b/src/mcm26b/sim/scenes.py @@ -0,0 +1,60 @@ +"""自检场景生成器（测试端专用）。 + +真值只进入测试与 CLI ``--self-test`` 入口，策略端仅见模拟器响应； +场景覆盖题面强调的边界情形：近圆域边界源、最小/最大接收半径两档， +以及问题4的定向源（含朝向背离圆心的朝外边界源）。 +""" + +from __future__ import annotations + +import math +from random import Random + +from .simulator import Source + + +def random_omni_scene(seed: int = 20260910, count: int = 10) -> list[Source]: + """全向源场景：频道互异，含近圆域边界点，接收半径 1000/1500 交替。""" + if not 0 <= count <= 20: + raise ValueError("源数量须在 0..20") + rng = Random(seed) + channels = rng.sample(range(1, 21), count) + sources = [] + for index, channel in enumerate(channels): + if index == 0: + radius = rng.uniform(1780.0, 1799.0) # 近圆域边界 + elif index == 1 and count > 1: + radius = rng.uniform(1780.0, 1799.0) + else: + radius = rng.uniform(0.0, 1795.0) + angle = rng.uniform(0.0, 2 * math.pi) + r_eff = 1000.0 if index % 2 == 0 else 1500.0 + sources.append(Source(channel=channel, + x=radius * math.cos(angle), + y=radius * math.sin(angle), + r_eff_m=r_eff)) + return sources + + +def mixed_scene(seed: int = 20260910, count: int = 10, + directional: int = 3) -> list[Source]: + """问题4混合场景：count 个源中 directional 个为定向源。 + + 改造 random_omni_scene 的前 directional 个源：近圆域边界的前两个定向源 + 朝向取"背离圆心"（发射半平面基本朝向域外，覆盖网格仍保证可发现， + 即"朝外边界源不漏"验收情形），其余定向源朝向随机。 + """ + if not 0 <= directional <= count: + raise ValueError("定向源数量须在 0..count") + sources = list(random_omni_scene(seed=seed, count=count)) + rng = Random(seed + 1) + for index in range(directional): + source = sources[index] + if index < 2 and count > 1 and math.hypot(source.x, source.y) > 1700.0: + heading = math.degrees(math.atan2(source.y, source.x)) % 360.0 + else: + heading = rng.uniform(0.0, 360.0) + sources[index] = source.__class__(channel=source.channel, x=source.x, + y=source.y, r_eff_m=source.r_eff_m, + directional=True, heading_deg=heading) + return sources diff --git a/tests/test_belief.py b/tests/test_belief.py new file mode 100644 index 0000000..6bbadfb --- /dev/null +++ b/tests/test_belief.py @@ -0,0 +1,148 @@ +"""P3 信念模块验收：保守性（真源保留）与角域/圆盘约束的几何正确性。 + +对应 docs/B题建模分析与实施计划.md 验收标准"自建已知真值案例中真源 +始终保留；跨角度边界不出错"。 +""" + +import math +import unittest +from dataclasses import replace +from random import Random +from unittest import TestCase + +from mcm26b.belief import (BeliefState, CandidateRegion, ChannelStatus, + circumscribed_polygon, clip_halfplane) +from mcm26b.config import ProblemConfig +from mcm26b.protocol.rules import DIRECTION, NEAR, NO_SIGNAL + +CONFIG = replace(ProblemConfig(), + search=replace(ProblemConfig().search, + bearing_rounding_margin_deg=0.005)) + + +class ClipGeometryTests(TestCase): + def test_circumscribed_polygon_contains_whole_disk(self): + """外切多边形必须包含圆盘内任意点（保守外包）。""" + region = CandidateRegion(circumscribed_polygon((100, -50), 800, 192)) + rng = Random(1) + for _ in range(200): + angle = rng.uniform(0, 2 * math.pi) + radius = 800 * math.sqrt(rng.random()) + point = (100 + radius * math.cos(angle), + -50 + radius * math.sin(angle)) + self.assertTrue(region.contains(point), f"圆盘内点 {point} 被外包排除") + + def test_clip_halfplane_keeps_inside_drops_outside(self): + square = [(0.0, 0.0), (10.0, 0.0), (10.0, 10.0), (0.0, 10.0)] + clipped = clip_halfplane(square, 1.0, 0.0, 6.0) + inside = CandidateRegion(clipped) + self.assertTrue(inside.contains((5.0, 5.0))) + self.assertFalse(inside.contains((8.0, 5.0), tol_m=0.001)) + + def test_wedge_clip_handles_0_360_wraparound(self): + region = CandidateRegion.disk((0.0, 0.0), 1500, 192) + region.clip_wedge((0.0, 0.0), 359.99, 1.005) + inside = (800 * math.cos(math.radians(0.001)), + 800 * math.sin(math.radians(0.001))) + self.assertTrue(region.contains(inside), "跨 0° 边界附近的真点不得被剪掉") + outside = (800 * math.cos(math.radians(5.0)), + 800 * math.sin(math.radians(5.0))) + self.assertFalse(region.contains(outside, tol_m=1.0)) + + def test_clip_away_everything_raises(self): + region = CandidateRegion.disk((0.0, 0.0), 100, 32) + with self.assertRaises(Exception): + # 与区域完全分离的半平面会把多边形剪空：必须显式报错而非静默 + for _ in range(4): + region._clip(1.0, 0.0, -1000.0) + + +class BeliefDirectionTests(TestCase): + def test_direction_region_retains_true_source(self): + """随机场景：构造合法 direction 读数后真源必须仍在候选集内。""" + rng = Random(7) + cases = 0 + while cases < 40: + source = (rng.uniform(-1790, 1790), rng.uniform(-1790, 1790)) + if math.hypot(*source) > 1790: + continue + station = (rng.uniform(-2100, 2100), rng.uniform(-2100, 2100)) + if math.dist(station, source) > CONFIG.field.r_eff_max_m: + continue + cases += 1 + true_bearing = math.degrees(math.atan2( + source[1] - station[1], source[0] - station[0])) % 360.0 + svd = round(true_bearing + rng.uniform(-1.0, 1.0), 2) % 360.0 + belief = BeliefState(CONFIG) + belief.update_measure(3, station, DIRECTION, svd) + region = belief.candidate_set(3) + self.assertIsNotNone(region) + self.assertEqual(belief.channels[3].status, ChannelStatus.DETECTED) + self.assertTrue(region.contains(source), + f"真源 {source} 被读数 {svd}@{station} 剪掉") + + def test_second_direction_shrinks_and_retains(self): + source = (600.0, 400.0) + + def svd_from(station): + bearing = math.degrees(math.atan2(source[1] - station[1], + source[0] - station[0])) % 360.0 + return round(bearing + 0.7, 2) % 360.0 + + belief = BeliefState(CONFIG) + belief.update_measure(1, (0.0, 0.0), DIRECTION, svd_from((0.0, 0.0))) + radius_first = belief.candidate_set(1).mec()[1] + belief.update_measure(1, (-200.0, 900.0), DIRECTION, svd_from((-200.0, 900.0))) + region = belief.candidate_set(1) + self.assertTrue(region.contains(source)) + self.assertLess(region.mec()[1], radius_first - 100.0, + "第二次测向后候选集应显著收缩") + + def test_rounding_margin_keeps_edge_reading(self): + """误差取满 1° 且舍入再偏 0.005°：有裕量的配置保留真源。 + + svd=2.01 的读数角域在有 0.005° 裕量时下边界恰为 1.005°， + 与真源方位重合；无裕量配置会把它剪掉——这就是裕量存在的理由。 + """ + source = (1200 * math.cos(math.radians(1.005)), + 1200 * math.sin(math.radians(1.005))) + with_margin = BeliefState(CONFIG) + with_margin.update_measure(1, (0.0, 0.0), DIRECTION, 2.01) + self.assertTrue(with_margin.candidate_set(1).contains(source, tol_m=0.5)) + no_margin = BeliefState(ProblemConfig()) + no_margin.update_measure(1, (0.0, 0.0), DIRECTION, 2.01) + self.assertFalse(no_margin.candidate_set(1).contains(source, tol_m=0.5)) + + def test_near_shrinks_to_five_meters(self): + belief = BeliefState(CONFIG) + belief.update_measure(2, (100.0, 100.0), NEAR, None) + center, radius = belief.candidate_set(2).mec() + self.assertLessEqual(radius, 5.1) + self.assertTrue(belief.candidate_set(2).contains((101.0, 101.0))) + self.assertEqual(belief.channels[2].status, ChannelStatus.DETECTED) + + def test_no_signal_keeps_region_unchanged(self): + belief = BeliefState(CONFIG) + belief.update_measure(4, (0.0, 0.0), DIRECTION, 45.0) + before = list(belief.candidate_set(4).polygon) + belief.update_measure(4, (300.0, 0.0), NO_SIGNAL, None) + self.assertEqual(belief.candidate_set(4).polygon, before) + self.assertEqual(belief.channels[4].status, ChannelStatus.DETECTED) + + def test_invalid_readings_rejected(self): + belief = BeliefState(CONFIG) + with self.assertRaises(ValueError): + belief.update_measure(1, (0.0, 0.0), DIRECTION, None) + with self.assertRaises(ValueError): + belief.update_measure(1, (0.0, 0.0), "garbage", None) + + def test_absent_and_cleared_transitions(self): + belief = BeliefState(CONFIG) + belief.mark_absent(7) + belief.mark_cleared(9) + self.assertEqual(belief.channels_by_status(ChannelStatus.ABSENT), [7]) + self.assertEqual(belief.channels_by_status(ChannelStatus.CLEARED), [9]) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/test_loop_q3.py b/tests/test_loop_q3.py new file mode 100644 index 0000000..320fa8e --- /dev/null +++ b/tests/test_loop_q3.py @@ -0,0 +1,137 @@ +"""P4 全向闭环端到端验收：10/16/0 源、边界源、预算耗尽与故障注入。 + +对应 docs/B题建模分析与实施计划.md P4 行验收门槛（自建部分）： +- 10/16 源场景正确退出，全清或数量上界证明； +- 近圆域边界源不漏搜、可清除； +- 虚拟时刻逐步复算与服务器响应一致； +- 现实预算耗尽退出记为未完成，不与成功混记； +- 网络故障重试（幂等回放）不污染闭环结果。 +""" + +import unittest +from dataclasses import replace +from itertools import count + +from mcm26b.config import ProblemConfig +from mcm26b.coverage import coverage_points +from mcm26b.evaluation import recompute_virtual_time +from mcm26b.planner import OmnidirectionalLoop +from mcm26b.protocol.client import SimulatorClient +from mcm26b.sim import (LocalSimTransport, LocalSimulator, + ScriptedFaultTransport, Source) +from mcm26b.sim.scenes import random_omni_scene + +CONFIG = replace(ProblemConfig(), + search=replace(ProblemConfig().search, + bearing_rounding_margin_deg=0.005)) + + +def _run_scene(sources, fault_script=None, clock=None): + simulator = LocalSimulator(sources, robot_id="T001", config=CONFIG) + transport = LocalSimTransport(simulator) + if fault_script is not None: + transport = ScriptedFaultTransport(transport, fault_script) + kwargs = {"clock": clock} if clock is not None else {} + client = SimulatorClient("T001", config=CONFIG, transport=transport, **kwargs) + loop = OmnidirectionalLoop(client, CONFIG) + summary = loop.run() + report = recompute_virtual_time(client.log.action_events()) + return summary, report, simulator + + +class LoopAcceptanceTests(unittest.TestCase): + def test_ten_sources_all_cleared(self): + sources = random_omni_scene(seed=5, count=10) + summary, report, simulator = _run_scene(sources) + truth = sorted(s.channel for s in sources) + self.assertTrue(summary["completed"]) + self.assertEqual(summary["stop_reason"], "all_resolved") + self.assertEqual(sorted(summary["cleared_channels"]), truth) + self.assertEqual(len(summary["absent_channels"]), 10) + self.assertEqual(summary["unresolved_channels"], []) + self.assertEqual(summary["anomalies"], [], "守恒性或证书不应被打破") + self.assertEqual(report["mismatches"], []) + self.assertEqual(summary["virtual_recompute_mismatches"], 0) + self.assertEqual(sorted(simulator.cleared), truth) + self.assertEqual(summary["exit_reason"], "user_exit") + self.assertLess(summary["virtual_time_s"], + CONFIG.field.max_virtual_duration_s) + + def test_sixteen_sources_stop_by_count_bound(self): + sources = random_omni_scene(seed=6, count=16) + summary, report, simulator = _run_scene(sources) + truth = sorted(s.channel for s in sources) + self.assertTrue(summary["completed"]) + self.assertEqual(summary["stop_reason"], "sixteen_cleared") + self.assertEqual(sorted(summary["cleared_channels"]), truth) + self.assertEqual(report["mismatches"], []) + self.assertEqual(sorted(simulator.cleared), truth) + + def test_empty_field_proves_absence_everywhere(self): + summary, report, _ = _run_scene([]) + self.assertTrue(summary["completed"]) + self.assertEqual(summary["stop_reason"], "all_resolved") + self.assertEqual(summary["cleared_channels"], []) + self.assertEqual(summary["absent_channels"], list(range(1, 21))) + self.assertEqual(summary["measures"], 49 * 20, "缺席证明要求全网格逐频道检测") + self.assertEqual(report["mismatches"], []) + + def test_boundary_source_close_to_domain_edge(self): + """半径 1790m、接收半径 1000m 的源：覆盖保证的最近网格点必须发现它。""" + source = Source(channel=13, x=1790.0, y=0.0, r_eff_m=1000.0) + summary, _, simulator = _run_scene([source]) + self.assertTrue(summary["completed"]) + self.assertEqual(sorted(summary["cleared_channels"]), [13]) + self.assertEqual(sorted(simulator.cleared), [13]) + + def test_source_on_grid_vertex(self): + source = Source(channel=20, x=-1400.0, y=700.0, r_eff_m=1500.0) + summary, _, _ = _run_scene([source]) + self.assertTrue(summary["completed"]) + self.assertEqual(sorted(summary["cleared_channels"]), [20]) + + def test_real_budget_exhaustion_exits_cleanly(self): + """现实预算耗尽：立即退出并记为未证明完成。""" + ticks = count(0.0, 200.0) # 每次查询时钟前进 200s，迅速耗尽 1200s 预算 + sources = random_omni_scene(seed=8, count=4) + summary, _, _ = _run_scene(sources, clock=lambda: next(ticks)) + self.assertFalse(summary["completed"]) + self.assertEqual(summary["stop_reason"], "real_budget_exhausted") + self.assertEqual(summary["exit_reason"], "user_exit") + + def test_fault_injection_during_coverage(self): + """覆盖中途注入读超时与断连：幂等回放保证闭环结果不变。""" + sources = random_omni_scene(seed=9, count=3) + script = [None, None, None, "timeout_after", None, + "drop_before", None, ("status", 500), None, None] + summary, report, simulator = _run_scene(sources, fault_script=script) + truth = sorted(s.channel for s in sources) + self.assertTrue(summary["completed"]) + self.assertEqual(sorted(summary["cleared_channels"]), truth) + self.assertEqual(report["mismatches"], []) + self.assertEqual(sorted(simulator.cleared), truth) + + +class CoverageLedgerIntegrationTests(unittest.TestCase): + def test_grid_matches_plan_derivation(self): + points = coverage_points(CONFIG) + self.assertEqual(len(points), 49) + self.assertEqual(min(p[0] for p in points), -2100.0) + self.assertEqual(max(p[1] for p in points), 2100.0) + # 单元内任意点到最近网格点 <= 700*sqrt(2) <= 1000 的覆盖保证 + worst = 0.0 + for gx in range(-20, 21): + for gy in range(-20, 21): + g = (gx * 90.0, gy * 90.0) + if (g[0] ** 2 + g[1] ** 2) ** 0.5 <= 1800.0: + nearest = min(math_dist(g, p) for p in points) + worst = max(worst, nearest) + self.assertLessEqual(worst, 700.0 * 2 ** 0.5 + 1e-9) + + +def math_dist(a, b): + return ((a[0] - b[0]) ** 2 + (a[1] - b[1]) ** 2) ** 0.5 + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/test_loop_q4.py b/tests/test_loop_q4.py new file mode 100644 index 0000000..12afc1f --- /dev/null +++ b/tests/test_loop_q4.py @@ -0,0 +1,244 @@ +"""P5 定向扩展端到端验收：朝外边界源不漏、真源不误删、格数与预算检查。 + +对应 docs/B题建模分析与实施计划.md P5 行验收门槛（自建部分）： +- 混合源（全向+定向）场景全部清除并正常退出；16 源触发数量上界停止； +- 朝向背离圆心的近边界定向源仍被 49 点覆盖发现并清除； +- no_signal 联合约束基线不删除空间候选（真源保留性）； +- 后备清除 <=20m 覆盖无空洞、格数有上界、格数/现实预算不足时不盲扫； +- 虚拟时刻逐步复算与服务器响应一致。 +""" + +import math +import unittest +from dataclasses import replace +from itertools import count + +from mcm26b.belief import BeliefState, CandidateRegion +from mcm26b.config import ProblemConfig +from mcm26b.evaluation import recompute_virtual_time +from mcm26b.planner import (DirectionalExtension, fallback_cell_bound, + fallback_clear_cells, safe_disk_radius) +from mcm26b.protocol.client import SimulatorClient +from mcm26b.protocol.rules import DIRECTION, NO_SIGNAL +from mcm26b.sim import LocalSimTransport, LocalSimulator, Source +from mcm26b.sim.scenes import mixed_scene + +CONFIG = replace(ProblemConfig(), + search=replace(ProblemConfig().search, + bearing_rounding_margin_deg=0.005)) + + +def _run_scene(sources, config=CONFIG, clock=None): + simulator = LocalSimulator(sources, robot_id="T001", config=config) + client = SimulatorClient("T001", config=config, + transport=LocalSimTransport(simulator), + **({"clock": clock} if clock else {})) + loop = DirectionalExtension(client, config) + summary = loop.run() + report = recompute_virtual_time(client.log.action_events()) + return summary, report, simulator, client, loop + + +class FallbackClearCellsTests(unittest.TestCase): + def test_cells_cover_every_polygon_witness(self): + """覆盖无空洞：多边形边界采样点到最近格心 <= 10*sqrt(2)。""" + region = CandidateRegion.disk((0.0, 0.0), 200.0, 192) + cells = fallback_clear_cells(region.polygon, CONFIG) + self.assertTrue(cells) + limit = 10.0 * 2 ** 0.5 + 1e-6 + for witness in region.witnesses(40.0): + nearest = min(math.dist(witness, cell) for cell in cells) + self.assertLessEqual(nearest, limit, + f"格心覆盖在 {witness} 附近出现空洞") + + def test_grid_line_polygon_keeps_both_sides(self): + """多边形横跨格线时两侧格子都保留（宁可多测）。""" + square = [(10.0, -10.0), (30.0, -10.0), (30.0, 10.0), (10.0, 10.0)] + cells = fallback_clear_cells(square, CONFIG) + columns = {cell[0] for cell in cells} + self.assertIn(10.0, columns) # 格线 x=20 左侧 + self.assertIn(30.0, columns) # 右侧 + + def test_degenerate_polygon_single_cell(self): + """点与线段候选集：覆盖格数最小化且无空洞。""" + point = [(500.0, 500.0)] + self.assertEqual(len(fallback_clear_cells(point, CONFIG)), 1) + segment = [(490.0, 500.0), (510.0, 500.0)] + cells = fallback_clear_cells(segment, CONFIG) + self.assertTrue(cells) + limit = 10.0 * 2 ** 0.5 + 1e-6 + for step in range(21): + sample = (490.0 + step, 500.0) + nearest = min(math.dist(sample, cell) for cell in cells) + self.assertLessEqual(nearest, limit) + + def test_count_within_plan_bound(self): + region = CandidateRegion.disk((0.0, 0.0), 200.0, 192) + xs = [p[0] for p in region.polygon] + ys = [p[1] for p in region.polygon] + bound = fallback_cell_bound(max(xs) - min(xs), max(ys) - min(ys), CONFIG) + cells = fallback_clear_cells(region.polygon, CONFIG) + self.assertLessEqual(len(cells), bound) + + +class MixedLoopAcceptanceTests(unittest.TestCase): + def test_mixed_ten_sources_all_cleared(self): + sources = mixed_scene(seed=5, count=10, directional=3) + summary, report, simulator, _, loop = _run_scene(sources) + truth = sorted(s.channel for s in sources) + self.assertTrue(summary["completed"]) + self.assertEqual(summary["stop_reason"], "all_resolved") + self.assertEqual(sorted(summary["cleared_channels"]), truth) + self.assertEqual(summary["unresolved_channels"], []) + self.assertEqual(summary["anomalies"], [], "守恒性或证书不应被打破") + self.assertEqual(report["mismatches"], []) + self.assertEqual(summary["virtual_recompute_mismatches"], 0) + self.assertEqual(sorted(simulator.cleared), truth) + self.assertEqual(summary["exit_reason"], "user_exit") + # 观测到的定向频道必须来自真定向源（观测推理不误标） + truth_directional = sorted(s.channel for s in sources if s.directional) + self.assertTrue(summary["directional_observed_channels"]) + self.assertTrue(set(summary["directional_observed_channels"]) + <= set(truth_directional)) + + def test_mixed_sixteen_sources_stop_by_count_bound(self): + sources = mixed_scene(seed=11, count=16, directional=6) + summary, report, simulator, _, _ = _run_scene(sources) + truth = sorted(s.channel for s in sources) + self.assertTrue(summary["completed"]) + self.assertEqual(summary["stop_reason"], "sixteen_cleared") + self.assertEqual(sorted(summary["cleared_channels"]), truth) + self.assertEqual(sorted(simulator.cleared), truth) + self.assertEqual(report["mismatches"], []) + + def test_outward_boundary_directional_source_not_missed(self): + """朝向背离圆心、r_eff=1000 的近边界源：覆盖保证仍发现并清除。""" + source = Source(channel=7, x=1790.0, y=0.0, r_eff_m=1000.0, + directional=True, heading_deg=0.0) + summary, _, simulator, _, _ = _run_scene([source]) + self.assertTrue(summary["completed"]) + self.assertEqual(summary["cleared_channels"], [7]) + self.assertEqual(sorted(simulator.cleared), [7]) + self.assertEqual(summary["directional_observed_channels"], [7]) + + def test_mixed_batch_seeds_never_degrade(self): + """固定批量种子回归：任何种子下不得全清失败或破坏守恒性。""" + for seed in range(5, 10): + sources = mixed_scene(seed=seed, count=10, directional=4) + summary, report, simulator, _, _ = _run_scene(sources) + truth = sorted(s.channel for s in sources) + self.assertTrue(summary["completed"], f"seed={seed}") + self.assertEqual(sorted(summary["cleared_channels"]), truth, + f"seed={seed}") + self.assertEqual(summary["anomalies"], [], f"seed={seed}") + self.assertEqual(report["mismatches"], [], f"seed={seed}") + + +class JointConstraintBaselineTests(unittest.TestCase): + def test_no_signal_keeps_candidates_and_true_source(self): + """联合约束基线：保证接收盘内 no_signal 不删空间候选（真源不误删）。""" + belief = BeliefState(CONFIG) + belief.update_measure(7, (0.0, 0.0), DIRECTION, 30.0) + region = belief.candidate_set(7) + snapshot = list(region.polygon) + self.assertGreater(safe_disk_radius(region, CONFIG), 0.0) + center, _ = region.mec() + belief.update_measure(7, tuple(center), NO_SIGNAL, None) + self.assertEqual(region.polygon, snapshot, "no_signal 不得收缩候选集") + # 候选集中任取的代表点（含生成读数的角域内部点）仍被保留 + for witness in region.witnesses(300.0): + self.assertTrue(region.contains(witness, tol_m=0.05)) + + def test_directional_source_retained_through_blocked_probes(self): + """端到端保留性：定向源被遮挡若干次后仍被清除，不因 no_signal 误删。""" + source = Source(channel=3, x=600.0, y=800.0, r_eff_m=1500.0, + directional=True, heading_deg=200.0) + summary, _, simulator, _, _ = _run_scene([source]) + self.assertTrue(summary["completed"]) + self.assertEqual(summary["cleared_channels"], [3]) + self.assertEqual(sorted(simulator.cleared), [3]) + + +class FallbackDisciplineTests(unittest.TestCase): + def _stall_config(self, **search_updates): + """迫使定位立即停滞进入后备清除的最小配置。""" + return replace(CONFIG, + search=replace(CONFIG.search, + max_localize_steps=1, + max_blocked_probes=0, + **search_updates)) + + def test_cell_cap_skips_instead_of_blind_scan(self): + """格数超硬顶：后备清除全部记录 skipped_cell_cap，频道保持未解决。""" + config = self._stall_config(fallback_max_cells=5) + sources = mixed_scene(seed=5, count=6, directional=2) + summary, report, _, _, _ = _run_scene(sources, config=config) + self.assertFalse(summary["completed"]) + self.assertEqual(summary["stop_reason"], "unresolved_channels") + self.assertTrue(summary["fallback_clears"]) + self.assertTrue(all(entry["outcome"] == "skipped_cell_cap" + for entry in summary["fallback_clears"])) + self.assertEqual(report["mismatches"], []) + + def test_real_budget_check_skips_fallback(self): + """现实预算核对：N*p95 + 退出余量超过剩余时不启动后备清除。""" + config = replace(self._stall_config(), + session=replace(CONFIG.session, real_rtt_p95_s=600.0)) + sources = mixed_scene(seed=6, count=6, directional=2) + summary, _, _, _, _ = _run_scene(sources, config=config) + self.assertFalse(summary["completed"]) + self.assertTrue(summary["fallback_clears"]) + self.assertTrue(all(entry["outcome"] == "skipped_budget" + for entry in summary["fallback_clears"])) + self.assertTrue(summary["unresolved_channels"]) + + def test_fallback_clear_succeeds_on_bounded_candidate(self): + """后备清除正路径：有界候选集逐格 /clear 命中真源。""" + source = Source(channel=7, x=500.0, y=500.0, r_eff_m=1500.0, + directional=True, heading_deg=180.0) + simulator = LocalSimulator([source], robot_id="T001", config=CONFIG) + client = SimulatorClient("T001", config=CONFIG, + transport=LocalSimTransport(simulator)) + loop = DirectionalExtension(client, CONFIG) + client.enter() + region = CandidateRegion.disk((500.0, 500.0), 30.0, 192) + self.assertTrue(loop._fallback_clear(7, region)) + self.assertEqual(loop.cleared, [7]) + self.assertEqual(simulator.cleared, {7}) + entry = loop.fallback_log[0] + self.assertEqual(entry["outcome"], "cleared") + self.assertLessEqual(entry["attempts"], entry["cells"]) + cells = fallback_clear_cells(region.polygon, CONFIG) + self.assertLessEqual(len(cells), CONFIG.search.fallback_max_cells) + + def test_fallback_exhaustion_records_anomaly_without_deletion(self): + """后备全失败（空域）：守恒性异常留痕，且不删除空间候选。""" + simulator = LocalSimulator([], robot_id="T001", config=CONFIG) + client = SimulatorClient("T001", config=CONFIG, + transport=LocalSimTransport(simulator)) + loop = DirectionalExtension(client, CONFIG) + client.enter() + region = CandidateRegion.disk((0.0, 0.0), 30.0, 192) + snapshot = list(region.polygon) + self.assertFalse(loop._fallback_clear(7, region)) + entry = loop.fallback_log[0] + self.assertEqual(entry["outcome"], "exhausted") + kinds = [item["kind"] for item in loop.anomalies] + self.assertIn("fallback_exhausted_without_success", kinds) + self.assertEqual(region.polygon, snapshot, "清除失败不得删除候选") + + +class RealBudgetExitTests(unittest.TestCase): + def test_budget_exhaustion_exits_incomplete(self): + """混合场景现实预算耗尽：立即退出且记为未证明完成。""" + ticks = count(0.0, 100.0) # 每次查询前进 100s，约 12 个动作内耗尽预算 + sources = mixed_scene(seed=7, count=8, directional=2) + summary, _, _, _, _ = _run_scene(sources, config=CONFIG, + clock=lambda: next(ticks)) + self.assertFalse(summary["completed"]) + self.assertEqual(summary["stop_reason"], "real_budget_exhausted") + self.assertEqual(summary["exit_reason"], "user_exit") + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/test_planner.py b/tests/test_planner.py new file mode 100644 index 0000000..a4ea5e9 --- /dev/null +++ b/tests/test_planner.py @@ -0,0 +1,123 @@ +"""P3 选点器验收：保证接收、后验收敛、清除证书阈值、与解析证书衔接。""" + +import math +import unittest +from dataclasses import replace +from unittest import TestCase + +from mcm26b.belief import CandidateRegion +from mcm26b.config import ProblemConfig +from mcm26b.geometry.localization import Measurement +from mcm26b.geometry.planning import first_bearing_candidate_disk +from mcm26b.planner import (bearing_deg, clear_certificate, select_measurement_point, + select_second_point) +from mcm26b.protocol.client import SimulatorClient +from mcm26b.sim import LocalSimTransport, LocalSimulator + +CONFIG = replace(ProblemConfig(), + search=replace(ProblemConfig().search, + bearing_rounding_margin_deg=0.005)) +FIRST_STATION = (0.0, 0.0) +FIRST_BEARING = 30.0 + + +def _k1_region() -> CandidateRegion: + region = CandidateRegion.initial(FIRST_STATION, CONFIG) + region.clip_wedge(FIRST_STATION, FIRST_BEARING, + CONFIG.field.bearing_error_half_deg + + CONFIG.search.bearing_rounding_margin_deg) + return region + + +class SelectionGuaranteeTests(TestCase): + def test_selected_point_receives_every_sector_candidate(self): + """安全盘内选点对解析角域 K1 内任意候选源的距离 <= 1000m。 + + 直接对截断角域 K1 = Ω ∩ W(s1, θ±δ) ∩ B(s1,1500) 稠密采样验证， + 不依赖外包多边形本身。 + """ + region = _k1_region() + plan = select_measurement_point(region, FIRST_STATION, CONFIG) + half = 1.005 + for step_phi in range(41): + phi = math.radians(FIRST_BEARING - half + 2 * half * step_phi / 40) + for step_t in range(60): + t = 5.0 + (1500.0 - 5.0) * step_t / 59 + g = (t * math.cos(phi), t * math.sin(phi)) + self.assertLessEqual( + math.dist(plan.position, g), 1000.0 + 1e-6, + f"候选源 {g} 距选点超过最小接收半径，将浪费一次 no_signal") + + def test_worst_posterior_shrinks_region(self): + region = _k1_region() + plan = select_measurement_point(region, FIRST_STATION, CONFIG) + self.assertLess(plan.worst_posterior_radius_m, region.mec()[1], + "最坏后验半径必须优于当前候选集，否则选点无进展") + self.assertGreater(plan.worst_posterior_radius_m, 0.0) + + def test_posterior_probe_retains_witness(self): + """后验探针（读数取区间端点）仍包含生成它的边界点：保守性抽查。""" + region = _k1_region() + plan = select_measurement_point(region, FIRST_STATION, CONFIG) + half = CONFIG.field.bearing_error_half_deg \ + + CONFIG.search.bearing_rounding_margin_deg + witnesses = region.witnesses(300.0) + self.assertGreater(len(witnesses), 4) + for g in witnesses: + phi = bearing_deg(plan.position, g) + for reading in (phi - half, phi + half): + probe = CandidateRegion(list(region.polygon), region.slack_m) + probe.clip_wedge(plan.position, reading, half) + self.assertTrue(probe.polygon and probe.contains(g, tol_m=0.05), + f"后验剪掉了生成读数的边界点 {g}") + + def test_move_cost_recorded_and_reason_explains(self): + region = _k1_region() + plan = select_measurement_point(region, (500.0, 500.0), CONFIG) + expected = math.dist((500.0, 500.0), plan.position) / CONFIG.cost.speed_mps + self.assertAlmostEqual(plan.move_cost_s, expected, places=6) + self.assertIn("安全盘", plan.reason) + + +class ClearCertificateTests(TestCase): + def test_near_region_is_certified(self): + region = CandidateRegion.disk((500.0, 500.0), 5.0, 192) + cert = clear_certificate(region, 7, CONFIG) + self.assertTrue(cert.certified) + self.assertLess(cert.limit_m, CONFIG.field.clear_radius_m) + self.assertGreater(cert.limit_m, 19.0) + + def test_first_bearing_region_is_not_certified(self): + cert = clear_certificate(_k1_region(), 7, CONFIG) + self.assertFalse(cert.certified) + self.assertGreater(cert.radius_m, cert.limit_m) + + +class SelectSecondPointTests(TestCase): + def test_plan_lies_in_analytic_safe_disk_or_is_equivalent(self): + """P3 入口输出与 geometry 解析证书一致（或提供同等保证）。""" + simulator = LocalSimulator([], robot_id="T001") + client = SimulatorClient("T001", transport=LocalSimTransport(simulator)) + client.enter() + plan = select_second_point(client, FIRST_STATION, FIRST_BEARING, CONFIG) + # 逐点保证：对解析角域采样，接收距离全部成立（测试见上）；此处只 + # 核对输出位置确实落在解析证书安全盘的合理邻域内（两者中心口径不同， + # 允许差一个外包裕量）。 + cert = first_bearing_candidate_disk( + Measurement(FIRST_STATION[0], FIRST_STATION[1], FIRST_BEARING, + 1.0 + CONFIG.search.bearing_rounding_margin_deg)) + disk = cert["sufficient_candidate_disk"] + self.assertLessEqual(math.dist(plan.position, tuple(disk["center"])), + disk["radius_m"] + 10.0) + + def test_exploration_fallback_when_rho_over_1000(self): + """ρ>1000 的一般候选集退化为探索 MEC 圆心，并如实标注可能 no_signal。""" + wide = CandidateRegion.disk((0.0, 0.0), 1200.0, 192) + plan = select_measurement_point(wide, (0.0, 0.0), CONFIG) + self.assertAlmostEqual(plan.position[0], 0.0, places=6) + self.assertAlmostEqual(plan.position[1], 0.0, places=6) + self.assertIn("探索", plan.reason) + + +if __name__ == "__main__": + unittest.main()