fix(autocal): 校验运动单位并等待命令成功后判断到位

阻止单位不一致的姿态命令,避免提前到达的位置快照误报成功或推进下一轴。
相关检查 164 项通过,2064 组目标及 8256 条运动调用与原基线完全一致。
This commit is contained in:
ykcheungab
2026-10-18 20:24:45 +08:00
parent 9352d0c816
commit cf3e638176
8 changed files with 259 additions and 56 deletions
+13 -1
View File
@@ -54,11 +54,13 @@ def make_service(**kwargs) -> MotionService:
)
service._spec = object() # noqa: SLF001 - connected, without opening anything
service._bus = BusStatus(bus_type="ecat", init_status=1, total_axes=5) # noqa: SLF001
# Physical-unit planning requires observed units, just as the live poll supplies.
service._last_axes = {axis: service._read_axis(axis) for axis in range(5)}
return service
class FakePoller:
"""Records submissions and runs them, so a plan's commands can be inspected."""
"""Records submissions and explicitly acknowledges simulated worker results."""
def __init__(self, service: MotionService) -> None:
self.service = service
@@ -71,6 +73,9 @@ class FakePoller:
# this test exists to check.
return len(self.commands)
def acknowledge(self, mover: PoseMover) -> None:
mover.on_command_finished(len(self.commands), MotionResult.success())
def reading(axis: int, dpos: float, *, idle: bool = True, alarm: bool = False) -> AxisState:
return AxisState(
@@ -284,6 +289,7 @@ def test_an_execute_plan_moves_the_four_axes_together_and_leaves_z():
assert len(poller.commands) == 1
poller.commands[0](service)
assert [axis for axis, _value in called[0]] == [3, 2, 0, 1]
poller.acknowledge(mover)
mover.on_snapshot(
snapshot_of(service, {0: 1000.0, 1: 0.0, 2: 0.0, 3: 0.0}, idle=False)
@@ -332,11 +338,13 @@ def test_the_mover_commands_one_axis_and_waits_for_it_to_arrive():
plan = plan_pose_move(service, {"x": 2.0, "y": 1.0})
assert mover.start(plan).ok
assert len(poller.commands) == 1, "more than the first axis was commanded at once"
poller.acknowledge(mover)
# x is at its target and idle -> the second axis goes out.
mover.on_snapshot(snapshot_of(service, {0: 1000.0, 1: 0.0}))
assert len(poller.commands) == 2
assert mover.state.step.role == "y"
poller.acknowledge(mover)
# y arrives -> the plan is done.
mover.on_snapshot(snapshot_of(service, {0: 1000.0, 1: 500.0}))
@@ -349,6 +357,7 @@ def test_the_mover_waits_while_an_axis_is_still_moving():
poller = FakePoller(service)
mover = PoseMover(service, poller.submit)
mover.start(plan_pose_move(service, {"x": 2.0, "y": 1.0}))
poller.acknowledge(mover)
mover.on_snapshot(snapshot_of(service, {0: 990.0, 1: 0.0}, idle=False))
assert len(poller.commands) == 1
assert mover.state.index == 0
@@ -370,6 +379,7 @@ def test_a_long_yaw_is_not_cut_off_while_it_is_still_moving(monkeypatch):
poller = FakePoller(service)
mover = PoseMover(service, poller.submit)
assert mover.start(plan_pose_move(service, {"yaw": 180.0})).ok
poller.acknowledge(mover)
clock["t"] += 120.0
mover.on_snapshot(snapshot_of(service, {3: 1_000.0}, idle=False))
assert mover.running
@@ -383,6 +393,7 @@ def test_the_mover_fails_when_an_axis_stops_short():
poller = FakePoller(service)
mover = PoseMover(service, poller.submit)
mover.start(plan_pose_move(service, {"x": 2.0, "y": 1.0}))
poller.acknowledge(mover)
mover.on_snapshot(snapshot_of(service, {0: 10.0, 1: 0.0}))
assert mover.running, "gave up before the stall grace elapsed"
mover.state._idle_since = time.monotonic() - 10.0 # noqa: SLF001
@@ -408,6 +419,7 @@ def test_a_tolerance_accepts_an_axis_that_reports_its_final_position_late():
mover = PoseMover(service, poller.submit)
plan = plan_pose_move(service, {"x": 2.0, "y": 1.0})
mover.start(plan)
poller.acknowledge(mover)
# 1 unit inside SETTLE_MM of 1000 units for 2 mm.
mover.on_snapshot(snapshot_of(service, {0: 1000.0 - 1.0, 1: 0.0}))
assert len(poller.commands) == 2, "a step within tolerance was treated as a miss"