新增 O30 右手行程标定功能包并完善相机恢复
支持三机位 Tag 两端边界标定、滑块示教、顺序避让恢复、同步四指扫描和 JSON 离线复算;修复相机时钟异常导致采集线程退出的问题,并忽略本地行程标定输出。
This commit is contained in:
@@ -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.
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -0,0 +1,230 @@
|
||||
# LinkerHand 有效指令行程标定
|
||||
|
||||
本包通过指尖 AprilTag 的图像运动,测量机械手实际有效的控制指令范围。首个型号为 O30 右手。
|
||||
输出的是 `0~255` 中的有效指令上下界,单位为 `u8`。没有角度反解、URDF 修改或三维融合。
|
||||
|
||||
**O30 SDK 源码、launch 和配置均保持原样。** 新包启动既有驱动可执行程序,并使用其原有话题与参数。
|
||||
SDK 启动时关闭自动摆位,打开界面本身不发送位置、速度或力矩命令。
|
||||
|
||||
## 1. 构建
|
||||
|
||||
环境:ROS 2 Jazzy、Python、PyQt5、NumPy、PyYAML、OpenCV、cv_bridge、apriltag_ros、image_proc。
|
||||
海康相机复用工作区 `linkerhand_calibration` 中的 `hikrobot_camera_node`,需要既有 MVS 环境和有效内参。
|
||||
O30 金属 CANFD 盒使用 SDK 已安装的 `libcanbus` 运行库。
|
||||
|
||||
明确选择两个源包,避免引入外部 SDK 仓库中与工作区重名的 GUI 包。构建目录位于本工作区:
|
||||
|
||||
```bash
|
||||
cd /home/lxp/projects/linkerhand_retarget_ros2
|
||||
source /opt/ros/jazzy/setup.bash
|
||||
source install/setup.bash
|
||||
colcon --log-base log/range_calibration build \
|
||||
--base-paths src/linkerhand_range_calibration \
|
||||
/home/lxp/projects/linkerhand-o30-ros2/linker_hand_o30_ros2_sdk \
|
||||
--packages-select linker_hand_o30_ros2_sdk linkerhand_range_calibration \
|
||||
--build-base build/range_calibration \
|
||||
--install-base install/range_calibration --symlink-install
|
||||
source install/range_calibration/setup.bash
|
||||
```
|
||||
|
||||
上述流程依赖已构建的相机驱动包;不会启动硬件。如果该包未安装,需要先构建工作区现有的 `linkerhand_calibration`。
|
||||
|
||||
## 2. 无硬件检查
|
||||
|
||||
```bash
|
||||
# 仅加载和检查配置,不初始化 ROS 或硬件。
|
||||
ros2 run linkerhand_range_calibration calibrate_range --profile o30_right --validate-only
|
||||
|
||||
# 假SDK + 虚拟时间,自动扫描全部17个任务;结果写在 SIMULATED_HAND 目录。
|
||||
ros2 run linkerhand_range_calibration calibrate_range --profile o30_right --simulate \
|
||||
--output /tmp/linkerhand_range_simulation
|
||||
|
||||
# 简易界面演示,只有假SDK和合成图像;不启动原 SDK 或相机。
|
||||
ros2 launch linkerhand_range_calibration calibrate.launch.py profile:=o30_right demo:=true
|
||||
```
|
||||
|
||||
模拟使用更快的虚拟调度,边界算法与实际标定相同。演示模式里的深色方块是合成观测,不能作为真实视觉验收。
|
||||
|
||||
## 3. 实机启动与示教
|
||||
|
||||
工位配置为 `config/station.yaml`:
|
||||
|
||||
| 机位 | 相机序列号 | 默认内参 |
|
||||
|---|---|---|
|
||||
| 正面 | DB2163742 | `~/.ros/camera_info/hikrobot_DB2163742.yaml` |
|
||||
| 侧面 | DB2163749 | `~/.ros/camera_info/hikrobot_DB2163749.yaml` |
|
||||
| 顶部 | DB2163739 | `~/.ros/camera_info/hikrobot_DB2163739.yaml` |
|
||||
|
||||
使用 `36h11` Tag。内参对应当前1624×1240图像和镜头配置,不使用相机外参文件。
|
||||
默认设备速度和力矩均为200。手动示教以30 Hz调度发送最新滑块目标,由设备速度设置控制实际运动;
|
||||
自动标定的准备姿态、避让和扫描轨迹限制为20指令单位/秒(`scan.rate`)。改变标定时的运动设置后应重新标定。
|
||||
|
||||
```bash
|
||||
ros2 launch linkerhand_range_calibration calibrate.launch.py profile:=o30_right
|
||||
|
||||
# 使用自己的工位配置
|
||||
ros2 launch linkerhand_range_calibration calibrate.launch.py \
|
||||
profile:=o30_right station:=/absolute/path/to/station.yaml
|
||||
```
|
||||
|
||||
启动文件会启动一份 O30 SDK、三路相机、检测器和界面。请关闭其他 SDK 实例和控制器;发现其他位置发布者会阻止运动。
|
||||
|
||||
首次操作:
|
||||
|
||||
1. 等待正确设备 UID、SDK 就绪和三路清晰图像。
|
||||
2. 直接拖动对应关节的滑块,机械手跟随最新目标;无需逐项输入或点击移动按钮。右侧数值框可精调,输入后按回车或移出焦点确认。
|
||||
3. 等待反馈稳定,将当前姿态保存为“全手基础姿态”。
|
||||
4. 必要时为单独任务保存“当前任务准备姿态覆盖”。避让值优先于覆盖值,防止覆盖已设置的避让。四指侧摆固定使用基础姿态,禁止覆盖。
|
||||
5. 先试标定一个任务,检查可见性和运动;之后执行全手标定。
|
||||
|
||||
O30 右手所有避让均使用固定指令,无需手动示教:`thumb_cmc_roll` 任务的食指侧摆固定为0,`thumb_mcp` 任务的拇指 `thumb_cmc_yaw` 固定为80,小指、无名指、中指的弯曲避让固定为255。
|
||||
示教下拉框只保留全手基础姿态和可选的任务准备姿态覆盖。
|
||||
|
||||
关节、SDK 下标、目标滑块、实际反馈、min、max 和状态合并在同一张表中,共用一个滚动条,同一关节始终在同一行。
|
||||
滑块与数值框同步显示目标,右侧紧邻实际反馈和标定结果;较长的状态原因可悬停查看完整内容。连续拖动时采用每个关节的最新目标,其他关节保持当前目标。
|
||||
手动示教采用与原O30 GUI相同的直接目标控制方式,不经过扫描的慢速插值;快速拖动或反向时不会排队执行旧目标。
|
||||
控制连接、身份和配置未变化时,手动示教不重复发送速度和力矩设置。目标发送完成后,仍需采集等待期之后的新反馈,确认稳定才能保存。
|
||||
打开界面和点击“同步当前目标”只更新显示,不下发运动。示教运动期间可继续拖动;自动标定及暂停中的任务须先取消,再进行手动调节。
|
||||
手动控制与自动标定使用同一套控制连接检查,位置命令沿用原 O30 GUI 使用的 SDK 话题。
|
||||
SDK 的运行时诊断(堵转、过温、过流、关节故障、心跳及通信诊断标志)不在标定层额外锁定滑块或暂停扫描;界面不显示诊断提示横幅,扫描日志保留诊断记录。
|
||||
SDK 自身的初始化检查和设备保护保持原样。标定仍要求 SDK 订阅存在、反馈有效且及时、设备身份和参数匹配、没有其他位置发布者;未连接或反馈断流时停止推进。
|
||||
暂停、取消会丢弃尚未执行的滑块调整,保持最后已发送目标;取消后再次拖动才会重新运动。
|
||||
示教保存已下发目标与稳定反馈,不会把尚未执行的滑块目标作为保存姿态。
|
||||
示教配置保存在 `~/.ros/linkerhand_range_calibration/<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_roll(SDK: 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('速度和力矩必须为0~255整数')
|
||||
self.setting_pub.publish(String(data=json.dumps({'setting_cmd': command, 'params': {
|
||||
'hand_type': self.profile.side, field: [value]*len(self.profile.joints)}})))
|
||||
@@ -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} 必须为0~255整数')
|
||||
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
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user