GrabBag/App/DiscHolePose/Doc/砂轮盘孔定位TCP协议文档.md

砂轮盘孔定位 / 砂轮盘架子定位 TCP 协议

1. 概述

• 传输协议：TCP/IP
• 数据编码：UTF-8 文本
• 服务模式：视觉侧作为 TCP 服务端
• 默认端口：7800
• 报文结束符：\r\n
• 当前模式：眼在手上

2. 请求报文

格式：

类型+相机索引_X_Y_Z_A_B_C\r\n

示例：

D1_-23.45_200.30_50.10_15.50_-2.30_45.00\r\n
R1_-23.45_200.30_50.10_15.50_-2.30_45.00\r\n

字段说明：

字段	说明
D1 / R1	D=砂轮盘孔检测，R=砂轮盘架子检测，后跟相机索引（1 或 2）
X	机械臂当前 X，单位 mm
Y	机械臂当前 Y，单位 mm
Z	机械臂当前 Z，单位 mm
A	机械臂当前姿态第 1 个角，单位 deg
B	机械臂当前姿态第 2 个角，单位 deg
C	机械臂当前姿态第 3 个角，单位 deg

3. 姿态输入顺序

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

支持顺序：

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

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

D1_-500.92_1553.084_267.836_176.850_74.55_89.211\r\n

表示：

X  = -500.92
Y  = 1553.084
Z  = 267.836
RZ = 176.850
RY = 74.55
RX = 89.211

4. 返回报文

格式：

PointNum_X_Y_Z_A_B_C\r\n

每次只发送一个目标（第一个），由 6 个浮点组成：目标点 (X/Y/Z + A/B/C)。

示例：

1_100.500_200.300_50.100_15.500_-2.300_45.000\r\n
0\r\n

字段说明：

字段	说明
PointNum	检测到的目标数量
X/Y/Z	目标中心坐标，单位 mm，保留 3 位小数
A/B/C	目标姿态，单位 deg，保留 3 位小数

5. 姿态输出顺序

返回报文中的 A/B/C 与请求报文一样，也由同一个 poseOutputOrder 控制。

这意味着：

• TCP 输入顺序
• TCP 输出顺序

使用同一个配置项统一控制，便于和机械臂侧对接。

6. 结果语义

• 砂轮盘孔检测输出：机械臂坐标系下的孔中心 目标点(X/Y/Z) + A/B/C
  • 孔姿态由法向量（normDir）经手眼变换后提取欧拉角
• 砂轮盘架子检测输出：机械臂坐标系下的架子中心 目标点(X/Y/Z) + A/B/C
  • 架子姿态由三轴向量（pose_x/pose_y/pose_z）构建旋转矩阵后提取欧拉角

说明：

• 内部姿态计算顺序由 eulerOrder 控制
• poseOutputOrder 只控制姿态字段在协议中的排列顺序
• 位置字段（X/Y/Z）保留 3 位小数，单位 mm
• 姿态字段 A/B/C 保留 3 位小数，单位 deg