fix(autocal): 校验运动单位并等待命令成功后判断到位
阻止单位不一致的姿态命令,避免提前到达的位置快照误报成功或推进下一轴。 相关检查 164 项通过,2064 组目标及 8256 条运动调用与原基线完全一致。
This commit is contained in:
@@ -12,7 +12,7 @@
|
||||
|
||||
[`PoseMover`](../src/orisys_autocal/motion/pose_move.py) 启动前检查计划中的轴编号、物理目标和已换算的用户单位目标仍对应当前配置。工作线程执行前再次读各目标轴的 `UNITS`,全部校验完成后才按原顺序发送原绝对目标。配置或角色映射在排队期间改变时,旧计划被拒绝,要求重新规划;不会偷偷套用新设置。
|
||||
|
||||
移动器按命令 token 接收工作线程的拒绝或失败,立即标记该次移动失败,不再靠后续位置恰好相同判定到位。其他命令的结果不会取消当前计划;界面继续使用已有的移动失败处理和停止路径。
|
||||
移动器按命令 token 等待工作线程确认成功,然后才用随后收到的位置快照判断到位或推进下一轴;等待期间仍检查报警及读取失败。提前收到的位置快照不缓存重放。对应命令被拒绝或发生异常时立即标记失败,其他命令的结果不解除等待;界面继续使用已有的移动失败处理和停止路径。
|
||||
|
||||
执行前增加的是只读参数检查,可能增加通信延迟,不保证与旧版完全相同的发送时刻。控制器被其他软件在最终读取和运动指令之间改动的并发窗口,本次也不能消除;操作期间仍需保证设备配置的单一写入者。
|
||||
|
||||
@@ -29,8 +29,8 @@
|
||||
|
||||
## 验证与限制
|
||||
|
||||
回归测试见 [`test_motion_unit_guards.py`](../tests/test_motion_unit_guards.py)。它覆盖既有五轴命令的确定数值和顺序、配置一致的线性换算、异常读回、旋转标定条件、排队期间配置改变、读取失败时整组零发送,以及原始单位调试移动的兼容性。现有测试夹具补充了真实流程所需的单位观测,没有降低断言或放宽限位。
|
||||
回归测试见 [`test_motion_unit_guards.py`](../tests/test_motion_unit_guards.py)。它覆盖既有五轴命令的确定数值和顺序、配置一致的线性换算、异常读回、旋转标定条件、排队期间配置改变、读取失败时整组零发送,以及原始单位调试移动的兼容性。另覆盖分步/整组的快照先于命令结果、拒绝及异常、token 匹配、逐轴确认与等待期间故障。现有测试夹具补充真实流程所需的单位观测和成功回调,没有降低断言或放宽限位。
|
||||
|
||||
修改前从本地 design 导出保存 1032 顶点,使用默认安装与当前偏好配置,共 2064 组姿态和规划目标。修改后逐元素严格相等,最大差值为 0;使用修改前源码与当前源码在记录型 DemoTransport 上分别执行相同计划,产生的 8256 条绝对运动调用也完全一致。这验证软件输出保持,不构成实机轨迹或物理接触精度证明。
|
||||
|
||||
最终相关检查 136 项通过;Autocal 全套为 720 项通过、3 项失败,无跳过。两项模拟采集失败及一项旧 scratch 路径假设与原始审查记录一致,本轮没有修复。回归测试在修复前复现单位风险,移除当量一致性判断的内存变异也被检出。详细结果见[架构审查](../../../../docs/workflow/autocal-architecture-review.zh.md)。证据位于 app `.scratch/autocal-unit-guards/`,不属于长期归档。
|
||||
首次单位防护验证为 136 项通过,Autocal 全套为 720 项通过、3 项失败。后续审查发现位置快照可能先于命令结果,提前误报成功;2026-10-18 增加上述确认门槛后,相关检查 164 项通过,原实现被新增测试检出 7 项失败。全套结果及限制见[架构审查](../../../../docs/workflow/autocal-architecture-review.zh.md)。证据位于 app `.scratch/autocal-unit-guards/` 与 `.scratch/autocal-command-ack/`,不属于长期归档。
|
||||
|
||||
@@ -31,11 +31,10 @@ import time
|
||||
from collections.abc import Mapping, Sequence
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
from PySide6.QtCore import QObject, Signal
|
||||
from PySide6.QtCore import QObject, Signal, Slot
|
||||
|
||||
from .models import MotionResult, Snapshot
|
||||
from .service import LINEAR_ROLES, MotionService
|
||||
from .units import user_units_to_deg
|
||||
|
||||
#: How close an axis must be to its target before the next one starts, per unit:
|
||||
#: millimetres for the linear roles, degrees for the rotary ones. One user unit is
|
||||
@@ -141,6 +140,33 @@ class MoveStep:
|
||||
return f"{self.role} {self.target:+.4f} {self.unit}"
|
||||
|
||||
|
||||
def _check_steps(
|
||||
service: MotionService, steps: Sequence[MoveStep], *, refresh_units: bool = False
|
||||
) -> MotionResult | None:
|
||||
"""Reject stale axis/scale bindings; never reinterpret an existing plan."""
|
||||
refusal = service.check_axes_move(
|
||||
{step.role: step.target for step in steps}, refresh_units=refresh_units
|
||||
)
|
||||
if refusal is not None:
|
||||
return refusal
|
||||
for step in steps:
|
||||
if step.axis != service.role_axis(step.role) or step.units != service.to_units(
|
||||
step.role, step.target
|
||||
):
|
||||
return MotionResult.refused(
|
||||
f"{step.role} 的轴映射或单位已改变,请重新规划 / axis mapping or units changed; replan"
|
||||
)
|
||||
return None
|
||||
|
||||
|
||||
def _execute_steps(service: MotionService, steps: Sequence[MoveStep]) -> MotionResult:
|
||||
"""Worker-only read check, then the original absolute targets in their order."""
|
||||
refusal = _check_steps(service, steps, refresh_units=True)
|
||||
if refusal is not None:
|
||||
return refusal
|
||||
return service.move_abs_many(tuple((step.axis, step.units) for step in steps))
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MovePlan:
|
||||
"""A sequence of steps, and what it is for."""
|
||||
@@ -254,6 +280,7 @@ class PoseMover(QObject):
|
||||
self._service = service
|
||||
self._submit = submit
|
||||
self.state = MoveState()
|
||||
self._command_token: int | None = None
|
||||
|
||||
# ------------------------------------------------------------------ control
|
||||
|
||||
@@ -271,9 +298,7 @@ class PoseMover(QObject):
|
||||
"""Validate and begin. Refusal is returned, not raised."""
|
||||
if self.running:
|
||||
return MotionResult.refused("已有运动在执行 / a move is already running")
|
||||
refusal = self._service.check_axes_move(
|
||||
{step.role: step.target for step in plan.steps}
|
||||
)
|
||||
refusal = _check_steps(self._service, plan.steps)
|
||||
if refusal is not None:
|
||||
return refusal
|
||||
if not plan.steps:
|
||||
@@ -312,19 +337,21 @@ class PoseMover(QObject):
|
||||
if not step.units == step.units: # NaN: no calibration for this role
|
||||
self._fail(f"{step.role} 无法换算成用户单位 / cannot convert {step.role}")
|
||||
return
|
||||
self._submit(lambda service, step=step: service.move_abs(step.axis, step.units))
|
||||
self._command_token = self._submit(
|
||||
lambda service, step=step: _execute_steps(service, (step,))
|
||||
)
|
||||
self.stepStarted.emit(step)
|
||||
|
||||
def _issue_together(self, plan: MovePlan) -> None:
|
||||
"""Command every step in one worker call."""
|
||||
moves: list[tuple[int, float]] = []
|
||||
for step in plan.steps:
|
||||
if not step.units == step.units:
|
||||
self._fail(f"{step.role} 无法换算成用户单位 / cannot convert {step.role}")
|
||||
return
|
||||
moves.append((step.axis, step.units))
|
||||
self.state._group_idle = {}
|
||||
self._submit(lambda service, moves=tuple(moves): service.move_abs_many(moves))
|
||||
self._command_token = self._submit(
|
||||
lambda service, steps=plan.steps: _execute_steps(service, steps)
|
||||
)
|
||||
if plan.steps:
|
||||
self.stepStarted.emit(plan.steps[0])
|
||||
|
||||
@@ -339,6 +366,20 @@ class PoseMover(QObject):
|
||||
self.state.plan = None
|
||||
self.failed.emit(message)
|
||||
|
||||
@Slot(int, object)
|
||||
def on_command_finished(self, token: int, result: object) -> None:
|
||||
"""A rejected queued move must not later be reported as an arrival."""
|
||||
if self.running and token == self._command_token:
|
||||
self._command_token = None
|
||||
if isinstance(result, MotionResult) and not result.ok:
|
||||
self._fail(result.message)
|
||||
|
||||
@Slot(int, str)
|
||||
def on_command_failed(self, token: int, message: str) -> None:
|
||||
if self.running and token == self._command_token:
|
||||
self._command_token = None
|
||||
self._fail(message)
|
||||
|
||||
# ---------------------------------------------------------------- progress
|
||||
|
||||
def on_snapshot(self, snapshot: Snapshot) -> None:
|
||||
@@ -364,12 +405,11 @@ class PoseMover(QObject):
|
||||
self._fail(f"{step.role} 报警 / alarm on {step.role}")
|
||||
return
|
||||
|
||||
# A linear role reads in millimetres, a rotary one in degrees; the service
|
||||
# converts lengths, the units module converts calibrated angles.
|
||||
# Arrival uses the same verified units as the planned physical target.
|
||||
if step.unit == "mm":
|
||||
travelled = self._service.to_mm(step.axis, state.dpos)
|
||||
else:
|
||||
travelled = user_units_to_deg(step.axis, state.dpos)
|
||||
travelled = self._service.to_deg(step.axis, state.dpos)
|
||||
tolerance = SETTLE_MM if step.unit == "mm" else SETTLE_DEG
|
||||
|
||||
if not state.idle:
|
||||
@@ -410,7 +450,7 @@ class PoseMover(QObject):
|
||||
if step.unit == "mm":
|
||||
travelled = self._service.to_mm(step.axis, state.dpos)
|
||||
else:
|
||||
travelled = user_units_to_deg(step.axis, state.dpos)
|
||||
travelled = self._service.to_deg(step.axis, state.dpos)
|
||||
tolerance = SETTLE_MM if step.unit == "mm" else SETTLE_DEG
|
||||
arrived = state.idle and abs(travelled - step.target) <= tolerance
|
||||
if arrived:
|
||||
|
||||
@@ -43,6 +43,7 @@ from .units import (
|
||||
mm_to_user_units,
|
||||
safe_float,
|
||||
units_per_degree,
|
||||
user_units_to_deg,
|
||||
user_units_to_mm,
|
||||
)
|
||||
|
||||
@@ -175,6 +176,7 @@ class MotionService:
|
||||
self._limits: dict[str, float] = dict(limits or {})
|
||||
|
||||
self._spec: ConnectionSpec | None = None
|
||||
self._last_axes: dict[int, AxisState] = {}
|
||||
self._bus = BusStatus()
|
||||
self._params: dict[int, AxisParams] = {
|
||||
axis: AxisParams() for axis in range(self.axis_count)
|
||||
@@ -273,25 +275,62 @@ class MotionService:
|
||||
key = "max_travel_mm" if role in LINEAR_ROLES else f"max_{role}_deg"
|
||||
return safe_float(self._limits.get(key), 0.0)
|
||||
|
||||
def units_problem(self, role: str, axis: int) -> str:
|
||||
"""Why a role cannot be commanded in its own unit, or ``""``."""
|
||||
if role in LINEAR_ROLES:
|
||||
return ""
|
||||
scale = units_per_degree(axis)
|
||||
if not math.isfinite(scale) or scale <= 0:
|
||||
def units_problem(self, role: str, axis: int, *, refresh: bool = False) -> str:
|
||||
"""Validate expected versus observed units, without changing either.
|
||||
|
||||
``refresh`` is only for the controller worker immediately before execution.
|
||||
UI/planning checks use the last poll and never call the transport.
|
||||
"""
|
||||
expected = self.params(axis).units
|
||||
if not math.isfinite(expected) or expected <= 0:
|
||||
return self._text(
|
||||
f"{role} 用轴 {axis},但该轴没有角度标定(用户单位/度)",
|
||||
f"{role} uses axis {axis}, which has no user-units-per-degree "
|
||||
f"calibration",
|
||||
f"轴 {axis} 的 UNITS 无效", f"Axis {axis} has invalid expected UNITS"
|
||||
)
|
||||
if role not in LINEAR_ROLES and not math.isfinite(
|
||||
units_per_degree(axis, units=expected)
|
||||
):
|
||||
return self._text(
|
||||
f"{role} 用轴 {axis},但该轴没有当前 UNITS={expected:g} 对应的角度标定",
|
||||
f"{role} uses axis {axis}, which has no rotary calibration for UNITS={expected:g}",
|
||||
)
|
||||
if refresh:
|
||||
try:
|
||||
observed = self._transport.get_units(axis)
|
||||
except (ZAuxError, OSError) as exc:
|
||||
return self._text(
|
||||
f"无法读取轴 {axis} 的 UNITS:{exc}",
|
||||
f"Cannot read axis {axis} UNITS: {exc}",
|
||||
)
|
||||
else:
|
||||
state = self._last_axes.get(axis)
|
||||
if state is None or not state.read_ok:
|
||||
return self._text(
|
||||
f"轴 {axis} 的 UNITS 尚未成功读回",
|
||||
f"Axis {axis} UNITS has no successful readback",
|
||||
)
|
||||
observed = state.units
|
||||
if (
|
||||
not math.isfinite(observed)
|
||||
or observed <= 0
|
||||
or not math.isclose(expected, observed, rel_tol=1e-7, abs_tol=0.0)
|
||||
):
|
||||
return self._text(
|
||||
f"轴 {axis} 的 UNITS 不一致:配置 {expected:g},读回 {observed:g};未重新换算运动目标",
|
||||
f"Axis {axis} UNITS mismatch: expected {expected:g}, read {observed:g}; motion was not rescaled",
|
||||
)
|
||||
return ""
|
||||
|
||||
def check_axes_move(self, targets: Mapping[str, float]) -> MotionResult | None:
|
||||
def check_axes_move(
|
||||
self, targets: Mapping[str, float], *, refresh_units: bool = False
|
||||
) -> MotionResult | None:
|
||||
"""Refuse a coordinated pose move, or return ``None`` to allow it.
|
||||
|
||||
``targets`` is per role, in the role's own unit: millimetres for x / y / z,
|
||||
degrees for yaw / pitch.
|
||||
|
||||
``refresh_units`` reads the current pulse equivalents and is worker-only.
|
||||
It never writes them or rescales any already planned target.
|
||||
|
||||
Every reason a move can be wrong is checked *before* the first axis is
|
||||
commanded. A pose move is several coordinated moves, and discovering at axis
|
||||
three that axis four is out of range would leave the machine stopped
|
||||
@@ -321,7 +360,7 @@ class MotionService:
|
||||
self._text(f"{role} 目标不是有效数字", f"{role} target is not a number")
|
||||
)
|
||||
axis = self._axis_roles[role]
|
||||
units_problem = self.units_problem(role, axis)
|
||||
units_problem = self.units_problem(role, axis, refresh=refresh_units)
|
||||
if units_problem:
|
||||
return MotionResult.refused(units_problem)
|
||||
for role, value in targets.items():
|
||||
@@ -350,11 +389,11 @@ class MotionService:
|
||||
if axis is None:
|
||||
return math.nan
|
||||
if role in LINEAR_ROLES:
|
||||
units = self.params(axis).units or self._units_per_mm
|
||||
units = self.params(axis).units
|
||||
return mm_to_user_units(
|
||||
value, units=units, pulses_per_mm=self._pulses_per_mm
|
||||
)
|
||||
return deg_to_user_units(axis, value)
|
||||
return deg_to_user_units(axis, value, units=self.params(axis).units)
|
||||
|
||||
def set_language(self, language: str) -> None:
|
||||
self.language = language
|
||||
@@ -399,6 +438,7 @@ class MotionService:
|
||||
return MotionResult.failed(code, message)
|
||||
|
||||
self._spec = spec
|
||||
self._last_axes.clear()
|
||||
# A controller may already have a BAS program running from a previous
|
||||
# session; adopt it rather than claiming the bus is uninitialised. The
|
||||
# adopted state is *not* marked verified: its globals are a latch, and
|
||||
@@ -610,6 +650,7 @@ class MotionService:
|
||||
)
|
||||
)
|
||||
|
||||
self._last_axes.update({state.axis: state for state in states})
|
||||
return Snapshot(
|
||||
connected=True,
|
||||
spec=self._spec,
|
||||
@@ -724,7 +765,6 @@ class MotionService:
|
||||
home_status = 0
|
||||
self._home_done[axis] = home_status == 1
|
||||
|
||||
params = self.params(axis)
|
||||
return AxisState(
|
||||
axis=axis,
|
||||
enabled=transport.get_axis_enable(axis) != 0,
|
||||
@@ -748,6 +788,8 @@ class MotionService:
|
||||
return MotionResult.refused(self._text("未连接控制器", "Not connected"))
|
||||
if not self._known_axis(axis):
|
||||
return MotionResult.refused(self._axis_range_message(axis))
|
||||
# A partial write may already change UNITS even if speed/accel then fails.
|
||||
self._last_axes.pop(axis, None)
|
||||
try:
|
||||
self._transport.set_units(axis, params.units)
|
||||
self._transport.set_speed(axis, params.speed)
|
||||
@@ -979,7 +1021,7 @@ class MotionService:
|
||||
return MotionResult.success(self._text("回零已启动", "Homing started"))
|
||||
|
||||
def _log_axis_readback(self, action: str) -> None:
|
||||
"""Read every axis and log it. Does not write parameters or start motion."""
|
||||
"""Log the last poll without issuing extra controller reads."""
|
||||
for axis in range(self.axis_count):
|
||||
self._log_command(action, axis, MotionResult.success("read"))
|
||||
|
||||
@@ -1000,23 +1042,17 @@ class MotionService:
|
||||
def _position_text(self, axis: int) -> str:
|
||||
"""Commanded and measured position, for the command log.
|
||||
|
||||
Reads only. A failed read is part of the line rather than a second error,
|
||||
because the command result itself is what the caller is waiting on.
|
||||
These are cached poll values, not a synchronous post-command read. Logging
|
||||
must not access the controller from the calling thread.
|
||||
"""
|
||||
if not self.connected:
|
||||
state = self._last_axes.get(axis)
|
||||
if not self.connected or state is None or not state.read_ok:
|
||||
return "dpos=unread mpos=unread status=unread enable=unread"
|
||||
try:
|
||||
return (
|
||||
f"dpos={self._transport.get_dpos(axis)} "
|
||||
f"mpos={self._transport.get_mpos(axis)} "
|
||||
f"status={self._transport.get_axis_status(axis)} "
|
||||
f"enable={self._transport.get_axis_enable(axis)} "
|
||||
f"atype={self._transport.get_atype(axis)} "
|
||||
f"units={self._transport.get_units(axis)} "
|
||||
f"speed={self._transport.get_speed(axis)}"
|
||||
)
|
||||
except (ZAuxError, OSError) as exc:
|
||||
return f"dpos=unread mpos=unread ({exc})"
|
||||
return (
|
||||
f"last_poll dpos={state.dpos} mpos={state.mpos} "
|
||||
f"status={state.status.raw} enable={state.enabled} "
|
||||
f"atype={state.atype} units={state.units} speed={state.speed}"
|
||||
)
|
||||
|
||||
def _known_axis(self, axis: int) -> bool:
|
||||
return 0 <= int(axis) < self.axis_count
|
||||
@@ -1099,11 +1135,19 @@ class MotionService:
|
||||
|
||||
def to_mm(self, axis: int, user_units: float) -> float:
|
||||
"""Displayed millimetres for a reading. Derived, never a measurement."""
|
||||
units = self.params(axis).units or self._units_per_mm
|
||||
if self.units_problem("x", axis):
|
||||
return math.nan
|
||||
units = self.params(axis).units
|
||||
return user_units_to_mm(
|
||||
user_units, units=units, pulses_per_mm=self._pulses_per_mm
|
||||
)
|
||||
|
||||
def to_deg(self, axis: int, user_units: float) -> float:
|
||||
"""Rotary readback in degrees, or NaN without matching calibrated units."""
|
||||
if self.units_problem("yaw", axis):
|
||||
return math.nan
|
||||
return user_units_to_deg(axis, user_units, units=self.params(axis).units)
|
||||
|
||||
|
||||
def _replace_bus_type(bus: BusStatus, bus_type: BusType) -> BusStatus:
|
||||
return BusStatus(
|
||||
|
||||
@@ -34,6 +34,10 @@ ROTARY_UNITS_PER_90_DEG: dict[int, float] = {
|
||||
3: 29500.0,
|
||||
}
|
||||
|
||||
#: Pulse equivalent at which both rotary user-unit scales were measured.
|
||||
#: Other equivalents require a new calibration; do not rescale commissioned moves.
|
||||
ROTARY_CALIBRATION_UNITS = 10.0
|
||||
|
||||
|
||||
def safe_float(text: object, fallback: float) -> float:
|
||||
"""Parse ``text`` as a finite float, falling back on anything unusable.
|
||||
@@ -80,25 +84,33 @@ def mm_to_user_units(
|
||||
return float(value) * float(pulses_per_mm) / float(units)
|
||||
|
||||
|
||||
def units_per_degree(axis: int) -> float:
|
||||
def units_per_degree(axis: int, *, units: float = ROTARY_CALIBRATION_UNITS) -> float:
|
||||
"""User units per degree for a rotary axis, or NaN if not calibrated."""
|
||||
per_90 = ROTARY_UNITS_PER_90_DEG.get(int(axis))
|
||||
if per_90 is None or per_90 <= 0:
|
||||
if (
|
||||
per_90 is None
|
||||
or per_90 <= 0
|
||||
or not math.isclose(units, ROTARY_CALIBRATION_UNITS, rel_tol=1e-7, abs_tol=0.0)
|
||||
):
|
||||
return math.nan
|
||||
return float(per_90) / 90.0
|
||||
|
||||
|
||||
def user_units_to_deg(axis: int, value: float) -> float:
|
||||
def user_units_to_deg(
|
||||
axis: int, value: float, *, units: float = ROTARY_CALIBRATION_UNITS
|
||||
) -> float:
|
||||
"""Convert controller user units to degrees for a calibrated rotary axis."""
|
||||
scale = units_per_degree(axis)
|
||||
scale = units_per_degree(axis, units=units)
|
||||
if not math.isfinite(scale) or scale == 0.0:
|
||||
return math.nan
|
||||
return float(value) / scale
|
||||
|
||||
|
||||
def deg_to_user_units(axis: int, degrees: float) -> float:
|
||||
def deg_to_user_units(
|
||||
axis: int, degrees: float, *, units: float = ROTARY_CALIBRATION_UNITS
|
||||
) -> float:
|
||||
"""Inverse of :func:`user_units_to_deg`."""
|
||||
scale = units_per_degree(axis)
|
||||
scale = units_per_degree(axis, units=units)
|
||||
if not math.isfinite(scale):
|
||||
return math.nan
|
||||
return float(degrees) * scale
|
||||
|
||||
@@ -857,6 +857,8 @@ class AutoCalMainWindow(QMainWindow):
|
||||
# hold the worker thread and a stop would have to wait for the move it is
|
||||
# meant to interrupt.
|
||||
self.mover = PoseMover(self.poller.service, self.poller.submit, self)
|
||||
self.poller.commandFinished.connect(self.mover.on_command_finished)
|
||||
self.poller.commandFailed.connect(self.mover.on_command_failed)
|
||||
self.mover.failed.connect(self._on_move_failed)
|
||||
self.mover.finished.connect(self._on_move_finished)
|
||||
self.mover.cancelled.connect(self._on_move_cancelled)
|
||||
@@ -912,7 +914,7 @@ class AutoCalMainWindow(QMainWindow):
|
||||
if state is None or not state.read_ok:
|
||||
return None
|
||||
if rotary:
|
||||
return user_units_to_deg(int(axis), state.dpos)
|
||||
return user_units_to_deg(int(axis), state.dpos, units=state.units)
|
||||
units = state.units or float(self.prefs.get("units_per_mm") or DEFAULT_UNITS)
|
||||
return user_units_to_mm(state.dpos, units=units)
|
||||
|
||||
|
||||
@@ -5,7 +5,15 @@ import math
|
||||
|
||||
import pytest
|
||||
|
||||
from orisys_autocal.motion.models import AxisParams, BusStatus, ConnectionSpec
|
||||
from orisys_autocal.motion.models import (
|
||||
AxisParams,
|
||||
AxisState,
|
||||
AxisStatusBits,
|
||||
BusStatus,
|
||||
ConnectionSpec,
|
||||
MotionResult,
|
||||
Snapshot,
|
||||
)
|
||||
from orisys_autocal.motion.pose_move import PoseMover, plan_pose_move
|
||||
from orisys_autocal.motion.service import MotionService
|
||||
from orisys_autocal.motion.transport import DemoTransport
|
||||
@@ -185,3 +193,86 @@ def test_raw_debug_moves_keep_the_existing_user_unit_contract():
|
||||
transport.set_units(2, 5.0)
|
||||
assert instance.move_abs(2, 90.0).ok
|
||||
assert transport.moves == [(2, 90.0)]
|
||||
|
||||
|
||||
@pytest.mark.parametrize("together", [False, True])
|
||||
@pytest.mark.parametrize("failure", ["refused", "exception"])
|
||||
def test_arrival_before_command_failure_never_reports_success(qt_app, together, failure):
|
||||
instance, transport = service()
|
||||
actions = []
|
||||
mover = PoseMover(instance, lambda action: actions.append(action) or len(actions))
|
||||
finished = []
|
||||
mover.finished.connect(finished.append)
|
||||
assert mover.start(plan_pose_move(instance, {"x": 0.0}, together=together)).ok
|
||||
mover.on_snapshot(Snapshot(axes=(instance._last_axes[0],)))
|
||||
assert not finished and mover.running
|
||||
|
||||
if failure == "refused":
|
||||
transport.set_units(0, 5.0)
|
||||
result = actions[0](instance)
|
||||
assert not result.ok and "UNITS" in result.message
|
||||
mover.on_command_finished(1, result)
|
||||
else:
|
||||
mover.on_command_failed(1, "read failed")
|
||||
assert mover.state.failure and not mover.state.finished
|
||||
assert not finished and not mover.running
|
||||
assert transport.moves == []
|
||||
|
||||
|
||||
@pytest.mark.parametrize("together", [False, True])
|
||||
def test_only_matching_success_and_a_later_snapshot_allow_arrival(qt_app, together):
|
||||
instance, _transport = service()
|
||||
actions = []
|
||||
mover = PoseMover(instance, lambda action: actions.append(action) or len(actions))
|
||||
assert mover.start(plan_pose_move(instance, {"x": 0.0}, together=together)).ok
|
||||
snapshot = Snapshot(axes=(instance._last_axes[0],))
|
||||
mover.on_command_finished(99, MotionResult.success())
|
||||
mover.on_snapshot(snapshot)
|
||||
assert mover.running and not mover.state.finished
|
||||
result = actions[0](instance)
|
||||
assert result.ok
|
||||
mover.on_command_finished(1, result)
|
||||
assert mover.running, "a pre-acknowledgement snapshot was reused"
|
||||
mover.on_snapshot(Snapshot(axes=(instance._last_axes[0],)))
|
||||
assert mover.state.finished and not mover.running
|
||||
|
||||
|
||||
def test_each_sequential_axis_waits_for_its_own_command_success(qt_app):
|
||||
instance, _transport = service()
|
||||
actions = []
|
||||
mover = PoseMover(instance, lambda action: actions.append(action) or len(actions))
|
||||
assert mover.start(plan_pose_move(instance, {"x": 0.0, "y": 0.0})).ok
|
||||
snapshot = Snapshot(axes=tuple(instance._last_axes[axis] for axis in (0, 1)))
|
||||
mover.on_snapshot(snapshot)
|
||||
assert len(actions) == 1 and mover.state.index == 0
|
||||
result = actions[0](instance)
|
||||
assert result.ok
|
||||
mover.on_command_finished(1, result)
|
||||
mover.on_snapshot(snapshot)
|
||||
assert len(actions) == 2 and mover.state.index == 1
|
||||
mover.on_command_finished(1, MotionResult.success())
|
||||
mover.on_snapshot(snapshot)
|
||||
assert mover.running and not mover.state.finished
|
||||
result = actions[1](instance)
|
||||
assert result.ok
|
||||
mover.on_command_finished(2, result)
|
||||
mover.on_snapshot(snapshot)
|
||||
assert mover.state.finished
|
||||
|
||||
|
||||
@pytest.mark.parametrize("together", [False, True])
|
||||
@pytest.mark.parametrize("fault", ["alarm", "read"])
|
||||
def test_axis_faults_remain_visible_while_command_is_pending(qt_app, together, fault):
|
||||
instance, _transport = service()
|
||||
mover = PoseMover(instance, lambda _action: 1)
|
||||
targets = {"x": 0.0, "y": 0.0} if together else {"y": 0.0}
|
||||
assert mover.start(plan_pose_move(instance, targets, together=together)).ok
|
||||
bad_axis = AxisState(
|
||||
axis=1,
|
||||
units=10.0,
|
||||
status=AxisStatusBits(raw=512 if fault == "alarm" else 0),
|
||||
read_ok=fault != "read",
|
||||
)
|
||||
mover.on_snapshot(Snapshot(axes=(AxisState(axis=0, units=10.0), bad_axis)))
|
||||
assert not mover.running and not mover.state.finished
|
||||
assert ("alarm" if fault == "alarm" else "read failed") in mover.state.failure
|
||||
|
||||
@@ -34,6 +34,8 @@ def _connected(make_window, *, confirm: bool = False):
|
||||
service = window.poller.service
|
||||
service._spec = object() # noqa: SLF001 - connected, without opening anything
|
||||
service._bus = BusStatus(bus_type="ecat", init_status=1, total_axes=5) # noqa: SLF001
|
||||
# A ready physical-unit move also has a successful unit readback.
|
||||
service._last_axes = {axis: service._read_axis(axis) for axis in range(5)}
|
||||
return window
|
||||
|
||||
|
||||
|
||||
@@ -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"
|
||||
|
||||
Reference in New Issue
Block a user