From 1641a0e029d79b631ac75b159f6a7c2a5dc7cda0 Mon Sep 17 00:00:00 2001 From: yiyi Date: Sat, 7 Feb 2026 23:46:44 +0800 Subject: [PATCH] =?UTF-8?q?=E7=B3=96=E5=8C=85=E6=8B=86=E7=BA=BF=20&=20?= =?UTF-8?q?=E6=9F=A5=E7=9C=8B=E7=82=B9=E4=BA=91=E8=BD=AF=E4=BB=B6=E5=A2=9E?= =?UTF-8?q?=E5=8A=A0=E5=A7=BF=E6=80=81=EF=BC=8C=E7=BA=BF=E6=9D=A1=E7=9A=84?= =?UTF-8?q?=E6=98=BE=E7=A4=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../BagThreadPositionApp.pro | 5 + .../Inc/BagThreadPositionPresenter.h | 13 + .../Src/BagThreadPositionPresenter.cpp | 116 ++++ App/BagThreadPosition/MODBUS_PROTOCOL.md | 101 +++ AppUtils/AppCommon/Inc/TCPServerProtocol.h | 4 +- AppUtils/AppCommon/Src/BasePresenter.cpp | 28 +- .../Src/GlLineLaserDevice.cpp | 393 ++++++------ .../_Inc/GlLineLaserDevice.h | 54 +- GrabBagPrj/buildArmPrj.sh | 2 +- GrabBagPrj/pkg_bagthreadposition.sh | 410 ++++++++++++ GrabBagPrj/pkg_workpiecehole.sh | 2 +- Tools/CloudView/Inc/CloudViewMainWindow.h | 64 ++ Tools/CloudView/Inc/PointCloudGLWidget.h | 54 ++ Tools/CloudView/Src/CloudViewMainWindow.cpp | 582 +++++++++++++++++- Tools/CloudView/Src/PointCloudGLWidget.cpp | 92 +++ Tools/CloudView/poses.txt | 17 + Tools/CloudView/segments.txt | 23 + 17 files changed, 1710 insertions(+), 250 deletions(-) create mode 100644 App/BagThreadPosition/MODBUS_PROTOCOL.md create mode 100644 GrabBagPrj/pkg_bagthreadposition.sh create mode 100644 Tools/CloudView/poses.txt create mode 100644 Tools/CloudView/segments.txt diff --git a/App/BagThreadPosition/BagThreadPositionApp/BagThreadPositionApp.pro b/App/BagThreadPosition/BagThreadPositionApp/BagThreadPositionApp.pro index 11e3b3e..7d421dc 100644 --- a/App/BagThreadPosition/BagThreadPositionApp/BagThreadPositionApp.pro +++ b/App/BagThreadPosition/BagThreadPositionApp/BagThreadPositionApp.pro @@ -135,17 +135,22 @@ INCLUDEPATH += ../../../SDK/Device/gl_linelaser_sdk/include win32:CONFIG(release, debug|release): { LIBS += -L$$PWD/../../../SDK/Device/VzNLSDK/Windows/x64/Release + LIBS += -lVzKernel -lVzNLDetect -lVzNLGraphics # gl_linelaser_sdk LIBS += -L$$PWD/../../../SDK/Device/gl_linelaser_sdk/x64 LIBS += -lgl_linelaser_sdk } else:win32:CONFIG(debug, debug|release): { LIBS += -L$$PWD/../../../SDK/Device/VzNLSDK/Windows/x64/Debug + LIBS += -lVzKerneld -lVzNLDetectd -lVzNLGraphicsd # gl_linelaser_sdk LIBS += -L$$PWD/../../../SDK/Device/gl_linelaser_sdk/x64 LIBS += -lgl_linelaser_sdk } else:unix:!macx: { + LIBS += -L$$PWD/../../../SDK/Device/VzNLSDK/Arm/aarch64 + LIBS += -lVzEyeSecurityLoader-shared -lVzKernel -lVzNLDetect -lVzNLGraphics + # gl_linelaser_sdk for Linux aarch64 LIBS += -L$$PWD/../../../SDK/Device/gl_linelaser_sdk/aarch64_linux -lgl_linelaser_sdk } diff --git a/App/BagThreadPosition/BagThreadPositionApp/Presenter/Inc/BagThreadPositionPresenter.h b/App/BagThreadPosition/BagThreadPositionApp/Presenter/Inc/BagThreadPositionPresenter.h index 98ea37a..2af6bb9 100644 --- a/App/BagThreadPosition/BagThreadPositionApp/Presenter/Inc/BagThreadPositionPresenter.h +++ b/App/BagThreadPosition/BagThreadPositionApp/Presenter/Inc/BagThreadPositionPresenter.h @@ -146,6 +146,15 @@ protected: */ int CreateDevice(IVrEyeDevice** ppDevice) override; + /** + * @brief Modbus写寄存器回调（重写虚函数） + * 实现协议： + * - 地址0: 请求启动相机扫描 + * - 地址2: 检测完成状态（成功1，失败2）- 只读，由系统自动更新 + * - 地址4: 检测成功后自动输出所有xyzu数据（每条拆线8个寄存器） + */ + void OnModbusWriteCallback(uint16_t startAddress, const uint16_t* data, uint16_t count) override; + private: // TCP服务器相关方法 int InitTcpServer(int nPort); @@ -188,6 +197,10 @@ private: // 手眼标定矩阵列表（从独立文件加载，暂时保留在Presenter中） std::vector m_clibMatrixList; + + // ModbusTCP协议相关 + DetectionResult m_lastDetectionResult; // 最新的检测结果 + std::mutex m_modbusResultMutex; // 保护检测结果的互斥锁 }; #endif // BAGTHREADPOSITIONPRESENTER_H diff --git a/App/BagThreadPosition/BagThreadPositionApp/Presenter/Src/BagThreadPositionPresenter.cpp b/App/BagThreadPosition/BagThreadPositionApp/Presenter/Src/BagThreadPositionPresenter.cpp index 710fe68..9fca92c 100644 --- a/App/BagThreadPosition/BagThreadPositionApp/Presenter/Src/BagThreadPositionPresenter.cpp +++ b/App/BagThreadPosition/BagThreadPositionApp/Presenter/Src/BagThreadPositionPresenter.cpp @@ -252,6 +252,14 @@ int BagThreadPositionPresenter::ProcessAlgoDetection(std::vector()) { if (auto pStatus = GetStatusCallback()) pStatus->OnStatusUpdate("检测处理器未初始化"); } + + // 更新ModbusTCP寄存器：地址2 = 2（检测失败） + if (IsModbusServerRunning()) { + uint16_t statusValue = 2; // 失败 + WriteModbusRegisters(2, &statusValue, 1); + LOG_INFO("ModbusTCP: 检测失败（处理器未初始化），地址2写入2\n"); + } + return ERR_CODE(DEV_NOT_FIND); } @@ -277,6 +285,16 @@ int BagThreadPositionPresenter::ProcessAlgoDetection(std::vectorOnDetectionResult(detectionResult); } + // 保存检测结果用于ModbusTCP输出 + { + std::lock_guard lock(m_modbusResultMutex); + m_lastDetectionResult = detectionResult; + } + + // 更新ModbusTCP寄存器：地址2 = 1（检测成功），并输出xyzu数据到地址4 + if (IsModbusServerRunning()) { + uint16_t statusValue = 1; // 成功 + WriteModbusRegisters(2, &statusValue, 1); + LOG_INFO("ModbusTCP: 检测成功，地址2写入1\n"); + + // 直接输出xyzu数据到地址4 + if (!detectionResult.threadInfoList.empty()) { + size_t threadCount = detectionResult.threadInfoList.size(); + std::vector outputData; + outputData.reserve(threadCount * 8); + + LOG_INFO("ModbusTCP: 准备输出 %zu 条拆线的xyzu数据\n", threadCount); + + // 将每个拆线的 x, y, z, u 转换为寄存器数据 + for (const auto& thread : detectionResult.threadInfoList) { + // x (centerX) + float x = static_cast(thread.centerX); + uint16_t* xPtr = reinterpret_cast(&x); + outputData.push_back(xPtr[0]); + outputData.push_back(xPtr[1]); + + // y (centerY) + float y = static_cast(thread.centerY); + uint16_t* yPtr = reinterpret_cast(&y); + outputData.push_back(yPtr[0]); + outputData.push_back(yPtr[1]); + + // z (centerZ) + float z = static_cast(thread.centerZ); + uint16_t* zPtr = reinterpret_cast(&z); + outputData.push_back(zPtr[0]); + outputData.push_back(zPtr[1]); + + // u (rotateAngle) + float u = static_cast(thread.rotateAngle); + uint16_t* uPtr = reinterpret_cast(&u); + outputData.push_back(uPtr[0]); + outputData.push_back(uPtr[1]); + + LOG_DEBUG("ModbusTCP: 拆线数据: x=%.3f, y=%.3f, z=%.3f, u=%.3f\n", + thread.centerX, thread.centerY, thread.centerZ, thread.rotateAngle); + } + + // 从地址4开始写入数据 + if (!outputData.empty()) { + int ret = WriteModbusRegisters(4, outputData.data(), static_cast(outputData.size())); + if (ret == 0) { + LOG_INFO("ModbusTCP: 成功输出 %zu 条拆线的xyzu数据到寄存器（地址4起，共%zu个寄存器）\n", + threadCount, outputData.size()); + } else { + LOG_ERROR("ModbusTCP: 输出xyzu数据失败，错误码: %d\n", ret); + } + } + } + } + // 更新状态 QString statusMsg = QString("检测完成，发现%1条拆线").arg(detectionResult.positions.size() / 2); if (auto pStatus = GetStatusCallback()) pStatus->OnStatusUpdate(statusMsg.toStdString()); @@ -695,3 +776,38 @@ int BagThreadPositionPresenter::CreateDevice(IVrEyeDevice** ppDevice) LOG_ERROR("[BagThreadPositionPresenter] Failed to create GlLineLaser device, error: %d\n", nRet); return ERR_CODE(DEV_OPEN_ERR); } + +// ============ ModbusTCP 协议实现 ============ + +void BagThreadPositionPresenter::OnModbusWriteCallback(uint16_t startAddress, const uint16_t* data, uint16_t count) +{ + if (!data || count == 0) { + LOG_WARNING("[ModbusTCP] 无效的写入参数\n"); + return; + } + + LOG_INFO("[ModbusTCP] 收到写寄存器请求: 地址=%d, 数量=%d, 值=%d\n", startAddress, count, data[0]); + + // 地址0: 请求启动相机扫描 + if (startAddress == 0) { + if (data[0] == 1) { + LOG_INFO("[ModbusTCP] 收到启动扫描请求\n"); + + // 重置检测状态寄存器（地址2）为0 + uint16_t statusValue = 0; + WriteModbusRegisters(2, &statusValue, 1); + + // 触发检测 + bool success = TriggerDetection(m_currentCameraIndex); + + if (!success) { + LOG_ERROR("[ModbusTCP] 启动扫描失败\n"); + // 写入失败状态到地址2 + statusValue = 2; // 失败 + WriteModbusRegisters(2, &statusValue, 1); + } else { + LOG_INFO("[ModbusTCP] 扫描已启动\n"); + } + } + } +} diff --git a/App/BagThreadPosition/MODBUS_PROTOCOL.md b/App/BagThreadPosition/MODBUS_PROTOCOL.md new file mode 100644 index 0000000..b8dab79 --- /dev/null +++ b/App/BagThreadPosition/MODBUS_PROTOCOL.md @@ -0,0 +1,101 @@ +# 糖包拆线 ModbusTCP 协议说明 + +**应用名称**: BagThreadPosition（糖包拆线） +**版本**: v1.0 +**日期**: 2026-02-07 + +## 概述 + +糖包拆线应用作为 ModbusTCP 服务端，使用标准 ModbusTCP 协议（端口 502）与外部设备（客户端）通信，实现相机扫描控制和检测结果输出。 + +### 基本信息 + +- **服务端**: 糖包拆线应用（BagThreadPosition） +- **客户端**: PLC、机器人控制器或其他支持 ModbusTCP 的设备 +- **协议**: ModbusTCP（基于 TCP/IP 的 Modbus 协议） +- **端口**: 502 +- **功能码**: 0x03（读保持寄存器）、0x10（写多个保持寄存器） + +## 寄存器地址映射 + +### 控制寄存器 + +| 地址 | 功能 | 读/写 | 数据类型 | 说明 | +|------|------|-------|----------|------| +| 0 | 启动扫描 | 写 | uint16 | 写入1启动相机扫描 | +| 2 | 检测状态 | 读 | uint16 | 0=未完成, 1=成功, 2=失败 | + +### 数据寄存器 + +| 地址 | 功能 | 读/写 | 数据类型 | 说明 | +|------|------|-------|----------|------| +| 4+ | xyzu数据 | 读 | float[] | 拆线位置数据（每条拆线8个寄存器） | + +## 使用流程 + +### 客户端操作步骤 + +### 1. 启动扫描 + +客户端写入寄存器[0] = 1 + +- 服务端启动相机扫描 +- 寄存器[2]被重置为0（检测中） + +### 2. 等待检测完成 + +客户端循环读取寄存器[2]，直到值不为0 + +- 值为1：检测成功，xyzu数据已自动写入寄存器[4]开始的位置 +- 值为2：检测失败 + +### 3. 读取检测结果 + +当寄存器[2] = 1（成功）时，客户端直接从寄存器[4]开始读取 xyzu 数据。 + +## 数据格式 + +### 寄存器布局 + +每条拆线占用 8 个寄存器（4个float，每个float占2个寄存器）： + +``` +寄存器[4+i*8+0:1] -> x (centerX, float) +寄存器[4+i*8+2:3] -> y (centerY, float) +寄存器[4+i*8+4:5] -> z (centerZ, float) +寄存器[4+i*8+6:7] -> u (rotateAngle, float) +``` + +其中 i 为拆线索引（从0开始）。 + +### xyzu 含义 + +- **x (centerX)**: 拆线端部中心点的X坐标（毫米） +- **y (centerY)**: 拆线端部中心点的Y坐标（毫米） +- **z (centerZ)**: 拆线端部中心点的Z坐标（毫米） +- **u (rotateAngle)**: 拆线旋转角度（度，范围 -30° ~ 30°） + +### Float 数据格式 + +每个 float 值占用 2 个 uint16 寄存器（IEEE 754 单精度浮点数）： +- 低位寄存器：float 的低16位 +- 高位寄存器：float 的高16位 + +## 注意事项 + +1. **服务端**: 糖包拆线应用启动后自动开启 ModbusTCP 服务端，监听端口 502 +2. **客户端**: 支持标准 ModbusTCP 协议的设备均可连接（PLC、机器人控制器等） +3. **数据顺序**: 拆线数据按检测顺序排列 +4. **自动输出**: 检测成功后，服务端自动将 xyzu 数据写入寄存器[4]开始的位置，客户端无需额外请求 +5. **字节序**: Float 数据使用小端字节序（Little Endian） +6. **连接数**: 支持多个客户端同时连接，但建议单客户端操作 +7. **超时处理**: 建议客户端设置合理的超时时间（如30秒）等待检测完成 + +## 错误处理 + +如果寄存器[2]返回2（失败），可能的原因： +- 相机未连接 +- 检测算法失败 +- 检测处理器未初始化 + +建议客户端在失败后重新启动扫描或通知操作人员检查设备状态。 diff --git a/AppUtils/AppCommon/Inc/TCPServerProtocol.h b/AppUtils/AppCommon/Inc/TCPServerProtocol.h index db72f07..cb7197f 100644 --- a/AppUtils/AppCommon/Inc/TCPServerProtocol.h +++ b/AppUtils/AppCommon/Inc/TCPServerProtocol.h @@ -53,10 +53,10 @@ public: /** * @brief 初始化TCP服务器 - * @param port TCP端口号，默认5020 + * @param port TCP端口号 * @return 0-成功，其他-错误码 */ - int Initialize(uint16_t port = 5020); + int Initialize(uint16_t port = 6800); /** * @brief 反初始化，停止服务 diff --git a/AppUtils/AppCommon/Src/BasePresenter.cpp b/AppUtils/AppCommon/Src/BasePresenter.cpp index c70f4e2..009a46e 100644 --- a/AppUtils/AppCommon/Src/BasePresenter.cpp +++ b/AppUtils/AppCommon/Src/BasePresenter.cpp @@ -624,11 +624,19 @@ void BasePresenter::_StaticDetectionCallback(EVzResultDataType eDataType, SVzLas if (pLaserLinePoint->p3DPoint && pLaserLinePoint->nPointCount > 0) { lineData.p3DPoint = new SVzNL3DPosition[pLaserLinePoint->nPointCount]; if (lineData.p3DPoint) { - memcpy(lineData.p3DPoint, pLaserLinePoint->p3DPoint, sizeof(SVzNL3DPosition) * pLaserLinePoint->nPointCount); + if(pLaserLinePoint->p3DPoint){ + memcpy(lineData.p3DPoint, pLaserLinePoint->p3DPoint, sizeof(SVzNL3DPosition) * pLaserLinePoint->nPointCount); + } else { + memset(lineData.p3DPoint, 0, sizeof(SVzNL3DPosition) * pLaserLinePoint->nPointCount); + } } lineData.p2DPoint = new SVzNL2DPosition[pLaserLinePoint->nPointCount]; - if (lineData.p2DPoint) { - memcpy(lineData.p2DPoint, pLaserLinePoint->p2DPoint, sizeof(SVzNL2DPosition) * pLaserLinePoint->nPointCount); + if (lineData.p2DPoint){ + if(pLaserLinePoint->p2DPoint) { + memcpy(lineData.p2DPoint, pLaserLinePoint->p2DPoint, sizeof(SVzNL2DPosition) * pLaserLinePoint->nPointCount); + } else { + memset(lineData.p2DPoint, 0, sizeof(SVzNL2DPosition) * pLaserLinePoint->nPointCount); + } } } } else if (eDataType == keResultDataType_PointXYZRGBA) { @@ -636,11 +644,19 @@ void BasePresenter::_StaticDetectionCallback(EVzResultDataType eDataType, SVzLas if (pLaserLinePoint->p3DPoint && pLaserLinePoint->nPointCount > 0) { lineData.p3DPoint = new SVzNLPointXYZRGBA[pLaserLinePoint->nPointCount]; if (lineData.p3DPoint) { - memcpy(lineData.p3DPoint, pLaserLinePoint->p3DPoint, sizeof(SVzNLPointXYZRGBA) * pLaserLinePoint->nPointCount); + if(pLaserLinePoint->p3DPoint){ + memcpy(lineData.p3DPoint, pLaserLinePoint->p3DPoint, sizeof(SVzNLPointXYZRGBA) * pLaserLinePoint->nPointCount); + } else { + memset(lineData.p3DPoint, 0, sizeof(SVzNLPointXYZRGBA) * pLaserLinePoint->nPointCount); + } } lineData.p2DPoint = new SVzNL2DLRPoint[pLaserLinePoint->nPointCount]; if (lineData.p2DPoint) { - memcpy(lineData.p2DPoint, pLaserLinePoint->p2DPoint, sizeof(SVzNL2DLRPoint) * pLaserLinePoint->nPointCount); + if(pLaserLinePoint->p2DPoint) { + memcpy(lineData.p2DPoint, pLaserLinePoint->p2DPoint, sizeof(SVzNL2DLRPoint) * pLaserLinePoint->nPointCount); + } else { + memset(lineData.p2DPoint, 0, sizeof(SVzNL2DLRPoint) * pLaserLinePoint->nPointCount); + } } } } @@ -708,7 +724,7 @@ void BasePresenter::_StaticCameraStatusCallback(EVzDeviceWorkStatus eStatus, voi } } - +// 相机一直重联 void BasePresenter::StartCameraReconnectTimer() { LOG_DEBUG("[BasePresenter] StartCameraReconnectTimer called\n"); diff --git a/Device/GlLineLaserDevice/Src/GlLineLaserDevice.cpp b/Device/GlLineLaserDevice/Src/GlLineLaserDevice.cpp index 7244c02..046dac2 100644 --- a/Device/GlLineLaserDevice/Src/GlLineLaserDevice.cpp +++ b/Device/GlLineLaserDevice/Src/GlLineLaserDevice.cpp @@ -6,6 +6,9 @@ #include #include +// 静态实例指针，供SDK回调访问 +CGlLineLaserDevice* CGlLineLaserDevice::s_pInstance = nullptr; + CGlLineLaserDevice::CGlLineLaserDevice() : m_nDeviceId(0) , m_bDeviceOpen(false) @@ -15,6 +18,7 @@ CGlLineLaserDevice::CGlLineLaserDevice() , m_nBatchLines(200) { memset(&m_modelInfo, 0, sizeof(GLX8_2_ModelInfo)); + s_pInstance = this; } CGlLineLaserDevice::~CGlLineLaserDevice() @@ -22,11 +26,13 @@ CGlLineLaserDevice::~CGlLineLaserDevice() if (m_bDeviceOpen) { CloseDevice(); } + if (s_pInstance == this) { + s_pInstance = nullptr; + } } int CGlLineLaserDevice::InitDevice() { - // 初始化 gl_linelaser_sdk int ret = GLX8_2_Initialize(); if (ret != 0) { LOG_ERROR("GLX8_2_Initialize failed: %d\n", ret); @@ -46,7 +52,6 @@ int CGlLineLaserDevice::SetStatusCallback(VzNL_OnNotifyStatusCBEx fNotify, void int CGlLineLaserDevice::OpenDevice(const char* sIP, bool bRGBD, bool bSwing, bool bFillLaser) { - // gl_linelaser_sdk 不支持 RGBD 和摆动模式，忽略这些参数 (void)bRGBD; (void)bSwing; (void)bFillLaser; @@ -56,14 +61,12 @@ int CGlLineLaserDevice::OpenDevice(const char* sIP, bool bRGBD, bool bSwing, boo return SUCCESS; } - // 解析IP地址 GLX8_2_ETHERNET_CONFIG ethConfig; memset(ðConfig, 0, sizeof(ethConfig)); - + if (sIP && strlen(sIP) > 0) { LOG_DEBUG("open IP address format: %s\n", sIP); - // 解析IP字符串 "x.x.x.x" int ip[4]; if (sscanf(sIP, "%d.%d.%d.%d", &ip[0], &ip[1], &ip[2], &ip[3]) == 4) { ethConfig.abyIpAddress[0] = (unsigned char)ip[0]; @@ -84,7 +87,6 @@ int CGlLineLaserDevice::OpenDevice(const char* sIP, bool bRGBD, bool bSwing, boo return ERR_CODE(DEV_NOT_FIND); } - // 使用第一个找到的设备 memcpy(ðConfig, &pDevices[0], sizeof(GLX8_2_ETHERNET_CONFIG)); char ipStr[32]; sprintf(ipStr, "%d.%d.%d.%d", ethConfig.abyIpAddress[0], ethConfig.abyIpAddress[1], ethConfig.abyIpAddress[2], ethConfig.abyIpAddress[3]); @@ -95,7 +97,7 @@ int CGlLineLaserDevice::OpenDevice(const char* sIP, bool bRGBD, bool bSwing, boo // 打开设备 int ret = GLX8_2_EthernetOpen(m_nDeviceId, ðConfig); if (ret != 0) { - LOG_ERROR("GLX8_2_Ethernet Open failed: %d\n", ret); + LOG_ERROR("GLX8_2_EthernetOpen failed: %d\n", ret); return ERR_CODE(DEV_OPEN_ERR); } @@ -114,14 +116,186 @@ int CGlLineLaserDevice::OpenDevice(const char* sIP, bool bRGBD, bool bSwing, boo m_modelInfo.Model, m_nProfileWidth, m_dXPitch, m_dYPitch); } - // 分配数据缓存 - m_profileBuffer.resize(static_cast(m_nProfileWidth) * m_nBatchLines); - m_intensityBuffer.resize(static_cast(m_nProfileWidth) * m_nBatchLines); - m_positionBuffer.resize(m_nProfileWidth); + // 注册SDK批处理回调（批处理参数在设备端软件预先配置） + ret = RegisterBatchCallback(); + if (ret != SUCCESS) { + LOG_ERROR("RegisterBatchCallback failed: %d\n", ret); + return ret; + } + + LOG_DEBUG("Device initialized successfully (callback mode)\n"); + return SUCCESS; +} + +// 配置批处理模式 +int CGlLineLaserDevice::ConfigureBatchMode() +{ + LOG_DEBUG("Configuring batch mode...\n"); + + char inval[4]; + uint32_t ival; + + // 1. 开启批处理测量 + ival = 1; + memcpy(inval, &ival, 4); + int ret = GLX8_2_SetSetting(m_nDeviceId, 1, 0, 0, 3, nullptr, inval, 4); + if (ret != 0) { + LOG_ERROR("Failed to enable batch mode: %d\n", ret); + return ERR_CODE(DEV_CTRL_ERR); + } + LOG_DEBUG("Batch mode enabled\n"); + + // 2. 设置批处理数量 + ival = m_nBatchLines; + memcpy(inval, &ival, 4); + ret = GLX8_2_SetSetting(m_nDeviceId, 1, 0, 0, 0x0a, nullptr, inval, 4); + if (ret != 0) { + LOG_ERROR("Failed to set batch lines: %d\n", ret); + return ERR_CODE(DEV_CTRL_ERR); + } + LOG_DEBUG("Batch lines set to %d\n", m_nBatchLines); + + // 3. 设置带亮度输出 + ival = 1; + memcpy(inval, &ival, 4); + ret = GLX8_2_SetSetting(m_nDeviceId, 1, 2, 0, 0x0b, nullptr, inval, 4); + if (ret != 0) { + LOG_ERROR("Failed to enable intensity output: %d\n", ret); + return ERR_CODE(DEV_CTRL_ERR); + } + LOG_DEBUG("Intensity output enabled\n"); return SUCCESS; } +// 注册SDK批处理回调 +int CGlLineLaserDevice::RegisterBatchCallback() +{ + s_pInstance = this; + + int ret = GLX8_2_SetBatchOneTimeDataHandler(m_nDeviceId, BatchOneTimeCallback); + if (ret != 0) { + LOG_ERROR("GLX8_2_SetBatchOneTimeDataHandler failed: %d\n", ret); + return ERR_CODE(DEV_CTRL_ERR); + } + + LOG_DEBUG("Batch callback registered\n"); + return SUCCESS; +} + +// SDK批处理回调（静态函数，由SDK线程调用） +void CGlLineLaserDevice::BatchOneTimeCallback(const GLX8_2_STR_CALLBACK_INFO* info, const GLX8_2_Data DataObj) +{ + if (s_pInstance == nullptr) { + return; + } + + if (!s_pInstance->m_bDetecting) { + return; + } + + s_pInstance->ProcessBatchData(info, DataObj); +} + +// 处理一次批处理回调数据 +void CGlLineLaserDevice::ProcessBatchData(const GLX8_2_STR_CALLBACK_INFO* info, const GLX8_2_Data DataObj) +{ + if (info == nullptr || DataObj == nullptr) { + LOG_ERROR("ProcessBatchData: null parameter\n"); + return; + } + + // 检查批处理状态 + if (info->returnStatus != 0) { + LOG_WARNING("Batch returnStatus: %d\n", info->returnStatus); + return; + } + + int batchCount = info->BatchPoints; + int width = info->xPoints; + + if (batchCount <= 0 || width <= 0) { + LOG_WARNING("Invalid batch data: batchCount=%d, width=%d\n", batchCount, width); + return; + } + + LOG_DEBUG("Received batch: %d lines, width: %d, startEncoder: %d, batchTimes: %d\n", batchCount, width, info->startEncoder, info->BatchTimes); + + // 使用回调info中的实时参数 + double xPitch = info->xPixth; + if (xPitch <= 0) { + xPitch = m_dXPitch; // 回退到设备初始化时获取的值 + } + + // 从SDK获取数据指针（SDK内部管理内存，无需自己分配） + int32_t* profileData = GLX8_2_GetBatchProfilePoint(DataObj, 0); + uint32_t* encoderData = GLX8_2_GetBatchEncoderPoint(DataObj, 0); + + if (profileData == nullptr) { + LOG_ERROR("GLX8_2_GetBatchProfilePoint returned null\n"); + return; + } + + // 局部位置缓存 + std::vector positionBuffer(width); + + // 逐行处理数据并回调给上层 + for (int lineIdx = 0; lineIdx < batchCount; lineIdx++) { + if (!m_bDetecting) { + break; + } + + // 计算当前行在批处理数据中的偏移 + const int32_t* lineProfile = profileData + static_cast(lineIdx) * width; + + // 转换为xyz坐标 + double yOffset = static_cast(m_ullFrameIndex) * m_dYPitch; + for (int i = 0; i < width; i++) { + SVzNL3DPosition& pos = positionBuffer[i]; + pos.nPointIdx = i; + pos.pt3D.x = (static_cast(i) - static_cast(width) / 2.0) * xPitch; + pos.pt3D.y = yOffset; + + int32_t rawZ = lineProfile[i]; + if (rawZ == 0x7FFFFFFF || rawZ < -100000000) { + pos.pt3D.z = 0.0; + } else { + pos.pt3D.z = static_cast(rawZ) * 0.00001; // 0.01um -> mm + } + } + + // 填充 SVzLaserLineData 结构 + SVzLaserLineData laserLineData; + memset(&laserLineData, 0, sizeof(SVzLaserLineData)); + + laserLineData.p3DPoint = positionBuffer.data(); + laserLineData.p2DPoint = nullptr; + laserLineData.nPointCount = width; + laserLineData.dTotleOffset = yOffset; + laserLineData.dStep = m_dYPitch; + laserLineData.llFrameIdx = m_ullFrameIndex; + laserLineData.llTimeStamp = std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()).count(); + laserLineData.nEncodeNo = encoderData ? encoderData[lineIdx] : 0; + laserLineData.fSwingAngle = 0.0f; + laserLineData.bEndOnceScan = (lineIdx == batchCount - 1) ? VzTrue : VzFalse; + + // 回调给上层应用 + if (m_pDetectCallback) { + m_pDetectCallback(m_eDataType, &laserLineData, m_pDetectCallbackParam); + } + + m_ullFrameIndex++; + } + + LOG_DEBUG("Processed %d lines, total frames: %llu\n", batchCount, m_ullFrameIndex); + + // 通知上层本次批处理数据处理完成 + if (m_pStatusCallback) { + m_pStatusCallback(keDeviceWorkStatus_Device_Swing_Finish, nullptr, 0, m_pStatusCallbackParam); + } +} + int CGlLineLaserDevice::GetVersion(SVzNLVersionInfo& sVersionInfo) { memset(&sVersionInfo, 0, sizeof(SVzNLVersionInfo)); @@ -131,7 +305,6 @@ int CGlLineLaserDevice::GetVersion(SVzNLVersionInfo& sVersionInfo) strncpy(sVersionInfo.szSDKVersion, sdkVersion, VZNL_VERSION_LENGTH - 1); } - // 填充其他版本信息 strncpy(sVersionInfo.szAppVersion, "GlLineLaser", VZNL_VERSION_LENGTH - 1); return SUCCESS; @@ -141,12 +314,10 @@ int CGlLineLaserDevice::GetDevInfo(SVzNLEyeDeviceInfoEx& sDeviceInfo) { memset(&sDeviceInfo, 0, sizeof(SVzNLEyeDeviceInfoEx)); - // 填充设备信息 strncpy(sDeviceInfo.sEyeCBInfo.byServerIP, m_strDeviceIP.c_str(), VZNL_SDK_NETWORK_IPv4_LENGTH - 1); strncpy(sDeviceInfo.sEyeCBInfo.szDeviceName, m_modelInfo.Model, VZNL_DEVICE_NAME_LENGTH - 1); strncpy(sDeviceInfo.sEyeCBInfo.szDeviceID, m_modelInfo.HeaderSerial, VZNL_GUID_LENGTH - 1); - // 设置分辨率 sDeviceInfo.sVideoRes.nFrameWidth = m_nProfileWidth; sDeviceInfo.sVideoRes.nFrameHeight = m_nBatchLines; @@ -191,14 +362,19 @@ int CGlLineLaserDevice::StartDetect(VzNL_AutoOutputLaserLineExCB fCallFunc, EVzR m_pDetectCallback = fCallFunc; m_pDetectCallbackParam = param; m_eDataType = eDataType; - m_bStopDetect = false; m_ullFrameIndex = 0; - // 启动数据采集线程（主动轮询模式） m_bDetecting = true; - m_detectThread = std::thread(&CGlLineLaserDevice::DetectThreadFunc, this); - LOG_DEBUG("Detection started\n"); + // 启动回调模式批处理（0=立即开始） + int ret = GLX8_2_StartMeasureWithCallback(m_nDeviceId, 0); + if (ret != 0) { + LOG_ERROR("GLX8_2_StartMeasureWithCallback failed: %d\n", ret); + m_bDetecting = false; + return ERR_CODE(DEV_CTRL_ERR); + } + + LOG_DEBUG("Detection started (callback mode)\n"); return SUCCESS; } @@ -214,7 +390,7 @@ int CGlLineLaserDevice::StopDetect() return SUCCESS; } - m_bStopDetect = true; + m_bDetecting = false; // 停止批处理 int ret = GLX8_2_StopMeasure(m_nDeviceId); @@ -222,13 +398,6 @@ int CGlLineLaserDevice::StopDetect() LOG_ERROR("GLX8_2_StopMeasure failed: %d\n", ret); } - // 等待线程结束 - if (m_detectThread.joinable()) { - m_detectThread.join(); - } - - m_bDetecting = false; - // 通知状态变化 if (m_pStatusCallback) { m_pStatusCallback(keDeviceWorkStatus_Device_Swing_Finish, nullptr, 0, m_pStatusCallbackParam); @@ -240,163 +409,12 @@ int CGlLineLaserDevice::StopDetect() return SUCCESS; } -// 数据采集线程函数（主动轮询模式，参考 test_deal_atch_datas） -void CGlLineLaserDevice::DetectThreadFunc() -{ - LOG_DEBUG("Detect thread started\n"); - - while (!m_bStopDetect) { - // 启动批处理（参考 test_deal_atch_datas） - int ret = GLX8_2_StartMeasure(m_nDeviceId, 5000); // 5秒超时 - if (ret != 0) { - LOG_ERROR("GLX8_2_StartMeasure failed: %d\n", ret); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - continue; - } - - // 等待一小段时间让设备准备好 - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - - // 获取批处理数据（参考 test_batch_datas） - GetBatchData(); - - // 批处理完成后等待一段时间再开始下一次 - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - - LOG_DEBUG("Detect thread stopped\n"); -} - -// 获取批处理数据（参考 test_batch_datas 的流程） -void CGlLineLaserDevice::GetBatchData() -{ - GLX8_2_STR_CALLBACK_INFO info; - memset(&info, 0, sizeof(info)); - - const int maxBatchLines = m_nBatchLines; - - // 确保缓存足够大 - size_t totalPoints = static_cast(m_nProfileWidth) * maxBatchLines; - if (m_profileBuffer.size() < totalPoints) { - m_profileBuffer.resize(totalPoints); - } - if (m_intensityBuffer.size() < totalPoints) { - m_intensityBuffer.resize(totalPoints); - } - - std::vector encoderBuffer(maxBatchLines); - - // 使用 GLX8_2_ReceiveDataAuto 顺序获取批处理数据 - int ret = GLX8_2_ReceiveDataAuto(m_nDeviceId, &info, - m_profileBuffer.data(), - m_intensityBuffer.data(), - encoderBuffer.data()); - - if (ret != 0) { - LOG_WARNING("GLX8_2_ReceiveDataAuto failed: %d\n", ret); - return; - } - - int batchCount = info.BatchPoints; - int width = info.xPoints; - - if (batchCount <= 0 || width <= 0) { - LOG_WARNING("Invalid batch data: batchCount=%d, width=%d\n", batchCount, width); - return; - } - - LOG_DEBUG("Received batch: %d lines, width: %d, startEncoder: %d\n", - batchCount, width, info.startEncoder); - - // 确保位置缓存足够大 - if (m_positionBuffer.size() < static_cast(width)) { - m_positionBuffer.resize(width); - } - - // 逐行处理数据并回调（转换为xyz点云数据） - for (int lineIdx = 0; lineIdx < batchCount; lineIdx++) { - if (m_bStopDetect) { - break; - } - - // 计算当前行在缓存中的偏移 - const int32_t* lineProfile = m_profileBuffer.data() + static_cast(lineIdx) * width; - - // 转换为xyz坐标 - ConvertProfileToPosition(lineProfile, width, lineIdx); - - // 填充 SVzLaserLineData 结构 - SVzLaserLineData laserLineData; - memset(&laserLineData, 0, sizeof(SVzLaserLineData)); - - laserLineData.p3DPoint = m_positionBuffer.data(); - laserLineData.p2DPoint = nullptr; // 线激光没有2D数据 - laserLineData.nPointCount = width; - laserLineData.dTotleOffset = static_cast(m_ullFrameIndex) * m_dYPitch; - laserLineData.dStep = m_dYPitch; - laserLineData.llFrameIdx = m_ullFrameIndex; - laserLineData.llTimeStamp = std::chrono::duration_cast( - std::chrono::steady_clock::now().time_since_epoch()).count(); - laserLineData.nEncodeNo = encoderBuffer[lineIdx]; // 使用实际的编码器值 - laserLineData.fSwingAngle = 0.0f; // 线激光没有摆动角度 - laserLineData.bEndOnceScan = (lineIdx == batchCount - 1) ? VzTrue : VzFalse; - - // 回调给上层应用 - if (m_pDetectCallback) { - m_pDetectCallback(m_eDataType, &laserLineData, m_pDetectCallbackParam); - } - - m_ullFrameIndex++; - } - - LOG_DEBUG("Processed %d lines, total frames: %llu\n", batchCount, m_ullFrameIndex); -} - -// 将轮廓数据转换为位置数据 -void CGlLineLaserDevice::ConvertProfileToPosition(const int32_t* profileData, int count, int lineIndex) -{ - // lineIndex 参数保留用于未来扩展，当前使用 m_ullFrameIndex 计算全局偏移 - (void)lineIndex; - - // 确保缓存足够大 - if (m_positionBuffer.size() < static_cast(count)) { - m_positionBuffer.resize(count); - } - - // 计算Y偏移（基于全局帧索引） - double yOffset = static_cast(m_ullFrameIndex) * m_dYPitch; - - // 转换每个点 - for (int i = 0; i < count; i++) { - SVzNL3DPosition& pos = m_positionBuffer[i]; - - pos.nPointIdx = i; - - // X 坐标：根据点索引和X间距计算 - // X范围居中：从 -width/2 * xPitch 到 width/2 * xPitch - pos.pt3D.x = (static_cast(i) - static_cast(count) / 2.0) * m_dXPitch; - - // Y 坐标：扫描方向偏移 - pos.pt3D.y = yOffset; - - // Z 坐标：高度值，gl_linelaser_sdk 单位是 0.01um，转换为 mm - // 无效值通常是最大值或特定值，这里假设 0x7FFFFFFF 或负值为无效 - int32_t rawZ = profileData[i]; - if (rawZ == 0x7FFFFFFF || rawZ < -100000000) { - pos.pt3D.z = 0.0; // 无效值设为0 - } else { - pos.pt3D.z = static_cast(rawZ) * 0.00001; // 0.01um -> mm - } - } -} - // ============ ROI相关（线激光不支持）============ int CGlLineLaserDevice::SetDetectROI(SVzNLRect& leftROI, SVzNLRect& rightROI) { (void)leftROI; (void)rightROI; - // 线激光不支持ROI设置 return SUCCESS; } @@ -407,19 +425,17 @@ int CGlLineLaserDevice::GetDetectROI(SVzNLRect& leftROI, SVzNLRect& rightROI) return SUCCESS; } -// ============ 曝光/增益相关（线激光通过SDK设置）============ +// ============ 曝光/增益相关 ============ int CGlLineLaserDevice::SetEyeExpose(unsigned int& exposeTime) { (void)exposeTime; - // gl_linelaser_sdk 通过 GLX8_2_SetSetting 设置曝光 - // 暂不实现详细接口 return SUCCESS; } int CGlLineLaserDevice::GetEyeExpose(unsigned int& exposeTime) { - exposeTime = 1000; // 默认值 + exposeTime = 1000; return SUCCESS; } @@ -431,7 +447,7 @@ int CGlLineLaserDevice::SetEyeGain(unsigned int& gain) int CGlLineLaserDevice::GetEyeGain(unsigned int& gain) { - gain = 100; // 默认值 + gain = 100; return SUCCESS; } @@ -440,13 +456,12 @@ int CGlLineLaserDevice::GetEyeGain(unsigned int& gain) int CGlLineLaserDevice::SetFrame(int& frame) { (void)frame; - // 线激光帧率由硬件决定 return SUCCESS; } int CGlLineLaserDevice::GetFrame(int& frame) { - frame = 100; // 默认帧率 + frame = 100; return SUCCESS; } @@ -454,7 +469,7 @@ int CGlLineLaserDevice::GetFrame(int& frame) bool CGlLineLaserDevice::IsSupport() { - return false; // 不支持RGBD + return false; } int CGlLineLaserDevice::SetRGBDExposeThres(float& value) @@ -520,7 +535,6 @@ int CGlLineLaserDevice::SetWorkRange(double& dMin, double& dMax) int CGlLineLaserDevice::GetWorkRange(double& dMin, double& dMax) { - // 从设备信息获取工作范围 dMin = m_modelInfo.zRangmin; dMax = m_modelInfo.zRangmax; return SUCCESS; @@ -558,11 +572,6 @@ int CGlLineLaserDevice::SetBatchLines(unsigned int batchLines) return ERR_CODE(DEV_ARG_INVAILD); } m_nBatchLines = batchLines; - - // 重新分配缓存 - m_profileBuffer.resize(static_cast(m_nProfileWidth) * m_nBatchLines); - m_intensityBuffer.resize(static_cast(m_nProfileWidth) * m_nBatchLines); - return SUCCESS; } @@ -602,8 +611,6 @@ int CGlLineLaserDevice::GetMeasureRange(double& xMin, double& xMax, double& zMin } // ============ 工厂方法实现 ============ -// 注意：IVrEyeDevice::CreateObject 在 VrEyeDevice 中实现，创建 VzNLSDK 设备 -// GlLineLaserDevice 只提供 IGlLineLaserDevice::CreateGlLineLaserObject 工厂方法 int IGlLineLaserDevice::CreateGlLineLaserObject(IGlLineLaserDevice** ppDevice) { diff --git a/Device/GlLineLaserDevice/_Inc/GlLineLaserDevice.h b/Device/GlLineLaserDevice/_Inc/GlLineLaserDevice.h index af287df..0969764 100644 --- a/Device/GlLineLaserDevice/_Inc/GlLineLaserDevice.h +++ b/Device/GlLineLaserDevice/_Inc/GlLineLaserDevice.h @@ -4,10 +4,7 @@ #include "IGlLineLaserDevice.h" #include "phoskey_ss.h" -#include #include -#include -#include #include #include @@ -15,6 +12,15 @@ * @brief CGlLineLaserDevice 实现类 * * 将 gl_linelaser_sdk (phoskey_ss) 的接口适配到 IVrEyeDevice 接口。 + * 使用 SDK 回调模式（GLX8_2_SetBatchOneTimeDataHandler）获取批处理数据。 + * + * 完整闭环流程： + * OpenDevice → 配置批处理参数 + 注册SDK回调 + * StartDetect → GLX8_2_StartMeasureWithCallback 启动采集 + * SDK回调 → BatchOneTimeCallback → ProcessBatchData → 逐行转换并回调上层 + * StopDetect → GLX8_2_StopMeasure 停止采集 + * CloseDevice → GLX8_2_CommClose 关闭设备 + * * 数据转换：gl_linelaser_sdk 的 int32_t 轮廓数据（单位0.01um）转换为 * SVzLaserLineData 的 SVzNL3DPosition 格式（单位mm）。 */ @@ -100,7 +106,7 @@ private: // 批处理参数 unsigned int m_nBatchLines = 200; // 批处理行数 - // 回调相关 + // 上层回调相关 VzNL_AutoOutputLaserLineExCB m_pDetectCallback = nullptr; void* m_pDetectCallbackParam = nullptr; EVzResultDataType m_eDataType = keResultDataType_Position; @@ -108,42 +114,40 @@ private: VzNL_OnNotifyStatusCBEx m_pStatusCallback = nullptr; void* m_pStatusCallbackParam = nullptr; - // 数据采集线程 - std::thread m_detectThread; + // 检测状态 std::atomic m_bDetecting{false}; - std::atomic m_bStopDetect{false}; - std::mutex m_detectMutex; - std::condition_variable m_detectCondition; - - // 数据缓存 - std::vector m_profileBuffer; // 轮廓数据缓存 - std::vector m_intensityBuffer; // 亮度数据缓存 - std::vector m_positionBuffer; // 转换后的位置数据 // 帧计数 unsigned long long m_ullFrameIndex = 0; - unsigned long long m_ullTimeStamp = 0; // 内部方法 /** - * @brief 数据采集线程函数（主动轮询模式，参考 test_deal_atch_datas） + * @brief 配置批处理模式 */ - void DetectThreadFunc(); + int ConfigureBatchMode(); /** - * @brief 获取批处理数据（参考 test_batch_datas 的流程） + * @brief 注册SDK批处理回调 */ - void GetBatchData(); + int RegisterBatchCallback(); /** - * @brief 将 gl_linelaser_sdk 的轮廓数据转换为 SVzNL3DPosition - * @param profileData 轮廓数据（int32_t，单位0.01um） - * @param count 数据点数 - * @param lineIndex 当前行索引 - * @return 转换后的位置数据数组 + * @brief SDK 批处理回调函数（静态，由SDK线程调用） + * @param info 批处理信息 + * @param DataObj 批处理数据对象，用于提取轮廓/亮度/编码器 */ - void ConvertProfileToPosition(const int32_t* profileData, int count, int lineIndex); + static void BatchOneTimeCallback(const GLX8_2_STR_CALLBACK_INFO* info, const GLX8_2_Data DataObj); + + /** + * @brief 处理一次批处理回调数据，逐行转换并回调给上层 + * @param info 批处理信息 + * @param DataObj 批处理数据对象 + */ + void ProcessBatchData(const GLX8_2_STR_CALLBACK_INFO* info, const GLX8_2_Data DataObj); + + // 静态回调需要通过实例指针访问成员，保存this指针供回调使用 + static CGlLineLaserDevice* s_pInstance; }; #endif // CGLLINELASERDEVICE_H diff --git a/GrabBagPrj/buildArmPrj.sh b/GrabBagPrj/buildArmPrj.sh index 8bd29c8..d58296e 100644 --- a/GrabBagPrj/buildArmPrj.sh +++ b/GrabBagPrj/buildArmPrj.sh @@ -1,7 +1,7 @@ #!/bin/bash # 可用的 App 列表 -AVAILABLE_APPS="GrabBag BeltTearing LapWeld Workpiece ParticleSize BinocularMark WorkpieceProject TunnelChannel WheelMeasure ScrewPosition WorkpieceHole" +AVAILABLE_APPS="GrabBag BeltTearing LapWeld Workpiece ParticleSize BinocularMark WorkpieceProject TunnelChannel WheelMeasure ScrewPosition WorkpieceHole BagThreadPosition" # 显示帮助信息 show_help() { diff --git a/GrabBagPrj/pkg_bagthreadposition.sh b/GrabBagPrj/pkg_bagthreadposition.sh new file mode 100644 index 0000000..5462523 --- /dev/null +++ b/GrabBagPrj/pkg_bagthreadposition.sh @@ -0,0 +1,410 @@ +#!/bin/bash + +#BagThreadPosition 版本配置 +PKG_NAME="BagThreadPosition" +PKG_ARCH="arm64" + +# 从Version.h文件中读取版本信息 +VERSION_FILE="../App/BagThreadPosition/BagThreadPositionApp/Version.h" + +if [ -f "${VERSION_FILE}" ]; then + # 读取版本号（从 BAGTHREADPOSITION_VERSION_STRING 中提取） + PKG_VERSION=$(grep '#define BAGTHREADPOSITION_VERSION_STRING' ${VERSION_FILE} | sed 's/.*"\(.*\)".*/\1/') + # 读取构建号（从 BAGTHREADPOSITION_BUILD_STRING 中提取） + BUILD_NUMBER=$(grep '#define BAGTHREADPOSITION_BUILD_STRING' ${VERSION_FILE} | sed 's/.*"\(.*\)".*/\1/') + + echo "从 ${VERSION_FILE} 读取版本信息:" + echo " 版本号: ${PKG_VERSION}" + echo " 构建号: ${BUILD_NUMBER}" + + # 如果读取失败，使用默认值 + if [ -z "${PKG_VERSION}" ]; then + PKG_VERSION="1.0.0" + echo "警告: 无法读取版本号，使用默认值: ${PKG_VERSION}" + fi + + if [ -z "${BUILD_NUMBER}" ]; then + BUILD_NUMBER="1" + echo "警告: 无法读取构建号，使用默认值: ${BUILD_NUMBER}" + fi +else + # Version.h文件不存在时的默认值 + PKG_VERSION="1.0.0" + BUILD_NUMBER="1" + echo "警告: ${VERSION_FILE} 文件不存在，使用默认版本信息" + echo " 版本号: ${PKG_VERSION}" + echo " 构建号: ${BUILD_NUMBER}" +fi + +PKG_PATH=$HOME/BagThreadPositionPkg +CODE_PATH=../ +RELEASE_PATH=../Publish + +echo "==========================================" +echo "开始打包 BagThreadPosition 应用程序 v${PKG_VERSION}..." +echo "==========================================" + +echo "清理所有旧的打包目录..." +rm -rf $HOME/*Pkg + +#QT depend +QT_PKG_PATH=/opt/firefly_qt5.15_arm64_20.04 +QT_LIB_PATH=/opt/sysroot/firefly-arm64-sysroot-20.04/lib/aarch64-linux-gnu + +echo "创建打包目录结构..." +mkdir -p ${PKG_PATH}/DEBIAN +mkdir -p ${PKG_PATH}/etc/profile.d +mkdir -p ${PKG_PATH}/etc/xdg/autostart +mkdir -p ${PKG_PATH}/opt/sysroot/lib +mkdir -p ${PKG_PATH}/usr/local/bin +mkdir -p ${PKG_PATH}/usr/lib +mkdir -p ${PKG_PATH}/usr/share/applications +mkdir -p ${PKG_PATH}/usr/share/pixmaps + +echo "复制 Qt 运行时环境..." +cp -rfd ${QT_PKG_PATH}/ext ${PKG_PATH}/opt/firefly_qt5.15 +cp ${QT_PKG_PATH}/target_qtEnv.sh ${PKG_PATH}/etc/profile.d/ + +# 复制 Qt 库文件 +for libfile in ${QT_LIB_PATH}/*.so*; do + # 获取文件名用于比较 + filename=$(basename "$libfile") + + # 跳过 LLVM、flite、clang 和 X11 相关库文件 + if [[ "$filename" == *icu* ]]; then + # 复制其他库文件，保持符号链接 + cp -rfd "$libfile" ${PKG_PATH}/opt/sysroot/lib/ + continue + fi +done + +echo "复制依赖库文件..." +#depend +cp -a ${CODE_PATH}/SDK/OpenCV320/Arm/aarch64/*opencv_core*.so* ${PKG_PATH}/usr/lib/ +cp -a ${CODE_PATH}/SDK/OpenCV320/Arm/aarch64/*opencv_imgproc*.so* ${PKG_PATH}/usr/lib/ +cp -a ${CODE_PATH}/SDK/OpenCV320/Arm/aarch64/*opencv_highgui*.so* ${PKG_PATH}/usr/lib/ + +cp ${CODE_PATH}/AppAlgo/bagThreadPositioning/Arm/aarch64/*.so ${PKG_PATH}/usr/lib/ +cp ${CODE_PATH}/SDK/Device/VzNLSDK/Arm/aarch64/*.so ${PKG_PATH}/usr/lib/ +cp ${CODE_PATH}/SDK/Device/gl_linelaser_sdk/aarch64_linux/*.so ${PKG_PATH}/usr/lib/ + +echo "复制应用程序主文件..." +#APP +cp ${CODE_PATH}/GrabBagPrj/buildarm/App/BagThreadPosition/BagThreadPositionApp/BagThreadPositionApp ${PKG_PATH}/usr/local/bin/ + +echo "复制应用程序图标..." +#LOGO +cp ${CODE_PATH}/App/BagThreadPosition/BagThreadPositionApp/resource/logo.png ${PKG_PATH}/usr/share/pixmaps/bagthreadposition.png + +echo "生成桌面自启动配置文件..." +#desktop autostart configuration +AUTOSTART_PATH=${PKG_PATH}/etc/xdg/autostart/bagthreadposition.desktop +cat > ${AUTOSTART_PATH} << EOF +[Desktop Entry] +Type=Application +Name=BagThreadPosition +Comment=BagThreadPosition Application Auto Start +Exec=/usr/local/bin/BagThreadPositionApp +Icon=/usr/share/pixmaps/bagthreadposition.png +Hidden=false +NoDisplay=false +X-GNOME-Autostart-enabled=true +AutostartCondition=GNOME3 unless-session gnome +EOF + +echo "生成 control 文件..." +#control +CONTROL_PATH=${PKG_PATH}/DEBIAN/control +echo "Package: ${PKG_NAME}" > ${CONTROL_PATH} +echo "Version: ${PKG_VERSION}" >> ${CONTROL_PATH} +echo "Section: bagthreadpositionapp" >> ${CONTROL_PATH} +echo "Architecture: ${PKG_ARCH}" >> ${CONTROL_PATH} +echo "Priority: optional" >> ${CONTROL_PATH} +echo "Maintainer: BagThreadPosition Team " >> ${CONTROL_PATH} +echo "Description: bagthreadposition app" >> ${CONTROL_PATH} + + + +echo "生成安装后脚本..." +#postinst install exec script +POSTINST_PATH=${PKG_PATH}/DEBIAN/postinst +cat > ${POSTINST_PATH} << 'EOF' +#!/bin/bash + +echo "配置 BagThreadPosition 应用程序..." + +# 创建应用注册目录和文件 +REGISTRY_DIR="/var/lib/vr-apps" +REGISTRY_FILE="${REGISTRY_DIR}/installed-apps.list" +APP_NAME="BagThreadPosition" + +mkdir -p "${REGISTRY_DIR}" +touch "${REGISTRY_FILE}" + +# 检查是否是首次安装VR应用 +FIRST_INSTALL=false +if [ ! -s "${REGISTRY_FILE}" ]; then + FIRST_INSTALL=true + echo "检测到首次安装VR应用，将安装公共资源..." +fi + +# 设置库文件路径(每个应用独立配置) +echo "/usr/lib" > /etc/ld.so.conf.d/bagthreadposition.conf +echo "/opt/sysroot/lib/" >> /etc/ld.so.conf.d/bagthreadposition.conf + +# 如果不是首次安装，公共库已经配置过，只需要更新 +if [ "${FIRST_INSTALL}" = true ]; then + echo "配置公共库路径..." + ldconfig +else + echo "公共库已存在，仅更新配置..." + ldconfig +fi + +# 确保应用程序可执行 +chmod +x /usr/local/bin/BagThreadPositionApp + +# 配置端口映射 502 -> 5020 +echo "配置端口映射 502 -> 5020..." + +# 检查并创建systemd服务确保重启后规则生效 +if [ ! -f /etc/systemd/system/vr-port-mapping.service ]; then + echo "创建端口映射服务..." + cat > /etc/systemd/system/vr-port-mapping.service << 'PORTEOF' +[Unit] +Description=VR Common Port Mapping Service +After=network.target + +[Service] +Type=oneshot +ExecStart=/bin/bash -c 'iptables-legacy -t nat -A PREROUTING -p tcp -m tcp --dport 502 -j REDIRECT --to-port 5020' +RemainAfterExit=yes + +[Install] +WantedBy=multi-user.target +PORTEOF + + systemctl enable vr-port-mapping.service + systemctl start vr-port-mapping.service +else + echo "端口映射服务已存在，跳过创建..." + # 确保服务处于启用状态 + systemctl enable vr-port-mapping.service 2>/dev/null || true + systemctl start vr-port-mapping.service 2>/dev/null || true +fi + +# 检查iptables-legacy规则是否已存在 +if ! iptables-legacy -t nat -C PREROUTING -p tcp -m tcp --dport 502 -j REDIRECT --to-port 5020 2>/dev/null; then + echo "添加iptables-legacy端口映射规则..." + iptables-legacy -t nat -A PREROUTING -p tcp -m tcp --dport 502 -j REDIRECT --to-port 5020 +else + echo "iptables-legacy规则已存在，跳过添加..." +fi + +# 检查并创建当前用户的桌面快捷方式 +echo "检查当前用户的桌面快捷方式..." + +# 获取当前执行安装的用户信息 +if [ -n "$SUDO_USER" ]; then + # 如果是通过sudo执行的，获取真实用户 + current_user="$SUDO_USER" + current_home=$(getent passwd "$current_user" | cut -d: -f6) +else + # 直接执行的情况 + current_user=$(whoami) + current_home="$HOME" +fi + +echo "当前用户: $current_user" +echo "用户主目录: $current_home" + +# 检查多种可能的桌面目录名称 +desktop_dirs=("Desktop" "桌面" "desktop") +desktop_dir="" +desktop_shortcut="" + +for dir_name in "${desktop_dirs[@]}"; do + potential_dir="$current_home/$dir_name" + if [ -d "$potential_dir" ]; then + desktop_dir="$potential_dir" + desktop_shortcut="$desktop_dir/bagthreadposition.desktop" + echo "找到桌面目录: $desktop_dir" + break + fi +done + +# 检查是否找到桌面目录 +if [ -n "$desktop_dir" ]; then + # 检查桌面上是否已有快捷方式 + if [ ! -f "$desktop_shortcut" ]; then + echo "为当前用户创建桌面快捷方式..." + # 复制桌面文件到用户桌面 + cp /usr/share/applications/bagthreadposition.desktop "$desktop_shortcut" + # 设置正确的所有者和权限 + chown $current_user:$current_user "$desktop_shortcut" 2>/dev/null || true + chmod 755 "$desktop_shortcut" + echo "已创建桌面快捷方式: $desktop_shortcut" + else + echo "桌面快捷方式已存在，跳过创建" + fi +else + echo "当前用户没有找到桌面目录(Desktop/桌面/desktop)，跳过桌面快捷方式创建" +fi + +# 注册应用到系统 +if ! grep -q "^${APP_NAME}$" "${REGISTRY_FILE}"; then + echo "${APP_NAME}" >> "${REGISTRY_FILE}" + echo "应用 ${APP_NAME} 已注册到系统" +else + echo "应用 ${APP_NAME} 已经注册，跳过..." +fi + +echo "BagThreadPosition 应用程序安装完成！" +echo "应用程序将在用户登录桌面后自动启动。" +echo "端口映射已配置：502 -> 5020" +echo "桌面快捷方式已创建（如果用户有Desktop目录）" +echo "如需立即启动，请运行: /usr/local/bin/BagThreadPositionApp" +echo "如需禁用自启动，请删除文件: ~/.config/autostart/bagthreadposition.desktop" +echo "已安装的VR应用: $(cat ${REGISTRY_FILE} | tr '\n' ' ')" +EOF + +chmod +x ${POSTINST_PATH} + +echo "生成卸载脚本..." +#postrm uninstall exec script +POSTRM_PATH=${PKG_PATH}/DEBIAN/postrm +cat > ${POSTRM_PATH} << 'EOF' +#!/bin/bash + +echo "卸载 BagThreadPosition 应用程序..." + +# 应用注册信息 +REGISTRY_DIR="/var/lib/vr-apps" +REGISTRY_FILE="${REGISTRY_DIR}/installed-apps.list" +APP_NAME="BagThreadPosition" + +# 从注册表中移除应用 +if [ -f "${REGISTRY_FILE}" ]; then + sed -i "/^${APP_NAME}$/d" "${REGISTRY_FILE}" + echo "应用 ${APP_NAME} 已从系统注册表中移除" +fi + +# 清理应用专属的库文件配置 +rm -f /etc/ld.so.conf.d/bagthreadposition.conf + +# 检查是否还有其他VR应用在使用公共资源 +REMAINING_APPS=0 +if [ -f "${REGISTRY_FILE}" ]; then + REMAINING_APPS=$(grep -c . "${REGISTRY_FILE}" 2>/dev/null || echo "0") +fi + +echo "系统中剩余的VR应用数量: ${REMAINING_APPS}" + +# 如果没有其他应用了，清理公共资源 +if [ "${REMAINING_APPS}" -eq 0 ]; then + echo "没有其他VR应用，开始清理公共资源..." + + # 清理端口映射配置 + echo "清理端口映射配置..." + systemctl stop vr-port-mapping.service 2>/dev/null || true + systemctl disable vr-port-mapping.service 2>/dev/null || true + rm -f /etc/systemd/system/vr-port-mapping.service + + # 清理iptables-legacy规则 + iptables-legacy -t nat -D PREROUTING -p tcp -m tcp --dport 502 -j REDIRECT --to-port 5020 2>/dev/null || true + + # 清理注册目录 + rm -rf "${REGISTRY_DIR}" + + echo "公共资源已清理" +else + echo "系统中还有 ${REMAINING_APPS} 个VR应用在运行，保留公共资源" + if [ -f "${REGISTRY_FILE}" ]; then + echo "剩余应用: $(cat ${REGISTRY_FILE} | tr '\n' ' ')" + fi +fi + +# 更新库缓存 +ldconfig + +# 重新加载systemd +systemctl daemon-reload + +# 清理当前用户的桌面快捷方式 +echo "清理当前用户的桌面快捷方式..." + +# 获取当前执行卸载的用户信息 +if [ -n "$SUDO_USER" ]; then + # 如果是通过sudo执行的，获取真实用户 + current_user="$SUDO_USER" + current_home=$(getent passwd "$current_user" | cut -d: -f6) +else + # 直接执行的情况 + current_user=$(whoami) + current_home="$HOME" +fi + +# 检查多种可能的桌面目录名称并清理快捷方式 +desktop_dirs=("Desktop" "桌面" "desktop") +shortcut_found=false + +for dir_name in "${desktop_dirs[@]}"; do + desktop_shortcut="$current_home/$dir_name/bagthreadposition.desktop" + if [ -f "$desktop_shortcut" ]; then + echo "删除当前用户的桌面快捷方式: $desktop_shortcut" + rm -f "$desktop_shortcut" + echo "已删除桌面快捷方式: $desktop_shortcut" + shortcut_found=true + fi +done + +if [ "$shortcut_found" = false ]; then + echo "当前用户没有找到桌面快捷方式，跳过清理" +fi + +echo "BagThreadPosition 应用程序卸载完成！" +if [ "${REMAINING_APPS}" -eq 0 ]; then + echo "所有VR应用已卸载，公共资源已清理" +else + echo "公共资源已保留供其他VR应用使用" +fi +echo "桌面快捷方式已清理" +echo "如需彻底清理自启动配置，请手动删除: ~/.config/autostart/bagthreadposition.desktop" +EOF + +chmod +x ${POSTRM_PATH} + +echo "生成桌面快捷方式..." +#desktop +DESKTOP_PATH=${PKG_PATH}/usr/share/applications/bagthreadposition.desktop +echo "[Desktop Entry]" > ${DESKTOP_PATH} +echo "Version=${PKG_VERSION}" >> ${DESKTOP_PATH} +echo "Name=BagThreadPosition" >> ${DESKTOP_PATH} +echo "Type=Application" >> ${DESKTOP_PATH} +echo "Comment=BagThreadPosition App" >> ${DESKTOP_PATH} +echo "Terminal=false" >> ${DESKTOP_PATH} +echo "Exec=/usr/local/bin/BagThreadPositionApp" >> ${DESKTOP_PATH} +echo "Icon=/usr/share/pixmaps/bagthreadposition.png" >> ${DESKTOP_PATH} +echo "Categories=Development;" >> ${DESKTOP_PATH} +echo "GenericName=BagThreadPosition App" >> ${DESKTOP_PATH} +echo "Keywords=bagthreadposition;app;" >> ${DESKTOP_PATH} +echo "StartupNotify=true" >> ${DESKTOP_PATH} + +echo "设置文件权限..." +# 设置usr目录权限（不包括DEBIAN） +chmod -R 755 ${PKG_PATH}/usr +chmod -R 755 ${PKG_PATH}/etc +chmod -R 755 ${PKG_PATH}/opt + +echo "开始构建 DEB 包..." +# 生成带时间戳和构建号的包文件名 +TIMESTAMP=$(date +%Y%m%d%H%M%S) +DEB_FILENAME="${RELEASE_PATH}/${PKG_NAME}_${PKG_VERSION}_${BUILD_NUMBER}_${PKG_ARCH}_${TIMESTAMP}.deb" + +fakeroot dpkg -b ${PKG_PATH} ${DEB_FILENAME} + +echo "==========================================" +echo "打包完成！" +echo "生成的包文件: ${DEB_FILENAME}" +echo "文件大小: $(ls -lh ${DEB_FILENAME} | awk '{print $5}')" +echo "==========================================" diff --git a/GrabBagPrj/pkg_workpiecehole.sh b/GrabBagPrj/pkg_workpiecehole.sh index 08e048c..a4f319c 100644 --- a/GrabBagPrj/pkg_workpiecehole.sh +++ b/GrabBagPrj/pkg_workpiecehole.sh @@ -85,7 +85,7 @@ cp -a ${CODE_PATH}/SDK/OpenCV320/Arm/aarch64/*opencv_imgproc*.so* ${PKG_PATH}/u cp -a ${CODE_PATH}/SDK/OpenCV320/Arm/aarch64/*opencv_highgui*.so* ${PKG_PATH}/usr/lib/ cp ${CODE_PATH}/AppAlgo/workpieceHolePositioning/Arm/aarch64/*.so ${PKG_PATH}/usr/lib/ -cp ${CODE_PATH}/SDK/Device/VzNLSDK/Arm/aarch64/*.so ${PKG_PATH}/usr/lib/ +cp ${CODE_PATH}/SDK/Device/VzNLSDK/Arm/aarch64/*.so ${PKG_PATH}/usr/lib/ echo "复制应用程序主文件..." #APP diff --git a/Tools/CloudView/Inc/CloudViewMainWindow.h b/Tools/CloudView/Inc/CloudViewMainWindow.h index 09e264c..6c0ef94 100644 --- a/Tools/CloudView/Inc/CloudViewMainWindow.h +++ b/Tools/CloudView/Inc/CloudViewMainWindow.h @@ -43,6 +43,16 @@ private slots: */ void onOpenFile(); + /** + * @brief 打开线段文件 + */ + void onOpenSegmentFile(); + + /** + * @brief 打开姿态点文件 + */ + void onOpenPoseFile(); + /** * @brief 清除所有点云 */ @@ -98,6 +108,26 @@ private slots: */ void onLinePointTableClicked(int row, int column); + /** + * @brief 显示点1的姿态 + */ + void onShowPose1(); + + /** + * @brief 显示点2的姿态 + */ + void onShowPose2(); + + /** + * @brief 显示输入的线段 + */ + void onShowInputLine(); + + /** + * @brief 清除输入的线段 + */ + void onClearInputLine(); + private: /** * @brief 初始化界面 @@ -124,6 +154,16 @@ private: */ QGroupBox* createMeasureGroup(); + /** + * @brief 创建选点测距页面 + */ + QWidget* createMeasurePage(); + + /** + * @brief 创建选线页面 + */ + QWidget* createLinePage(); + /** * @brief 创建选线拟合组 */ @@ -157,6 +197,8 @@ private: // 文件操作控件 QPushButton* m_btnOpenFile; + QPushButton* m_btnOpenSegment; + QPushButton* m_btnOpenPose; QPushButton* m_btnClearAll; QPushButton* m_btnResetView; @@ -167,6 +209,18 @@ private: QLabel* m_lblPoint2; QLabel* m_lblDistance; + // 点1姿态输入控件 + QLineEdit* m_editRx1; + QLineEdit* m_editRy1; + QLineEdit* m_editRz1; + QPushButton* m_btnShowPose1; + + // 点2姿态输入控件 + QLineEdit* m_editRx2; + QLineEdit* m_editRy2; + QLineEdit* m_editRz2; + QPushButton* m_btnShowPose2; + // 选线拟合控件 QPushButton* m_btnClearLine; QPushButton* m_btnShowLinePoints; @@ -177,6 +231,16 @@ private: QLabel* m_lblLineIndex; QLabel* m_lblLinePointCount; + // 输入线段控件 + QLineEdit* m_editLineX1; + QLineEdit* m_editLineY1; + QLineEdit* m_editLineZ1; + QLineEdit* m_editLineX2; + QLineEdit* m_editLineY2; + QLineEdit* m_editLineZ2; + QPushButton* m_btnShowLine; + QPushButton* m_btnClearLine2; + // 点云列表 QListWidget* m_cloudList; diff --git a/Tools/CloudView/Inc/PointCloudGLWidget.h b/Tools/CloudView/Inc/PointCloudGLWidget.h index ee1fc2c..2e450a6 100644 --- a/Tools/CloudView/Inc/PointCloudGLWidget.h +++ b/Tools/CloudView/Inc/PointCloudGLWidget.h @@ -65,6 +65,34 @@ struct SelectedLineInfo SelectedLineInfo() : valid(false), cloudIndex(-1), lineIndex(-1), pointIndex(-1), pointCount(0), mode(LineSelectMode::Vertical) {} }; +/** + * @brief 线段数据 + */ +struct LineSegment +{ + float x1, y1, z1; // 起点 + float x2, y2, z2; // 终点 + float r, g, b; // 颜色 (0-1) + + LineSegment() : x1(0), y1(0), z1(0), x2(0), y2(0), z2(0), r(1), g(1), b(1) {} + LineSegment(float _x1, float _y1, float _z1, float _x2, float _y2, float _z2, float _r = 1.0f, float _g = 1.0f, float _b = 1.0f) + : x1(_x1), y1(_y1), z1(_z1), x2(_x2), y2(_y2), z2(_z2), r(_r), g(_g), b(_b) {} +}; + +/** + * @brief 姿态点数据 + */ +struct PosePoint +{ + float x, y, z; // 位置 + float rx, ry, rz; // 欧拉角（度） + float scale; // 坐标系大小 + + PosePoint() : x(0), y(0), z(0), rx(0), ry(0), rz(0), scale(10.0f) {} + PosePoint(float _x, float _y, float _z, float _rx, float _ry, float _rz, float _scale = 10.0f) + : x(_x), y(_y), z(_z), rx(_rx), ry(_ry), rz(_rz), scale(_scale) {} +}; + /** * @brief 点云 OpenGL 渲染控件 */ @@ -145,6 +173,26 @@ public: */ size_t getCloudCount() const { return m_pointClouds.size(); } + /** + * @brief 添加线段 + */ + void addLineSegments(const QVector& segments); + + /** + * @brief 清除所有线段 + */ + void clearLineSegments(); + + /** + * @brief 添加姿态点 + */ + void addPosePoints(const QVector& poses); + + /** + * @brief 清除所有姿态点 + */ + void clearPosePoints(); + signals: void pointSelected(const SelectedPointInfo& point); void twoPointsSelected(const SelectedPointInfo& p1, const SelectedPointInfo& p2, float distance); @@ -168,6 +216,8 @@ private: void drawMeasurementLine(); void drawAxis(); void drawSelectedLine(); // 绘制选中的线 + void drawLineSegments(); // 绘制线段 + void drawPosePoints(); // 绘制姿态点 struct PointCloudData { @@ -216,6 +266,10 @@ private: int m_colorIndex; // 颜色轮换索引 static const int COLOR_COUNT = 7; // 可用颜色数量 + + // 线段和姿态点数据 + QVector m_lineSegments; + QVector m_posePoints; }; #endif // POINT_CLOUD_GL_WIDGET_H diff --git a/Tools/CloudView/Src/CloudViewMainWindow.cpp b/Tools/CloudView/Src/CloudViewMainWindow.cpp index c5c33bb..8e7e3e9 100644 --- a/Tools/CloudView/Src/CloudViewMainWindow.cpp +++ b/Tools/CloudView/Src/CloudViewMainWindow.cpp @@ -5,6 +5,12 @@ #include #include #include +#include +#include +#include +#include +#include +#include #include "VrLog.h" CloudViewMainWindow::CloudViewMainWindow(QWidget* parent) @@ -81,7 +87,7 @@ QWidget* CloudViewMainWindow::createViewerArea() QWidget* CloudViewMainWindow::createControlPanel() { QWidget* widget = new QWidget(this); - widget->setMaximumWidth(300); + widget->setMaximumWidth(400); // 加宽到400 QVBoxLayout* layout = new QVBoxLayout(widget); layout->setContentsMargins(5, 5, 5, 5); layout->setSpacing(10); @@ -89,11 +95,11 @@ QWidget* CloudViewMainWindow::createControlPanel() // 文件操作组 layout->addWidget(createFileGroup()); - // 选点测距组 - layout->addWidget(createMeasureGroup()); - - // 选线拟合组 - layout->addWidget(createLineGroup()); + // 创建 Tab 控件 + QTabWidget* tabWidget = new QTabWidget(widget); + tabWidget->addTab(createMeasurePage(), "选点测距"); + tabWidget->addTab(createLinePage(), "选线"); + layout->addWidget(tabWidget); // 点云列表组 layout->addWidget(createCloudListGroup()); @@ -107,14 +113,22 @@ QWidget* CloudViewMainWindow::createControlPanel() QGroupBox* CloudViewMainWindow::createFileGroup() { QGroupBox* group = new QGroupBox("文件操作", this); - group->setMaximumWidth(300); + group->setMaximumWidth(400); QVBoxLayout* layout = new QVBoxLayout(group); - m_btnOpenFile = new QPushButton("打开文件", group); + m_btnOpenFile = new QPushButton("打开点云", group); m_btnOpenFile->setMinimumHeight(30); connect(m_btnOpenFile, &QPushButton::clicked, this, &CloudViewMainWindow::onOpenFile); layout->addWidget(m_btnOpenFile); + m_btnOpenSegment = new QPushButton("打开线段 {x,y,z}-{x,y,z}", group); + connect(m_btnOpenSegment, &QPushButton::clicked, this, &CloudViewMainWindow::onOpenSegmentFile); + layout->addWidget(m_btnOpenSegment); + + m_btnOpenPose = new QPushButton("打开姿态点 {x,y,z}-{r,p,y}", group); + connect(m_btnOpenPose, &QPushButton::clicked, this, &CloudViewMainWindow::onOpenPoseFile); + layout->addWidget(m_btnOpenPose); + m_btnClearAll = new QPushButton("清除所有", group); connect(m_btnClearAll, &QPushButton::clicked, this, &CloudViewMainWindow::onClearAll); layout->addWidget(m_btnClearAll); @@ -126,10 +140,98 @@ QGroupBox* CloudViewMainWindow::createFileGroup() return group; } +QWidget* CloudViewMainWindow::createMeasurePage() +{ + QWidget* page = new QWidget(this); + QVBoxLayout* layout = new QVBoxLayout(page); + layout->setContentsMargins(0, 5, 0, 0); + layout->addWidget(createMeasureGroup()); + layout->addStretch(); + return page; +} + +QWidget* CloudViewMainWindow::createLinePage() +{ + QWidget* page = new QWidget(this); + QVBoxLayout* layout = new QVBoxLayout(page); + layout->setContentsMargins(0, 5, 0, 0); + + // 选线拟合组 + layout->addWidget(createLineGroup()); + + // 输入线段组 + QGroupBox* inputLineGroup = new QGroupBox("输入线段", page); + QVBoxLayout* inputLayout = new QVBoxLayout(inputLineGroup); + + // 提示 + QLabel* lblTip = new QLabel("输入两点坐标显示线段", inputLineGroup); + lblTip->setStyleSheet("color: gray; font-size: 10px;"); + inputLayout->addWidget(lblTip); + + // 点1坐标 + QLabel* lblPoint1 = new QLabel("点1:", inputLineGroup); + lblPoint1->setStyleSheet("font-weight: bold;"); + inputLayout->addWidget(lblPoint1); + + QHBoxLayout* p1Layout = new QHBoxLayout(); + p1Layout->addWidget(new QLabel("X:", inputLineGroup)); + m_editLineX1 = new QLineEdit("0.0", inputLineGroup); + m_editLineX1->setMaximumWidth(70); + p1Layout->addWidget(m_editLineX1); + + p1Layout->addWidget(new QLabel("Y:", inputLineGroup)); + m_editLineY1 = new QLineEdit("0.0", inputLineGroup); + m_editLineY1->setMaximumWidth(70); + p1Layout->addWidget(m_editLineY1); + + p1Layout->addWidget(new QLabel("Z:", inputLineGroup)); + m_editLineZ1 = new QLineEdit("0.0", inputLineGroup); + m_editLineZ1->setMaximumWidth(70); + p1Layout->addWidget(m_editLineZ1); + inputLayout->addLayout(p1Layout); + + // 点2坐标 + QLabel* lblPoint2 = new QLabel("点2:", inputLineGroup); + lblPoint2->setStyleSheet("font-weight: bold;"); + inputLayout->addWidget(lblPoint2); + + QHBoxLayout* p2Layout = new QHBoxLayout(); + p2Layout->addWidget(new QLabel("X:", inputLineGroup)); + m_editLineX2 = new QLineEdit("100.0", inputLineGroup); + m_editLineX2->setMaximumWidth(70); + p2Layout->addWidget(m_editLineX2); + + p2Layout->addWidget(new QLabel("Y:", inputLineGroup)); + m_editLineY2 = new QLineEdit("100.0", inputLineGroup); + m_editLineY2->setMaximumWidth(70); + p2Layout->addWidget(m_editLineY2); + + p2Layout->addWidget(new QLabel("Z:", inputLineGroup)); + m_editLineZ2 = new QLineEdit("100.0", inputLineGroup); + m_editLineZ2->setMaximumWidth(70); + p2Layout->addWidget(m_editLineZ2); + inputLayout->addLayout(p2Layout); + + // 按钮 + QHBoxLayout* btnLayout = new QHBoxLayout(); + m_btnShowLine = new QPushButton("显示线段", inputLineGroup); + connect(m_btnShowLine, &QPushButton::clicked, this, &CloudViewMainWindow::onShowInputLine); + btnLayout->addWidget(m_btnShowLine); + + m_btnClearLine2 = new QPushButton("清除线段", inputLineGroup); + connect(m_btnClearLine2, &QPushButton::clicked, this, &CloudViewMainWindow::onClearInputLine); + btnLayout->addWidget(m_btnClearLine2); + inputLayout->addLayout(btnLayout); + + layout->addWidget(inputLineGroup); + layout->addStretch(); + return page; +} + QGroupBox* CloudViewMainWindow::createMeasureGroup() { QGroupBox* group = new QGroupBox("选点测距", this); - group->setMaximumWidth(300); + group->setMaximumWidth(400); QVBoxLayout* layout = new QVBoxLayout(group); // 操作说明 @@ -156,23 +258,91 @@ QGroupBox* CloudViewMainWindow::createMeasureGroup() connect(m_btnClearPoints, &QPushButton::clicked, this, &CloudViewMainWindow::onClearSelectedPoints); layout->addWidget(m_btnClearPoints); - // 点1信息 - QHBoxLayout* point1Layout = new QHBoxLayout(); + // 分隔线 + QFrame* line1 = new QFrame(group); + line1->setFrameShape(QFrame::HLine); + line1->setFrameShadow(QFrame::Sunken); + layout->addWidget(line1); + + // ========== 点1信息和姿态 ========== QLabel* lblPoint1Title = new QLabel("点1:", group); + lblPoint1Title->setStyleSheet("font-weight: bold;"); + layout->addWidget(lblPoint1Title); + m_lblPoint1 = new QLabel("--", group); m_lblPoint1->setWordWrap(true); - point1Layout->addWidget(lblPoint1Title); - point1Layout->addWidget(m_lblPoint1, 1); - layout->addLayout(point1Layout); + layout->addWidget(m_lblPoint1); - // 点2信息 - QHBoxLayout* point2Layout = new QHBoxLayout(); + // 点1姿态输入 + QHBoxLayout* pose1Layout = new QHBoxLayout(); + pose1Layout->addWidget(new QLabel("RX:", group)); + m_editRx1 = new QLineEdit("0.0", group); + m_editRx1->setMaximumWidth(60); + pose1Layout->addWidget(m_editRx1); + pose1Layout->addWidget(new QLabel("°", group)); + + pose1Layout->addWidget(new QLabel("RY:", group)); + m_editRy1 = new QLineEdit("0.0", group); + m_editRy1->setMaximumWidth(60); + pose1Layout->addWidget(m_editRy1); + pose1Layout->addWidget(new QLabel("°", group)); + + pose1Layout->addWidget(new QLabel("RZ:", group)); + m_editRz1 = new QLineEdit("0.0", group); + m_editRz1->setMaximumWidth(60); + pose1Layout->addWidget(m_editRz1); + pose1Layout->addWidget(new QLabel("°", group)); + layout->addLayout(pose1Layout); + + m_btnShowPose1 = new QPushButton("显示点1姿态", group); + connect(m_btnShowPose1, &QPushButton::clicked, this, &CloudViewMainWindow::onShowPose1); + layout->addWidget(m_btnShowPose1); + + // 分隔线 + QFrame* line2 = new QFrame(group); + line2->setFrameShape(QFrame::HLine); + line2->setFrameShadow(QFrame::Sunken); + layout->addWidget(line2); + + // ========== 点2信息和姿态 ========== QLabel* lblPoint2Title = new QLabel("点2:", group); + lblPoint2Title->setStyleSheet("font-weight: bold;"); + layout->addWidget(lblPoint2Title); + m_lblPoint2 = new QLabel("--", group); m_lblPoint2->setWordWrap(true); - point2Layout->addWidget(lblPoint2Title); - point2Layout->addWidget(m_lblPoint2, 1); - layout->addLayout(point2Layout); + layout->addWidget(m_lblPoint2); + + // 点2姿态输入 + QHBoxLayout* pose2Layout = new QHBoxLayout(); + pose2Layout->addWidget(new QLabel("RX:", group)); + m_editRx2 = new QLineEdit("0.0", group); + m_editRx2->setMaximumWidth(60); + pose2Layout->addWidget(m_editRx2); + pose2Layout->addWidget(new QLabel("°", group)); + + pose2Layout->addWidget(new QLabel("RY:", group)); + m_editRy2 = new QLineEdit("0.0", group); + m_editRy2->setMaximumWidth(60); + pose2Layout->addWidget(m_editRy2); + pose2Layout->addWidget(new QLabel("°", group)); + + pose2Layout->addWidget(new QLabel("RZ:", group)); + m_editRz2 = new QLineEdit("0.0", group); + m_editRz2->setMaximumWidth(60); + pose2Layout->addWidget(m_editRz2); + pose2Layout->addWidget(new QLabel("°", group)); + layout->addLayout(pose2Layout); + + m_btnShowPose2 = new QPushButton("显示点2姿态", group); + connect(m_btnShowPose2, &QPushButton::clicked, this, &CloudViewMainWindow::onShowPose2); + layout->addWidget(m_btnShowPose2); + + // 分隔线 + QFrame* line3 = new QFrame(group); + line3->setFrameShape(QFrame::HLine); + line3->setFrameShadow(QFrame::Sunken); + layout->addWidget(line3); // 距离信息 QHBoxLayout* distLayout = new QHBoxLayout(); @@ -189,7 +359,7 @@ QGroupBox* CloudViewMainWindow::createMeasureGroup() QGroupBox* CloudViewMainWindow::createLineGroup() { QGroupBox* group = new QGroupBox("选线", this); - group->setMaximumWidth(300); + group->setMaximumWidth(400); QVBoxLayout* layout = new QVBoxLayout(group); // 操作说明 @@ -250,7 +420,7 @@ QGroupBox* CloudViewMainWindow::createLineGroup() QGroupBox* CloudViewMainWindow::createCloudListGroup() { QGroupBox* group = new QGroupBox("点云列表", this); - group->setMaximumWidth(300); + group->setMaximumWidth(400); QVBoxLayout* layout = new QVBoxLayout(group); m_cloudList = new QListWidget(group); @@ -317,6 +487,168 @@ void CloudViewMainWindow::onOpenFile() statusBar()->showMessage(QString("已加载 %1 个点").arg(m_converter->getLoadedPointCount())); } +void CloudViewMainWindow::onOpenSegmentFile() +{ + QString fileName = QFileDialog::getOpenFileName( + this, + "打开线段文件", + QString(), + "文本文件 (*.txt);;所有文件 (*.*)" + ); + + if (fileName.isEmpty()) { + return; + } + + statusBar()->showMessage("正在加载线段..."); + + QFile file(fileName); + if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { + QMessageBox::critical(this, "错误", QString("无法打开文件: %1").arg(fileName)); + statusBar()->showMessage("加载失败"); + return; + } + + QVector segments; + QTextStream in(&file); + int lineNum = 0; + int validCount = 0; + + while (!in.atEnd()) { + QString line = in.readLine().trimmed(); + lineNum++; + + // 跳过空行和注释 + if (line.isEmpty() || line.startsWith('#')) { + continue; + } + + // 解析格式：{x,y,z}-{x,y,z} + QRegExp regex("\\{([^}]+)\\}-\\{([^}]+)\\}"); + if (regex.indexIn(line) == -1) { + LOG_WARN("[CloudView] Line %d: Invalid format, expected {x,y,z}-{x,y,z}\n", lineNum); + continue; + } + + QString point1Str = regex.cap(1); + QString point2Str = regex.cap(2); + + QStringList p1 = point1Str.split(','); + QStringList p2 = point2Str.split(','); + + if (p1.size() != 3 || p2.size() != 3) { + LOG_WARN("[CloudView] Line %d: Invalid point format\n", lineNum); + continue; + } + + bool ok = true; + float x1 = p1[0].toFloat(&ok); if (!ok) continue; + float y1 = p1[1].toFloat(&ok); if (!ok) continue; + float z1 = p1[2].toFloat(&ok); if (!ok) continue; + float x2 = p2[0].toFloat(&ok); if (!ok) continue; + float y2 = p2[1].toFloat(&ok); if (!ok) continue; + float z2 = p2[2].toFloat(&ok); if (!ok) continue; + + // 默认白色 + segments.append(LineSegment(x1, y1, z1, x2, y2, z2, 1.0f, 1.0f, 1.0f)); + validCount++; + } + + file.close(); + + if (segments.isEmpty()) { + QMessageBox::warning(this, "警告", "文件中没有有效的线段数据"); + statusBar()->showMessage("加载失败"); + return; + } + + m_glWidget->addLineSegments(segments); + statusBar()->showMessage(QString("已加载 %1 条线段").arg(validCount)); + LOG_INFO("[CloudView] Loaded %d line segments from %s\n", validCount, fileName.toStdString().c_str()); +} + +void CloudViewMainWindow::onOpenPoseFile() +{ + QString fileName = QFileDialog::getOpenFileName( + this, + "打开姿态点文件", + QString(), + "文本文件 (*.txt);;所有文件 (*.*)" + ); + + if (fileName.isEmpty()) { + return; + } + + statusBar()->showMessage("正在加载姿态点..."); + + QFile file(fileName); + if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { + QMessageBox::critical(this, "错误", QString("无法打开文件: %1").arg(fileName)); + statusBar()->showMessage("加载失败"); + return; + } + + QVector poses; + QTextStream in(&file); + int lineNum = 0; + int validCount = 0; + + while (!in.atEnd()) { + QString line = in.readLine().trimmed(); + lineNum++; + + // 跳过空行和注释 + if (line.isEmpty() || line.startsWith('#')) { + continue; + } + + // 解析格式：{x,y,z}-{r,p,y} + QRegExp regex("\\{([^}]+)\\}-\\{([^}]+)\\}"); + if (regex.indexIn(line) == -1) { + LOG_WARN("[CloudView] Line %d: Invalid format, expected {x,y,z}-{r,p,y}\n", lineNum); + continue; + } + + QString posStr = regex.cap(1); + QString rotStr = regex.cap(2); + + QStringList pos = posStr.split(','); + QStringList rot = rotStr.split(','); + + if (pos.size() != 3 || rot.size() != 3) { + LOG_WARN("[CloudView] Line %d: Invalid point format\n", lineNum); + continue; + } + + bool ok = true; + float x = pos[0].toFloat(&ok); if (!ok) continue; + float y = pos[1].toFloat(&ok); if (!ok) continue; + float z = pos[2].toFloat(&ok); if (!ok) continue; + float roll = rot[0].toFloat(&ok); if (!ok) continue; + float pitch = rot[1].toFloat(&ok); if (!ok) continue; + float yaw = rot[2].toFloat(&ok); if (!ok) continue; + + // 固定大小为10 + float scale = 10.0f; + + poses.append(PosePoint(x, y, z, roll, pitch, yaw, scale)); + validCount++; + } + + file.close(); + + if (poses.isEmpty()) { + QMessageBox::warning(this, "警告", "文件中没有有效的姿态点数据"); + statusBar()->showMessage("加载失败"); + return; + } + + m_glWidget->addPosePoints(poses); + statusBar()->showMessage(QString("已加载 %1 个姿态点").arg(validCount)); + LOG_INFO("[CloudView] Loaded %d pose points from %s\n", validCount, fileName.toStdString().c_str()); +} + void CloudViewMainWindow::onClearAll() { m_glWidget->clearPointClouds(); @@ -335,7 +667,7 @@ void CloudViewMainWindow::onClearAll() m_lblLineIndex->setText("--"); m_lblLinePointCount->setText("--"); - statusBar()->showMessage("已清除所有点云"); + statusBar()->showMessage("已清除所有数据"); } void CloudViewMainWindow::onResetView() @@ -347,6 +679,7 @@ void CloudViewMainWindow::onResetView() void CloudViewMainWindow::onClearSelectedPoints() { m_glWidget->clearSelectedPoints(); + m_glWidget->clearPosePoints(); // 清除选点时也清除姿态 m_lblPoint1->setText("--"); m_lblPoint2->setText("--"); m_lblDistance->setText("--"); @@ -359,6 +692,9 @@ void CloudViewMainWindow::onPointSelected(const SelectedPointInfo& point) return; } + // 选择新点时清除之前的姿态显示 + m_glWidget->clearPosePoints(); + updateSelectedPointsDisplay(); // 状态栏显示：坐标、线号、索引号 @@ -705,3 +1041,205 @@ void CloudViewMainWindow::onLinePointTableClicked(int row, int column) } } +void CloudViewMainWindow::onShowPose1() +{ + auto selectedPoints = m_glWidget->getSelectedPoints(); + if (selectedPoints.isEmpty() || !selectedPoints[0].valid) { + QMessageBox::warning(this, "提示", "请先选择点1（Ctrl+左键点击点云）"); + return; + } + + const auto& point = selectedPoints[0]; + + // 读取姿态参数 + bool ok = true; + float rx = m_editRx1->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点1 RX 值无效"); + return; + } + + float ry = m_editRy1->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点1 RY 值无效"); + return; + } + + float rz = m_editRz1->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点1 RZ 值无效"); + return; + } + + // 固定大小为10 + float scale = 10.0f; + + // 清除之前的姿态点 + m_glWidget->clearPosePoints(); + + // 创建点1的姿态点 + PosePoint pose1(point.x, point.y, point.z, rx, ry, rz, scale); + QVector poses; + poses.append(pose1); + + // 如果点2也存在，添加点2的姿态 + if (selectedPoints.size() >= 2 && selectedPoints[1].valid) { + const auto& point2 = selectedPoints[1]; + float rx2 = m_editRx2->text().toFloat(&ok); + float ry2 = m_editRy2->text().toFloat(&ok); + float rz2 = m_editRz2->text().toFloat(&ok); + if (ok) { + PosePoint pose2(point2.x, point2.y, point2.z, rx2, ry2, rz2, scale); + poses.append(pose2); + } + } + + // 添加到显示 + m_glWidget->addPosePoints(poses); + + statusBar()->showMessage(QString("已显示点1姿态 (%.3f, %.3f, %.3f) 旋转(%.1f°, %.1f°, %.1f°)") + .arg(point.x).arg(point.y).arg(point.z) + .arg(rx).arg(ry).arg(rz)); + + LOG_INFO("[CloudView] Show pose1 at (%.3f, %.3f, %.3f) with rotation (%.1f, %.1f, %.1f)\n", + point.x, point.y, point.z, rx, ry, rz); +} + +void CloudViewMainWindow::onShowPose2() +{ + auto selectedPoints = m_glWidget->getSelectedPoints(); + if (selectedPoints.size() < 2 || !selectedPoints[1].valid) { + QMessageBox::warning(this, "提示", "请先选择点2（启用测距后，Ctrl+左键点击第二个点）"); + return; + } + + const auto& point = selectedPoints[1]; + + // 读取姿态参数 + bool ok = true; + float rx = m_editRx2->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点2 RX 值无效"); + return; + } + + float ry = m_editRy2->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点2 RY 值无效"); + return; + } + + float rz = m_editRz2->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点2 RZ 值无效"); + return; + } + + // 固定大小为10 + float scale = 10.0f; + + // 清除之前的姿态点 + m_glWidget->clearPosePoints(); + + // 创建点2的姿态点 + PosePoint pose2(point.x, point.y, point.z, rx, ry, rz, scale); + QVector poses; + + // 如果点1也存在，添加点1的姿态 + if (selectedPoints[0].valid) { + const auto& point1 = selectedPoints[0]; + float rx1 = m_editRx1->text().toFloat(&ok); + float ry1 = m_editRy1->text().toFloat(&ok); + float rz1 = m_editRz1->text().toFloat(&ok); + if (ok) { + PosePoint pose1(point1.x, point1.y, point1.z, rx1, ry1, rz1, scale); + poses.append(pose1); + } + } + + poses.append(pose2); + + // 添加到显示 + m_glWidget->addPosePoints(poses); + + statusBar()->showMessage(QString("已显示点2姿态 (%.3f, %.3f, %.3f) 旋转(%.1f°, %.1f°, %.1f°)") + .arg(point.x).arg(point.y).arg(point.z) + .arg(rx).arg(ry).arg(rz)); + + LOG_INFO("[CloudView] Show pose2 at (%.3f, %.3f, %.3f) with rotation (%.1f, %.1f, %.1f)\n", + point.x, point.y, point.z, rx, ry, rz); +} + +void CloudViewMainWindow::onShowInputLine() +{ + // 读取点1坐标 + bool ok = true; + float x1 = m_editLineX1->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点1 X 值无效"); + return; + } + + float y1 = m_editLineY1->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点1 Y 值无效"); + return; + } + + float z1 = m_editLineZ1->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点1 Z 值无效"); + return; + } + + // 读取点2坐标 + float x2 = m_editLineX2->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点2 X 值无效"); + return; + } + + float y2 = m_editLineY2->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点2 Y 值无效"); + return; + } + + float z2 = m_editLineZ2->text().toFloat(&ok); + if (!ok) { + QMessageBox::warning(this, "错误", "点2 Z 值无效"); + return; + } + + // 清除之前的线段 + m_glWidget->clearLineSegments(); + + // 创建线段（红色） + LineSegment segment(x1, y1, z1, x2, y2, z2, 1.0f, 0.0f, 0.0f); + QVector segments; + segments.append(segment); + + // 添加到显示 + m_glWidget->addLineSegments(segments); + + // 计算距离 + float dx = x2 - x1; + float dy = y2 - y1; + float dz = z2 - z1; + float distance = std::sqrt(dx * dx + dy * dy + dz * dz); + + statusBar()->showMessage(QString("已显示线段 (%1,%2,%3) → (%4,%5,%6) 长度: %7") + .arg(x1).arg(y1).arg(z1) + .arg(x2).arg(y2).arg(z2) + .arg(distance)); + + LOG_INFO("[CloudView] Show input line from (%.3f, %.3f, %.3f) to (%.3f, %.3f, %.3f) length=%.3f\n", + x1, y1, z1, x2, y2, z2, distance); +} + +void CloudViewMainWindow::onClearInputLine() +{ + m_glWidget->clearLineSegments(); + statusBar()->showMessage("已清除输入的线段"); +} + diff --git a/Tools/CloudView/Src/PointCloudGLWidget.cpp b/Tools/CloudView/Src/PointCloudGLWidget.cpp index 96dc89f..eb3ced7 100644 --- a/Tools/CloudView/Src/PointCloudGLWidget.cpp +++ b/Tools/CloudView/Src/PointCloudGLWidget.cpp @@ -116,6 +116,8 @@ void PointCloudGLWidget::paintGL() drawSelectedPoints(); drawMeasurementLine(); drawSelectedLine(); + drawLineSegments(); + drawPosePoints(); // 最后绘制坐标系指示器（覆盖在所有内容之上） drawAxis(); @@ -248,6 +250,8 @@ void PointCloudGLWidget::clearPointClouds() m_pointClouds.clear(); m_selectedPoints.clear(); m_selectedLine = SelectedLineInfo(); + m_lineSegments.clear(); + m_posePoints.clear(); m_minBound = QVector3D(-50, -50, -50); m_maxBound = QVector3D(50, 50, 50); m_center = QVector3D(0, 0, 0); @@ -978,3 +982,91 @@ void PointCloudGLWidget::replaceFirstCloud(const PointCloudXYZ& cloud, const QSt resetView(); update(); } + +void PointCloudGLWidget::addLineSegments(const QVector& segments) +{ + m_lineSegments.append(segments); + update(); +} + +void PointCloudGLWidget::clearLineSegments() +{ + m_lineSegments.clear(); + update(); +} + +void PointCloudGLWidget::addPosePoints(const QVector& poses) +{ + m_posePoints.append(poses); + update(); +} + +void PointCloudGLWidget::clearPosePoints() +{ + m_posePoints.clear(); + update(); +} + +void PointCloudGLWidget::drawLineSegments() +{ + if (m_lineSegments.isEmpty()) { + return; + } + + glLineWidth(2.0f); + glBegin(GL_LINES); + + for (const auto& seg : m_lineSegments) { + glColor3f(seg.r, seg.g, seg.b); + glVertex3f(seg.x1, seg.y1, seg.z1); + glVertex3f(seg.x2, seg.y2, seg.z2); + } + + glEnd(); + glLineWidth(1.0f); +} + +void PointCloudGLWidget::drawPosePoints() +{ + if (m_posePoints.isEmpty()) { + return; + } + + glLineWidth(2.0f); + + for (const auto& pose : m_posePoints) { + glPushMatrix(); + + // 移动到姿态点位置 + glTranslatef(pose.x, pose.y, pose.z); + + // 应用欧拉角旋转（ZYX顺序） + glRotatef(pose.rz, 0, 0, 1); // Z轴旋转 + glRotatef(pose.ry, 0, 1, 0); // Y轴旋转 + glRotatef(pose.rx, 1, 0, 0); // X轴旋转 + + // 绘制坐标系 + glBegin(GL_LINES); + + // X轴 - 红色 + glColor3f(1.0f, 0.0f, 0.0f); + glVertex3f(0.0f, 0.0f, 0.0f); + glVertex3f(pose.scale, 0.0f, 0.0f); + + // Y轴 - 绿色 + glColor3f(0.0f, 1.0f, 0.0f); + glVertex3f(0.0f, 0.0f, 0.0f); + glVertex3f(0.0f, pose.scale, 0.0f); + + // Z轴 - 蓝色 + glColor3f(0.0f, 0.0f, 1.0f); + glVertex3f(0.0f, 0.0f, 0.0f); + glVertex3f(0.0f, 0.0f, pose.scale); + + glEnd(); + + glPopMatrix(); + } + + glLineWidth(1.0f); +} diff --git a/Tools/CloudView/poses.txt b/Tools/CloudView/poses.txt new file mode 100644 index 0000000..794cb64 --- /dev/null +++ b/Tools/CloudView/poses.txt @@ -0,0 +1,17 @@ +# 姿态点示例 +# 格式：{x,y,z}-{r,p,y} + +# 原点 +{0,0,0}-{0,0,0} + +# X轴上 +{100,0,0}-{0,0,45} + +# Y轴上 +{0,100,0}-{0,0,90} + +# Z轴上 +{0,0,100}-{0,45,0} + +# 对角线 +{50,50,50}-{30,30,30} diff --git a/Tools/CloudView/segments.txt b/Tools/CloudView/segments.txt new file mode 100644 index 0000000..abbba89 --- /dev/null +++ b/Tools/CloudView/segments.txt @@ -0,0 +1,23 @@ +# 线段示例 +# 格式：{x,y,z}-{x,y,z} + +# 坐标轴 +{0,0,0}-{100,0,0} +{0,0,0}-{0,100,0} +{0,0,0}-{0,0,100} + +# 立方体 +{50,50,50}-{150,50,50} +{150,50,50}-{150,150,50} +{150,150,50}-{50,150,50} +{50,150,50}-{50,50,50} + +{50,50,150}-{150,50,150} +{150,50,150}-{150,150,150} +{150,150,150}-{50,150,150} +{50,150,150}-{50,50,150} + +{50,50,50}-{50,50,150} +{150,50,50}-{150,50,150} +{150,150,50}-{150,150,150} +{50,150,50}-{50,150,150}