diff --git a/App/GrabBag/Doc/视觉方案.docx b/App/GrabBag/Doc/视觉方案.docx index fcc445d..4a686f7 100644 --- a/App/GrabBag/Doc/视觉方案.docx +++ b/App/GrabBag/Doc/视觉方案.docx @@ -1,15 +1,15 @@ 一、设备布局缩略图: 二、工艺流程: -1、启动机械臂 -2、机械臂运行至拍照位启动拍照 -3、相机拍照,给出位置信号(位置、角度、高度、有无料);若无料至第8步 -4、机械臂抓料 -5、机械臂破袋下料 -6、机械臂扔袋子、启动拍照;若无料至第8步 -7、到第3步 -8、机械臂等待新的物料到位 -9、到第2步 + 1、启动机械臂 + 2、机械臂运行至拍照位启动拍照 + 3、相机拍照,给出位置信号(位置、角度、高度、有无料);若无料至第8步 + 4、机械臂抓料 + 5、机械臂破袋下料 + 6、机械臂扔袋子、启动拍照;若无料至第8步 + 7、到第3步 + 8、机械臂等待新的物料到位 + 9、到第2步 三、功能要求: 1、一台触控一体机连两台视觉。 2、袋子有9种,实际可制作的视觉模版要大于9,通过机械臂发送调用几号视觉模版和视觉拍照启动信号,视觉反馈给机械臂位置信号。 diff --git a/App/GrabBag/GrabBagApp/GrabBagApp.pro b/App/GrabBag/GrabBagApp/GrabBagApp.pro index 5e2e5e5..3459b97 100644 --- a/App/GrabBag/GrabBagApp/GrabBagApp.pro +++ b/App/GrabBag/GrabBagApp/GrabBagApp.pro @@ -6,11 +6,22 @@ greaterThan(QT_MAJOR_VERSION, 4): QT += widgets TEMPLATE = app CONFIG += c++17 -# Add /utf-8 flag only for MSVC builds to enforce UTF-8 encoding -win32-msvc { - QMAKE_CXXFLAGS += /utf-8 +# 为MinGW编译器添加更多UTF-8支持 +win32-g++ { + QMAKE_CXXFLAGS += -fexec-charset=UTF-8 -finput-charset=UTF-8 } +# 为MSVC编译器添加更多UTF-8支持 +win32-msvc { + QMAKE_CXXFLAGS += /utf-8 + QMAKE_CFLAGS += /utf-8 +} + +# Windows平台特定设置 +win32 { + # 确保链接Windows控制台API + LIBS += -lkernel32 +} # You can make your code fail to compile if it uses deprecated APIs. # In order to do so, uncomment the following line. @@ -179,3 +190,6 @@ unix { qnx: target.path = /tmp/$${TARGET}/bin else: unix:!android: target.path = /opt/$${TARGET}/bin !isEmpty(target.path): INSTALLS += target + +DISTFILES += \ + Version.md diff --git a/App/GrabBag/GrabBagApp/Presenter/Inc/GrabBagPresenter.h b/App/GrabBag/GrabBagApp/Presenter/Inc/GrabBagPresenter.h index 7e8418d..f5eae0a 100644 --- a/App/GrabBag/GrabBagApp/Presenter/Inc/GrabBagPresenter.h +++ b/App/GrabBag/GrabBagApp/Presenter/Inc/GrabBagPresenter.h @@ -33,6 +33,7 @@ struct CurrentExecutionParams VrCameraParams cameraParam; // 相机参数 SSG_planeCalibPara planeCalibParam; // 调平参数 CalibMatrix handEyeCalibMatrix; // 手眼标定参数 + VrBagParam bagParam; // 包裹参数(尺寸) }; class GrabBagPresenter : public QObject, public IVrConfigChangeNotify, public IConfigChangeListener @@ -50,7 +51,7 @@ public: void SetStatusCallback(IYGrabBagStatus* status); // 开始检测 - int StartDetection(int cameraIndex = -1, bool isAuto = true); // cameraIndex: -1表示所有相机,1/2...表示特定相机 + int StartDetection(int cameraIndex = -1, bool isAuto = true, bool updateParams = true); // cameraIndex: -1表示所有相机,1/2...表示特定相机; updateParams: 是否更新执行参数 // 停止检测 int StopDetection(); @@ -124,6 +125,12 @@ private: // TCP客户端连接状态回调处理方法 void onTcpClientEventFromCallback(const TCPClient* pClient, TCPServerEventType eventType); + + // TCP协议解析和处理函数 + bool _ParseNewFormatProtocol(const QString& qData, int& cameraIndex, int& templateIndex, QString& triggerCmd); + bool _ParseOldFormatProtocol(const QString& qData, int& cameraIndex, int& templateIndex, QString& triggerCmd); + bool _ParseSimpleProtocol(const QString& qData, int& cameraIndex, int& templateIndex, QString& triggerCmd); + bool _HandleTrigCommand(const TCPClient* pClient, int cameraIndex, int templateIndex); // 算法初始化接口 int InitAlgorithmParams(); @@ -138,6 +145,9 @@ private: // 连接状态检查和更新 void CheckAndUpdateWorkStatus(); + + // 设置工作状态(同时通知UI) + void SetWorkStatus(WorkStatus status); // 打开相机 int _OpenDevice(int cameraIndex, const char* cameraName, const char* cameraIp, ProjectType& projectType); @@ -169,6 +179,7 @@ private: void _LoadCurrentWorkPositionAndPackageType(); int _ApplyCameraParameters(); void _UpdateCurrentExecutionParams(); + void _UpdateCurrentExecutionParams(const WorkPositionConfig* workPosition, const CameraConfig* camera, const PackageTypeConfig* package, int cameraIndex); // 相机重连定时器相关方法 void _StartCameraReconnectTimer(); @@ -224,12 +235,7 @@ private: std::vector m_cameraConfigList; // 保存配置的相机列表 int m_expectedCameraCount = 0; // 期望的相机数量 - // 工作点、相机和包裹类型管理(三级结构:工作点->相机->包裹) - std::string m_currentWorkPositionId; // 当前工作点ID - std::string m_currentCameraId; // 当前相机ID - std::string m_currentPackageTypeId; // 当前包裹类型ID - int m_currentWorkPositionIndex; // 当前工作点索引(-1表示使用全局默认参数) - int m_currentPackageTypeIndex; // 当前包裹类型索引(-1表示使用全局默认参数) + // 当前执行参数(包含工作点、相机、包裹的完整配置信息) CurrentExecutionParams m_currentExecutionParams; // 当前执行参数 }; diff --git a/App/GrabBag/GrabBagApp/Presenter/Src/GrabBagPresenter.cpp b/App/GrabBag/GrabBagApp/Presenter/Src/GrabBagPresenter.cpp index d375e1a..1b0f862 100644 --- a/App/GrabBag/GrabBagApp/Presenter/Src/GrabBagPresenter.cpp +++ b/App/GrabBag/GrabBagApp/Presenter/Src/GrabBagPresenter.cpp @@ -36,8 +36,6 @@ GrabBagPresenter::GrabBagPresenter(QObject *parent) , m_bCameraConnected(false) , m_bRobotConnected(false) , m_currentWorkStatus(WorkStatus::Error) - , m_currentWorkPositionIndex(-1) - , m_currentPackageTypeIndex(-1) { m_detectionDataCache.clear(); @@ -291,7 +289,7 @@ int GrabBagPresenter::InitCamera(std::vector& cameraList) m_pStatus->OnStatusUpdate("启动重新连接相机..."); _StartCameraReconnectTimer(); } else if (allCamerasConnected) { - LOG_INFO("All cameras connected successfully\\n"); + LOG_INFO("All cameras connected successfully \n"); m_pStatus->OnStatusUpdate("所有相机连接成功"); // 确保定时器停止 _StopCameraReconnectTimer(); @@ -457,225 +455,241 @@ void GrabBagPresenter::onTcpDataReceivedFromCallback(const TCPClient* pClient, c LOG_INFO("TCP data received from client %p: %s\n", pClient, qData.toStdString().c_str()); - // 生成客户端ID用于标识 - QString clientId = QString("Client_%1").arg(reinterpret_cast(pClient)); - if (m_pStatus) { m_pStatus->OnStatusUpdate("收到TCP数据: " + qData.toStdString()); } - // 解析命令格式:@,视觉号,视觉模版号,启动信息,$ - // 示例:@,1,3,Trig,$ 表示触发1号视觉(相机)拍照,调用3号视觉模版,发出Trig拍照命令 - // 兼容旧格式:@3,Trig$ 或 Trig - int cameraIndex = -1; // 相机编号(视觉号) int templateIndex = -1; // 视觉模板编号 QString triggerCmd; // 启动信息(Trig等) - // 方法1:尝试解析新格式 @,视觉号,视觉模版号,启动信息,$ - // 正则表达式:@[,,]\s*(-?\d+)\s*[,,]\s*(-?\d+)\s*[,,]\s*(\w+) - // 支持负数(-1表示使用默认相机/模板) - QRegularExpression newFormatRegex("@[,,]\\s*(-?\\d+)\\s*[,,]\\s*(-?\\d+)\\s*[,,]\\s*(\\w+)"); - QRegularExpressionMatch newFormatMatch = newFormatRegex.match(qData); + // 尝试按优先级解析协议:新格式 -> 旧格式 -> 简单格式 + bool parsed = _ParseNewFormatProtocol(qData, cameraIndex, templateIndex, triggerCmd); + if (!parsed) { + parsed = _ParseOldFormatProtocol(qData, cameraIndex, templateIndex, triggerCmd); + } + if (!parsed) { + parsed = _ParseSimpleProtocol(qData, cameraIndex, templateIndex, triggerCmd); + } - if (newFormatMatch.hasMatch()) { - // 新格式解析成功 - cameraIndex = newFormatMatch.captured(1).toInt(); - templateIndex = newFormatMatch.captured(2).toInt(); - triggerCmd = newFormatMatch.captured(3); - - LOG_INFO("Parsed new format command: camera=%d, template=%d, trigger=%s\n", cameraIndex, templateIndex, triggerCmd.toStdString().c_str()); - - // 如果相机索引为-1,使用当前默认相机 - if (cameraIndex == -1) { - cameraIndex = m_currentCameraIndex; - LOG_INFO("Camera index is -1, using current default camera: %d\n", cameraIndex); - } - - // 如果模板索引为-1或0,表示使用当前模板(不切换) - if (templateIndex == -1 || templateIndex == 0) { - LOG_INFO("Template index is %d, will use current template (no switch)\n", templateIndex); - templateIndex = -1; // 标准化为-1表示不切换 - } - } else { - // 方法2:尝试解析旧格式 @模板编号,Trig$ 或 @模板编号Trig - QRegularExpression oldFormatRegex("@(\\d+).*Trig"); - QRegularExpressionMatch oldFormatMatch = oldFormatRegex.match(qData); - - if (oldFormatMatch.hasMatch()) { - // 旧格式解析成功(只有模板编号) - templateIndex = oldFormatMatch.captured(1).toInt(); - cameraIndex = m_currentCameraIndex; // 使用当前默认相机 - triggerCmd = "Trig"; - - LOG_INFO("Parsed old format command: template=%d, using current camera=%d\n", templateIndex, cameraIndex); - } else if (qData.contains("Trig", Qt::CaseInsensitive)) { - // 方法3:简单的Trig命令(不切换模板和相机) - cameraIndex = m_currentCameraIndex; - templateIndex = -1; // 不切换模板 - triggerCmd = "Trig"; - - LOG_INFO("Parsed simple Trig command, using current camera=%d and template\n", cameraIndex); - } else { - // 未识别的命令 - LOG_WARNING("Unknown TCP command received: %s\n", qData.toStdString().c_str()); - if (m_pStatus) { - m_pStatus->OnStatusUpdate("未知TCP命令: " + qData.toStdString()); - } - - // 发送错误响应 - std::string err = "Unknown_Command"; - m_pTcpServer->SendData(pClient, err.c_str(), err.length()); - return; + // 如果所有格式都无法解析,返回错误 + if (!parsed) { + LOG_WARNING("Unknown TCP command received: %s\n", qData.toStdString().c_str()); + if (m_pStatus) { + m_pStatus->OnStatusUpdate("未知TCP命令: " + qData.toStdString()); } + std::string err = "Unknown_Command"; + m_pTcpServer->SendData(pClient, err.c_str(), err.length()); + return; } // 处理Trig命令 if (triggerCmd.contains("Trig", Qt::CaseInsensitive)) { - // 1. 验证相机编号有效性 - if (cameraIndex < 1 || cameraIndex > static_cast(m_vrEyeDeviceList.size())) { - LOG_ERROR("Invalid camera index: %d, valid range: 1-%zu\n", cameraIndex, m_vrEyeDeviceList.size()); - - if (m_pStatus) { - m_pStatus->OnStatusUpdate(QString("无效的相机编号: %1").arg(cameraIndex).toStdString()); - } - - // 发送错误响应 - QString response = QString("Error_Invalid_Camera_%1").arg(cameraIndex); - m_pTcpServer->SendData(pClient, response.toStdString().c_str(), response.toStdString().length()); - return; + bool bRet = _HandleTrigCommand(pClient, cameraIndex, templateIndex); + if(!bRet){ + SetWorkStatus(WorkStatus::Error); } - - // 检查相机是否已连接 - if (m_vrEyeDeviceList[cameraIndex - 1].second == nullptr) { - LOG_ERROR("Camera %d is not connected\n", cameraIndex); - - if (m_pStatus) { - QString cameraName = QString::fromStdString(m_vrEyeDeviceList[cameraIndex - 1].first); - m_pStatus->OnStatusUpdate(QString("相机%1未连接").arg(cameraName).toStdString()); - } - - // 发送错误响应 - QString response = QString("Error_Camera_%1_Not_Connected").arg(cameraIndex); - m_pTcpServer->SendData(pClient, response.toStdString().c_str(), response.toStdString().length()); - return; - } - - // 2. 如果指定了模板编号,进行模板切换 - if (templateIndex > 0) { - // 在新的三级结构中,需要找到指定相机下的包裹 - // 首先获取当前工作点 - WorkPositionConfig* currentWorkPosition = nullptr; - for (auto& wp : m_configResult.workPositions) { - if (wp.id == m_currentWorkPositionId) { - currentWorkPosition = ℘ - break; - } - } - - if (!currentWorkPosition) { - LOG_ERROR("No current work position found (ID: %s), cannot switch template\n", - m_currentWorkPositionId.c_str()); - QString response = QString("Error_No_Work_Position"); - m_pTcpServer->SendData(pClient, response.toStdString().c_str(), response.toStdString().length()); - return; - } - - // 从工作点中找到对应cameraIndex的相机配置 - CameraConfig* targetCamera = currentWorkPosition->GetCameraByIndex(cameraIndex); - if (!targetCamera) { - LOG_ERROR("No camera config found for camera index %d in work position %s\n", - cameraIndex, m_currentWorkPositionId.c_str()); - QString response = QString("Error_No_Camera_Config_%1").arg(cameraIndex); - m_pTcpServer->SendData(pClient, response.toStdString().c_str(), response.toStdString().length()); - return; - } - - // 验证模板编号的有效性(1-based索引,对应相机下的包裹列表) - if (templateIndex < 1 || templateIndex > static_cast(targetCamera->packages.size())) { - LOG_ERROR("Invalid template index: %d for camera %d, valid range: 1-%zu\n", - templateIndex, cameraIndex, targetCamera->packages.size()); - - if (m_pStatus) { - m_pStatus->OnStatusUpdate(QString("相机%1的无效模板编号: %2").arg(cameraIndex).arg(templateIndex).toStdString()); - } - - // 发送错误��应 - QString response = QString("Error_Invalid_Template_%1_For_Camera_%2").arg(templateIndex).arg(cameraIndex); - m_pTcpServer->SendData(pClient, response.toStdString().c_str(), response.toStdString().length()); - return; - } - - // 切换到指定的包裹类型(视觉模板) - const PackageTypeConfig& selectedPackage = targetCamera->packages[templateIndex - 1]; - m_currentCameraId = targetCamera->id; - m_currentPackageTypeId = selectedPackage.id; - std::string templateName = selectedPackage.name; - - LOG_INFO("Switched to template %d for camera %d: %s (Camera ID: %s, Package ID: %s)\n", - templateIndex, cameraIndex, templateName.c_str(), - m_currentCameraId.c_str(), m_currentPackageTypeId.c_str()); - - if (m_pStatus) { - m_pStatus->OnStatusUpdate(QString("切换到相机%1的视觉模板%2: %3") - .arg(cameraIndex) - .arg(templateIndex) - .arg(QString::fromStdString(templateName)).toStdString()); - } - - // 更新执行参数(使用新的模板配置) - // 注意:这里需要先设置相机索引,以便_UpdateCurrentExecutionParams使用正确的相机 - int savedCameraIndex = m_currentCameraIndex; - m_currentCameraIndex = cameraIndex; - _UpdateCurrentExecutionParams(); - m_currentCameraIndex = savedCameraIndex; // 恢复原来的默认相机 - } - - // 3. 显示状态信息 - QString cameraName = (cameraIndex >= 1 && cameraIndex <= static_cast(m_vrEyeDeviceList.size())) ? - QString::fromStdString(m_vrEyeDeviceList[cameraIndex - 1].first) : - QString("相机%1").arg(cameraIndex); - - if (m_pStatus) { - if (templateIndex > 0) { - m_pStatus->OnStatusUpdate(QString("收到触发命令[相机%1,模板%2],启动%3检测") - .arg(cameraIndex).arg(templateIndex).arg(cameraName).toStdString()); - } else { - m_pStatus->OnStatusUpdate(QString("收到触发命令,启动%1检测") - .arg(cameraName).toStdString()); - } - } - - LOG_INFO("Starting detection with camera %d (template %d)\n", cameraIndex, templateIndex); - - // 4. 启动指定相机的检测(非自动模式) - int result = StartDetection(cameraIndex, false); - - // 5. 发送响应给客户端 - QString response; - if (result != SUCCESS) { - response = QString("Error_%1").arg(result); - LOG_ERROR("Failed to start camera %d detection, error: %d\n", cameraIndex, result); - } else { - response = QString("OK_Camera_%1_Template_%2").arg(cameraIndex).arg(templateIndex); - LOG_INFO("Detection started successfully: camera %d, template %d\n", cameraIndex, templateIndex); - } - - // 向客户端发送响应 - m_pTcpServer->SendData(pClient, response.toStdString().c_str(), response.toStdString().length()); } else { // 其他启动命令(未来扩展) LOG_WARNING("Unknown trigger command: %s\n", triggerCmd.toStdString().c_str()); if (m_pStatus) { m_pStatus->OnStatusUpdate("未知触发命令: " + triggerCmd.toStdString()); } - - // 发送错误响应 std::string err = "Unknown_Trigger_Command"; m_pTcpServer->SendData(pClient, err.c_str(), err.length()); } } +// 解析新格式协议:@,视觉号,视觉模版号,启动信息,$ +// 示例:@,1,3,Trig,$ 表示触发1号视觉(相机)拍照,调用3号视觉模版,发出Trig拍照命令 +bool GrabBagPresenter::_ParseNewFormatProtocol(const QString& qData, int& cameraIndex, int& templateIndex, QString& triggerCmd) +{ + // 正则表达式:@[,,]\s*(-?\d+)\s*[,,]\s*(-?\d+)\s*[,,]\s*(\w+) + // 支持负数(-1表示使用默认相机/模板) + QRegularExpression newFormatRegex("@[,,]\\s*(-?\\d+)\\s*[,,]\\s*(-?\\d+)\\s*[,,]\\s*(\\w+)"); + QRegularExpressionMatch newFormatMatch = newFormatRegex.match(qData); + + if (!newFormatMatch.hasMatch()) { + return false; + } + + // 新格式解析成功(相机编号、模板编号、触发命令) + cameraIndex = newFormatMatch.captured(1).toInt(); + templateIndex = newFormatMatch.captured(2).toInt(); + triggerCmd = newFormatMatch.captured(3); + + LOG_INFO("Parsed new format command: camera=%d, template=%d, trigger=%s\n", cameraIndex, templateIndex, triggerCmd.toStdString().c_str()); + + // 如果相机索引为-1,使用当前默认相机 + if (cameraIndex == -1) { + cameraIndex = m_currentCameraIndex; + LOG_INFO("Camera index is -1, using current default camera: %d\n", cameraIndex); + } + + // 如果模板索引为-1或0,表示使用当前模板(不切换) + if (templateIndex == -1 || templateIndex == 0) { + LOG_INFO("Template index is %d, will use current template (no switch)\n", templateIndex); + templateIndex = -1; // 标准化为-1表示不切换 + } + + return true; +} + +// 解析旧格式协议:@模板编号,Trig$ 或 @模板编号Trig +// 示例:@3,Trig$ 表示调用3号视觉模版 +bool GrabBagPresenter::_ParseOldFormatProtocol(const QString& qData, int& cameraIndex, int& templateIndex, QString& triggerCmd) +{ + QRegularExpression oldFormatRegex("@(\\d+).*Trig"); + QRegularExpressionMatch oldFormatMatch = oldFormatRegex.match(qData); + + if (!oldFormatMatch.hasMatch()) { + return false; + } + + // 旧格式解析成功(只有模板编号) + templateIndex = oldFormatMatch.captured(1).toInt(); + cameraIndex = m_currentCameraIndex; // 使用当前默认相机 + triggerCmd = "Trig"; + + LOG_INFO("Parsed old format command: template=%d, using current camera=%d\n", templateIndex, cameraIndex); + return true; +} + +// 解析简单协议:Trig +// 示例:Trig 表示使用当前相机和当前模板触发检测 +bool GrabBagPresenter::_ParseSimpleProtocol(const QString& qData, int& cameraIndex, int& templateIndex, QString& triggerCmd) +{ + if (!qData.contains("Trig", Qt::CaseInsensitive)) { + return false; + } + + // 简单的Trig命令(不切换模板和相机) + cameraIndex = m_currentCameraIndex; + templateIndex = -1; // 不切换模板 + triggerCmd = "Trig"; + + LOG_INFO("Parsed simple Trig command, using current camera=%d and template\n", cameraIndex); + return true; +} + +// 处理Trig命令 +bool GrabBagPresenter::_HandleTrigCommand(const TCPClient* pClient, int cameraIndex, int templateIndex) +{ + SetWorkStatus(WorkStatus::Working); + // 1. 验证相机编号有效性 + if (cameraIndex < 1 || cameraIndex > static_cast(m_vrEyeDeviceList.size())) { + LOG_ERROR("Invalid camera index: %d, valid range: 1-%zu\n", cameraIndex, m_vrEyeDeviceList.size()); + + if (m_pStatus) { + m_pStatus->OnStatusUpdate(QString("无效的相机编号: %1").arg(cameraIndex).toStdString()); + } + + // 发送错误响应 + QString response = QString("Error_Invalid_Camera_%1").arg(cameraIndex); + m_pTcpServer->SendData(pClient, response.toStdString().c_str(), response.toStdString().length()); + return false; + } + + // 检查相机是否已连接 + if (m_vrEyeDeviceList[cameraIndex - 1].second == nullptr) { + LOG_ERROR("Camera %d is not connected\n", cameraIndex); + + if (m_pStatus) { + QString cameraName = QString::fromStdString(m_vrEyeDeviceList[cameraIndex - 1].first); + m_pStatus->OnStatusUpdate(QString("相机%1未连接").arg(cameraName).toStdString()); + } + + // 发送错误响应 + QString response = QString("Error_Camera_%1_Not_Connected").arg(cameraIndex); + m_pTcpServer->SendData(pClient, response.toStdString().c_str(), response.toStdString().length()); + return false; + } + + // 2. 如果指定了模板编号,进行模板切换 + if (templateIndex > 0) { + // 通过包裹ID(全局唯一)在配置文件中查询包裹、相机和工位信息 + std::string packageId = "pkg_" + std::to_string(templateIndex); + + WorkPositionConfig* foundWorkPosition = nullptr; + CameraConfig* foundCamera = nullptr; + PackageTypeConfig* foundPackage = nullptr; + + // 遍历所有工位、相机、包裹,查找匹配的包裹ID + for (auto& wp : m_configResult.workPositions) { + for (auto& cam : wp.cameras) { + for (auto& pkg : cam.packages) { + if (pkg.id == packageId) { + foundWorkPosition = ℘ + foundCamera = &cam; + foundPackage = &pkg; + break; + } + } + if (foundPackage) break; + } + if (foundPackage) break; + } + + // 检查是否找到包裹 + if (!foundPackage || !foundCamera || !foundWorkPosition) { + LOG_ERROR("Package not found with ID: %s (template index: %d)\n", packageId.c_str(), templateIndex); + + if (m_pStatus) { + m_pStatus->OnStatusUpdate(QString("未找到包裹ID: %1 (模板编号: %2)") + .arg(QString::fromStdString(packageId)) + .arg(templateIndex).toStdString()); + } + + QString response = QString("Error_Package_Not_Found_%1").arg(templateIndex); + m_pTcpServer->SendData(pClient, response.toStdString().c_str(), response.toStdString().length()); + return false; + } + + // 验证找到的相机索引是否与请求的相机索引匹配 + if (foundCamera->cameraIndex != cameraIndex) { + LOG_WARNING("Package %s belongs to camera %d, but requested camera is %d. Using package's camera.\n", + packageId.c_str(), foundCamera->cameraIndex, cameraIndex); + // 更新相机索引为包裹所属的相机 + cameraIndex = foundCamera->cameraIndex; + } + + // 记录找到的配置信息 + std::string templateName = foundPackage->name; + + LOG_INFO("Switched to package by ID: %s (template index: %d)\n", packageId.c_str(), templateIndex); + LOG_INFO(" Work Position: %s (ID: %s)\n", foundWorkPosition->name.c_str(), foundWorkPosition->id.c_str()); + LOG_INFO(" Camera: %s (ID: %s, Index: %d)\n", foundCamera->cameraName.c_str(), foundCamera->id.c_str(), foundCamera->cameraIndex); + LOG_INFO(" Package: %s (ID: %s)\n", templateName.c_str(), foundPackage->id.c_str()); + + if (m_pStatus) { + m_pStatus->OnStatusUpdate(QString("切换到[工位:%1, 相机:%2, 包裹:%3]") + .arg(QString::fromStdString(foundWorkPosition->name)) + .arg(QString::fromStdString(foundCamera->cameraName)) + .arg(QString::fromStdString(foundPackage->name)).toStdString()); + } + + // 更新执行参数(使用新的模板配置) + _UpdateCurrentExecutionParams(foundWorkPosition, foundCamera, foundPackage, cameraIndex); + } + + // 3. 显示状态信息 + LOG_INFO("Starting detection with camera %d (template %d)\n", cameraIndex, templateIndex); + + // 4. 启动指定相机的检测(非自动模式) + // 如果已经切换了模板(templateIndex > 0),则不需要再更新参数 + // 如果没有切换模板,则需要在 StartDetection 中更新参数 + bool needUpdateParams = (templateIndex <= 0); + int result = StartDetection(cameraIndex, false, needUpdateParams); + + // 5. 发送响应给客户端 + if (result != SUCCESS) { + QString response = QString("Error_%1").arg(result); + LOG_ERROR("Failed to start camera %d detection, error: %d\n", cameraIndex, result); + m_pTcpServer->SendData(pClient, response.toStdString().c_str(), response.toStdString().length()); + } + return result == SUCCESS; +} + // TCP客户端连接状态回调处理方法 void GrabBagPresenter::onTcpClientEventFromCallback(const TCPClient* pClient, TCPServerEventType eventType) { @@ -812,11 +826,11 @@ void GrabBagPresenter::SetStatusCallback(IYGrabBagStatus* status) } // 模拟检测函数,用于演示 -int GrabBagPresenter::StartDetection(int cameraIdx, bool isAuto) -{ +int GrabBagPresenter::StartDetection(int cameraIdx, bool isAuto, bool updateParams) +{ LOG_INFO("--------------------------------\n"); - LOG_INFO("Start detection with camera index: %d\n", cameraIdx); - + LOG_INFO("Start detection with camera index: %d, updateParams: %d\n", cameraIdx, updateParams); + // 检查设备状态是否准备就绪 if (isAuto && m_currentWorkStatus != WorkStatus::Ready) { LOG_INFO("Device not ready, cannot start detection\n"); @@ -826,19 +840,17 @@ int GrabBagPresenter::StartDetection(int cameraIdx, bool isAuto) return ERR_CODE(DEV_BUSY); } - // 保存当前使用的相机ID(从1开始编号) - if(-1 != cameraIdx){ - m_currentCameraIndex = cameraIdx; - } - int cameraIndex = m_currentCameraIndex; + // 确定要使用的相机索引(不修改默认相机索引) + int cameraIndex = (cameraIdx != -1) ? cameraIdx : m_currentCameraIndex; - m_currentWorkStatus = WorkStatus::Working; + LOG_INFO("Using camera index: %d (default: %d)\n", cameraIndex, m_currentCameraIndex); + + m_currentWorkStatus = WorkStatus::Detecting; - // 通知UI工作状态变更为"正在工作" if (m_pStatus) { - m_pStatus->OnWorkStatusChanged(WorkStatus::Working); + m_pStatus->OnWorkStatusChanged(m_currentWorkStatus); } - + // 设置机械臂工作状态为忙碌 if (m_pRobotProtocol) { m_pRobotProtocol->SetWorkStatus(RobotProtocol::WORK_STATUS_BUSY); @@ -855,8 +867,14 @@ int GrabBagPresenter::StartDetection(int cameraIdx, bool isAuto) // 清空检测数据缓存(释放之前的内存) _ClearDetectionDataCache(); - // 更新当前执行参数(包括相机参数、调平参数、手眼标定参数) - _UpdateCurrentExecutionParams(); + // 更新当前执行参数(如果需要) + if (updateParams) { + // 临时保存默认相机索引 + int savedDefaultCameraIndex = m_currentCameraIndex; + m_currentCameraIndex = cameraIndex; + _UpdateCurrentExecutionParams(); + m_currentCameraIndex = savedDefaultCameraIndex; // 恢复默认相机索引 + } // 配置相机参数 int nRet = _ApplyCameraParameters(); @@ -1033,7 +1051,10 @@ int GrabBagPresenter::_OpenDevice(int cameraIndex, const char* cameraName, const // 先设置状态回调 nRet = pDevice->SetStatusCallback(&GrabBagPresenter::_StaticCameraNotify, this); - LOG_DEBUG("SetStatusCallback result: %d\n", nRet); + if(nRet != SUCCESS){ + delete pDevice; + LOG_ERROR("SetStatusCallback failed, error code: %d\n", nRet); + } ERR_CODE_RETURN(nRet); // 尝试打开相机1 @@ -1261,13 +1282,25 @@ void GrabBagPresenter::CheckAndUpdateWorkStatus() } } +void GrabBagPresenter::SetWorkStatus(WorkStatus status) +{ + m_currentWorkStatus = status; + if (m_pStatus) { + m_pStatus->OnWorkStatusChanged(m_currentWorkStatus); + } + + if(WorkStatus::Error == status){ + m_currentWorkStatus = WorkStatus::Ready; + } +} + void GrabBagPresenter::_AlgoDetectThread() { while(m_bAlgoDetectThreadRunning) { std::unique_lock lock(m_algoDetectMutex); m_algoDetectCondition.wait(lock, [this]() { - return m_currentWorkStatus == WorkStatus::Working; + return m_currentWorkStatus == WorkStatus::Working || m_currentWorkStatus == WorkStatus::Detecting; }); if(!m_bAlgoDetectThreadRunning){ @@ -1312,63 +1345,48 @@ int GrabBagPresenter::_DetectTask() return ERR_CODE(DEV_ARG_INVAILD); } - // 2. 准备算法参数 - 使用 ParameterManager 获取参数 + // 2. 准备算法参数 - 直接从 m_currentExecutionParams 和全局配置获取 SG_bagPositionParam detectionAlgoParam; SSG_planeCalibPara detectionCalibParam; CalibMatrix detectionHandEyeMatrix; - // 初始化调平参数为单位矩阵 - double identityCalibMatrix[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; - memcpy(detectionCalibParam.planeCalib, identityCalibMatrix, sizeof(identityCalibMatrix)); - memcpy(detectionCalibParam.invRMatrix, identityCalibMatrix, sizeof(identityCalibMatrix)); - detectionCalibParam.planeHeight = -1.0; + // 2.1 初始化算法参数结构 + memset(&detectionAlgoParam, 0, sizeof(SG_bagPositionParam)); - // 初始化手眼矩阵为单位矩阵 - double identityHandEyeMatrix[16] = { - 1.0, 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 0.0, 1.0 - }; - memcpy(detectionHandEyeMatrix.clibMatrix, identityHandEyeMatrix, sizeof(identityHandEyeMatrix)); + // 从全局配置中获取算法参数 + const VrAlgorithmParams& xmlParams = m_configResult.algorithmParams; - LOG_INFO("[Algo Thread] work position index: %d, camera index: %d, package index: %d \n", m_currentWorkPositionIndex, m_currentCameraIndex, m_currentPackageTypeIndex); - // 2.1 获取包裹类型参数(优先级:指定索引 > 默认配置 > 全局默认) - bool packageParamsSuccess = false; - try { - packageParamsSuccess = m_pParameterManager->GetPackageTypeParams(m_currentPackageTypeIndex, detectionAlgoParam); - } catch (const std::exception& e) { - LOG_ERROR("[Algo Thread] Exception in GetPackageTypeParams: %s\n", e.what()); - if (m_pStatus) { - m_pStatus->OnStatusUpdate("获取包裹参数异常"); - } - return ERR_CODE(DEV_ARG_INVAILD); - } catch (...) { - LOG_ERROR("[Algo Thread] Unknown exception in GetPackageTypeParams\n"); - if (m_pStatus) { - m_pStatus->OnStatusUpdate("获取包裹参数异常"); - } - return ERR_CODE(DEV_ARG_INVAILD); - } + // 滤波参数 + detectionAlgoParam.filterParam.continuityTh = xmlParams.filterParam.continuityTh; + detectionAlgoParam.filterParam.outlierTh = xmlParams.filterParam.outlierTh; - // 2.2 获取工作点参数(优先级:指定索引 > 默认配置 > 当前执行参数) - try { - LOG_INFO("[Algo Thread] Getting work position params...\n"); - bool ret2 = m_pParameterManager->GetWorkPositionParams(m_currentWorkPositionIndex, m_currentCameraIndex, detectionCalibParam, detectionHandEyeMatrix); - LOG_INFO("[Algo Thread] GetWorkPositionParams returned: %d\n", ret2); - } catch (const std::exception& e) { - LOG_ERROR("[Algo Thread] Exception in GetWorkPositionParams: %s\n", e.what()); - if (m_pStatus) { - m_pStatus->OnStatusUpdate("获取工作点参数异常"); - } - return ERR_CODE(DEV_ARG_INVAILD); - } catch (...) { - LOG_ERROR("[Algo Thread] Unknown exception in GetWorkPositionParams\n"); - if (m_pStatus) { - m_pStatus->OnStatusUpdate("获取工作点参数异常"); - } - return ERR_CODE(DEV_ARG_INVAILD); - } + // 角点参数 + detectionAlgoParam.cornerParam.cornerTh = xmlParams.cornerParam.cornerTh; + detectionAlgoParam.cornerParam.scale = xmlParams.cornerParam.scale; + detectionAlgoParam.cornerParam.minEndingGap = xmlParams.cornerParam.minEndingGap; + detectionAlgoParam.cornerParam.minEndingGap_z = xmlParams.cornerParam.minEndingGap_z; + detectionAlgoParam.cornerParam.jumpCornerTh_1 = xmlParams.cornerParam.jumpCornerTh_1; + detectionAlgoParam.cornerParam.jumpCornerTh_2 = xmlParams.cornerParam.jumpCornerTh_2; + + // 生长参数 + detectionAlgoParam.growParam.maxLineSkipNum = xmlParams.growParam.maxLineSkipNum; + detectionAlgoParam.growParam.yDeviation_max = xmlParams.growParam.yDeviation_max; + detectionAlgoParam.growParam.maxSkipDistance = xmlParams.growParam.maxSkipDistance; + detectionAlgoParam.growParam.zDeviation_max = xmlParams.growParam.zDeviation_max; + detectionAlgoParam.growParam.minLTypeTreeLen = xmlParams.growParam.minLTypeTreeLen; + detectionAlgoParam.growParam.minVTypeTreeLen = xmlParams.growParam.minVTypeTreeLen; + + // 支持旋转 + detectionAlgoParam.supportRotate = xmlParams.supportRotate; + + // 使用 m_currentExecutionParams 中的包裹参数 + detectionAlgoParam.bagParam.bagL = m_currentExecutionParams.bagParam.bagL; + detectionAlgoParam.bagParam.bagW = m_currentExecutionParams.bagParam.bagW; + detectionAlgoParam.bagParam.bagH = m_currentExecutionParams.bagParam.bagH; + + // 2.2 直接使用 m_currentExecutionParams 中的调平参数和手眼标定参数 + detectionCalibParam = m_currentExecutionParams.planeCalibParam; + detectionHandEyeMatrix = m_currentExecutionParams.handEyeCalibMatrix; // 3. 准备检测 unsigned int lineNum = 0; @@ -1903,9 +1921,7 @@ void GrabBagPresenter::_LoadCurrentWorkPositionAndPackageType() WorkPositionConfig* defaultWorkPosition = nullptr; if (!m_configResult.workPositions.empty()) { defaultWorkPosition = &m_configResult.workPositions[0]; - m_currentWorkPositionId = defaultWorkPosition->id; - LOG_INFO("Using first work position: %s (name: %s)\n", - m_currentWorkPositionId.c_str(), defaultWorkPosition->name.c_str()); + LOG_INFO("Using first work position: %s\n", defaultWorkPosition->name.c_str()); } else { LOG_ERROR("No work positions configured!\n"); return; @@ -1914,40 +1930,34 @@ void GrabBagPresenter::_LoadCurrentWorkPositionAndPackageType() // 2. 从工作点中获取默认相机 CameraConfig* defaultCamera = defaultWorkPosition->GetDefaultCamera(); if (defaultCamera) { - m_currentCameraId = defaultCamera->id; - LOG_INFO("Using default camera: %s (name: %s, index: %d)\n", - m_currentCameraId.c_str(), defaultCamera->cameraName.c_str(), - defaultCamera->cameraIndex); + LOG_INFO("Using default camera: %s (index: %d)\n", + defaultCamera->cameraName.c_str(), defaultCamera->cameraIndex); } else if (!defaultWorkPosition->cameras.empty()) { // 如果没有默认配置,使用第一个相机 - m_currentCameraId = defaultWorkPosition->cameras[0].id; defaultCamera = &defaultWorkPosition->cameras[0]; LOG_WARNING("No default camera configured for work position %s, using first camera: %s\n", - m_currentWorkPositionId.c_str(), m_currentCameraId.c_str()); + defaultWorkPosition->name.c_str(), defaultCamera->cameraName.c_str()); } else { - LOG_ERROR("No cameras configured for work position %s!\n", m_currentWorkPositionId.c_str()); + LOG_ERROR("No cameras configured for work position %s!\n", defaultWorkPosition->name.c_str()); return; } // 3. 从相机中获取默认包裹 PackageTypeConfig* defaultPackage = defaultCamera->GetDefaultPackage(); if (defaultPackage) { - m_currentPackageTypeId = defaultPackage->id; - LOG_INFO("Using default package: %s (name: %s)\n", - m_currentPackageTypeId.c_str(), defaultPackage->name.c_str()); + LOG_INFO("Using default package: %s\n", defaultPackage->name.c_str()); } else if (!defaultCamera->packages.empty()) { // 如果没有默认配置,使用第一个包裹 - m_currentPackageTypeId = defaultCamera->packages[0].id; defaultPackage = &defaultCamera->packages[0]; LOG_WARNING("No default package configured for camera %s, using first package: %s\n", - m_currentCameraId.c_str(), m_currentPackageTypeId.c_str()); + defaultCamera->cameraName.c_str(), defaultPackage->name.c_str()); } else { - LOG_ERROR("No packages configured for camera %s!\n", m_currentCameraId.c_str()); + LOG_ERROR("No packages configured for camera %s!\n", defaultCamera->cameraName.c_str()); return; } - // 4. 更新当前执行参数 - _UpdateCurrentExecutionParams(); + // 4. 更新当前执行参数(使用找到的默认配置) + _UpdateCurrentExecutionParams(defaultWorkPosition, defaultCamera, defaultPackage, defaultCamera->cameraIndex); } int GrabBagPresenter::_ApplyCameraParameters() @@ -2033,7 +2043,7 @@ int GrabBagPresenter::_ApplyCameraParameters() void GrabBagPresenter::_UpdateCurrentExecutionParams() { - LOG_INFO("Updating current execution parameters\n"); + LOG_INFO("Updating current execution parameters using m_currentCameraIndex: %d\n", m_currentCameraIndex); // 1. 设置相机序号(使用当前默认相机) m_currentExecutionParams.cameraIndex = m_currentCameraIndex; @@ -2045,7 +2055,6 @@ void GrabBagPresenter::_UpdateCurrentExecutionParams() m_currentExecutionParams.planeCalibParam.invRMatrix[i] = identityMatrix9[i]; } m_currentExecutionParams.planeCalibParam.planeHeight = -1.0; - LOG_INFO("Initialized plane calibration parameters to identity matrix\n"); // 3. 初始化手眼标定矩阵为单位矩阵(默认值) double identityMatrix16[16] = { @@ -2057,52 +2066,33 @@ void GrabBagPresenter::_UpdateCurrentExecutionParams() for (int i = 0; i < 16; i++) { m_currentExecutionParams.handEyeCalibMatrix.clibMatrix[i] = identityMatrix16[i]; } - LOG_INFO("Initialized hand-eye calibration matrix to identity matrix\n"); - // 4. 按照三级结构导航:工作点->相机->包裹 - - // 4.1 获取当前工作点配置 + // 4. 通过 m_currentCameraIndex 查找配置 const WorkPositionConfig* currentWorkPosition = nullptr; - for (const auto& wp : m_configResult.workPositions) { - if (wp.id == m_currentWorkPositionId) { - currentWorkPosition = ℘ - break; - } - } - - if (!currentWorkPosition) { - LOG_WARNING("No current work position found (ID: %s), using default parameters\n", - m_currentWorkPositionId.c_str()); - return; - } - - // 4.2 获取当前相机配置 const CameraConfig* currentCamera = nullptr; - for (const auto& cam : currentWorkPosition->cameras) { - if (cam.id == m_currentCameraId) { - currentCamera = &cam; - break; - } - } - - if (!currentCamera) { - LOG_WARNING("No current camera found (ID: %s) in work position %s, using default parameters\n", - m_currentCameraId.c_str(), m_currentWorkPositionId.c_str()); - return; - } - - // 4.3 获取当前包裹配置 const PackageTypeConfig* currentPackage = nullptr; - for (const auto& pkg : currentCamera->packages) { - if (pkg.id == m_currentPackageTypeId) { - currentPackage = &pkg; - break; + + // 遍历所有工作点、相机,找到匹配 cameraIndex 的相机配置 + for (const auto& wp : m_configResult.workPositions) { + for (const auto& cam : wp.cameras) { + if (cam.cameraIndex == m_currentCameraIndex) { + currentWorkPosition = ℘ + currentCamera = &cam; + // 获取该相机的默认包裹或第一个包裹 + if (!cam.packages.empty()) { + currentPackage = cam.GetDefaultPackage(); + if (!currentPackage) { + currentPackage = &cam.packages[0]; + } + } + break; + } } + if (currentCamera) break; } - if (!currentPackage) { - LOG_WARNING("No current package found (ID: %s) in camera %s, using default parameters\n", - m_currentPackageTypeId.c_str(), m_currentCameraId.c_str()); + if (!currentWorkPosition || !currentCamera || !currentPackage) { + LOG_WARNING("Failed to find configuration for camera index %d, using default parameters\n", m_currentCameraIndex); return; } @@ -2127,6 +2117,11 @@ void GrabBagPresenter::_UpdateCurrentExecutionParams() m_currentExecutionParams.cameraParam.swingStopAngle = 180.0; } + // 5.1 设置包裹参数(从包裹配置中获取) + m_currentExecutionParams.bagParam = currentPackage->bagParam; + LOG_INFO("Using bag parameters: L=%.1f, W=%.1f, H=%.1f\n", + currentPackage->bagParam.bagL, currentPackage->bagParam.bagW, currentPackage->bagParam.bagH); + // 6. 设置调平参数(从相机配置中获取) if (currentCamera->planeCalibParam.isCalibrated && currentCamera->planeCalibParam.cameraIndex == m_currentCameraIndex) { @@ -2157,6 +2152,108 @@ void GrabBagPresenter::_UpdateCurrentExecutionParams() } LOG_INFO("Current execution parameters updated successfully\n"); + LOG_INFO(" Work Position: %s\n", currentWorkPosition->name.c_str()); + LOG_INFO(" Camera: %s (Index: %d)\n", currentCamera->cameraName.c_str(), m_currentCameraIndex); + LOG_INFO(" Package: %s\n", currentPackage->name.c_str()); + + // 更新 ParameterManager 的当前执行参数 + m_pParameterManager->SetCurrentExecutionParams( + m_currentExecutionParams.planeCalibParam, + m_currentExecutionParams.handEyeCalibMatrix + ); +} + +// 重载版本:直接使用配置指针更新执行参数 +void GrabBagPresenter::_UpdateCurrentExecutionParams(const WorkPositionConfig* workPosition, const CameraConfig* camera, const PackageTypeConfig* package, int cameraIndex) +{ + LOG_INFO("Updating current execution parameters with provided configuration\n"); + + if (!workPosition || !camera || !package) { + LOG_ERROR("Invalid configuration pointers provided\n"); + return; + } + + // 1. 设置相机序号 + m_currentExecutionParams.cameraIndex = cameraIndex; + + // 2. 初始化调平参数为单位矩阵(默认值) + double identityMatrix9[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + for (int i = 0; i < 9; i++) { + m_currentExecutionParams.planeCalibParam.planeCalib[i] = identityMatrix9[i]; + m_currentExecutionParams.planeCalibParam.invRMatrix[i] = identityMatrix9[i]; + } + m_currentExecutionParams.planeCalibParam.planeHeight = -1.0; + + // 3. 初始化手眼标定矩阵为单位矩阵(默认值) + double identityMatrix16[16] = { + 1.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0 + }; + for (int i = 0; i < 16; i++) { + m_currentExecutionParams.handEyeCalibMatrix.clibMatrix[i] = identityMatrix16[i]; + } + + // 4. 设置相机参数(从包裹配置中获取) + const VrCameraParams* cameraParam = package->GetCameraParam(cameraIndex); + + if (cameraParam) { + m_currentExecutionParams.cameraParam = *cameraParam; + LOG_INFO("Using camera parameters from package %s for camera %d: exposure=%.2f, gain=%.2f, frameRate=%.2f\n", + package->name.c_str(), cameraIndex, + cameraParam->exposure, cameraParam->gain, cameraParam->frameRate); + } else { + LOG_WARNING("No camera parameters found for camera %d in package %s, using defaults\n", + cameraIndex, package->name.c_str()); + // 使用默认参数 + m_currentExecutionParams.cameraParam.cameraIndex = cameraIndex; + m_currentExecutionParams.cameraParam.exposure = 100.0; + m_currentExecutionParams.cameraParam.gain = 1.0; + m_currentExecutionParams.cameraParam.frameRate = 30.0; + m_currentExecutionParams.cameraParam.swingSpeed = 10.0; + m_currentExecutionParams.cameraParam.swingStartAngle = 0.0; + m_currentExecutionParams.cameraParam.swingStopAngle = 180.0; + } + + // 4.1 设置包裹参数(从包裹配置中获取) + m_currentExecutionParams.bagParam = package->bagParam; + LOG_INFO("Using bag parameters: L=%.1f, W=%.1f, H=%.1f\n", + package->bagParam.bagL, package->bagParam.bagW, package->bagParam.bagH); + + // 5. 设置调平参数(从相机配置中获取) + if (camera->planeCalibParam.isCalibrated && + camera->planeCalibParam.cameraIndex == cameraIndex) { + // 使用实际标定矩阵 + for (int i = 0; i < 9; i++) { + m_currentExecutionParams.planeCalibParam.planeCalib[i] = camera->planeCalibParam.planeCalib[i]; + m_currentExecutionParams.planeCalibParam.invRMatrix[i] = camera->planeCalibParam.invRMatrix[i]; + } + m_currentExecutionParams.planeCalibParam.planeHeight = camera->planeCalibParam.planeHeight; + LOG_INFO("Using calibrated plane parameters from camera %s (height=%.3f)\n", + camera->cameraName.c_str(), camera->planeCalibParam.planeHeight); + } else { + LOG_INFO("Camera %s is not calibrated, using identity matrix for plane calibration\n", + camera->cameraName.c_str()); + } + + // 6. 设置手眼标定参数(从相机配置中获取) + if (camera->handEyeCalibParam.isCalibrated && + camera->handEyeCalibParam.cameraIndex == cameraIndex) { + // 使用实际标定矩阵 + for (int i = 0; i < 16; i++) { + m_currentExecutionParams.handEyeCalibMatrix.clibMatrix[i] = camera->handEyeCalibParam.transformMatrix[i]; + } + LOG_INFO("Using calibrated hand-eye matrix from camera %s\n", camera->cameraName.c_str()); + } else { + LOG_INFO("Camera %s hand-eye is not calibrated, using identity matrix\n", + camera->cameraName.c_str()); + } + + LOG_INFO("Current execution parameters updated successfully\n"); + LOG_INFO(" Work Position: %s\n", workPosition->name.c_str()); + LOG_INFO(" Camera: %s (Index: %d)\n", camera->cameraName.c_str(), cameraIndex); + LOG_INFO(" Package: %s\n", package->name.c_str()); // 更新 ParameterManager 的当前执行参数 m_pParameterManager->SetCurrentExecutionParams( diff --git a/App/GrabBag/GrabBagApp/Presenter/Src/ParameterManager.cpp b/App/GrabBag/GrabBagApp/Presenter/Src/ParameterManager.cpp index b0dafef..6ce21b1 100644 --- a/App/GrabBag/GrabBagApp/Presenter/Src/ParameterManager.cpp +++ b/App/GrabBag/GrabBagApp/Presenter/Src/ParameterManager.cpp @@ -524,8 +524,7 @@ bool ParameterManager::GetWorkPositionParams(int workPositionIndex, int cameraIn } } - LOG_INFO("[ParameterManager] Found work position: %s (ID: %s)\n", - workPosition->name.c_str(), workPosition->id.c_str()); + LOG_INFO("[ParameterManager] Found work position: %s (ID: %s)\n", workPosition->name.c_str(), workPosition->id.c_str()); // 第2步:从工作点找到相机配置 if (cameraIndex >= 0) { diff --git a/App/GrabBag/GrabBagApp/Version.h b/App/GrabBag/GrabBagApp/Version.h index 9b7a438..338895f 100644 --- a/App/GrabBag/GrabBagApp/Version.h +++ b/App/GrabBag/GrabBagApp/Version.h @@ -3,8 +3,8 @@ #define GRABBAG_VERSION_STRING "1.3.2" -#define GRABBAG_BUILD_STRING "0" -#define GRABBAG_FULL_VERSION_STRING "V1.3.2_0" +#define GRABBAG_BUILD_STRING "2" +#define GRABBAG_FULL_VERSION_STRING "V1.3.2_2" // 获取版本信息的便捷函数 inline const char* GetGrabBagVersion() { diff --git a/App/GrabBag/GrabBagApp/Version.md b/App/GrabBag/GrabBagApp/Version.md index 807861d..93cd81d 100644 --- a/App/GrabBag/GrabBagApp/Version.md +++ b/App/GrabBag/GrabBagApp/Version.md @@ -1,5 +1,12 @@ -#1.3.2 2025-10-28 -## build_0 +#1.3.2 +## build_2 2025-11-23 +1. config 修改默认数据 + +## build_1 2025-11-23 +1. 修改协议触发错误 +2. 包裹ID修改为唯一标识 + +## build_0 2025-10-28 1. 摆动机构默认参数无效 2. 修改帧率范围 3. 相机IP配置:通过配置文件config.xml 配置相机IP,如果没有没有配置,搜索相机进行打开 diff --git a/App/GrabBag/GrabBagApp/devstatus.cpp b/App/GrabBag/GrabBagApp/devstatus.cpp index 2893958..899ae4d 100644 --- a/App/GrabBag/GrabBagApp/devstatus.cpp +++ b/App/GrabBag/GrabBagApp/devstatus.cpp @@ -52,7 +52,8 @@ void devstatus::setCameraStatusImage(QWidget* widget, bool isOnline) { QString imagePath = isOnline ? ":/common/resource/camera_online.png" : ":/common/resource/camera_offline.png"; - widget->setStyleSheet(QString("image: url(%1);").arg(imagePath)); + // 使用 border-image 而不是 image,这是 QWidget 支持的正确属性 + widget->setStyleSheet(QString("QWidget { border-image: url(%1); }").arg(imagePath)); // widget->setFixedSize(32, 32); // 调整大小以适应图片 } @@ -62,7 +63,8 @@ void devstatus::setRobotStatusImage(QWidget* widget, bool isOnline) { QString imagePath = isOnline ? ":/common/resource/robot_online.png" : ":/common/resource/robot_offline.png"; - widget->setStyleSheet(QString("image: url(%1); ").arg(imagePath)); + // 使用 border-image 而不是 image,这是 QWidget 支持的正确属性 + widget->setStyleSheet(QString("QWidget { border-image: url(%1); }").arg(imagePath)); // widget->setFixedSize(32, 32); // 调整大小以适应图片 } diff --git a/App/GrabBag/GrabBagApp/devstatus.ui b/App/GrabBag/GrabBagApp/devstatus.ui index 327457e..6145320 100644 --- a/App/GrabBag/GrabBagApp/devstatus.ui +++ b/App/GrabBag/GrabBagApp/devstatus.ui @@ -65,7 +65,7 @@ - image: url(:/common/resource/robot_offline.png); + QWidget { border-image: url(:/common/resource/robot_offline.png); } @@ -118,7 +118,7 @@ - image: url(:/common/resource/camera_offline.png); + QWidget { border-image: url(:/common/resource/camera_offline.png); } @@ -171,7 +171,7 @@ - image: url(:/common/resource/camera_offline.png); + QWidget { border-image: url(:/common/resource/camera_offline.png); } diff --git a/App/GrabBag/GrabBagApp/dialogcameralevel.cpp b/App/GrabBag/GrabBagApp/dialogcameralevel.cpp index 0a931e7..1609770 100644 --- a/App/GrabBag/GrabBagApp/dialogcameralevel.cpp +++ b/App/GrabBag/GrabBagApp/dialogcameralevel.cpp @@ -194,8 +194,7 @@ void DialogCameraLevel::updateCameraList() void DialogCameraLevel::on_btn_apply_clicked() { ui->label_level_result->setAlignment(Qt::AlignLeft); - -#ifndef LEVEL_DEBUG_MODE + // 检查是否有可用的相机 if (m_cameraList.empty()) { QMessageBox::warning(this, "错误", "无可用相机设备!"); @@ -208,7 +207,6 @@ void DialogCameraLevel::on_btn_apply_clicked() QMessageBox::warning(this, "错误", "请选择有效的相机!"); return; } -#endif // 清空之前的结果显示 ui->label_level_result->setText("调平计算中,请稍候..."); @@ -221,7 +219,7 @@ void DialogCameraLevel::on_btn_apply_clicked() // 执行相机调平 if (performCameraLeveling()) { // 调平成功,关闭对话框(这会触发析构函数中的回调恢复) - accept(); + // accept(); } else { // 显示失败信息到界面 ui->label_level_result->setText("调平失败!\n\n请检查:\n1. 相机连接是否正常\n2. 地面扫描数据是否充足\n3. 扫描区域是否有足够的地面"); @@ -249,24 +247,7 @@ bool DialogCameraLevel::performCameraLeveling() // 1. 设置调平状态回调 setLevelingStatusCallback(); - -#ifdef LEVEL_DEBUG_MODE - // Debug模式:使用文件对话框选择测试数据 - LOG_INFO("=== DEBUG MODE ENABLED ===\n"); - - // 选择debug数据文件 - QString debugDataFile = selectDebugDataFile(); - if (debugDataFile.isEmpty()) { - LOG_INFO("Debug data file selection cancelled\n"); - return false; - } - - // 使用选择的debug数据进行模拟扫描 - if (!loadDebugDataAndSimulateScan(debugDataFile)) { - LOG_ERROR("Failed to load debug data for camera leveling\n"); - return false; - } -#else + // 正常模式:使用真实相机 if (selectedIndex < 0 || selectedIndex >= m_cameraList.size()) { LOG_ERROR("Invalid camera index: %d\n", selectedIndex); @@ -305,7 +286,6 @@ bool DialogCameraLevel::performCameraLeveling() } else if (waitTime >= maxWaitTime) { LOG_WARNING("Timeout waiting for camera swing finish signal\n"); } -#endif // 5. 检查是否收集到足够的数据 // 6. 调用调平算法计算 @@ -329,12 +309,6 @@ bool DialogCameraLevel::performCameraLeveling() QString cameraName; QString workPosId; -#ifdef LEVEL_DEBUG_MODE - // Debug模式下使用默认名称 - cameraIndex = 1; - cameraName = QString("Camera_%1").arg(cameraIndex); - workPosId = "default_wp"; // Debug模式下的默认工位ID -#else // 正常模式下从列表获取名称 if (m_currentCameraIndex >= 0 && m_currentCameraIndex < static_cast(m_cameraList.size())) { cameraName = QString::fromStdString(m_cameraList[m_currentCameraIndex].first); @@ -350,7 +324,6 @@ bool DialogCameraLevel::performCameraLeveling() LOG_ERROR("Invalid work position index: %d\n", m_currentWorkPosIndex); return false; } -#endif if (!saveLevelingResults(planeCalib, planeHeight, invRMatrix, cameraIndex, cameraName, workPosId)) { LOG_ERROR("Failed to save leveling results\n"); @@ -556,35 +529,7 @@ bool DialogCameraLevel::calculatePlaneCalibration(double planeCalib[9], double& LOG_INFO("Plane calibration calculated: height=%.3f, rotX=%.2f°, rotY=%.2f°\n", planeHeight, rotAngleX, rotAngleY); - -#ifdef LEVEL_DEBUG_MODE - // 统计点云数据 - int totalPoints = 0; - double avgHeight = 0.0; - for (const auto& scanLine : m_scanDataCache) { - for (int i = 0; i < scanLine.nPositionCnt; i++) { - if (scanLine.p3DPosition[i].pt3D.z > -1000 && scanLine.p3DPosition[i].pt3D.z < 1000) { - avgHeight += scanLine.p3DPosition[i].pt3D.z; - totalPoints++; - } - } - } - - LOG_INFO("=== DEBUG MODE CALIBRATION RESULTS ===\n"); - LOG_INFO("Algorithm results:\n"); - LOG_INFO(" Calculated plane height: %.3f mm\n", calibResult.planeHeight); - LOG_INFO("Calibration matrix:\n"); - for (int i = 0; i < 3; i++) { - LOG_INFO(" [%.6f, %.6f, %.6f]\n", calibResult.planeCalib[i*3], calibResult.planeCalib[i*3+1], calibResult.planeCalib[i*3+2]); - } - LOG_INFO("Inverse rotation matrix:\n"); - for (int i = 0; i < 3; i++) { - LOG_INFO(" [%.6f, %.6f, %.6f]\n", calibResult.invRMatrix[i*3], calibResult.invRMatrix[i*3+1], calibResult.invRMatrix[i*3+2]); - } - LOG_INFO("=======================================\n"); -#endif - return true; } catch (const std::exception& e) { diff --git a/App/GrabBag/GrabBagApp/dialogconfigtree.cpp b/App/GrabBag/GrabBagApp/dialogconfigtree.cpp index eff3571..67852ab 100644 --- a/App/GrabBag/GrabBagApp/dialogconfigtree.cpp +++ b/App/GrabBag/GrabBagApp/dialogconfigtree.cpp @@ -107,7 +107,19 @@ void DialogConfigTree::initTree() // 包裹节点 for (auto& pkg : cam.packages) { QTreeWidgetItem* pkgItem = new QTreeWidgetItem(camItem); - QString pkgName = QString::fromStdString(pkg.name); + + // 从包裹ID中提取数字编号(格式:pkg_1 -> 1) + QString pkgIdStr = QString::fromStdString(pkg.id); + QString pkgNumber = pkgIdStr; + if (pkgIdStr.startsWith("pkg_")) { + pkgNumber = pkgIdStr.mid(4); // 提取 "pkg_" 后面的数字 + } + + // 显示格式:包裹名称 (ID编号) + QString pkgName = QString("%1 (%2)") + .arg(QString::fromStdString(pkg.name)) + .arg(pkgNumber); + if (pkg.id == defaultPkgId && cam.id == defaultCamId && wp.id == defaultWpId) { pkgName += " [默认]"; } @@ -673,12 +685,30 @@ void DialogConfigTree::addCamera(const QString& wpId, const QString& name, int c void DialogConfigTree::addPackage(const QString& wpId, const QString& camId, const QString& name) { + // 计算全局包裹ID(遍历所有工位、所有相机下的所有包裹) + int maxPkgId = 0; + for (const auto& wp : m_configResult->workPositions) { + for (const auto& cam : wp.cameras) { + for (const auto& pkg : cam.packages) { + // 从包裹ID中提取数字部分(格式:pkg_1, pkg_2, ...) + std::string pkgIdStr = pkg.id; + if (pkgIdStr.find("pkg_") == 0) { + int pkgNum = std::stoi(pkgIdStr.substr(4)); + if (pkgNum > maxPkgId) { + maxPkgId = pkgNum; + } + } + } + } + } + for (auto& wp : m_configResult->workPositions) { if (wp.id == wpId.toStdString()) { CameraConfig* cam = wp.GetCamera(camId.toStdString()); if (cam) { PackageTypeConfig pkg; - pkg.id = "pkg_" + std::to_string(cam->packages.size() + 1); + // 使用全局唯一的包裹ID + pkg.id = "pkg_" + std::to_string(maxPkgId + 1); pkg.name = name.toStdString(); // 使用ConfigResult中的默认值初始化包裹参数 @@ -928,11 +958,16 @@ void DialogConfigTree::onCameraLevelClicked() int x = parentGeometry.left() + (parentGeometry.width() - dlg.width()) / 2; int y = parentGeometry.top() + (parentGeometry.height() - dlg.height()) / 2; dlg.move(x, y); - +#if 0 if (dlg.exec() == QDialog::Accepted) { // 调平成功后,刷新相机配置显示 showCameraConfig(m_currentWorkPos, m_currentCamera); } +#else + dlg.exec(); + showCameraConfig(m_currentWorkPos, m_currentCamera); +#endif + } void DialogConfigTree::displayHandEyeCalibInline() diff --git a/App/GrabBag/GrabBagApp/dialogconfigtree.ui b/App/GrabBag/GrabBagApp/dialogconfigtree.ui index 5c89e99..f8addfb 100644 --- a/App/GrabBag/GrabBagApp/dialogconfigtree.ui +++ b/App/GrabBag/GrabBagApp/dialogconfigtree.ui @@ -7,7 +7,7 @@ 0 0 896 - 619 + 642 @@ -216,8 +216,8 @@ color: rgb(221, 225, 233); 0 0 - 243 - 436 + 536 + 569 @@ -251,12 +251,12 @@ color: rgb(221, 225, 233); 16 - - true - + + true + @@ -278,12 +278,12 @@ color: rgb(221, 225, 233); 16 - - true - + + true + @@ -619,7 +619,7 @@ color: rgb(221, 225, 233); 0 0 550 - 520 + 543 diff --git a/App/GrabBag/GrabBagApp/main.cpp b/App/GrabBag/GrabBagApp/main.cpp index d4542f2..e1770f9 100644 --- a/App/GrabBag/GrabBagApp/main.cpp +++ b/App/GrabBag/GrabBagApp/main.cpp @@ -5,10 +5,35 @@ #include #include #include +#include +#ifdef _WIN32 +#include +#include +#endif + +#ifdef _WIN32 +// Windows平台特定设置,解决中文乱码问题 +void setupWindowsConsoleEncoding() { + // 设置控制台代码页为UTF-8 + SetConsoleOutputCP(CP_UTF8); + SetConsoleCP(CP_UTF8); +} +#endif int main(int argc, char *argv[]) { QApplication a(argc, argv); + +#ifdef _WIN32 + // Windows平台特定设置,解决中文乱码问题 + setupWindowsConsoleEncoding(); +#endif + +#if (QT_VERSION < QT_VERSION_CHECK(5, 0, 0)) + // Qt4及更早版本需要显式设置编码 + QTextCodec *codec = QTextCodec::codecForName("UTF-8"); + QTextCodec::setCodecForLocale(codec); +#endif // 设置应用程序图标 a.setWindowIcon(QIcon(":/common/resource/logo.png")); @@ -30,4 +55,4 @@ int main(int argc, char *argv[]) w.show(); return a.exec(); -} \ No newline at end of file +} diff --git a/App/GrabBag/GrabBagApp/mainwindow.ui b/App/GrabBag/GrabBagApp/mainwindow.ui index 491750c..a14ccd7 100644 --- a/App/GrabBag/GrabBagApp/mainwindow.ui +++ b/App/GrabBag/GrabBagApp/mainwindow.ui @@ -96,7 +96,7 @@ background-color: rgba(255, 255, 255, 0); - 1175 + 1110 21 220 80 @@ -119,7 +119,7 @@ border: none; - 930 + 865 21 220 80 @@ -142,7 +142,7 @@ border: none; - 1175 + 1110 21 220 80 @@ -165,7 +165,7 @@ border: none; - 685 + 620 21 220 80 @@ -237,7 +237,7 @@ background-color: rgba(255, 255, 255, 0); - 1525 + 1470 21 80 80 @@ -259,7 +259,7 @@ background-color: rgba(255, 255, 255, 0); - 1645 + 1590 21 80 80 @@ -353,7 +353,7 @@ background-color: rgba(255, 255, 255, 0); 0 0 1920 - 19 + 21 diff --git a/App/GrabBag/GrabBagConfig/Inc/IVrConfig.h b/App/GrabBag/GrabBagConfig/Inc/IVrConfig.h index 12b09ba..22948bb 100644 --- a/App/GrabBag/GrabBagConfig/Inc/IVrConfig.h +++ b/App/GrabBag/GrabBagConfig/Inc/IVrConfig.h @@ -91,9 +91,9 @@ struct VrOutlierFilterParam struct VrCornerParam { double minEndingGap = 20.0; - double minEndingGap_z = 20.0; - double scale = 15.0; - double cornerTh = 30.0; + double minEndingGap_z = 40.0; + double scale = 50.0; + double cornerTh = 45.0; double jumpCornerTh_1 = 60.0; double jumpCornerTh_2 = 15.0; }; @@ -283,7 +283,7 @@ struct VrAlgorithmParams VrHsvCmpParam hsvCmpParam; VrRgbColorPattern rgbColorPattern; VrColorTemplateParam colorTemplateParam; - int supportRotate = 0; + int supportRotate = 1; }; /**