diff --git a/App/ScrewPosition/Doc/螺杆定位ModbusTCP协议文档.md b/App/ScrewPosition/Doc/螺杆定位ModbusTCP协议文档.md index 334b5cc..8d74d3f 100644 --- a/App/ScrewPosition/Doc/螺杆定位ModbusTCP协议文档.md +++ b/App/ScrewPosition/Doc/螺杆定位ModbusTCP协议文档.md @@ -1,106 +1,84 @@ -# 螺杆/工具盘定位 ModbusTCP 协议文档 +# 螺杆定位 / 工具盘定位 ModbusTCP 协议 ## 1. 概述 -- 协议: Modbus TCP -- 服务端默认端口: `5020` +- 协议:Modbus TCP +- 默认端口:`5020` - 当前实现使用保持寄存器 -- 地址说明均为保持寄存器地址 ## 2. 寄存器定义 | 地址 | 长度 | 访问 | 说明 | -|------|------|------|------| -| `0` | 1 | 写 | 触发检测,`1=螺杆检测`,`2=工具盘检测`,写入后服务端会自动清零 | +|---|---:|---|---| +| `0` | 1 | 写 | 触发检测,`1=螺杆检测`,`2=工具盘检测` | | `1` | 1 | 读 | 工作状态,`0=空闲`,`1=检测中`,`2=完成`,`3=异常` | -| `2` | 2 | 写 | 机械臂 `X`,`float32` | -| `4` | 2 | 写 | 机械臂 `Y`,`float32` | -| `6` | 2 | 写 | 机械臂 `Z`,`float32` | -| `8` | 2 | 写 | 机械臂 `RX`,`float32` | -| `10` | 2 | 写 | 机械臂 `RY`,`float32` | -| `12` | 2 | 写 | 机械臂 `RZ`,`float32` | +| `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 | 读 | 结果 `RX`,`float32` | -| `22` | 2 | 读 | 结果 `RY`,`float32` | -| `24` | 2 | 读 | 结果 `RZ`,`float32` | +| `20` | 2 | 读 | 结果 `A`,`float32` | +| `22` | 2 | 读 | 结果 `B`,`float32` | +| `24` | 2 | 读 | 结果 `C`,`float32` | -说明: +说明: -- 地址 `2~13` 为机械臂当前位姿(眼在手上模式),触发前写入 -- 地址 `14~25` 为检测结果输出 `XYZ RX RY RZ` -- 每个 `float32` 占用 2 个保持寄存器,字序为高字在前、低字在后 -- 当前版本只输出首个目标位姿 -- 当没有结果或检测失败时,`14~25` 地址返回全 0 +- `2~13` 为机械臂当前位姿输入 +- `14~25` 为检测结果输出 +- 每个 `float32` 占 2 个保持寄存器 +- 当前仅输出第一个目标结果 -## 3. 触发方式 +## 3. 姿态输入/输出顺序 -### 3.1 写入机械臂位姿 +`A/B/C` 的实际含义由网络配置中的 `poseOutputOrder` 统一控制。 -触发前,先将机械臂当前位姿写入地址 `2~13`。 +支持顺序: -### 3.2 触发螺杆检测 +- `RX-RY-RZ` +- `RX-RZ-RY` +- `RY-RX-RZ` +- `RY-RZ-RX` +- `RZ-RX-RY` +- `RZ-RY-RX` -向地址 `0` 写入: +同一个配置同时作用于: -```text -1 -``` +- Modbus 输入 `A/B/C` +- Modbus 输出 `A/B/C` -### 3.3 触发工具盘检测 +例如,当 `poseOutputOrder = RZ-RY-RX` 时: -向地址 `0` 写入: - -```text -2 -``` - -工具盘检测会调用 `sx_getLocationPlatePose` 算法,输出定位盘中心位姿。 +- 地址 `8~13` 表示输入 `RZ/RY/RX` +- 地址 `20~25` 表示输出 `RZ/RY/RX` ## 4. 工作状态说明 | 状态值 | 含义 | -|--------|------| -| `0` | 空闲,可接收下一次触发 | +|---:|---| +| `0` | 空闲 | | `1` | 检测中 | -| `2` | 检测完成,结果已写入地址 `14~25` | -| `3` | 检测异常或当前请求不被支持 | +| `2` | 检测完成 | +| `3` | 异常或请求不支持 | -## 5. 位姿数据格式 +## 5. 触发流程 -### 机械臂位姿(输入,地址 2~13) - -| 字段 | 地址范围 | -|------|----------| -| `X` | `2~3` | -| `Y` | `4~5` | -| `Z` | `6~7` | -| `RX` | `8~9` | -| `RY` | `10~11` | -| `RZ` | `12~13` | - -### 检测结果(输出,地址 14~25) - -| 字段 | 地址范围 | -|------|----------| -| `X` | `14~15` | -| `Y` | `16~17` | -| `Z` | `18~19` | -| `RX` | `20~21` | -| `RY` | `22~23` | -| `RZ` | `24~25` | - -其中: - -- 螺杆检测输出为机械臂坐标系下的 `XYZ + RX/RY/RZ` -- 工具盘检测输出为机械臂坐标系下的定位盘中心 `XYZ + RX/RY/RZ`(法向量转欧拉角) - -## 6. 典型流程 - -1. 读取地址 `1`,确认当前为 `0` -2. 将机械臂当前位姿写入地址 `2~13` +1. 确认地址 `1` 当前为 `0` +2. 将机械臂当前位姿写入 `2~13` 3. 向地址 `0` 写入 `1` 或 `2` 4. 轮询地址 `1` -5. 当地址 `1=2` 时,读取地址 `14~25` +5. 当地址 `1=2` 时,读取 `14~25` 6. 当地址 `1=3` 时,本次检测失败 + +## 6. 结果语义 + +- 螺杆检测输出:机械臂坐标系下的 `XYZ + A/B/C` +- 工具盘检测输出:机械臂坐标系下的定位盘中心 `XYZ + A/B/C` + +说明: + +- 内部姿态求解顺序由 `eulerOrder` 控制 +- `poseOutputOrder` 只控制寄存器中姿态字段的排列顺序 diff --git a/App/ScrewPosition/Doc/螺杆定位TCP协议文档.md b/App/ScrewPosition/Doc/螺杆定位TCP协议文档.md index fe9796c..0273398 100644 --- a/App/ScrewPosition/Doc/螺杆定位TCP协议文档.md +++ b/App/ScrewPosition/Doc/螺杆定位TCP协议文档.md @@ -1,78 +1,111 @@ -# 螺杆/工具盘定位 TCP/IP 通信协议文档 +# 螺杆定位 / 工具盘定位 TCP 协议 -## 1. 协议概述 +## 1. 概述 -- 传输协议: TCP/IP -- 数据格式: 文本(UTF-8),以换行符 `\n` 分隔 -- 服务模式: 服务端,支持多客户端连接 -- 默认端口: 7800 -- 模式: 眼在手上(Eye-in-Hand) +- 传输协议:TCP/IP +- 数据编码:UTF-8 文本 +- 服务模式:视觉侧作为 TCP 服务端 +- 默认端口:`7800` +- 报文结束符:`\r\n` +- 当前模式:眼在手上 -## 2. 触发格式(客户端→服务端) +## 2. 请求报文 -格式: `类型+相机索引_X_Y_Z_RX_RY_RZ\n`,字段以 `_` 分隔。 - -### 2.1 螺杆检测 +格式: ```text -S1_-23.45_200.30_50.10_15.50_-2.30_45.00\n +类型+相机索引_X_Y_Z_A_B_C\r\n ``` -### 2.2 工具盘检测 +示例: ```text -T1_-23.45_200.30_50.10_15.50_-2.30_45.00\n +S1_-23.45_200.30_50.10_15.50_-2.30_45.00\r\n +T1_-23.45_200.30_50.10_15.50_-2.30_45.00\r\n ``` -字段说明: - -| 位置 | 说明 | -|------|------| -| 第1段 | `S`=螺杆 / `T`=工具盘,后跟相机索引(1 或 2) | -| 第2段 | 机械臂当前 X(mm) | -| 第3段 | 机械臂当前 Y(mm) | -| 第4段 | 机械臂当前 Z(mm) | -| 第5段 | 机械臂当前 RX(deg) | -| 第6段 | 机械臂当前 RY(deg) | -| 第7段 | 机械臂当前 RZ(deg) | - -## 3. 结果格式(服务端→客户端) - -```text -PointNum_X1_Y1_Z1_RX1_RY1_RZ1/X2_Y2_Z2_RX2_RY2_RZ2/\n -``` - -示例(检测到 2 个目标): - -```text -2_100.50_200.30_50.10_15.50_-2.30_45.00/120.00_210.50_48.00_-5.00_1.20_30.00/\n -``` - -示例(检测到 0 个目标): - -```text -0\n -``` - -字段说明: +字段说明: | 字段 | 说明 | -|------|------| -| 第一个数字 | 检测到的目标数量 | -| `_` | 同一目标各分量分隔符 | -| `/` | 不同目标之间分隔符 | -| XYZ | 目标中心坐标(mm) | -| RX/RY/RZ | 目标姿态角(deg) | +|---|---| +| `S1` / `T1` | `S`=螺杆检测,`T`=工具盘检测,后跟相机索引 | +| `X` | 机械臂当前 X,单位 mm | +| `Y` | 机械臂当前 Y,单位 mm | +| `Z` | 机械臂当前 Z,单位 mm | +| `A` | 机械臂当前姿态第 1 个角,单位 deg | +| `B` | 机械臂当前姿态第 2 个角,单位 deg | +| `C` | 机械臂当前姿态第 3 个角,单位 deg | -## 4. 位姿字段说明 +## 3. 姿态输入顺序 -| 字段 | 类型 | 单位 | 说明 | -|------|------|------|------| -| `X` | double | mm | 目标中心点 X | -| `Y` | double | mm | 目标中心点 Y | -| `Z` | double | mm | 目标中心点 Z | -| `RX` | double | deg | 绕 X 轴旋转角 | -| `RY` | double | deg | 绕 Y 轴旋转角 | -| `RZ` | double | deg | 绕 Z 轴旋转角 | +`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` 时: + +```text +T1_-500.92_1553.084_267.836_176.850_74.55_89.211\r\n +``` + +表示: + +```text +X = -500.92 +Y = 1553.084 +Z = 267.836 +RZ = 176.850 +RY = 74.55 +RX = 89.211 +``` + +## 4. 返回报文 + +格式: + +```text +PointNum_X1_Y1_Z1_A1_B1_C1/X2_Y2_Z2_A2_B2_C2/\r\n +``` + +示例: + +```text +2_100.50_200.30_50.10_15.50_-2.30_45.00/120.00_210.50_48.00_-5.00_1.20_30.00/\r\n +0\r\n +``` + +字段说明: + +| 字段 | 说明 | +|---|---| +| `PointNum` | 检测到的目标数量 | +| `X/Y/Z` | 目标中心坐标,单位 mm | +| `A/B/C` | 目标姿态三个角,单位 deg | + +## 5. 姿态输出顺序 + +返回报文中的 `A/B/C` 与请求报文一样,也由同一个 `poseOutputOrder` 控制。 + +这意味着: + +- TCP 输入顺序 +- TCP 输出顺序 + +使用同一个配置项统一控制,便于和机械臂侧对接。 + +## 6. 结果语义 + +- 螺杆检测输出:机械臂坐标系下的 `XYZ + A/B/C` +- 工具盘检测输出:机械臂坐标系下的定位盘中心 `XYZ + A/B/C` + +说明: + +- 内部姿态计算顺序由 `eulerOrder` 控制 +- `poseOutputOrder` 只控制姿态字段在协议中的排列顺序 diff --git a/App/ScrewPosition/ScrewPositionApp/Presenter/Inc/DetectPresenter.h b/App/ScrewPosition/ScrewPositionApp/Presenter/Inc/DetectPresenter.h index 6b1f9fa..3acac7a 100644 --- a/App/ScrewPosition/ScrewPositionApp/Presenter/Inc/DetectPresenter.h +++ b/App/ScrewPosition/ScrewPositionApp/Presenter/Inc/DetectPresenter.h @@ -35,6 +35,7 @@ public: const double clibMatrix[16], const RobotPose6D& robotPose, int eulerOrder, + int outputEulerOrder, int poseOutputOrder, int dirVectorInvert, int longAxisDir, @@ -49,6 +50,7 @@ public: const double clibMatrix[16], const RobotPose6D& robotPose, int eulerOrder, + int outputEulerOrder, int poseOutputOrder, int dirVectorInvert, int longAxisDir, diff --git a/App/ScrewPosition/ScrewPositionApp/Presenter/Inc/ScrewPositionTCPProtocol.h b/App/ScrewPosition/ScrewPositionApp/Presenter/Inc/ScrewPositionTCPProtocol.h index 9b9f846..b5b97a3 100644 --- a/App/ScrewPosition/ScrewPositionApp/Presenter/Inc/ScrewPositionTCPProtocol.h +++ b/App/ScrewPosition/ScrewPositionApp/Presenter/Inc/ScrewPositionTCPProtocol.h @@ -2,51 +2,32 @@ #define SCREWPOSITIONTCPPROTOCOL_H #include -#include -#include #include -#include -#include "IYTCPServer.h" -#include "IVrConfig.h" +#include +#include + +#include + +#include "IVrConfig.h" +#include "IYTCPServer.h" -/** - * @brief 机器人法兰位姿(触发信号中携带) - */ struct RobotPose6D { double x = 0.0; double y = 0.0; double z = 0.0; - double rx = 0.0; - double ry = 0.0; - double rz = 0.0; + double a = 0.0; + double b = 0.0; + double c = 0.0; }; -/** - * @brief ScrewPosition 文本协议 TCP 服务器 - * - * 触发格式(客户端→服务端,换行符结尾): - * S1 RbtX-23.45 RbtY-23.45 RbtZ-23.45 RbtRX-23.45 RbtRY-123.45 RbtRZ-23.45\n - * S 表示螺杆检测,T 表示工具盘检测,后跟相机索引 - * - * 返回格式(服务端→客户端): - * PointNum_X1_Y1_Z1_RX1_RY1_RZ1/X2_Y2_Z2_RX2_RY2_RZ2/\n - */ class ScrewPositionTCPProtocol { public: using ConnectionCallback = std::function; - - /** - * @brief 检测触发回调 - * @param cameraIndex 相机索引 - * @param detectionType 检测类型 - * @param robotPose 机器人当前位姿 - * @return 是否成功触发 - */ using DetectionTriggerCallback = std::function; + DetectionType detectionType, + const RobotPose6D& robotPose)>; ScrewPositionTCPProtocol(); ~ScrewPositionTCPProtocol(); @@ -58,11 +39,9 @@ public: void SetConnectionCallback(const ConnectionCallback& callback); void SetDetectionTriggerCallback(const DetectionTriggerCallback& callback); - /** - * @brief 发送文本结果 - * @param text 文本内容(不含换行,内部自动添加) - * @param pClient nullptr 则广播 - */ + // Protocol: + // request : T1_X_Y_Z_A_B_C\r\n + // response: PointNum_X1_Y1_Z1_A1_B1_C1/.../\r\n int SendTextResult(const std::string& text, const TCPClient* pClient = nullptr); private: diff --git a/App/ScrewPosition/ScrewPositionApp/Presenter/Src/DetectPresenter.cpp b/App/ScrewPosition/ScrewPositionApp/Presenter/Src/DetectPresenter.cpp index 646e162..da9c029 100644 --- a/App/ScrewPosition/ScrewPositionApp/Presenter/Src/DetectPresenter.cpp +++ b/App/ScrewPosition/ScrewPositionApp/Presenter/Src/DetectPresenter.cpp @@ -58,32 +58,29 @@ QImage BuildToolDiskPointCloudImage(const std::vector 1.0) { - return 1.0; + switch (poseOutputOrder) { + case 1: + rxDeg = robotPose.a; + ryDeg = robotPose.b; + rzDeg = robotPose.c; + break; + case 2: + rxDeg = robotPose.b; + ryDeg = robotPose.a; + rzDeg = robotPose.c; + break; + case 3: + rxDeg = robotPose.c; + ryDeg = robotPose.a; + rzDeg = robotPose.b; + break; + case 4: + rxDeg = robotPose.b; + ryDeg = robotPose.c; + rzDeg = robotPose.a; + break; + case 5: + rxDeg = robotPose.c; + ryDeg = robotPose.b; + rzDeg = robotPose.a; + break; + case 0: + default: + rxDeg = robotPose.a; + ryDeg = robotPose.b; + rzDeg = robotPose.c; + break; } - if (value < -1.0) { - return -1.0; - } - return value; } +// 将角度归一化到 (-180°, 180°] +double WrapDegreesTo180(double deg) +{ + while (deg > 180.0) { + deg -= 360.0; + } + while (deg <= -180.0) { + deg += 360.0; + } + return deg; +} + +// 对 Tait-Bryan 欧拉角做等价归一化,使 pitch ∈ [-90°, 90°] +// 依据:(α, β, γ) ≡ (α±180°, 180°-β, γ±180°) 以及 (α±180°, -180°-β, γ±180°) +// 适用于 CTEulerOrder 中所有 12 种 Tait-Bryan 顺序(内旋/外旋一致) +void NormalizePitchRange(double& rollDeg, double& pitchDeg, double& yawDeg) +{ + pitchDeg = WrapDegreesTo180(pitchDeg); + if (pitchDeg > 90.0) { + pitchDeg = 180.0 - pitchDeg; + rollDeg += 180.0; + yawDeg += 180.0; + } else if (pitchDeg < -90.0) { + pitchDeg = -180.0 - pitchDeg; + rollDeg += 180.0; + yawDeg += 180.0; + } + rollDeg = WrapDegreesTo180(rollDeg); + yawDeg = WrapDegreesTo180(yawDeg); +} + +// 统一走 CCoordinateTransform::rotationMatrixToEuler(与 eulerToRotationMatrix 成对互逆), +// 再把 pitch 压入 [-90°, 90°],保证对机器人侧输出的姿态语义稳定。 void RotationMatrixToConfiguredEulerDegrees(const CTRotationMatrix& rotation, CTEulerOrder order, double& rollDeg, double& pitchDeg, double& yawDeg) { - if (order == CTEulerOrder::sZYX) { - // External Z-Y-X: R = Rz(yaw) * Ry(pitch) * Rx(roll) - // Keep this decomposition aligned with the robot-side convention used in ScrewPosition. - const double pitch = std::asin(ClampUnit(rotation.at(0, 2))); - const double cosPitch = std::cos(pitch); - constexpr double kSingularThreshold = 1e-6; + const CTEulerAngles euler = CCoordinateTransform::rotationMatrixToEuler(rotation, order); + euler.toDegrees(rollDeg, pitchDeg, yawDeg); + NormalizePitchRange(rollDeg, pitchDeg, yawDeg); +} - double roll = 0.0; - double yaw = 0.0; - if (std::abs(cosPitch) > kSingularThreshold) { - yaw = std::atan2(-rotation.at(0, 1) / cosPitch, - rotation.at(0, 0) / cosPitch); - roll = std::atan2(rotation.at(1, 2) / cosPitch, - rotation.at(2, 2) / cosPitch); - } else { - // Near gimbal lock, keep yaw fixed and solve the residual X rotation. - yaw = 0.0; - roll = std::atan2(rotation.at(1, 0), rotation.at(1, 1)); - } - - rollDeg = roll * 180.0 / M_PI; - pitchDeg = pitch * 180.0 / M_PI; - yawDeg = yaw * 180.0 / M_PI; +// 万向锁消歧:当 |pitch| 接近 90° 时,Rx(roll)·Ry(pitch)·Rz(yaw) 的分解退化, +// 任意满足 roll+yaw = const (pitch>0) 或 roll-yaw = const (pitch<0) 的组合 +// 都表示同一旋转矩阵。本函数把 yaw 锚定到 refYaw 附近,把余下自由度全部分配到 roll, +// 让不同帧的输出稳定落在参考附近(通常传入机器人法兰的 rx、rz 作为参考)。 +// +// 【强消歧配置】:阈值覆盖 80°-85°,优先保证数值稳定性,代价是 pitch∈[80°,85°] +// 的过渡带会改变真实旋转矩阵(X 轴可能偏 3°-5°)。如需精确定姿请收窄阈值或换欧拉顺序。 +// |pitch| < kSoftThreshold:完全不改(保留真实分解) +// |pitch| ≥ kHardThreshold:完全锁到参考(对 R 影响最小) +// kSoftThreshold ≤ |pitch| < kHardThreshold:线性插值过渡 +void ResolveGimbalAmbiguity(double& rollDeg, double pitchDeg, double& yawDeg, + double refRollDeg, double refYawDeg) +{ + constexpr double kSoftThresholdDeg = 80.0; + constexpr double kHardThresholdDeg = 85.0; + const double absPitch = std::abs(pitchDeg); + if (absPitch < kSoftThresholdDeg) { return; } - const CTEulerAngles robotEuler = CCoordinateTransform::rotationMatrixToEuler(rotation, order); - robotEuler.toDegrees(rollDeg, pitchDeg, yawDeg); + // 计算"完全消歧"后的参考目标 + const bool positivePitch = (pitchDeg >= 0.0); + const double sum = positivePitch ? (rollDeg + yawDeg) : (rollDeg - yawDeg); + const double refSum = positivePitch ? (refRollDeg + refYawDeg) : (refRollDeg - refYawDeg); + + double adjustedSum = sum; + while (adjustedSum - refSum > 180.0) { adjustedSum -= 360.0; } + while (adjustedSum - refSum < -180.0) { adjustedSum += 360.0; } + + const double fullYaw = WrapDegreesTo180(refYawDeg); + const double fullRoll = WrapDegreesTo180(positivePitch ? (adjustedSum - fullYaw) + : (adjustedSum + fullYaw)); + + if (absPitch >= kHardThresholdDeg) { + rollDeg = fullRoll; + yawDeg = fullYaw; + return; + } + + // 过渡带:alpha 从 0(kSoftThreshold)线性增到 1(kHardThreshold) + const double alpha = (absPitch - kSoftThresholdDeg) / + (kHardThresholdDeg - kSoftThresholdDeg); + auto lerpAngle = [alpha](double from, double to) { + const double diff = WrapDegreesTo180(to - from); + return WrapDegreesTo180(from + alpha * diff); + }; + rollDeg = lerpAngle(rollDeg, fullRoll); + yawDeg = lerpAngle(yawDeg, fullYaw); } CTVec3D NormalizeVector(const CTVec3D& vector) @@ -416,6 +499,7 @@ int DetectPresenter::DetectScrew( const double clibMatrix[16], const RobotPose6D& robotPose, int eulerOrder, + int outputEulerOrder, int poseOutputOrder, int dirVectorInvert, int longAxisDir, @@ -455,8 +539,8 @@ int DetectPresenter::DetectScrew( growParam.yDeviation_max, growParam.zDeviation_max, growParam.maxLineSkipNum, growParam.maxSkipDistance, growParam.minLTypeTreeLen, growParam.minVTypeTreeLen); LOG_INFO("[Algo Thread] Filter: continuityTh=%.1f, outlierTh=%.1f\n", filterParam.continuityTh, filterParam.outlierTh); - LOG_INFO("[Algo Thread] Pose Config: eulerOrder=%d, poseOutputOrder=%d, dirVectorInvert=%d, longAxisDir=%d\n", - eulerOrder, poseOutputOrder, dirVectorInvert, longAxisDir); + LOG_INFO("[Algo Thread] Pose Config: eulerOrder=%d, outputEulerOrder=%d, poseOutputOrder=%d, dirVectorInvert=%d, longAxisDir=%d\n", + eulerOrder, outputEulerOrder, poseOutputOrder, dirVectorInvert, longAxisDir); } int errCode = 0; @@ -486,15 +570,19 @@ int DetectPresenter::DetectScrew( detectionResult.image = BuildScrewPointCloudImage(xyzData, screwInfo); const CTHomogeneousMatrix handEyeMatrix = BuildHandEyeMatrix(clibMatrix); const CTEulerOrder order = static_cast(eulerOrder); - const CTRobotPose flangePose = CTRobotPose::fromDegrees(robotPose.x, robotPose.y, robotPose.z, robotPose.rx, robotPose.ry, robotPose.rz); + const CTEulerOrder outputOrder = static_cast(outputEulerOrder); + + double rx, ry, rz; + ResolveRobotPoseAnglesDegrees(robotPose, poseOutputOrder, rx, ry, rz); + const CTRobotPose flangePose = CTRobotPose::fromDegrees(robotPose.x, robotPose.y, robotPose.z, rx, ry, rz); const CTRobotPoseConvention poseConvention = ResolveRobotPoseConvention(poseOutputOrder); + // 输入侧用 eulerOrder(机器人法兰 (rx,ry,rz) 的合成约定) const CTHomogeneousMatrix eyeInHandTransform = CCoordinateTransform::sixAxisEyeInHandBuildTransform( flangePose, order, handEyeMatrix, poseConvention); if (debugParam.enableDebug && debugParam.printDetailLog) { LOG_INFO("[Algo Thread] Robot flange pose fields: X=%.3f, Y=%.3f, Z=%.3f, A1=%.3f, A2=%.3f, A3=%.3f\n", - robotPose.x, robotPose.y, robotPose.z, - robotPose.rx, robotPose.ry, robotPose.rz); + robotPose.x, robotPose.y, robotPose.z, rx, ry, rz); } for (size_t i = 0; i < screwInfo.size(); ++i) { @@ -523,7 +611,8 @@ int DetectPresenter::DetectScrew( frameReady = TransformAxes(eyeAxes, eyeInHandTransform, robotAxes); if (frameReady) { const CTRotationMatrix rotation = BuildRotationMatrix(robotAxes); - RotationMatrixToConfiguredEulerDegrees(rotation, order, pos.roll, pos.pitch, pos.yaw); + RotationMatrixToConfiguredEulerDegrees(rotation, outputOrder, pos.roll, pos.pitch, pos.yaw); + ResolveGimbalAmbiguity(pos.roll, pos.pitch, pos.yaw, rx, rz); } } } @@ -568,6 +657,7 @@ int DetectPresenter::DetectToolDisk( const double clibMatrix[16], const RobotPose6D& robotPose, int eulerOrder, + int outputEulerOrder, int poseOutputOrder, int dirVectorInvert, int longAxisDir, @@ -595,10 +685,11 @@ int DetectPresenter::DetectToolDisk( clibMatrix[4], clibMatrix[5], clibMatrix[6], clibMatrix[7], clibMatrix[8], clibMatrix[9], clibMatrix[10], clibMatrix[11], clibMatrix[12], clibMatrix[13], clibMatrix[14], clibMatrix[15]); + LOG_INFO("[Algo Thread] ToolDisk Corner: cornerTh=%.1f, scale=%.1f, minEndingGap=%.1f, minEndingGap_z=%.1f\n", cornerParam.cornerTh, cornerParam.scale, cornerParam.minEndingGap, cornerParam.minEndingGap_z); - LOG_INFO("[Algo Thread] ToolDisk Pose Config: eulerOrder=%d, poseOutputOrder=%d, dirVectorInvert=%d, longAxisDir=%d\n", - eulerOrder, poseOutputOrder, dirVectorInvert, longAxisDir); + LOG_INFO("[Algo Thread] ToolDisk Pose Config: eulerOrder=%d, outputEulerOrder=%d, poseOutputOrder=%d, dirVectorInvert=%d, longAxisDir=%d\n", + eulerOrder, outputEulerOrder, poseOutputOrder, dirVectorInvert, longAxisDir); } int errCode = 0; @@ -614,16 +705,18 @@ int DetectPresenter::DetectToolDisk( detectionResult.success = true; detectionResult.errorCode = 0; - detectionResult.message = QStringLiteral("\u5de5\u5177\u76d8\u68c0\u6d4b\u6210\u529f"); + detectionResult.message = QStringLiteral("工具盘检测成功"); detectionResult.image = BuildToolDiskPointCloudImage(xyzData, poseInfo, true); const CTHomogeneousMatrix handEyeMatrix = BuildHandEyeMatrix(clibMatrix); const CTEulerOrder order = static_cast(eulerOrder); - const CTRobotPose flangePose = CTRobotPose::fromDegrees( - robotPose.x, robotPose.y, robotPose.z, - robotPose.rx, robotPose.ry, robotPose.rz); + const CTEulerOrder outputOrder = static_cast(outputEulerOrder); + + double rx, ry, rz; + ResolveRobotPoseAnglesDegrees(robotPose, poseOutputOrder, rx, ry, rz); + const CTRobotPose flangePose = CTRobotPose::fromDegrees( robotPose.x, robotPose.y, robotPose.z, robotPose.a, robotPose.b, robotPose.c); const CTRobotPoseConvention poseConvention = ResolveRobotPoseConvention(poseOutputOrder); - const CTHomogeneousMatrix eyeInHandTransform = CCoordinateTransform::sixAxisEyeInHandBuildTransform( - flangePose, order, handEyeMatrix, poseConvention); + // 输入侧用 eulerOrder(机器人法兰 (rx,ry,rz) 的合成约定) + const CTHomogeneousMatrix eyeInHandTransform = CCoordinateTransform::sixAxisEyeInHandBuildTransform(flangePose, order, handEyeMatrix, poseConvention); // 将定位盘中心点通过手眼标定转换为机器人坐标 const CTVec3D robotCenter = eyeInHandTransform.transformPoint(ToCTVec3D(poseInfo.center)); @@ -641,26 +734,82 @@ int DetectPresenter::DetectToolDisk( double eyeYaw = 0.0; bool eyeEulerReady = false; // 法向量转欧拉角:roll=0, pitch=atan2(-nz, sqrt(nx^2+ny^2)), yaw=atan2(ny, nx) - const bool useConfiguredPose = true; + const bool usePointBasedPose = true; // 使用基于点位置的姿态计算方案 bool frameReady = false; - { + + if (usePointBasedPose) { + // 1. Eye 坐标系下的 XDir / YDir / ZDir 轴(直接使用算法输出) + const std::array eyeAxes = {eyeXAxis, eyeYAxis, eyeNormalDir}; + + // 2. 构建 Eye 旋转矩阵(列向量形式:每列对应一个轴在 Eye 坐标系下的表达) + const CTRotationMatrix eyeRotation = BuildRotationMatrix(eyeAxes); + + // 3. Eye 矩阵转欧拉角(按 outputEulerOrder,β ∈ [-90°, 90°]) + RotationMatrixToConfiguredEulerDegrees(eyeRotation, outputOrder, eyeRoll, eyePitch, eyeYaw); + eyeEulerReady = true; + + // 4. 将 Eye 轴向量经手眼矩阵转换到 Robot 坐标系 + std::array robotAxes; + frameReady = TransformAxes(eyeAxes, eyeInHandTransform, robotAxes); + + if (frameReady) { + // 5. 构建 Robot 旋转矩阵 + const CTRotationMatrix robotRotation = BuildRotationMatrix(robotAxes); + + // 6. Robot 矩阵转欧拉角(按 outputEulerOrder,β ∈ [-90°, 90°]) + RotationMatrixToConfiguredEulerDegrees(robotRotation, outputOrder, pos.roll, pos.pitch, pos.yaw); + ResolveGimbalAmbiguity(pos.roll, pos.pitch, pos.yaw, rx, rz); + + if (debugParam.enableDebug && debugParam.printDetailLog) { + LOG_INFO("[Algo Thread] === Tool Disk Pose Debug ===\n"); + LOG_INFO("[Algo Thread] Eye XDir: (%.6f, %.6f, %.6f)\n", eyeAxes[0].x, eyeAxes[0].y, eyeAxes[0].z); + LOG_INFO("[Algo Thread] Eye YDir: (%.6f, %.6f, %.6f)\n", eyeAxes[1].x, eyeAxes[1].y, eyeAxes[1].z); + LOG_INFO("[Algo Thread] Eye ZDir: (%.6f, %.6f, %.6f)\n", eyeAxes[2].x, eyeAxes[2].y, eyeAxes[2].z); + LOG_INFO("[Algo Thread] Eye Rotation Matrix:\n"); + LOG_INFO(" [%.6f, %.6f, %.6f]\n", eyeRotation.at(0, 0), eyeRotation.at(0, 1), eyeRotation.at(0, 2)); + LOG_INFO(" [%.6f, %.6f, %.6f]\n", eyeRotation.at(1, 0), eyeRotation.at(1, 1), eyeRotation.at(1, 2)); + LOG_INFO(" [%.6f, %.6f, %.6f]\n", eyeRotation.at(2, 0), eyeRotation.at(2, 1), eyeRotation.at(2, 2)); + LOG_INFO("[Algo Thread] Eye Pos: (%.6f, %.6f, %.6f)\n", poseInfo.center.x, poseInfo.center.y, poseInfo.center.z); + LOG_INFO("[Algo Thread] Eye Euler (outputOrder=%d ): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n", + static_cast(outputOrder), eyeRoll, eyePitch, eyeYaw); + + LOG_INFO("[Algo Thread] Robot XDir: (%.6f, %.6f, %.6f)\n", robotAxes[0].x, robotAxes[0].y, robotAxes[0].z); + LOG_INFO("[Algo Thread] Robot YDir: (%.6f, %.6f, %.6f)\n", robotAxes[1].x, robotAxes[1].y, robotAxes[1].z); + LOG_INFO("[Algo Thread] Robot ZDir: (%.6f, %.6f, %.6f)\n", robotAxes[2].x, robotAxes[2].y, robotAxes[2].z); + LOG_INFO("[Algo Thread] Robot Rotation Matrix:\n"); + LOG_INFO(" [%.6f, %.6f, %.6f]\n", robotRotation.at(0, 0), robotRotation.at(0, 1), robotRotation.at(0, 2)); + LOG_INFO(" [%.6f, %.6f, %.6f]\n", robotRotation.at(1, 0), robotRotation.at(1, 1), robotRotation.at(1, 2)); + LOG_INFO(" [%.6f, %.6f, %.6f]\n", robotRotation.at(2, 0), robotRotation.at(2, 1), robotRotation.at(2, 2)); + LOG_INFO("[Algo Thread] Robot Pos: (%.6f, %.6f, %.6f)\n", robotCenter.x, robotCenter.y, robotCenter.z); + LOG_INFO("[Algo Thread] Robot Pos Euler (outputOrder=%d ): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n", + static_cast(outputOrder), pos.roll, pos.pitch, pos.yaw); + } + } + } + + if (!frameReady) { + // std::array eyeAxes; + + // 标准构建:直接使用算法输出的轴 frameReady = BuildFrameFromXYAxes(eyeXAxis, eyeYAxis, eyeNormalDir, eyeAxes); if (!frameReady) { frameReady = BuildFrameFromZAxis(eyeNormalDir, eyeAxes); } + if (frameReady) { ApplyLongAxisDir(eyeAxes, longAxisDir); ApplyDirVectorInvert(eyeAxes, dirVectorInvert); const CTRotationMatrix eyeRotation = BuildRotationMatrix(eyeAxes); - RotationMatrixToConfiguredEulerDegrees(eyeRotation, order, eyeRoll, eyePitch, eyeYaw); + RotationMatrixToConfiguredEulerDegrees(eyeRotation, outputOrder, eyeRoll, eyePitch, eyeYaw); eyeEulerReady = true; std::array robotAxes; frameReady = TransformAxes(eyeAxes, eyeInHandTransform, robotAxes); if (frameReady) { const CTRotationMatrix rotation = BuildRotationMatrix(robotAxes); - RotationMatrixToConfiguredEulerDegrees(rotation, order, pos.roll, pos.pitch, pos.yaw); + RotationMatrixToConfiguredEulerDegrees(rotation, outputOrder, pos.roll, pos.pitch, pos.yaw); + ResolveGimbalAmbiguity(pos.roll, pos.pitch, pos.yaw, rx, rz); } } } @@ -674,21 +823,6 @@ int DetectPresenter::DetectToolDisk( } detectionResult.positions.push_back(pos); - if (debugParam.enableDebug && debugParam.printDetailLog) { - LOG_INFO("[Algo Thread] ToolDisk Eye Coords: X=%.2f, Y=%.2f, Z=%.2f\n", poseInfo.center.x, poseInfo.center.y, poseInfo.center.z); - LOG_INFO("[Algo Thread] ToolDisk XDir Eye: X=%.3f, Y=%.3f, Z=%.3f\n", poseInfo.xDir.x, poseInfo.xDir.y, poseInfo.xDir.z); - LOG_INFO("[Algo Thread] ToolDisk YDir Eye: X=%.3f, Y=%.3f, Z=%.3f\n", poseInfo.yDir.x, poseInfo.yDir.y, poseInfo.yDir.z); - LOG_INFO("[Algo Thread] ToolDisk ZDir Eye: X=%.3f, Y=%.3f, Z=%.3f\n", poseInfo.normalDir.x, poseInfo.normalDir.y, poseInfo.normalDir.z); - if (eyeEulerReady) { - LOG_INFO("[Algo Thread] ToolDisk Euler Eye: Roll=%.2f, Pitch=%.2f, Yaw=%.2f\n", - eyeRoll, eyePitch, eyeYaw); - } else { - LOG_INFO("[Algo Thread] ToolDisk Euler Eye: unavailable\n"); - } - LOG_INFO("[Algo Thread] ToolDisk Robot Coords: X=%.2f, Y=%.2f, Z=%.2f\n", pos.x, pos.y, pos.z); - LOG_INFO("[Algo Thread] ToolDisk Euler: Roll=%.2f, Pitch=%.2f, Yaw=%.2f\n", pos.roll, pos.pitch, pos.yaw); - } - SaveDebugImageIfNeeded(cameraIndex, debugParam, detectionResult.image, QStringLiteral("ToolDisk_Image")); return SUCCESS; } diff --git a/App/ScrewPosition/ScrewPositionApp/Presenter/Src/ScrewPositionPresenter.cpp b/App/ScrewPosition/ScrewPositionApp/Presenter/Src/ScrewPositionPresenter.cpp index 8f4a6bb..fcf0eec 100644 --- a/App/ScrewPosition/ScrewPositionApp/Presenter/Src/ScrewPositionPresenter.cpp +++ b/App/ScrewPosition/ScrewPositionApp/Presenter/Src/ScrewPositionPresenter.cpp @@ -235,12 +235,13 @@ int ScrewPositionPresenter::ProcessAlgoDetection(std::vectorGetConfigResult(); const VrDebugParam debugParam = configResult.debugParam; const int eulerOrder = configResult.eulerOrder; + const int outputEulerOrder = configResult.outputEulerOrder; const int poseOutputOrder = configResult.poseOutputOrder; const int dirVectorInvert = configResult.dirVectorInvert; const int longAxisDir = configResult.longAxisDir; - LOG_INFO("[Algo Thread] Using euler order: %d, pose output order: %d, dir vector invert: %d, long axis dir: %d\n", - eulerOrder, poseOutputOrder, dirVectorInvert, longAxisDir); + LOG_INFO("[Algo Thread] Using euler order: %d, output euler order: %d, pose output order: %d, dir vector invert: %d, long axis dir: %d\n", + eulerOrder, outputEulerOrder, poseOutputOrder, dirVectorInvert, longAxisDir); DetectionResult detectionResult; detectionResult.cameraIndex = m_currentCameraIndex; @@ -256,6 +257,7 @@ int ScrewPositionPresenter::ProcessAlgoDetection(std::vector float { - uint32_t raw = (static_cast(m_modbusRobotPoseRegs[offset]) << 16) | - static_cast(m_modbusRobotPoseRegs[offset + 1]); + uint32_t raw = (static_cast(m_modbusRobotPoseRegs[offset]) << 16) | static_cast(m_modbusRobotPoseRegs[offset + 1]); float val = 0; std::memcpy(&val, &raw, sizeof(val)); return val; @@ -480,13 +482,12 @@ void ScrewPositionPresenter::OnModbusWriteCallback(uint16_t startAddress, const robotPose.x = regToFloat(0); robotPose.y = regToFloat(2); robotPose.z = regToFloat(4); - robotPose.rx = regToFloat(6); - robotPose.ry = regToFloat(8); - robotPose.rz = regToFloat(10); + robotPose.a = regToFloat(6); + robotPose.b = regToFloat(8); + robotPose.c = regToFloat(10); LOG_INFO("Modbus trigger: type=%u, robotPose=(%.3f, %.3f, %.3f, %.3f, %.3f, %.3f)\n", - triggerValue, robotPose.x, robotPose.y, robotPose.z, - robotPose.rx, robotPose.ry, robotPose.rz); + triggerValue, robotPose.x, robotPose.y, robotPose.z, robotPose.a, robotPose.b, robotPose.c); TriggerDetection(-1, static_cast(triggerValue), robotPose); return; diff --git a/App/ScrewPosition/ScrewPositionApp/Presenter/Src/ScrewPositionTCPProtocol.cpp b/App/ScrewPosition/ScrewPositionApp/Presenter/Src/ScrewPositionTCPProtocol.cpp index a3a9e3d..012685b 100644 --- a/App/ScrewPosition/ScrewPositionApp/Presenter/Src/ScrewPositionTCPProtocol.cpp +++ b/App/ScrewPosition/ScrewPositionApp/Presenter/Src/ScrewPositionTCPProtocol.cpp @@ -1,5 +1,7 @@ #include "ScrewPositionTCPProtocol.h" + #include "VrLog.h" + #include #include @@ -80,12 +82,12 @@ int ScrewPositionTCPProtocol::SendTextResult(const std::string& text, const TCPC return -1; } - std::string sendData = text + "\n"; + const std::string sendData = text + "\r\n"; bool success = false; if (pClient) { - success = m_pTCPServer->SendData(pClient, sendData.c_str(), sendData.size()); + success = m_pTCPServer->SendData(pClient, sendData.c_str(), static_cast(sendData.size())); } else { - success = m_pTCPServer->SendAllData(sendData.c_str(), sendData.size()); + success = m_pTCPServer->SendAllData(sendData.c_str(), static_cast(sendData.size())); } return success ? 0 : -2; @@ -94,42 +96,51 @@ int ScrewPositionTCPProtocol::SendTextResult(const std::string& text, const TCPC void ScrewPositionTCPProtocol::OnTCPEvent(const TCPClient* pClient, TCPServerEventType eventType) { switch (eventType) { - case TCP_EVENT_CLIENT_CONNECTED: - LOG_DEBUG("TCP client connected: %p\n", pClient); - if (m_connectionCallback) { - m_connectionCallback(true); - } - break; - case TCP_EVENT_CLIENT_DISCONNECTED: - LOG_DEBUG("TCP client disconnected: %p\n", pClient); - m_clientBuffers.erase(pClient); - if (m_connectionCallback) { - m_connectionCallback(false); - } - break; - case TCP_EVENT_CLIENT_EXCEPTION: - LOG_WARNING("TCP client exception: %p\n", pClient); - m_clientBuffers.erase(pClient); - if (m_connectionCallback) { - m_connectionCallback(false); - } - break; + case TCP_EVENT_CLIENT_CONNECTED: + LOG_DEBUG("TCP client connected: %p\n", pClient); + if (m_connectionCallback) { + m_connectionCallback(true); + } + break; + case TCP_EVENT_CLIENT_DISCONNECTED: + LOG_DEBUG("TCP client disconnected: %p\n", pClient); + m_clientBuffers.erase(pClient); + if (m_connectionCallback) { + m_connectionCallback(false); + } + break; + case TCP_EVENT_CLIENT_EXCEPTION: + LOG_WARNING("TCP client exception: %p\n", pClient); + m_clientBuffers.erase(pClient); + if (m_connectionCallback) { + m_connectionCallback(false); + } + break; } } void ScrewPositionTCPProtocol::OnTCPDataReceived(const TCPClient* pClient, const char* pData, unsigned int nLen) { - if (!pData || nLen == 0) return; + if (!pData || nLen == 0) { + return; + } m_clientBuffers[pClient].append(pData, nLen); QByteArray& buffer = m_clientBuffers[pClient]; while (true) { - int idx = buffer.indexOf('\n'); - if (idx < 0) break; + int idx = buffer.indexOf("\r\n"); + int terminatorLen = 2; + if (idx < 0) { + idx = buffer.indexOf('\n'); + terminatorLen = 1; + } + if (idx < 0) { + break; + } QByteArray line = buffer.left(idx).trimmed(); - buffer.remove(0, idx + 1); + buffer.remove(0, idx + terminatorLen); if (!line.isEmpty()) { ParseTextCommand(pClient, line); @@ -139,8 +150,9 @@ void ScrewPositionTCPProtocol::OnTCPDataReceived(const TCPClient* pClient, const void ScrewPositionTCPProtocol::ParseTextCommand(const TCPClient* pClient, const QByteArray& line) { - // 协议格式:S1_-23.45_200.30_50.10_15.50_-2.30_45.00 - // S=螺杆检测, T=工具盘检测, 后跟相机索引, _分隔 X Y Z RX RY RZ + Q_UNUSED(pClient); + + // Protocol format: T1_X_Y_Z_A_B_C QString strLine = QString::fromUtf8(line); QStringList tokens = strLine.split('_', QString::SkipEmptyParts); @@ -152,7 +164,6 @@ void ScrewPositionTCPProtocol::ParseTextCommand(const TCPClient* pClient, const return; } - // 解析类型和相机索引:S1/T1 或 S2/T2 QString typeToken = tokens[0].toUpper(); DetectionType detectionType = DETECTION_TYPE_SCREW; if (typeToken.startsWith('S')) { @@ -169,14 +180,15 @@ void ScrewPositionTCPProtocol::ParseTextCommand(const TCPClient* pClient, const cameraIndex = 1; } - // 解析机器人位姿:X Y Z RX RY RZ RobotPose6D robotPose; - double* values[] = {&robotPose.x, &robotPose.y, &robotPose.z, - &robotPose.rx, &robotPose.ry, &robotPose.rz}; + double* values[] = { + &robotPose.x, &robotPose.y, &robotPose.z, + &robotPose.a, &robotPose.b, &robotPose.c + }; - for (int i = 0; i < 6; i++) { + for (int i = 0; i < 6; ++i) { bool ok = false; - double val = tokens[i + 1].toDouble(&ok); + const double val = tokens[i + 1].toDouble(&ok); if (!ok) { LOG_ERROR("Invalid numeric value at position %d: '%s'\n", i + 1, tokens[i + 1].toStdString().c_str()); @@ -188,7 +200,7 @@ void ScrewPositionTCPProtocol::ParseTextCommand(const TCPClient* pClient, const LOG_INFO("TCP trigger: type=%d, camera=%d, pose=(%.3f, %.3f, %.3f, %.3f, %.3f, %.3f)\n", static_cast(detectionType), cameraIndex, robotPose.x, robotPose.y, robotPose.z, - robotPose.rx, robotPose.ry, robotPose.rz); + robotPose.a, robotPose.b, robotPose.c); if (m_detectionTriggerCallback) { m_detectionTriggerCallback(cameraIndex, detectionType, robotPose); diff --git a/App/ScrewPosition/ScrewPositionApp/Version.h b/App/ScrewPosition/ScrewPositionApp/Version.h index 24d6525..cbd3c74 100644 --- a/App/ScrewPosition/ScrewPositionApp/Version.h +++ b/App/ScrewPosition/ScrewPositionApp/Version.h @@ -4,7 +4,7 @@ #define SCREWPOSITION_APP_NAME "螺杆定位" #define SCREWPOSITION_VERSION_STRING "1.1.3" -#define SCREWPOSITION_BUILD_STRING "3" +#define SCREWPOSITION_BUILD_STRING "6" #define SCREWPOSITION_FULL_VERSION_STRING "V" SCREWPOSITION_VERSION_STRING "_" SCREWPOSITION_BUILD_STRING // 获取版本信息的便捷函数 diff --git a/App/ScrewPosition/ScrewPositionApp/dialogalgoarg.cpp b/App/ScrewPosition/ScrewPositionApp/dialogalgoarg.cpp index eb99fdb..054daa0 100644 --- a/App/ScrewPosition/ScrewPositionApp/dialogalgoarg.cpp +++ b/App/ScrewPosition/ScrewPositionApp/dialogalgoarg.cpp @@ -240,6 +240,7 @@ void DialogAlgoArg::loadNetworkConfig() NetworkConfigData netConfig; netConfig.tcpServerPort = configResult.tcpPort; netConfig.eulerOrder = configResult.eulerOrder; + netConfig.outputEulerOrder = configResult.outputEulerOrder; netConfig.poseOutputOrder = configResult.poseOutputOrder; netConfig.dirVectorInvert = configResult.dirVectorInvert; netConfig.byteOrder = configResult.byteOrder; @@ -322,6 +323,7 @@ bool DialogAlgoArg::saveParams() systemConfig.configResult.tcpPort = static_cast(netConfig.tcpServerPort); systemConfig.configResult.eulerOrder = netConfig.eulerOrder; + systemConfig.configResult.outputEulerOrder = netConfig.outputEulerOrder; systemConfig.configResult.poseOutputOrder = netConfig.poseOutputOrder; systemConfig.configResult.dirVectorInvert = netConfig.dirVectorInvert; systemConfig.configResult.byteOrder = netConfig.byteOrder; diff --git a/App/ScrewPosition/ScrewPositionConfig/Inc/IVrConfig.h b/App/ScrewPosition/ScrewPositionConfig/Inc/IVrConfig.h index e6d6845..f4b6f7e 100644 --- a/App/ScrewPosition/ScrewPositionConfig/Inc/IVrConfig.h +++ b/App/ScrewPosition/ScrewPositionConfig/Inc/IVrConfig.h @@ -59,6 +59,7 @@ struct ConfigResult SerialConfig serialConfig; uint16_t tcpPort = 7800; int eulerOrder = 11; + int outputEulerOrder = 10; int poseOutputOrder = 0; int dirVectorInvert = 0; int byteOrder = 0; @@ -74,6 +75,7 @@ struct ConfigResult serialConfig = other.serialConfig; tcpPort = other.tcpPort; eulerOrder = other.eulerOrder; + outputEulerOrder = other.outputEulerOrder; poseOutputOrder = other.poseOutputOrder; dirVectorInvert = other.dirVectorInvert; byteOrder = other.byteOrder; @@ -90,6 +92,7 @@ struct ConfigResult , serialConfig(other.serialConfig) , tcpPort(other.tcpPort) , eulerOrder(other.eulerOrder) + , outputEulerOrder(other.outputEulerOrder) , poseOutputOrder(other.poseOutputOrder) , dirVectorInvert(other.dirVectorInvert) , byteOrder(other.byteOrder) diff --git a/App/ScrewPosition/ScrewPositionConfig/Src/VrConfig.cpp b/App/ScrewPosition/ScrewPositionConfig/Src/VrConfig.cpp index 7dc6164..c5bbbb0 100644 --- a/App/ScrewPosition/ScrewPositionConfig/Src/VrConfig.cpp +++ b/App/ScrewPosition/ScrewPositionConfig/Src/VrConfig.cpp @@ -104,6 +104,8 @@ int CVrConfig::LoadConfig(const std::string& filePath, ConfigResult& configResul configResult.tcpPort = static_cast(networkElement->IntAttribute("tcpServerPort")); if (networkElement->Attribute("eulerOrder")) configResult.eulerOrder = networkElement->IntAttribute("eulerOrder"); + if (networkElement->Attribute("outputEulerOrder")) + configResult.outputEulerOrder = networkElement->IntAttribute("outputEulerOrder"); if (networkElement->Attribute("poseOutputOrder")) configResult.poseOutputOrder = networkElement->IntAttribute("poseOutputOrder"); if (networkElement->Attribute("dirVectorInvert")) @@ -186,6 +188,7 @@ bool CVrConfig::SaveConfig(const std::string& filePath, ConfigResult& configResu XMLElement* networkElement = doc.NewElement("NetworkConfig"); networkElement->SetAttribute("tcpServerPort", configResult.tcpPort); networkElement->SetAttribute("eulerOrder", configResult.eulerOrder); + networkElement->SetAttribute("outputEulerOrder", configResult.outputEulerOrder); networkElement->SetAttribute("poseOutputOrder", configResult.poseOutputOrder); networkElement->SetAttribute("dirVectorInvert", configResult.dirVectorInvert); networkElement->SetAttribute("byteOrder", configResult.byteOrder); diff --git a/App/ScrewPosition/ScrewPositionConfig/config/config.xml b/App/ScrewPosition/ScrewPositionConfig/config/config.xml index 2bcf127..2baab0d 100644 --- a/App/ScrewPosition/ScrewPositionConfig/config/config.xml +++ b/App/ScrewPosition/ScrewPositionConfig/config/config.xml @@ -43,6 +43,6 @@ - + diff --git a/App/WorkpieceHole/Doc/MODBUS_PROTOCOL.md b/App/WorkpieceHole/Doc/MODBUS_PROTOCOL.md deleted file mode 100644 index b6d76fc..0000000 --- a/App/WorkpieceHole/Doc/MODBUS_PROTOCOL.md +++ /dev/null @@ -1,149 +0,0 @@ -# WorkpieceHole 工件孔定位 - Modbus TCP 通信协议 - -## 1. 系统架构 - -``` -┌─────────────────┐ Modbus TCP ┌─────────────────────┐ -│ │◄────────────────────►│ │ -│ 3D视觉系统 │ 192.168.0.88 │ 汇川PLC-easy320 │ -│ (Modbus客户端) │ :502 │ (Modbus服务端) │ -│ │ │ │ -└─────────────────┘ └─────────────────────┘ -``` - -## 2. 网络配置 - -| 设备 | IP地址 | 端口 | 角色 | -|------|--------|------|------| -| 3D视觉系统 | - | - | Modbus TCP 客户端 | -| 汇川PLC-easy320 | 192.168.0.88 | 502 | Modbus TCP 服务端 | - -## 3. 地址映射规则 - -汇川PLC D寄存器与Modbus保持寄存器地址映射关系: - -``` -代码协议地址 = D寄存器地址 + 1 - -示例: - D1000 -> 代码协议地址 1001 - D1002 -> 代码协议地址 1003 - D2000 -> 代码协议地址 2001 -``` - -## 4. 寄存器地址定义 - -### 4.1 控制寄存器 - -| PLC寄存器 | 代码协议地址 | 类型 | 读/写 | 说明 | -|-----------|------------|------|-------|------| -| D1000 | 1001 | uint16 | R/W | 拍照请求(1=相机1检测,2=相机2检测) | -| D1002 | 1003 | uint16 | W | 检测结果状态码(见下表) | - -### 4.2 检测结果状态码(D1002) - -| 状态码 | 含义 | 说明 | -|--------|------|------| -| 1 | 检测成功 | 检测正确完成且有结果,坐标数据已写入D2000 | -| 2 | 异常物料 | 算法返回异常物料错误码(`SX_ERR_UNKNOWN_OBJECT = -2501`) | -| 3 | 无产品 | 算法返回无产品错误码(`SX_ERR_ZERO_OBJECTS = -1010`,兼容空目标类错误) | -| 11 | 检测失败 | 其他算法或系统异常 | -| 12 | 相机失败 | 相机打开或连接失败 | - -### 4.3 坐标数据寄存器(D2000起,1个点) - -检测成功时,将第一个工件的坐标写入 D2000-D2011,包含6个float值(X, Y, Z, Pitch, Roll, Yaw),每个float占用2个寄存器(大端序),共12个寄存器。 - -| PLC寄存器 | 代码协议地址 | 类型 | 说明 | -|-----------|------------|------|------| -| D2000-D2001 | 2001-2002 | float (大端) | X坐标 (mm) | -| D2002-D2003 | 2003-2004 | float (大端) | Y坐标 (mm) | -| D2004-D2005 | 2005-2006 | float (大端) | Z坐标 (mm) | -| D2006-D2007 | 2007-2008 | float (大端) | Pitch角度 (°) | -| D2008-D2009 | 2009-2010 | float (大端) | Roll角度 (°) | -| D2010-D2011 | 2011-2012 | float (大端) | Yaw角度 (°) | - -> **注意**: -> - 每个float值占用2个寄存器,采用IEEE 754单精度浮点数格式 -> - 大端序(Big-Endian):高位字在低地址寄存器,低位字在高地址寄存器 -> - 仅写入第一个工件的坐标数据 - -## 5. 通信流程 - -``` -时序图: - -3D视觉系统 汇川PLC - │ │ - │ 轮询读取D1000(地址1001) │ - │◄─────────────────────────────│ - │ │ - │ D1000=1或2 (拍照请求) │ - │◄─────────────────────────────│ - │ (1=相机1检测, 2=相机2检测) │ - │ │ - │ 写0到D1000(地址1001) │ - │─────────────────────────────►│ - │ │ - │ [执行拍照和算法处理] │ - │ │ - │ 写坐标到D2000(地址2001)开始│ - │─────────────────────────────►│ (仅检测成功时) - │ │ - │ 写状态码到D1002(地址1003) │ - │─────────────────────────────►│ (1/2/3/11/12) - │ │ -``` - -### 5.1 详细流程说明 - -1. **轮询拍照请求** - - 3D系统持续轮询读取PLC的D1000寄存器(代码协议地址1001) - - 轮询间隔:100ms(可配置) - - 使用边沿检测:只在0→非零变化时触发 - - D1000=1 触发相机1检测,D1000=2 触发相机2检测 - -2. **拍照执行** - - 检测到D1000为非零后,立即写0到D1000清除请求 - - 暂停轮询,根据D1000的值选择对应相机 - - 触发相机拍照,执行工件孔定位算法 - -3. **坐标输出**(仅检测成功时) - - 算法处理完成后,将坐标数据写入PLC的D2000开始的寄存器 - - 代码协议地址:2001-2012(单点12个寄存器,6个float值) - - 数据格式:IEEE 754单精度浮点数,大端序 - -4. **结果通知** - - 无论成功还是失败,均写状态码到D1002(代码协议地址1003) - - 状态码:1=成功有结果,2=异常物料,3=无产品,11=检测失败,12=相机失败 - - 写入状态码后恢复拍照请求轮询 - -## 6. 连接管理 - -### 6.1 自动重连机制 - -- 初始化时尝试连接PLC -- 连接失败时自动启动重连定时器 -- 重连间隔:3000ms(可配置) -- 持续重连直到连接成功或软件关闭 - -### 6.2 优雅退出 - -- 设置关闭标志,阻止新的重连尝试 -- 停止所有定时器(轮询、重连) -- 断开Modbus连接 -- 释放资源 - -## 7. 错误处理 - -| 错误类型 | 处理方式 | D1002状态码 | -|----------|----------|------------| -| 相机连接失败 | 通知PLC,恢复轮询 | 12 | -| 异常物料(`SX_ERR_UNKNOWN_OBJECT = -2501`) | 通知PLC,恢复轮询 | 2 | -| 无产品(`SX_ERR_ZERO_OBJECTS = -1010`,兼容空目标类错误) | 通知PLC,恢复轮询 | 3 | -| 其他算法检测失败 | 通知PLC,恢复轮询 | 11 | -| 检测成功有结果 | 发送坐标,通知PLC,恢复轮询 | 1 | -| 连接断开 | 自动重连,发送错误信号 | - | -| 读取失败 | 记录日志,主动断开触发重连 | - | -| 写入失败 | 记录日志,返回失败状态 | - | -| 超时 | 默认1秒超时,触发重连 | - | diff --git a/App/WorkpieceHole/Doc/workpiecehole_plc.mthings b/App/WorkpieceHole/Doc/workpiecehole_plc.mthings index fb18595..66ce9fc 100644 --- a/App/WorkpieceHole/Doc/workpiecehole_plc.mthings +++ b/App/WorkpieceHole/Doc/workpiecehole_plc.mthings @@ -4,6 +4,7 @@ + diff --git a/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/WorkpieceHolePresenter.cpp b/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/WorkpieceHolePresenter.cpp index 3191fe6..b29bd18 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/WorkpieceHolePresenter.cpp +++ b/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/WorkpieceHolePresenter.cpp @@ -841,8 +841,8 @@ void WorkpieceHolePresenter::SendCoordinateDataToPLC(const WorkpieceHoleDetectio int poseOutputOrder = configResult.plcRobotServerConfig.poseOutputOrder; LOG_INFO("Using pose output order: %d\n", poseOutputOrder); - // 只发送第一个工件的坐标数据 - const auto& pos = result.positions[0]; + // 协议单点输出改为最后一个工件。 + const auto& pos = result.positions.back(); PLCModbusClient::CoordinateData coord; // 使用 float 类型存储坐标数据 diff --git a/App/WorkpieceHole/WorkpieceHoleApp/Version.h b/App/WorkpieceHole/WorkpieceHoleApp/Version.h index 9e11699..cb12ef4 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/Version.h +++ b/App/WorkpieceHole/WorkpieceHoleApp/Version.h @@ -5,7 +5,7 @@ #define WORKPIECEHOLE_APP_NAME "工件孔定位" // 版本字符串 -#define WORKPIECEHOLE_VERSION_STRING "1.1.0" +#define WORKPIECEHOLE_VERSION_STRING "1.1.2" #define WORKPIECEHOLE_BUILD_STRING "1" #define WORKPIECEHOLE_FULL_VERSION_STRING "V" WORKPIECEHOLE_VERSION_STRING "_" WORKPIECEHOLE_BUILD_STRING diff --git a/App/WorkpieceHole/WorkpieceHoleApp/Version.md b/App/WorkpieceHole/WorkpieceHoleApp/Version.md index 643db49..7f408ac 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/Version.md +++ b/App/WorkpieceHole/WorkpieceHoleApp/Version.md @@ -1,3 +1,15 @@ +# 1.1.2 +## build_1 2026-04-17 +1. 协议和页面显示倒叙输出 + +# 1.1.1 +## build_2 2026-04-17 +1. arm库不对 + +## build_1 2026-04-17 +1. 更新算法 +2. 更新协议 + # 1.1.0 ## build_0 2026-04-28 1. 更新算法 diff --git a/App/WorkpieceHole/WorkpieceHoleApp/mainwindow.cpp b/App/WorkpieceHole/WorkpieceHoleApp/mainwindow.cpp index 92d5ce2..d057132 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/mainwindow.cpp +++ b/App/WorkpieceHole/WorkpieceHoleApp/mainwindow.cpp @@ -234,13 +234,13 @@ void MainWindow::addDetectionResult(const WorkpieceHoleDetectionResult& result) int gridWidth = currentGridSize.width(); int gridHeight = currentGridSize.height(); - // 显示每个工件的中心点坐标 - for (size_t i = 0; i < result.positions.size(); i++) { + // 倒序显示,界面从后往前展示检测结果。 + for (size_t i = result.positions.size(); i > 0; --i) { XyzRpyItem* resultWidget = new XyzRpyItem(); resultWidget->setAutoFillBackground(true); - // 设置中心点数据 - resultWidget->setResultData(i + 1, "工件", result.positions[i]); + // 保留原始序号,显示顺序改为从后往前。 + resultWidget->setResultData(static_cast(i), "工件", result.positions[i - 1]); // 创建QListWidgetItem QListWidgetItem* item = new QListWidgetItem(); diff --git a/AppAlgo/rodAndBarDetection/Arm/aarch64/librodAndBarDetection.so b/AppAlgo/rodAndBarDetection/Arm/aarch64/librodAndBarDetection.so index bc64ef0..0d42a7c 100644 Binary files a/AppAlgo/rodAndBarDetection/Arm/aarch64/librodAndBarDetection.so and b/AppAlgo/rodAndBarDetection/Arm/aarch64/librodAndBarDetection.so differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.dll b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.dll index 79bb1c5..aba1cc3 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.dll and b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.dll differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.pdb b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.pdb index 563db75..ca4356c 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.pdb and b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/baseAlgorithm.pdb differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.dll b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.dll index 28c4b77..2c14a9d 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.dll and b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.dll differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.pdb b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.pdb index aeb062c..c867dac 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.pdb and b/AppAlgo/rodAndBarDetection/Windows/x64/Debug/rodAndBarDetection.pdb differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.dll b/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.dll index ef9c835..f04ccf8 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.dll and b/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.dll differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.pdb b/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.pdb index 0016ec3..98a7156 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.pdb and b/AppAlgo/rodAndBarDetection/Windows/x64/Release/baseAlgorithm.pdb differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.dll b/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.dll index 009bc2d..343c539 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.dll and b/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.dll differ diff --git a/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.pdb b/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.pdb index 982f277..1fe8a2a 100644 Binary files a/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.pdb and b/AppAlgo/rodAndBarDetection/Windows/x64/Release/rodAndBarDetection.pdb differ diff --git a/AppAlgo/workpieceHolePositioning/Arm/aarch64/libworkpieceHolePositioning.so b/AppAlgo/workpieceHolePositioning/Arm/aarch64/libworkpieceHolePositioning.so index a4f606b..570cf78 100644 Binary files a/AppAlgo/workpieceHolePositioning/Arm/aarch64/libworkpieceHolePositioning.so and b/AppAlgo/workpieceHolePositioning/Arm/aarch64/libworkpieceHolePositioning.so differ diff --git a/AppUtils/AppCommon/Src/BasePresenter.cpp b/AppUtils/AppCommon/Src/BasePresenter.cpp index 1b3f2fa..4989422 100644 --- a/AppUtils/AppCommon/Src/BasePresenter.cpp +++ b/AppUtils/AppCommon/Src/BasePresenter.cpp @@ -897,8 +897,7 @@ int BasePresenter::WriteModbusRegisters(uint16_t startAddress, const uint16_t* d // 转换为vector并写入 std::vector values(data, data + count); m_modbusServer->updateHoldingRegisters(startAddress, values); - - LOG_DEBUG("[BasePresenter] Modbus写入: 地址=%d, 数量=%d\n", startAddress, count); + return SUCCESS; } diff --git a/AppUtils/UICommon/Inc/NetworkConfigWidget.h b/AppUtils/UICommon/Inc/NetworkConfigWidget.h index 16f3633..55b8f77 100644 --- a/AppUtils/UICommon/Inc/NetworkConfigWidget.h +++ b/AppUtils/UICommon/Inc/NetworkConfigWidget.h @@ -10,6 +10,7 @@ struct NetworkConfigData { int eulerOrder = 11; + int outputEulerOrder = 10; // 输出欧拉角解码约定(仅影响旋转矩阵 → 欧拉角的展示/下发,不影响坐标转换) int poseOutputOrder = 0; // 机器人姿态输入/输出顺序 int dirVectorInvert = 0; int byteOrder = 0; @@ -72,6 +73,7 @@ public: NetworkConfigData getConfig() const; int eulerOrder() const; + int outputEulerOrder() const; int poseOutputOrder() const; int dirVectorInvert() const; int byteOrder() const; @@ -80,6 +82,7 @@ public: private: void setupUI(bool showPlcConfig, bool showTcpConfig); void initEulerOrderComboBox(); + void initOutputEulerOrderComboBox(); void initPoseOutputOrderComboBox(); void initDirVectorInvertComboBox(); void initByteOrderComboBox(); @@ -87,6 +90,7 @@ private: private: QComboBox* m_comboEulerOrder = nullptr; + QComboBox* m_comboOutputEulerOrder = nullptr; QComboBox* m_comboPoseOutputOrder = nullptr; QComboBox* m_comboDirVectorInvert = nullptr; QComboBox* m_comboByteOrder = nullptr; diff --git a/AppUtils/UICommon/Src/NetworkConfigWidget.cpp b/AppUtils/UICommon/Src/NetworkConfigWidget.cpp index 361023c..95cc1f2 100644 --- a/AppUtils/UICommon/Src/NetworkConfigWidget.cpp +++ b/AppUtils/UICommon/Src/NetworkConfigWidget.cpp @@ -43,6 +43,15 @@ void NetworkConfigWidget::setupUI(bool showPlcConfig, bool showTcpConfig) labelEuler->setStyleSheet(labelStyle); commonForm->addRow(labelEuler, m_comboEulerOrder); + m_comboOutputEulerOrder = new QComboBox(this); + m_comboOutputEulerOrder->setFont(font); + m_comboOutputEulerOrder->setStyleSheet(comboStyle); + initOutputEulerOrderComboBox(); + QLabel* labelOutputEuler = new QLabel(QStringLiteral("输出欧拉角顺序:"), this); + labelOutputEuler->setFont(font); + labelOutputEuler->setStyleSheet(labelStyle); + commonForm->addRow(labelOutputEuler, m_comboOutputEulerOrder); + m_comboPoseOutputOrder = new QComboBox(this); m_comboPoseOutputOrder->setFont(font); m_comboPoseOutputOrder->setStyleSheet(comboStyle); @@ -163,6 +172,17 @@ void NetworkConfigWidget::initEulerOrderComboBox() m_comboEulerOrder->setCurrentIndex(0); } +void NetworkConfigWidget::initOutputEulerOrderComboBox() +{ + m_comboOutputEulerOrder->addItem(QStringLiteral("RZ-RY-RX (外旋ZYX)"), EULER_ORDER_ZYX); + m_comboOutputEulerOrder->addItem(QStringLiteral("RX-RY-RZ (外旋XYZ)"), EULER_ORDER_XYZ); + m_comboOutputEulerOrder->addItem(QStringLiteral("RZ-RX-RY (外旋ZXY)"), EULER_ORDER_ZXY); + m_comboOutputEulerOrder->addItem(QStringLiteral("RY-RX-RZ (外旋YXZ)"), EULER_ORDER_YXZ); + m_comboOutputEulerOrder->addItem(QStringLiteral("RY-RZ-RX (外旋YZX)"), EULER_ORDER_YZX); + m_comboOutputEulerOrder->addItem(QStringLiteral("RX-RZ-RY (外旋XZY)"), EULER_ORDER_XZY); + m_comboOutputEulerOrder->setCurrentIndex(0); +} + void NetworkConfigWidget::initPoseOutputOrderComboBox() { m_comboPoseOutputOrder->addItem(QStringLiteral("RX-RY-RZ"), NET_POSE_OUTPUT_RX_RY_RZ); @@ -204,6 +224,11 @@ void NetworkConfigWidget::setConfig(const NetworkConfigData& config) m_comboEulerOrder->setCurrentIndex(idx); } + idx = m_comboOutputEulerOrder->findData(config.outputEulerOrder); + if (idx >= 0) { + m_comboOutputEulerOrder->setCurrentIndex(idx); + } + idx = m_comboPoseOutputOrder->findData(config.poseOutputOrder); if (idx >= 0) { m_comboPoseOutputOrder->setCurrentIndex(idx); @@ -248,6 +273,7 @@ NetworkConfigData NetworkConfigWidget::getConfig() const { NetworkConfigData config; config.eulerOrder = m_comboEulerOrder->currentData().toInt(); + config.outputEulerOrder = m_comboOutputEulerOrder->currentData().toInt(); config.poseOutputOrder = m_comboPoseOutputOrder->currentData().toInt(); config.dirVectorInvert = m_comboDirVectorInvert->currentData().toInt(); config.byteOrder = m_comboByteOrder->currentData().toInt(); @@ -280,6 +306,11 @@ int NetworkConfigWidget::eulerOrder() const return m_comboEulerOrder->currentData().toInt(); } +int NetworkConfigWidget::outputEulerOrder() const +{ + return m_comboOutputEulerOrder->currentData().toInt(); +} + int NetworkConfigWidget::poseOutputOrder() const { return m_comboPoseOutputOrder->currentData().toInt(); diff --git a/AppUtils/UICommon/Src/XyzRpyItem.cpp b/AppUtils/UICommon/Src/XyzRpyItem.cpp index 9b41bdd..6ff240c 100644 --- a/AppUtils/UICommon/Src/XyzRpyItem.cpp +++ b/AppUtils/UICommon/Src/XyzRpyItem.cpp @@ -16,8 +16,7 @@ XyzRpyItem::~XyzRpyItem() delete ui; } -void XyzRpyItem::setResultData(int index, const QString& titlePrefix, - const PositionData& position, int precision) +void XyzRpyItem::setResultData(int index, const QString& titlePrefix, const PositionData& position, int precision) { // 璁剧疆鏍囬锛氬墠缂€ + 缂栧彿 ui->result_id->setText(QString("%1 %2").arg(titlePrefix).arg(index)); @@ -28,7 +27,7 @@ void XyzRpyItem::setResultData(int index, const QString& titlePrefix, ui->result_z->setText(QString("Z: %1").arg(position.z, 0, 'f', precision)); // 濮挎€佽锛氭瘡琛屼竴涓? - ui->result_r->setText(QString("RX: %1").arg(position.roll, 0, 'f', precision)); - ui->result_p->setText(QString("RY: %1").arg(position.pitch, 0, 'f', precision)); - ui->result_yaw->setText(QString("RZ: %1").arg(position.yaw, 0, 'f', precision)); + ui->result_r->setText(QString("A: %1").arg(position.roll, 0, 'f', precision)); + ui->result_p->setText(QString("B: %1").arg(position.pitch, 0, 'f', precision)); + ui->result_yaw->setText(QString("C: %1").arg(position.yaw, 0, 'f', precision)); } diff --git a/AppUtils/UICommon/XyzRpyItem.ui b/AppUtils/UICommon/XyzRpyItem.ui index 1435737..086625a 100644 --- a/AppUtils/UICommon/XyzRpyItem.ui +++ b/AppUtils/UICommon/XyzRpyItem.ui @@ -174,7 +174,7 @@ background-color: transparent; border: none; - RX: 0.000 + A: 0.000 @@ -192,7 +192,7 @@ background-color: transparent; border: none; - RY: 0.000 + B: 0.000 @@ -210,7 +210,7 @@ background-color: transparent; border: none; - RZ: 0.000 + C: 0.000 diff --git a/Device/VrEyeDevice/Src/VrEyeDevice.cpp b/Device/VrEyeDevice/Src/VrEyeDevice.cpp index b8574bd..fbf91de 100644 --- a/Device/VrEyeDevice/Src/VrEyeDevice.cpp +++ b/Device/VrEyeDevice/Src/VrEyeDevice.cpp @@ -9,6 +9,7 @@ #ifdef VR_EYE_DEBUG_MODE #include "LaserDataLoader.h" #include +#include #endif CVrEyeDevice::CVrEyeDevice() @@ -496,11 +497,28 @@ void CVrEyeDevice::DebugThreadFunc() // const std::string debugFilePath = "C:\\project\\QT\\GrabBag\\TestData\\workpiece\\new\\test\\9_LazerData_Hi229229.txt"; // const std::string debugFilePath = "C:\\project\\QT\\GrabBag\\TestData\\bag\\LaserLine1_grid.txt"; #ifdef _WIN32 - const std::string debugFilePath = "C:\\project\\QT\\GrabBag\\TestData\\eye_test.txt"; + const std::string debugFileDir = "C:\\project\\QT\\GrabBag\\TestData\\"; #else - const std::string debugFilePath = "/home/linaro/eye_test.txt"; + const std::string debugFileDir = "/home/linaro/"; #endif + // 循环读取 eye_test_N.txt (N 从 1 开始累加),若下一索引的文件缺失则回到 1 + static int s_fileIndex = 1; + auto buildPath = [&debugFileDir](int idx) { + return debugFileDir + "eye_test_" + std::to_string(idx) + ".txt"; + }; + + std::string debugFilePath = buildPath(s_fileIndex); + { + std::ifstream probe(debugFilePath); + if (!probe.good() && s_fileIndex != 1) { + s_fileIndex = 1; + debugFilePath = buildPath(s_fileIndex); + } + } + + LOG_DEBUG("VR_EYE_DEBUG_MODE: Debug loading file: %s\n", debugFilePath.c_str()); + LaserDataLoader loader; std::vector> laserLines; int lineNum = 0; @@ -533,6 +551,10 @@ void CVrEyeDevice::DebugThreadFunc() // 释放数据 loader.FreeLaserScanData(laserLines); - LOG_DEBUG("VR_EYE_DEBUG_MODE: Debug thread stopped\n"); + + // 推进到下一个索引,便于下次调用顺序读取 + ++s_fileIndex; + + LOG_DEBUG("VR_EYE_DEBUG_MODE: Debug thread stopped, next index: %d\n", s_fileIndex); } #endif diff --git a/GrabBagPrj/AppList.md b/GrabBagPrj/AppList.md index 89044b2..3456618 100644 --- a/GrabBagPrj/AppList.md +++ b/GrabBagPrj/AppList.md @@ -15,8 +15,8 @@ | 9 | 颗粒尺寸检测 | ParticleSize | 1.0.0.0 | | 10 | 双目标记检测 | BinocularMarkServer | 1.0.0.4 | | 11 | 铁路隧道槽道测量 | TunnelChannel | 1.0.0.3 | -| 12 | 螺杆定位 | ScrewPosition | 1.1.3.3 | +| 12 | 螺杆定位 | ScrewPosition | 1.1.3.6 | | 13 | 包裹拆线位置定位 | BagThreadPosition | 1.0.0.4 | -| 14 | 工件孔定位 | WorkpieceHole | 1.0.3.0 | +| 14 | 工件孔定位 | WorkpieceHole | 1.1.2.1 | | 16 | 坑孔定位 | HolePitPosition | 无 | diff --git a/Module/HandEyeCalib/Utils/Inc/CoordinateTypes.h b/Module/HandEyeCalib/Utils/Inc/CoordinateTypes.h index 2916b41..6f01221 100644 --- a/Module/HandEyeCalib/Utils/Inc/CoordinateTypes.h +++ b/Module/HandEyeCalib/Utils/Inc/CoordinateTypes.h @@ -336,12 +336,11 @@ struct CTRobotPose double rz = 0.0; // 绕Z轴旋转 (弧度) CTRobotPose() = default; - CTRobotPose(double _x, double _y, double _z, double _rx, double _ry, double _rz) - : x(_x), y(_y), z(_z), rx(_rx), ry(_ry), rz(_rz) {} + CTRobotPose(double _x, double _y, double _z, double _a, double _b, double _c) + : x(_x), y(_y), z(_z), rx(_a), ry(_b), rz(_c) {} // 从角度创建 - static CTRobotPose fromDegrees(double x, double y, double z, - double rx_deg, double ry_deg, double rz_deg) + static CTRobotPose fromDegrees(double x, double y, double z, double rx_deg, double ry_deg, double rz_deg) { const double deg2rad = M_PI / 180.0; return CTRobotPose(x, y, z, rx_deg * deg2rad, ry_deg * deg2rad, rz_deg * deg2rad);