From af2dc9c38f2b2246cb74f2b740c2c209a137bb1d Mon Sep 17 00:00:00 2001 From: lxp <2770281812@qq.com> Date: Thu, 17 Sep 2026 12:41:53 +0800 Subject: [PATCH] =?UTF-8?q?=E6=96=B0=E5=A2=9E=20O30=20=E5=8F=B3=E6=89=8B?= =?UTF-8?q?=E8=A1=8C=E7=A8=8B=E6=A0=87=E5=AE=9A=E5=8A=9F=E8=83=BD=E5=8C=85?= =?UTF-8?q?=E5=B9=B6=E5=AE=8C=E5=96=84=E7=9B=B8=E6=9C=BA=E6=81=A2=E5=A4=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 支持三机位 Tag 两端边界标定、滑块示教、顺序避让恢复、同步四指扫描和 JSON 离线复算；修复相机时钟异常导致采集线程退出的问题，并忽略本地行程标定输出。 --- .gitignore | 1 + src/linkerhand_calibration/README.md | 1 + .../linkerhand_calibration/camera_timing.py | 19 +- .../linkerhand_calibration/hikrobot_camera.py | 17 +- .../test/test_camera_timing.py | 34 + .../test/test_hikrobot_camera.py | 32 + src/linkerhand_range_calibration/README.md | 230 +++++++ .../config/profiles/o30_right.yaml | 64 ++ .../config/station.yaml | 31 + .../launch/calibrate.launch.py | 88 +++ .../linkerhand_range_calibration/__init__.py | 1 + .../adapters/__init__.py | 1 + .../adapters/base.py | 19 + .../adapters/fake.py | 50 ++ .../adapters/o30_ros.py | 178 +++++ .../adapters/registry.py | 12 + .../linkerhand_range_calibration/cli.py | 79 +++ .../core/__init__.py | 1 + .../core/analysis.py | 78 +++ .../core/endpoints.py | 99 +++ .../core/engine.py | 371 ++++++++++ .../core/errors.py | 5 + .../core/trajectory.py | 19 + .../linkerhand_range_calibration/profiles.py | 203 ++++++ .../linkerhand_range_calibration/replay.py | 63 ++ .../linkerhand_range_calibration/runtime.py | 347 ++++++++++ .../simulation.py | 57 ++ .../linkerhand_range_calibration/storage.py | 161 +++++ .../ui/__init__.py | 1 + .../ui/joint_control.py | 51 ++ .../linkerhand_range_calibration/ui/window.py | 198 ++++++ .../vision/__init__.py | 1 + .../vision/observations.py | 73 ++ src/linkerhand_range_calibration/package.xml | 29 + .../resource/linkerhand_range_calibration | 0 src/linkerhand_range_calibration/setup.cfg | 6 + src/linkerhand_range_calibration/setup.py | 21 + .../test/conftest.py | 4 + .../test/test_core.py | 649 ++++++++++++++++++ .../test/test_endpoints.py | 246 +++++++ .../test/test_gui.py | 211 ++++++ .../test/test_ros_interface.py | 152 ++++ 42 files changed, 3900 insertions(+), 3 deletions(-) create mode 100644 src/linkerhand_range_calibration/README.md create mode 100644 src/linkerhand_range_calibration/config/profiles/o30_right.yaml create mode 100644 src/linkerhand_range_calibration/config/station.yaml create mode 100644 src/linkerhand_range_calibration/launch/calibrate.launch.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/__init__.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/adapters/__init__.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/adapters/base.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/adapters/fake.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/adapters/o30_ros.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/adapters/registry.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/cli.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/core/__init__.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/core/analysis.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/core/endpoints.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/core/engine.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/core/errors.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/core/trajectory.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/profiles.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/replay.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/runtime.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/simulation.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/storage.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/ui/__init__.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/ui/joint_control.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/ui/window.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/vision/__init__.py create mode 100644 src/linkerhand_range_calibration/linkerhand_range_calibration/vision/observations.py create mode 100644 src/linkerhand_range_calibration/package.xml create mode 100644 src/linkerhand_range_calibration/resource/linkerhand_range_calibration create mode 100644 src/linkerhand_range_calibration/setup.cfg create mode 100644 src/linkerhand_range_calibration/setup.py create mode 100644 src/linkerhand_range_calibration/test/conftest.py create mode 100644 src/linkerhand_range_calibration/test/test_core.py create mode 100644 src/linkerhand_range_calibration/test/test_endpoints.py create mode 100644 src/linkerhand_range_calibration/test/test_gui.py create mode 100644 src/linkerhand_range_calibration/test/test_ros_interface.py diff --git a/.gitignore b/.gitignore index ec9bd89..87d3cc1 100644 --- a/.gitignore +++ b/.gitignore @@ -62,6 +62,7 @@ Thumbs.db # src/linkerhand_retarget/resource/linkerforce_v2/profiles/. /profiles/ /calibration_output/ +/range_calibration_output/ /config/*_three_camera_extrinsics.yaml # Superseded local O6 camera calibrations. Keep the active extrinsics and # intrinsics referenced by o6_right_product.yaml available for version control. diff --git a/src/linkerhand_calibration/README.md b/src/linkerhand_calibration/README.md index d76e324..5ff69ec 100644 --- a/src/linkerhand_calibration/README.md +++ b/src/linkerhand_calibration/README.md @@ -717,6 +717,7 @@ ros2 run linkerhand_calibration compare_calibration_urdfs \ 连续扫描将 CAN 实测反馈与相机曝光中点配对。MVS 取帧/图像发布时间不参与替代曝光时间； 设备时钟由独立锁存换算到 ROS 时钟，换算依据保存在会话的 `camera_timing_.jsonl`。 +运行中的时钟校验失败会暂停出图并重新执行独立锁存校验，通过后恢复采集；校验失败期间不会继续使用旧映射，也不会永久退出采集线程。恢复前后的图像时间戳仍须严格递增；若主机时钟向后跳变，早于已发布图像的帧继续丢弃。 四型号共用 `unified_engine_v8_all_view_images`，保留设备曝光时间策略；旧采集只能按其原策略诊断，不能混用。 报告中的 `feedback_u8` 表示来自字节通道的测量值：插值到曝光时间后可以是小数， 拟合、内存映射和报告读取均按声明的分段线性曲线计算，不再次取整。 diff --git a/src/linkerhand_calibration/linkerhand_calibration/camera_timing.py b/src/linkerhand_calibration/linkerhand_calibration/camera_timing.py index 9a73ad2..d596de1 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/camera_timing.py +++ b/src/linkerhand_calibration/linkerhand_calibration/camera_timing.py @@ -84,6 +84,7 @@ class MvsCameraTiming: self.camera, self.mvs, self.clock_ns = camera, mvs, clock_ns self.exposure_us = exposure_us self.frame_clock = None + self.last_stamp_ns = None self.next_refresh = 0. self.journal = None if journal_path: @@ -143,16 +144,32 @@ class MvsCameraTiming: self.next_refresh = time.monotonic()+.5 def refresh_if_due(self): - if time.monotonic() >= self.next_refresh: + if self.frame_clock is None: + self.start() + elif time.monotonic() >= self.next_refresh: self._update([self._latch() for _ in range(3)]) + def invalidate(self, reason): + """Stop using an uncertain mapping until a new startup check succeeds.""" + self.frame_clock = None + self.next_refresh = 0. + self._write(dict(kind="camera_clock_invalidated", reason=str(reason), + last_stamp_ns=self.last_stamp_ns)) + if self.journal is not None: + self.journal.flush() + def timestamp(self, info, received_ns): + if self.frame_clock is None: + raise ValueError("camera frame has no synchronized clock") tick = (int(info.nDevTimeStampHigh)<<32)|int(info.nDevTimeStampLow) # Auto-exposure preview uses the exact exposure-start timestamp when # the SDK supplies no per-frame duration. Calibration fixes exposure. measured_exposure = float(info.fExposureTime) exposure = measured_exposure if measured_exposure > 0 else self.exposure_us stamp = self.frame_clock.timestamp(tick, received_ns=received_ns, exposure_us=exposure) + if self.last_stamp_ns is not None and stamp <= self.last_stamp_ns: + raise ValueError("camera exposure time is not increasing after clock synchronization") + self.last_stamp_ns = stamp self._write(dict(kind="camera_frame_time", frame_number=int(info.nFrameNum), device_tick=tick, sdk_host_stamp_ms=int(info.nHostTimeStamp), received_ns=received_ns, stamp_ns=stamp, exposure_us=exposure, diff --git a/src/linkerhand_calibration/linkerhand_calibration/hikrobot_camera.py b/src/linkerhand_calibration/linkerhand_calibration/hikrobot_camera.py index 1cd7262..ddb4b98 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/hikrobot_camera.py +++ b/src/linkerhand_calibration/linkerhand_calibration/hikrobot_camera.py @@ -472,12 +472,25 @@ class HikrobotCameraNode(Node): assert self._camera is not None assert self._mvs is not None timeout_ms = int(self.get_parameter("grab_timeout_ms").value) + recovering_clock = False while not self._stop_event.is_set(): try: self._camera_timing.refresh_if_due() except ValueError as error: - self.get_logger().error(f"Camera timestamp source failed: {error}") - return + if not recovering_clock: + self.get_logger().error( + f"Camera timestamp source failed: {error}; " + "pausing images and retrying clock synchronization" + ) + self._camera_timing.invalidate(error) + recovering_clock = True + self._stop_event.wait(0.2) + continue + if recovering_clock: + self.get_logger().info( + "Camera clock synchronized again; resuming image acquisition" + ) + recovering_clock = False frame = self._mvs.MV_FRAME_OUT() memset(byref(frame), 0, sizeof(frame)) result = self._camera.MV_CC_GetImageBuffer(frame, timeout_ms) diff --git a/src/linkerhand_calibration/test/test_camera_timing.py b/src/linkerhand_calibration/test/test_camera_timing.py index bd2ac72..3155a18 100644 --- a/src/linkerhand_calibration/test/test_camera_timing.py +++ b/src/linkerhand_calibration/test/test_camera_timing.py @@ -4,6 +4,7 @@ import gzip import json from pathlib import Path import statistics +from types import SimpleNamespace import pytest @@ -66,6 +67,39 @@ def test_uncertain_control_transaction_does_not_refresh_the_clock(): assert timing.frame_clock.anchor == latch(600_000_000) +@pytest.mark.parametrize('host_jump', [0, 10_000_000, -1_000_000_000]) +def test_recovery_revalidates_clock_and_preserves_frame_time_order(monkeypatch, host_jump): + timing = MvsCameraTiming(None, None, lambda: 0, exposure_us=5000.) + timing.frame_clock = initialized() + + def frame(elapsed_ns): + tick = latch(elapsed_ns).device_tick + return SimpleNamespace(nDevTimeStampHigh=tick >> 32, nDevTimeStampLow=tick & 0xffffffff, + fExposureTime=5000., nFrameNum=1, nHostTimeStamp=0) + + previous = timing.timestamp(frame(200_000_000), T0+220_000_000) + with pytest.raises(ValueError, match='discontinuity'): + timing._update([latch(400_000_000, host_jump=10_000_000)]) + timing.invalidate('clock mismatch') + with pytest.raises(ValueError, match='no synchronized clock'): + timing.timestamp(frame(500_000_000), T0+520_000_000) + + recovery_latches = iter(latch(500_000_000+i*20_000_000, host_jump=host_jump) for i in range(8)) + monkeypatch.setattr(timing, '_integer', lambda name: 100_000_000) + monkeypatch.setattr(timing, '_latch', lambda: next(recovery_latches)) + monkeypatch.setattr('linkerhand_calibration.camera_timing.time.sleep', lambda seconds: None) + timing.refresh_if_due() + assert timing.frame_clock.anchor is not None + assert timing.last_stamp_ns == previous + if host_jump < 0: + with pytest.raises(ValueError, match='not increasing'): + timing.timestamp(frame(700_000_000), T0+720_000_000+host_jump) + assert timing.last_stamp_ns == previous + else: + assert timing.timestamp(frame(700_000_000), T0+720_000_000+host_jump) == T0+702_500_000+host_jump + assert timing.timestamp(frame(1_500_000_000), T0+1_520_000_000+host_jump) == T0+1_502_500_000+host_jump + + @pytest.mark.parametrize('serial', ['DB2163742','DB2163749','DB2163739']) def test_real_camera_counter_units_and_exposure_events(serial): fixture = Path(__file__).parent/'fixtures/camera_clock_receipts.json.gz' diff --git a/src/linkerhand_calibration/test/test_hikrobot_camera.py b/src/linkerhand_calibration/test/test_hikrobot_camera.py index e66c210..fc75b61 100644 --- a/src/linkerhand_calibration/test/test_hikrobot_camera.py +++ b/src/linkerhand_calibration/test/test_hikrobot_camera.py @@ -1,10 +1,15 @@ from pathlib import Path +from ctypes import c_uint8 +from threading import Event +from types import SimpleNamespace +from unittest.mock import Mock import pytest import yaml from linkerhand_calibration.hikrobot_camera import ( DeviceDescriptor, + HikrobotCameraNode, decode_c_string, load_camera_calibration, resolve_camera_info_path, @@ -102,3 +107,30 @@ def test_camera_info_url_only_accepts_local_files(tmp_path: Path) -> None: assert resolve_camera_info_path("") is None with pytest.raises(ValueError, match="filesystem path"): resolve_camera_info_path("package://example/front.yaml") + + +def test_grab_loop_retries_clock_failures_without_publishing_or_exiting(monkeypatch): + stop = Event() + waits = [] + monkeypatch.setattr(stop, 'wait', lambda seconds: waits.append(seconds)) + timing = Mock() + timing.refresh_if_due.side_effect = [ValueError('clock mismatch'), ValueError('latch failed'), None] + camera, logger = Mock(), Mock() + camera.MV_CC_GetImageBuffer.return_value = 0 + camera.MV_CC_FreeImageBuffer.return_value = 0 + published = [] + + def publish(frame): + assert timing.refresh_if_due.call_count == 3 + assert timing.invalidate.call_count == 2 + published.append(frame) + stop.set() + + node = SimpleNamespace(_camera=camera, _mvs=SimpleNamespace(MV_FRAME_OUT=c_uint8), + _camera_timing=timing, _stop_event=stop, + get_parameter=lambda name: SimpleNamespace(value=1000), + get_logger=lambda: logger, _publish_frame=publish) + HikrobotCameraNode._grab_loop(node) + assert len(published) == 1 and waits == [.2, .2] + assert camera.MV_CC_GetImageBuffer.call_count == camera.MV_CC_FreeImageBuffer.call_count == 1 + assert logger.error.call_count == logger.info.call_count == 1 diff --git a/src/linkerhand_range_calibration/README.md b/src/linkerhand_range_calibration/README.md new file mode 100644 index 0000000..523763f --- /dev/null +++ b/src/linkerhand_range_calibration/README.md @@ -0,0 +1,230 @@ +# LinkerHand 有效指令行程标定 + +本包通过指尖 AprilTag 的图像运动，测量机械手实际有效的控制指令范围。首个型号为 O30 右手。 +输出的是 `0～255` 中的有效指令上下界，单位为 `u8`。没有角度反解、URDF 修改或三维融合。 + +**O30 SDK 源码、launch 和配置均保持原样。** 新包启动既有驱动可执行程序，并使用其原有话题与参数。 +SDK 启动时关闭自动摆位，打开界面本身不发送位置、速度或力矩命令。 + +## 1. 构建 + +环境：ROS 2 Jazzy、Python、PyQt5、NumPy、PyYAML、OpenCV、cv_bridge、apriltag_ros、image_proc。 +海康相机复用工作区 `linkerhand_calibration` 中的 `hikrobot_camera_node`，需要既有 MVS 环境和有效内参。 +O30 金属 CANFD 盒使用 SDK 已安装的 `libcanbus` 运行库。 + +明确选择两个源包，避免引入外部 SDK 仓库中与工作区重名的 GUI 包。构建目录位于本工作区： + +```bash +cd /home/lxp/projects/linkerhand_retarget_ros2 +source /opt/ros/jazzy/setup.bash +source install/setup.bash +colcon --log-base log/range_calibration build \ + --base-paths src/linkerhand_range_calibration \ + /home/lxp/projects/linkerhand-o30-ros2/linker_hand_o30_ros2_sdk \ + --packages-select linker_hand_o30_ros2_sdk linkerhand_range_calibration \ + --build-base build/range_calibration \ + --install-base install/range_calibration --symlink-install +source install/range_calibration/setup.bash +``` + +上述流程依赖已构建的相机驱动包；不会启动硬件。如果该包未安装，需要先构建工作区现有的 `linkerhand_calibration`。 + +## 2. 无硬件检查 + +```bash +# 仅加载和检查配置，不初始化 ROS 或硬件。 +ros2 run linkerhand_range_calibration calibrate_range --profile o30_right --validate-only + +# 假SDK + 虚拟时间，自动扫描全部17个任务；结果写在 SIMULATED_HAND 目录。 +ros2 run linkerhand_range_calibration calibrate_range --profile o30_right --simulate \ + --output /tmp/linkerhand_range_simulation + +# 简易界面演示，只有假SDK和合成图像；不启动原 SDK 或相机。 +ros2 launch linkerhand_range_calibration calibrate.launch.py profile:=o30_right demo:=true +``` + +模拟使用更快的虚拟调度，边界算法与实际标定相同。演示模式里的深色方块是合成观测，不能作为真实视觉验收。 + +## 3. 实机启动与示教 + +工位配置为 `config/station.yaml`： + +| 机位 | 相机序列号 | 默认内参 | +|---|---|---| +| 正面 | DB2163742 | `~/.ros/camera_info/hikrobot_DB2163742.yaml` | +| 侧面 | DB2163749 | `~/.ros/camera_info/hikrobot_DB2163749.yaml` | +| 顶部 | DB2163739 | `~/.ros/camera_info/hikrobot_DB2163739.yaml` | + +使用 `36h11` Tag。内参对应当前1624×1240图像和镜头配置，不使用相机外参文件。 +默认设备速度和力矩均为200。手动示教以30 Hz调度发送最新滑块目标，由设备速度设置控制实际运动； +自动标定的准备姿态、避让和扫描轨迹限制为20指令单位/秒（`scan.rate`）。改变标定时的运动设置后应重新标定。 + +```bash +ros2 launch linkerhand_range_calibration calibrate.launch.py profile:=o30_right + +# 使用自己的工位配置 +ros2 launch linkerhand_range_calibration calibrate.launch.py \ + profile:=o30_right station:=/absolute/path/to/station.yaml +``` + +启动文件会启动一份 O30 SDK、三路相机、检测器和界面。请关闭其他 SDK 实例和控制器；发现其他位置发布者会阻止运动。 + +首次操作： + +1. 等待正确设备 UID、SDK 就绪和三路清晰图像。 +2. 直接拖动对应关节的滑块，机械手跟随最新目标；无需逐项输入或点击移动按钮。右侧数值框可精调，输入后按回车或移出焦点确认。 +3. 等待反馈稳定，将当前姿态保存为“全手基础姿态”。 +4. 必要时为单独任务保存“当前任务准备姿态覆盖”。避让值优先于覆盖值，防止覆盖已设置的避让。四指侧摆固定使用基础姿态，禁止覆盖。 +5. 先试标定一个任务，检查可见性和运动；之后执行全手标定。 + +O30 右手所有避让均使用固定指令，无需手动示教：`thumb_cmc_roll` 任务的食指侧摆固定为0，`thumb_mcp` 任务的拇指 `thumb_cmc_yaw` 固定为80，小指、无名指、中指的弯曲避让固定为255。 +示教下拉框只保留全手基础姿态和可选的任务准备姿态覆盖。 + +关节、SDK 下标、目标滑块、实际反馈、min、max 和状态合并在同一张表中，共用一个滚动条，同一关节始终在同一行。 +滑块与数值框同步显示目标，右侧紧邻实际反馈和标定结果；较长的状态原因可悬停查看完整内容。连续拖动时采用每个关节的最新目标，其他关节保持当前目标。 +手动示教采用与原O30 GUI相同的直接目标控制方式，不经过扫描的慢速插值；快速拖动或反向时不会排队执行旧目标。 +控制连接、身份和配置未变化时，手动示教不重复发送速度和力矩设置。目标发送完成后，仍需采集等待期之后的新反馈，确认稳定才能保存。 +打开界面和点击“同步当前目标”只更新显示，不下发运动。示教运动期间可继续拖动；自动标定及暂停中的任务须先取消，再进行手动调节。 +手动控制与自动标定使用同一套控制连接检查，位置命令沿用原 O30 GUI 使用的 SDK 话题。 +SDK 的运行时诊断（堵转、过温、过流、关节故障、心跳及通信诊断标志）不在标定层额外锁定滑块或暂停扫描；界面不显示诊断提示横幅，扫描日志保留诊断记录。 +SDK 自身的初始化检查和设备保护保持原样。标定仍要求 SDK 订阅存在、反馈有效且及时、设备身份和参数匹配、没有其他位置发布者；未连接或反馈断流时停止推进。 +暂停、取消会丢弃尚未执行的滑块调整，保持最后已发送目标；取消后再次拖动才会重新运动。 +示教保存已下发目标与稳定反馈，不会把尚未执行的滑块目标作为保存姿态。 +示教配置保存在 `~/.ros/linkerhand_range_calibration//teaching.yaml`。 +不同设备 UID 不会自动复用示教。 +已有示教文件可继续使用，只需已有全手基础姿态，无需补存避让项。 +旧文件中的 `index_roll_for_thumb`、`pinky_fold`、`ring_fold`、`middle_fold` 记录可以保留；运行时采用型号配置中的固定避让值，食指侧摆为0，三指弯曲为255。 + +## 4. 任务与 Tag + +| 机位 / ID | 标定关节 | SDK 数组下标（从0开始） | +|---|---|---| +| 正面 / 0 | thumb_cmc_roll、thumb_mcp、thumb_ip | 0、6、15 | +| 正面 / 4 | index_mcp_roll | 2 | +| 正面 / 3 | middle_mcp_roll | 3 | +| 正面 / 2 | ring_mcp_roll | 4 | +| 正面 / 1 | pinky_mcp_roll | 5 | +| 侧面 / 5 | pinky_mcp_pitch、pinky_pip、pinky_dip | 10、14、19 | +| 侧面 / 6 | ring_mcp_pitch、ring_pip、ring_dip | 9、13、18 | +| 侧面 / 7 | middle_mcp_pitch、middle_pip、middle_dip | 8、12、17 | +| 侧面 / 8 | index_mcp_pitch、index_pip、index_dip | 7、11、16 | +| 顶部 / 9 | thumb_cmc_yaw | 1 | + +任务顺序：正面拇指3项 → 四指侧摆同步1项 → 侧面弯曲12项 → 顶部拇指1项。 + +标定 `thumb_cmc_roll` 时，准备姿态中的 `index_mcp_roll`（SDK 下标2）固定为0。 +准备姿态稳定后开始拇指扫描，食指侧摆始终保持0；准备过程不计入边界采样。 +测完最小和最大值后，严格按以下顺序恢复；单任务试标定也执行相同流程： + +1. `thumb_cmc_roll` 沿限速轨迹恢复为已保存的全手基础姿态目标，此时 `index_mcp_roll` 继续保持避让值0。 +2. 等待拇指恢复后的新反馈稳定，再让 `index_mcp_roll` 恢复为全手基础姿态目标；拇指 roll 保持基础目标。 +3. 等待食指恢复后的新反馈稳定，才结束单任务或准备下一任务。 + +每一步仅改变正在恢复的关节。恢复期间不采集边界样本，也不要求 Tag 可见；界面显示“测后恢复基础姿态”和当前恢复的关节名。 + +开始 `thumb_mcp` 前，准备姿态中的 `thumb_cmc_yaw`（SDK 下标1）固定为80，食指侧摆恢复使用基础姿态（或该任务的姿态覆盖）。 +准备姿态稳定后才扫描 `thumb_mcp`，扫描期间拇指 yaw 始终保持80。 +该避让仅用于 `thumb_mcp`，进入后续 `thumb_ip` 时按照基础姿态及对应任务配置重新准备。 + +四指侧摆使用相同指令，同时更新SDK下标2、3、4、5，不应用避让配置；各Tag独立计算范围。 +一指先确认起动边界后保留该值，整组继续同步逐1移动；四指均确认该端边界后，共同切换到另一端。 +若某指扫描到对端仍不能确认运动，该指记为失败；其余手指继续测量，不为已失败手指重复寻找另一端。 + +侧面按小指→无名指→中指→食指测量，每指依次测 mcp_pitch、pip、dip。 +测无名指时小指弯曲；测中指时小指和无名指弯曲；测食指时前三指弯曲。 +参与避让的每根手指，其 `mcp_pitch`、`pip`、`dip` 三个关节目标均固定为255： + +| 避让组 | 固定目标 | +|---|---| +| `pinky_fold` | `pinky_mcp_pitch=255`、`pinky_pip=255`、`pinky_dip=255` | +| `ring_fold` | `ring_mcp_pitch=255`、`ring_pip=255`、`ring_dip=255` | +| `middle_fold` | `middle_mcp_pitch=255`、`middle_pip=255`、`middle_dip=255` | + +当前手指非目标关节的目标指令保持准备姿态，切换任务时重新构建完整姿态。 +扫描只使用当前任务绑定的 Tag，其他关节的连带运动、反馈变化及无关 Tag 的移动或丢失不会中断扫描。 +例如标定 `pinky_pip` 时，只使用侧面 ID5；正面 ID1 的连带运动和 `pinky_mcp_roll` 的反馈变化不参与边界判定。 + +## 5. 算法与暂停 + +默认只测两端，一轮包含以下步骤： + +1. 到达任务准备姿态并稳定。 +2. 沿限速轨迹到达0，采集稳定的低端参考角点和噪声。 +3. 按1、2、3……逐1递增，与低端参考比较；连续3个有效采样点确认离开参考姿态后，保留第一次触发运动的指令。 +4. 沿限速轨迹直接移动到255，中间不停车采样；稳定后重新采集高端参考角点和噪声。 +5. 按254、253、252……逐1递减，与高端参考比较；确认离开参考姿态后记录第一次触发指令，结束测量。 +6. 保存结果；若任务配置了测后恢复关节，按配置顺序逐个恢复为基础姿态，每一步都等待新反馈稳定，再结束任务或进入下一任务。 + +确认期间若观测重新回到参考姿态附近，会清除候选值，继续寻找。 +默认只执行一轮（`scan.endpoint_repetitions: 1`）。设为2或更大时，按相同顺序重新寻找两端， +同一端复测差异最多2单位（`repeat_tolerance`），通过后取区间交集。旧工位文件的 `coarse_step`、`fine_radius`、`fine_repetitions` 仍可加载，但不影响新流程。 + +只比较同一个Tag的四个有序角点，结合稳定窗口、端点平台、边界附近多个观测点的确认和静止噪声。 +每点至少等待0.3秒，随后采集8张稳定新图像；扫描期间只检查本任务主动关节的反馈稳定性，不要求反馈等于目标指令。 +准备姿态、测后恢复和示教保存继续等待全手反馈稳定；SDK 连接及反馈时效检查保留。 +旧配置中的 `non_target_tolerance` 仍可加载，但不再用于扫描判定。 +每端独立计算阈值 `max(0.5 px, 5×该端静止噪声)`。比较对象始终是该端参考姿态，细小的累计运动也能被检测。 +JSON 沿用边界定义：`min = 低端首次运动指令 - 1`，`max = 高端反向首次运动指令 + 1`。 +例如从0递增到7首次运动、从255递减到242首次运动，结果为6～243；加减1对应 O30 的指令分辨率，其他型号使用自身分辨率。 +默认3点确认时，该例只采集0～9和255～240共26个指令点，两端之间只移动。 +界面显示端点定位、低端/高端搜索、当前指令和轮次；进度按已确认或已判失败的边界计算。 + +范围只描述两端的指令边界，中间是否持续运动不参与成功/失败判定。 +例如低端静止到6、高端从243开始静止，即使中途停了一段再恢复运动，仍可得到6～243。 +新日志的离线复算与在线标定共用端点搜索逻辑。全程没有可确认运动、端点观测不足、两端边界重叠或启用复测后边界不一致时，无法给出可靠范围。 + +单步分辨率为1，不代表真实边界误差必然为1。过小运动、噪声、速度和力矩设置都会影响可检测边界。 +本方法测量低端正向起动和高端反向起动边界，回差或反向空行程可能使结果收窄。 +手掌和相机应在采样时固定，手指无外部接触；当前标签布局无法分离整体装夹移动。 + +当前任务所需 Tag 丢失、旧图像或重复时间戳不会计为静止。短时丢失停止推进；所需 Tag 持续丢失、稳定超时或控制连接失效会暂停。 +若相机日志出现 `Camera timestamp source failed`，相机驱动暂停出图并重新校验设备与主机时钟，通过后自动恢复采集；不会使用未经校验的时间戳。标定界面已暂停的任务仍需点击“继续”重新采集当前任务。 +“继续”会重新采集当前任务，四指同步任务整体重做，已经完成的任务保留。 +暂停和取消停止推进目标，设备保持最后已下发目标，不代表硬件急停；不会自动快速张手。 +任务数据无法识别有效范围时记为失败，继续其他任务。 + +SDK 诊断只进入日志，保留标定关节名、SDK 原名和具体内容，例如 +`thumb_cmc_roll（SDK: thumb_roll）：执行器层判定堵转`。 +诊断标志出现时继续执行两端搜索，通过新图像确认运动或静止。 +诊断标志不作为边界证据，范围由发送指令对应的角点运动决定。 +扫描期间诊断发生变化或消失时，独立 JSONL 日志记录 `diagnostic_warning` 事件（消失时 `reason` 为null），包含目标、反馈、任务与时间；正式结果JSON不增加字段。 +控制连接和视觉采样导致的暂停会显示具体原因。条件恢复后，点击“继续”重做当前任务；也可取消后手动调整姿态再重测。 + +## 6. 文件与离线复算 + +结果位置：`range_calibration_output//<时间>/o30_right_ranges.json`。 +仅包含 `schema_version`、`model`、`side`、`device_uid`、`command_unit`、`joints`。 +20个关节始终存在，每项只有min/max；失败或未完成均为null。 +每个任务结束、暂停或取消时原子保存。再次点击开始/重测创建新会话，不覆盖历史文件。 + +同目录 `samples.jsonl` 保存型号配置、示教、运动设置、命令、反馈、角点、时间戳和失败原因。 +新日志以 `scan_method: endpoint_search_v1` 标明流程，记录各端参考观测、首次运动指令、确认指令和最终边界；旧版粗扫/细扫日志仍按原采样结构复算。 +离线复算读取观测，重新运行范围算法，不连接硬件、不直接复制既有结果： + +```bash +ros2 run linkerhand_range_calibration calibrate_range \ + --replay /path/to/session/samples.jsonl --output /tmp/recomputed_ranges.json +``` + +## 7. 新型号与测试 + +型号配置提供关节映射、指令范围与分辨率、Tag、任务分组和避让关系。 +避让组支持两种配置：关节名称列表表示需要示教；`targets: {关节名: 指令值}` 表示固定目标，加载时校验范围与分辨率。 +两种配置共用任务调度，固定目标优先于历史示教和任务姿态覆盖。旧日志中的列表式型号配置仍可离线复算。 +任务可配置有序列表 `restore_after: [thumb_cmc_roll, index_mcp_roll]`：测量结束后按列表顺序逐个恢复为基础姿态中保存的目标，每一步等待新反馈稳定后才进行下一步，其他关节保持最后目标。已经处于基础目标的步骤跳过；未配置时直接结束任务；暂停或取消停止推进恢复动作。 +一个任务可以含一个或多个通道；并行通道使用相同指令范围与分辨率，并且各有独立观测源。 +新增协议实现 `adapters/base.py` 中的适配接口及 `launch_parameters`，在 `adapters/registry.py` 注册；不修改扫描或范围算法。 +适配器通过 `control_error` 提供统一的控制连接检查，通过 `diagnostic_warning` 提供只写日志的诊断，不将显示文案用于运动判定。 +适配器提供相应驱动的启动参数绑定。设备示教继续通过同一界面完成。 + +```bash +python3 -m pytest -q src/linkerhand_range_calibration/test +colcon --log-base log/range_calibration_test test \ + --base-paths src/linkerhand_range_calibration \ + --build-base build/range_calibration --install-base install/range_calibration \ + --packages-select linkerhand_range_calibration +colcon test-result --test-result-base build/range_calibration/linkerhand_range_calibration +``` + +ROS接口测试使用独立ROS域和假SDK。实机验收需要操作者完成示教后，依次验证单关节、四指同步、侧面避让和全手复测。 +软件测试通过不等于实机测量精度通过。 diff --git a/src/linkerhand_range_calibration/config/profiles/o30_right.yaml b/src/linkerhand_range_calibration/config/profiles/o30_right.yaml new file mode 100644 index 0000000..2a839a9 --- /dev/null +++ b/src/linkerhand_range_calibration/config/profiles/o30_right.yaml @@ -0,0 +1,64 @@ +schema_version: 1 +model: O30 +side: right +command_unit: u8 +adapter: o30_ros +command: {minimum: 0, maximum: 255, resolution: 1} +sdk: + package: linker_hand_o30_ros2_sdk + executable: linker_hand_o30_ros2_sdk + node: /linkerhand_range_sdk + command_topic: /cb_right_hand_control_cmd + feedback_topic: /cb_right_hand_state + setting_topic: /cb_right_hand_setting_cmd + info_topic: /cb_right_hand_info +joints: + - {name: thumb_cmc_roll, sdk_name: thumb_roll, index: 0, view: front, tag_id: 0} + - {name: thumb_cmc_yaw, sdk_name: thumb_yaw, index: 1, view: top, tag_id: 9} + - {name: index_mcp_roll, sdk_name: index_yaw, index: 2, view: front, tag_id: 4} + - {name: middle_mcp_roll, sdk_name: middle_yaw, index: 3, view: front, tag_id: 3} + - {name: ring_mcp_roll, sdk_name: ring_yaw, index: 4, view: front, tag_id: 2} + - {name: pinky_mcp_roll, sdk_name: little_yaw, index: 5, view: front, tag_id: 1} + - {name: thumb_mcp, sdk_name: thumb_root1, index: 6, view: front, tag_id: 0} + - {name: index_mcp_pitch, sdk_name: index_root1, index: 7, view: side, tag_id: 8} + - {name: middle_mcp_pitch, sdk_name: middle_root1, index: 8, view: side, tag_id: 7} + - {name: ring_mcp_pitch, sdk_name: ring_root1, index: 9, view: side, tag_id: 6} + - {name: pinky_mcp_pitch, sdk_name: little_root1, index: 10, view: side, tag_id: 5} + - {name: index_pip, sdk_name: index_root2, index: 11, view: side, tag_id: 8} + - {name: middle_pip, sdk_name: middle_root2, index: 12, view: side, tag_id: 7} + - {name: ring_pip, sdk_name: ring_root2, index: 13, view: side, tag_id: 6} + - {name: pinky_pip, sdk_name: little_root2, index: 14, view: side, tag_id: 5} + - {name: thumb_ip, sdk_name: thumb_tip, index: 15, view: front, tag_id: 0} + - {name: index_dip, sdk_name: index_tip, index: 16, view: side, tag_id: 8} + - {name: middle_dip, sdk_name: middle_tip, index: 17, view: side, tag_id: 7} + - {name: ring_dip, sdk_name: ring_tip, index: 18, view: side, tag_id: 6} + - {name: pinky_dip, sdk_name: little_tip, index: 19, view: side, tag_id: 5} +clearances: + index_roll_for_thumb: + targets: {index_mcp_roll: 0} + thumb_yaw_for_mcp: + targets: {thumb_cmc_yaw: 80} + pinky_fold: + targets: {pinky_mcp_pitch: 255, pinky_pip: 255, pinky_dip: 255} + ring_fold: + targets: {ring_mcp_pitch: 255, ring_pip: 255, ring_dip: 255} + middle_fold: + targets: {middle_mcp_pitch: 255, middle_pip: 255, middle_dip: 255} +tasks: + - {name: thumb_cmc_roll, joints: [thumb_cmc_roll], clearances: [index_roll_for_thumb], restore_after: [thumb_cmc_roll, index_mcp_roll]} + - {name: thumb_mcp, joints: [thumb_mcp], clearances: [thumb_yaw_for_mcp]} + - {name: thumb_ip, joints: [thumb_ip]} + - {name: four_finger_roll, joints: [index_mcp_roll, middle_mcp_roll, ring_mcp_roll, pinky_mcp_roll], allow_override: false} + - {name: pinky_mcp_pitch, joints: [pinky_mcp_pitch]} + - {name: pinky_pip, joints: [pinky_pip]} + - {name: pinky_dip, joints: [pinky_dip]} + - {name: ring_mcp_pitch, joints: [ring_mcp_pitch], clearances: [pinky_fold]} + - {name: ring_pip, joints: [ring_pip], clearances: [pinky_fold]} + - {name: ring_dip, joints: [ring_dip], clearances: [pinky_fold]} + - {name: middle_mcp_pitch, joints: [middle_mcp_pitch], clearances: [pinky_fold, ring_fold]} + - {name: middle_pip, joints: [middle_pip], clearances: [pinky_fold, ring_fold]} + - {name: middle_dip, joints: [middle_dip], clearances: [pinky_fold, ring_fold]} + - {name: index_mcp_pitch, joints: [index_mcp_pitch], clearances: [pinky_fold, ring_fold, middle_fold]} + - {name: index_pip, joints: [index_pip], clearances: [pinky_fold, ring_fold, middle_fold]} + - {name: index_dip, joints: [index_dip], clearances: [pinky_fold, ring_fold, middle_fold]} + - {name: thumb_cmc_yaw, joints: [thumb_cmc_yaw]} diff --git a/src/linkerhand_range_calibration/config/station.yaml b/src/linkerhand_range_calibration/config/station.yaml new file mode 100644 index 0000000..1e488a8 --- /dev/null +++ b/src/linkerhand_range_calibration/config/station.yaml @@ -0,0 +1,31 @@ +cameras: + front: {serial: DB2163742, intrinsics: '~/.ros/camera_info/hikrobot_DB2163742.yaml'} + side: {serial: DB2163749, intrinsics: '~/.ros/camera_info/hikrobot_DB2163749.yaml'} + top: {serial: DB2163739, intrinsics: '~/.ros/camera_info/hikrobot_DB2163739.yaml'} +camera: + width: 1624 + height: 1240 + frame_rate: 30.0 + exposure_time_us: 5000.0 + family: 36h11 + tag_size_m: 0.016 # 检测器接口参数；范围计算只使用像素角点。 +sdk: {comm_type: libcanbus, canfd_device: 0} +motion: {speed: 200, torque: 200} # 设备速度和力矩；手动示教直接跟随滑块目标。 +output_root: range_calibration_output +teaching_root: '~/.ros/linkerhand_range_calibration' +scan: + endpoint_repetitions: 1 # 每轮从0向上、从255向下各寻找一次；可设2进行复核。 + rate: 20 # 仅用于自动标定的准备姿态、避让及扫描轨迹，不限制手动示教。 + settle_seconds: 0.3 + stable_frames: 8 + point_timeout: 3 + freshness: 0.5 + feedback_tolerance: 2 + repeat_tolerance: 2 + motion_floor_px: 0.5 + noise_multiplier: 5 + stability_px: 1 + confirmation_points: 3 + max_hamming: 0 + min_margin: 30 + min_edge_px: 30 diff --git a/src/linkerhand_range_calibration/launch/calibrate.launch.py b/src/linkerhand_range_calibration/launch/calibrate.launch.py new file mode 100644 index 0000000..6d3b372 --- /dev/null +++ b/src/linkerhand_range_calibration/launch/calibrate.launch.py @@ -0,0 +1,88 @@ +"""Own launch wiring; the external vendor SDK and camera driver stay unchanged.""" +from pathlib import Path + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler, EmitEvent +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def build_actions(profile, station, profile_arg, station_arg, demo=False): + from linkerhand_range_calibration.runtime import NAMESPACE + from linkerhand_range_calibration.adapters.registry import adapter_type + arguments = ['--profile', profile_arg] + if station_arg: + arguments += ['--station', station_arg] + if demo: + arguments += ['--demo'] + controller = Node(package='linkerhand_range_calibration', executable='calibrate_range', + arguments=arguments, output='screen') + actions = [] + if not demo: + # Explicit package/executable invocation avoids the SDK launch's auto-init defaults. + actions.append(Node(package=profile.sdk['package'], executable=profile.sdk['executable'], + name=profile.sdk['node'].strip('/'), output='screen', + parameters=[adapter_type(profile.adapter).launch_parameters(profile, station)])) + components = [] + camera = station['camera'] + for view in profile.views: + device = station['cameras'][view] + intrinsics = Path(device['intrinsics']).expanduser().resolve() + if not intrinsics.is_file(): + raise ValueError(f'{view} 内参文件不存在: {intrinsics}') + prefix = f'{NAMESPACE}/{view}' + actions.append(Node(package='linkerhand_calibration', executable='hikrobot_camera_node', + name='hikrobot_camera', namespace=prefix+'/camera', output='screen', + parameters=[{'serial_number': device['serial'], + 'camera_name': 'hikrobot_'+device['serial'], + 'frame_id': f'range_{view}_optical_frame', + 'image_width': camera['width'], 'image_height': camera['height'], + 'camera_info_url': intrinsics.as_uri(), + 'frame_rate': float(camera['frame_rate']), + 'exposure_time_us': float(camera['exposure_time_us']), + 'gain_db': 0.0, 'auto_exposure': False}])) + components.extend([ + ComposableNode(package='image_proc', plugin='image_proc::RectifyNode', name='rectify', + namespace=prefix+'/camera', + remappings=[('image',prefix+'/camera/image_raw'), + ('camera_info',prefix+'/camera/camera_info'), + ('image_rect',prefix+'/camera/image_rect')], + parameters=[{'queue_size':1}], + extra_arguments=[{'use_intra_process_comms':True}]), + ComposableNode(package='apriltag_ros', plugin='AprilTagNode', name='apriltag', + namespace=prefix+'/apriltag', + remappings=[('image_rect',prefix+'/camera/image_rect'), + ('camera_info',prefix+'/camera/camera_info')], + parameters=[{'family':camera['family'], 'size':float(camera['tag_size_m']), + 'qos_profile':'sensor_data', 'max_hamming':0, + 'detector.threads':2, 'detector.decimate':1.0, + 'detector.refine':True}], + extra_arguments=[{'use_intra_process_comms':True}]), + ]) + actions.append(ComposableNodeContainer(name='range_vision', namespace=NAMESPACE, + package='rclcpp_components', executable='component_container_mt', + composable_node_descriptions=components, output='screen')) + actions.append(controller) + actions.append(RegisterEventHandler(OnProcessExit(target_action=controller, + on_exit=[EmitEvent(event=Shutdown(reason='标定界面已退出'))]))) + return actions + + +def launch_stack(context): + from linkerhand_range_calibration.profiles import load_profile, load_station + profile_arg = LaunchConfiguration('profile').perform(context) + station_arg = LaunchConfiguration('station').perform(context) + demo = LaunchConfiguration('demo').perform(context).lower() == 'true' + return build_actions(load_profile(profile_arg), load_station(station_arg or None), profile_arg, station_arg, demo) + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument('profile', default_value='o30_right'), + DeclareLaunchArgument('station', default_value=''), + DeclareLaunchArgument('demo', default_value='false', choices=['true','false']), + OpaqueFunction(function=launch_stack), + ]) diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/__init__.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/__init__.py new file mode 100644 index 0000000..4d1dd4e --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/__init__.py @@ -0,0 +1 @@ +"""Visual command-range calibration, independent of vendor SDK source code.""" diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/adapters/__init__.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/adapters/__init__.py new file mode 100644 index 0000000..3842304 --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/adapters/__init__.py @@ -0,0 +1 @@ +"""Hardware interfaces and a deterministic simulator.""" diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/adapters/base.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/adapters/base.py new file mode 100644 index 0000000..5fa4d81 --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/adapters/base.py @@ -0,0 +1,19 @@ +from dataclasses import dataclass +from typing import Protocol + + +@dataclass(frozen=True) +class Feedback: + stamp_ns: int + received: float + positions: tuple[float, ...] + + +class HandAdapter(Protocol): + uid: str + feedback: Feedback | None + + def control_error(self, now: float) -> str | None: ... + def diagnostic_warning(self) -> str | None: ... + def send_positions(self, positions): ... + def set_motion(self, speed, torque): ... diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/adapters/fake.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/adapters/fake.py new file mode 100644 index 0000000..a021dcd --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/adapters/fake.py @@ -0,0 +1,50 @@ +"""No ROS imports or hardware access: virtual hand and projected tag observations.""" +import numpy as np + +from .base import Feedback + + +class FakeAdapter: + def __init__(self, profile, ranges=None): + self.profile, self.uid = profile, 'SIMULATED_HAND' + self.target = tuple(j.minimum for j in profile.joints) + self.feedback = Feedback(0, 0, self.target) + self.ranges = ranges or {j.name: (j.minimum, j.maximum) for j in profile.joints} + self.sent, self.settings_sent = [], [] + self.error = None + self.warning = None + self.external_publishers = 0 + + def control_error(self, now): + if self.external_publishers: + return '存在其他位置命令发布者' + return self.error + + def diagnostic_warning(self): + return self.warning + + def send_positions(self, positions): + self.target = self.profile.vector(positions) + self.sent.append(self.target) + + def set_motion(self, speed, torque): + self.settings_sent.append((speed, torque)) + + def advance(self, now, stamp_ns): + # Feedback intentionally echoes targets. Only image motion identifies limits. + self.feedback = Feedback(stamp_ns, now, self.target) + + def frames(self, noise=0, rng=None): + frames = {view: {} for view in self.profile.views} + displacements = {} + for j in self.profile.joints: + lo, hi = self.ranges[j.name] + key = j.view, j.tag_id + displacements[key] = displacements.get(key, 0) + .8*float(np.clip(self.target[j.index], lo, hi)) + for (view, tag_id), displacement in displacements.items(): + points = np.array([[100, 100], [140, 100], [140, 140], [100, 140]], dtype=float) + points += [tag_id*65 + displacement, 50] + if noise: + points += rng.normal(0, noise, points.shape) + frames[view][tag_id] = points.tolist() + return frames diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/adapters/o30_ros.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/adapters/o30_ros.py new file mode 100644 index 0000000..efc2a3d --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/adapters/o30_ros.py @@ -0,0 +1,178 @@ +"""O30 ROS boundary. Does not import, copy, patch or open the vendor SDK.""" +import json +import math +import time + +from rcl_interfaces.srv import GetParameters +from sensor_msgs.msg import JointState +from std_msgs.msg import String + +from .base import Feedback + + +class O30RosAdapter: + @staticmethod + def launch_parameters(profile, station): + return {**station['sdk'], 'hand_type': profile.side, 'hand_joint': profile.model, + 'auto_init_pose': False, 'is_touch': False, 'state_rate': 30.0, + 'strict_device_check': True, 'ignore_joint_faults': False, + 'joint_limit_min': [int(j.minimum) for j in profile.joints], + 'joint_limit_max': [int(j.maximum) for j in profile.joints], 'cmd_timeout': 0.0} + + def __init__(self, node, profile, settings): + self.node, self.profile, self.settings = node, profile, settings + self.joints_by_sdk_name = {joint.sdk_name: joint for joint in profile.joints} + self.uid, self.feedback, self.info = '', None, None + self.feedback_error, self.info_error = '', '' + self.info_at = float('-inf') + self.parameters_error = '尚未确认 SDK 启动参数和限位' + self.parameters_ok = False + self.next_check = 0 + self.next_graph = 0 + self.graph_error = '尚未检查控制话题' + self.parameter_future = None + self.publisher = node.create_publisher(JointState, profile.sdk['command_topic'], 1) + self.setting_pub = node.create_publisher(String, profile.sdk['setting_topic'], 10) + self.state_sub = node.create_subscription(JointState, profile.sdk['feedback_topic'], self._feedback, 20) + self.info_sub = node.create_subscription(String, profile.sdk['info_topic'], self._info, 10) + self.param_client = node.create_client(GetParameters, profile.sdk['node'].rstrip('/') + '/get_parameters') + self.timer = node.create_timer(0.5, self._check_parameters) + + def _feedback(self, message): + expected = [j.sdk_name for j in self.profile.joints] + positions = list(message.position) + if list(message.name) != expected or len(positions) != len(expected): + self.feedback_error = 'SDK 反馈关节名称、顺序或数量不符' + return + if any(not math.isfinite(x) or x < j.minimum or x > j.maximum + for j, x in zip(self.profile.joints, positions)): + self.feedback_error = 'SDK 反馈数值无效' + return + stamp = message.header.stamp.sec*1_000_000_000 + message.header.stamp.nanosec + now_stamp = self.node.get_clock().now().nanoseconds + if stamp <= 0 or abs(now_stamp-stamp) > int(self.settings.freshness*1e9): + self.feedback_error = 'SDK 反馈时间戳过期或不在同一时钟域' + return + if self.feedback and stamp <= self.feedback.stamp_ns: + return + self.feedback_error = '' + self.feedback = Feedback(stamp, time.monotonic(), tuple(positions)) + + def _info(self, message): + try: + data = json.loads(message.data) + if not isinstance(data, dict): + raise ValueError('诊断信息不是对象') + self.info, self.info_at = data, time.monotonic() + self.uid = str(data.get('uid') or '').strip() + self.info_error = '' + except (ValueError, TypeError) as error: + self.info_error = f'SDK 诊断解析失败: {error}' + + def _check_parameters(self): + now = time.monotonic() + if self.parameter_future and not self.parameter_future.done(): + if now-self.request_at > 3: + self.parameter_future.cancel() + self.parameter_future = None + self.parameters_ok = False + self.parameters_error = '读取 SDK 参数超时' + return + if now < self.next_check or not self.param_client.service_is_ready(): + return + self.next_check, self.request_at = now+2, now + names = ['hand_type', 'hand_joint', 'auto_init_pose', 'joint_limit_min', + 'joint_limit_max', 'cmd_timeout'] + self.parameter_future = self.param_client.call_async(GetParameters.Request(names=names)) + self.parameter_future.add_done_callback(self._parameters) + + def _parameters(self, future): + try: + values = future.result().values + if len(values) != 6: + raise ValueError('参数数量不符') + if values[0].string_value != self.profile.side or values[1].string_value != self.profile.model: + raise ValueError('SDK 型号或左右手参数不符') + if values[2].bool_value or values[5].double_value != 0: + raise ValueError('SDK 必须关闭自动摆位和命令超时自动动作') + lows, highs = list(values[3].integer_array_value), list(values[4].integer_array_value) + if lows != [int(j.minimum) for j in self.profile.joints] or highs != [int(j.maximum) for j in self.profile.joints]: + raise ValueError('SDK 软件限位未覆盖完整扫描范围') + self.parameters_ok, self.parameters_error = True, '' + except Exception as error: + self.parameters_ok, self.parameters_error = False, f'SDK 参数检查失败: {error}' + + def control_error(self, now): + """Require the command endpoint and valid feedback, as used by calibration. + + Runtime SDK diagnostic flags are telemetry, not another motion interlock. + SDK initialization and device protections remain the SDK's responsibility. + """ + if now >= self.next_graph: + self.next_graph = now+0.2 + publishers = self.node.get_publishers_info_by_topic(self.profile.sdk['command_topic']) + others = [p for p in publishers + if (p.node_name, p.node_namespace) != (self.node.get_name(), self.node.get_namespace())] + self.graph_error = ('存在其他位置命令发布者或发布者数量异常，请关闭其他控制器' if others or len(publishers) != 1 else + ('SDK 未订阅位置命令' if self.publisher.get_subscription_count() != 1 else '')) + if self.graph_error: + return self.graph_error + if not self.parameters_ok: + return self.parameters_error + if self.feedback_error or self.info_error: + return self.feedback_error or self.info_error + if not self.feedback or now-self.feedback.received > self.settings.freshness: + return 'SDK 位置反馈未收到或已断流' + if self.info is None or now-self.info_at > 3: + return 'SDK 诊断未收到或已断流' + info = self.info + if (info.get('model'), str(info.get('side', '')).lower(), info.get('hand_type')) != ( + self.profile.model, self.profile.side, self.profile.side): + return '实机型号或左右手身份不符' + if not self.uid or info.get('joint_names') != [j.sdk_name for j in self.profile.joints]: + return '设备 UID 或诊断关节名称不符' + return None + + def _joint_label(self, sdk_name): + joint = self.joints_by_sdk_name.get(sdk_name) + return f'{joint.name}（SDK: {sdk_name}）' if joint else sdk_name + + def diagnostic_warning(self): + """Keep runtime diagnostics in the journal without gating commands.""" + info = self.info or {} + reasons = [] + faults = [] + for sdk_name, entries in info.get('joint_faults', {}).items(): + if entries: + detail = '、'.join(map(str, entries)) if isinstance(entries, list) else str(entries) + faults.append(f'{self._joint_label(sdk_name)}：{detail}') + if faults: + reasons.append('SDK 关节故障：' + '；'.join(faults)) + temperatures = [] + for entry in info.get('over_temp', []): + sdk_name, separator, value = str(entry).partition(':') + temperatures.append(f'{self._joint_label(sdk_name)}：{value}°C' if separator else str(entry)) + if temperatures: + reasons.append('SDK 温度异常：' + '；'.join(temperatures)) + if info.get('online') is False: + reasons.append('SDK 心跳诊断：未确认在线') + comm_error = info.get('comm_error', {}) + if comm_error.get('code', 0): + reasons.append('SDK 通信诊断：' + json.dumps(comm_error, ensure_ascii=False, sort_keys=True)) + return '；'.join(reasons) or None + + def send_positions(self, positions): + values = self.profile.vector(positions) + msg = JointState() + msg.header.stamp = self.node.get_clock().now().to_msg() + msg.name = [j.sdk_name for j in self.profile.joints] + msg.position = [float(x) for x in values] + self.publisher.publish(msg) + + def set_motion(self, speed, torque): + for command, field, value in [('set_speed', 'speed', speed), + ('set_max_torque_limits', 'torque', torque)]: + if not isinstance(value, int) or not 0 <= value <= 255: + raise ValueError('速度和力矩必须为0～255整数') + self.setting_pub.publish(String(data=json.dumps({'setting_cmd': command, 'params': { + 'hand_type': self.profile.side, field: [value]*len(self.profile.joints)}}))) diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/adapters/registry.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/adapters/registry.py new file mode 100644 index 0000000..78f00e9 --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/adapters/registry.py @@ -0,0 +1,12 @@ +"""One explicit registration per SDK; common launch/runtime have no model branches.""" +from importlib import import_module + + +ADAPTERS = {'o30_ros': ('linkerhand_range_calibration.adapters.o30_ros', 'O30RosAdapter')} + + +def adapter_type(key): + if key not in ADAPTERS: + raise ValueError(f'未注册 SDK 适配器: {key}') + module, name = ADAPTERS[key] + return getattr(import_module(module), name) diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/cli.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/cli.py new file mode 100644 index 0000000..90755c4 --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/cli.py @@ -0,0 +1,79 @@ +"""Entry point: static validation and replay intentionally precede ROS imports.""" +import argparse +from pathlib import Path +import signal +import sys +from threading import Thread + +from .profiles import load_profile, load_station, Settings + + +def main(argv=None): + parser = argparse.ArgumentParser(description='LinkerHand 视觉行程标定') + parser.add_argument('--profile', default='o30_right') + parser.add_argument('--station') + mode = parser.add_mutually_exclusive_group() + mode.add_argument('--validate-only', action='store_true', help='仅检查配置，不启动ROS或硬件') + mode.add_argument('--simulate', action='store_true', help='虚拟时间模拟全手扫描，不连接硬件') + mode.add_argument('--demo', action='store_true', help='使用假SDK和合成预览打开界面') + mode.add_argument('--replay', help='从 samples.jsonl 离线复算') + parser.add_argument('--output', help='模拟输出目录或离线复算JSON路径') + args, ros_args = parser.parse_known_args(argv) + try: + if args.replay: + from .replay import replay + output = args.output or str(Path(args.replay).with_name('recomputed_ranges.json')) + replay(args.replay, output) + print(f'离线复算完成：{output}') + return 0 + profile, station = load_profile(args.profile), load_station(args.station) + if not set(profile.views) <= set(station['cameras']): + raise ValueError('工位配置缺少所需相机') + settings = Settings.from_dict(station.get('scan', {})) + if args.validate_only: + print(f'配置通过：{profile.model} {profile.side}，{len(profile.joints)} 关节，{len(profile.tasks)} 任务。') + for view in profile.views: + path = Path(station['cameras'][view]['intrinsics']).expanduser() + print(f'{view}: {station["cameras"][view]["serial"]}；内参文件：{"存在" if path.is_file() else "尚不存在"}') + print('未启动 ROS、SDK 或相机。实机身份、示教和内参有效性将在运行时检查。') + return 0 + if args.simulate: + from .simulation import simulate + path = simulate(profile, settings, args.output or station['output_root']) + print(f'模拟全手标定完成：{path}') + return 0 + except (ValueError, OSError, KeyError) as error: + parser.error(str(error)) + import rclpy + from rclpy.executors import SingleThreadedExecutor + from PyQt5.QtWidgets import QApplication + from .runtime import CalibrationRuntime + from .ui.window import CalibrationWindow + + rclpy.init(args=ros_args) + app = QApplication.instance() or QApplication([sys.argv[0]]) + runtime = CalibrationRuntime(profile, station, demo=args.demo) + executor = SingleThreadedExecutor(); executor.add_node(runtime) + def spin(): + from rclpy.executors import ExternalShutdownException + try: + executor.spin() + except ExternalShutdownException: + pass + thread = Thread(target=spin, daemon=True); thread.start() + window = CalibrationWindow(runtime); window.show() + signal.signal(signal.SIGINT, lambda *unused: app.quit()) + signal.signal(signal.SIGTERM, lambda *unused: app.quit()) + try: + return app.exec_() + finally: + executor.shutdown() + thread.join(timeout=3) + runtime.close_session() + runtime.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/core/__init__.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/__init__.py new file mode 100644 index 0000000..4b68001 --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/__init__.py @@ -0,0 +1 @@ +"""Deterministic calibration logic without ROS or Qt imports.""" diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/core/analysis.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/analysis.py new file mode 100644 index 0000000..ebc272a --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/analysis.py @@ -0,0 +1,78 @@ +"""Full-sweep analysis retained for replay of older coarse/fine journals.""" +import numpy as np + +from ..vision.observations import rms_delta +from .errors import Unmeasurable + + +class RefineWindow(ValueError): + pass + + +def grid(lo, hi, step): + values = [float(x) for x in np.arange(lo, hi + step * 1e-6, step)] + if not values or abs(values[-1] - hi) > 1e-6: + values.append(float(hi)) + return values + + +def _departure(distances, threshold, count): + """Confirm displacement from an endpoint, not motion between adjacent samples.""" + for index in range(1, len(distances) - count + 1): + if all(d > threshold for d in distances[index:index+count]): + return index + raise Unmeasurable('未检测到足够观测点确认离开端点姿态') + + +def direction_range(samples, joint, threshold, settings, coarse=False): + """Find the outer command boundaries regardless of motion within the range.""" + samples = sorted(samples, key=lambda s: s['command']) + if len(samples) < settings.confirmation_points+1 or len({s['command'] for s in samples}) != len(samples): + raise Unmeasurable('采样点不足或重复') + commands = [s['command'] for s in samples] + points = [s['observations'][joint.name]['corners'] for s in samples] + if abs(commands[0]-joint.minimum) > 1e-6 or abs(commands[-1]-joint.maximum) > 1e-6: + raise Unmeasurable('缺少指令端点观测') + lower = _departure([rms_delta(p, points[0]) for p in points], threshold, + settings.confirmation_points) + reverse = _departure([rms_delta(p, points[-1]) for p in reversed(points)], threshold, + settings.confirmation_points) + upper = len(points) - 1 - reverse + lo, hi = commands[lower-1], commands[upper+1] + if lo >= hi: + raise Unmeasurable('两端平台重叠或运动范围无法区分') + brackets = ((commands[lower-1], commands[lower]), (commands[upper], commands[upper+1])) + if not coarse and any(b-a > joint.resolution + 1e-6 for a, b in brackets): + raise RefineWindow('细扫窗口未完整覆盖边界，需扩大窗口') + return {'min': lo, 'max': hi, 'brackets': brackets} + + +def fine_commands(joint, estimates, radius): + values = {joint.minimum, joint.maximum} + for estimate in estimates: + for a, b in estimate['brackets']: + first = max(0, int(np.floor((a-radius-joint.minimum)/joint.resolution))) + last = min(round((joint.maximum-joint.minimum)/joint.resolution), + int(np.ceil((b+radius-joint.minimum)/joint.resolution))) + values.update(joint.minimum + i*joint.resolution for i in range(first, last+1)) + return sorted(values) + + +def aggregate(joint, sweeps, threshold, settings): + expected = {(r, d) for r in range(settings.fine_repetitions) for d in ('up', 'down')} + if set(sweeps) != expected: + raise Unmeasurable('正反向复测数据不完整') + estimates = {key: direction_range(samples, joint, threshold, settings) + for key, samples in sweeps.items()} + for direction in ('up', 'down'): + selected = [v for (r, d), v in estimates.items() if d == direction] + for bound in ('min', 'max'): + values = [v[bound] for v in selected] + if max(values)-min(values) > settings.repeat_tolerance: + raise Unmeasurable(f'{direction} 方向 {bound} 复测差异过大') + lo = max(v['min'] for v in estimates.values()) + hi = min(v['max'] for v in estimates.values()) + if lo >= hi: + raise Unmeasurable('正反向复测区间无有效交集') + return {'min': joint.validate_value(lo), 'max': joint.validate_value(hi), 'status': '成功', + 'directions': {f'{r}_{d}': v for (r, d), v in estimates.items()}} diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/core/endpoints.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/endpoints.py new file mode 100644 index 0000000..380dc71 --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/endpoints.py @@ -0,0 +1,99 @@ +"""Incremental searches from both command limits, shared by live scans and replay.""" +import math + +import numpy as np + +from .errors import Unmeasurable +from ..vision.observations import rms_delta + + +SCAN_METHOD = 'endpoint_search_v1' + + +class EndpointSearch: + """Confirm the first departure from a fixed endpoint reference. + + Observations are stable summaries at consecutive command values. Confirmation + retains the first trigger; later points never move an already confirmed bound. + """ + + def __init__(self, joint, direction, settings): + if direction not in ('up', 'down'): + raise ValueError('端点搜索方向无效') + self.joint, self.direction, self.settings = joint, direction, settings + self.sign = 1 if direction == 'up' else -1 + self.origin = joint.minimum if direction == 'up' else joint.maximum + self.terminal = joint.maximum if direction == 'up' else joint.minimum + self.steps = 0 + self.reference, self.threshold = None, None + self.candidate, self.support = None, 0 + self.boundary, self.error = None, None + + @property + def done(self): + return self.boundary is not None or self.error is not None + + def add(self, command, observation): + if self.done: + return + command = self.joint.validate_value(command) + expected = self.origin + self.sign * self.steps * self.joint.resolution + if not math.isclose(command, expected, rel_tol=0, abs_tol=1e-6): + raise Unmeasurable('端点搜索采样必须从指令端点按分辨率连续推进，不能缺点或重复') + points = np.asarray(observation['corners'], dtype=float) + if points.shape != (4, 2) or not np.isfinite(points).all(): + raise Unmeasurable('端点角点观测无效') + if self.reference is None: + noise = float(observation['noise']) + if not math.isfinite(noise) or noise < 0: + raise Unmeasurable('端点噪声估计无效') + self.reference = points.copy() + self.threshold = max(self.settings.motion_floor_px, self.settings.noise_multiplier * noise) + elif rms_delta(points, self.reference) > self.threshold: + if self.candidate is None: + self.candidate = command + self.support += 1 + if self.support >= self.settings.confirmation_points: + bound = self.candidate - self.sign * self.joint.resolution + self.boundary = { + 'bound': self.joint.validate_value(bound), 'trigger': self.candidate, + 'confirmed_at': command, 'threshold': self.threshold, + } + else: + self.candidate, self.support = None, 0 + self.steps += 1 + if self.boundary is None and math.isclose(command, self.terminal, rel_tol=0, abs_tol=1e-6): + end = '低端' if self.direction == 'up' else '高端' + self.error = end + '已扫描至对端，未获得足够观测点确认起动边界' + + def result(self): + if self.boundary is None: + raise Unmeasurable(self.error or '端点搜索数据不足，起动边界尚未确认') + return dict(self.boundary) + + +def aggregate_endpoints(joint, endpoints, settings): + expected = {(r, d) for r in range(settings.endpoint_repetitions) for d in ('up', 'down')} + if set(endpoints) != expected: + raise Unmeasurable('两端边界数据不完整') + limits = {} + for direction, bound in (('up', 'min'), ('down', 'max')): + values = [value['bound'] for (repeat, side), value in endpoints.items() if side == direction] + if max(values) - min(values) > settings.repeat_tolerance: + raise Unmeasurable(f'{bound} 端点复测差异过大') + limits[bound] = joint.validate_value(max(values) if bound == 'min' else min(values)) + if limits['min'] >= limits['max']: + raise Unmeasurable('两端边界重叠或复测区间无有效交集') + return {**limits, 'status': '成功', + 'endpoints': {f'{r}_{d}': value for (r, d), value in endpoints.items()}} + + +def analyze_endpoint_samples(joint, sweeps, settings): + endpoints = {} + for repeat in range(settings.endpoint_repetitions): + for direction in ('up', 'down'): + search = EndpointSearch(joint, direction, settings) + for sample in sweeps.get((repeat, direction), []): + search.add(sample['command'], sample['observations'][joint.name]) + endpoints[(repeat, direction)] = search.result() + return aggregate_endpoints(joint, endpoints, settings) diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/core/engine.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/engine.py new file mode 100644 index 0000000..8b8c727 --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/engine.py @@ -0,0 +1,371 @@ +"""Event-driven calibration engine. Only this object owns motion commands.""" +from collections import deque + +import numpy as np + +from .endpoints import SCAN_METHOD, EndpointSearch, aggregate_endpoints +from .errors import Unmeasurable +from .trajectory import Trajectory +from ..vision.observations import Observation, StableWindow + + +class Engine: + AUTOMATIC = {'PREPARING', 'SCANNING', 'RESTORING'} + ACTIVE = AUTOMATIC | {'TEACHING'} + + def __init__(self, profile, settings, adapter, emit=lambda event: None): + self.profile, self.settings, self.adapter, self.emit = profile, settings, adapter, emit + self.state, self.reason = 'IDLE', '' + self.command = None + self.task = None + self.tasks = [] + self.task_index = 0 + self.results = {j.name: {'min': None, 'max': None, 'status': '未完成'} for j in profile.joints} + self.latest, self.view_stamps = {}, {} + self.feedbacks = deque(maxlen=64) + self.windows = {} + self.trajectory = None + self.restoration_steps, self.restoring_joint = deque(), '' + self.manual_target, self.manual_uid = None, None + self.manual_pending, self.manual_gate_ns = False, 0 + self.manual_finished = float('-inf') + self.phase = '' + self.stage, self.direction, self.repeat = '', '', 0 + self.progress = (0, 0) + self.last_diagnostic_warning = None + + @property + def active(self): + return self.state in self.ACTIVE + + @property + def manual_allowed(self): + return self.state in ('IDLE', 'TEACHING', 'CANCELLED', 'COMPLETED') + + def _require_control(self, now): + reason = self.adapter.control_error(now) + if reason: + raise ValueError(reason) + + def _record(self, kind, **data): + self.emit({'kind': kind, **data}) + + def start(self, teaching, now, stamp_ns, task_names=None): + if self.active or self.state == 'PAUSED': + raise ValueError('请先结束当前任务') + self._require_control(now) + self.tasks = [t for t in self.profile.tasks if task_names is None or t.name in task_names] + if not self.tasks: + raise ValueError('没有选中的任务') + if task_names and set(task_names) != {t.name for t in self.tasks}: + raise ValueError('包含未知任务') + if teaching.missing(self.tasks): + raise ValueError('缺少示教: ' + ', '.join(teaching.missing(self.tasks))) + self.teaching, self.session_uid, self.task_index = teaching, self.adapter.uid, 0 + self.results = {j.name: {'min': None, 'max': None, 'status': '未完成'} for j in self.profile.joints} + self.command = self.profile.vector(self.adapter.feedback.positions) + self.last_diagnostic_warning = None + self._begin_task(now, stamp_ns) + + def _begin_task(self, now, stamp_ns): + self.restoration_steps.clear() + self.restoring_joint = '' + self.task = self.tasks[self.task_index] + self.prepare_pose = self.teaching.prepare(self.task) + self.specs = [self.profile.by_name[name] for name in self.task.joints] + self.active_indices = {j.index for j in self.specs} + self.keys = {j.name: (j.view, j.tag_id) for j in self.specs} + self.windows = {name: StableWindow(self.settings.stable_frames) for name in self.task.joints} + self.samples, self.failures = {}, {} + self.endpoint_results = {name: {} for name in self.task.joints} + self.searches = {} + self.reason = '' + self.stage, self.direction, self.repeat = 'prepare', '', 0 + self.progress = (0, 2 * len(self.specs) * self.settings.endpoint_repetitions) + self.state = 'PREPARING' + self._record('task_started', task=self.task.name, joints=list(self.task.joints), scan_method=SCAN_METHOD) + self._move(self.prepare_pose, now) + + def _move(self, target, now): + self.trajectory = Trajectory(self.profile, self.command, target, self.settings.rate, now) + self.phase = 'moving' + self.blocked_since = None + for window in self.windows.values(): + window.clear() + + def observe(self, view, stamp_ns, accepted, now): + """One whole detector frame; missing IDs explicitly invalidate observations.""" + if stamp_ns <= self.view_stamps.get(view, -1): + return + self.view_stamps[view] = stamp_ns + for key in [k for k in self.latest if k[0] == view]: + self.latest.pop(key) + for tag_id, corners in accepted.items(): + self.latest[(view, tag_id)] = Observation(stamp_ns, now, tuple(tuple(p) for p in corners)) + if self.state not in ('PREPARING', 'SCANNING') or self.phase != 'waiting': + return + required = [(name, key) for name, key in self.keys.items() if key[0] == view] + if any(key not in self.latest for _, key in required): + for window in self.windows.values(): + window.clear() + return + for name, key in required: + obs = self.latest[key] + if obs.stamp_ns >= self.gate_ns: + self.windows[name].add(obs) + + def feedback_stable(self, after_ns=0, indices=None): + """Check selected channels; preparation and teaching use the whole hand.""" + count = self.settings.stable_frames + selected = [f for f in self.feedbacks if f.stamp_ns >= after_ns][-count:] + if len(selected) < count: + return False + points = np.asarray([f.positions for f in selected]) + if indices is not None: + points = points[:, list(indices)] + return bool((np.ptp(points, axis=0) <= self.settings.feedback_tolerance).all()) + + def can_capture_teaching(self, now): + return (not self.active and self.state != 'PAUSED' and self.feedback_stable(self.manual_gate_ns) + and now-self.manual_finished >= self.settings.settle_seconds + and not self.adapter.control_error(now)) + + def manual_move(self, target, now): + """Queue the operator's latest target; scan trajectories do not shape teaching.""" + if not self.manual_allowed: + raise ValueError('标定或暂停期间不能手动调节') + target = self.profile.vector(target) + self._require_control(now) + if self.state == 'TEACHING' and self.adapter.uid != self.manual_uid: + raise ValueError('设备 UID 改变，请取消后重新示教') + self.manual_target, self.manual_pending = target, True + self.manual_uid = self.adapter.uid + self.trajectory = None + self.tasks, self.task, self.task_index = [], None, 0 + self.stage, self.direction, self.repeat, self.progress = '', '', 0, (0, 0) + self.state, self.reason = 'TEACHING', '' + + def manual_adjust(self, targets, now): + """Change selected joints while retaining every other current target.""" + if self.state == 'TEACHING': + target = list(self.manual_target) + else: + target = list(self.command if self.command is not None else self.adapter.feedback.positions) + for name, value in targets.items(): + joint = self.profile.by_name.get(name) + if joint is None: + raise ValueError(f'未知关节: {name}') + target[joint.index] = joint.validate_value(value) + self.manual_move(target, now) + + def pause(self, reason): + self.state, self.reason = 'PAUSED', str(reason) + self.trajectory = None + self.manual_target, self.manual_pending = None, False + for window in self.windows.values(): + window.clear() + self._record('paused', reason=self.reason) + + def resume(self, now, stamp_ns): + if self.state != 'PAUSED' or not self.tasks: + raise ValueError('没有可恢复的扫描任务，请取消后重新操作') + self._require_control(now) + if self.adapter.uid != self.session_uid: + raise ValueError('设备 UID 已改变，不能继续该会话') + # Always reacquire the interrupted task, never combine incompatible passes. + self.command = self.profile.vector(self.adapter.feedback.positions) + self._begin_task(now, stamp_ns) + + def cancel(self): + self.state, self.reason, self.trajectory = 'CANCELLED', '已取消，保持最后目标', None + self.manual_target, self.manual_pending = None, False + self._record('cancelled') + + def _visible(self, now): + return all(key in self.latest and now-self.latest[key].received <= self.settings.freshness + for key in self.keys.values()) + + def tick(self, now, stamp_ns): + feedback = self.adapter.feedback + if feedback and (not self.feedbacks or feedback.stamp_ns > self.feedbacks[-1].stamp_ns): + self.feedbacks.append(feedback) + if not self.active: + return + if self.state in self.AUTOMATIC: + warning = self.adapter.diagnostic_warning() + if warning != self.last_diagnostic_warning: + self._record('diagnostic_warning', reason=warning, time=now, stamp_ns=stamp_ns, + task=self.task.name, stage=self.stage, direction=self.direction, + repeat=self.repeat, target=list(self.command), + feedback=list(feedback.positions) if feedback else None) + self.last_diagnostic_warning = warning + error = self.adapter.control_error(now) + expected_uid = self.manual_uid if self.state == 'TEACHING' else self.session_uid + if not error and self.adapter.uid != expected_uid: + error = '设备 UID 改变' + if error: + self.pause(error) + return + if self.state == 'TEACHING': + self._tick_manual(now, stamp_ns) + return + if self.phase == 'moving': + hold = self.state == 'SCANNING' and not self._visible(now) + if hold: + if self.blocked_since is None: + self.blocked_since = now + if now-self.blocked_since > self.settings.point_timeout: + self.pause('目标 Tag 持续不可见或观测过期') + return + else: + self.blocked_since = None + values, done = self.trajectory.advance(now, hold=hold) + if not hold: + self._send(values, now, stamp_ns) + if done and not hold: + self.phase = 'waiting' + self.arrived = now + self.gate_ns = stamp_ns + int(self.settings.settle_seconds*1e9) + for window in self.windows.values(): + window.clear() + return + if now-self.arrived > self.settings.point_timeout: + self.pause('测后回位稳定超时：检查反馈' if self.state == 'RESTORING' + else '单点稳定采样超时：检查 Tag 可见性、图像质量和反馈') + return + if self.state == 'RESTORING': + if self.feedback_stable(self.gate_ns): + self._record('restored', task=self.task.name, joints=[self.restoring_joint], + target=list(self.command), + feedback=list(feedback.positions), stamp_ns=stamp_ns) + self._next_restoration(now) + return + feedback_indices = self.active_indices if self.state == 'SCANNING' else None + if not self._visible(now) or not self.feedback_stable(self.gate_ns, feedback_indices): + for window in self.windows.values(): + window.clear() + return + summaries = {name: window.summary(self.settings.stability_px) + for name, window in self.windows.items()} + if any(value is None for value in summaries.values()): + return + if self.state == 'PREPARING': + self._record('prepared', task=self.task.name, target=list(self.command), + feedback=list(feedback.positions)) + self.state = 'SCANNING' + self._begin_endpoint('up', now) + else: + sample = {'command': self.point_command, 'observations': summaries, + 'feedback': list(feedback.positions), 'target': list(self.command), + 'feedback_stamp_ns': feedback.stamp_ns} + key = (self.repeat, self.direction) + self.samples.setdefault(key, []).append(sample) + self._record('sample', task=self.task.name, stage='endpoint', repeat=self.repeat, + direction=self.direction, **sample) + self._accept_endpoint_sample(summaries, now) + + def _tick_manual(self, now, stamp_ns): + if self.manual_pending: + self._send(self.manual_target, now, stamp_ns) + self.manual_pending = False + self.manual_finished = now + self.manual_gate_ns = stamp_ns + int(self.settings.settle_seconds * 1e9) + elif (now - self.manual_finished >= self.settings.settle_seconds and + self.feedback_stable(self.manual_gate_ns)): + # A published target does not mean that the real hand has settled. + self.state = 'IDLE' + + def _send(self, values, now, stamp_ns): + self.adapter.send_positions(values) + self.command = tuple(values) + self._record('command', target=list(values), stamp_ns=stamp_ns, time=now, + state=self.state, task=self.task.name if self.task else None) + + def _begin_endpoint(self, direction, now): + self.direction, self.stage = direction, 'reference' + self.searches = {joint.name: EndpointSearch(joint, direction, self.settings) + for joint in self.specs if joint.name not in self.failures} + self.point_index = 0 + self._move_to_endpoint_point(now) + + def _move_to_endpoint_point(self, now): + joint = self.specs[0] + origin, sign = (joint.minimum, 1) if self.direction == 'up' else (joint.maximum, -1) + self.point_command = joint.validate_value(origin + sign * self.point_index * joint.resolution) + goal = list(self.prepare_pose) + for joint in self.specs: + goal[joint.index] = self.point_command + self._move(goal, now) + + def _accept_endpoint_sample(self, summaries, now): + for name, search in self.searches.items(): + if search.done: + continue + search.add(self.point_command, summaries[name]) + if search.boundary is not None: + boundary = search.result() + self.endpoint_results[name][(self.repeat, self.direction)] = boundary + self._record('boundary_found', task=self.task.name, joint=name, repeat=self.repeat, + direction=self.direction, **boundary) + elif search.error: + self.failures[name] = search.error + self._record('boundary_failed', task=self.task.name, joint=name, repeat=self.repeat, + direction=self.direction, reason=search.error) + total_per_joint = 2 * self.settings.endpoint_repetitions + self.progress = (sum(total_per_joint if name in self.failures else len(values) + for name, values in self.endpoint_results.items()), self.progress[1]) + if not all(search.done for search in self.searches.values()): + self.stage = 'search' + self.point_index += 1 + self._move_to_endpoint_point(now) + elif len(self.failures) == len(self.specs): + self._finish_task({}, now) + elif self.direction == 'up': + self._begin_endpoint('down', now) + elif self.repeat + 1 < self.settings.endpoint_repetitions: + self.repeat += 1 + self._begin_endpoint('up', now) + else: + self._endpoints_finished(now) + + def _endpoints_finished(self, now): + outcomes = {} + for joint in self.specs: + if joint.name in self.failures: + continue + try: + outcomes[joint.name] = aggregate_endpoints(joint, self.endpoint_results[joint.name], self.settings) + except Unmeasurable as error: + self.failures[joint.name] = str(error) + self._finish_task(outcomes, now) + + def _finish_task(self, outcomes, now): + for name in self.task.joints: + self.results[name] = outcomes.get(name, {'min': None, 'max': None, 'status': '失败', + 'reason': self.failures.get(name, '数据不足')}) + self._record('task_result', task=self.task.name, + results={name: self.results[name] for name in self.task.joints}) + self.restoration_steps = deque(self.teaching.restoration_steps(self.task, self.command)) + self._next_restoration(now) + + def _next_restoration(self, now): + while self.restoration_steps: + name, target = self.restoration_steps.popleft() + if target == self.command: + continue + self.restoring_joint = name + self.state, self.stage, self.direction = 'RESTORING', 'restore', '' + self._record('restoration_started', task=self.task.name, joints=[name], + target=list(target)) + self._move(target, now) + return + self.restoring_joint = '' + self._advance_task(now) + + def _advance_task(self, now): + self.task_index += 1 + if self.task_index == len(self.tasks): + self.state, self.reason = 'COMPLETED', '标定结束，请查看每个关节的结果' + self._record('completed') + else: + self._begin_task(now, 0) diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/core/errors.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/errors.py new file mode 100644 index 0000000..0ff4cb7 --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/errors.py @@ -0,0 +1,5 @@ +"""Shared measurement errors for live scans and offline analysis.""" + + +class Unmeasurable(ValueError): + pass diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/core/trajectory.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/trajectory.py new file mode 100644 index 0000000..f23b843 --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/core/trajectory.py @@ -0,0 +1,19 @@ +"""Rate-limited vector motion; a held tick never accumulates catch-up time.""" +import numpy as np + + +class Trajectory: + def __init__(self, profile, start, goal, rate, now): + self.profile = profile + self.value = np.asarray(profile.vector(start), dtype=float) + self.goal = np.asarray(profile.vector(goal), dtype=float) + self.rate, self.previous = rate, now + + def advance(self, now, hold=False): + dt = max(0.0, min(now-self.previous, 0.1)) + self.previous = now + if not hold: + self.value += np.clip(self.goal-self.value, -self.rate*dt, self.rate*dt) + values = [j.minimum + round((self.value[j.index]-j.minimum)/j.resolution)*j.resolution + for j in self.profile.joints] + return self.profile.vector(values), bool(np.allclose(self.value, self.goal, atol=1e-8)) diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/profiles.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/profiles.py new file mode 100644 index 0000000..b99ab99 --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/profiles.py @@ -0,0 +1,203 @@ +"""Validated model/station contracts shared by launch, GUI and offline replay.""" +from dataclasses import asdict, dataclass, fields +import math +from pathlib import Path +import re + +import yaml + + +@dataclass(frozen=True) +class Joint: + name: str + sdk_name: str + index: int + minimum: float + maximum: float + resolution: float + view: str + tag_id: int + + def validate_value(self, value): + value = float(value) + if not math.isfinite(value) or not self.minimum <= value <= self.maximum: + raise ValueError(f'{self.name}: 指令越界或非有限数值') + steps = (value - self.minimum) / self.resolution + if abs(steps - round(steps)) > 1e-6: + raise ValueError(f'{self.name}: 指令不符合分辨率 {self.resolution}') + return int(value) if value.is_integer() else value + + +@dataclass(frozen=True) +class Task: + name: str + joints: tuple[str, ...] + clearances: tuple[str, ...] = () + allow_override: bool = True + # Restore one joint at a time in this order, waiting for feedback at each step. + restore_after: tuple[str, ...] = () + + +@dataclass(frozen=True) +class Clearance: + joints: tuple[str, ...] + targets: dict[str, float] | None = None + + +@dataclass(frozen=True) +class Profile: + model: str + side: str + command_unit: str + adapter: str + joints: tuple[Joint, ...] + tasks: tuple[Task, ...] + clearances: dict[str, Clearance] + sdk: dict + raw: dict + + @property + def by_name(self): + return {j.name: j for j in self.joints} + + @property + def views(self): + return tuple(dict.fromkeys(j.view for j in self.joints)) + + @property + def teaching_clearances(self): + return {name: clearance for name, clearance in self.clearances.items() + if clearance.targets is None} + + def vector(self, values): + if len(values) != len(self.joints): + raise ValueError(f'位置数组必须包含 {len(self.joints)} 项') + return tuple(j.validate_value(values[j.index]) for j in self.joints) + + +@dataclass(frozen=True) +class Settings: + endpoint_repetitions: int = 1 + # Accepted for historical journals/station files; live scans use endpoint searches. + coarse_step: float = 8 + fine_radius: float = 16 + fine_repetitions: int = 2 + rate: float = 20 + settle_seconds: float = 0.3 + stable_frames: int = 8 + point_timeout: float = 3 + freshness: float = 0.5 + feedback_tolerance: float = 2 + non_target_tolerance: float = 3 # Legacy configuration only; unrelated motion does not gate scans. + repeat_tolerance: float = 2 + motion_floor_px: float = 0.5 + noise_multiplier: float = 5 + stability_px: float = 1 + confirmation_points: int = 3 + max_hamming: int = 0 + min_margin: float = 30 + min_edge_px: float = 30 + + @classmethod + def from_dict(cls, data): + unknown = set(data) - {f.name for f in fields(cls)} + if unknown: + raise ValueError(f'未知扫描参数: {sorted(unknown)}') + result = cls(**data) + for name, value in asdict(result).items(): + if not isinstance(value, (float, int)) or not math.isfinite(value): + raise ValueError(f'扫描参数 {name} 必须为有限数值') + if value < 0 or (name != 'max_hamming' and value == 0): + raise ValueError(f'扫描参数 {name} 范围不合法') + for name in ('endpoint_repetitions', 'fine_repetitions', 'stable_frames', 'confirmation_points', 'max_hamming'): + if not isinstance(getattr(result, name), int): + raise ValueError(f'{name} 必须为整数') + if result.stable_frames < 4 or result.fine_repetitions < 2: + raise ValueError('稳定帧至少4张，细扫复测至少2轮') + if result.point_timeout <= result.settle_seconds: + raise ValueError('单点超时必须大于最短等待') + return result + + +def resources(): + try: + from ament_index_python.packages import get_package_share_directory + return Path(get_package_share_directory('linkerhand_range_calibration')) + except (ImportError, LookupError): + return Path(__file__).resolve().parents[1] + + +def load_profile(selection='o30_right'): + path = Path(selection).expanduser() + if not path.is_file(): + if not re.fullmatch(r'[A-Za-z0-9_-]+', selection): + raise ValueError(f'型号配置不存在: {selection}') + path = resources() / 'config' / 'profiles' / (selection + '.yaml') + return profile_from_dict(yaml.safe_load(path.read_text())) + + +def profile_from_dict(data): + if not isinstance(data, dict) or not re.fullmatch(r'[A-Za-z0-9_-]+', str(data.get('model', ''))): + raise ValueError('型号配置必须为对象，型号名称必须为字母、数字或下划线') + if data.get('schema_version') != 1 or data.get('side') not in ('left', 'right'): + raise ValueError('型号配置版本或左右手无效') + domain = data['command'] + joints = tuple(Joint(**j, **domain) for j in data['joints']) + joints = tuple(sorted(joints, key=lambda j: j.index)) + if not joints or [j.index for j in joints] != list(range(len(joints))): + raise ValueError('SDK 下标必须连续且唯一，从0开始') + names = [j.name for j in joints] + if len(set(names)) != len(names) or len({j.sdk_name for j in joints}) != len(joints): + raise ValueError('关节名称重复') + for j in joints: + if j.minimum >= j.maximum or j.resolution <= 0 or j.tag_id < 0: + raise ValueError(f'{j.name}: 指令范围或 Tag 配置无效') + j.validate_value(j.maximum) + tasks = tuple(Task(t['name'], tuple(t['joints']), tuple(t.get('clearances', [])), + t.get('allow_override', True), tuple(t.get('restore_after', []))) + for t in data['tasks']) + used = [name for t in tasks for name in t.joints] + if sorted(used) != sorted(names) or len({t.name for t in tasks}) != len(tasks): + raise ValueError('每个关节必须恰好出现在一个任务中，任务名称必须唯一') + clearances = {} + by_name = {joint.name: joint for joint in joints} + for group, specification in data.get('clearances', {}).items(): + if isinstance(specification, list): + members, targets = specification, None + elif (isinstance(specification, dict) and set(specification) == {'targets'} + and isinstance(specification['targets'], dict)): + targets = specification['targets'] + members = list(targets) + else: + raise ValueError(f'避让组 {group} 必须为示教关节列表或固定 targets 配置') + if not members or len(set(members)) != len(members) or not set(members) <= set(names): + raise ValueError(f'避让组 {group} 无效') + if targets is not None: + targets = {name: by_name[name].validate_value(value) for name, value in targets.items()} + clearances[group] = Clearance(tuple(members), targets) + for t in tasks: + if not t.joints or not set(t.clearances) <= set(clearances): + raise ValueError(f'任务 {t.name} 无效') + if len(set(t.restore_after)) != len(t.restore_after) or not set(t.restore_after) <= set(names): + raise ValueError(f'任务 {t.name} 的测后恢复关节无效或重复') + if any(set(clearances[c].joints) & set(t.joints) for c in t.clearances): + raise ValueError(f'任务 {t.name} 的主动关节不能同时参与避让') + observations = [(j.view, j.tag_id) for j in joints if j.name in t.joints] + if len(set(observations)) != len(observations): + raise ValueError(f'并行任务 {t.name} 必须有独立观测源') + return Profile(data['model'], data['side'], data['command_unit'], data['adapter'], + joints, tasks, clearances, data['sdk'], data) + + +def load_station(path=None): + path = Path(path).expanduser() if path else resources() / 'config' / 'station.yaml' + data = yaml.safe_load(path.read_text()) + Settings.from_dict(data.get('scan', {})) + cameras = data['cameras'] + if len({c['serial'] for c in cameras.values()}) != len(cameras): + raise ValueError('相机序列号重复') + for key in ('speed', 'torque'): + value = data['motion'][key] + if not isinstance(value, int) or not 0 <= value <= 255: + raise ValueError(f'{key} 必须为0～255整数') + return data diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/replay.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/replay.py new file mode 100644 index 0000000..6074472 --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/replay.py @@ -0,0 +1,63 @@ +"""Recompute completed tasks from their samples, not from saved result values.""" +import json +from pathlib import Path + +from .core.analysis import aggregate, direction_range, Unmeasurable, RefineWindow +from .core.endpoints import SCAN_METHOD, analyze_endpoint_samples +from .profiles import profile_from_dict, Settings +from .storage import write_json + + +def replay(journal, output): + samples, thresholds = {}, {} + document, profile, settings = None, None, None + scan_method = None + with Path(journal).open() as stream: + for line in stream: + event = json.loads(line) + kind = event['kind'] + if kind == 'metadata': + profile = profile_from_dict(event['profile']) + settings = Settings.from_dict(event['settings']) + scan_method = event.get('scan_method', 'coarse_fine_v1') + if scan_method not in (SCAN_METHOD, 'coarse_fine_v1'): + raise ValueError(f'不支持的日志扫描方法: {scan_method}') + document = {'schema_version': 1, 'model': profile.model, 'side': profile.side, + 'device_uid': event['uid'], 'command_unit': profile.command_unit, + 'joints': {j.name: {'min': None, 'max': None} for j in profile.joints}} + elif document is None: + raise ValueError('日志缺少首行 metadata') + elif kind == 'task_started': + samples[event['task']] = {} + for name in event['joints']: + document['joints'][name] = {'min': None, 'max': None} + elif kind == 'baseline': + thresholds[event['task']] = event['thresholds'] + elif kind == 'fine_reset': + data = samples[event['task']] + samples[event['task']] = {k: v for k, v in data.items() if k[0] == 'coarse'} + elif kind == 'sample': + key = event['stage'], event['repeat'], event['direction'] + samples[event['task']].setdefault(key, []).append(event) + elif kind == 'task_result': + task = next(t for t in profile.tasks if t.name == event['task']) + data = samples[task.name] + for name in task.joints: + joint = profile.by_name[name] + try: + if scan_method == SCAN_METHOD: + sweeps = {(r, d): v for (s, r, d), v in data.items() if s == 'endpoint'} + result = analyze_endpoint_samples(joint, sweeps, settings) + else: + threshold = thresholds[task.name][name] + for direction in ('up', 'down'): + direction_range(data[('coarse', 0, direction)], joint, threshold, settings, coarse=True) + result = aggregate(joint, {(r, d): v for (s, r, d), v in data.items() if s == 'fine'}, + threshold, settings) + document['joints'][name] = {k: result[k] for k in ('min', 'max')} + except (Unmeasurable, RefineWindow, KeyError): + document['joints'][name] = {'min': None, 'max': None} + if document is None: + raise ValueError('空日志') + write_json(output, document) + return document diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/runtime.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/runtime.py new file mode 100644 index 0000000..fd097ca --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/runtime.py @@ -0,0 +1,347 @@ +"""ROS host. All engine/adapter mutations run on the ROS executor thread.""" +from collections import OrderedDict +import copy +from dataclasses import asdict +from pathlib import Path +from queue import Empty, Queue +from threading import Lock +import time + +import cv2 +import numpy as np +from apriltag_msgs.msg import AprilTagDetectionArray +from cv_bridge import CvBridge +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data +from sensor_msgs.msg import CameraInfo, Image + +from .adapters.fake import FakeAdapter +from .adapters.registry import adapter_type +from .core.engine import Engine +from .profiles import Settings +from .simulation import simulated_teaching +from .storage import Session, Teaching, safe_uid +from .vision.observations import quality_error + + +NAMESPACE = '/linkerhand_range_calibration' + + +class CalibrationRuntime(Node): + def __init__(self, profile, station, demo=False): + super().__init__('range_controller', namespace=NAMESPACE) + self.profile, self.station, self.demo = profile, station, demo + self.settings = Settings.from_dict(station.get('scan', {})) + self.adapter = FakeAdapter(profile) if demo else adapter_type(profile.adapter)(self, profile, self.settings) + self.engine = Engine(profile, self.settings, self.adapter) + self.queue, self.lock = Queue(), Lock() + self.input_lock = Lock() + self.pending_jog, self.manual_epoch = {}, 0 + self.applied_epoch = 0 + self.snapshot_data = {} + self.bridge = CvBridge() + self.session, self.teaching, self.teaching_path = None, None, None + self.device_uid, self.message = '', '' + self.motion_settings_key = None + self.frames, self.tag_frames, self.tags = {}, {}, {} + self.camera_info, self.camera_received, self.image_times = {}, {}, {} + self.camera_error = {} + self.subscriptions_owned = [] + self.preview_at = {} + if not demo: + for view in profile.views: + prefix = f'{NAMESPACE}/{view}' + self.subscriptions_owned.extend([ + self.create_subscription(CameraInfo, prefix+'/camera/camera_info', + lambda m,v=view:self._camera(v,m), qos_profile_sensor_data), + self.create_subscription(Image, prefix+'/camera/image_rect', + lambda m,v=view:self._image(v,m), qos_profile_sensor_data), + self.create_subscription(AprilTagDetectionArray, prefix+'/apriltag/detections', + lambda m,v=view:self._detections(v,m), qos_profile_sensor_data), + ]) + self.timer = self.create_timer(1/30, self._tick) + + def submit(self, action, **data): + with self.input_lock: + if action == 'jog': + # One latest target per joint, consumed at the existing 30 Hz tick. + if data['epoch'] == self.manual_epoch: + self.pending_jog.update(data['targets']) + else: + # A control action invalidates gestures from the previous UI state. + self.manual_epoch += 1 + self.pending_jog.clear() + self.queue.put((action, data)) + + def _take_inputs(self): + with self.input_lock: + actions = [] + while True: + try: + actions.append(self.queue.get_nowait()) + except Empty: + break + targets, self.pending_jog = self.pending_jog, {} + if targets: + actions.append(('jog', {'targets': targets, 'epoch': self.manual_epoch})) + return actions, self.manual_epoch + + def snapshot(self): + with self.lock: + return copy.deepcopy(self.snapshot_data) + + def previews(self): + with self.lock: + return {view: (stamp, array, copy.deepcopy(self.tag_frames.get(view, {}).get(stamp, {}))) + for view, (stamp, array) in self.frames.items()} + + def _camera(self, view, message): + signature = (message.width, message.height, tuple(message.k), tuple(message.d), + tuple(message.r), tuple(message.p)) + valid = (message.width == self.station['camera']['width'] and + message.height == self.station['camera']['height'] and message.k[0] > 0 and + message.k[4] > 0 and message.p[0] > 0 and message.p[5] > 0 and + all(np.isfinite(v).all() for v in (message.k, message.d, message.r, message.p))) + if not valid: + self.camera_error[view] = '相机内参或图像尺寸无效' + else: + self.camera_error.pop(view, None) + if view in self.camera_info and signature != self.camera_info[view] and self.engine.active: + self.engine.pause(f'{view} 相机参数发生变化，请重新检查') + self.camera_info[view], self.camera_received[view] = signature, time.monotonic() + + def _image(self, view, message): + now = time.monotonic() + if now-self.preview_at.get(view, float('-inf')) < .1: + return + self.preview_at[view] = now + stamp = message.header.stamp.sec*1_000_000_000+message.header.stamp.nanosec + try: + frame = self.bridge.imgmsg_to_cv2(message, desired_encoding='bgr8') + with self.lock: + self.frames[view] = (stamp, np.ascontiguousarray(frame)) + except Exception as error: + self.message = f'预览解码失败: {error}' + + def _detections(self, view, message): + now = time.monotonic() + stamp = message.header.stamp.sec*1_000_000_000+message.header.stamp.nanosec + if abs(self.get_clock().now().nanoseconds-stamp) > int(self.settings.freshness*1e9): + return + if view not in self.camera_info or view in self.camera_error: + return + width, height = self.camera_info[view][:2] + expected = {j.tag_id for j in self.profile.joints if j.view == view} + statuses = {tag: {'error': '未检测到', 'corners': []} for tag in expected} + accepted, seen = {}, set() + for detection in message.detections: + tag = int(detection.id) + if tag not in expected: + continue + points = [(float(p.x), float(p.y)) for p in detection.corners] + error = quality_error(points, width, height, detection.hamming, + detection.decision_margin, self.settings) + if detection.family.removeprefix('tag') != self.station['camera']['family'].removeprefix('tag'): + error = 'Tag 家族不符' + if tag in seen: + error = '同一帧出现重复 Tag ID' + seen.add(tag) + statuses[tag] = {'error': error, 'corners': points} + if error: + accepted.pop(tag, None) + else: + accepted[tag] = points + self.tags[view], self.image_times[view] = statuses, now + with self.lock: + cache = self.tag_frames.setdefault(view, OrderedDict()) + cache[stamp] = statuses + while len(cache) > 40: + cache.popitem(last=False) + self.engine.observe(view, stamp, accepted, now) + + def _vision_error(self, views, now): + if self.demo: + return None + for view in views: + if view in self.camera_error: + return f'{view}: {self.camera_error[view]}' + if now-self.camera_received.get(view, float('-inf')) > 1: + return f'{view}: CameraInfo 未收到或断流' + if now-self.image_times.get(view, float('-inf')) > self.settings.freshness: + return f'{view}: 有效时间戳的检测流未收到或断流' + return None + + def _device(self): + uid = self.adapter.uid + if not uid or uid == self.device_uid or self.engine.active or self.engine.state == 'PAUSED': + return + safe_uid(uid) + with self.input_lock: + self.manual_epoch += 1 + self.pending_jog.clear() + if self.session: + self.session.close() + self.session = None + self.engine = Engine(self.profile, self.settings, self.adapter) + root = Path(self.station['teaching_root']).expanduser() + self.teaching_path = root / uid / 'teaching.yaml' + self.teaching = (simulated_teaching(self.profile, uid) if self.demo else + Teaching.load(self.teaching_path, self.profile, uid)) + self.device_uid = uid + self.motion_settings_key = None + + def _ensure_motion_settings(self, force=False): + motion = self.station['motion'] + key = (self.adapter.uid, motion['speed'], motion['torque']) + if force or key != self.motion_settings_key: + self.motion_settings_key = None + self.adapter.set_motion(**motion) + self.motion_settings_key = key + + def _action(self, action, data, now, stamp): + e = self.engine + if action == 'cancel': + e.cancel() + elif action == 'pause': + if e.active: + e.pause('用户暂停；继续时重新采集当前任务') + elif action == 'resume': + e.resume(now, stamp) + elif action in ('jog', 'start', 'save'): + error = self.adapter.control_error(now) + if error: + self.motion_settings_key = None + raise ValueError(error) + if not self.teaching: + raise ValueError('设备示教配置尚未加载') + if action == 'jog': + with self.input_lock: + if data['epoch'] != self.manual_epoch: + return + e.manual_adjust(data['targets'], now) + try: + self._ensure_motion_settings() + except Exception: + self.motion_settings_key = None + e.pause('速度或力矩设置失败，请检查 SDK 后取消并重新示教') + raise + elif action == 'save': + if not e.can_capture_teaching(now): + raise ValueError('请等待运动完成和反馈稳定后保存示教') + target = e.command if e.command is not None else self.adapter.feedback.positions + self.teaching.capture(data['kind'], target, self.adapter.feedback.positions, data.get('key')) + self.teaching.save(self.teaching_path) + self.message = f'示教已保存: {self.teaching_path}' + else: + if e.active or e.state == 'PAUSED': + raise ValueError('请先结束当前任务') + names = data.get('tasks') + tasks = [t for t in self.profile.tasks if names is None or t.name in names] + if not tasks or (names and set(names) != {t.name for t in tasks}): + raise ValueError('扫描任务不存在') + missing = self.teaching.missing(tasks) + if missing: + raise ValueError('缺少示教: ' + ', '.join(missing)) + views = {self.profile.by_name[n].view for t in tasks for n in t.joints} + error = self._vision_error(views, now) + if error: + raise ValueError(error) + session = Session(self.station['output_root'], self.profile, self.device_uid, + asdict(self.settings), self.teaching, self.station['motion']) + if self.session: + self.session.close() + self.session = session + e.emit = self.session.emit + self._ensure_motion_settings(force=True) + e.start(self.teaching, now, stamp, names) + self.message = f'结果目录: {self.session.directory}' + else: + raise ValueError(f'未知操作 {action}') + + def _demo_frame(self, now, stamp): + self.adapter.advance(now, stamp) + for view, detections in self.adapter.frames().items(): + self.engine.observe(view, stamp, detections, now) + self.image_times[view] = now + if now-self.preview_at.get(view, float('-inf')) < .1: + continue + self.preview_at[view] = now + frame = np.full((1240, 1624, 3), 245, dtype=np.uint8) + statuses = {} + for tag, points in detections.items(): + cv2.fillConvexPoly(frame, np.asarray(points, dtype=np.int32), (70,70,70)) + statuses[tag] = {'error': None, 'corners': points} + self.tags[view] = statuses + with self.lock: + self.frames[view] = (stamp, frame) + self.tag_frames[view] = {stamp: statuses} + + def _tick(self): + now, stamp = time.monotonic(), self.get_clock().now().nanoseconds + try: + if self.demo: + self._demo_frame(now, stamp) + self._device() + actions, epoch = self._take_inputs() + for action, data in actions: + try: + self._action(action, data, now, stamp) + except (ValueError, OSError) as error: + self.message = str(error) + self.applied_epoch = epoch + if self.engine.state in ('PREPARING', 'SCANNING'): + views = {self.profile.by_name[n].view for n in self.engine.task.joints} + error = self._vision_error(views, now) + if error: + self.engine.pause(error) + self.engine.tick(now, stamp) + except Exception as error: + self.message = f'运行异常: {error}' + if self.engine.active: + try: + self.engine.pause(self.message) + except OSError as storage_error: + self.message += f'；日志写入失败: {storage_error}' + self.get_logger().error(self.message) + feedback = self.adapter.feedback + if self.engine.state == 'TEACHING': + manual_target = self.engine.manual_target + else: + manual_target = self.engine.command or (feedback.positions if feedback else None) + tags = copy.deepcopy(self.tags) + control_error = self.adapter.control_error(now) + if control_error: + self.motion_settings_key = None + for view, entries in tags.items(): + if now-self.image_times.get(view, float('-inf')) > self.settings.freshness: + for entry in entries.values(): + entry['error'] = '观测过期' + with self.lock: + self.snapshot_data = { + 'state': self.engine.state, 'reason': self.engine.reason, 'message': self.message, + 'uid': self.adapter.uid, + 'control_error': control_error, + 'positions': list(feedback.positions) if feedback else None, + 'command': self.engine.command, 'results': copy.deepcopy(self.engine.results), + 'manual_target': manual_target, 'manual_epoch': self.applied_epoch, + 'manual_allowed': self.engine.manual_allowed, + 'task': self.engine.task.name if self.engine.task else '', + 'task_index': self.engine.task_index, 'task_count': len(self.engine.tasks), + 'stage': self.engine.stage, 'direction': self.engine.direction, + 'restoring_joint': self.engine.restoring_joint, + 'scan_command': (self.engine.command[self.engine.specs[0].index] + if self.engine.state == 'SCANNING' else None), + 'repeat': self.engine.repeat, 'progress': self.engine.progress, + 'active': self.engine.active, + 'can_save': self.engine.can_capture_teaching(now), + 'missing': self.teaching.missing(self.profile.tasks) if self.teaching else ['等待设备身份'], + 'tags': tags, + 'output': str(self.session.result_path) if self.session else '', + } + + def close_session(self): + self.timer.cancel() + if self.engine.active or self.engine.state == 'PAUSED': + self.engine.cancel() + if self.session: + self.session.close() diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/simulation.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/simulation.py new file mode 100644 index 0000000..240cb09 --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/simulation.py @@ -0,0 +1,57 @@ +"""Fast virtual-time end-to-end exercise. Never imports ROS or a real adapter.""" +from dataclasses import asdict, replace + +from .adapters.fake import FakeAdapter +from .core.engine import Engine +from .storage import Teaching, Session + + +def simulated_teaching(profile, uid): + teaching = Teaching(profile, uid) + base = [j.minimum + round((j.maximum-j.minimum)/j.resolution/4)*j.resolution for j in profile.joints] + teaching.capture('baseline', base, base) + for group, clearance in profile.teaching_clearances.items(): + folded = list(base) + for name in clearance.joints: + j = profile.by_name[name] + folded[j.index] = j.maximum + teaching.capture('clearances', folded, folded, group) + return teaching + + +def simulate(profile, settings, output, task_names=None): + ranges = {} + for i, j in enumerate(profile.joints): + if j.minimum == 0 and j.maximum == 255: + ranges[j.name] = [(0, 230), (7, 255), (6, 243), (0, 255)][i % 4] + else: + ranges[j.name] = (j.minimum, j.maximum) + adapter = FakeAdapter(profile, ranges) + teaching = simulated_teaching(profile, adapter.uid) + # Only scheduling is accelerated; image thresholds and range logic are unchanged. + settings = replace(settings, rate=10000, settle_seconds=0.01, stable_frames=4) + session = Session(output, profile, adapter.uid, asdict(settings), teaching, {'simulated': True}) + engine = Engine(profile, settings, adapter, session.emit) + now = 1.0 + adapter.advance(now, int(now*1e9)) + engine.start(teaching, now, int(now*1e9), task_names) + try: + for _ in range(300000): + now += 0.1 + stamp = round(now*1e9) + adapter.advance(now, stamp) + for view, detections in adapter.frames().items(): + engine.observe(view, stamp, detections, now) + engine.tick(now, stamp) + if engine.state in ('COMPLETED', 'PAUSED', 'CANCELLED'): + break + if engine.state != 'COMPLETED': + raise RuntimeError(f'模拟流程未完成: {engine.state} {engine.reason}') + selected = {n for t in engine.tasks for n in t.joints} + for name in selected: + result = engine.results[name] + if (result['min'], result['max']) != ranges[name]: + raise RuntimeError(f'模拟边界不符: {name}: {result}; expected={ranges[name]}') + return session.result_path + finally: + session.close() diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/storage.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/storage.py new file mode 100644 index 0000000..bc274fe --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/storage.py @@ -0,0 +1,161 @@ +"""Small public result files; full provenance stays in an append-only journal.""" +from datetime import datetime +import json +import os +from pathlib import Path +import re +import tempfile + +import yaml + +from .core.endpoints import SCAN_METHOD + + +def safe_uid(uid): + if not isinstance(uid, str) or not re.fullmatch(r'[A-Za-z0-9_-][A-Za-z0-9_.-]*', uid): + raise ValueError('设备 UID 为空或含不支持的路径字符') + return uid + + +def atomic_text(path, text): + path = Path(path) + path.parent.mkdir(parents=True, exist_ok=True) + fd, temporary = tempfile.mkstemp(prefix=path.name + '.', dir=path.parent) + try: + with os.fdopen(fd, 'w', encoding='utf-8') as stream: + stream.write(text) + stream.flush() + os.fsync(stream.fileno()) + os.replace(temporary, path) + finally: + if os.path.exists(temporary): + os.unlink(temporary) + + +def write_json(path, value): + atomic_text(path, json.dumps(value, ensure_ascii=False, indent=2, allow_nan=False) + '\n') + + +class Teaching: + def __init__(self, profile, uid, data=None): + self.profile, self.uid = profile, safe_uid(uid) + self.data = data or {'schema_version': 1, 'model': profile.model, + 'side': profile.side, 'device_uid': uid, + 'baseline': None, 'clearances': {}, 'overrides': {}} + if (self.data.get('schema_version'), self.data.get('model'), self.data.get('side'), + self.data.get('device_uid')) != (1, profile.model, profile.side, uid): + raise ValueError('示教文件与当前型号/左右手/设备 UID 不符') + if self.data['baseline']: + profile.vector(self.data['baseline']['target']) + profile.vector(self.data['baseline']['feedback']) + tasks = {t.name for t in profile.tasks} + for kind in ('clearances', 'overrides'): + for key, record in self.data[kind].items(): + if key not in (profile.clearances if kind == 'clearances' else tasks): + raise ValueError(f'未知示教项 {key}') + if kind == 'overrides' and not next(t for t in profile.tasks if t.name == key).allow_override: + raise ValueError(f'{key} 固定使用基础姿态，不允许覆盖') + values = record['target'] + expected = set(profile.clearances[key].joints) if kind == 'clearances' else set(profile.by_name) + if set(values) != expected: + raise ValueError(f'示教项 {key} 的关节集合不完整') + for name, value in values.items(): + profile.by_name[name].validate_value(value) + + @classmethod + def load(cls, path, profile, uid): + path = Path(path) + return cls(profile, uid, yaml.safe_load(path.read_text()) if path.exists() else None) + + def save(self, path): + atomic_text(path, yaml.safe_dump(self.data, allow_unicode=True, sort_keys=False)) + + def capture(self, kind, target, feedback, key=None): + target, feedback = self.profile.vector(target), self.profile.vector(feedback) + record = {'target': list(target), 'feedback': list(feedback)} + if kind == 'baseline': + self.data['baseline'] = record + return + if kind == 'clearances': + clearance = self.profile.clearances[key] + if clearance.targets is not None: + raise ValueError('该避让姿态由型号配置固定，无需手动示教') + names = clearance.joints + elif kind == 'overrides' and key in {t.name for t in self.profile.tasks}: + if not next(t for t in self.profile.tasks if t.name == key).allow_override: + raise ValueError('该任务固定使用基础姿态，不应用姿态覆盖或避让') + names = self.profile.by_name + else: + raise ValueError('未知示教类别') + self.data[kind][key] = {field: {name: values[self.profile.by_name[name].index] + for name in names} for field, values in record.items()} + + def missing(self, tasks): + missing = [] if self.data['baseline'] else ['全手基础姿态'] + missing.extend(c for c in dict.fromkeys(c for t in tasks for c in t.clearances) + if self.profile.clearances[c].targets is None and c not in self.data['clearances']) + return missing + + def prepare(self, task): + missing = self.missing([task]) + if missing: + raise ValueError('缺少示教: ' + ', '.join(missing)) + pose = list(self.data['baseline']['target']) + # Task overrides describe the visible working pose; accumulated clearance + # always wins, so an override cannot accidentally unfold an occluder. + updates = dict(self.data['overrides'].get(task.name, {}).get('target', {})) + for group in task.clearances: + clearance = self.profile.clearances[group] + targets = (clearance.targets if clearance.targets is not None + else self.data['clearances'][group]['target']) + updates.update(targets) + for name, value in updates.items(): + pose[self.profile.by_name[name].index] = value + return self.profile.vector(pose) + + def restoration_steps(self, task, current): + """Build ordered baseline moves, changing only one joint per step.""" + pose = list(self.profile.vector(current)) + if task.restore_after and not self.data['baseline']: + raise ValueError('缺少示教: 全手基础姿态') + steps = [] + for name in task.restore_after: + index = self.profile.by_name[name].index + pose[index] = self.data['baseline']['target'][index] + steps.append((name, self.profile.vector(pose))) + return steps + + +class Session: + def __init__(self, root, profile, uid, settings, teaching, motion): + self.profile, self.uid = profile, safe_uid(uid) + self.directory = Path(root).expanduser() / uid / datetime.now().strftime('%Y%m%d_%H%M%S_%f') + self.directory.mkdir(parents=True, exist_ok=False) + self.result_path = self.directory / f'{profile.model.lower()}_{profile.side}_ranges.json' + self.journal = (self.directory / 'samples.jsonl').open('a', encoding='utf-8', buffering=1) + self.results = {j.name: {'min': None, 'max': None} for j in profile.joints} + self.emit({'kind': 'metadata', 'profile': profile.raw, 'settings': settings, + 'teaching': teaching.data, 'motion': motion, 'uid': uid, 'scan_method': SCAN_METHOD}) + self.save() + + def emit(self, event): + self.journal.write(json.dumps(event, ensure_ascii=False, allow_nan=False) + '\n') + if event['kind'] == 'task_result': + for name, result in event['results'].items(): + self.results[name] = {k: result.get(k) for k in ('min', 'max')} + self.save() + elif event['kind'] in ('cancelled', 'paused', 'completed'): + self.save() + + def document(self): + return {'schema_version': 1, 'model': self.profile.model, 'side': self.profile.side, + 'device_uid': self.uid, 'command_unit': self.profile.command_unit, + 'joints': self.results} + + def save(self): + write_json(self.result_path, self.document()) + + def close(self): + if not self.journal.closed: + self.save() + self.journal.close() diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/ui/__init__.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/ui/__init__.py new file mode 100644 index 0000000..961af5f --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/ui/__init__.py @@ -0,0 +1 @@ +"""Qt user interface.""" diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/ui/joint_control.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/ui/joint_control.py new file mode 100644 index 0000000..b9db13e --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/ui/joint_control.py @@ -0,0 +1,51 @@ +"""Joint slider with a precise numeric editor and silent display updates.""" +from decimal import Decimal + +from PyQt5.QtCore import QSignalBlocker, Qt, pyqtSignal +from PyQt5.QtWidgets import QDoubleSpinBox, QHBoxLayout, QSlider, QWidget + + +class JointControl(QWidget): + target_changed = pyqtSignal(float) + + def __init__(self, joint, parent=None): + super().__init__(parent) + self.joint = joint + self.slider = QSlider(Qt.Horizontal) + self.slider.setRange(0, round((joint.maximum - joint.minimum) / joint.resolution)) + self.slider.setPageStep(max(1, self.slider.maximum() // 25)) + self.slider.setMinimumWidth(160) + self.slider.setAccessibleName(joint.name + ' 目标滑块') + self.editor = QDoubleSpinBox() + self.editor.setDecimals(max(0, min(8, -Decimal(str(joint.resolution)).as_tuple().exponent))) + self.editor.setRange(joint.minimum, joint.maximum) + self.editor.setSingleStep(joint.resolution) + self.editor.setKeyboardTracking(False) + self.editor.setFixedWidth(80) + self.editor.setAccessibleName(joint.name + ' 精确目标') + layout = QHBoxLayout(self) + layout.setContentsMargins(6, 0, 6, 0) + layout.addWidget(self.slider, 1) + layout.addWidget(self.editor) + self.slider.valueChanged.connect(self._slider_changed) + self.editor.valueChanged.connect(self._editor_changed) + self.set_target(joint.minimum) + + def set_target(self, value): + """Refresh from runtime without issuing a motion request.""" + tick = round((value - self.joint.minimum) / self.joint.resolution) + tick = max(self.slider.minimum(), min(self.slider.maximum(), tick)) + slider_block = QSignalBlocker(self.slider) + editor_block = QSignalBlocker(self.editor) + self.slider.setValue(tick) + self.editor.setValue(self.joint.minimum + tick * self.joint.resolution) + del slider_block, editor_block + + def _slider_changed(self, tick): + value = self.joint.minimum + tick * self.joint.resolution + self.set_target(value) + self.target_changed.emit(value) + + def _editor_changed(self, value): + self.set_target(value) + self.target_changed.emit(self.editor.value()) diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/ui/window.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/ui/window.py new file mode 100644 index 0000000..531f7d5 --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/ui/window.py @@ -0,0 +1,198 @@ +"""Thin Qt view: queue commands and render immutable runtime snapshots.""" +import cv2 +import numpy as np +from PyQt5.QtCore import Qt, QTimer +from PyQt5.QtGui import QImage, QPixmap +from PyQt5.QtWidgets import ( + QComboBox, QHBoxLayout, QLabel, QMainWindow, QProgressBar, + QPushButton, QTableWidget, QTableWidgetItem, QVBoxLayout, QWidget, + QHeaderView, +) + +from .joint_control import JointControl + + +STATES = {'IDLE': '待操作', 'PREPARING': '准备姿态 / 静止采样', 'SCANNING': '扫描中', + 'RESTORING': '测后恢复基础姿态', 'TEACHING': '示教运动中', 'PAUSED': '已暂停', + 'CANCELLED': '已取消', 'COMPLETED': '已完成'} + + +class CalibrationWindow(QMainWindow): + def __init__(self, runtime): + super().__init__() + self.runtime, self.profile = runtime, runtime.profile + self.last_frames, self.initialized_uid = {}, '' + self.display_epoch, self.was_manual_ready = None, False + self.setWindowTitle(f'{self.profile.model} {self.profile.side} 行程标定' + + (' — 模拟设备' if runtime.demo else '')) + self.resize(1280, 930) + root = QWidget(); self.setCentralWidget(root) + layout = QVBoxLayout(root) + self.identity, self.status, self.message, self.teaching_status = QLabel(), QLabel(), QLabel(), QLabel() + self.identity.setWordWrap(True) + self.message.setWordWrap(True) + self.message.setTextInteractionFlags(Qt.TextSelectableByMouse) + for label in (self.identity, self.status, self.teaching_status): + layout.addWidget(label) + previews = QHBoxLayout(); self.previews_by_view = {}; self.tag_labels = {} + names = {'front': '正面', 'side': '侧面', 'top': '顶部'} + for view in (v for v in runtime.station['cameras'] if v in self.profile.views): + column = QVBoxLayout(); column.addWidget(QLabel(names.get(view, view))) + preview = QLabel('等待图像'); preview.setAlignment(Qt.AlignCenter) + preview.setMinimumSize(240, 165); preview.setMaximumHeight(230) + preview.setStyleSheet('background:#18222c; color:#d5e1ea; border-radius:4px') + column.addWidget(preview); previews.addLayout(column) + self.previews_by_view[view] = preview + tag_status = QLabel('等待 Tag 检测') + tag_status.setWordWrap(True) + column.addWidget(tag_status) + self.tag_labels[view] = tag_status + layout.addLayout(previews) + controls = QHBoxLayout() + self.task = QComboBox() + for task in self.profile.tasks: + self.task.addItem('四指侧摆（同步）' if task.name == 'four_finger_roll' else task.name, task.name) + controls.addWidget(self.task) + self.start_one = self.button('试标定 / 重测所选任务', lambda: runtime.submit('start', tasks=[self.task.currentData()]), controls) + self.start_all = self.button('全手标定', lambda: runtime.submit('start'), controls) + self.pause = self.button('暂停', lambda: runtime.submit('pause'), controls) + self.resume = self.button('继续（重做当前任务）', lambda: runtime.submit('resume'), controls) + self.cancel = self.button('取消 / 保持目标', lambda: runtime.submit('cancel'), controls) + layout.addLayout(controls) + self.progress = QProgressBar(); layout.addWidget(self.progress) + teach_controls = QHBoxLayout() + self.fill = self.button('同步当前目标', self.sync_targets, teach_controls) + self.save_kind = QComboBox() + self.save_kind.addItem('全手基础姿态', ('baseline', None)) + for group, clearance in self.profile.teaching_clearances.items(): + self.save_kind.addItem('避让姿态：' + group, ('clearances', group)) + self.save_kind.setItemData(self.save_kind.count() - 1, + '仅保存这些关节：' + '、'.join(clearance.joints), Qt.ToolTipRole) + self.save_kind.addItem('当前任务准备姿态覆盖', ('overrides', None)) + teach_controls.addWidget(self.save_kind) + self.save = self.button('保存当前已到达姿态', self.save_teaching, teach_controls) + layout.addLayout(teach_controls) + hint = QLabel('拖动滑块直接控制关节；数值框支持精调，输入后按回车确认。等待运动完成、反馈稳定后保存姿态。') + hint.setWordWrap(True) + layout.addWidget(hint) + self.joints_table = QTableWidget(len(self.profile.joints), 7) + self.joints_table.setHorizontalHeaderLabels( + ['关节', 'SDK 下标', '示教目标（拖动即运动）', '实际反馈', 'min', 'max', '状态 / 原因']) + self.joint_controls = [] + for row, joint in enumerate(self.profile.joints): + for col, value in enumerate((joint.name, str(joint.index), None, '—', '—', '—', '未完成')): + if value is not None: + self.joints_table.setItem(row, col, QTableWidgetItem(value)) + self.joints_table.item(row, 0).setToolTip(joint.sdk_name) + control = JointControl(joint) + control.target_changed.connect(lambda value, name=joint.name: self.jog(name, value)) + self.joints_table.setCellWidget(row, 2, control) + self.joint_controls.append(control) + self.joints_table.setRowHeight(row, max(34, control.sizeHint().height() + 4)) + self.joints_table.setEditTriggers(QTableWidget.NoEditTriggers) + self.joints_table.setSelectionBehavior(QTableWidget.SelectRows) + self.joints_table.setWordWrap(False) + self.joints_table.verticalHeader().setVisible(False) + header = self.joints_table.horizontalHeader() + header.setSectionResizeMode(QHeaderView.ResizeToContents) + header.setSectionResizeMode(2, QHeaderView.Stretch) + header.setSectionResizeMode(6, QHeaderView.Interactive) + self.joints_table.setColumnWidth(6, max(220, self.fontMetrics().horizontalAdvance('状') * 18)) + layout.addWidget(self.joints_table, 1) + layout.addWidget(self.message) + self.output = QLabel(); self.output.setWordWrap(True) + self.output.setTextInteractionFlags(Qt.TextSelectableByMouse); layout.addWidget(self.output) + self.timer = QTimer(self); self.timer.timeout.connect(self.refresh); self.timer.start(100) + + def button(self, text, callback, layout): + button = QPushButton(text); button.clicked.connect(callback); layout.addWidget(button) + return button + + def sync_targets(self): + values = self.runtime.snapshot().get('manual_target') + if values: + for control, value in zip(self.joint_controls, values): + control.set_target(value) + + def jog(self, name, value): + snap = self.runtime.snapshot() + if not snap or snap['control_error'] or not snap['uid'] or not snap['manual_allowed']: + return + self.runtime.submit('jog', targets={name: value}, epoch=snap['manual_epoch']) + + def save_teaching(self): + kind, key = self.save_kind.currentData() + self.runtime.submit('save', kind=kind, key=self.task.currentData() if kind == 'overrides' else key) + + def refresh(self): + snap = self.runtime.snapshot() + if not snap: + return + control_error = snap['control_error'] + self.identity.setText(f'设备：{self.profile.model} / {self.profile.side} / {snap["uid"] or "等待UID"} | ' + f'{control_error or "SDK 控制连接已就绪"}') + search_label = f'逐{self.profile.joints[0].resolution:g}寻找边界' + restore_label = f'{snap["restoring_joint"]} 恢复基础姿态' if snap['restoring_joint'] else '' + self.status.setText(f'{STATES.get(snap["state"], snap["state"])} | {snap["task"]} | ' + f'{ {"prepare":"准备姿态", "reference":"端点定位 / 参考采样", "search":search_label, "restore":restore_label}.get(snap["stage"], "")} ' + f'{ {"up":"低端递增", "down":"高端递减"}.get(snap["direction"], "")} ' + f'指令 {snap["scan_command"] if snap["scan_command"] is not None else "—"} ' + f'第{snap["repeat"]+1}轮 | ' + f'任务 {min(snap["task_index"]+1,snap["task_count"])}/{snap["task_count"]}') + self.teaching_status.setText('待示教：' + '、'.join(snap['missing']) if snap['missing'] else '所需示教已完成') + self.message.setText(snap['reason'] + ('\n' if snap['reason'] else '') + snap['message']) + self.output.setText('结果文件：' + (snap['output'] or '开始标定后创建')) + value, maximum = snap['progress']; self.progress.setRange(0, max(1, maximum)); self.progress.setValue(value) + blocked = snap['active'] or snap['state'] == 'PAUSED' + ready = not blocked and not control_error + for control in (self.fill, self.task, self.save_kind): + control.setEnabled(ready) + for control in (self.start_one, self.start_all): + control.setEnabled(ready) + self.save.setEnabled(snap['can_save']) + self.pause.setEnabled(snap['active']); self.cancel.setEnabled(blocked) + self.resume.setEnabled(snap['state'] == 'PAUSED' and bool(snap['task']) and not control_error) + manual_ready = (not control_error and bool(snap['uid']) and + snap['manual_allowed']) + for control in self.joint_controls: + control.setEnabled(manual_ready) + if (snap['manual_epoch'] != self.display_epoch or snap['uid'] != self.initialized_uid or + not manual_ready or not self.was_manual_ready): + self.sync_targets() + self.display_epoch, self.initialized_uid = snap['manual_epoch'], snap['uid'] + self.was_manual_ready = manual_ready + for row, joint in enumerate(self.profile.joints): + if snap['positions']: + self.joints_table.item(row, 3).setText(f'{snap["positions"][row]:g}') + result = snap['results'][joint.name] + for col, key in ((4, 'min'), (5, 'max')): + self.joints_table.item(row, col).setText('—' if result[key] is None else str(result[key])) + status = result.get('reason') or result['status'] + self.joints_table.item(row, 6).setText(status) + self.joints_table.item(row, 6).setToolTip(status) + for view, label in self.tag_labels.items(): + tags = snap['tags'].get(view, {}) + label.setText(' '.join(f'ID{tag} {"✓" if not entry["error"] else "×"}' + for tag, entry in sorted(tags.items())) or '等待 Tag 检测') + label.setToolTip('\n'.join(f'ID{tag}: {entry["error"] or "有效"}' for tag, entry in sorted(tags.items()))) + for view, (stamp, frame, tags) in self.runtime.previews().items(): + if self.last_frames.get(view) == stamp: + continue + self.last_frames[view] = stamp + draw = frame.copy() + for tag, entry in tags.items(): + if not entry['corners']: + continue + points = np.asarray(entry['corners'], dtype=np.int32) + color = (70,180,60) if not entry['error'] else (40,40,230) + cv2.polylines(draw,[points],True,color,3) + cv2.putText(draw,str(tag),tuple(points[0]),cv2.FONT_HERSHEY_SIMPLEX,1,color,2) + rgb = cv2.cvtColor(draw,cv2.COLOR_BGR2RGB) + image = QImage(rgb.data,rgb.shape[1],rgb.shape[0],rgb.strides[0],QImage.Format_RGB888).copy() + label = self.previews_by_view[view] + label.setPixmap(QPixmap.fromImage(image).scaled(label.size(),Qt.KeepAspectRatio,Qt.SmoothTransformation)) + + def closeEvent(self, event): + self.timer.stop() + self.runtime.submit('cancel') + event.accept() diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/vision/__init__.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/vision/__init__.py new file mode 100644 index 0000000..09957a3 --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/vision/__init__.py @@ -0,0 +1 @@ +"""Image-coordinate observations; no extrinsics or pose reconstruction.""" diff --git a/src/linkerhand_range_calibration/linkerhand_range_calibration/vision/observations.py b/src/linkerhand_range_calibration/linkerhand_range_calibration/vision/observations.py new file mode 100644 index 0000000..5faceae --- /dev/null +++ b/src/linkerhand_range_calibration/linkerhand_range_calibration/vision/observations.py @@ -0,0 +1,73 @@ +"""Tag quality and stable windows in image coordinates.""" +from collections import deque +from dataclasses import dataclass + +import numpy as np + + +@dataclass(frozen=True) +class Observation: + stamp_ns: int + received: float + corners: tuple + + +def rms_delta(a, b): + delta = np.asarray(a, dtype=float) - np.asarray(b, dtype=float) + return float(np.sqrt(np.mean(np.sum(delta * delta, axis=-1)))) + + +def noise_sigma(points): + values = np.asarray(points, dtype=float) + median = np.median(values, axis=0) + mad = 1.4826 * np.median(np.abs(values - median), axis=0) + return float(np.sqrt(np.mean(np.sum(mad * mad, axis=-1)))) + + +def quality_error(corners, width, height, hamming, margin, settings): + p = np.asarray(corners, dtype=float) + if p.shape != (4, 2) or not np.isfinite(p).all() or not np.isfinite(margin): + return '角点或质量数值无效' + if hamming > settings.max_hamming or margin < settings.min_margin: + return 'Tag 解码质量不足' + if width <= 0 or height <= 0 or (p[:, 0] < 1).any() or (p[:, 0] >= width-1).any() or ( + p[:, 1] < 1).any() or (p[:, 1] >= height-1).any(): + return 'Tag 角点超出画面边界' + edges = np.roll(p, -1, axis=0) - p + if np.linalg.norm(edges, axis=1).min() < settings.min_edge_px: + return 'Tag 像素尺寸不足' + following = np.roll(edges, -1, axis=0) + cross = edges[:, 0]*following[:, 1] - edges[:, 1]*following[:, 0] + if not ((cross > 0).all() or (cross < 0).all()): + return 'Tag 四边形无效' + return None + + +class StableWindow: + def __init__(self, count): + self.values = deque(maxlen=count) + self.last_stamp = -1 + self.count = count + + def clear(self): + self.values.clear() + + def add(self, observation): + if observation.stamp_ns <= self.last_stamp: + return False + self.last_stamp = observation.stamp_ns + self.values.append(observation) + return True + + def summary(self, tolerance): + if len(self.values) < self.count: + return None + points = np.asarray([v.corners for v in self.values]) + split = len(points)//2 + if rms_delta(np.median(points[:split], axis=0), np.median(points[split:], axis=0)) > tolerance: + return None + if noise_sigma(points) > tolerance: + return None + return {'corners': np.median(points, axis=0).tolist(), 'noise': noise_sigma(points), + 'first_stamp_ns': self.values[0].stamp_ns, + 'last_stamp_ns': self.values[-1].stamp_ns, 'frames': len(points)} diff --git a/src/linkerhand_range_calibration/package.xml b/src/linkerhand_range_calibration/package.xml new file mode 100644 index 0000000..ada1611 --- /dev/null +++ b/src/linkerhand_range_calibration/package.xml @@ -0,0 +1,29 @@ + + + linkerhand_range_calibration + 0.1.0 + Profile-driven visual command range calibration. + lxp + Apache-2.0 + ament_python + ament_index_python + rclpy + sensor_msgs + std_msgs + rcl_interfaces + apriltag_msgs + apriltag_ros + image_proc + cv_bridge + launch_ros + launch + rclcpp_components + linkerhand_calibration + linker_hand_o30_ros2_sdk + python3-numpy + python3-opencv + python3-yaml + python3-pyqt5 + python3-pytest + ament_python + diff --git a/src/linkerhand_range_calibration/resource/linkerhand_range_calibration b/src/linkerhand_range_calibration/resource/linkerhand_range_calibration new file mode 100644 index 0000000..e69de29 diff --git a/src/linkerhand_range_calibration/setup.cfg b/src/linkerhand_range_calibration/setup.cfg new file mode 100644 index 0000000..3937277 --- /dev/null +++ b/src/linkerhand_range_calibration/setup.cfg @@ -0,0 +1,6 @@ +[develop] +script_dir=$base/lib/linkerhand_range_calibration +[install] +install_scripts=$base/lib/linkerhand_range_calibration +[tool:pytest] +testpaths=test diff --git a/src/linkerhand_range_calibration/setup.py b/src/linkerhand_range_calibration/setup.py new file mode 100644 index 0000000..2a91362 --- /dev/null +++ b/src/linkerhand_range_calibration/setup.py @@ -0,0 +1,21 @@ +from glob import glob +from setuptools import find_packages, setup + +name = 'linkerhand_range_calibration' +setup( + name=name, version='0.1.0', packages=find_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + name]), + ('share/' + name, ['package.xml', 'README.md']), + ('share/' + name + '/config', glob('config/*.yaml')), + ('share/' + name + '/config/profiles', glob('config/profiles/*.yaml')), + ('share/' + name + '/launch', glob('launch/*.launch.py')), + ], + install_requires=['setuptools', 'numpy', 'PyYAML'], + tests_require=['pytest'], zip_safe=False, + maintainer='lxp', maintainer_email='support@linker-robotics.com', + description='Visual command range calibration', license='Apache-2.0', + entry_points={'console_scripts': [ + 'calibrate_range = linkerhand_range_calibration.cli:main', + ]}, +) diff --git a/src/linkerhand_range_calibration/test/conftest.py b/src/linkerhand_range_calibration/test/conftest.py new file mode 100644 index 0000000..5801bea --- /dev/null +++ b/src/linkerhand_range_calibration/test/conftest.py @@ -0,0 +1,4 @@ +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) diff --git a/src/linkerhand_range_calibration/test/test_core.py b/src/linkerhand_range_calibration/test/test_core.py new file mode 100644 index 0000000..b94ba2e --- /dev/null +++ b/src/linkerhand_range_calibration/test/test_core.py @@ -0,0 +1,649 @@ +from dataclasses import replace +import copy +import json + +import numpy as np +import pytest + +from linkerhand_range_calibration.profiles import load_profile, profile_from_dict, Settings +from linkerhand_range_calibration.storage import Teaching +from linkerhand_range_calibration.core.analysis import direction_range, aggregate, fine_commands, Unmeasurable, RefineWindow +from linkerhand_range_calibration.core.engine import Engine +from linkerhand_range_calibration.adapters.fake import FakeAdapter +from linkerhand_range_calibration.simulation import simulated_teaching, simulate +from linkerhand_range_calibration.replay import replay +from linkerhand_range_calibration.vision.observations import Observation, StableWindow, quality_error + + +def samples(joint, lo, hi, direction='up', scale=2, commands=range(256), transform=None): + values = [] + for c in commands: + x = float(np.clip(c, lo, hi)) + corners = np.array([[100, 100], [140, 100], [140, 140], [100, 140]], dtype=float) + if transform: + corners = transform(corners, x) + else: + corners[:, 0] += x*scale + values.append({'command': c, 'observations': {joint.name: {'corners': corners.tolist()}}}) + return values if direction == 'up' else values[::-1] + + +@pytest.mark.parametrize('bounds', [(0, 230), (7, 255), (6, 243), (0, 255)]) +def test_exact_limits(bounds): + joint = load_profile().joints[0] + settings = Settings() + sweeps = {(r, d): samples(joint, *bounds, direction=d) for r in range(2) for d in ('up', 'down')} + result = aggregate(joint, sweeps, .5, settings) + assert (result['min'], result['max']) == bounds + + +def test_hysteresis_and_repeat_failure(): + joint = load_profile().joints[0] + sweeps = {(r, d): samples(joint, *( (6,243) if d == 'up' else (8,240))) + for r in range(2) for d in ('up','down')} + result = aggregate(joint, sweeps, .5, Settings()) + assert (result['min'],result['max']) == (8,240) + sweeps[(1,'up')] = samples(joint, 20, 243) + with pytest.raises(Unmeasurable, match='差异'): + aggregate(joint,sweeps,.5,Settings()) + sweeps = {(r,d):samples(joint,*( (0,100) if d=='up' else (150,255))) + for r in range(2) for d in ('up','down')} + with pytest.raises(Unmeasurable,match='交集'): + aggregate(joint,sweeps,.5,Settings()) + + +def test_no_motion_and_refinement(): + j = load_profile().joints[0] + with pytest.raises(Unmeasurable): + direction_range(samples(j, 0, 0), j, .5, Settings()) + with pytest.raises(RefineWindow): + direction_range(samples(j,6,243,commands=[0,16,32,64,128,192,224,255]),j,.5,Settings()) + + +@pytest.mark.parametrize('bounds', [(0, 230), (7, 255), (6, 243), (0, 255)]) +def test_endpoint_ranges_allow_internal_stops(bounds): + joint, settings = load_profile().joints[0], Settings() + sweeps = {} + for repeat in range(settings.fine_repetitions): + for direction in ('up', 'down'): + # Internal stops may differ between directions/repeats; only the ends matter. + offset = 8 * (repeat + (direction == 'down')) + def stopped(points, x): + distance = x - np.clip(x - 70 - offset, 0, 30) - np.clip(x - 140 - offset, 0, 40) + return points + [2 * distance, 0] + coarse_samples = samples(joint, *bounds, direction=direction, transform=stopped, + commands=list(range(0, 249, 8)) + [255]) + estimate = direction_range(coarse_samples, joint, .5, settings, coarse=True) + for (a, b), expected in zip(estimate['brackets'], bounds): + assert a <= expected <= b + commands = fine_commands(joint, [estimate], settings.fine_radius) + sweeps[(repeat, direction)] = samples(joint, *bounds, direction=direction, + transform=stopped, commands=commands) + result = aggregate(joint, sweeps, .5, settings) + assert (result['min'], result['max']) == bounds + + +def test_rotation_and_small_cumulative_movement(): + j = load_profile().joints[0] + def rotate(p,x): + angle=x*.005 + rotation=np.array([[np.cos(angle),-np.sin(angle)],[np.sin(angle),np.cos(angle)]]) + return (p-120) @ rotation +120 + result=direction_range(samples(j,6,243,transform=rotate),j,.5,Settings()) + assert result['max'] > result['min'] + result=direction_range(samples(j,6,243,scale=.1),j,.5,Settings()) + assert 6 <= result['min'] < 20 and 230 < result['max'] <= 243 + + +def test_profile_coverage_and_clearance(): + p = load_profile() + assert len(p.joints)==20 and len(p.tasks)==17 + group=next(t for t in p.tasks if t.name=='four_finger_roll') + assert [p.by_name[n].index for n in group.joints]==[2,3,4,5] + teach=simulated_teaching(p,'TEST') + assert teach.prepare(group)==tuple(teach.data['baseline']['target']) + with pytest.raises(ValueError,match='基础姿态'): + teach.capture('overrides',[255]*20,[255]*20,group.name) + for name, expected in [('pinky_pip',[]),('ring_pip',['pinky_fold']), + ('middle_pip',['pinky_fold','ring_fold']), + ('index_pip',['pinky_fold','ring_fold','middle_fold'])]: + task=next(t for t in p.tasks if t.name==name) + assert list(task.clearances)==expected + pose=teach.prepare(task) + for c in expected: + assert all(pose[p.by_name[n].index]==255 for n in p.clearances[c].joints) + bad=copy.deepcopy(p.raw);bad['tasks'][0]['joints']=['index_mcp_roll'] + with pytest.raises(ValueError): profile_from_dict(bad) + + +def test_missing_and_foreign_teaching(): + p=load_profile(); teach=Teaching(p,'TEST') + assert '全手基础姿态' in teach.missing(p.tasks) + with pytest.raises(ValueError): teach.prepare(p.tasks[0]) + with pytest.raises(ValueError): Teaching(p,'OTHER',simulated_teaching(p,'TEST').data) + + +def test_fixed_clearances_need_no_teaching_and_override_old_values(): + profile = load_profile() + teaching = Teaching(profile, 'TEST') + base = [40] * 20 + teaching.capture('baseline', base, base) + assert teaching.missing(profile.tasks) == [] + assert not profile.teaching_clearances + # Old device files remain readable, but their taught clearance values no longer win. + legacy = copy.deepcopy(teaching.data) + for group, clearance in profile.clearances.items(): + if clearance.targets is not None: + legacy['clearances'][group] = { + 'target': {name: 37 for name in clearance.joints}, + 'feedback': {name: 36 for name in clearance.joints}} + teaching = Teaching(profile, 'TEST', legacy) + tasks = {task.name: task for task in profile.tasks} + teaching.capture('overrides', [20] * 20, [20] * 20, 'index_pip') + for task in profile.tasks: + pose = teaching.prepare(task) + fixed = {name: value for group in task.clearances + for name, value in profile.clearances[group].targets.items()} + for joint in profile.joints: + assert pose[joint.index] == fixed.get(joint.name, 20 if task.name == 'index_pip' else 40) + assert teaching.prepare(tasks['four_finger_roll']) == tuple(base) + for group in profile.clearances: + with pytest.raises(ValueError, match='固定'): + teaching.capture('clearances', base, base, group) + + +def test_fixed_clearance_validation_and_legacy_profile_loading(): + data = copy.deepcopy(load_profile().raw) + data['clearances']['pinky_fold']['targets']['pinky_dip'] = 256 + with pytest.raises(ValueError, match='越界'): + profile_from_dict(data) + data['clearances']['pinky_fold']['targets']['pinky_dip'] = 254.5 + with pytest.raises(ValueError, match='分辨率'): + profile_from_dict(data) + # Older journal metadata contains only lists of joints and must keep that meaning. + data = copy.deepcopy(load_profile().raw) + for group, specification in data['clearances'].items(): + if isinstance(specification, dict): + data['clearances'][group] = list(specification['targets']) + profile = profile_from_dict(data) + assert set(profile.teaching_clearances) == set(profile.clearances) + teaching = Teaching(profile, 'OLD_LOG') + teaching.capture('baseline', [0] * 20, [0] * 20) + task = next(task for task in profile.tasks if task.name == 'ring_pip') + assert teaching.missing([task]) == ['pinky_fold'] + teaching.capture('clearances', [17] * 20, [17] * 20, 'pinky_fold') + pose = teaching.prepare(task) + assert all(pose[profile.by_name[name].index] == 17 + for name in profile.clearances['pinky_fold'].joints) + + +def test_thumb_clearance_uses_zero_with_or_without_old_teaching(): + profile = load_profile() + old_data = copy.deepcopy(simulated_teaching(profile, 'TEST').data) + tasks = {task.name: task for task in profile.tasks} + assert not old_data['clearances'] + teaching = Teaching(profile, 'TEST', old_data) + assert not teaching.missing(profile.tasks) + assert teaching.prepare(tasks['thumb_cmc_roll'])[2] == 0 + old_data['clearances']['index_roll_for_thumb'] = { + 'target': {'index_mcp_roll': 37}, 'feedback': {'index_mcp_roll': 36}} + before = copy.deepcopy(old_data) + teaching = Teaching(profile, 'TEST', old_data) + assert teaching.prepare(tasks['thumb_cmc_roll'])[2] == 0 + assert teaching.data == before + teaching.capture('overrides', [90] * 20, [90] * 20, 'thumb_cmc_roll') + assert teaching.prepare(tasks['thumb_cmc_roll'])[2] == 0 + assert teaching.prepare(tasks['four_finger_roll']) == tuple(before['baseline']['target']) + assert [task.name for task in profile.tasks if 'index_roll_for_thumb' in task.clearances] == ['thumb_cmc_roll'] + + # Restore the thumb first, then the index, using baseline targets rather than overrides. + current = [11] * 20 + steps = teaching.restoration_steps(tasks['thumb_cmc_roll'], current) + assert [name for name, _ in steps] == ['thumb_cmc_roll', 'index_mcp_roll'] + for name, restored in steps: + index = profile.by_name[name].index + assert restored[index] == before['baseline']['target'][index] + assert all(restored[i] == current[i] for i in range(20) if i != index) + current = restored + teaching.capture('overrides', [90] * 20, [90] * 20, 'thumb_mcp') + assert teaching.prepare(tasks['thumb_mcp'])[1] == 80 + assert [task.name for task in profile.tasks if 'thumb_yaw_for_mcp' in task.clearances] == ['thumb_mcp'] + for names in (['unknown_joint'], ['index_mcp_roll', 'index_mcp_roll']): + bad = copy.deepcopy(profile.raw) + bad['tasks'][0]['restore_after'] = names + with pytest.raises(ValueError, match='测后恢复'): + profile_from_dict(bad) + + +def test_thumb_task_transitions_restore_thumb_then_index_and_prepare_yaw_clearance(): + profile = load_profile() + settings = Settings(rate=10000, stable_frames=4, settle_seconds=.01) + adapter = FakeAdapter(profile) + teaching = simulated_teaching(profile, adapter.uid) + events = [] + engine = Engine(profile, settings, adapter, emit=events.append) + now = 1.0 + adapter.advance(now, 1_000_000_000) + engine.start(teaching, now, 1_000_000_000, ['thumb_cmc_roll', 'thumb_mcp', 'thumb_ip']) + for _ in range(10000): + now += .1 + stamp = round(now * 1e9) + adapter.advance(now, stamp) + for view, observations in adapter.frames().items(): + engine.observe(view, stamp, observations, now) + engine.tick(now, stamp) + assert engine.state != 'PAUSED', engine.reason + if engine.task.name == 'thumb_ip' and engine.state == 'SCANNING': + break + else: + pytest.fail('拇指侧摆未完成或下一任务未恢复基础姿态') + assert engine.results['thumb_cmc_roll']['min'] == 0 + assert engine.results['thumb_cmc_roll']['max'] == 255 + scanning = [event for event in events if event['kind'] == 'command' + and event['task'] == 'thumb_cmc_roll' and event['state'] == 'SCANNING'] + assert scanning and all(event['target'][2] == 0 for event in scanning) + started = [i for i, event in enumerate(events) if event['kind'] == 'restoration_started'] + restored = [i for i, event in enumerate(events) if event['kind'] == 'restored'] + mcp_started = next(i for i, event in enumerate(events) + if event['kind'] == 'task_started' and event['task'] == 'thumb_mcp') + assert len(started) == len(restored) == 2 + assert started[0] < restored[0] < started[1] < restored[1] < mcp_started + assert [events[i]['joints'] for i in started] == [['thumb_cmc_roll'], ['index_mcp_roll']] + boundaries = [i for i, event in enumerate(events) + if event['kind'] == 'boundary_found' and event['task'] == 'thumb_cmc_roll'] + assert [events[i]['direction'] for i in boundaries] == ['up', 'down'] + assert max(boundaries) < started[0] + baseline = teaching.data['baseline']['target'] + assert events[restored[0]]['target'][0] == baseline[0] + assert events[restored[0]]['target'][2] == 0 + assert events[restored[1]]['target'][0] == baseline[0] + assert events[restored[1]]['target'][2] == baseline[2] + for begin, end, held_index, held_value in ((started[0], restored[0], 2, 0), + (started[1], restored[1], 0, baseline[0])): + interval = events[begin:end] + commands = [event for event in interval if event['kind'] == 'command'] + assert commands and all(event['target'][held_index] == held_value for event in commands) + assert not any(event['kind'] == 'sample' for event in interval) + mcp_commands = [event for event in events if event['kind'] == 'command' + and event['task'] == 'thumb_mcp' and event['state'] == 'SCANNING'] + assert mcp_commands and all(event['target'][1] == 80 and + event['target'][2] == teaching.data['baseline']['target'][2] + for event in mcp_commands) + assert engine.command[2] == teaching.data['baseline']['target'][2] + assert engine.command[1] == teaching.data['baseline']['target'][1] + + +@pytest.mark.parametrize('finish', ['complete', 'cancel_thumb', 'cancel_index', 'timeout_thumb']) +def test_single_thumb_task_restores_in_order_and_waits_for_feedback(finish): + profile = load_profile() + adapter = FakeAdapter(profile) + settings = Settings(rate=10000, stable_frames=4, settle_seconds=.01) + teaching = simulated_teaching(profile, adapter.uid) + engine = Engine(profile, settings, adapter) + now = 1.0 + adapter.advance(now, round(now * 1e9)) + engine.start(teaching, now, round(now * 1e9), ['thumb_cmc_roll']) + for _ in range(1000): + now += .1 + stamp = round(now * 1e9) + adapter.advance(now, stamp) + for view, frame in adapter.frames().items(): + engine.observe(view, stamp, frame, now) + engine.tick(now, stamp) + assert engine.state != 'PAUSED', engine.reason + if engine.state == 'RESTORING': + break + assert engine.state == 'RESTORING' and engine.active and not engine.manual_allowed + assert engine.command[2] == 0 + # Each joint needs its own fresh, stable feedback. No Tag frames arrive during restoration. + for joint, cancellation in (('thumb_cmc_roll', 'cancel_thumb'), ('index_mcp_roll', 'cancel_index')): + assert engine.state == 'RESTORING' and engine.restoring_joint == joint + assert not engine.manual_allowed + with pytest.raises(ValueError, match='不能手动'): + engine.manual_adjust({'index_mcp_roll': 255}, now) + if finish == cancellation: + before, held = len(adapter.sent), engine.command + engine.pause('用户暂停回位') + engine.cancel() + engine.tick(now + 1, round((now + 1) * 1e9)) + assert len(adapter.sent) == before and engine.command == held + assert engine.command[2] == 0 + assert engine.results['thumb_cmc_roll']['status'] == '成功' + return + now += .1 + engine.tick(now, round(now * 1e9)) + assert engine.phase == 'waiting' + if joint == 'thumb_cmc_roll': + assert engine.command[2] == 0 + assert engine.command[0] == teaching.data['baseline']['target'][0] + now += .5 + engine.tick(now, round(now * 1e9)) + assert engine.state == 'RESTORING' and engine.restoring_joint == joint + if finish == 'timeout_thumb': + before = len(adapter.sent) + now += settings.point_timeout + adapter.advance(now, round(now * 1e9)) + engine.tick(now, round(now * 1e9)) + assert engine.state == 'PAUSED' and '回位稳定超时' in engine.reason + assert len(adapter.sent) == before and engine.command[2] == 0 + return + # Fresh but moving feedback must not release the next joint either. + index = profile.by_name[joint].index + for step in range(settings.stable_frames): + now += .1 + adapter.advance(now, round(now * 1e9)) + positions = list(adapter.feedback.positions) + positions[index] += 10 if step % 2 else -10 + adapter.feedback = replace(adapter.feedback, positions=tuple(positions)) + engine.tick(now, round(now * 1e9)) + assert engine.state == 'RESTORING' and engine.restoring_joint == joint + for _ in range(settings.stable_frames): + now += .1 + adapter.advance(now, round(now * 1e9)) + engine.tick(now, round(now * 1e9)) + assert engine.state == 'COMPLETED' + assert engine.command[0] == teaching.data['baseline']['target'][0] + assert engine.command[2] == teaching.data['baseline']['target'][2] + assert engine.manual_allowed + + +def test_manual_drag_sends_latest_target_without_scan_rate_limit(): + profile = load_profile() + adapter = FakeAdapter(profile) + engine = Engine(profile, Settings(rate=20), adapter) + adapter.advance(0, 0) + thumb, yaw = profile.joints[:2] + engine.manual_adjust({thumb.name: 100, yaw.name: 60}, 0) + engine.manual_adjust({thumb.name: 255}, .01) + assert not adapter.sent + engine.tick(1 / 30, 33_333_333) + assert engine.command[:2] == (255, 60) + assert len(adapter.sent) == 1 + # Reversing a drag is reflected in the very next control tick. + engine.manual_adjust({thumb.name: 0}, .04) + engine.tick(2 / 30, 66_666_666) + assert engine.command[:2] == (0, 60) + for step in range(1, 31): + now = (step + 2) / 30 + engine.manual_adjust({thumb.name: step}, now) + engine.tick(now, round(now * 1e9)) + assert engine.command[0] == step + assert all(target[2:] == (0,) * 18 for target in adapter.sent) + engine.manual_adjust({thumb.name: 255, yaw.name: 255}, now) + before = len(adapter.sent) + engine.pause('测试手动暂停') + with pytest.raises(ValueError, match='暂停'): + engine.manual_adjust({thumb.name: 255}, now) + engine.tick(1.2, 1_200_000_000) + assert len(adapter.sent) == before + engine.cancel() + engine.manual_adjust({thumb.name: 25}, 1.2) + assert engine.manual_target[1] == 60 # unsent cancelled goals must not return + + +def test_direct_teaching_waits_for_fresh_feedback_and_preserves_scan_rate(): + profile = load_profile() + adapter = FakeAdapter(profile) + engine = Engine(profile, Settings(rate=20, stable_frames=4), adapter) + for step in range(1, 5): + adapter.advance(step / 10, step * 100_000_000) + engine.tick(step / 10, step * 100_000_000) + assert engine.can_capture_teaching(.4) + engine.manual_adjust({profile.joints[0].name: 255}, .4) + engine.tick(.5, 500_000_000) + assert adapter.target[0] == 255 + # Even well after sending, old stable frames cannot complete teaching. + engine.tick(1, 1_000_000_000) + assert engine.state == 'TEACHING' and not engine.can_capture_teaching(1) + for step in range(11, 15): + adapter.advance(step / 10, step * 100_000_000) + engine.tick(step / 10, step * 100_000_000) + assert engine.state == 'IDLE' and engine.can_capture_teaching(1.4) + assert len(adapter.sent) == 1 # no repeated idle position commands + engine.start(simulated_teaching(profile, adapter.uid), 1.4, 1_400_000_000, + [profile.tasks[0].name]) + engine.tick(1.5, 1_500_000_000) + assert engine.state == 'PREPARING' and engine.command[0] == 253 + + +def test_diagnostics_allow_control_and_scan_but_connection_failures_block(): + profile = load_profile() + adapter = FakeAdapter(profile) + engine = Engine(profile, Settings(), adapter) + adapter.advance(0, 0) + adapter.warning = 'thumb_cmc_roll：执行器层判定堵转；thumb_cmc_yaw：执行器过温' + engine.manual_adjust({'thumb_cmc_roll':30, 'index_mcp_roll':40}, 0) + engine.tick(.03, 30_000_000) + assert adapter.target[0] == 30 and adapter.target[2] == 40 + engine.cancel() + engine.start(simulated_teaching(profile, adapter.uid), .04, 40_000_000) + assert engine.state == 'PREPARING' + engine.cancel() + engine.manual_adjust({'thumb_cmc_yaw':30}, .05) + engine.manual_adjust({'index_mcp_roll':50}, .06) + engine.tick(.07, 70_000_000) + assert adapter.target[2] == 50 and adapter.target[1] == 30 + engine.manual_adjust({'index_mcp_roll':60}, .08) + adapter.error = 'SDK 已断流' + before = len(adapter.sent) + engine.tick(.09, 90_000_000) + assert engine.state == 'PAUSED' and len(adapter.sent) == before + engine.cancel() + with pytest.raises(ValueError, match='断流'): + engine.manual_adjust({'thumb_cmc_roll':20}, .1) + with pytest.raises(ValueError, match='断流'): + engine.start(simulated_teaching(profile, adapter.uid), .1, 100_000_000) + + +@pytest.mark.parametrize('task_name', ['thumb_cmc_roll', 'four_finger_roll']) +def test_scan_uses_outer_visual_ranges_despite_internal_stops_and_sdk_diagnostics(task_name): + profile = load_profile() + task = next(task for task in profile.tasks if task.name == task_name) + specs = [profile.by_name[name] for name in task.joints] + bounds = [(0, 230), (7, 255), (6, 243), (0, 255)] + ranges = {joint.name: (joint.minimum, joint.maximum) for joint in profile.joints} + ranges.update({joint.name: bounds[i] for i, joint in enumerate(specs)}) + adapter = FakeAdapter(profile, ranges) + settings = Settings(rate=10000, stable_frames=4, settle_seconds=.01) + events = [] + engine = Engine(profile, settings, adapter, emit=events.append) + now = 1.0 + adapter.advance(now, 1_000_000_000) + engine.start(simulated_teaching(profile, adapter.uid), now, 1_000_000_000, [task_name]) + for _ in range(30000): + now += .1 + stamp = round(now * 1e9) + adapter.advance(now, stamp) + # Actual feedback stops at physical limits while commanded values keep increasing. + positions = list(adapter.feedback.positions) + for joint in specs: + lo, hi = ranges[joint.name] + positions[joint.index] = min(hi, max(lo, positions[joint.index])) + adapter.feedback = replace(adapter.feedback, positions=tuple(positions)) + adapter.warning = ('SDK 堵转提示：执行器层判定堵转' + if adapter.target[specs[0].index] > ranges[specs[0].name][1] else None) + frames = adapter.frames() + for joint in specs: + x = positions[joint.index] + stopped_distance = np.clip(x - 100, 0, 50) + corners = np.asarray(frames[joint.view][joint.tag_id]) + frames[joint.view][joint.tag_id] = (corners - [.8 * stopped_distance, 0]).tolist() + for view, observations in frames.items(): + engine.observe(view, stamp, observations, now) + engine.tick(now, stamp) + assert engine.state != 'PAUSED', engine.reason + if engine.state == 'COMPLETED': + break + else: + pytest.fail('带堵转提示的扫描未完成') + for joint in specs: + result = engine.results[joint.name] + assert (result['min'], result['max']) == ranges[joint.name] + warnings = [event for event in events if event['kind'] == 'diagnostic_warning'] + assert warnings and any(event['reason'] is None for event in warnings) + assert all(event['reason'] != previous['reason'] for previous, event in zip(warnings, warnings[1:])) + assert any(event['target'][specs[0].index] > event['feedback'][specs[0].index] + for event in warnings if event['reason']) + samples = [event for event in events if event['kind'] == 'sample'] + assert {event['direction'] for event in samples} == {'up', 'down'} + assert any(event['command'] == 255 for event in samples) + + +def test_quality_and_duplicate_frames(): + s=Settings(stable_frames=4) + corners=((100,100),(140,100),(140,140),(100,140)) + assert quality_error(corners,640,480,0,60,s) is None + assert quality_error(corners,640,480,1,60,s) + assert quality_error(corners,120,480,0,60,s) + w=StableWindow(4) + for _ in range(10):w.add(Observation(1,1,corners)) + assert w.summary(1) is None + for stamp in range(2,5):w.add(Observation(stamp,stamp,corners)) + assert w.summary(1)['frames']==4 + + +def test_group_holds_and_restart_preserves_completed_results(): + p=load_profile();s=Settings(stable_frames=4,rate=1000,settle_seconds=.01) + ranges={joint.name:(0,255) for joint in p.joints} + ranges['middle_mcp_roll']=(7,255) + a=FakeAdapter(p,ranges);e=Engine(p,s,a);teach=simulated_teaching(p,a.uid) + now=1.;a.advance(now,int(now*1e9));e.start(teach,now,int(now*1e9),['thumb_cmc_roll','four_finger_roll']) + for _ in range(400): + now+=.1;stamp=round(now*1e9);a.advance(now,stamp) + for view,frame in a.frames().items():e.observe(view,stamp,frame,now) + e.tick(now,stamp) + if (e.task.name=='four_finger_roll' and e.direction=='up' and e.phase=='moving' + and any(e.endpoint_results.values())):break + assert e.results['thumb_cmc_roll']['status']=='成功' + assert e.endpoint_results['index_mcp_roll'] and not e.endpoint_results['middle_mcp_roll'] + before=len(a.sent) + for _ in range(5): + now+=.1;stamp=round(now*1e9);a.advance(now,stamp) + frame=a.frames()['front'];frame.pop(4) + e.observe('front',stamp,frame,now);e.tick(now,stamp) + assert len(a.sent)==before + e.pause('测试暂停');a.advance(now,stamp);e.resume(now,stamp) + assert not e.samples and e.state=='PREPARING' + assert not any(e.endpoint_results.values()) and e.progress==(0,8) + assert e.results['thumb_cmc_roll']['status']=='成功' + e.cancel();before=len(a.sent);e.tick(now+1,stamp+1000000000) + assert len(a.sent)==before + + +def test_complete_simulation_and_replay(tmp_path): + p=load_profile() + output=simulate(p,Settings(),tmp_path) + document=json.loads(output.read_text()) + assert len(document['joints'])==20 + assert all(v['min'] is not None for v in document['joints'].values()) + assert set(document)=={'schema_version','model','side','device_uid','command_unit','joints'} + recovered=replay(output.parent/'samples.jsonl',tmp_path/'recomputed.json') + assert recovered==document + + +def test_generic_non_o30_domain(tmp_path): + data=copy.deepcopy(load_profile().raw) + data.update(model='VIRTUAL',command_unit='native',command={'minimum':0,'maximum':32,'resolution':1}) + data['joints']=data['joints'][:2] + data['tasks']=[{'name':'pair','joints':[j['name'] for j in data['joints']]}] + data['clearances']={} + p=profile_from_dict(data) + output=simulate(p,Settings(fine_radius=4),tmp_path) + assert len(json.loads(output.read_text())['joints'])==2 + + +def test_noise_and_stale_samples(): + p=load_profile();j=p.joints[0];rng=np.random.default_rng(42) + noisy=samples(j,6,243) + for item in noisy: + c=np.asarray(item['observations'][j.name]['corners']) + item['observations'][j.name]['corners']=(c+rng.normal(0,.04,c.shape)).tolist() + result=direction_range(noisy,j,.5,Settings()) + assert (result['min'],result['max'])==(6,243) + a=FakeAdapter(p);s=Settings(stable_frames=4,rate=1000,settle_seconds=.01) + e=Engine(p,s,a);teach=simulated_teaching(p,a.uid) + now=1.;a.advance(now,int(now*1e9));e.start(teach,now,int(now*1e9),['thumb_cmc_roll']) + for _ in range(400): + now+=.1;stamp=round(now*1e9);a.advance(now,stamp) + for view,frame in a.frames().items():e.observe(view,stamp,frame,now) + e.tick(now,stamp) + if e.state=='SCANNING':break + before=e.view_stamps['front'] + e.observe('front',before-1,{},now) + assert e.view_stamps['front']==before and ('front',0) in e.latest + + +@pytest.mark.parametrize('scenario', ['moving_front', 'missing_front', 'missing_side', 'moving_active_feedback']) +def test_pinky_pip_uses_its_own_tag_and_feedback_despite_coupled_motion(scenario): + profile = load_profile() + ranges = {joint.name: (joint.minimum, joint.maximum) for joint in profile.joints} + ranges['pinky_pip'] = (5, 240) + adapter = FakeAdapter(profile, ranges) + # Historical station files still load; their non-target tolerance no longer gates scans. + settings = Settings.from_dict({'rate': 10000, 'stable_frames': 4, 'settle_seconds': .01, + 'non_target_tolerance': .1}) + teaching = simulated_teaching(profile, adapter.uid) + events = [] + engine = Engine(profile, settings, adapter, events.append) + now = 1.0 + adapter.advance(now, round(now * 1e9)) + engine.start(teaching, now, round(now * 1e9), ['pinky_pip']) + assert engine.keys == {'pinky_pip': ('side', 5)} + for step in range(2000): + now += .1 + stamp = round(now * 1e9) + adapter.advance(now, stamp) + frames = adapter.frames() + if engine.state == 'SCANNING': + positions = list(adapter.feedback.positions) + positions[5] = 0 if step % 2 else 255 # continuous unrelated feedback movement + if scenario == 'moving_active_feedback': + positions[14] = 0 if step % 2 else 255 + adapter.feedback = replace(adapter.feedback, positions=tuple(positions)) + if scenario == 'missing_side': + frames['side'].pop(5) + if scenario == 'missing_front': + frames['front'] = {} + else: + frames['front'][1] = (np.asarray(frames['front'][1]) + [step * 10, 0]).tolist() + # Other side-camera Tags can be absent throughout this single-joint task. + frames['side'] = {tag: points for tag, points in frames['side'].items() if tag == 5} + for view, frame in frames.items(): + engine.observe(view, stamp, frame, now) + engine.tick(now, stamp) + if engine.state in ('COMPLETED', 'PAUSED'): + break + commands = [event for event in events if event['kind'] == 'command'] + assert commands and all(event['target'][5] == teaching.data['baseline']['target'][5] + for event in commands) + if scenario in ('missing_side', 'moving_active_feedback'): + assert engine.state == 'PAUSED' + assert ('Tag' if scenario == 'missing_side' else '稳定采样超时') in engine.reason + assert not engine.samples + else: + assert engine.state == 'COMPLETED', engine.reason + result = engine.results['pinky_pip'] + assert (result['min'], result['max']) == (5, 240) + samples = [event for event in events if event['kind'] == 'sample'] + assert {event['direction'] for event in samples} == {'up', 'down'} + assert all(set(event['observations']) == {'pinky_pip'} for event in samples) + assert {feedback.positions[5] for feedback in engine.feedbacks} == {0, 255} + assert not engine.feedback_stable() # unrelated feedback never settled + assert engine.feedback_stable(indices={14}) + + +def test_response_delay_does_not_become_endpoint(): + p=load_profile();a=FakeAdapter(p);s=Settings(stable_frames=4,rate=1000,settle_seconds=.3) + e=Engine(p,s,a);teach=simulated_teaching(p,a.uid) + now=1.;a.advance(now,int(now*1e9));e.start(teach,now,int(now*1e9),['thumb_cmc_roll']) + for _ in range(300): + now+=.05;stamp=round(now*1e9);a.advance(now,stamp) + for view,frame in a.frames().items():e.observe(view,stamp,frame,now) + e.tick(now,stamp) + if e.state=='SCANNING' and e.phase=='waiting':break + assert not e.samples + gate=e.gate_ns + for offset in range(1,5): + # New delivery of frames captured before the settle gate cannot satisfy it. + e.observe('front',min(gate-1,stamp+offset),a.frames()['front'],now) + assert not e.windows['thumb_cmc_roll'].values diff --git a/src/linkerhand_range_calibration/test/test_endpoints.py b/src/linkerhand_range_calibration/test/test_endpoints.py new file mode 100644 index 0000000..f2a6b3e --- /dev/null +++ b/src/linkerhand_range_calibration/test/test_endpoints.py @@ -0,0 +1,246 @@ +"""Endpoint-only measurements, early stopping and compatible journal replay.""" +from dataclasses import asdict, replace +import json + +import numpy as np +import pytest + +from linkerhand_range_calibration.adapters.fake import FakeAdapter +from linkerhand_range_calibration.core.endpoints import ( + SCAN_METHOD, EndpointSearch, aggregate_endpoints, analyze_endpoint_samples, +) +from linkerhand_range_calibration.core.engine import Engine +from linkerhand_range_calibration.core.errors import Unmeasurable +from linkerhand_range_calibration.profiles import Settings, load_profile, profile_from_dict +from linkerhand_range_calibration.replay import replay +from linkerhand_range_calibration.simulation import simulated_teaching +from linkerhand_range_calibration.storage import Session + + +def observed(x, scale=2, noise=0): + corners = np.array([[100, 100], [140, 100], [140, 140], [100, 140]], dtype=float) + return {'corners': (corners + [x * scale, 0]).tolist(), 'noise': noise} + + +def endpoint_samples(joint, direction, bounds, settings, observe=observed): + commands = range(256) if direction == 'up' else range(255, -1, -1) + search, samples = EndpointSearch(joint, direction, settings), [] + for command in commands: + observation = observe(float(np.clip(command, *bounds))) + samples.append({'command': command, 'observations': {joint.name: observation}}) + search.add(command, observation) + if search.done: + break + return search, samples + + +@pytest.mark.parametrize('bounds', [(0, 230), (7, 255), (6, 243), (0, 255)]) +def test_search_confirms_first_trigger_and_stops_early(bounds): + joint, settings = load_profile().joints[0], Settings() + sweeps = {} + for direction in ('up', 'down'): + search, samples = endpoint_samples(joint, direction, bounds, settings) + sweeps[(0, direction)] = samples + boundary = search.result() + if direction == 'up': + assert boundary == {'bound': bounds[0], 'trigger': bounds[0] + 1, + 'confirmed_at': bounds[0] + 3, 'threshold': .5} + else: + assert boundary == {'bound': bounds[1], 'trigger': bounds[1] - 1, + 'confirmed_at': bounds[1] - 3, 'threshold': .5} + assert len(samples) < 35 + result = analyze_endpoint_samples(joint, sweeps, settings) + assert (result['min'], result['max']) == bounds + + +def test_transient_outlier_is_rejected_and_cumulative_motion_is_detected(): + joint, settings = load_profile().joints[0], Settings() + search = EndpointSearch(joint, 'up', settings) + for command in range(10): + x = 50 if command == 2 else float(np.clip(command, 6, 243)) + search.add(command, observed(x)) + assert search.result()['trigger'] == 7 + assert search.result()['bound'] == 6 + # Adjacent positions differ by less than the threshold, but the endpoint remains fixed. + search, _ = endpoint_samples(joint, 'up', (6, 243), settings, + observe=lambda x: observed(x, scale=.1)) + assert 6 < search.result()['bound'] < 20 + + +def test_rotation_and_independent_noise_reference_at_each_end(): + joint, settings = load_profile().joints[0], Settings() + def rotate(x): + corners = np.asarray(observed(0)['corners']) - 120 + angle = .05 * x + rotation = np.array([[np.cos(angle), -np.sin(angle)], [np.sin(angle), np.cos(angle)]]) + return {'corners': (corners @ rotation + 120).tolist(), 'noise': 0} + for direction, expected in (('up', 6), ('down', 243)): + search, _ = endpoint_samples(joint, direction, (6, 243), settings, observe=rotate) + assert search.result()['bound'] == expected + low = EndpointSearch(joint, 'up', settings) + high = EndpointSearch(joint, 'down', settings) + low.add(0, observed(0, noise=.01)) + high.add(255, observed(255, noise=.2)) + assert low.threshold == .5 and high.threshold == 1 + + +def test_search_rejects_missing_duplicate_and_unconfirmed_samples(): + joint, settings = load_profile().joints[0], Settings() + for commands in ([1], [0, 0], [0, 2]): + search = EndpointSearch(joint, 'up', settings) + with pytest.raises(Unmeasurable, match='连续推进'): + for command in commands: + search.add(command, observed(command)) + for direction in ('up', 'down'): + search, samples = endpoint_samples(joint, direction, (0, 0), settings) + assert len(samples) == 256 and search.done + with pytest.raises(Unmeasurable, match='未获得足够'): + search.result() + search, _ = endpoint_samples(joint, 'up', (254, 255), settings) + with pytest.raises(Unmeasurable, match='未获得足够'): + search.result() + + +def test_optional_repeat_and_disjoint_boundary_evidence(): + joint, settings = load_profile().joints[0], Settings(endpoint_repetitions=2) + boundaries = {} + for repeat, bounds in enumerate(((6, 243), (8, 241))): + for direction in ('up', 'down'): + search, _ = endpoint_samples(joint, direction, bounds, settings) + boundaries[(repeat, direction)] = search.result() + result = aggregate_endpoints(joint, boundaries, settings) + assert (result['min'], result['max']) == (8, 241) + boundaries[(1, 'up')]['bound'] = 20 + with pytest.raises(Unmeasurable, match='复测差异'): + aggregate_endpoints(joint, boundaries, settings) + settings = replace(settings, endpoint_repetitions=1) + with pytest.raises(Unmeasurable, match='交集'): + aggregate_endpoints(joint, {(0, 'up'): {'bound': 150}, (0, 'down'): {'bound': 100}}, settings) + for value in (0, -1, 1.5): + with pytest.raises(ValueError): + Settings.from_dict({'endpoint_repetitions': value}) + + +def run_scan(tmp_path, task_name, bounds, settings=None, profile=None): + profile = profile or load_profile() + settings = settings or Settings(rate=10000, stable_frames=4, settle_seconds=.01) + task = next(task for task in profile.tasks if task.name == task_name) + ranges = {joint.name: (joint.minimum, joint.maximum) for joint in profile.joints} + ranges.update(dict(zip(task.joints, bounds))) + adapter = FakeAdapter(profile, ranges) + teaching = simulated_teaching(profile, adapter.uid) + session = Session(tmp_path, profile, adapter.uid, asdict(settings), teaching, {'simulated': True}) + engine = Engine(profile, settings, adapter, session.emit) + now = 1.0 + adapter.advance(now, 1_000_000_000) + engine.start(teaching, now, 1_000_000_000, [task_name]) + try: + for _ in range(10000): + now += .1 + stamp = round(now * 1e9) + adapter.advance(now, stamp) + for view, observations in adapter.frames().items(): + engine.observe(view, stamp, observations, now) + engine.tick(now, stamp) + assert engine.state != 'PAUSED', engine.reason + if engine.state == 'COMPLETED': + break + assert engine.state == 'COMPLETED' + finally: + session.close() + events = [json.loads(line) for line in (session.directory / 'samples.jsonl').read_text().splitlines()] + return engine, session, events + + +def test_single_task_samples_only_endpoint_neighborhoods_and_replays(tmp_path): + engine, session, events = run_scan(tmp_path, 'thumb_cmc_roll', [(6, 243)]) + points = [event for event in events if event['kind'] == 'sample'] + assert [(p['direction'], p['command']) for p in points] == ( + [('up', c) for c in range(10)] + [('down', c) for c in range(255, 239, -1)]) + assert len(points) == 26 + assert {p['stage'] for p in points} == {'endpoint'} + assert events[0]['scan_method'] == SCAN_METHOD + assert engine.progress == (2, 2) + # Recompute from observations even if stored success and boundary events are falsified. + for event in events: + if event['kind'] == 'boundary_found': + event['bound'] = 100 + elif event['kind'] == 'task_result': + event['results']['thumb_cmc_roll'].update(min=100, max=101) + journal = tmp_path / 'modified.jsonl' + journal.write_text('\n'.join(json.dumps(event) for event in events)) + result = replay(journal, tmp_path / 'recomputed.json') + assert result == json.loads(session.result_path.read_text()) + assert result['joints']['thumb_cmc_roll'] == {'min': 6, 'max': 243} + + +def test_four_fingers_share_commands_and_stop_when_all_boundaries_confirmed(tmp_path): + bounds = [(0, 230), (7, 255), (6, 243), (0, 255)] + engine, session, events = run_scan(tmp_path, 'four_finger_roll', bounds) + for name, expected in zip(engine.task.joints, bounds): + assert (engine.results[name]['min'], engine.results[name]['max']) == expected + points = [event for event in events if event['kind'] == 'sample'] + assert [p['command'] for p in points if p['direction'] == 'up'] == list(range(11)) + assert [p['command'] for p in points if p['direction'] == 'down'] == list(range(255, 226, -1)) + assert all(len(set(event['target'][2:6])) == 1 for event in events + if event['kind'] == 'command' and event['state'] == 'SCANNING') + assert engine.progress == (8, 8) + assert replay(session.directory / 'samples.jsonl', tmp_path / 'recomputed.json') == session.document() + + +def test_stationary_finger_fails_without_repeating_its_search_at_other_end(tmp_path): + engine, session, events = run_scan(tmp_path, 'four_finger_roll', [(0, 0), (7, 255), (6, 243), (0, 255)]) + name = engine.task.joints[0] + assert engine.results[name]['min'] is None and engine.results[name]['max'] is None + assert all(engine.results[other]['status'] == '成功' for other in engine.task.joints[1:]) + points = [event for event in events if event['kind'] == 'sample'] + assert len([p for p in points if p['direction'] == 'up']) == 256 + assert len([p for p in points if p['direction'] == 'down']) == 16 + assert replay(session.directory / 'samples.jsonl', tmp_path / 'recomputed.json') == session.document() + + +def test_fully_stationary_task_finishes_with_null_range(tmp_path): + engine, session, events = run_scan(tmp_path, 'thumb_cmc_roll', [(0, 0)]) + assert engine.results['thumb_cmc_roll']['status'] == '失败' + assert session.document()['joints']['thumb_cmc_roll'] == {'min': None, 'max': None} + samples = [event for event in events if event['kind'] == 'sample'] + assert len(samples) == 256 and {s['direction'] for s in samples} == {'up'} + assert replay(session.directory / 'samples.jsonl', tmp_path / 'recomputed.json') == session.document() + + +def test_live_optional_repetitions_and_fractional_non_o30_domain(tmp_path): + data = json.loads(json.dumps(load_profile().raw)) + data.update(model='VIRTUAL', command_unit='native', command={'minimum': -2, 'maximum': 8, 'resolution': .5}) + data['joints'] = data['joints'][:1] + data['tasks'] = [{'name': 'virtual', 'joints': [data['joints'][0]['name']]}] + data['clearances'] = {} + profile = profile_from_dict(data) + settings = Settings(rate=10000, stable_frames=4, settle_seconds=.01, + endpoint_repetitions=2, motion_floor_px=.2) + engine, session, events = run_scan(tmp_path, 'virtual', [(-1, 7)], settings, profile) + assert session.document()['joints']['thumb_cmc_roll'] == {'min': -1, 'max': 7} + assert len([e for e in events if e['kind'] == 'boundary_found']) == 4 + assert replay(session.directory / 'samples.jsonl', tmp_path / 'recomputed.json') == session.document() + + +def test_legacy_full_sweep_journal_still_replays(tmp_path): + profile, settings = load_profile(), Settings() + joint, task = profile.joints[0], profile.tasks[0] + events = [ + {'kind': 'metadata', 'profile': profile.raw, 'settings': asdict(settings), 'uid': 'OLD_LOG'}, + {'kind': 'task_started', 'task': task.name, 'joints': list(task.joints)}, + {'kind': 'baseline', 'task': task.name, 'thresholds': {joint.name: .5}}, + ] + for stage, repetitions, commands in [('coarse', 1, list(range(0, 249, 8)) + [255]), + ('fine', 2, list(range(256)))]: + for repeat in range(repetitions): + for direction in ('up', 'down'): + for command in commands if direction == 'up' else commands[::-1]: + events.append({'kind': 'sample', 'task': task.name, 'stage': stage, 'repeat': repeat, + 'direction': direction, 'command': command, + 'observations': {joint.name: observed(float(np.clip(command, 6, 243)))}}) + events.append({'kind': 'task_result', 'task': task.name, 'results': {joint.name: {'min': 0, 'max': 255}}}) + journal = tmp_path / 'old.jsonl' + journal.write_text('\n'.join(json.dumps(event) for event in events)) + result = replay(journal, tmp_path / 'recomputed.json') + assert result['joints'][joint.name] == {'min': 6, 'max': 243} diff --git a/src/linkerhand_range_calibration/test/test_gui.py b/src/linkerhand_range_calibration/test/test_gui.py new file mode 100644 index 0000000..1957f6a --- /dev/null +++ b/src/linkerhand_range_calibration/test/test_gui.py @@ -0,0 +1,211 @@ +"""Exercise real Qt widgets against the ROS host with its fake adapter.""" +import os +from pathlib import Path +import subprocess +import sys +import textwrap + + +def test_demo_gui_teach_scan_pause_resume_cancel(tmp_path): + program = r''' +import json,time,sys +from pathlib import Path +import rclpy +from PyQt5.QtCore import Qt +from PyQt5.QtTest import QTest +from PyQt5.QtWidgets import QApplication,QLabel +from linkerhand_range_calibration.profiles import load_profile,load_station +from linkerhand_range_calibration.runtime import CalibrationRuntime +from linkerhand_range_calibration.ui.window import CalibrationWindow + +rclpy.init();app=QApplication([]) +station=load_station();station['output_root']=str(Path(sys.argv[1])/'output') +station['teaching_root']=str(Path(sys.argv[1])/'teaching') +station['scan'].update(rate=1000,stable_frames=4,settle_seconds=.01) +node=CalibrationRuntime(load_profile(),station,demo=True) +window=CalibrationWindow(node);window.show() +def until(condition,timeout=6): + end=time.monotonic()+timeout + while time.monotonic()1) +warnings=[event for event in events if event['kind']=='diagnostic_warning'] +assert len(warnings)==1 +assert 'thumb_cmc_roll' in warnings[0]['reason'] and 'thumb_roll' in warnings[0]['reason'] +assert warnings[0]['stamp_ns']>0 and len(warnings[0]['target'])==20 + +# Running SDK diagnostics do not add interlocks to its existing command path. +for update,detail in [ + ({'joint_faults':{'thumb_roll':['执行器层判定堵转','执行器过流'],'thumb_yaw':['执行器过温']}},'执行器过流'), + ({'over_temp':['little_yaw:85']},'85°C'), + ({'joint_faults':{'thumb_yaw':['执行器离线','执行器异常']}},'执行器离线'), + ({'joint_faults':{'unknown_joint':['未知故障']}},'未知故障'), + ({'online':False,'comm_error':{'code':2,'names':['模拟通信诊断']}},'模拟通信诊断'), +]: + diagnostics.update(update) + until(lambda:detail in (a.diagnostic_warning() or '')) + assert a.control_error(time.monotonic()) is None + before=len(commands) + engine.tick(time.monotonic(),owner.get_clock().now().nanoseconds) + assert engine.state=='PREPARING' + until(lambda:len(commands)==before+1) + assert len(settings)==2 +engine.pause('用户暂停') +engine.resume(time.monotonic(),owner.get_clock().now().nanoseconds) +assert engine.state=='PREPARING' +engine.cancel() +manual=Engine(p,Settings(),a) +before=len(commands) +manual.manual_adjust({'thumb_cmc_roll':10,'thumb_cmc_yaw':25,'pinky_mcp_roll':100},time.monotonic()) +manual.tick(time.monotonic(),owner.get_clock().now().nanoseconds) +until(lambda:len(commands)==before+1) +assert commands[-1].position[0]==10 and commands[-1].position[1]==25 +assert commands[-1].position[5]==100 +diagnostics.update(joint_faults={},over_temp=[],online=True,comm_error={'code':0}) +until(lambda:a.diagnostic_warning() is None) +assert a.control_error(time.monotonic()) is None + +# SDK startup checks are still enabled; identity and the sole command owner are required. +parameters=a.launch_parameters(p,{'sdk':{}}) +assert parameters['strict_device_check'] and not parameters['ignore_joint_faults'] +diagnostics['side']='LEFT' +until(lambda:'身份不符' in (a.control_error(time.monotonic()) or '')) +diagnostics.pop('side') +until(lambda:a.control_error(time.monotonic()) is None) + +other=Node('other_controller');ex.add_node(other) +other_pub=other.create_publisher(JointState,p.sdk['command_topic'],10) +until(lambda:'发布者' in (a.control_error(time.monotonic()) or '')) +other.destroy_publisher(other_pub) +until(lambda:a.control_error(time.monotonic()) is None) +timer.cancel() +until(lambda:'断流' in (a.control_error(time.monotonic()) or ''),timeout=3) +before=len(commands) +manual.tick(time.monotonic(),owner.get_clock().now().nanoseconds) +assert manual.state=='PAUSED' and len(commands)==before +ex.shutdown();other.destroy_node();owner.destroy_node();sdk.destroy_node();rclpy.shutdown() +print('isolated ROS adapter passed') +''' + env = dict(os.environ, ROS_DOMAIN_ID='198', ROS_LOCALHOST_ONLY='1') + env['PYTHONPATH'] = str(Path(__file__).resolve().parents[1]) + os.pathsep + env.get('PYTHONPATH','') + result = subprocess.run([sys.executable,'-c',textwrap.dedent(program)],env=env, + capture_output=True,text=True,timeout=25) + assert result.returncode==0, result.stdout+result.stderr + + +def test_static_validation_never_imports_hardware(): + program = ''' +import sys +from linkerhand_range_calibration.cli import main +assert main(['--validate-only'])==0 +assert 'rclpy' not in sys.modules +assert 'linkerhand_range_calibration.adapters.o30_ros' not in sys.modules +assert 'PyQt5' not in sys.modules +''' + env = dict(os.environ) + env['PYTHONPATH'] = str(Path(__file__).resolve().parents[1]) + os.pathsep + env.get('PYTHONPATH','') + result = subprocess.run([sys.executable,'-c',textwrap.dedent(program)],env=env, + capture_output=True,text=True,timeout=10) + assert result.returncode==0, result.stdout+result.stderr