GrabBag/App/TireHolePose/Doc/轮胎定位ModbusTCP协议文档.md

轮胎孔定位 ModbusTCP 协议

1. 概述

• 协议：Modbus TCP
• 默认端口：502
• 当前实现使用保持寄存器
• 内部实现端口：5020

2. 寄存器定义

地址	长度	访问	说明
0	1	写	触发检测，1=轮胎孔检测
1	1	读	工作状态，0=空闲，1=检测中，2=完成，3=异常
2	2	写	机械臂当前 X，float32
4	2	写	机械臂当前 Y，float32
6	2	写	机械臂当前 Z，float32
8	2	写	机械臂当前 A，float32
10	2	写	机械臂当前 B，float32
12	2	写	机械臂当前 C，float32
14	2	读	目标点 X，float32
16	2	读	目标点 Y，float32
18	2	读	目标点 Z，float32
20	2	读	目标点 A，float32
22	2	读	目标点 B，float32
24	2	读	目标点 C，float32

说明：

• 2~13 为机械臂当前位姿输入
• 14~25 为目标点输出（6 个 float32）
• 每个 float32 占 2 个保持寄存器
• 当前仅输出第一个目标结果

3. 姿态输入/输出顺序

A/B/C 的实际含义由网络配置中的 poseOutputOrder 统一控制。

支持顺序：

• RX-RY-RZ
• RX-RZ-RY
• RY-RX-RZ
• RY-RZ-RX
• RZ-RX-RY
• RZ-RY-RX

同一个配置同时作用于：

• Modbus 输入 A/B/C
• Modbus 输出 A/B/C

例如，当 poseOutputOrder = RZ-RY-RX 时：

• 地址 8~13 表示输入 RZ/RY/RX
• 地址 20~25 表示目标点输出 RZ/RY/RX

4. 工作状态说明

状态值	含义
0	空闲
1	检测中
2	检测完成
3	异常或请求不支持

5. 触发流程

1. 确认地址 1 当前为 0（空闲）
2. 将机械臂当前位姿写入 2~13
3. 向地址 0 写入 1（轮胎孔检测）
4. 轮询地址 1
5. 当地址 1=2 时，读取 14~25（目标点）
6. 机械臂运动到目标点 14~25
7. 当地址 1=3 时，本次检测失败

6. 结果语义

• 轮胎孔检测输出：机械臂坐标系下的孔中心 目标点(X/Y/Z) + A/B/C
  • 孔姿态由法向量（normDir）经手眼变换后提取欧拉角
  • 检测结果可能包含多个孔，Modbus 仅输出第一个

说明：

• 内部姿态求解顺序由 eulerOrder 控制
• poseOutputOrder 只控制寄存器中姿态字段的排列顺序
• 位置字段（X/Y/Z）保留 3 位小数，单位 mm
• 姿态字段 A/B/C 保留 3 位小数，单位 deg

7. 算法说明

使用 workpieceHolePositioning 算法库：

• 轮胎孔定位：sx_getTireHolePose()
  • 输入：扫描线数据 + 角点检测参数（cornerParam）+ 地面标定参数（groundCalibPara）+ 轮胎参数（tireParam: diameter, thickness）
  • 输出：孔中心位置列表 + 法向量方向列表（vector<WD_HolePositionInfo>）

8. TCP 文本协议

除 ModbusTCP 外，还支持 TCP 文本协议（默认端口 7800）：

• 请求：D1_X_Y_Z_A_B_C\r\n（D=轮胎孔检测）
  • 数字表示相机编号（1 或 2）
• 成功响应：count_X_Y_Z_A_B_C\r\n（6 个 float：目标点）
• 失败响应：0\r\n