1
This commit is contained in:
@@ -16,11 +16,12 @@ src/mcm26b/
|
|||||||
├── protocol/ # P0/P1 字段表、计时模型、状态机、串行幂等客户端(已完工)
|
├── protocol/ # P0/P1 字段表、计时模型、状态机、串行幂等客户端(已完工)
|
||||||
├── geometry/ # P2 角域交会、直径、MEC、问题二安全候选盘证书(已完工)
|
├── geometry/ # P2 角域交会、直径、MEC、问题二安全候选盘证书(已完工)
|
||||||
├── sim/ # 自建模拟器、故障注入、演练日志回放(测试专用,已完工)
|
├── sim/ # 自建模拟器、故障注入、演练日志回放(测试专用,已完工)
|
||||||
├── belief/ # P3 逐频道候选集合 K_c(骨架)
|
├── belief/ # P3 逐频道候选集合 K_c:保守外包多边形 + 半平面裁剪(已完工)
|
||||||
├── coverage/ # P4 49点覆盖账本(骨架 + 网格生成)
|
├── coverage/ # P4 49点覆盖账本(网格生成 + 逐频道记录)
|
||||||
├── planner/ # P3 选点 / P4 全向闭环 / P5 定向扩展(骨架)
|
├── planner/ # P3 保证接收盘选点器 / P4 全向闭环 / P5 混合源定向闭环(已完工)
|
||||||
└── evaluation/ # 日志复算与统计(复算已完工)
|
├── evaluation/ # 日志复算与统计(已完工)
|
||||||
tests/ # pytest(兼容 unittest 风格),53 项
|
└── sim/scenes.py # 自检场景生成器:全向与混合源(测试端专用)
|
||||||
|
tests/ # pytest(兼容 unittest 风格),95 项
|
||||||
scripts/ # 采集、基准、可视化与校核脚本
|
scripts/ # 采集、基准、可视化与校核脚本
|
||||||
output/ examples/ # 演练证据、算例与生成产物
|
output/ examples/ # 演练证据、算例与生成产物
|
||||||
```
|
```
|
||||||
@@ -33,6 +34,9 @@ Python ≥ 3.10,依赖仅 `requests`(测试另需 `pytest`)。
|
|||||||
uv sync # 安装(可编辑挂载本包)
|
uv sync # 安装(可编辑挂载本包)
|
||||||
uv run pytest # 全部测试
|
uv run pytest # 全部测试
|
||||||
uv run mcm26b sim-check # P0 自检:复现附件计时示例 105/111/194/199
|
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 mcm26b q1 examples/q1/normal.json # 问题一求解(等价 python -m mcm26b.geometry ...)
|
||||||
uv run python main.py examples/q1/triangle.json --rounding-margin-deg 0.005
|
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) | ✅ 完成 |
|
| P0 规则固定(规则表/状态机/字段表,示例逐步 105/111/194/199) | ✅ 完成 |
|
||||||
| P1 协议基线(客户端 + 自建回放/故障注入测试) | ✅ 完成 |
|
| P1 协议基线(客户端 + 自建回放/故障注入测试) | ✅ 完成 |
|
||||||
| P2 几何内核(角域交、直径、MEC、证书) | ✅ 完成(迁入 `mcm26b.geometry`) |
|
| P2 几何内核(角域交、直径、MEC、证书) | ✅ 完成(迁入 `mcm26b.geometry`) |
|
||||||
| P3 问题二选点(K1、候选区域 Q、采样评估) | 🦴 骨架(证书已完工) |
|
| P3 问题二选点(K1、候选区域 Q、采样评估) | ✅ 完成(belief + 选点器,测试固化) |
|
||||||
| P4 问题3全向闭环(49点覆盖、定位队列、清除、停止) | 🦴 骨架 |
|
| P4 问题3全向闭环(49点覆盖、定位队列、清除、停止) | ✅ 自建完成(官方演练待执行) |
|
||||||
| P5 问题4定向扩展(联合约束、后备清除、预算) | 🦴 骨架 |
|
| P5 问题4定向扩展(联合约束、后备清除、预算) | ✅ 自建完成(官方演练待执行) |
|
||||||
| P6 加速与消融 | 🦴 未开始 |
|
| P6 加速与消融 | 🦴 未开始 |
|
||||||
|
|
||||||
## 采集与正式测试约束
|
## 采集与正式测试约束
|
||||||
|
|||||||
+20
-7
@@ -187,13 +187,13 @@ AI 的适合用法是把这些模块逐一变成可测试程序、生成边界
|
|||||||
|
|
||||||
| 阶段 | 交付与执行任务 | 验收门槛 | 当前状态 |
|
| 阶段 | 交付与执行任务 | 验收门槛 | 当前状态 |
|
||||||
| --- | --- | --- | --- |
|
| --- | --- | --- | --- |
|
||||||
| P0 规则固定 | 规则表、状态机、四类API字段与成本 | 示例逐步105/111/194/199秒;clear不切换频道 | 已核对规则,完整状态机待工程化 |
|
| 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和超时重试不污染状态;主动退出;故障注入回放 | 已有少量测向采集器及真实计时回放;完整客户端待实现 |
|
| P1 协议基线 | enter/measure/clear/exit客户端;串行、幂等、截止时间 | accepted=false和超时重试不污染状态;主动退出;故障注入回放 | 已完成:`protocol/client`,`ScriptedFaultTransport`/`PlaybackTransport` 回放,`tests/test_protocol_client.py` 全过 |
|
||||||
| P2 几何内核 | 问题一解答、区域分类、直径圆证据、MEC、清除余量 | 跨界、退化、无界、解析反例、独立算法对照 | 已实现并优化,20项自动化测试通过 |
|
| P2 几何内核 | 问题一解答、区域分类、直径圆证据、MEC、清除余量 | 跨界、退化、无界、解析反例、独立算法对照 | 已实现并优化,20项自动化测试通过 |
|
||||||
| P3 问题二选点 | $K_1$ 先验、安全候选盘、后验半径采样与 `near` 分支 | 距离证书有效;采样加密;不把探索当接收保证 | 解析非空证明及证书已实现;选点器与实验待实现 |
|
| P3 问题二选点 | $K_1$ 先验、安全候选盘、后验半径采样与 `near` 分支 | 距离证书有效;采样加密;不把探索当接收保证 | 已实现:`belief` 保守候选集 + `planner` 保证接收盘选点器(worst-posterior 采样、1/4 间距加密复核);策略对比实验待做 |
|
||||||
| P4 全向闭环 | 49点覆盖记录、定位队列、清除、停止、日志统计 | 10/16源、边界源和错误响应案例;官方演练全清并正常退出 | 待实现 |
|
| P4 全向闭环 | 49点覆盖记录、定位队列、清除、停止、日志统计 | 10/16源、边界源和错误响应案例;官方演练全清并正常退出 | 自建闭环已完成:`planner.OmnidirectionalLoop`(10/16/0 源、近边界源、预算耗尽、故障注入测试通过);官方演练全清待团队执行 |
|
||||||
| P5 定向扩展 | no_signal联合约束、覆盖发现、有限清除后备与预算 | 朝外边界源不漏;真源不误删;格数与退出预算检查 | 已有证明和设计;闭环待实现 |
|
| P5 定向扩展 | no_signal联合约束、覆盖发现、有限清除后备与预算 | 朝外边界源不漏;真源不误删;格数与退出预算检查 | 已实现:`planner.DirectionalExtension`(49点覆盖对定向源成立、保证接收盘内 no_signal 记为观测到定向、对跖+环探梯子、`fallback_clear_cells` 有限圆盘覆盖后备清除带格数/预算双硬顶);`tests/test_loop_q4.py` 全过;官方混合源演练待执行 |
|
||||||
| P6 加速消融 | 共享测点、自适应覆盖、目标排序、提前清除 | 配对案例全清不退化,时间/请求数改善可复算 | 可删减;复杂学习与朝向搜索后置 |
|
| P6 加速消融 | 共享测点、自适应覆盖、目标排序、提前清除 | 配对案例全清不退化,时间/请求数改善可复算 | 可删减;闭环基线数据已可用于消融对照 |
|
||||||
| P7 正式与论文 | 固定版本、论文表1、日志原名、上传核查 | 问题3/4各3份正式记录与程序版本对应 | 团队后续安排;本任务不执行正式测试 |
|
| 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后不能启动新的演练或正式测试;已进入上传队列的日志仍会继续上传。此处引用所给附件安排,运行前核对模拟器界面公告;这份排期不改变当前禁止使用正式测试的限制。
|
按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基准只反映本地几何计算,不能据此宣称请求延迟、完整策略或官方成绩得到同样提升。
|
可复现报告见 `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 加速投入。
|
||||||
|
|||||||
@@ -121,3 +121,74 @@ remaining_real_duration_s 为准,不得假定恒为 1200);测试窗口 25
|
|||||||
进程崩溃不丢已发记录,回放测试与 evaluation 复算共用同一格式;
|
进程崩溃不丢已发记录,回放测试与 evaluation 复算共用同一格式;
|
||||||
- **本地预校验**:channel 范围、坐标有限性与 2e6 上限、robot_id/request_id 格式
|
- **本地预校验**:channel 范围、坐标有限性与 2e6 上限、robot_id/request_id 格式
|
||||||
在发送前拒绝(与服务器 400 语义一致,省一次往返)。
|
在发送前拒绝(与服务器 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。
|
||||||
|
|||||||
+245
-18
@@ -1,24 +1,38 @@
|
|||||||
"""P3 逐频道信念状态:候选集合 K_c 维护与频道处理状态。
|
"""P3 逐频道信念状态:候选集合 K_c 维护与频道处理状态。
|
||||||
|
|
||||||
现状:骨架(P3 起填充)。接口与不变式先行固定,防止后续实现漂移。
|
|
||||||
|
|
||||||
核心不变式(验收标准):
|
核心不变式(验收标准):
|
||||||
- 保守性:真源永远属于维护中的候选集合;任何更新不得因误差、
|
- 保守性:真源永远属于维护中的候选集合。候选集用"圆盘外切多边形 ∩
|
||||||
圆弧离散或浮点容差把真点排除(自建真值案例中真源始终保留);
|
测向角域半平面"的凸多边形表示——外切多边形严格包含圆盘,半平面裁剪
|
||||||
- direction 读数:K_c ⊆ Ω ∩ W(s, θ±δ) ∩ B(s, 1500)(首次测向构造见
|
带正容差,因此每步更新只会放大而不会缩小真实候选集;
|
||||||
geometry.planning.first_bearing_candidate_disk 的解析证书);
|
- direction 读数:K_c ⊆ Ω ∩ W(s, θ±δ_eff) ∩ B(s, 1500),后续读数再交
|
||||||
- no_signal 读数(P4,全向源):仅可排除 B(s, 1000);用于定向源时
|
同样的约束;δ_eff = 误差半角 + 示向度舍入裕量(服务器保留两位小数,
|
||||||
是"位置×朝向"联合约束,单独维护空间候选会误删真源(P5 才启用);
|
舍入本身引入 ≤0.005° 偏差);
|
||||||
|
- no_signal 读数(P4,全向源):基线不据此删空间候选——对定向源它是
|
||||||
|
"位置×朝向"联合约束,单独维护空间候选会误删真源(P5 才启用);
|
||||||
|
"该频道不存在源"的证明由覆盖账本(coverage.CoverageLedger)给出;
|
||||||
|
- near 读数:源在 5m 内,候选集收缩为 B(s, 5),可直接产生清除证书;
|
||||||
- /clear 成功:状态转 CLEARED;失败只说明"该频道 20m 内无可清除源",
|
- /clear 成功:状态转 CLEARED;失败只说明"该频道 20m 内无可清除源",
|
||||||
不得直接删除超出 20m 的候选。
|
不得直接删除超出 20m 的候选,必须先补测再重试。
|
||||||
"""
|
"""
|
||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import math
|
||||||
from dataclasses import dataclass, field
|
from dataclasses import dataclass, field
|
||||||
from enum import Enum
|
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):
|
class ChannelStatus(Enum):
|
||||||
@@ -29,11 +43,185 @@ class ChannelStatus(Enum):
|
|||||||
ABSENT = "absent" # 覆盖证明不存在(P4)
|
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
|
@dataclass
|
||||||
class ChannelBelief:
|
class ChannelBelief:
|
||||||
channel: int
|
channel: int
|
||||||
status: ChannelStatus = ChannelStatus.UNKNOWN
|
status: ChannelStatus = ChannelStatus.UNKNOWN
|
||||||
measurements: list = field(default_factory=list) # (位置, 结果, svd_deg) 记录
|
region: CandidateRegion | None = None
|
||||||
|
measurements: list = field(default_factory=list) # 逐步观测记录(审计用)
|
||||||
|
|
||||||
|
|
||||||
class BeliefState:
|
class BeliefState:
|
||||||
@@ -45,14 +233,53 @@ class BeliefState:
|
|||||||
for channel in range(config.field.channel_min,
|
for channel in range(config.field.channel_min,
|
||||||
config.field.channel_max + 1)}
|
config.field.channel_max + 1)}
|
||||||
|
|
||||||
def update_measure(self, channel: int, position, result: str,
|
@property
|
||||||
svd_deg: float | None) -> None:
|
def effective_half_angle_deg(self) -> float:
|
||||||
"""按读数收敛候选集合。未实现:需要几何模块的在线圆盘角域交。"""
|
"""示向度误差半角 + 舍入裕量:服务器保留两位小数引入 ≤0.005° 偏差。"""
|
||||||
raise NotImplementedError("P3:接入 geometry 的圆盘约束角域交后实现")
|
return (self.config.field.bearing_error_half_deg
|
||||||
|
+ self.config.search.bearing_rounding_margin_deg)
|
||||||
|
|
||||||
def candidate_set(self, channel: int):
|
def update_measure(self, channel: int, position: Vec2, result: str,
|
||||||
"""返回该频道当前候选集合(保守外包形式)。未实现。"""
|
svd_deg: float | None) -> None:
|
||||||
raise NotImplementedError("P3")
|
"""按读数收敛候选集合;真源保留性由保守表示保证。"""
|
||||||
|
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:
|
def mark_cleared(self, channel: int) -> None:
|
||||||
self.channels[channel].status = ChannelStatus.CLEARED
|
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)
|
||||||
|
|||||||
+52
-5
@@ -2,7 +2,8 @@
|
|||||||
|
|
||||||
- q1 问题一角域交会求解(P2 已完工)
|
- q1 问题一角域交会求解(P2 已完工)
|
||||||
- sim-check 用自建模拟器复现附件计时示例(P0 自检)
|
- sim-check 用自建模拟器复现附件计时示例(P0 自检)
|
||||||
- run 全向闭环(P4 骨架,未实现)
|
- run 闭环:发现—定位—清除—退出(--problem 3 全向 / 4 混合源;
|
||||||
|
--self-test 走本地模拟器)
|
||||||
"""
|
"""
|
||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
@@ -10,12 +11,17 @@ from __future__ import annotations
|
|||||||
import argparse
|
import argparse
|
||||||
import json
|
import json
|
||||||
import sys
|
import sys
|
||||||
|
from dataclasses import replace
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
from .config import ProblemConfig
|
from .config import ProblemConfig
|
||||||
from .geometry.__main__ import main as q1_main
|
from .geometry.__main__ import main as q1_main
|
||||||
|
from .planner import DirectionalExtension, OmnidirectionalLoop
|
||||||
from .protocol.client import SimulatorClient
|
from .protocol.client import SimulatorClient
|
||||||
|
from .protocol.log import SessionLog
|
||||||
from .protocol.rules import EXIT_USER
|
from .protocol.rules import EXIT_USER
|
||||||
from .sim import LocalSimTransport, LocalSimulator, Source
|
from .sim import LocalSimTransport, LocalSimulator, Source
|
||||||
|
from .sim.scenes import mixed_scene, random_omni_scene
|
||||||
|
|
||||||
|
|
||||||
def _sim_check(args) -> int:
|
def _sim_check(args) -> int:
|
||||||
@@ -40,8 +46,36 @@ def _sim_check(args) -> int:
|
|||||||
|
|
||||||
|
|
||||||
def _run(args) -> int:
|
def _run(args) -> int:
|
||||||
raise NotImplementedError(
|
"""问题3/4闭环。--self-test 时真值只存在于测试端入口,策略端仅见响应。"""
|
||||||
"P4 全向闭环尚未实现;当前可用子命令:q1(问题一求解)、sim-check(协议自检)")
|
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:
|
def main(argv=None) -> int:
|
||||||
@@ -62,8 +96,21 @@ def main(argv=None) -> int:
|
|||||||
check.add_argument("--robot-id", default="2026001")
|
check.add_argument("--robot-id", default="2026001")
|
||||||
check.set_defaults(func=_sim_check)
|
check.set_defaults(func=_sim_check)
|
||||||
|
|
||||||
run = subparsers.add_parser("run", help="P4 全向闭环(骨架)")
|
run = subparsers.add_parser("run", help="闭环:发现—定位—清除—退出(问题3/4)")
|
||||||
run.add_argument("--robot-id", required=True)
|
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)
|
run.set_defaults(func=_run)
|
||||||
|
|
||||||
args = parser.parse_args(argv)
|
args = parser.parse_args(argv)
|
||||||
|
|||||||
@@ -73,6 +73,17 @@ class SearchConfig:
|
|||||||
grid_half_cells: int = 3 # i,j ∈ [-3,3],49 点覆盖 [-2100,2100]^2 ⊇ Ω
|
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
|
fallback_clear_grid_m: float = 20.0 # P5 后备清除网格;10*sqrt(2)<=20
|
||||||
bearing_rounding_margin_deg: float = 0.0 # 默认不加舍入裕量;敏感性分析时显式设 0.005
|
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)
|
@dataclass(frozen=True)
|
||||||
|
|||||||
+508
-49
@@ -1,80 +1,539 @@
|
|||||||
"""P3/P4/P5 策略层:主动选点、全向闭环与定向扩展。
|
"""P3/P4/P5 策略层:主动选点、全向闭环与定向扩展。
|
||||||
|
|
||||||
现状:骨架。各入口先固定签名与验收标准,逐阶段填充实现。
|
|
||||||
|
|
||||||
P3 第二检测点(验收:保证收到信号;Q 空时退化;near 分支):
|
P3 第二检测点(验收:保证收到信号;Q 空时退化;near 分支):
|
||||||
- K1 = Ω ∩ W(s1, θ1±δ) ∩ B(s1, 1500);安全候选盘证书见
|
- K1 = Ω ∩ W(s1, θ1±δ) ∩ B(s1, 1500);安全候选盘证书见
|
||||||
geometry.planning.first_bearing_candidate_disk(δ<=1.005° 时非空);
|
geometry.planning.first_bearing_candidate_disk(δ<=1.005° 时非空);
|
||||||
- 在证书圆盘内围绕测向轴两侧采样候选点,以"最坏后验包围圆半径"
|
- 在保证接收盘 B(mec_center, 1000-ρ) 内环形采样候选点,以
|
||||||
为主指标、移动+检测代价为次指标;
|
"最坏后验包围圆半径"为主指标、移动代价为次指标;
|
||||||
|
- 采样是启发式(目标边界点 × 读数区间端点),不是严格最坏情形
|
||||||
|
最优解;选定后按 1/4 间距加密复核并如实上报数值;
|
||||||
- 返回 near 时:候选源在 5m 内,直接对当前频道尝试 /clear。
|
- 返回 near 时:候选源在 5m 内,直接对当前频道尝试 /clear。
|
||||||
|
|
||||||
P4 全向闭环(验收:10/16 源正确退出;边界朝外不漏;停止条件正确):
|
P4 全向闭环(验收:10/16 源正确退出;边界朝外不漏;停止条件正确):
|
||||||
发现(coverage.CoverageLedger)→ 定位(belief 收敛)→ 清除 → 停止判定
|
发现(coverage.CoverageLedger)→ 定位(belief 收敛)→ 清除 → 停止判定
|
||||||
(清除数达 16,或每频道均有清除成功/不存在证明)→ /exit。
|
(清除数达 16,或每频道均有清除成功/不存在证明)→ /exit。
|
||||||
发现 10 源、连续无信号、候选采样为空都不是停止条件。
|
发现 10 源、连续无信号、候选采样为空都不是停止条件;
|
||||||
|
现实预算耗尽退出记为未证明完成(completed=false),不与成功混记。
|
||||||
|
|
||||||
P5 定向扩展(验收:朝外边界源能发现;失去信号能清除;不误删真源):
|
P5 定向扩展(问题4混合源闭环):
|
||||||
no_signal 是"位置×朝向"联合约束,基线不据此删空间候选;
|
- 49 点覆盖的发现保证对定向源同样成立(源在单元凸包内,任意发射半平面
|
||||||
失去信号后对有界候选做 <=20m 有限圆盘覆盖逐点 /clear,
|
含至少一个顶点),覆盖阶段不区分源类型;
|
||||||
并按预算上界(格数×请求耗时)决定是否执行。
|
- 保证接收盘内的 no_signal 只能由朝向遮挡解释 [观测推理]:频道记为
|
||||||
|
"观测到定向",对跖探点复核一次;联合约束基线不删空间候选(真源不误删);
|
||||||
|
- 定位停滞时对有界候选集构造 <=20m 有限圆盘覆盖(fallback_clear_cells),
|
||||||
|
预算核对后逐格 /clear(清除与朝向无关);格数超硬顶或预算不足即跳过,
|
||||||
|
绝不盲扫。
|
||||||
"""
|
"""
|
||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import math
|
||||||
from dataclasses import dataclass
|
from dataclasses import dataclass
|
||||||
|
|
||||||
from ..config import ProblemConfig
|
from ..belief import (BeliefState, CandidateRegion, ChannelStatus,
|
||||||
from ..protocol.client import SimulatorClient
|
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:
|
class SecondPointPlan:
|
||||||
"""第二检测点的候选与依据(P3 输出形态预览)。"""
|
"""下一检测点的候选与依据(P3 输出)。"""
|
||||||
|
|
||||||
position: tuple[float, float]
|
position: Vec2
|
||||||
worst_posterior_radius_m: float
|
worst_posterior_radius_m: float
|
||||||
move_cost_s: float
|
move_cost_s: float
|
||||||
reason: str
|
reason: str
|
||||||
|
|
||||||
|
|
||||||
def select_second_point(client: SimulatorClient,
|
@dataclass(frozen=True)
|
||||||
first_position, first_bearing_deg: float,
|
class ClearCertificate:
|
||||||
config: ProblemConfig = ProblemConfig()):
|
"""清除证书:候选集外包半径(含近似误差)不超过 20m 阈值。"""
|
||||||
"""在安全候选盘内选择第二检测点。未实现:P3。"""
|
|
||||||
raise NotImplementedError("P3:先实现 belief 候选集,再实现采样评估")
|
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:
|
class OmnidirectionalLoop:
|
||||||
"""问题3全向"发现—定位—清除—停止"闭环。未实现:P4。"""
|
"""问题3全向"发现—定位—清除—停止"闭环(P4 基线)。
|
||||||
|
|
||||||
def __init__(self, client: SimulatorClient,
|
串行驱动 SimulatorClient:覆盖阶段在 49 点网格上对每个 UNKNOWN 频道
|
||||||
config: ProblemConfig = ProblemConfig()):
|
检测;发现 direction 即进入该频道的定位子循环(选点→测量→收敛→
|
||||||
self.client = client
|
证书清除),near 直接清除。停止条件只有三种:清除数达 16(数量上界
|
||||||
self.config = config
|
证明)、每频道均 CLEARED/ABSENT、现实预算耗尽(记为未完成)。
|
||||||
|
|
||||||
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(先由演练实测请求预算决定启用规模)。
|
|
||||||
"""
|
"""
|
||||||
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
|
||||||
|
|||||||
@@ -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
|
||||||
@@ -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()
|
||||||
@@ -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()
|
||||||
@@ -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()
|
||||||
@@ -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()
|
||||||
Reference in New Issue
Block a user