Compare commits
9 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 admin
|
286581bcba | ||
|
admin
|
4dadfb954b | ||
|
admin
|
83c69b48c2 | ||
|
admin
|
4e594ddb09 | ||
|
admin
|
ef65681230 | ||
|
admin
|
a609d521a0 | ||
|
admin
|
41ff4a61a9 | ||
|
admin
|
4107da4c22 | ||
|
admin
|
5d206bcb73 |
+11
-1
@@ -50,6 +50,7 @@ Thumbs.db
|
||||
|
||||
# Runtime and calibration scratch files
|
||||
/logs/
|
||||
/MvSdkLog/
|
||||
*.tmp
|
||||
*.log
|
||||
*.bak
|
||||
@@ -61,13 +62,20 @@ Thumbs.db
|
||||
# src/linkerhand_retarget/resource/linkerforce_v2/profiles/.
|
||||
/profiles/
|
||||
/calibration_output/
|
||||
/config/*_three_camera_extrinsics.yaml
|
||||
*.wear_check.json
|
||||
*.checkpoint.json
|
||||
*.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_calibrated_*.urdf
|
||||
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_zero_calibrated_*.urdf
|
||||
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right_cmc_pitch_*.urdf
|
||||
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right_calibrated_*.urdf
|
||||
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right_zero_calibrated_*.urdf
|
||||
/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_G20_RIGHT_tag.urdf
|
||||
|
||||
# ROS bag / MCAP recordings and CAN captures
|
||||
rosbag2_*/
|
||||
@@ -84,3 +92,5 @@ candump-*
|
||||
# Local Codex/agent workspace metadata
|
||||
/.agents/
|
||||
/.codex/
|
||||
/.codebuddy/
|
||||
/.zcode/
|
||||
|
||||
@@ -1,6 +1,561 @@
|
||||
# G20 左手拇指正面 AprilTag 标定
|
||||
# G20 左右手 AprilTag 标定
|
||||
|
||||
该包启动 RealSense、彩色图像校正、`apriltag_ros`、Linker Hand SDK 和标定状态机，
|
||||
## G20右手正式一键标定
|
||||
|
||||
固定三相机和19张Tag安装完成后，用户只运行：
|
||||
|
||||
```bash
|
||||
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right
|
||||
```
|
||||
|
||||
完全独立地只标定大拇指4项任务时，使用：
|
||||
|
||||
```bash
|
||||
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
|
||||
--scope thumb
|
||||
```
|
||||
|
||||
该模式不读取任何已标定四指数据。拇指零位求解只使用5条拇指轴、两条顶部同相机
|
||||
方向观测以及拇指自己的机械端点；输出URDF从原始CAD生成，只修改4个拇指主动
|
||||
关节，12个四指关节保持CAD零位。独立结果发布到`latest_thumb_passed`，其JSON是
|
||||
拇指标定/诊断产物，不冒充可直接运行的完整整手曲线JSON。
|
||||
|
||||
如果确实需要把新的拇指结果合并到一份已经通过的完整整手标定，才额外使用：
|
||||
|
||||
```bash
|
||||
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
|
||||
--scope thumb \
|
||||
--base-session calibration_output/G20_RIGHT_001/latest_passed
|
||||
```
|
||||
|
||||
合并模式会冻结基础会话中的12个四指主动零位；发布前再次读取基础会话JSON核对，
|
||||
任何四指零位变化都会拒绝发布。基础四指数据仍不参与4个拇指零位的数值求解。
|
||||
|
||||
拇指专项结果重复性通过后，不必再做原来的16项整手扫描。以该拇指会话为基础，
|
||||
只重新采集12项四指任务并合成完整整手URDF：
|
||||
|
||||
```bash
|
||||
ros2 run g20_thumb_apriltag_calibration calibrate_g20_right \
|
||||
--scope fingers \
|
||||
--base-session calibration_output/G20_RIGHT_001/<已通过的拇指会话时间戳>
|
||||
```
|
||||
|
||||
`fingers`模式严格冻结基础会话中的4个拇指主动零位；最终完整整手URDF中的拇指
|
||||
零位与专项会话schema-v4数值完全一致。默认`full`也先调用与`thumb`完全相同的
|
||||
独立拇指内核，再冻结这4个结果求解12个四指零位，因此四指数据不能反向改写
|
||||
拇指结果。原来的默认`full`仍保留，用于需要16项全部重新采集的情况。
|
||||
|
||||
采样文件中的运动域是显式且不可混用的：
|
||||
`requested_command_u8` 表示下发命令，`feedback_u8` 表示电机反馈。
|
||||
在线拟合和离线重放通过同一个数据契约投影到曲线索引；新会话不会把含糊的
|
||||
`command_u8` 写入 `raw_samples.jsonl`。基础会话导入期间状态会显示为
|
||||
`IMPORTING_BASE` 和 `REVALIDATING_INHERITED`，完成复核后才允许机械手运动。
|
||||
|
||||
提供`--base-session`时，它必须解析到同一序列号目录下的完整PASS会话；启动前会校验源CAD
|
||||
URDF、相机外参和标定配置哈希。新会话从原始CAD重新生成完整URDF，不在旧校准
|
||||
URDF上叠加。两种thumb模式都只重采`thumb_cmc_pitch`、`thumb_cmc_roll`、
|
||||
`thumb_cmc_yaw`、`thumb_mcp/thumb_ip`四项物理任务，其余任务的原始记录导入后仍按
|
||||
数据契约和产物哈希检查，但不会以历史四指拟合结果否决本次拇指专项标定。
|
||||
|
||||
开发阶段若上一次会话失败，同一命令会自动校验硬件/几何哈希，并恢复已经
|
||||
完整提交的关节任务；失败中的当前任务始终丢弃重做，位于它后面但已经完整通过的
|
||||
独立任务仍会复用，不再因“连续前缀”限制整段重采。导入的任务会立即用与
|
||||
最终验收相同的硬门限复检（不含视口实时有效率）：只以预警带余量通过的旧数据
|
||||
当场剔除并从其在扫掠顺序中的原始位置重采，避免全部任务采完后才在最终验收
|
||||
失败、把会话拉回靠前的关节。方向级自动重扫事件是追加日志中的持久失效标记；
|
||||
恢复时只读取该标记之后的替代采集，不能把同一尝试编号下重扫前后的稳态点合并。
|
||||
因此已经在线硬门限验收的任务保持已完成，暂停中的任务从任务开头重采，不会因
|
||||
日志中仍保留被自动重扫淘汰的旧点而倒退到更早任务。运行中的多视角任务按正面主测量和侧面校验测量
|
||||
独立保留；单轮转轴异常且其余三轮形成一致簇时只补扫异常轮的两个方向。侧面
|
||||
轴线位置若也能明确定位为单轮异常，同样只补扫该轮；补扫会保留任务预检和前次
|
||||
采集确定的PnP分支参考，不会因重新初始化切换到另一组平面Tag镜像解。侧面
|
||||
校验视角的任务级有效率只记录为诊断；G20右手预检若逐帧识别率低于标称值，
|
||||
但同步有效位姿已经完整覆盖端点、中点、最小分箱数和最大分箱空洞，也按完整
|
||||
轨迹通过。正式扫描仍逐方向执行相同的硬分箱覆盖检查，轴线、曲线和模型质量
|
||||
门限保持不变。每个任务的低速往返预检、首轮交接和四轮双向正式扫描属于同一
|
||||
采集事务：相邻方向共享已验证端点和任务级PnP参考。G20右手正式扫描固定使用
|
||||
产品审定速度，不再根据单次识别密度自动提速，确保不同会话测量的是同一动态
|
||||
过程。顶部单目`thumb_cmc_yaw`在最终求解后另做零偏轮次重复性检查：前三轮
|
||||
极差默认不得超过0.5°，95%置信半宽不得超过0.75°。若两轮形成不超过门限20%的
|
||||
紧密簇、仅另一轮越界，自动补采该轮两个方向；无法明确定位时只重采完整yaw任务，
|
||||
不会回退重采整手。与`latest_passed`中上一正式结果相差超过0.75°时另写入
|
||||
`thumb_yaw_cross_session_diagnostic`提示检查机械手位置和Tag安装，但该历史差值
|
||||
不直接否决当前会话，也不会用旧结果约束新零位。需要强制从第一个关节
|
||||
重新采集时使用 `--no-resume`。升级前已经分别完成的正面/侧面roll也会合并为
|
||||
一个完整同步任务断点；只有两边数据都完整时才复用。
|
||||
|
||||
命令自动完成产品哈希预检、运动、当前任务补扫、前三轮训练、第四轮隔离留出、
|
||||
16个会话数据求解主动关节URDF零位修正、
|
||||
21条视觉实测命令曲线发布；四指PIP/DIP的动态曲线均实测，四指DIP静态零位保留CAD。
|
||||
终端只显示中文进度和问题；失败时复制“请复制以下内容给开发者”块即可。
|
||||
|
||||
四指末端的16 mm Tag允许使用刚性延长杆避挡；软件不假设末端Tag平面与中节Tag
|
||||
平面平行。延长杆和Tag在一次标定期间必须完全刚性，不能晃动、扭转或重新调整。
|
||||
侧面掌部基准Tag（ID 4）与各活动指节Tag也不要求安装面平行：首次联合PnP使用
|
||||
静态多帧刚性、重投影误差和跨轮任务参考选择分支，不再用固定15°安装角门限阻断扫描。
|
||||
单Tag独立位姿仍保留75°倾角保护；对包含锁定掌部基准和完整父子链的任务，倾角保护只
|
||||
限制独立选择，不会在联合选择前删除正深度、低重投影的IPPE候选。联合跟踪继续用相邻帧
|
||||
绝对/相对位姿连续性约束这些斜视候选，最终轴线残差、四轮重复性和隔离留出门限不放宽。
|
||||
终端中的Tag计数表示“可见”；等待扫描起点时会另列PnP初始化进度和累计拒绝原因。
|
||||
若联合候选仍然失败，`raw_samples.jsonl` 会按8个反馈计数的区间保存
|
||||
`group_pnp_candidate_event`，其中包含缺失角色、逐Tag候选数/倾角/重投影、角点和相机内参
|
||||
哈希，可直接定位运动到哪个机械位置后开始失效，而不需要再次盲扫整条流程。
|
||||
|
||||
正式结果位于 `calibration_output/G20_RIGHT_001/latest_passed`。该指针只在
|
||||
JSON、URDF数值等价、mesh完整性、21条曲线CAD限位、被动关节保护和隔离留出验证
|
||||
全部通过后更新。
|
||||
|
||||
## G20右手19-Tag底层调试入口
|
||||
|
||||
以下内容仅保留给旧会话回放和开发调试；正式一键命令只发布上面的精简
|
||||
schema v4 JSON，不再生成schema v5运行文件。
|
||||
|
||||
新布局用独立参数启用，原有左右手11-Tag流程仍默认使用
|
||||
`tag_layout:=legacy_11`，两套配置和结果schema互不覆盖：
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration three_camera_calibration.launch.py \
|
||||
hand_type:=right \
|
||||
tag_layout:=g20_right_19 \
|
||||
serial_number:=G20_RIGHT_001 \
|
||||
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
|
||||
source_urdf_expected_sha256:=<CAD负责人确认的G20右手源URDF_SHA256>
|
||||
```
|
||||
|
||||
当前产品布局共19张`tag36h11`，所有Tag的黑色码区边长均为16 mm；四指末节ID为
|
||||
`7,14,16,18`。正面为`0,1,2,3,10,11,12,13`，侧面为
|
||||
`4,5,6,7,14,15,16,17,18`，上面为`8,9`。详细角色和逐ID尺寸以
|
||||
`config/three_camera_tags_g20_right_19.yaml`为唯一软件配置源。程序执行4项拇指任务，
|
||||
以及小指、无名指、中指、食指各自的正面+侧面同步roll、侧面pitch、
|
||||
侧面PIP/DIP联合任务，共16个物理运动任务。同步roll只驱动电机一次，但两台相机仍分别拟合并通过
|
||||
各自的观测质量门限。每个PIP任务只驱动一次对应电机，同时用“手掌→中节Tag”实测PIP、
|
||||
用“中节Tag→末节Tag”实测被动DIP；四个DIP不再由URDF mimic系数生成，并参加完整视觉
|
||||
拟合和质量门限。每项正式四轮之前自动低速往返预检
|
||||
0/127/255可见性；
|
||||
|
||||
基准形态恢复完成后，程序先用至少30帧稳健锁定正面ID 0、侧面ID 4和顶部ID 8的
|
||||
固定掌部位姿。小指和无名指弯曲避让会遮住正面ID 0，因此四指正面+侧面同步roll中
|
||||
允许ID 0暂时不可见，并使用本会话基准锁定值；运动连杆Tag仍必须实时可见，门限不
|
||||
放宽。终端用`锁`表示该固定参考有效，例如`正面[0锁,12✓]`，`✗`才表示需要处理的
|
||||
实时Tag。锁定后本次标定运行中不得再移动相机、手掌底座或整只手，否则缓存参考
|
||||
失效，必须重新启动标定；两次独立会话之间轻微调整整只手的位置不会改变机械端点
|
||||
零位基准。任务级Tag有效率门限按"当前任务所需角色生效期间的采集帧"统计；
|
||||
任务结束后角色要求会切回预检全套标签，静止期帧不参与该门限，避免把
|
||||
采集质量良好的任务误判为可见性失败。
|
||||
|
||||
恢复末端Tag后，程序能够独立实测四指PIP和DIP的转轴及动态命令曲线。但同一次
|
||||
相机外参和同一套Tag安装下的重复扫描无法排除固定安装相位偏差；实体手在反馈0端
|
||||
能够触掌是独立的机械端点约束。三个thumb CMC主动轴、`thumb_mcp`、四指MCP pitch
|
||||
和PIP的URDF静态零位均由本次实测全行程与CAD机械端点之差求出，不再把跨相机的
|
||||
平面PnP绝对相位直接当作编码器零位，也不写死为0。生成URDF时同步修正这些关节的
|
||||
坐标上限以及相关被动关节的mimic坐标偏置，
|
||||
保证非零零偏不会缩短最大闭合量。末节Tag继续用于DIP动态曲线、轴线质量、遮挡和
|
||||
第四轮留出检查。正式数据求解静态修正范围为拇指CMC三个主动关节、`thumb_mcp`、
|
||||
四指MCP roll、MCP pitch及PIP，共16个。`thumb_mcp`与四指屈伸关节一样使用
|
||||
实测全屈曲行程和CAD机械端点联合求解，不直接采用MCP/IP耦合运动的单目PnP相位。
|
||||
侧面累计避障按“PIP→MCP pitch→roll”的安全顺序分阶段进入，并按逆序分阶段退出；
|
||||
同类辅助电机（全部邻指滚转、全部PIP、全部MCP pitch）合并为同一个并行航点同时
|
||||
运动，被测通道最后单独进入。“滚转全部回中前不展开弯曲手指”“每指pitch先于PIP”
|
||||
等已评审不变量保持不变，过渡仍受类别限速、逐航点到位确认、停滞检测和超时保护。
|
||||
`parallel_pose_transitions`（默认true）置false可回退旧的逐电机顺序。
|
||||
同一任务的预检和四轮正式扫描会保持完整避障姿态连续执行，只在任务切换时退出，
|
||||
不再每轮重复展开/弯曲辅助手指。跨手指组切换时，下一组避障姿态仍然需要、且
|
||||
当前已经在位（含反馈容差）的辅助电机保持原位，只有下一组不再使用的避障电机
|
||||
退回基准，避免"先展开回基准、马上又折回"的多余动作；已评审的
|
||||
"滚转先回中再展开""先滚开再弯曲"顺序保持不变。预检正反方向若都保留至少64个电机分箱且最大空缺
|
||||
不超过8，只作为采集能力诊断。G20右手四轮正式速度始终使用产品配置的固定值，
|
||||
不会因本次预检帧率或识别密度而改变；旧11-Tag布局仍保留自适应速度兼容逻辑。
|
||||
四指roll不再把同一反馈127误当成方向无关的唯一机械姿态：以`255→127`为标准物理
|
||||
零位，反向到达127的实测偏差保留在`increasing_rad`中。方向分支间隙上限1.5°、
|
||||
四轮间隙极差上限0.3°；其他关节仍使用严格的0.5°baseline回差门限。
|
||||
预检、正式四轮和拟合重扫始终使用速度5。
|
||||
正式roll的每个方向会在经过127时先到位保持0.5秒，再独立保存至少10帧静止Tag/反馈；
|
||||
方向分支检查和动态曲线的127相位都使用这两组双向静止数据，运动中经过127的帧不再
|
||||
替代静态保持姿态。
|
||||
前三轮只用于训练，第四轮完全留出；留出轮不参与显著性、Student-t置信区间或最终重拟合。
|
||||
每个任务只在低速递减预检起点执行一次8帧PnP静态初始化；预检往返和四轮正式
|
||||
扫描连续复用同一帧间分支与任务参考，不再让每一轮独立选择平面Tag解。同一任务第1轮
|
||||
已确立的端点相对姿态作为后3轮的分支锚点，防止独立初始化选到相反的
|
||||
IPPE镜像解。baseline标准接近和全部质量门限保持不变。
|
||||
电机15任务会利用源URDF中已确认的`thumb_ip mimic=1.03`，只在逐帧IPPE双解中
|
||||
排除与MCP同步运动明显矛盾（残差超过7.5°）的ID3镜像候选。该先验不生成或缩放
|
||||
`thumb_ip`曲线；通过分支选择后的`ID2→ID3`姿态仍独立拟合并接受完整留出验证。
|
||||
|
||||
当前19-Tag产品流程发布精简schema v4：21条运行时曲线全部来自当前会话的视觉实测。
|
||||
URDF零位字段覆盖拇指4个主动关节和四指各自的`mcp_roll/mcp_pitch/pip`，共16个，
|
||||
其中三个thumb CMC轴、`thumb_mcp`及四指`mcp_pitch/pip`共12个字段由实测旋转行程
|
||||
与机械端点联合求解；
|
||||
写出非零`thumb_mcp`零偏时同步平移其关节坐标上限，并更新被动`thumb_ip`的
|
||||
`mimic offset`，因此不会改变CAD定义的最大屈曲实体姿态；
|
||||
`thumb_ip`及四指DIP静态零位保留源CAD。旧schema v5文件仅作历史回放兼容，
|
||||
当前一键流程不再生成它。正面/侧面roll在同一次运动中独立拟合；方向、
|
||||
轴线和动态曲线均通过时做不确定度加权轴融合。侧面PIP连杆标签在滚转扫掠中
|
||||
相对侧相机视线倾斜约13°~20°，平面标签的单目IPPE姿态二义性会给侧视姿态引入
|
||||
数度的系统性"绕视线"偏差（亚像素重投影无法发现，会话20260820_105535实测
|
||||
前后轴向稳定相差11.4°），因此侧视PIP连杆姿态不再参与MCP轴向融合或角曲线验收，
|
||||
正侧姿态差只写入`cross_view_roll_axis_diagnostic`。四根MCP侧摆轴在产品URDF中
|
||||
严格平行：小指作为先采集的参考轴，其余三指复用该公共方向并各自独立拟合轴线位置，
|
||||
避免平面PnP分支在不同会话中改变轴向。前视侧摆连杆受丝杆平移影响，其纯旋转拟合得到的是
|
||||
随手指结构变化的伪轴线，不能与侧视PIP连杆的物理轴线使用统一距离门限；
|
||||
两者线距仅记录在诊断中。侧视校验通道
|
||||
（`*_mcp_roll_side`）的
|
||||
姿态分支间隙跨轮极差和独立姿态轴方向极差只作诊断，不触发重复采集；这两个量来自
|
||||
近掠射平面Tag的非发布姿态分量。绝对分支间隙1.5°上限保持不变，真正发布的正面主轴
|
||||
仍使用原跨轮严格门限。侧视逐帧`axis_pose_line_rms`同样只作诊断，组合轴线改用四轮
|
||||
位置RMS验收；径向、平面、圆一致性及可见性门限全部保留。
|
||||
侧面端视roll的圆轨迹方向已经受
|
||||
姿态轴约束，因此自由三维圆平面与姿态轴的夹角只保留诊断，不再被重复作为硬门限；
|
||||
径向残差和四轮轴线位置RMS仍是硬门限。正式MCP动态曲线统一使用正面Tag中心的
|
||||
二维投影圆角度，侧面姿态曲线仅保留为诊断；任一正式视角自身四轮不重复或第四轮
|
||||
留出失败仍会拒绝发布。任一静态目标、第四轮留出、
|
||||
遮挡、PnP或跨机位检查失败时，只保留原始轨迹和`passed:false`诊断，不发布正式URDF。
|
||||
8个组合姿态仅保留为开发诊断，正式产品默认不执行。轴线零位求解不提供适合绝对笛卡尔
|
||||
位置验收的手基座变换，因此不能用该诊断推翻已经通过的单关节隔离留出结果。三个CMC轴恢复使用
|
||||
`a609d521`验证过的完整四轮相对旋转曲线；全部实测关节均由隔离第四轮逐关节验收。
|
||||
现场需要区分某根手指的roll机构回差与单机位误差时，可设置
|
||||
`cross_view_roll_diagnostic_finger:=pinky|ring|middle|index`。该会话只执行目标手指的一次
|
||||
正面+侧面同步roll，共10个预检/正式方向；任一机位数据不足会重扫同一物理任务，
|
||||
双机位数据齐全后即使存在轴质量失败也不再自动重采，而是把失败项随双机位结果
|
||||
一起写入`cross_view_roll_diagnostic`并立即暂停。诊断会话永久
|
||||
锁定URDF发布，不能用`resume`转换成正式标定。
|
||||
schema v5明确声明曲线输入域为真实反馈u8；运行桥默认订阅
|
||||
`/g20/cb_right_hand_state`，并按反馈增减方向选择正程/反程曲线，停止时锁存最后运动
|
||||
方向。尚未观察到运动方向时使用`255→127`标准分支，不使用两个机械分支的平均值。
|
||||
schema v4继续兼容旧
|
||||
命令域。两者都只发布动态角度，不重复叠加已写入URDF的静态偏移。
|
||||
|
||||
## 三机位三维关节轴零位标定（schema v4）
|
||||
|
||||
正式入口同时使用三台海康 `MV-CS020-10U/10UM` 黑白全局快门相机，只有
|
||||
`/g20_calibration` 一个节点拥有机械手命令发布权。相机不需要水平，Tag方向也不需要
|
||||
贴正；相机和Tag在一次标定中必须固定。默认绑定为：
|
||||
|
||||
```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 | 正面；右手电机0时也由侧面观测 | 拇指 CMC 后连杆 |
|
||||
| 2 | 正面 | 拇指 MCP 后连杆 |
|
||||
| 3 | 正面 | 拇指 IP 后末节 |
|
||||
| 4 | 侧面 | 掌壳侧面固定基准（最底下） |
|
||||
| 5 | 侧面 | 左手食指/右手小指 MCP 后连杆 |
|
||||
| 6 | 侧面 | 左手食指/右手小指 PIP 后连杆 |
|
||||
| 7 | 侧面 | 左手食指/右手小指 DIP 后末节 |
|
||||
| 8 | 上面 | 上面相机可见的掌壳/底座固定基准 |
|
||||
| 9 | 上面 | 拇指 CMC yaw 运动件 |
|
||||
| 10 | 正面 | 左手食指/右手小指根部侧摆运动件 |
|
||||
|
||||
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
|
||||
```
|
||||
|
||||
### 1. 一次性三相机外参
|
||||
|
||||
三相机第一次安装、任何相机移动、镜头重新聚焦或内参变化后，必须重标外参。使用
|
||||
`8x5` 内角点、实测方格边长 `27 mm`、粘在硬质平板上的棋盘：
|
||||
|
||||
```bash
|
||||
mkdir -p /home/lxp/projects/linkerhand_retarget_ros2/config
|
||||
ros2 launch g20_thumb_apriltag_calibration \
|
||||
three_camera_extrinsics.launch.py \
|
||||
output_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
|
||||
checkerboard_columns:=8 checkerboard_rows:=5 square_size_m:=0.027
|
||||
```
|
||||
|
||||
启动后默认打开 `G20 Three-Camera Extrinsics` 交互窗口。可切换
|
||||
`FRONT + SIDE` 和 `FRONT + TOP`；窗口实时显示棋盘角点、单相机/组合
|
||||
RMS、时间差、联合拟合稳定性和候选/内点数量。单张只要棋盘完整、
|
||||
同步、RMS和姿态差异合格，`ADD CANDIDATE` 就会变绿；不再用单张
|
||||
PnP的最终外参偏差锁死采集。点击 `AUTO ON` 后，棋盘稳定1秒会自动
|
||||
采集，移到新姿态后再自动采下一组。
|
||||
|
||||
外参分两组采集。让棋盘静止且同时出现在正面/侧面画面中，每改变一次位置和倾角添加
|
||||
一个候选；随后以相同方法采集正面/上面。程序使用固定内参的
|
||||
`stereoCalibrate` 联合优化唯一旋转/平移。采集准入和最终验收分离：FRONT和
|
||||
配对相机的单帧RMS分别不得超过1.5 px，同时组合RMS不得超过1.2 px；界面中
|
||||
单相机1.2 px以内显示绿色、1.2～1.5 px显示黄色且仍可采集、超过1.5 px显示红色。
|
||||
新姿态会与全部已采姿态比较，避免在少数姿态间反复采集。拟合先剔除粗大异常组，
|
||||
再在不低于15个内点的前提下有界裁剪联合误差最高的候选，最终1.2 px门限不会被
|
||||
放宽。两组均得到
|
||||
至少15个内点且联合RMS、三折稳定性合格后 `SAVE` 才变绿。
|
||||
|
||||
```bash
|
||||
ros2 service call /g20_camera_extrinsics/capture_front_side std_srvs/srv/Trigger {}
|
||||
ros2 service call /g20_camera_extrinsics/capture_front_top std_srvs/srv/Trigger {}
|
||||
ros2 service call /g20_camera_extrinsics/save std_srvs/srv/Trigger {}
|
||||
```
|
||||
|
||||
采集时可分别查看 `/g20_extrinsics/{front,side,top}/camera/image_rect`。界面始终
|
||||
显示当前配对的整批RMS、旋转稳定性和平移稳定性；`BATCH FAIL` 后会直接列出
|
||||
`INLIERS`、`RMS`、`ROT` 或 `TRANS` 失败项。保存门限为：联合重投影RMS不超过
|
||||
1.2 px、三折重拟外参最大旋转差不超过0.3°、最大平移差不超过1.5 mm。文件同时
|
||||
绑定三台相机序列号、分辨率和内参哈希；不满足任一项时不会保存通过结果，正式
|
||||
标定也不会运动。
|
||||
|
||||
### 2. 预检和正式标定
|
||||
|
||||
先使用禁止运动模式检查三个机位、外参、内参和标签：
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration \
|
||||
three_camera_calibration.launch.py \
|
||||
hand_type:=left \
|
||||
serial_number:=G20_LEFT_001 \
|
||||
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
|
||||
source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf \
|
||||
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
|
||||
```
|
||||
|
||||
确认全行程安全、MVS客户端已关闭且没有其他命令发布者后，重启正式流程：
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration \
|
||||
three_camera_calibration.launch.py \
|
||||
hand_type:=left \
|
||||
serial_number:=G20_LEFT_001 \
|
||||
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
|
||||
source_urdf_path:=/home/lxp/projects/linkerhand_retarget_ros2/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf \
|
||||
can_interface:=can0
|
||||
```
|
||||
|
||||
右手使用同一入口；默认自动选择右手SDK话题和原始URDF：
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration \
|
||||
three_camera_calibration.launch.py \
|
||||
hand_type:=right \
|
||||
serial_number:=G20_RIGHT_001 \
|
||||
camera_extrinsics_file:=/home/lxp/projects/linkerhand_retarget_ros2/config/g20_three_camera_extrinsics.yaml \
|
||||
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`，右手依次扫描
|
||||
`0/5/15/9/4/19/10`，每项三轮 `255→0→255`。轨迹角由父/子Tag完整相对
|
||||
四元数的旋转向量投影到三维拟合轴得到。
|
||||
轴方向使用相对姿态旋转轴和可信上游轴约束；仅对斜视、非约束关节将中心圆作为独立
|
||||
交叉检查并参与融合。轴线上一点则由整段
|
||||
相对SE(3)轨迹的 `(I-R)p=t` 方程鲁棒拟合，不再把单目Tag中心自由三维圆的圆心直接
|
||||
当成机械轴心。正面/侧面端视关节只使用图像平面内可观分量，丢弃无法由单目确定的
|
||||
光轴深度；斜视轨迹仍保留姿态轴和独立三维圆轴的交叉检查。每条主动曲线在其baseline命令
|
||||
严格归零：普通通道255，四指侧摆127。左手将食指动态轨迹、右手将小指动态轨迹继承
|
||||
给其余三指。11-Tag布局只能可靠恢复参考指的动态命令—角度曲线，不能证明四根独立
|
||||
电机的绝对装配相位相同；因此四指全部MCP侧摆、MCP屈伸和PIP静态URDF零偏都保留
|
||||
原始CAD的0，只继承动态曲线，避免参考指弯曲或四指整体同向倾斜。
|
||||
|
||||
零位求解使用行程更充分的根轴方向和保持原始CAD直立的参考指MCP pitch实测轴方向确定
|
||||
掌部朝向；两条平行根轴线只确定平移，不再用其单目三维深度间距确定绕根轴的旋转，避免
|
||||
稳定PnP深度偏差被写成拇指roll零偏。另一条短行程根轴方向只作诊断。随后按两条运动链逐关节
|
||||
进行一维鲁棒求解，避免远端异常把已确定的上游零位一起拖到边界。非平行相邻轴先将上下游
|
||||
轴投影到父轴法平面，再计算精确有符号方位角；父子轴夹角是零位无法改变的几何不变量，偏差
|
||||
超过5°直接判定模型失败。平行相邻轴比较两轴之间的径向相位，三维路径忽略连杆长度和沿轴
|
||||
Tag位置，接近轴向观察时改用相机图像平面相位并丢弃单目PnP深度。轴线SE(3)拟合RMS超过
|
||||
1 mm也不允许写URDF。四指静态零位不参与相机相位覆盖，拇指可观测零偏上限20°；
|
||||
小于0.3°或未超过3倍不确定度的稳定偏移保留原始零位0。
|
||||
|
||||
`thumb_mcp` 的动态角度曲线仍由电机15的三轮轨迹直接测量，但其绝对静态零位只可通过
|
||||
被动 `thumb_ip` 的轴线圆心相位间接推断。固定正面单目机位下这条浅圆弧的姿态轴/圆轨迹轴
|
||||
偏差可达数十度，重复性不能排除稳定系统误差，因此不得把该相位写入URDF；左右手
|
||||
`thumb_mcp` 都保留原始CAD零位0。该保护只冻结静态 `origin.rpy`，不会冻结或复制其
|
||||
`angle_rad[256]` 实测轨迹。
|
||||
|
||||
前两轮拟合，第三轮强制留出验证；轨迹与零位角度MAE必须≤1°、P95≤2°，三轮轴/零位
|
||||
差≤0.75°、径向RMS≤3 mm、轴线SE(3)残差≤1 mm。非零修正必须在第三轮优于原始URDF，并通过按三轮分组的
|
||||
训练周期Student-t 95%改善下界检查。最终门限不会因自动重试而放宽。
|
||||
|
||||
单轮姿态相对理想固定轴的轴外RMS与跨轮重复性分别判定：主动关节上限2.5°，被动
|
||||
耦合关节上限7.5°。较宽的被动模型门限只容纳可重复的机构耦合和双Tag PnP系统误差，
|
||||
不会替代三轮轴方向≤0.75°和第三轮MAE/P95留出验证。
|
||||
|
||||
四指参考源的MCP pitch虽有约70°大行程，但侧面机位接近沿转轴观察，单目PnP深度偏差
|
||||
仍可能把低残差的Tag中心圆平面稳定地倾斜。因此MCP pitch与其他端视关节一样，始终用
|
||||
完整相对姿态确定轴方向，Tag中心轨迹只参与轴线位置拟合；不再按10°分界在两种轴模型
|
||||
之间切换。固定Tag安装旋转会在相对旋转中抵消，不需要中心圆回退。被动PIP/DIP继承
|
||||
上游轴方向时不重复报告同一项跨轮轴失败。
|
||||
|
||||
四指MCP侧摆的动态曲线仍由参考指三轮实测并继承，但绝对静态侧摆零位固定使用原始CAD
|
||||
的0。仅凭下游pitch轴相对CAD掌坐标反推roll相位，会把稳定的跨视角/固定几何偏差写成
|
||||
约4°的整指倾斜；重复扫描与同源留出不能排除这种系统偏差，因此不得写入URDF。
|
||||
|
||||
四指MCP屈伸和PIP采用同一静态策略：参考指轨迹仍参与动态曲线、轴质量和机构诊断，
|
||||
但拟合出的共同掌坐标相位不写入四指 `origin.rpy`；只发布各指相对四指中值的实测
|
||||
装配偏差。拇指CMC roll/yaw/pitch的非零修正来自当前会话的完整相对旋转行程与机械
|
||||
端点，视觉轴链继续用于轴线、PnP和留出诊断；代码和配置中不保存任何按左右手或
|
||||
序列号写死的拇指零位角。电机5的256点动态曲线同样使用本机四轮实测结果。
|
||||
|
||||
视觉依赖链仍为：yaw轴检查拇指roll、pitch轴检查拇指yaw、MCP轴线相位检查拇指
|
||||
pitch、IP轴线相位检查拇指MCP。该链用于几何和PnP诊断，不再决定四个具有机械端点
|
||||
的拇指主动关节绝对零位；四指PIP/DIP轴线相位也继续用于机构诊断。
|
||||
原始URDF的 `origin.xyz`、`axis.xyz`、连杆长度、mesh和被动结构固定。yaw扫描时电机5
|
||||
保持145，求解器使用实测 `angle_rad[145]` 还原该条件，不会把145误当成baseline。
|
||||
偏移超过各关节专用上限时整次失败。数值求解会在更宽的诊断范围内继续估计，因此状态和原始JSONL
|
||||
会显示实际估计值及配置上限，而不是把所有超限结果都截断成恰好±20°或±3°；该诊断搜索
|
||||
不会放宽正式结果的硬门限。
|
||||
|
||||
生成修正URDF时只修改通过验收的主动关节 `origin.rpy`，不会修改任何关节的
|
||||
`origin.xyz`、转轴、mimic关系或原始CAD/机械安全限位。256项实测轨迹只保存在最终
|
||||
JSON；任一曲线点越过CAD限位都会阻止正式发布，程序不会自动扩大URDF限位。
|
||||
|
||||
坏帧只丢弃。短时Tag丢失、同步帧中断、扫描超时、端点/分箱不足会自动保持当前位置、
|
||||
重置当前机位PnP、返回基准后重扫当前方向，最多3次；速度依次降为80%/60%/50%，
|
||||
端点保持延长到0.75/1.0/1.25秒，扫描超时按降速比例同步延长。若反馈在远离目标时
|
||||
连续8秒没有至少1个u8的进展，则按机械碰撞/摩擦或硬件故障立即保持当前反馈位置并
|
||||
暂停，不消耗三次采样重试预算。单轮拟合失败只重扫该轮两个方向，全局不一致才重扫
|
||||
完整关节，每关节最多自动重采2轮。过程指标落在最终门限的1.25倍内时会标记为黄色
|
||||
预警，但只要仍超过硬门限，就在当前关节立即使用剩余重试预算
|
||||
（`provisional_fit_warning_rescan`）；第三次仍超限则当场暂停，不允许预警数据继续到
|
||||
后续关节。最终拟合仍按原硬门限验收，因此不会在全部任务采完后才回头重采靠前
|
||||
关节。零位触边、稳定留出误差或URDF几何无法解释属于模型失败，程序
|
||||
只暂停一次且不再自动重扫，防止重复运动；此时也拒绝`resume`形成死循环。其他可恢复
|
||||
失败在预算耗尽后才暂停，`resume`从最小失败单元继续，已通过数据保留。所有失败尝试
|
||||
仍保存在 `raw_samples.jsonl`。若连续两次完整重扫出现轮次和数值都重复的
|
||||
PnP双簇行程，程序将它判为系统性分支失败并当场停止，不再浪费第3次全关节重扫。
|
||||
|
||||
每个新机位/Tag组合开始运动前，不使用单个端点帧直接决定平面Tag的IPPE姿态分支。
|
||||
程序在静止端点联合8帧候选，按相邻Tag相对姿态的跨帧稳定性和重投影误差选择整组
|
||||
分支；侧面Tag 4/5/6/7贴面在该端点应近似平行，初始化还会比较相邻Tag法向，避免
|
||||
错误镜像分支虽然8帧稳定且重投影很小仍被选中。每轮 `255→0` 前都会在静止端点独立
|
||||
重置并重新选择分支，使第三轮同时成为PnP初始化留出，而不是三轮共享同一错误分支。
|
||||
再开始正式轨迹采集。初始化帧不写入轨迹；最终单轴、跨轮和留出门限不变。
|
||||
|
||||
左手测食指roll时将电机7/8/9固定到0；右手测小指roll时因左右手侧摆机构镜像，
|
||||
将电机6/7/8固定到255。两者均为相机画面向右的物理避挡方向，速度分别为
|
||||
`[15,5,15,15,15]` 和 `[15,15,15,15,5]`。参考指MCP pitch/PIP扫描分别使用
|
||||
电机1/16（左）或4/19（右），参考指速度10。
|
||||
|
||||
右手扫描拇指CMC俯仰（电机0）前，程序将拇指横摆电机10和拇指侧摆电机5都固定到
|
||||
255，确认两个辅助关节到位后才允许电机0执行全行程。该关节由正面机位使用掌部
|
||||
Tag 0和运动Tag 1同帧测量。每帧都会保留两个辅助关节
|
||||
的实测条件值，轴线经外参转换到公共坐标系，零位求解按URDF上游关节链补偿；左手
|
||||
仍沿用原有正面机位和基准姿态。
|
||||
|
||||
标定 `thumb_cmc_yaw`（电机10）时，程序将
|
||||
`thumb_cmc_roll`（电机5）固定为145，并在它到位后才开始采样，以保持运动Tag
|
||||
ID 9的可见性和PnP稳定性。当前方向自动重试、失败轮次重试和人工 `resume` 都保持
|
||||
电机5为145，只让电机10返回待重扫方向的起点；电机10全部三轮完成后，电机5才
|
||||
恢复基准值255。自动恢复直接发送恢复目标，不会短暂发送保持当前位置命令；操作员
|
||||
暂停/终止、恢复预算耗尽或机械停滞时仍保持当前位置。最终JSON的
|
||||
`baseline_command_u8` 不变。
|
||||
|
||||
右手标定小指PIP（电机19）时，命令0对应的固件反馈可能稳定饱和在5。只有电机19
|
||||
的0端使用±5反馈容差，并将该实测机械端点归入命令0端点分箱；255端和其他电机仍
|
||||
使用默认±2。轨迹仍须覆盖至少240个u8并通过完整拟合门限，所以中途卡滞不会被误判
|
||||
为端点到达。
|
||||
|
||||
右手拇指横摆电机10在命令255时多次实测稳定饱和在250，因此仅右手电机10的255端
|
||||
使用±5反馈容差；其命令0端实测反馈为4，仍使用±4，其他电机和中间位置不放宽。基准姿态、作为
|
||||
电机0辅助避挡姿态以及电机10自身扫描端点都使用同一条专用判定。
|
||||
|
||||
三机位流程默认 `validation_enabled:=false`，即不增加随机机械动作，但第三轮留出验证
|
||||
始终启用且不能关闭；最终 `quality.validation_mae_rad/p95_rad` 正是第三轮轨迹误差。
|
||||
状态中的扫描进度和总体进度分开显示：42/42只表示计划轨迹已采完，总体进度在拟合和
|
||||
验证完成、正式JSON与URDF成功写入之前不会显示100%。
|
||||
|
||||
上面机位在Tag二维质量合格但PnP连续无效达到1秒时，会自动重置该机位的单Tag
|
||||
和双Tag连续性跟踪器，并从下一帧重新建链，早于3秒采集超时。暂停恢复时只要求
|
||||
当前活动机位就绪；首次调用 `start` 仍要求三个机位全部通过预检。
|
||||
|
||||
### 3. 输出
|
||||
|
||||
通过后生成精简JSON和一个新URDF：
|
||||
|
||||
```text
|
||||
calibration_output/G20_LEFT_001/<时间戳>/
|
||||
g20_left_G20_LEFT_001_calibration.json
|
||||
linkerhand_g20_left_zero_calibrated_G20_LEFT_001_<时间戳>.urdf
|
||||
meshes/*.STL
|
||||
|
||||
calibration_output/G20_RIGHT_001/<时间戳>/
|
||||
g20_right_G20_RIGHT_001_calibration.json
|
||||
linkerhand_g20_right_zero_calibrated_G20_RIGHT_001_<时间戳>.urdf
|
||||
meshes/*.STL
|
||||
```
|
||||
|
||||
文件包含21个关节的256项 `angle_rad`、16个主动关节的 `zero_command_u8`、
|
||||
`zero_angles.urdf_zero_offset_rad`、5个被动标记、模板来源和总体质量。新URDF每次从
|
||||
指定原始CAD文件生成，采用 `T_original × Rot(axis, offset)`，绝不叠加旧校准文件或
|
||||
覆盖原文件；只有通过独立求解验证或明确机械装配基准授权的主动关节 `origin.rpy` 可能
|
||||
改变；当前19-Tag右手的16个主动静态零位字段全部由本会话数据求解；四指DIP和
|
||||
`thumb_ip`为被动关节，发布实测动态曲线并保留CAD静态零位，其mimic坐标偏置只随
|
||||
上游主动关节坐标系变换作等价调整。
|
||||
未观测关节和其他URDF文本保持不变。源URDF中的相对mesh资源会按原相对路径复制到
|
||||
同一会话，保证会话内URDF可独立加载，并在正式发布时逐文件记录SHA256。每帧Tag SE(3)、
|
||||
图像时间戳、20通道状态、同步误差和PnP误差只进入 `raw_samples.jsonl`。
|
||||
|
||||
完整 `raw_samples.jsonl` 已存在时，可以按当前算法离线重放，不连接相机、不发送电机
|
||||
命令。`--output-tag` 为新产物增加安全后缀，已有JSON、URDF和验证报告不会被覆盖：
|
||||
|
||||
```bash
|
||||
python3 -m g20_thumb_apriltag_calibration.offline_replay \
|
||||
calibration_output/G20_RIGHT_001/20260811_120146 \
|
||||
--output-tag AXIS_FRAME_V3 \
|
||||
--write
|
||||
```
|
||||
|
||||
下面保留原有正面拇指独立标定说明和兼容入口。
|
||||
|
||||
### 4. 修正URDF的运行时关节映射
|
||||
|
||||
修正URDF已经把 `zero_angles.urdf_zero_offset_rad` 写入关节
|
||||
`origin.rpy`。仿真运行时只能再使用同一台、同一侧机械手JSON中的256点
|
||||
`angle_rad` 动态曲线，不能把 `urdf_zero_offset_rad` 再加一次，也不能把左手曲线
|
||||
用于右手URDF。可用桥接节点将GUI的20通道u8命令转换为完整21关节
|
||||
`JointState`（包括5个被动关节）：
|
||||
|
||||
```bash
|
||||
ros2 launch g20_thumb_apriltag_calibration calibrated_joint_state_bridge.launch.py \
|
||||
hand_type:=right \
|
||||
calibration_file:=$PWD/calibration_output/G20_RIGHT_001/20260811_120146/g20_right_G20_RIGHT_001_calibration.json
|
||||
```
|
||||
|
||||
schema v5默认订阅 `/g20/cb_right_hand_state`；schema v4默认订阅
|
||||
`/g20/cb_right_hand_control_cmd`。两者均发布
|
||||
`/sim/mujoco/g20/right/joint_state`。启动前必须停止任何旧的同名话题桥，避免两个
|
||||
发布者同时驱动仿真。节点会拒绝左右手不匹配、质量未通过、字段不完整或非有限命令，
|
||||
因此不会静默退回旧标定。
|
||||
|
||||
该包启动海康机器人 MVS USB3 Vision 黑白相机、图像校正、`apriltag_ros`、
|
||||
Linker Hand SDK 和标定状态机，
|
||||
只扫描 G20 左手命令下标 `0`、`15`。默认使用单终点连续模式：每个方向只发送一次
|
||||
终点命令，速度保持在固件能稳定响应的 `15`。SDK 以独立时间戳反馈实际 20 维位置，
|
||||
程序把每帧 AprilTag 三维中心与同一时刻的实际电机位置插值配对并按整数位置分箱。
|
||||
@@ -36,7 +591,8 @@ PnP 双分支跟踪和整段刚体复核仍保留，用于选出稳定的三维
|
||||
`T5` 固定在最末节。四张 Tag 必须与所在刚性件完全固定，不能跨关节或贴在软胶上。
|
||||
- 当前实物使用 `tag36h11` 的 ID `0/1/2/3`，依次对应 T0/T3/T4/T5。如果实物 ID 改变，同时修改
|
||||
`config/front_tags.yaml` 里检测节点和标定节点的两组数组。
|
||||
- `tag.sizes`/`tag_sizes_m` 必须填写每张 Tag 的实测有效边长（米），当前配置为 `0.010`。
|
||||
- `tag.sizes`/`tag_sizes_m` 必须填写每张 Tag 的实测有效边长（米），当前实物黑色正方形实测为
|
||||
`16 mm`，因此配置为 `0.016`。
|
||||
测量检测角点所围成的正方形边长，不包含外围白色留边。
|
||||
- 当前试标定允许四张 Tag 的有效边长至少 30 px（实测静态约 32～38 px），最终仍由
|
||||
静止角度 RMS 和随机复测误差决定是否合格。四张 Tag 必须在全行程内均可见。需要短时检查标记时，
|
||||
@@ -50,11 +606,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 +619,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 会固定使用 `rmw_fastrtps_cpp`，并通过新旧两个 Fast DDS 环境变量
|
||||
加载 `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 +682,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 +693,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 +702,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 +851,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 组件运行
|
||||
在同一个多线程容器内并启用进程内传输，避免在处理链路中重复序列化、复制大图像。
|
||||
@@ -260,7 +870,7 @@ D405 的彩色流来自 `depth_module`，启动文件会同时设置
|
||||
不影响 AprilTag 的 ROI 输入。
|
||||
|
||||
静态预检先在单 Tag 层拒绝高重投影误差，再检查三组相对中心的位置内点率和毫米级 RMS。
|
||||
当前 30～38 px 的 10 mm Tag 属于试标定尺寸，如果中心位置 RMS 持续不合格，应优先增加照明、缩短
|
||||
当前末节16 mm Tag如果中心位置 RMS 持续不合格，应优先增加照明、缩短
|
||||
相机距离或提高 Tag 有效像素，而不是放宽最终随机复测精度。
|
||||
|
||||
启用 rosbag 后保存裁剪后的原始图像和配套 `CameraInfo`，避免新增一个全分辨率图像
|
||||
|
||||
@@ -44,7 +44,7 @@ g20_thumb_calibration:
|
||||
minimum_detection_hz: 15.0
|
||||
maximum_hamming: 0
|
||||
minimum_decision_margin: 30.0
|
||||
# Trial threshold for the current 10 mm tags (observed at 32-38 px).
|
||||
# Trial threshold for small/far tags (historically observed at 32-38 px).
|
||||
# Final acceptance is still guarded by static RMS and random validation.
|
||||
minimum_edge_pixels: 30.0
|
||||
# Current 30 px tags measure about 0.50-0.53 deg RMS while stationary.
|
||||
|
||||
@@ -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>
|
||||
@@ -6,7 +6,7 @@
|
||||
# from back-pressuring image_proc's reliable image publisher.
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.01
|
||||
size: 0.016
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
@@ -20,11 +20,11 @@
|
||||
tag:
|
||||
ids: [0, 1, 2, 3]
|
||||
frames: [tag_t0, tag_t3, tag_t4, tag_t5]
|
||||
sizes: [0.010, 0.010, 0.010, 0.010]
|
||||
sizes: [0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
g20_thumb_calibration:
|
||||
ros__parameters:
|
||||
tag_roles: [t0, t3, t4, t5]
|
||||
tag_ids: [0, 1, 2, 3]
|
||||
tag_frames: [tag_t0, tag_t3, tag_t4, tag_t5]
|
||||
tag_sizes_m: [0.010, 0.010, 0.010, 0.010]
|
||||
tag_sizes_m: [0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
@@ -0,0 +1,37 @@
|
||||
schema_version: 1
|
||||
model: G20
|
||||
side: right
|
||||
tag_layout: g20_right_19
|
||||
serial_number: G20_RIGHT_001
|
||||
can_interface: can0
|
||||
output_root: calibration_output
|
||||
|
||||
cameras:
|
||||
front:
|
||||
serial_number: DB2163742
|
||||
camera_name: hikrobot_front_DB2163742
|
||||
camera_info: ~/.ros/camera_info/hikrobot_DB2163742.yaml
|
||||
side:
|
||||
serial_number: DB2163749
|
||||
camera_name: hikrobot_side_DB2163749
|
||||
camera_info: ~/.ros/camera_info/hikrobot_DB2163749.yaml
|
||||
top:
|
||||
serial_number: DB2163739
|
||||
camera_name: hikrobot_top_DB2163739
|
||||
camera_info: ~/.ros/camera_info/hikrobot_DB2163739.yaml
|
||||
|
||||
artifacts:
|
||||
source_urdf: src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf
|
||||
source_urdf_sha256: eeb6ffb0e95d2a6acd4c26331ae68062e0d74160de4b552b4f6d395cce5ca4e8
|
||||
camera_extrinsics: config/g20_three_camera_extrinsics.yaml
|
||||
camera_extrinsics_sha256: dd623572df3cb83fdefcbe92204dab54a60f2c68eb3a8c9bdb08407e8f0e5d80
|
||||
calibration_config: src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml
|
||||
calibration_config_sha256: 0faaf891ebb616c4c8a3bb3052c48fa4b6c8aa0c5fdc5abaaa89f4fc29cca1c3
|
||||
tag_config: src/g20_thumb_apriltag_calibration/config/three_camera_tags_g20_right_19.yaml
|
||||
tag_config_sha256: b1ab45e97ae42d57b0a3a63c725107b5aa3828c06b2ced16f8222d6e9ebadc41
|
||||
|
||||
release:
|
||||
# Each task already contains three training cycles plus an isolated fourth
|
||||
# holdout, so a second complete hardware session duplicates hours of motion.
|
||||
required_independent_passes: 1
|
||||
static_repeatability_deg: 1.0
|
||||
@@ -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,193 @@
|
||||
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
|
||||
# 19-Tag产品预检仍使用上面保守速度；只有正反预检都留出至少双倍正式分箱余量，
|
||||
# 才把非roll任务正式扫描最多提速1.5倍。四指roll受0.5°回差门限约束，
|
||||
# 始终保持速度5；任一方向采样余量不足也保持原速度。
|
||||
adaptive_formal_speed_enabled: true
|
||||
adaptive_formal_speed_max_scale: 1.5
|
||||
adaptive_formal_speed_minimum_bins: 64
|
||||
adaptive_formal_speed_maximum_bin_gap: 8
|
||||
speed_setting_settle_seconds: 0.25
|
||||
|
||||
# tag36h11尺寸是检测角点围成的黑色正方形边长，不包含外围白边。
|
||||
# 19张Tag的黑色码区外边长均为16 mm。自定义PnP必须与
|
||||
# apriltag_ros逐ID尺寸一致，禁止用纸张/白边尺寸代替码区尺寸。
|
||||
tag_size_m: 0.016
|
||||
# ROS 2无法从YAML空数组推断整数/浮点数组类型。这四个
|
||||
# 末端Tag仍显式写16 mm，防止节点启动时得到未初始化参数。
|
||||
tag_size_override_ids: [7, 14, 16, 18]
|
||||
tag_size_overrides_m: [0.016, 0.016, 0.016, 0.016]
|
||||
repetitions: 3
|
||||
# 19-Tag产品正式零位使用前三轮训练、最后一轮完全留出；旧11-Tag仍读取repetitions=3。
|
||||
g20_right_19_repetitions: 4
|
||||
preflight_frames: 60
|
||||
minimum_detection_rate: 0.95
|
||||
minimum_detection_hz: 15.0
|
||||
minimum_feedback_hz: 25.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
|
||||
# 标定任务不再用第一帧决定平面Tag的IPPE分支；静止端点联合8帧选择整组最稳定解。
|
||||
pnp_group_initialization_frames: 8
|
||||
# 侧面当前任务所需Tag在初始化端点的贴面法向应一致；用此先验消除静态IPPE镜像双解。
|
||||
pnp_group_normal_alignment_scale_deg: 5.0
|
||||
pnp_group_maximum_normal_alignment_deg: 15.0
|
||||
# 三个拇指顶部任务共用预检时冻结的Tag 8位姿。Tag 8仍须实时可见；
|
||||
# 任一角点相对会话基准漂移超过2 px并连续5帧时，判定标定中基准被移动。
|
||||
fixed_base_maximum_corner_drift_px: 2.0
|
||||
fixed_base_movement_confirmation_frames: 5
|
||||
# 仅在拇指MCP/IP同步运动且至少一个候选落入可信区间时，用源URDF mimic
|
||||
# 关系辅助选择IPPE分支；若全部候选超限则退回纯视觉，绝不丢帧，也不生成、
|
||||
# 缩放或替代被动IP的自身Tag实测曲线。
|
||||
thumb_ip_pnp_coupling_multiplier: 1.03
|
||||
thumb_ip_pnp_coupling_scale_deg: 3.0
|
||||
thumb_ip_pnp_maximum_coupling_residual_deg: 7.5
|
||||
top_pnp_invalid_reset_seconds: 1.0
|
||||
# 三维位姿必须与实测20通道状态严格按时间戳配对。
|
||||
maximum_state_image_skew_ms: 50.0
|
||||
|
||||
axis_maximum_plane_rms_m: 0.003
|
||||
# 被动耦合轴只用轨迹确定轴线位置，允许更大的轴向深度噪声；径向和跨轮
|
||||
# 轴线一致性仍沿用严格检查。
|
||||
passive_axis_maximum_plane_rms_m: 0.004
|
||||
axis_maximum_radial_rms_m: 0.003
|
||||
# 整段相对SE(3)运动拟合轴线点；端视关节会投影掉单目PnP光轴深度。
|
||||
axis_maximum_pose_line_rms_m: 0.001
|
||||
# 仅用于运动平面在三维中可观测的斜视关节；近图像平面关节使用姿态轴
|
||||
# 约束三维圆，不让单目平面Tag的深度噪声自由决定转轴方向。
|
||||
axis_maximum_rotation_circle_difference_deg: 1.0
|
||||
# 单轴模型残差与跨轮重复误差分开判定：主动刚性关节要求更严；被动耦合
|
||||
# 关节允许可重复的非理想单轴分量，但仍须通过0.75°跨轮轴差及最终轮留出。
|
||||
active_maximum_rotation_orthogonal_rms_deg: 2.5
|
||||
passive_maximum_rotation_orthogonal_rms_deg: 7.5
|
||||
zero_maximum_axis_cycle_difference_deg: 0.75
|
||||
# 零位无法改变父子轴夹角；超过该值属于CAD/PnP几何错误，不能吸收到零位。
|
||||
zero_maximum_axis_cone_mismatch_deg: 5.0
|
||||
zero_maximum_observability_condition_number: 10000000000.0
|
||||
zero_maximum_offset_deg: 20.0
|
||||
# 四指MCP roll保留严格的装配保护范围。thumb CMC三轴由多轴视觉几何
|
||||
# 求解且不假定电气端点等于CAD上限；thumb_mcp及四指MCP pitch/PIP
|
||||
# 静态零位由实测全行程与CAD机械端点联合求解，不写死为0。
|
||||
zero_finger_maximum_offset_deg: 3.0
|
||||
# 只对实物已确认等同CAD端点的关节使用该限制；CMC电气端点不作此假设。
|
||||
mechanical_endpoint_maximum_offset_deg: 5.0
|
||||
|
||||
endpoint_tolerance_u8: 2.0
|
||||
# 请求命令与固件反馈是两个标定域。稳态检查点允许小幅死区，但反馈
|
||||
# 必须已经稳定；大残差仍由机械卡滞保护处理。
|
||||
steady_checkpoint_command_feedback_tolerance_u8: 8.0
|
||||
steady_checkpoint_maximum_feedback_range_u8: 2.0
|
||||
# 电机10在命令0时实测会稳定反馈为4；该0端使用±4。
|
||||
thumb_yaw_zero_endpoint_tolerance_u8: 4.0
|
||||
# 右手电机10在命令255时多次实测稳定反馈为250；仅右手该端点使用±5。
|
||||
right_thumb_yaw_255_endpoint_tolerance_u8: 5.0
|
||||
# 右手小指PIP电机19在命令0时固件反馈稳定饱和为5；仅其0端使用±5。
|
||||
pinky_pip_zero_endpoint_tolerance_u8: 5.0
|
||||
endpoint_hold_seconds: 0.5
|
||||
# roll零位127必须从两个方向到位并静止采集，禁止用运动中经过127的帧判回差。
|
||||
baseline_hold_seconds: 0.5
|
||||
minimum_baseline_hold_frames: 10
|
||||
# 19-Tag产品每项正式四轮前先做一次低速往返，端点Tag稳定至少2秒。
|
||||
task_precheck_hold_seconds: 2.0
|
||||
position_timeout_seconds: 30.0
|
||||
sweep_timeout_seconds: 90.0
|
||||
# 启动宽限1秒后，反馈连续2秒没有至少1个u8的进展，按机械卡滞立即暂停；
|
||||
# 这类故障不进入遮挡/超时的三次自动重扫。
|
||||
# 低速5也应持续产生反馈进展；5秒无进展即停，减少机构持续顶死时间。
|
||||
motor_stall_timeout_seconds: 2.0
|
||||
motor_stall_startup_grace_seconds: 1.0
|
||||
motor_stall_minimum_progress_u8: 1.0
|
||||
invalid_timeout_seconds: 3.0
|
||||
minimum_sweep_frames: 40
|
||||
minimum_state_span_u8: 240.0
|
||||
minimum_sweep_bins: 32
|
||||
maximum_bin_gap: 16
|
||||
# 可恢复的采样失败自动重扫当前方向；超过次数才暂停等待人工处理。
|
||||
automatic_sweep_retry_limit: 2
|
||||
# 轨迹拟合失败优先只重扫失败轮次；零位/URDF模型失败不重复运动。
|
||||
automatic_fit_retry_limit: 2
|
||||
automatic_motion_retry_limit: 2
|
||||
# 留空为正式标定；设为pinky/ring/middle/index时只采该指正面+侧面roll，
|
||||
# 即使正面baseline回差失败也继续完成侧面对照，并永久锁定本会话URDF发布。
|
||||
cross_view_roll_diagnostic_finger: ""
|
||||
# 过程检查允许25%的黄色预警带，最终验收仍使用下面的严格门限。
|
||||
provisional_warning_ratio: 1.25
|
||||
retry_minimum_speed: 3
|
||||
retry_speed_scales: [0.8, 0.6]
|
||||
retry_endpoint_hold_seconds: [0.75, 1.0]
|
||||
|
||||
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
|
||||
# 以下二维参数只供旧轨迹工具兼容，三机位v4零位不使用二维投影。
|
||||
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
|
||||
maximum_monotonic_correction_deg: 2.0
|
||||
# 旧布局仍用连续扫描正反程差门限；19-Tag产品的连续运动包含速度相关滞后，
|
||||
# 由方向曲线和最终留出验证建模，不再重复硬判。其绝对正反程门禁使用下面
|
||||
# 的九点稳态command_maximum_direction_gap_deg。
|
||||
maximum_hysteresis_deg: 2.0
|
||||
# 19-Tag产品模式额外要求每轮正反方向在各自baseline处绕实测关节轴的角度差
|
||||
# 不超过0.5°；四指roll例外：127以255→127为唯一物理零位，反向分支
|
||||
# 保留实测偏差，并改为检查分支间隙上限及跨轮稳定性。
|
||||
baseline_maximum_hysteresis_deg: 0.5
|
||||
directional_zero_maximum_branch_gap_deg: 2.0
|
||||
directional_zero_maximum_branch_gap_range_deg: 0.3
|
||||
cross_view_roll_maximum_branch_gap_difference_deg: 0.3
|
||||
# 正面roll是Tag中心的二维投影角，侧面roll是三维姿态角。允许一个有界的
|
||||
# 固定比例吸收Tag安装倾角/偏置带来的投影缩放，再严格比较两条曲线形状；
|
||||
# 比例过大、方向相反、形状RMS及两视角各自的四轮重复性仍会失败。
|
||||
cross_view_roll_maximum_shape_rms_deg: 1.25
|
||||
cross_view_roll_maximum_projection_scale_ratio: 1.5
|
||||
passive_maximum_monotonic_correction_deg: 3.0
|
||||
passive_maximum_hysteresis_deg: 2.0
|
||||
# 九点姿态先按实测反馈重新对齐，再用此门限检查固有机械回差；请求命令域
|
||||
# 的两条运行曲线仍原样保留固件方向死区，不能把command/feedback差算成回差。
|
||||
command_maximum_direction_gap_deg: 2.0
|
||||
|
||||
# 默认无额外随机动作；19-Tag产品最终一轮始终作为不可关闭的留出验证。
|
||||
validation_enabled: false
|
||||
# 第四轮留出求解后必须再走8个固定安全组合姿态；三机位规定Tag全部可见
|
||||
# 且实测20通道到位才允许发布。只保存Tag位姿，不保存原始图像。
|
||||
# Developer diagnostic only. The formal fourth sweep cycle already gives
|
||||
# every isolated PIP/DIP pair an independent holdout.
|
||||
combination_validation_enabled: false
|
||||
combination_validation_frames: 10
|
||||
combination_maximum_position_p95_m: 0.003
|
||||
combination_maximum_orientation_p95_deg: 2.0
|
||||
validation_command_count: 3
|
||||
validation_frames: 10
|
||||
validation_seed: 20260804
|
||||
validation_timeout_seconds: 20.0
|
||||
maximum_validation_mae_deg: 1.0
|
||||
maximum_validation_p95_deg: 2.0
|
||||
# 19-Tag产品模式使用更严格的任一点及静态零偏95%置信区间门限。
|
||||
maximum_validation_error_deg: 3.0
|
||||
zero_maximum_confidence_half_width_deg: 1.5
|
||||
@@ -0,0 +1,62 @@
|
||||
/g20_calibration/front/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
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.016, 0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
/g20_calibration/side/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
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.016, 0.016, 0.016, 0.016]
|
||||
|
||||
/g20_calibration/top/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
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.016, 0.016]
|
||||
@@ -0,0 +1,62 @@
|
||||
/g20_calibration/front/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
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, 11, 12, 13]
|
||||
frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll, ring_roll, middle_roll, index_roll]
|
||||
sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
/g20_calibration/side/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
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, 15, 17]
|
||||
frames: [side_base, ring_pip, pinky_pip, middle_pip, index_pip]
|
||||
sizes: [0.016, 0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
/g20_calibration/top/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
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.016, 0.016]
|
||||
@@ -0,0 +1,63 @@
|
||||
/g20_calibration/front/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
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, 11, 12, 13]
|
||||
frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, pinky_roll, ring_roll, middle_roll, index_roll]
|
||||
sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
/g20_calibration/side/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
profile: false
|
||||
max_hamming: 0
|
||||
detector:
|
||||
threads: 4
|
||||
# Distal Tags use the same measured 16 mm black-code edge as all others.
|
||||
decimate: 1.0
|
||||
blur: 0.0
|
||||
refine: true
|
||||
sharpening: 0.25
|
||||
debug: false
|
||||
pose_estimation_method: pnp
|
||||
tag:
|
||||
ids: [4, 5, 6, 7, 14, 15, 16, 17, 18]
|
||||
frames: [side_base, ring_pip, pinky_pip, pinky_dip, ring_dip, middle_pip, middle_dip, index_pip, index_dip]
|
||||
sizes: [0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016, 0.016]
|
||||
|
||||
/g20_calibration/top/apriltag/apriltag:
|
||||
ros__parameters:
|
||||
image_transport: raw
|
||||
qos_profile: sensor_data
|
||||
family: 36h11
|
||||
size: 0.016
|
||||
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.016, 0.016]
|
||||
@@ -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()
|
||||
+297
@@ -0,0 +1,297 @@
|
||||
"""Map G20 u8 feedback to URDF joint angles using one calibration JSON.
|
||||
|
||||
The static encoder-zero corrections in ``zero_angles`` are already baked into
|
||||
the corrected URDF joint origins. This bridge therefore publishes only the
|
||||
dynamic ``angle_rad`` values and never adds the static offsets a second time.
|
||||
|
||||
Schema-v5 trajectories are fitted against timestamp-synchronised hardware
|
||||
feedback, not controller set-points. They must therefore be queried with the
|
||||
SDK ``hand_state`` topic. The retained schema-v4 path is command-indexed for
|
||||
backwards compatibility only.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import JointState
|
||||
|
||||
from .full_hand import (
|
||||
get_hand_calibration_profile,
|
||||
infer_compact_payload_layout,
|
||||
validate_compact_payload,
|
||||
)
|
||||
|
||||
|
||||
G20_COMMAND_NAMES: tuple[str, ...] = (
|
||||
"thumb_cmc_pitch",
|
||||
"index_mcp_pitch",
|
||||
"middle_mcp_pitch",
|
||||
"ring_mcp_pitch",
|
||||
"pinky_mcp_pitch",
|
||||
"thumb_cmc_roll",
|
||||
"index_mcp_roll",
|
||||
"middle_mcp_roll",
|
||||
"ring_mcp_roll",
|
||||
"pinky_mcp_roll",
|
||||
"thumb_cmc_yaw",
|
||||
"reserved_11",
|
||||
"reserved_12",
|
||||
"reserved_13",
|
||||
"reserved_14",
|
||||
"thumb_mcp",
|
||||
"index_pip",
|
||||
"middle_pip",
|
||||
"ring_pip",
|
||||
"pinky_pip",
|
||||
)
|
||||
|
||||
# Match the stable ordering used by the existing MuJoCo bridge. JointState
|
||||
# consumers must use names, but retaining the ordering also keeps logs and
|
||||
# direct comparisons deterministic.
|
||||
G20_URDF_JOINT_NAMES: tuple[str, ...] = (
|
||||
"index_dip",
|
||||
"index_mcp_pitch",
|
||||
"index_mcp_roll",
|
||||
"index_pip",
|
||||
"middle_dip",
|
||||
"middle_mcp_pitch",
|
||||
"middle_mcp_roll",
|
||||
"middle_pip",
|
||||
"pinky_dip",
|
||||
"pinky_mcp_pitch",
|
||||
"pinky_mcp_roll",
|
||||
"pinky_pip",
|
||||
"ring_dip",
|
||||
"ring_mcp_pitch",
|
||||
"ring_mcp_roll",
|
||||
"ring_pip",
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_cmc_roll",
|
||||
"thumb_cmc_yaw",
|
||||
"thumb_ip",
|
||||
"thumb_mcp",
|
||||
)
|
||||
|
||||
|
||||
class CalibratedCommandMapper:
|
||||
"""Validated, side-specific lookup from G20 u8 values to URDF radians."""
|
||||
|
||||
def __init__(
|
||||
self, payload: Mapping[str, Any], *, expected_side: str | None = None
|
||||
) -> None:
|
||||
validate_compact_payload(payload)
|
||||
side = str(payload["side"]).lower()
|
||||
if expected_side is not None and side != str(expected_side).lower():
|
||||
raise ValueError(
|
||||
f"calibration side {side!r} does not match requested side "
|
||||
f"{str(expected_side).lower()!r}"
|
||||
)
|
||||
quality = payload["quality"]
|
||||
if quality.get("passed") is not True:
|
||||
raise ValueError("calibration quality.passed must be true")
|
||||
layout_id = infer_compact_payload_layout(payload)
|
||||
profile = get_hand_calibration_profile(side, layout_id)
|
||||
self.side = side
|
||||
self.layout_id = layout_id
|
||||
self.serial_number = str(payload["serial_number"])
|
||||
schema_version = int(payload["schema_version"])
|
||||
self.input_domain = str(
|
||||
payload.get(
|
||||
"curve_input_domain",
|
||||
"command_u8" if schema_version == 4 else "",
|
||||
)
|
||||
)
|
||||
if self.input_domain not in {"command_u8", "feedback_u8"}:
|
||||
raise ValueError("calibration curve_input_domain is invalid")
|
||||
self._motor_by_joint = {
|
||||
name: int(profile.joint_specs[name].motor_index)
|
||||
for name in G20_URDF_JOINT_NAMES
|
||||
}
|
||||
self._curves = {
|
||||
name: tuple(
|
||||
float(value)
|
||||
for value in payload["joints"][name]["angle_rad"]
|
||||
)
|
||||
for name in G20_URDF_JOINT_NAMES
|
||||
}
|
||||
self._decreasing_curves = {
|
||||
name: tuple(
|
||||
float(value)
|
||||
for value in payload["joints"][name].get(
|
||||
"decreasing_rad", payload["joints"][name]["angle_rad"]
|
||||
)
|
||||
)
|
||||
for name in G20_URDF_JOINT_NAMES
|
||||
}
|
||||
self._increasing_curves = {
|
||||
name: tuple(
|
||||
float(value)
|
||||
for value in payload["joints"][name].get(
|
||||
"increasing_rad", payload["joints"][name]["angle_rad"]
|
||||
)
|
||||
)
|
||||
for name in G20_URDF_JOINT_NAMES
|
||||
}
|
||||
self._previous_by_motor: dict[int, float] = {}
|
||||
self._direction_by_motor: dict[int, str] = {}
|
||||
self.direction_deadband_u8 = 0.5
|
||||
|
||||
@staticmethod
|
||||
def _command_index(value: float) -> int:
|
||||
command = float(value)
|
||||
if not math.isfinite(command):
|
||||
raise ValueError("G20 command positions must be finite")
|
||||
return max(0, min(255, int(math.floor(command + 0.5))))
|
||||
|
||||
def map_positions(
|
||||
self, positions: Sequence[float], names: Sequence[str] = ()
|
||||
) -> tuple[float, ...]:
|
||||
values = tuple(float(value) for value in positions)
|
||||
if names:
|
||||
if len(names) != len(values):
|
||||
raise ValueError(
|
||||
"JointState names and positions must have equal length"
|
||||
)
|
||||
if len(set(names)) != len(names):
|
||||
raise ValueError("JointState names must be unique")
|
||||
by_name = dict(zip((str(name) for name in names), values))
|
||||
missing = [name for name in G20_COMMAND_NAMES if name not in by_name]
|
||||
if missing:
|
||||
raise ValueError(
|
||||
"G20 command is missing named channels: " + ",".join(missing)
|
||||
)
|
||||
command = tuple(by_name[name] for name in G20_COMMAND_NAMES)
|
||||
else:
|
||||
if len(values) != len(G20_COMMAND_NAMES):
|
||||
raise ValueError(
|
||||
"unnamed G20 command must contain exactly 20 positions"
|
||||
)
|
||||
command = values
|
||||
indices = tuple(self._command_index(value) for value in command)
|
||||
direction_by_motor: dict[int, str | None] = {}
|
||||
for motor, value in enumerate(command):
|
||||
previous = self._previous_by_motor.get(motor)
|
||||
direction = self._direction_by_motor.get(motor)
|
||||
if previous is not None:
|
||||
if value > previous + self.direction_deadband_u8:
|
||||
direction = "increasing"
|
||||
elif value < previous - self.direction_deadband_u8:
|
||||
direction = "decreasing"
|
||||
direction_by_motor[motor] = direction
|
||||
result: list[float] = []
|
||||
for name in G20_URDF_JOINT_NAMES:
|
||||
motor = self._motor_by_joint[name]
|
||||
direction = direction_by_motor[motor]
|
||||
curves = (
|
||||
self._increasing_curves
|
||||
if direction == "increasing"
|
||||
else self._decreasing_curves
|
||||
if direction == "decreasing"
|
||||
else self._curves
|
||||
)
|
||||
result.append(curves[name][indices[motor]])
|
||||
for motor, value in enumerate(command):
|
||||
self._previous_by_motor[motor] = value
|
||||
direction = direction_by_motor[motor]
|
||||
if direction is not None:
|
||||
self._direction_by_motor[motor] = direction
|
||||
return tuple(result)
|
||||
|
||||
|
||||
def load_calibrated_command_mapper(
|
||||
calibration_file: str | Path, *, expected_side: str | None = None
|
||||
) -> CalibratedCommandMapper:
|
||||
path = Path(calibration_file).expanduser().resolve()
|
||||
if not path.is_file():
|
||||
raise ValueError(f"calibration JSON does not exist: {path}")
|
||||
payload = json.loads(path.read_text(encoding="utf-8"))
|
||||
return CalibratedCommandMapper(payload, expected_side=expected_side)
|
||||
|
||||
|
||||
def default_input_topic(hand_type: str, input_domain: str) -> str:
|
||||
side = str(hand_type).lower()
|
||||
if side not in {"left", "right"}:
|
||||
raise ValueError("hand_type must be left or right")
|
||||
if input_domain == "feedback_u8":
|
||||
return f"/g20/cb_{side}_hand_state"
|
||||
if input_domain == "command_u8":
|
||||
return f"/g20/cb_{side}_hand_control_cmd"
|
||||
raise ValueError("calibration curve_input_domain is invalid")
|
||||
|
||||
|
||||
class CalibratedJointStateBridge(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("g20_calibrated_joint_state_bridge")
|
||||
self.declare_parameter("hand_type", "right")
|
||||
self.declare_parameter("calibration_file", "")
|
||||
self.declare_parameter("input_topic", "")
|
||||
self.declare_parameter("output_topic", "")
|
||||
|
||||
hand_type = str(self.get_parameter("hand_type").value).lower()
|
||||
if hand_type not in {"left", "right"}:
|
||||
raise ValueError("hand_type must be left or right")
|
||||
calibration_file = str(self.get_parameter("calibration_file").value)
|
||||
if not calibration_file:
|
||||
raise ValueError("calibration_file is required")
|
||||
self.mapper = load_calibrated_command_mapper(
|
||||
calibration_file, expected_side=hand_type
|
||||
)
|
||||
input_topic = str(self.get_parameter("input_topic").value).strip()
|
||||
output_topic = str(self.get_parameter("output_topic").value).strip()
|
||||
self.input_topic = input_topic or default_input_topic(
|
||||
hand_type, self.mapper.input_domain
|
||||
)
|
||||
self.output_topic = (
|
||||
output_topic or f"/sim/mujoco/g20/{hand_type}/joint_state"
|
||||
)
|
||||
self.publisher = self.create_publisher(JointState, self.output_topic, 10)
|
||||
self.subscription = self.create_subscription(
|
||||
JointState, self.input_topic, self._command_callback, 10
|
||||
)
|
||||
self._last_error = ""
|
||||
self.get_logger().info(
|
||||
f"loaded {hand_type} G20 calibration for {self.mapper.serial_number}: "
|
||||
f"{self.input_topic} ({self.mapper.input_domain}) -> "
|
||||
f"{self.output_topic}"
|
||||
)
|
||||
|
||||
def _command_callback(self, command: JointState) -> None:
|
||||
try:
|
||||
positions = self.mapper.map_positions(command.position, command.name)
|
||||
except ValueError as error:
|
||||
message = str(error)
|
||||
if message != self._last_error:
|
||||
self.get_logger().error(message)
|
||||
self._last_error = message
|
||||
return
|
||||
self._last_error = ""
|
||||
result = JointState()
|
||||
result.header = command.header
|
||||
result.name = list(G20_URDF_JOINT_NAMES)
|
||||
result.position = list(positions)
|
||||
self.publisher.publish(result)
|
||||
|
||||
|
||||
def main(args: Sequence[str] | None = None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node: CalibratedJointStateBridge | None = None
|
||||
try:
|
||||
node = CalibratedJointStateBridge()
|
||||
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,227 @@
|
||||
"""Camera-extrinsic data model shared by calibration and runtime nodes."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import json
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
import numpy as np
|
||||
import yaml
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
|
||||
VIEWS: tuple[str, ...] = ("front", "side", "top")
|
||||
|
||||
|
||||
def camera_info_fingerprint(
|
||||
*,
|
||||
width: int,
|
||||
height: int,
|
||||
camera_matrix: Sequence[Sequence[float]] | Sequence[float],
|
||||
distortion: Sequence[float] = (),
|
||||
rectification: Sequence[float] = (),
|
||||
projection: Sequence[float] = (),
|
||||
) -> str:
|
||||
"""Return a stable fingerprint for rectified image geometry."""
|
||||
matrix = np.asarray(camera_matrix, dtype=float).reshape(3, 3)
|
||||
payload = {
|
||||
"width": int(width),
|
||||
"height": int(height),
|
||||
"camera_matrix": [round(float(value), 12) for value in matrix.flat],
|
||||
"distortion": [round(float(value), 12) for value in distortion],
|
||||
"rectification": [round(float(value), 12) for value in rectification],
|
||||
"projection": [round(float(value), 12) for value in projection],
|
||||
}
|
||||
encoded = json.dumps(
|
||||
payload, sort_keys=True, separators=(",", ":")
|
||||
).encode("utf-8")
|
||||
return hashlib.sha256(encoded).hexdigest()
|
||||
|
||||
|
||||
def transform_matrix(
|
||||
translation_xyz_m: Sequence[float],
|
||||
quaternion_xyzw: Sequence[float],
|
||||
) -> np.ndarray:
|
||||
translation = np.asarray(translation_xyz_m, dtype=float)
|
||||
quaternion = np.asarray(quaternion_xyzw, dtype=float)
|
||||
if translation.shape != (3,) or not np.all(np.isfinite(translation)):
|
||||
raise ValueError("translation_xyz_m must contain three finite values")
|
||||
if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)):
|
||||
raise ValueError("quaternion_xyzw must contain four finite values")
|
||||
norm = float(np.linalg.norm(quaternion))
|
||||
if norm < 1.0e-12:
|
||||
raise ValueError("quaternion_xyzw has zero norm")
|
||||
result = np.eye(4, dtype=float)
|
||||
result[:3, :3] = Rotation.from_quat(quaternion / norm).as_matrix()
|
||||
result[:3, 3] = translation
|
||||
return result
|
||||
|
||||
|
||||
def matrix_payload(matrix: Sequence[Sequence[float]]) -> dict[str, list[float]]:
|
||||
value = np.asarray(matrix, dtype=float)
|
||||
if value.shape != (4, 4) or not np.all(np.isfinite(value)):
|
||||
raise ValueError("transform must be a finite 4x4 matrix")
|
||||
return {
|
||||
"translation_xyz_m": [float(item) for item in value[:3, 3]],
|
||||
"quaternion_xyzw": [
|
||||
float(item) for item in Rotation.from_matrix(value[:3, :3]).as_quat()
|
||||
],
|
||||
}
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CameraCalibrationIdentity:
|
||||
serial_number: str
|
||||
width: int
|
||||
height: int
|
||||
intrinsics_sha256: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ThreeCameraExtrinsics:
|
||||
"""Transforms points from each camera optical frame into front optical."""
|
||||
|
||||
cameras: Mapping[str, CameraCalibrationIdentity]
|
||||
front_from_view: Mapping[str, np.ndarray]
|
||||
quality: Mapping[str, float]
|
||||
|
||||
def transform(self, view: str) -> np.ndarray:
|
||||
if view not in self.front_from_view:
|
||||
raise KeyError(f"extrinsics do not contain view {view}")
|
||||
return np.asarray(self.front_from_view[view], dtype=float).copy()
|
||||
|
||||
def camera_matches(
|
||||
self,
|
||||
view: str,
|
||||
*,
|
||||
serial_number: str,
|
||||
width: int,
|
||||
height: int,
|
||||
intrinsics_sha256: str,
|
||||
) -> bool:
|
||||
expected = self.cameras.get(view)
|
||||
return bool(
|
||||
expected is not None
|
||||
and expected.serial_number == str(serial_number)
|
||||
and expected.width == int(width)
|
||||
and expected.height == int(height)
|
||||
and expected.intrinsics_sha256 == str(intrinsics_sha256)
|
||||
)
|
||||
|
||||
|
||||
def validate_extrinsics_payload(payload: Mapping[str, Any]) -> None:
|
||||
if int(payload.get("schema_version", -1)) != 1:
|
||||
raise ValueError("camera extrinsics schema_version must be 1")
|
||||
if payload.get("reference_view") != "front":
|
||||
raise ValueError("camera extrinsics reference_view must be front")
|
||||
cameras = payload.get("cameras")
|
||||
transforms = payload.get("front_from_view")
|
||||
quality = payload.get("quality")
|
||||
if not isinstance(cameras, Mapping) or set(cameras) != set(VIEWS):
|
||||
raise ValueError("camera extrinsics must contain front/side/top cameras")
|
||||
if not isinstance(transforms, Mapping) or set(transforms) != set(VIEWS):
|
||||
raise ValueError("camera extrinsics must contain all three transforms")
|
||||
if not isinstance(quality, Mapping) or not bool(quality.get("passed")):
|
||||
raise ValueError("camera extrinsics quality is not passed")
|
||||
quality_limits = {
|
||||
"reprojection_rms_px": 1.2,
|
||||
"maximum_rotation_repeatability_deg": 0.3,
|
||||
"maximum_translation_repeatability_m": 0.0015,
|
||||
}
|
||||
for key, limit in quality_limits.items():
|
||||
value = float(quality.get(key, float("inf")))
|
||||
if not np.isfinite(value) or value > limit:
|
||||
raise ValueError(
|
||||
f"camera extrinsics {key}={value} exceeds {limit}"
|
||||
)
|
||||
for key in ("front_side_captures", "front_top_captures"):
|
||||
if int(quality.get(key, 0)) < 15:
|
||||
raise ValueError(f"camera extrinsics {key} must be at least 15")
|
||||
for view in VIEWS:
|
||||
identity = cameras[view]
|
||||
if not isinstance(identity, Mapping):
|
||||
raise ValueError(f"{view} camera identity must be an object")
|
||||
if not str(identity.get("serial_number", "")):
|
||||
raise ValueError(f"{view} camera serial_number is missing")
|
||||
if int(identity.get("width", 0)) <= 0 or int(identity.get("height", 0)) <= 0:
|
||||
raise ValueError(f"{view} camera image dimensions are invalid")
|
||||
fingerprint = str(identity.get("intrinsics_sha256", ""))
|
||||
if len(fingerprint) != 64:
|
||||
raise ValueError(f"{view} camera intrinsics fingerprint is invalid")
|
||||
transform = transforms[view]
|
||||
if not isinstance(transform, Mapping):
|
||||
raise ValueError(f"{view} transform must be an object")
|
||||
matrix = transform_matrix(
|
||||
transform.get("translation_xyz_m", ()),
|
||||
transform.get("quaternion_xyzw", ()),
|
||||
)
|
||||
if view == "front" and not np.allclose(matrix, np.eye(4), atol=1.0e-9):
|
||||
raise ValueError("front_from_view.front must be identity")
|
||||
serials = [str(cameras[view]["serial_number"]) for view in VIEWS]
|
||||
if len(set(serials)) != len(VIEWS):
|
||||
raise ValueError("camera extrinsics serial numbers must be unique")
|
||||
|
||||
|
||||
def load_three_camera_extrinsics(path: str | Path) -> ThreeCameraExtrinsics:
|
||||
source = Path(path).expanduser().resolve()
|
||||
if not source.is_file():
|
||||
raise ValueError(f"camera extrinsics file does not exist: {source}")
|
||||
with source.open("r", encoding="utf-8") as stream:
|
||||
payload = yaml.safe_load(stream)
|
||||
if not isinstance(payload, Mapping):
|
||||
raise ValueError("camera extrinsics file must contain an object")
|
||||
validate_extrinsics_payload(payload)
|
||||
cameras = {
|
||||
view: CameraCalibrationIdentity(
|
||||
serial_number=str(payload["cameras"][view]["serial_number"]),
|
||||
width=int(payload["cameras"][view]["width"]),
|
||||
height=int(payload["cameras"][view]["height"]),
|
||||
intrinsics_sha256=str(
|
||||
payload["cameras"][view]["intrinsics_sha256"]
|
||||
),
|
||||
)
|
||||
for view in VIEWS
|
||||
}
|
||||
transforms = {
|
||||
view: transform_matrix(
|
||||
payload["front_from_view"][view]["translation_xyz_m"],
|
||||
payload["front_from_view"][view]["quaternion_xyzw"],
|
||||
)
|
||||
for view in VIEWS
|
||||
}
|
||||
return ThreeCameraExtrinsics(
|
||||
cameras=cameras,
|
||||
front_from_view=transforms,
|
||||
quality={
|
||||
str(key): float(value) if isinstance(value, (int, float)) else value
|
||||
for key, value in payload["quality"].items()
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
def dump_three_camera_extrinsics(
|
||||
path: str | Path,
|
||||
*,
|
||||
cameras: Mapping[str, Mapping[str, Any]],
|
||||
front_from_view: Mapping[str, Sequence[Sequence[float]]],
|
||||
quality: Mapping[str, Any],
|
||||
) -> None:
|
||||
payload = {
|
||||
"schema_version": 1,
|
||||
"reference_view": "front",
|
||||
"cameras": {view: dict(cameras[view]) for view in VIEWS},
|
||||
"front_from_view": {
|
||||
view: matrix_payload(front_from_view[view]) for view in VIEWS
|
||||
},
|
||||
"quality": dict(quality),
|
||||
}
|
||||
validate_extrinsics_payload(payload)
|
||||
destination = Path(path).expanduser().resolve()
|
||||
destination.parent.mkdir(parents=True, exist_ok=True)
|
||||
temporary = destination.with_suffix(destination.suffix + ".tmp")
|
||||
with temporary.open("w", encoding="utf-8") as stream:
|
||||
yaml.safe_dump(payload, stream, allow_unicode=True, sort_keys=False)
|
||||
temporary.replace(destination)
|
||||
+1569
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -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
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,727 @@
|
||||
"""One-command runner for a registered hand-calibration product."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from datetime import datetime
|
||||
import json
|
||||
import os
|
||||
from pathlib import Path
|
||||
import signal
|
||||
import subprocess
|
||||
import sys
|
||||
import time
|
||||
import traceback
|
||||
from typing import Any, Mapping
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import String
|
||||
from std_srvs.srv import Trigger
|
||||
|
||||
from .hikrobot_camera import configure_fastdds_large_image_transport
|
||||
from .operator_report import ProgressEstimator, build_failure_report, render_progress_zh
|
||||
from .product import ProductConfig, load_product_config, sha256_file
|
||||
from .publication import atomic_session_pointer, finalize_session_artifacts
|
||||
from .storage import atomic_write_json
|
||||
|
||||
|
||||
EXIT_PASS = 0
|
||||
EXIT_QUALITY = 2
|
||||
EXIT_SAFETY = 3
|
||||
STATUS_TIMEOUT_SECONDS = 90.0
|
||||
FITTING_STATUS_TIMEOUT_SECONDS = 600.0
|
||||
|
||||
|
||||
def _status_timeout_seconds(status: Mapping[str, Any]) -> float:
|
||||
"""Return the watchdog deadline for the node's current phase.
|
||||
|
||||
Motion and acquisition are expected to publish twice a second and retain
|
||||
the strict transport watchdog. The final 3-D fit is intentionally a
|
||||
synchronous, CPU-bound operation, so its executor cannot service the
|
||||
status timer until the fit returns. The node publishes an explicit
|
||||
FITTING status immediately before entering that operation; allow that
|
||||
known phase enough time without weakening motion safety checks.
|
||||
"""
|
||||
if str(status.get("state", "")) == "FITTING":
|
||||
return FITTING_STATUS_TIMEOUT_SECONDS
|
||||
return STATUS_TIMEOUT_SECONDS
|
||||
|
||||
|
||||
def _calibration_node_exited_before_status(log_path: Path) -> bool:
|
||||
"""Detect a launch child crash while the parent launch is still alive."""
|
||||
try:
|
||||
with log_path.open("rb") as stream:
|
||||
stream.seek(0, os.SEEK_END)
|
||||
size = stream.tell()
|
||||
stream.seek(max(0, size - 65536), os.SEEK_SET)
|
||||
tail = stream.read().decode("utf-8", errors="replace")
|
||||
except OSError:
|
||||
return False
|
||||
return (
|
||||
"[three_camera_calibration_node-" in tail
|
||||
and "]: process has died" in tail
|
||||
)
|
||||
|
||||
|
||||
class CalibrationMonitor(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("g20_calibration_product_runner")
|
||||
self.latest_status: dict[str, Any] = {}
|
||||
self.last_status_at = time.monotonic()
|
||||
self.start_requested = False
|
||||
self.start_future: Any = None
|
||||
self.abort_future: Any = None
|
||||
self.create_subscription(String, "/g20_calibration/status", self._status, 10)
|
||||
self.start_client = self.create_client(Trigger, "/g20_calibration/start")
|
||||
self.abort_client = self.create_client(Trigger, "/g20_calibration/abort")
|
||||
|
||||
def _status(self, message: String) -> None:
|
||||
try:
|
||||
payload = json.loads(message.data)
|
||||
except (TypeError, json.JSONDecodeError):
|
||||
return
|
||||
if isinstance(payload, dict):
|
||||
self.latest_status = payload
|
||||
self.last_status_at = time.monotonic()
|
||||
|
||||
def maybe_start(self) -> None:
|
||||
if self.start_requested or self.latest_status.get("state") != "WAIT_START":
|
||||
return
|
||||
if not self.start_client.service_is_ready():
|
||||
self.start_client.wait_for_service(timeout_sec=0.05)
|
||||
return
|
||||
self.start_requested = True
|
||||
self.start_future = self.start_client.call_async(Trigger.Request())
|
||||
|
||||
def abort(self) -> None:
|
||||
if not self.abort_client.service_is_ready():
|
||||
self.abort_client.wait_for_service(timeout_sec=1.0)
|
||||
if self.abort_client.service_is_ready():
|
||||
self.abort_future = self.abort_client.call_async(Trigger.Request())
|
||||
|
||||
|
||||
class ProgressConsole:
|
||||
def __init__(self, serial_number: str) -> None:
|
||||
self.serial_number = serial_number
|
||||
self.estimator = ProgressEstimator.start()
|
||||
self.last_text = ""
|
||||
self.last_issue = ""
|
||||
|
||||
def update(self, status: Mapping[str, Any]) -> None:
|
||||
text = render_progress_zh(self.serial_number, status, self.estimator)
|
||||
if text == self.last_text:
|
||||
return
|
||||
self.last_text = text
|
||||
if sys.stdout.isatty():
|
||||
sys.stdout.write("\x1b[2J\x1b[H" + text + "\n")
|
||||
sys.stdout.flush()
|
||||
else:
|
||||
print(text, flush=True)
|
||||
reason = str(status.get("reason", ""))
|
||||
if reason.startswith("automatic_retry_") and reason != self.last_issue:
|
||||
self.last_issue = reason
|
||||
active = status.get("active", {})
|
||||
print(
|
||||
"\n".join(
|
||||
[
|
||||
f"⚠ 当前任务出现问题：{reason.removeprefix('automatic_retry_')}",
|
||||
f"系统处理：只重扫当前任务（第 {active.get('automatic_retry_count', 1)}/2 次）",
|
||||
]
|
||||
),
|
||||
flush=True,
|
||||
)
|
||||
|
||||
|
||||
def _default_product_config() -> Path:
|
||||
try:
|
||||
installed = Path(
|
||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
||||
) / "config" / "g20_right_product.yaml"
|
||||
if installed.is_file():
|
||||
return installed
|
||||
except Exception:
|
||||
pass
|
||||
return (
|
||||
Path.cwd()
|
||||
/ "src/g20_thumb_apriltag_calibration/config/g20_right_product.yaml"
|
||||
).resolve()
|
||||
|
||||
|
||||
def _launch_command(
|
||||
config: ProductConfig,
|
||||
session: Path,
|
||||
*,
|
||||
resume_from: Path | None = None,
|
||||
recalibration_scope: str = "full",
|
||||
) -> list[str]:
|
||||
values = {
|
||||
"model": config.model,
|
||||
"hand_type": config.side,
|
||||
"tag_layout": config.tag_layout,
|
||||
"serial_number": config.serial_number,
|
||||
"can_interface": config.can_interface,
|
||||
"session_dir": str(session),
|
||||
"output_root": str(config.output_root),
|
||||
"camera_extrinsics_file": str(config.camera_extrinsics),
|
||||
"source_urdf_path": str(config.source_urdf),
|
||||
"source_urdf_expected_sha256": config.source_urdf_sha256,
|
||||
"corrected_urdf_output_dir": str(session),
|
||||
"calibration_config": str(config.calibration_config),
|
||||
"tag_config": str(config.tag_config),
|
||||
"commands_enabled": "true",
|
||||
"start_cameras": "true",
|
||||
"start_sdk": "true",
|
||||
"record_bag": "false",
|
||||
"validation_enabled": "false",
|
||||
"recalibration_scope": recalibration_scope,
|
||||
}
|
||||
if resume_from is not None:
|
||||
values["resume_raw_samples_path"] = str(
|
||||
resume_from / "raw_samples.jsonl"
|
||||
)
|
||||
for view, camera in config.cameras.items():
|
||||
values[f"{view}_camera_serial"] = camera["serial_number"]
|
||||
values[f"{view}_camera_name"] = camera["camera_name"]
|
||||
values[f"{view}_camera_info_url"] = camera["camera_info"]
|
||||
return [
|
||||
"ros2",
|
||||
"launch",
|
||||
"g20_thumb_apriltag_calibration",
|
||||
"three_camera_calibration.launch.py",
|
||||
*(f"{name}:={value}" for name, value in values.items()),
|
||||
]
|
||||
|
||||
|
||||
def _stop_stack(process: subprocess.Popen[Any]) -> None:
|
||||
if process.poll() is not None:
|
||||
return
|
||||
try:
|
||||
os.killpg(process.pid, signal.SIGINT)
|
||||
except ProcessLookupError:
|
||||
return
|
||||
try:
|
||||
process.wait(timeout=15.0)
|
||||
except subprocess.TimeoutExpired:
|
||||
try:
|
||||
os.killpg(process.pid, signal.SIGTERM)
|
||||
except ProcessLookupError:
|
||||
return
|
||||
try:
|
||||
process.wait(timeout=5.0)
|
||||
except subprocess.TimeoutExpired:
|
||||
try:
|
||||
os.killpg(process.pid, signal.SIGKILL)
|
||||
except ProcessLookupError:
|
||||
return
|
||||
process.wait(timeout=5.0)
|
||||
|
||||
|
||||
def _write_trace(log_path: Path, error: BaseException) -> None:
|
||||
with log_path.open("a", encoding="utf-8") as stream:
|
||||
stream.write("\n[one-command exception]\n")
|
||||
traceback.print_exception(type(error), error, error.__traceback__, file=stream)
|
||||
|
||||
|
||||
def _request_safe_abort(monitor: CalibrationMonitor, timeout_seconds: float = 35.0) -> None:
|
||||
monitor.abort()
|
||||
deadline = time.monotonic() + float(timeout_seconds)
|
||||
while time.monotonic() < deadline and rclpy.ok():
|
||||
rclpy.spin_once(monitor, timeout_sec=0.1)
|
||||
if monitor.latest_status.get("state") == "ABORTED":
|
||||
return
|
||||
|
||||
|
||||
def _run_hardware_session(
|
||||
config: ProductConfig,
|
||||
session: Path,
|
||||
*,
|
||||
resume_from: Path | None = None,
|
||||
recalibration_scope: str = "full",
|
||||
) -> tuple[dict[str, Any], int]:
|
||||
session.mkdir(parents=True, exist_ok=False)
|
||||
(session / "raw_samples.jsonl").touch()
|
||||
log_path = session / "calibration.log"
|
||||
log_stream = log_path.open("a", encoding="utf-8", buffering=1)
|
||||
atomic_session_pointer(config.session_root, "latest_attempt", session)
|
||||
monitor = CalibrationMonitor()
|
||||
console = ProgressConsole(config.serial_number)
|
||||
process: subprocess.Popen[Any] | None = None
|
||||
latest_status: dict[str, Any] = {
|
||||
"state": "PREFLIGHT",
|
||||
"reason": "starting_ros_stack",
|
||||
"progress": 0.0,
|
||||
"views": {},
|
||||
"feedback_hz": 0.0,
|
||||
}
|
||||
exit_code = EXIT_QUALITY
|
||||
try:
|
||||
process = subprocess.Popen(
|
||||
_launch_command(
|
||||
config,
|
||||
session,
|
||||
resume_from=resume_from,
|
||||
recalibration_scope=recalibration_scope,
|
||||
),
|
||||
cwd=config.workspace,
|
||||
stdout=log_stream,
|
||||
stderr=subprocess.STDOUT,
|
||||
text=True,
|
||||
start_new_session=True,
|
||||
)
|
||||
launched_at = time.monotonic()
|
||||
last_render = 0.0
|
||||
last_startup_log_check = 0.0
|
||||
while True:
|
||||
rclpy.spin_once(monitor, timeout_sec=0.1)
|
||||
if monitor.latest_status:
|
||||
latest_status = monitor.latest_status
|
||||
monitor.maybe_start()
|
||||
now = time.monotonic()
|
||||
if now - last_render >= 0.5:
|
||||
console.update(latest_status)
|
||||
last_render = now
|
||||
if monitor.start_future is not None and monitor.start_future.done():
|
||||
response = monitor.start_future.result()
|
||||
if response is None or not response.success:
|
||||
message = "start service failed" if response is None else response.message
|
||||
raise RuntimeError(f"CFG-START-008:{message}")
|
||||
monitor.start_future = None
|
||||
state = str(latest_status.get("state", ""))
|
||||
if state == "COMPLETE":
|
||||
exit_code = EXIT_PASS
|
||||
break
|
||||
if state in {"PAUSED", "ABORTED"}:
|
||||
reason = str(latest_status.get("reason", "calibration_paused"))
|
||||
exit_code = EXIT_SAFETY if "stall" in reason or state == "ABORTED" else EXIT_QUALITY
|
||||
if state == "PAUSED" and "stall" not in reason:
|
||||
# Ordinary quality failures return to the reviewed baseline
|
||||
# before the process tree is stopped. Mechanical stalls
|
||||
# deliberately skip this path and keep the current pose.
|
||||
failure_status = dict(latest_status)
|
||||
_request_safe_abort(monitor)
|
||||
latest_status = failure_status
|
||||
break
|
||||
if process.poll() is not None:
|
||||
raise RuntimeError(f"PUB-STACK-602:ROS stack exited with {process.returncode}")
|
||||
if (
|
||||
not monitor.latest_status
|
||||
and now - last_startup_log_check >= 0.5
|
||||
):
|
||||
last_startup_log_check = now
|
||||
log_stream.flush()
|
||||
if _calibration_node_exited_before_status(log_path):
|
||||
raise RuntimeError(
|
||||
"CAM-STATUS-202:calibration node exited before status"
|
||||
)
|
||||
if (
|
||||
not monitor.latest_status
|
||||
and now - launched_at > STATUS_TIMEOUT_SECONDS
|
||||
):
|
||||
raise RuntimeError("CAM-STATUS-202:no calibration status received")
|
||||
if (
|
||||
monitor.latest_status
|
||||
and now - monitor.last_status_at
|
||||
> _status_timeout_seconds(monitor.latest_status)
|
||||
):
|
||||
raise RuntimeError("MOTION-COMM-303:calibration status stopped")
|
||||
except KeyboardInterrupt as error:
|
||||
latest_status["state"] = "ABORTED"
|
||||
latest_status["reason"] = "operator_abort"
|
||||
_request_safe_abort(monitor)
|
||||
_write_trace(log_path, error)
|
||||
exit_code = EXIT_SAFETY
|
||||
except BaseException as error:
|
||||
latest_status["state"] = "PAUSED"
|
||||
latest_status["reason"] = str(error)
|
||||
_write_trace(log_path, error)
|
||||
exit_code = EXIT_QUALITY
|
||||
finally:
|
||||
if process is not None:
|
||||
_stop_stack(process)
|
||||
monitor.destroy_node()
|
||||
log_stream.flush()
|
||||
os.fsync(log_stream.fileno())
|
||||
log_stream.close()
|
||||
|
||||
if exit_code != EXIT_PASS:
|
||||
_, block = build_failure_report(
|
||||
config,
|
||||
session,
|
||||
latest_status,
|
||||
reason=str(latest_status.get("reason", "unknown_failure")),
|
||||
)
|
||||
print(block, flush=True)
|
||||
return latest_status, exit_code
|
||||
|
||||
|
||||
def _startup_failure_block(path: Path, error: BaseException) -> str:
|
||||
return "\n".join(
|
||||
[
|
||||
"========== 请复制以下内容给开发者 ==========",
|
||||
"结果：FAIL",
|
||||
"错误代码：CFG-PRODUCT-001",
|
||||
"失败阶段：启动静态预检",
|
||||
f"问题：{error}",
|
||||
f"产品配置：{path}",
|
||||
"自动处理：未启动相机、SDK或机械手运动",
|
||||
"建议：复制本诊断块给开发者，不要手工修改哈希绕过检查。",
|
||||
"========== 复制结束 ==========",
|
||||
]
|
||||
)
|
||||
|
||||
|
||||
def _automatic_resume_candidate(config: ProductConfig) -> Path | None:
|
||||
"""Return the newest compatible failed attempt, never a passed session.
|
||||
|
||||
Do not trust only ``latest_attempt``. A process interrupted during the
|
||||
device-only startup gate may have already moved that pointer while still
|
||||
containing no ``session_start`` checkpoint. In that case walk backwards
|
||||
to the preceding usable failed session instead of throwing away hours of
|
||||
completed tasks.
|
||||
"""
|
||||
root = config.session_root
|
||||
try:
|
||||
resolved_root = root.resolve(strict=True)
|
||||
except OSError:
|
||||
return None
|
||||
|
||||
candidates: list[Path] = []
|
||||
pointer = config.session_root / "latest_attempt"
|
||||
if pointer.exists():
|
||||
try:
|
||||
candidates.append(pointer.resolve(strict=True))
|
||||
except OSError:
|
||||
pass
|
||||
try:
|
||||
candidates.extend(
|
||||
sorted(
|
||||
(
|
||||
path
|
||||
for path in root.iterdir()
|
||||
if path.is_dir() and not path.name.startswith("latest_")
|
||||
),
|
||||
key=lambda path: path.name,
|
||||
reverse=True,
|
||||
)
|
||||
)
|
||||
except OSError:
|
||||
return None
|
||||
|
||||
passed_pointer = config.session_root / "latest_passed"
|
||||
passed: Path | None = None
|
||||
if passed_pointer.exists():
|
||||
try:
|
||||
passed = passed_pointer.resolve(strict=True)
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
seen: set[Path] = set()
|
||||
for unresolved in candidates:
|
||||
try:
|
||||
candidate = unresolved.resolve(strict=True)
|
||||
except OSError:
|
||||
continue
|
||||
if candidate in seen:
|
||||
continue
|
||||
seen.add(candidate)
|
||||
if candidate.parent != resolved_root or not candidate.is_dir():
|
||||
continue
|
||||
# A failed attempt older than the current formal release is stale and
|
||||
# must not seed a new independent calibration.
|
||||
if passed is not None and candidate.name <= passed.name:
|
||||
continue
|
||||
raw_path = candidate / "raw_samples.jsonl"
|
||||
if not raw_path.is_file():
|
||||
continue
|
||||
summary_path = candidate / "calibration_summary_zh.json"
|
||||
summary: dict[str, Any] | None = None
|
||||
if summary_path.is_file():
|
||||
try:
|
||||
loaded = json.loads(summary_path.read_text(encoding="utf-8"))
|
||||
except (OSError, json.JSONDecodeError):
|
||||
continue
|
||||
if not isinstance(loaded, dict) or loaded.get("result") != "FAIL":
|
||||
continue
|
||||
summary = loaded
|
||||
hashes = summary.get("hashes", {})
|
||||
if not isinstance(hashes, Mapping):
|
||||
continue
|
||||
if (
|
||||
str(hashes.get("source_urdf_sha256", ""))
|
||||
!= config.source_urdf_sha256
|
||||
or str(hashes.get("camera_extrinsics_sha256", ""))
|
||||
!= config.camera_extrinsics_sha256
|
||||
):
|
||||
continue
|
||||
start: dict[str, Any] | None = None
|
||||
try:
|
||||
with raw_path.open("r", encoding="utf-8") as stream:
|
||||
for line in stream:
|
||||
if not line.strip():
|
||||
continue
|
||||
value = json.loads(line)
|
||||
if (
|
||||
isinstance(value, dict)
|
||||
and value.get("kind") == "session_start"
|
||||
):
|
||||
start = value
|
||||
break
|
||||
except (OSError, json.JSONDecodeError):
|
||||
continue
|
||||
if (
|
||||
start is None
|
||||
or start.get("hand_type") != config.side
|
||||
or start.get("tag_layout") != config.tag_layout
|
||||
or start.get("source_urdf_sha256")
|
||||
!= config.source_urdf_sha256
|
||||
):
|
||||
continue
|
||||
if summary is None:
|
||||
# Ctrl+C can terminate the ROS launch tree before the wrapper gets
|
||||
# a chance to create calibration_summary_zh.json. The immutable
|
||||
# checkpoint itself is enough to resume only after independently
|
||||
# proving that its external geometry still matches the product.
|
||||
try:
|
||||
checkpoint_extrinsics = Path(
|
||||
str(start["camera_extrinsics_file"])
|
||||
).expanduser().resolve(strict=True)
|
||||
if sha256_file(checkpoint_extrinsics) != (
|
||||
config.camera_extrinsics_sha256
|
||||
):
|
||||
continue
|
||||
except (KeyError, OSError, ValueError):
|
||||
continue
|
||||
return candidate
|
||||
return None
|
||||
|
||||
|
||||
def _resolve_partial_base_session(
|
||||
config: ProductConfig, base_session: str | Path | None
|
||||
) -> Path:
|
||||
"""Validate the complete passed session that donates non-target tasks."""
|
||||
if base_session is None or not str(base_session).strip():
|
||||
raise ValueError(
|
||||
"partial scope requires --base-session pointing to a passed "
|
||||
"complete G20 right session"
|
||||
)
|
||||
candidate = Path(base_session).expanduser().resolve(strict=True)
|
||||
root = config.session_root.resolve()
|
||||
if candidate.parent != root or not candidate.is_dir():
|
||||
raise ValueError(
|
||||
"base session must resolve to a direct session directory under "
|
||||
f"{root}"
|
||||
)
|
||||
raw_path = candidate / "raw_samples.jsonl"
|
||||
summary_path = candidate / "calibration_summary_zh.json"
|
||||
payload_path = (
|
||||
candidate
|
||||
/ f"g20_right_{config.serial_number}_calibration.json"
|
||||
)
|
||||
for required in (raw_path, summary_path, payload_path):
|
||||
if not required.is_file():
|
||||
raise ValueError(f"base session is missing required artifact: {required}")
|
||||
try:
|
||||
summary = json.loads(summary_path.read_text(encoding="utf-8"))
|
||||
except json.JSONDecodeError as error:
|
||||
raise ValueError("base session summary is invalid JSON") from error
|
||||
if (
|
||||
not isinstance(summary, dict)
|
||||
or summary.get("result") != "PASS"
|
||||
or not bool(summary.get("quality", {}).get("passed"))
|
||||
):
|
||||
raise ValueError("base session is not a formally passed session")
|
||||
hashes = summary.get("hashes", {})
|
||||
expected_hashes = {
|
||||
"source_urdf_sha256": config.source_urdf_sha256,
|
||||
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
|
||||
"calibration_config_sha256": config.calibration_config_sha256,
|
||||
}
|
||||
if not isinstance(hashes, Mapping) or any(
|
||||
str(hashes.get(name, "")) != expected
|
||||
for name, expected in expected_hashes.items()
|
||||
):
|
||||
raise ValueError(
|
||||
"base session source URDF, camera extrinsics or calibration "
|
||||
"configuration differs from the current product"
|
||||
)
|
||||
return candidate
|
||||
|
||||
|
||||
def run(
|
||||
config_path: str | Path,
|
||||
*,
|
||||
workspace: str | Path | None = None,
|
||||
preflight_only: bool = False,
|
||||
allow_resume: bool = True,
|
||||
scope: str = "full",
|
||||
base_session: str | Path | None = None,
|
||||
) -> int:
|
||||
path = Path(config_path).expanduser().resolve()
|
||||
try:
|
||||
# Resolve every file and camera identity before allowing a hardware
|
||||
# process to start. A second load enables the real CAN existence gate.
|
||||
config = load_product_config(path, workspace=workspace, check_can=False)
|
||||
load_product_config(path, workspace=workspace, check_can=True)
|
||||
selected_scope = str(scope).strip().lower()
|
||||
if selected_scope not in {"full", "thumb", "fingers"}:
|
||||
raise ValueError("scope must be one of: full, thumb, fingers")
|
||||
if selected_scope == "full" and base_session is not None:
|
||||
raise ValueError(
|
||||
"--base-session is valid only with --scope thumb/fingers"
|
||||
)
|
||||
partial_base = None
|
||||
if selected_scope == "fingers" or base_session is not None:
|
||||
partial_base = _resolve_partial_base_session(config, base_session)
|
||||
except BaseException as error:
|
||||
print(_startup_failure_block(path, error), flush=True)
|
||||
return EXIT_QUALITY
|
||||
if preflight_only:
|
||||
print("PASS：产品文件、相机内外参、19张Tag配置和CAN接口静态预检通过。")
|
||||
return EXIT_PASS
|
||||
|
||||
config.session_root.mkdir(parents=True, exist_ok=True)
|
||||
resume_candidate = (
|
||||
partial_base
|
||||
if selected_scope != "full"
|
||||
else (_automatic_resume_candidate(config) if allow_resume else None)
|
||||
)
|
||||
if resume_candidate is not None:
|
||||
if selected_scope == "thumb":
|
||||
print(
|
||||
"拇指专项标定：四指任务继承自已通过会话 "
|
||||
f"{resume_candidate.name}；4项拇指任务将全部重新采集，"
|
||||
"四指零位保持不变。",
|
||||
flush=True,
|
||||
)
|
||||
elif selected_scope == "fingers":
|
||||
print(
|
||||
"四指专项标定：拇指任务和4个拇指零位继承自已通过会话 "
|
||||
f"{resume_candidate.name}；12项四指任务将全部重新采集。",
|
||||
flush=True,
|
||||
)
|
||||
else:
|
||||
print(
|
||||
"检测到兼容的失败会话，将恢复已完整通过的关节任务："
|
||||
f"{resume_candidate.name}。失败中的当前任务会从头重做。",
|
||||
flush=True,
|
||||
)
|
||||
elif selected_scope == "thumb":
|
||||
print(
|
||||
"独立拇指标定：不导入四指会话；仅采集4项拇指任务，"
|
||||
"四指URDF零位保持原始CAD值。",
|
||||
flush=True,
|
||||
)
|
||||
maximum_sessions = config.required_independent_passes
|
||||
for pass_index in range(maximum_sessions):
|
||||
stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||
session = config.session_root / stamp
|
||||
while session.exists():
|
||||
time.sleep(1.0)
|
||||
stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||
session = config.session_root / stamp
|
||||
if maximum_sessions > 1:
|
||||
print(f"正式标定复验：第 {pass_index + 1}/{maximum_sessions} 次", flush=True)
|
||||
status, code = _run_hardware_session(
|
||||
config,
|
||||
session,
|
||||
resume_from=(
|
||||
resume_candidate
|
||||
if selected_scope != "full" or pass_index == 0
|
||||
else None
|
||||
),
|
||||
recalibration_scope=selected_scope,
|
||||
)
|
||||
if code != EXIT_PASS:
|
||||
return code
|
||||
# Keep the exact node-side completion contract durable before the
|
||||
# independent publication layer starts. If publication itself fails,
|
||||
# developers can re-run artifact checks without repeating motion or
|
||||
# inventing lost combination-validation metrics.
|
||||
atomic_write_json(session / "node_status.json", status)
|
||||
try:
|
||||
summary, release_ready = finalize_session_artifacts(
|
||||
config, session, node_status=status
|
||||
)
|
||||
except BaseException as error:
|
||||
_write_trace(session / "calibration.log", error)
|
||||
status = dict(status)
|
||||
status["state"] = "PAUSED"
|
||||
status["reason"] = f"PUB-ARTIFACT-601:{error}"
|
||||
_, block = build_failure_report(config, session, status, reason=status["reason"])
|
||||
print(block, flush=True)
|
||||
return EXIT_QUALITY
|
||||
if release_ready:
|
||||
result_pointer = (
|
||||
config.session_root / "latest_thumb_passed"
|
||||
if selected_scope == "thumb" and partial_base is None
|
||||
else config.session_root / "latest_passed"
|
||||
)
|
||||
print(
|
||||
"\n".join(
|
||||
[
|
||||
f"PASS：{config.model} {config.side} 标定、URDF修正和复验全部通过。",
|
||||
f"正式结果：{result_pointer}",
|
||||
f"JSON：{session / summary['artifacts']['json']}",
|
||||
f"URDF：{summary['artifacts']['urdf']}",
|
||||
]
|
||||
),
|
||||
flush=True,
|
||||
)
|
||||
return EXIT_PASS
|
||||
print("本次会话质量PASS；正在自动执行第二次独立完整复验。", flush=True)
|
||||
return EXIT_QUALITY
|
||||
|
||||
|
||||
def main(args: list[str] | None = None) -> None:
|
||||
parser = argparse.ArgumentParser(description="配置驱动的机械手精密标定")
|
||||
parser.add_argument("--config", default=str(_default_product_config()))
|
||||
parser.add_argument("--workspace", default=None)
|
||||
parser.add_argument("--preflight-only", action="store_true")
|
||||
parser.add_argument(
|
||||
"--scope",
|
||||
choices=("full", "thumb", "fingers"),
|
||||
default="full",
|
||||
help=(
|
||||
"full重新标定全手；thumb仅重采4项拇指任务；"
|
||||
"fingers复用已认证拇指并仅重采12项四指任务"
|
||||
),
|
||||
)
|
||||
parser.add_argument(
|
||||
"--base-session",
|
||||
default=None,
|
||||
help=(
|
||||
"可选：thumb模式将结果合并到该完整会话；"
|
||||
"fingers模式必须提供该基础会话"
|
||||
),
|
||||
)
|
||||
parser.add_argument(
|
||||
"--no-resume",
|
||||
action="store_true",
|
||||
help="忽略失败会话，从第一个关节开始全新采集",
|
||||
)
|
||||
arguments = parser.parse_args(args)
|
||||
configure_fastdds_large_image_transport()
|
||||
ros_log_dir = Path(
|
||||
os.environ.setdefault("ROS_LOG_DIR", "/tmp/g20_calibration_ros_logs")
|
||||
)
|
||||
ros_log_dir.mkdir(parents=True, exist_ok=True)
|
||||
rclpy.init()
|
||||
try:
|
||||
code = run(
|
||||
arguments.config,
|
||||
workspace=arguments.workspace,
|
||||
preflight_only=arguments.preflight_only,
|
||||
allow_resume=not arguments.no_resume,
|
||||
scope=arguments.scope,
|
||||
base_session=arguments.base_session,
|
||||
)
|
||||
finally:
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
raise SystemExit(code)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,590 @@
|
||||
"""Stable Chinese progress and copy/paste diagnostics for non-expert users."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
import time
|
||||
from typing import Any, Mapping
|
||||
|
||||
from .product import ProductConfig
|
||||
from .storage import atomic_write_json
|
||||
from .three_camera_diagnostics import JOINT_NAMES_ZH, STATE_NAMES_ZH, three_camera_reason_zh
|
||||
|
||||
|
||||
def _duration(seconds: float | None) -> str:
|
||||
if seconds is None or not math.isfinite(seconds) or seconds < 0.0:
|
||||
return "计算中"
|
||||
value = int(round(seconds))
|
||||
return f"{value // 60}分{value % 60:02d}秒"
|
||||
|
||||
|
||||
def _task_tag_id_status(views: Mapping[str, Any]) -> str:
|
||||
"""Render every currently required Tag ID with its live visibility."""
|
||||
labels = {"front": "正面", "side": "侧面", "top": "顶部"}
|
||||
ordered_views = [
|
||||
*[name for name in ("front", "side", "top") if name in views],
|
||||
*sorted(name for name in views if name not in labels),
|
||||
]
|
||||
groups: list[str] = []
|
||||
for name in ordered_views:
|
||||
item = views.get(name, {})
|
||||
if not isinstance(item, Mapping):
|
||||
continue
|
||||
required = sorted({int(value) for value in item.get("required_tag_ids", [])})
|
||||
if not required:
|
||||
continue
|
||||
visible = {int(value) for value in item.get("detected_tag_ids", [])}
|
||||
locked = {
|
||||
int(value)
|
||||
for value in item.get("locked_reference_tag_ids", [])
|
||||
}
|
||||
ids = ",".join(
|
||||
f"{tag_id}{'锁' if tag_id in locked else '✓' if tag_id in visible else '✗'}"
|
||||
for tag_id in required
|
||||
)
|
||||
groups.append(f"{labels.get(name, name)}[{ids}]")
|
||||
if not groups:
|
||||
return ""
|
||||
return (
|
||||
" 当前任务ID："
|
||||
+ " ".join(groups)
|
||||
+ "（✓实时可见/锁=基准锁定/✗不可用）"
|
||||
)
|
||||
|
||||
|
||||
def _pnp_wait_status(views: Mapping[str, Any]) -> str:
|
||||
"""Explain why visible Tags have not produced a usable 3-D pose."""
|
||||
labels = {"front": "正面", "side": "侧面", "top": "顶部"}
|
||||
details: list[str] = []
|
||||
for name in ("front", "side", "top"):
|
||||
item = views.get(name, {})
|
||||
if not isinstance(item, Mapping) or item.get("pnp_pose_valid"):
|
||||
continue
|
||||
parts: list[str] = []
|
||||
progress = item.get("pnp_initialization_progress")
|
||||
if isinstance(progress, Mapping):
|
||||
parts.append(
|
||||
"初始化"
|
||||
f"{int(progress.get('accepted', 0))}/"
|
||||
f"{int(progress.get('required', 0))}"
|
||||
)
|
||||
counts: dict[str, int] = {}
|
||||
for field in ("pnp_rejection_counts", "group_pnp_rejection_counts"):
|
||||
values = item.get(field, {})
|
||||
if not isinstance(values, Mapping):
|
||||
continue
|
||||
for reason, count in values.items():
|
||||
counts[str(reason)] = counts.get(str(reason), 0) + int(count)
|
||||
if counts:
|
||||
common = sorted(
|
||||
counts.items(), key=lambda pair: (-pair[1], pair[0])
|
||||
)[:2]
|
||||
parts.append(
|
||||
"拒绝=" + ",".join(f"{reason}×{count}" for reason, count in common)
|
||||
)
|
||||
if parts:
|
||||
details.append(f"{labels[name]} " + "；".join(parts))
|
||||
if not details:
|
||||
return ""
|
||||
return "PnP：" + " | ".join(details) + "（Tag可见不等于三维位姿有效）"
|
||||
|
||||
|
||||
@dataclass
|
||||
class ProgressEstimator:
|
||||
started_at: float
|
||||
last_progress: float = 0.0
|
||||
last_progress_at: float = 0.0
|
||||
seconds_per_fraction: float | None = None
|
||||
|
||||
@classmethod
|
||||
def start(cls) -> "ProgressEstimator":
|
||||
now = time.monotonic()
|
||||
return cls(started_at=now, last_progress_at=now)
|
||||
|
||||
def remaining(self, progress: float, now: float | None = None) -> float | None:
|
||||
current = time.monotonic() if now is None else float(now)
|
||||
value = float(max(0.0, min(1.0, progress)))
|
||||
delta = value - self.last_progress
|
||||
elapsed = current - self.last_progress_at
|
||||
if delta >= 0.002 and elapsed > 0.0:
|
||||
estimate = elapsed / delta
|
||||
self.seconds_per_fraction = (
|
||||
estimate
|
||||
if self.seconds_per_fraction is None
|
||||
else 0.8 * self.seconds_per_fraction + 0.2 * estimate
|
||||
)
|
||||
self.last_progress = value
|
||||
self.last_progress_at = current
|
||||
if value >= 1.0:
|
||||
return 0.0
|
||||
if self.seconds_per_fraction is not None:
|
||||
return max(0.0, self.seconds_per_fraction * (1.0 - value))
|
||||
if value >= 0.02:
|
||||
return max(0.0, (current - self.started_at) * (1.0 - value) / value)
|
||||
return None
|
||||
|
||||
|
||||
def render_progress_zh(
|
||||
serial_number: str,
|
||||
status: Mapping[str, Any],
|
||||
estimator: ProgressEstimator,
|
||||
) -> str:
|
||||
progress = float(status.get("progress", 0.0))
|
||||
active = status.get("active", {})
|
||||
if not isinstance(active, Mapping):
|
||||
active = {}
|
||||
state = str(status.get("state", "PREFLIGHT"))
|
||||
stage = STATE_NAMES_ZH.get(state, state)
|
||||
preflight_mode = str(status.get("preflight_mode", ""))
|
||||
if preflight_mode == "device_only_before_baseline":
|
||||
stage = "设备连接预检"
|
||||
elif preflight_mode == "recovering_baseline":
|
||||
stage = "安全恢复基准形态"
|
||||
elif preflight_mode == "baseline_tags_after_recovery":
|
||||
stage = "基准姿态标签预检"
|
||||
elif str(status.get("reason", "")) == (
|
||||
"holding_same_finger_clearance_before_next_task"
|
||||
):
|
||||
stage = "保持避让姿态，切换同指下一项"
|
||||
elif str(status.get("reason", "")) == (
|
||||
"waiting_for_task_tags_at_sweep_start"
|
||||
):
|
||||
stage = "已到扫描起点，等待任务Tag"
|
||||
joints = active.get("joints", [])
|
||||
joint = "/".join(JOINT_NAMES_ZH.get(str(name), str(name)) for name in joints)
|
||||
if str(active.get("task_name", "")).endswith("_roll_multiview") and joints:
|
||||
joint = (
|
||||
JOINT_NAMES_ZH.get(str(joints[0]), str(joints[0]))
|
||||
+ "（正面+侧面同步）"
|
||||
)
|
||||
if not joint:
|
||||
import_progress = status.get("base_import", {})
|
||||
import_fraction = (
|
||||
float(import_progress.get("fraction", 0.0))
|
||||
if isinstance(import_progress, Mapping)
|
||||
else 0.0
|
||||
)
|
||||
joint = str(active.get("label_zh", "")) or (
|
||||
f"导入基础会话 {100.0 * import_fraction:.1f}%"
|
||||
if state in {"IMPORTING_BASE", "REVALIDATING_INHERITED"}
|
||||
else (
|
||||
"等待设备" if state in {"PREFLIGHT", "WAIT_START"} else "全手"
|
||||
)
|
||||
)
|
||||
direction = {
|
||||
"decreasing": "递减",
|
||||
"increasing": "递增",
|
||||
}.get(str(active.get("direction", "")), "-")
|
||||
cycle = active.get("cycle", "-")
|
||||
repetitions = active.get("repetitions", 4)
|
||||
requested = active.get(
|
||||
"current_motion_target_u8",
|
||||
active.get("command_u8", active.get("pose_name", "-")),
|
||||
)
|
||||
actual = active.get("actual_u8", "-")
|
||||
views = status.get("views", {})
|
||||
if not isinstance(views, Mapping):
|
||||
views = {}
|
||||
ready_cameras = sum(
|
||||
bool(item.get("camera_info_valid") and item.get("camera_extrinsics_valid"))
|
||||
for item in views.values()
|
||||
if isinstance(item, Mapping)
|
||||
)
|
||||
required_tags = sum(
|
||||
len(item.get("required_tag_ids", []))
|
||||
for item in views.values()
|
||||
if isinstance(item, Mapping)
|
||||
)
|
||||
visible_tags = sum(
|
||||
len(
|
||||
{
|
||||
*item.get("detected_tag_ids", []),
|
||||
*item.get("locked_reference_tag_ids", []),
|
||||
}
|
||||
)
|
||||
for item in views.values()
|
||||
if isinstance(item, Mapping)
|
||||
)
|
||||
locked_tag_count = sum(
|
||||
len(item.get("locked_reference_tag_ids", []))
|
||||
for item in views.values()
|
||||
if isinstance(item, Mapping)
|
||||
)
|
||||
configured_tags = sum(
|
||||
len(item.get("configured_tag_ids", item.get("required_tag_ids", [])))
|
||||
for item in views.values()
|
||||
if isinstance(item, Mapping)
|
||||
)
|
||||
visible_configured_tags = sum(
|
||||
len(
|
||||
item.get(
|
||||
"visible_configured_tag_ids",
|
||||
item.get("detected_tag_ids", []),
|
||||
)
|
||||
)
|
||||
for item in views.values()
|
||||
if isinstance(item, Mapping)
|
||||
)
|
||||
occlusion_allowed = bool(
|
||||
state in {"PREFLIGHT", "WAIT_START"}
|
||||
or active.get("pose_name")
|
||||
)
|
||||
if preflight_mode == "device_only_before_baseline":
|
||||
tag_status = (
|
||||
f"当前 {visible_configured_tags}/{configured_tags}"
|
||||
"（基准恢复后检查）"
|
||||
)
|
||||
elif preflight_mode == "recovering_baseline":
|
||||
tag_status = (
|
||||
f"当前 {visible_configured_tags}/{configured_tags}"
|
||||
"（恢复中不作为门槛）"
|
||||
)
|
||||
elif occlusion_allowed:
|
||||
tag_status = (
|
||||
f"当前 {visible_configured_tags}/{configured_tags}（允许遮挡） "
|
||||
f"本阶段必需 {visible_tags}/{required_tags}"
|
||||
)
|
||||
else:
|
||||
tag_status = (
|
||||
f"{visible_tags}/{required_tags} 可见"
|
||||
+ (f"（含锁定 {locked_tag_count}）" if locked_tag_count else "")
|
||||
)
|
||||
task_tag_ids = (
|
||||
_task_tag_id_status(views)
|
||||
if state
|
||||
in {
|
||||
"PREPARE_SWEEP",
|
||||
"SWEEP",
|
||||
"VALIDATION_MOVE",
|
||||
"VALIDATION_CAPTURE",
|
||||
}
|
||||
else ""
|
||||
)
|
||||
retry = int(active.get("automatic_retry_count", 0))
|
||||
fit_attempt = int(active.get("fit_attempt", 1))
|
||||
fit_attempt_limit = int(active.get("fit_attempt_limit", 1))
|
||||
feedback_hz = float(status.get("feedback_hz", 0.0))
|
||||
eta = (
|
||||
"等待Tag"
|
||||
if str(status.get("reason", ""))
|
||||
== "waiting_for_task_tags_at_sweep_start"
|
||||
else _duration(estimator.remaining(progress))
|
||||
)
|
||||
lines = [
|
||||
f"[{serial_number}] 标定中 {progress * 100:5.1f}% 预计剩余 {eta}",
|
||||
f"阶段：{stage}（第 {cycle}/{repetitions} 轮）",
|
||||
f"任务：{joint} 命令/反馈：{requested}/{actual} 方向：{direction}",
|
||||
f"Tag：{tag_status}{task_tag_ids} 相机：{ready_cameras}/3 正常 反馈：{feedback_hz:.1f} Hz",
|
||||
f"质量：有效帧 {active.get('valid_frames', 0)} 已自动重扫 {retry} 次",
|
||||
]
|
||||
if str(status.get("reason", "")) == "waiting_for_task_tags_at_sweep_start":
|
||||
pnp_wait = _pnp_wait_status(views)
|
||||
if pnp_wait:
|
||||
lines.append(pnp_wait)
|
||||
fit_retry_cycles = [
|
||||
int(cycle) for cycle in active.get("fit_retry_cycles", [])
|
||||
]
|
||||
if fit_attempt > 1 and fit_retry_cycles:
|
||||
cycle_text = "/".join(str(cycle) for cycle in fit_retry_cycles)
|
||||
if len(fit_retry_cycles) >= repetitions:
|
||||
lines.append(
|
||||
"拟合：首次拟合未能定位单一异常轮，"
|
||||
f"联合补采第 {cycle_text} 轮双向"
|
||||
)
|
||||
else:
|
||||
lines.append(
|
||||
f"拟合：首次拟合定位第 {cycle_text} 轮需复核，"
|
||||
"仅补采对应轮次双向；其余轮次数据保留"
|
||||
)
|
||||
elif fit_attempt_limit > 1:
|
||||
lines.append(
|
||||
f"拟合：任务级硬门限检查（最多允许 "
|
||||
f"{fit_attempt_limit - 1} 次异常轮补采）"
|
||||
)
|
||||
resume = status.get("resume", {})
|
||||
if isinstance(resume, Mapping) and resume.get("used"):
|
||||
lines.append(
|
||||
"断点：已恢复 "
|
||||
f"{int(resume.get('completed_task_count', 0))}/"
|
||||
f"{int(resume.get('total_task_count', 16))} 个完整任务；"
|
||||
"失败任务已丢弃并重新采集"
|
||||
)
|
||||
return "\n".join(lines)
|
||||
|
||||
|
||||
def classify_error(reason: str, status: Mapping[str, Any]) -> tuple[str, str, str]:
|
||||
value = str(reason)
|
||||
active = status.get("active", {})
|
||||
if not isinstance(active, Mapping):
|
||||
active = {}
|
||||
views = status.get("views", {})
|
||||
missing = [
|
||||
tag
|
||||
for item in views.values()
|
||||
if isinstance(item, Mapping)
|
||||
for tag in item.get("missing_tag_ids", [])
|
||||
] if isinstance(views, Mapping) else []
|
||||
group_pnp_reasons = [
|
||||
str(item.get("group_pnp_reason"))
|
||||
for item in views.values()
|
||||
if isinstance(item, Mapping) and item.get("group_pnp_reason")
|
||||
] if isinstance(views, Mapping) else []
|
||||
pnp_diagnostics_present = bool(
|
||||
group_pnp_reasons
|
||||
or (
|
||||
isinstance(views, Mapping)
|
||||
and any(
|
||||
isinstance(item, Mapping)
|
||||
and any(
|
||||
item.get(field)
|
||||
for field in (
|
||||
"pnp_rejection_counts",
|
||||
"group_pnp_rejection_counts",
|
||||
"pnp_initialization_progress",
|
||||
"group_missing_candidate_roles",
|
||||
)
|
||||
)
|
||||
for item in views.values()
|
||||
)
|
||||
)
|
||||
)
|
||||
if value.startswith("CFG-"):
|
||||
return value, "产品配置或文件预检失败", "不要移动相机；复制本诊断块给开发者。"
|
||||
if value.startswith("DATA-CONTRACT-"):
|
||||
return (
|
||||
value.split(":", 1)[0],
|
||||
"标定样本的请求命令与反馈值数据契约不完整",
|
||||
"机械手无需重新采集；保留原始会话并用离线重放验证，"
|
||||
"复制契约错误给开发者。",
|
||||
)
|
||||
if value.startswith("CAM-STATUS-202"):
|
||||
return (
|
||||
"CAM-STATUS-202",
|
||||
"标定节点启动后未发布状态",
|
||||
"机械手尚未开始运动；查看calibration.log中首个节点异常后从断点继续。",
|
||||
)
|
||||
if value.startswith("MOTION-COMM-303"):
|
||||
return (
|
||||
"MOTION-COMM-303",
|
||||
"标定节点状态心跳停止更新",
|
||||
"已采集的完整任务会保留；检查calibration.log后从断点继续，不要重新采集。",
|
||||
)
|
||||
if value == "fixed_base_reference_moved":
|
||||
return (
|
||||
"CAM-REFERENCE-204",
|
||||
"顶部基准Tag在本次标定开始后发生移动",
|
||||
"Tag 8可在新会话预检前重新摆放；固定Tag 8和顶部相机后启动新会话。",
|
||||
)
|
||||
if "palm_orientation_quality_failed" in value:
|
||||
return (
|
||||
"VAL-QUALITY-501",
|
||||
"掌部方向校正的可观测轨迹不足",
|
||||
"保持Tag安装不变；调整标定前手位，使正面Tag 10–13至少三枚"
|
||||
"在对应运动起始段可见。",
|
||||
)
|
||||
if "motor_state_stalled" in value:
|
||||
return "MOTION-STALL-301", "电机反馈停止向目标推进，程序已保持当前位置", "先检查机械卡阻，未排除前不要重复强推。"
|
||||
if any(
|
||||
token in value
|
||||
for token in (
|
||||
"sweep_missing_endpoint_bin",
|
||||
"sweep_bins_too_few",
|
||||
"sweep_bin_gap_too_large",
|
||||
"task_precheck_missing_command_127",
|
||||
)
|
||||
):
|
||||
return "OBS-SAMPLE-104", "当前任务的有效视觉轨迹不完整", "根据诊断中的关节、机位和缺失区间处理遮挡或反光后重新运行。"
|
||||
if (
|
||||
("synchronised" in value or "sweep_start_tag_timeout" in value)
|
||||
and not missing
|
||||
and pnp_diagnostics_present
|
||||
):
|
||||
problem = (
|
||||
"Tag可见，但三维PnP轨迹在运动中失效"
|
||||
if value.startswith("synchronised")
|
||||
and int(active.get("valid_frames", 0) or 0) > 0
|
||||
else "Tag可见，但三维PnP位姿初始化未完成"
|
||||
)
|
||||
return (
|
||||
"CAM-GEOMETRY-201",
|
||||
problem,
|
||||
"不要根据二维码可见性调整Tag；复制累计PnP拒绝原因给开发者。",
|
||||
)
|
||||
if missing or "tag" in value or "detection" in value or "synchronised" in value:
|
||||
return "OBS-TAG-103", f"所需Tag不可用或持续丢失：{missing}", "检查Tag是否脱落、翘起、反光或被遮挡后重新运行。"
|
||||
if "state" in value and ("timeout" in value or "lost" in value):
|
||||
return "MOTION-COMM-302", "机械手反馈中断或频率不足", "检查CAN接口和机械手供电后重新运行。"
|
||||
if "camera" in value or "pnp" in value or "extrinsics" in value:
|
||||
return "CAM-GEOMETRY-201", "相机、内外参或位姿求解未通过", "确认三台相机没有移动，然后复制本诊断块给开发者。"
|
||||
if "checkpoint" in value:
|
||||
return "FIT-CHECKPOINT-402", "稳态检查点采集流程未完整结束", "完整样本已保留；复制本诊断块给开发者检查采集状态机。"
|
||||
if "fit" in value or "trajectory" in value or "axis" in value:
|
||||
return "FIT-MODEL-401", "关节轴或动态曲线拟合未达到精度门限", "不要放宽门限；复制本诊断块给开发者分析原始样本。"
|
||||
if (
|
||||
"endpoint_zero_offsets' is not defined" in value
|
||||
or "validated_endpoint_zero_state" in value
|
||||
):
|
||||
return (
|
||||
"PUB-ARTIFACT-601",
|
||||
"已验证零位状态未能完整传递到URDF发布阶段",
|
||||
"采集与验证数据仍可保留；不要移动相机或Tag，复制诊断块给开发者。",
|
||||
)
|
||||
if "validation" in value or "combination" in value or "quality" in value or "zero" in value:
|
||||
return "VAL-QUALITY-501", "留出验证或URDF零位验证未通过", "结果不会发布；复制本诊断块给开发者。"
|
||||
if "publish" in value or "artifact" in value or "URDF" in value:
|
||||
return "PUB-ARTIFACT-601", "结果文件校验或原子发布失败", "原始URDF未被覆盖；复制本诊断块给开发者。"
|
||||
return "PUB-UNEXPECTED-699", "标定程序出现未分类异常", "复制本诊断块给开发者，完整堆栈已写入calibration.log。"
|
||||
|
||||
|
||||
def build_failure_report(
|
||||
config: ProductConfig,
|
||||
session: str | Path,
|
||||
status: Mapping[str, Any],
|
||||
*,
|
||||
reason: str,
|
||||
) -> tuple[dict[str, Any], str]:
|
||||
directory = Path(session).resolve()
|
||||
code, problem, suggestion = classify_error(reason, status)
|
||||
active = status.get("active", {})
|
||||
if not isinstance(active, Mapping):
|
||||
active = {}
|
||||
else:
|
||||
active = dict(active)
|
||||
views = status.get("views", {})
|
||||
if isinstance(views, Mapping):
|
||||
group_pnp_reasons = {
|
||||
str(view): str(item.get("group_pnp_reason"))
|
||||
for view, item in views.items()
|
||||
if isinstance(item, Mapping) and item.get("group_pnp_reason")
|
||||
}
|
||||
if group_pnp_reasons:
|
||||
active["group_pnp_reasons"] = group_pnp_reasons
|
||||
for field in (
|
||||
"pnp_rejection_counts",
|
||||
"group_pnp_rejection_counts",
|
||||
"pnp_initialization_progress",
|
||||
"group_missing_candidate_roles",
|
||||
"pnp_candidate_diagnostics",
|
||||
):
|
||||
values = {
|
||||
str(view): item.get(field)
|
||||
for view, item in views.items()
|
||||
if isinstance(item, Mapping) and item.get(field)
|
||||
}
|
||||
if values:
|
||||
active[field] = values
|
||||
try:
|
||||
explanation, automatic_action = three_camera_reason_zh(
|
||||
str(status.get("state", "")), reason, active
|
||||
)
|
||||
except Exception:
|
||||
explanation, automatic_action = problem, "已停止本次发布并保留全部诊断数据"
|
||||
camera_state = {
|
||||
view: (
|
||||
"OK"
|
||||
if isinstance(item, Mapping)
|
||||
and item.get("camera_info_valid", item.get("ready"))
|
||||
and item.get("camera_extrinsics_valid", item.get("ready"))
|
||||
and item.get("stream_alive", item.get("ready"))
|
||||
else "NOT_READY"
|
||||
)
|
||||
for view, item in (views.items() if isinstance(views, Mapping) else [])
|
||||
}
|
||||
session_id = f"{config.serial_number}_{directory.name}"
|
||||
metrics = {
|
||||
"valid_frames": active.get("valid_frames"),
|
||||
"sample": active.get("sample", {}),
|
||||
"detection_frames": active.get("detection_frames"),
|
||||
"detection_valid_frames": active.get("detection_valid_frames"),
|
||||
"detection_rate": active.get("detection_rate"),
|
||||
"detection_rate_by_view": active.get("detection_rate_by_view", {}),
|
||||
"automatic_retry_count": active.get("automatic_retry_count", 0),
|
||||
}
|
||||
pnp_diagnostics = {
|
||||
str(view): {
|
||||
field: item.get(field)
|
||||
for field in (
|
||||
"group_pnp_reason",
|
||||
"group_missing_candidate_roles",
|
||||
"pnp_rejections",
|
||||
"pnp_rejection_counts",
|
||||
"group_pnp_rejection_counts",
|
||||
"pnp_initialization_progress",
|
||||
"pnp_candidate_diagnostics",
|
||||
)
|
||||
if item.get(field)
|
||||
}
|
||||
for view, item in (
|
||||
views.items() if isinstance(views, Mapping) else []
|
||||
)
|
||||
if isinstance(item, Mapping)
|
||||
and any(
|
||||
item.get(field)
|
||||
for field in (
|
||||
"group_pnp_reason",
|
||||
"group_missing_candidate_roles",
|
||||
"pnp_rejections",
|
||||
"pnp_rejection_counts",
|
||||
"group_pnp_rejection_counts",
|
||||
"pnp_initialization_progress",
|
||||
"pnp_candidate_diagnostics",
|
||||
)
|
||||
)
|
||||
}
|
||||
payload: dict[str, Any] = {
|
||||
"schema_version": 1,
|
||||
"serial_number": config.serial_number,
|
||||
"session_id": session_id,
|
||||
"result": "FAIL",
|
||||
"error_code": code,
|
||||
"stage": str(status.get("state", "startup")),
|
||||
"reason": str(reason),
|
||||
"problem_zh": problem,
|
||||
"explanation_zh": explanation,
|
||||
"automatic_action_zh": automatic_action,
|
||||
"suggestion_zh": suggestion,
|
||||
"metrics": metrics,
|
||||
"pnp_diagnostics": pnp_diagnostics,
|
||||
"camera_state": camera_state,
|
||||
"feedback_hz": status.get("feedback_hz", 0.0),
|
||||
"hashes": {
|
||||
"product_config_sha256": __import__("hashlib").sha256(config.path.read_bytes()).hexdigest(),
|
||||
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
|
||||
"calibration_config_sha256": config.calibration_config_sha256,
|
||||
"source_urdf_sha256": config.source_urdf_sha256,
|
||||
},
|
||||
"session_dir": str(directory),
|
||||
"quality": {"passed": False},
|
||||
}
|
||||
atomic_write_json(directory / "calibration_summary_zh.json", payload)
|
||||
block = "\n".join(
|
||||
[
|
||||
"========== 请复制以下内容给开发者 ==========",
|
||||
f"会话编号：{session_id}",
|
||||
"结果：FAIL",
|
||||
f"错误代码：{code}",
|
||||
f"失败阶段：{payload['stage']}",
|
||||
f"问题：{problem}",
|
||||
f"详细说明：{explanation}",
|
||||
f"自动处理：{automatic_action}",
|
||||
f"关键指标：{json.dumps(metrics, ensure_ascii=False, separators=(',', ':'))}",
|
||||
"PnP诊断："
|
||||
+ json.dumps(
|
||||
pnp_diagnostics,
|
||||
ensure_ascii=False,
|
||||
separators=(",", ":"),
|
||||
),
|
||||
f"相机状态：{json.dumps(camera_state, ensure_ascii=False, separators=(',', ':'))}",
|
||||
f"反馈状态：{float(payload['feedback_hz'] or 0.0):.1f} Hz",
|
||||
f"配置哈希：{payload['hashes']['product_config_sha256']}",
|
||||
f"外参哈希：{config.camera_extrinsics_sha256}",
|
||||
f"源 URDF 哈希：{config.source_urdf_sha256}",
|
||||
f"会话目录：{directory}",
|
||||
f"建议：{suggestion}",
|
||||
"========== 复制结束 ==========",
|
||||
]
|
||||
)
|
||||
return payload, block
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,403 @@
|
||||
"""Immutable product identity and shared calibration preflight.
|
||||
|
||||
Model-specific kinematics live in registered calibration contracts. The
|
||||
loader itself only verifies that the selected contract, Tag configuration,
|
||||
cameras and immutable artifacts agree, so adding another hand does not
|
||||
require another copy of the one-command runner.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
import hashlib
|
||||
from pathlib import Path
|
||||
import re
|
||||
from typing import Any, Mapping
|
||||
|
||||
import yaml
|
||||
|
||||
from .extrinsics import camera_info_fingerprint, load_three_camera_extrinsics
|
||||
from .full_hand import (
|
||||
G20_RIGHT_19_LAYOUT,
|
||||
HandCalibrationProfile,
|
||||
get_hand_calibration_profile,
|
||||
)
|
||||
from .urdf_zero import ZeroCalibrationProfile, get_zero_calibration_profile
|
||||
|
||||
|
||||
VIEWS = ("front", "side", "top")
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ProductCalibrationContract:
|
||||
"""Declarative boundary between one hand model and the shared engine."""
|
||||
|
||||
model: str
|
||||
side: str
|
||||
layout_id: str
|
||||
profile: HandCalibrationProfile
|
||||
zero_profile: ZeroCalibrationProfile
|
||||
|
||||
@property
|
||||
def required_tag_ids(self) -> frozenset[int]:
|
||||
return frozenset(
|
||||
int(tag_id)
|
||||
for tags in self.profile.view_tags.values()
|
||||
for tag_id in tags.values()
|
||||
)
|
||||
|
||||
@property
|
||||
def views(self) -> tuple[str, ...]:
|
||||
return tuple(self.profile.view_tags)
|
||||
|
||||
|
||||
_PRODUCT_CONTRACTS: dict[
|
||||
tuple[str, str, str], ProductCalibrationContract
|
||||
] = {}
|
||||
|
||||
|
||||
def register_product_calibration_contract(
|
||||
contract: ProductCalibrationContract,
|
||||
) -> None:
|
||||
"""Register one model/side/layout without modifying shared workflow code."""
|
||||
model = str(contract.model).strip().upper()
|
||||
side = str(contract.side).strip().lower()
|
||||
layout = str(contract.layout_id).strip().lower()
|
||||
if not model or side not in {"left", "right"} or not layout:
|
||||
raise ValueError("product calibration contract identity is invalid")
|
||||
if contract.profile.side != side:
|
||||
raise ValueError("product contract side differs from hand profile")
|
||||
if contract.profile.layout_id.lower() != layout:
|
||||
raise ValueError("product contract layout differs from hand profile")
|
||||
if contract.zero_profile.hand != contract.profile:
|
||||
raise ValueError("zero-calibration profile differs from hand profile")
|
||||
if len(contract.profile.baseline_command) != contract.profile.command_count:
|
||||
raise ValueError("profile baseline and command names differ in length")
|
||||
if not contract.required_tag_ids:
|
||||
raise ValueError("product contract must declare at least one Tag")
|
||||
key = (model, side, layout)
|
||||
existing = _PRODUCT_CONTRACTS.get(key)
|
||||
if existing is not None and existing != contract:
|
||||
raise ValueError(f"product calibration contract already registered: {key}")
|
||||
_PRODUCT_CONTRACTS[key] = contract
|
||||
|
||||
|
||||
def get_product_calibration_contract(
|
||||
model: str, side: str, layout_id: str
|
||||
) -> ProductCalibrationContract:
|
||||
key = (
|
||||
str(model).strip().upper(),
|
||||
str(side).strip().lower(),
|
||||
str(layout_id).strip().lower(),
|
||||
)
|
||||
try:
|
||||
return _PRODUCT_CONTRACTS[key]
|
||||
except KeyError as error:
|
||||
supported = ", ".join("/".join(item) for item in sorted(_PRODUCT_CONTRACTS))
|
||||
raise ValueError(
|
||||
f"unsupported calibration product {key}; registered={supported}"
|
||||
) from error
|
||||
|
||||
|
||||
register_product_calibration_contract(
|
||||
ProductCalibrationContract(
|
||||
model="G20",
|
||||
side="right",
|
||||
layout_id=G20_RIGHT_19_LAYOUT,
|
||||
profile=get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT),
|
||||
zero_profile=get_zero_calibration_profile(
|
||||
"right", G20_RIGHT_19_LAYOUT
|
||||
),
|
||||
)
|
||||
)
|
||||
for _legacy_side in ("left", "right"):
|
||||
register_product_calibration_contract(
|
||||
ProductCalibrationContract(
|
||||
model="G20",
|
||||
side=_legacy_side,
|
||||
layout_id="legacy_11",
|
||||
profile=get_hand_calibration_profile(_legacy_side, "legacy_11"),
|
||||
zero_profile=get_zero_calibration_profile(
|
||||
_legacy_side, "legacy_11"
|
||||
),
|
||||
)
|
||||
)
|
||||
|
||||
|
||||
def sha256_file(path: str | Path) -> str:
|
||||
digest = hashlib.sha256()
|
||||
with Path(path).open("rb") as stream:
|
||||
for chunk in iter(lambda: stream.read(1024 * 1024), b""):
|
||||
digest.update(chunk)
|
||||
return digest.hexdigest()
|
||||
|
||||
|
||||
def _mapping(value: Any, name: str) -> Mapping[str, Any]:
|
||||
if not isinstance(value, Mapping):
|
||||
raise ValueError(f"{name} must be a mapping")
|
||||
return value
|
||||
|
||||
|
||||
def _resolve_path(value: Any, *, workspace: Path, name: str) -> Path:
|
||||
text = str(value).strip()
|
||||
if not text:
|
||||
raise ValueError(f"{name} is required")
|
||||
candidate = Path(text).expanduser()
|
||||
if not candidate.is_absolute():
|
||||
candidate = workspace / candidate
|
||||
return candidate.resolve()
|
||||
|
||||
|
||||
def _camera_info_fingerprint(path: Path) -> str:
|
||||
with path.open("r", encoding="utf-8") as stream:
|
||||
payload = _mapping(yaml.safe_load(stream), str(path))
|
||||
|
||||
def values(key: str) -> list[float]:
|
||||
item = _mapping(payload.get(key, {}), key)
|
||||
return [float(value) for value in item.get("data", [])]
|
||||
|
||||
return camera_info_fingerprint(
|
||||
width=int(payload["image_width"]),
|
||||
height=int(payload["image_height"]),
|
||||
camera_matrix=values("camera_matrix"),
|
||||
distortion=values("distortion_coefficients"),
|
||||
rectification=values("rectification_matrix"),
|
||||
projection=values("projection_matrix"),
|
||||
)
|
||||
|
||||
|
||||
def _tag_sizes_m_by_id(path: Path) -> dict[int, float]:
|
||||
with path.open("r", encoding="utf-8") as stream:
|
||||
payload = _mapping(yaml.safe_load(stream), str(path))
|
||||
result: dict[int, float] = {}
|
||||
for node in payload.values():
|
||||
parameters = _mapping(_mapping(node, "tag node").get("ros__parameters"), "ros__parameters")
|
||||
tag = _mapping(parameters.get("tag"), "tag")
|
||||
ids = [int(value) for value in tag.get("ids", [])]
|
||||
sizes = [float(value) for value in tag.get("sizes", [])]
|
||||
if len(ids) != len(sizes):
|
||||
raise ValueError("Tag IDs and sizes must have the same length")
|
||||
if set(result).intersection(ids):
|
||||
raise ValueError("Tag IDs must be unique across the three views")
|
||||
if any(not 0.0 < size < 0.1 for size in sizes):
|
||||
raise ValueError("Tag sizes must be in (0, 0.1) metres")
|
||||
result.update(zip(ids, sizes))
|
||||
return result
|
||||
|
||||
|
||||
def _custom_pnp_tag_sizes_m_by_id(
|
||||
path: Path, configured_ids: set[int]
|
||||
) -> dict[int, float]:
|
||||
with path.open("r", encoding="utf-8") as stream:
|
||||
payload = _mapping(yaml.safe_load(stream), str(path))
|
||||
parameters = _mapping(
|
||||
_mapping(payload.get("g20_calibration"), "g20_calibration").get(
|
||||
"ros__parameters"
|
||||
),
|
||||
"ros__parameters",
|
||||
)
|
||||
default = float(parameters.get("tag_size_m", 0.0))
|
||||
ids = [int(value) for value in parameters.get("tag_size_override_ids", [])]
|
||||
sizes = [float(value) for value in parameters.get("tag_size_overrides_m", [])]
|
||||
if len(ids) != len(sizes) or len(set(ids)) != len(ids):
|
||||
raise ValueError("custom PnP Tag-size overrides are invalid")
|
||||
if any(tag_id not in configured_ids for tag_id in ids):
|
||||
raise ValueError("custom PnP Tag-size override ID is not configured")
|
||||
result = {tag_id: default for tag_id in configured_ids}
|
||||
result.update(zip(ids, sizes))
|
||||
return result
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ProductConfig:
|
||||
path: Path
|
||||
workspace: Path
|
||||
model: str
|
||||
side: str
|
||||
tag_layout: str
|
||||
calibration_contract: ProductCalibrationContract
|
||||
serial_number: str
|
||||
can_interface: str
|
||||
source_urdf: Path
|
||||
source_urdf_sha256: str
|
||||
camera_extrinsics: Path
|
||||
camera_extrinsics_sha256: str
|
||||
calibration_config: Path
|
||||
calibration_config_sha256: str
|
||||
tag_config: Path
|
||||
tag_config_sha256: str
|
||||
output_root: Path
|
||||
cameras: Mapping[str, Mapping[str, str]]
|
||||
required_independent_passes: int
|
||||
static_repeatability_rad: float
|
||||
|
||||
@property
|
||||
def session_root(self) -> Path:
|
||||
return self.output_root / self.serial_number
|
||||
|
||||
|
||||
def load_product_config(
|
||||
path: str | Path,
|
||||
*,
|
||||
workspace: str | Path | None = None,
|
||||
check_can: bool = True,
|
||||
) -> ProductConfig:
|
||||
"""Load and verify a product against its registered calibration contract."""
|
||||
source = Path(path).expanduser().resolve()
|
||||
if not source.is_file():
|
||||
raise ValueError(f"product config does not exist: {source}")
|
||||
with source.open("r", encoding="utf-8") as stream:
|
||||
raw = _mapping(yaml.safe_load(stream), str(source))
|
||||
if int(raw.get("schema_version", -1)) != 1:
|
||||
raise ValueError("product config schema_version must be 1")
|
||||
root = Path.cwd().resolve() if workspace is None else Path(workspace).resolve()
|
||||
serial = str(raw.get("serial_number", ""))
|
||||
if re.fullmatch(r"[A-Za-z0-9_.-]+", serial) is None:
|
||||
raise ValueError("serial_number is invalid")
|
||||
model = str(raw.get("model", "")).strip().upper()
|
||||
side = str(raw.get("side", "")).strip().lower()
|
||||
# Keep the deployed G20 schema compatible while making the layout an
|
||||
# explicit product choice for all new configurations.
|
||||
layout = str(
|
||||
raw.get(
|
||||
"tag_layout",
|
||||
G20_RIGHT_19_LAYOUT if (model, side) == ("G20", "right") else "",
|
||||
)
|
||||
).strip().lower()
|
||||
contract = get_product_calibration_contract(model, side, layout)
|
||||
can_interface = str(raw.get("can_interface", "")).strip()
|
||||
if not can_interface:
|
||||
raise ValueError("can_interface is required")
|
||||
if check_can and not (Path("/sys/class/net") / can_interface).exists():
|
||||
raise ValueError(f"CAN interface does not exist: {can_interface}")
|
||||
|
||||
artifacts = _mapping(raw.get("artifacts"), "artifacts")
|
||||
source_urdf = _resolve_path(
|
||||
artifacts.get("source_urdf"), workspace=root, name="source_urdf"
|
||||
)
|
||||
extrinsics = _resolve_path(
|
||||
artifacts.get("camera_extrinsics"),
|
||||
workspace=root,
|
||||
name="camera_extrinsics",
|
||||
)
|
||||
calibration_config = _resolve_path(
|
||||
artifacts.get("calibration_config"),
|
||||
workspace=root,
|
||||
name="calibration_config",
|
||||
)
|
||||
tag_config = _resolve_path(
|
||||
artifacts.get("tag_config"), workspace=root, name="tag_config"
|
||||
)
|
||||
for name, candidate in (
|
||||
("source URDF", source_urdf),
|
||||
("camera extrinsics", extrinsics),
|
||||
("calibration config", calibration_config),
|
||||
("Tag config", tag_config),
|
||||
):
|
||||
if not candidate.is_file():
|
||||
raise ValueError(f"{name} does not exist: {candidate}")
|
||||
|
||||
expected_source_hash = str(artifacts.get("source_urdf_sha256", "")).lower()
|
||||
expected_extrinsics_hash = str(
|
||||
artifacts.get("camera_extrinsics_sha256", "")
|
||||
).lower()
|
||||
expected_tag_hash = str(artifacts.get("tag_config_sha256", "")).lower()
|
||||
expected_calibration_hash = str(
|
||||
artifacts.get("calibration_config_sha256", "")
|
||||
).lower()
|
||||
hashes = (
|
||||
("source URDF", source_urdf, expected_source_hash),
|
||||
("camera extrinsics", extrinsics, expected_extrinsics_hash),
|
||||
("Tag config", tag_config, expected_tag_hash),
|
||||
("calibration config", calibration_config, expected_calibration_hash),
|
||||
)
|
||||
for name, candidate, expected in hashes:
|
||||
if re.fullmatch(r"[0-9a-f]{64}", expected) is None:
|
||||
raise ValueError(f"{name} expected SHA-256 is invalid")
|
||||
actual = sha256_file(candidate)
|
||||
if actual != expected:
|
||||
raise ValueError(f"{name} SHA-256 mismatch: expected={expected} actual={actual}")
|
||||
|
||||
expected_tag_ids = set(contract.required_tag_ids)
|
||||
tag_sizes = _tag_sizes_m_by_id(tag_config)
|
||||
if set(tag_sizes) != expected_tag_ids:
|
||||
raise ValueError(
|
||||
f"{model}/{side}/{layout} Tag IDs differ from the registered "
|
||||
f"profile: expected={sorted(expected_tag_ids)} "
|
||||
f"actual={sorted(tag_sizes)}"
|
||||
)
|
||||
if _custom_pnp_tag_sizes_m_by_id(
|
||||
calibration_config, expected_tag_ids
|
||||
) != tag_sizes:
|
||||
raise ValueError(
|
||||
"custom PnP Tag sizes do not match the apriltag_ros Tag config"
|
||||
)
|
||||
|
||||
camera_raw = _mapping(raw.get("cameras"), "cameras")
|
||||
required_views = tuple(contract.views)
|
||||
if set(required_views) != set(VIEWS):
|
||||
raise ValueError(
|
||||
"the current shared engine requires front/side/top views"
|
||||
)
|
||||
if set(camera_raw) != set(required_views):
|
||||
raise ValueError(f"cameras must contain {'/'.join(required_views)}")
|
||||
loaded_extrinsics = load_three_camera_extrinsics(extrinsics)
|
||||
cameras: dict[str, dict[str, str]] = {}
|
||||
serials: set[str] = set()
|
||||
for view in required_views:
|
||||
item = _mapping(camera_raw[view], f"cameras.{view}")
|
||||
camera_serial = str(item.get("serial_number", ""))
|
||||
info_path = _resolve_path(
|
||||
item.get("camera_info"), workspace=root, name=f"{view}.camera_info"
|
||||
)
|
||||
if not info_path.is_file():
|
||||
raise ValueError(f"{view} camera info does not exist: {info_path}")
|
||||
if camera_serial in serials:
|
||||
raise ValueError("the three camera serial numbers must be unique")
|
||||
serials.add(camera_serial)
|
||||
identity = loaded_extrinsics.cameras[view]
|
||||
fingerprint = _camera_info_fingerprint(info_path)
|
||||
if camera_serial != identity.serial_number:
|
||||
raise ValueError(f"{view} camera serial does not match extrinsics")
|
||||
if fingerprint != identity.intrinsics_sha256:
|
||||
raise ValueError(f"{view} camera intrinsics do not match extrinsics")
|
||||
cameras[view] = {
|
||||
"serial_number": camera_serial,
|
||||
"camera_info": str(info_path),
|
||||
"camera_name": str(item.get("camera_name", f"hikrobot_{view}_{camera_serial}")),
|
||||
}
|
||||
|
||||
output_root = _resolve_path(
|
||||
raw.get("output_root", "calibration_output"),
|
||||
workspace=root,
|
||||
name="output_root",
|
||||
)
|
||||
release = _mapping(raw.get("release", {}), "release")
|
||||
passes = int(release.get("required_independent_passes", 2))
|
||||
if passes not in {1, 2}:
|
||||
raise ValueError("required_independent_passes must be 1 or 2")
|
||||
repeatability_deg = float(release.get("static_repeatability_deg", 1.0))
|
||||
if not 0.0 < repeatability_deg <= 5.0:
|
||||
raise ValueError("static_repeatability_deg must be in (0, 5]")
|
||||
return ProductConfig(
|
||||
path=source,
|
||||
workspace=root,
|
||||
model=model,
|
||||
side=side,
|
||||
tag_layout=layout,
|
||||
calibration_contract=contract,
|
||||
serial_number=serial,
|
||||
can_interface=can_interface,
|
||||
source_urdf=source_urdf,
|
||||
source_urdf_sha256=expected_source_hash,
|
||||
camera_extrinsics=extrinsics,
|
||||
camera_extrinsics_sha256=expected_extrinsics_hash,
|
||||
calibration_config=calibration_config,
|
||||
calibration_config_sha256=expected_calibration_hash,
|
||||
tag_config=tag_config,
|
||||
tag_config_sha256=expected_tag_hash,
|
||||
output_root=output_root,
|
||||
cameras=cameras,
|
||||
required_independent_passes=passes,
|
||||
static_repeatability_rad=repeatability_deg * 3.141592653589793 / 180.0,
|
||||
)
|
||||
@@ -0,0 +1,890 @@
|
||||
"""Validate and atomically publish one inseparable G20 JSON/URDF session."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from datetime import datetime, timezone
|
||||
import copy
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
from pathlib import Path
|
||||
import re
|
||||
from typing import Any, Mapping, Sequence
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from .full_hand import (
|
||||
G20_COMBINATION_REQUIRED_TARGET_KEYS,
|
||||
G20_RIGHT_19_LAYOUT,
|
||||
get_hand_calibration_profile,
|
||||
validate_compact_payload,
|
||||
)
|
||||
from .product import ProductConfig, sha256_file
|
||||
from .storage import atomic_write_json
|
||||
from .urdf_zero import (
|
||||
RIGHT_19_MECHANICAL_ENDPOINT_JOINTS,
|
||||
get_zero_calibration_profile,
|
||||
)
|
||||
|
||||
|
||||
PASSIVE_JOINTS = frozenset(
|
||||
{"thumb_ip", "index_dip", "middle_dip", "ring_dip", "pinky_dip"}
|
||||
)
|
||||
VISUALLY_MEASURED_FINGER_DIPS = frozenset(
|
||||
{"index_dip", "middle_dip", "ring_dip", "pinky_dip"}
|
||||
)
|
||||
ACTIVE_ZERO_JOINTS = frozenset(
|
||||
get_zero_calibration_profile("right", G20_RIGHT_19_LAYOUT).direct_zero_joints
|
||||
)
|
||||
RETAINED_ACTIVE_ZERO_JOINTS = frozenset(
|
||||
get_hand_calibration_profile("right", G20_RIGHT_19_LAYOUT).active_joints
|
||||
) - ACTIVE_ZERO_JOINTS
|
||||
SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD = 5.1e-9
|
||||
|
||||
|
||||
def _load_json(path: Path) -> dict[str, Any]:
|
||||
with path.open("r", encoding="utf-8") as stream:
|
||||
payload = json.load(stream)
|
||||
if not isinstance(payload, dict):
|
||||
raise ValueError(f"{path} must contain a JSON object")
|
||||
return payload
|
||||
|
||||
|
||||
def _joint_blocks(text: str) -> dict[str, str]:
|
||||
pattern = re.compile(
|
||||
r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
|
||||
re.DOTALL,
|
||||
)
|
||||
return {match.group("name"): match.group(0) for match in pattern.finditer(text)}
|
||||
|
||||
|
||||
def _mask_origin_rpy(block: str) -> str:
|
||||
return re.sub(
|
||||
r"(<origin\b[^>]*\brpy\s*=\s*)([\"'])[^\"']*\2",
|
||||
r"\1\2__CALIBRATED_RPY__\2",
|
||||
block,
|
||||
count=1,
|
||||
)
|
||||
|
||||
|
||||
def _mask_active_origin_rpy_fields(text: str) -> str:
|
||||
pattern = re.compile(
|
||||
r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
|
||||
re.DOTALL,
|
||||
)
|
||||
|
||||
def replace(match: re.Match[str]) -> str:
|
||||
block = match.group(0)
|
||||
if match.group("name") not in ACTIVE_ZERO_JOINTS:
|
||||
return block
|
||||
masked = _mask_origin_rpy(block)
|
||||
if masked == block:
|
||||
raise ValueError(
|
||||
f"active joint {match.group('name')} has no origin.rpy"
|
||||
)
|
||||
return masked
|
||||
|
||||
return pattern.sub(replace, text)
|
||||
|
||||
|
||||
def _mask_endpoint_coordinate_fields(text: str) -> str:
|
||||
"""Mask only limit/mimic fields induced by endpoint zero coordinates."""
|
||||
pattern = re.compile(
|
||||
r"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>.*?</joint>",
|
||||
re.DOTALL,
|
||||
)
|
||||
|
||||
def replace(match: re.Match[str]) -> str:
|
||||
block = match.group(0)
|
||||
name = match.group("name")
|
||||
if name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS:
|
||||
return re.sub(
|
||||
r"(<limit\b[^>]*\bupper\s*=\s*)([\"'])[^\"']*\2",
|
||||
r"\1\2__CALIBRATED_UPPER__\2",
|
||||
block,
|
||||
count=1,
|
||||
)
|
||||
mimic = re.search(
|
||||
r"<mimic\b[^>]*\bjoint\s*=\s*([\"'])(?P<source>[^\"']+)\1[^>]*>",
|
||||
block,
|
||||
re.DOTALL,
|
||||
)
|
||||
if (
|
||||
mimic is not None
|
||||
and mimic.group("source") in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
|
||||
):
|
||||
return re.sub(
|
||||
r"(<mimic\b[^>]*\boffset\s*=\s*)([\"'])[^\"']*\2",
|
||||
r"\1\2__CALIBRATED_MIMIC_OFFSET__\2",
|
||||
block,
|
||||
count=1,
|
||||
)
|
||||
return block
|
||||
|
||||
return pattern.sub(replace, text)
|
||||
|
||||
|
||||
def _triplet(value: str) -> np.ndarray:
|
||||
result = np.asarray([float(item) for item in value.split()], dtype=float)
|
||||
if result.shape != (3,) or not np.all(np.isfinite(result)):
|
||||
raise ValueError(f"invalid URDF triplet: {value}")
|
||||
return result
|
||||
|
||||
|
||||
def _joint_elements(path: str | Path) -> dict[str, ET.Element]:
|
||||
return {
|
||||
str(joint.get("name")): joint
|
||||
for joint in ET.parse(Path(path)).getroot().findall("joint")
|
||||
}
|
||||
|
||||
|
||||
def _verify_expected_origin_offsets(
|
||||
source: str | Path,
|
||||
corrected: str | Path,
|
||||
expected_offsets_rad: Mapping[str, float],
|
||||
) -> None:
|
||||
offsets = {str(name): float(value) for name, value in expected_offsets_rad.items()}
|
||||
if set(offsets) != ACTIVE_ZERO_JOINTS or any(
|
||||
not math.isfinite(value) for value in offsets.values()
|
||||
):
|
||||
raise ValueError("expected offsets must contain all finite active static-zero values")
|
||||
before = _joint_elements(source)
|
||||
after = _joint_elements(corrected)
|
||||
maximum_rotation_error = 0.0
|
||||
for name, original_joint in before.items():
|
||||
corrected_joint = after[name]
|
||||
original_origin = original_joint.find("origin")
|
||||
corrected_origin = corrected_joint.find("origin")
|
||||
if original_origin is None or corrected_origin is None:
|
||||
if original_origin is not corrected_origin:
|
||||
raise ValueError(f"corrected URDF changed origin presence for {name}")
|
||||
continue
|
||||
original_xyz = _triplet(original_origin.get("xyz", "0 0 0"))
|
||||
corrected_xyz = _triplet(corrected_origin.get("xyz", "0 0 0"))
|
||||
if not np.allclose(original_xyz, corrected_xyz, atol=1.0e-12, rtol=0.0):
|
||||
raise ValueError(f"corrected URDF changed origin.xyz for {name}")
|
||||
original_rotation = Rotation.from_euler(
|
||||
"xyz", _triplet(original_origin.get("rpy", "0 0 0"))
|
||||
)
|
||||
corrected_rotation = Rotation.from_euler(
|
||||
"xyz", _triplet(corrected_origin.get("rpy", "0 0 0"))
|
||||
)
|
||||
expected_rotation = original_rotation
|
||||
if name in offsets:
|
||||
axis_node = original_joint.find("axis")
|
||||
axis = _triplet(
|
||||
"1 0 0" if axis_node is None else axis_node.get("xyz", "1 0 0")
|
||||
)
|
||||
norm = float(np.linalg.norm(axis))
|
||||
if norm <= 1.0e-12:
|
||||
raise ValueError(f"source URDF joint {name} has a degenerate axis")
|
||||
expected_rotation = expected_rotation * Rotation.from_rotvec(
|
||||
axis / norm * offsets[name]
|
||||
)
|
||||
error = float((expected_rotation.inv() * corrected_rotation).magnitude())
|
||||
maximum_rotation_error = max(maximum_rotation_error, error)
|
||||
# Schema v4 intentionally stores zero offsets at eight decimal places.
|
||||
# Older sessions wrote the corrected URDF from the full-precision solve,
|
||||
# so comparing that file with the published JSON necessarily permits half
|
||||
# of one last-place unit. This is about 2.9e-7 degrees and is far below
|
||||
# any calibration or URDF numerical significance.
|
||||
if maximum_rotation_error > SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD:
|
||||
raise ValueError(
|
||||
"corrected URDF origin.rpy does not match the published zero offsets"
|
||||
)
|
||||
|
||||
|
||||
def verify_corrected_urdf(
|
||||
source: str | Path,
|
||||
corrected: str | Path,
|
||||
*,
|
||||
expected_offsets_rad: Mapping[str, float] | None = None,
|
||||
endpoint_anchored_offsets_rad: Mapping[str, float] | None = None,
|
||||
) -> tuple[str, ...]:
|
||||
"""Prove that only active-joint origin.rpy attributes changed.
|
||||
|
||||
Passive joint blocks, including their mimic elements, are compared as raw
|
||||
UTF-8 text so formatting and numeric spelling are protected as well.
|
||||
"""
|
||||
source_text = Path(source).read_text(encoding="utf-8")
|
||||
corrected_text = Path(corrected).read_text(encoding="utf-8")
|
||||
before = _joint_blocks(source_text)
|
||||
after = _joint_blocks(corrected_text)
|
||||
if set(before) != set(after):
|
||||
raise ValueError("corrected URDF changed the joint set")
|
||||
# Text patchers may normalize only the final newline. It has no URDF/XML
|
||||
# semantics; every joint block and every non-rpy byte remains guarded
|
||||
# below.
|
||||
masked_source = _mask_endpoint_coordinate_fields(
|
||||
_mask_active_origin_rpy_fields(source_text)
|
||||
).rstrip("\r\n")
|
||||
masked_corrected = _mask_endpoint_coordinate_fields(
|
||||
_mask_active_origin_rpy_fields(corrected_text)
|
||||
).rstrip("\r\n")
|
||||
if masked_source != masked_corrected:
|
||||
raise ValueError(
|
||||
"corrected URDF changed content outside active origin.rpy or "
|
||||
"endpoint-calibrated fields"
|
||||
)
|
||||
changed: list[str] = []
|
||||
for name in before:
|
||||
if before[name] == after[name]:
|
||||
continue
|
||||
if name not in ACTIVE_ZERO_JOINTS and name not in PASSIVE_JOINTS:
|
||||
raise ValueError(f"corrected URDF changed protected joint {name}")
|
||||
if name in ACTIVE_ZERO_JOINTS:
|
||||
changed.append(name)
|
||||
if expected_offsets_rad is not None:
|
||||
_verify_expected_origin_offsets(source, corrected, expected_offsets_rad)
|
||||
endpoint_offsets = {
|
||||
str(name): float(value)
|
||||
for name, value in dict(endpoint_anchored_offsets_rad or {}).items()
|
||||
}
|
||||
if endpoint_offsets:
|
||||
source_joints = _joint_elements(source)
|
||||
corrected_joints = _joint_elements(corrected)
|
||||
for name, offset in endpoint_offsets.items():
|
||||
original_limit = source_joints[name].find("limit")
|
||||
corrected_limit = corrected_joints[name].find("limit")
|
||||
expected_upper = float(original_limit.get("upper")) - offset
|
||||
# Schema v4 stores the corresponding zero at eight decimal
|
||||
# places, while the URDF is written from the full-precision solve.
|
||||
# Match the half-last-place tolerance used for origin rotations.
|
||||
if (
|
||||
abs(float(corrected_limit.get("upper")) - expected_upper)
|
||||
> SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD
|
||||
):
|
||||
raise ValueError(f"corrected URDF has invalid {name} upper limit")
|
||||
for name in PASSIVE_JOINTS:
|
||||
original_mimic = source_joints[name].find("mimic")
|
||||
corrected_mimic = corrected_joints[name].find("mimic")
|
||||
if original_mimic is None or corrected_mimic is None:
|
||||
continue
|
||||
source_name = str(original_mimic.get("joint"))
|
||||
multiplier = float(original_mimic.get("multiplier", "1"))
|
||||
expected = float(original_mimic.get("offset", "0"))
|
||||
expected += multiplier * endpoint_offsets.get(source_name, 0.0)
|
||||
# The active endpoint offset comes from schema-v4 JSON rounded to
|
||||
# eight decimal places, while the URDF mimic was written from the
|
||||
# full-precision solve. Propagate exactly the same accepted
|
||||
# quantization through the mimic multiplier; retain a much
|
||||
# smaller allowance for XML decimal formatting itself.
|
||||
tolerance = (
|
||||
abs(multiplier)
|
||||
* SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD
|
||||
+ 1.0e-12
|
||||
)
|
||||
if abs(float(corrected_mimic.get("offset", "0")) - expected) > tolerance:
|
||||
raise ValueError(f"corrected URDF has invalid {name} mimic offset")
|
||||
return tuple(sorted(changed))
|
||||
|
||||
|
||||
def verify_urdf_mesh_resources(urdf: str | Path) -> dict[str, Path]:
|
||||
"""Return every session-local mesh after proving it resolves safely."""
|
||||
path = Path(urdf).expanduser().resolve()
|
||||
root = ET.parse(path).getroot()
|
||||
resources: dict[str, Path] = {}
|
||||
for mesh in root.findall(".//mesh"):
|
||||
filename = str(mesh.get("filename", "")).strip()
|
||||
if not filename or "://" in filename or filename.startswith("package:"):
|
||||
continue
|
||||
relative = Path(filename)
|
||||
if relative.is_absolute() or ".." in relative.parts:
|
||||
raise ValueError(f"URDF has an unsafe local mesh path: {filename}")
|
||||
resolved = (path.parent / relative).resolve()
|
||||
try:
|
||||
resolved.relative_to(path.parent)
|
||||
except ValueError as error:
|
||||
raise ValueError(f"URDF mesh escapes the session: {filename}") from error
|
||||
if not resolved.is_file():
|
||||
raise ValueError(f"URDF mesh resource is missing: {filename}")
|
||||
resources[relative.as_posix()] = resolved
|
||||
return dict(sorted(resources.items()))
|
||||
|
||||
|
||||
def validate_runtime_curves_against_urdf_limits(
|
||||
payload: Mapping[str, Any], runtime_urdf: str | Path
|
||||
) -> None:
|
||||
"""Reject a curve whose q leaves its runtime URDF coordinate limits.
|
||||
|
||||
Curves in schema v4 are expressed in the corrected URDF joint coordinate,
|
||||
not in the source-CAD coordinate. Endpoint zero calibration can therefore
|
||||
move a corrected coordinate limit while preserving the same physical CAD
|
||||
endpoint; callers publishing a calibrated pair must pass that corrected
|
||||
URDF here.
|
||||
"""
|
||||
validate_compact_payload(payload)
|
||||
runtime_joints = _joint_elements(runtime_urdf)
|
||||
for name, calibration in payload["joints"].items():
|
||||
joint = runtime_joints.get(str(name))
|
||||
if joint is None:
|
||||
raise ValueError(f"runtime URDF is missing joint {name}")
|
||||
limit = joint.find("limit")
|
||||
if limit is None or limit.get("lower") is None or limit.get("upper") is None:
|
||||
raise ValueError(f"runtime URDF joint {name} has no finite position limit")
|
||||
lower = float(limit.get("lower"))
|
||||
upper = float(limit.get("upper"))
|
||||
if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper:
|
||||
raise ValueError(f"runtime URDF joint {name} has invalid position limits")
|
||||
curve = np.asarray(calibration["angle_rad"], dtype=float)
|
||||
minimum = float(np.min(curve))
|
||||
maximum = float(np.max(curve))
|
||||
tolerance = 1.0e-7
|
||||
if minimum < lower - tolerance or maximum > upper + tolerance:
|
||||
raise ValueError(
|
||||
f"runtime curve exceeds runtime URDF limit for {name}: "
|
||||
f"[{minimum:.9g}, {maximum:.9g}] not within "
|
||||
f"[{lower:.9g}, {upper:.9g}]"
|
||||
)
|
||||
|
||||
|
||||
def clamp_compact_payload_to_urdf_limits(
|
||||
payload: Mapping[str, Any], runtime_urdf: str | Path
|
||||
) -> tuple[dict[str, Any], dict[str, int]]:
|
||||
"""Return a schema-preserving payload bounded in its runtime coordinates."""
|
||||
result = copy.deepcopy(dict(payload))
|
||||
validate_compact_payload(result)
|
||||
runtime_joints = _joint_elements(runtime_urdf)
|
||||
clipped_by_joint: dict[str, int] = {}
|
||||
for name, calibration in result["joints"].items():
|
||||
joint = runtime_joints.get(str(name))
|
||||
limit = None if joint is None else joint.find("limit")
|
||||
if (
|
||||
limit is None
|
||||
or limit.get("lower") is None
|
||||
or limit.get("upper") is None
|
||||
):
|
||||
raise ValueError(f"runtime URDF joint {name} has no finite position limit")
|
||||
lower = float(limit.get("lower"))
|
||||
upper = float(limit.get("upper"))
|
||||
if not math.isfinite(lower) or not math.isfinite(upper) or lower > upper:
|
||||
raise ValueError(f"runtime URDF joint {name} has invalid position limits")
|
||||
source = np.asarray(calibration["angle_rad"], dtype=float)
|
||||
bounded = np.clip(source, lower, upper)
|
||||
count = int(np.count_nonzero(bounded != source))
|
||||
if count:
|
||||
clipped_by_joint[str(name)] = count
|
||||
calibration["angle_rad"] = [
|
||||
round(float(value), 8) for value in bounded
|
||||
]
|
||||
validate_compact_payload(result)
|
||||
return result, clipped_by_joint
|
||||
|
||||
|
||||
def build_mujoco_validation_commands(
|
||||
baseline: Sequence[int],
|
||||
) -> dict[str, Any]:
|
||||
if len(baseline) != 20:
|
||||
raise ValueError("baseline must contain exactly 20 commands")
|
||||
base = [int(value) for value in baseline]
|
||||
if any(not 0 <= value <= 255 for value in base):
|
||||
raise ValueError("baseline commands must be in [0, 255]")
|
||||
|
||||
def pose(name: str, label: str, changes: Mapping[int, int]) -> dict[str, Any]:
|
||||
values = list(base)
|
||||
for index, value in changes.items():
|
||||
values[int(index)] = int(value)
|
||||
return {"name": name, "label_zh": label, "command_u8": values}
|
||||
|
||||
return {
|
||||
"schema_version": 1,
|
||||
"topic": "/g20/cb_right_hand_control_cmd",
|
||||
"serial_number": "G20_RIGHT_001",
|
||||
"poses": [
|
||||
pose("all_open", "全开", {}),
|
||||
pose("thumb_middle", "拇指中位", {0: 160, 5: 160, 10: 160, 15: 160}),
|
||||
pose("index_middle", "食指中位", {1: 160, 6: 127, 16: 160}),
|
||||
pose("middle_middle", "中指中位", {2: 160, 7: 127, 17: 160}),
|
||||
pose("ring_middle", "无名指中位", {3: 160, 8: 127, 18: 160}),
|
||||
pose("pinky_middle", "小指中位", {4: 160, 9: 127, 19: 160}),
|
||||
pose(
|
||||
"half_grip",
|
||||
"四指半握",
|
||||
{1: 160, 2: 160, 3: 160, 4: 160, 16: 160, 17: 160, 18: 160, 19: 160},
|
||||
),
|
||||
pose(
|
||||
"light_pinch",
|
||||
"轻捏",
|
||||
{0: 176, 5: 176, 10: 176, 15: 176, 1: 176, 6: 127, 16: 176},
|
||||
),
|
||||
],
|
||||
}
|
||||
|
||||
|
||||
def atomic_session_pointer(root: str | Path, name: str, session: str | Path) -> Path:
|
||||
parent = Path(root).resolve()
|
||||
target = Path(session).resolve()
|
||||
if target.parent != parent:
|
||||
raise ValueError("session pointer target must be a direct child of the serial root")
|
||||
if not target.is_dir():
|
||||
raise ValueError(f"session directory does not exist: {target}")
|
||||
if name not in {"latest_attempt", "latest_passed", "latest_thumb_passed"}:
|
||||
raise ValueError("unsupported session pointer name")
|
||||
parent.mkdir(parents=True, exist_ok=True)
|
||||
destination = parent / name
|
||||
temporary = parent / f".{name}.{os.getpid()}.tmp"
|
||||
if temporary.is_symlink():
|
||||
temporary.unlink()
|
||||
elif temporary.exists():
|
||||
raise ValueError(f"temporary pointer path is occupied: {temporary}")
|
||||
os.symlink(target.name, temporary, target_is_directory=True)
|
||||
os.replace(temporary, destination)
|
||||
return destination
|
||||
|
||||
|
||||
def active_offsets(payload: Mapping[str, Any]) -> dict[str, float]:
|
||||
validate_compact_payload(payload)
|
||||
result = {
|
||||
name: float(payload["joints"][name]["zero_angles"]["urdf_zero_offset_rad"])
|
||||
for name in ACTIVE_ZERO_JOINTS
|
||||
}
|
||||
if set(result) != ACTIVE_ZERO_JOINTS or any(
|
||||
not math.isfinite(value) for value in result.values()
|
||||
):
|
||||
raise ValueError(
|
||||
"payload does not contain all finite observable active zero offsets"
|
||||
)
|
||||
return result
|
||||
|
||||
|
||||
def standalone_thumb_offsets(payload: Mapping[str, Any]) -> dict[str, float]:
|
||||
"""Validate a standalone thumb artifact and expand CAD finger zeros."""
|
||||
if (
|
||||
payload.get("schema_version") != 1
|
||||
or payload.get("artifact_type")
|
||||
!= "g20_right_standalone_thumb_calibration"
|
||||
or payload.get("side") != "right"
|
||||
or payload.get("non_thumb_zero_policy") != "source_cad_unchanged"
|
||||
or not bool(payload.get("quality", {}).get("passed"))
|
||||
):
|
||||
raise ValueError("standalone thumb JSON contract is invalid")
|
||||
joints = payload.get("joints")
|
||||
expected_joint_names = {
|
||||
"thumb_cmc_roll",
|
||||
"thumb_cmc_yaw",
|
||||
"thumb_cmc_pitch",
|
||||
"thumb_mcp",
|
||||
"thumb_ip",
|
||||
}
|
||||
if not isinstance(joints, Mapping) or set(joints) != expected_joint_names:
|
||||
raise ValueError("standalone thumb JSON has the wrong joint set")
|
||||
thumb_names = expected_joint_names - {"thumb_ip"}
|
||||
result = {name: 0.0 for name in ACTIVE_ZERO_JOINTS}
|
||||
for name in thumb_names:
|
||||
try:
|
||||
value = float(joints[name]["zero_angles"]["urdf_zero_offset_rad"])
|
||||
except (KeyError, TypeError, ValueError) as error:
|
||||
raise ValueError(
|
||||
f"standalone thumb JSON is missing zero for {name}"
|
||||
) from error
|
||||
curve = np.asarray(joints[name].get("angle_rad", ()), dtype=float)
|
||||
if curve.shape != (256,) or not np.all(np.isfinite(curve)):
|
||||
raise ValueError(f"standalone thumb JSON curve is invalid: {name}")
|
||||
result[name] = value
|
||||
passive_curve = np.asarray(
|
||||
joints["thumb_ip"].get("angle_rad", ()), dtype=float
|
||||
)
|
||||
if (
|
||||
passive_curve.shape != (256,)
|
||||
or not np.all(np.isfinite(passive_curve))
|
||||
or not bool(joints["thumb_ip"].get("passive"))
|
||||
or any(not math.isfinite(value) for value in result.values())
|
||||
):
|
||||
raise ValueError("standalone thumb JSON passive curve is invalid")
|
||||
return result
|
||||
|
||||
|
||||
def verify_partial_scope_preserves_certified_zeros(
|
||||
*,
|
||||
scope: str,
|
||||
source_session: str | Path,
|
||||
serial_root: str | Path | None = None,
|
||||
serial_number: str,
|
||||
current_offsets: Mapping[str, float],
|
||||
) -> None:
|
||||
"""Reject a partial artifact that changes any certified non-target zero."""
|
||||
selected = str(scope).strip().lower()
|
||||
if selected not in {"thumb", "fingers"}:
|
||||
return
|
||||
source = Path(source_session).expanduser()
|
||||
if not source.is_absolute():
|
||||
if serial_root is None:
|
||||
raise ValueError(
|
||||
"relative partial base session requires its serial root"
|
||||
)
|
||||
source = Path(serial_root).expanduser() / source
|
||||
source = source.resolve()
|
||||
base_payload = _load_json(
|
||||
source / f"g20_right_{serial_number}_calibration.json"
|
||||
)
|
||||
base_offsets = (
|
||||
standalone_thumb_offsets(base_payload)
|
||||
if base_payload.get("artifact_type")
|
||||
== "g20_right_standalone_thumb_calibration"
|
||||
else active_offsets(base_payload)
|
||||
)
|
||||
preserve_thumb = selected == "fingers"
|
||||
changed = {
|
||||
name: abs(float(current_offsets[name]) - float(base_offsets[name]))
|
||||
for name in ACTIVE_ZERO_JOINTS
|
||||
if name.startswith("thumb_") == preserve_thumb
|
||||
and abs(float(current_offsets[name]) - float(base_offsets[name]))
|
||||
> SCHEMA_V4_OFFSET_QUANTIZATION_TOLERANCE_RAD
|
||||
}
|
||||
if changed:
|
||||
raise ValueError(
|
||||
f"partial {selected} calibration changed certified non-target "
|
||||
"zeros: "
|
||||
+ ",".join(sorted(changed))
|
||||
)
|
||||
|
||||
|
||||
def compare_session_offsets(
|
||||
first: Mapping[str, Any],
|
||||
second: Mapping[str, Any],
|
||||
*,
|
||||
maximum_difference_rad: float,
|
||||
) -> dict[str, float]:
|
||||
first_standalone = (
|
||||
first.get("artifact_type")
|
||||
== "g20_right_standalone_thumb_calibration"
|
||||
)
|
||||
second_standalone = (
|
||||
second.get("artifact_type")
|
||||
== "g20_right_standalone_thumb_calibration"
|
||||
)
|
||||
if first_standalone != second_standalone:
|
||||
raise ValueError("cannot compare standalone thumb and full-hand artifacts")
|
||||
left = (
|
||||
standalone_thumb_offsets(first)
|
||||
if first_standalone
|
||||
else active_offsets(first)
|
||||
)
|
||||
right = (
|
||||
standalone_thumb_offsets(second)
|
||||
if second_standalone
|
||||
else active_offsets(second)
|
||||
)
|
||||
differences = {
|
||||
name: abs(left[name] - right[name])
|
||||
for name in sorted(left)
|
||||
}
|
||||
failed = {name: value for name, value in differences.items() if value > maximum_difference_rad}
|
||||
if failed:
|
||||
details = ", ".join(
|
||||
f"{name}={math.degrees(value):.3f}deg" for name, value in failed.items()
|
||||
)
|
||||
raise ValueError(f"independent-session static-zero mismatch: {details}")
|
||||
return differences
|
||||
|
||||
|
||||
def session_artifact_paths(session: str | Path, serial_number: str) -> dict[str, Path]:
|
||||
directory = Path(session).resolve()
|
||||
urdfs = sorted(directory.glob(f"*zero_calibrated_{serial_number}_*.urdf"))
|
||||
if len(urdfs) != 1:
|
||||
raise ValueError("session must contain exactly one corrected URDF")
|
||||
return {
|
||||
"json": directory / f"g20_right_{serial_number}_calibration.json",
|
||||
"urdf": urdfs[0],
|
||||
"summary": directory / "calibration_summary_zh.json",
|
||||
"commands": directory / "mujoco_validation_commands.json",
|
||||
"raw": directory / "raw_samples.jsonl",
|
||||
"log": directory / "calibration.log",
|
||||
}
|
||||
|
||||
|
||||
def find_compatible_prior_session(
|
||||
config: ProductConfig,
|
||||
current: str | Path,
|
||||
current_payload: Mapping[str, Any],
|
||||
) -> tuple[Path | None, dict[str, float]]:
|
||||
current_path = Path(current).resolve()
|
||||
for candidate in sorted(config.session_root.glob("20??????_??????"), reverse=True):
|
||||
if candidate.resolve() == current_path:
|
||||
continue
|
||||
json_path = candidate / f"g20_right_{config.serial_number}_calibration.json"
|
||||
summary_path = candidate / "calibration_summary_zh.json"
|
||||
if not json_path.is_file() or not summary_path.is_file():
|
||||
continue
|
||||
try:
|
||||
summary = _load_json(summary_path)
|
||||
if not bool(summary.get("quality", {}).get("passed")):
|
||||
continue
|
||||
payload = _load_json(json_path)
|
||||
differences = compare_session_offsets(
|
||||
payload,
|
||||
current_payload,
|
||||
maximum_difference_rad=config.static_repeatability_rad,
|
||||
)
|
||||
except (OSError, ValueError, KeyError, TypeError, json.JSONDecodeError):
|
||||
continue
|
||||
return candidate, differences
|
||||
return None, {}
|
||||
|
||||
|
||||
def _verify_combination_validation(combination: Mapping[str, Any]) -> None:
|
||||
# The formal product uses the independently held-out fourth sweep cycle.
|
||||
# The optional eight-pose check remains a developer diagnostic because the
|
||||
# axis-line zero solve does not establish an absolute Cartesian hand-base
|
||||
# transform. When explicitly enabled, retain its strict coverage and
|
||||
# error checks.
|
||||
if not bool(combination.get("enabled")):
|
||||
return
|
||||
position_p95 = float(combination.get("position_p95_m", float("inf")))
|
||||
orientation_p95 = float(
|
||||
combination.get("orientation_p95_rad", float("inf"))
|
||||
)
|
||||
required = {str(value) for value in combination.get("required_targets", ())}
|
||||
expected = set(G20_COMBINATION_REQUIRED_TARGET_KEYS)
|
||||
observations = combination.get("observation_counts")
|
||||
validations = combination.get("validation_counts")
|
||||
if required != expected:
|
||||
raise ValueError("combination validation has the wrong required-target set")
|
||||
if not isinstance(observations, Mapping) or not isinstance(validations, Mapping):
|
||||
raise ValueError("combination validation is missing per-target coverage")
|
||||
missing_observations = sorted(
|
||||
key for key in expected if int(observations.get(key, 0)) < 2
|
||||
)
|
||||
missing_validations = sorted(
|
||||
key for key in expected if int(validations.get(key, 0)) < 1
|
||||
)
|
||||
if missing_observations or missing_validations:
|
||||
raise ValueError(
|
||||
"combination validation target coverage is incomplete: "
|
||||
f"observations={','.join(missing_observations) or '-'}; "
|
||||
f"validations={','.join(missing_validations) or '-'}"
|
||||
)
|
||||
if (
|
||||
not bool(combination.get("completed"))
|
||||
or int(combination.get("completed_poses", 0)) != 8
|
||||
or position_p95 > 0.003
|
||||
or orientation_p95 > math.radians(2.0)
|
||||
):
|
||||
raise ValueError(
|
||||
"eight-pose four-finger combination validation is incomplete or failed"
|
||||
)
|
||||
|
||||
|
||||
def finalize_session_artifacts(
|
||||
config: ProductConfig,
|
||||
session: str | Path,
|
||||
*,
|
||||
node_status: Mapping[str, Any],
|
||||
) -> tuple[dict[str, Any], bool]:
|
||||
"""Create diagnostics and update latest_passed only after every guard."""
|
||||
directory = Path(session).resolve()
|
||||
paths = session_artifact_paths(directory, config.serial_number)
|
||||
for name in ("json", "urdf", "raw", "log"):
|
||||
if not paths[name].is_file():
|
||||
raise ValueError(f"session is missing {name}: {paths[name]}")
|
||||
payload = _load_json(paths["json"])
|
||||
standalone_thumb = bool(
|
||||
payload.get("artifact_type")
|
||||
== "g20_right_standalone_thumb_calibration"
|
||||
)
|
||||
if standalone_thumb:
|
||||
standalone_thumb_offsets(payload)
|
||||
else:
|
||||
validate_compact_payload(payload)
|
||||
if payload.get("schema_version") != 4 or payload.get("side") != "right":
|
||||
raise ValueError("runtime JSON is not the compact right-hand schema v4")
|
||||
if not bool(payload.get("quality", {}).get("passed")):
|
||||
raise ValueError("runtime JSON quality is not passed")
|
||||
combination = node_status.get("combination_validation")
|
||||
if not isinstance(combination, Mapping):
|
||||
raise ValueError("node status is missing combination validation")
|
||||
_verify_combination_validation(combination)
|
||||
resume = node_status.get("resume", {})
|
||||
if not isinstance(resume, Mapping):
|
||||
raise ValueError("node status has invalid resume provenance")
|
||||
calibration_scope = str(
|
||||
resume.get("recalibration_scope", "full")
|
||||
).strip().lower()
|
||||
if calibration_scope not in {"full", "thumb", "fingers"}:
|
||||
raise ValueError("node status has an unsupported recalibration scope")
|
||||
recalibration_tasks = tuple(
|
||||
str(value) for value in resume.get("recalibration_task_keys", ())
|
||||
)
|
||||
if calibration_scope == "thumb" and not standalone_thumb and (
|
||||
not bool(resume.get("used"))
|
||||
or not str(resume.get("source_session", ""))
|
||||
or len(recalibration_tasks) != 4
|
||||
or any("thumb_" not in name for name in recalibration_tasks)
|
||||
):
|
||||
raise ValueError(
|
||||
"thumb recalibration is missing its passed base-session provenance"
|
||||
)
|
||||
if calibration_scope == "fingers" and (
|
||||
not bool(resume.get("used"))
|
||||
or not str(resume.get("source_session", ""))
|
||||
or len(recalibration_tasks) != 12
|
||||
or any("thumb_" in name for name in recalibration_tasks)
|
||||
):
|
||||
raise ValueError(
|
||||
"finger recalibration is missing its certified thumb-session "
|
||||
"provenance"
|
||||
)
|
||||
if standalone_thumb and calibration_scope != "thumb":
|
||||
raise ValueError("standalone thumb artifact has the wrong node scope")
|
||||
offsets = (
|
||||
standalone_thumb_offsets(payload)
|
||||
if standalone_thumb
|
||||
else active_offsets(payload)
|
||||
)
|
||||
if calibration_scope != "full" and not standalone_thumb:
|
||||
verify_partial_scope_preserves_certified_zeros(
|
||||
scope=calibration_scope,
|
||||
source_session=str(resume.get("source_session", "")),
|
||||
serial_root=directory.parent,
|
||||
serial_number=config.serial_number,
|
||||
current_offsets=offsets,
|
||||
)
|
||||
endpoint_offsets = {
|
||||
name: offsets[name]
|
||||
for name in RIGHT_19_MECHANICAL_ENDPOINT_JOINTS
|
||||
}
|
||||
changed_joints = verify_corrected_urdf(
|
||||
config.source_urdf,
|
||||
paths["urdf"],
|
||||
expected_offsets_rad=offsets,
|
||||
endpoint_anchored_offsets_rad=endpoint_offsets,
|
||||
)
|
||||
if standalone_thumb:
|
||||
clipped_runtime_joints = {}
|
||||
else:
|
||||
payload, clipped_runtime_joints = clamp_compact_payload_to_urdf_limits(
|
||||
payload, paths["urdf"]
|
||||
)
|
||||
if clipped_runtime_joints:
|
||||
atomic_write_json(paths["json"], payload)
|
||||
validate_runtime_curves_against_urdf_limits(payload, paths["urdf"])
|
||||
mesh_resources = verify_urdf_mesh_resources(paths["urdf"])
|
||||
mesh_hashes = {
|
||||
name: sha256_file(path) for name, path in mesh_resources.items()
|
||||
}
|
||||
commands = (
|
||||
{
|
||||
"artifact_type": "standalone_thumb_no_full_hand_validation",
|
||||
"poses": [],
|
||||
}
|
||||
if standalone_thumb
|
||||
else build_mujoco_validation_commands(payload["baseline_command_u8"])
|
||||
)
|
||||
atomic_write_json(paths["commands"], commands)
|
||||
|
||||
prior, differences = find_compatible_prior_session(config, directory, payload)
|
||||
release_ready = config.required_independent_passes == 1 or prior is not None
|
||||
quality = dict(payload["quality"])
|
||||
preserved_certified_zeros = sorted(
|
||||
name
|
||||
for name in ACTIVE_ZERO_JOINTS
|
||||
if (
|
||||
calibration_scope == "thumb"
|
||||
and not name.startswith("thumb_")
|
||||
)
|
||||
or (
|
||||
calibration_scope == "fingers"
|
||||
and name.startswith("thumb_")
|
||||
)
|
||||
)
|
||||
if standalone_thumb:
|
||||
preserved_certified_zeros = []
|
||||
calibrated_zero_joints = (
|
||||
sorted(
|
||||
name for name in ACTIVE_ZERO_JOINTS if name.startswith("thumb_")
|
||||
)
|
||||
if standalone_thumb
|
||||
else sorted(ACTIVE_ZERO_JOINTS)
|
||||
)
|
||||
summary: dict[str, Any] = {
|
||||
"schema_version": 1,
|
||||
"serial_number": config.serial_number,
|
||||
"session_id": f"{config.serial_number}_{directory.name}",
|
||||
"calibration_scope": calibration_scope,
|
||||
"inherited_base_session": (
|
||||
None
|
||||
if calibration_scope == "full" or standalone_thumb
|
||||
else str(resume.get("source_session"))
|
||||
),
|
||||
"freshly_calibrated_task_keys": list(recalibration_tasks),
|
||||
"preserved_certified_zero_joints": preserved_certified_zeros,
|
||||
"result": "PASS" if release_ready else "PASS_AWAITING_SECOND_SESSION",
|
||||
"quality": quality,
|
||||
"runtime_limit_clipped_bins": clipped_runtime_joints,
|
||||
"runtime_curve_domain": (
|
||||
"thumb_diagnostic_only"
|
||||
if standalone_thumb
|
||||
else "requested_command_u8"
|
||||
),
|
||||
"static_zero_calibrated_joints": calibrated_zero_joints,
|
||||
"non_thumb_zero_policy": (
|
||||
"source_cad_unchanged" if standalone_thumb else None
|
||||
),
|
||||
"retained_active_urdf_zero_joints": sorted(
|
||||
RETAINED_ACTIVE_ZERO_JOINTS
|
||||
),
|
||||
"retained_passive_urdf_joints": sorted(PASSIVE_JOINTS),
|
||||
"mimic_derived_dynamic_joints": [],
|
||||
"visually_measured_passive_joints": sorted(
|
||||
VISUALLY_MEASURED_FINGER_DIPS
|
||||
),
|
||||
"changed_urdf_joint_origins": list(changed_joints),
|
||||
"hashes": {
|
||||
"source_urdf_sha256": config.source_urdf_sha256,
|
||||
"camera_extrinsics_sha256": config.camera_extrinsics_sha256,
|
||||
"calibration_config_sha256": config.calibration_config_sha256,
|
||||
"corrected_urdf_sha256": sha256_file(paths["urdf"]),
|
||||
"mesh_resources_sha256": mesh_hashes,
|
||||
"calibration_json_sha256": sha256_file(paths["json"]),
|
||||
"raw_samples_sha256": sha256_file(paths["raw"]),
|
||||
},
|
||||
"holdout": {
|
||||
"training_cycles": [0, 1, 2],
|
||||
"validation_cycle": 3,
|
||||
"isolated": True,
|
||||
},
|
||||
"combination_validation": dict(combination),
|
||||
"formal_release": {
|
||||
"passed": release_ready,
|
||||
"required_independent_passes": config.required_independent_passes,
|
||||
"comparison_session": None if prior is None else prior.name,
|
||||
"maximum_static_difference_deg": (
|
||||
None if not differences else math.degrees(max(differences.values()))
|
||||
),
|
||||
},
|
||||
"node_status": dict(node_status),
|
||||
"created_at_utc": datetime.now(timezone.utc).isoformat(),
|
||||
"artifacts": {name: path.name for name, path in paths.items()},
|
||||
}
|
||||
atomic_write_json(paths["summary"], summary)
|
||||
# Recompute hashes after all files exist and validate the pair once more
|
||||
# immediately before the one atomic publication operation.
|
||||
if sha256_file(config.source_urdf) != config.source_urdf_sha256:
|
||||
raise ValueError("source URDF changed during calibration")
|
||||
verify_corrected_urdf(
|
||||
config.source_urdf,
|
||||
paths["urdf"],
|
||||
expected_offsets_rad=offsets,
|
||||
endpoint_anchored_offsets_rad=endpoint_offsets,
|
||||
)
|
||||
# Revalidate the same coordinate contract immediately before publication:
|
||||
# verify_corrected_urdf proves that corrected endpoint limits map back to
|
||||
# the original physical CAD endpoints, while the runtime curves must stay
|
||||
# inside those corrected-coordinate limits.
|
||||
if not standalone_thumb:
|
||||
validate_runtime_curves_against_urdf_limits(payload, paths["urdf"])
|
||||
final_mesh_hashes = {
|
||||
name: sha256_file(path)
|
||||
for name, path in verify_urdf_mesh_resources(paths["urdf"]).items()
|
||||
}
|
||||
if final_mesh_hashes != mesh_hashes:
|
||||
raise ValueError("URDF mesh resources changed during publication")
|
||||
if release_ready:
|
||||
atomic_session_pointer(
|
||||
config.session_root,
|
||||
"latest_thumb_passed" if standalone_thumb else "latest_passed",
|
||||
directory,
|
||||
)
|
||||
return summary, release_ready
|
||||
@@ -0,0 +1,192 @@
|
||||
"""Canonical command/feedback schema for calibration observations.
|
||||
|
||||
The hand command and its measured motor feedback are different physical
|
||||
domains. Durable samples always retain both. Fitting code may still use the
|
||||
historical ``command_u8`` key, but it is created only as an explicit projection
|
||||
of a canonical record at the fitting boundary.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from typing import Any, Iterable, Literal, Mapping
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
SAMPLE_KINDS = frozenset(
|
||||
{
|
||||
'sample',
|
||||
'baseline_hold_sample',
|
||||
'steady_command_sample',
|
||||
'palm_axis_sample',
|
||||
}
|
||||
)
|
||||
|
||||
FitDomain = Literal['default', 'requested', 'feedback']
|
||||
|
||||
|
||||
class SampleDataContractError(ValueError):
|
||||
"""A calibration observation mixes or omits command domains."""
|
||||
|
||||
|
||||
def _finite_u8(value: Any, field: str, *, integral: bool) -> int | float:
|
||||
try:
|
||||
number = float(value)
|
||||
except (TypeError, ValueError) as error:
|
||||
raise SampleDataContractError(
|
||||
f'DATA-CONTRACT-701:{field} must be numeric'
|
||||
) from error
|
||||
if not math.isfinite(number) or not 0.0 <= number <= 255.0:
|
||||
raise SampleDataContractError(
|
||||
f'DATA-CONTRACT-701:{field} must be finite and in [0, 255]'
|
||||
)
|
||||
if integral:
|
||||
rounded = int(round(number))
|
||||
if not math.isclose(number, rounded, rel_tol=0.0, abs_tol=1.0e-9):
|
||||
raise SampleDataContractError(
|
||||
f'DATA-CONTRACT-701:{field} must be an integer command'
|
||||
)
|
||||
return rounded
|
||||
return number
|
||||
|
||||
|
||||
def explicit_domain_value(
|
||||
source: Mapping[str, Any], domain: Literal['requested', 'feedback']
|
||||
) -> int | float:
|
||||
"""Read and validate one explicitly named domain from any observation."""
|
||||
field = (
|
||||
'requested_command_u8' if domain == 'requested' else 'feedback_u8'
|
||||
)
|
||||
if field not in source or source[field] is None:
|
||||
raise SampleDataContractError(
|
||||
f'DATA-CONTRACT-701:observation is missing explicit {field}'
|
||||
)
|
||||
return _finite_u8(source[field], field, integral=domain == 'requested')
|
||||
|
||||
|
||||
def canonical_sample_record(
|
||||
source: Mapping[str, Any],
|
||||
*,
|
||||
allow_legacy_command: bool = False,
|
||||
) -> dict[str, Any]:
|
||||
"""Return one durable, unambiguous calibration observation.
|
||||
|
||||
``allow_legacy_command`` is restricted to importing historical sessions
|
||||
and unit fixtures. New online observations must provide both explicit
|
||||
fields and therefore cannot silently reinterpret ``command_u8``.
|
||||
"""
|
||||
record = dict(source)
|
||||
kind = str(record.get('kind', ''))
|
||||
if not kind and allow_legacy_command:
|
||||
# Old in-memory steady-curve fixtures predate durable sample kinds.
|
||||
# This adapter is never enabled by the new online/import contract.
|
||||
kind = 'steady_command_sample'
|
||||
record['kind'] = kind
|
||||
if kind not in SAMPLE_KINDS:
|
||||
raise SampleDataContractError(
|
||||
f'DATA-CONTRACT-701:unsupported calibration sample kind {kind!r}'
|
||||
)
|
||||
|
||||
requested = record.get('requested_command_u8')
|
||||
feedback = record.get('feedback_u8')
|
||||
legacy = record.get('command_u8')
|
||||
if requested is None or feedback is None:
|
||||
if not allow_legacy_command or legacy is None:
|
||||
missing = [
|
||||
name
|
||||
for name, value in (
|
||||
('requested_command_u8', requested),
|
||||
('feedback_u8', feedback),
|
||||
)
|
||||
if value is None
|
||||
]
|
||||
raise SampleDataContractError(
|
||||
'DATA-CONTRACT-701:'
|
||||
f'{kind} is missing explicit {",".join(missing)}'
|
||||
)
|
||||
# Historical in-memory records used requested commands for settled
|
||||
# checkpoints and feedback bins for dense/baseline/palm observations.
|
||||
if requested is None:
|
||||
requested = legacy
|
||||
if feedback is None:
|
||||
feedback = legacy
|
||||
|
||||
record.pop('command_u8', None)
|
||||
record['requested_command_u8'] = explicit_domain_value(
|
||||
{'requested_command_u8': requested}, 'requested'
|
||||
)
|
||||
record['feedback_u8'] = explicit_domain_value(
|
||||
{'feedback_u8': feedback}, 'feedback'
|
||||
)
|
||||
return record
|
||||
|
||||
|
||||
def fitting_sample_record(
|
||||
source: Mapping[str, Any],
|
||||
*,
|
||||
domain: FitDomain = 'default',
|
||||
allow_legacy_command: bool = False,
|
||||
snap_requested_endpoints: bool = False,
|
||||
) -> dict[str, Any]:
|
||||
"""Project a canonical sample into the legacy curve-fitter interface."""
|
||||
record = canonical_sample_record(
|
||||
source, allow_legacy_command=allow_legacy_command
|
||||
)
|
||||
kind = str(record['kind'])
|
||||
selected = domain
|
||||
if selected == 'default':
|
||||
selected = (
|
||||
'requested' if kind == 'steady_command_sample' else 'feedback'
|
||||
)
|
||||
if selected not in {'requested', 'feedback'}:
|
||||
raise SampleDataContractError(
|
||||
f'DATA-CONTRACT-701:unsupported fitting domain {domain!r}'
|
||||
)
|
||||
requested = int(record['requested_command_u8'])
|
||||
if selected == 'requested' or (
|
||||
snap_requested_endpoints and requested in {0, 255}
|
||||
):
|
||||
index = requested
|
||||
else:
|
||||
index = int(
|
||||
np.clip(np.rint(float(record['feedback_u8'])), 0, 255)
|
||||
)
|
||||
record['command_u8'] = index
|
||||
return record
|
||||
|
||||
|
||||
def fitting_sample_records(
|
||||
records: Iterable[Mapping[str, Any]],
|
||||
*,
|
||||
domain: FitDomain = 'default',
|
||||
allow_legacy_command: bool = False,
|
||||
snap_requested_endpoints: bool = False,
|
||||
) -> list[dict[str, Any]]:
|
||||
"""Project several canonical samples into one explicit fitting domain."""
|
||||
return [
|
||||
fitting_sample_record(
|
||||
record,
|
||||
domain=domain,
|
||||
allow_legacy_command=allow_legacy_command,
|
||||
snap_requested_endpoints=snap_requested_endpoints,
|
||||
)
|
||||
for record in records
|
||||
]
|
||||
|
||||
|
||||
def validate_sample_records(
|
||||
records: Iterable[Mapping[str, Any]],
|
||||
*,
|
||||
allow_legacy_command: bool = False,
|
||||
) -> None:
|
||||
"""Validate a collection without changing its representation."""
|
||||
for index, record in enumerate(records):
|
||||
try:
|
||||
canonical_sample_record(
|
||||
record, allow_legacy_command=allow_legacy_command
|
||||
)
|
||||
except SampleDataContractError as error:
|
||||
raise SampleDataContractError(
|
||||
f'{error};record_index={index}'
|
||||
) from error
|
||||
@@ -30,6 +30,24 @@ def append_jsonl(path: str | Path, payload: Mapping[str, Any]) -> None:
|
||||
os.fsync(stream.fileno())
|
||||
|
||||
|
||||
def append_jsonl_many(
|
||||
path: str | Path, payloads: Iterable[Mapping[str, Any]]
|
||||
) -> None:
|
||||
"""Durably append a batch while paying the fsync cost only once."""
|
||||
destination = Path(path)
|
||||
destination.parent.mkdir(parents=True, exist_ok=True)
|
||||
lines = [
|
||||
json.dumps(payload, ensure_ascii=False, separators=(",", ":"))
|
||||
for payload in payloads
|
||||
]
|
||||
if not lines:
|
||||
return
|
||||
with destination.open("a", encoding="utf-8") as stream:
|
||||
stream.write("\n".join(lines) + "\n")
|
||||
stream.flush()
|
||||
os.fsync(stream.fileno())
|
||||
|
||||
|
||||
def load_jsonl(path: str | Path) -> list[dict[str, Any]]:
|
||||
source = Path(path)
|
||||
if not source.exists():
|
||||
|
||||
+893
@@ -0,0 +1,893 @@
|
||||
"""Chinese, operator-facing diagnostics for three-camera calibration."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
from typing import Any, Mapping, Sequence
|
||||
|
||||
|
||||
STATE_NAMES_ZH = {
|
||||
"PREFLIGHT": "设备和标签预检",
|
||||
"WAIT_START": "等待开始标定",
|
||||
"IMPORTING_BASE": "正在读取基础标定会话",
|
||||
"REVALIDATING_INHERITED": "正在复核继承的四指数据",
|
||||
"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（被动）",
|
||||
"middle_mcp_roll": "中指MCP侧摆",
|
||||
"middle_mcp_pitch": "中指MCP屈伸",
|
||||
"middle_pip": "中指PIP",
|
||||
"middle_dip": "中指DIP（被动）",
|
||||
"ring_mcp_roll": "无名指MCP侧摆",
|
||||
"ring_mcp_pitch": "无名指MCP屈伸",
|
||||
"ring_pip": "无名指PIP",
|
||||
"ring_dip": "无名指DIP（被动）",
|
||||
"pinky_mcp_roll": "小指MCP侧摆",
|
||||
"pinky_mcp_pitch": "小指MCP屈伸",
|
||||
"pinky_pip": "小指PIP",
|
||||
"pinky_dip": "小指DIP（被动）",
|
||||
"thumb_cmc_yaw": "拇指CMC侧摆",
|
||||
"index_mcp_roll_side": "食指MCP侧摆（侧面校验）",
|
||||
"middle_mcp_roll_side": "中指MCP侧摆（侧面校验）",
|
||||
"ring_mcp_roll_side": "无名指MCP侧摆（侧面校验）",
|
||||
"pinky_mcp_roll_side": "小指MCP侧摆（侧面校验）",
|
||||
}
|
||||
|
||||
|
||||
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") == "zero_model_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}零位/URDF验证失败，"
|
||||
f"电机{active.get('motor_index')}，不会自动重扫"
|
||||
)
|
||||
if active.get("kind") == "motion_stall":
|
||||
return (
|
||||
f"电机{active.get('motor_index', '?')}运动停滞，目标"
|
||||
f"{_format_u8(active.get('target_u8'))}、实际"
|
||||
f"{_format_u8(active.get('actual_u8'))}"
|
||||
)
|
||||
if active.get("kind") == "cross_view_roll_diagnostic":
|
||||
return (
|
||||
f"{active.get('finger', '?')}侧摆跨机位诊断完成："
|
||||
f"正面最大{float(active.get('front_maximum_deg', 0.0)):.2f}°，"
|
||||
f"侧面最大{float(active.get('side_maximum_deg', 0.0)):.2f}°"
|
||||
)
|
||||
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", "?")
|
||||
task = (
|
||||
f"{view}机位，{joint_text}，电机{active.get('motor_index')}，"
|
||||
f"第{cycle}/{repetitions}轮，{start}→{target}"
|
||||
)
|
||||
fit_attempt = int(active.get("fit_attempt", 1))
|
||||
if fit_attempt > 1:
|
||||
retry_cycles = active.get("fit_retry_cycles", [])
|
||||
if retry_cycles:
|
||||
task += "（补采异常轮" + "/".join(
|
||||
str(cycle) for cycle in retry_cycles
|
||||
) + "）"
|
||||
else:
|
||||
task += f"（拟合补采第{fit_attempt}次）"
|
||||
return task
|
||||
|
||||
|
||||
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.startswith("motor_state_stalled:"):
|
||||
fields = reason.split(":")
|
||||
context = fields[1] if len(fields) > 1 else "unknown"
|
||||
error_match = re.search(r"error_u8=([0-9.]+)", reason)
|
||||
error = error_match.group(1) if error_match else "未知"
|
||||
timeout_match = re.search(r"timeout_seconds=([0-9.]+)", reason)
|
||||
timeout_value = active.get("timeout_seconds")
|
||||
if timeout_value is None and timeout_match is not None:
|
||||
timeout_value = float(timeout_match.group(1))
|
||||
duration = (
|
||||
f"连续{float(timeout_value):g}秒"
|
||||
if timeout_value is not None
|
||||
else "在规定时间内"
|
||||
)
|
||||
motor = active.get("motor_index")
|
||||
if motor is not None:
|
||||
return (
|
||||
f"电机{motor}反馈{duration}没有向目标推进；目标"
|
||||
f"{_format_u8(active.get('target_u8'))}、实际"
|
||||
f"{_format_u8(active.get('actual_u8'))}、误差{error} u8，"
|
||||
f"允许容差±{_format_u8(active.get('tolerance_u8'))} u8"
|
||||
f"（阶段={context}）。程序已保持当前位置。",
|
||||
"若实际反馈是稳定的固件端点，应只配置该电机该端点的专用容差后"
|
||||
"重启；若仍在变化或有摩擦，则先排查机械问题，不要反复resume强推。",
|
||||
)
|
||||
return (
|
||||
f"电机反馈{duration}没有向目标推进；停止位置距目标{error}个u8"
|
||||
f"（阶段={context}）。程序已保持当前位置，防止机械碰撞或摩擦加重。",
|
||||
"检查该电机是否在机械端点稳定饱和或存在碰撞。若实际反馈已是该型号的"
|
||||
"正常端点，应配置该电机专用端点容差后重启标定；不要反复调用resume强推。",
|
||||
)
|
||||
|
||||
base_reason, separator, reason_detail = reason.partition(":")
|
||||
if base_reason in {
|
||||
"sweep_missing_endpoint_bin",
|
||||
"sweep_bins_too_few",
|
||||
"sweep_bin_gap_too_large",
|
||||
"task_precheck_missing_command_127",
|
||||
"task_precheck_detection_rate_too_low",
|
||||
"synchronised_tag_state_timeout",
|
||||
}:
|
||||
reason = base_reason
|
||||
detail_label = (
|
||||
JOINT_NAMES_ZH.get(reason_detail, reason_detail)
|
||||
if separator and reason_detail
|
||||
else ""
|
||||
)
|
||||
detail_prefix = f"{detail_label}：" if detail_label else ""
|
||||
|
||||
if "URDF zero offset reached the configured" in reason:
|
||||
bound_match = re.search(
|
||||
r"configured\s+([0-9.]+)\s+degree bound", reason
|
||||
)
|
||||
bound = bound_match.group(1) if bound_match else "配置的"
|
||||
hit_text = ""
|
||||
if "bound:" in reason:
|
||||
hit_text = reason.split("bound:", 1)[1].split(
|
||||
"; all_offsets:", 1
|
||||
)[0]
|
||||
for name, label in JOINT_NAMES_ZH.items():
|
||||
hit_text = hit_text.replace(name, label)
|
||||
hit_suffix = f"；触边关节：{hit_text}" if hit_text else ""
|
||||
return (
|
||||
f"联合URDF零位求解触及±{bound}°安全边界{hit_suffix}。这不是可靠的"
|
||||
"零位结果，而是三机位米制位姿或固定关节轴链无法由纯零位旋转共同解释。",
|
||||
"不要调用resume，也不要增大零位边界。先确认Tag有效黑框边长、三相机"
|
||||
"内外参和原始CAD URDF；Tag尺寸修正后必须调用start重新采集，旧尺度"
|
||||
"产生的轨迹不能直接生成修正URDF。",
|
||||
)
|
||||
|
||||
if reason == "sweep_missing_endpoint_bin":
|
||||
missing_text = "、".join(str(value) for value in missing) or "0或255"
|
||||
return (
|
||||
f"{detail_prefix}本方向已有{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"{detail_prefix}有效电机分箱只有{sample.get('bin_count', 0)}个，要求至少"
|
||||
f"{sample.get('minimum_bin_count', '?')}个；当前采样范围{sample_range}。",
|
||||
"检查Tag连续识别和电机状态频率，修正后调用resume重新扫描当前方向。",
|
||||
)
|
||||
if reason == "sweep_bin_gap_too_large":
|
||||
gap_start = sample.get("maximum_bin_gap_start_u8")
|
||||
gap_end = sample.get("maximum_bin_gap_end_u8")
|
||||
gap_range = (
|
||||
""
|
||||
if gap_start is None or gap_end is None
|
||||
else f"（{gap_start}→{gap_end}）"
|
||||
)
|
||||
return (
|
||||
f"{detail_prefix}轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')}，"
|
||||
f"{gap_range}允许值不超过"
|
||||
f"{sample.get('allowed_maximum_bin_gap', '?')}。",
|
||||
"检查运动中Tag是否间歇丢失；修正遮挡、反光或对焦后调用resume。",
|
||||
)
|
||||
if reason == "synchronised_tag_state_timeout":
|
||||
group_reasons = active.get("group_pnp_reasons", {})
|
||||
if isinstance(group_reasons, Mapping) and group_reasons:
|
||||
tag_rejections = active.get("pnp_rejection_counts", {})
|
||||
group_rejections = active.get(
|
||||
"group_pnp_rejection_counts", {}
|
||||
)
|
||||
missing_roles = active.get(
|
||||
"group_missing_candidate_roles", {}
|
||||
)
|
||||
candidate_diagnostics = active.get(
|
||||
"pnp_candidate_diagnostics", {}
|
||||
)
|
||||
view_details: list[str] = []
|
||||
for view, value in group_reasons.items():
|
||||
view_name = str(view)
|
||||
parts = [str(value)]
|
||||
missing = (
|
||||
missing_roles.get(view_name, ())
|
||||
if isinstance(missing_roles, Mapping)
|
||||
else ()
|
||||
)
|
||||
if isinstance(missing, Sequence) and not isinstance(
|
||||
missing, (str, bytes)
|
||||
) and missing:
|
||||
parts.append(
|
||||
"缺候选=" + ",".join(str(role) for role in missing)
|
||||
)
|
||||
counts: dict[str, int] = {}
|
||||
for source in (tag_rejections, group_rejections):
|
||||
values = (
|
||||
source.get(view_name)
|
||||
if isinstance(source, Mapping)
|
||||
else None
|
||||
)
|
||||
if isinstance(values, Mapping):
|
||||
for name, count in values.items():
|
||||
counts[str(name)] = counts.get(str(name), 0) + int(
|
||||
count
|
||||
)
|
||||
if counts:
|
||||
common = sorted(
|
||||
counts.items(), key=lambda pair: (-pair[1], pair[0])
|
||||
)[:3]
|
||||
parts.append(
|
||||
"累计拒绝="
|
||||
+ ",".join(
|
||||
f"{name}×{count}" for name, count in common
|
||||
)
|
||||
)
|
||||
view_candidates = (
|
||||
candidate_diagnostics.get(view_name, {})
|
||||
if isinstance(candidate_diagnostics, Mapping)
|
||||
else {}
|
||||
)
|
||||
if isinstance(view_candidates, Mapping) and missing:
|
||||
summaries: list[str] = []
|
||||
for role in missing:
|
||||
diagnostic = view_candidates.get(str(role), {})
|
||||
if not isinstance(diagnostic, Mapping):
|
||||
continue
|
||||
summaries.append(
|
||||
f"{role}(solve="
|
||||
f"{int(diagnostic.get('solved_candidate_count', 0))},"
|
||||
"reproj="
|
||||
f"{int(diagnostic.get('reprojection_candidate_count', 0))},"
|
||||
"tilt="
|
||||
f"{int(diagnostic.get('independent_tilt_candidate_count', 0))})"
|
||||
)
|
||||
if summaries:
|
||||
parts.append("候选统计=" + ",".join(summaries))
|
||||
view_details.append(
|
||||
f"{VIEW_NAMES_ZH.get(view_name, view_name)}="
|
||||
+ "；".join(parts)
|
||||
)
|
||||
reason_text = "、".join(view_details)
|
||||
return (
|
||||
f"{detail_prefix}已经取得部分有效轨迹，但Tag仍可见且反馈正常时，"
|
||||
"后续连续图像帧"
|
||||
"被整组PnP几何检查拒绝"
|
||||
f"（{reason_text}），因此无法与电机状态形成有效轨迹帧。",
|
||||
"不要调整或反复粘贴Tag；保留当前会话中的"
|
||||
"group_pnp_candidate_event，"
|
||||
"按缺失角色的候选统计检查PnP分支逻辑。",
|
||||
)
|
||||
return (
|
||||
f"{detail_prefix}运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态"
|
||||
"按时间戳配对”的图像帧。",
|
||||
"查看下面活动机位的缺失Tag，确认状态话题仍在更新；修正后调用resume，"
|
||||
"程序会重扫当前方向。",
|
||||
)
|
||||
if reason == "task_precheck_missing_command_127":
|
||||
return (
|
||||
f"{detail_prefix}低速预检没有取得反馈127附近的同步Tag样本。",
|
||||
"检查中位姿态的Tag遮挡和反光；程序只会重扫当前物理任务。",
|
||||
)
|
||||
if reason == "task_precheck_detection_rate_too_low":
|
||||
return (
|
||||
f"{detail_prefix}低速预检的有效Tag识别率低于门限。",
|
||||
"检查该机位当前任务Tag的遮挡、反光和对焦；程序只会重扫当前物理任务。",
|
||||
)
|
||||
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_start_tag_timeout":
|
||||
group_reasons = active.get("group_pnp_reasons", {})
|
||||
progress_by_view = active.get("pnp_initialization_progress", {})
|
||||
tag_rejections = active.get("pnp_rejection_counts", {})
|
||||
group_rejections = active.get("group_pnp_rejection_counts", {})
|
||||
if any(
|
||||
isinstance(value, Mapping) and bool(value)
|
||||
for value in (
|
||||
group_reasons,
|
||||
progress_by_view,
|
||||
tag_rejections,
|
||||
group_rejections,
|
||||
)
|
||||
):
|
||||
details: list[str] = []
|
||||
views = set()
|
||||
for value in (
|
||||
group_reasons,
|
||||
progress_by_view,
|
||||
tag_rejections,
|
||||
group_rejections,
|
||||
):
|
||||
if isinstance(value, Mapping):
|
||||
views.update(str(view) for view in value)
|
||||
for view in sorted(views):
|
||||
parts: list[str] = []
|
||||
progress = (
|
||||
progress_by_view.get(view)
|
||||
if isinstance(progress_by_view, Mapping)
|
||||
else None
|
||||
)
|
||||
if isinstance(progress, Mapping):
|
||||
parts.append(
|
||||
"初始化"
|
||||
f"{int(progress.get('accepted', 0))}/"
|
||||
f"{int(progress.get('required', 0))}"
|
||||
)
|
||||
counts: dict[str, int] = {}
|
||||
for source in (tag_rejections, group_rejections):
|
||||
values = (
|
||||
source.get(view)
|
||||
if isinstance(source, Mapping)
|
||||
else None
|
||||
)
|
||||
if isinstance(values, Mapping):
|
||||
for name, count in values.items():
|
||||
counts[str(name)] = (
|
||||
counts.get(str(name), 0) + int(count)
|
||||
)
|
||||
if counts:
|
||||
common = sorted(
|
||||
counts.items(), key=lambda pair: (-pair[1], pair[0])
|
||||
)[:3]
|
||||
parts.append(
|
||||
"累计拒绝="
|
||||
+ ",".join(
|
||||
f"{name}×{count}" for name, count in common
|
||||
)
|
||||
)
|
||||
latest = (
|
||||
group_reasons.get(view)
|
||||
if isinstance(group_reasons, Mapping)
|
||||
else None
|
||||
)
|
||||
if latest and not str(latest).startswith(
|
||||
"group_initializing:"
|
||||
):
|
||||
parts.append(f"最后状态={latest}")
|
||||
if parts:
|
||||
details.append(
|
||||
f"{VIEW_NAMES_ZH.get(view, view)}=" + "；".join(parts)
|
||||
)
|
||||
reason_text = "、".join(details) or "未形成完整初始化窗口"
|
||||
return (
|
||||
"被测电机已经到达扫描起点，所需Tag也可见，但三维PnP位姿初始化"
|
||||
f"没有完成（{reason_text}），因此没有生成同步端点帧。",
|
||||
"不要根据可见性重复粘贴Tag；保留累计拒绝原因并检查PnP候选选择。",
|
||||
)
|
||||
return (
|
||||
"被测电机已经到达扫描起点，但当前任务所需的实时运动Tag没有形成足够的"
|
||||
"同步有效帧。允许遮挡的固定掌部Tag会显示为“锁”，不会触发此错误。",
|
||||
"只检查标记为✗的实时运动Tag、反光和外部遮挡；不要移动相机或手掌底座。",
|
||||
)
|
||||
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 == "palm_orientation_quality_failed":
|
||||
failures = active.get("failures", [])
|
||||
detail = (
|
||||
str(failures[0].get("reason", "方向观测不足"))
|
||||
if failures
|
||||
else "方向观测不足"
|
||||
)
|
||||
if "thumb_cmc_" in detail:
|
||||
return (
|
||||
"拇指CMC yaw无法由顶部Tag 8/9的零位邻近短轨迹稳定确定："
|
||||
+ detail,
|
||||
"保持Tag安装不变；确保顶部Tag 8/9在拇指CMC pitch和roll"
|
||||
"从零位开始的前1/4行程持续可见后重新标定。",
|
||||
)
|
||||
return (
|
||||
"掌部公共方向无法由至少三根手指的短时正面轨迹稳定确定："
|
||||
+ detail,
|
||||
"保持Tag安装不变；让正面Tag 10–13在对应MCP-pitch起始段"
|
||||
"至少可见15°行程后重新标定。",
|
||||
)
|
||||
if reason in {"joint_fit_check_failed", "joint_fit_systematic_failure"}:
|
||||
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": "最大正反程差",
|
||||
"baseline_hysteresis_deg": "baseline正反程关节角差",
|
||||
"baseline_directional_gap_deg": "baseline方向分支间隙",
|
||||
"baseline_directional_gap_range_deg": "baseline分支间隙跨轮极差",
|
||||
"cycle_travel_range_deg": "三轮行程差",
|
||||
"rotation_orthogonal_rms_deg": "三维旋转轴外残差RMS",
|
||||
"axis_plane_rms_mm": "三维圆轴向RMS",
|
||||
"axis_radial_rms_mm": "三维圆半径RMS",
|
||||
"axis_pose_line_rms_mm": "姿态轨迹轴线RMS",
|
||||
"axis_line_cycle_rms_mm": "四轮轴线位置RMS",
|
||||
"rotation_circle_axis_difference_deg": "姿态轴与圆轨迹轴夹角",
|
||||
"axis_cycle_difference_deg": "各轮转轴方向极差",
|
||||
"cross_view_roll_curve": "正面/侧面关节角曲线差异RMS",
|
||||
"third_cycle_axis_holdout_deg": "第三轮留出零位可观测轴向误差",
|
||||
"third_cycle_axis_line_rms_mm": "第三轮留出轴线RMS",
|
||||
"third_cycle_trajectory_p95_deg": "第三轮留出轨迹误差P95",
|
||||
"zero_cycle_offset_range_deg": "训练轮零位极差",
|
||||
"zero_confidence_95_half_width_deg": "零位95%置信半宽",
|
||||
"state_image_sync_p95_ms": "图像与电机状态同步误差P95",
|
||||
"tag_valid_rate_percent": "所需Tag同时有效率",
|
||||
}
|
||||
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": "°",
|
||||
"baseline_hysteresis_deg": "°",
|
||||
"baseline_directional_gap_deg": "°",
|
||||
"baseline_directional_gap_range_deg": "°",
|
||||
"cycle_travel_range_deg": "°",
|
||||
"rotation_orthogonal_rms_deg": "°",
|
||||
"axis_plane_rms_mm": "mm",
|
||||
"axis_radial_rms_mm": "mm",
|
||||
"axis_pose_line_rms_mm": "mm",
|
||||
"axis_line_cycle_rms_mm": "mm",
|
||||
"rotation_circle_axis_difference_deg": "°",
|
||||
"axis_cycle_difference_deg": "°",
|
||||
"third_cycle_axis_holdout_deg": "°",
|
||||
"third_cycle_axis_line_rms_mm": "mm",
|
||||
"third_cycle_trajectory_p95_deg": "°",
|
||||
"zero_cycle_offset_range_deg": "°",
|
||||
"zero_confidence_95_half_width_deg": "°",
|
||||
"state_image_sync_p95_ms": "ms",
|
||||
"tag_valid_rate_percent": "%",
|
||||
"cross_view_roll_curve": "°",
|
||||
}
|
||||
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
|
||||
) + "）"
|
||||
cycle_values = failure.get("cycle_values_deg", [])
|
||||
if not cycle_values:
|
||||
cycle_values = failure.get("cycle_offset_deg", [])
|
||||
if cycle_values:
|
||||
detail += "（各轮=" + "/".join(
|
||||
f"{float(value):.2f}°" for value in cycle_values
|
||||
) + "）"
|
||||
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 "当前关节的轨迹拟合未通过"
|
||||
directional_gap_failure = any(
|
||||
str(failure.get("metric", "")).startswith(
|
||||
"baseline_directional_gap"
|
||||
)
|
||||
for failure in active.get("failures", [])
|
||||
)
|
||||
if reason == "joint_fit_systematic_failure":
|
||||
cross_view_systematic = any(
|
||||
failure.get("classification")
|
||||
in {
|
||||
"stable_cross_view_installation_or_model_bias",
|
||||
"stable_cross_view_direction_conflict",
|
||||
}
|
||||
for failure in active.get("failures", [])
|
||||
)
|
||||
suggestion = (
|
||||
"四轮都出现稳定的正面/侧面差异，属于Tag安装外参或跨视角模型偏差，"
|
||||
"继续重扫不会改善；检查Tag刚性安装与跨视角安装变换，不要放宽门限。"
|
||||
if cross_view_systematic
|
||||
else "各轮重复出现同一模型冲突，继续运动不会改善；程序已禁止自动重扫。"
|
||||
"请直接复制诊断块给开发者，不要放宽门限。"
|
||||
)
|
||||
else:
|
||||
source_task_names = set(active.get("source_task_names", []))
|
||||
thumb_yaw_source_retry = source_task_names == {
|
||||
"thumb_cmc_pitch_front",
|
||||
"thumb_cmc_roll_front",
|
||||
}
|
||||
suggestion = (
|
||||
"方向分支已由软件保留，不要放宽门限；请检查传动回差或高支架刚度，"
|
||||
"处理后重新执行一键标定命令，程序会从最近可靠断点继续。"
|
||||
if directional_gap_failure
|
||||
else (
|
||||
"保持顶部Tag 8/9无遮挡；程序只替换决定yaw零位的"
|
||||
"CMC pitch/roll顶部轴观测并重扫"
|
||||
f"{active.get('directions_to_rescan', 16)}个方向，"
|
||||
"不会无效重扫yaw侧摆。"
|
||||
if thumb_yaw_source_retry
|
||||
else "修正Tag位置、遮挡或机械行程后重新执行一键标定命令；"
|
||||
"程序只清除当前失败关节的数据并重扫"
|
||||
f"{active.get('directions_to_rescan', 6)}个方向，"
|
||||
"不需要手工调用ROS服务。"
|
||||
)
|
||||
)
|
||||
return (
|
||||
detail_text + "。程序已在当前关节结束后立即停止后续步骤。",
|
||||
suggestion,
|
||||
)
|
||||
if reason == "zero_model_validation_failed":
|
||||
reason_names = {
|
||||
"zero_offset_reached_configured_bound": "零位解触及安全边界",
|
||||
"zero_offset_exceeds_configured_limit": "零位估计超过安全范围",
|
||||
"zero_offset_reached_diagnostic_bound": "零位估计仍触及诊断搜索边界",
|
||||
"zero_offset_cycle_difference_too_large": "三轮零位离散过大",
|
||||
"zero_offset_not_statistically_significant": "零位偏移未达到统计显著性",
|
||||
"zero_axis_cone_mismatch_too_large": (
|
||||
"父子轴夹角与原始URDF不一致，零位旋转无法解释"
|
||||
),
|
||||
"zero_phase_axis_line_residual_too_large": (
|
||||
"整段SE(3)运动无法稳定确定平行轴线相位"
|
||||
),
|
||||
"zero_offset_did_not_improve_with_95pct_confidence": (
|
||||
"第三轮留出验证未以95%置信度改善"
|
||||
),
|
||||
}
|
||||
details: list[str] = []
|
||||
for failure in active.get("failures", []):
|
||||
joint = JOINT_NAMES_ZH.get(
|
||||
str(failure.get("joint")), str(failure.get("joint"))
|
||||
)
|
||||
if failure.get("metric") == "zero_guard":
|
||||
reason_text = reason_names.get(
|
||||
str(failure.get("reason")), str(failure.get("reason"))
|
||||
)
|
||||
if "actual_deg" in failure and "limit_deg" in failure:
|
||||
reason_text += (
|
||||
f"（估计{float(failure['actual_deg']):+.2f}°，"
|
||||
f"允许±{float(failure['limit_deg']):.2f}°）"
|
||||
)
|
||||
details.append(f"{joint}：{reason_text}")
|
||||
return (
|
||||
"轨迹采集已完成，但零位/URDF几何验证失败"
|
||||
+ ("（" + "；".join(details) + "）" if details else "")
|
||||
+ "。程序没有生成正式JSON或修正URDF。",
|
||||
"该类稳定模型失败不能靠重复运动修复，程序不会自动重扫；"
|
||||
"请检查Tag固定、相机外参和原始URDF后重新启动新标定。",
|
||||
)
|
||||
if reason in {"waiting_for_devices_and_sdk", "device_preflight_lost"}:
|
||||
return (
|
||||
"正在等待三台相机数据、内外参身份以及机械手SDK反馈就绪；此阶段不以Tag可见性阻止基准恢复。",
|
||||
"保持机械手运动范围无障碍；设备就绪后系统会先安全恢复基准形态，再检查掌部Tag。",
|
||||
)
|
||||
if reason in {"waiting_for_three_cameras_tags_and_sdk", "preflight_lost"}:
|
||||
return (
|
||||
"正在等待三台相机内参、外参身份匹配、帧率、全部必需Tag以及机械手SDK同时就绪。",
|
||||
"根据下面每个机位的缺失Tag和有效率排查；全部就绪后程序会进入等待开始状态。",
|
||||
)
|
||||
if reason == "waiting_for_baseline_tags_after_recovery":
|
||||
return (
|
||||
"机械手已经稳定恢复到基准形态，正在用新采集的画面确认三台相机各自的固定掌部Tag。",
|
||||
"若某个掌部Tag持续缺失，只调整遮挡手指或检查Tag固定情况，不要移动相机和手掌底座。",
|
||||
)
|
||||
if reason == "locking_fixed_base_references":
|
||||
return (
|
||||
"基准形态Tag预检已通过，正在把三台相机的固定掌部Tag稳健锁定为本会话参考。",
|
||||
"无需操作；锁定完成后允许任务姿态遮挡固定掌部Tag。",
|
||||
)
|
||||
if reason == "fixed_base_reference_moved":
|
||||
return (
|
||||
"顶部Tag 8在本会话基准锁定后连续多帧发生角点位移；程序已立即保持机械手当前位置，"
|
||||
"本会话中已采集数据不再用于发布。",
|
||||
"Tag 8允许在下一次标定预检前重新摆放，但本次不能继续；固定Tag 8和顶部相机后"
|
||||
"重新启动新会话。",
|
||||
)
|
||||
if reason == "waiting_for_task_tags_at_sweep_start":
|
||||
return (
|
||||
"电机已到扫描起点，正在等待当前任务的实时运动Tag；显示为“锁”的固定掌部Tag"
|
||||
"允许被手指遮挡。",
|
||||
"只检查标记为✗的实时运动Tag；若均为✓或锁，程序会自动开始运动。",
|
||||
)
|
||||
if reason == "call_start_for_baseline_recovery":
|
||||
return (
|
||||
"相机数据和机械手反馈已就绪，等待一键程序触发安全基准恢复。",
|
||||
"保持机械手运动范围无障碍；程序会自动开始，无需手工调用ROS服务。",
|
||||
)
|
||||
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 == "collecting_dedicated_baseline_hold":
|
||||
return (
|
||||
"正在从当前方向到达关节baseline并静止采集Tag与电机反馈；这批数据单独用于回差验收。",
|
||||
"无需操作，保持相机、标签和底座不动。",
|
||||
)
|
||||
if reason == "steady checkpoint target is missing":
|
||||
return (
|
||||
"首轮稳态检查点已经到达最终端点，但采集状态没有及时切换到端点完成阶段。",
|
||||
"程序已停止发布并保留已采样数据；这是软件状态切换问题，不需要调整相机、Tag或机械手。",
|
||||
)
|
||||
if reason == "cross_view_roll_front_failure_deferred":
|
||||
return (
|
||||
"正面侧摆回差不合格已保留，诊断模式将继续采集同一手指的侧面数据。",
|
||||
"无需操作；该诊断会锁定URDF发布。",
|
||||
)
|
||||
if reason == "cross_view_roll_diagnostic_complete":
|
||||
interpretation = str(active.get("interpretation", ""))
|
||||
explanations = {
|
||||
"both_views_confirm_direction_dependent_pose": (
|
||||
"正面和侧面都确认了方向相关姿态，优先判断为roll输出机构或共同下游链的真实回差。"
|
||||
),
|
||||
"front_only_difference_check_roll_tag_bracket_or_front_pnp": (
|
||||
"只有正面差异超限，优先检查roll Tag高支架刚度和正面PnP。"
|
||||
),
|
||||
"side_only_difference_check_side_tag_chain_or_side_pnp": (
|
||||
"只有侧面差异超限，优先检查侧面Tag链和侧面PnP。"
|
||||
),
|
||||
"both_views_within_formal_hysteresis_limit": (
|
||||
"两个机位的静止回差均满足正式门限。"
|
||||
),
|
||||
}
|
||||
return (
|
||||
explanations.get(interpretation, "四指侧摆跨机位诊断已经完成。")
|
||||
+ " 本次为诊断会话，不会生成或发布URDF。",
|
||||
"保存当前状态和raw_samples.jsonl；根据两机位结论处理后重新启动正式标定。",
|
||||
)
|
||||
if reason == "capturing_random_validation_pose":
|
||||
return "正在当前随机命令位置采集复测数据。", "无需操作，保持设备不动。"
|
||||
if reason in {"calibration_passed", "calibration_complete"}:
|
||||
return "三维轨迹、关节轴零位和第三轮留出验证已经完成。", "检查JSON、修正URDF路径和quality.passed。"
|
||||
if reason == "quality_failed":
|
||||
return "标定流程完成，但拟合或随机复测质量没有达到验收阈值。", "检查最终JSON的quality以及启动终端中的拟合日志。"
|
||||
if reason.startswith("validated_endpoint_zero_state"):
|
||||
return (
|
||||
"轨迹和URDF零位验证已经通过，但发布前检测到端点零位状态缺失或与"
|
||||
"已验证模型不一致；这是程序内部状态生命周期错误，结果未发布。",
|
||||
"不要移动相机、Tag或机械手底座；保留当前会话并把原因码交给开发者。",
|
||||
)
|
||||
if reason.startswith("PUB-ARTIFACT-601:"):
|
||||
return (
|
||||
"标定节点已经生成通过质量门限的JSON和候选URDF，但一键程序在正式发布前"
|
||||
"发现这对产物的坐标、限位、哈希或资源一致性检查失败；原始URDF未被覆盖。",
|
||||
"不要重新标定相机或调整Tag；保留本会话产物和启动日志供开发者检查发布契约。",
|
||||
)
|
||||
if reason == "combination_pose_prediction_failed":
|
||||
return (
|
||||
"单关节、零位和URDF几何验证已通过，但当前多关节组合姿态的Tag实测位姿与模型预测超过门限。",
|
||||
"程序会在原姿态重新初始化PnP并自动复测；若最终仍失败，请把raw_samples.jsonl中的"
|
||||
"combination_validation_failure记录交给开发者，不要重新采集16个单关节任务。",
|
||||
)
|
||||
if reason.startswith("prepare_") or state == "PREPARE_SWEEP":
|
||||
return "正在把当前电机移动到本方向的扫描起点并等待稳定。", "无需操作。"
|
||||
if reason == "holding_same_finger_clearance_before_next_task":
|
||||
return (
|
||||
"同一根手指的上一项已经完成；相邻手指继续保持当前避让姿态，只调整"
|
||||
"被测关节以衔接下一项。",
|
||||
"无需操作，不要手动展开正在避让的手指。",
|
||||
)
|
||||
if state == "RETURN_BASELINE":
|
||||
return "正在把已使用的标定电机恢复到目标姿态。", "无需操作。"
|
||||
if state == "FITTING":
|
||||
return "所有扫描已经完成，正在联合拟合三维机械轴和URDF零位偏移。", "无需操作。"
|
||||
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)
|
||||
scan_progress = float(
|
||||
payload.get(
|
||||
"scan_progress",
|
||||
0.0 if not total else float(completed) / float(total),
|
||||
)
|
||||
)
|
||||
lines = [
|
||||
f"状态：{STATE_NAMES_ZH.get(state, state)}（{state}）",
|
||||
f"原因：{reason_zh}",
|
||||
f"建议：{action_zh}",
|
||||
f"总体进度：{progress:.1%}（计划扫描{completed}/{total}个方向，"
|
||||
f"扫描进度{scan_progress:.1%}）",
|
||||
f"当前任务：{_task_text(active)}",
|
||||
]
|
||||
if state == "RETURN_BASELINE":
|
||||
baseline_command = payload.get("baseline_command_u8", [])
|
||||
return_command = payload.get("return_command_u8", baseline_command)
|
||||
label = "恢复姿态" if return_command != baseline_command else "基准姿态"
|
||||
lines.append(f"正在确认{label}：{return_command}")
|
||||
if active and active.get("kind") not in {
|
||||
"fit_failure",
|
||||
"zero_model_failure",
|
||||
"motion_stall",
|
||||
}:
|
||||
retry_count = int(active.get("automatic_retry_count", 0))
|
||||
if retry_count:
|
||||
lines.append(
|
||||
"自动重试：当前方向已自动重扫"
|
||||
f"{retry_count}/{active.get('automatic_retry_limit', '?')}次，"
|
||||
f"速度比例{float(active.get('retry_speed_scale', 1.0)):.0%}，"
|
||||
f"端点保持{float(active.get('endpoint_hold_seconds', 0.0)):.2f}s"
|
||||
)
|
||||
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'))}"
|
||||
)
|
||||
detection_frames = int(active.get("detection_frames", 0))
|
||||
if detection_frames:
|
||||
lines.append(
|
||||
"本方向Tag检出："
|
||||
f"{float(active.get('detection_rate', 0.0)):.1%}"
|
||||
f"（{active.get('detection_valid_frames', 0)}/"
|
||||
f"{detection_frames}帧）"
|
||||
)
|
||||
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')}"
|
||||
)
|
||||
if active.get("sweep_timeout_seconds") is not None:
|
||||
lines.append(
|
||||
"运动保护：扫描超时"
|
||||
f"{float(active['sweep_timeout_seconds']):.1f}s，"
|
||||
"连续"
|
||||
f"{float(active.get('motor_stall_timeout_seconds', 0.0)):.1f}s"
|
||||
"进展不足"
|
||||
f"{float(active.get('motor_stall_minimum_progress_u8', 0.0)):.1f}"
|
||||
"则立即暂停"
|
||||
)
|
||||
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"外参{'匹配' if view.get('camera_extrinsics_valid') 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}"
|
||||
)
|
||||
extrinsics_error = payload.get("camera_extrinsics_error")
|
||||
if extrinsics_error:
|
||||
lines.append(f"外参文件：{extrinsics_error}")
|
||||
lines.append(f"JSON结果：{payload.get('result_path') or '尚未生成'}")
|
||||
lines.append(
|
||||
f"修正URDF：{payload.get('corrected_urdf_path') or '尚未生成'}"
|
||||
)
|
||||
return "\n".join(lines)
|
||||
+13013
File diff suppressed because it is too large
Load Diff
@@ -634,6 +634,7 @@ def _fit_joint_curve(
|
||||
values: Sequence[float],
|
||||
*,
|
||||
endpoint_reference: Mapping[str, Sequence[float]] | None = None,
|
||||
preserve_direction_offset: bool = False,
|
||||
) -> tuple[dict[str, Any], float, float]:
|
||||
by_direction: dict[str, list[list[float]]] = {
|
||||
direction: [[] for _ in range(256)] for direction in DIRECTIONS
|
||||
@@ -669,9 +670,11 @@ def _fit_joint_curve(
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
raw -= raw[-1]
|
||||
if not preserve_direction_offset:
|
||||
raw -= raw[-1]
|
||||
projected_samples = isotonic_nonincreasing(raw)
|
||||
projected_samples -= projected_samples[-1]
|
||||
if not preserve_direction_offset:
|
||||
projected_samples -= projected_samples[-1]
|
||||
maximum_correction = max(
|
||||
maximum_correction,
|
||||
float(np.max(np.abs(projected_samples - raw))),
|
||||
@@ -681,7 +684,8 @@ def _fit_joint_curve(
|
||||
commands.astype(float),
|
||||
projected_samples,
|
||||
)
|
||||
curve -= curve[255]
|
||||
if not preserve_direction_offset:
|
||||
curve -= curve[255]
|
||||
if endpoint_reference is not None:
|
||||
curve = _regularize_coupled_zero_tail(
|
||||
curve,
|
||||
@@ -695,7 +699,8 @@ def _fit_joint_curve(
|
||||
increasing = np.asarray(fitted[DIRECTION_INCREASING], dtype=float)
|
||||
hysteresis = float(np.max(np.abs(decreasing - increasing)))
|
||||
combined = 0.5 * (decreasing + increasing)
|
||||
combined -= combined[255]
|
||||
if not preserve_direction_offset:
|
||||
combined -= combined[255]
|
||||
return (
|
||||
{
|
||||
"angle_rad": [round(float(value), 8) for value in combined],
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,37 @@
|
||||
"""Publish calibrated G20 URDF angles from raw command/feedback u8 values."""
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
return LaunchDescription(
|
||||
[
|
||||
DeclareLaunchArgument("hand_type", default_value="right"),
|
||||
DeclareLaunchArgument("calibration_file"),
|
||||
DeclareLaunchArgument("input_topic", default_value=""),
|
||||
DeclareLaunchArgument("output_topic", default_value=""),
|
||||
Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
executable="calibrated_joint_state_bridge",
|
||||
name=[
|
||||
"g20_calibrated_joint_state_bridge_",
|
||||
LaunchConfiguration("hand_type"),
|
||||
],
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
parameters=[
|
||||
{
|
||||
"hand_type": LaunchConfiguration("hand_type"),
|
||||
"calibration_file": LaunchConfiguration(
|
||||
"calibration_file"
|
||||
),
|
||||
"input_topic": LaunchConfiguration("input_topic"),
|
||||
"output_topic": LaunchConfiguration("output_topic"),
|
||||
}
|
||||
],
|
||||
),
|
||||
]
|
||||
)
|
||||
@@ -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,502 @@
|
||||
"""Launch three Hikrobot views and one complete-G20 calibration owner."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from datetime import datetime
|
||||
import hashlib
|
||||
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 _default_source_urdf(hand_type: str) -> Path:
|
||||
relative = Path(
|
||||
"assets/robots/hands/linker_hand"
|
||||
) / f"g20_{hand_type}" / f"linkerhand_g20_{hand_type}.urdf"
|
||||
workspace = Path.cwd() / "src/linkerhand_retarget/linkerhand_retarget" / relative
|
||||
try:
|
||||
installed = Path(get_package_share_directory("linkerhand_retarget")) / relative
|
||||
except Exception:
|
||||
installed = workspace
|
||||
return workspace if workspace.is_file() else installed
|
||||
|
||||
|
||||
def _launch_stack(context):
|
||||
from g20_thumb_apriltag_calibration.product import (
|
||||
get_product_calibration_contract,
|
||||
)
|
||||
|
||||
model = LaunchConfiguration("model").perform(context).strip().upper()
|
||||
hand_type = LaunchConfiguration("hand_type").perform(context).lower()
|
||||
if hand_type not in {"left", "right"}:
|
||||
raise RuntimeError("hand_type must be left or right")
|
||||
tag_layout = LaunchConfiguration("tag_layout").perform(context).lower()
|
||||
try:
|
||||
contract = get_product_calibration_contract(
|
||||
model, hand_type, tag_layout
|
||||
)
|
||||
except ValueError as error:
|
||||
raise RuntimeError(str(error)) from error
|
||||
requested_tag_config = LaunchConfiguration("tag_config").perform(context)
|
||||
package_share = Path(
|
||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
||||
)
|
||||
tag_config = (
|
||||
Path(requested_tag_config).expanduser().resolve()
|
||||
if requested_tag_config
|
||||
else package_share
|
||||
/ "config"
|
||||
/ (
|
||||
"three_camera_tags_g20_right_19.yaml"
|
||||
if tag_layout == "g20_right_19"
|
||||
else "three_camera_tags_g20_right_15.yaml"
|
||||
if tag_layout == "g20_right_15"
|
||||
else "three_camera_tags.yaml"
|
||||
)
|
||||
)
|
||||
if not tag_config.is_file():
|
||||
raise RuntimeError(f"tag config does not exist: {tag_config}")
|
||||
topic_prefix = f"/{model.lower()}"
|
||||
command_topic = f"{topic_prefix}/cb_{hand_type}_hand_control_cmd"
|
||||
state_topic = f"{topic_prefix}/cb_{hand_type}_hand_state"
|
||||
info_topic = f"{topic_prefix}/cb_{hand_type}_hand_info"
|
||||
requested_source = LaunchConfiguration("source_urdf_path").perform(context)
|
||||
source_urdf = (
|
||||
Path(requested_source).expanduser().resolve()
|
||||
if requested_source
|
||||
else _default_source_urdf(hand_type).resolve()
|
||||
)
|
||||
if not source_urdf.is_file():
|
||||
raise RuntimeError(f"source URDF does not exist: {source_urdf}")
|
||||
expected_source_hash = LaunchConfiguration(
|
||||
"source_urdf_expected_sha256"
|
||||
).perform(context).strip().lower()
|
||||
if contract.profile.supports("urdf_zero_publication"):
|
||||
if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None:
|
||||
raise RuntimeError(
|
||||
"G20 right product layout requires source_urdf_expected_sha256 confirmed "
|
||||
"by the CAD/hardware owner"
|
||||
)
|
||||
actual_source_hash = hashlib.sha256(source_urdf.read_bytes()).hexdigest()
|
||||
if actual_source_hash != expected_source_hash:
|
||||
raise RuntimeError(
|
||||
"source_urdf_expected_sha256 does not match source_urdf_path"
|
||||
)
|
||||
|
||||
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=[
|
||||
str(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": hand_type,
|
||||
"hand_joint": model,
|
||||
"can": LaunchConfiguration("can_interface"),
|
||||
"modbus": "None",
|
||||
"topic_prefix": topic_prefix,
|
||||
"move_on_startup": False,
|
||||
"startup_speed": ParameterValue(
|
||||
LaunchConfiguration("calibration_speed"), value_type=int
|
||||
),
|
||||
"startup_torque": 80,
|
||||
# Match 30 Hz cameras so state/image p95 skew stays below 50 ms.
|
||||
"state_poll_rate": 30.0,
|
||||
# Calibration does not consume measured joint velocity. A
|
||||
# G20 velocity read sends another five synchronous CAN
|
||||
# queries, so keep it off the trajectory-critical path.
|
||||
"velocity_poll_rate": 1.0,
|
||||
# Calibration sends one endpoint command per sweep. Keep
|
||||
# polling the real motor state during the following motion;
|
||||
# otherwise the SDK republishes a stale state for 0.2 s and
|
||||
# creates 17-27 command-unit holes in the trajectory bins.
|
||||
"defer_state_reads_while_commanding": False,
|
||||
"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,
|
||||
"model": model,
|
||||
"hand_type": hand_type,
|
||||
"tag_layout": tag_layout,
|
||||
"session_dir": str(session_dir),
|
||||
"resume_raw_samples_path": LaunchConfiguration(
|
||||
"resume_raw_samples_path"
|
||||
),
|
||||
"recalibration_scope": LaunchConfiguration(
|
||||
"recalibration_scope"
|
||||
),
|
||||
# The SDK performs roughly 25 synchronous CAN queries whenever
|
||||
# cb_<side>_hand_info has a subscriber. Calibration only used
|
||||
# that topic to display a speed diagnostic, while those reads
|
||||
# created 17-33 command-unit holes in position trajectories.
|
||||
"info_topic": "/g20_calibration/disabled_hand_info",
|
||||
"command_topic": command_topic,
|
||||
"state_topic": state_topic,
|
||||
"camera_extrinsics_file": LaunchConfiguration(
|
||||
"camera_extrinsics_file"
|
||||
),
|
||||
"source_urdf_path": str(source_urdf),
|
||||
"source_urdf_expected_sha256": LaunchConfiguration(
|
||||
"source_urdf_expected_sha256"
|
||||
),
|
||||
"corrected_urdf_output_dir": LaunchConfiguration(
|
||||
"corrected_urdf_output_dir"
|
||||
),
|
||||
**{
|
||||
f"{view}_camera_serial": camera_serials[view]
|
||||
for view in VIEWS
|
||||
},
|
||||
"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,
|
||||
),
|
||||
"adaptive_formal_speed_enabled": ParameterValue(
|
||||
LaunchConfiguration("adaptive_formal_speed_enabled"),
|
||||
value_type=bool,
|
||||
),
|
||||
"cross_view_roll_diagnostic_finger": LaunchConfiguration(
|
||||
"cross_view_roll_diagnostic_finger"
|
||||
),
|
||||
"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,
|
||||
command_topic,
|
||||
state_topic,
|
||||
info_topic,
|
||||
"/g20_calibration/status",
|
||||
],
|
||||
output="screen",
|
||||
)
|
||||
return [
|
||||
LogInfo(
|
||||
msg=(
|
||||
f"G20 {hand_type} {tag_layout} three-camera session: {session_dir}; "
|
||||
f"source_urdf={source_urdf}"
|
||||
)
|
||||
),
|
||||
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(
|
||||
[
|
||||
# Camera processes publish ~2 MB frames across DDS. Force the
|
||||
# matching RMW and provide both current and legacy profile names
|
||||
# so the configured 64 MB shared-memory segment is actually used.
|
||||
SetEnvironmentVariable(
|
||||
name="RMW_IMPLEMENTATION",
|
||||
value="rmw_fastrtps_cpp",
|
||||
),
|
||||
SetEnvironmentVariable(
|
||||
name="FASTDDS_DEFAULT_PROFILES_FILE",
|
||||
value=str(package_share / "config" / "fastdds_large_images.xml"),
|
||||
),
|
||||
SetEnvironmentVariable(
|
||||
name="FASTRTPS_DEFAULT_PROFILES_FILE",
|
||||
value=str(package_share / "config" / "fastdds_large_images.xml"),
|
||||
),
|
||||
DeclareLaunchArgument("model", default_value="G20"),
|
||||
DeclareLaunchArgument("hand_type", default_value="left"),
|
||||
DeclareLaunchArgument("tag_layout", default_value="legacy_11"),
|
||||
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(
|
||||
"adaptive_formal_speed_enabled", default_value="true"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"cross_view_roll_diagnostic_finger", default_value=""
|
||||
),
|
||||
DeclareLaunchArgument("validation_enabled", default_value="false"),
|
||||
DeclareLaunchArgument(
|
||||
"camera_extrinsics_file",
|
||||
default_value=str(
|
||||
Path.cwd() / "config" / "g20_three_camera_extrinsics.yaml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"source_urdf_path", default_value=""
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"source_urdf_expected_sha256", default_value=""
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"corrected_urdf_output_dir", default_value=""
|
||||
),
|
||||
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("resume_raw_samples_path", default_value=""),
|
||||
DeclareLaunchArgument("recalibration_scope", default_value="full"),
|
||||
DeclareLaunchArgument(
|
||||
"calibration_config",
|
||||
default_value=str(
|
||||
package_share / "config" / "three_camera_calibration.yaml"
|
||||
),
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"tag_config",
|
||||
default_value="",
|
||||
),
|
||||
OpaqueFunction(function=_launch_stack),
|
||||
]
|
||||
)
|
||||
@@ -0,0 +1,199 @@
|
||||
"""Launch three Hikrobot cameras for one-time checkerboard extrinsics."""
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetEnvironmentVariable
|
||||
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(context):
|
||||
cameras = []
|
||||
rectifiers = []
|
||||
serials = {}
|
||||
for view in VIEWS:
|
||||
serial = LaunchConfiguration(f"{view}_camera_serial").perform(context)
|
||||
if not serial:
|
||||
raise RuntimeError(f"{view}_camera_serial is required")
|
||||
serials[view] = serial
|
||||
namespace = f"/g20_extrinsics/{view}/camera"
|
||||
cameras.append(
|
||||
Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
executable="hikrobot_camera_node",
|
||||
name="hikrobot_camera",
|
||||
namespace=namespace,
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
parameters=[
|
||||
{
|
||||
"serial_number": serial,
|
||||
"expected_model": LaunchConfiguration("camera_model"),
|
||||
"camera_name": LaunchConfiguration(
|
||||
f"{view}_camera_name"
|
||||
),
|
||||
"frame_id": f"g20_extrinsics_{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": False,
|
||||
"camera_info_url": LaunchConfiguration(
|
||||
f"{view}_camera_info_url"
|
||||
),
|
||||
}
|
||||
],
|
||||
)
|
||||
)
|
||||
rectifiers.append(
|
||||
ComposableNode(
|
||||
package="image_proc",
|
||||
plugin="image_proc::RectifyNode",
|
||||
name=f"rectify_{view}",
|
||||
namespace=namespace,
|
||||
remappings=[
|
||||
("image", f"{namespace}/image_raw"),
|
||||
("camera_info", f"{namespace}/camera_info"),
|
||||
("image_rect", f"{namespace}/image_rect"),
|
||||
],
|
||||
parameters=[{"queue_size": 1}],
|
||||
extra_arguments=[{"use_intra_process_comms": True}],
|
||||
)
|
||||
)
|
||||
container = ComposableNodeContainer(
|
||||
name="g20_extrinsics_vision",
|
||||
namespace="/",
|
||||
package="rclcpp_components",
|
||||
executable="component_container_mt",
|
||||
composable_node_descriptions=rectifiers,
|
||||
output="screen",
|
||||
)
|
||||
solver = Node(
|
||||
package="g20_thumb_apriltag_calibration",
|
||||
executable="three_camera_extrinsics_node",
|
||||
name="g20_camera_extrinsics",
|
||||
output="screen",
|
||||
emulate_tty=True,
|
||||
parameters=[
|
||||
{
|
||||
"output_file": LaunchConfiguration("output_file"),
|
||||
"checkerboard_columns": ParameterValue(
|
||||
LaunchConfiguration("checkerboard_columns"), value_type=int
|
||||
),
|
||||
"checkerboard_rows": ParameterValue(
|
||||
LaunchConfiguration("checkerboard_rows"), value_type=int
|
||||
),
|
||||
"square_size_m": ParameterValue(
|
||||
LaunchConfiguration("square_size_m"), value_type=float
|
||||
),
|
||||
"enable_gui": ParameterValue(
|
||||
LaunchConfiguration("enable_gui"), value_type=bool
|
||||
),
|
||||
"gui_refresh_hz": ParameterValue(
|
||||
LaunchConfiguration("gui_refresh_hz"), value_type=float
|
||||
),
|
||||
"maximum_reprojection_rms_px": ParameterValue(
|
||||
LaunchConfiguration("maximum_reprojection_rms_px"),
|
||||
value_type=float,
|
||||
),
|
||||
"maximum_single_camera_reprojection_rms_px": ParameterValue(
|
||||
LaunchConfiguration(
|
||||
"maximum_single_camera_reprojection_rms_px"
|
||||
),
|
||||
value_type=float,
|
||||
),
|
||||
"auto_capture_default": ParameterValue(
|
||||
LaunchConfiguration("auto_capture_default"),
|
||||
value_type=bool,
|
||||
),
|
||||
"auto_capture_stable_seconds": ParameterValue(
|
||||
LaunchConfiguration("auto_capture_stable_seconds"),
|
||||
value_type=float,
|
||||
),
|
||||
**{
|
||||
f"{view}_camera_serial": serials[view]
|
||||
for view in VIEWS
|
||||
},
|
||||
}
|
||||
],
|
||||
)
|
||||
return [*cameras, container, solver]
|
||||
|
||||
|
||||
def generate_launch_description() -> LaunchDescription:
|
||||
package_share = Path(
|
||||
get_package_share_directory("g20_thumb_apriltag_calibration")
|
||||
)
|
||||
camera_info = Path.home() / ".ros" / "camera_info"
|
||||
return LaunchDescription(
|
||||
[
|
||||
# Keep the large-image transport deterministic even when the
|
||||
# calling shell selected another ROS 2 RMW implementation.
|
||||
SetEnvironmentVariable(
|
||||
name="RMW_IMPLEMENTATION",
|
||||
value="rmw_fastrtps_cpp",
|
||||
),
|
||||
SetEnvironmentVariable(
|
||||
name="FASTDDS_DEFAULT_PROFILES_FILE",
|
||||
value=str(
|
||||
package_share / "config" / "fastdds_large_images.xml"
|
||||
),
|
||||
),
|
||||
SetEnvironmentVariable(
|
||||
name="FASTRTPS_DEFAULT_PROFILES_FILE",
|
||||
value=str(
|
||||
package_share / "config" / "fastdds_large_images.xml"
|
||||
),
|
||||
),
|
||||
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(camera_info / "hikrobot_DB2163742.yaml")),
|
||||
DeclareLaunchArgument("side_camera_info_url", default_value=str(camera_info / "hikrobot_DB2163749.yaml")),
|
||||
DeclareLaunchArgument("top_camera_info_url", default_value=str(camera_info / "hikrobot_DB2163739.yaml")),
|
||||
DeclareLaunchArgument("camera_frame_rate", default_value="15.0"),
|
||||
DeclareLaunchArgument("exposure_time_us", default_value="5000.0"),
|
||||
DeclareLaunchArgument("gain_db", default_value="0.0"),
|
||||
DeclareLaunchArgument("checkerboard_columns", default_value="8"),
|
||||
DeclareLaunchArgument("checkerboard_rows", default_value="5"),
|
||||
DeclareLaunchArgument("square_size_m", default_value="0.027"),
|
||||
DeclareLaunchArgument("enable_gui", default_value="true"),
|
||||
DeclareLaunchArgument("gui_refresh_hz", default_value="2.0"),
|
||||
DeclareLaunchArgument(
|
||||
"maximum_reprojection_rms_px", default_value="1.2"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"maximum_single_camera_reprojection_rms_px",
|
||||
default_value="1.5",
|
||||
),
|
||||
DeclareLaunchArgument("auto_capture_default", default_value="false"),
|
||||
DeclareLaunchArgument(
|
||||
"auto_capture_stable_seconds", default_value="1.0"
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"output_file",
|
||||
default_value=str(Path.cwd() / "config" / "g20_three_camera_extrinsics.yaml"),
|
||||
),
|
||||
OpaqueFunction(function=_launch),
|
||||
]
|
||||
)
|
||||
@@ -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>One-command three-camera AprilTag calibration and zero-URDF correction for the G20 right 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="Profile-driven three-camera AprilTag hand calibration",
|
||||
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,35 @@ setup(
|
||||
"cmc_roll_calibration_node = "
|
||||
"g20_thumb_apriltag_calibration.zero_node:main"
|
||||
),
|
||||
(
|
||||
"three_camera_calibration_node = "
|
||||
"g20_thumb_apriltag_calibration.three_camera_node:main"
|
||||
),
|
||||
(
|
||||
"three_camera_extrinsics_node = "
|
||||
"g20_thumb_apriltag_calibration.extrinsics_node:main"
|
||||
),
|
||||
(
|
||||
"offline_replay = "
|
||||
"g20_thumb_apriltag_calibration.offline_replay:main"
|
||||
),
|
||||
(
|
||||
"camera_alignment_view = "
|
||||
"g20_thumb_apriltag_calibration.alignment_view:main"
|
||||
),
|
||||
(
|
||||
"calibrated_joint_state_bridge = "
|
||||
"g20_thumb_apriltag_calibration."
|
||||
"calibrated_joint_state_bridge:main"
|
||||
),
|
||||
(
|
||||
"calibrate_g20_right = "
|
||||
"g20_thumb_apriltag_calibration.one_command:main"
|
||||
),
|
||||
(
|
||||
"calibrate_hand = "
|
||||
"g20_thumb_apriltag_calibration.one_command: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
|
||||
@@ -0,0 +1,177 @@
|
||||
import copy
|
||||
|
||||
import pytest
|
||||
|
||||
from g20_thumb_apriltag_calibration.calibrated_joint_state_bridge import (
|
||||
G20_COMMAND_NAMES,
|
||||
G20_URDF_JOINT_NAMES,
|
||||
CalibratedCommandMapper,
|
||||
default_input_topic,
|
||||
)
|
||||
from g20_thumb_apriltag_calibration.full_hand import (
|
||||
JointCurveFit,
|
||||
build_compact_payload,
|
||||
get_hand_calibration_profile,
|
||||
)
|
||||
from g20_thumb_apriltag_calibration.urdf_zero import (
|
||||
get_zero_calibration_profile,
|
||||
)
|
||||
|
||||
|
||||
def _payload(side: str = "right") -> dict:
|
||||
profile = get_hand_calibration_profile(side)
|
||||
curve = [round((255 - command) * 0.001, 8) for command in range(256)]
|
||||
joints = {}
|
||||
for name, spec in profile.joint_specs.items():
|
||||
joint = {
|
||||
"motor_index": spec.motor_index,
|
||||
"angle_rad": list(curve),
|
||||
}
|
||||
if spec.active:
|
||||
joint["zero_command_u8"] = 255
|
||||
joint["zero_angles"] = {"urdf_zero_offset_rad": 0.0}
|
||||
else:
|
||||
joint["passive"] = True
|
||||
if spec.source_joint is not None:
|
||||
joint["source_joint"] = spec.source_joint
|
||||
joints[name] = joint
|
||||
return {
|
||||
"schema_version": 4,
|
||||
"model": "G20",
|
||||
"side": side,
|
||||
"serial_number": "TEST_RIGHT" if side == "right" else "TEST_LEFT",
|
||||
"angle_unit": "rad",
|
||||
"command_range": [0, 255],
|
||||
"baseline_command_u8": [255] * 20,
|
||||
"joints": joints,
|
||||
"quality": {
|
||||
"passed": True,
|
||||
"validation_mae_rad": 0.01,
|
||||
"validation_p95_rad": 0.02,
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def test_mapper_uses_each_joint_motor_and_includes_passive_joints() -> None:
|
||||
mapper = CalibratedCommandMapper(_payload(), expected_side="right")
|
||||
command = list(range(20))
|
||||
result = dict(
|
||||
zip(G20_URDF_JOINT_NAMES, mapper.map_positions(command))
|
||||
)
|
||||
|
||||
assert result["thumb_cmc_pitch"] == pytest.approx(0.255)
|
||||
assert result["thumb_cmc_roll"] == pytest.approx(0.250)
|
||||
assert result["thumb_cmc_yaw"] == pytest.approx(0.245)
|
||||
assert result["thumb_mcp"] == pytest.approx(0.240)
|
||||
assert result["thumb_ip"] == pytest.approx(0.240)
|
||||
assert result["pinky_pip"] == pytest.approx(0.236)
|
||||
assert result["pinky_dip"] == pytest.approx(0.236)
|
||||
|
||||
|
||||
def test_mapper_uses_names_instead_of_message_order() -> None:
|
||||
mapper = CalibratedCommandMapper(_payload(), expected_side="right")
|
||||
command = list(range(20))
|
||||
names = list(reversed(G20_COMMAND_NAMES))
|
||||
positions = list(reversed(command))
|
||||
|
||||
assert mapper.map_positions(positions, names) == mapper.map_positions(command)
|
||||
|
||||
|
||||
def test_mapper_rejects_wrong_side_and_unapproved_payload() -> None:
|
||||
with pytest.raises(ValueError, match="does not match"):
|
||||
CalibratedCommandMapper(_payload("left"), expected_side="right")
|
||||
|
||||
payload = copy.deepcopy(_payload())
|
||||
payload["quality"]["passed"] = False
|
||||
with pytest.raises(ValueError, match="quality.passed"):
|
||||
CalibratedCommandMapper(payload, expected_side="right")
|
||||
|
||||
|
||||
def test_mapper_rejects_incomplete_named_command() -> None:
|
||||
mapper = CalibratedCommandMapper(_payload(), expected_side="right")
|
||||
with pytest.raises(ValueError, match="missing named channels"):
|
||||
mapper.map_positions([255.0], ["thumb_cmc_pitch"])
|
||||
|
||||
|
||||
def test_right_19_schema_v4_mapper_uses_requested_command_midpoint_curve() -> None:
|
||||
profile = get_hand_calibration_profile("right", "g20_right_15")
|
||||
zero = get_zero_calibration_profile("right", "g20_right_15")
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [127] * 4
|
||||
fits = {}
|
||||
diagnostics = {}
|
||||
for name, spec in profile.joint_specs.items():
|
||||
zero_command = baseline[spec.motor_index]
|
||||
average = tuple(
|
||||
0.001 * (zero_command - value) for value in range(256)
|
||||
)
|
||||
decreasing = tuple(1.1 * value for value in average)
|
||||
increasing = tuple(0.9 * value for value in average)
|
||||
fits[name] = JointCurveFit(
|
||||
average,
|
||||
decreasing,
|
||||
increasing,
|
||||
{},
|
||||
0.0,
|
||||
0.0,
|
||||
{"rotation_orthogonal_rms_rad": 0.0, "arc_rad": 0.5},
|
||||
)
|
||||
diagnostics[name] = {
|
||||
"cycle_travel_rad": [0.25] * 4,
|
||||
"cycle_travel_range_rad": 0.0,
|
||||
"baseline_hysteresis_by_cycle_rad": [0.0] * 4,
|
||||
"holdout_cycle": 3,
|
||||
"holdout_cycle_mae_rad": 0.0,
|
||||
"holdout_cycle_max_rad": 0.0,
|
||||
}
|
||||
payload = build_compact_payload(
|
||||
serial_number="TEST_RIGHT_V5",
|
||||
measured_fits=fits,
|
||||
urdf_zero_offsets_rad={name: 0.0 for name in profile.active_joints},
|
||||
validation_errors_rad=[0.0],
|
||||
passed=True,
|
||||
baseline=baseline,
|
||||
side="right",
|
||||
layout_id="g20_right_15",
|
||||
zero_uncertainty_rad={name: 0.0 for name in zero.direct_zero_joints},
|
||||
zero_cycle_offsets_rad={
|
||||
name: (0.0, 0.0, 0.0) for name in zero.direct_zero_joints
|
||||
},
|
||||
zero_observers=zero.offset_observer_joint,
|
||||
artifact_hashes={
|
||||
"source_urdf_sha256": "0" * 64,
|
||||
"camera_extrinsics_sha256": "1" * 64,
|
||||
"corrected_urdf_sha256": "2" * 64,
|
||||
},
|
||||
cross_view_roll_metrics={
|
||||
f"{finger}_mcp_roll": {"angle_rad_rms_difference_rad": 0.0}
|
||||
for finger in ("index", "middle", "ring", "pinky")
|
||||
},
|
||||
joint_dynamic_diagnostics=diagnostics,
|
||||
zero_geometry_diagnostics={
|
||||
"training_cycles": (0, 1, 2),
|
||||
"validation_cycle": 3,
|
||||
"axis_line_rms_m": 0.0,
|
||||
"validation_line_error_by_joint_m": {},
|
||||
},
|
||||
)
|
||||
mapper = CalibratedCommandMapper(payload, expected_side="right")
|
||||
command = [255.0] * 20
|
||||
command[0] = 100.0
|
||||
mapped = dict(zip(G20_URDF_JOINT_NAMES, mapper.map_positions(command)))
|
||||
|
||||
assert mapper.input_domain == "command_u8"
|
||||
assert mapper.layout_id == "g20_right_19"
|
||||
# midpoint(1.1, 0.9) is the original 0.001-rad/u8 curve.
|
||||
assert mapped["thumb_cmc_pitch"] == pytest.approx(0.155)
|
||||
assert payload["joints"]["pinky_dip"]["passive"] is True
|
||||
assert "source_joint" not in payload["joints"]["pinky_dip"]
|
||||
|
||||
|
||||
def test_default_topics_use_the_g20_sdk_namespace() -> None:
|
||||
assert default_input_topic("right", "command_u8") == (
|
||||
"/g20/cb_right_hand_control_cmd"
|
||||
)
|
||||
assert default_input_topic("right", "feedback_u8") == (
|
||||
"/g20/cb_right_hand_state"
|
||||
)
|
||||
@@ -1,4 +1,5 @@
|
||||
from pathlib import Path
|
||||
from xml.etree import ElementTree
|
||||
|
||||
import yaml
|
||||
|
||||
@@ -6,6 +7,36 @@ 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_three_camera_launches_force_fastdds_large_image_transport() -> None:
|
||||
for launch_name in (
|
||||
"three_camera_calibration.launch.py",
|
||||
"three_camera_extrinsics.launch.py",
|
||||
):
|
||||
launch_text = (PACKAGE_ROOT / "launch" / launch_name).read_text()
|
||||
assert 'name="RMW_IMPLEMENTATION"' in launch_text
|
||||
assert 'value="rmw_fastrtps_cpp"' in launch_text
|
||||
assert 'name="FASTDDS_DEFAULT_PROFILES_FILE"' in launch_text
|
||||
assert 'name="FASTRTPS_DEFAULT_PROFILES_FILE"' in launch_text
|
||||
|
||||
|
||||
def test_front_tag_parameters_match_namespaced_detector() -> None:
|
||||
config = yaml.safe_load(
|
||||
(PACKAGE_ROOT / "config" / "front_tags.yaml").read_text()
|
||||
@@ -24,6 +55,180 @@ def test_front_tag_parameters_match_namespaced_detector() -> None:
|
||||
assert detector["detector"]["debug"] is False
|
||||
|
||||
|
||||
def test_three_camera_tag_ids_and_topics_use_eleven_unique_tags() -> 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["size"] == 0.016
|
||||
assert parameters["tag"]["sizes"] == [0.016] * len(ids)
|
||||
assert parameters["qos_profile"] == "sensor_data"
|
||||
assert parameters["detector"]["decimate"] == 1.5
|
||||
all_ids.update(ids)
|
||||
assert all_ids == set(range(11))
|
||||
assert tags[
|
||||
"/g20_calibration/side/apriltag/apriltag"
|
||||
]["ros__parameters"]["tag"]["frames"][0] == "side_base"
|
||||
|
||||
|
||||
def test_right_19_tag_config_matches_the_physical_layout() -> None:
|
||||
tags = yaml.safe_load(
|
||||
(
|
||||
PACKAGE_ROOT
|
||||
/ "config"
|
||||
/ "three_camera_tags_g20_right_19.yaml"
|
||||
).read_text()
|
||||
)
|
||||
expected = {
|
||||
"front": [0, 1, 2, 3, 10, 11, 12, 13],
|
||||
"side": [4, 5, 6, 7, 14, 15, 16, 17, 18],
|
||||
"top": [8, 9],
|
||||
}
|
||||
all_ids: set[int] = set()
|
||||
for view, ids in expected.items():
|
||||
parameters = tags[
|
||||
f"/g20_calibration/{view}/apriltag/apriltag"
|
||||
]["ros__parameters"]
|
||||
assert parameters["tag"]["ids"] == ids
|
||||
expected_sizes = [0.016 for _tag_id in ids]
|
||||
assert parameters["tag"]["sizes"] == expected_sizes
|
||||
all_ids.update(ids)
|
||||
assert all_ids == set(range(19))
|
||||
side_frames = tags[
|
||||
"/g20_calibration/side/apriltag/apriltag"
|
||||
]["ros__parameters"]["tag"]["frames"]
|
||||
assert side_frames == [
|
||||
"side_base", "ring_pip", "pinky_pip", "pinky_dip", "ring_dip",
|
||||
"middle_pip", "middle_dip", "index_pip", "index_dip",
|
||||
]
|
||||
|
||||
|
||||
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["tag_size_m"] == 0.016
|
||||
assert parameters["tag_size_override_ids"] == [7, 14, 16, 18]
|
||||
assert parameters["tag_size_overrides_m"] == [0.016] * 4
|
||||
|
||||
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["adaptive_formal_speed_enabled"] is True
|
||||
assert parameters["adaptive_formal_speed_max_scale"] == 1.5
|
||||
assert parameters["adaptive_formal_speed_minimum_bins"] == 64
|
||||
assert parameters["adaptive_formal_speed_maximum_bin_gap"] == 8
|
||||
assert parameters["right_thumb_yaw_255_endpoint_tolerance_u8"] == 5.0
|
||||
assert parameters["speed_setting_settle_seconds"] >= 0.2
|
||||
assert parameters["top_pnp_invalid_reset_seconds"] == 1.0
|
||||
assert parameters["pnp_group_initialization_frames"] == 8
|
||||
assert parameters["pnp_group_normal_alignment_scale_deg"] == 5.0
|
||||
assert parameters["pnp_group_maximum_normal_alignment_deg"] == 15.0
|
||||
assert parameters["thumb_ip_pnp_coupling_multiplier"] == 1.03
|
||||
assert parameters["thumb_ip_pnp_coupling_scale_deg"] == 3.0
|
||||
assert parameters["thumb_ip_pnp_maximum_coupling_residual_deg"] == 7.5
|
||||
assert parameters["baseline_hold_seconds"] == 0.5
|
||||
assert parameters["minimum_baseline_hold_frames"] == 10
|
||||
assert parameters["directional_zero_maximum_branch_gap_deg"] == 2.0
|
||||
assert parameters["directional_zero_maximum_branch_gap_range_deg"] == 0.3
|
||||
assert (
|
||||
parameters["cross_view_roll_maximum_branch_gap_difference_deg"]
|
||||
== 0.3
|
||||
)
|
||||
assert parameters["cross_view_roll_diagnostic_finger"] == ""
|
||||
assert parameters["repetitions"] == 3
|
||||
assert parameters["g20_right_19_repetitions"] >= 4
|
||||
assert parameters["validation_enabled"] is False
|
||||
assert parameters["combination_validation_enabled"] is False
|
||||
assert parameters["combination_validation_frames"] >= 10
|
||||
assert parameters["combination_maximum_position_p95_m"] <= 0.003
|
||||
assert parameters["combination_maximum_orientation_p95_deg"] <= 2.0
|
||||
assert parameters["minimum_detection_rate"] == 0.95
|
||||
assert parameters["minimum_detection_hz"] == 15.0
|
||||
assert parameters["minimum_state_span_u8"] >= 240.0
|
||||
assert parameters["endpoint_tolerance_u8"] == 2.0
|
||||
assert parameters["thumb_yaw_zero_endpoint_tolerance_u8"] == 4.0
|
||||
assert parameters["pinky_pip_zero_endpoint_tolerance_u8"] == 5.0
|
||||
assert parameters["minimum_sweep_bins"] >= 32
|
||||
assert parameters["maximum_bin_gap"] <= 16
|
||||
assert parameters["automatic_sweep_retry_limit"] == 2
|
||||
assert parameters["automatic_fit_retry_limit"] == 2
|
||||
assert parameters["motor_stall_timeout_seconds"] == 2.0
|
||||
assert parameters["motor_stall_startup_grace_seconds"] == 1.0
|
||||
assert parameters["motor_stall_minimum_progress_u8"] == 1.0
|
||||
assert parameters["automatic_motion_retry_limit"] == 2
|
||||
assert parameters["provisional_warning_ratio"] == 1.25
|
||||
assert parameters["retry_speed_scales"] == [0.8, 0.6]
|
||||
assert parameters["retry_endpoint_hold_seconds"] == [0.75, 1.0]
|
||||
assert parameters["position_timeout_seconds"] >= 20.0
|
||||
assert parameters["maximum_state_image_skew_ms"] <= 50.0
|
||||
assert parameters["axis_maximum_rotation_circle_difference_deg"] <= 1.0
|
||||
assert parameters["axis_maximum_pose_line_rms_m"] <= 0.001
|
||||
assert parameters["passive_axis_maximum_plane_rms_m"] == 0.004
|
||||
assert parameters["active_maximum_rotation_orthogonal_rms_deg"] == 2.5
|
||||
assert parameters["passive_maximum_rotation_orthogonal_rms_deg"] == 7.5
|
||||
assert parameters["zero_maximum_axis_cycle_difference_deg"] <= 0.75
|
||||
assert parameters["zero_maximum_axis_cone_mismatch_deg"] <= 5.0
|
||||
assert parameters["zero_maximum_observability_condition_number"] >= 1.0
|
||||
assert parameters["zero_maximum_offset_deg"] <= 20.0
|
||||
assert parameters["zero_finger_maximum_offset_deg"] <= 3.0
|
||||
assert parameters["mechanical_endpoint_maximum_offset_deg"] == 5.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["maximum_hysteresis_deg"] <= 2.0
|
||||
assert parameters["passive_maximum_hysteresis_deg"] <= 2.0
|
||||
assert parameters["command_maximum_direction_gap_deg"] <= 2.0
|
||||
assert parameters["maximum_validation_mae_deg"] <= 1.0
|
||||
assert parameters["maximum_validation_p95_deg"] <= 2.0
|
||||
assert parameters["maximum_validation_error_deg"] <= 3.0
|
||||
assert parameters["zero_maximum_confidence_half_width_deg"] <= 1.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 +292,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,326 @@
|
||||
"""Focused tests for checkerboard frame pairing."""
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
import g20_thumb_apriltag_calibration.extrinsics_node as extrinsics_node
|
||||
from g20_thumb_apriltag_calibration.extrinsics_node import (
|
||||
BoardPose,
|
||||
StereoCapture,
|
||||
_fit_stereo_robust,
|
||||
_individual_reprojection_passes,
|
||||
_minimum_history_skew_ns,
|
||||
_pose_is_novel,
|
||||
_select_latest_synchronised_pair,
|
||||
_summarize_transform_repeatability,
|
||||
)
|
||||
|
||||
|
||||
def _pose(stamp_ns: int) -> BoardPose:
|
||||
return BoardPose(
|
||||
stamp_ns=stamp_ns,
|
||||
camera_from_board_candidates=(),
|
||||
reprojection_rms_px=0.1,
|
||||
)
|
||||
|
||||
|
||||
def test_pairing_uses_recent_history_instead_of_mismatched_latest_frames():
|
||||
front = [_pose(1_000_000_000), _pose(1_200_000_000)]
|
||||
side = [_pose(1_030_000_000), _pose(1_370_000_000)]
|
||||
|
||||
pair = _select_latest_synchronised_pair(front, side, 100_000_000)
|
||||
|
||||
assert pair is not None
|
||||
selected_front, selected_side, skew = pair
|
||||
assert selected_front.stamp_ns == 1_000_000_000
|
||||
assert selected_side.stamp_ns == 1_030_000_000
|
||||
assert skew == 30_000_000
|
||||
|
||||
|
||||
def test_pairing_prefers_newest_valid_common_pair():
|
||||
front = [_pose(1_000_000_000), _pose(1_200_000_000)]
|
||||
side = [_pose(1_010_000_000), _pose(1_240_000_000)]
|
||||
|
||||
pair = _select_latest_synchronised_pair(front, side, 100_000_000)
|
||||
|
||||
assert pair is not None
|
||||
selected_front, selected_side, skew = pair
|
||||
assert selected_front.stamp_ns == 1_200_000_000
|
||||
assert selected_side.stamp_ns == 1_240_000_000
|
||||
assert skew == 40_000_000
|
||||
|
||||
|
||||
def test_pairing_reports_minimum_skew_when_no_pair_passes():
|
||||
front = [_pose(1_000_000_000), _pose(1_200_000_000)]
|
||||
side = [_pose(1_370_000_000)]
|
||||
|
||||
assert _select_latest_synchronised_pair(
|
||||
front, side, 100_000_000
|
||||
) is None
|
||||
assert _minimum_history_skew_ns(front, side) == 170_000_000
|
||||
|
||||
|
||||
def _transform(rotation_deg: float, translation_m: float) -> np.ndarray:
|
||||
value = np.eye(4)
|
||||
value[:3, :3] = Rotation.from_euler(
|
||||
"z", rotation_deg, degrees=True
|
||||
).as_matrix()
|
||||
value[0, 3] = translation_m
|
||||
return value
|
||||
|
||||
|
||||
def test_repeatability_summary_accepts_consistent_capture_set():
|
||||
captures = [
|
||||
[_transform(-0.1, -0.0005)],
|
||||
[_transform(0.0, 0.0)],
|
||||
[_transform(0.1, 0.0005)],
|
||||
]
|
||||
|
||||
_, selected, rotation_error, translation_error = (
|
||||
_summarize_transform_repeatability(captures)
|
||||
)
|
||||
|
||||
assert len(selected) == 3
|
||||
np.testing.assert_allclose(
|
||||
np.degrees(rotation_error), 0.1, atol=1.0e-6
|
||||
)
|
||||
np.testing.assert_allclose(translation_error, 0.0005, atol=1.0e-9)
|
||||
|
||||
|
||||
def test_repeatability_summary_exposes_current_capture_outlier():
|
||||
captures = [
|
||||
[_transform(0.0, 0.0)],
|
||||
[_transform(0.1, 0.0005)],
|
||||
[_transform(1.0, 0.008)],
|
||||
]
|
||||
|
||||
_, _, rotation_error, translation_error = (
|
||||
_summarize_transform_repeatability(captures)
|
||||
)
|
||||
|
||||
assert np.degrees(rotation_error) > 0.3
|
||||
assert translation_error > 0.0015
|
||||
|
||||
|
||||
def test_each_camera_must_pass_reprojection_gate_independently():
|
||||
assert _individual_reprojection_passes(0.8, 1.1, 1.2)
|
||||
assert not _individual_reprojection_passes(0.5, 1.4, 1.2)
|
||||
assert _individual_reprojection_passes(0.5, 1.4, 1.5)
|
||||
assert not _individual_reprojection_passes(0.5, 1.6, 1.5)
|
||||
assert not _individual_reprojection_passes(float("nan"), 0.5, 1.2)
|
||||
|
||||
|
||||
def test_pose_novelty_is_checked_against_all_previous_captures():
|
||||
previous = [_transform(0.0, 0.0), _transform(10.0, 0.10)]
|
||||
|
||||
assert not _pose_is_novel(
|
||||
_transform(10.5, 0.105),
|
||||
previous,
|
||||
minimum_rotation_rad=np.deg2rad(2.0),
|
||||
minimum_translation_m=0.015,
|
||||
)
|
||||
assert _pose_is_novel(
|
||||
_transform(14.0, 0.13),
|
||||
previous,
|
||||
minimum_rotation_rad=np.deg2rad(2.0),
|
||||
minimum_translation_m=0.015,
|
||||
)
|
||||
|
||||
|
||||
def test_joint_fit_trims_moderate_bad_views_without_relaxing_gate(
|
||||
monkeypatch,
|
||||
):
|
||||
captures = [
|
||||
StereoCapture(
|
||||
front_points_px=np.zeros((40, 2), dtype=np.float32),
|
||||
other_points_px=np.zeros((40, 2), dtype=np.float32),
|
||||
transform_candidates=(np.eye(4),),
|
||||
pair_reprojection_rms_px=1.0,
|
||||
skew_ns=1,
|
||||
)
|
||||
for _ in range(20)
|
||||
]
|
||||
|
||||
def fake_fit_once(
|
||||
captures_arg,
|
||||
indices,
|
||||
object_points,
|
||||
front_matrix,
|
||||
other_matrix,
|
||||
image_size,
|
||||
):
|
||||
del (
|
||||
captures_arg,
|
||||
object_points,
|
||||
front_matrix,
|
||||
other_matrix,
|
||||
image_size,
|
||||
)
|
||||
errors = np.asarray(
|
||||
[1.1 if index < 15 else 1.5 for index in indices],
|
||||
dtype=float,
|
||||
)
|
||||
return float(np.sqrt(np.mean(np.square(errors)))), np.eye(4), errors
|
||||
|
||||
monkeypatch.setattr(extrinsics_node, "_stereo_fit_once", fake_fit_once)
|
||||
result = _fit_stereo_robust(
|
||||
captures,
|
||||
np.zeros((40, 3), dtype=np.float32),
|
||||
np.eye(3),
|
||||
np.eye(3),
|
||||
(640, 480),
|
||||
minimum_inliers=15,
|
||||
maximum_rms_px=1.2,
|
||||
maximum_rotation_stability_rad=np.deg2rad(0.3),
|
||||
maximum_translation_stability_m=0.0015,
|
||||
)
|
||||
|
||||
assert result.passed
|
||||
assert result.stereo_rms_px <= 1.2
|
||||
assert len(result.inlier_indices) >= 15
|
||||
assert result.rejected_indices
|
||||
assert set(result.rejected_indices) <= set(range(15, 20))
|
||||
|
||||
|
||||
def test_joint_fit_reports_finite_provisional_metrics_before_minimum(
|
||||
monkeypatch,
|
||||
):
|
||||
captures = [
|
||||
StereoCapture(
|
||||
front_points_px=np.zeros((40, 2), dtype=np.float32),
|
||||
other_points_px=np.zeros((40, 2), dtype=np.float32),
|
||||
transform_candidates=(np.eye(4),),
|
||||
pair_reprojection_rms_px=0.8,
|
||||
skew_ns=1,
|
||||
)
|
||||
for _ in range(10)
|
||||
]
|
||||
|
||||
def fake_fit_once(
|
||||
captures_arg,
|
||||
indices,
|
||||
object_points,
|
||||
front_matrix,
|
||||
other_matrix,
|
||||
image_size,
|
||||
):
|
||||
del (
|
||||
captures_arg,
|
||||
object_points,
|
||||
front_matrix,
|
||||
other_matrix,
|
||||
image_size,
|
||||
)
|
||||
return 0.8, np.eye(4), np.full(len(indices), 0.8)
|
||||
|
||||
monkeypatch.setattr(extrinsics_node, "_stereo_fit_once", fake_fit_once)
|
||||
result = _fit_stereo_robust(
|
||||
captures,
|
||||
np.zeros((40, 3), dtype=np.float32),
|
||||
np.eye(3),
|
||||
np.eye(3),
|
||||
(640, 480),
|
||||
minimum_inliers=15,
|
||||
maximum_rms_px=1.2,
|
||||
maximum_rotation_stability_rad=np.deg2rad(0.3),
|
||||
maximum_translation_stability_m=0.0015,
|
||||
)
|
||||
|
||||
assert not result.passed
|
||||
assert result.stereo_rms_px == 0.8
|
||||
assert len(result.inlier_indices) == 10
|
||||
|
||||
|
||||
def test_joint_stereo_fit_recovers_transform_and_rejects_bad_view():
|
||||
random = np.random.default_rng(7)
|
||||
object_points = np.zeros((40, 3), dtype=np.float32)
|
||||
object_points[:, :2] = (
|
||||
np.mgrid[0:8, 0:5].T.reshape(-1, 2) * 0.027
|
||||
)
|
||||
matrix = np.asarray(
|
||||
[[1800.0, 0.0, 812.0], [0.0, 1795.0, 620.0], [0.0, 0.0, 1.0]]
|
||||
)
|
||||
other_from_front = np.eye(4)
|
||||
other_from_front[:3, :3] = Rotation.from_euler(
|
||||
"xyz", [2.0, 18.0, -1.0], degrees=True
|
||||
).as_matrix()
|
||||
other_from_front[:3, 3] = [0.20, -0.01, 0.04]
|
||||
expected_front_from_other = np.linalg.inv(other_from_front)
|
||||
captures = []
|
||||
for index in range(21):
|
||||
front_from_board = np.eye(4)
|
||||
front_from_board[:3, :3] = Rotation.from_euler(
|
||||
"xyz",
|
||||
[
|
||||
-8.0 + index * 0.7,
|
||||
5.0 + (index % 5) * 2.0,
|
||||
-5.0 + (index % 4) * 3.0,
|
||||
],
|
||||
degrees=True,
|
||||
).as_matrix()
|
||||
front_from_board[:3, 3] = [
|
||||
-0.08 + (index % 5) * 0.035,
|
||||
-0.04 + (index % 4) * 0.025,
|
||||
0.75 + (index % 3) * 0.08,
|
||||
]
|
||||
other_from_board = other_from_front @ front_from_board
|
||||
front_rvec = Rotation.from_matrix(
|
||||
front_from_board[:3, :3]
|
||||
).as_rotvec()
|
||||
other_rvec = Rotation.from_matrix(
|
||||
other_from_board[:3, :3]
|
||||
).as_rotvec()
|
||||
front_points, _ = cv2.projectPoints(
|
||||
object_points,
|
||||
front_rvec,
|
||||
front_from_board[:3, 3],
|
||||
matrix,
|
||||
np.zeros(5),
|
||||
)
|
||||
other_points, _ = cv2.projectPoints(
|
||||
object_points,
|
||||
other_rvec,
|
||||
other_from_board[:3, 3],
|
||||
matrix,
|
||||
np.zeros(5),
|
||||
)
|
||||
front_points = front_points.reshape(-1, 2)
|
||||
other_points = other_points.reshape(-1, 2)
|
||||
front_points += random.normal(0.0, 0.12, front_points.shape)
|
||||
other_points += random.normal(0.0, 0.12, other_points.shape)
|
||||
if index == 20:
|
||||
other_points += random.normal(0.0, 4.0, other_points.shape)
|
||||
captures.append(
|
||||
StereoCapture(
|
||||
front_points_px=front_points.astype(np.float32),
|
||||
other_points_px=other_points.astype(np.float32),
|
||||
transform_candidates=(expected_front_from_other.copy(),),
|
||||
pair_reprojection_rms_px=0.2,
|
||||
skew_ns=10_000_000,
|
||||
)
|
||||
)
|
||||
|
||||
result = _fit_stereo_robust(
|
||||
captures,
|
||||
object_points,
|
||||
matrix,
|
||||
matrix,
|
||||
(1624, 1240),
|
||||
minimum_inliers=15,
|
||||
maximum_rms_px=1.2,
|
||||
maximum_rotation_stability_rad=np.deg2rad(0.3),
|
||||
maximum_translation_stability_m=0.0015,
|
||||
)
|
||||
|
||||
assert result.passed
|
||||
assert 20 in result.rejected_indices
|
||||
assert len(result.inlier_indices) >= 15
|
||||
rotation_error = Rotation.from_matrix(
|
||||
result.front_from_other[:3, :3]
|
||||
).inv() * Rotation.from_matrix(expected_front_from_other[:3, :3])
|
||||
assert np.degrees(rotation_error.magnitude()) < 0.05
|
||||
assert np.linalg.norm(
|
||||
result.front_from_other[:3, 3]
|
||||
- expected_front_from_other[:3, 3]
|
||||
) < 0.001
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -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,69 @@
|
||||
import pytest
|
||||
|
||||
from g20_thumb_apriltag_calibration.offline_replay import (
|
||||
_latest_attempt_records,
|
||||
_latest_palm_axis_records,
|
||||
_output_suffix,
|
||||
)
|
||||
|
||||
|
||||
def _sample(joint: str, cycle: int, direction: str, attempt: int) -> dict:
|
||||
return {
|
||||
"kind": "sample",
|
||||
"joint": joint,
|
||||
"cycle": cycle,
|
||||
"direction": direction,
|
||||
"attempt": attempt,
|
||||
}
|
||||
|
||||
|
||||
def test_latest_attempt_is_selected_per_joint_cycle_and_direction() -> None:
|
||||
rows = [
|
||||
{"kind": "session_start"},
|
||||
_sample("pinky_pip", 0, "decreasing", 1),
|
||||
_sample("pinky_pip", 0, "decreasing", 3),
|
||||
_sample("pinky_pip", 0, "increasing", 1),
|
||||
_sample("pinky_pip", 1, "decreasing", 2),
|
||||
_sample("thumb_cmc_yaw", 0, "decreasing", 1),
|
||||
]
|
||||
|
||||
selected = _latest_attempt_records(rows)
|
||||
|
||||
assert [
|
||||
record["attempt"] for record in selected["pinky_pip"]
|
||||
] == [3, 1, 2]
|
||||
assert [
|
||||
record["attempt"] for record in selected["thumb_cmc_yaw"]
|
||||
] == [1]
|
||||
|
||||
|
||||
def test_latest_palm_axis_attempt_is_selected_per_cycle_and_direction() -> None:
|
||||
def sample(cycle: int, direction: str, attempt: int) -> dict:
|
||||
return {
|
||||
"kind": "palm_axis_sample",
|
||||
"source_joint": "pinky_mcp_pitch_front_axis",
|
||||
"cycle": cycle,
|
||||
"direction": direction,
|
||||
"attempt": attempt,
|
||||
}
|
||||
|
||||
selected = _latest_palm_axis_records(
|
||||
[
|
||||
sample(0, "decreasing", 1),
|
||||
sample(0, "decreasing", 2),
|
||||
sample(0, "increasing", 1),
|
||||
]
|
||||
)
|
||||
|
||||
assert [
|
||||
record["attempt"]
|
||||
for record in selected["pinky_mcp_pitch_front_axis"]
|
||||
] == [2, 1]
|
||||
|
||||
|
||||
def test_output_suffix_is_safe_and_explicit() -> None:
|
||||
assert _output_suffix(None) == ""
|
||||
assert _output_suffix("MEASURED_ZERO_V2") == "_MEASURED_ZERO_V2"
|
||||
for invalid in ("", "../escape", "/absolute", "contains space", "x" * 65):
|
||||
with pytest.raises(ValueError, match="output tag"):
|
||||
_output_suffix(invalid)
|
||||
@@ -234,6 +234,147 @@ def _pose(
|
||||
)
|
||||
|
||||
|
||||
def test_group_tracker_receives_oblique_reprojection_valid_candidates() -> None:
|
||||
per_tag = SquareTagPoseTracker(
|
||||
maximum_reprojection_error_px=1.5,
|
||||
reprojection_tie_px=1.5,
|
||||
maximum_pose_jump_rad=np.deg2rad(35.0),
|
||||
maximum_translation_jump_m=0.04,
|
||||
maximum_tag_tilt_rad=np.deg2rad(75.0),
|
||||
reset_after_seconds=5.0,
|
||||
)
|
||||
oblique_rotation = Rotation.from_euler("y", 89.0, degrees=True)
|
||||
|
||||
independent, reason = per_tag.estimate(
|
||||
"child",
|
||||
_project(oblique_rotation, np.asarray([0.03, 0.0, 0.25])),
|
||||
tag_size_m=0.01,
|
||||
camera_matrix=_camera_matrix(),
|
||||
stamp_ns=1_000_000_000,
|
||||
)
|
||||
|
||||
assert independent is None
|
||||
assert reason == "no_pose_within_reprojection_or_tilt_limit"
|
||||
assert per_tag.last_candidates_by_role["child"]
|
||||
diagnostics = per_tag.last_candidate_diagnostics_by_role["child"]
|
||||
assert diagnostics["reprojection_candidate_count"] == 2
|
||||
assert diagnostics["independent_tilt_candidate_count"] == 0
|
||||
assert diagnostics["minimum_candidate_tilt_deg"] > 75.0
|
||||
|
||||
group = SquareTagGroupPoseTracker(
|
||||
roles=("base", "child"),
|
||||
adjacent_pairs=(("base", "child"),),
|
||||
maximum_pose_jump_rad=np.deg2rad(35.0),
|
||||
maximum_translation_jump_m=0.04,
|
||||
relative_rotation_scale_rad=np.deg2rad(5.0),
|
||||
relative_translation_scale_m=0.01,
|
||||
reprojection_scale_px=0.1,
|
||||
reprojection_weight=0.05,
|
||||
reset_after_seconds=5.0,
|
||||
)
|
||||
selected, group_reason = group.select(
|
||||
{
|
||||
"base": (_pose(0.0, 0.0, 0.05),),
|
||||
"child": per_tag.last_candidates_by_role["child"],
|
||||
},
|
||||
stamp_ns=1_000_000_000,
|
||||
)
|
||||
|
||||
assert group_reason == ""
|
||||
assert selected is not None
|
||||
assert "child" in selected
|
||||
|
||||
|
||||
def test_group_tracker_exposes_roles_without_candidates() -> None:
|
||||
tracker = SquareTagGroupPoseTracker(
|
||||
roles=("base", "parent", "child"),
|
||||
adjacent_pairs=(("base", "parent"), ("parent", "child")),
|
||||
maximum_pose_jump_rad=np.deg2rad(35.0),
|
||||
maximum_translation_jump_m=0.04,
|
||||
relative_rotation_scale_rad=np.deg2rad(5.0),
|
||||
relative_translation_scale_m=0.01,
|
||||
reprojection_scale_px=0.1,
|
||||
reprojection_weight=0.05,
|
||||
reset_after_seconds=5.0,
|
||||
)
|
||||
|
||||
selected, reason = tracker.select(
|
||||
{
|
||||
"base": (_pose(0.0, 0.0, 0.05),),
|
||||
"parent": (),
|
||||
},
|
||||
stamp_ns=1_000_000_000,
|
||||
)
|
||||
|
||||
assert selected is None
|
||||
assert reason == "group_missing_pose_candidates"
|
||||
assert tracker.last_missing_roles == ("parent", "child")
|
||||
|
||||
|
||||
def test_group_tracker_uses_coupling_to_choose_branch_but_never_rejects_measurement() -> None:
|
||||
tracker = SquareTagGroupPoseTracker(
|
||||
roles=("base", "mcp", "ip"),
|
||||
adjacent_pairs=(("base", "mcp"), ("mcp", "ip")),
|
||||
maximum_pose_jump_rad=np.deg2rad(360.0),
|
||||
maximum_translation_jump_m=0.04,
|
||||
relative_rotation_scale_rad=np.deg2rad(1000.0),
|
||||
relative_translation_scale_m=1.0,
|
||||
reprojection_scale_px=0.1,
|
||||
reprojection_weight=1.0,
|
||||
reset_after_seconds=5.0,
|
||||
coupled_rotation_pairs=(
|
||||
("base", "mcp", "mcp", "ip", 1.03),
|
||||
),
|
||||
coupled_rotation_scale_rad=np.deg2rad(3.0),
|
||||
maximum_coupled_rotation_residual_rad=np.deg2rad(7.5),
|
||||
)
|
||||
baseline = {
|
||||
"base": (_pose(0.0, 0.00, 0.05),),
|
||||
"mcp": (_pose(0.0, 0.03, 0.05),),
|
||||
"ip": (_pose(0.0, 0.06, 0.05),),
|
||||
}
|
||||
selected, reason = tracker.select(
|
||||
baseline,
|
||||
stamp_ns=1_000_000_000,
|
||||
trajectory_command_u8=255,
|
||||
trajectory_direction="decreasing",
|
||||
)
|
||||
assert selected is not None
|
||||
assert reason == ""
|
||||
|
||||
driver = _pose(10.0, 0.03, 0.05)
|
||||
measured_ip = _pose(20.3, 0.06, 0.20)
|
||||
lower_reprojection_mirror = _pose(30.0, 0.06, 0.05)
|
||||
selected, reason = tracker.select(
|
||||
{
|
||||
"base": (_pose(0.0, 0.00, 0.05),),
|
||||
"mcp": (driver,),
|
||||
"ip": (lower_reprojection_mirror, measured_ip),
|
||||
},
|
||||
stamp_ns=1_033_000_000,
|
||||
trajectory_command_u8=128,
|
||||
trajectory_direction="decreasing",
|
||||
)
|
||||
|
||||
assert reason == ""
|
||||
assert selected is not None
|
||||
assert selected["ip"] == measured_ip
|
||||
|
||||
fallback, fallback_reason = tracker.select(
|
||||
{
|
||||
"base": (_pose(0.0, 0.00, 0.05),),
|
||||
"mcp": (driver,),
|
||||
"ip": (lower_reprojection_mirror,),
|
||||
},
|
||||
stamp_ns=1_066_000_000,
|
||||
trajectory_command_u8=128,
|
||||
trajectory_direction="decreasing",
|
||||
)
|
||||
assert fallback is not None
|
||||
assert fallback_reason == ""
|
||||
assert fallback["ip"] == lower_reprojection_mirror
|
||||
|
||||
|
||||
def test_group_tracker_prevents_incompatible_t4_t5_branch_switch() -> None:
|
||||
tracker = SquareTagGroupPoseTracker(
|
||||
roles=("t0", "t3", "t4", "t5"),
|
||||
@@ -324,6 +465,234 @@ def test_group_tracker_keeps_same_pair_across_sweep_turnaround() -> None:
|
||||
assert selected == {"t4": return_t4, "t5": return_t5}
|
||||
|
||||
|
||||
def test_group_tracker_uses_outbound_pose_at_same_command_on_return() -> None:
|
||||
tracker = SquareTagGroupPoseTracker(
|
||||
roles=("parent", "child"),
|
||||
adjacent_pairs=(("parent", "child"),),
|
||||
maximum_pose_jump_rad=np.deg2rad(35.0),
|
||||
maximum_translation_jump_m=0.04,
|
||||
relative_rotation_scale_rad=np.deg2rad(5.0),
|
||||
relative_translation_scale_m=0.01,
|
||||
reprojection_scale_px=0.1,
|
||||
reprojection_weight=0.05,
|
||||
reset_after_seconds=5.0,
|
||||
)
|
||||
parent = _pose(0.0, 0.00, 0.05)
|
||||
for stamp, command, angle in (
|
||||
(1_000_000_000, 255, 20.0),
|
||||
(1_033_000_000, 64, 45.0),
|
||||
(1_066_000_000, 0, 50.0),
|
||||
):
|
||||
selected, reason = tracker.select(
|
||||
{"parent": (parent,), "child": (_pose(angle, 0.03, 0.05),)},
|
||||
stamp_ns=stamp,
|
||||
trajectory_command_u8=command,
|
||||
trajectory_direction="decreasing",
|
||||
)
|
||||
assert reason == ""
|
||||
assert selected is not None
|
||||
|
||||
# The true return contains 3 deg of real hysteresis along the learned
|
||||
# y-axis. The lower-error mirror candidate is temporally smoother but
|
||||
# adds a 2 deg tilt outside that physical motion axis.
|
||||
true_return = _pose(42.0, 0.03, 0.20)
|
||||
smoother_mirror = SquareTagPose(
|
||||
quaternion_xyzw=tuple(
|
||||
Rotation.from_euler("xy", [2.0, 49.0], degrees=True).as_quat()
|
||||
),
|
||||
translation_xyz_m=(0.03, 0.0, 0.25),
|
||||
reprojection_error_px=0.01,
|
||||
)
|
||||
selected, reason = tracker.select(
|
||||
{
|
||||
"parent": (parent,),
|
||||
"child": (smoother_mirror, true_return),
|
||||
},
|
||||
stamp_ns=1_099_000_000,
|
||||
trajectory_command_u8=64,
|
||||
trajectory_direction="increasing",
|
||||
)
|
||||
|
||||
assert reason == ""
|
||||
assert selected == {"parent": parent, "child": true_return}
|
||||
|
||||
|
||||
def test_group_tracker_initializes_from_multiple_static_frames() -> None:
|
||||
tracker = SquareTagGroupPoseTracker(
|
||||
roles=("parent", "child"),
|
||||
adjacent_pairs=(("parent", "child"),),
|
||||
maximum_pose_jump_rad=np.deg2rad(35.0),
|
||||
maximum_translation_jump_m=0.04,
|
||||
relative_rotation_scale_rad=np.deg2rad(5.0),
|
||||
relative_translation_scale_m=0.01,
|
||||
reprojection_scale_px=0.1,
|
||||
reprojection_weight=0.05,
|
||||
reset_after_seconds=5.0,
|
||||
initialization_frames=8,
|
||||
)
|
||||
parent = _pose(0.0, 0.00, 0.05)
|
||||
true_child = _pose(20.0, 0.03, 0.10)
|
||||
|
||||
for index in range(7):
|
||||
false_child = _pose(5.0 * index, 0.03, 0.05)
|
||||
selected, reason = tracker.select(
|
||||
{
|
||||
"parent": (parent,),
|
||||
"child": (false_child, true_child),
|
||||
},
|
||||
stamp_ns=1_000_000_000 + index * 33_000_000,
|
||||
)
|
||||
assert selected is None
|
||||
assert reason == f"group_initializing:{index + 1}/8"
|
||||
|
||||
false_child = _pose(35.0, 0.03, 0.05)
|
||||
selected, reason = tracker.select(
|
||||
{
|
||||
"parent": (parent,),
|
||||
"child": (false_child, true_child),
|
||||
},
|
||||
stamp_ns=1_231_000_000,
|
||||
)
|
||||
|
||||
assert reason == ""
|
||||
assert selected == {"parent": parent, "child": true_child}
|
||||
assert (
|
||||
tracker.last_initialization_quality["initialization_search"]
|
||||
== "static_reference"
|
||||
)
|
||||
assert tracker.last_initialization_quality[
|
||||
"p95_pair_rotation_drift_rad"
|
||||
] < np.deg2rad(1.0)
|
||||
|
||||
|
||||
def test_group_tracker_preserves_task_branch_anchor_across_cycles() -> None:
|
||||
tracker = SquareTagGroupPoseTracker(
|
||||
roles=("parent", "child"),
|
||||
adjacent_pairs=(("parent", "child"),),
|
||||
maximum_pose_jump_rad=np.deg2rad(35.0),
|
||||
maximum_translation_jump_m=0.04,
|
||||
relative_rotation_scale_rad=np.deg2rad(5.0),
|
||||
relative_translation_scale_m=0.01,
|
||||
reprojection_scale_px=0.1,
|
||||
reprojection_weight=0.05,
|
||||
reset_after_seconds=5.0,
|
||||
initialization_frames=8,
|
||||
)
|
||||
parent = _pose(0.0, 0.00, 0.05)
|
||||
anchored_child = _pose(20.0, 0.03, 0.20)
|
||||
for index in range(8):
|
||||
selected, reason = tracker.select(
|
||||
{"parent": (parent,), "child": (anchored_child,)},
|
||||
stamp_ns=1_000_000_000 + index * 33_000_000,
|
||||
trajectory_command_u8=255,
|
||||
trajectory_direction="decreasing",
|
||||
)
|
||||
assert reason == ""
|
||||
assert selected == {"parent": parent, "child": anchored_child}
|
||||
|
||||
tracker.reset(preserve_task_reference=True)
|
||||
lower_error_mirror = _pose(5.0, 0.03, 0.01)
|
||||
for index in range(8):
|
||||
selected, reason = tracker.select(
|
||||
{
|
||||
"parent": (parent,),
|
||||
"child": (lower_error_mirror, anchored_child),
|
||||
},
|
||||
stamp_ns=2_000_000_000 + index * 33_000_000,
|
||||
trajectory_command_u8=255,
|
||||
trajectory_direction="decreasing",
|
||||
)
|
||||
|
||||
assert reason == ""
|
||||
assert selected == {"parent": parent, "child": anchored_child}
|
||||
assert tracker.last_initialization_quality["task_reference_used"] == "true"
|
||||
|
||||
tracker.reset()
|
||||
for index in range(8):
|
||||
selected, reason = tracker.select(
|
||||
{
|
||||
"parent": (parent,),
|
||||
"child": (lower_error_mirror, anchored_child),
|
||||
},
|
||||
stamp_ns=3_000_000_000 + index * 33_000_000,
|
||||
trajectory_command_u8=255,
|
||||
trajectory_direction="decreasing",
|
||||
)
|
||||
assert reason == ""
|
||||
assert selected == {"parent": parent, "child": lower_error_mirror}
|
||||
|
||||
|
||||
def test_static_group_normal_prior_rejects_stable_ippe_mirror() -> None:
|
||||
tracker = SquareTagGroupPoseTracker(
|
||||
roles=("mcp", "pip", "dip"),
|
||||
adjacent_pairs=(("mcp", "pip"), ("pip", "dip")),
|
||||
normal_alignment_pairs=(("mcp", "pip"), ("pip", "dip")),
|
||||
normal_alignment_scale_rad=np.deg2rad(5.0),
|
||||
maximum_pose_jump_rad=np.deg2rad(35.0),
|
||||
maximum_translation_jump_m=0.04,
|
||||
relative_rotation_scale_rad=np.deg2rad(5.0),
|
||||
relative_translation_scale_m=0.01,
|
||||
reprojection_scale_px=0.1,
|
||||
reprojection_weight=0.05,
|
||||
reset_after_seconds=5.0,
|
||||
initialization_frames=8,
|
||||
maximum_normal_alignment_rad=np.deg2rad(15.0),
|
||||
)
|
||||
mcp = _pose(12.0, 0.00, 0.05)
|
||||
correct_pip = _pose(14.0, 0.03, 0.10)
|
||||
mirror_pip = _pose(50.0, 0.03, 0.05)
|
||||
dip = _pose(15.0, 0.06, 0.05)
|
||||
|
||||
for index in range(8):
|
||||
selected, reason = tracker.select(
|
||||
{
|
||||
"mcp": (mcp,),
|
||||
"pip": (mirror_pip, correct_pip),
|
||||
"dip": (dip,),
|
||||
},
|
||||
stamp_ns=1_000_000_000 + index * 33_000_000,
|
||||
)
|
||||
|
||||
assert reason == ""
|
||||
assert selected == {
|
||||
"mcp": mcp,
|
||||
"pip": correct_pip,
|
||||
"dip": dip,
|
||||
}
|
||||
assert tracker.last_initialization_quality[
|
||||
"maximum_normal_alignment_rad"
|
||||
] < np.deg2rad(3.0)
|
||||
|
||||
|
||||
def test_static_group_rejects_when_no_aligned_branch_exists() -> None:
|
||||
tracker = SquareTagGroupPoseTracker(
|
||||
roles=("base", "moving"),
|
||||
adjacent_pairs=(("base", "moving"),),
|
||||
normal_alignment_pairs=(("base", "moving"),),
|
||||
normal_alignment_scale_rad=np.deg2rad(5.0),
|
||||
maximum_normal_alignment_rad=np.deg2rad(15.0),
|
||||
maximum_pose_jump_rad=np.deg2rad(35.0),
|
||||
maximum_translation_jump_m=0.04,
|
||||
relative_rotation_scale_rad=np.deg2rad(5.0),
|
||||
relative_translation_scale_m=0.01,
|
||||
reprojection_scale_px=0.1,
|
||||
reprojection_weight=0.05,
|
||||
reset_after_seconds=5.0,
|
||||
initialization_frames=8,
|
||||
)
|
||||
base = _pose(0.0, 0.00, 0.05)
|
||||
mirror = _pose(38.0, 0.03, 0.05)
|
||||
|
||||
for index in range(8):
|
||||
selected, reason = tracker.select(
|
||||
{"base": (base,), "moving": (mirror,)},
|
||||
stamp_ns=1_000_000_000 + index * 33_000_000,
|
||||
)
|
||||
|
||||
assert selected is None
|
||||
assert reason == "group_normal_alignment"
|
||||
|
||||
|
||||
def test_whole_trajectory_recovers_rigid_group_from_mirror_drift() -> None:
|
||||
roles = ("t3", "t4", "t5")
|
||||
mount_rotations = {
|
||||
|
||||
@@ -0,0 +1,69 @@
|
||||
"""Tests for the explicit calibration sample command domains."""
|
||||
|
||||
from g20_thumb_apriltag_calibration.sample_schema import (
|
||||
SampleDataContractError,
|
||||
canonical_sample_record,
|
||||
fitting_sample_record,
|
||||
fitting_sample_records,
|
||||
)
|
||||
|
||||
import pytest
|
||||
|
||||
|
||||
def _sample(kind: str = 'sample') -> dict[str, object]:
|
||||
return {
|
||||
'kind': kind,
|
||||
'joint': 'thumb_cmc_roll',
|
||||
'cycle': 0,
|
||||
'direction': 'decreasing',
|
||||
'requested_command_u8': 0,
|
||||
'feedback_u8': 17.6,
|
||||
}
|
||||
|
||||
|
||||
def test_online_and_serialized_replay_use_identical_fit_projection() -> None:
|
||||
"""Serialization must not change the curve fitter's input record."""
|
||||
durable = canonical_sample_record(_sample())
|
||||
online = fitting_sample_record(durable)
|
||||
serialized = canonical_sample_record(online)
|
||||
replay = fitting_sample_record(serialized)
|
||||
|
||||
assert 'command_u8' not in durable
|
||||
assert serialized == durable
|
||||
assert replay == online
|
||||
assert online['command_u8'] == 18
|
||||
|
||||
|
||||
def test_default_domain_is_requested_only_for_steady_checkpoints() -> None:
|
||||
"""Settled checkpoints and dense trajectories use declared domains."""
|
||||
steady = _sample('steady_command_sample')
|
||||
|
||||
assert fitting_sample_record(steady)['command_u8'] == 0
|
||||
assert fitting_sample_record(steady, domain='feedback')['command_u8'] == 18
|
||||
|
||||
|
||||
def test_feedback_projection_can_snap_proven_requested_endpoints() -> None:
|
||||
"""A settled endpoint remains exact after feedback-domain projection."""
|
||||
records = fitting_sample_records(
|
||||
[_sample()],
|
||||
domain='feedback',
|
||||
snap_requested_endpoints=True,
|
||||
)
|
||||
|
||||
assert records[0]['command_u8'] == 0
|
||||
assert records[0]['feedback_u8'] == pytest.approx(17.6)
|
||||
|
||||
|
||||
def test_new_sample_rejects_missing_explicit_feedback_domain() -> None:
|
||||
"""A new observation cannot rely on the compatibility command key."""
|
||||
ambiguous = {
|
||||
'kind': 'sample',
|
||||
'requested_command_u8': 0,
|
||||
'command_u8': 0,
|
||||
}
|
||||
|
||||
with pytest.raises(
|
||||
SampleDataContractError,
|
||||
match='DATA-CONTRACT-701.*feedback_u8',
|
||||
):
|
||||
canonical_sample_record(ambiguous)
|
||||
@@ -0,0 +1,697 @@
|
||||
import json
|
||||
|
||||
import numpy as np
|
||||
|
||||
from g20_thumb_apriltag_calibration.three_camera_diagnostics import (
|
||||
_task_text,
|
||||
render_three_camera_status_text_zh,
|
||||
three_camera_reason_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个方向，扫描进度14.3%" 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_endpoint_zero_lifecycle_failure_is_not_reported_as_quality() -> None:
|
||||
payload = {
|
||||
"state": "PAUSED",
|
||||
"reason": (
|
||||
"validated_endpoint_zero_state_incomplete:"
|
||||
"missing=index_pip;extra=-"
|
||||
),
|
||||
"active": {},
|
||||
"progress": 1.0,
|
||||
"completed_sweeps": 16,
|
||||
"total_sweeps": 16,
|
||||
"views": {},
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert "程序内部状态生命周期错误" in text
|
||||
assert "不要移动相机、Tag或机械手底座" in text
|
||||
|
||||
|
||||
def test_artifact_failure_explains_that_collection_need_not_restart() -> None:
|
||||
payload = {
|
||||
"state": "PAUSED",
|
||||
"reason": (
|
||||
"PUB-ARTIFACT-601:runtime curve exceeds runtime URDF limit "
|
||||
"for index_mcp_pitch"
|
||||
),
|
||||
"active": {},
|
||||
"progress": 1.0,
|
||||
"completed_sweeps": 16,
|
||||
"total_sweeps": 16,
|
||||
"views": {},
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert "正式发布前" in text
|
||||
assert "原始URDF未被覆盖" in text
|
||||
assert "不要重新标定相机或调整Tag" in text
|
||||
|
||||
|
||||
def test_combination_prediction_failure_is_not_reported_as_unclassified() -> None:
|
||||
explanation, action = three_camera_reason_zh(
|
||||
"PAUSED", "combination_pose_prediction_failed", {}
|
||||
)
|
||||
|
||||
assert "多关节组合姿态" in explanation
|
||||
assert "未分类原因码" not in explanation
|
||||
assert "combination_validation_failure" in action
|
||||
assert "不要重新采集16个单关节任务" in action
|
||||
|
||||
|
||||
def test_sweep_start_visible_tags_reports_group_pnp_rejection() -> None:
|
||||
explanation, action = three_camera_reason_zh(
|
||||
"PAUSED",
|
||||
"sweep_start_tag_timeout",
|
||||
{
|
||||
"group_pnp_reasons": {
|
||||
"side": "group_initializing:5/8",
|
||||
},
|
||||
"pnp_initialization_progress": {
|
||||
"side": {"accepted": 5, "required": 8},
|
||||
},
|
||||
"pnp_rejection_counts": {
|
||||
"side": {
|
||||
"ring_pip:no_pose_within_reprojection_or_tilt_limit": 12,
|
||||
},
|
||||
},
|
||||
},
|
||||
)
|
||||
|
||||
assert "所需Tag也可见" in explanation
|
||||
assert "初始化5/8" in explanation
|
||||
assert "no_pose_within_reprojection_or_tilt_limit×12" in explanation
|
||||
assert "不要根据可见性重复粘贴" in action
|
||||
|
||||
|
||||
def test_mid_sweep_pnp_failure_names_missing_candidate_role() -> None:
|
||||
explanation, action = three_camera_reason_zh(
|
||||
"PAUSED",
|
||||
"synchronised_tag_state_timeout:side",
|
||||
{
|
||||
"valid_frames": 150,
|
||||
"group_pnp_reasons": {
|
||||
"side": "group_missing_pose_candidates",
|
||||
},
|
||||
"group_missing_candidate_roles": {
|
||||
"side": ["index_dip"],
|
||||
},
|
||||
"pnp_rejection_counts": {
|
||||
"side": {
|
||||
"index_dip:no_pose_within_reprojection_or_tilt_limit": 90,
|
||||
},
|
||||
},
|
||||
"pnp_candidate_diagnostics": {
|
||||
"side": {
|
||||
"index_dip": {
|
||||
"solved_candidate_count": 2,
|
||||
"reprojection_candidate_count": 0,
|
||||
"independent_tilt_candidate_count": 0,
|
||||
},
|
||||
},
|
||||
},
|
||||
},
|
||||
)
|
||||
|
||||
assert "已经取得部分有效轨迹" in explanation
|
||||
assert "缺候选=index_dip" in explanation
|
||||
assert "solve=2,reproj=0,tilt=0" in explanation
|
||||
assert "group_pnp_candidate_event" in action
|
||||
|
||||
|
||||
def test_multiview_failure_names_the_camera_specific_joint() -> None:
|
||||
explanation, action = three_camera_reason_zh(
|
||||
"PAUSED",
|
||||
"sweep_bin_gap_too_large:pinky_mcp_roll_side",
|
||||
{
|
||||
"sample": {
|
||||
"maximum_bin_gap": 20,
|
||||
"maximum_bin_gap_start_u8": 100,
|
||||
"maximum_bin_gap_end_u8": 120,
|
||||
"allowed_maximum_bin_gap": 16,
|
||||
}
|
||||
},
|
||||
)
|
||||
|
||||
assert "小指MCP侧摆（侧面校验）" in explanation
|
||||
assert "100→120" in explanation
|
||||
assert "resume" in action
|
||||
|
||||
|
||||
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_palm_orientation_failure_explains_thumb_top_coverage() -> None:
|
||||
explanation, suggestion = three_camera_reason_zh(
|
||||
"PAUSED",
|
||||
"palm_orientation_quality_failed",
|
||||
{
|
||||
"failures": [
|
||||
{
|
||||
"reason": (
|
||||
"palm orientation cycle 2 has 1/2 usable sources: "
|
||||
"thumb_cmc_roll_top_axis:cycle2=too few samples"
|
||||
)
|
||||
}
|
||||
]
|
||||
},
|
||||
)
|
||||
|
||||
assert "顶部Tag 8/9" in explanation
|
||||
assert "1/2 usable sources" in explanation
|
||||
assert "CMC pitch和roll" in suggestion
|
||||
|
||||
|
||||
def test_cross_view_side_line_rms_names_side_source() -> None:
|
||||
payload = {
|
||||
"state": "PAUSED",
|
||||
"reason": "joint_fit_check_failed",
|
||||
"progress": 0.1,
|
||||
"completed_sweeps": 8,
|
||||
"total_sweeps": 80,
|
||||
"active": {
|
||||
"kind": "fit_failure",
|
||||
"view": "front",
|
||||
"motor_index": 9,
|
||||
"joints": ["pinky_mcp_roll", "pinky_mcp_roll_side"],
|
||||
"attempt": 1,
|
||||
"directions_to_rescan": 8,
|
||||
"failures": [
|
||||
{
|
||||
"joint": "pinky_mcp_roll_side",
|
||||
"model_joint": "pinky_mcp_roll",
|
||||
"quality_source_joints": ["pinky_mcp_roll_side"],
|
||||
"metric": "axis_line_cycle_rms_mm",
|
||||
"actual": 1.2,
|
||||
"limit": 1.0,
|
||||
"comparison": "maximum",
|
||||
}
|
||||
],
|
||||
},
|
||||
"views": {},
|
||||
"result_path": "",
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert "小指MCP侧摆（侧面校验）的四轮轴线位置RMS为1.20mm" in text
|
||||
assert "要求不超过1.00mm" in text
|
||||
|
||||
|
||||
def test_baseline_hysteresis_failure_shows_values_instead_of_unknown() -> None:
|
||||
payload = {
|
||||
"state": "PAUSED",
|
||||
"reason": "joint_fit_check_failed",
|
||||
"progress": 0.15,
|
||||
"completed_sweeps": 24,
|
||||
"total_sweeps": 160,
|
||||
"active": {
|
||||
"kind": "fit_failure",
|
||||
"view": "front",
|
||||
"motor_index": 15,
|
||||
"joints": ["thumb_ip"],
|
||||
"attempt": 3,
|
||||
"directions_to_rescan": 6,
|
||||
"failures": [
|
||||
{
|
||||
"joint": "thumb_ip",
|
||||
"metric": "baseline_hysteresis_deg",
|
||||
"actual": 1.34,
|
||||
"limit": 0.5,
|
||||
"comparison": "maximum",
|
||||
"cycle_values_deg": [1.34, 0.04, 0.14],
|
||||
}
|
||||
],
|
||||
},
|
||||
"views": {},
|
||||
"result_path": "",
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert "baseline正反程关节角差为1.34°" in text
|
||||
assert "要求不超过0.50°" in text
|
||||
assert "各轮=1.34°/0.04°/0.14°" in text
|
||||
assert "未知原因" not in text
|
||||
|
||||
|
||||
def test_thumb_yaw_repeatability_failure_names_real_source_retry() -> None:
|
||||
explanation, suggestion = three_camera_reason_zh(
|
||||
"PAUSED",
|
||||
"joint_fit_check_failed",
|
||||
{
|
||||
"source_task_names": [
|
||||
"thumb_cmc_pitch_front",
|
||||
"thumb_cmc_roll_front",
|
||||
],
|
||||
"directions_to_rescan": 16,
|
||||
"failures": [
|
||||
{
|
||||
"joint": "thumb_cmc_yaw",
|
||||
"metric": "zero_cycle_offset_range_deg",
|
||||
"actual": 0.555836,
|
||||
"limit": 0.5,
|
||||
"comparison": "maximum",
|
||||
"cycle": 3,
|
||||
"cycle_offset_deg": [-2.650924, -2.615277, -2.095088],
|
||||
}
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
assert "训练轮零位极差为0.56°" in explanation
|
||||
assert "各轮=-2.65°/-2.62°/-2.10°" in explanation
|
||||
assert "未知原因" not in explanation
|
||||
assert "CMC pitch/roll顶部轴观测" in suggestion
|
||||
assert "重扫16个方向" in suggestion
|
||||
assert "不会无效重扫yaw侧摆" in suggestion
|
||||
|
||||
|
||||
def test_sweep_gap_status_names_gap_bounds_and_full_detection_rate() -> None:
|
||||
payload = {
|
||||
"state": "PAUSED",
|
||||
"reason": "sweep_bin_gap_too_large",
|
||||
"progress": 0.1,
|
||||
"completed_sweeps": 1,
|
||||
"total_sweeps": 10,
|
||||
"active": {
|
||||
"kind": "sweep",
|
||||
"view": "top",
|
||||
"motor_index": 10,
|
||||
"joints": ["thumb_cmc_yaw"],
|
||||
"cycle": 1,
|
||||
"repetitions": 3,
|
||||
"start_u8": 255,
|
||||
"target_u8": 0,
|
||||
"actual_u8": 3.0,
|
||||
"valid_frames": 610,
|
||||
"detection_frames": 644,
|
||||
"detection_valid_frames": 610,
|
||||
"detection_rate": 610 / 644,
|
||||
"sample": {
|
||||
"minimum_u8": 3.0,
|
||||
"maximum_u8": 252.0,
|
||||
"maximum_bin_gap": 17,
|
||||
"maximum_bin_gap_start_u8": 99,
|
||||
"maximum_bin_gap_end_u8": 116,
|
||||
"allowed_maximum_bin_gap": 16,
|
||||
},
|
||||
},
|
||||
"views": {},
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert "最大空缺为17，（99→116）" in text
|
||||
assert "本方向Tag检出：94.7%（610/644帧）" in text
|
||||
|
||||
|
||||
def test_zero_model_failure_explains_that_rescan_will_not_help() -> None:
|
||||
payload = {
|
||||
"state": "PAUSED",
|
||||
"reason": "zero_model_validation_failed",
|
||||
"progress": 0.92,
|
||||
"scan_progress": 1.0,
|
||||
"completed_sweeps": 42,
|
||||
"total_sweeps": 42,
|
||||
"active": {
|
||||
"kind": "zero_model_failure",
|
||||
"view": "front",
|
||||
"motor_index": 15,
|
||||
"joints": ["thumb_mcp", "thumb_ip"],
|
||||
"directions_to_rescan": 0,
|
||||
"failures": [
|
||||
{
|
||||
"joint": "thumb_mcp",
|
||||
"metric": "zero_guard",
|
||||
"reason": "zero_offset_exceeds_configured_limit",
|
||||
"actual_deg": -39.81,
|
||||
"limit_deg": 20.0,
|
||||
}
|
||||
],
|
||||
},
|
||||
"views": {},
|
||||
"result_path": "",
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert "拇指MCP：零位估计超过安全范围" in text
|
||||
assert "估计-39.81°，允许±20.00°" in text
|
||||
assert "不会自动重扫" in text
|
||||
assert "总体进度：92.0%" in text
|
||||
assert "扫描进度100.0%" in text
|
||||
assert "运动采样：" not in text
|
||||
|
||||
|
||||
def test_sweep_status_shows_localized_fit_retry_cycle() -> None:
|
||||
active = {
|
||||
"kind": "sweep",
|
||||
"view": "front",
|
||||
"motor_index": 15,
|
||||
"joints": ["thumb_mcp", "thumb_ip"],
|
||||
"cycle": 1,
|
||||
"repetitions": 3,
|
||||
"start_u8": 255,
|
||||
"target_u8": 0,
|
||||
"fit_attempt": 2,
|
||||
"fit_attempt_limit": 3,
|
||||
"fit_retry_cycles": [2],
|
||||
}
|
||||
|
||||
assert "补采异常轮2" in _task_text(active)
|
||||
|
||||
|
||||
def test_motor_stall_reason_is_explained_in_chinese() -> None:
|
||||
reason, action = three_camera_reason_zh(
|
||||
"PAUSED",
|
||||
"motor_state_stalled:sweep_motor_19:error_u8=5.000",
|
||||
{},
|
||||
)
|
||||
|
||||
assert "距目标5.000个u8" in reason
|
||||
assert "机械端点" in action
|
||||
assert "不要反复调用resume" in action
|
||||
|
||||
|
||||
def test_baseline_stall_names_motor_target_actual_and_tolerance() -> None:
|
||||
payload = {
|
||||
"state": "PAUSED",
|
||||
"reason": (
|
||||
"motor_state_stalled:return_baseline:motor_index=10:"
|
||||
"target_u8=255.0:actual_u8=250.0:tolerance_u8=4.0:"
|
||||
"timeout_seconds=2.000:"
|
||||
"error_u8=5.000"
|
||||
),
|
||||
"progress": 0.0,
|
||||
"scan_progress": 0.0,
|
||||
"completed_sweeps": 0,
|
||||
"total_sweeps": 42,
|
||||
"active": {
|
||||
"kind": "motion_stall",
|
||||
"stage": "return_baseline",
|
||||
"motor_index": 10,
|
||||
"target_u8": 255.0,
|
||||
"actual_u8": 250.0,
|
||||
"error_u8": 5.0,
|
||||
"tolerance_u8": 4.0,
|
||||
"timeout_seconds": 2.0,
|
||||
},
|
||||
"views": {},
|
||||
"result_path": "",
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert "电机10反馈连续2秒" in text
|
||||
assert "目标255.0、实际250.0、误差5.000 u8" in text
|
||||
assert "允许容差±4.0 u8" in text
|
||||
assert "当前任务：电机10运动停滞，目标255.0、实际250.0" 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_return_recovery_prints_thumb_yaw_start_and_clearance_pose() -> None:
|
||||
baseline = [255] * 20
|
||||
baseline[6:10] = [127, 127, 127, 127]
|
||||
recovery = list(baseline)
|
||||
recovery[5] = 145
|
||||
recovery[10] = 0
|
||||
payload = {
|
||||
"state": "RETURN_BASELINE",
|
||||
"reason": "return_baseline_before_retry_sweep",
|
||||
"progress": 0.5,
|
||||
"completed_sweeps": 36,
|
||||
"total_sweeps": 42,
|
||||
"baseline_command_u8": baseline,
|
||||
"return_command_u8": recovery,
|
||||
"active": {},
|
||||
"views": {},
|
||||
"result_path": "",
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert f"正在确认恢复姿态：{recovery}" 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_urdf_zero_bound_has_specific_chinese_scale_guidance() -> None:
|
||||
payload = {
|
||||
"state": "PAUSED",
|
||||
"reason": (
|
||||
"URDF zero offset reached the configured 20.000 degree bound: "
|
||||
"thumb_cmc_roll=+20.000deg, index_pip=-20.000deg; "
|
||||
"all_offsets: thumb_cmc_roll=+20.000deg"
|
||||
),
|
||||
"progress": 1.0,
|
||||
"completed_sweeps": 42,
|
||||
"total_sweeps": 42,
|
||||
"active": {},
|
||||
"views": {},
|
||||
"result_path": "",
|
||||
}
|
||||
|
||||
text = render_three_camera_status_text_zh(payload)
|
||||
|
||||
assert "触及±20.000°安全边界" in text
|
||||
assert "拇指CMC滚转=+20.000deg" in text
|
||||
assert "食指PIP=-20.000deg" in text
|
||||
assert "不要调用resume" in text
|
||||
assert "Tag有效黑框边长" in text
|
||||
assert "未分类原因码" not in text
|
||||
|
||||
|
||||
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,
|
||||
"automatic_retry_count": 1,
|
||||
"automatic_retry_limit": 2,
|
||||
"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
|
||||
assert "自动重试：当前方向已自动重扫1/2次" in text
|
||||
|
||||
|
||||
def test_same_finger_transition_explains_that_clearance_stays_parked() -> None:
|
||||
explanation, action = three_camera_reason_zh(
|
||||
"RETURN_BASELINE",
|
||||
"holding_same_finger_clearance_before_next_task",
|
||||
{},
|
||||
)
|
||||
|
||||
assert "继续保持当前避让姿态" in explanation
|
||||
assert "只调整被测关节" in explanation
|
||||
assert "不要手动展开" in action
|
||||
|
||||
|
||||
def test_fixed_base_reference_movement_requires_a_new_session() -> None:
|
||||
explanation, action = three_camera_reason_zh(
|
||||
"PAUSED",
|
||||
"fixed_base_reference_moved",
|
||||
{},
|
||||
)
|
||||
|
||||
assert "Tag 8" in explanation
|
||||
assert "基准锁定后" in explanation
|
||||
assert "下一次标定预检前重新摆放" in action
|
||||
assert "重新启动新会话" in action
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -75,10 +75,10 @@ _HAND_CONFIGS: Dict[str, HandConfig] = {
|
||||
"点赞": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
|
||||
"握拳": [96, 0, 0, 0, 0, 0, 193, 158, 128, 91, 132, 255, 255, 255, 255, 144, 0, 0, 0, 0],
|
||||
"张开": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
|
||||
"OK": [148, 110, 255, 255, 255, 44, 164, 100, 114, 127, 178, 255, 255, 255, 255, 94, 71, 255, 255, 255],
|
||||
"拇指对中指": [191, 255, 55, 255, 255, 96, 95, 100, 114, 127, 105, 255, 255, 255, 255, 94, 255, 108, 255, 255],
|
||||
"拇指对无名指": [191, 255, 255, 72, 255, 115, 95, 100, 114, 127, 60, 255, 255, 255, 255, 94, 255, 255, 97, 255],
|
||||
"拇指对小指": [191, 255, 255, 255, 55, 0, 95, 100, 114, 121, 70, 255, 255, 255, 255, 94, 255, 255, 255, 100],
|
||||
"OK": [0, 0, 255, 255, 255, 138, 147, 148, 105, 42, 109, 255, 255, 255, 255, 255, 211, 255, 255, 255],
|
||||
"拇指对中指": [0, 255, 0, 255, 255, 107, 149, 148, 105, 42, 109, 255, 255, 255, 255, 255, 225, 202, 255, 255],
|
||||
"拇指对无名指": [0, 255, 255, 0, 255, 88, 171, 148, 105, 42, 59, 255, 255, 255, 255, 255, 255, 255, 206, 254],
|
||||
"拇指对小指": [0, 255, 255, 255, 0, 32, 170, 148, 105, 42, 109, 255, 255, 255, 255, 255, 255, 255, 255, 203],
|
||||
"准备1": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
|
||||
"壹": [96, 255, 0, 0, 0, 0, 190, 161, 127, 80, 68, 255, 255, 255, 255, 144, 255, 0, 0, 0],
|
||||
"贰": [96, 255, 255, 0, 0, 0, 190, 66, 127, 80, 68, 255, 255, 255, 255, 144, 255, 255, 0, 0],
|
||||
|
||||
@@ -1,31 +0,0 @@
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
Node(
|
||||
package='linker_hand_ros2_sdk',
|
||||
executable='linker_hand_sdk',
|
||||
name='linker_hand_sdk_left',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'hand_type': 'left',
|
||||
'hand_joint': "L10", # 这里需要修改为实际Linker Hand的型号 L7、L10、L20、L21、L25
|
||||
'is_touch': True, # 是否带有压力传感器
|
||||
'can': 'can0', # 这里需要修改为实际的CAN总线名称
|
||||
}],
|
||||
),
|
||||
|
||||
Node(
|
||||
package='linker_hand_ros2_sdk',
|
||||
executable='linker_hand_sdk',
|
||||
name='linker_hand_sdk_right',
|
||||
output='screen',
|
||||
parameters=[{
|
||||
'hand_type': 'right',
|
||||
'hand_joint': "L10", # 这里需要修改为实际Linker Hand的型号 L7、L10、L20、L21、L25
|
||||
'is_touch': True, # 是否带有压力传感器
|
||||
'can': 'can0', # 这里需要修改为实际的CAN总线名称
|
||||
}],
|
||||
),
|
||||
])
|
||||
-444
@@ -1,444 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
from pymodbus.client import ModbusSerialClient
|
||||
from typing import List, Dict
|
||||
import numpy as np
|
||||
|
||||
_INTERVAL = 0.006 # 8 ms
|
||||
|
||||
class LinkerHandL6RS485:
|
||||
"""L6机械手 Modbus-RTU 控制类"""
|
||||
|
||||
# 6个关节名称
|
||||
JOINT_NAMES = ["thumb_pitch", "thumb_yaw", "index_pitch",
|
||||
"middle_pitch", "ring_pitch", "little_pitch"]
|
||||
|
||||
# 手指名称
|
||||
FINGER_NAMES = ["thumb", "index", "middle", "ring", "little"]
|
||||
|
||||
def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200):
|
||||
"""
|
||||
初始化L6机械手
|
||||
hand_id: 右手0x27(39), 左手0x28(40)
|
||||
modbus_port: 串口设备路径
|
||||
baudrate: 波特率，固定115200
|
||||
"""
|
||||
self.slave = hand_id
|
||||
self.cli = ModbusSerialClient(
|
||||
port=modbus_port,
|
||||
baudrate=baudrate,
|
||||
bytesize=8,
|
||||
parity="N",
|
||||
stopbits=1,
|
||||
timeout=0.05
|
||||
)
|
||||
# pymodbus 3.5.1 需要显式连接
|
||||
self.connected = self.cli.connect()
|
||||
if not self.connected:
|
||||
raise ConnectionError(f"RS485连接失败，端口: {modbus_port}")
|
||||
|
||||
def _read_input_registers(self, address: int, count: int) -> List[int]:
|
||||
"""读取输入寄存器"""
|
||||
time.sleep(_INTERVAL)
|
||||
result = self.cli.read_input_registers(address=address, count=count, slave=self.slave)
|
||||
if result.isError():
|
||||
raise RuntimeError(f"读取输入寄存器失败: address={address}, count={count}")
|
||||
return result.registers
|
||||
|
||||
def _write_register(self, address: int, value: int):
|
||||
"""写入单个寄存器"""
|
||||
time.sleep(_INTERVAL)
|
||||
result = self.cli.write_register(address=address, value=value, slave=self.slave)
|
||||
if result.isError():
|
||||
raise RuntimeError(f"写入寄存器失败: address={address}, value={value}")
|
||||
|
||||
def _write_registers(self, address: int, values: List[int]):
|
||||
"""写入多个寄存器"""
|
||||
time.sleep(_INTERVAL)
|
||||
result = self.cli.write_registers(address=address, values=values, slave=self.slave)
|
||||
if result.isError():
|
||||
raise RuntimeError(f"写入多个寄存器失败: address={address}, values={values}")
|
||||
|
||||
# --------------------------------------------------
|
||||
# 基础读取接口
|
||||
# --------------------------------------------------
|
||||
|
||||
def read_angles(self) -> List[int]:
|
||||
"""读取6个关节角度 (输入寄存器 0-5)"""
|
||||
return self._read_input_registers(0, 6)
|
||||
|
||||
def read_torques(self) -> List[int]:
|
||||
"""读取6个关节转矩 (输入寄存器 6-11)"""
|
||||
return self._read_input_registers(6, 6)
|
||||
|
||||
def read_speeds(self) -> List[int]:
|
||||
"""读取6个关节速度 (输入寄存器 12-17)"""
|
||||
return self._read_input_registers(12, 6)
|
||||
|
||||
def read_temperatures(self) -> List[int]:
|
||||
"""读取6个关节温度 (输入寄存器 18-23)"""
|
||||
return self._read_input_registers(18, 6)
|
||||
|
||||
def read_error_codes(self) -> List[int]:
|
||||
"""读取6个关节错误码 (输入寄存器 24-29)"""
|
||||
return self._read_input_registers(24, 6)
|
||||
|
||||
# --------------------------------------------------
|
||||
# 压力传感器接口
|
||||
# --------------------------------------------------
|
||||
|
||||
# def _pressure(self, finger: int) -> List[int]:
|
||||
# """内部：选手指 → 读压力数据"""
|
||||
# # 选择手指 (保持寄存器 36)
|
||||
# self._write_register(36, finger)
|
||||
# time.sleep(_INTERVAL)
|
||||
# # 读取压力数据 (输入寄存器 52-122)
|
||||
# return np.array(self._read_input_registers(52, 71))
|
||||
def _pressure(self, finger: int) -> np.ndarray:
|
||||
"""
|
||||
6x12 (72点) 矩阵尺寸。
|
||||
Modbus 地址 60/62。
|
||||
"""
|
||||
rows = 12 # 12 行
|
||||
cols = 6 # 6 列
|
||||
finger_size = rows * cols # 72 个数据点
|
||||
|
||||
# modbus 地址和计数
|
||||
write_address = 60 # 写入手指选择
|
||||
read_address = 62 # 读取压力数据
|
||||
read_count = 96 # 读取 96 个寄存器
|
||||
skip_count = 10 # 跳过前 10 个校验点
|
||||
|
||||
# 0. 参数校验和手指写入值确定
|
||||
if finger < 1 or finger > 5:
|
||||
raise ValueError(f"无效的手指编号: {finger}。手指编号应在 1 到 5 之间。")
|
||||
|
||||
finger_write_value = finger
|
||||
|
||||
# 1. 写入手指选择寄存器 (地址 60)
|
||||
time.sleep(0.008)
|
||||
wrsp = self.cli.write_register(address=write_address, value=finger_write_value, slave=self.slave)
|
||||
if wrsp.isError():
|
||||
raise RuntimeError(f"写入手指选择 {finger} 到地址 {write_address} 失败: {wrsp}")
|
||||
|
||||
# 写入后等待片刻
|
||||
time.sleep(0.008)
|
||||
|
||||
# 2. 读取地址 62 的数据
|
||||
rrsp = self.cli.read_input_registers(address=read_address, count=read_count, slave=self.slave)
|
||||
|
||||
if rrsp.isError():
|
||||
raise RuntimeError(f"读取地址 {read_address} 压力数据失败: {rrsp}")
|
||||
|
||||
registers_16bit: List[int] = rrsp.registers
|
||||
|
||||
# 3. 核心数据处理
|
||||
# a. 提取低 8 位数据 (得到 96 个 8 位数据点)
|
||||
final_data_96 = [reg_value & 255 for reg_value in registers_16bit]
|
||||
|
||||
# b. 跳过前 10 个校验/头部数据点 (得到 86 个有效数据点)
|
||||
effective_data = np.array(final_data_96[skip_count:], dtype=np.uint8)
|
||||
# c. 截取当前手指的矩阵数据 (从 86 个有效点中截取 72 个点)
|
||||
start_idx = 0
|
||||
end_idx = finger_size # 72
|
||||
|
||||
finger_data_flat = effective_data[start_idx:end_idx]
|
||||
|
||||
# d. 验证数据长度
|
||||
if finger_data_flat.size != finger_size:
|
||||
raise ValueError(
|
||||
f"数据提取失败。期望 {finger_size} 点 ({rows}x{cols})，"
|
||||
f"但仅截取到 {finger_data_flat.size} 点。请检查协议，确认地址 62 是否一次性返回了所有手指数据。"
|
||||
)
|
||||
|
||||
# e. 重塑为二维矩阵 (12 行 6 列)
|
||||
finger_matrix = finger_data_flat.reshape((rows, cols))
|
||||
|
||||
return finger_matrix
|
||||
|
||||
def read_pressure_thumb(self) -> np.ndarray:
|
||||
"""读取大拇指压力数据"""
|
||||
return np.array(self._pressure(1), dtype=np.uint8)
|
||||
|
||||
def read_pressure_index(self) -> np.ndarray:
|
||||
"""读取食指压力数据"""
|
||||
return np.array(self._pressure(2), dtype=np.uint8)
|
||||
|
||||
def read_pressure_middle(self) -> np.ndarray:
|
||||
"""读取中指压力数据"""
|
||||
return np.array(self._pressure(3), dtype=np.uint8)
|
||||
|
||||
def read_pressure_ring(self) -> np.ndarray:
|
||||
"""读取无名指压力数据"""
|
||||
return np.array(self._pressure(4), dtype=np.uint8)
|
||||
|
||||
def read_pressure_little(self) -> np.ndarray:
|
||||
"""读取小拇指压力数据"""
|
||||
return np.array(self._pressure(5), dtype=np.uint8)
|
||||
|
||||
# --------------------------------------------------
|
||||
# 版本信息接口
|
||||
# --------------------------------------------------
|
||||
|
||||
def read_versions(self) -> Dict[str, int]:
|
||||
"""读取版本信息 (输入寄存器 148-155)"""
|
||||
result = self._read_input_registers(148, 8)
|
||||
|
||||
return {
|
||||
"hand_freedom": result[0],
|
||||
"hand_version": result[1],
|
||||
"hand_number": result[2],
|
||||
"hand_direction": result[3],
|
||||
"software_version_major": result[4],
|
||||
"software_version_minor": result[5] if len(result) > 5 else 0,
|
||||
"software_version_revision": result[6] if len(result) > 6 else 0,
|
||||
"hardware_version": result[7] if len(result) > 7 else 0
|
||||
}
|
||||
|
||||
# --------------------------------------------------
|
||||
# 写入接口
|
||||
# --------------------------------------------------
|
||||
|
||||
def write_angles(self, vals: List[int]):
|
||||
"""设置6个关节角度 (保持寄存器 0-5)"""
|
||||
vals = [int(x) for x in vals]
|
||||
if not self.is_valid_6xuint8(vals):
|
||||
raise ValueError("需要6个0-255的整数")
|
||||
self._write_registers(0, vals)
|
||||
|
||||
def write_torques(self, vals: List[int]):
|
||||
"""设置6个关节转矩 (保持寄存器 6-11)"""
|
||||
vals = [int(x) for x in vals]
|
||||
if not self.is_valid_6xuint8(vals):
|
||||
raise ValueError("需要6个0-255的整数")
|
||||
self._write_registers(6, vals)
|
||||
|
||||
def write_speeds(self, vals: List[int]):
|
||||
"""设置6个关节速度 (保持寄存器 12-17)"""
|
||||
vals = [int(x) for x in vals]
|
||||
if not self.is_valid_6xuint8(vals):
|
||||
raise ValueError("需要6个0-255的整数")
|
||||
self._write_registers(12, vals)
|
||||
|
||||
# --------------------------------------------------
|
||||
# 上下文管理
|
||||
# --------------------------------------------------
|
||||
|
||||
def close(self):
|
||||
"""关闭连接"""
|
||||
if self.connected:
|
||||
self.cli.close()
|
||||
self.connected = False
|
||||
|
||||
def __enter__(self):
|
||||
return self
|
||||
|
||||
def __exit__(self, exc_type, exc_val, exc_tb):
|
||||
self.close()
|
||||
|
||||
# --------------------------------------------------
|
||||
# API固定接口函数
|
||||
# --------------------------------------------------
|
||||
|
||||
def is_valid_6xuint8(self, lst) -> bool:
|
||||
"""验证6个0-255的整数列表"""
|
||||
if len(lst) != 6:
|
||||
return False
|
||||
return all(isinstance(x, int) and 0 <= x <= 255 for x in lst)
|
||||
|
||||
def set_joint_positions(self, joint_angles=None):
|
||||
"""设置关节位置"""
|
||||
joint_angles = joint_angles or [0] * 6
|
||||
self.write_angles(joint_angles)
|
||||
|
||||
def set_speed(self, speed=None):
|
||||
"""设置速度"""
|
||||
speed = speed or [200] * 6
|
||||
self.write_speeds(speed)
|
||||
|
||||
def set_torque(self, torque=None):
|
||||
"""设置扭矩"""
|
||||
torque = torque or [200] * 6
|
||||
self.write_torques(torque)
|
||||
|
||||
def set_current(self, current=None):
|
||||
"""设置电流 (L6不支持)"""
|
||||
print("当前L6不支持设置电流", flush=True)
|
||||
|
||||
def get_version(self) -> list:
|
||||
"""获取版本信息"""
|
||||
versions = self.read_versions()
|
||||
return [
|
||||
versions.get("hand_freedom", 0),
|
||||
versions.get("hand_version", 0),
|
||||
versions.get("hand_number", 0),
|
||||
versions.get("hand_direction", 0),
|
||||
versions.get("software_version_major", 0),
|
||||
versions.get("hardware_version", 0)
|
||||
]
|
||||
|
||||
def get_current(self):
|
||||
"""获取电流 (L6不支持)"""
|
||||
print("当前L6不支持获取电流", flush=True)
|
||||
return []
|
||||
|
||||
def get_state(self) -> list:
|
||||
"""获取关节状态"""
|
||||
return self.read_angles()
|
||||
|
||||
def get_state_for_pub(self) -> list:
|
||||
return self.get_state()
|
||||
|
||||
def get_current_status(self) -> list:
|
||||
return self.get_state()
|
||||
|
||||
def get_speed(self) -> list:
|
||||
"""获取当前速度"""
|
||||
return self.read_speeds()
|
||||
|
||||
def get_joint_speed(self) -> list:
|
||||
return self.get_speed()
|
||||
|
||||
def get_touch_type(self) -> int:
|
||||
"""获取压感类型 (2=矩阵式)"""
|
||||
return 2
|
||||
|
||||
def get_normal_force(self) -> list:
|
||||
"""获取压感数据：点式"""
|
||||
return [-1] * 5
|
||||
|
||||
def get_tangential_force(self) -> list:
|
||||
"""获取压感数据：点式"""
|
||||
return [-1] * 5
|
||||
|
||||
def get_approach_inc(self) -> list:
|
||||
"""获取压感数据：点式"""
|
||||
return [-1] * 5
|
||||
|
||||
def get_touch(self) -> list:
|
||||
return [-1] * 5
|
||||
|
||||
def get_thumb_matrix_touch(self,sleep_time=0):
|
||||
return self._pressure(1)
|
||||
|
||||
def get_index_matrix_touch(self,sleep_time=0):
|
||||
return self._pressure(2)
|
||||
|
||||
def get_middle_matrix_touch(self,sleep_time=0):
|
||||
return self._pressure(3)
|
||||
|
||||
def get_ring_matrix_touch(self,sleep_time=0):
|
||||
return self._pressure(4)
|
||||
|
||||
def get_little_matrix_touch(self,sleep_time=0):
|
||||
return self._pressure(5)
|
||||
|
||||
def get_matrix_touch(self) -> list:
|
||||
"""获取压感数据：矩阵式"""
|
||||
return [self._pressure(1), self._pressure(2), self._pressure(3),
|
||||
self._pressure(4), self._pressure(5)]
|
||||
|
||||
def get_matrix_touch_v2(self) -> list:
|
||||
"""获取压感数据：矩阵式"""
|
||||
return self.get_matrix_touch()
|
||||
|
||||
def get_torque(self) -> list:
|
||||
"""获取当前扭矩"""
|
||||
return self.read_torques()
|
||||
|
||||
def get_temperature(self) -> list:
|
||||
"""获取当前电机温度"""
|
||||
return self.read_temperatures()
|
||||
|
||||
def get_fault(self) -> list:
|
||||
"""获取当前电机故障码"""
|
||||
return self.read_error_codes()
|
||||
|
||||
def get_serial_number(self):
|
||||
return [0] * 6
|
||||
|
||||
# --------------------------------------------------
|
||||
# 便捷方法
|
||||
# --------------------------------------------------
|
||||
|
||||
def relax(self):
|
||||
"""所有手指伸直"""
|
||||
self.set_joint_positions([255] * 6)
|
||||
|
||||
def fist(self):
|
||||
"""所有手指握拳"""
|
||||
self.set_joint_positions([0] * 6)
|
||||
|
||||
def dump_status(self):
|
||||
"""打印状态信息"""
|
||||
print("=" * 50)
|
||||
print("L6机械手状态信息")
|
||||
print("=" * 50)
|
||||
|
||||
try:
|
||||
# 关节状态
|
||||
angles = self.read_angles()
|
||||
torques = self.read_torques()
|
||||
speeds = self.read_speeds()
|
||||
temps = self.read_temperatures()
|
||||
errors = self.read_error_codes()
|
||||
|
||||
print("关节状态:")
|
||||
for i, name in enumerate(self.JOINT_NAMES):
|
||||
print(f" {name:15s}: 角度={angles[i]:3d}, 扭矩={torques[i]:3d}, "
|
||||
f"速度={speeds[i]:3d}, 温度={temps[i]:2d}℃, 错误={errors[i]:2d}")
|
||||
|
||||
# 版本信息
|
||||
versions = self.read_versions()
|
||||
print("\n版本信息:")
|
||||
for key, value in versions.items():
|
||||
print(f" {key:20s}: {value}")
|
||||
|
||||
# 压力传感器测试
|
||||
print("\n压力传感器测试:")
|
||||
thumb_pressure = self.read_pressure_thumb()
|
||||
print(f"大拇指压力数据长度: {len(thumb_pressure)}")
|
||||
|
||||
except Exception as e:
|
||||
print(f"读取状态时出错: {e}")
|
||||
|
||||
print("=" * 50)
|
||||
|
||||
|
||||
# ------------------- 演示程序 -------------------
|
||||
if __name__ == "__main__":
|
||||
# 使用示例
|
||||
try:
|
||||
with LinkerHandL6RS485(hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200) as hand:
|
||||
print("连接成功!")
|
||||
|
||||
# 打印状态信息
|
||||
hand.dump_status()
|
||||
|
||||
# 测试基本控制
|
||||
print("\n测试控制功能...")
|
||||
print("伸直手指...")
|
||||
hand.relax()
|
||||
time.sleep(2)
|
||||
|
||||
print("握拳...")
|
||||
hand.fist()
|
||||
time.sleep(2)
|
||||
|
||||
print("恢复伸直...")
|
||||
hand.relax()
|
||||
|
||||
# 测试压力传感器
|
||||
print("\n测试压力传感器...")
|
||||
thumb_matrix = hand.get_thumb_matrix_touch()
|
||||
print(f"大拇指压力数据: {len(thumb_matrix)}个点")
|
||||
|
||||
# 获取所有手指压力数据
|
||||
all_matrices = hand.get_matrix_touch()
|
||||
for i, name in enumerate(hand.FINGER_NAMES):
|
||||
matrix = all_matrices[i]
|
||||
print(f"{name}手指压力数据长度: {len(matrix)}")
|
||||
|
||||
except Exception as e:
|
||||
print(f"错误: {e}")
|
||||
-1157
File diff suppressed because it is too large
Load Diff
@@ -1,414 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
# -*- coding: utf-8 -*-
|
||||
'''
|
||||
编译: colcon build --symlink-install
|
||||
启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
|
||||
'''
|
||||
from re import A
|
||||
import rclpy,sys # ROS2 Python接口库
|
||||
import time
|
||||
import numpy as np
|
||||
from rclpy.node import Node # ROS2 节点类
|
||||
from rclpy.clock import Clock
|
||||
from std_msgs.msg import String, Header, Float32MultiArray
|
||||
from sensor_msgs.msg import JointState, PointCloud2, PointField
|
||||
import time, json, threading
|
||||
from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
|
||||
from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
|
||||
from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
|
||||
|
||||
|
||||
class LinkerHand(Node):
|
||||
def __init__(self, name):
|
||||
super().__init__(name)
|
||||
# 声明参数（带默认值）
|
||||
self.declare_parameter('hand_type', 'left')
|
||||
self.declare_parameter('hand_joint', 'L6')
|
||||
self.declare_parameter('is_touch', False)
|
||||
self.declare_parameter('can', 'can0')
|
||||
self.declare_parameter('modbus', "None")
|
||||
|
||||
# ros时间获取
|
||||
self.stamp_clock = Clock()
|
||||
# 获取参数值
|
||||
self.hand_type = self.get_parameter('hand_type').value
|
||||
self.hand_joint = self.get_parameter('hand_joint').value
|
||||
self.is_touch = self.get_parameter('is_touch').value
|
||||
self.can = self.get_parameter('can').value
|
||||
self.modbus = self.get_parameter('modbus').value
|
||||
self.sdk_v = 2
|
||||
self.sleep_time = 0.005
|
||||
self.cmd_lock = False
|
||||
self.last_hand_post_cmd = None # 最新手指位置命令
|
||||
self.last_hand_vel_cmd = None # 最新手指速度命令
|
||||
self.last_hand_eff_cmd = None # 最新手指力矩命令
|
||||
|
||||
self.last_hand_state = [-1] * 10
|
||||
self.last_hand_vel = [-1] * 10
|
||||
self.force = [[-1] * 5] * 4
|
||||
self.matrix_dic = {
|
||||
"stamp":{
|
||||
"sec": 0,
|
||||
"nanosec": 0,
|
||||
},
|
||||
"thumb_matrix":[[-1] * 6 for _ in range(12)],
|
||||
"index_matrix":[[-1] * 6 for _ in range(12)],
|
||||
"middle_matrix":[[-1] * 6 for _ in range(12)],
|
||||
"ring_matrix":[[-1] * 6 for _ in range(12)],
|
||||
"little_matrix":[[-1] * 6 for _ in range(12)]
|
||||
}
|
||||
# 压感矩阵合值，单位g 克
|
||||
self.matrix_mass_dic = {
|
||||
"stamp":{
|
||||
"secs": 0,
|
||||
"nsecs": 0,
|
||||
},
|
||||
"thumb_mass":[-1],
|
||||
"index_mass":[-1],
|
||||
"middle_mass":[-1],
|
||||
"ring_mass":[-1],
|
||||
"little_mass":[-1]
|
||||
}
|
||||
self.last_hand_info = {
|
||||
"version": [-1], # Dexterous hand version number
|
||||
"hand_joint": self.hand_joint, # Dexterous hand joint type
|
||||
"speed": [-1] * 10, # Current speed threshold of the dexterous hand
|
||||
"current": [-1] * 10, # Current of the dexterous hand
|
||||
"fault": [-1] * 10, # Current fault of the dexterous hand
|
||||
"motor_temperature": [-1] * 10, # Current motor temperature of the dexterous hand
|
||||
"torque": [-1] * 10, # Current torque of the dexterous hand
|
||||
"is_touch":self.is_touch,
|
||||
"touch_type": -1,
|
||||
"finger_order": None # Finger motor order
|
||||
}
|
||||
self.version = []
|
||||
self.touch_type = -1
|
||||
self.hz = 1.0/60.0
|
||||
|
||||
self.hand_setting_sub = self.create_subscription(String,'/cb_hand_setting_cmd', self.hand_setting_cb, 10)
|
||||
self._init_hand()
|
||||
time.sleep(1)
|
||||
self.run_count = 0 # 计数器，用于记录运行次数
|
||||
self.timer = self.create_timer(0.01, self.run) # 100 Hz
|
||||
self.thread_pub_state = threading.Thread(target=self.pub_state)
|
||||
self.thread_pub_state.daemon = True
|
||||
self.thread_pub_state.start()
|
||||
|
||||
def _init_hand(self):
|
||||
self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
|
||||
time.sleep(0.1)
|
||||
self.touch_type = self.api.get_touch_type()
|
||||
self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
|
||||
self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
|
||||
self.hand_info_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_info', 10)
|
||||
if self.is_touch == True:
|
||||
if self.touch_type > 1:
|
||||
ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
|
||||
self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
|
||||
self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
|
||||
self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
|
||||
elif self.touch_type != -1:
|
||||
ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
|
||||
self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
|
||||
else:
|
||||
ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
|
||||
self.is_touch = False
|
||||
self.embedded_version = self.api.get_embedded_version()
|
||||
pose = None
|
||||
torque = [200, 200, 200, 200, 200]
|
||||
speed = [200, 250, 250, 250, 250]
|
||||
if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6" or self.hand_joint.upper() == "L6P":
|
||||
pose = [200, 255, 255, 255, 255, 180]
|
||||
torque = [250, 250, 250, 250, 250, 250]
|
||||
# O6 最大速度阈值
|
||||
speed = [200, 250, 250, 250, 250, 250]
|
||||
elif self.hand_joint == "L7":
|
||||
# The data length of L7 is 7, reinitialize here
|
||||
pose = [255, 200, 255, 255, 255, 255, 180]
|
||||
torque = [250, 250, 250, 250, 250, 250, 250]
|
||||
speed = [120, 250, 250, 250, 250, 250, 250]
|
||||
elif self.hand_joint == "L10":
|
||||
torque = [255] * 10
|
||||
pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41]
|
||||
speed = [200, 250, 250, 250, 250, 250, 250, 250, 250, 250]
|
||||
elif self.hand_joint == "L20":
|
||||
pose = [255,255,255,255,255,255,10,100,180,240,245,255,255,255,255,255,255,255,255,255]
|
||||
elif self.hand_joint == "L21":
|
||||
pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||
elif self.hand_joint == "L25":
|
||||
pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
|
||||
if pose is not None:
|
||||
for i in range(1):
|
||||
self.api.set_speed(speed=speed)
|
||||
time.sleep(0.1)
|
||||
self.api.set_torque(torque=torque)
|
||||
time.sleep(0.1)
|
||||
self.api.finger_move(pose=pose)
|
||||
time.sleep(0.1)
|
||||
|
||||
def list_check(self,pose):
|
||||
if isinstance(pose, list) == False:
|
||||
return False
|
||||
if len(self.last_hand_post_cmd) != len(pose):
|
||||
return False
|
||||
return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
|
||||
|
||||
def hand_control_cb(self, msg):
|
||||
if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
|
||||
self.last_hand_post_cmd = msg.position
|
||||
if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
|
||||
self.last_hand_vel_cmd = msg.velocity
|
||||
if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
|
||||
self.last_hand_eff_cmd = msg.effort
|
||||
|
||||
def run(self):
|
||||
if self.sdk_v == 1:
|
||||
self.sleep_time = 0.009
|
||||
if self.hand_state_pub.get_subscription_count() > 0:
|
||||
# 优先获取手指状态并且发布
|
||||
self.last_hand_state = self.api.get_state()
|
||||
time.sleep(0.003)
|
||||
self.last_hand_vel = self.api.get_joint_speed()
|
||||
time.sleep(0.002)
|
||||
if self.cmd_lock == False:
|
||||
if self.last_hand_post_cmd != None:
|
||||
self.api.finger_move(pose=self.last_hand_post_cmd)
|
||||
self.last_hand_post_cmd = None
|
||||
if self.last_hand_vel_cmd != None:
|
||||
vel = list(self.last_hand_vel_cmd)
|
||||
if all(x == 0 for x in vel):
|
||||
pass
|
||||
else:
|
||||
if (str(self.hand_joint).upper() == "O6" or str(self.hand_joint).upper() == "L6" or str(self.hand_joint).upper() == "L6P") and len(vel) == 6:
|
||||
speed = vel
|
||||
self.api.set_joint_speed(speed=speed)
|
||||
elif self.hand_joint == "L7" and len(vel) == 7:
|
||||
speed = vel
|
||||
self.api.set_joint_speed(speed=speed)
|
||||
elif self.hand_joint == "L10" and len(vel) == 10:
|
||||
speed = [vel[0],vel[2],vel[3],vel[4],vel[5]]
|
||||
self.api.set_joint_speed(speed=speed)
|
||||
elif self.hand_joint == "L20" and len(vel) == 20:
|
||||
speed = [vel[10],vel[1],vel[2],vel[3],vel[4]]
|
||||
self.api.set_joint_speed(speed=speed)
|
||||
elif self.hand_joint == "L21" and len(vel) == 25:
|
||||
speed = vel
|
||||
self.api.set_joint_speed(speed=speed)
|
||||
elif self.hand_joint == "L25" and len(vel) == 25:
|
||||
speed = vel
|
||||
self.api.set_joint_speed(speed=speed)
|
||||
self.last_hand_vel_cmd = None
|
||||
time.sleep(0.003)
|
||||
if self.run_count == 3 and self.is_touch == True and self.touch_type == 1 and self.touch_pub.get_subscription_count() > 0:
|
||||
"""单点式压力传感器"""
|
||||
self.force = self.api.get_force()
|
||||
if self.is_touch == True and self.touch_type > 1 and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0):
|
||||
"""矩阵式压力传感器"""
|
||||
if self.run_count == 3:
|
||||
self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=self.sleep_time).tolist()
|
||||
if self.run_count == 4:
|
||||
self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=self.sleep_time).tolist()
|
||||
if self.run_count == 5:
|
||||
self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=self.sleep_time).tolist()
|
||||
if self.run_count == 6:
|
||||
self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=self.sleep_time).tolist()
|
||||
if self.run_count == 7:
|
||||
self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=self.sleep_time).tolist()
|
||||
time.sleep(0.005)
|
||||
if self.run_count == 8 and self.hand_info_pub.get_subscription_count() > 0:
|
||||
"""手部信息"""
|
||||
self.last_hand_info = {
|
||||
"version": self.embedded_version, # Dexterous hand version number
|
||||
"hand_joint": self.hand_joint, # Dexterous hand joint type
|
||||
"speed": self.api.get_speed(), # Current speed threshold of the dexterous hand
|
||||
"current": self.api.get_current(), # Current of the dexterous hand
|
||||
"fault": self.api.get_fault(), # Current fault of the dexterous hand
|
||||
"motor_temperature": self.api.get_temperature(), # Current motor temperature of the dexterous hand
|
||||
"torque": self.api.get_torque(), # Current torque of the dexterous hand
|
||||
"is_touch":self.is_touch,
|
||||
"touch_type": self.touch_type,
|
||||
"finger_order": self.api.get_finger_order() # Finger motor order
|
||||
}
|
||||
if self.run_count == 9:
|
||||
self.run_count = 0
|
||||
self.run_count += 1
|
||||
time.sleep(0.003)
|
||||
|
||||
|
||||
def pub_state(self):
|
||||
while True:
|
||||
if self.hand_state_pub.get_subscription_count() > 0:
|
||||
msg = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
|
||||
self.hand_state_pub.publish(msg)
|
||||
if self.is_touch == True and self.touch_type == 1 and self.touch_pub.get_subscription_count() > 0:
|
||||
msg = Float32MultiArray()
|
||||
msg.data = [float(val) for sublist in self.force for val in sublist]
|
||||
self.touch_pub.publish(msg)
|
||||
if self.is_touch == True and self.touch_type > 1 and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0):
|
||||
# 发布矩阵压感数据JSON格式
|
||||
self.pub_matrix_dic()
|
||||
# 发布矩阵压感和值JSON格式
|
||||
self.pub_matrix_mass(dic=self.matrix_dic)
|
||||
# 发布矩阵压感点云格式
|
||||
self.pub_matrix_point_cloud()
|
||||
if self.hand_info_pub.get_subscription_count() > 0:
|
||||
msg = String()
|
||||
msg.data = json.dumps(self.last_hand_info)
|
||||
self.hand_info_pub.publish(msg)
|
||||
time.sleep(self.hz)
|
||||
|
||||
def pub_matrix_mass(self, dic):
|
||||
"""发布矩阵数据合值 单位g 克 JSON格式"""
|
||||
msg = String()
|
||||
# 获取当前的 ROS 时间
|
||||
current_time = self.stamp_clock.now()
|
||||
# 提取 secs 和 nsecs
|
||||
t_secs = current_time.to_msg().sec
|
||||
t_nsecs = current_time.to_msg().nanosec
|
||||
self.matrix_mass_dic["stamp"]["secs"] = t_secs
|
||||
self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
|
||||
self.matrix_mass_dic["unit"] = "g"
|
||||
self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
|
||||
self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
|
||||
self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
|
||||
self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
|
||||
self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
|
||||
msg.data = json.dumps(self.matrix_mass_dic)
|
||||
self.matrix_touch_mass_pub.publish(msg)
|
||||
|
||||
def pub_matrix_point_cloud(self):
|
||||
"""发布矩阵数据点云格式"""
|
||||
tmp_dic = self.matrix_dic.copy()
|
||||
del tmp_dic['stamp'] # 去掉时间戳字段
|
||||
all_matrices = list(tmp_dic.values()) # 5 帧，每帧 6×12=72 个数
|
||||
# 摊平到一维：360 个 float
|
||||
flat_list = [v for frame in all_matrices for v in frame] # 360
|
||||
flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
|
||||
fields = [PointField(
|
||||
name='val',
|
||||
offset=0,
|
||||
datatype=PointField.UINT8,
|
||||
count=1
|
||||
)]
|
||||
pc = PointCloud2()
|
||||
pc.header.stamp = self.stamp_clock.now().to_msg()
|
||||
pc.header.frame_id = ''
|
||||
pc.height = 1
|
||||
pc.width = flat.size # 360
|
||||
pc.fields = fields
|
||||
pc.is_bigendian = False
|
||||
pc.point_step = 1 # 1 个 float32
|
||||
pc.row_step = pc.point_step * pc.width
|
||||
pc.data = flat.tobytes() # 1440 字节
|
||||
self.matrix_touch_pub_pc.publish(pc)
|
||||
|
||||
def pub_matrix_dic(self):
|
||||
"""发布矩阵数据JSON格式"""
|
||||
msg = String()
|
||||
# 获取当前的 ROS 时间
|
||||
current_time = self.stamp_clock.now()
|
||||
# 提取 secs 和 nsecs
|
||||
t_secs = current_time.to_msg().sec
|
||||
t_nsecs = current_time.to_msg().nanosec
|
||||
self.matrix_dic["stamp"]["secs"] = t_secs
|
||||
self.matrix_dic["stamp"]["nsecs"] = t_nsecs
|
||||
msg.data = json.dumps(self.matrix_dic)
|
||||
self.matrix_touch_pub.publish(msg)
|
||||
|
||||
def joint_state_msg(self, pose,vel=[]):
|
||||
joint_state = JointState()
|
||||
joint_state.header = Header()
|
||||
joint_state.header.stamp = self.get_clock().now().to_msg()
|
||||
joint_state.name = self.api.get_finger_order()
|
||||
joint_state.position = [float(x) for x in pose]
|
||||
if len(vel) > 1:
|
||||
joint_state.velocity = [float(x) for x in vel]
|
||||
else:
|
||||
joint_state.velocity = [0.0] * len(pose)
|
||||
joint_state.effort = [0.0] * len(pose)
|
||||
return joint_state
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
def hand_setting_cb(self,msg):
|
||||
'''控制命令回调'''
|
||||
data = json.loads(msg.data)
|
||||
print(f"Received setting command: {data['setting_cmd']}",flush=True)
|
||||
try:
|
||||
if data["params"]["hand_type"] == "left":
|
||||
hand = self.api
|
||||
hand_left = True
|
||||
elif data["params"]["hand_type"] == "right":
|
||||
hand = self.api
|
||||
hand_right = True
|
||||
else:
|
||||
print("Please specify the hand part to be set",flush=True)
|
||||
return
|
||||
self.cmd_lock = True
|
||||
# Set maximum torque
|
||||
if data["setting_cmd"] == "set_max_torque_limits": # Set maximum torque
|
||||
torque = list(data["params"]["torque"])
|
||||
hand.set_torque(torque=torque)
|
||||
|
||||
if data["setting_cmd"] == "set_speed": # Set speed
|
||||
if isinstance(data["params"]["speed"], list) == True:
|
||||
speed = data["params"]["speed"]
|
||||
hand.set_speed(speed=speed)
|
||||
else:
|
||||
ColorMsg(msg=f"Speed parameter error, speed must be a list", color="red")
|
||||
if data["setting_cmd"] == "clear_faults": # Clear faults
|
||||
if hand_left == True and self.hand_joint == "L10" :
|
||||
ColorMsg(msg=f"L10 left hand cannot clear faults")
|
||||
elif hand_right == True and self.hand_joint == "L10" :
|
||||
ColorMsg(msg=f"L10 right hand cannot clear faults")
|
||||
else:
|
||||
hand.clear_faults()
|
||||
if data["setting_cmd"] == "get_faults": # Get faults
|
||||
f = hand.get_fault()
|
||||
ColorMsg(msg=f"Get faults: {f}")
|
||||
if data["setting_cmd"] == "electric_current": # Get current
|
||||
ColorMsg(msg=f"Get current: {hand.get_current()}")
|
||||
if data["setting_cmd"] == "set_electric_current": # Set current
|
||||
if isinstance(data["params"]["current"], list) == True:
|
||||
hand.set_current(data["params"]["current"])
|
||||
if data["setting_cmd"] == "show_fun_table": # Get faults
|
||||
f = hand.show_fun_table()
|
||||
except:
|
||||
print("命令参数错误")
|
||||
self.cmd_lock = False
|
||||
finally:
|
||||
self.cmd_lock = False
|
||||
|
||||
|
||||
def close_can(self):
|
||||
self.api.open_can.close_can(can=self.can)
|
||||
sys.exit(0)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
try:
|
||||
rclpy.init(args=args)
|
||||
node = LinkerHand("linker_hand_sdk")
|
||||
embedded_version = node.embedded_version
|
||||
if len(embedded_version) == 3 or node.hand_joint.upper() == "O6" or node.hand_joint.upper() == "L6" or node.hand_joint.upper() == "G20":
|
||||
ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
|
||||
node.sdk_v = 2
|
||||
elif len(embedded_version) == 6 and node.hand_joint == "L10":
|
||||
ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
|
||||
node.sdk_v = 2
|
||||
elif len(embedded_version) > 4 and ((embedded_version[0]==10 and embedded_version[4]>35) or (embedded_version[0]==7 and embedded_version[4]>50) or (embedded_version[0] == 6)):
|
||||
ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
|
||||
node.sdk_v = 2
|
||||
else:
|
||||
ColorMsg(msg=f"SDK V1", color="green")
|
||||
node.sdk_v = 1
|
||||
rclpy.spin(node) # 主循环，监听 ROS 回调
|
||||
except KeyboardInterrupt:
|
||||
print("收到 Ctrl+C，准备退出...")
|
||||
finally:
|
||||
# node.close_can() # 关闭 CAN 或其他硬件资源
|
||||
# node.destroy_node() # 销毁 ROS 节点
|
||||
# rclpy.shutdown() # 关闭 ROS
|
||||
print("程序已退出。")
|
||||
Reference in New Issue
Block a user