diff --git a/.gitignore b/.gitignore index 88691e7..f6bcdff 100644 --- a/.gitignore +++ b/.gitignore @@ -50,6 +50,7 @@ Thumbs.db # Runtime and calibration scratch files /logs/ +/MvSdkLog/ *.tmp *.log *.bak @@ -66,8 +67,9 @@ Thumbs.db *.verification.json *_mapping_quality.json -# Device-specific robot descriptions derived from local CMC pitch calibration +# Device-specific robot descriptions derived from local calibration runs /src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_cmc_pitch_*.urdf +/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left_zero_calibrated_*.urdf # ROS bag / MCAP recordings and CAN captures rosbag2_*/ diff --git a/src/g20_thumb_apriltag_calibration/README.md b/src/g20_thumb_apriltag_calibration/README.md index 46d1c32..9d49964 100644 --- a/src/g20_thumb_apriltag_calibration/README.md +++ b/src/g20_thumb_apriltag_calibration/README.md @@ -1,6 +1,175 @@ -# G20 左手拇指正面 AprilTag 标定 +# G20 左手 AprilTag 标定 -该包启动 RealSense、彩色图像校正、`apriltag_ros`、Linker Hand SDK 和标定状态机， +## 三机位全手一键标定 + +正式全手入口同时使用三台海康 `MV-CS020-10U/10UM` 黑白全局快门相机， +但只有 `/g20_calibration` 一个节点拥有机械手命令发布权。默认机位绑定为： + +```text +front = DB2163742，Tag 0/1/2/3/10 +side = DB2163749，Tag 4/5/6/7 +top = DB2163739，Tag 8/9 +``` + +11 张 `tag36h11` 的程序角色必须与贴纸所在刚性件一致： + +| ID | 机位 | 固定位置/运动件 | +|---:|---|---| +| 0 | 正面 | 正面掌壳固定基准 | +| 1 | 正面 | 拇指 CMC 后连杆 | +| 2 | 正面 | 拇指 MCP 后连杆 | +| 3 | 正面 | 拇指 IP 后末节 | +| 4 | 侧面 | 掌壳侧面固定基准（最底下） | +| 5 | 侧面 | 食指 MCP 后连杆 | +| 6 | 侧面 | 食指 PIP 后连杆 | +| 7 | 侧面 | 食指 DIP 后末节 | +| 8 | 上面 | 上面相机可见的掌壳/底座固定基准 | +| 9 | 上面 | 拇指 CMC yaw 运动件 | +| 10 | 正面 | 食指根部侧摆运动件（index_mcp_roll） | + +ID 4 不贴在侧面相机看不到的掌心正面；ID 8 必须始终固定且可见， +ID 9 必须在拇指横摆的完整行程中持续可见。 +贴纸不能跨关节、贴在软胶上或在扫描过程中翘起。 + +每台相机必须有独立的内参文件： + +```text +~/.ros/camera_info/hikrobot_DB2163742.yaml +~/.ros/camera_info/hikrobot_DB2163749.yaml +~/.ros/camera_info/hikrobot_DB2163739.yaml +``` + +先使用禁止运动模式检查三个机位、内参和标签： + +```bash +ros2 launch g20_thumb_apriltag_calibration \ + three_camera_calibration.launch.py \ + serial_number:=G20_LEFT_001 \ + commands_enabled:=false +``` + +分别查看三个相机画面： + +```bash +ros2 run image_view image_view --ros-args \ + --remap image:=/g20_calibration/front/camera/image_rect +ros2 run image_view image_view --ros-args \ + --remap image:=/g20_calibration/side/camera/image_rect +ros2 run image_view image_view --ros-args \ + --remap image:=/g20_calibration/top/camera/image_rect +``` + +安装相机时可以按机位启动红/蓝线对准辅助节点。红线是画面理想水平线， +蓝线是在画面下部检测到的桌边、底座边或临时刚性直尺；两线夹角不超过 +`±0.5°` 且上下构图偏差不超过 `±12 px` 时显示 `ALIGNED`。Tag只画绿色 +识别框，其角点方向完全不参与红/蓝线角度计算，因此Tag无需为了相机对准而贴正。 + +下面以正面机位为例，先启动辅助节点： + +```bash +ros2 run g20_thumb_apriltag_calibration camera_alignment_view \ + --ros-args -p view:=front +``` + +再打开它发布的叠加画面： + +```bash +ros2 run image_view image_view --ros-args \ + --remap image:=/g20_camera_alignment_view/image +``` + +侧面和上面分别把 `view:=front` 改为 `view:=side`、`view:=top`。建议一次只开 +一个机位完成调整；侧面或上面没有合适长边时，临时放置与目标机械轴平行的刚性 +直尺。调整完成后退出辅助节点和 +`image_view`，再进行正式标定，以免额外的200万像素图像订阅影响采集帧率。 +这组红/蓝线只检查图像平面滚转角，不检查相机距离、俯仰、偏航，也不会阻止 +`/g20_calibration/start`。 + +确认所有目标关节的 `0～255` 行程安全、MVS 客户端已关闭且没有其他命令发布者后， +重新启动正式流程： + +```bash +ros2 launch g20_thumb_apriltag_calibration \ + three_camera_calibration.launch.py \ + serial_number:=G20_LEFT_001 \ + can_interface:=can0 +``` + +状态显示三个机位均“就绪”后只调用一次： + +```bash +ros2 topic echo /g20_calibration/status_text +ros2 service call /g20_calibration/start std_srvs/srv/Trigger {} +``` + +收到 `start` 后，程序先下发并确认以下20通道基准姿态，稳定保持0.5秒后才开始 +第一条轨迹扫描： + +```text +[255, 255, 255, 255, 255, 255, 127, 127, 127, 127, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255] +``` + +程序依次完成正面电机 `0/5/15/6`、侧面电机 `1/16`、上面电机 `10` 的三轮 +往返扫描。中指、无名指、小指复制食指模板。四指侧摆先以命令255 +为原始0角测出总行程，再减去总行程的一半；最终满足命令0为正、命令255为负， +零位命令是实测曲线上最接近角度中点的整数命令。 + +每个直接测量任务完成 `3轮×2方向=6个扫描方向` 后，程序立即试拟合 +该电机对应的所有主动/被动关节。`thumb_cmc_pitch`、`thumb_cmc_roll`、 +`thumb_mcp`、`thumb_ip`、`index_mcp_roll`、`index_mcp_pitch` 和 `index_pip` 使用图像平面的 +连杆相对中心圆相位，避免小尺寸平面 Tag 的PnP深度双解把稳定的二维圆轨迹扭曲成 +错误三维轨迹。上面斜视的 `thumb_cmc_yaw` 和侧面的被动 `index_dip` 继续使用 +父Tag坐标系下的三维相对圆。`thumb_ip` 理论上也适合相对三维，但当前正面小Tag的 +PnP深度在三轮间不稳定，实测会让三维行程漂移，因此继续采用可重复的二维投影轨迹。 +程序分别检查二维圆残差/半径或三维平面RMS/圆残差/半径，并统一检查最小圆弧、 +单调修正量、正反程回差、三轮行程一致性和直接零位拟合。主动关节三轮行程最大差 +默认不超过3°；被动耦合关节允许不超过10°，但仍必须通过其余质量门限。 +任一指标失败时会立即暂停，中文状态显示关节名、实测值和阈值，不再等到42个方向 +全部结束。修正现场问题后调用 `resume`， +程序只清除该电机任务的内存样本并重扫它的6个方向；前面已通过的关节保留。 +失败样本不从 `raw_samples.jsonl` 删除，而是使用 `attempt` 和 `retry` 记录区分， +便于调试；最终拟合只使用当前通过尝试的内存数据。 + +标定食指 `index_mcp_roll`（电机6）及其随机复测时，为避免中指遮挡ID 10， +程序将中指、无名指和小指的侧摆电机7/8/9固定为0；开始采样前会同时确认 +电机6到达扫描起点且电机7/8/9均已到达0。离开该标定项后恢复统一基准姿态。 +该项目还会通过SDK设置接口把五指速度临时设为 `[15,5,15,15,15]`，即只把 +食指速度从15降为5；离开该项目后恢复 `[15,15,15,15,15]`。 +侧面标定 `index_mcp_pitch`（电机1）和 `index_pip/index_dip`（电机16）时， +五指速度设为 `[15,10,15,15,15]`，即食指屈伸使用第三档速度10；其余直接 +测量关节保持普通速度15。 + +标定 `thumb_cmc_yaw`（电机10）及其随机复测时，程序将 +`thumb_cmc_roll`（电机5）固定为145，并在它到位后才开始采样，以保持运动Tag +ID 9的可见性和PnP稳定性。离开该标定项后，电机5恢复基准值255； +最终JSON的 `baseline_command_u8` 不变。 + +三机位流程默认设置 `validation_enabled:=false`，因此拟合完成后会直接 +恢复基准姿态并生成JSON，不再进入 `VALIDATION_MOVE/VALIDATION_CAPTURE`。 +此时 `quality.passed` 只由轨迹与零位拟合质量决定，`validation_mae_rad` 和 +`validation_p95_rad` 为 `null`。需要恢复随机复测时，启动参数加 +`validation_enabled:=true`。 + +上面机位在Tag二维质量合格但PnP连续无效达到1秒时，会自动重置该机位的单Tag +和双Tag连续性跟踪器，并从下一帧重新建链，早于3秒采集超时。暂停恢复时只要求 +当前活动机位就绪；首次调用 `start` 仍要求三个机位全部通过预检。 + +对外只生成一个精简运行时结果： + +```text +calibration_output/G20_LEFT_001/<时间戳>/ + g20_left_G20_LEFT_001_calibration.json +``` + +文件包含21个关节的256项 `angle_rad`、16个主动关节的 `zero_command_u8`、 +5个被动标记、模板来源和总体质量。相机、Tag、正反程及每轮质量只进入状态、日志和 +`raw_samples.jsonl`，不写入最终运行时 JSON。 + +下面保留原有正面拇指独立标定说明和兼容入口。 + +该包启动海康机器人 MVS USB3 Vision 黑白相机、图像校正、`apriltag_ros`、 +Linker Hand SDK 和标定状态机， 只扫描 G20 左手命令下标 `0`、`15`。默认使用单终点连续模式：每个方向只发送一次 终点命令，速度保持在固件能稳定响应的 `15`。SDK 以独立时间戳反馈实际 20 维位置， 程序把每帧 AprilTag 三维中心与同一时刻的实际电机位置插值配对并按整数位置分箱。 @@ -50,11 +219,11 @@ PnP 双分支跟踪和整段刚体复核仍保留，用于选出稳定的三维 ```bash sudo apt-get update sudo apt-get install -y \ - ros-jazzy-realsense2-camera \ - ros-jazzy-realsense2-description \ ros-jazzy-image-pipeline \ ros-jazzy-apriltag-ros \ - ros-jazzy-apriltag-msgs + ros-jazzy-apriltag-msgs \ + ros-jazzy-camera-calibration \ + python3-yaml cd /home/lxp/projects/linkerhand_retarget_ros2 source /opt/ros/jazzy/setup.bash @@ -63,6 +232,58 @@ colcon build --symlink-install \ source install/setup.bash ``` +相机节点直接使用海康 MVS SDK。当前机器的默认安装位置是 `/opt/MVS`，需要存在： + +```text +/opt/MVS/lib/64/libMvCameraControl.so +/opt/MVS/Samples/64/Python/MvImport/MvCameraControl_class.py +``` + +正面相机默认按序列号 `DB2163742` 绑定（MVS 显示的 GUID 是 +`2BDFB2163742`），型号校验为 `MV-CS020-10UM`。三台相机同时连接时程序不会按枚举 +顺序猜测机位。启动 ROS 节点前必须关闭 MVS 客户端中的相机连接，否则设备可能被占用。 + +`1624x1240 mono8` 每帧约 2.0 MB，超过 Fast DDS 2.14 默认约 512 KB 的共享内存段。 +相机节点和三个标定 launch 会自动加载 `config/fastdds_large_images.xml`，使用 64 MB +共享内存段；否则相机内部虽为 30 Hz，大图订阅端通常只能收到约 1～4 Hz。修改配置后 +必须重启相关 ROS 进程才能生效。 + +首次使用必须先标定该相机和当前镜头的内参。主 launch 默认从 +`~/.ros/camera_info/hikrobot_DB2163742.yaml` 加载标准 ROS CameraInfo YAML；文件缺失时 +仍可预览 `mono8` 原图，但发布的内参无效，轨迹标定预检不会解锁运动。 + +先单独启动相机（不会连接机械手，也不会发送关节命令）： + +```bash +ros2 run g20_thumb_apriltag_calibration hikrobot_camera_node --ros-args \ + --remap __ns:=/camera/camera/color \ + -p serial_number:=DB2163742 \ + -p camera_info_url:=$HOME/.ros/camera_info/hikrobot_DB2163742.yaml +``` + +测速时优先检查同帧发布的小消息和原图；两者正常值都应接近 30 Hz： + +```bash +ros2 topic hz /camera/camera/color/camera_info +ros2 topic hz /camera/camera/color/image_raw +``` + +使用标定板采集内参。下面的 `8x6` 是内角点数量、`0.020` 是单格边长 20 mm，必须按 +实际标定板修改： + +```bash +ros2 run camera_calibration cameracalibrator \ + --size 8x6 --square 0.020 \ + --camera_name hikrobot_front_DB2163742 \ + --ros-args \ + --remap image:=/camera/camera/color/image_raw \ + --remap camera/set_camera_info:=/camera/camera/color/set_camera_info +``` + +在标定界面完成采样后点击 `CALIBRATE`，确认重投影误差，再点击 `COMMIT`。相机节点会 +原子写入上述 YAML，并立即开始发布有效内参。内参只适用于标定时的镜头焦距、对焦、 +分辨率和 ROI；改变任何一项都要重新标定。 + 连接 CAN 后先确认 `can0` 已启动。不要同时运行其他会发布 `/g20/cb_left_hand_control_cmd` 的程序。 @@ -74,6 +295,7 @@ source install/setup.bash ```bash ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \ serial_number:=G20_LEFT_001 \ + camera_serial_number:=DB2163742 \ commands_enabled:=false ``` @@ -84,6 +306,7 @@ ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \ ```bash ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \ serial_number:=G20_LEFT_001 \ + camera_serial_number:=DB2163742 \ can_interface:=can0 \ calibration_speed:=15 \ continuous_motion_mode:=endpoint \ @@ -92,7 +315,7 @@ ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \ use_roi:=false ``` -默认关闭 ROI，AprilTag 使用完整的 1280×720 校正画面。查看实际送入 AprilTag +默认关闭 ROI，AprilTag 使用完整的 1624×1240 校正画面。查看实际送入 AprilTag 的完整画面： ```bash @@ -241,16 +464,16 @@ for name, joint in data["joints"].items(): ``` 如果相机或 SDK 已由外部进程启动，可传 -`start_camera:=false` 或 `start_sdk:=false`。用 `camera_serial_number:=<序列号>` -可绑定指定 RealSense。 +`start_camera:=false` 或 `start_sdk:=false`。`camera_serial_number` 同时接受 MVS +序列号和 GUID，但推荐使用稳定且简短的序列号 `DB2163742`。 -D405 的彩色流来自 `depth_module`，启动文件会同时设置 -`depth_module.color_profile` 和 `rgb_camera.color_profile`，默认均为 -`1280x720x30`。连续模式默认关闭深度和 rosbag，以减少 USB、CPU 和磁盘负担； -它们都不参与角度计算。需要完整诊断留档时可增加 -`enable_depth:=true record_bag:=true`。 +海康相机默认输出 `1624x1240@30Hz mono8`，全局快门，曝光时间 `5000us`、增益 +`0dB`，并使用“只取最新帧”策略避免视觉延迟。现场亮度不足时优先增加照明；必要时可用 +`exposure_time_us`、`gain_db` 调整，或临时传 `auto_exposure:=true`。正式轨迹采集建议固定 +曝光，避免自动曝光在运动过程中改变角点质量。rosbag 默认关闭；需要诊断留档时增加 +`record_bag:=true`。 -默认对完整 1280×720 原图进行畸变校正和 AprilTag 检测。三维中心 PnP 必须使用 +默认对完整 1624×1240 原图进行畸变校正和 AprilTag 检测。三维中心 PnP 必须使用 `image_rect` 的角点及同一条处理链对应的 `CameraInfo`，启动文件已自动保证二者配对。 校正和 AprilTag 组件运行 在同一个多线程容器内并启用进程内传输，避免在处理链路中重复序列化、复制大图像。 diff --git a/src/g20_thumb_apriltag_calibration/config/fastdds_large_images.xml b/src/g20_thumb_apriltag_calibration/config/fastdds_large_images.xml new file mode 100644 index 0000000..4a866ea --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/config/fastdds_large_images.xml @@ -0,0 +1,32 @@ + + + + + + g20_udp_transport + UDPv4 + 10485760 + 10485760 + + + g20_shm_transport + SHM + 67108864 + 512 + 1000 + + + + + + + g20_udp_transport + g20_shm_transport + + false + + + + diff --git a/src/g20_thumb_apriltag_calibration/config/realsense_color_qos.yaml b/src/g20_thumb_apriltag_calibration/config/realsense_color_qos.yaml deleted file mode 100644 index 1261ee8..0000000 --- a/src/g20_thumb_apriltag_calibration/config/realsense_color_qos.yaml +++ /dev/null @@ -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 diff --git a/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml b/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml new file mode 100644 index 0000000..ea727e4 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/config/three_camera_calibration.yaml @@ -0,0 +1,76 @@ +g20_calibration: + ros__parameters: + command_topic: /g20/cb_left_hand_control_cmd + state_topic: /g20/cb_left_hand_state + info_topic: /g20/cb_left_hand_info + setting_topic: /g20/cb_hand_setting_cmd + front_camera_info_topic: /g20_calibration/front/camera/camera_info + front_detections_topic: /g20_calibration/front/apriltag/detections + side_camera_info_topic: /g20_calibration/side/camera/camera_info + side_detections_topic: /g20_calibration/side/apriltag/detections + top_camera_info_topic: /g20_calibration/top/camera/camera_info + top_detections_topic: /g20_calibration/top/apriltag/detections + + # /start先下发并确认这个20通道基准姿态，稳定后才进入第一条扫描。 + baseline_command_u8: [255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255] + normal_calibration_speed: 15 + index_roll_calibration_speed: 5 + index_flex_calibration_speed: 10 + speed_setting_settle_seconds: 0.25 + + tag_size_m: 0.010 + repetitions: 3 + preflight_frames: 60 + minimum_detection_rate: 0.95 + minimum_detection_hz: 15.0 + maximum_hamming: 0 + minimum_decision_margin: 30.0 + minimum_edge_pixels: 30.0 + + pnp_maximum_reprojection_error_px: 1.5 + pnp_reprojection_tie_px: 1.5 + pnp_maximum_pose_jump_deg: 35.0 + pnp_maximum_translation_jump_m: 0.04 + pnp_maximum_tag_tilt_deg: 75.0 + pnp_tracker_reset_seconds: 5.0 + top_pnp_invalid_reset_seconds: 1.0 + maximum_state_image_skew_ms: 150.0 + + endpoint_tolerance_u8: 2.0 + endpoint_hold_seconds: 0.5 + baseline_hold_seconds: 0.5 + position_timeout_seconds: 30.0 + sweep_timeout_seconds: 90.0 + invalid_timeout_seconds: 3.0 + minimum_sweep_frames: 40 + minimum_state_span_u8: 240.0 + minimum_sweep_bins: 32 + maximum_bin_gap: 16 + + trajectory_maximum_plane_rms_m: 0.004 + trajectory_maximum_radial_rms_m: 0.004 + trajectory_minimum_radius_m: 0.003 + trajectory_minimum_arc_deg: 15.0 + # 正面拇指pitch/MCP/IP使用二维圆相位，避免平面Tag的PnP深度歧义。 + image_trajectory_maximum_radial_rms_px: 2.0 + image_trajectory_maximum_radial_p95_px: 3.5 + image_trajectory_minimum_radius_px: 20.0 + trajectory_maximum_cycle_travel_difference_deg: 3.0 + passive_maximum_cycle_travel_difference_deg: 10.0 + zero_minimum_radius_px: 20.0 + zero_maximum_radial_rms_px: 2.0 + zero_maximum_radial_p95_px: 3.5 + zero_maximum_round_difference_deg: 1.0 + maximum_monotonic_correction_deg: 2.0 + maximum_hysteresis_deg: 5.0 + passive_maximum_monotonic_correction_deg: 3.0 + passive_maximum_hysteresis_deg: 7.5 + + # 默认跳过耗时的随机复测；需要验收精度时可在launch中设为true。 + validation_enabled: false + validation_command_count: 3 + validation_frames: 10 + validation_seed: 20260804 + validation_timeout_seconds: 20.0 + maximum_validation_mae_deg: 2.0 + maximum_validation_p95_deg: 3.0 diff --git a/src/g20_thumb_apriltag_calibration/config/three_camera_tags.yaml b/src/g20_thumb_apriltag_calibration/config/three_camera_tags.yaml new file mode 100644 index 0000000..5f24344 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/config/three_camera_tags.yaml @@ -0,0 +1,62 @@ +/g20_calibration/front/apriltag/apriltag: + ros__parameters: + image_transport: raw + qos_profile: sensor_data + family: 36h11 + size: 0.010 + profile: false + max_hamming: 0 + detector: + threads: 4 + decimate: 1.5 + blur: 0.0 + refine: true + sharpening: 0.25 + debug: false + pose_estimation_method: pnp + tag: + ids: [0, 1, 2, 3, 10] + frames: [front_base, thumb_cmc, thumb_mcp, thumb_ip, index_roll] + sizes: [0.010, 0.010, 0.010, 0.010, 0.010] + +/g20_calibration/side/apriltag/apriltag: + ros__parameters: + image_transport: raw + qos_profile: sensor_data + family: 36h11 + size: 0.010 + profile: false + max_hamming: 0 + detector: + threads: 4 + decimate: 1.5 + blur: 0.0 + refine: true + sharpening: 0.25 + debug: false + pose_estimation_method: pnp + tag: + ids: [4, 5, 6, 7] + frames: [side_base, index_mcp, index_pip, index_dip] + sizes: [0.010, 0.010, 0.010, 0.010] + +/g20_calibration/top/apriltag/apriltag: + ros__parameters: + image_transport: raw + qos_profile: sensor_data + family: 36h11 + size: 0.010 + profile: false + max_hamming: 0 + detector: + threads: 4 + decimate: 1.5 + blur: 0.0 + refine: true + sharpening: 0.25 + debug: false + pose_estimation_method: pnp + tag: + ids: [8, 9] + frames: [top_base, thumb_yaw] + sizes: [0.010, 0.010] diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/acquisition.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/acquisition.py index d3eadeb..d2e6b57 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/acquisition.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/acquisition.py @@ -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 diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/alignment_view.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/alignment_view.py new file mode 100644 index 0000000..be019dd --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/alignment_view.py @@ -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() diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py new file mode 100644 index 0000000..b21d431 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/full_hand.py @@ -0,0 +1,756 @@ +"""Pure three-camera G20 calibration model and compact runtime schema. + +The hardware node records one parent/child AprilTag trajectory for each +directly observable joint. This module deliberately contains no ROS imports: +curve fitting, four-finger splay centring, inheritance, and schema validation +remain deterministic and unit-testable without connected cameras or a hand. +""" + +from __future__ import annotations + +from dataclasses import dataclass, replace +import math +from typing import Any, Mapping, Sequence + +import numpy as np + +from .core import BASELINE_COMMAND +from .trajectory import ( + _angle_for_circle, + _fit_circle_with_axis, + _fit_joint_curve, + _fit_plane_axis, + _orient_circle_positive, +) +from .zero_calibration import ( + _fit_circle, + _trajectory_arc_rad, + circular_median_rad, + maximum_pairwise_angle_difference_rad, + wrap_angle_rad, +) + + +@dataclass(frozen=True) +class JointSpec: + name: str + motor_index: int + active: bool + view: str | None + parent_role: str | None + child_role: str | None + source_joint: str | None = None + zero_kind: str | None = None + + @property + def measured(self) -> bool: + return self.source_joint is None + + +@dataclass(frozen=True) +class SweepSpec: + view: str + motor_index: int + joints: tuple[str, ...] + + +@dataclass(frozen=True) +class JointCurveFit: + angle_rad: tuple[float, ...] + decreasing_rad: tuple[float, ...] + increasing_rad: tuple[float, ...] + circle: Mapping[str, Any] + maximum_monotonic_correction_rad: float + maximum_hysteresis_rad: float + quality: Mapping[str, float] + zero_offset_rad: float = 0.0 + + +VIEW_TAGS: dict[str, dict[str, int]] = { + "front": { + "front_base": 0, + "thumb_cmc": 1, + "thumb_mcp": 2, + "thumb_ip": 3, + "index_roll": 10, + }, + "side": { + "side_base": 4, + "index_mcp": 5, + "index_pip": 6, + "index_dip": 7, + }, + "top": { + "top_base": 8, + "thumb_yaw": 9, + }, +} + + +JOINT_SPECS: dict[str, JointSpec] = { + "thumb_cmc_pitch": JointSpec( + "thumb_cmc_pitch", 0, True, "front", "front_base", "thumb_cmc", + zero_kind="projected", + ), + "index_mcp_pitch": JointSpec( + "index_mcp_pitch", 1, True, "side", "side_base", "index_mcp", + zero_kind="projected", + ), + "middle_mcp_pitch": JointSpec( + "middle_mcp_pitch", 2, True, None, None, None, + source_joint="index_mcp_pitch", zero_kind="inherited", + ), + "ring_mcp_pitch": JointSpec( + "ring_mcp_pitch", 3, True, None, None, None, + source_joint="index_mcp_pitch", zero_kind="inherited", + ), + "pinky_mcp_pitch": JointSpec( + "pinky_mcp_pitch", 4, True, None, None, None, + source_joint="index_mcp_pitch", zero_kind="inherited", + ), + "thumb_cmc_roll": JointSpec( + "thumb_cmc_roll", 5, True, "front", "front_base", "thumb_cmc", + zero_kind="projected", + ), + "index_mcp_roll": JointSpec( + "index_mcp_roll", 6, True, "front", "front_base", "index_roll", + zero_kind="travel_midpoint", + ), + "middle_mcp_roll": JointSpec( + "middle_mcp_roll", 7, True, None, None, None, + source_joint="index_mcp_roll", zero_kind="inherited", + ), + "ring_mcp_roll": JointSpec( + "ring_mcp_roll", 8, True, None, None, None, + source_joint="index_mcp_roll", zero_kind="inherited", + ), + "pinky_mcp_roll": JointSpec( + "pinky_mcp_roll", 9, True, None, None, None, + source_joint="index_mcp_roll", zero_kind="inherited", + ), + "thumb_cmc_yaw": JointSpec( + "thumb_cmc_yaw", 10, True, "top", "top_base", "thumb_yaw", + zero_kind="projected", + ), + "thumb_mcp": JointSpec( + "thumb_mcp", 15, True, "front", "thumb_cmc", "thumb_mcp", + zero_kind="projected", + ), + "index_pip": JointSpec( + "index_pip", 16, True, "side", "index_mcp", "index_pip", + zero_kind="projected", + ), + "middle_pip": JointSpec( + "middle_pip", 17, True, None, None, None, + source_joint="index_pip", zero_kind="inherited", + ), + "ring_pip": JointSpec( + "ring_pip", 18, True, None, None, None, + source_joint="index_pip", zero_kind="inherited", + ), + "pinky_pip": JointSpec( + "pinky_pip", 19, True, None, None, None, + source_joint="index_pip", zero_kind="inherited", + ), + "thumb_ip": JointSpec( + "thumb_ip", 15, False, "front", "thumb_mcp", "thumb_ip", + ), + "index_dip": JointSpec( + "index_dip", 16, False, "side", "index_pip", "index_dip", + ), + "middle_dip": JointSpec( + "middle_dip", 17, False, None, None, None, + source_joint="index_dip", + ), + "ring_dip": JointSpec( + "ring_dip", 18, False, None, None, None, + source_joint="index_dip", + ), + "pinky_dip": JointSpec( + "pinky_dip", 19, False, None, None, None, + source_joint="index_dip", + ), +} + + +SWEEP_SPECS: tuple[SweepSpec, ...] = ( + SweepSpec("front", 0, ("thumb_cmc_pitch",)), + SweepSpec("front", 5, ("thumb_cmc_roll",)), + SweepSpec("front", 15, ("thumb_mcp", "thumb_ip")), + SweepSpec("front", 6, ("index_mcp_roll",)), + SweepSpec("side", 1, ("index_mcp_pitch",)), + SweepSpec("side", 16, ("index_pip", "index_dip")), + SweepSpec("top", 10, ("thumb_cmc_yaw",)), +) + +MEASURED_JOINTS: tuple[str, ...] = tuple( + name for name, spec in JOINT_SPECS.items() if spec.measured +) +ACTIVE_JOINTS: tuple[str, ...] = tuple( + name for name, spec in JOINT_SPECS.items() if spec.active +) +PASSIVE_JOINTS: tuple[str, ...] = tuple( + name for name, spec in JOINT_SPECS.items() if not spec.active +) +SPLAY_JOINTS: tuple[str, ...] = ( + "index_mcp_roll", + "middle_mcp_roll", + "ring_mcp_roll", + "pinky_mcp_roll", +) + +# These directly actuated motions have a fixed parent link during their sweep +# and a motion plane that is close to the active camera's image plane. Their +# projected circles are substantially more repeatable than the difference of +# two independently estimated planar-Tag PnP depths. thumb_ip remains projected +# because its front-view PnP depth is not repeatable enough for a 3-D fit; the +# side-view index_dip and oblique top-view yaw remain parent-relative 3-D. +IMAGE_TRAJECTORY_JOINTS: frozenset[str] = frozenset( + { + "thumb_cmc_pitch", + "thumb_cmc_roll", + "thumb_mcp", + "thumb_ip", + "index_mcp_roll", + "index_mcp_pitch", + "index_pip", + } +) + +# Keep the three unmeasured finger-roll motors away from the front camera's +# line of sight while index_mcp_roll is measured. +INDEX_ROLL_CLEARANCE_COMMANDS: dict[int, int] = { + 7: 0, + 8: 0, + 9: 0, +} + +# Hold thumb CMC roll at a camera-friendly pose while thumb CMC yaw is +# measured. This keeps the moving top-view tag sufficiently front-facing. +THUMB_YAW_CLEARANCE_COMMANDS: dict[int, int] = { + 5: 145, +} + + +def calibration_auxiliary_commands(spec: SweepSpec) -> dict[int, int]: + """Return motors that must remain fixed throughout one calibration task.""" + if spec.motor_index == 6: + return dict(INDEX_ROLL_CLEARANCE_COMMANDS) + if spec.motor_index == 10: + return dict(THUMB_YAW_CLEARANCE_COMMANDS) + return {} + + +def build_full_hand_command( + motor_index: int, + command_u8: int, + baseline: Sequence[int] = BASELINE_COMMAND, +) -> list[int]: + if len(baseline) != 20: + raise ValueError("baseline must contain exactly 20 values") + motor = int(motor_index) + if motor not in {spec.motor_index for spec in JOINT_SPECS.values()}: + raise ValueError("motor_index is not a controlled G20 calibration channel") + command = int(command_u8) + if not 0 <= command <= 255: + raise ValueError("command_u8 must be in [0, 255]") + result = [int(value) for value in baseline] + if any(not 0 <= value <= 255 for value in result): + raise ValueError("baseline values must be in [0, 255]") + result[motor] = command + return result + + +def build_calibration_motion_command( + spec: SweepSpec, + command_u8: int, + baseline: Sequence[int] = BASELINE_COMMAND, +) -> list[int]: + """Build a sweep command, including any required clearance pose.""" + result = build_full_hand_command( + spec.motor_index, + command_u8, + baseline=baseline, + ) + for motor_index, auxiliary_command in calibration_auxiliary_commands( + spec + ).items(): + result[motor_index] = auxiliary_command + return result + + +def build_calibration_speed_profile( + spec: SweepSpec, + *, + normal_speed: int, + index_roll_speed: int, + index_flex_speed: int, +) -> list[int]: + """Return G20's five per-finger speeds for one calibration task.""" + normal = int(normal_speed) + index_roll = int(index_roll_speed) + index_flex = int(index_flex_speed) + if not all( + 0 <= speed <= 255 for speed in (normal, index_roll, index_flex) + ): + raise ValueError("calibration speeds must be in [0, 255]") + speeds = [normal] * 5 + if spec.motor_index == 6: + speeds[1] = index_roll + elif spec.motor_index in {1, 16}: + speeds[1] = index_flex + return speeds + + +def _record_vector(record: Mapping[str, Any]) -> np.ndarray: + value = np.asarray(record.get("relative_translation_xyz_m"), dtype=float) + if value.shape != (3,) or not np.all(np.isfinite(value)): + raise ValueError("record relative_translation_xyz_m must contain 3 values") + return value + + +def _record_image_vector(record: Mapping[str, Any]) -> np.ndarray: + value = np.asarray(record.get("image_relative_xy_px"), dtype=float) + if value.shape != (2,) or not np.all(np.isfinite(value)): + raise ValueError("record image_relative_xy_px must contain 2 values") + return value + + +def fit_joint_center_curve( + records: Sequence[Mapping[str, Any]], + *, + maximum_plane_rms_m: float = 0.004, + maximum_radial_rms_m: float = 0.004, + minimum_radius_m: float = 0.003, + minimum_arc_rad: float = math.radians(15.0), +) -> JointCurveFit: + """Fit one command-indexed curve from a parent-frame centre trajectory.""" + samples = [dict(record) for record in records] + if len(samples) < 12: + raise ValueError("joint trajectory requires at least 12 samples") + points = np.asarray([_record_vector(record) for record in samples]) + axis, common_plane_rms = _fit_plane_axis([points]) + circle = _fit_circle_with_axis(points, axis) + circle = _orient_circle_positive(circle, samples, list(points)) + values = [_angle_for_circle(point, circle) for point in points] + curves, correction, hysteresis = _fit_joint_curve(samples, values) + quality = { + "plane_rms_m": max( + float(common_plane_rms), float(circle["plane_rms_m"]) + ), + "radial_rms_m": float(circle["radial_rms_m"]), + "radius_m": float(circle["radius_m"]), + "arc_rad": float(circle["observed_arc_rad"]), + } + failures: list[str] = [] + if quality["plane_rms_m"] > float(maximum_plane_rms_m): + failures.append("plane_rms") + if quality["radial_rms_m"] > float(maximum_radial_rms_m): + failures.append("radial_rms") + if quality["radius_m"] < float(minimum_radius_m): + failures.append("radius") + if quality["arc_rad"] < float(minimum_arc_rad): + failures.append("arc") + if failures: + raise ValueError("joint_trajectory_quality_failed:" + ",".join(failures)) + return JointCurveFit( + angle_rad=tuple(float(value) for value in curves["angle_rad"]), + decreasing_rad=tuple( + float(value) for value in curves["decreasing_rad"] + ), + increasing_rad=tuple( + float(value) for value in curves["increasing_rad"] + ), + circle=dict(circle), + maximum_monotonic_correction_rad=float(correction), + maximum_hysteresis_rad=float(hysteresis), + quality=quality, + ) + + +def _signed_image_circle_angle( + point_xy_px: Sequence[float], circle: Mapping[str, Any] +) -> float: + point = np.asarray(point_xy_px, dtype=float) + centre = np.asarray(circle["center_xy_px"], dtype=float) + reference = np.asarray(circle["reference_xy_px"], dtype=float) + vector = point - centre + if point.shape != (2,) or not np.all(np.isfinite(point)): + raise ValueError("image point must contain 2 finite values") + if float(np.linalg.norm(vector)) < 1.0e-9: + raise ValueError("image point lies at the fitted circle centre") + return float(circle["orientation_sign"]) * math.atan2( + float(reference[0] * vector[1] - reference[1] * vector[0]), + float(np.dot(reference, vector)), + ) + + +def fit_joint_image_curve( + records: Sequence[Mapping[str, Any]], + *, + maximum_radial_rms_px: float = 2.0, + maximum_radial_p95_px: float = 3.5, + minimum_radius_px: float = 20.0, + minimum_arc_rad: float = math.radians(15.0), +) -> JointCurveFit: + """Fit angular phase from a stable projected parent/child circle.""" + samples = [dict(record) for record in records] + if len(samples) < 12: + raise ValueError("joint image trajectory requires at least 12 samples") + points = np.asarray([_record_image_vector(record) for record in samples]) + centre, radius = _fit_circle(points) + radial_error = np.abs(np.linalg.norm(points - centre, axis=1) - radius) + quality = { + "radial_rms_px": float(np.sqrt(np.mean(np.square(radial_error)))), + "radial_p95_px": float(np.percentile(radial_error, 95.0)), + "radius_px": float(radius), + "arc_rad": float(_trajectory_arc_rad(points, centre)), + } + failures: list[str] = [] + if quality["radial_rms_px"] > float(maximum_radial_rms_px): + failures.append("radial_rms") + if quality["radial_p95_px"] > float(maximum_radial_p95_px): + failures.append("radial_p95") + if quality["radius_px"] < float(minimum_radius_px): + failures.append("radius") + if quality["arc_rad"] < float(minimum_arc_rad): + failures.append("arc") + if failures: + raise ValueError( + "joint_image_trajectory_quality_failed:" + ",".join(failures) + ) + + endpoint_points = np.asarray( + [ + point + for record, point in zip(samples, points) + if int(record["command_u8"]) == 255 + ] + ) + if endpoint_points.size == 0: + raise ValueError("joint image trajectory is missing command 255") + reference = np.median(endpoint_points, axis=0) - centre + reference_norm = float(np.linalg.norm(reference)) + if reference_norm < 1.0e-9: + raise ValueError("joint image trajectory endpoint is degenerate") + reference /= reference_norm + circle: dict[str, Any] = { + "space": "image_2d", + "center_xy_px": [float(value) for value in centre], + "reference_xy_px": [float(value) for value in reference], + "orientation_sign": 1.0, + "radius_px": float(radius), + } + values = [_signed_image_circle_angle(point, circle) for point in points] + command_zero_values = [ + value + for record, value in zip(samples, values) + if int(record["command_u8"]) == 0 + ] + if not command_zero_values: + raise ValueError("joint image trajectory is missing command 0") + if float(np.median(command_zero_values)) < 0.0: + circle["orientation_sign"] = -1.0 + values = [-value for value in values] + curves, correction, hysteresis = _fit_joint_curve(samples, values) + return JointCurveFit( + angle_rad=tuple(float(value) for value in curves["angle_rad"]), + decreasing_rad=tuple( + float(value) for value in curves["decreasing_rad"] + ), + increasing_rad=tuple( + float(value) for value in curves["increasing_rad"] + ), + circle=circle, + maximum_monotonic_correction_rad=float(correction), + maximum_hysteresis_rad=float(hysteresis), + quality=quality, + ) + + +def fit_measured_joint_curve( + joint_name: str, + records: Sequence[Mapping[str, Any]], + *, + maximum_plane_rms_m: float = 0.004, + maximum_radial_rms_m: float = 0.004, + minimum_radius_m: float = 0.003, + minimum_arc_rad: float = math.radians(15.0), + image_maximum_radial_rms_px: float = 2.0, + image_maximum_radial_p95_px: float = 3.5, + image_minimum_radius_px: float = 20.0, +) -> JointCurveFit: + """Select the stable trajectory representation for one measured joint.""" + if str(joint_name) in IMAGE_TRAJECTORY_JOINTS: + return fit_joint_image_curve( + records, + maximum_radial_rms_px=image_maximum_radial_rms_px, + maximum_radial_p95_px=image_maximum_radial_p95_px, + minimum_radius_px=image_minimum_radius_px, + minimum_arc_rad=minimum_arc_rad, + ) + return fit_joint_center_curve( + records, + maximum_plane_rms_m=maximum_plane_rms_m, + maximum_radial_rms_m=maximum_radial_rms_m, + minimum_radius_m=minimum_radius_m, + minimum_arc_rad=minimum_arc_rad, + ) + + +def center_splay_curve(fit: JointCurveFit) -> tuple[JointCurveFit, int, float]: + """Centre a measured 255->0 splay travel on its angular midpoint.""" + raw = np.asarray(fit.angle_rad, dtype=float) + if raw.shape != (256,) or not np.all(np.isfinite(raw)): + raise ValueError("splay curve must contain 256 finite values") + if np.any(np.diff(raw) > 1.0e-7): + raise ValueError("raw splay curve must be non-increasing") + travel_midpoint = 0.5 * float(raw[0] + raw[255]) + if travel_midpoint <= 0.0: + raise ValueError("splay travel must be positive") + centred = raw - travel_midpoint + # Make the public endpoint symmetry exact after decimal rounding. + endpoint = round(0.5 * float(raw[0] - raw[255]), 8) + centred[0] = endpoint + centred[255] = -endpoint + centred = np.asarray([round(float(value), 8) for value in centred]) + zero_command = int(np.argmin(np.abs(centred))) + return ( + replace( + fit, + angle_rad=tuple(float(value) for value in centred), + zero_offset_rad=round(float(travel_midpoint), 8), + ), + zero_command, + round(float(travel_midpoint), 8), + ) + + +def measure_joint_vector(fit: JointCurveFit, vector_xyz_m: Sequence[float]) -> float: + """Measure one vector with a fitted model, including splay centring.""" + return float( + _angle_for_circle(vector_xyz_m, fit.circle) - fit.zero_offset_rad + ) + + +def measure_joint_observation( + fit: JointCurveFit, + *, + vector_xyz_m: Sequence[float], + image_vector_xy_px: Sequence[float], +) -> float: + """Measure an observation in the same space used to fit its curve.""" + if fit.circle.get("space") == "image_2d": + return float( + _signed_image_circle_angle(image_vector_xy_px, fit.circle) + - fit.zero_offset_rad + ) + return measure_joint_vector(fit, vector_xyz_m) + + +def fit_projected_zero( + records: Sequence[Mapping[str, Any]], + *, + minimum_radius_px: float = 20.0, + minimum_arc_rad: float = math.radians(15.0), + maximum_radial_rms_px: float = 2.0, + maximum_radial_p95_px: float = 3.5, + maximum_round_difference_rad: float = math.radians(1.0), +) -> float: + """Return the image/table-projected zero from three sweep rounds.""" + samples = [dict(record) for record in records] + points = np.asarray( + [record.get("image_relative_xy_px") for record in samples], dtype=float + ) + if points.ndim != 2 or points.shape[1] != 2 or len(points) < 12: + raise ValueError("projected zero requires 2-D trajectory samples") + if not np.all(np.isfinite(points)): + raise ValueError("projected zero samples must be finite") + centre, radius = _fit_circle(points) + radial_error = np.abs(np.linalg.norm(points - centre, axis=1) - radius) + radial_rms = float(np.sqrt(np.mean(np.square(radial_error)))) + radial_p95 = float(np.percentile(radial_error, 95.0)) + arc = _trajectory_arc_rad(points, centre) + if radius < minimum_radius_px: + raise ValueError("projected_zero_radius_too_small") + if arc < minimum_arc_rad: + raise ValueError("projected_zero_arc_too_small") + if radial_rms > maximum_radial_rms_px: + raise ValueError("projected_zero_radial_rms_too_large") + if radial_p95 > maximum_radial_p95_px: + raise ValueError("projected_zero_radial_p95_too_large") + + cycles = sorted({int(record["cycle"]) for record in samples}) + if len(cycles) < 3: + raise ValueError("projected zero requires three sweep rounds") + angles: list[float] = [] + for cycle in cycles: + zero_points = np.asarray( + [ + record["image_relative_xy_px"] + for record in samples + if int(record["cycle"]) == cycle + and int(record["command_u8"]) == 255 + ], + dtype=float, + ) + if zero_points.size == 0: + raise ValueError("projected zero is missing command 255") + point = np.median(zero_points, axis=0) + inward = centre - point + if float(np.linalg.norm(inward)) < 1.0e-9: + raise ValueError("projected zero point is degenerate") + angles.append( + wrap_angle_rad(math.atan2(-float(inward[1]), float(inward[0]))) + ) + if maximum_pairwise_angle_difference_rad(angles) > maximum_round_difference_rad: + raise ValueError("projected_zero_round_difference_too_large") + return round(float(circular_median_rad(angles)), 8) + + +def build_compact_payload( + *, + serial_number: str, + measured_fits: Mapping[str, JointCurveFit], + projected_zeros_rad: Mapping[str, float], + splay_zero_command_u8: int, + splay_midpoint_rad: float, + validation_errors_rad: Sequence[float], + passed: bool, + baseline: Sequence[int] = BASELINE_COMMAND, +) -> dict[str, Any]: + if set(measured_fits) != set(MEASURED_JOINTS): + raise ValueError("measured_fits must contain all directly measured joints") + expected_projected = { + name for name, spec in JOINT_SPECS.items() + if spec.zero_kind == "projected" + } + if set(projected_zeros_rad) != expected_projected: + raise ValueError("projected_zeros_rad has the wrong joint set") + if not 0 <= int(splay_zero_command_u8) <= 255: + raise ValueError("splay zero command must be in [0, 255]") + + joints: dict[str, dict[str, Any]] = {} + for name, spec in JOINT_SPECS.items(): + source_name = spec.source_joint or name + fit = measured_fits[source_name] + joint: dict[str, Any] = { + "motor_index": int(spec.motor_index), + "angle_rad": [round(float(value), 8) for value in fit.angle_rad], + } + if spec.active: + joint["zero_command_u8"] = ( + int(splay_zero_command_u8) + if name in SPLAY_JOINTS + else 255 + ) + else: + joint["passive"] = True + if spec.source_joint is not None: + joint["source_joint"] = spec.source_joint + elif spec.zero_kind == "projected": + joint["zero_angles"] = { + "table_projected_zero_rad": round( + float(projected_zeros_rad[name]), 8 + ) + } + elif spec.zero_kind == "travel_midpoint": + joint["zero_angles"] = { + "travel_midpoint_rad": round(float(splay_midpoint_rad), 8) + } + joints[name] = joint + + errors = np.abs(np.asarray(validation_errors_rad, dtype=float)) + mae = float(np.mean(errors)) if errors.size else float("nan") + p95 = float(np.percentile(errors, 95.0)) if errors.size else float("nan") + payload = { + "schema_version": 3, + "model": "G20", + "side": "left", + "serial_number": str(serial_number), + "angle_unit": "rad", + "command_range": [0, 255], + "baseline_command_u8": [int(value) for value in baseline], + "joints": joints, + "quality": { + "passed": bool(passed), + "validation_mae_rad": ( + None if not math.isfinite(mae) else round(mae, 8) + ), + "validation_p95_rad": ( + None if not math.isfinite(p95) else round(p95, 8) + ), + }, + } + validate_compact_payload(payload) + return payload + + +def validate_compact_payload(payload: Mapping[str, Any]) -> None: + expected_top = { + "schema_version", "model", "side", "serial_number", "angle_unit", + "command_range", "baseline_command_u8", "joints", "quality", + } + if set(payload) != expected_top: + raise ValueError("compact calibration has unexpected top-level fields") + if payload["schema_version"] != 3: + raise ValueError("schema_version must be 3") + if payload["model"] != "G20" or payload["side"] != "left": + raise ValueError("payload must describe a left G20") + if payload["angle_unit"] != "rad" or payload["command_range"] != [0, 255]: + raise ValueError("payload angle or command units are invalid") + baseline = payload["baseline_command_u8"] + if not isinstance(baseline, list) or len(baseline) != 20: + raise ValueError("baseline_command_u8 must contain 20 values") + joints = payload["joints"] + if not isinstance(joints, Mapping) or set(joints) != set(JOINT_SPECS): + raise ValueError("payload must contain exactly 21 G20 joints") + + for name, spec in JOINT_SPECS.items(): + joint = joints[name] + allowed = {"motor_index", "angle_rad"} + allowed.add("zero_command_u8" if spec.active else "passive") + if spec.source_joint is not None: + allowed.add("source_joint") + elif spec.zero_kind in {"projected", "travel_midpoint"}: + allowed.add("zero_angles") + if set(joint) != allowed: + raise ValueError(f"{name} has unexpected fields") + if int(joint["motor_index"]) != spec.motor_index: + raise ValueError(f"{name} has the wrong motor_index") + curve = np.asarray(joint["angle_rad"], dtype=float) + if curve.shape != (256,) or not np.all(np.isfinite(curve)): + raise ValueError(f"{name}.angle_rad must contain 256 finite values") + if np.any(np.diff(curve) > 1.0e-7): + raise ValueError(f"{name}.angle_rad must be non-increasing") + if spec.active: + zero = joint["zero_command_u8"] + if not isinstance(zero, int) or not 0 <= zero <= 255: + raise ValueError(f"{name}.zero_command_u8 is invalid") + elif joint.get("passive") is not True: + raise ValueError(f"{name} must be marked passive") + if spec.source_joint is not None: + if joint.get("source_joint") != spec.source_joint: + raise ValueError(f"{name} has the wrong source_joint") + source = joints[spec.source_joint] + if joint["angle_rad"] != source["angle_rad"]: + raise ValueError(f"{name} must copy its source curve exactly") + if spec.active and joint["zero_command_u8"] != source["zero_command_u8"]: + raise ValueError(f"{name} must copy its source zero command") + + index_roll = np.asarray(joints["index_mcp_roll"]["angle_rad"], dtype=float) + if not index_roll[0] > 0.0 or not index_roll[255] < 0.0: + raise ValueError("index_mcp_roll endpoints must be positive then negative") + if abs(float(index_roll[0] + index_roll[255])) > 1.0e-7: + raise ValueError("index_mcp_roll endpoints must be symmetric") + for name, spec in JOINT_SPECS.items(): + if name not in SPLAY_JOINTS and spec.source_joint != "index_mcp_roll": + curve = np.asarray(joints[name]["angle_rad"], dtype=float) + if abs(float(curve[255])) > 1.0e-6: + raise ValueError(f"{name}.angle_rad[255] must be zero") + + quality = payload["quality"] + if set(quality) != { + "passed", "validation_mae_rad", "validation_p95_rad" + }: + raise ValueError("quality must contain only compact summary fields") diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/hikrobot_camera.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/hikrobot_camera.py new file mode 100644 index 0000000..f2a6694 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/hikrobot_camera.py @@ -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() diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/node.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/node.py index cda9daa..5ecc605 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/node.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/node.py @@ -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 diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py new file mode 100644 index 0000000..2869978 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_diagnostics.py @@ -0,0 +1,306 @@ +"""Chinese, operator-facing diagnostics for three-camera calibration.""" + +from __future__ import annotations + +from typing import Any, Mapping + + +STATE_NAMES_ZH = { + "PREFLIGHT": "设备和标签预检", + "WAIT_START": "等待开始标定", + "RETURN_BASELINE": "正在返回基准姿态", + "PREPARE_SWEEP": "正在到达扫描起点", + "SWEEP": "正在采集轨迹", + "FITTING": "正在拟合轨迹和零位", + "VALIDATION_MOVE": "正在移动到随机复测位置", + "VALIDATION_CAPTURE": "正在采集随机复测数据", + "PAUSED": "标定已暂停", + "ABORTED": "标定已终止", + "COMPLETE": "标定已完成", +} + +VIEW_NAMES_ZH = { + "front": "正面", + "side": "侧面", + "top": "上面", +} + +JOINT_NAMES_ZH = { + "thumb_cmc_pitch": "拇指CMC俯仰", + "thumb_cmc_roll": "拇指CMC滚转", + "thumb_mcp": "拇指MCP", + "thumb_ip": "拇指IP（被动）", + "index_mcp_roll": "食指MCP侧摆", + "index_mcp_pitch": "食指MCP屈伸", + "index_pip": "食指PIP", + "index_dip": "食指DIP（被动）", + "thumb_cmc_yaw": "拇指CMC侧摆", +} + + +def _format_u8(value: Any) -> str: + if value is None: + return "尚无反馈" + return f"{float(value):.1f}" + + +def _task_text(active: Mapping[str, Any]) -> str: + if not active: + return "尚无活动任务" + view = VIEW_NAMES_ZH.get(str(active.get("view", "")), str(active.get("view", ""))) + if active.get("kind") == "fit_failure": + joints = active.get("joints", []) + joint_text = "/".join( + JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints + ) + return ( + f"{view}机位，{joint_text}拟合检查失败，" + f"电机{active.get('motor_index')}，" + f"第{active.get('attempt', 1)}次尝试" + ) + if active.get("kind") == "validation": + return ( + f"{view}机位，随机复测，电机{active.get('motor_index')}，" + f"目标命令{active.get('command_u8')}" + ) + joints = active.get("joints", []) + joint_text = "/".join( + JOINT_NAMES_ZH.get(str(joint), str(joint)) for joint in joints + ) + start = active.get("start_u8") + target = active.get("target_u8") + cycle = active.get("cycle", "?") + repetitions = active.get("repetitions", "?") + return ( + f"{view}机位，{joint_text}，电机{active.get('motor_index')}，" + f"第{cycle}/{repetitions}轮，{start}→{target}" + ) + + +def three_camera_reason_zh( + state: str, + reason: str, + active: Mapping[str, Any], +) -> tuple[str, str]: + """Translate a reason code and provide one concrete operator action.""" + reason = str(reason) + sample = active.get("sample", {}) if active else {} + missing = [int(value) for value in sample.get("missing_endpoint_u8", [])] + sample_range = ( + f"{_format_u8(sample.get('minimum_u8'))}～" + f"{_format_u8(sample.get('maximum_u8'))}" + ) + tolerance = sample.get("endpoint_tolerance_u8", "?") + + if reason == "sweep_missing_endpoint_bin": + missing_text = "、".join(str(value) for value in missing) or "0或255" + return ( + f"本方向已有{active.get('valid_frames', 0)}帧同步有效数据，但缺少" + f"电机端点{missing_text}附近的有效分箱；采样到的实际电机范围为" + f"{sample_range}，端点容差为±{tolerance}。这通常表示电机虽然运动到" + "端点，但该时刻没有同时取得有效Tag图像和电机状态。", + "确认当前机位所需Tag在整个行程（尤其缺失端点）均可见，然后调用" + "/g20_calibration/resume；程序会重新扫描当前方向，不要调用start。", + ) + if reason == "sweep_bins_too_few": + return ( + f"有效电机分箱只有{sample.get('bin_count', 0)}个，要求至少" + f"{sample.get('minimum_bin_count', '?')}个；当前采样范围{sample_range}。", + "检查Tag连续识别和电机状态频率，修正后调用resume重新扫描当前方向。", + ) + if reason == "sweep_bin_gap_too_large": + return ( + f"轨迹相邻有效电机分箱的最大空缺为{sample.get('maximum_bin_gap', '?')}，" + f"允许值不超过{sample.get('allowed_maximum_bin_gap', '?')}。", + "检查运动中Tag是否间歇丢失；修正遮挡、反光或对焦后调用resume。", + ) + if reason == "synchronised_tag_state_timeout": + return ( + "运动过程中连续超过允许时间没有取得“所需Tag全部有效且能与电机状态" + "按时间戳配对”的图像帧。", + "查看下面活动机位的缺失Tag，确认状态话题仍在更新；修正后调用resume，" + "程序会重扫当前方向。", + ) + if reason == "sweep_start_position_timeout": + return ( + f"电机{active.get('motor_index')}未在规定时间到达扫描起点" + f"{active.get('start_u8')}，当前实际值{_format_u8(active.get('actual_u8'))}。", + "检查CAN、机械手使能和是否存在机械卡阻，确认安全后调用resume。", + ) + if reason == "sweep_timeout": + return ( + "当前方向在规定时间内未完成端点到达、有效帧数和行程覆盖要求。", + "检查电机实际值、Tag连续识别和标定速度，修正后调用resume。", + ) + if reason == "return_baseline_timeout": + return ( + "一个或多个标定电机未在规定时间返回基准命令。", + "检查机械手状态、CAN和机械卡阻，确认安全后调用resume。", + ) + if reason == "validation_move_timeout": + return ( + "随机复测时电机未在规定时间到达目标命令。", + "检查机械手状态和机械卡阻，确认安全后调用resume。", + ) + if reason == "validation_capture_timeout": + return ( + "随机复测位置没有采集到足够的同步有效Tag帧。", + "检查当前机位Tag可见性后调用resume。", + ) + if reason == "joint_fit_check_failed": + metric_names = { + "plane_rms_mm": "平面拟合RMS", + "radial_rms_mm": "圆半径拟合RMS", + "radius_mm": "拟合半径", + "image_radial_rms_px": "二维圆半径拟合RMS", + "image_radial_p95_px": "二维圆半径误差P95", + "image_radius_px": "二维拟合半径", + "arc_deg": "实测圆弧", + "monotonic_correction_deg": "最大单调修正", + "hysteresis_deg": "最大正反程差", + "cycle_travel_range_deg": "三轮行程差", + } + metric_units = { + "plane_rms_mm": "mm", + "radial_rms_mm": "mm", + "radius_mm": "mm", + "image_radial_rms_px": "px", + "image_radial_p95_px": "px", + "image_radius_px": "px", + "arc_deg": "°", + "monotonic_correction_deg": "°", + "hysteresis_deg": "°", + "cycle_travel_range_deg": "°", + } + details: list[str] = [] + for failure in active.get("failures", []): + joint = JOINT_NAMES_ZH.get( + str(failure.get("joint")), str(failure.get("joint")) + ) + metric = str(failure.get("metric", "")) + if metric in metric_names: + comparison = str(failure.get("comparison", "")) + requirement = "不超过" if comparison == "maximum" else "至少" + unit = metric_units[metric] + detail = ( + f"{joint}的{metric_names[metric]}为" + f"{float(failure.get('actual', 0.0)):.2f}{unit}，" + f"要求{requirement}{float(failure.get('limit', 0.0)):.2f}{unit}" + ) + cycle_travel = failure.get("cycle_travel_deg", []) + if cycle_travel: + detail += "（三轮=" + "/".join( + f"{float(value):.2f}°" for value in cycle_travel + ) + "）" + details.append(detail) + else: + cycle = failure.get("cycle") + cycle_text = "" if cycle is None else f"第{cycle}轮" + details.append( + f"{joint}的{cycle_text}{metric or '轨迹'}拟合失败：" + f"{failure.get('reason', '未知原因')}" + ) + detail_text = "；".join(details) or "当前关节的轨迹拟合未通过" + return ( + detail_text + "。程序已在当前关节结束后立即停止后续步骤。", + "修正Tag位置、遮挡或机械行程后调用" + "/g20_calibration/resume；程序只清除当前失败关节的数据" + f"并重扫{active.get('directions_to_rescan', 6)}个方向，不要调用start。", + ) + if reason in {"waiting_for_three_cameras_tags_and_sdk", "preflight_lost"}: + return ( + "正在等待三台相机内参、帧率、全部必需Tag以及机械手SDK同时就绪。", + "根据下面每个机位的缺失Tag和有效率排查；全部就绪后程序会进入等待开始状态。", + ) + if reason == "call_start": + return ( + "三机位预检已经通过，等待操作员确认开始。", + "清空机械手运动范围后调用/g20_calibration/start。", + ) + if reason == "operator_pause": + return "操作员主动暂停了标定。", "确认安全后调用/g20_calibration/resume。" + if reason == "operator_abort": + return "操作员终止了本次标定，程序保持终止时的当前姿态。", "需要重新启动一次新标定。" + if reason == "collecting_timestamp_synchronised_tag_centres": + return "正在按时间戳配对Tag图像和电机状态并采集当前轨迹。", "无需操作，保持相机、标签和底座不动。" + if reason == "capturing_random_validation_pose": + return "正在当前随机命令位置采集复测数据。", "无需操作，保持设备不动。" + if reason in {"calibration_passed", "calibration_complete"}: + return "轨迹、零位和随机复测已经完成。", "检查结果路径和quality.passed。" + if reason == "quality_failed": + return "标定流程完成，但拟合或随机复测质量没有达到验收阈值。", "检查最终JSON的quality以及启动终端中的拟合日志。" + if reason.startswith("prepare_") or state == "PREPARE_SWEEP": + return "正在把当前电机移动到本方向的扫描起点并等待稳定。", "无需操作。" + if state == "RETURN_BASELINE": + return "正在把已使用的标定电机恢复到统一基准命令。", "无需操作。" + if state == "FITTING": + return "所有扫描已经完成，正在拟合21个关节的轨迹和零位。", "无需操作。" + return f"未分类原因码：{reason}", "保留该原因码和启动终端日志用于进一步定位。" + + +def render_three_camera_status_text_zh(payload: Mapping[str, Any]) -> str: + """Render the complete operator status; the JSON topic remains unchanged.""" + state = str(payload.get("state", "")) + active = payload.get("active", {}) + reason_zh, action_zh = three_camera_reason_zh( + state, str(payload.get("reason", "")), active + ) + progress = float(payload.get("progress", 0.0)) + completed = payload.get("completed_sweeps", 0) + total = payload.get("total_sweeps", 0) + lines = [ + f"状态：{STATE_NAMES_ZH.get(state, state)}（{state}）", + f"原因：{reason_zh}", + f"建议：{action_zh}", + f"进度：{progress:.1%}（已完成{completed}/{total}个扫描方向）", + f"当前任务：{_task_text(active)}", + ] + if state == "RETURN_BASELINE": + lines.append( + f"正在确认基准姿态：{payload.get('baseline_command_u8', [])}" + ) + if active and active.get("kind") != "fit_failure": + sample = active.get("sample", {}) + motion_progress = active.get("motion_progress") + motion_text = ( + "未知" if motion_progress is None else f"{float(motion_progress):.1%}" + ) + lines.append( + "运动采样：" + f"目标{active.get('target_u8', active.get('command_u8', '?'))}，" + f"实际{_format_u8(active.get('actual_u8'))}，" + f"本方向{motion_text}，有效帧{active.get('valid_frames', 0)}，" + f"实际采样范围{_format_u8(sample.get('minimum_u8'))}～" + f"{_format_u8(sample.get('maximum_u8'))}" + ) + auxiliary = active.get("auxiliary_motors", []) + if auxiliary: + lines.append( + "避挡姿态：" + + "，".join( + f"电机{item.get('motor_index')}目标" + f"{item.get('command_u8')}、实际" + f"{_format_u8(item.get('actual_u8'))}" + for item in auxiliary + ) + ) + speed = active.get("speed", {}) + if speed: + lines.append( + "阶段速度：五指目标" + f"{speed.get('commanded_finger_speed')}，SDK报告" + f"{speed.get('reported_finger_speed')}" + ) + lines.append("机位：") + for name, view in payload.get("views", {}).items(): + missing = view.get("missing_tag_ids", []) + missing_text = "无" if not missing else ",".join(map(str, missing)) + lines.append( + f"- {VIEW_NAMES_ZH.get(str(name), str(name))}：" + f"{'就绪' if view.get('ready') else '等待'}，" + f"{float(view.get('detection_hz', 0.0)):.1f}Hz，" + f"全部必需Tag同时有效率{float(view.get('valid_rate', 0.0)):.1%}，" + f"当前缺失Tag={missing_text}" + ) + lines.append(f"结果：{payload.get('result_path') or '尚未生成'}") + return "\n".join(lines) diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_node.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_node.py new file mode 100644 index 0000000..6e8acee --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/three_camera_node.py @@ -0,0 +1,1944 @@ +"""One-command three-camera calibration for the complete left G20 hand.""" + +from __future__ import annotations + +from collections import deque +from dataclasses import dataclass, field +import json +import math +from pathlib import Path +import random +import re +import time +from typing import Any, Mapping + +import numpy as np +import rclpy +from apriltag_msgs.msg import AprilTagDetectionArray +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data +from scipy.spatial.transform import Rotation +from sensor_msgs.msg import CameraInfo, JointState +from std_msgs.msg import String +from std_srvs.srv import Trigger + +from .acquisition import ( + StateSample, + TagQuality, + interpolate_state_u8, + required_resume_views, + tag_quality_is_valid, + update_pnp_reset_watchdog, +) +from .core import ( + BASELINE_COMMAND, + COMMAND_NAMES, + DIRECTION_DECREASING, + DIRECTION_INCREASING, +) +from .full_hand import ( + IMAGE_TRAJECTORY_JOINTS, + JOINT_SPECS, + MEASURED_JOINTS, + SWEEP_SPECS, + VIEW_TAGS, + JointCurveFit, + SweepSpec, + build_calibration_motion_command, + build_calibration_speed_profile, + build_compact_payload, + calibration_auxiliary_commands, + center_splay_curve, + fit_measured_joint_curve, + fit_projected_zero, + measure_joint_observation, +) +from .hikrobot_camera import configure_fastdds_large_image_transport +from .pnp import ( + SquareTagGroupPoseTracker, + SquareTagPose, + SquareTagPoseTracker, +) +from .storage import append_jsonl, atomic_write_json +from .three_camera_diagnostics import render_three_camera_status_text_zh + + +STATE_PREFLIGHT = "PREFLIGHT" +STATE_WAIT_START = "WAIT_START" +STATE_RETURN_BASELINE = "RETURN_BASELINE" +STATE_PREPARE_SWEEP = "PREPARE_SWEEP" +STATE_SWEEP = "SWEEP" +STATE_FITTING = "FITTING" +STATE_VALIDATION_MOVE = "VALIDATION_MOVE" +STATE_VALIDATION_CAPTURE = "VALIDATION_CAPTURE" +STATE_PAUSED = "PAUSED" +STATE_ABORTED = "ABORTED" +STATE_COMPLETE = "COMPLETE" + + +def _stamp_ns(stamp: Any) -> int: + return int(stamp.sec) * 1_000_000_000 + int(stamp.nanosec) + + +def _safe_name(value: str) -> str: + safe = re.sub(r"[^A-Za-z0-9_.-]+", "_", str(value).strip()) + return safe or "UNSET" + + +def _view_pairs(view: str) -> tuple[tuple[str, str], ...]: + if view == "front": + return ( + ("front_base", "thumb_cmc"), + ("thumb_cmc", "thumb_mcp"), + ("thumb_mcp", "thumb_ip"), + ("front_base", "index_roll"), + ) + if view == "side": + return ( + ("side_base", "index_mcp"), + ("index_mcp", "index_pip"), + ("index_pip", "index_dip"), + ) + if view == "top": + return (("top_base", "thumb_yaw"),) + raise ValueError(f"unknown view {view}") + + +@dataclass(frozen=True) +class FrameObservation: + stamp_ns: int + received_at: float + view: str + state_u8: tuple[float, ...] + state_sync_error_ns: int + joint_vectors_xyz_m: Mapping[str, tuple[float, float, float]] + image_vectors_xy_px: Mapping[str, tuple[float, float]] + + +@dataclass(frozen=True) +class SweepItem: + spec: SweepSpec + cycle: int + direction: str + + @property + def start_u8(self) -> int: + return 255 if self.direction == DIRECTION_DECREASING else 0 + + @property + def target_u8(self) -> int: + return 0 if self.direction == DIRECTION_DECREASING else 255 + + +@dataclass(frozen=True) +class ValidationItem: + spec: SweepSpec + command_u8: int + + +@dataclass +class ViewRuntime: + name: str + tag_size_m: float + preflight_frames: int + tracker: SquareTagPoseTracker + group_tracker: SquareTagGroupPoseTracker + role_by_id: dict[int, str] = field(default_factory=dict) + camera_matrix: np.ndarray | None = None + camera_info_valid: bool = False + camera_frame: str = "" + valid_flags: deque[bool] = field(default_factory=deque) + detection_times: deque[float] = field(default_factory=deque) + latest_tag_quality: dict[str, TagQuality] = field(default_factory=dict) + latest_pnp_rejections: dict[str, str] = field(default_factory=dict) + latest_group_pnp_reason: str = "" + pnp_invalid_since: float | None = None + pnp_reset_count: int = 0 + last_message_at: float = 0.0 + last_valid_at: float = 0.0 + + def __post_init__(self) -> None: + self.role_by_id = { + tag_id: role for role, tag_id in VIEW_TAGS[self.name].items() + } + self.valid_flags = deque(maxlen=self.preflight_frames) + self.detection_times = deque(maxlen=self.preflight_frames) + + @property + def roles(self) -> tuple[str, ...]: + return tuple(VIEW_TAGS[self.name]) + + @property + def valid_rate(self) -> float: + if not self.valid_flags: + return 0.0 + return float(sum(self.valid_flags) / len(self.valid_flags)) + + @property + def detection_hz(self) -> float: + if len(self.detection_times) < 2: + return 0.0 + elapsed = self.detection_times[-1] - self.detection_times[0] + return 0.0 if elapsed <= 0.0 else (len(self.detection_times) - 1) / elapsed + + +class G20ThreeCameraCalibrationNode(Node): + """Own all hand commands while observing three fixed Hikrobot views.""" + + def __init__(self) -> None: + super().__init__("g20_calibration") + self._declare_parameters() + self._load_parameters() + + self.session_dir.mkdir(parents=True, exist_ok=True) + self.raw_path = self.session_dir / "raw_samples.jsonl" + self.final_path = self.session_dir / ( + f"g20_left_{_safe_name(self.serial_number)}_calibration.json" + ) + + self.views = { + name: self._make_view_runtime(name) + for name in ("front", "side", "top") + } + self.latest_state_u8: tuple[float, ...] = () + self.state_history: deque[StateSample] = deque(maxlen=1200) + self.latest_hand_info: dict[str, Any] = {} + self.commanded_speed_profile: tuple[int, ...] = () + self.speed_commanded_at = 0.0 + + self.state = STATE_PREFLIGHT + self.reason = "waiting_for_three_cameras_tags_and_sdk" + self.paused_reason = "" + self.started = False + self.baseline_after = "" + self.position_hold_since: float | None = None + self.motion_stage_started_at = 0.0 + + self.sweep_items: list[SweepItem] = [] + self.sweep_index = 0 + self.active_sweep: SweepItem | None = None + self.sweep_frames: list[FrameObservation] = [] + self.sweep_started_at = 0.0 + self.sweep_last_valid_at = 0.0 + self.sweep_endpoint_since: float | None = None + self.retry_sweep_spec: SweepSpec | None = None + self.fit_failure: dict[str, Any] = {} + self.sweep_attempts: dict[int, int] = { + spec.motor_index: 1 for spec in SWEEP_SPECS + } + self.records_by_joint: dict[str, list[dict[str, Any]]] = { + name: [] for name in MEASURED_JOINTS + } + + self.measured_fits: dict[str, JointCurveFit] = {} + self.projected_zeros_rad: dict[str, float] = {} + self.splay_zero_command_u8 = 0 + self.splay_midpoint_rad = 0.0 + self.fit_quality_passed = False + + self.validation_items: list[ValidationItem] = [] + self.validation_index = 0 + self.active_validation: ValidationItem | None = None + self.validation_frames_buffer: list[FrameObservation] = [] + self.validation_errors_rad: list[float] = [] + self.validation_stage_started_at = 0.0 + self.completed_payload: dict[str, Any] | None = None + + self.command_publisher = self.create_publisher( + JointState, self.command_topic, 1 + ) + self.setting_publisher = self.create_publisher( + String, self.setting_topic, 10 + ) + self.status_publisher = self.create_publisher(String, "~/status", 10) + self.status_text_publisher = self.create_publisher( + String, "~/status_text", 10 + ) + self.create_subscription( + JointState, self.state_topic, self._state_callback, 10 + ) + self.create_subscription( + String, self.info_topic, self._info_callback, 10 + ) + for name, runtime in self.views.items(): + del runtime + self.create_subscription( + CameraInfo, + self.camera_info_topics[name], + lambda message, view=name: self._camera_info_callback( + view, message + ), + qos_profile_sensor_data, + ) + self.create_subscription( + AprilTagDetectionArray, + self.detections_topics[name], + lambda message, view=name: self._detections_callback( + view, message + ), + qos_profile_sensor_data, + ) + + self.create_service(Trigger, "~/start", self._start_callback) + self.create_service(Trigger, "~/pause", self._pause_callback) + self.create_service(Trigger, "~/resume", self._resume_callback) + self.create_service(Trigger, "~/abort", self._abort_callback) + self.last_status_publish = 0.0 + self.timer = self.create_timer(0.05, self._timer_callback) + self.get_logger().info( + f"Three-camera calibration session: {self.session_dir}" + ) + + def _declare_parameters(self) -> None: + self.declare_parameter("serial_number", "UNSET") + self.declare_parameter("session_dir", "calibration_output/session") + self.declare_parameter("commands_enabled", True) + self.declare_parameter("command_topic", "/g20/cb_left_hand_control_cmd") + self.declare_parameter("state_topic", "/g20/cb_left_hand_state") + self.declare_parameter("info_topic", "/g20/cb_left_hand_info") + self.declare_parameter( + "setting_topic", "/g20/cb_hand_setting_cmd" + ) + for view in ("front", "side", "top"): + self.declare_parameter( + f"{view}_camera_info_topic", + f"/g20_calibration/{view}/camera/camera_info", + ) + self.declare_parameter( + f"{view}_detections_topic", + f"/g20_calibration/{view}/apriltag/detections", + ) + self.declare_parameter("tag_size_m", 0.010) + self.declare_parameter("baseline_command_u8", list(BASELINE_COMMAND)) + self.declare_parameter("normal_calibration_speed", 15) + self.declare_parameter("index_roll_calibration_speed", 5) + self.declare_parameter("index_flex_calibration_speed", 10) + self.declare_parameter("speed_setting_settle_seconds", 0.25) + self.declare_parameter("repetitions", 3) + self.declare_parameter("preflight_frames", 60) + self.declare_parameter("minimum_detection_rate", 0.95) + self.declare_parameter("minimum_detection_hz", 15.0) + self.declare_parameter("maximum_hamming", 0) + self.declare_parameter("minimum_decision_margin", 30.0) + self.declare_parameter("minimum_edge_pixels", 30.0) + self.declare_parameter("pnp_maximum_reprojection_error_px", 1.5) + self.declare_parameter("pnp_reprojection_tie_px", 1.5) + self.declare_parameter("pnp_maximum_pose_jump_deg", 35.0) + self.declare_parameter("pnp_maximum_translation_jump_m", 0.04) + self.declare_parameter("pnp_maximum_tag_tilt_deg", 75.0) + self.declare_parameter("pnp_tracker_reset_seconds", 5.0) + self.declare_parameter("top_pnp_invalid_reset_seconds", 1.0) + self.declare_parameter("maximum_state_image_skew_ms", 150.0) + self.declare_parameter("endpoint_tolerance_u8", 2.0) + self.declare_parameter("endpoint_hold_seconds", 0.5) + self.declare_parameter("baseline_hold_seconds", 0.5) + self.declare_parameter("position_timeout_seconds", 30.0) + self.declare_parameter("sweep_timeout_seconds", 90.0) + self.declare_parameter("invalid_timeout_seconds", 3.0) + self.declare_parameter("minimum_sweep_frames", 40) + self.declare_parameter("minimum_state_span_u8", 240.0) + self.declare_parameter("minimum_sweep_bins", 32) + self.declare_parameter("maximum_bin_gap", 16) + self.declare_parameter("trajectory_maximum_plane_rms_m", 0.004) + self.declare_parameter("trajectory_maximum_radial_rms_m", 0.004) + self.declare_parameter("trajectory_minimum_radius_m", 0.003) + self.declare_parameter("trajectory_minimum_arc_deg", 15.0) + self.declare_parameter("image_trajectory_maximum_radial_rms_px", 2.0) + self.declare_parameter("image_trajectory_maximum_radial_p95_px", 3.5) + self.declare_parameter("image_trajectory_minimum_radius_px", 20.0) + self.declare_parameter( + "trajectory_maximum_cycle_travel_difference_deg", 3.0 + ) + self.declare_parameter( + "passive_maximum_cycle_travel_difference_deg", 10.0 + ) + self.declare_parameter("zero_minimum_radius_px", 20.0) + self.declare_parameter("zero_maximum_radial_rms_px", 2.0) + self.declare_parameter("zero_maximum_radial_p95_px", 3.5) + self.declare_parameter("zero_maximum_round_difference_deg", 1.0) + self.declare_parameter("maximum_monotonic_correction_deg", 2.0) + self.declare_parameter("maximum_hysteresis_deg", 5.0) + self.declare_parameter("passive_maximum_monotonic_correction_deg", 3.0) + self.declare_parameter("passive_maximum_hysteresis_deg", 7.5) + self.declare_parameter("validation_enabled", False) + self.declare_parameter("validation_command_count", 3) + self.declare_parameter("validation_frames", 10) + self.declare_parameter("validation_seed", 20260804) + self.declare_parameter("validation_timeout_seconds", 20.0) + self.declare_parameter("maximum_validation_mae_deg", 2.0) + self.declare_parameter("maximum_validation_p95_deg", 3.0) + + def _load_parameters(self) -> None: + def value(name: str) -> Any: + return self.get_parameter(name).value + + self.serial_number = str(value("serial_number")) + if self.serial_number == "UNSET": + raise ValueError("serial_number is required") + self.session_dir = Path(str(value("session_dir"))).expanduser().resolve() + self.commands_enabled = bool(value("commands_enabled")) + self.command_topic = str(value("command_topic")) + self.state_topic = str(value("state_topic")) + self.info_topic = str(value("info_topic")) + self.setting_topic = str(value("setting_topic")) + self.camera_info_topics = { + view: str(value(f"{view}_camera_info_topic")) + for view in ("front", "side", "top") + } + self.detections_topics = { + view: str(value(f"{view}_detections_topic")) + for view in ("front", "side", "top") + } + self.tag_size_m = float(value("tag_size_m")) + self.baseline_command = tuple( + int(item) for item in value("baseline_command_u8") + ) + self.normal_calibration_speed = int(value("normal_calibration_speed")) + self.index_roll_calibration_speed = int( + value("index_roll_calibration_speed") + ) + self.index_flex_calibration_speed = int( + value("index_flex_calibration_speed") + ) + self.speed_setting_settle_seconds = float( + value("speed_setting_settle_seconds") + ) + self.repetitions = int(value("repetitions")) + self.preflight_frames = int(value("preflight_frames")) + self.minimum_detection_rate = float(value("minimum_detection_rate")) + self.minimum_detection_hz = float(value("minimum_detection_hz")) + 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.pnp_maximum_reprojection_error_px = float( + value("pnp_maximum_reprojection_error_px") + ) + self.pnp_reprojection_tie_px = float(value("pnp_reprojection_tie_px")) + self.pnp_maximum_pose_jump_rad = math.radians( + float(value("pnp_maximum_pose_jump_deg")) + ) + self.pnp_maximum_translation_jump_m = float( + value("pnp_maximum_translation_jump_m") + ) + self.pnp_maximum_tag_tilt_rad = math.radians( + float(value("pnp_maximum_tag_tilt_deg")) + ) + self.pnp_tracker_reset_seconds = float(value("pnp_tracker_reset_seconds")) + self.top_pnp_invalid_reset_seconds = float( + value("top_pnp_invalid_reset_seconds") + ) + self.maximum_state_image_skew_ns = int( + float(value("maximum_state_image_skew_ms")) * 1_000_000.0 + ) + self.endpoint_tolerance_u8 = float(value("endpoint_tolerance_u8")) + self.endpoint_hold_seconds = float(value("endpoint_hold_seconds")) + self.baseline_hold_seconds = float(value("baseline_hold_seconds")) + self.position_timeout_seconds = float(value("position_timeout_seconds")) + self.sweep_timeout_seconds = float(value("sweep_timeout_seconds")) + self.invalid_timeout_seconds = float(value("invalid_timeout_seconds")) + self.minimum_sweep_frames = int(value("minimum_sweep_frames")) + self.minimum_state_span_u8 = float(value("minimum_state_span_u8")) + self.minimum_sweep_bins = int(value("minimum_sweep_bins")) + self.maximum_bin_gap = int(value("maximum_bin_gap")) + self.trajectory_maximum_plane_rms_m = float( + value("trajectory_maximum_plane_rms_m") + ) + self.trajectory_maximum_radial_rms_m = float( + value("trajectory_maximum_radial_rms_m") + ) + self.trajectory_minimum_radius_m = float( + value("trajectory_minimum_radius_m") + ) + self.trajectory_minimum_arc_rad = math.radians( + float(value("trajectory_minimum_arc_deg")) + ) + self.image_trajectory_maximum_radial_rms_px = float( + value("image_trajectory_maximum_radial_rms_px") + ) + self.image_trajectory_maximum_radial_p95_px = float( + value("image_trajectory_maximum_radial_p95_px") + ) + self.image_trajectory_minimum_radius_px = float( + value("image_trajectory_minimum_radius_px") + ) + self.trajectory_maximum_cycle_travel_difference_rad = math.radians( + float(value("trajectory_maximum_cycle_travel_difference_deg")) + ) + self.passive_maximum_cycle_travel_difference_rad = math.radians( + float(value("passive_maximum_cycle_travel_difference_deg")) + ) + self.zero_minimum_radius_px = float(value("zero_minimum_radius_px")) + self.zero_maximum_radial_rms_px = float( + value("zero_maximum_radial_rms_px") + ) + self.zero_maximum_radial_p95_px = float( + value("zero_maximum_radial_p95_px") + ) + self.zero_maximum_round_difference_rad = math.radians( + float(value("zero_maximum_round_difference_deg")) + ) + self.maximum_monotonic_correction_rad = math.radians( + float(value("maximum_monotonic_correction_deg")) + ) + self.maximum_hysteresis_rad = math.radians( + float(value("maximum_hysteresis_deg")) + ) + self.passive_maximum_monotonic_correction_rad = math.radians( + float(value("passive_maximum_monotonic_correction_deg")) + ) + self.passive_maximum_hysteresis_rad = math.radians( + float(value("passive_maximum_hysteresis_deg")) + ) + self.validation_enabled = bool(value("validation_enabled")) + self.validation_command_count = int(value("validation_command_count")) + self.validation_frames = int(value("validation_frames")) + self.validation_seed = int(value("validation_seed")) + self.validation_timeout_seconds = float( + value("validation_timeout_seconds") + ) + self.maximum_validation_mae_rad = math.radians( + float(value("maximum_validation_mae_deg")) + ) + self.maximum_validation_p95_rad = math.radians( + float(value("maximum_validation_p95_deg")) + ) + if len(self.baseline_command) != 20: + raise ValueError("baseline_command_u8 must contain exactly 20 values") + if any(value < 0 or value > 255 for value in self.baseline_command): + raise ValueError("baseline_command_u8 values must be in [0, 255]") + if not 0 <= self.normal_calibration_speed <= 255: + raise ValueError("normal_calibration_speed must be in [0, 255]") + if not 0 <= self.index_roll_calibration_speed <= 255: + raise ValueError("index_roll_calibration_speed must be in [0, 255]") + if not 0 <= self.index_flex_calibration_speed <= 255: + raise ValueError("index_flex_calibration_speed must be in [0, 255]") + if self.speed_setting_settle_seconds < 0.0: + raise ValueError("speed_setting_settle_seconds must be non-negative") + if self.top_pnp_invalid_reset_seconds <= 0.0: + raise ValueError("top_pnp_invalid_reset_seconds must be positive") + if not all( + value > 0.0 + for value in ( + self.image_trajectory_maximum_radial_rms_px, + self.image_trajectory_maximum_radial_p95_px, + self.image_trajectory_minimum_radius_px, + self.trajectory_maximum_cycle_travel_difference_rad, + self.passive_maximum_cycle_travel_difference_rad, + self.passive_maximum_monotonic_correction_rad, + self.passive_maximum_hysteresis_rad, + ) + ): + raise ValueError("trajectory quality thresholds must be positive") + if self.repetitions != 3: + raise ValueError("three-camera calibration requires exactly 3 repetitions") + if self.preflight_frames < 10 or self.minimum_sweep_bins < 3: + raise ValueError("preflight_frames or minimum_sweep_bins is too small") + if not 1 <= self.validation_command_count <= 10: + raise ValueError("validation_command_count must be in [1, 10]") + if self.validation_frames < 3: + raise ValueError("validation_frames must be at least 3") + + def _make_view_runtime(self, name: str) -> ViewRuntime: + tracker = SquareTagPoseTracker( + maximum_reprojection_error_px=self.pnp_maximum_reprojection_error_px, + reprojection_tie_px=self.pnp_reprojection_tie_px, + maximum_pose_jump_rad=self.pnp_maximum_pose_jump_rad, + maximum_translation_jump_m=self.pnp_maximum_translation_jump_m, + maximum_tag_tilt_rad=self.pnp_maximum_tag_tilt_rad, + reset_after_seconds=self.pnp_tracker_reset_seconds, + ) + group = SquareTagGroupPoseTracker( + roles=tuple(VIEW_TAGS[name]), + adjacent_pairs=_view_pairs(name), + maximum_pose_jump_rad=self.pnp_maximum_pose_jump_rad, + maximum_translation_jump_m=self.pnp_maximum_translation_jump_m, + relative_rotation_scale_rad=math.radians(5.0), + relative_translation_scale_m=0.01, + reprojection_scale_px=0.1, + reprojection_weight=0.05, + reset_after_seconds=self.pnp_tracker_reset_seconds, + ) + return ViewRuntime( + name=name, + tag_size_m=self.tag_size_m, + preflight_frames=self.preflight_frames, + tracker=tracker, + group_tracker=group, + ) + + def _camera_info_callback(self, view: str, message: CameraInfo) -> None: + runtime = self.views[view] + valid = ( + int(message.width) > 0 + and int(message.height) > 0 + and len(message.p) == 12 + and float(message.p[0]) > 0.0 + and float(message.p[5]) > 0.0 + ) + matrix = None + if valid: + matrix = np.asarray(message.p, dtype=float).reshape(3, 4)[:, :3] + valid = bool( + np.all(np.isfinite(matrix)) + and matrix[0, 0] > 0.0 + and matrix[1, 1] > 0.0 + ) + if ( + matrix is not None + and runtime.camera_matrix is not None + and not np.allclose(matrix, runtime.camera_matrix) + ): + runtime.tracker.reset() + runtime.group_tracker.reset() + runtime.camera_info_valid = valid + runtime.camera_matrix = matrix if valid else None + runtime.camera_frame = str(message.header.frame_id) + + def _state_callback(self, message: JointState) -> None: + if len(message.position) != 20: + return + if len(message.name) == 20 and set(message.name) == set(COMMAND_NAMES): + lookup = dict(zip(message.name, message.position)) + state = tuple(float(lookup[name]) for name in COMMAND_NAMES) + else: + state = tuple(float(value) for value in message.position) + stamp = _stamp_ns(message.header.stamp) + if stamp <= 0: + stamp = int(self.get_clock().now().nanoseconds) + self.latest_state_u8 = state + if not self.state_history or stamp > self.state_history[-1].stamp_ns: + self.state_history.append(StateSample(stamp, state)) + + def _info_callback(self, message: String) -> None: + try: + value = json.loads(message.data) + except json.JSONDecodeError: + return + if isinstance(value, dict): + self.latest_hand_info = value + + def _quality_valid(self, quality: TagQuality, *, include_pnp: bool) -> bool: + return tag_quality_is_valid( + quality, + maximum_hamming=self.maximum_hamming, + minimum_decision_margin=self.minimum_decision_margin, + minimum_edge_pixels=self.minimum_edge_pixels, + maximum_reprojection_error_px=( + self.pnp_maximum_reprojection_error_px if include_pnp else None + ), + ) + + def _detections_callback( + self, view: str, message: AprilTagDetectionArray + ) -> None: + runtime = self.views[view] + now = time.monotonic() + stamp = _stamp_ns(message.header.stamp) + qualities: dict[str, TagQuality] = {} + corners_by_role: dict[str, np.ndarray] = {} + centres_by_role: dict[str, np.ndarray] = {} + for detection in message.detections: + role = runtime.role_by_id.get(int(detection.id)) + if role is None: + continue + corners = np.asarray( + [[float(point.x), float(point.y)] for point in detection.corners], + dtype=float, + ) + if corners.shape != (4, 2): + continue + edges = np.linalg.norm(corners - np.roll(corners, -1, axis=0), axis=1) + qualities[role] = TagQuality( + hamming=int(detection.hamming), + decision_margin=float(detection.decision_margin), + edge_pixels=float(np.mean(edges)), + ) + corners_by_role[role] = corners + centres_by_role[role] = np.mean(corners, axis=0) + + detection_good = set(qualities) == set(runtime.roles) and all( + self._quality_valid(quality, include_pnp=False) + for quality in qualities.values() + ) + selected: dict[str, SquareTagPose] | None = None + pnp_rejections: dict[str, str] = {} + group_pnp_reason = "" + if detection_good and runtime.camera_matrix is not None: + candidates: dict[str, tuple[SquareTagPose, ...]] = {} + for role in runtime.roles: + _, rejection = runtime.tracker.estimate( + role, + corners_by_role[role], + tag_size_m=runtime.tag_size_m, + camera_matrix=runtime.camera_matrix, + stamp_ns=stamp, + ) + if rejection: + pnp_rejections[role] = rejection + candidates[role] = runtime.tracker.last_candidates_by_role.get( + role, () + ) + selected, group_pnp_reason = runtime.group_tracker.select( + candidates, stamp_ns=stamp + ) + if selected is not None: + for role, pose in selected.items(): + qualities[role] = TagQuality( + hamming=qualities[role].hamming, + decision_margin=qualities[role].decision_margin, + edge_pixels=qualities[role].edge_pixels, + reprojection_error_px=pose.reprojection_error_px, + ) + valid = bool( + selected is not None + and all(self._quality_valid(q, include_pnp=True) for q in qualities.values()) + ) + runtime.latest_pnp_rejections = pnp_rejections + runtime.latest_group_pnp_reason = group_pnp_reason + if view == "top": + runtime.pnp_invalid_since, reset_due = update_pnp_reset_watchdog( + detection_good=bool( + detection_good and runtime.camera_matrix is not None + ), + pnp_valid=valid, + now=now, + invalid_since=runtime.pnp_invalid_since, + reset_after_seconds=self.top_pnp_invalid_reset_seconds, + ) + if reset_due: + runtime.tracker.reset() + runtime.group_tracker.reset() + runtime.pnp_reset_count += 1 + self.get_logger().warning( + "Top-view PnP was continuously invalid for " + f"{self.top_pnp_invalid_reset_seconds:.1f}s; reset trackers " + f"(count={runtime.pnp_reset_count}, " + f"group_reason={group_pnp_reason or 'none'}, " + f"tag_rejections={pnp_rejections})" + ) + runtime.latest_tag_quality = qualities + runtime.last_message_at = now + runtime.detection_times.append(now) + runtime.valid_flags.append(valid) + if not valid or selected is None: + return + runtime.last_valid_at = now + + matched = interpolate_state_u8( + list(self.state_history), + stamp, + maximum_skew_ns=self.maximum_state_image_skew_ns, + ) + if matched is None: + return + state_u8, sync_error_ns = matched + joint_vectors: dict[str, tuple[float, float, float]] = {} + image_vectors: dict[str, tuple[float, float]] = {} + for name, spec in JOINT_SPECS.items(): + if not spec.measured or spec.view != view: + continue + assert spec.parent_role is not None and spec.child_role is not None + parent = selected[spec.parent_role] + child = selected[spec.child_role] + parent_rotation = Rotation.from_quat(parent.quaternion_xyzw) + relative = parent_rotation.inv().apply( + np.asarray(child.translation_xyz_m, dtype=float) + - np.asarray(parent.translation_xyz_m, dtype=float) + ) + image_relative = ( + centres_by_role[spec.child_role] + - centres_by_role[spec.parent_role] + ) + joint_vectors[name] = tuple(float(value) for value in relative) + image_vectors[name] = tuple(float(value) for value in image_relative) + observation = FrameObservation( + stamp_ns=stamp, + received_at=now, + view=view, + state_u8=state_u8, + state_sync_error_ns=int(sync_error_ns), + joint_vectors_xyz_m=joint_vectors, + image_vectors_xy_px=image_vectors, + ) + self._accept_frame(observation) + + def _accept_frame(self, observation: FrameObservation) -> None: + if self.state == STATE_SWEEP and self.active_sweep is not None: + if observation.view != self.active_sweep.spec.view: + return + motor = self.active_sweep.spec.motor_index + value = float(observation.state_u8[motor]) + if not -3.0 <= value <= 258.0: + return + if not self._motion_command_reached( + self.active_sweep.spec, + int(np.clip(np.rint(value), 0, 255)), + observation.state_u8, + ): + return + self.sweep_frames.append(observation) + self.sweep_last_valid_at = observation.received_at + return + if ( + self.state == STATE_VALIDATION_CAPTURE + and self.active_validation is not None + and observation.view == self.active_validation.spec.view + ): + if self._motion_command_reached( + self.active_validation.spec, + self.active_validation.command_u8, + observation.state_u8, + ): + self.validation_frames_buffer.append(observation) + + def _view_ready(self, runtime: ViewRuntime, now: float) -> bool: + minimum_frames = min(30, self.preflight_frames) + return bool( + runtime.camera_info_valid + and len(runtime.valid_flags) >= minimum_frames + and runtime.valid_rate >= self.minimum_detection_rate + and runtime.detection_hz >= self.minimum_detection_hz + and now - runtime.last_message_at <= 1.0 + ) + + def _all_preflight_ready(self, now: float) -> bool: + return bool( + len(self.latest_state_u8) == 20 + and all(self._view_ready(runtime, now) for runtime in self.views.values()) + ) + + def _active_view_for_resume(self) -> str | None: + if self.active_sweep is not None: + return self.active_sweep.spec.view + if self.active_validation is not None: + return self.active_validation.spec.view + if self.retry_sweep_spec is not None: + return self.retry_sweep_spec.view + return None + + def _resume_preflight_ready(self, now: float) -> bool: + if len(self.latest_state_u8) != 20: + return False + active_view = self._active_view_for_resume() + return all( + self._view_ready(self.views[view], now) + for view in required_resume_views(active_view) + ) + + def _prepare_failed_sweep_retry(self) -> SweepSpec: + """Discard only a failed spec's in-memory samples before its retry.""" + if self.retry_sweep_spec is None: + raise RuntimeError("no failed sweep is pending retry") + spec = self.retry_sweep_spec + for joint_name in spec.joints: + self.records_by_joint[joint_name].clear() + self.sweep_attempts[spec.motor_index] = ( + self.sweep_attempts.get(spec.motor_index, 1) + 1 + ) + append_jsonl( + self.raw_path, + { + "kind": "retry", + "view": spec.view, + "motor_index": spec.motor_index, + "joints": list(spec.joints), + "attempt": self.sweep_attempts[spec.motor_index], + "reason": self.paused_reason, + }, + ) + self.retry_sweep_spec = None + self.fit_failure = {} + return spec + + def _start_callback( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + del request + if self.state != STATE_WAIT_START: + response.success = False + response.message = f"not ready: state={self.state} reason={self.reason}" + return response + if not self.commands_enabled: + response.success = False + response.message = "commands_enabled=false; preview cannot move the hand" + return response + if self.command_publisher.get_subscription_count() < 1: + response.success = False + response.message = "hand SDK is not subscribed to the command topic" + return response + if self.setting_publisher.get_subscription_count() < 1: + response.success = False + response.message = "hand SDK is not subscribed to the setting topic" + return response + if len(self.get_publishers_info_by_topic(self.command_topic)) > 1: + response.success = False + response.message = "another node is publishing hand commands" + return response + self.started = True + self.sweep_items = [ + SweepItem(spec, cycle, direction) + for spec in SWEEP_SPECS + for cycle in range(self.repetitions) + for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING) + ] + self.sweep_index = 0 + self.retry_sweep_spec = None + self.fit_failure = {} + self.sweep_attempts = { + spec.motor_index: 1 for spec in SWEEP_SPECS + } + self.records_by_joint = {name: [] for name in MEASURED_JOINTS} + self._begin_return_baseline("next_sweep") + response.success = True + response.message = "three-camera calibration started" + return response + + def _pause_callback( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + del request + if self.state in {STATE_COMPLETE, STATE_ABORTED, STATE_PREFLIGHT, STATE_WAIT_START}: + response.success = False + response.message = f"cannot pause from {self.state}" + return response + self._pause("operator_pause") + response.success = True + response.message = "calibration paused and current pose held" + return response + + def _resume_callback( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + del request + if self.state != STATE_PAUSED: + response.success = False + response.message = "calibration is not paused" + return response + now = time.monotonic() + active_view = self._active_view_for_resume() + if not self._resume_preflight_ready(now): + response.success = False + required = ",".join(required_resume_views(active_view)) + response.message = ( + f"resume preflight is not ready; required_views={required}" + ) + return response + for view in required_resume_views(active_view): + runtime = self.views[view] + runtime.tracker.reset() + runtime.group_tracker.reset() + runtime.pnp_invalid_since = None + self.sweep_frames.clear() + self.validation_frames_buffer.clear() + if self.retry_sweep_spec is not None: + self._prepare_failed_sweep_retry() + self._begin_return_baseline("resume_sweep") + response.success = True + response.message = ( + "calibration resumed; only the failed six-direction joint " + "sweep will restart" + ) + return response + if self.active_sweep is not None and self.sweep_index < len(self.sweep_items): + self._begin_return_baseline("resume_sweep") + elif self.active_validation is not None: + self._begin_return_baseline("resume_validation") + else: + self._begin_return_baseline("next_sweep") + response.success = True + response.message = "calibration resumed; active step will restart" + return response + + def _abort_callback( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + del request + self._publish_speed_profile(self._normal_speed_profile()) + self._publish_hold_current() + self.state = STATE_ABORTED + self.reason = "operator_abort" + response.success = True + response.message = "calibration aborted and current pose held" + return response + + def _publish_command(self, values: list[int]) -> None: + if not self.commands_enabled: + return + message = JointState() + message.header.stamp = self.get_clock().now().to_msg() + message.name = list(COMMAND_NAMES) + message.position = [float(value) for value in values] + self.command_publisher.publish(message) + + def _publish_speed_profile(self, speeds: list[int]) -> None: + if not self.commands_enabled: + return + profile = tuple(int(value) for value in speeds) + if profile == self.commanded_speed_profile: + return + message = String() + message.data = json.dumps( + { + "setting_cmd": "set_speed", + "params": { + "hand_type": "left", + "speed": list(profile), + }, + } + ) + self.setting_publisher.publish(message) + self.commanded_speed_profile = profile + self.speed_commanded_at = time.monotonic() + + def _normal_speed_profile(self) -> list[int]: + return [self.normal_calibration_speed] * 5 + + def _speed_profile_for_spec(self, spec: SweepSpec) -> list[int]: + return build_calibration_speed_profile( + spec, + normal_speed=self.normal_calibration_speed, + index_roll_speed=self.index_roll_calibration_speed, + index_flex_speed=self.index_flex_calibration_speed, + ) + + def _publish_hold_current(self) -> None: + if len(self.latest_state_u8) != 20: + return + values = [ + int(np.clip(np.rint(value), 0, 255)) + for value in self.latest_state_u8 + ] + self._publish_command(values) + + def _begin_return_baseline(self, after: str) -> None: + self.baseline_after = str(after) + self.position_hold_since = None + self.motion_stage_started_at = time.monotonic() + self.state = STATE_RETURN_BASELINE + self.reason = f"return_baseline_before_{after}" + self._publish_speed_profile(self._normal_speed_profile()) + self._publish_command(list(self.baseline_command)) + + def _baseline_reached(self) -> bool: + if len(self.latest_state_u8) != 20: + return False + indices = sorted({spec.motor_index for spec in JOINT_SPECS.values()}) + return all( + abs(float(self.latest_state_u8[index]) - self.baseline_command[index]) + <= self.endpoint_tolerance_u8 + for index in indices + ) + + def _motion_command_reached( + self, + spec: SweepSpec, + command_u8: int, + state_u8: tuple[float, ...] | None = None, + ) -> bool: + """Check the swept motor and all clearance motors together.""" + state = self.latest_state_u8 if state_u8 is None else state_u8 + if len(state) != 20: + return False + expected = build_calibration_motion_command( + spec, + command_u8, + baseline=self.baseline_command, + ) + indices = {spec.motor_index} + indices.update(calibration_auxiliary_commands(spec)) + return all( + abs(float(state[index]) - expected[index]) + <= self.endpoint_tolerance_u8 + for index in indices + ) + + def _start_next_sweep(self) -> None: + if self.sweep_index >= len(self.sweep_items): + self.active_sweep = None + self._begin_return_baseline("fit") + return + self.active_sweep = self.sweep_items[self.sweep_index] + item = self.active_sweep + self.position_hold_since = None + self.motion_stage_started_at = time.monotonic() + self.state = STATE_PREPARE_SWEEP + self.reason = ( + f"prepare_{item.spec.view}_motor_{item.spec.motor_index}_" + f"cycle_{item.cycle}_{item.direction}" + ) + self._publish_speed_profile(self._speed_profile_for_spec(item.spec)) + self._publish_command( + build_calibration_motion_command( + item.spec, + item.start_u8, + baseline=self.baseline_command, + ) + ) + + def _begin_active_sweep(self, now: float) -> None: + assert self.active_sweep is not None + self.sweep_frames.clear() + self.sweep_started_at = now + self.sweep_last_valid_at = now + self.sweep_endpoint_since = None + self.state = STATE_SWEEP + self.reason = "collecting_timestamp_synchronised_tag_centres" + self._publish_command( + build_calibration_motion_command( + self.active_sweep.spec, + self.active_sweep.target_u8, + baseline=self.baseline_command, + ) + ) + + def _sweep_spec_start_index(self, spec: SweepSpec) -> int: + return next( + index + for index, item in enumerate(self.sweep_items) + if item.spec == spec + ) + + def _fit_joint_records( + self, + joint_name: str, + records: list[dict[str, Any]], + *, + relaxed: bool = False, + ) -> JointCurveFit: + return fit_measured_joint_curve( + joint_name, + records, + maximum_plane_rms_m=( + float("inf") + if relaxed + else self.trajectory_maximum_plane_rms_m + ), + maximum_radial_rms_m=( + float("inf") + if relaxed + else self.trajectory_maximum_radial_rms_m + ), + minimum_radius_m=( + 0.0 if relaxed else self.trajectory_minimum_radius_m + ), + minimum_arc_rad=( + 0.0 if relaxed else self.trajectory_minimum_arc_rad + ), + image_maximum_radial_rms_px=( + float("inf") + if relaxed + else self.image_trajectory_maximum_radial_rms_px + ), + image_maximum_radial_p95_px=( + float("inf") + if relaxed + else self.image_trajectory_maximum_radial_p95_px + ), + image_minimum_radius_px=( + 0.0 if relaxed else self.image_trajectory_minimum_radius_px + ), + ) + + def _provisional_fit_failures( + self, spec: SweepSpec + ) -> list[dict[str, Any]]: + """Check geometry, repeatability and zero after one six-way task.""" + failures: list[dict[str, Any]] = [] + fits: dict[str, JointCurveFit] = {} + for joint_name in spec.joints: + try: + fit = self._fit_joint_records( + joint_name, + self.records_by_joint[joint_name], + relaxed=True, + ) + except Exception as error: + failures.append( + { + "joint": joint_name, + "metric": "trajectory_fit", + "reason": str(error), + } + ) + continue + fits[joint_name] = fit + quality = fit.quality + if joint_name in IMAGE_TRAJECTORY_JOINTS: + checks = ( + ( + "image_radial_rms_px", + float(quality["radial_rms_px"]), + self.image_trajectory_maximum_radial_rms_px, + "maximum", + ), + ( + "image_radial_p95_px", + float(quality["radial_p95_px"]), + self.image_trajectory_maximum_radial_p95_px, + "maximum", + ), + ( + "image_radius_px", + float(quality["radius_px"]), + self.image_trajectory_minimum_radius_px, + "minimum", + ), + ( + "arc_deg", + math.degrees(float(quality["arc_rad"])), + math.degrees(self.trajectory_minimum_arc_rad), + "minimum", + ), + ) + else: + checks = ( + ( + "plane_rms_mm", + 1000.0 * float(quality["plane_rms_m"]), + 1000.0 * self.trajectory_maximum_plane_rms_m, + "maximum", + ), + ( + "radial_rms_mm", + 1000.0 * float(quality["radial_rms_m"]), + 1000.0 * self.trajectory_maximum_radial_rms_m, + "maximum", + ), + ( + "radius_mm", + 1000.0 * float(quality["radius_m"]), + 1000.0 * self.trajectory_minimum_radius_m, + "minimum", + ), + ( + "arc_deg", + math.degrees(float(quality["arc_rad"])), + math.degrees(self.trajectory_minimum_arc_rad), + "minimum", + ), + ) + joint_spec = JOINT_SPECS[joint_name] + monotonic_limit = ( + self.maximum_monotonic_correction_rad + if joint_spec.active + else self.passive_maximum_monotonic_correction_rad + ) + hysteresis_limit = ( + self.maximum_hysteresis_rad + if joint_spec.active + else self.passive_maximum_hysteresis_rad + ) + checks += ( + ( + "monotonic_correction_deg", + math.degrees(fit.maximum_monotonic_correction_rad), + math.degrees(monotonic_limit), + "maximum", + ), + ( + "hysteresis_deg", + math.degrees(fit.maximum_hysteresis_rad), + math.degrees(hysteresis_limit), + "maximum", + ), + ) + for metric, actual, limit, comparison in checks: + failed = ( + actual > limit + if comparison == "maximum" + else actual < limit + ) + if failed: + failures.append( + { + "joint": joint_name, + "metric": metric, + "actual": round(float(actual), 6), + "limit": round(float(limit), 6), + "comparison": comparison, + } + ) + + cycle_travels: list[float] = [] + for cycle in range(self.repetitions): + cycle_records = [ + record + for record in self.records_by_joint[joint_name] + if int(record["cycle"]) == cycle + ] + try: + cycle_fit = self._fit_joint_records( + joint_name, cycle_records + ) + except Exception as error: + failures.append( + { + "joint": joint_name, + "metric": "cycle_fit", + "cycle": cycle + 1, + "reason": str(error), + } + ) + continue + cycle_travels.append( + abs( + float(cycle_fit.angle_rad[0]) + - float(cycle_fit.angle_rad[255]) + ) + ) + if len(cycle_travels) == self.repetitions: + travel_range = max(cycle_travels) - min(cycle_travels) + cycle_limit = ( + self.trajectory_maximum_cycle_travel_difference_rad + if joint_spec.active + else self.passive_maximum_cycle_travel_difference_rad + ) + if travel_range > cycle_limit: + failures.append( + { + "joint": joint_name, + "metric": "cycle_travel_range_deg", + "actual": round(math.degrees(travel_range), 6), + "limit": round(math.degrees(cycle_limit), 6), + "comparison": "maximum", + "cycle_travel_deg": [ + round(math.degrees(value), 6) + for value in cycle_travels + ], + } + ) + + failed_joints = {str(item["joint"]) for item in failures} + for joint_name in spec.joints: + if joint_name in failed_joints or joint_name not in fits: + continue + joint_spec = JOINT_SPECS[joint_name] + if joint_spec.zero_kind == "projected": + try: + fit_projected_zero( + self.records_by_joint[joint_name], + minimum_radius_px=self.zero_minimum_radius_px, + minimum_arc_rad=self.trajectory_minimum_arc_rad, + maximum_radial_rms_px=self.zero_maximum_radial_rms_px, + maximum_radial_p95_px=self.zero_maximum_radial_p95_px, + maximum_round_difference_rad=( + self.zero_maximum_round_difference_rad + ), + ) + except Exception as error: + failures.append( + { + "joint": joint_name, + "metric": "projected_zero", + "reason": str(error), + } + ) + elif joint_spec.zero_kind == "travel_midpoint": + try: + center_splay_curve(fits[joint_name]) + except Exception as error: + failures.append( + { + "joint": joint_name, + "metric": "travel_midpoint", + "reason": str(error), + } + ) + return failures + + def _pause_for_provisional_fit_failure( + self, spec: SweepSpec, failures: list[dict[str, Any]] + ) -> None: + start_index = self._sweep_spec_start_index(spec) + attempt = self.sweep_attempts.get(spec.motor_index, 1) + self.retry_sweep_spec = spec + self.sweep_index = start_index + self.fit_failure = { + "kind": "fit_failure", + "view": spec.view, + "motor_index": spec.motor_index, + "joints": list(spec.joints), + "attempt": attempt, + "directions_to_rescan": 2 * self.repetitions, + "failures": failures, + } + append_jsonl( + self.raw_path, + { + "kind": "fit_failure", + **self.fit_failure, + }, + ) + self._pause("joint_fit_check_failed") + + def _finish_active_sweep(self) -> None: + assert self.active_sweep is not None + item = self.active_sweep + motor = item.spec.motor_index + states = np.asarray( + [float(frame.state_u8[motor]) for frame in self.sweep_frames] + ) + bins: dict[int, list[FrameObservation]] = {} + for frame, state in zip(self.sweep_frames, states): + if abs(state - item.start_u8) <= self.endpoint_tolerance_u8: + command = item.start_u8 + elif abs(state - item.target_u8) <= self.endpoint_tolerance_u8: + command = item.target_u8 + else: + command = int(np.clip(np.rint(state), 0, 255)) + bins.setdefault(command, []).append(frame) + commands = sorted(bins) + if len(commands) < self.minimum_sweep_bins: + self._pause("sweep_bins_too_few") + return + if commands[0] != 0 or commands[-1] != 255: + self._pause("sweep_missing_endpoint_bin") + return + if max(np.diff(commands), default=0) > self.maximum_bin_gap: + self._pause("sweep_bin_gap_too_large") + return + + for joint_name in item.spec.joints: + for command in commands: + frames = bins[command] + vector = np.median( + np.asarray( + [frame.joint_vectors_xyz_m[joint_name] for frame in frames] + ), + axis=0, + ) + image_vector = np.median( + np.asarray( + [frame.image_vectors_xy_px[joint_name] for frame in frames] + ), + axis=0, + ) + record = { + "kind": "sample", + "attempt": self.sweep_attempts.get(motor, 1), + "view": item.spec.view, + "joint": joint_name, + "motor_index": motor, + "cycle": item.cycle, + "direction": item.direction, + "command_u8": int(command), + "relative_translation_xyz_m": [ + float(value) for value in vector + ], + "image_relative_xy_px": [ + float(value) for value in image_vector + ], + "valid_frames": len(frames), + } + self.records_by_joint[joint_name].append(record) + append_jsonl(self.raw_path, record) + + previous_spec = item.spec + self.sweep_index += 1 + self.active_sweep = None + spec_complete = bool( + self.sweep_index >= len(self.sweep_items) + or self.sweep_items[self.sweep_index].spec != previous_spec + ) + if spec_complete: + failures = self._provisional_fit_failures(previous_spec) + if failures: + self._pause_for_provisional_fit_failure( + previous_spec, failures + ) + return + if self.retry_sweep_spec == previous_spec: + self.retry_sweep_spec = None + self.fit_failure = {} + if self.sweep_index >= len(self.sweep_items): + self._begin_return_baseline("fit") + elif self.sweep_items[self.sweep_index].spec != previous_spec: + self._begin_return_baseline("next_sweep") + else: + self._start_next_sweep() + + def _fit_all_curves(self) -> None: + self.state = STATE_FITTING + self.reason = "fitting_21_joint_compact_calibration" + measured: dict[str, JointCurveFit] = {} + for name in MEASURED_JOINTS: + measured[name] = self._fit_joint_records( + name, self.records_by_joint[name] + ) + centred, zero_command, midpoint = center_splay_curve( + measured["index_mcp_roll"] + ) + measured["index_mcp_roll"] = centred + projected: dict[str, float] = {} + for name, spec in JOINT_SPECS.items(): + if spec.zero_kind != "projected": + continue + projected[name] = fit_projected_zero( + self.records_by_joint[name], + minimum_radius_px=self.zero_minimum_radius_px, + minimum_arc_rad=self.trajectory_minimum_arc_rad, + maximum_radial_rms_px=self.zero_maximum_radial_rms_px, + maximum_radial_p95_px=self.zero_maximum_radial_p95_px, + maximum_round_difference_rad=( + self.zero_maximum_round_difference_rad + ), + ) + self.measured_fits = measured + self.projected_zeros_rad = projected + self.splay_zero_command_u8 = zero_command + self.splay_midpoint_rad = midpoint + self.fit_quality_passed = all( + fit.maximum_monotonic_correction_rad + <= ( + self.maximum_monotonic_correction_rad + if JOINT_SPECS[name].active + else self.passive_maximum_monotonic_correction_rad + ) + and fit.maximum_hysteresis_rad + <= ( + self.maximum_hysteresis_rad + if JOINT_SPECS[name].active + else self.passive_maximum_hysteresis_rad + ) + for name, fit in measured.items() + ) + if self.validation_enabled: + self._build_validation_items() + self._start_next_validation() + else: + self.validation_items.clear() + self.validation_index = 0 + self.validation_errors_rad.clear() + self._begin_return_baseline("finalize") + + def _build_validation_items(self) -> None: + generator = random.Random(self.validation_seed) + self.validation_items = [] + for spec in SWEEP_SPECS: + commands = generator.sample( + list(range(16, 240)), self.validation_command_count + ) + self.validation_items.extend( + ValidationItem(spec, command) for command in commands + ) + self.validation_index = 0 + self.validation_errors_rad.clear() + + def _start_next_validation(self) -> None: + if self.validation_index >= len(self.validation_items): + self.active_validation = None + self._begin_return_baseline("finalize") + return + self.active_validation = self.validation_items[self.validation_index] + self.validation_frames_buffer.clear() + self.position_hold_since = None + self.validation_stage_started_at = time.monotonic() + self.state = STATE_VALIDATION_MOVE + self.reason = ( + f"validation_{self.active_validation.spec.view}_motor_" + f"{self.active_validation.spec.motor_index}_" + f"command_{self.active_validation.command_u8}" + ) + self._publish_speed_profile( + self._speed_profile_for_spec(self.active_validation.spec) + ) + self._publish_command( + build_calibration_motion_command( + self.active_validation.spec, + self.active_validation.command_u8, + baseline=self.baseline_command, + ) + ) + + def _finish_validation_capture(self) -> None: + assert self.active_validation is not None + item = self.active_validation + command = item.command_u8 + for name in item.spec.joints: + vectors = np.asarray( + [frame.joint_vectors_xyz_m[name] for frame in self.validation_frames_buffer] + ) + image_vectors = np.asarray( + [ + frame.image_vectors_xy_px[name] + for frame in self.validation_frames_buffer + ] + ) + observed = measure_joint_observation( + self.measured_fits[name], + vector_xyz_m=np.median(vectors, axis=0), + image_vector_xy_px=np.median(image_vectors, axis=0), + ) + expected = float(self.measured_fits[name].angle_rad[command]) + self.validation_errors_rad.append(observed - expected) + previous_spec = item.spec + self.validation_index += 1 + self.active_validation = None + if self.validation_index >= len(self.validation_items): + self._begin_return_baseline("finalize") + elif self.validation_items[self.validation_index].spec != previous_spec: + self._begin_return_baseline("validation_next") + else: + self._start_next_validation() + + def _finalize(self) -> None: + errors = np.abs(np.asarray(self.validation_errors_rad, dtype=float)) + validation_passed = bool( + not self.validation_enabled + or ( + errors.size > 0 + and float(np.mean(errors)) <= self.maximum_validation_mae_rad + and float(np.percentile(errors, 95.0)) + <= self.maximum_validation_p95_rad + ) + ) + passed = bool(self.fit_quality_passed and validation_passed) + payload = build_compact_payload( + serial_number=self.serial_number, + measured_fits=self.measured_fits, + projected_zeros_rad=self.projected_zeros_rad, + splay_zero_command_u8=self.splay_zero_command_u8, + splay_midpoint_rad=self.splay_midpoint_rad, + validation_errors_rad=self.validation_errors_rad, + passed=passed, + baseline=self.baseline_command, + ) + atomic_write_json(self.final_path, payload) + self.completed_payload = payload + self.state = STATE_COMPLETE + self.reason = "calibration_passed" if passed else "quality_failed" + self.get_logger().info( + f"Three-camera calibration complete: passed={passed}; {self.final_path}" + ) + + def _pause(self, reason: str) -> None: + self._publish_hold_current() + self.paused_reason = str(reason) + self.state = STATE_PAUSED + self.reason = str(reason) + self.position_hold_since = None + + def _timer_callback(self) -> None: + now = time.monotonic() + try: + self._advance(now) + except Exception as error: + self.get_logger().error(f"Calibration paused: {error}") + self._pause(str(error)) + if now - self.last_status_publish >= 0.5: + self.last_status_publish = now + self._publish_status(now) + + def _advance(self, now: float) -> None: + if self.state == STATE_PREFLIGHT: + if self._all_preflight_ready(now): + self.state = STATE_WAIT_START + self.reason = "call_start" + return + if self.state == STATE_WAIT_START: + if not self._all_preflight_ready(now): + self.state = STATE_PREFLIGHT + self.reason = "preflight_lost" + return + if self.state == STATE_RETURN_BASELINE: + if now - self.motion_stage_started_at > self.position_timeout_seconds: + self._pause("return_baseline_timeout") + return + if self._baseline_reached(): + if self.position_hold_since is None: + self.position_hold_since = now + elif now - self.position_hold_since >= self.baseline_hold_seconds: + after = self.baseline_after + self.position_hold_since = None + if after in {"next_sweep", "resume_sweep"}: + self._start_next_sweep() + elif after == "fit": + self._fit_all_curves() + elif after in {"validation_next", "resume_validation"}: + self._start_next_validation() + elif after == "finalize": + self._finalize() + else: + self.position_hold_since = None + return + if self.state == STATE_PREPARE_SWEEP: + assert self.active_sweep is not None + if now - self.motion_stage_started_at > self.position_timeout_seconds: + self._pause("sweep_start_position_timeout") + return + reached = self._motion_command_reached( + self.active_sweep.spec, + self.active_sweep.start_u8, + ) + speed_ready = bool( + now - self.speed_commanded_at + >= self.speed_setting_settle_seconds + ) + if reached and speed_ready: + if self.position_hold_since is None: + self.position_hold_since = now + elif now - self.position_hold_since >= self.endpoint_hold_seconds: + self.position_hold_since = None + self._begin_active_sweep(now) + else: + self.position_hold_since = None + return + if self.state == STATE_SWEEP: + assert self.active_sweep is not None + motor = self.active_sweep.spec.motor_index + if now - self.sweep_started_at > self.sweep_timeout_seconds: + self._pause("sweep_timeout") + return + if now - self.sweep_last_valid_at > self.invalid_timeout_seconds: + self._pause("synchronised_tag_state_timeout") + return + values = [ + float(frame.state_u8[motor]) for frame in self.sweep_frames + ] + span = 0.0 if not values else max(values) - min(values) + target_reached = self._motion_command_reached( + self.active_sweep.spec, + self.active_sweep.target_u8, + ) + if target_reached: + if self.sweep_endpoint_since is None: + self.sweep_endpoint_since = now + else: + self.sweep_endpoint_since = None + if ( + self.sweep_endpoint_since is not None + and now - self.sweep_endpoint_since >= self.endpoint_hold_seconds + and len(self.sweep_frames) >= self.minimum_sweep_frames + and span >= self.minimum_state_span_u8 + ): + self._finish_active_sweep() + return + if self.state == STATE_VALIDATION_MOVE: + assert self.active_validation is not None + reached = self._motion_command_reached( + self.active_validation.spec, + self.active_validation.command_u8, + ) + speed_ready = bool( + now - self.speed_commanded_at + >= self.speed_setting_settle_seconds + ) + if reached and speed_ready: + if self.position_hold_since is None: + self.position_hold_since = now + elif now - self.position_hold_since >= self.endpoint_hold_seconds: + self.position_hold_since = None + self.validation_frames_buffer.clear() + self.validation_stage_started_at = now + self.state = STATE_VALIDATION_CAPTURE + self.reason = "capturing_random_validation_pose" + else: + self.position_hold_since = None + if now - self.validation_stage_started_at > self.validation_timeout_seconds: + self._pause("validation_move_timeout") + return + if self.state == STATE_VALIDATION_CAPTURE: + if len(self.validation_frames_buffer) >= self.validation_frames: + self._finish_validation_capture() + elif now - self.validation_stage_started_at > self.validation_timeout_seconds: + self._pause("validation_capture_timeout") + + def _publish_status(self, now: float) -> None: + views = { + name: { + "ready": self._view_ready(runtime, now), + "camera_info_valid": runtime.camera_info_valid, + "detection_hz": round(runtime.detection_hz, 2), + "valid_rate": round(runtime.valid_rate, 4), + "required_tag_ids": sorted(runtime.role_by_id), + "detected_tag_ids": sorted( + tag_id + for tag_id, role in runtime.role_by_id.items() + if role in runtime.latest_tag_quality + ), + "missing_tag_ids": sorted( + tag_id + for tag_id, role in runtime.role_by_id.items() + if role not in runtime.latest_tag_quality + ), + "pnp_rejections": dict(runtime.latest_pnp_rejections), + "group_pnp_reason": runtime.latest_group_pnp_reason, + "pnp_reset_count": runtime.pnp_reset_count, + "pnp_invalid_seconds": ( + 0.0 + if runtime.pnp_invalid_since is None + else round(max(0.0, now - runtime.pnp_invalid_since), 3) + ), + } + for name, runtime in self.views.items() + } + active: dict[str, Any] = {} + if self.active_sweep is not None: + motor = self.active_sweep.spec.motor_index + values = [ + float(frame.state_u8[motor]) for frame in self.sweep_frames + ] + bins = sorted( + { + ( + self.active_sweep.start_u8 + if abs(value - self.active_sweep.start_u8) + <= self.endpoint_tolerance_u8 + else self.active_sweep.target_u8 + if abs(value - self.active_sweep.target_u8) + <= self.endpoint_tolerance_u8 + else int(np.clip(np.rint(value), 0, 255)) + ) + for value in values + } + ) + missing_endpoints = [ + endpoint for endpoint in (0, 255) if endpoint not in bins + ] + actual = ( + None + if len(self.latest_state_u8) != 20 + else float(self.latest_state_u8[motor]) + ) + motion_progress = None + if actual is not None: + motion_progress = float( + np.clip( + abs(actual - self.active_sweep.start_u8) / 255.0, + 0.0, + 1.0, + ) + ) + active = { + "kind": "sweep", + "view": self.active_sweep.spec.view, + "motor_index": motor, + "joints": list(self.active_sweep.spec.joints), + "cycle": self.active_sweep.cycle + 1, + "repetitions": self.repetitions, + "direction": self.active_sweep.direction, + "start_u8": self.active_sweep.start_u8, + "target_u8": self.active_sweep.target_u8, + "actual_u8": actual, + "motion_progress": motion_progress, + "valid_frames": len(self.sweep_frames), + "sample": { + "minimum_u8": None if not values else min(values), + "maximum_u8": None if not values else max(values), + "span_u8": 0.0 if not values else max(values) - min(values), + "bin_count": len(bins), + "minimum_bin_count": self.minimum_sweep_bins, + "maximum_bin_gap": int(max(np.diff(bins), default=0)), + "allowed_maximum_bin_gap": self.maximum_bin_gap, + "missing_endpoint_u8": missing_endpoints, + "endpoint_tolerance_u8": self.endpoint_tolerance_u8, + }, + "auxiliary_motors": [ + { + "motor_index": index, + "command_u8": command, + "actual_u8": ( + None + if len(self.latest_state_u8) != 20 + else float(self.latest_state_u8[index]) + ), + } + for index, command in calibration_auxiliary_commands( + self.active_sweep.spec + ).items() + ], + "speed": { + "commanded_finger_speed": self._speed_profile_for_spec( + self.active_sweep.spec + ), + "reported_finger_speed": self.latest_hand_info.get("speed"), + "normal_speed": self.normal_calibration_speed, + "index_roll_speed": self.index_roll_calibration_speed, + "index_flex_speed": self.index_flex_calibration_speed, + }, + } + elif self.active_validation is not None: + motor = self.active_validation.spec.motor_index + active = { + "kind": "validation", + "view": self.active_validation.spec.view, + "motor_index": motor, + "command_u8": self.active_validation.command_u8, + "actual_u8": ( + None + if len(self.latest_state_u8) != 20 + else float(self.latest_state_u8[motor]) + ), + "valid_frames": len(self.validation_frames_buffer), + "auxiliary_motors": [ + { + "motor_index": index, + "command_u8": command, + "actual_u8": ( + None + if len(self.latest_state_u8) != 20 + else float(self.latest_state_u8[index]) + ), + } + for index, command in calibration_auxiliary_commands( + self.active_validation.spec + ).items() + ], + "speed": { + "commanded_finger_speed": self._speed_profile_for_spec( + self.active_validation.spec + ), + "reported_finger_speed": self.latest_hand_info.get("speed"), + "normal_speed": self.normal_calibration_speed, + "index_roll_speed": self.index_roll_calibration_speed, + "index_flex_speed": self.index_flex_calibration_speed, + }, + } + elif self.fit_failure: + active = dict(self.fit_failure) + payload = { + "state": self.state, + "reason": self.reason, + "progress": ( + 0.0 + if not self.sweep_items + else round(self.sweep_index / len(self.sweep_items), 4) + ), + "completed_sweeps": self.sweep_index, + "total_sweeps": len(self.sweep_items), + "baseline_command_u8": list(self.baseline_command), + "views": views, + "active": active, + "result_path": ( + str(self.final_path) if self.completed_payload is not None else "" + ), + "quality": ( + {} if self.completed_payload is None else self.completed_payload["quality"] + ), + } + status = String() + status.data = json.dumps(payload, ensure_ascii=False) + self.status_publisher.publish(status) + text = String() + text.data = render_three_camera_status_text_zh(payload) + self.status_text_publisher.publish(text) + + +def main(args: list[str] | None = None) -> None: + configure_fastdds_large_image_transport() + rclpy.init(args=args) + node: G20ThreeCameraCalibrationNode | None = None + try: + node = G20ThreeCameraCalibrationNode() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + if node is not None: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/g20_thumb_apriltag_calibration/launch/front_cmc_pitch_zero.launch.py b/src/g20_thumb_apriltag_calibration/launch/front_cmc_pitch_zero.launch.py index 50c8aa8..4cb6d93 100644 --- a/src/g20_thumb_apriltag_calibration/launch/front_cmc_pitch_zero.launch.py +++ b/src/g20_thumb_apriltag_calibration/launch/front_cmc_pitch_zero.launch.py @@ -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( diff --git a/src/g20_thumb_apriltag_calibration/launch/front_cmc_roll_calibration.launch.py b/src/g20_thumb_apriltag_calibration/launch/front_cmc_roll_calibration.launch.py index a396a4f..0ce1548 100644 --- a/src/g20_thumb_apriltag_calibration/launch/front_cmc_roll_calibration.launch.py +++ b/src/g20_thumb_apriltag_calibration/launch/front_cmc_roll_calibration.launch.py @@ -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( diff --git a/src/g20_thumb_apriltag_calibration/launch/front_thumb_calibration.launch.py b/src/g20_thumb_apriltag_calibration/launch/front_thumb_calibration.launch.py index df9c0c5..f8cd2d9 100644 --- a/src/g20_thumb_apriltag_calibration/launch/front_thumb_calibration.launch.py +++ b/src/g20_thumb_apriltag_calibration/launch/front_thumb_calibration.launch.py @@ -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"), diff --git a/src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py b/src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py new file mode 100644 index 0000000..d976c67 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/launch/three_camera_calibration.launch.py @@ -0,0 +1,341 @@ +"""Launch three Hikrobot views and one complete-G20 calibration owner.""" + +from __future__ import annotations + +from datetime import datetime +from pathlib import Path +import re + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + LogInfo, + OpaqueFunction, + SetEnvironmentVariable, +) +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer, Node +from launch_ros.descriptions import ComposableNode +from launch_ros.parameter_descriptions import ParameterValue + + +VIEWS = ("front", "side", "top") + + +def _launch_stack(context): + hand_serial = LaunchConfiguration("serial_number").perform(context) + if ( + not hand_serial + or hand_serial == "UNSET" + or re.fullmatch(r"[A-Za-z0-9_.-]+", hand_serial) is None + or hand_serial in {".", ".."} + ): + raise RuntimeError("serial_number must be a safe non-empty hand serial") + + requested_session = LaunchConfiguration("session_dir").perform(context) + output_root = Path( + LaunchConfiguration("output_root").perform(context) + ).expanduser().resolve() + if requested_session: + session_dir = Path(requested_session).expanduser().resolve() + else: + session_dir = ( + output_root + / hand_serial + / datetime.now().strftime("%Y%m%d_%H%M%S") + ) + session_dir.mkdir(parents=True, exist_ok=True) + + camera_serials = { + view: LaunchConfiguration(f"{view}_camera_serial").perform(context) + for view in VIEWS + } + if any(not serial for serial in camera_serials.values()): + raise RuntimeError("all three camera serial numbers are required") + if len(set(camera_serials.values())) != 3: + raise RuntimeError("front/side/top camera serial numbers must be unique") + + cameras = [] + components = [] + raw_topics = [] + info_topics = [] + detection_topics = [] + for view in VIEWS: + namespace = f"/g20_calibration/{view}/camera" + raw_topic = f"{namespace}/image_raw" + info_topic = f"{namespace}/camera_info" + rect_topic = f"{namespace}/image_rect" + detector_namespace = f"/g20_calibration/{view}/apriltag" + detection_topic = f"{detector_namespace}/detections" + raw_topics.append(raw_topic) + info_topics.append(info_topic) + detection_topics.append(detection_topic) + cameras.append( + Node( + package="g20_thumb_apriltag_calibration", + executable="hikrobot_camera_node", + name="hikrobot_camera", + namespace=namespace, + output="screen", + emulate_tty=True, + condition=IfCondition(LaunchConfiguration("start_cameras")), + parameters=[ + { + "serial_number": LaunchConfiguration( + f"{view}_camera_serial" + ), + "expected_model": LaunchConfiguration("camera_model"), + "camera_name": LaunchConfiguration( + f"{view}_camera_name" + ), + "frame_id": f"g20_calibration_{view}_optical_frame", + "image_width": 1624, + "image_height": 1240, + "frame_rate": ParameterValue( + LaunchConfiguration("camera_frame_rate"), + value_type=float, + ), + "exposure_time_us": ParameterValue( + LaunchConfiguration("exposure_time_us"), + value_type=float, + ), + "gain_db": ParameterValue( + LaunchConfiguration("gain_db"), value_type=float + ), + "auto_exposure": ParameterValue( + LaunchConfiguration("auto_exposure"), value_type=bool + ), + "camera_info_url": LaunchConfiguration( + f"{view}_camera_info_url" + ), + } + ], + ) + ) + components.extend( + [ + ComposableNode( + package="image_proc", + plugin="image_proc::RectifyNode", + name=f"rectify_{view}", + namespace=namespace, + remappings=[ + ("image", raw_topic), + ("camera_info", info_topic), + ("image_rect", rect_topic), + ], + parameters=[{"queue_size": 1}], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ComposableNode( + package="apriltag_ros", + plugin="AprilTagNode", + name="apriltag", + namespace=detector_namespace, + parameters=[ + LaunchConfiguration("tag_config"), + { + "detector.decimate": ParameterValue( + LaunchConfiguration("apriltag_decimate"), + value_type=float, + ) + }, + ], + remappings=[ + ("image_rect", rect_topic), + ("camera_info", info_topic), + ], + extra_arguments=[{"use_intra_process_comms": True}], + ), + ] + ) + + vision = ComposableNodeContainer( + name="g20_three_camera_vision", + namespace="/", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=components, + output="screen", + emulate_tty=True, + ) + sdk = Node( + package="linker_hand_ros2_sdk", + executable="linker_hand_sdk", + name="linker_hand_sdk", + output="screen", + condition=IfCondition(LaunchConfiguration("start_sdk")), + parameters=[ + { + "hand_type": "left", + "hand_joint": "G20", + "can": LaunchConfiguration("can_interface"), + "modbus": "None", + "topic_prefix": "/g20", + "move_on_startup": False, + "startup_speed": ParameterValue( + LaunchConfiguration("calibration_speed"), value_type=int + ), + "startup_torque": 80, + "state_poll_rate": 10.0, + "repeat_position_commands": False, + "is_touch": False, + } + ], + ) + calibration = Node( + package="g20_thumb_apriltag_calibration", + executable="three_camera_calibration_node", + name="g20_calibration", + output="screen", + emulate_tty=True, + parameters=[ + LaunchConfiguration("calibration_config"), + { + "serial_number": hand_serial, + "session_dir": str(session_dir), + "commands_enabled": ParameterValue( + LaunchConfiguration("commands_enabled"), value_type=bool + ), + "normal_calibration_speed": ParameterValue( + LaunchConfiguration("calibration_speed"), value_type=int + ), + "index_roll_calibration_speed": ParameterValue( + LaunchConfiguration("index_roll_calibration_speed"), + value_type=int, + ), + "index_flex_calibration_speed": ParameterValue( + LaunchConfiguration("index_flex_calibration_speed"), + value_type=int, + ), + "validation_enabled": ParameterValue( + LaunchConfiguration("validation_enabled"), value_type=bool + ), + }, + ], + ) + bag = ExecuteProcess( + condition=IfCondition(LaunchConfiguration("record_bag")), + cmd=[ + "ros2", + "bag", + "record", + "--storage", + "mcap", + "--storage-preset-profile", + "zstd_fast", + "--max-bag-size", + "10737418240", + "--output", + str(session_dir / "rosbag"), + *raw_topics, + *info_topics, + *detection_topics, + "/g20/cb_left_hand_control_cmd", + "/g20/cb_left_hand_state", + "/g20/cb_left_hand_info", + "/g20_calibration/status", + ], + output="screen", + ) + return [ + LogInfo(msg=f"G20 three-camera session: {session_dir}"), + LogInfo( + msg=( + "Camera mapping: front=" + f"{camera_serials['front']} side={camera_serials['side']} " + f"top={camera_serials['top']}" + ) + ), + *cameras, + vision, + sdk, + calibration, + bag, + ] + + +def generate_launch_description() -> LaunchDescription: + package_share = Path( + get_package_share_directory("g20_thumb_apriltag_calibration") + ) + info_root = Path.home() / ".ros" / "camera_info" + return LaunchDescription( + [ + SetEnvironmentVariable( + name="FASTRTPS_DEFAULT_PROFILES_FILE", + value=str(package_share / "config" / "fastdds_large_images.xml"), + ), + DeclareLaunchArgument("serial_number", default_value="UNSET"), + DeclareLaunchArgument( + "front_camera_serial", default_value="DB2163742" + ), + DeclareLaunchArgument( + "side_camera_serial", default_value="DB2163749" + ), + DeclareLaunchArgument( + "top_camera_serial", default_value="DB2163739" + ), + DeclareLaunchArgument("camera_model", default_value="MV-CS020-10U"), + DeclareLaunchArgument( + "front_camera_name", default_value="hikrobot_front_DB2163742" + ), + DeclareLaunchArgument( + "side_camera_name", default_value="hikrobot_side_DB2163749" + ), + DeclareLaunchArgument( + "top_camera_name", default_value="hikrobot_top_DB2163739" + ), + DeclareLaunchArgument( + "front_camera_info_url", + default_value=str(info_root / "hikrobot_DB2163742.yaml"), + ), + DeclareLaunchArgument( + "side_camera_info_url", + default_value=str(info_root / "hikrobot_DB2163749.yaml"), + ), + DeclareLaunchArgument( + "top_camera_info_url", + default_value=str(info_root / "hikrobot_DB2163739.yaml"), + ), + DeclareLaunchArgument("camera_frame_rate", default_value="30.0"), + DeclareLaunchArgument("exposure_time_us", default_value="5000.0"), + DeclareLaunchArgument("gain_db", default_value="0.0"), + DeclareLaunchArgument("auto_exposure", default_value="false"), + DeclareLaunchArgument("apriltag_decimate", default_value="1.5"), + DeclareLaunchArgument("can_interface", default_value="can0"), + DeclareLaunchArgument("calibration_speed", default_value="15"), + DeclareLaunchArgument( + "index_roll_calibration_speed", default_value="5" + ), + DeclareLaunchArgument( + "index_flex_calibration_speed", default_value="10" + ), + DeclareLaunchArgument("validation_enabled", default_value="false"), + DeclareLaunchArgument("commands_enabled", default_value="true"), + DeclareLaunchArgument("start_cameras", default_value="true"), + DeclareLaunchArgument("start_sdk", default_value="true"), + DeclareLaunchArgument("record_bag", default_value="false"), + DeclareLaunchArgument( + "output_root", + default_value=str(Path.cwd() / "calibration_output"), + ), + DeclareLaunchArgument("session_dir", default_value=""), + DeclareLaunchArgument( + "calibration_config", + default_value=str( + package_share / "config" / "three_camera_calibration.yaml" + ), + ), + DeclareLaunchArgument( + "tag_config", + default_value=str( + package_share / "config" / "three_camera_tags.yaml" + ), + ), + OpaqueFunction(function=_launch_stack), + ] + ) diff --git a/src/g20_thumb_apriltag_calibration/package.xml b/src/g20_thumb_apriltag_calibration/package.xml index 08eb9fd..ed71c5b 100644 --- a/src/g20_thumb_apriltag_calibration/package.xml +++ b/src/g20_thumb_apriltag_calibration/package.xml @@ -3,7 +3,7 @@ g20_thumb_apriltag_calibration 0.1.0 - Front RealSense AprilTag calibration for the left G20 thumb. + Three-view Hikrobot AprilTag calibration for the complete left G20 hand. lxp MIT @@ -17,7 +17,6 @@ linker_hand_ros2_sdk rclcpp_components rclpy - realsense2_camera rosbag2 sensor_msgs std_msgs @@ -26,6 +25,7 @@ python3-numpy python3-opencv python3-scipy + python3-yaml python3-pytest diff --git a/src/g20_thumb_apriltag_calibration/setup.py b/src/g20_thumb_apriltag_calibration/setup.py index a615a18..ab423d8 100644 --- a/src/g20_thumb_apriltag_calibration/setup.py +++ b/src/g20_thumb_apriltag_calibration/setup.py @@ -15,18 +15,25 @@ setup( ["resource/" + package_name], ), ("share/" + package_name, ["package.xml", "README.md"]), - ("share/" + package_name + "/config", glob("config/*.yaml")), + ( + "share/" + package_name + "/config", + glob("config/*.yaml") + glob("config/*.xml"), + ), ("share/" + package_name + "/launch", glob("launch/*.launch.py")), ], - install_requires=["setuptools", "numpy", "scipy"], + install_requires=["setuptools", "numpy", "scipy", "PyYAML"], tests_require=["pytest"], zip_safe=True, maintainer="lxp", maintainer_email="support@linker-robotics.com", - description="Front RealSense AprilTag calibration for the left G20 thumb", + description="Three-view Hikrobot AprilTag calibration for the complete left G20 hand", license="MIT", entry_points={ "console_scripts": [ + ( + "hikrobot_camera_node = " + "g20_thumb_apriltag_calibration.hikrobot_camera:main" + ), "calibration_node = g20_thumb_apriltag_calibration.node:main", ( "cmc_pitch_zero_node = " @@ -36,6 +43,14 @@ setup( "cmc_roll_calibration_node = " "g20_thumb_apriltag_calibration.zero_node:main" ), + ( + "three_camera_calibration_node = " + "g20_thumb_apriltag_calibration.three_camera_node:main" + ), + ( + "camera_alignment_view = " + "g20_thumb_apriltag_calibration.alignment_view:main" + ), ], }, ) diff --git a/src/g20_thumb_apriltag_calibration/test/test_acquisition.py b/src/g20_thumb_apriltag_calibration/test/test_acquisition.py index ae0d484..c32738f 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_acquisition.py +++ b/src/g20_thumb_apriltag_calibration/test/test_acquisition.py @@ -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) diff --git a/src/g20_thumb_apriltag_calibration/test/test_alignment_view.py b/src/g20_thumb_apriltag_calibration/test/test_alignment_view.py new file mode 100644 index 0000000..095fecf --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/test/test_alignment_view.py @@ -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 diff --git a/src/g20_thumb_apriltag_calibration/test/test_config.py b/src/g20_thumb_apriltag_calibration/test/test_config.py index 01a4477..4b9797c 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_config.py +++ b/src/g20_thumb_apriltag_calibration/test/test_config.py @@ -1,4 +1,5 @@ from pathlib import Path +from xml.etree import ElementTree import yaml @@ -6,6 +7,24 @@ import yaml PACKAGE_ROOT = Path(__file__).resolve().parents[1] +def test_fastdds_profile_has_capacity_for_full_resolution_images() -> None: + root = ElementTree.parse( + PACKAGE_ROOT / "config" / "fastdds_large_images.xml" + ).getroot() + namespace = {"dds": "http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles"} + profiles = root.find("dds:profiles", namespace) + assert profiles is not None + segment = profiles.find( + ".//dds:transport_descriptor[dds:type='SHM']/dds:segment_size", + namespace, + ) + assert segment is not None + assert int(segment.text) >= 64 * 1024 * 1024 + participant = profiles.find("dds:participant", namespace) + assert participant is not None + assert participant.attrib["is_default_profile"] == "true" + + def test_front_tag_parameters_match_namespaced_detector() -> None: config = yaml.safe_load( (PACKAGE_ROOT / "config" / "front_tags.yaml").read_text() @@ -24,6 +43,85 @@ def test_front_tag_parameters_match_namespaced_detector() -> None: assert detector["detector"]["debug"] is False +def test_three_camera_tag_ids_and_topics_are_disjoint() -> None: + tags = yaml.safe_load( + (PACKAGE_ROOT / "config" / "three_camera_tags.yaml").read_text() + ) + expected = { + "front": [0, 1, 2, 3, 10], + "side": [4, 5, 6, 7], + "top": [8, 9], + } + all_ids = set() + for view, ids in expected.items(): + key = f"/g20_calibration/{view}/apriltag/apriltag" + parameters = tags[key]["ros__parameters"] + assert parameters["tag"]["ids"] == ids + assert parameters["qos_profile"] == "sensor_data" + assert parameters["detector"]["decimate"] == 1.5 + all_ids.update(ids) + assert all_ids == set(range(11)) + + +def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None: + config = yaml.safe_load( + (PACKAGE_ROOT / "config" / "three_camera_calibration.yaml").read_text() + ) + parameters = config["g20_calibration"]["ros__parameters"] + + assert parameters["baseline_command_u8"] == [ + 255, + 255, + 255, + 255, + 255, + 255, + 127, + 127, + 127, + 127, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + 255, + ] + assert parameters["setting_topic"] == "/g20/cb_hand_setting_cmd" + assert parameters["normal_calibration_speed"] == 15 + assert parameters["index_roll_calibration_speed"] == 5 + assert parameters["index_flex_calibration_speed"] == 10 + assert parameters["speed_setting_settle_seconds"] >= 0.2 + assert parameters["top_pnp_invalid_reset_seconds"] == 1.0 + assert parameters["repetitions"] == 3 + assert parameters["validation_enabled"] is False + assert parameters["minimum_detection_rate"] == 0.95 + assert parameters["minimum_detection_hz"] == 15.0 + assert parameters["minimum_state_span_u8"] >= 240.0 + assert parameters["minimum_sweep_bins"] >= 32 + assert parameters["maximum_bin_gap"] <= 16 + assert parameters["position_timeout_seconds"] >= 20.0 + assert parameters["zero_maximum_round_difference_deg"] <= 1.0 + assert parameters["image_trajectory_maximum_radial_rms_px"] <= 2.0 + assert parameters["image_trajectory_maximum_radial_p95_px"] <= 3.5 + assert parameters["image_trajectory_minimum_radius_px"] >= 20.0 + assert parameters["trajectory_maximum_cycle_travel_difference_deg"] <= 3.0 + assert parameters["passive_maximum_cycle_travel_difference_deg"] <= 10.0 + assert parameters["passive_maximum_monotonic_correction_deg"] <= 3.0 + assert parameters["passive_maximum_hysteresis_deg"] <= 7.5 + for view in ("front", "side", "top"): + assert parameters[f"{view}_camera_info_topic"].startswith( + f"/g20_calibration/{view}/" + ) + assert parameters[f"{view}_detections_topic"] == ( + f"/g20_calibration/{view}/apriltag/detections" + ) + + def test_trial_uses_centre_trajectory_and_thirty_pixel_tags() -> None: config = yaml.safe_load( (PACKAGE_ROOT / "config" / "calibration.yaml").read_text() @@ -87,15 +185,6 @@ def test_trial_uses_centre_trajectory_and_thirty_pixel_tags() -> None: assert parameters["continuous_prepare_timeout_seconds"] >= 30.0 -def test_realsense_color_and_camera_info_use_matching_qos() -> None: - config = yaml.safe_load( - (PACKAGE_ROOT / "config" / "realsense_color_qos.yaml").read_text() - ) - - assert config["color_qos"] == "DEFAULT" - assert config["color_info_qos"] == "DEFAULT" - - def test_cmc_pitch_zero_config_uses_three_trajectory_circle_rounds() -> None: config = yaml.safe_load( (PACKAGE_ROOT / "config" / "cmc_pitch_zero.yaml").read_text() diff --git a/src/g20_thumb_apriltag_calibration/test/test_full_hand.py b/src/g20_thumb_apriltag_calibration/test/test_full_hand.py new file mode 100644 index 0000000..0c78b75 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/test/test_full_hand.py @@ -0,0 +1,335 @@ +import math + +import numpy as np +import pytest + +from g20_thumb_apriltag_calibration.full_hand import ( + ACTIVE_JOINTS, + IMAGE_TRAJECTORY_JOINTS, + JOINT_SPECS, + MEASURED_JOINTS, + PASSIVE_JOINTS, + SPLAY_JOINTS, + SWEEP_SPECS, + VIEW_TAGS, + build_calibration_motion_command, + build_calibration_speed_profile, + build_compact_payload, + build_full_hand_command, + center_splay_curve, + fit_joint_center_curve, + fit_joint_image_curve, + fit_measured_joint_curve, + fit_projected_zero, + measure_joint_observation, + validate_compact_payload, +) + + +def _records() -> list[dict[str, object]]: + commands = list(range(0, 256, 16)) + if commands[-1] != 255: + commands.append(255) + records: list[dict[str, object]] = [] + centre = np.asarray([0.006, -0.004, 0.012]) + radius = 0.025 + image_centre = np.asarray([30.0, -12.0]) + image_radius = 100.0 + for cycle in range(3): + for direction, sequence in ( + ("decreasing", reversed(commands)), + ("increasing", commands), + ): + for command in sequence: + angle = 0.70 * (255.0 - command) / 255.0 + point = centre + np.asarray( + [radius * math.cos(angle), radius * math.sin(angle), 0.0] + ) + # At command 255 the inward vector points along image +x, so + # table_projected_zero_rad is exactly zero. + image_point = image_centre + np.asarray( + [ + -image_radius * math.cos(angle), + image_radius * math.sin(angle), + ] + ) + records.append( + { + "cycle": cycle, + "direction": direction, + "command_u8": command, + "relative_translation_xyz_m": point.tolist(), + "image_relative_xy_px": image_point.tolist(), + } + ) + return records + + +def test_joint_layout_covers_16_active_and_5_passive_joints() -> None: + assert len(JOINT_SPECS) == 21 + assert len(ACTIVE_JOINTS) == 16 + assert len(PASSIVE_JOINTS) == 5 + assert {tag for tags in VIEW_TAGS.values() for tag in tags.values()} == set( + range(11) + ) + assert VIEW_TAGS["front"]["index_roll"] == 10 + assert VIEW_TAGS["side"] == { + "side_base": 4, + "index_mcp": 5, + "index_pip": 6, + "index_dip": 7, + } + assert VIEW_TAGS["top"] == {"top_base": 8, "thumb_yaw": 9} + assert [spec.motor_index for spec in SWEEP_SPECS] == [0, 5, 15, 6, 1, 16, 10] + + +def test_joint_trajectory_spaces_match_observation_geometry() -> None: + assert IMAGE_TRAJECTORY_JOINTS == { + "thumb_cmc_pitch", + "thumb_cmc_roll", + "thumb_mcp", + "thumb_ip", + "index_mcp_roll", + "index_mcp_pitch", + "index_pip", + } + records = _records() + for name in IMAGE_TRAJECTORY_JOINTS: + fit = fit_measured_joint_curve(name, records) + assert fit.circle["space"] == "image_2d" + for name in ("index_dip", "thumb_cmc_yaw"): + assert "space" not in fit_measured_joint_curve(name, records).circle + + +def test_full_hand_command_changes_exactly_one_controlled_motor() -> None: + result = build_full_hand_command(6, 27) + assert result[6] == 27 + assert result[:6] == [255] * 6 + with pytest.raises(ValueError, match="controlled"): + build_full_hand_command(11, 27) + + +def test_index_roll_motion_moves_other_three_roll_motors_out_of_view() -> None: + baseline = [255] * 20 + baseline[6:10] = [127, 127, 127, 127] + index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6) + + result = build_calibration_motion_command(index_roll, 27, baseline) + + assert result[6:10] == [27, 0, 0, 0] + assert result[:6] == baseline[:6] + assert result[10:] == baseline[10:] + + +def test_non_index_roll_motion_keeps_clearance_motors_at_baseline() -> None: + baseline = [255] * 20 + baseline[6:10] = [127, 127, 127, 127] + thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0) + + result = build_calibration_motion_command(thumb_pitch, 17, baseline) + + assert result[0] == 17 + assert result[6:10] == [127, 127, 127, 127] + + +def test_thumb_yaw_motion_holds_thumb_roll_at_camera_clearance_pose() -> None: + baseline = [255] * 20 + baseline[6:10] = [127, 127, 127, 127] + thumb_yaw = next(spec for spec in SWEEP_SPECS if spec.motor_index == 10) + + result = build_calibration_motion_command(thumb_yaw, 27, baseline) + + assert result[5] == 145 + assert result[10] == 27 + assert result[:5] == baseline[:5] + assert result[6:10] == baseline[6:10] + assert result[11:] == baseline[11:] + + +def test_only_index_roll_uses_the_slow_index_finger_speed() -> None: + index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6) + index_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 1) + + assert build_calibration_speed_profile( + index_roll, + normal_speed=15, + index_roll_speed=5, + index_flex_speed=10, + ) == [15, 5, 15, 15, 15] + assert build_calibration_speed_profile( + index_pitch, + normal_speed=15, + index_roll_speed=5, + index_flex_speed=10, + ) == [15, 10, 15, 15, 15] + + index_pip = next(spec for spec in SWEEP_SPECS if spec.motor_index == 16) + assert build_calibration_speed_profile( + index_pip, + normal_speed=15, + index_roll_speed=5, + index_flex_speed=10, + ) == [15, 10, 15, 15, 15] + + thumb_pitch = next(spec for spec in SWEEP_SPECS if spec.motor_index == 0) + assert build_calibration_speed_profile( + thumb_pitch, + normal_speed=15, + index_roll_speed=5, + index_flex_speed=10, + ) == [15, 15, 15, 15, 15] + + +def test_calibration_speed_profile_rejects_out_of_range_values() -> None: + index_roll = next(spec for spec in SWEEP_SPECS if spec.motor_index == 6) + with pytest.raises(ValueError, match="speeds"): + build_calibration_speed_profile( + index_roll, + normal_speed=15, + index_roll_speed=256, + index_flex_speed=10, + ) + + +def test_curve_fit_and_projected_zero_recover_synthetic_geometry() -> None: + records = _records() + fit = fit_joint_center_curve(records) + assert fit.angle_rad[255] == pytest.approx(0.0, abs=1.0e-8) + assert fit.angle_rad[0] == pytest.approx(0.70, abs=1.0e-4) + assert fit.maximum_hysteresis_rad == pytest.approx(0.0, abs=1.0e-8) + assert fit_projected_zero(records) == pytest.approx(0.0, abs=1.0e-6) + + +def test_thumb_image_curve_avoids_corrupted_pnp_depth() -> None: + records = _records() + for record in records: + command = int(record["command_u8"]) + record["relative_translation_xyz_m"] = [ + 0.001 * command, + 0.0, + 0.0, + ] + + fit = fit_measured_joint_curve("thumb_mcp", records) + + assert fit.circle["space"] == "image_2d" + assert fit.angle_rad[0] == pytest.approx(0.70, abs=0.02) + assert fit.angle_rad[255] == pytest.approx(0.0) + assert math.degrees(fit.maximum_monotonic_correction_rad) < 0.01 + assert math.degrees(fit.maximum_hysteresis_rad) < 0.01 + + command_zero = next( + record for record in records if int(record["command_u8"]) == 0 + ) + observed = measure_joint_observation( + fit, + vector_xyz_m=command_zero["relative_translation_xyz_m"], + image_vector_xy_px=command_zero["image_relative_xy_px"], + ) + assert observed == pytest.approx(0.70, abs=0.02) + + +def test_image_curve_rejects_insufficient_projected_arc() -> None: + records = _records() + for record in records: + command = float(record["command_u8"]) + angle = math.radians(2.0) * (255.0 - command) / 255.0 + record["image_relative_xy_px"] = [ + 100.0 * math.cos(angle), + 100.0 * math.sin(angle), + ] + + with pytest.raises( + ValueError, match="joint_image_trajectory_quality_failed:arc" + ): + fit_joint_image_curve(records) + + +def test_splay_uses_angular_midpoint_not_fixed_command_midpoint() -> None: + fit = fit_joint_center_curve(_records()) + centred, zero_command, midpoint = center_splay_curve(fit) + assert midpoint == pytest.approx(0.35, abs=1.0e-4) + assert centred.angle_rad[0] == pytest.approx(0.35, abs=1.0e-4) + assert centred.angle_rad[255] == pytest.approx(-0.35, abs=1.0e-4) + assert zero_command in {127, 128} + assert abs(centred.angle_rad[zero_command]) <= 0.002 + + +def test_compact_payload_contains_only_runtime_fields_and_inheritance() -> None: + base = fit_joint_center_curve(_records()) + splay, zero_command, midpoint = center_splay_curve(base) + measured = { + name: splay if name == "index_mcp_roll" else base + for name in MEASURED_JOINTS + } + projected = { + name: 0.01 * index + for index, (name, spec) in enumerate(JOINT_SPECS.items()) + if spec.zero_kind == "projected" + } + payload = build_compact_payload( + serial_number="G20_LEFT_001", + measured_fits=measured, + projected_zeros_rad=projected, + splay_zero_command_u8=zero_command, + splay_midpoint_rad=midpoint, + validation_errors_rad=[0.01, -0.02], + passed=True, + ) + validate_compact_payload(payload) + assert set(payload) == { + "schema_version", + "model", + "side", + "serial_number", + "angle_unit", + "command_range", + "baseline_command_u8", + "joints", + "quality", + } + assert len(payload["joints"]) == 21 + assert all(len(joint["angle_rad"]) == 256 for joint in payload["joints"].values()) + assert sum("zero_command_u8" in joint for joint in payload["joints"].values()) == 16 + assert sum(joint.get("passive") is True for joint in payload["joints"].values()) == 5 + assert payload["joints"]["middle_mcp_roll"]["source_joint"] == "index_mcp_roll" + assert ( + payload["joints"]["middle_mcp_roll"]["angle_rad"] + == payload["joints"]["index_mcp_roll"]["angle_rad"] + ) + assert payload["joints"]["index_mcp_roll"]["zero_angles"] == { + "travel_midpoint_rad": pytest.approx(0.35, abs=1.0e-4) + } + for name in SPLAY_JOINTS: + assert payload["joints"][name]["zero_command_u8"] == zero_command + + +def test_compact_payload_allows_skipped_random_validation() -> None: + base = fit_joint_center_curve(_records()) + splay, zero_command, midpoint = center_splay_curve(base) + measured = { + name: splay if name == "index_mcp_roll" else base + for name in MEASURED_JOINTS + } + projected = { + name: 0.0 + for name, spec in JOINT_SPECS.items() + if spec.zero_kind == "projected" + } + + payload = build_compact_payload( + serial_number="G20_LEFT_001", + measured_fits=measured, + projected_zeros_rad=projected, + splay_zero_command_u8=zero_command, + splay_midpoint_rad=midpoint, + validation_errors_rad=[], + passed=True, + ) + + validate_compact_payload(payload) + assert payload["quality"] == { + "passed": True, + "validation_mae_rad": None, + "validation_p95_rad": None, + } diff --git a/src/g20_thumb_apriltag_calibration/test/test_hikrobot_camera.py b/src/g20_thumb_apriltag_calibration/test/test_hikrobot_camera.py new file mode 100644 index 0000000..e7d91c9 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/test/test_hikrobot_camera.py @@ -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") diff --git a/src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py b/src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py new file mode 100644 index 0000000..08caa10 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/test/test_three_camera_diagnostics.py @@ -0,0 +1,217 @@ +import json + +import numpy as np + +from g20_thumb_apriltag_calibration.three_camera_diagnostics import ( + render_three_camera_status_text_zh, +) + + +def test_missing_endpoint_has_specific_chinese_reason_and_recovery() -> None: + payload = { + "state": "PAUSED", + "reason": "sweep_missing_endpoint_bin", + "progress": 0.1429, + "completed_sweeps": 6, + "total_sweeps": 42, + "active": { + "kind": "sweep", + "view": "front", + "motor_index": 5, + "joints": ["thumb_cmc_roll"], + "cycle": 1, + "repetitions": 3, + "start_u8": 255, + "target_u8": 0, + "actual_u8": 0.4, + "motion_progress": 0.998, + "valid_frames": 239, + "sample": { + "minimum_u8": 0.4, + "maximum_u8": 248.2, + "bin_count": 180, + "minimum_bin_count": 32, + "maximum_bin_gap": 3, + "allowed_maximum_bin_gap": 16, + "missing_endpoint_u8": [255], + "endpoint_tolerance_u8": 2.0, + }, + }, + "views": { + "front": { + "ready": False, + "detection_hz": 30.04, + "valid_rate": 0.0, + "missing_tag_ids": [2], + "pnp_rejections": {}, + "group_pnp_reason": "group_pose_jump", + "pnp_invalid_seconds": 0.8, + "pnp_reset_count": 2, + }, + "side": { + "ready": True, + "detection_hz": 30.02, + "valid_rate": 1.0, + "missing_tag_ids": [], + }, + }, + "result_path": "", + } + + text = render_three_camera_status_text_zh(payload) + + assert "标定已暂停（PAUSED）" in text + assert "缺少电机端点255" in text + assert "实际电机范围为0.4～248.2" in text + assert "第1/3轮，255→0" in text + assert "已完成6/42个扫描方向" in text + assert "当前缺失Tag=2" in text + assert "group_pose_jump" not in text + assert "PnP拒绝" not in text + assert "自动重置" not in text + assert "/g20_calibration/resume" in text + + +def test_preflight_lists_missing_tags_in_chinese() -> None: + payload = { + "state": "PREFLIGHT", + "reason": "waiting_for_three_cameras_tags_and_sdk", + "progress": 0.0, + "completed_sweeps": 0, + "total_sweeps": 0, + "active": {}, + "views": { + "top": { + "ready": False, + "detection_hz": 30.0, + "valid_rate": 0.0, + "missing_tag_ids": [9], + } + }, + "result_path": "", + } + + text = render_three_camera_status_text_zh(payload) + + assert "设备和标签预检（PREFLIGHT）" in text + assert "当前缺失Tag=9" in text + assert "全部必需Tag同时有效率0.0%" in text + + +def test_joint_fit_failure_names_metric_and_selective_retry() -> None: + payload = { + "state": "PAUSED", + "reason": "joint_fit_check_failed", + "progress": 18 / 42, + "completed_sweeps": 18, + "total_sweeps": 42, + "active": { + "kind": "fit_failure", + "view": "front", + "motor_index": 6, + "joints": ["index_mcp_roll"], + "attempt": 1, + "directions_to_rescan": 6, + "failures": [ + { + "joint": "index_mcp_roll", + "metric": "arc_deg", + "actual": 3.98, + "limit": 15.0, + "comparison": "minimum", + } + ], + }, + "views": {}, + "result_path": "", + } + + text = render_three_camera_status_text_zh(payload) + + assert "食指MCP侧摆的实测圆弧为3.98°" in text + assert "要求至少15.00°" in text + assert "只清除当前失败关节的数据并重扫6个方向" in text + assert "第1次尝试" in text + assert "运动采样：" not in text + + +def test_return_baseline_prints_the_exact_command() -> None: + baseline = [255] * 20 + baseline[6:10] = [127, 127, 127, 127] + payload = { + "state": "RETURN_BASELINE", + "reason": "return_baseline_before_next_sweep", + "progress": 0.0, + "completed_sweeps": 0, + "total_sweeps": 42, + "baseline_command_u8": baseline, + "active": {}, + "views": {}, + "result_path": "", + } + + text = render_three_camera_status_text_zh(payload) + + assert "正在返回基准姿态（RETURN_BASELINE）" in text + assert f"正在确认基准姿态：{baseline}" in text + + +def test_status_numeric_diagnostics_are_json_serializable() -> None: + bins = [0, 16, 255] + payload = { + "state": "SWEEP", + "reason": "collecting_timestamp_synchronised_tag_centres", + "active": { + "sample": { + # np.diff返回NumPy标量；节点必须在放入状态前转成原生int。 + "maximum_bin_gap": int(max(np.diff(bins), default=0)), + } + }, + } + + encoded = json.dumps(payload, ensure_ascii=False) + + assert '"maximum_bin_gap": 239' in encoded + + +def test_index_roll_status_prints_clearance_motor_feedback() -> None: + payload = { + "state": "SWEEP", + "reason": "collecting_timestamp_synchronised_tag_centres", + "progress": 0.43, + "completed_sweeps": 18, + "total_sweeps": 42, + "active": { + "kind": "sweep", + "view": "front", + "motor_index": 6, + "joints": ["index_mcp_roll"], + "cycle": 1, + "repetitions": 3, + "start_u8": 255, + "target_u8": 0, + "actual_u8": 44.0, + "motion_progress": 0.827, + "valid_frames": 971, + "sample": {"minimum_u8": 40.0, "maximum_u8": 253.0}, + "auxiliary_motors": [ + {"motor_index": 7, "command_u8": 0, "actual_u8": 0.0}, + {"motor_index": 8, "command_u8": 0, "actual_u8": 1.0}, + {"motor_index": 9, "command_u8": 0, "actual_u8": 0.0}, + ], + "speed": { + "commanded_finger_speed": [15, 5, 15, 15, 15], + "reported_finger_speed": [15, 5, 15, 15, 15], + }, + }, + "views": {}, + "result_path": "", + } + + text = render_three_camera_status_text_zh(payload) + + assert "避挡姿态：电机7目标0、实际0.0" in text + assert "电机8目标0、实际1.0" in text + assert "电机9目标0、实际0.0" in text + assert "阶段速度：五指目标[15, 5, 15, 15, 15]" in text + assert "SDK报告[15, 5, 15, 15, 15]" in text diff --git a/src/g20_thumb_apriltag_calibration/test/test_three_camera_retry.py b/src/g20_thumb_apriltag_calibration/test/test_three_camera_retry.py new file mode 100644 index 0000000..ca933e7 --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/test/test_three_camera_retry.py @@ -0,0 +1,170 @@ +import json +import math +from types import SimpleNamespace + +from g20_thumb_apriltag_calibration.core import ( + DIRECTION_DECREASING, + DIRECTION_INCREASING, +) +from g20_thumb_apriltag_calibration.full_hand import SWEEP_SPECS +from g20_thumb_apriltag_calibration.three_camera_node import ( + G20ThreeCameraCalibrationNode, + SweepItem, +) + + +def _sweep_items() -> list[SweepItem]: + return [ + SweepItem(spec, cycle, direction) + for spec in SWEEP_SPECS + for cycle in range(3) + for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING) + ] + + +def _image_cycle_records(travels_rad: list[float]) -> list[dict]: + commands = list(range(0, 256, 16)) + [255] + records = [] + for cycle, travel in enumerate(travels_rad): + for direction in (DIRECTION_DECREASING, DIRECTION_INCREASING): + for command in commands: + angle = travel * (255.0 - command) / 255.0 + records.append( + { + "cycle": cycle, + "direction": direction, + "command_u8": command, + "relative_translation_xyz_m": [0.01, 0.0, 0.0], + "image_relative_xy_px": [ + 100.0 * math.cos(angle), + 100.0 * math.sin(angle), + ], + } + ) + return records + + +def _fit_check_node(records_by_joint: dict) -> SimpleNamespace: + node = SimpleNamespace( + records_by_joint=records_by_joint, + repetitions=3, + trajectory_maximum_plane_rms_m=0.004, + trajectory_maximum_radial_rms_m=0.004, + trajectory_minimum_radius_m=0.003, + trajectory_minimum_arc_rad=math.radians(15.0), + image_trajectory_maximum_radial_rms_px=2.0, + image_trajectory_maximum_radial_p95_px=3.5, + image_trajectory_minimum_radius_px=20.0, + trajectory_maximum_cycle_travel_difference_rad=math.radians(3.0), + passive_maximum_cycle_travel_difference_rad=math.radians(10.0), + maximum_monotonic_correction_rad=math.radians(2.0), + maximum_hysteresis_rad=math.radians(5.0), + passive_maximum_monotonic_correction_rad=math.radians(3.0), + passive_maximum_hysteresis_rad=math.radians(7.5), + zero_minimum_radius_px=20.0, + zero_maximum_radial_rms_px=2.0, + zero_maximum_radial_p95_px=3.5, + zero_maximum_round_difference_rad=math.radians(1.0), + ) + node._fit_joint_records = lambda name, records, relaxed=False: ( + G20ThreeCameraCalibrationNode._fit_joint_records( + node, name, records, relaxed=relaxed + ) + ) + return node + + +def test_fit_failure_rewinds_to_failed_specs_first_direction(tmp_path) -> None: + spec = next(item for item in SWEEP_SPECS if item.motor_index == 6) + node = SimpleNamespace( + sweep_items=_sweep_items(), + sweep_attempts={spec.motor_index: 1}, + retry_sweep_spec=None, + fit_failure={}, + sweep_index=24, + raw_path=tmp_path / "raw_samples.jsonl", + repetitions=3, + ) + node._sweep_spec_start_index = lambda selected: ( + G20ThreeCameraCalibrationNode._sweep_spec_start_index(node, selected) + ) + node._pause = lambda reason: setattr(node, "paused_reason", reason) + + G20ThreeCameraCalibrationNode._pause_for_provisional_fit_failure( + node, + spec, + [ + { + "joint": "index_mcp_roll", + "metric": "arc_deg", + "actual": 3.98, + "limit": 15.0, + "comparison": "minimum", + } + ], + ) + + assert node.sweep_index == 18 + assert node.retry_sweep_spec == spec + assert node.fit_failure["directions_to_rescan"] == 6 + assert node.paused_reason == "joint_fit_check_failed" + + +def test_resume_discards_only_failed_specs_samples(tmp_path) -> None: + spec = next(item for item in SWEEP_SPECS if item.motor_index == 15) + records = { + "thumb_mcp": [{"old": "mcp"}], + "thumb_ip": [{"old": "ip"}], + "thumb_cmc_pitch": [{"keep": True}], + } + node = SimpleNamespace( + retry_sweep_spec=spec, + records_by_joint=records, + sweep_attempts={spec.motor_index: 1}, + raw_path=tmp_path / "raw_samples.jsonl", + paused_reason="joint_fit_check_failed", + fit_failure={"failures": ["old"]}, + ) + + selected = G20ThreeCameraCalibrationNode._prepare_failed_sweep_retry(node) + + assert selected == spec + assert records["thumb_mcp"] == [] + assert records["thumb_ip"] == [] + assert records["thumb_cmc_pitch"] == [{"keep": True}] + assert node.sweep_attempts[15] == 2 + assert node.retry_sweep_spec is None + assert node.fit_failure == {} + event = json.loads((tmp_path / "raw_samples.jsonl").read_text()) + assert event == { + "kind": "retry", + "view": "front", + "motor_index": 15, + "joints": ["thumb_mcp", "thumb_ip"], + "attempt": 2, + "reason": "joint_fit_check_failed", + } + + +def test_provisional_fit_rejects_inconsistent_cycle_travel() -> None: + spec = next(item for item in SWEEP_SPECS if item.motor_index == 0) + node = _fit_check_node( + { + "thumb_cmc_pitch": _image_cycle_records( + [math.radians(47.0), math.radians(47.2), math.radians(55.0)] + ) + } + ) + + failures = G20ThreeCameraCalibrationNode._provisional_fit_failures( + node, spec + ) + + cycle_failure = next( + item + for item in failures + if item["metric"] == "cycle_travel_range_deg" + ) + assert cycle_failure["joint"] == "thumb_cmc_pitch" + assert cycle_failure["actual"] == 8.0 + assert cycle_failure["limit"] == 3.0