新增 O30 右手行程标定功能包并完善相机恢复

支持三机位 Tag 两端边界标定、滑块示教、顺序避让恢复、同步四指扫描和 JSON 离线复算;修复相机时钟异常导致采集线程退出的问题,并忽略本地行程标定输出。
This commit is contained in:
lxp
2026-09-17 12:41:53 +08:00
parent 7a04780b52
commit af2dc9c38f
42 changed files with 3900 additions and 3 deletions
+1
View File
@@ -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.
+1
View File
@@ -717,6 +717,7 @@ ros2 run linkerhand_calibration compare_calibration_urdfs \
连续扫描将 CAN 实测反馈与相机曝光中点配对。MVS 取帧/图像发布时间不参与替代曝光时间;
设备时钟由独立锁存换算到 ROS 时钟,换算依据保存在会话的 `camera_timing_<view>.jsonl`
运行中的时钟校验失败会暂停出图并重新执行独立锁存校验,通过后恢复采集;校验失败期间不会继续使用旧映射,也不会永久退出采集线程。恢复前后的图像时间戳仍须严格递增;若主机时钟向后跳变,早于已发布图像的帧继续丢弃。
四型号共用 `unified_engine_v8_all_view_images`,保留设备曝光时间策略;旧采集只能按其原策略诊断,不能混用。
报告中的 `feedback_u8` 表示来自字节通道的测量值:插值到曝光时间后可以是小数,
拟合、内存映射和报告读取均按声明的分段线性曲线计算,不再次取整。
@@ -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,
@@ -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)
@@ -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'
@@ -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
+230
View File
@@ -0,0 +1,230 @@
# LinkerHand 有效指令行程标定
本包通过指尖 AprilTag 的图像运动,测量机械手实际有效的控制指令范围。首个型号为 O30 右手。
输出的是 `0255` 中的有效指令上下界,单位为 `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/<UID>/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_rollSDK: thumb_roll):执行器层判定堵转`
诊断标志出现时继续执行两端搜索,通过新图像确认运动或静止。
诊断标志不作为边界证据,范围由发送指令对应的角点运动决定。
扫描期间诊断发生变化或消失时,独立 JSONL 日志记录 `diagnostic_warning` 事件(消失时 `reason` 为null),包含目标、反馈、任务与时间;正式结果JSON不增加字段。
控制连接和视觉采样导致的暂停会显示具体原因。条件恢复后,点击“继续”重做当前任务;也可取消后手动调整姿态再重测。
## 6. 文件与离线复算
结果位置:`range_calibration_output/<UID>/<时间>/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。实机验收需要操作者完成示教后,依次验证单关节、四指同步、侧面避让和全手复测。
软件测试通过不等于实机测量精度通过。
@@ -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]}
@@ -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
@@ -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),
])
@@ -0,0 +1 @@
"""Visual command-range calibration, independent of vendor SDK source code."""
@@ -0,0 +1 @@
"""Hardware interfaces and a deterministic simulator."""
@@ -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): ...
@@ -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
@@ -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('速度和力矩必须为0255整数')
self.setting_pub.publish(String(data=json.dumps({'setting_cmd': command, 'params': {
'hand_type': self.profile.side, field: [value]*len(self.profile.joints)}})))
@@ -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)
@@ -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())
@@ -0,0 +1 @@
"""Deterministic calibration logic without ROS or Qt imports."""
@@ -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()}}
@@ -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)
@@ -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)
@@ -0,0 +1,5 @@
"""Shared measurement errors for live scans and offline analysis."""
class Unmeasurable(ValueError):
pass
@@ -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))
@@ -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} 必须为0255整数')
return data
@@ -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
@@ -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()
@@ -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()
@@ -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()
@@ -0,0 +1 @@
"""Qt user interface."""
@@ -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())
@@ -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()
@@ -0,0 +1 @@
"""Image-coordinate observations; no extrinsics or pose reconstruction."""
@@ -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)}
@@ -0,0 +1,29 @@
<?xml version="1.0"?>
<package format="3">
<name>linkerhand_range_calibration</name>
<version>0.1.0</version>
<description>Profile-driven visual command range calibration.</description>
<maintainer email="support@linker-robotics.com">lxp</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_python</buildtool_depend>
<exec_depend>ament_index_python</exec_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>rcl_interfaces</exec_depend>
<exec_depend>apriltag_msgs</exec_depend>
<exec_depend>apriltag_ros</exec_depend>
<exec_depend>image_proc</exec_depend>
<exec_depend>cv_bridge</exec_depend>
<exec_depend>launch_ros</exec_depend>
<exec_depend>launch</exec_depend>
<exec_depend>rclcpp_components</exec_depend>
<exec_depend>linkerhand_calibration</exec_depend>
<exec_depend>linker_hand_o30_ros2_sdk</exec_depend>
<exec_depend>python3-numpy</exec_depend>
<exec_depend>python3-opencv</exec_depend>
<exec_depend>python3-yaml</exec_depend>
<exec_depend>python3-pyqt5</exec_depend>
<test_depend>python3-pytest</test_depend>
<export><build_type>ament_python</build_type></export>
</package>
@@ -0,0 +1,6 @@
[develop]
script_dir=$base/lib/linkerhand_range_calibration
[install]
install_scripts=$base/lib/linkerhand_range_calibration
[tool:pytest]
testpaths=test
+21
View File
@@ -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',
]},
)
@@ -0,0 +1,4 @@
import sys
from pathlib import Path
sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
@@ -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
@@ -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}
@@ -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()<end:
rclpy.spin_once(node,timeout_sec=.015);app.processEvents();window.refresh()
if condition():return
raise AssertionError(str(node.snapshot()))
until(lambda:node.snapshot().get('can_save',False))
assert not node.adapter.sent and not node.adapter.settings_sent
assert len(node.previews())==3
assert len(window.joint_controls)==20
clearance_choices=[window.save_kind.itemData(i)[1] for i in range(window.save_kind.count())
if window.save_kind.itemData(i)[0]=='clearances']
assert clearance_choices==[]
first,second=window.joint_controls[:2]
assert first.slider.minimum()==0 and first.slider.maximum()==255
window.sync_targets();window.fill.click()
node._tick()
assert not node.adapter.sent and not node.adapter.settings_sent
# Many drag changes before one runtime tick keep only the latest target.
for value in range(20,101):first.slider.setValue(value)
assert not node.adapter.sent
node._tick();window.refresh()
assert node.engine.state=='TEACHING' and node.adapter.target[0]==100
assert first.isEnabled() and not window.save.isEnabled()
assert len(node.adapter.settings_sent)==1
second.slider.setValue(45)
first.slider.setValue(30)
node._tick()
assert node.adapter.target[:2]==(30,45)
assert first.editor.value()==30 and second.editor.value()==45
assert len(node.adapter.settings_sent)==1
until(lambda:node.engine.state=='IDLE')
assert node.adapter.target[:2]==(30,45)
assert node.adapter.target[2:]==(0,)*18
# Numeric typing waits for confirmation, and then drives the same slider path.
first.editor.setFocus();first.editor.selectAll()
QTest.keyClicks(first.editor,'35')
node._tick()
assert node.adapter.target[0]==30 and not node.pending_jog
QTest.keyClick(first.editor,Qt.Key_Return)
until(lambda:node.engine.state=='IDLE' and node.adapter.target[0]==35)
assert first.slider.value()==35
assert len(node.adapter.settings_sent)==1 # idle between gestures does not resend settings
# Finishing one joint must not overwrite another joint's unconfirmed numeric input.
second.slider.setValue(80);node._tick();window.refresh()
first.editor.setFocus();first.editor.selectAll()
QTest.keyClicks(first.editor,'36')
until(lambda:node.engine.state=='IDLE')
assert first.editor.text()=='36' and node.adapter.target[0]==35
QTest.keyClick(first.editor,Qt.Key_Escape)
window.sync_targets()
# Cancel discards a pending gesture and rejects late events from that UI epoch.
old_epoch=node.snapshot()['manual_epoch']
before=len(node.adapter.sent)
first.slider.setValue(200)
node.submit('cancel')
node.submit('jog',targets={node.profile.joints[0].name:240},epoch=old_epoch)
node._tick();window.refresh()
assert node.engine.state=='CANCELLED' and len(node.adapter.sent)==before
assert first.slider.value()==35
# Pause holds the last sent target, and manual controls remain locked until cancel.
first.slider.setValue(200);node._tick();window.refresh()
assert node.engine.state=='TEACHING'
window.pause.click();until(lambda:node.engine.state=='PAUSED')
assert not first.isEnabled() and not window.resume.isEnabled()
before=len(node.adapter.sent)
node.submit('jog',targets={node.profile.joints[0].name:255},epoch=node.snapshot()['manual_epoch'])
node._tick()
assert len(node.adapter.sent)==before
window.cancel.click();until(lambda:node.engine.state=='CANCELLED')
window.start_one.click()
until(lambda:node.engine.state=='SCANNING')
assert not first.isEnabled() and not window.fill.isEnabled()
window.pause.click();until(lambda:node.engine.state=='PAUSED')
window.resume.click();until(lambda:node.engine.state=='PREPARING')
window.cancel.click();until(lambda:node.engine.state=='CANCELLED')
document=json.loads(node.session.result_path.read_text())
assert len(document['joints'])==20 and all(v=={'min':None,'max':None} for v in document['joints'].values())
held=node.engine.command
first.slider.setValue(10)
until(lambda:node.engine.state=='IDLE' and node.adapter.target[0]==10)
assert node.adapter.target[1:]==held[1:]
until(lambda:node.snapshot()['can_save'])
window.save.click();until(lambda:node.teaching_path.exists())
assert node.teaching.data['baseline']['target'][0]==10
# Saving a nonzero baseline keeps the fixed thumb clearance at zero.
index_control=window.joint_controls[2]
index_control.slider.setValue(37)
until(lambda:node.snapshot()['can_save'] and node.adapter.target[2]==37)
window.save.click()
until(lambda:node.teaching.data['baseline']['target'][2]==37)
saved=json.loads(json.dumps(node.teaching.data))
assert saved['baseline']['target'][0]==10
assert not saved['clearances']
tasks={task.name:task for task in node.profile.tasks}
assert node.teaching.prepare(tasks['thumb_cmc_roll'])[2]==0
assert node.teaching.prepare(tasks['four_finger_roll'])[2]==37
# A command received during a tick must not expose its epoch before it is applied.
original_tick=node.engine.tick
def cancel_during_tick(now,stamp):
node.submit('cancel')
original_tick(now,stamp)
node.engine.tick=cancel_during_tick
node._tick()
node.engine.tick=original_tick
assert node.snapshot()['manual_epoch']<node.manual_epoch
node.submit('jog',targets={node.profile.joints[0].name:255},epoch=node.snapshot()['manual_epoch'])
node._tick()
assert node.engine.state=='CANCELLED' and not node.pending_jog
# SDK diagnostics remain in the journal and never add a banner or lock controls.
node.adapter.warning='thumb_cmc_roll:执行器层判定堵转;thumb_cmc_yaw:执行器过温'
node._tick();window.refresh()
assert window.start_one.isEnabled() and all(control.isEnabled() for control in window.joint_controls)
assert not any('自动标定不可用' in label.text() or '堵转' in label.text() or '过温' in label.text()
for label in window.findChildren(QLabel))
window.start_one.click();until(lambda:node.engine.state=='SCANNING')
events=[json.loads(line) for line in (node.session.directory/'samples.jsonl').read_text().splitlines()]
assert any(event['kind']=='diagnostic_warning' and '过温' in event['reason'] for event in events)
window.pause.click();until(lambda:node.engine.state=='PAUSED')
assert window.resume.isEnabled()
window.resume.click();until(lambda:node.engine.state=='PREPARING')
window.cancel.click();until(lambda:node.engine.state=='CANCELLED')
# Manual teaching uses the same readiness check, including the joint named in a diagnostic.
node._tick();window.refresh()
assert first.isEnabled() and index_control.isEnabled() and second.isEnabled()
assert window.start_one.isEnabled()
settings_with_warning=len(node.adapter.settings_sent)
second.slider.setValue(75)
index_control.slider.setValue(45)
until(lambda:node.snapshot()['can_save'] and node.adapter.target[2]==45 and node.adapter.target[1]==75)
assert node.engine.state=='IDLE'
assert window.save.isEnabled() and len(node.adapter.settings_sent)==settings_with_warning
window.save.click()
until(lambda:node.teaching.data['baseline']['target'][2]==45)
node.adapter.warning=None
node._tick();window.refresh()
assert second.isEnabled() and window.start_one.isEnabled()
# After a connection fault, the next deliberate gesture reapplies settings once.
settings_before=len(node.adapter.settings_sent)
node.adapter.error='模拟反馈断流'
node._tick();window.refresh()
assert not first.isEnabled() and node.motion_settings_key is None
node.adapter.error=None
node._tick();window.refresh()
first.slider.setValue(11)
node._tick()
assert node.adapter.target[0]==11
assert len(node.adapter.settings_sent)==settings_before+1
until(lambda:node.engine.state=='IDLE')
# A single thumb-roll task restores the thumb before releasing the index clearance.
window.start_one.click();until(lambda:node.engine.state=='RESTORING')
assert not any(control.isEnabled() for control in window.joint_controls)
assert not window.save.isEnabled() and window.cancel.isEnabled()
assert '测后恢复基础姿态' in window.status.text()
assert node.engine.restoring_joint=='thumb_cmc_roll'
assert 'thumb_cmc_roll 恢复基础姿态' in window.status.text()
assert node.engine.command[2]==0
until(lambda:node.engine.restoring_joint=='index_mcp_roll')
assert 'index_mcp_roll 恢复基础姿态' in window.status.text()
assert node.engine.command[0]==node.teaching.data['baseline']['target'][0]
assert not any(control.isEnabled() for control in window.joint_controls)
until(lambda:node.engine.state=='COMPLETED')
assert first.slider.value()==node.teaching.data['baseline']['target'][0]
assert index_control.slider.value()==node.teaching.data['baseline']['target'][2]
assert all(control.isEnabled() for control in window.joint_controls)
window.close();node.close_session();node.destroy_node();rclpy.shutdown()
print('GUI workflow passed')
'''
env = dict(os.environ, ROS_DOMAIN_ID='199', ROS_LOCALHOST_ONLY='1', QT_QPA_PLATFORM='offscreen')
env['PYTHONPATH'] = str(Path(__file__).resolve().parents[1])+os.pathsep+env.get('PYTHONPATH','')
result = subprocess.run([sys.executable,'-c',textwrap.dedent(program),str(tmp_path)],env=env,
capture_output=True,text=True,timeout=25)
assert result.returncode==0, result.stdout+result.stderr
@@ -0,0 +1,152 @@
"""Real ROS messages, isolated domain, fake driver; no vendor imports or CAN access."""
import json
import os
from pathlib import Path
import subprocess
import sys
import textwrap
def test_ros_adapter_in_isolated_process():
program = r'''
import json,time
import rclpy
from rclpy.node import Node
from rclpy.executors import SingleThreadedExecutor
from sensor_msgs.msg import JointState
from std_msgs.msg import String
from linkerhand_range_calibration.profiles import load_profile,Settings
from linkerhand_range_calibration.adapters.o30_ros import O30RosAdapter
from linkerhand_range_calibration.core.engine import Engine
from linkerhand_range_calibration.simulation import simulated_teaching
rclpy.init()
p=load_profile();sdk=Node('linkerhand_range_sdk');owner=Node('range_test_owner')
for key,value in {'hand_type':'right','hand_joint':'O30','auto_init_pose':False,
'joint_limit_min':[0]*20,'joint_limit_max':[255]*20,'cmd_timeout':0.0}.items():
sdk.declare_parameter(key,value)
positions=[20.0]*20; commands=[]; settings=[]
diagnostics={'joint_faults':{},'over_temp':[],'read_fail':7,'online':True,'comm_error':{'code':0}}
def command(msg):
commands.append(msg);positions[:]=msg.position
def setting(msg):settings.append(json.loads(msg.data))
sub=sdk.create_subscription(JointState,p.sdk['command_topic'],command,10)
setting_sub=sdk.create_subscription(String,p.sdk['setting_topic'],setting,10)
state_pub=sdk.create_publisher(JointState,p.sdk['feedback_topic'],10)
info_pub=sdk.create_publisher(String,p.sdk['info_topic'],10)
def publish():
m=JointState();m.header.stamp=sdk.get_clock().now().to_msg();m.name=[j.sdk_name for j in p.joints];m.position=list(positions)
state_pub.publish(m)
info_pub.publish(String(data=json.dumps({'uid':'FAKE_O30_TEST','model':'O30','side':'RIGHT','hand_type':'right',
'joint_names':m.name,**diagnostics})))
timer=sdk.create_timer(.03,publish)
a=O30RosAdapter(owner,p,Settings());ex=SingleThreadedExecutor();ex.add_node(sdk);ex.add_node(owner)
def until(condition,timeout=8):
end=time.monotonic()+timeout
while time.monotonic()<end:
ex.spin_once(timeout_sec=.03)
if condition():return
raise AssertionError('timeout: '+str(a.control_error(time.monotonic())))
until(lambda:a.control_error(time.monotonic()) is None)
assert not commands and not settings, 'preview must not send commands/settings'
a.set_motion(200,200)
target=[20]*20
for i in (2,3,4,5):target[i]=60
a.send_positions(target)
until(lambda:len(commands)==1 and len(settings)==2)
assert list(commands[0].position)==target
assert len(commands[0].position)==20 and not commands[0].velocity and not commands[0].effort
assert {s['setting_cmd'] for s in settings}=={'set_speed','set_max_torque_limits'}
assert all(s['params']['hand_type']=='right' for s in settings)
assert all(len(s['params'].get('speed',s['params'].get('torque')))==20 for s in settings)
# Both SDK stall flags permit starting and advancing an automatic scan.
diagnostics['joint_faults']={'thumb_roll':['灵巧手层判定堵转','执行器层判定堵转']}
until(lambda:'执行器层判定堵转' in (a.diagnostic_warning() or ''))
assert '灵巧手层判定堵转' in a.diagnostic_warning()
assert a.control_error(time.monotonic()) is None
events=[]
engine=Engine(p,Settings(),a,emit=events.append)
engine.start(simulated_teaching(p,a.uid),time.monotonic(),owner.get_clock().now().nanoseconds,
['thumb_cmc_roll'])
engine.tick(time.monotonic(),owner.get_clock().now().nanoseconds)
assert engine.state=='PREPARING'
until(lambda:len(commands)>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