diff --git a/src/linkerhand_calibration/O30_RAW_PROTOCOL_IMPLEMENTATION.md b/src/linkerhand_calibration/O30_RAW_PROTOCOL_IMPLEMENTATION.md index be73e6c..09ff7d2 100644 --- a/src/linkerhand_calibration/O30_RAW_PROTOCOL_IMPLEMENTATION.md +++ b/src/linkerhand_calibration/O30_RAW_PROTOCOL_IMPLEMENTATION.md @@ -34,6 +34,11 @@ ros2 run linkerhand_calibration calibrate_hand \ 后续恢复省略 `--no-resume`,仍需提供当前安装条件下的光学观测。若安装参考不可核验,按拒绝原因处理,不能改哈希绕过。 +2026-09-22 用户明确要求复用已完成的相机标定:可将上述光学观测参数换为 +`--reuse-camera-calibration`。此入口仍核对已有相机标定质量、受保护文件和实时 +CameraInfo;日志及发布审计记录 `current_optical_verification: not_performed`, +不宣称本次重新做过光学核验。第三轮独立关节精度、候选不确定度与 URDF 发布检查不变。 + 现有 `three_camera_extrinsics.launch.py` 增加 `verification_extrinsics_file`:指定受保护外参文件、另设 JSON `output_file`,沿用棋盘格采集及保存操作。该模式不重拟合相机参数,保存同步多姿态角点,并验证冻结内外参;每个机位至少 15 组,跨机位同步不超过 50 ms,同时检查图像/倾角覆盖及原重投影门限。正常外参标定模式不变。核验模式的角点 JSON 用于上述启动参数。 关键文件: diff --git a/src/linkerhand_calibration/TESTING.md b/src/linkerhand_calibration/TESTING.md index 95ad9e0..a1fafdd 100644 --- a/src/linkerhand_calibration/TESTING.md +++ b/src/linkerhand_calibration/TESTING.md @@ -554,6 +554,13 @@ Schur 协方差与完整逆矩阵一致、冻结模型拒绝无法解释的阶 ## O30 raw_joint 正式链路(2026-09-22) +用户指定停滞重试(2026-09-22):仅 O30 的 `mechanical_stall` 增加同一运动 +首次尝试加两次重试,每次重发前等待 3 秒,第 3 次失败停止。等待不发送新位置 +指令,不作为有效采样;失联、控制权冲突及其他保护仍检查。真实 coordinator +虚拟时钟验证三次失败、两次等待和等待期间保护:3 passed,1.00 秒。 +既有安全及反馈组 44 passed;另一条旧启动测试补充显式复用相机参数后通过。 +测试夹具曾缺少 SDK 心跳,已补齐模拟心跳,未因此改变生产通信保护。 + 新默认协议及完整边界见 `O30_RAW_PROTOCOL_IMPLEMENTATION.md`。 针对性验证(不同组有重叠,不相加宣称一次全量回归): diff --git a/src/linkerhand_calibration/launch/three_camera_calibration.launch.py b/src/linkerhand_calibration/launch/three_camera_calibration.launch.py index 8dd6f66..18aa155 100644 --- a/src/linkerhand_calibration/launch/three_camera_calibration.launch.py +++ b/src/linkerhand_calibration/launch/three_camera_calibration.launch.py @@ -306,6 +306,7 @@ def _launch_stack(context): "capture_plan_expected_sha256": ParameterValue( LaunchConfiguration("capture_plan_expected_sha256"), value_type=str), "initial_command_file": LaunchConfiguration("initial_command_file"), + "reuse_camera_calibration": ParameterValue(LaunchConfiguration("reuse_camera_calibration"), value_type=bool), "camera_optical_observations_file": LaunchConfiguration("camera_optical_observations_file"), "command_topic": command_topic, "state_topic": state_topic, @@ -469,6 +470,7 @@ def generate_launch_description() -> LaunchDescription: DeclareLaunchArgument("preparation_witnesses_json", default_value=""), DeclareLaunchArgument("capture_plan_expected_sha256", default_value=""), DeclareLaunchArgument("initial_command_file", default_value=""), + DeclareLaunchArgument("reuse_camera_calibration", default_value="false"), DeclareLaunchArgument("camera_optical_observations_file", default_value=""), DeclareLaunchArgument( "calibration_config", diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/raw_joint_finalization.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/raw_joint_finalization.py index 0f2bd10..9c2059b 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/raw_joint_finalization.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/raw_joint_finalization.py @@ -82,10 +82,18 @@ def finalize_raw_joint_session(*, profile, session_dir, serial_number, source_ur quality_limits=profile.vision.extrinsics_quality_limits, minimum_capture_counts=profile.vision.minimum_capture_counts) checks = [r for r in records if r.get('kind') == 'measured_optical_verification'] - if not checks: + reuse = [r for r in records if r.get('kind') == 'existing_camera_calibration_reused'] + if checks: + optical = verify_optical_observations(checks[-1]['observations'], extrinsics, matrices, + extrinsics_sha256=protected_inputs['camera_extrinsics_sha256']) + elif len(reuse) == 1: + from ..optical_verification import accept_existing_camera_calibration + optical = accept_existing_camera_calibration(profile, extrinsics_path, + protected_inputs['camera_extrinsics_sha256']) + if reuse[0]['acceptance'] != optical: + raise ValueError('raw_joint_camera_reuse_evidence_changed') + else: raise ValueError('raw_joint_measured_optical_verification_missing') - optical = verify_optical_observations(checks[-1]['observations'], extrinsics, matrices, - extrinsics_sha256=protected_inputs['camera_extrinsics_sha256']) model = UrdfKinematicModel(source) bundle = UrdfCommandImages(profile, model, extrinsics, training, compile_tag_links(profile, model)) audit = [] @@ -125,7 +133,9 @@ def finalize_raw_joint_session(*, profile, session_dir, serial_number, source_ur serial_number=serial_number, protected_inputs=protected_inputs) from ...core.domain.capture_plan import evidence_digest report['raw_joint_audit'].update(installation_reference_sha256=evidence_digest(reference), - optical_observations_sha256=optical['observations_sha256']) + camera_acceptance=optical) + if 'observations_sha256' in optical: + report['raw_joint_audit']['optical_observations_sha256'] = optical['observations_sha256'] naming = dict(serial_number=serial_number, side=profile.key.side, model=profile.key.model.lower()) json_path = directory/profile.artifacts.calibration_filename.format(**naming) urdf_path = directory/profile.artifacts.corrected_urdf_filename.format(**naming) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/raw_joint_validator.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/raw_joint_validator.py index b5da23a..66e17df 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/raw_joint_validator.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/artifacts/raw_joint_validator.py @@ -28,7 +28,11 @@ class RawJointArtifactValidator: report = json.loads((json_path.parent / REPORT_FILENAME).read_text(encoding='utf-8')) if evidence_digest(report) != evidence_digest(self.report) or payload != from_report(self.report): raise ValueError('raw_joint_serialized_report_changed') - if not self.validation['passed'] or not self.optical['passed']: + camera_accepted = self.optical.get('passed') is True or ( + self.optical.get('policy') == 'reuse_validated_camera_calibration_v1' + and self.optical.get('accepted') is True + and self.optical.get('camera_extrinsics_sha256') == self.report['protected_inputs']['camera_extrinsics_sha256']) + if not self.validation['passed'] or not camera_accepted: raise ValueError('raw_joint_independent_acceptance_failed') bundle, profile = self.bundle, self.bundle.profile offsets = bundle.zero_offsets(self.candidate.parameters) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py index e6616d1..dd73ba3 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/coordinator.py @@ -85,6 +85,8 @@ class CalibrationCoordinator: from .training import TrainingWorker self.training_worker = TrainingWorker() self.safety = SafetyPolicy(profile) + from .stall_retry import StallRetry + self.stall_retry = StallRetry() if profile.key.model.upper() == 'O30' else None self.reference_lock = self._new_reference_lock() self.trackers = parameters.new_trackers(self.required_observation_views) self.capture = ObservationCapture(profile, reference_lock=self.reference_lock, @@ -512,6 +514,8 @@ class CalibrationCoordinator: if row.get("kind") in {"diagnostic_observation_frame", "raw_observation_frame"}: row.update(session_epoch=epoch, motion_version=motion_version, motion_started_ns=self._motion_started_ns) + if self.stall_retry is not None and self.stall_retry.waiting: + row.update(sample_phase='stall_retry_wait', accepted_sample_window_open=False) if motion.phase == "steady" and motion.minimum_hold_seconds > 0: row.update(diagnostic_hold_timing(command_history, target=motion.target, image_stamp_ns=stamp, session_epoch=epoch, motion_version=motion_version, @@ -658,12 +662,38 @@ class CalibrationCoordinator: self.state_receive_times[-1] if self.state_receive_times else None, tuple(current), tuple(self.latest_feedback), fresh, competing_controller=self.ports.command_publisher_count() > 1, - motion_expected=self.segment.motion_expected if self.segment else False, + motion_expected=(self.segment.motion_expected if self.segment else False) + and not (self.stall_retry and self.stall_retry.waiting), motion_id=self.segment.identity if self.segment else "", motion_goals=self.segment.goals if self.segment else ())) if not decision.safe: + if (decision.code == 'mechanical_stall' and self.stall_retry is not None + and self.segment is not None): + retry = self.stall_retry.failed(self.segment.identity, now) + append_jsonl(self.raw_path, dict(kind='motion_stall_retry', + stamp_ns=self.ports.clock_ns(), motion_id=self.segment.identity, + failure_count=self.stall_retry.failures, maximum_failures=3, + delay_seconds=3.0 if retry else 0., retry_scheduled=retry, + target=list(self.segment.command.target), reason=decision.details)) + if retry: + self._steady_capture_after_ns = None + self.reason = f'motion_stall_retry_wait:{self.stall_retry.failures}/3:delay_seconds=3' + return + self._pause(f'{decision.code}:{decision.reason}:{decision.details},consecutive_failures=3') + return self._pause(f"{decision.code}:{decision.reason}:{decision.details}") return + if self.stall_retry is not None and self.stall_retry.waiting: + elapsed = self.stall_retry.resume(now) + if elapsed is None: + self.reason = f'motion_stall_retry_wait:{self.stall_retry.failures}/3:remaining={self.stall_retry.remaining(now):.1f}s' + return + self.segment.suspend_elapsed(elapsed) + self._steady_capture_after_ns = None + self._steady_rows.clear() + append_jsonl(self.raw_path, dict(kind='motion_stall_retry_resumed', + stamp_ns=self.ports.clock_ns(), motion_id=self.segment.identity, + attempt=self.stall_retry.failures+1, waited_seconds=elapsed)) if self.visual_motion is not None and self.segment is not None: reason = self.visual_motion.check(self.segment, now) if reason: @@ -1543,6 +1573,17 @@ class CalibrationCoordinator: from .optical_verification import verify_optical_observations from .camera_preflight import CameraCheck path = self.parameters.camera_optical_observations_file + if path is None and self.parameters.reuse_camera_calibration: + from .optical_verification import accept_existing_camera_calibration + report = accept_existing_camera_calibration(self.profile, + self.parameters.camera_extrinsics_file, + self.parameters.protected_inputs['camera_extrinsics_sha256']) + append_jsonl(self.raw_path, dict(kind='existing_camera_calibration_reused', + stamp_ns=self.ports.clock_ns(), acceptance=report)) + atomic_write_json(self.parameters.session_dir / 'camera_preflight.json', + dict(kind='camera_preflight', stage='at_start', + **self._camera_preflight.as_dict(), camera_acceptance=report)) + return if path is None: raise ValueError('missing_measured_optical_observations') dataset = json.loads(path.read_text(encoding='utf-8')) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py index 8f41465..7d94551 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/motion_execution.py @@ -132,6 +132,14 @@ class MotionExecution: raise ValueError("invalid measured feedback target") self.reference_tolerance = 2*self.stability + def suspend_elapsed(self, seconds): + """Exclude a retry delay from motion and settling clocks.""" + self.started_at += seconds + if self.finished_at is not None: + self.finished_at += seconds + self.history.clear() + self.stamped_history.clear() + def sample(self, now: float) -> tuple[float, ...]: distance = max((abs(v) for v in self.deltas), default=0.0) elapsed = max(0.0, float(now)-self.started_at) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/optical_verification.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/optical_verification.py index 20a13ab..3504bfe 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/optical_verification.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/optical_verification.py @@ -12,6 +12,24 @@ from ..core.geometry.tag_pose.urdf_command_images import rigid from ..core.geometry.tag_pose.parameters import DEFAULT_POSE_TRACKING_PARAMETERS +def accept_existing_camera_calibration(profile, path, expected_sha256): + """Explicit reuse of prior calibration, without claiming a fresh optical check.""" + import hashlib + from pathlib import Path + from ..core.geometry.extrinsics import load_camera_extrinsics + source = Path(path) + if hashlib.sha256(source.read_bytes()).hexdigest() != expected_sha256: + raise ValueError('existing_camera_calibration_changed') + calibrated = load_camera_extrinsics(source, required_views=profile.vision.view_names, + reference_view=profile.vision.extrinsic_reference_view, + quality_limits=profile.vision.extrinsics_quality_limits, + minimum_capture_counts=profile.vision.minimum_capture_counts) + return dict(policy='reuse_validated_camera_calibration_v1', accepted=True, + passed=False, current_optical_verification='not_performed', + camera_extrinsics_sha256=expected_sha256, prior_quality=dict(calibrated.quality), + intrinsics_updated=False, extrinsics_updated=False) + + def verify_optical_observations(dataset, extrinsics, matrices, *, extrinsics_sha256): if (dataset.get('protocol') != 'rectified_optical_check_v1' or dataset.get('camera_extrinsics_sha256') != extrinsics_sha256 diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/parameters.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/parameters.py index e0808f5..5ca83cf 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/parameters.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/parameters.py @@ -43,6 +43,7 @@ class RuntimeParameters: initial_command: tuple[float, ...] = () initial_device_uid: str = "" resume_mode: str = "verify" + reuse_camera_calibration: bool = False camera_optical_observations_file: Path | None = None def new_trackers(self, views): diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/raw_resume.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/raw_resume.py index f521ed8..9b6bc1b 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/raw_resume.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/raw_resume.py @@ -1,5 +1,5 @@ """Reference-bound replay of the raw protocol's finite acquisition schedule.""" -from dataclasses import dataclass +from dataclasses import dataclass, replace from collections import defaultdict, deque import numpy as np @@ -76,7 +76,11 @@ class RawCheckpoint: checker = ResumeVerifier(required_fixed_views=profile.vision.view_names, required_fixed_poses=profile.vision.view_names, required_hashes=current['protected_hashes']) - decision = checker.compare(fingerprint_from_mapping(previous), fingerprint_from_mapping(current)) + # Moving-tag installation is verified below from baseline pixels and + # feedback. A PnP branch is not an installation identity in this protocol. + decision = checker.compare( + replace(fingerprint_from_mapping(previous), moving_tag_poses={}), + replace(fingerprint_from_mapping(current), moving_tag_poses={})) if not decision.reuse: raise ValueError('raw_resume_reference_changed:' + str(decision.incompatible_fields)) old = self.fixed_reference.get('raw_installation_reference', {}) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py index a5d4d85..fec47cc 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/reporting/reasons_zh.py @@ -170,6 +170,10 @@ def reason_zh( if reason.startswith("mechanical_stall:"): timeout = re.search(r"(?:[:,])timeout_seconds=([0-9]+(?:\.[0-9]+)?)(?:,|$)", reason) duration = f" {timeout.group(1)} 秒" if timeout else "两秒" + if 'consecutive_failures=3' in reason: + return ('MOTION-STALL-303', + f'同一运动连续 3 次失败,每次反馈{duration}未推进;两次重发前均等待 3 秒,已停止。', + '检查机械阻挡、执行器和实际行程;本次重试预算已用尽。') return ( "MOTION-STALL-303", f"目标电机连续{duration}没有向目标推进,程序已保持当前位置。", diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/parameters.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/parameters.py index 40e4623..6bb5964 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/parameters.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/ros/parameters.py @@ -33,6 +33,7 @@ def parameter_defaults(profile) -> dict[str, Any]: "preparation_witnesses_json": "", "capture_plan_expected_sha256": "", "initial_command_file": "", + "reuse_camera_calibration": False, "camera_optical_observations_file": "", "command_topic": f"/{model}/cb_right_hand_control_cmd", "state_topic": f"/{model}/cb_right_hand_state", @@ -175,6 +176,7 @@ def load_runtime_parameters(profile, value: Callable[[str], Any]) -> RuntimePara diagnostic_capture=diagnostic_capture, initial_command=initial_command, initial_device_uid=device_uid, resume_mode=resume_mode, + reuse_camera_calibration=bool(value("reuse_camera_calibration")), camera_optical_observations_file=(Path(str(value("camera_optical_observations_file"))).expanduser().resolve() if value("camera_optical_observations_file") else None), ) diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py index 3e52cb7..2618553 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner.py @@ -117,6 +117,7 @@ def run_online( config: ProductConfig, *, record_bag: bool = False, commands_enabled: bool = True, allow_resume: bool = True, diagnostic_capture: str = "", startup_timeout: float = 120.0, + reuse_camera_calibration: bool = False, camera_optical_observations_file: Path | None = None, ) -> int: import rclpy @@ -231,6 +232,8 @@ def run_online( else: print(f"{diagnostic_capture_label(diagnostic.mode)}:仅执行声明任务各一次往返,保留准备和零位采集;不复用断点、不拟合或发布标定产物。", flush=True) launch_options = {"diagnostic_capture": diagnostic_capture} if diagnostic is not None else {} + if reuse_camera_calibration: + launch_options["reuse_camera_calibration"] = True if camera_optical_observations_file is not None: launch_options["camera_optical_observations_file"] = camera_optical_observations_file if commands_enabled and profile.vision_motion: @@ -346,6 +349,8 @@ def main(args: list[str] | None = None) -> None: diagnostic_options.add_argument("--diagnostic-capture", choices=sorted(DIAGNOSTIC_CAPTURE_PRESETS), default="", help="执行已声明的诊断任务,只保存诊断数据,不发布标定产物") parser.add_argument("--offline-raw", default="") + parser.add_argument("--reuse-camera-calibration", action="store_true", + help="明确复用已验收相机标定,不执行新的棋盘格核验;记录本次未复核光学状态") parser.add_argument("--camera-optical-observations", type=Path, help="当前安装下的多姿态同步标定板角点 JSON;新 O30 协议启动前重新计算光学核验") parser.add_argument("--offline-output", default="") @@ -422,6 +427,8 @@ def main(args: list[str] | None = None) -> None: if selected.offline_output or selected.publish_offline: parser.error("--offline-output/--publish-offline require --offline-raw") online_options = {"diagnostic_capture": diagnostic_capture} if diagnostic is not None else {} + if selected.reuse_camera_calibration: + online_options["reuse_camera_calibration"] = True if selected.camera_optical_observations is not None: online_options["camera_optical_observations_file"] = selected.camera_optical_observations if selected.startup_timeout_seconds is not None: diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner_support.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner_support.py index d8f983d..587b89e 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/runtime/runner_support.py +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/runner_support.py @@ -98,10 +98,12 @@ def launch_command( resume_from: Path | None = None, diagnostic_capture: str = "", initial_command_file: Path | None = None, + reuse_camera_calibration: bool = False, camera_optical_observations_file: Path | None = None, ) -> list[str]: """Build the single launch invocation entirely from the product contract.""" arguments = { + "reuse_camera_calibration": str(reuse_camera_calibration).lower(), "camera_optical_observations_file": str(camera_optical_observations_file or ""), "model": config.model, "hand_type": config.side, diff --git a/src/linkerhand_calibration/linkerhand_calibration/runtime/stall_retry.py b/src/linkerhand_calibration/linkerhand_calibration/runtime/stall_retry.py new file mode 100644 index 0000000..dca9291 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/runtime/stall_retry.py @@ -0,0 +1,32 @@ +"""Bounded, clock-driven retries of the same motion after a progress timeout.""" +from dataclasses import dataclass + + +@dataclass +class StallRetry: + maximum_failures: int = 3 + delay_seconds: float = 3.0 + motion_id: str = '' + failures: int = 0 + waiting_since: float | None = None + + @property + def waiting(self): + return self.waiting_since is not None + + def failed(self, motion_id, now): + if self.motion_id != motion_id: + self.motion_id, self.failures = motion_id, 0 + self.failures += 1 + self.waiting_since = now if self.failures < self.maximum_failures else None + return self.waiting + + def remaining(self, now): + return max(0., self.delay_seconds-(now-self.waiting_since)) if self.waiting else 0. + + def resume(self, now): + if not self.waiting or self.remaining(now) > 0: + return None + elapsed = now-self.waiting_since + self.waiting_since = None + return elapsed diff --git a/src/linkerhand_calibration/test/test_native_position_feedback.py b/src/linkerhand_calibration/test/test_native_position_feedback.py index 1b1638d..6690c79 100644 --- a/src/linkerhand_calibration/test/test_native_position_feedback.py +++ b/src/linkerhand_calibration/test/test_native_position_feedback.py @@ -192,6 +192,7 @@ def test_ten_unit_tracking_offset_allows_motion_and_stable_capture(phase): def test_production_o30_requires_fresh_feedback_before_start_and_during_motion(tmp_path): from runtime_host_fixture import coordinator_fixture host, clock = coordinator_fixture(tmp_path, model="o30") + host.parameters = replace(host.parameters, reuse_camera_calibration=True) try: for view in host.profile.vision.view_names: host.cameras.matrices[view] = np.eye(3) diff --git a/src/linkerhand_calibration/test/test_optical_verification.py b/src/linkerhand_calibration/test/test_optical_verification.py index 575f167..081332d 100644 --- a/src/linkerhand_calibration/test/test_optical_verification.py +++ b/src/linkerhand_calibration/test/test_optical_verification.py @@ -44,3 +44,26 @@ def test_configuration_identity_cannot_replace_optical_observations(optical, err dataset['captures'][0]['views']['b']['image_stamp_ns'] += 60_000_000 with pytest.raises(ValueError, match='optical_'): verify_optical_observations(dataset, extrinsics, matrices, extrinsics_sha256='a'*64) + + +def test_explicit_camera_reuse_checks_prior_quality_without_claiming_new_measurement(tmp_path): + import hashlib + from pathlib import Path + import yaml + from linkerhand_calibration.profiles import load_bundled_hand_profile + from linkerhand_calibration.runtime.optical_verification import accept_existing_camera_calibration + profile = load_bundled_hand_profile('o30_right_18') + source = Path(__file__).resolve().parents[3]/'config/o30_three_camera_extrinsics_20260922_101452.yaml' + path = tmp_path/'cameras.yaml' + path.write_bytes(source.read_bytes()) + digest = hashlib.sha256(path.read_bytes()).hexdigest() + report = accept_existing_camera_calibration(profile, path, digest) + assert report['accepted'] is True and report['passed'] is False + assert report['current_optical_verification'] == 'not_performed' + with pytest.raises(ValueError, match='existing_camera_calibration_changed'): + accept_existing_camera_calibration(profile, path, '0'*64) + payload = yaml.safe_load(path.read_text()) + payload['quality']['reprojection_rms_px'] = 100. + path.write_text(yaml.safe_dump(payload)) + with pytest.raises(ValueError): + accept_existing_camera_calibration(profile, path, hashlib.sha256(path.read_bytes()).hexdigest()) diff --git a/src/linkerhand_calibration/test/test_raw_resume.py b/src/linkerhand_calibration/test/test_raw_resume.py index c3c8b39..ad5c41d 100644 --- a/src/linkerhand_calibration/test/test_raw_resume.py +++ b/src/linkerhand_calibration/test/test_raw_resume.py @@ -66,6 +66,30 @@ def test_reordered_checkpoint_cannot_skip_work(): checkpoint.restore(session) +def test_reference_uses_raw_installation_pixels_not_moving_pnp_branch(): + from copy import deepcopy + from linkerhand_calibration.runtime.engine import ACQUISITION_POLICY_VERSION + selected = profile() + corners = [[0., 0.], [10., 0.], [10., 10.], [0., 10.]] + pose = dict(rotation_xyzw=[0., 0., 0., 1.], translation_xyz_m=[0., 0., 1.]) + reference = dict(profile_id=selected.key.profile_id, + acquisition_policy_version=ACQUISITION_POLICY_VERSION, + protected_hashes={'source_urdf_sha256': 'a'*64}, + fixed_corners_by_view={v: corners for v in selected.vision.view_names}, + fixed_poses={v: pose for v in selected.vision.view_names}, + moving_tag_poses={'front:thumb_mcp': pose}, + raw_installation_reference={'front:thumb_mcp': dict(corners_xy=deepcopy(corners), + feedback=list(selected.command.baseline_values))}) + current = deepcopy(reference) + current['moving_tag_poses'] = {} + checkpoint = RawCheckpoint({}, reference, (dict(kind='raw_observation_frame', + view='front', tags={'thumb_mcp': {}}),), frozenset()) + checkpoint.verify_reference(selected, current) + current['raw_installation_reference']['front:thumb_mcp']['corners_xy'][0][0] += 20. + with pytest.raises(ValueError, match='raw_resume_installation_changed'): + checkpoint.verify_reference(selected, current) + + def test_discovery_preserves_interrupted_first_attempt_without_any_passed_unit(tmp_path): import json from linkerhand_calibration.core.domain.capture_plan import CapturePlan diff --git a/src/linkerhand_calibration/test/test_stall_retry.py b/src/linkerhand_calibration/test/test_stall_retry.py new file mode 100644 index 0000000..265969c --- /dev/null +++ b/src/linkerhand_calibration/test/test_stall_retry.py @@ -0,0 +1,87 @@ +"""Exercise actual coordinator command delivery with a stationary motor.""" +import json +from dataclasses import replace + +import pytest + +from runtime_host_fixture import coordinator_fixture, ready +from linkerhand_calibration.runtime.motion_execution import MotionCommand, MotionExecution +from linkerhand_calibration.runtime.session import CalibrationPhase as Phase + + +def blocked_host(tmp_path, monkeypatch): + host, clock = coordinator_fixture(tmp_path, model='o30') + ready(host, clock) + host.parameters = replace(host.parameters, reuse_camera_calibration=True) + assert host.start().success + host.execution.session.phase = Phase.SWEEP + start = host.profile.command.baseline_values + target = list(start); target[0] = 255. + task = host.profile.motion.tasks[0] + motion = MotionCommand('sweep', tuple(target), 200., task_key=task.key, + command_index=0, cycle=0, direction='increasing') + host._motion = motion + host.last_command = start + host.segment = MotionExecution(host.profile, motion, initial_command=start, + initial_feedback=start, now=clock.now, identity='test_stalled_segment') + monkeypatch.setattr(host.execution, 'motion', lambda current: motion) + return host, clock, start + + +def heartbeat(host, clock): + clock.health_receiver(json.dumps(dict(hand_type='right', model='O30', side='RIGHT', + uid='OFFLINE_DEVICE', joint_names=list(host.profile.command.names), + online=True, position_mode=True, active_faults=[], joint_faults={}))) + + +def advance(host, clock, feedback, seconds=.1): + clock.now += seconds + heartbeat(host, clock) + host.receive_feedback(host.profile.command.names, feedback, host.ports.clock_ns()) + host.tick() + + +def test_two_delayed_resends_then_third_failure_stops(tmp_path, monkeypatch): + host, clock, feedback = blocked_host(tmp_path, monkeypatch) + try: + for _ in range(500): + if host.state == 'PAUSED': + break + waiting = host.stall_retry.waiting and host.stall_retry.remaining(clock.now) > .11 + sent = len(clock.positions) + advance(host, clock, feedback) + if waiting: + assert len(clock.positions) == sent + assert host.state == 'PAUSED' + assert 'consecutive_failures=3' in host.reason + rows = [json.loads(line) for line in host.raw_path.read_text().splitlines()] + failures = [r for r in rows if r.get('kind') == 'motion_stall_retry'] + resumes = [r for r in rows if r.get('kind') == 'motion_stall_retry_resumed'] + assert [r['failure_count'] for r in failures] == [1, 2, 3] + assert [r['attempt'] for r in resumes] == [2, 3] + assert all(3. <= r['waited_seconds'] < 3.11 for r in resumes) + assert all(a['stamp_ns'] < b['stamp_ns'] for a,b in zip(failures,resumes)) + finally: + host.close() + + +@pytest.mark.parametrize('fault', ['feedback_stale', 'duplicate_controller']) +def test_other_protection_still_stops_during_retry_delay(tmp_path, monkeypatch, fault): + host, clock, feedback = blocked_host(tmp_path, monkeypatch) + try: + for _ in range(200): + advance(host, clock, feedback) + if host.stall_retry.waiting: + break + assert host.stall_retry.waiting + if fault == 'feedback_stale': + clock.now += host.profile.acquisition.feedback_stale_seconds+.1 + else: + clock.publishers = 2 + heartbeat(host, clock) + host.tick() + assert host.state == 'PAUSED' + assert fault in host.reason + assert host.stall_retry.failures == 1 + finally: + host.close()