143 lines
6.7 KiB
Python
143 lines
6.7 KiB
Python
"""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 (_worst_posterior_radius, bearing_deg,
|
||
clear_certificate, select_measurement_point,
|
||
select_second_point)
|
||
from mcm26b.protocol.client import SimulatorClient
|
||
from mcm26b.harness 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_gets_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_posterior_scores_near_branch(self):
|
||
"""后验评分必须包含 near 分支，否则低报最坏后验半径。
|
||
|
||
候选集为线段 [-4,4]×{0}、测点为原点时，任意合法读数必为 near
|
||
（源距原点 <=4 < 5m），后验仍为整段线段（半径 4m）；只枚举
|
||
direction 端点会错误报出 2m。
|
||
"""
|
||
region = CandidateRegion([(-4.0, 0.0), (4.0, 0.0)])
|
||
half = 1.005
|
||
witnesses = region.witnesses(60.0)
|
||
self.assertEqual(witnesses, [(-4.0, 0.0), (4.0, 0.0)])
|
||
worst = _worst_posterior_radius(region, (0.0, 0.0), witnesses, half,
|
||
CONFIG.field.near_radius_m,
|
||
CONFIG.search.polygon_sides)
|
||
self.assertGreaterEqual(worst, 4.0 - 1e-6,
|
||
"near 分支后验半径应逼近候选集半径 4m")
|
||
self.assertLessEqual(worst, 4.0 + region.slack_m + 0.1)
|
||
|
||
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_in_analytic_safe_disk_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()
|