1
This commit is contained in:
+245
-18
@@ -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)
|
||||
|
||||
+52
-5
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
|
||||
+508
-49
@@ -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
|
||||
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user