三机位标定
This commit is contained in:
+3
-1
@@ -50,6 +50,7 @@ Thumbs.db
|
||||
|
||||
# Runtime and calibration scratch files
|
||||
/logs/
|
||||
/MvSdkLog/
|
||||
*.tmp
|
||||
*.log
|
||||
*.bak
|
||||
@@ -66,8 +67,9 @@ Thumbs.db
|
||||
*.verification.json
|
||||
*_mapping_quality.json
|
||||
|
||||
# Device-specific robot descriptions derived from local CMC pitch calibration
|
||||
# Device-specific robot descriptions derived from local calibration runs
|
||||
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_cmc_pitch_*.urdf
|
||||
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_zero_calibrated_*.urdf
|
||||
|
||||
# ROS bag / MCAP recordings and CAN captures
|
||||
rosbag2_*/
|
||||
|
||||
@@ -1,6 +1,175 @@
|
||||
# G20 左手拇指正面 AprilTag 标定
|
||||
# G20 左手 AprilTag 标定
|
||||
|
||||
该包启动 RealSense、彩色图像校正、`apriltag_ros`、Linker Hand SDK 和标定状态机，
|
||||
## 三机位全手一键标定
|
||||
|
||||
正式全手入口同时使用三台海康 `MV-CS020-10U/10UM` 黑白全局快门相机，
|
||||
但只有 `/g20_calibration` 一个节点拥有机械手命令发布权。默认机位绑定为：
|
||||
|
||||
```text
|
||||
front = DB2163742，Tag 0/1/2/3/10
|
||||
side = DB2163749，Tag 4/5/6/7
|
||||
top = DB2163739，Tag 8/9
|
||||
```
|
||||
|
||||
11 张 `tag36h11` 的程序角色必须与贴纸所在刚性件一致：
|
||||
|
||||
| ID | 机位 | 固定位置/运动件 |
|
||||
|---:|---|---|
|
||||
| 0 | 正面 | 正面掌壳固定基准 |
|
||||
| 1 | 正面 | 拇指 CMC 后连杆 |
|
||||
| 2 | 正面 | 拇指 MCP 后连杆 |
|
||||
| 3 | 正面 | 拇指 IP 后末节 |
|
||||
| 4 | 侧面 | 掌壳侧面固定基准（最底下） |
|
||||
| 5 | 侧面 | 食指 MCP 后连杆 |
|
||||
| 6 | 侧面 | 食指 PIP 后连杆 |
|
||||
| 7 | 侧面 | 食指 DIP 后末节 |
|
||||
| 8 | 上面 | 上面相机可见的掌壳/底座固定基准 |
|
||||
| 9 | 上面 | 拇指 CMC yaw 运动件 |
|
||||
| 10 | 正面 | 食指根部侧摆运动件（index_mcp_roll） |
|
||||
|
||||
ID 4 不贴在侧面相机看不到的掌心正面；ID 8 必须始终固定且可见，
|
||||
ID 9 必须在拇指横摆的完整行程中持续可见。
|
||||
贴纸不能跨关节、贴在软胶上或在扫描过程中翘起。
|
||||
|
||||
每台相机必须有独立的内参文件：
|
||||
|
||||
```text
|
||||
~/.ros/camera_info/hikrobot_DB2163742.yaml
|
||||
~/.ros/camera_info/hikrobot_DB2163749.yaml
|
||||
~/.ros/camera_info/hikrobot_DB2163739.yaml
|
||||
```
|
||||
|
||||
先使用禁止运动模式检查三个机位、内参和标签：
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration \
|
||||
three_camera_calibration.launch.py \
|
||||
serial_number:=G20_LEFT_001 \
|
||||
commands_enabled:=false
|
||||
```
|
||||
|
||||
分别查看三个相机画面：
|
||||
|
||||
```bash
|
||||
ros2 run image_view image_view --ros-args \
|
||||
--remap image:=/g20_calibration/front/camera/image_rect
|
||||
ros2 run image_view image_view --ros-args \
|
||||
--remap image:=/g20_calibration/side/camera/image_rect
|
||||
ros2 run image_view image_view --ros-args \
|
||||
--remap image:=/g20_calibration/top/camera/image_rect
|
||||
```
|
||||
|
||||
安装相机时可以按机位启动红/蓝线对准辅助节点。红线是画面理想水平线，
|
||||
蓝线是在画面下部检测到的桌边、底座边或临时刚性直尺；两线夹角不超过
|
||||
`±0.5°` 且上下构图偏差不超过 `±12 px` 时显示 `ALIGNED`。Tag只画绿色
|
||||
识别框，其角点方向完全不参与红/蓝线角度计算，因此Tag无需为了相机对准而贴正。
|
||||
|
||||
下面以正面机位为例，先启动辅助节点：
|
||||
|
||||
```bash
|
||||
ros2 run g20_thumb_apriltag_calibration camera_alignment_view \
|
||||
--ros-args -p view:=front
|
||||
```
|
||||
|
||||
再打开它发布的叠加画面：
|
||||
|
||||
```bash
|
||||
ros2 run image_view image_view --ros-args \
|
||||
--remap image:=/g20_camera_alignment_view/image
|
||||
```
|
||||
|
||||
侧面和上面分别把 `view:=front` 改为 `view:=side`、`view:=top`。建议一次只开
|
||||
一个机位完成调整；侧面或上面没有合适长边时，临时放置与目标机械轴平行的刚性
|
||||
直尺。调整完成后退出辅助节点和
|
||||
`image_view`，再进行正式标定，以免额外的200万像素图像订阅影响采集帧率。
|
||||
这组红/蓝线只检查图像平面滚转角，不检查相机距离、俯仰、偏航，也不会阻止
|
||||
`/g20_calibration/start`。
|
||||
|
||||
确认所有目标关节的 `0～255` 行程安全、MVS 客户端已关闭且没有其他命令发布者后，
|
||||
重新启动正式流程：
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration \
|
||||
three_camera_calibration.launch.py \
|
||||
serial_number:=G20_LEFT_001 \
|
||||
can_interface:=can0
|
||||
```
|
||||
|
||||
状态显示三个机位均“就绪”后只调用一次：
|
||||
|
||||
```bash
|
||||
ros2 topic echo /g20_calibration/status_text
|
||||
ros2 service call /g20_calibration/start std_srvs/srv/Trigger {}
|
||||
```
|
||||
|
||||
收到 `start` 后，程序先下发并确认以下20通道基准姿态，稳定保持0.5秒后才开始
|
||||
第一条轨迹扫描：
|
||||
|
||||
```text
|
||||
[255, 255, 255, 255, 255, 255, 127, 127, 127, 127,
|
||||
255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||
```
|
||||
|
||||
程序依次完成正面电机 `0/5/15/6`、侧面电机 `1/16`、上面电机 `10` 的三轮
|
||||
往返扫描。中指、无名指、小指复制食指模板。四指侧摆先以命令255
|
||||
为原始0角测出总行程，再减去总行程的一半；最终满足命令0为正、命令255为负，
|
||||
零位命令是实测曲线上最接近角度中点的整数命令。
|
||||
|
||||
每个直接测量任务完成 `3轮×2方向=6个扫描方向` 后，程序立即试拟合
|
||||
该电机对应的所有主动/被动关节。`thumb_cmc_pitch`、`thumb_cmc_roll`、
|
||||
`thumb_mcp`、`thumb_ip`、`index_mcp_roll`、`index_mcp_pitch` 和 `index_pip` 使用图像平面的
|
||||
连杆相对中心圆相位，避免小尺寸平面 Tag 的PnP深度双解把稳定的二维圆轨迹扭曲成
|
||||
错误三维轨迹。上面斜视的 `thumb_cmc_yaw` 和侧面的被动 `index_dip` 继续使用
|
||||
父Tag坐标系下的三维相对圆。`thumb_ip` 理论上也适合相对三维，但当前正面小Tag的
|
||||
PnP深度在三轮间不稳定，实测会让三维行程漂移，因此继续采用可重复的二维投影轨迹。
|
||||
程序分别检查二维圆残差/半径或三维平面RMS/圆残差/半径，并统一检查最小圆弧、
|
||||
单调修正量、正反程回差、三轮行程一致性和直接零位拟合。主动关节三轮行程最大差
|
||||
默认不超过3°；被动耦合关节允许不超过10°，但仍必须通过其余质量门限。
|
||||
任一指标失败时会立即暂停，中文状态显示关节名、实测值和阈值，不再等到42个方向
|
||||
全部结束。修正现场问题后调用 `resume`，
|
||||
程序只清除该电机任务的内存样本并重扫它的6个方向；前面已通过的关节保留。
|
||||
失败样本不从 `raw_samples.jsonl` 删除，而是使用 `attempt` 和 `retry` 记录区分，
|
||||
便于调试；最终拟合只使用当前通过尝试的内存数据。
|
||||
|
||||
标定食指 `index_mcp_roll`（电机6）及其随机复测时，为避免中指遮挡ID 10，
|
||||
程序将中指、无名指和小指的侧摆电机7/8/9固定为0；开始采样前会同时确认
|
||||
电机6到达扫描起点且电机7/8/9均已到达0。离开该标定项后恢复统一基准姿态。
|
||||
该项目还会通过SDK设置接口把五指速度临时设为 `[15,5,15,15,15]`，即只把
|
||||
食指速度从15降为5；离开该项目后恢复 `[15,15,15,15,15]`。
|
||||
侧面标定 `index_mcp_pitch`（电机1）和 `index_pip/index_dip`（电机16）时，
|
||||
五指速度设为 `[15,10,15,15,15]`，即食指屈伸使用第三档速度10；其余直接
|
||||
测量关节保持普通速度15。
|
||||
|
||||
标定 `thumb_cmc_yaw`（电机10）及其随机复测时，程序将
|
||||
`thumb_cmc_roll`（电机5）固定为145，并在它到位后才开始采样，以保持运动Tag
|
||||
ID 9的可见性和PnP稳定性。离开该标定项后，电机5恢复基准值255；
|
||||
最终JSON的 `baseline_command_u8` 不变。
|
||||
|
||||
三机位流程默认设置 `validation_enabled:=false`，因此拟合完成后会直接
|
||||
恢复基准姿态并生成JSON，不再进入 `VALIDATION_MOVE/VALIDATION_CAPTURE`。
|
||||
此时 `quality.passed` 只由轨迹与零位拟合质量决定，`validation_mae_rad` 和
|
||||
`validation_p95_rad` 为 `null`。需要恢复随机复测时，启动参数加
|
||||
`validation_enabled:=true`。
|
||||
|
||||
上面机位在Tag二维质量合格但PnP连续无效达到1秒时，会自动重置该机位的单Tag
|
||||
和双Tag连续性跟踪器，并从下一帧重新建链，早于3秒采集超时。暂停恢复时只要求
|
||||
当前活动机位就绪；首次调用 `start` 仍要求三个机位全部通过预检。
|
||||
|
||||
对外只生成一个精简运行时结果：
|
||||
|
||||
```text
|
||||
calibration_output/G20_LEFT_001/<时间戳>/
|
||||
g20_left_G20_LEFT_001_calibration.json
|
||||
```
|
||||
|
||||
文件包含21个关节的256项 `angle_rad`、16个主动关节的 `zero_command_u8`、
|
||||
5个被动标记、模板来源和总体质量。相机、Tag、正反程及每轮质量只进入状态、日志和
|
||||
`raw_samples.jsonl`，不写入最终运行时 JSON。
|
||||
|
||||
下面保留原有正面拇指独立标定说明和兼容入口。
|
||||
|
||||
该包启动海康机器人 MVS USB3 Vision 黑白相机、图像校正、`apriltag_ros`、
|
||||
Linker Hand SDK 和标定状态机，
|
||||
只扫描 G20 左手命令下标 `0`、`15`。默认使用单终点连续模式：每个方向只发送一次
|
||||
终点命令，速度保持在固件能稳定响应的 `15`。SDK 以独立时间戳反馈实际 20 维位置，
|
||||
程序把每帧 AprilTag 三维中心与同一时刻的实际电机位置插值配对并按整数位置分箱。
|
||||
@@ -50,11 +219,11 @@ PnP 双分支跟踪和整段刚体复核仍保留，用于选出稳定的三维
|
||||
```bash
|
||||
sudo apt-get update
|
||||
sudo apt-get install -y \
|
||||
ros-jazzy-realsense2-camera \
|
||||
ros-jazzy-realsense2-description \
|
||||
ros-jazzy-image-pipeline \
|
||||
ros-jazzy-apriltag-ros \
|
||||
ros-jazzy-apriltag-msgs
|
||||
ros-jazzy-apriltag-msgs \
|
||||
ros-jazzy-camera-calibration \
|
||||
python3-yaml
|
||||
|
||||
cd /home/lxp/projects/linkerhand_retarget_ros2
|
||||
source /opt/ros/jazzy/setup.bash
|
||||
@@ -63,6 +232,58 @@ colcon build --symlink-install \
|
||||
source install/setup.bash
|
||||
```
|
||||
|
||||
相机节点直接使用海康 MVS SDK。当前机器的默认安装位置是 `/opt/MVS`，需要存在：
|
||||
|
||||
```text
|
||||
/opt/MVS/lib/64/libMvCameraControl.so
|
||||
/opt/MVS/Samples/64/Python/MvImport/MvCameraControl_class.py
|
||||
```
|
||||
|
||||
正面相机默认按序列号 `DB2163742` 绑定（MVS 显示的 GUID 是
|
||||
`2BDFB2163742`），型号校验为 `MV-CS020-10UM`。三台相机同时连接时程序不会按枚举
|
||||
顺序猜测机位。启动 ROS 节点前必须关闭 MVS 客户端中的相机连接，否则设备可能被占用。
|
||||
|
||||
`1624x1240 mono8` 每帧约 2.0 MB，超过 Fast DDS 2.14 默认约 512 KB 的共享内存段。
|
||||
相机节点和三个标定 launch 会自动加载 `config/fastdds_large_images.xml`，使用 64 MB
|
||||
共享内存段；否则相机内部虽为 30 Hz，大图订阅端通常只能收到约 1～4 Hz。修改配置后
|
||||
必须重启相关 ROS 进程才能生效。
|
||||
|
||||
首次使用必须先标定该相机和当前镜头的内参。主 launch 默认从
|
||||
`~/.ros/camera_info/hikrobot_DB2163742.yaml` 加载标准 ROS CameraInfo YAML；文件缺失时
|
||||
仍可预览 `mono8` 原图，但发布的内参无效，轨迹标定预检不会解锁运动。
|
||||
|
||||
先单独启动相机（不会连接机械手，也不会发送关节命令）：
|
||||
|
||||
```bash
|
||||
ros2 run g20_thumb_apriltag_calibration hikrobot_camera_node --ros-args \
|
||||
--remap __ns:=/camera/camera/color \
|
||||
-p serial_number:=DB2163742 \
|
||||
-p camera_info_url:=$HOME/.ros/camera_info/hikrobot_DB2163742.yaml
|
||||
```
|
||||
|
||||
测速时优先检查同帧发布的小消息和原图；两者正常值都应接近 30 Hz：
|
||||
|
||||
```bash
|
||||
ros2 topic hz /camera/camera/color/camera_info
|
||||
ros2 topic hz /camera/camera/color/image_raw
|
||||
```
|
||||
|
||||
使用标定板采集内参。下面的 `8x6` 是内角点数量、`0.020` 是单格边长 20 mm，必须按
|
||||
实际标定板修改：
|
||||
|
||||
```bash
|
||||
ros2 run camera_calibration cameracalibrator \
|
||||
--size 8x6 --square 0.020 \
|
||||
--camera_name hikrobot_front_DB2163742 \
|
||||
--ros-args \
|
||||
--remap image:=/camera/camera/color/image_raw \
|
||||
--remap camera/set_camera_info:=/camera/camera/color/set_camera_info
|
||||
```
|
||||
|
||||
在标定界面完成采样后点击 `CALIBRATE`，确认重投影误差，再点击 `COMMIT`。相机节点会
|
||||
原子写入上述 YAML，并立即开始发布有效内参。内参只适用于标定时的镜头焦距、对焦、
|
||||
分辨率和 ROI；改变任何一项都要重新标定。
|
||||
|
||||
连接 CAN 后先确认 `can0` 已启动。不要同时运行其他会发布
|
||||
`/g20/cb_left_hand_control_cmd` 的程序。
|
||||
|
||||
@@ -74,6 +295,7 @@ source install/setup.bash
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
|
||||
serial_number:=G20_LEFT_001 \
|
||||
camera_serial_number:=DB2163742 \
|
||||
commands_enabled:=false
|
||||
```
|
||||
|
||||
@@ -84,6 +306,7 @@ ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
|
||||
serial_number:=G20_LEFT_001 \
|
||||
camera_serial_number:=DB2163742 \
|
||||
can_interface:=can0 \
|
||||
calibration_speed:=15 \
|
||||
continuous_motion_mode:=endpoint \
|
||||
@@ -92,7 +315,7 @@ ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
|
||||
use_roi:=false
|
||||
```
|
||||
|
||||
默认关闭 ROI，AprilTag 使用完整的 1280×720 校正画面。查看实际送入 AprilTag
|
||||
默认关闭 ROI，AprilTag 使用完整的 1624×1240 校正画面。查看实际送入 AprilTag
|
||||
的完整画面：
|
||||
|
||||
```bash
|
||||
@@ -241,16 +464,16 @@ for name, joint in data["joints"].items():
|
||||
```
|
||||
|
||||
如果相机或 SDK 已由外部进程启动，可传
|
||||
`start_camera:=false` 或 `start_sdk:=false`。用 `camera_serial_number:=<序列号>`
|
||||
可绑定指定 RealSense。
|
||||
`start_camera:=false` 或 `start_sdk:=false`。`camera_serial_number` 同时接受 MVS
|
||||
序列号和 GUID，但推荐使用稳定且简短的序列号 `DB2163742`。
|
||||
|
||||
D405 的彩色流来自 `depth_module`，启动文件会同时设置
|
||||
`depth_module.color_profile` 和 `rgb_camera.color_profile`，默认均为
|
||||
`1280x720x30`。连续模式默认关闭深度和 rosbag，以减少 USB、CPU 和磁盘负担；
|
||||
它们都不参与角度计算。需要完整诊断留档时可增加
|
||||
`enable_depth:=true record_bag:=true`。
|
||||
海康相机默认输出 `1624x1240@30Hz mono8`，全局快门，曝光时间 `5000us`、增益
|
||||
`0dB`，并使用“只取最新帧”策略避免视觉延迟。现场亮度不足时优先增加照明；必要时可用
|
||||
`exposure_time_us`、`gain_db` 调整，或临时传 `auto_exposure:=true`。正式轨迹采集建议固定
|
||||
曝光，避免自动曝光在运动过程中改变角点质量。rosbag 默认关闭；需要诊断留档时增加
|
||||
`record_bag:=true`。
|
||||
|
||||
默认对完整 1280×720 原图进行畸变校正和 AprilTag 检测。三维中心 PnP 必须使用
|
||||
默认对完整 1624×1240 原图进行畸变校正和 AprilTag 检测。三维中心 PnP 必须使用
|
||||
`image_rect` 的角点及同一条处理链对应的 `CameraInfo`，启动文件已自动保证二者配对。
|
||||
校正和 AprilTag 组件运行
|
||||
在同一个多线程容器内并启用进程内传输，避免在处理链路中重复序列化、复制大图像。
|
||||
|
||||
@@ -0,0 +1,32 @@
|
||||
<?xml version="1.0" encoding="UTF-8" ?>
|
||||
<dds>
|
||||
<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
|
||||
<transport_descriptors>
|
||||
<transport_descriptor>
|
||||
<transport_id>g20_udp_transport</transport_id>
|
||||
<type>UDPv4</type>
|
||||
<sendBufferSize>10485760</sendBufferSize>
|
||||
<receiveBufferSize>10485760</receiveBufferSize>
|
||||
</transport_descriptor>
|
||||
<transport_descriptor>
|
||||
<transport_id>g20_shm_transport</transport_id>
|
||||
<type>SHM</type>
|
||||
<segment_size>67108864</segment_size>
|
||||
<port_queue_capacity>512</port_queue_capacity>
|
||||
<healthy_check_timeout_ms>1000</healthy_check_timeout_ms>
|
||||
</transport_descriptor>
|
||||
</transport_descriptors>
|
||||
|
||||
<participant
|
||||
profile_name="g20_large_image_participant"
|
||||
is_default_profile="true">
|
||||
<rtps>
|
||||
<userTransports>
|
||||
<transport_id>g20_udp_transport</transport_id>
|
||||
<transport_id>g20_shm_transport</transport_id>
|
||||
</userTransports>
|
||||
<useBuiltinTransports>false</useBuiltinTransports>
|
||||
</rtps>
|
||||
</participant>
|
||||
</profiles>
|
||||
</dds>
|
||||
@@ -1,5 +0,0 @@
|
||||
# This is a flat parameter mapping consumed by realsense2_camera/rs_launch.py.
|
||||
# Keep image and CameraInfo durability identical so image_transport can
|
||||
# synchronize them for image_proc and apriltag_ros.
|
||||
color_qos: DEFAULT
|
||||
color_info_qos: DEFAULT
|
||||
@@ -0,0 +1,76 @@
|
||||
g20_calibration:
|
||||
ros__parameters:
|
||||
command_topic: /g20/cb_left_hand_control_cmd
|
||||
state_topic: /g20/cb_left_hand_state
|
||||
info_topic: /g20/cb_left_hand_info
|
||||
setting_topic: /g20/cb_hand_setting_cmd
|
||||
front_camera_info_topic: /g20_calibration/front/camera/camera_info
|
||||
front_detections_topic: /g20_calibration/front/apriltag/detections
|
||||
side_camera_info_topic: /g20_calibration/side/camera/camera_info
|
||||
side_detections_topic: /g20_calibration/side/apriltag/detections
|
||||
top_camera_info_topic: /g20_calibration/top/camera/camera_info
|
||||
top_detections_topic: /g20_calibration/top/apriltag/detections
|
||||
|
||||
# /start先下发并确认这个20通道基准姿态，稳定后才进入第一条扫描。
|
||||
baseline_command_u8: [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||
normal_calibration_speed: 15
|
||||
index_roll_calibration_speed: 5
|
||||
index_flex_calibration_speed: 10
|
||||
speed_setting_settle_seconds: 0.25
|
||||
|
||||
tag_size_m: 0.010
|
||||
repetitions: 3
|
||||
preflight_frames: 60
|
||||
minimum_detection_rate: 0.95
|
||||
minimum_detection_hz: 15.0
|
||||
maximum_hamming: 0
|
||||
minimum_decision_margin: 30.0
|
||||
minimum_edge_pixels: 30.0
|
||||
|
||||
pnp_maximum_reprojection_error_px: 1.5
|
||||
pnp_reprojection_tie_px: 1.5
|
||||
pnp_maximum_pose_jump_deg: 35.0
|
||||
pnp_maximum_translation_jump_m: 0.04
|
||||
pnp_maximum_tag_tilt_deg: 75.0
|
||||
pnp_tracker_reset_seconds: 5.0
|
||||
top_pnp_invalid_reset_seconds: 1.0
|
||||
maximum_state_image_skew_ms: 150.0
|
||||
|
||||
endpoint_tolerance_u8: 2.0
|
||||
endpoint_hold_seconds: 0.5
|
||||
baseline_hold_seconds: 0.5
|
||||
position_timeout_seconds: 30.0
|
||||
sweep_timeout_seconds: 90.0
|
||||
invalid_timeout_seconds: 3.0
|
||||
minimum_sweep_frames: 40
|
||||
minimum_state_span_u8: 240.0
|
||||
minimum_sweep_bins: 32
|
||||
maximum_bin_gap: 16
|
||||
|
||||
trajectory_maximum_plane_rms_m: 0.004
|
||||
trajectory_maximum_radial_rms_m: 0.004
|
||||
trajectory_minimum_radius_m: 0.003
|
||||
trajectory_minimum_arc_deg: 15.0
|
||||
# 正面拇指pitch/MCP/IP使用二维圆相位，避免平面Tag的PnP深度歧义。
|
||||
image_trajectory_maximum_radial_rms_px: 2.0
|
||||
image_trajectory_maximum_radial_p95_px: 3.5
|
||||
image_trajectory_minimum_radius_px: 20.0
|
||||
trajectory_maximum_cycle_travel_difference_deg: 3.0
|
||||
passive_maximum_cycle_travel_difference_deg: 10.0
|
||||
zero_minimum_radius_px: 20.0
|
||||
zero_maximum_radial_rms_px: 2.0
|
||||
zero_maximum_radial_p95_px: 3.5
|
||||
zero_maximum_round_difference_deg: 1.0
|
||||
maximum_monotonic_correction_deg: 2.0
|
||||
maximum_hysteresis_deg: 5.0
|
||||
passive_maximum_monotonic_correction_deg: 3.0
|
||||
passive_maximum_hysteresis_deg: 7.5
|
||||
|
||||
# 默认跳过耗时的随机复测；需要验收精度时可在launch中设为true。
|
||||
validation_enabled: false
|
||||
validation_command_count: 3
|
||||
validation_frames: 10
|
||||
validation_seed: 20260804
|
||||
validation_timeout_seconds: 20.0
|
||||
maximum_validation_mae_deg: 2.0
|
||||
maximum_validation_p95_deg: 3.0
|
||||
@@ -0,0 +1,62 @@
|
||||
/g20_calibration/front/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.010
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.5
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [0, 1, 2, 3, 10]
|
||||
frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, index_roll]
|
||||
sizes: [0.010, 0.010, 0.010, 0.010, 0.010]
|
||||
|
||||
/g20_calibration/side/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.010
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.5
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [4, 5, 6, 7]
|
||||
frames: [side_base, index_mcp, index_pip, index_dip]
|
||||
sizes: [0.010, 0.010, 0.010, 0.010]
|
||||
|
||||
/g20_calibration/top/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.010
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
decimate: 1.5
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [8, 9]
|
||||
frames: [top_base, thumb_yaw]
|
||||
sizes: [0.010, 0.010]
|
||||
@@ -51,6 +51,39 @@ def tag_quality_is_valid(
|
||||
)
|
||||
|
||||
|
||||
def update_pnp_reset_watchdog(
|
||||
*,
|
||||
detection_good: bool,
|
||||
pnp_valid: bool,
|
||||
now: float,
|
||||
invalid_since: float | None,
|
||||
reset_after_seconds: float,
|
||||
) -> tuple[float | None, bool]:
|
||||
"""Track continuous PnP-only failures and request a throttled reset."""
|
||||
reset_after = float(reset_after_seconds)
|
||||
if reset_after <= 0.0:
|
||||
raise ValueError("reset_after_seconds must be positive")
|
||||
if not detection_good or pnp_valid:
|
||||
return None, False
|
||||
since = float(now) if invalid_since is None else float(invalid_since)
|
||||
if float(now) - since >= reset_after:
|
||||
# Start a new interval so a permanently bad view is not reset on every
|
||||
# frame. The next valid frame clears the interval.
|
||||
return float(now), True
|
||||
return since, False
|
||||
|
||||
|
||||
def required_resume_views(active_view: str | None) -> tuple[str, ...]:
|
||||
"""Require only the active view on resume; start still checks all views."""
|
||||
all_views = ("front", "side", "top")
|
||||
if active_view is None:
|
||||
return all_views
|
||||
view = str(active_view)
|
||||
if view not in all_views:
|
||||
raise ValueError(f"unknown calibration view: {view}")
|
||||
return (view,)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Observation:
|
||||
stamp_ns: int
|
||||
|
||||
@@ -0,0 +1,358 @@
|
||||
"""Visual roll-alignment aid for one G20 calibration camera."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from collections import deque
|
||||
import math
|
||||
import time
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
from apriltag_msgs.msg import AprilTagDetectionArray
|
||||
import cv2
|
||||
from cv_bridge import CvBridge
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import Image
|
||||
|
||||
from .full_hand import VIEW_TAGS
|
||||
from .hikrobot_camera import configure_fastdds_large_image_transport
|
||||
from .zero_calibration import detect_reference_alignment_line
|
||||
|
||||
|
||||
VIEWS = ("front", "side", "top")
|
||||
|
||||
|
||||
def summarize_alignment_measurements(
|
||||
measurements: Sequence[Mapping[str, Any] | None],
|
||||
) -> dict[str, Any] | None:
|
||||
"""Return a median-smoothed physical reference-line measurement."""
|
||||
valid = [measurement for measurement in measurements if measurement]
|
||||
if not valid:
|
||||
return None
|
||||
return {
|
||||
"line_xyxy_px": np.median(
|
||||
np.asarray(
|
||||
[measurement["line_xyxy_px"] for measurement in valid],
|
||||
dtype=float,
|
||||
),
|
||||
axis=0,
|
||||
).tolist(),
|
||||
"angle_rad": float(
|
||||
np.median(
|
||||
[float(measurement["angle_rad"]) for measurement in valid]
|
||||
)
|
||||
),
|
||||
"vertical_offset_px": float(
|
||||
np.median(
|
||||
[
|
||||
float(measurement["vertical_offset_px"])
|
||||
for measurement in valid
|
||||
]
|
||||
)
|
||||
),
|
||||
"detected_frames": len(valid),
|
||||
"window_frames": len(measurements),
|
||||
}
|
||||
|
||||
|
||||
class G20CameraAlignmentView(Node):
|
||||
"""Publish a red/blue roll aid based on a physical scene edge."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
"""Configure one view without taking ownership of hand commands."""
|
||||
super().__init__("g20_camera_alignment_view")
|
||||
self.declare_parameter("view", "front")
|
||||
view = str(self.get_parameter("view").value).strip().lower()
|
||||
if view not in VIEWS:
|
||||
raise ValueError(f"view must be one of {VIEWS}")
|
||||
self.view = view
|
||||
|
||||
namespace = f"/g20_calibration/{view}"
|
||||
self.required_tag_ids = {
|
||||
int(value) for value in VIEW_TAGS[view].values()
|
||||
}
|
||||
self.declare_parameter("image_topic", f"{namespace}/camera/image_rect")
|
||||
self.declare_parameter(
|
||||
"detections_topic", f"{namespace}/apriltag/detections"
|
||||
)
|
||||
self.declare_parameter("reference_y_ratio", 0.90)
|
||||
self.declare_parameter("roi_y_min_ratio", 0.55)
|
||||
self.declare_parameter("roi_y_max_ratio", 0.98)
|
||||
self.declare_parameter("minimum_line_length_ratio", 0.30)
|
||||
self.declare_parameter("maximum_candidate_angle_deg", 15.0)
|
||||
self.declare_parameter("maximum_alignment_error_deg", 0.5)
|
||||
self.declare_parameter("maximum_vertical_offset_px", 12.0)
|
||||
self.declare_parameter("maximum_hamming", 0)
|
||||
self.declare_parameter("minimum_decision_margin", 20.0)
|
||||
self.declare_parameter("minimum_edge_pixels", 20.0)
|
||||
self.declare_parameter("smoothing_frames", 10)
|
||||
self.declare_parameter("maximum_line_age_seconds", 1.0)
|
||||
self.declare_parameter("maximum_tag_age_seconds", 1.0)
|
||||
self.declare_parameter("maximum_publish_rate_hz", 10.0)
|
||||
self.declare_parameter("output_scale", 0.75)
|
||||
|
||||
def value(name: str) -> Any:
|
||||
return self.get_parameter(name).value
|
||||
|
||||
self.image_topic = str(value("image_topic"))
|
||||
self.detections_topic = str(value("detections_topic"))
|
||||
self.reference_y_ratio = float(value("reference_y_ratio"))
|
||||
self.roi_y_min_ratio = float(value("roi_y_min_ratio"))
|
||||
self.roi_y_max_ratio = float(value("roi_y_max_ratio"))
|
||||
self.minimum_line_length_ratio = float(
|
||||
value("minimum_line_length_ratio")
|
||||
)
|
||||
self.maximum_candidate_angle_rad = math.radians(
|
||||
float(value("maximum_candidate_angle_deg"))
|
||||
)
|
||||
self.maximum_alignment_error_rad = math.radians(
|
||||
float(value("maximum_alignment_error_deg"))
|
||||
)
|
||||
self.maximum_vertical_offset_px = float(
|
||||
value("maximum_vertical_offset_px")
|
||||
)
|
||||
self.maximum_hamming = int(value("maximum_hamming"))
|
||||
self.minimum_decision_margin = float(value("minimum_decision_margin"))
|
||||
self.minimum_edge_pixels = float(value("minimum_edge_pixels"))
|
||||
self.maximum_line_age_seconds = float(
|
||||
value("maximum_line_age_seconds")
|
||||
)
|
||||
self.maximum_tag_age_seconds = float(value("maximum_tag_age_seconds"))
|
||||
self.maximum_publish_rate_hz = float(value("maximum_publish_rate_hz"))
|
||||
self.output_scale = float(value("output_scale"))
|
||||
smoothing_frames = int(value("smoothing_frames"))
|
||||
|
||||
if not (
|
||||
0.0
|
||||
<= self.roi_y_min_ratio
|
||||
< self.reference_y_ratio
|
||||
< self.roi_y_max_ratio
|
||||
<= 1.0
|
||||
):
|
||||
raise ValueError(
|
||||
"ratios must satisfy 0 <= roi_min < reference < roi_max <= 1"
|
||||
)
|
||||
if not 0.0 < self.minimum_line_length_ratio <= 1.0:
|
||||
raise ValueError("minimum_line_length_ratio must be in (0, 1]")
|
||||
if not (
|
||||
0.0
|
||||
< self.maximum_alignment_error_rad
|
||||
< self.maximum_candidate_angle_rad
|
||||
< math.pi / 2.0
|
||||
):
|
||||
raise ValueError(
|
||||
"angle limits must satisfy 0 < alignment < candidate < 90"
|
||||
)
|
||||
if self.maximum_vertical_offset_px <= 0.0:
|
||||
raise ValueError("maximum_vertical_offset_px must be positive")
|
||||
if smoothing_frames < 1:
|
||||
raise ValueError("smoothing_frames must be positive")
|
||||
if self.maximum_line_age_seconds <= 0.0:
|
||||
raise ValueError("maximum_line_age_seconds must be positive")
|
||||
if self.maximum_tag_age_seconds <= 0.0:
|
||||
raise ValueError("maximum_tag_age_seconds must be positive")
|
||||
if self.maximum_publish_rate_hz <= 0.0:
|
||||
raise ValueError("maximum_publish_rate_hz must be positive")
|
||||
if not 0.1 <= self.output_scale <= 1.0:
|
||||
raise ValueError("output_scale must be in [0.1, 1.0]")
|
||||
|
||||
self.bridge = CvBridge()
|
||||
self.line_history: deque[dict[str, Any] | None] = deque(
|
||||
maxlen=smoothing_frames
|
||||
)
|
||||
self.last_line_at = 0.0
|
||||
self.latest_tag_corners: dict[int, np.ndarray] = {}
|
||||
self.latest_tag_at: dict[int, float] = {}
|
||||
self.last_publish_at = 0.0
|
||||
self.publisher = self.create_publisher(
|
||||
Image, "~/image", qos_profile_sensor_data
|
||||
)
|
||||
self.create_subscription(
|
||||
AprilTagDetectionArray,
|
||||
self.detections_topic,
|
||||
self._detections_callback,
|
||||
qos_profile_sensor_data,
|
||||
)
|
||||
self.create_subscription(
|
||||
Image,
|
||||
self.image_topic,
|
||||
self._image_callback,
|
||||
qos_profile_sensor_data,
|
||||
)
|
||||
self.get_logger().info(
|
||||
f"{view} alignment view uses physical long-edge detection; "
|
||||
f"Tag orientation is ignored; input={self.image_topic}; "
|
||||
f"output={self.get_name()}/image"
|
||||
)
|
||||
|
||||
def _detections_callback(self, message: AprilTagDetectionArray) -> None:
|
||||
now = time.monotonic()
|
||||
for detection in message.detections:
|
||||
tag_id = int(detection.id)
|
||||
if tag_id not in self.required_tag_ids:
|
||||
continue
|
||||
corners = np.asarray(
|
||||
[
|
||||
[float(point.x), float(point.y)]
|
||||
for point in detection.corners
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
if corners.shape != (4, 2) or not np.all(np.isfinite(corners)):
|
||||
continue
|
||||
edges = np.linalg.norm(
|
||||
corners - np.roll(corners, -1, axis=0), axis=1
|
||||
)
|
||||
if (
|
||||
int(detection.hamming) > self.maximum_hamming
|
||||
or float(detection.decision_margin)
|
||||
< self.minimum_decision_margin
|
||||
or float(np.mean(edges)) < self.minimum_edge_pixels
|
||||
):
|
||||
continue
|
||||
self.latest_tag_corners[tag_id] = corners
|
||||
self.latest_tag_at[tag_id] = now
|
||||
|
||||
def _draw_tags(self, image: np.ndarray, now: float) -> None:
|
||||
for tag_id in sorted(self.required_tag_ids):
|
||||
corners = self.latest_tag_corners.get(tag_id)
|
||||
detected_at = self.latest_tag_at.get(tag_id, 0.0)
|
||||
if (
|
||||
corners is None
|
||||
or now - detected_at > self.maximum_tag_age_seconds
|
||||
):
|
||||
continue
|
||||
points = np.rint(corners * self.output_scale).astype(np.int32)
|
||||
cv2.polylines(image, [points], True, (0, 220, 0), 2)
|
||||
centre = np.rint(np.mean(points, axis=0)).astype(int)
|
||||
cv2.putText(
|
||||
image,
|
||||
f"ID {tag_id}",
|
||||
(int(centre[0]) + 5, int(centre[1]) - 7),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.55,
|
||||
(0, 220, 0),
|
||||
2,
|
||||
)
|
||||
|
||||
def _image_callback(self, message: Image) -> None:
|
||||
# Avoid conversion and Hough work until an image viewer subscribes.
|
||||
if self.publisher.get_subscription_count() < 1:
|
||||
return
|
||||
now = time.monotonic()
|
||||
if now - self.last_publish_at < 1.0 / self.maximum_publish_rate_hz:
|
||||
return
|
||||
self.last_publish_at = now
|
||||
try:
|
||||
image = self.bridge.imgmsg_to_cv2(message, desired_encoding="bgr8")
|
||||
except Exception as error:
|
||||
self.get_logger().warning(
|
||||
f"alignment image conversion failed: {error}"
|
||||
)
|
||||
return
|
||||
if self.output_scale != 1.0:
|
||||
image = cv2.resize(
|
||||
image,
|
||||
None,
|
||||
fx=self.output_scale,
|
||||
fy=self.output_scale,
|
||||
interpolation=cv2.INTER_AREA,
|
||||
)
|
||||
|
||||
height, width = image.shape[:2]
|
||||
reference_y = self.reference_y_ratio * float(height - 1)
|
||||
detected = detect_reference_alignment_line(
|
||||
image,
|
||||
reference_y_px=reference_y,
|
||||
roi_y_min_ratio=self.roi_y_min_ratio,
|
||||
roi_y_max_ratio=self.roi_y_max_ratio,
|
||||
minimum_length_ratio=self.minimum_line_length_ratio,
|
||||
maximum_candidate_angle_rad=self.maximum_candidate_angle_rad,
|
||||
)
|
||||
self.line_history.append(detected)
|
||||
if detected is not None:
|
||||
self.last_line_at = now
|
||||
measurement = summarize_alignment_measurements(self.line_history)
|
||||
if now - self.last_line_at > self.maximum_line_age_seconds:
|
||||
measurement = None
|
||||
|
||||
red_y = int(round(reference_y))
|
||||
cv2.line(
|
||||
image,
|
||||
(15, red_y),
|
||||
(max(15, width - 15), red_y),
|
||||
(0, 0, 255),
|
||||
4,
|
||||
)
|
||||
if measurement is not None:
|
||||
line = np.rint(measurement["line_xyxy_px"]).astype(int)
|
||||
blue_ok, blue_start, blue_end = cv2.clipLine(
|
||||
(0, 0, width, height),
|
||||
(int(line[0]), int(line[1])),
|
||||
(int(line[2]), int(line[3])),
|
||||
)
|
||||
if blue_ok:
|
||||
cv2.line(image, blue_start, blue_end, (255, 0, 0), 3)
|
||||
angle_rad = float(measurement["angle_rad"])
|
||||
offset_px = float(measurement["vertical_offset_px"])
|
||||
aligned = bool(
|
||||
abs(angle_rad) <= self.maximum_alignment_error_rad
|
||||
and abs(offset_px) <= self.maximum_vertical_offset_px
|
||||
)
|
||||
status = "ALIGNED" if aligned else "ADJUST CAMERA"
|
||||
status_text = (
|
||||
f"{self.view.upper()} red-blue "
|
||||
f"{math.degrees(angle_rad):+.2f} deg "
|
||||
f"dy {offset_px:+.1f}px {status}"
|
||||
)
|
||||
status_color = (0, 220, 0) if aligned else (0, 165, 255)
|
||||
else:
|
||||
status_text = (
|
||||
f"{self.view.upper()} PHYSICAL REFERENCE LINE NOT DETECTED"
|
||||
)
|
||||
status_color = (0, 165, 255)
|
||||
cv2.putText(
|
||||
image,
|
||||
status_text,
|
||||
(20, 34),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.72,
|
||||
status_color,
|
||||
2,
|
||||
)
|
||||
self._draw_tags(image, now)
|
||||
cv2.putText(
|
||||
image,
|
||||
"RED=target BLUE=physical edge GREEN=Tags (angle ignored)",
|
||||
(20, max(64, height - 24)),
|
||||
cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.60,
|
||||
(255, 255, 255),
|
||||
2,
|
||||
)
|
||||
output = self.bridge.cv2_to_imgmsg(image, encoding="bgr8")
|
||||
output.header = message.header
|
||||
self.publisher.publish(output)
|
||||
|
||||
|
||||
def main(args: list[str] | None = None) -> None:
|
||||
"""Run the single-view alignment helper."""
|
||||
configure_fastdds_large_image_transport()
|
||||
rclpy.init(args=args)
|
||||
node: G20CameraAlignmentView | None = None
|
||||
try:
|
||||
node = G20CameraAlignmentView()
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
if node is not None:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,756 @@
|
||||
"""Pure three-camera G20 calibration model and compact runtime schema.
|
||||
|
||||
The hardware node records one parent/child AprilTag trajectory for each
|
||||
directly observable joint. This module deliberately contains no ROS imports:
|
||||
curve fitting, four-finger splay centring, inheritance, and schema validation
|
||||
remain deterministic and unit-testable without connected cameras or a hand.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, replace
|
||||
import math
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .core import BASELINE_COMMAND
|
||||
from .trajectory import (
|
||||
_angle_for_circle,
|
||||
_fit_circle_with_axis,
|
||||
_fit_joint_curve,
|
||||
_fit_plane_axis,
|
||||
_orient_circle_positive,
|
||||
)
|
||||
from .zero_calibration import (
|
||||
_fit_circle,
|
||||
_trajectory_arc_rad,
|
||||
circular_median_rad,
|
||||
maximum_pairwise_angle_difference_rad,
|
||||
wrap_angle_rad,
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class JointSpec:
|
||||
name: str
|
||||
motor_index: int
|
||||
active: bool
|
||||
view: str | None
|
||||
parent_role: str | None
|
||||
child_role: str | None
|
||||
source_joint: str | None = None
|
||||
zero_kind: str | None = None
|
||||
|
||||
@property
|
||||
def measured(self) -> bool:
|
||||
return self.source_joint is None
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SweepSpec:
|
||||
view: str
|
||||
motor_index: int
|
||||
joints: tuple[str, ...]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class JointCurveFit:
|
||||
angle_rad: tuple[float, ...]
|
||||
decreasing_rad: tuple[float, ...]
|
||||
increasing_rad: tuple[float, ...]
|
||||
circle: Mapping[str, Any]
|
||||
maximum_monotonic_correction_rad: float
|
||||
maximum_hysteresis_rad: float
|
||||
quality: Mapping[str, float]
|
||||
zero_offset_rad: float = 0.0
|
||||
|
||||
|
||||
VIEW_TAGS: dict[str, dict[str, int]] = {
|
||||
"front": {
|
||||
"front_base": 0,
|
||||
"thumb_cmc": 1,
|
||||
"thumb_mcp": 2,
|
||||
"thumb_ip": 3,
|
||||
"index_roll": 10,
|
||||
},
|
||||
"side": {
|
||||
"side_base": 4,
|
||||
"index_mcp": 5,
|
||||
"index_pip": 6,
|
||||
"index_dip": 7,
|
||||
},
|
||||
"top": {
|
||||
"top_base": 8,
|
||||
"thumb_yaw": 9,
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
JOINT_SPECS: dict[str, JointSpec] = {
|
||||
"thumb_cmc_pitch": JointSpec(
|
||||
"thumb_cmc_pitch", 0, True, "front", "front_base", "thumb_cmc",
|
||||
zero_kind="projected",
|
||||
),
|
||||
"index_mcp_pitch": JointSpec(
|
||||
"index_mcp_pitch", 1, True, "side", "side_base", "index_mcp",
|
||||
zero_kind="projected",
|
||||
),
|
||||
"middle_mcp_pitch": JointSpec(
|
||||
"middle_mcp_pitch", 2, True, None, None, None,
|
||||
source_joint="index_mcp_pitch", zero_kind="inherited",
|
||||
),
|
||||
"ring_mcp_pitch": JointSpec(
|
||||
"ring_mcp_pitch", 3, True, None, None, None,
|
||||
source_joint="index_mcp_pitch", zero_kind="inherited",
|
||||
),
|
||||
"pinky_mcp_pitch": JointSpec(
|
||||
"pinky_mcp_pitch", 4, True, None, None, None,
|
||||
source_joint="index_mcp_pitch", zero_kind="inherited",
|
||||
),
|
||||
"thumb_cmc_roll": JointSpec(
|
||||
"thumb_cmc_roll", 5, True, "front", "front_base", "thumb_cmc",
|
||||
zero_kind="projected",
|
||||
),
|
||||
"index_mcp_roll": JointSpec(
|
||||
"index_mcp_roll", 6, True, "front", "front_base", "index_roll",
|
||||
zero_kind="travel_midpoint",
|
||||
),
|
||||
"middle_mcp_roll": JointSpec(
|
||||
"middle_mcp_roll", 7, True, None, None, None,
|
||||
source_joint="index_mcp_roll", zero_kind="inherited",
|
||||
),
|
||||
"ring_mcp_roll": JointSpec(
|
||||
"ring_mcp_roll", 8, True, None, None, None,
|
||||
source_joint="index_mcp_roll", zero_kind="inherited",
|
||||
),
|
||||
"pinky_mcp_roll": JointSpec(
|
||||
"pinky_mcp_roll", 9, True, None, None, None,
|
||||
source_joint="index_mcp_roll", zero_kind="inherited",
|
||||
),
|
||||
"thumb_cmc_yaw": JointSpec(
|
||||
"thumb_cmc_yaw", 10, True, "top", "top_base", "thumb_yaw",
|
||||
zero_kind="projected",
|
||||
),
|
||||
"thumb_mcp": JointSpec(
|
||||
"thumb_mcp", 15, True, "front", "thumb_cmc", "thumb_mcp",
|
||||
zero_kind="projected",
|
||||
),
|
||||
"index_pip": JointSpec(
|
||||
"index_pip", 16, True, "side", "index_mcp", "index_pip",
|
||||
zero_kind="projected",
|
||||
),
|
||||
"middle_pip": JointSpec(
|
||||
"middle_pip", 17, True, None, None, None,
|
||||
source_joint="index_pip", zero_kind="inherited",
|
||||
),
|
||||
"ring_pip": JointSpec(
|
||||
"ring_pip", 18, True, None, None, None,
|
||||
source_joint="index_pip", zero_kind="inherited",
|
||||
),
|
||||
"pinky_pip": JointSpec(
|
||||
"pinky_pip", 19, True, None, None, None,
|
||||
source_joint="index_pip", zero_kind="inherited",
|
||||
),
|
||||
"thumb_ip": JointSpec(
|
||||
"thumb_ip", 15, False, "front", "thumb_mcp", "thumb_ip",
|
||||
),
|
||||
"index_dip": JointSpec(
|
||||
"index_dip", 16, False, "side", "index_pip", "index_dip",
|
||||
),
|
||||
"middle_dip": JointSpec(
|
||||
"middle_dip", 17, False, None, None, None,
|
||||
source_joint="index_dip",
|
||||
),
|
||||
"ring_dip": JointSpec(
|
||||
"ring_dip", 18, False, None, None, None,
|
||||
source_joint="index_dip",
|
||||
),
|
||||
"pinky_dip": JointSpec(
|
||||
"pinky_dip", 19, False, None, None, None,
|
||||
source_joint="index_dip",
|
||||
),
|
||||
}
|
||||
|
||||
|
||||
SWEEP_SPECS: tuple[SweepSpec, ...] = (
|
||||
SweepSpec("front", 0, ("thumb_cmc_pitch",)),
|
||||
SweepSpec("front", 5, ("thumb_cmc_roll",)),
|
||||
SweepSpec("front", 15, ("thumb_mcp", "thumb_ip")),
|
||||
SweepSpec("front", 6, ("index_mcp_roll",)),
|
||||
SweepSpec("side", 1, ("index_mcp_pitch",)),
|
||||
SweepSpec("side", 16, ("index_pip", "index_dip")),
|
||||
SweepSpec("top", 10, ("thumb_cmc_yaw",)),
|
||||
)
|
||||
|
||||
MEASURED_JOINTS: tuple[str, ...] = tuple(
|
||||
name for name, spec in JOINT_SPECS.items() if spec.measured
|
||||
)
|
||||
ACTIVE_JOINTS: tuple[str, ...] = tuple(
|
||||
name for name, spec in JOINT_SPECS.items() if spec.active
|
||||
)
|
||||
PASSIVE_JOINTS: tuple[str, ...] = tuple(
|
||||
name for name, spec in JOINT_SPECS.items() if not spec.active
|
||||
)
|
||||
SPLAY_JOINTS: tuple[str, ...] = (
|
||||
"index_mcp_roll",
|
||||
"middle_mcp_roll",
|
||||
"ring_mcp_roll",
|
||||
"pinky_mcp_roll",
|
||||
)
|
||||
|
||||
# These directly actuated motions have a fixed parent link during their sweep
|
||||
# and a motion plane that is close to the active camera's image plane. Their
|
||||
# projected circles are substantially more repeatable than the difference of
|
||||
# two independently estimated planar-Tag PnP depths. thumb_ip remains projected
|
||||
# because its front-view PnP depth is not repeatable enough for a 3-D fit; the
|
||||
# side-view index_dip and oblique top-view yaw remain parent-relative 3-D.
|
||||
IMAGE_TRAJECTORY_JOINTS: frozenset[str] = frozenset(
|
||||
{
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_cmc_roll",
|
||||
"thumb_mcp",
|
||||
"thumb_ip",
|
||||
"index_mcp_roll",
|
||||
"index_mcp_pitch",
|
||||
"index_pip",
|
||||
}
|
||||
)
|
||||
|
||||
# Keep the three unmeasured finger-roll motors away from the front camera's
|
||||
# line of sight while index_mcp_roll is measured.
|
||||
INDEX_ROLL_CLEARANCE_COMMANDS: dict[int, int] = {
|
||||
7: 0,
|
||||
8: 0,
|
||||
9: 0,
|
||||
}
|
||||
|
||||
# Hold thumb CMC roll at a camera-friendly pose while thumb CMC yaw is
|
||||
# measured. This keeps the moving top-view tag sufficiently front-facing.
|
||||
THUMB_YAW_CLEARANCE_COMMANDS: dict[int, int] = {
|
||||
5: 145,
|
||||
}
|
||||
|
||||
|
||||
def calibration_auxiliary_commands(spec: SweepSpec) -> dict[int, int]:
|
||||
"""Return motors that must remain fixed throughout one calibration task."""
|
||||
if spec.motor_index == 6:
|
||||
return dict(INDEX_ROLL_CLEARANCE_COMMANDS)
|
||||
if spec.motor_index == 10:
|
||||
return dict(THUMB_YAW_CLEARANCE_COMMANDS)
|
||||
return {}
|
||||
|
||||
|
||||
def build_full_hand_command(
|
||||
motor_index: int,
|
||||
command_u8: int,
|
||||
baseline: Sequence[int] = BASELINE_COMMAND,
|
||||
) -> list[int]:
|
||||
if len(baseline) != 20:
|
||||
raise ValueError("baseline must contain exactly 20 values")
|
||||
motor = int(motor_index)
|
||||
if motor not in {spec.motor_index for spec in JOINT_SPECS.values()}:
|
||||
raise ValueError("motor_index is not a controlled G20 calibration channel")
|
||||
command = int(command_u8)
|
||||
if not 0 <= command <= 255:
|
||||
raise ValueError("command_u8 must be in [0, 255]")
|
||||
result = [int(value) for value in baseline]
|
||||
if any(not 0 <= value <= 255 for value in result):
|
||||
raise ValueError("baseline values must be in [0, 255]")
|
||||
result[motor] = command
|
||||
return result
|
||||
|
||||
|
||||
def build_calibration_motion_command(
|
||||
spec: SweepSpec,
|
||||
command_u8: int,
|
||||
baseline: Sequence[int] = BASELINE_COMMAND,
|
||||
) -> list[int]:
|
||||
"""Build a sweep command, including any required clearance pose."""
|
||||
result = build_full_hand_command(
|
||||
spec.motor_index,
|
||||
command_u8,
|
||||
baseline=baseline,
|
||||
)
|
||||
for motor_index, auxiliary_command in calibration_auxiliary_commands(
|
||||
spec
|
||||
).items():
|
||||
result[motor_index] = auxiliary_command
|
||||
return result
|
||||
|
||||
|
||||
def build_calibration_speed_profile(
|
||||
spec: SweepSpec,
|
||||
*,
|
||||
normal_speed: int,
|
||||
index_roll_speed: int,
|
||||
index_flex_speed: int,
|
||||
) -> list[int]:
|
||||
"""Return G20's five per-finger speeds for one calibration task."""
|
||||
normal = int(normal_speed)
|
||||
index_roll = int(index_roll_speed)
|
||||
index_flex = int(index_flex_speed)
|
||||
if not all(
|
||||
0 <= speed <= 255 for speed in (normal, index_roll, index_flex)
|
||||
):
|
||||
raise ValueError("calibration speeds must be in [0, 255]")
|
||||
speeds = [normal] * 5
|
||||
if spec.motor_index == 6:
|
||||
speeds[1] = index_roll
|
||||
elif spec.motor_index in {1, 16}:
|
||||
speeds[1] = index_flex
|
||||
return speeds
|
||||
|
||||
|
||||
def _record_vector(record: Mapping[str, Any]) -> np.ndarray:
|
||||
value = np.asarray(record.get("relative_translation_xyz_m"), dtype=float)
|
||||
if value.shape != (3,) or not np.all(np.isfinite(value)):
|
||||
raise ValueError("record relative_translation_xyz_m must contain 3 values")
|
||||
return value
|
||||
|
||||
|
||||
def _record_image_vector(record: Mapping[str, Any]) -> np.ndarray:
|
||||
value = np.asarray(record.get("image_relative_xy_px"), dtype=float)
|
||||
if value.shape != (2,) or not np.all(np.isfinite(value)):
|
||||
raise ValueError("record image_relative_xy_px must contain 2 values")
|
||||
return value
|
||||
|
||||
|
||||
def fit_joint_center_curve(
|
||||
records: Sequence[Mapping[str, Any]],
|
||||
*,
|
||||
maximum_plane_rms_m: float = 0.004,
|
||||
maximum_radial_rms_m: float = 0.004,
|
||||
minimum_radius_m: float = 0.003,
|
||||
minimum_arc_rad: float = math.radians(15.0),
|
||||
) -> JointCurveFit:
|
||||
"""Fit one command-indexed curve from a parent-frame centre trajectory."""
|
||||
samples = [dict(record) for record in records]
|
||||
if len(samples) < 12:
|
||||
raise ValueError("joint trajectory requires at least 12 samples")
|
||||
points = np.asarray([_record_vector(record) for record in samples])
|
||||
axis, common_plane_rms = _fit_plane_axis([points])
|
||||
circle = _fit_circle_with_axis(points, axis)
|
||||
circle = _orient_circle_positive(circle, samples, list(points))
|
||||
values = [_angle_for_circle(point, circle) for point in points]
|
||||
curves, correction, hysteresis = _fit_joint_curve(samples, values)
|
||||
quality = {
|
||||
"plane_rms_m": max(
|
||||
float(common_plane_rms), float(circle["plane_rms_m"])
|
||||
),
|
||||
"radial_rms_m": float(circle["radial_rms_m"]),
|
||||
"radius_m": float(circle["radius_m"]),
|
||||
"arc_rad": float(circle["observed_arc_rad"]),
|
||||
}
|
||||
failures: list[str] = []
|
||||
if quality["plane_rms_m"] > float(maximum_plane_rms_m):
|
||||
failures.append("plane_rms")
|
||||
if quality["radial_rms_m"] > float(maximum_radial_rms_m):
|
||||
failures.append("radial_rms")
|
||||
if quality["radius_m"] < float(minimum_radius_m):
|
||||
failures.append("radius")
|
||||
if quality["arc_rad"] < float(minimum_arc_rad):
|
||||
failures.append("arc")
|
||||
if failures:
|
||||
raise ValueError("joint_trajectory_quality_failed:" + ",".join(failures))
|
||||
return JointCurveFit(
|
||||
angle_rad=tuple(float(value) for value in curves["angle_rad"]),
|
||||
decreasing_rad=tuple(
|
||||
float(value) for value in curves["decreasing_rad"]
|
||||
),
|
||||
increasing_rad=tuple(
|
||||
float(value) for value in curves["increasing_rad"]
|
||||
),
|
||||
circle=dict(circle),
|
||||
maximum_monotonic_correction_rad=float(correction),
|
||||
maximum_hysteresis_rad=float(hysteresis),
|
||||
quality=quality,
|
||||
)
|
||||
|
||||
|
||||
def _signed_image_circle_angle(
|
||||
point_xy_px: Sequence[float], circle: Mapping[str, Any]
|
||||
) -> float:
|
||||
point = np.asarray(point_xy_px, dtype=float)
|
||||
centre = np.asarray(circle["center_xy_px"], dtype=float)
|
||||
reference = np.asarray(circle["reference_xy_px"], dtype=float)
|
||||
vector = point - centre
|
||||
if point.shape != (2,) or not np.all(np.isfinite(point)):
|
||||
raise ValueError("image point must contain 2 finite values")
|
||||
if float(np.linalg.norm(vector)) < 1.0e-9:
|
||||
raise ValueError("image point lies at the fitted circle centre")
|
||||
return float(circle["orientation_sign"]) * math.atan2(
|
||||
float(reference[0] * vector[1] - reference[1] * vector[0]),
|
||||
float(np.dot(reference, vector)),
|
||||
)
|
||||
|
||||
|
||||
def fit_joint_image_curve(
|
||||
records: Sequence[Mapping[str, Any]],
|
||||
*,
|
||||
maximum_radial_rms_px: float = 2.0,
|
||||
maximum_radial_p95_px: float = 3.5,
|
||||
minimum_radius_px: float = 20.0,
|
||||
minimum_arc_rad: float = math.radians(15.0),
|
||||
) -> JointCurveFit:
|
||||
"""Fit angular phase from a stable projected parent/child circle."""
|
||||
samples = [dict(record) for record in records]
|
||||
if len(samples) < 12:
|
||||
raise ValueError("joint image trajectory requires at least 12 samples")
|
||||
points = np.asarray([_record_image_vector(record) for record in samples])
|
||||
centre, radius = _fit_circle(points)
|
||||
radial_error = np.abs(np.linalg.norm(points - centre, axis=1) - radius)
|
||||
quality = {
|
||||
"radial_rms_px": float(np.sqrt(np.mean(np.square(radial_error)))),
|
||||
"radial_p95_px": float(np.percentile(radial_error, 95.0)),
|
||||
"radius_px": float(radius),
|
||||
"arc_rad": float(_trajectory_arc_rad(points, centre)),
|
||||
}
|
||||
failures: list[str] = []
|
||||
if quality["radial_rms_px"] > float(maximum_radial_rms_px):
|
||||
failures.append("radial_rms")
|
||||
if quality["radial_p95_px"] > float(maximum_radial_p95_px):
|
||||
failures.append("radial_p95")
|
||||
if quality["radius_px"] < float(minimum_radius_px):
|
||||
failures.append("radius")
|
||||
if quality["arc_rad"] < float(minimum_arc_rad):
|
||||
failures.append("arc")
|
||||
if failures:
|
||||
raise ValueError(
|
||||
"joint_image_trajectory_quality_failed:" + ",".join(failures)
|
||||
)
|
||||
|
||||
endpoint_points = np.asarray(
|
||||
[
|
||||
point
|
||||
for record, point in zip(samples, points)
|
||||
if int(record["command_u8"]) == 255
|
||||
]
|
||||
)
|
||||
if endpoint_points.size == 0:
|
||||
raise ValueError("joint image trajectory is missing command 255")
|
||||
reference = np.median(endpoint_points, axis=0) - centre
|
||||
reference_norm = float(np.linalg.norm(reference))
|
||||
if reference_norm < 1.0e-9:
|
||||
raise ValueError("joint image trajectory endpoint is degenerate")
|
||||
reference /= reference_norm
|
||||
circle: dict[str, Any] = {
|
||||
"space": "image_2d",
|
||||
"center_xy_px": [float(value) for value in centre],
|
||||
"reference_xy_px": [float(value) for value in reference],
|
||||
"orientation_sign": 1.0,
|
||||
"radius_px": float(radius),
|
||||
}
|
||||
values = [_signed_image_circle_angle(point, circle) for point in points]
|
||||
command_zero_values = [
|
||||
value
|
||||
for record, value in zip(samples, values)
|
||||
if int(record["command_u8"]) == 0
|
||||
]
|
||||
if not command_zero_values:
|
||||
raise ValueError("joint image trajectory is missing command 0")
|
||||
if float(np.median(command_zero_values)) < 0.0:
|
||||
circle["orientation_sign"] = -1.0
|
||||
values = [-value for value in values]
|
||||
curves, correction, hysteresis = _fit_joint_curve(samples, values)
|
||||
return JointCurveFit(
|
||||
angle_rad=tuple(float(value) for value in curves["angle_rad"]),
|
||||
decreasing_rad=tuple(
|
||||
float(value) for value in curves["decreasing_rad"]
|
||||
),
|
||||
increasing_rad=tuple(
|
||||
float(value) for value in curves["increasing_rad"]
|
||||
),
|
||||
circle=circle,
|
||||
maximum_monotonic_correction_rad=float(correction),
|
||||
maximum_hysteresis_rad=float(hysteresis),
|
||||
quality=quality,
|
||||
)
|
||||
|
||||
|
||||
def fit_measured_joint_curve(
|
||||
joint_name: str,
|
||||
records: Sequence[Mapping[str, Any]],
|
||||
*,
|
||||
maximum_plane_rms_m: float = 0.004,
|
||||
maximum_radial_rms_m: float = 0.004,
|
||||
minimum_radius_m: float = 0.003,
|
||||
minimum_arc_rad: float = math.radians(15.0),
|
||||
image_maximum_radial_rms_px: float = 2.0,
|
||||
image_maximum_radial_p95_px: float = 3.5,
|
||||
image_minimum_radius_px: float = 20.0,
|
||||
) -> JointCurveFit:
|
||||
"""Select the stable trajectory representation for one measured joint."""
|
||||
if str(joint_name) in IMAGE_TRAJECTORY_JOINTS:
|
||||
return fit_joint_image_curve(
|
||||
records,
|
||||
maximum_radial_rms_px=image_maximum_radial_rms_px,
|
||||
maximum_radial_p95_px=image_maximum_radial_p95_px,
|
||||
minimum_radius_px=image_minimum_radius_px,
|
||||
minimum_arc_rad=minimum_arc_rad,
|
||||
)
|
||||
return fit_joint_center_curve(
|
||||
records,
|
||||
maximum_plane_rms_m=maximum_plane_rms_m,
|
||||
maximum_radial_rms_m=maximum_radial_rms_m,
|
||||
minimum_radius_m=minimum_radius_m,
|
||||
minimum_arc_rad=minimum_arc_rad,
|
||||
)
|
||||
|
||||
|
||||
def center_splay_curve(fit: JointCurveFit) -> tuple[JointCurveFit, int, float]:
|
||||
"""Centre a measured 255->0 splay travel on its angular midpoint."""
|
||||
raw = np.asarray(fit.angle_rad, dtype=float)
|
||||
if raw.shape != (256,) or not np.all(np.isfinite(raw)):
|
||||
raise ValueError("splay curve must contain 256 finite values")
|
||||
if np.any(np.diff(raw) > 1.0e-7):
|
||||
raise ValueError("raw splay curve must be non-increasing")
|
||||
travel_midpoint = 0.5 * float(raw[0] + raw[255])
|
||||
if travel_midpoint <= 0.0:
|
||||
raise ValueError("splay travel must be positive")
|
||||
centred = raw - travel_midpoint
|
||||
# Make the public endpoint symmetry exact after decimal rounding.
|
||||
endpoint = round(0.5 * float(raw[0] - raw[255]), 8)
|
||||
centred[0] = endpoint
|
||||
centred[255] = -endpoint
|
||||
centred = np.asarray([round(float(value), 8) for value in centred])
|
||||
zero_command = int(np.argmin(np.abs(centred)))
|
||||
return (
|
||||
replace(
|
||||
fit,
|
||||
angle_rad=tuple(float(value) for value in centred),
|
||||
zero_offset_rad=round(float(travel_midpoint), 8),
|
||||
),
|
||||
zero_command,
|
||||
round(float(travel_midpoint), 8),
|
||||
)
|
||||
|
||||
|
||||
def measure_joint_vector(fit: JointCurveFit, vector_xyz_m: Sequence[float]) -> float:
|
||||
"""Measure one vector with a fitted model, including splay centring."""
|
||||
return float(
|
||||
_angle_for_circle(vector_xyz_m, fit.circle) - fit.zero_offset_rad
|
||||
)
|
||||
|
||||
|
||||
def measure_joint_observation(
|
||||
fit: JointCurveFit,
|
||||
*,
|
||||
vector_xyz_m: Sequence[float],
|
||||
image_vector_xy_px: Sequence[float],
|
||||
) -> float:
|
||||
"""Measure an observation in the same space used to fit its curve."""
|
||||
if fit.circle.get("space") == "image_2d":
|
||||
return float(
|
||||
_signed_image_circle_angle(image_vector_xy_px, fit.circle)
|
||||
- fit.zero_offset_rad
|
||||
)
|
||||
return measure_joint_vector(fit, vector_xyz_m)
|
||||
|
||||
|
||||
def fit_projected_zero(
|
||||
records: Sequence[Mapping[str, Any]],
|
||||
*,
|
||||
minimum_radius_px: float = 20.0,
|
||||
minimum_arc_rad: float = math.radians(15.0),
|
||||
maximum_radial_rms_px: float = 2.0,
|
||||
maximum_radial_p95_px: float = 3.5,
|
||||
maximum_round_difference_rad: float = math.radians(1.0),
|
||||
) -> float:
|
||||
"""Return the image/table-projected zero from three sweep rounds."""
|
||||
samples = [dict(record) for record in records]
|
||||
points = np.asarray(
|
||||
[record.get("image_relative_xy_px") for record in samples], dtype=float
|
||||
)
|
||||
if points.ndim != 2 or points.shape[1] != 2 or len(points) < 12:
|
||||
raise ValueError("projected zero requires 2-D trajectory samples")
|
||||
if not np.all(np.isfinite(points)):
|
||||
raise ValueError("projected zero samples must be finite")
|
||||
centre, radius = _fit_circle(points)
|
||||
radial_error = np.abs(np.linalg.norm(points - centre, axis=1) - radius)
|
||||
radial_rms = float(np.sqrt(np.mean(np.square(radial_error))))
|
||||
radial_p95 = float(np.percentile(radial_error, 95.0))
|
||||
arc = _trajectory_arc_rad(points, centre)
|
||||
if radius < minimum_radius_px:
|
||||
raise ValueError("projected_zero_radius_too_small")
|
||||
if arc < minimum_arc_rad:
|
||||
raise ValueError("projected_zero_arc_too_small")
|
||||
if radial_rms > maximum_radial_rms_px:
|
||||
raise ValueError("projected_zero_radial_rms_too_large")
|
||||
if radial_p95 > maximum_radial_p95_px:
|
||||
raise ValueError("projected_zero_radial_p95_too_large")
|
||||
|
||||
cycles = sorted({int(record["cycle"]) for record in samples})
|
||||
if len(cycles) < 3:
|
||||
raise ValueError("projected zero requires three sweep rounds")
|
||||
angles: list[float] = []
|
||||
for cycle in cycles:
|
||||
zero_points = np.asarray(
|
||||
[
|
||||
record["image_relative_xy_px"]
|
||||
for record in samples
|
||||
if int(record["cycle"]) == cycle
|
||||
and int(record["command_u8"]) == 255
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
if zero_points.size == 0:
|
||||
raise ValueError("projected zero is missing command 255")
|
||||
point = np.median(zero_points, axis=0)
|
||||
inward = centre - point
|
||||
if float(np.linalg.norm(inward)) < 1.0e-9:
|
||||
raise ValueError("projected zero point is degenerate")
|
||||
angles.append(
|
||||
wrap_angle_rad(math.atan2(-float(inward[1]), float(inward[0])))
|
||||
)
|
||||
if maximum_pairwise_angle_difference_rad(angles) > maximum_round_difference_rad:
|
||||
raise ValueError("projected_zero_round_difference_too_large")
|
||||
return round(float(circular_median_rad(angles)), 8)
|
||||
|
||||
|
||||
def build_compact_payload(
|
||||
*,
|
||||
serial_number: str,
|
||||
measured_fits: Mapping[str, JointCurveFit],
|
||||
projected_zeros_rad: Mapping[str, float],
|
||||
splay_zero_command_u8: int,
|
||||
splay_midpoint_rad: float,
|
||||
validation_errors_rad: Sequence[float],
|
||||
passed: bool,
|
||||
baseline: Sequence[int] = BASELINE_COMMAND,
|
||||
) -> dict[str, Any]:
|
||||
if set(measured_fits) != set(MEASURED_JOINTS):
|
||||
raise ValueError("measured_fits must contain all directly measured joints")
|
||||
expected_projected = {
|
||||
name for name, spec in JOINT_SPECS.items()
|
||||
if spec.zero_kind == "projected"
|
||||
}
|
||||
if set(projected_zeros_rad) != expected_projected:
|
||||
raise ValueError("projected_zeros_rad has the wrong joint set")
|
||||
if not 0 <= int(splay_zero_command_u8) <= 255:
|
||||
raise ValueError("splay zero command must be in [0, 255]")
|
||||
|
||||
joints: dict[str, dict[str, Any]] = {}
|
||||
for name, spec in JOINT_SPECS.items():
|
||||
source_name = spec.source_joint or name
|
||||
fit = measured_fits[source_name]
|
||||
joint: dict[str, Any] = {
|
||||
"motor_index": int(spec.motor_index),
|
||||
"angle_rad": [round(float(value), 8) for value in fit.angle_rad],
|
||||
}
|
||||
if spec.active:
|
||||
joint["zero_command_u8"] = (
|
||||
int(splay_zero_command_u8)
|
||||
if name in SPLAY_JOINTS
|
||||
else 255
|
||||
)
|
||||
else:
|
||||
joint["passive"] = True
|
||||
if spec.source_joint is not None:
|
||||
joint["source_joint"] = spec.source_joint
|
||||
elif spec.zero_kind == "projected":
|
||||
joint["zero_angles"] = {
|
||||
"table_projected_zero_rad": round(
|
||||
float(projected_zeros_rad[name]), 8
|
||||
)
|
||||
}
|
||||
elif spec.zero_kind == "travel_midpoint":
|
||||
joint["zero_angles"] = {
|
||||
"travel_midpoint_rad": round(float(splay_midpoint_rad), 8)
|
||||
}
|
||||
joints[name] = joint
|
||||
|
||||
errors = np.abs(np.asarray(validation_errors_rad, dtype=float))
|
||||
mae = float(np.mean(errors)) if errors.size else float("nan")
|
||||
p95 = float(np.percentile(errors, 95.0)) if errors.size else float("nan")
|
||||
payload = {
|
||||
"schema_version": 3,
|
||||
"model": "G20",
|
||||
"side": "left",
|
||||
"serial_number": str(serial_number),
|
||||
"angle_unit": "rad",
|
||||
"command_range": [0, 255],
|
||||
"baseline_command_u8": [int(value) for value in baseline],
|
||||
"joints": joints,
|
||||
"quality": {
|
||||
"passed": bool(passed),
|
||||
"validation_mae_rad": (
|
||||
None if not math.isfinite(mae) else round(mae, 8)
|
||||
),
|
||||
"validation_p95_rad": (
|
||||
None if not math.isfinite(p95) else round(p95, 8)
|
||||
),
|
||||
},
|
||||
}
|
||||
validate_compact_payload(payload)
|
||||
return payload
|
||||
|
||||
|
||||
def validate_compact_payload(payload: Mapping[str, Any]) -> None:
|
||||
expected_top = {
|
||||
"schema_version", "model", "side", "serial_number", "angle_unit",
|
||||
"command_range", "baseline_command_u8", "joints", "quality",
|
||||
}
|
||||
if set(payload) != expected_top:
|
||||
raise ValueError("compact calibration has unexpected top-level fields")
|
||||
if payload["schema_version"] != 3:
|
||||
raise ValueError("schema_version must be 3")
|
||||
if payload["model"] != "G20" or payload["side"] != "left":
|
||||
raise ValueError("payload must describe a left G20")
|
||||
if payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255]:
|
||||
raise ValueError("payload angle or command units are invalid")
|
||||
baseline = payload["baseline_command_u8"]
|
||||
if not isinstance(baseline, list) or len(baseline) != 20:
|
||||
raise ValueError("baseline_command_u8 must contain 20 values")
|
||||
joints = payload["joints"]
|
||||
if not isinstance(joints, Mapping) or set(joints) != set(JOINT_SPECS):
|
||||
raise ValueError("payload must contain exactly 21 G20 joints")
|
||||
|
||||
for name, spec in JOINT_SPECS.items():
|
||||
joint = joints[name]
|
||||
allowed = {"motor_index", "angle_rad"}
|
||||
allowed.add("zero_command_u8" if spec.active else "passive")
|
||||
if spec.source_joint is not None:
|
||||
allowed.add("source_joint")
|
||||
elif spec.zero_kind in {"projected", "travel_midpoint"}:
|
||||
allowed.add("zero_angles")
|
||||
if set(joint) != allowed:
|
||||
raise ValueError(f"{name} has unexpected fields")
|
||||
if int(joint["motor_index"]) != spec.motor_index:
|
||||
raise ValueError(f"{name} has the wrong motor_index")
|
||||
curve = np.asarray(joint["angle_rad"], dtype=float)
|
||||
if curve.shape != (256,) or not np.all(np.isfinite(curve)):
|
||||
raise ValueError(f"{name}.angle_rad must contain 256 finite values")
|
||||
if np.any(np.diff(curve) > 1.0e-7):
|
||||
raise ValueError(f"{name}.angle_rad must be non-increasing")
|
||||
if spec.active:
|
||||
zero = joint["zero_command_u8"]
|
||||
if not isinstance(zero, int) or not 0 <= zero <= 255:
|
||||
raise ValueError(f"{name}.zero_command_u8 is invalid")
|
||||
elif joint.get("passive") is not True:
|
||||
raise ValueError(f"{name} must be marked passive")
|
||||
if spec.source_joint is not None:
|
||||
if joint.get("source_joint") != spec.source_joint:
|
||||
raise ValueError(f"{name} has the wrong source_joint")
|
||||
source = joints[spec.source_joint]
|
||||
if joint["angle_rad"] != source["angle_rad"]:
|
||||
raise ValueError(f"{name} must copy its source curve exactly")
|
||||
if spec.active and joint["zero_command_u8"] != source["zero_command_u8"]:
|
||||
raise ValueError(f"{name} must copy its source zero command")
|
||||
|
||||
index_roll = np.asarray(joints["index_mcp_roll"]["angle_rad"], dtype=float)
|
||||
if not index_roll[0] > 0.0 or not index_roll[255] < 0.0:
|
||||
raise ValueError("index_mcp_roll endpoints must be positive then negative")
|
||||
if abs(float(index_roll[0] + index_roll[255])) > 1.0e-7:
|
||||
raise ValueError("index_mcp_roll endpoints must be symmetric")
|
||||
for name, spec in JOINT_SPECS.items():
|
||||
if name not in SPLAY_JOINTS and spec.source_joint != "index_mcp_roll":
|
||||
curve = np.asarray(joints[name]["angle_rad"], dtype=float)
|
||||
if abs(float(curve[255])) > 1.0e-6:
|
||||
raise ValueError(f"{name}.angle_rad[255] must be zero")
|
||||
|
||||
quality = payload["quality"]
|
||||
if set(quality) != {
|
||||
"passed", "validation_mae_rad", "validation_p95_rad"
|
||||
}:
|
||||
raise ValueError("quality must contain only compact summary fields")
|
||||
@@ -0,0 +1,644 @@
|
||||
"""ROS 2 image publisher for Hikrobot MVS USB3 Vision cameras."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from ctypes import byref, cast, memset, POINTER, sizeof, string_at
|
||||
from dataclasses import dataclass
|
||||
import importlib
|
||||
import os
|
||||
from pathlib import Path
|
||||
import sys
|
||||
from threading import Event, Lock, Thread
|
||||
from typing import Any, Sequence
|
||||
from urllib.parse import unquote, urlparse
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import CameraInfo, Image
|
||||
from sensor_msgs.srv import SetCameraInfo
|
||||
import yaml
|
||||
|
||||
|
||||
DEFAULT_MVS_PYTHON_PATH = "/opt/MVS/Samples/64/Python/MvImport"
|
||||
DEFAULT_MVS_RUNTIME_PATH = "/opt/MVS/lib"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DeviceDescriptor:
|
||||
"""Stable identity fields for one enumerated USB3 Vision camera."""
|
||||
|
||||
index: int
|
||||
model: str
|
||||
serial: str
|
||||
guid: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CameraCalibration:
|
||||
"""Validated subset of a standard ROS camera calibration YAML file."""
|
||||
|
||||
camera_name: str
|
||||
width: int
|
||||
height: int
|
||||
distortion_model: str
|
||||
d: tuple[float, ...]
|
||||
k: tuple[float, ...]
|
||||
r: tuple[float, ...]
|
||||
p: tuple[float, ...]
|
||||
|
||||
|
||||
def decode_c_string(value: Any) -> str:
|
||||
"""Decode a null-terminated MVS SDK character array."""
|
||||
raw = bytes(value).split(b"\0", 1)[0]
|
||||
for encoding in ("utf-8", "gbk", "latin-1"):
|
||||
try:
|
||||
return raw.decode(encoding)
|
||||
except UnicodeDecodeError:
|
||||
continue
|
||||
return raw.decode("latin-1", errors="replace")
|
||||
|
||||
|
||||
def select_device(
|
||||
devices: Sequence[DeviceDescriptor],
|
||||
selector: str,
|
||||
expected_model: str,
|
||||
) -> DeviceDescriptor:
|
||||
"""Select exactly one camera by serial or GUID and check its model."""
|
||||
if not devices:
|
||||
raise RuntimeError("MVS did not enumerate any USB3 Vision cameras")
|
||||
selector = selector.strip()
|
||||
if selector:
|
||||
matches = [
|
||||
device
|
||||
for device in devices
|
||||
if selector in {device.serial, device.guid}
|
||||
]
|
||||
if not matches:
|
||||
available = ", ".join(
|
||||
f"{device.model}:{device.serial}:{device.guid}"
|
||||
for device in devices
|
||||
)
|
||||
raise RuntimeError(
|
||||
f"camera selector {selector!r} was not found; "
|
||||
f"available cameras: {available}"
|
||||
)
|
||||
elif len(devices) == 1:
|
||||
matches = [devices[0]]
|
||||
else:
|
||||
available = ", ".join(device.serial for device in devices)
|
||||
raise RuntimeError(
|
||||
"camera selector is required when multiple cameras are present; "
|
||||
f"available serials: {available}"
|
||||
)
|
||||
if len(matches) != 1:
|
||||
raise RuntimeError(f"camera selector {selector!r} is not unique")
|
||||
selected = matches[0]
|
||||
if expected_model and expected_model not in selected.model:
|
||||
raise RuntimeError(
|
||||
f"camera {selected.serial} is {selected.model!r}, expected a model "
|
||||
f"containing {expected_model!r}"
|
||||
)
|
||||
return selected
|
||||
|
||||
|
||||
def resolve_camera_info_path(url_or_path: str) -> Path | None:
|
||||
"""Resolve a plain path or file:// URL used for CameraInfo persistence."""
|
||||
value = url_or_path.strip()
|
||||
if not value:
|
||||
return None
|
||||
parsed = urlparse(value)
|
||||
if parsed.scheme not in {"", "file"}:
|
||||
raise ValueError(
|
||||
"camera_info_url must be a filesystem path or file:// URL"
|
||||
)
|
||||
if parsed.scheme == "file":
|
||||
if parsed.netloc not in {"", "localhost"}:
|
||||
raise ValueError("remote file:// camera_info_url is not supported")
|
||||
value = unquote(parsed.path)
|
||||
return Path(value).expanduser().resolve()
|
||||
|
||||
|
||||
def _matrix_data(mapping: dict[str, Any], name: str, length: int) -> tuple[float, ...]:
|
||||
section = mapping.get(name)
|
||||
if not isinstance(section, dict):
|
||||
raise ValueError(f"camera calibration is missing {name}")
|
||||
data = section.get("data")
|
||||
if not isinstance(data, list) or len(data) != length:
|
||||
raise ValueError(f"{name}.data must contain exactly {length} values")
|
||||
result = tuple(float(value) for value in data)
|
||||
if not all(value == value and abs(value) != float("inf") for value in result):
|
||||
raise ValueError(f"{name}.data contains a non-finite value")
|
||||
return result
|
||||
|
||||
|
||||
def load_camera_calibration(path: Path) -> CameraCalibration:
|
||||
"""Load and validate a standard ROS camera calibration YAML file."""
|
||||
mapping = yaml.safe_load(path.read_text(encoding="utf-8"))
|
||||
if not isinstance(mapping, dict):
|
||||
raise ValueError("camera calibration YAML root must be a mapping")
|
||||
width = int(mapping.get("image_width", 0))
|
||||
height = int(mapping.get("image_height", 0))
|
||||
if width <= 0 or height <= 0:
|
||||
raise ValueError("camera calibration image dimensions must be positive")
|
||||
distortion = mapping.get("distortion_coefficients")
|
||||
if not isinstance(distortion, dict) or not isinstance(
|
||||
distortion.get("data"), list
|
||||
):
|
||||
raise ValueError(
|
||||
"camera calibration is missing distortion_coefficients.data"
|
||||
)
|
||||
d = tuple(float(value) for value in distortion["data"])
|
||||
calibration = CameraCalibration(
|
||||
camera_name=str(mapping.get("camera_name", "hikrobot_camera")),
|
||||
width=width,
|
||||
height=height,
|
||||
distortion_model=str(mapping.get("distortion_model", "plumb_bob")),
|
||||
d=d,
|
||||
k=_matrix_data(mapping, "camera_matrix", 9),
|
||||
r=_matrix_data(mapping, "rectification_matrix", 9),
|
||||
p=_matrix_data(mapping, "projection_matrix", 12),
|
||||
)
|
||||
if calibration.k[0] <= 0.0 or calibration.k[4] <= 0.0:
|
||||
raise ValueError("camera calibration focal lengths must be positive")
|
||||
if calibration.p[0] <= 0.0 or calibration.p[5] <= 0.0:
|
||||
raise ValueError("camera calibration projection focal lengths must be positive")
|
||||
return calibration
|
||||
|
||||
|
||||
def calibration_to_mapping(
|
||||
camera_name: str, camera_info: CameraInfo
|
||||
) -> dict[str, Any]:
|
||||
"""Convert CameraInfo into the standard ROS YAML representation."""
|
||||
return {
|
||||
"image_width": int(camera_info.width),
|
||||
"image_height": int(camera_info.height),
|
||||
"camera_name": camera_name,
|
||||
"camera_matrix": {
|
||||
"rows": 3,
|
||||
"cols": 3,
|
||||
"data": [float(value) for value in camera_info.k],
|
||||
},
|
||||
"distortion_model": camera_info.distortion_model,
|
||||
"distortion_coefficients": {
|
||||
"rows": 1,
|
||||
"cols": len(camera_info.d),
|
||||
"data": [float(value) for value in camera_info.d],
|
||||
},
|
||||
"rectification_matrix": {
|
||||
"rows": 3,
|
||||
"cols": 3,
|
||||
"data": [float(value) for value in camera_info.r],
|
||||
},
|
||||
"projection_matrix": {
|
||||
"rows": 3,
|
||||
"cols": 4,
|
||||
"data": [float(value) for value in camera_info.p],
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def configure_fastdds_large_image_transport() -> Path:
|
||||
"""Select the package's large-image SHM profile before creating ROS nodes."""
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
profile = (
|
||||
Path(get_package_share_directory("g20_thumb_apriltag_calibration"))
|
||||
/ "config"
|
||||
/ "fastdds_large_images.xml"
|
||||
)
|
||||
if not profile.is_file():
|
||||
raise RuntimeError(f"Fast DDS large-image profile not found: {profile}")
|
||||
os.environ.setdefault("FASTRTPS_DEFAULT_PROFILES_FILE", str(profile))
|
||||
return profile
|
||||
|
||||
|
||||
class HikrobotCameraNode(Node):
|
||||
"""Publish synchronized Mono8 Image and CameraInfo messages from MVS."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("hikrobot_camera")
|
||||
self._declare_parameters()
|
||||
self._stop_event = Event()
|
||||
self._camera_info_lock = Lock()
|
||||
self._grab_thread: Thread | None = None
|
||||
self._camera: Any | None = None
|
||||
self._mvs: Any | None = None
|
||||
self._sdk_initialized = False
|
||||
self._handle_created = False
|
||||
self._camera_opened = False
|
||||
self._grabbing = False
|
||||
self._closed = False
|
||||
self._frame_count = 0
|
||||
self._timeout_count = 0
|
||||
|
||||
self.selector = str(self.get_parameter("serial_number").value)
|
||||
self.expected_model = str(self.get_parameter("expected_model").value)
|
||||
self.frame_id = str(self.get_parameter("frame_id").value)
|
||||
self.camera_name = str(self.get_parameter("camera_name").value)
|
||||
self.image_width = int(self.get_parameter("image_width").value)
|
||||
self.image_height = int(self.get_parameter("image_height").value)
|
||||
if self.image_width <= 0 or self.image_height <= 0:
|
||||
raise ValueError("image_width and image_height must be positive")
|
||||
if float(self.get_parameter("frame_rate").value) <= 0.0:
|
||||
raise ValueError("frame_rate must be positive")
|
||||
if int(self.get_parameter("grab_timeout_ms").value) <= 0:
|
||||
raise ValueError("grab_timeout_ms must be positive")
|
||||
self.camera_info_path = resolve_camera_info_path(
|
||||
str(self.get_parameter("camera_info_url").value)
|
||||
)
|
||||
self._calibration = self._load_calibration()
|
||||
|
||||
self.image_publisher = self.create_publisher(
|
||||
Image, "image_raw", qos_profile_sensor_data
|
||||
)
|
||||
self.camera_info_publisher = self.create_publisher(
|
||||
CameraInfo, "camera_info", qos_profile_sensor_data
|
||||
)
|
||||
self.create_service(
|
||||
SetCameraInfo, "set_camera_info", self._set_camera_info_callback
|
||||
)
|
||||
|
||||
try:
|
||||
self._open_camera()
|
||||
self._grab_thread = Thread(
|
||||
target=self._grab_loop,
|
||||
name="hikrobot_mvs_grab",
|
||||
daemon=True,
|
||||
)
|
||||
self._grab_thread.start()
|
||||
except Exception:
|
||||
self.close()
|
||||
raise
|
||||
|
||||
def _declare_parameters(self) -> None:
|
||||
self.declare_parameter("serial_number", "")
|
||||
self.declare_parameter("expected_model", "MV-CS020-10UM")
|
||||
self.declare_parameter("camera_name", "hikrobot_front")
|
||||
self.declare_parameter("frame_id", "camera_color_optical_frame")
|
||||
self.declare_parameter("image_width", 1624)
|
||||
self.declare_parameter("image_height", 1240)
|
||||
self.declare_parameter("frame_rate", 30.0)
|
||||
self.declare_parameter("exposure_time_us", 5000.0)
|
||||
self.declare_parameter("gain_db", 0.0)
|
||||
self.declare_parameter("auto_exposure", False)
|
||||
self.declare_parameter("camera_info_url", "")
|
||||
self.declare_parameter("mvs_python_path", DEFAULT_MVS_PYTHON_PATH)
|
||||
self.declare_parameter("mvs_runtime_path", DEFAULT_MVS_RUNTIME_PATH)
|
||||
self.declare_parameter("grab_timeout_ms", 1000)
|
||||
|
||||
def _load_calibration(self) -> CameraCalibration | None:
|
||||
if self.camera_info_path is None or not self.camera_info_path.exists():
|
||||
target = self.camera_info_path or "an unset camera_info_url"
|
||||
self.get_logger().warning(
|
||||
f"No camera calibration found at {target}; publishing "
|
||||
"uncalibrated CameraInfo. Trajectory calibration will remain "
|
||||
"locked until valid intrinsics are installed."
|
||||
)
|
||||
return None
|
||||
calibration = load_camera_calibration(self.camera_info_path)
|
||||
self._validate_calibration_dimensions(calibration)
|
||||
self.get_logger().info(
|
||||
f"Loaded camera intrinsics from {self.camera_info_path}"
|
||||
)
|
||||
return calibration
|
||||
|
||||
def _validate_calibration_dimensions(
|
||||
self, calibration: CameraCalibration
|
||||
) -> None:
|
||||
if (
|
||||
calibration.width != self.image_width
|
||||
or calibration.height != self.image_height
|
||||
):
|
||||
raise ValueError(
|
||||
"camera calibration dimensions "
|
||||
f"{calibration.width}x{calibration.height} do not match "
|
||||
f"configured stream {self.image_width}x{self.image_height}"
|
||||
)
|
||||
|
||||
def _import_mvs(self) -> Any:
|
||||
python_path = Path(
|
||||
str(self.get_parameter("mvs_python_path").value)
|
||||
).expanduser()
|
||||
runtime_path = Path(
|
||||
str(self.get_parameter("mvs_runtime_path").value)
|
||||
).expanduser()
|
||||
if not python_path.is_dir():
|
||||
raise RuntimeError(f"MVS Python bindings not found: {python_path}")
|
||||
if not runtime_path.is_dir():
|
||||
raise RuntimeError(f"MVS runtime not found: {runtime_path}")
|
||||
os.environ.setdefault("MVCAM_COMMON_RUNENV", str(runtime_path))
|
||||
if str(python_path) not in sys.path:
|
||||
sys.path.insert(0, str(python_path))
|
||||
return importlib.import_module("MvCameraControl_class")
|
||||
|
||||
def _enumerate_usb_cameras(
|
||||
self, mvs: Any
|
||||
) -> tuple[Any, list[DeviceDescriptor]]:
|
||||
device_list = mvs.MV_CC_DEVICE_INFO_LIST()
|
||||
result = mvs.MvCamera.MV_CC_EnumDevices(
|
||||
mvs.MV_USB_DEVICE, device_list
|
||||
)
|
||||
self._require_ok("enumerate USB3 Vision devices", result)
|
||||
descriptors: list[DeviceDescriptor] = []
|
||||
for index in range(device_list.nDeviceNum):
|
||||
device_info = cast(
|
||||
device_list.pDeviceInfo[index], POINTER(mvs.MV_CC_DEVICE_INFO)
|
||||
).contents
|
||||
usb_info = device_info.SpecialInfo.stUsb3VInfo
|
||||
descriptors.append(
|
||||
DeviceDescriptor(
|
||||
index=index,
|
||||
model=decode_c_string(usb_info.chModelName),
|
||||
serial=decode_c_string(usb_info.chSerialNumber),
|
||||
guid=decode_c_string(usb_info.chDeviceGUID),
|
||||
)
|
||||
)
|
||||
return device_list, descriptors
|
||||
|
||||
def _open_camera(self) -> None:
|
||||
mvs = self._import_mvs()
|
||||
self._mvs = mvs
|
||||
self._require_ok("initialize MVS SDK", mvs.MvCamera.MV_CC_Initialize())
|
||||
self._sdk_initialized = True
|
||||
device_list, descriptors = self._enumerate_usb_cameras(mvs)
|
||||
selected = select_device(descriptors, self.selector, self.expected_model)
|
||||
device_info = cast(
|
||||
device_list.pDeviceInfo[selected.index],
|
||||
POINTER(mvs.MV_CC_DEVICE_INFO),
|
||||
).contents
|
||||
|
||||
camera = mvs.MvCamera()
|
||||
self._require_ok("create camera handle", camera.MV_CC_CreateHandle(device_info))
|
||||
self._camera = camera
|
||||
self._handle_created = True
|
||||
self._require_ok(
|
||||
"open camera",
|
||||
camera.MV_CC_OpenDevice(mvs.MV_ACCESS_Exclusive, 0),
|
||||
)
|
||||
self._camera_opened = True
|
||||
|
||||
self._set_required_enum("AcquisitionMode", "Continuous")
|
||||
self._set_required_enum("TriggerMode", "Off")
|
||||
self._set_required_enum("PixelFormat", "Mono8")
|
||||
self._set_required_integer("OffsetX", 0)
|
||||
self._set_required_integer("OffsetY", 0)
|
||||
self._set_required_integer("Width", self.image_width)
|
||||
self._set_required_integer("Height", self.image_height)
|
||||
self._set_optional_bool("AcquisitionFrameRateEnable", True)
|
||||
self._set_optional_float(
|
||||
"AcquisitionFrameRate",
|
||||
float(self.get_parameter("frame_rate").value),
|
||||
)
|
||||
if bool(self.get_parameter("auto_exposure").value):
|
||||
self._set_required_enum("ExposureAuto", "Continuous")
|
||||
else:
|
||||
self._set_required_enum("ExposureAuto", "Off")
|
||||
self._set_optional_float(
|
||||
"ExposureTime",
|
||||
float(self.get_parameter("exposure_time_us").value),
|
||||
)
|
||||
self._set_required_enum("GainAuto", "Off")
|
||||
self._set_optional_float(
|
||||
"Gain", float(self.get_parameter("gain_db").value)
|
||||
)
|
||||
|
||||
self._require_ok(
|
||||
"set latest-frame grab strategy",
|
||||
camera.MV_CC_SetGrabStrategy(mvs.MV_GrabStrategy_LatestImagesOnly),
|
||||
)
|
||||
self._require_ok("start image acquisition", camera.MV_CC_StartGrabbing())
|
||||
self._grabbing = True
|
||||
self.get_logger().info(
|
||||
"Opened Hikrobot camera "
|
||||
f"model={selected.model} serial={selected.serial} "
|
||||
f"guid={selected.guid} stream={self.image_width}x{self.image_height} "
|
||||
"encoding=mono8"
|
||||
)
|
||||
|
||||
def _set_required_enum(self, name: str, value: str) -> None:
|
||||
assert self._camera is not None
|
||||
self._require_ok(
|
||||
f"set {name}={value}",
|
||||
self._camera.MV_CC_SetEnumValueByString(name, value),
|
||||
)
|
||||
|
||||
def _set_required_integer(self, name: str, value: int) -> None:
|
||||
assert self._camera is not None
|
||||
self._require_ok(
|
||||
f"set {name}={value}",
|
||||
self._camera.MV_CC_SetIntValueEx(name, value),
|
||||
)
|
||||
|
||||
def _set_optional_bool(self, name: str, value: bool) -> None:
|
||||
assert self._camera is not None
|
||||
result = self._camera.MV_CC_SetBoolValue(name, value)
|
||||
if result != 0:
|
||||
self.get_logger().warning(
|
||||
f"MVS could not set {name}={value}: 0x{result:08x}"
|
||||
)
|
||||
|
||||
def _set_optional_float(self, name: str, value: float) -> None:
|
||||
assert self._camera is not None
|
||||
result = self._camera.MV_CC_SetFloatValue(name, value)
|
||||
if result != 0:
|
||||
self.get_logger().warning(
|
||||
f"MVS could not set {name}={value}: 0x{result:08x}"
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def _require_ok(operation: str, result: int) -> None:
|
||||
if result != 0:
|
||||
hint = ""
|
||||
if result == 0x80000203:
|
||||
hint = (
|
||||
"; access denied: stop acquisition and close the MVS "
|
||||
"viewer or any other process using this camera"
|
||||
)
|
||||
raise RuntimeError(
|
||||
f"MVS failed to {operation}: 0x{result:08x}{hint}"
|
||||
)
|
||||
|
||||
def _grab_loop(self) -> None:
|
||||
assert self._camera is not None
|
||||
assert self._mvs is not None
|
||||
timeout_ms = int(self.get_parameter("grab_timeout_ms").value)
|
||||
while not self._stop_event.is_set():
|
||||
frame = self._mvs.MV_FRAME_OUT()
|
||||
memset(byref(frame), 0, sizeof(frame))
|
||||
result = self._camera.MV_CC_GetImageBuffer(frame, timeout_ms)
|
||||
if result != 0:
|
||||
self._timeout_count += 1
|
||||
if self._timeout_count == 1 or self._timeout_count % 10 == 0:
|
||||
self.get_logger().warning(
|
||||
"MVS image acquisition timed out or failed: "
|
||||
f"0x{result:08x}; consecutive={self._timeout_count}"
|
||||
)
|
||||
continue
|
||||
try:
|
||||
self._timeout_count = 0
|
||||
self._publish_frame(frame)
|
||||
except Exception as error:
|
||||
self.get_logger().error(f"Failed to publish MVS frame: {error}")
|
||||
finally:
|
||||
free_result = self._camera.MV_CC_FreeImageBuffer(frame)
|
||||
if free_result != 0:
|
||||
self.get_logger().error(
|
||||
f"MVS failed to release frame: 0x{free_result:08x}"
|
||||
)
|
||||
|
||||
def _publish_frame(self, frame: Any) -> None:
|
||||
assert self._mvs is not None
|
||||
info = frame.stFrameInfo
|
||||
width = int(info.nWidth)
|
||||
height = int(info.nHeight)
|
||||
if width != self.image_width or height != self.image_height:
|
||||
raise RuntimeError(
|
||||
f"received {width}x{height}, expected "
|
||||
f"{self.image_width}x{self.image_height}"
|
||||
)
|
||||
if int(info.enPixelType) != int(self._mvs.PixelType_Gvsp_Mono8):
|
||||
raise RuntimeError(
|
||||
f"received pixel type 0x{int(info.enPixelType):x}, expected Mono8"
|
||||
)
|
||||
expected_size = width * height
|
||||
if int(info.nFrameLen) < expected_size:
|
||||
raise RuntimeError(
|
||||
f"frame contains {info.nFrameLen} bytes, expected {expected_size}"
|
||||
)
|
||||
stamp = self.get_clock().now().to_msg()
|
||||
image = Image()
|
||||
image.header.stamp = stamp
|
||||
image.header.frame_id = self.frame_id
|
||||
image.height = height
|
||||
image.width = width
|
||||
image.encoding = "mono8"
|
||||
image.is_bigendian = 0
|
||||
image.step = width
|
||||
image.data = string_at(frame.pBufAddr, expected_size)
|
||||
|
||||
camera_info = self._make_camera_info(width, height)
|
||||
camera_info.header.stamp = stamp
|
||||
camera_info.header.frame_id = self.frame_id
|
||||
self.image_publisher.publish(image)
|
||||
self.camera_info_publisher.publish(camera_info)
|
||||
self._frame_count += 1
|
||||
|
||||
def _make_camera_info(self, width: int, height: int) -> CameraInfo:
|
||||
message = CameraInfo()
|
||||
message.width = width
|
||||
message.height = height
|
||||
with self._camera_info_lock:
|
||||
calibration = self._calibration
|
||||
if calibration is None:
|
||||
message.distortion_model = "plumb_bob"
|
||||
message.d = []
|
||||
message.k = [0.0] * 9
|
||||
message.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
|
||||
message.p = [0.0] * 12
|
||||
return message
|
||||
message.distortion_model = calibration.distortion_model
|
||||
message.d = list(calibration.d)
|
||||
message.k = list(calibration.k)
|
||||
message.r = list(calibration.r)
|
||||
message.p = list(calibration.p)
|
||||
return message
|
||||
|
||||
def _set_camera_info_callback(
|
||||
self,
|
||||
request: SetCameraInfo.Request,
|
||||
response: SetCameraInfo.Response,
|
||||
) -> SetCameraInfo.Response:
|
||||
try:
|
||||
if self.camera_info_path is None:
|
||||
raise ValueError(
|
||||
"camera_info_url is empty; set it before saving calibration"
|
||||
)
|
||||
message = request.camera_info
|
||||
if (
|
||||
int(message.width) != self.image_width
|
||||
or int(message.height) != self.image_height
|
||||
):
|
||||
raise ValueError(
|
||||
f"calibration is {message.width}x{message.height}, expected "
|
||||
f"{self.image_width}x{self.image_height}"
|
||||
)
|
||||
mapping = calibration_to_mapping(self.camera_name, message)
|
||||
self.camera_info_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
temporary_path = self.camera_info_path.with_suffix(
|
||||
self.camera_info_path.suffix + ".tmp"
|
||||
)
|
||||
temporary_path.write_text(
|
||||
yaml.safe_dump(mapping, sort_keys=False), encoding="utf-8"
|
||||
)
|
||||
temporary_path.replace(self.camera_info_path)
|
||||
calibration = load_camera_calibration(self.camera_info_path)
|
||||
self._validate_calibration_dimensions(calibration)
|
||||
with self._camera_info_lock:
|
||||
self._calibration = calibration
|
||||
response.success = True
|
||||
response.status_message = (
|
||||
f"saved camera calibration to {self.camera_info_path}"
|
||||
)
|
||||
self.get_logger().info(response.status_message)
|
||||
except Exception as error:
|
||||
response.success = False
|
||||
response.status_message = str(error)
|
||||
self.get_logger().error(
|
||||
f"Rejected camera calibration: {response.status_message}"
|
||||
)
|
||||
return response
|
||||
|
||||
def close(self) -> None:
|
||||
if self._closed:
|
||||
return
|
||||
self._closed = True
|
||||
self._stop_event.set()
|
||||
if self._grab_thread is not None:
|
||||
self._grab_thread.join(timeout=2.0)
|
||||
if self._camera is not None and self._camera_opened:
|
||||
if self._grabbing:
|
||||
result = self._camera.MV_CC_StopGrabbing()
|
||||
if result != 0:
|
||||
self.get_logger().warning(
|
||||
f"MVS failed to stop grabbing: 0x{result:08x}"
|
||||
)
|
||||
self._grabbing = False
|
||||
result = self._camera.MV_CC_CloseDevice()
|
||||
if result != 0:
|
||||
self.get_logger().warning(
|
||||
f"MVS failed to close camera: 0x{result:08x}"
|
||||
)
|
||||
self._camera.MV_CC_DestroyHandle()
|
||||
elif self._camera is not None and self._handle_created:
|
||||
self._camera.MV_CC_DestroyHandle()
|
||||
self._camera = None
|
||||
self._handle_created = False
|
||||
self._camera_opened = False
|
||||
if self._sdk_initialized and self._mvs is not None:
|
||||
self._mvs.MvCamera.MV_CC_Finalize()
|
||||
self._sdk_initialized = False
|
||||
|
||||
def destroy_node(self) -> None:
|
||||
self.close()
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
def main(args: list[str] | None = None) -> None:
|
||||
configure_fastdds_large_image_transport()
|
||||
rclpy.init(args=args)
|
||||
node: HikrobotCameraNode | None = None
|
||||
try:
|
||||
node = HikrobotCameraNode()
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
if node is not None:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,4 +1,4 @@
|
||||
"""ROS 2 hardware calibration node for a front-facing RealSense camera."""
|
||||
"""ROS 2 hardware calibration node for a front-facing industrial camera."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
|
||||
+306
@@ -0,0 +1,306 @@
|
||||
"""Chinese, operator-facing diagnostics for three-camera calibration."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any, Mapping
|
||||
|
||||
|
||||
STATE_NAMES_ZH = {
|
||||
"PREFLIGHT": "设备和标签预检",
|
||||
"WAIT_START": "等待开始标定",
|
||||
"RETURN_BASELINE": "正在返回基准姿态",
|
||||
"PREPARE_SWEEP": "正在到达扫描起点",
|
||||
"SWEEP": "正在采集轨迹",
|
||||
"FITTING": "正在拟合轨迹和零位",
|
||||
"VALIDATION_MOVE": "正在移动到随机复测位置",
|
||||
"VALIDATION_CAPTURE": "正在采集随机复测数据",
|
||||
"PAUSED": "标定已暂停",
|
||||
"ABORTED": "标定已终止",
|
||||
"COMPLETE": "标定已完成",
|
||||
}
|
||||
|
||||
VIEW_NAMES_ZH = {
|
||||
"front": "正面",
|
||||
"side": "侧面",
|
||||
"top": "上面",
|
||||
}
|
||||
|
||||
JOINT_NAMES_ZH = {
|
||||
"thumb_cmc_pitch": "拇指CMC俯仰",
|
||||
"thumb_cmc_roll": "拇指CMC滚转",
|
||||
"thumb_mcp": "拇指MCP",
|
||||
"thumb_ip": "拇指IP（被动）",
|
||||
"index_mcp_roll": "食指MCP侧摆",
|
||||
"index_mcp_pitch": "食指MCP屈伸",
|
||||
"index_pip": "食指PIP",
|
||||
"index_dip": "食指DIP（被动）",
|
||||
"thumb_cmc_yaw": "拇指CMC侧摆",
|
||||
}
|
||||
|
||||
|
||||
def _format_u8(value: Any) -> str:
|
||||
if value is None:
|
||||
return "尚无反馈"
|
||||
return f"{float(value):.1f}"
|
||||
|
||||
|
||||
def _task_text(active: Mapping[str, Any]) -> str:
|
||||
if not active:
|
||||
return "尚无活动任务"
|
||||
view = VIEW_NAMES_ZH.get(str(active.get("view", "")), str(active.get("view", "")))
|
||||
if active.get("kind") == "fit_failure":
|
||||
joints = active.get("joints", [])
|
||||
joint_text = "/".join(
|
||||
JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
|
||||
)
|
||||
return (
|
||||
f"{view}机位，{joint_text}拟合检查失败，"
|
||||
f"电机{active.get('motor_index')}，"
|
||||
f"第{active.get('attempt', 1)}次尝试"
|
||||
)
|
||||
if active.get("kind") == "validation":
|
||||
return (
|
||||
f"{view}机位，随机复测，电机{active.get('motor_index')}，"
|
||||
f"目标命令{active.get('command_u8')}"
|
||||
)
|
||||
joints = active.get("joints", [])
|
||||
joint_text = "/".join(
|
||||
JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints
|
||||
)
|
||||
start = active.get("start_u8")
|
||||
target = active.get("target_u8")
|
||||
cycle = active.get("cycle", "?")
|
||||
repetitions = active.get("repetitions", "?")
|
||||
return (
|
||||
f"{view}机位，{joint_text}，电机{active.get('motor_index')}，"
|
||||
f"第{cycle}/{repetitions}轮，{start}→{target}"
|
||||
)
|
||||
|
||||
|
||||
def three_camera_reason_zh(
|
||||
state: str,
|
||||
reason: str,
|
||||
active: Mapping[str, Any],
|
||||
) -> tuple[str, str]:
|
||||
"""Translate a reason code and provide one concrete operator action."""
|
||||
reason = str(reason)
|
||||
sample = active.get("sample", {}) if active else {}
|
||||
missing = [int(value) for value in sample.get("missing_endpoint_u8", [])]
|
||||
sample_range = (
|
||||
f"{_format_u8(sample.get('minimum_u8'))}～"
|
||||
f"{_format_u8(sample.get('maximum_u8'))}"
|
||||
)
|
||||
tolerance = sample.get("endpoint_tolerance_u8", "?")
|
||||
|
||||
if reason == "sweep_missing_endpoint_bin":
|
||||
missing_text = "、".join(str(value) for value in missing) or "0或255"
|
||||
return (
|
||||
f"本方向已有{active.get('valid_frames', 0)}帧同步有效数据，但缺少"
|
||||
f"电机端点{missing_text}附近的有效分箱；采样到的实际电机范围为"
|
||||
f"{sample_range}，端点容差为±{tolerance}。这通常表示电机虽然运动到"
|
||||
"端点，但该时刻没有同时取得有效Tag图像和电机状态。",
|
||||
"确认当前机位所需Tag在整个行程（尤其缺失端点）均可见，然后调用"
|
||||
"/g20_calibration/resume；程序会重新扫描当前方向，不要调用start。",
|
||||
)
|
||||
if reason == "sweep_bins_too_few":
|
||||
return (
|
||||
f"有效电机分箱只有{sample.get('bin_count', 0)}个，要求至少"
|
||||
f"{sample.get('minimum_bin_count', '?')}个；当前采样范围{sample_range}。",
|
||||
"检查Tag连续识别和电机状态频率，修正后调用resume重新扫描当前方向。",
|
||||
)
|
||||
if reason == "sweep_bin_gap_too_large":
|
||||
return (
|
||||
f"轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')}，"
|
||||
f"允许值不超过{sample.get('allowed_maximum_bin_gap', '?')}。",
|
||||
"检查运动中Tag是否间歇丢失；修正遮挡、反光或对焦后调用resume。",
|
||||
)
|
||||
if reason == "synchronised_tag_state_timeout":
|
||||
return (
|
||||
"运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态"
|
||||
"按时间戳配对”的图像帧。",
|
||||
"查看下面活动机位的缺失Tag，确认状态话题仍在更新；修正后调用resume，"
|
||||
"程序会重扫当前方向。",
|
||||
)
|
||||
if reason == "sweep_start_position_timeout":
|
||||
return (
|
||||
f"电机{active.get('motor_index')}未在规定时间到达扫描起点"
|
||||
f"{active.get('start_u8')}，当前实际值{_format_u8(active.get('actual_u8'))}。",
|
||||
"检查CAN、机械手使能和是否存在机械卡阻，确认安全后调用resume。",
|
||||
)
|
||||
if reason == "sweep_timeout":
|
||||
return (
|
||||
"当前方向在规定时间内未完成端点到达、有效帧数和行程覆盖要求。",
|
||||
"检查电机实际值、Tag连续识别和标定速度，修正后调用resume。",
|
||||
)
|
||||
if reason == "return_baseline_timeout":
|
||||
return (
|
||||
"一个或多个标定电机未在规定时间返回基准命令。",
|
||||
"检查机械手状态、CAN和机械卡阻，确认安全后调用resume。",
|
||||
)
|
||||
if reason == "validation_move_timeout":
|
||||
return (
|
||||
"随机复测时电机未在规定时间到达目标命令。",
|
||||
"检查机械手状态和机械卡阻，确认安全后调用resume。",
|
||||
)
|
||||
if reason == "validation_capture_timeout":
|
||||
return (
|
||||
"随机复测位置没有采集到足够的同步有效Tag帧。",
|
||||
"检查当前机位Tag可见性后调用resume。",
|
||||
)
|
||||
if reason == "joint_fit_check_failed":
|
||||
metric_names = {
|
||||
"plane_rms_mm": "平面拟合RMS",
|
||||
"radial_rms_mm": "圆半径拟合RMS",
|
||||
"radius_mm": "拟合半径",
|
||||
"image_radial_rms_px": "二维圆半径拟合RMS",
|
||||
"image_radial_p95_px": "二维圆半径误差P95",
|
||||
"image_radius_px": "二维拟合半径",
|
||||
"arc_deg": "实测圆弧",
|
||||
"monotonic_correction_deg": "最大单调修正",
|
||||
"hysteresis_deg": "最大正反程差",
|
||||
"cycle_travel_range_deg": "三轮行程差",
|
||||
}
|
||||
metric_units = {
|
||||
"plane_rms_mm": "mm",
|
||||
"radial_rms_mm": "mm",
|
||||
"radius_mm": "mm",
|
||||
"image_radial_rms_px": "px",
|
||||
"image_radial_p95_px": "px",
|
||||
"image_radius_px": "px",
|
||||
"arc_deg": "°",
|
||||
"monotonic_correction_deg": "°",
|
||||
"hysteresis_deg": "°",
|
||||
"cycle_travel_range_deg": "°",
|
||||
}
|
||||
details: list[str] = []
|
||||
for failure in active.get("failures", []):
|
||||
joint = JOINT_NAMES_ZH.get(
|
||||
str(failure.get("joint")), str(failure.get("joint"))
|
||||
)
|
||||
metric = str(failure.get("metric", ""))
|
||||
if metric in metric_names:
|
||||
comparison = str(failure.get("comparison", ""))
|
||||
requirement = "不超过" if comparison == "maximum" else "至少"
|
||||
unit = metric_units[metric]
|
||||
detail = (
|
||||
f"{joint}的{metric_names[metric]}为"
|
||||
f"{float(failure.get('actual', 0.0)):.2f}{unit}，"
|
||||
f"要求{requirement}{float(failure.get('limit', 0.0)):.2f}{unit}"
|
||||
)
|
||||
cycle_travel = failure.get("cycle_travel_deg", [])
|
||||
if cycle_travel:
|
||||
detail += "（三轮=" + "/".join(
|
||||
f"{float(value):.2f}°" for value in cycle_travel
|
||||
) + "）"
|
||||
details.append(detail)
|
||||
else:
|
||||
cycle = failure.get("cycle")
|
||||
cycle_text = "" if cycle is None else f"第{cycle}轮"
|
||||
details.append(
|
||||
f"{joint}的{cycle_text}{metric or '轨迹'}拟合失败："
|
||||
f"{failure.get('reason', '未知原因')}"
|
||||
)
|
||||
detail_text = "；".join(details) or "当前关节的轨迹拟合未通过"
|
||||
return (
|
||||
detail_text + "。程序已在当前关节结束后立即停止后续步骤。",
|
||||
"修正Tag位置、遮挡或机械行程后调用"
|
||||
"/g20_calibration/resume；程序只清除当前失败关节的数据"
|
||||
f"并重扫{active.get('directions_to_rescan', 6)}个方向，不要调用start。",
|
||||
)
|
||||
if reason in {"waiting_for_three_cameras_tags_and_sdk", "preflight_lost"}:
|
||||
return (
|
||||
"正在等待三台相机内参、帧率、全部必需Tag以及机械手SDK同时就绪。",
|
||||
"根据下面每个机位的缺失Tag和有效率排查；全部就绪后程序会进入等待开始状态。",
|
||||
)
|
||||
if reason == "call_start":
|
||||
return (
|
||||
"三机位预检已经通过，等待操作员确认开始。",
|
||||
"清空机械手运动范围后调用/g20_calibration/start。",
|
||||
)
|
||||
if reason == "operator_pause":
|
||||
return "操作员主动暂停了标定。", "确认安全后调用/g20_calibration/resume。"
|
||||
if reason == "operator_abort":
|
||||
return "操作员终止了本次标定，程序保持终止时的当前姿态。", "需要重新启动一次新标定。"
|
||||
if reason == "collecting_timestamp_synchronised_tag_centres":
|
||||
return "正在按时间戳配对Tag图像和电机状态并采集当前轨迹。", "无需操作，保持相机、标签和底座不动。"
|
||||
if reason == "capturing_random_validation_pose":
|
||||
return "正在当前随机命令位置采集复测数据。", "无需操作，保持设备不动。"
|
||||
if reason in {"calibration_passed", "calibration_complete"}:
|
||||
return "轨迹、零位和随机复测已经完成。", "检查结果路径和quality.passed。"
|
||||
if reason == "quality_failed":
|
||||
return "标定流程完成，但拟合或随机复测质量没有达到验收阈值。", "检查最终JSON的quality以及启动终端中的拟合日志。"
|
||||
if reason.startswith("prepare_") or state == "PREPARE_SWEEP":
|
||||
return "正在把当前电机移动到本方向的扫描起点并等待稳定。", "无需操作。"
|
||||
if state == "RETURN_BASELINE":
|
||||
return "正在把已使用的标定电机恢复到统一基准命令。", "无需操作。"
|
||||
if state == "FITTING":
|
||||
return "所有扫描已经完成，正在拟合21个关节的轨迹和零位。", "无需操作。"
|
||||
return f"未分类原因码：{reason}", "保留该原因码和启动终端日志用于进一步定位。"
|
||||
|
||||
|
||||
def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str:
|
||||
"""Render the complete operator status; the JSON topic remains unchanged."""
|
||||
state = str(payload.get("state", ""))
|
||||
active = payload.get("active", {})
|
||||
reason_zh, action_zh = three_camera_reason_zh(
|
||||
state, str(payload.get("reason", "")), active
|
||||
)
|
||||
progress = float(payload.get("progress", 0.0))
|
||||
completed = payload.get("completed_sweeps", 0)
|
||||
total = payload.get("total_sweeps", 0)
|
||||
lines = [
|
||||
f"状态：{STATE_NAMES_ZH.get(state, state)}（{state}）",
|
||||
f"原因：{reason_zh}",
|
||||
f"建议：{action_zh}",
|
||||
f"进度：{progress:.1%}（已完成{completed}/{total}个扫描方向）",
|
||||
f"当前任务：{_task_text(active)}",
|
||||
]
|
||||
if state == "RETURN_BASELINE":
|
||||
lines.append(
|
||||
f"正在确认基准姿态：{payload.get('baseline_command_u8', [])}"
|
||||
)
|
||||
if active and active.get("kind") != "fit_failure":
|
||||
sample = active.get("sample", {})
|
||||
motion_progress = active.get("motion_progress")
|
||||
motion_text = (
|
||||
"未知" if motion_progress is None else f"{float(motion_progress):.1%}"
|
||||
)
|
||||
lines.append(
|
||||
"运动采样："
|
||||
f"目标{active.get('target_u8', active.get('command_u8', '?'))}，"
|
||||
f"实际{_format_u8(active.get('actual_u8'))}，"
|
||||
f"本方向{motion_text}，有效帧{active.get('valid_frames', 0)}，"
|
||||
f"实际采样范围{_format_u8(sample.get('minimum_u8'))}～"
|
||||
f"{_format_u8(sample.get('maximum_u8'))}"
|
||||
)
|
||||
auxiliary = active.get("auxiliary_motors", [])
|
||||
if auxiliary:
|
||||
lines.append(
|
||||
"避挡姿态："
|
||||
+ "，".join(
|
||||
f"电机{item.get('motor_index')}目标"
|
||||
f"{item.get('command_u8')}、实际"
|
||||
f"{_format_u8(item.get('actual_u8'))}"
|
||||
for item in auxiliary
|
||||
)
|
||||
)
|
||||
speed = active.get("speed", {})
|
||||
if speed:
|
||||
lines.append(
|
||||
"阶段速度：五指目标"
|
||||
f"{speed.get('commanded_finger_speed')}，SDK报告"
|
||||
f"{speed.get('reported_finger_speed')}"
|
||||
)
|
||||
lines.append("机位：")
|
||||
for name, view in payload.get("views", {}).items():
|
||||
missing = view.get("missing_tag_ids", [])
|
||||
missing_text = "无" if not missing else ",".join(map(str, missing))
|
||||
lines.append(
|
||||
f"- {VIEW_NAMES_ZH.get(str(name), str(name))}："
|
||||
f"{'就绪' if view.get('ready') else '等待'}，"
|
||||
f"{float(view.get('detection_hz', 0.0)):.1f}Hz，"
|
||||
f"全部必需Tag同时有效率{float(view.get('valid_rate', 0.0)):.1%}，"
|
||||
f"当前缺失Tag={missing_text}"
|
||||
)
|
||||
lines.append(f"结果：{payload.get('result_path') or '尚未生成'}")
|
||||
return "\n".join(lines)
|
||||
+1944
File diff suppressed because it is too large
Load Diff
@@ -10,12 +10,11 @@ from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
IncludeLaunchDescription,
|
||||
LogInfo,
|
||||
OpaqueFunction,
|
||||
SetEnvironmentVariable,
|
||||
)
|
||||
from launch.conditions import IfCondition
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import ComposableNodeContainer, Node
|
||||
from launch_ros.descriptions import ComposableNode
|
||||
@@ -48,29 +47,41 @@ def _launch_stack(context):
|
||||
|
||||
tag_config = LaunchConfiguration("tag_config").perform(context)
|
||||
zero_config = LaunchConfiguration("zero_config").perform(context)
|
||||
realsense_config = LaunchConfiguration("realsense_config").perform(context)
|
||||
realsense_launch = (
|
||||
Path(get_package_share_directory("realsense2_camera"))
|
||||
/ "launch"
|
||||
/ "rs_launch.py"
|
||||
)
|
||||
|
||||
camera = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(str(realsense_launch)),
|
||||
camera = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
executable="hikrobot_camera_node",
|
||||
name="hikrobot_camera",
|
||||
namespace="/camera/camera/color",
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
condition=IfCondition(LaunchConfiguration("start_camera")),
|
||||
launch_arguments={
|
||||
"camera_name": "camera",
|
||||
"camera_namespace": "camera",
|
||||
"serial_no": LaunchConfiguration("camera_serial_number"),
|
||||
"config_file": realsense_config,
|
||||
"enable_color": "true",
|
||||
"rgb_camera.color_profile": LaunchConfiguration("color_profile"),
|
||||
"depth_module.color_profile": LaunchConfiguration("color_profile"),
|
||||
"enable_depth": "false",
|
||||
"enable_sync": "false",
|
||||
"align_depth.enable": "false",
|
||||
"diagnostics_period": "1.0",
|
||||
}.items(),
|
||||
parameters=[
|
||||
{
|
||||
"serial_number": LaunchConfiguration("camera_serial_number"),
|
||||
"expected_model": LaunchConfiguration("camera_model"),
|
||||
"camera_name": LaunchConfiguration("camera_name"),
|
||||
"frame_id": LaunchConfiguration("camera_frame_id"),
|
||||
"image_width": ParameterValue(
|
||||
LaunchConfiguration("image_width"), value_type=int
|
||||
),
|
||||
"image_height": ParameterValue(
|
||||
LaunchConfiguration("image_height"), value_type=int
|
||||
),
|
||||
"frame_rate": ParameterValue(
|
||||
LaunchConfiguration("camera_frame_rate"), value_type=float
|
||||
),
|
||||
"exposure_time_us": ParameterValue(
|
||||
LaunchConfiguration("exposure_time_us"), value_type=float
|
||||
),
|
||||
"gain_db": ParameterValue(
|
||||
LaunchConfiguration("gain_db"), value_type=float
|
||||
),
|
||||
"auto_exposure": ParameterValue(
|
||||
LaunchConfiguration("auto_exposure"), value_type=bool
|
||||
),
|
||||
"camera_info_url": LaunchConfiguration("camera_info_url"),
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
raw_topic = "/camera/camera/color/image_raw"
|
||||
@@ -196,10 +207,39 @@ def generate_launch_description() -> LaunchDescription:
|
||||
)
|
||||
return LaunchDescription(
|
||||
[
|
||||
SetEnvironmentVariable(
|
||||
name="FASTRTPS_DEFAULT_PROFILES_FILE",
|
||||
value=str(
|
||||
package_share / "config" / "fastdds_large_images.xml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument("serial_number", default_value="UNSET"),
|
||||
DeclareLaunchArgument("camera_serial_number", default_value=""),
|
||||
DeclareLaunchArgument(
|
||||
"color_profile", default_value="1280x720x30"
|
||||
"camera_serial_number", default_value="DB2163742"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"camera_model", default_value="MV-CS020-10UM"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"camera_name", default_value="hikrobot_front_DB2163742"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"camera_frame_id", default_value="camera_color_optical_frame"
|
||||
),
|
||||
DeclareLaunchArgument("image_width", default_value="1624"),
|
||||
DeclareLaunchArgument("image_height", default_value="1240"),
|
||||
DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
|
||||
DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
|
||||
DeclareLaunchArgument("gain_db", default_value="0.0"),
|
||||
DeclareLaunchArgument("auto_exposure", default_value="false"),
|
||||
DeclareLaunchArgument(
|
||||
"camera_info_url",
|
||||
default_value=str(
|
||||
Path.home()
|
||||
/ ".ros"
|
||||
/ "camera_info"
|
||||
/ "hikrobot_DB2163742.yaml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument("can_interface", default_value="can0"),
|
||||
DeclareLaunchArgument("calibration_speed", default_value="15"),
|
||||
@@ -221,12 +261,6 @@ def generate_launch_description() -> LaunchDescription:
|
||||
package_share / "config" / "cmc_pitch_zero.yaml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"realsense_config",
|
||||
default_value=str(
|
||||
package_share / "config" / "realsense_color_qos.yaml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"tag_config",
|
||||
default_value=str(
|
||||
|
||||
@@ -10,12 +10,11 @@ from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
IncludeLaunchDescription,
|
||||
LogInfo,
|
||||
OpaqueFunction,
|
||||
SetEnvironmentVariable,
|
||||
)
|
||||
from launch.conditions import IfCondition
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import ComposableNodeContainer, Node
|
||||
from launch_ros.descriptions import ComposableNode
|
||||
@@ -50,29 +49,41 @@ def _launch_stack(context):
|
||||
calibration_config = LaunchConfiguration(
|
||||
"calibration_config"
|
||||
).perform(context)
|
||||
realsense_config = LaunchConfiguration("realsense_config").perform(context)
|
||||
realsense_launch = (
|
||||
Path(get_package_share_directory("realsense2_camera"))
|
||||
/ "launch"
|
||||
/ "rs_launch.py"
|
||||
)
|
||||
|
||||
camera = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(str(realsense_launch)),
|
||||
camera = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
executable="hikrobot_camera_node",
|
||||
name="hikrobot_camera",
|
||||
namespace="/camera/camera/color",
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
condition=IfCondition(LaunchConfiguration("start_camera")),
|
||||
launch_arguments={
|
||||
"camera_name": "camera",
|
||||
"camera_namespace": "camera",
|
||||
"serial_no": LaunchConfiguration("camera_serial_number"),
|
||||
"config_file": realsense_config,
|
||||
"enable_color": "true",
|
||||
"rgb_camera.color_profile": LaunchConfiguration("color_profile"),
|
||||
"depth_module.color_profile": LaunchConfiguration("color_profile"),
|
||||
"enable_depth": "false",
|
||||
"enable_sync": "false",
|
||||
"align_depth.enable": "false",
|
||||
"diagnostics_period": "1.0",
|
||||
}.items(),
|
||||
parameters=[
|
||||
{
|
||||
"serial_number": LaunchConfiguration("camera_serial_number"),
|
||||
"expected_model": LaunchConfiguration("camera_model"),
|
||||
"camera_name": LaunchConfiguration("camera_name"),
|
||||
"frame_id": LaunchConfiguration("camera_frame_id"),
|
||||
"image_width": ParameterValue(
|
||||
LaunchConfiguration("image_width"), value_type=int
|
||||
),
|
||||
"image_height": ParameterValue(
|
||||
LaunchConfiguration("image_height"), value_type=int
|
||||
),
|
||||
"frame_rate": ParameterValue(
|
||||
LaunchConfiguration("camera_frame_rate"), value_type=float
|
||||
),
|
||||
"exposure_time_us": ParameterValue(
|
||||
LaunchConfiguration("exposure_time_us"), value_type=float
|
||||
),
|
||||
"gain_db": ParameterValue(
|
||||
LaunchConfiguration("gain_db"), value_type=float
|
||||
),
|
||||
"auto_exposure": ParameterValue(
|
||||
LaunchConfiguration("auto_exposure"), value_type=bool
|
||||
),
|
||||
"camera_info_url": LaunchConfiguration("camera_info_url"),
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
raw_topic = "/camera/camera/color/image_raw"
|
||||
@@ -198,10 +209,39 @@ def generate_launch_description() -> LaunchDescription:
|
||||
)
|
||||
return LaunchDescription(
|
||||
[
|
||||
SetEnvironmentVariable(
|
||||
name="FASTRTPS_DEFAULT_PROFILES_FILE",
|
||||
value=str(
|
||||
package_share / "config" / "fastdds_large_images.xml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument("serial_number", default_value="UNSET"),
|
||||
DeclareLaunchArgument("camera_serial_number", default_value=""),
|
||||
DeclareLaunchArgument(
|
||||
"color_profile", default_value="1280x720x30"
|
||||
"camera_serial_number", default_value="DB2163742"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"camera_model", default_value="MV-CS020-10UM"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"camera_name", default_value="hikrobot_front_DB2163742"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"camera_frame_id", default_value="camera_color_optical_frame"
|
||||
),
|
||||
DeclareLaunchArgument("image_width", default_value="1624"),
|
||||
DeclareLaunchArgument("image_height", default_value="1240"),
|
||||
DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
|
||||
DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
|
||||
DeclareLaunchArgument("gain_db", default_value="0.0"),
|
||||
DeclareLaunchArgument("auto_exposure", default_value="false"),
|
||||
DeclareLaunchArgument(
|
||||
"camera_info_url",
|
||||
default_value=str(
|
||||
Path.home()
|
||||
/ ".ros"
|
||||
/ "camera_info"
|
||||
/ "hikrobot_DB2163742.yaml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument("can_interface", default_value="can0"),
|
||||
DeclareLaunchArgument("calibration_speed", default_value="15"),
|
||||
@@ -225,12 +265,6 @@ def generate_launch_description() -> LaunchDescription:
|
||||
/ "cmc_roll_zero_travel.yaml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"realsense_config",
|
||||
default_value=str(
|
||||
package_share / "config" / "realsense_color_qos.yaml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"tag_config",
|
||||
default_value=str(
|
||||
|
||||
@@ -11,12 +11,11 @@ from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
ExecuteProcess,
|
||||
IncludeLaunchDescription,
|
||||
LogInfo,
|
||||
OpaqueFunction,
|
||||
SetEnvironmentVariable,
|
||||
)
|
||||
from launch.conditions import IfCondition
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import ComposableNodeContainer, Node
|
||||
from launch_ros.descriptions import ComposableNode
|
||||
@@ -48,7 +47,6 @@ def _launch_stack(context):
|
||||
|
||||
calibration_config = LaunchConfiguration("calibration_config").perform(context)
|
||||
tag_config = LaunchConfiguration("tag_config").perform(context)
|
||||
realsense_config = LaunchConfiguration("realsense_config").perform(context)
|
||||
use_roi_text = LaunchConfiguration("use_roi").perform(context).strip().lower()
|
||||
if use_roi_text not in {"true", "false"}:
|
||||
raise RuntimeError("use_roi must be true or false")
|
||||
@@ -66,47 +64,58 @@ def _launch_stack(context):
|
||||
if roi_values["roi_width"] <= 0 or roi_values["roi_height"] <= 0:
|
||||
raise RuntimeError("roi_width and roi_height must be positive")
|
||||
|
||||
color_profile = LaunchConfiguration("color_profile").perform(context)
|
||||
profile_match = re.fullmatch(r"(\d+)x(\d+)x(\d+)", color_profile)
|
||||
if use_roi and profile_match is not None:
|
||||
image_width = int(profile_match.group(1))
|
||||
image_height = int(profile_match.group(2))
|
||||
try:
|
||||
image_width = int(LaunchConfiguration("image_width").perform(context))
|
||||
image_height = int(LaunchConfiguration("image_height").perform(context))
|
||||
except ValueError as error:
|
||||
raise RuntimeError("image_width and image_height must be integers") from error
|
||||
if image_width <= 0 or image_height <= 0:
|
||||
raise RuntimeError("image_width and image_height must be positive")
|
||||
if use_roi:
|
||||
if (
|
||||
roi_values["roi_x"] + roi_values["roi_width"] > image_width
|
||||
or roi_values["roi_y"] + roi_values["roi_height"] > image_height
|
||||
):
|
||||
raise RuntimeError(
|
||||
"ROI lies outside color_profile "
|
||||
"ROI lies outside camera image "
|
||||
f"{image_width}x{image_height}"
|
||||
)
|
||||
|
||||
realsense_launch = Path(
|
||||
get_package_share_directory("realsense2_camera")
|
||||
) / "launch" / "rs_launch.py"
|
||||
|
||||
camera = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(str(realsense_launch)),
|
||||
camera = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
executable="hikrobot_camera_node",
|
||||
name="hikrobot_camera",
|
||||
namespace="/camera/camera/color",
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
condition=IfCondition(LaunchConfiguration("start_camera")),
|
||||
launch_arguments={
|
||||
"camera_name": "camera",
|
||||
"camera_namespace": "camera",
|
||||
"serial_no": LaunchConfiguration("camera_serial_number"),
|
||||
"config_file": realsense_config,
|
||||
"enable_color": "true",
|
||||
# D405 exposes color from the stereo/depth module. Keep the RGB
|
||||
# camera argument as well so the same launch file also works with
|
||||
# D435/D455-class devices.
|
||||
"rgb_camera.color_profile": LaunchConfiguration("color_profile"),
|
||||
"depth_module.color_profile": LaunchConfiguration("color_profile"),
|
||||
"enable_depth": LaunchConfiguration("enable_depth"),
|
||||
"depth_module.depth_profile": LaunchConfiguration("depth_profile"),
|
||||
# Depth is archival/diagnostic only. Synchronising and aligning it
|
||||
# adds significant D405 processing latency without affecting the
|
||||
# relative AprilTag rotation calculation.
|
||||
"enable_sync": "false",
|
||||
"align_depth.enable": "false",
|
||||
"diagnostics_period": "1.0",
|
||||
}.items(),
|
||||
parameters=[
|
||||
{
|
||||
"serial_number": LaunchConfiguration("camera_serial_number"),
|
||||
"expected_model": LaunchConfiguration("camera_model"),
|
||||
"camera_name": LaunchConfiguration("camera_name"),
|
||||
"frame_id": LaunchConfiguration("camera_frame_id"),
|
||||
"image_width": ParameterValue(
|
||||
LaunchConfiguration("image_width"), value_type=int
|
||||
),
|
||||
"image_height": ParameterValue(
|
||||
LaunchConfiguration("image_height"), value_type=int
|
||||
),
|
||||
"frame_rate": ParameterValue(
|
||||
LaunchConfiguration("camera_frame_rate"), value_type=float
|
||||
),
|
||||
"exposure_time_us": ParameterValue(
|
||||
LaunchConfiguration("exposure_time_us"), value_type=float
|
||||
),
|
||||
"gain_db": ParameterValue(
|
||||
LaunchConfiguration("gain_db"), value_type=float
|
||||
),
|
||||
"auto_exposure": ParameterValue(
|
||||
LaunchConfiguration("auto_exposure"), value_type=bool
|
||||
),
|
||||
"camera_info_url": LaunchConfiguration("camera_info_url"),
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
vision_components = []
|
||||
@@ -275,7 +284,6 @@ def _launch_stack(context):
|
||||
str(bag_path),
|
||||
processed_image_raw_topic,
|
||||
processed_camera_info_topic,
|
||||
"/camera/camera/depth/image_rect_raw",
|
||||
"/apriltag/detections",
|
||||
"/tf",
|
||||
"/g20/cb_left_hand_control_cmd",
|
||||
@@ -312,15 +320,40 @@ def generate_launch_description() -> LaunchDescription:
|
||||
default_output = str(Path.cwd() / "calibration_output")
|
||||
return LaunchDescription(
|
||||
[
|
||||
SetEnvironmentVariable(
|
||||
name="FASTRTPS_DEFAULT_PROFILES_FILE",
|
||||
value=str(
|
||||
package_share / "config" / "fastdds_large_images.xml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument("serial_number", default_value="UNSET"),
|
||||
DeclareLaunchArgument("camera_serial_number", default_value=""),
|
||||
DeclareLaunchArgument(
|
||||
"color_profile", default_value="1280x720x30"
|
||||
"camera_serial_number", default_value="DB2163742"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"depth_profile", default_value="640x480x30"
|
||||
"camera_model", default_value="MV-CS020-10UM"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"camera_name", default_value="hikrobot_front_DB2163742"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"camera_frame_id", default_value="camera_color_optical_frame"
|
||||
),
|
||||
DeclareLaunchArgument("image_width", default_value="1624"),
|
||||
DeclareLaunchArgument("image_height", default_value="1240"),
|
||||
DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
|
||||
DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
|
||||
DeclareLaunchArgument("gain_db", default_value="0.0"),
|
||||
DeclareLaunchArgument("auto_exposure", default_value="false"),
|
||||
DeclareLaunchArgument(
|
||||
"camera_info_url",
|
||||
default_value=str(
|
||||
Path.home()
|
||||
/ ".ros"
|
||||
/ "camera_info"
|
||||
/ "hikrobot_DB2163742.yaml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument("enable_depth", default_value="false"),
|
||||
DeclareLaunchArgument(
|
||||
"publish_debug_image", default_value="false"
|
||||
),
|
||||
@@ -349,12 +382,6 @@ def generate_launch_description() -> LaunchDescription:
|
||||
"calibration_config",
|
||||
default_value=str(package_share / "config" / "calibration.yaml"),
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"realsense_config",
|
||||
default_value=str(
|
||||
package_share / "config" / "realsense_color_qos.yaml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"tag_config",
|
||||
default_value=str(package_share / "config" / "front_tags.yaml"),
|
||||
|
||||
@@ -0,0 +1,341 @@
|
||||
"""Launch three Hikrobot views and one complete-G20 calibration owner."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from datetime import datetime
|
||||
from pathlib import Path
|
||||
import re
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
ExecuteProcess,
|
||||
LogInfo,
|
||||
OpaqueFunction,
|
||||
SetEnvironmentVariable,
|
||||
)
|
||||
from launch.conditions import IfCondition
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import ComposableNodeContainer, Node
|
||||
from launch_ros.descriptions import ComposableNode
|
||||
from launch_ros.parameter_descriptions import ParameterValue
|
||||
|
||||
|
||||
VIEWS = ("front", "side", "top")
|
||||
|
||||
|
||||
def _launch_stack(context):
|
||||
hand_serial = LaunchConfiguration("serial_number").perform(context)
|
||||
if (
|
||||
not hand_serial
|
||||
or hand_serial == "UNSET"
|
||||
or re.fullmatch(r"[A-Za-z0-9_.-]+", hand_serial) is None
|
||||
or hand_serial in {".", ".."}
|
||||
):
|
||||
raise RuntimeError("serial_number must be a safe non-empty hand serial")
|
||||
|
||||
requested_session = LaunchConfiguration("session_dir").perform(context)
|
||||
output_root = Path(
|
||||
LaunchConfiguration("output_root").perform(context)
|
||||
).expanduser().resolve()
|
||||
if requested_session:
|
||||
session_dir = Path(requested_session).expanduser().resolve()
|
||||
else:
|
||||
session_dir = (
|
||||
output_root
|
||||
/ hand_serial
|
||||
/ datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||
)
|
||||
session_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
camera_serials = {
|
||||
view: LaunchConfiguration(f"{view}_camera_serial").perform(context)
|
||||
for view in VIEWS
|
||||
}
|
||||
if any(not serial for serial in camera_serials.values()):
|
||||
raise RuntimeError("all three camera serial numbers are required")
|
||||
if len(set(camera_serials.values())) != 3:
|
||||
raise RuntimeError("front/side/top camera serial numbers must be unique")
|
||||
|
||||
cameras = []
|
||||
components = []
|
||||
raw_topics = []
|
||||
info_topics = []
|
||||
detection_topics = []
|
||||
for view in VIEWS:
|
||||
namespace = f"/g20_calibration/{view}/camera"
|
||||
raw_topic = f"{namespace}/image_raw"
|
||||
info_topic = f"{namespace}/camera_info"
|
||||
rect_topic = f"{namespace}/image_rect"
|
||||
detector_namespace = f"/g20_calibration/{view}/apriltag"
|
||||
detection_topic = f"{detector_namespace}/detections"
|
||||
raw_topics.append(raw_topic)
|
||||
info_topics.append(info_topic)
|
||||
detection_topics.append(detection_topic)
|
||||
cameras.append(
|
||||
Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
executable="hikrobot_camera_node",
|
||||
name="hikrobot_camera",
|
||||
namespace=namespace,
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
condition=IfCondition(LaunchConfiguration("start_cameras")),
|
||||
parameters=[
|
||||
{
|
||||
"serial_number": LaunchConfiguration(
|
||||
f"{view}_camera_serial"
|
||||
),
|
||||
"expected_model": LaunchConfiguration("camera_model"),
|
||||
"camera_name": LaunchConfiguration(
|
||||
f"{view}_camera_name"
|
||||
),
|
||||
"frame_id": f"g20_calibration_{view}_optical_frame",
|
||||
"image_width": 1624,
|
||||
"image_height": 1240,
|
||||
"frame_rate": ParameterValue(
|
||||
LaunchConfiguration("camera_frame_rate"),
|
||||
value_type=float,
|
||||
),
|
||||
"exposure_time_us": ParameterValue(
|
||||
LaunchConfiguration("exposure_time_us"),
|
||||
value_type=float,
|
||||
),
|
||||
"gain_db": ParameterValue(
|
||||
LaunchConfiguration("gain_db"), value_type=float
|
||||
),
|
||||
"auto_exposure": ParameterValue(
|
||||
LaunchConfiguration("auto_exposure"), value_type=bool
|
||||
),
|
||||
"camera_info_url": LaunchConfiguration(
|
||||
f"{view}_camera_info_url"
|
||||
),
|
||||
}
|
||||
],
|
||||
)
|
||||
)
|
||||
components.extend(
|
||||
[
|
||||
ComposableNode(
|
||||
package="image_proc",
|
||||
plugin="image_proc::RectifyNode",
|
||||
name=f"rectify_{view}",
|
||||
namespace=namespace,
|
||||
remappings=[
|
||||
("image", raw_topic),
|
||||
("camera_info", info_topic),
|
||||
("image_rect", rect_topic),
|
||||
],
|
||||
parameters=[{"queue_size": 1}],
|
||||
extra_arguments=[{"use_intra_process_comms": True}],
|
||||
),
|
||||
ComposableNode(
|
||||
package="apriltag_ros",
|
||||
plugin="AprilTagNode",
|
||||
name="apriltag",
|
||||
namespace=detector_namespace,
|
||||
parameters=[
|
||||
LaunchConfiguration("tag_config"),
|
||||
{
|
||||
"detector.decimate": ParameterValue(
|
||||
LaunchConfiguration("apriltag_decimate"),
|
||||
value_type=float,
|
||||
)
|
||||
},
|
||||
],
|
||||
remappings=[
|
||||
("image_rect", rect_topic),
|
||||
("camera_info", info_topic),
|
||||
],
|
||||
extra_arguments=[{"use_intra_process_comms": True}],
|
||||
),
|
||||
]
|
||||
)
|
||||
|
||||
vision = ComposableNodeContainer(
|
||||
name="g20_three_camera_vision",
|
||||
namespace="/",
|
||||
package="rclcpp_components",
|
||||
executable="component_container_mt",
|
||||
composable_node_descriptions=components,
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
)
|
||||
sdk = Node(
|
||||
package="linker_hand_ros2_sdk",
|
||||
executable="linker_hand_sdk",
|
||||
name="linker_hand_sdk",
|
||||
output="screen",
|
||||
condition=IfCondition(LaunchConfiguration("start_sdk")),
|
||||
parameters=[
|
||||
{
|
||||
"hand_type": "left",
|
||||
"hand_joint": "G20",
|
||||
"can": LaunchConfiguration("can_interface"),
|
||||
"modbus": "None",
|
||||
"topic_prefix": "/g20",
|
||||
"move_on_startup": False,
|
||||
"startup_speed": ParameterValue(
|
||||
LaunchConfiguration("calibration_speed"), value_type=int
|
||||
),
|
||||
"startup_torque": 80,
|
||||
"state_poll_rate": 10.0,
|
||||
"repeat_position_commands": False,
|
||||
"is_touch": False,
|
||||
}
|
||||
],
|
||||
)
|
||||
calibration = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
executable="three_camera_calibration_node",
|
||||
name="g20_calibration",
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
parameters=[
|
||||
LaunchConfiguration("calibration_config"),
|
||||
{
|
||||
"serial_number": hand_serial,
|
||||
"session_dir": str(session_dir),
|
||||
"commands_enabled": ParameterValue(
|
||||
LaunchConfiguration("commands_enabled"), value_type=bool
|
||||
),
|
||||
"normal_calibration_speed": ParameterValue(
|
||||
LaunchConfiguration("calibration_speed"), value_type=int
|
||||
),
|
||||
"index_roll_calibration_speed": ParameterValue(
|
||||
LaunchConfiguration("index_roll_calibration_speed"),
|
||||
value_type=int,
|
||||
),
|
||||
"index_flex_calibration_speed": ParameterValue(
|
||||
LaunchConfiguration("index_flex_calibration_speed"),
|
||||
value_type=int,
|
||||
),
|
||||
"validation_enabled": ParameterValue(
|
||||
LaunchConfiguration("validation_enabled"), value_type=bool
|
||||
),
|
||||
},
|
||||
],
|
||||
)
|
||||
bag = ExecuteProcess(
|
||||
condition=IfCondition(LaunchConfiguration("record_bag")),
|
||||
cmd=[
|
||||
"ros2",
|
||||
"bag",
|
||||
"record",
|
||||
"--storage",
|
||||
"mcap",
|
||||
"--storage-preset-profile",
|
||||
"zstd_fast",
|
||||
"--max-bag-size",
|
||||
"10737418240",
|
||||
"--output",
|
||||
str(session_dir / "rosbag"),
|
||||
*raw_topics,
|
||||
*info_topics,
|
||||
*detection_topics,
|
||||
"/g20/cb_left_hand_control_cmd",
|
||||
"/g20/cb_left_hand_state",
|
||||
"/g20/cb_left_hand_info",
|
||||
"/g20_calibration/status",
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
return [
|
||||
LogInfo(msg=f"G20 three-camera session: {session_dir}"),
|
||||
LogInfo(
|
||||
msg=(
|
||||
"Camera mapping: front="
|
||||
f"{camera_serials['front']} side={camera_serials['side']} "
|
||||
f"top={camera_serials['top']}"
|
||||
)
|
||||
),
|
||||
*cameras,
|
||||
vision,
|
||||
sdk,
|
||||
calibration,
|
||||
bag,
|
||||
]
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
package_share = Path(
|
||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
||||
)
|
||||
info_root = Path.home() / ".ros" / "camera_info"
|
||||
return LaunchDescription(
|
||||
[
|
||||
SetEnvironmentVariable(
|
||||
name="FASTRTPS_DEFAULT_PROFILES_FILE",
|
||||
value=str(package_share / "config" / "fastdds_large_images.xml"),
|
||||
),
|
||||
DeclareLaunchArgument("serial_number", default_value="UNSET"),
|
||||
DeclareLaunchArgument(
|
||||
"front_camera_serial", default_value="DB2163742"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"side_camera_serial", default_value="DB2163749"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"top_camera_serial", default_value="DB2163739"
|
||||
),
|
||||
DeclareLaunchArgument("camera_model", default_value="MV-CS020-10U"),
|
||||
DeclareLaunchArgument(
|
||||
"front_camera_name", default_value="hikrobot_front_DB2163742"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"side_camera_name", default_value="hikrobot_side_DB2163749"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"top_camera_name", default_value="hikrobot_top_DB2163739"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"front_camera_info_url",
|
||||
default_value=str(info_root / "hikrobot_DB2163742.yaml"),
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"side_camera_info_url",
|
||||
default_value=str(info_root / "hikrobot_DB2163749.yaml"),
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"top_camera_info_url",
|
||||
default_value=str(info_root / "hikrobot_DB2163739.yaml"),
|
||||
),
|
||||
DeclareLaunchArgument("camera_frame_rate", default_value="30.0"),
|
||||
DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
|
||||
DeclareLaunchArgument("gain_db", default_value="0.0"),
|
||||
DeclareLaunchArgument("auto_exposure", default_value="false"),
|
||||
DeclareLaunchArgument("apriltag_decimate", default_value="1.5"),
|
||||
DeclareLaunchArgument("can_interface", default_value="can0"),
|
||||
DeclareLaunchArgument("calibration_speed", default_value="15"),
|
||||
DeclareLaunchArgument(
|
||||
"index_roll_calibration_speed", default_value="5"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"index_flex_calibration_speed", default_value="10"
|
||||
),
|
||||
DeclareLaunchArgument("validation_enabled", default_value="false"),
|
||||
DeclareLaunchArgument("commands_enabled", default_value="true"),
|
||||
DeclareLaunchArgument("start_cameras", default_value="true"),
|
||||
DeclareLaunchArgument("start_sdk", default_value="true"),
|
||||
DeclareLaunchArgument("record_bag", default_value="false"),
|
||||
DeclareLaunchArgument(
|
||||
"output_root",
|
||||
default_value=str(Path.cwd() / "calibration_output"),
|
||||
),
|
||||
DeclareLaunchArgument("session_dir", default_value=""),
|
||||
DeclareLaunchArgument(
|
||||
"calibration_config",
|
||||
default_value=str(
|
||||
package_share / "config" / "three_camera_calibration.yaml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"tag_config",
|
||||
default_value=str(
|
||||
package_share / "config" / "three_camera_tags.yaml"
|
||||
),
|
||||
),
|
||||
OpaqueFunction(function=_launch_stack),
|
||||
]
|
||||
)
|
||||
@@ -3,7 +3,7 @@
|
||||
<package format="3">
|
||||
<name>g20_thumb_apriltag_calibration</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Front RealSense AprilTag calibration for the left G20 thumb.</description>
|
||||
<description>Three-view Hikrobot AprilTag calibration for the complete left G20 hand.</description>
|
||||
<maintainer email="support@linker-robotics.com">lxp</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
@@ -17,7 +17,6 @@
|
||||
<exec_depend>linker_hand_ros2_sdk</exec_depend>
|
||||
<exec_depend>rclcpp_components</exec_depend>
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>realsense2_camera</exec_depend>
|
||||
<exec_depend>rosbag2</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
@@ -26,6 +25,7 @@
|
||||
<exec_depend>python3-numpy</exec_depend>
|
||||
<exec_depend>python3-opencv</exec_depend>
|
||||
<exec_depend>python3-scipy</exec_depend>
|
||||
<exec_depend>python3-yaml</exec_depend>
|
||||
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
|
||||
@@ -15,18 +15,25 @@ setup(
|
||||
["resource/" + package_name],
|
||||
),
|
||||
("share/" + package_name, ["package.xml", "README.md"]),
|
||||
("share/" + package_name + "/config", glob("config/*.yaml")),
|
||||
(
|
||||
"share/" + package_name + "/config",
|
||||
glob("config/*.yaml") + glob("config/*.xml"),
|
||||
),
|
||||
("share/" + package_name + "/launch", glob("launch/*.launch.py")),
|
||||
],
|
||||
install_requires=["setuptools", "numpy", "scipy"],
|
||||
install_requires=["setuptools", "numpy", "scipy", "PyYAML"],
|
||||
tests_require=["pytest"],
|
||||
zip_safe=True,
|
||||
maintainer="lxp",
|
||||
maintainer_email="support@linker-robotics.com",
|
||||
description="Front RealSense AprilTag calibration for the left G20 thumb",
|
||||
description="Three-view Hikrobot AprilTag calibration for the complete left G20 hand",
|
||||
license="MIT",
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
(
|
||||
"hikrobot_camera_node = "
|
||||
"g20_thumb_apriltag_calibration.hikrobot_camera:main"
|
||||
),
|
||||
"calibration_node = g20_thumb_apriltag_calibration.node:main",
|
||||
(
|
||||
"cmc_pitch_zero_node = "
|
||||
@@ -36,6 +43,14 @@ setup(
|
||||
"cmc_roll_calibration_node = "
|
||||
"g20_thumb_apriltag_calibration.zero_node:main"
|
||||
),
|
||||
(
|
||||
"three_camera_calibration_node = "
|
||||
"g20_thumb_apriltag_calibration.three_camera_node:main"
|
||||
),
|
||||
(
|
||||
"camera_alignment_view = "
|
||||
"g20_thumb_apriltag_calibration.alignment_view:main"
|
||||
),
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
@@ -15,11 +15,60 @@ from g20_thumb_apriltag_calibration.acquisition import (
|
||||
aggregate_observations,
|
||||
aggregate_sweep_observations,
|
||||
interpolate_state_u8,
|
||||
required_resume_views,
|
||||
tag_quality_is_valid,
|
||||
update_pnp_reset_watchdog,
|
||||
)
|
||||
from g20_thumb_apriltag_calibration.core import PAIR_NAMES
|
||||
|
||||
|
||||
def test_pnp_watchdog_resets_after_one_continuous_invalid_second() -> None:
|
||||
since, reset = update_pnp_reset_watchdog(
|
||||
detection_good=True,
|
||||
pnp_valid=False,
|
||||
now=10.0,
|
||||
invalid_since=None,
|
||||
reset_after_seconds=1.0,
|
||||
)
|
||||
assert since == 10.0
|
||||
assert reset is False
|
||||
|
||||
since, reset = update_pnp_reset_watchdog(
|
||||
detection_good=True,
|
||||
pnp_valid=False,
|
||||
now=11.01,
|
||||
invalid_since=since,
|
||||
reset_after_seconds=1.0,
|
||||
)
|
||||
assert since == 11.01
|
||||
assert reset is True
|
||||
|
||||
|
||||
def test_pnp_watchdog_clears_on_valid_pose_or_bad_detection() -> None:
|
||||
assert update_pnp_reset_watchdog(
|
||||
detection_good=True,
|
||||
pnp_valid=True,
|
||||
now=11.0,
|
||||
invalid_since=10.0,
|
||||
reset_after_seconds=1.0,
|
||||
) == (None, False)
|
||||
assert update_pnp_reset_watchdog(
|
||||
detection_good=False,
|
||||
pnp_valid=False,
|
||||
now=11.0,
|
||||
invalid_since=10.0,
|
||||
reset_after_seconds=1.0,
|
||||
) == (None, False)
|
||||
|
||||
|
||||
def test_resume_requires_only_the_active_view() -> None:
|
||||
assert required_resume_views("top") == ("top",)
|
||||
assert required_resume_views("front") == ("front",)
|
||||
assert required_resume_views(None) == ("front", "side", "top")
|
||||
with pytest.raises(ValueError, match="unknown"):
|
||||
required_resume_views("rear")
|
||||
|
||||
|
||||
def _observation(index: int, angle_rad: float = 0.0) -> Observation:
|
||||
quaternion = tuple(
|
||||
float(value)
|
||||
|
||||
@@ -0,0 +1,39 @@
|
||||
"""Tests for the independent physical-line alignment overlay."""
|
||||
|
||||
import pytest
|
||||
|
||||
from g20_thumb_apriltag_calibration.alignment_view import (
|
||||
summarize_alignment_measurements,
|
||||
)
|
||||
|
||||
|
||||
def _measurement(angle: float, offset: float, y: float) -> dict:
|
||||
return {
|
||||
"line_xyxy_px": [0.0, y, 100.0, y - angle * 100.0],
|
||||
"angle_rad": angle,
|
||||
"vertical_offset_px": offset,
|
||||
}
|
||||
|
||||
|
||||
def test_line_summary_smooths_only_physical_line_measurements() -> None:
|
||||
"""The overlay smooths scene lines without any Tag orientation input."""
|
||||
result = summarize_alignment_measurements(
|
||||
[
|
||||
_measurement(-0.02, -4.0, 80.0),
|
||||
None,
|
||||
_measurement(0.00, 0.0, 82.0),
|
||||
_measurement(0.02, 4.0, 84.0),
|
||||
]
|
||||
)
|
||||
|
||||
assert result is not None
|
||||
assert result["angle_rad"] == pytest.approx(0.0)
|
||||
assert result["vertical_offset_px"] == pytest.approx(0.0)
|
||||
assert result["line_xyxy_px"] == pytest.approx([0.0, 82.0, 100.0, 82.0])
|
||||
assert result["detected_frames"] == 3
|
||||
assert result["window_frames"] == 4
|
||||
|
||||
|
||||
def test_line_summary_returns_none_without_scene_line() -> None:
|
||||
"""No blue line is fabricated when the scene has no valid long edge."""
|
||||
assert summarize_alignment_measurements([None, None]) is None
|
||||
@@ -1,4 +1,5 @@
|
||||
from pathlib import Path
|
||||
from xml.etree import ElementTree
|
||||
|
||||
import yaml
|
||||
|
||||
@@ -6,6 +7,24 @@ import yaml
|
||||
PACKAGE_ROOT = Path(__file__).resolve().parents[1]
|
||||
|
||||
|
||||
def test_fastdds_profile_has_capacity_for_full_resolution_images() -> None:
|
||||
root = ElementTree.parse(
|
||||
PACKAGE_ROOT / "config" / "fastdds_large_images.xml"
|
||||
).getroot()
|
||||
namespace = {"dds": "http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles"}
|
||||
profiles = root.find("dds:profiles", namespace)
|
||||
assert profiles is not None
|
||||
segment = profiles.find(
|
||||
".//dds:transport_descriptor[dds:type='SHM']/dds:segment_size",
|
||||
namespace,
|
||||
)
|
||||
assert segment is not None
|
||||
assert int(segment.text) >= 64 * 1024 * 1024
|
||||
participant = profiles.find("dds:participant", namespace)
|
||||
assert participant is not None
|
||||
assert participant.attrib["is_default_profile"] == "true"
|
||||
|
||||
|
||||
def test_front_tag_parameters_match_namespaced_detector() -> None:
|
||||
config = yaml.safe_load(
|
||||
(PACKAGE_ROOT / "config" / "front_tags.yaml").read_text()
|
||||
@@ -24,6 +43,85 @@ def test_front_tag_parameters_match_namespaced_detector() -> None:
|
||||
assert detector["detector"]["debug"] is False
|
||||
|
||||
|
||||
def test_three_camera_tag_ids_and_topics_are_disjoint() -> None:
|
||||
tags = yaml.safe_load(
|
||||
(PACKAGE_ROOT / "config" / "three_camera_tags.yaml").read_text()
|
||||
)
|
||||
expected = {
|
||||
"front": [0, 1, 2, 3, 10],
|
||||
"side": [4, 5, 6, 7],
|
||||
"top": [8, 9],
|
||||
}
|
||||
all_ids = set()
|
||||
for view, ids in expected.items():
|
||||
key = f"/g20_calibration/{view}/apriltag/apriltag"
|
||||
parameters = tags[key]["ros__parameters"]
|
||||
assert parameters["tag"]["ids"] == ids
|
||||
assert parameters["qos_profile"] == "sensor_data"
|
||||
assert parameters["detector"]["decimate"] == 1.5
|
||||
all_ids.update(ids)
|
||||
assert all_ids == set(range(11))
|
||||
|
||||
|
||||
def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None:
|
||||
config = yaml.safe_load(
|
||||
(PACKAGE_ROOT / "config" / "three_camera_calibration.yaml").read_text()
|
||||
)
|
||||
parameters = config["g20_calibration"]["ros__parameters"]
|
||||
|
||||
assert parameters["baseline_command_u8"] == [
|
||||
255,
|
||||
255,
|
||||
255,
|
||||
255,
|
||||
255,
|
||||
255,
|
||||
127,
|
||||
127,
|
||||
127,
|
||||
127,
|
||||
255,
|
||||
255,
|
||||
255,
|
||||
255,
|
||||
255,
|
||||
255,
|
||||
255,
|
||||
255,
|
||||
255,
|
||||
255,
|
||||
]
|
||||
assert parameters["setting_topic"] == "/g20/cb_hand_setting_cmd"
|
||||
assert parameters["normal_calibration_speed"] == 15
|
||||
assert parameters["index_roll_calibration_speed"] == 5
|
||||
assert parameters["index_flex_calibration_speed"] == 10
|
||||
assert parameters["speed_setting_settle_seconds"] >= 0.2
|
||||
assert parameters["top_pnp_invalid_reset_seconds"] == 1.0
|
||||
assert parameters["repetitions"] == 3
|
||||
assert parameters["validation_enabled"] is False
|
||||
assert parameters["minimum_detection_rate"] == 0.95
|
||||
assert parameters["minimum_detection_hz"] == 15.0
|
||||
assert parameters["minimum_state_span_u8"] >= 240.0
|
||||
assert parameters["minimum_sweep_bins"] >= 32
|
||||
assert parameters["maximum_bin_gap"] <= 16
|
||||
assert parameters["position_timeout_seconds"] >= 20.0
|
||||
assert parameters["zero_maximum_round_difference_deg"] <= 1.0
|
||||
assert parameters["image_trajectory_maximum_radial_rms_px"] <= 2.0
|
||||
assert parameters["image_trajectory_maximum_radial_p95_px"] <= 3.5
|
||||
assert parameters["image_trajectory_minimum_radius_px"] >= 20.0
|
||||
assert parameters["trajectory_maximum_cycle_travel_difference_deg"] <= 3.0
|
||||
assert parameters["passive_maximum_cycle_travel_difference_deg"] <= 10.0
|
||||
assert parameters["passive_maximum_monotonic_correction_deg"] <= 3.0
|
||||
assert parameters["passive_maximum_hysteresis_deg"] <= 7.5
|
||||
for view in ("front", "side", "top"):
|
||||
assert parameters[f"{view}_camera_info_topic"].startswith(
|
||||
f"/g20_calibration/{view}/"
|
||||
)
|
||||
assert parameters[f"{view}_detections_topic"] == (
|
||||
f"/g20_calibration/{view}/apriltag/detections"
|
||||
)
|
||||
|
||||
|
||||
def test_trial_uses_centre_trajectory_and_thirty_pixel_tags() -> None:
|
||||
config = yaml.safe_load(
|
||||
(PACKAGE_ROOT / "config" / "calibration.yaml").read_text()
|
||||
@@ -87,15 +185,6 @@ def test_trial_uses_centre_trajectory_and_thirty_pixel_tags() -> None:
|
||||
assert parameters["continuous_prepare_timeout_seconds"] >= 30.0
|
||||
|
||||
|
||||
def test_realsense_color_and_camera_info_use_matching_qos() -> None:
|
||||
config = yaml.safe_load(
|
||||
(PACKAGE_ROOT / "config" / "realsense_color_qos.yaml").read_text()
|
||||
)
|
||||
|
||||
assert config["color_qos"] == "DEFAULT"
|
||||
assert config["color_info_qos"] == "DEFAULT"
|
||||
|
||||
|
||||
def test_cmc_pitch_zero_config_uses_three_trajectory_circle_rounds() -> None:
|
||||
config = yaml.safe_load(
|
||||
(PACKAGE_ROOT / "config" / "cmc_pitch_zero.yaml").read_text()
|
||||
|
||||
@@ -0,0 +1,335 @@
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from g20_thumb_apriltag_calibration.full_hand import (
|
||||
ACTIVE_JOINTS,
|
||||
IMAGE_TRAJECTORY_JOINTS,
|
||||
JOINT_SPECS,
|
||||
MEASURED_JOINTS,
|
||||
PASSIVE_JOINTS,
|
||||
SPLAY_JOINTS,
|
||||
SWEEP_SPECS,
|
||||
VIEW_TAGS,
|
||||
build_calibration_motion_command,
|
||||
build_calibration_speed_profile,
|
||||
build_compact_payload,
|
||||
build_full_hand_command,
|
||||
center_splay_curve,
|
||||
fit_joint_center_curve,
|
||||
fit_joint_image_curve,
|
||||
fit_measured_joint_curve,
|
||||
fit_projected_zero,
|
||||
measure_joint_observation,
|
||||
validate_compact_payload,
|
||||
)
|
||||
|
||||
|
||||
def _records() -> list[dict[str, object]]:
|
||||
commands = list(range(0, 256, 16))
|
||||
if commands[-1] != 255:
|
||||
commands.append(255)
|
||||
records: list[dict[str, object]] = []
|
||||
centre = np.asarray([0.006, -0.004, 0.012])
|
||||
radius = 0.025
|
||||
image_centre = np.asarray([30.0, -12.0])
|
||||
image_radius = 100.0
|
||||
for cycle in range(3):
|
||||
for direction, sequence in (
|
||||
("decreasing", reversed(commands)),
|
||||
("increasing", commands),
|
||||
):
|
||||
for command in sequence:
|
||||
angle = 0.70 * (255.0 - command) / 255.0
|
||||
point = centre + np.asarray(
|
||||
[radius * math.cos(angle), radius * math.sin(angle), 0.0]
|
||||
)
|
||||
# At command 255 the inward vector points along image +x, so
|
||||
# table_projected_zero_rad is exactly zero.
|
||||
image_point = image_centre + np.asarray(
|
||||
[
|
||||
-image_radius * math.cos(angle),
|
||||
image_radius * math.sin(angle),
|
||||
]
|
||||
)
|
||||
records.append(
|
||||
{
|
||||
"cycle": cycle,
|
||||
"direction": direction,
|
||||
"command_u8": command,
|
||||
"relative_translation_xyz_m": point.tolist(),
|
||||
"image_relative_xy_px": image_point.tolist(),
|
||||
}
|
||||
)
|
||||
return records
|
||||
|
||||
|
||||
def test_joint_layout_covers_16_active_and_5_passive_joints() -> None:
|
||||
assert len(JOINT_SPECS) == 21
|
||||
assert len(ACTIVE_JOINTS) == 16
|
||||
assert len(PASSIVE_JOINTS) == 5
|
||||
assert {tag for tags in VIEW_TAGS.values() for tag in tags.values()} == set(
|
||||
range(11)
|
||||
)
|
||||
assert VIEW_TAGS["front"]["index_roll"] == 10
|
||||
assert VIEW_TAGS["side"] == {
|
||||
"side_base": 4,
|
||||
"index_mcp": 5,
|
||||
"index_pip": 6,
|
||||
"index_dip": 7,
|
||||
}
|
||||
assert VIEW_TAGS["top"] == {"top_base": 8, "thumb_yaw": 9}
|
||||
assert [spec.motor_index for spec in SWEEP_SPECS] == [0, 5, 15, 6, 1, 16, 10]
|
||||
|
||||
|
||||
def test_joint_trajectory_spaces_match_observation_geometry() -> None:
|
||||
assert IMAGE_TRAJECTORY_JOINTS == {
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_cmc_roll",
|
||||
"thumb_mcp",
|
||||
"thumb_ip",
|
||||
"index_mcp_roll",
|
||||
"index_mcp_pitch",
|
||||
"index_pip",
|
||||
}
|
||||
records = _records()
|
||||
for name in IMAGE_TRAJECTORY_JOINTS:
|
||||
fit = fit_measured_joint_curve(name, records)
|
||||
assert fit.circle["space"] == "image_2d"
|
||||
for name in ("index_dip", "thumb_cmc_yaw"):
|
||||
assert "space" not in fit_measured_joint_curve(name, records).circle
|
||||
|
||||
|
||||
def test_full_hand_command_changes_exactly_one_controlled_motor() -> None:
|
||||
result = build_full_hand_command(6, 27)
|
||||
assert result[6] == 27
|
||||
assert result[:6] == [255] * 6
|
||||
with pytest.raises(ValueError, match="controlled"):
|
||||
build_full_hand_command(11, 27)
|
||||
|
||||
|
||||
def test_index_roll_motion_moves_other_three_roll_motors_out_of_view() -> None:
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [127, 127, 127, 127]
|
||||
index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6)
|
||||
|
||||
result = build_calibration_motion_command(index_roll, 27, baseline)
|
||||
|
||||
assert result[6:10] == [27, 0, 0, 0]
|
||||
assert result[:6] == baseline[:6]
|
||||
assert result[10:] == baseline[10:]
|
||||
|
||||
|
||||
def test_non_index_roll_motion_keeps_clearance_motors_at_baseline() -> None:
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [127, 127, 127, 127]
|
||||
thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0)
|
||||
|
||||
result = build_calibration_motion_command(thumb_pitch, 17, baseline)
|
||||
|
||||
assert result[0] == 17
|
||||
assert result[6:10] == [127, 127, 127, 127]
|
||||
|
||||
|
||||
def test_thumb_yaw_motion_holds_thumb_roll_at_camera_clearance_pose() -> None:
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [127, 127, 127, 127]
|
||||
thumb_yaw = next(spec for spec in SWEEP_SPECS if spec.motor_index == 10)
|
||||
|
||||
result = build_calibration_motion_command(thumb_yaw, 27, baseline)
|
||||
|
||||
assert result[5] == 145
|
||||
assert result[10] == 27
|
||||
assert result[:5] == baseline[:5]
|
||||
assert result[6:10] == baseline[6:10]
|
||||
assert result[11:] == baseline[11:]
|
||||
|
||||
|
||||
def test_only_index_roll_uses_the_slow_index_finger_speed() -> None:
|
||||
index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6)
|
||||
index_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 1)
|
||||
|
||||
assert build_calibration_speed_profile(
|
||||
index_roll,
|
||||
normal_speed=15,
|
||||
index_roll_speed=5,
|
||||
index_flex_speed=10,
|
||||
) == [15, 5, 15, 15, 15]
|
||||
assert build_calibration_speed_profile(
|
||||
index_pitch,
|
||||
normal_speed=15,
|
||||
index_roll_speed=5,
|
||||
index_flex_speed=10,
|
||||
) == [15, 10, 15, 15, 15]
|
||||
|
||||
index_pip = next(spec for spec in SWEEP_SPECS if spec.motor_index == 16)
|
||||
assert build_calibration_speed_profile(
|
||||
index_pip,
|
||||
normal_speed=15,
|
||||
index_roll_speed=5,
|
||||
index_flex_speed=10,
|
||||
) == [15, 10, 15, 15, 15]
|
||||
|
||||
thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0)
|
||||
assert build_calibration_speed_profile(
|
||||
thumb_pitch,
|
||||
normal_speed=15,
|
||||
index_roll_speed=5,
|
||||
index_flex_speed=10,
|
||||
) == [15, 15, 15, 15, 15]
|
||||
|
||||
|
||||
def test_calibration_speed_profile_rejects_out_of_range_values() -> None:
|
||||
index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6)
|
||||
with pytest.raises(ValueError, match="speeds"):
|
||||
build_calibration_speed_profile(
|
||||
index_roll,
|
||||
normal_speed=15,
|
||||
index_roll_speed=256,
|
||||
index_flex_speed=10,
|
||||
)
|
||||
|
||||
|
||||
def test_curve_fit_and_projected_zero_recover_synthetic_geometry() -> None:
|
||||
records = _records()
|
||||
fit = fit_joint_center_curve(records)
|
||||
assert fit.angle_rad[255] == pytest.approx(0.0, abs=1.0e-8)
|
||||
assert fit.angle_rad[0] == pytest.approx(0.70, abs=1.0e-4)
|
||||
assert fit.maximum_hysteresis_rad == pytest.approx(0.0, abs=1.0e-8)
|
||||
assert fit_projected_zero(records) == pytest.approx(0.0, abs=1.0e-6)
|
||||
|
||||
|
||||
def test_thumb_image_curve_avoids_corrupted_pnp_depth() -> None:
|
||||
records = _records()
|
||||
for record in records:
|
||||
command = int(record["command_u8"])
|
||||
record["relative_translation_xyz_m"] = [
|
||||
0.001 * command,
|
||||
0.0,
|
||||
0.0,
|
||||
]
|
||||
|
||||
fit = fit_measured_joint_curve("thumb_mcp", records)
|
||||
|
||||
assert fit.circle["space"] == "image_2d"
|
||||
assert fit.angle_rad[0] == pytest.approx(0.70, abs=0.02)
|
||||
assert fit.angle_rad[255] == pytest.approx(0.0)
|
||||
assert math.degrees(fit.maximum_monotonic_correction_rad) < 0.01
|
||||
assert math.degrees(fit.maximum_hysteresis_rad) < 0.01
|
||||
|
||||
command_zero = next(
|
||||
record for record in records if int(record["command_u8"]) == 0
|
||||
)
|
||||
observed = measure_joint_observation(
|
||||
fit,
|
||||
vector_xyz_m=command_zero["relative_translation_xyz_m"],
|
||||
image_vector_xy_px=command_zero["image_relative_xy_px"],
|
||||
)
|
||||
assert observed == pytest.approx(0.70, abs=0.02)
|
||||
|
||||
|
||||
def test_image_curve_rejects_insufficient_projected_arc() -> None:
|
||||
records = _records()
|
||||
for record in records:
|
||||
command = float(record["command_u8"])
|
||||
angle = math.radians(2.0) * (255.0 - command) / 255.0
|
||||
record["image_relative_xy_px"] = [
|
||||
100.0 * math.cos(angle),
|
||||
100.0 * math.sin(angle),
|
||||
]
|
||||
|
||||
with pytest.raises(
|
||||
ValueError, match="joint_image_trajectory_quality_failed:arc"
|
||||
):
|
||||
fit_joint_image_curve(records)
|
||||
|
||||
|
||||
def test_splay_uses_angular_midpoint_not_fixed_command_midpoint() -> None:
|
||||
fit = fit_joint_center_curve(_records())
|
||||
centred, zero_command, midpoint = center_splay_curve(fit)
|
||||
assert midpoint == pytest.approx(0.35, abs=1.0e-4)
|
||||
assert centred.angle_rad[0] == pytest.approx(0.35, abs=1.0e-4)
|
||||
assert centred.angle_rad[255] == pytest.approx(-0.35, abs=1.0e-4)
|
||||
assert zero_command in {127, 128}
|
||||
assert abs(centred.angle_rad[zero_command]) <= 0.002
|
||||
|
||||
|
||||
def test_compact_payload_contains_only_runtime_fields_and_inheritance() -> None:
|
||||
base = fit_joint_center_curve(_records())
|
||||
splay, zero_command, midpoint = center_splay_curve(base)
|
||||
measured = {
|
||||
name: splay if name == "index_mcp_roll" else base
|
||||
for name in MEASURED_JOINTS
|
||||
}
|
||||
projected = {
|
||||
name: 0.01 * index
|
||||
for index, (name, spec) in enumerate(JOINT_SPECS.items())
|
||||
if spec.zero_kind == "projected"
|
||||
}
|
||||
payload = build_compact_payload(
|
||||
serial_number="G20_LEFT_001",
|
||||
measured_fits=measured,
|
||||
projected_zeros_rad=projected,
|
||||
splay_zero_command_u8=zero_command,
|
||||
splay_midpoint_rad=midpoint,
|
||||
validation_errors_rad=[0.01, -0.02],
|
||||
passed=True,
|
||||
)
|
||||
validate_compact_payload(payload)
|
||||
assert set(payload) == {
|
||||
"schema_version",
|
||||
"model",
|
||||
"side",
|
||||
"serial_number",
|
||||
"angle_unit",
|
||||
"command_range",
|
||||
"baseline_command_u8",
|
||||
"joints",
|
||||
"quality",
|
||||
}
|
||||
assert len(payload["joints"]) == 21
|
||||
assert all(len(joint["angle_rad"]) == 256 for joint in payload["joints"].values())
|
||||
assert sum("zero_command_u8" in joint for joint in payload["joints"].values()) == 16
|
||||
assert sum(joint.get("passive") is True for joint in payload["joints"].values()) == 5
|
||||
assert payload["joints"]["middle_mcp_roll"]["source_joint"] == "index_mcp_roll"
|
||||
assert (
|
||||
payload["joints"]["middle_mcp_roll"]["angle_rad"]
|
||||
== payload["joints"]["index_mcp_roll"]["angle_rad"]
|
||||
)
|
||||
assert payload["joints"]["index_mcp_roll"]["zero_angles"] == {
|
||||
"travel_midpoint_rad": pytest.approx(0.35, abs=1.0e-4)
|
||||
}
|
||||
for name in SPLAY_JOINTS:
|
||||
assert payload["joints"][name]["zero_command_u8"] == zero_command
|
||||
|
||||
|
||||
def test_compact_payload_allows_skipped_random_validation() -> None:
|
||||
base = fit_joint_center_curve(_records())
|
||||
splay, zero_command, midpoint = center_splay_curve(base)
|
||||
measured = {
|
||||
name: splay if name == "index_mcp_roll" else base
|
||||
for name in MEASURED_JOINTS
|
||||
}
|
||||
projected = {
|
||||
name: 0.0
|
||||
for name, spec in JOINT_SPECS.items()
|
||||
if spec.zero_kind == "projected"
|
||||
}
|
||||
|
||||
payload = build_compact_payload(
|
||||
serial_number="G20_LEFT_001",
|
||||
measured_fits=measured,
|
||||
projected_zeros_rad=projected,
|
||||
splay_zero_command_u8=zero_command,
|
||||
splay_midpoint_rad=midpoint,
|
||||
validation_errors_rad=[],
|
||||
passed=True,
|
||||
)
|
||||
|
||||
validate_compact_payload(payload)
|
||||
assert payload["quality"] == {
|
||||
"passed": True,
|
||||
"validation_mae_rad": None,
|
||||
"validation_p95_rad": None,
|
||||
}
|
||||
@@ -0,0 +1,104 @@
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
import yaml
|
||||
|
||||
from g20_thumb_apriltag_calibration.hikrobot_camera import (
|
||||
DeviceDescriptor,
|
||||
decode_c_string,
|
||||
load_camera_calibration,
|
||||
resolve_camera_info_path,
|
||||
select_device,
|
||||
)
|
||||
|
||||
|
||||
def test_decode_c_string_stops_at_first_null() -> None:
|
||||
assert decode_c_string(b"DB2163742\0ignored") == "DB2163742"
|
||||
|
||||
|
||||
def test_select_device_accepts_serial_or_guid() -> None:
|
||||
devices = [
|
||||
DeviceDescriptor(
|
||||
index=0,
|
||||
model="MV-CS020-10UM",
|
||||
serial="DB2163742",
|
||||
guid="2BDFB2163742",
|
||||
),
|
||||
DeviceDescriptor(
|
||||
index=1,
|
||||
model="MV-CS020-10UM",
|
||||
serial="DB2163739",
|
||||
guid="2BDFB2163739",
|
||||
),
|
||||
]
|
||||
|
||||
assert select_device(devices, "DB2163742", "MV-CS020-10UM").index == 0
|
||||
assert select_device(devices, "2BDFB2163739", "MV-CS020-10UM").index == 1
|
||||
|
||||
|
||||
def test_select_device_never_guesses_when_multiple_cameras_exist() -> None:
|
||||
devices = [
|
||||
DeviceDescriptor(0, "MV-CS020-10UM", "one", "guid-one"),
|
||||
DeviceDescriptor(1, "MV-CS020-10UM", "two", "guid-two"),
|
||||
]
|
||||
|
||||
with pytest.raises(RuntimeError, match="selector is required"):
|
||||
select_device(devices, "", "MV-CS020-10UM")
|
||||
|
||||
|
||||
def test_select_device_rejects_wrong_model() -> None:
|
||||
devices = [DeviceDescriptor(0, "other", "DB2163742", "guid")]
|
||||
|
||||
with pytest.raises(RuntimeError, match="expected a model"):
|
||||
select_device(devices, "DB2163742", "MV-CS020-10UM")
|
||||
|
||||
|
||||
def test_load_standard_camera_calibration(tmp_path: Path) -> None:
|
||||
path = tmp_path / "camera.yaml"
|
||||
path.write_text(
|
||||
yaml.safe_dump(
|
||||
{
|
||||
"image_width": 1624,
|
||||
"image_height": 1240,
|
||||
"camera_name": "hikrobot_front_DB2163742",
|
||||
"camera_matrix": {
|
||||
"rows": 3,
|
||||
"cols": 3,
|
||||
"data": [1000.0, 0.0, 812.0, 0.0, 1001.0, 620.0, 0.0, 0.0, 1.0],
|
||||
},
|
||||
"distortion_model": "plumb_bob",
|
||||
"distortion_coefficients": {
|
||||
"rows": 1,
|
||||
"cols": 5,
|
||||
"data": [0.1, -0.2, 0.0, 0.0, 0.1],
|
||||
},
|
||||
"rectification_matrix": {
|
||||
"rows": 3,
|
||||
"cols": 3,
|
||||
"data": [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0],
|
||||
},
|
||||
"projection_matrix": {
|
||||
"rows": 3,
|
||||
"cols": 4,
|
||||
"data": [1000.0, 0.0, 812.0, 0.0, 0.0, 1001.0, 620.0, 0.0, 0.0, 0.0, 1.0, 0.0],
|
||||
},
|
||||
},
|
||||
sort_keys=False,
|
||||
),
|
||||
encoding="utf-8",
|
||||
)
|
||||
|
||||
calibration = load_camera_calibration(path)
|
||||
|
||||
assert calibration.width == 1624
|
||||
assert calibration.height == 1240
|
||||
assert calibration.k[0] == 1000.0
|
||||
assert calibration.p[5] == 1001.0
|
||||
|
||||
|
||||
def test_camera_info_url_only_accepts_local_files(tmp_path: Path) -> None:
|
||||
path = resolve_camera_info_path(str(tmp_path / "front.yaml"))
|
||||
assert path == (tmp_path / "front.yaml").resolve()
|
||||
assert resolve_camera_info_path("") is None
|
||||
with pytest.raises(ValueError, match="filesystem path"):
|
||||
resolve_camera_info_path("package://example/front.yaml")
|
||||
@@ -0,0 +1,217 @@
|
||||
import json
|
||||
|
||||
import numpy as np
|
||||
|
||||
from g20_thumb_apriltag_calibration.three_camera_diagnostics import (
|
||||
render_three_camera_status_text_zh,
|
||||
)
|
||||
|
||||
|
||||
def test_missing_endpoint_has_specific_chinese_reason_and_recovery() -> None:
|
||||
payload = {
|
||||
"state": "PAUSED",
|
||||
"reason": "sweep_missing_endpoint_bin",
|
||||
"progress": 0.1429,
|
||||
"completed_sweeps": 6,
|
||||
"total_sweeps": 42,
|
||||
"active": {
|
||||
"kind": "sweep",
|
||||
"view": "front",
|
||||
"motor_index": 5,
|
||||
"joints": ["thumb_cmc_roll"],
|
||||
"cycle": 1,
|
||||
"repetitions": 3,
|
||||
"start_u8": 255,
|
||||
"target_u8": 0,
|
||||
"actual_u8": 0.4,
|
||||
"motion_progress": 0.998,
|
||||
"valid_frames": 239,
|
||||
"sample": {
|
||||
"minimum_u8": 0.4,
|
||||
"maximum_u8": 248.2,
|
||||
"bin_count": 180,
|
||||
"minimum_bin_count": 32,
|
||||
"maximum_bin_gap": 3,
|
||||
"allowed_maximum_bin_gap": 16,
|
||||
"missing_endpoint_u8": [255],
|
||||
"endpoint_tolerance_u8": 2.0,
|
||||
},
|
||||
},
|
||||
"views": {
|
||||
"front": {
|
||||
"ready": False,
|
||||
"detection_hz": 30.04,
|
||||
"valid_rate": 0.0,
|
||||
"missing_tag_ids": [2],
|
||||
"pnp_rejections": {},
|
||||
"group_pnp_reason": "group_pose_jump",
|
||||
"pnp_invalid_seconds": 0.8,
|
||||
"pnp_reset_count": 2,
|
||||
},
|
||||
"side": {
|
||||
"ready": True,
|
||||
"detection_hz": 30.02,
|
||||
"valid_rate": 1.0,
|
||||
"missing_tag_ids": [],
|
||||
},
|
||||
},
|
||||
"result_path": "",
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert "标定已暂停（PAUSED）" in text
|
||||
assert "缺少电机端点255" in text
|
||||
assert "实际电机范围为0.4～248.2" in text
|
||||
assert "第1/3轮，255→0" in text
|
||||
assert "已完成6/42个扫描方向" in text
|
||||
assert "当前缺失Tag=2" in text
|
||||
assert "group_pose_jump" not in text
|
||||
assert "PnP拒绝" not in text
|
||||
assert "自动重置" not in text
|
||||
assert "/g20_calibration/resume" in text
|
||||
|
||||
|
||||
def test_preflight_lists_missing_tags_in_chinese() -> None:
|
||||
payload = {
|
||||
"state": "PREFLIGHT",
|
||||
"reason": "waiting_for_three_cameras_tags_and_sdk",
|
||||
"progress": 0.0,
|
||||
"completed_sweeps": 0,
|
||||
"total_sweeps": 0,
|
||||
"active": {},
|
||||
"views": {
|
||||
"top": {
|
||||
"ready": False,
|
||||
"detection_hz": 30.0,
|
||||
"valid_rate": 0.0,
|
||||
"missing_tag_ids": [9],
|
||||
}
|
||||
},
|
||||
"result_path": "",
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert "设备和标签预检（PREFLIGHT）" in text
|
||||
assert "当前缺失Tag=9" in text
|
||||
assert "全部必需Tag同时有效率0.0%" in text
|
||||
|
||||
|
||||
def test_joint_fit_failure_names_metric_and_selective_retry() -> None:
|
||||
payload = {
|
||||
"state": "PAUSED",
|
||||
"reason": "joint_fit_check_failed",
|
||||
"progress": 18 / 42,
|
||||
"completed_sweeps": 18,
|
||||
"total_sweeps": 42,
|
||||
"active": {
|
||||
"kind": "fit_failure",
|
||||
"view": "front",
|
||||
"motor_index": 6,
|
||||
"joints": ["index_mcp_roll"],
|
||||
"attempt": 1,
|
||||
"directions_to_rescan": 6,
|
||||
"failures": [
|
||||
{
|
||||
"joint": "index_mcp_roll",
|
||||
"metric": "arc_deg",
|
||||
"actual": 3.98,
|
||||
"limit": 15.0,
|
||||
"comparison": "minimum",
|
||||
}
|
||||
],
|
||||
},
|
||||
"views": {},
|
||||
"result_path": "",
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert "食指MCP侧摆的实测圆弧为3.98°" in text
|
||||
assert "要求至少15.00°" in text
|
||||
assert "只清除当前失败关节的数据并重扫6个方向" in text
|
||||
assert "第1次尝试" in text
|
||||
assert "运动采样：" not in text
|
||||
|
||||
|
||||
def test_return_baseline_prints_the_exact_command() -> None:
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [127, 127, 127, 127]
|
||||
payload = {
|
||||
"state": "RETURN_BASELINE",
|
||||
"reason": "return_baseline_before_next_sweep",
|
||||
"progress": 0.0,
|
||||
"completed_sweeps": 0,
|
||||
"total_sweeps": 42,
|
||||
"baseline_command_u8": baseline,
|
||||
"active": {},
|
||||
"views": {},
|
||||
"result_path": "",
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert "正在返回基准姿态（RETURN_BASELINE）" in text
|
||||
assert f"正在确认基准姿态：{baseline}" in text
|
||||
|
||||
|
||||
def test_status_numeric_diagnostics_are_json_serializable() -> None:
|
||||
bins = [0, 16, 255]
|
||||
payload = {
|
||||
"state": "SWEEP",
|
||||
"reason": "collecting_timestamp_synchronised_tag_centres",
|
||||
"active": {
|
||||
"sample": {
|
||||
# np.diff返回NumPy标量；节点必须在放入状态前转成原生int。
|
||||
"maximum_bin_gap": int(max(np.diff(bins), default=0)),
|
||||
}
|
||||
},
|
||||
}
|
||||
|
||||
encoded = json.dumps(payload, ensure_ascii=False)
|
||||
|
||||
assert '"maximum_bin_gap": 239' in encoded
|
||||
|
||||
|
||||
def test_index_roll_status_prints_clearance_motor_feedback() -> None:
|
||||
payload = {
|
||||
"state": "SWEEP",
|
||||
"reason": "collecting_timestamp_synchronised_tag_centres",
|
||||
"progress": 0.43,
|
||||
"completed_sweeps": 18,
|
||||
"total_sweeps": 42,
|
||||
"active": {
|
||||
"kind": "sweep",
|
||||
"view": "front",
|
||||
"motor_index": 6,
|
||||
"joints": ["index_mcp_roll"],
|
||||
"cycle": 1,
|
||||
"repetitions": 3,
|
||||
"start_u8": 255,
|
||||
"target_u8": 0,
|
||||
"actual_u8": 44.0,
|
||||
"motion_progress": 0.827,
|
||||
"valid_frames": 971,
|
||||
"sample": {"minimum_u8": 40.0, "maximum_u8": 253.0},
|
||||
"auxiliary_motors": [
|
||||
{"motor_index": 7, "command_u8": 0, "actual_u8": 0.0},
|
||||
{"motor_index": 8, "command_u8": 0, "actual_u8": 1.0},
|
||||
{"motor_index": 9, "command_u8": 0, "actual_u8": 0.0},
|
||||
],
|
||||
"speed": {
|
||||
"commanded_finger_speed": [15, 5, 15, 15, 15],
|
||||
"reported_finger_speed": [15, 5, 15, 15, 15],
|
||||
},
|
||||
},
|
||||
"views": {},
|
||||
"result_path": "",
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert "避挡姿态：电机7目标0、实际0.0" in text
|
||||
assert "电机8目标0、实际1.0" in text
|
||||
assert "电机9目标0、实际0.0" in text
|
||||
assert "阶段速度：五指目标[15, 5, 15, 15, 15]" in text
|
||||
assert "SDK报告[15, 5, 15, 15, 15]" in text
|
||||
@@ -0,0 +1,170 @@
|
||||
import json
|
||||
import math
|
||||
from types import SimpleNamespace
|
||||
|
||||
from g20_thumb_apriltag_calibration.core import (
|
||||
DIRECTION_DECREASING,
|
||||
DIRECTION_INCREASING,
|
||||
)
|
||||
from g20_thumb_apriltag_calibration.full_hand import SWEEP_SPECS
|
||||
from g20_thumb_apriltag_calibration.three_camera_node import (
|
||||
G20ThreeCameraCalibrationNode,
|
||||
SweepItem,
|
||||
)
|
||||
|
||||
|
||||
def _sweep_items() -> list[SweepItem]:
|
||||
return [
|
||||
SweepItem(spec, cycle, direction)
|
||||
for spec in SWEEP_SPECS
|
||||
for cycle in range(3)
|
||||
for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING)
|
||||
]
|
||||
|
||||
|
||||
def _image_cycle_records(travels_rad: list[float]) -> list[dict]:
|
||||
commands = list(range(0, 256, 16)) + [255]
|
||||
records = []
|
||||
for cycle, travel in enumerate(travels_rad):
|
||||
for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING):
|
||||
for command in commands:
|
||||
angle = travel * (255.0 - command) / 255.0
|
||||
records.append(
|
||||
{
|
||||
"cycle": cycle,
|
||||
"direction": direction,
|
||||
"command_u8": command,
|
||||
"relative_translation_xyz_m": [0.01, 0.0, 0.0],
|
||||
"image_relative_xy_px": [
|
||||
100.0 * math.cos(angle),
|
||||
100.0 * math.sin(angle),
|
||||
],
|
||||
}
|
||||
)
|
||||
return records
|
||||
|
||||
|
||||
def _fit_check_node(records_by_joint: dict) -> SimpleNamespace:
|
||||
node = SimpleNamespace(
|
||||
records_by_joint=records_by_joint,
|
||||
repetitions=3,
|
||||
trajectory_maximum_plane_rms_m=0.004,
|
||||
trajectory_maximum_radial_rms_m=0.004,
|
||||
trajectory_minimum_radius_m=0.003,
|
||||
trajectory_minimum_arc_rad=math.radians(15.0),
|
||||
image_trajectory_maximum_radial_rms_px=2.0,
|
||||
image_trajectory_maximum_radial_p95_px=3.5,
|
||||
image_trajectory_minimum_radius_px=20.0,
|
||||
trajectory_maximum_cycle_travel_difference_rad=math.radians(3.0),
|
||||
passive_maximum_cycle_travel_difference_rad=math.radians(10.0),
|
||||
maximum_monotonic_correction_rad=math.radians(2.0),
|
||||
maximum_hysteresis_rad=math.radians(5.0),
|
||||
passive_maximum_monotonic_correction_rad=math.radians(3.0),
|
||||
passive_maximum_hysteresis_rad=math.radians(7.5),
|
||||
zero_minimum_radius_px=20.0,
|
||||
zero_maximum_radial_rms_px=2.0,
|
||||
zero_maximum_radial_p95_px=3.5,
|
||||
zero_maximum_round_difference_rad=math.radians(1.0),
|
||||
)
|
||||
node._fit_joint_records = lambda name, records, relaxed=False: (
|
||||
G20ThreeCameraCalibrationNode._fit_joint_records(
|
||||
node, name, records, relaxed=relaxed
|
||||
)
|
||||
)
|
||||
return node
|
||||
|
||||
|
||||
def test_fit_failure_rewinds_to_failed_specs_first_direction(tmp_path) -> None:
|
||||
spec = next(item for item in SWEEP_SPECS if item.motor_index == 6)
|
||||
node = SimpleNamespace(
|
||||
sweep_items=_sweep_items(),
|
||||
sweep_attempts={spec.motor_index: 1},
|
||||
retry_sweep_spec=None,
|
||||
fit_failure={},
|
||||
sweep_index=24,
|
||||
raw_path=tmp_path / "raw_samples.jsonl",
|
||||
repetitions=3,
|
||||
)
|
||||
node._sweep_spec_start_index = lambda selected: (
|
||||
G20ThreeCameraCalibrationNode._sweep_spec_start_index(node, selected)
|
||||
)
|
||||
node._pause = lambda reason: setattr(node, "paused_reason", reason)
|
||||
|
||||
G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure(
|
||||
node,
|
||||
spec,
|
||||
[
|
||||
{
|
||||
"joint": "index_mcp_roll",
|
||||
"metric": "arc_deg",
|
||||
"actual": 3.98,
|
||||
"limit": 15.0,
|
||||
"comparison": "minimum",
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
assert node.sweep_index == 18
|
||||
assert node.retry_sweep_spec == spec
|
||||
assert node.fit_failure["directions_to_rescan"] == 6
|
||||
assert node.paused_reason == "joint_fit_check_failed"
|
||||
|
||||
|
||||
def test_resume_discards_only_failed_specs_samples(tmp_path) -> None:
|
||||
spec = next(item for item in SWEEP_SPECS if item.motor_index == 15)
|
||||
records = {
|
||||
"thumb_mcp": [{"old": "mcp"}],
|
||||
"thumb_ip": [{"old": "ip"}],
|
||||
"thumb_cmc_pitch": [{"keep": True}],
|
||||
}
|
||||
node = SimpleNamespace(
|
||||
retry_sweep_spec=spec,
|
||||
records_by_joint=records,
|
||||
sweep_attempts={spec.motor_index: 1},
|
||||
raw_path=tmp_path / "raw_samples.jsonl",
|
||||
paused_reason="joint_fit_check_failed",
|
||||
fit_failure={"failures": ["old"]},
|
||||
)
|
||||
|
||||
selected = G20ThreeCameraCalibrationNode._prepare_failed_sweep_retry(node)
|
||||
|
||||
assert selected == spec
|
||||
assert records["thumb_mcp"] == []
|
||||
assert records["thumb_ip"] == []
|
||||
assert records["thumb_cmc_pitch"] == [{"keep": True}]
|
||||
assert node.sweep_attempts[15] == 2
|
||||
assert node.retry_sweep_spec is None
|
||||
assert node.fit_failure == {}
|
||||
event = json.loads((tmp_path / "raw_samples.jsonl").read_text())
|
||||
assert event == {
|
||||
"kind": "retry",
|
||||
"view": "front",
|
||||
"motor_index": 15,
|
||||
"joints": ["thumb_mcp", "thumb_ip"],
|
||||
"attempt": 2,
|
||||
"reason": "joint_fit_check_failed",
|
||||
}
|
||||
|
||||
|
||||
def test_provisional_fit_rejects_inconsistent_cycle_travel() -> None:
|
||||
spec = next(item for item in SWEEP_SPECS if item.motor_index == 0)
|
||||
node = _fit_check_node(
|
||||
{
|
||||
"thumb_cmc_pitch": _image_cycle_records(
|
||||
[math.radians(47.0), math.radians(47.2), math.radians(55.0)]
|
||||
)
|
||||
}
|
||||
)
|
||||
|
||||
failures = G20ThreeCameraCalibrationNode._provisional_fit_failures(
|
||||
node, spec
|
||||
)
|
||||
|
||||
cycle_failure = next(
|
||||
item
|
||||
for item in failures
|
||||
if item["metric"] == "cycle_travel_range_deg"
|
||||
)
|
||||
assert cycle_failure["joint"] == "thumb_cmc_pitch"
|
||||
assert cycle_failure["actual"] == 8.0
|
||||
assert cycle_failure["limit"] == 3.0
|
||||
Reference in New Issue
Block a user