#include "visionservice.h" #include "visiondllworker.h" #include #include #include #include #include #include #include namespace { QString newId() { return QUuid::createUuid().toString(QUuid::WithoutBraces); } bool validPose(const QJsonValue &value) { if (!value.isArray() || value.toArray().size() != 6) return false; for (const QJsonValue &axis : value.toArray()) { if (!axis.isDouble() || !std::isfinite(axis.toDouble())) return false; } return true; } bool nonnegativeInteger(const QJsonValue &value) { return value.isDouble() && std::isfinite(value.toDouble()) && value.toDouble() >= 0 && std::floor(value.toDouble()) == value.toDouble(); } QString epochToIso(const QJsonValue &value) { if (!value.isDouble() || !std::isfinite(value.toDouble())) return QString(); return QDateTime::fromMSecsSinceEpoch(qint64(value.toDouble()), Qt::UTC) .toString(Qt::ISODateWithMs); } } VisionService::VisionService(const VisionConfig &config, QObject *parent) : QObject(parent), m_config(config), m_thread(new QThread(this)) , m_worker(new VisionDllWorker), m_requestTimer(new QTimer(this)) , m_avoidanceTimer(new QTimer(this)), m_avoidanceTimeout(new QTimer(this)) { qRegisterMetaType("QJsonObject"); m_requestTimer->setSingleShot(true); m_avoidanceTimer->setSingleShot(true); m_avoidanceTimeout->setSingleShot(true); connect(m_avoidanceTimer, &QTimer::timeout, this, &VisionService::pollAvoidance); connect(m_avoidanceTimeout, &QTimer::timeout, this, [this]() { if (m_avoidanceRequestId.isEmpty()) return; m_avoidanceDiscarded = true; m_worker->cancelOperation(m_avoidanceRequestId); logAvoidance("ERROR", "避障检测超时,等待 DLL 结束本次操作后重试"); }); m_worker->moveToThread(m_thread); connect(m_thread, &QThread::finished, m_worker, &QObject::deleteLater); connect(m_worker, &VisionDllWorker::sdkLoaded, this, &VisionService::onSdkLoaded, Qt::QueuedConnection); connect(m_worker, &VisionDllWorker::devicesInitialized, this, &VisionService::onDevicesInitialized, Qt::QueuedConnection); connect(m_worker, &VisionDllWorker::operationCompleted, this, &VisionService::onOperationCompleted, Qt::QueuedConnection); connect(m_worker, &VisionDllWorker::released, this, &VisionService::onReleased, Qt::QueuedConnection); connect(m_worker, &VisionDllWorker::logMessage, this, &VisionService::logMessage, Qt::QueuedConnection); connect(m_requestTimer, &QTimer::timeout, this, [this]() { const QString id = m_request.value("request_id").toString(); if (id.isEmpty()) return; m_cancelledRequests.insert(id); QString cancelError; m_worker->cancelOperation(id, &cancelError); if (!cancelError.isEmpty()) emit logMessage(cancelError); failRequest("CLIENT_TIMEOUT", "视觉识别等待超时,本次结果已作废"); }); m_thread->start(); } VisionService::~VisionService() { shutdown(); if (m_thread->isRunning()) m_thread->wait(); } QJsonObject VisionService::standardParameters() { return {{"camera_mode", "left_only"}, {"refinement", "least_squares_high"}, {"deadline_ms", 30000}, {"min_score", 0.70}, {"greediness", 0.80}, {"yolo_selection", "center"}, {"pose_display", "model_origin"}, {"grasp_offset_mm", 4.40}}; } QJsonObject VisionService::standardConvention() { return {{"frame", "world"}, {"tool", "TOOL1"}, {"rotation", "Rz(A)Ry(B)Rx(C)"}, {"linear_unit", "mm"}, {"angular_unit", "deg"}}; } bool VisionService::isEnabled() const { return m_config.enabled; } bool VisionService::isReady() const { return isEnabled() && m_ready && !m_session.isEmpty(); } bool VisionService::isBusy() const { return m_loading || m_initializing || m_releasing || m_workerBusy || !m_avoidanceRequestId.isEmpty(); } bool VisionService::canInitialize() const { return isEnabled() && !m_session.isEmpty() && !isBusy() && (!m_productionActive || m_maintenanceAllowed); } QString VisionService::sessionId() const { return m_session; } void VisionService::startService() { if (m_shuttingDown) return; if (!isEnabled()) { emit operationFeedback("start", "info", "视觉功能未启用(vision/enabled=false)"); return; } m_avoidanceEnabled = true; m_avoidanceTimer->start(0); if (!m_session.isEmpty()) { emit operationFeedback("start", "success", "视觉 DLL 已加载"); return; } if (isBusy()) { emit operationFeedback("start", "info", "视觉 DLL 正在处理其他操作"); return; } m_loading = true; publishAvailability(); QMetaObject::invokeMethod(m_worker, "loadSdk", Qt::QueuedConnection, Q_ARG(QString, m_config.sdkRoot), Q_ARG(int, m_config.initializeTimeoutMs)); } void VisionService::onSdkLoaded(bool success, const QJsonObject &status, const QString &message) { m_loading = false; if (m_releasing || m_shuttingDown) return; if (success) { m_session = newId(); emit statusChanged(status); emit logMessage(message); emit operationFeedback("start", "success", message); } else { m_session.clear(); m_ready = false; emit logMessage(message); emit operationFeedback("start", "error", message); } publishAvailability(); if (m_avoidanceEnabled) { if (!success) logAvoidance("ERROR", "避障检测未启动:" + message); m_avoidanceTimer->start(success ? 0 : 5000); } } void VisionService::initialize() { if (!canInitialize()) { const QString message = !isEnabled() ? "视觉功能未启用(vision/enabled=false)" : "初始化需要 DLL 已加载、资源空闲且生产已停止"; emit logMessage(message); emit operationFeedback("initialize", "error", message); return; } m_ready = false; m_initializing = true; publishAvailability(); QMetaObject::invokeMethod(m_worker, "initializeDevices", Qt::QueuedConnection, Q_ARG(int, m_config.initializeTimeoutMs)); } void VisionService::onDevicesInitialized(bool success, const QJsonObject &status, const QString &message) { m_initializing = false; if (m_releasing || m_shuttingDown) return; m_ready = success; if (!status.isEmpty()) emit statusChanged(status); emit logMessage(message); emit operationFeedback("initialize", success ? "success" : "error", message); publishAvailability(); if (m_avoidanceEnabled) { if (!success) logAvoidance("ERROR", "避障设备初始化失败:" + message); m_avoidanceTimer->start(success ? 0 : 5000); } } void VisionService::pollAvoidance() { if (!isEnabled() || !m_avoidanceEnabled || m_shuttingDown || m_releasing) return; if (!std::isfinite(m_config.warningDistanceMm) || m_config.warningDistanceMm < 1 || m_config.warningDistanceMm > 5000) { logAvoidance("ERROR", "避障配置无效:vision/warningDistanceMm 必须在 1~5000 mm 范围内"); return; } if (isBusy()) { m_avoidanceTimer->start(500); return; } if (m_session.isEmpty()) { startService(); return; } if (!isReady()) { // Automatic camera recovery does not send PLC or robot commands. m_initializing = true; publishAvailability(); QMetaObject::invokeMethod(m_worker, "initializeDevices", Qt::QueuedConnection, Q_ARG(int, m_config.initializeTimeoutMs)); return; } m_avoidanceRequestId = newId(); m_avoidanceDiscarded = false; const QJsonObject request{{"request_id", m_avoidanceRequestId}, {"input_source", "camera"}, {"threshold_mm", m_config.warningDistanceMm}, {"max_time_ms", m_config.requestTimeoutMs}}; publishAvailability(); QMetaObject::invokeMethod(m_worker, "planAvoidance", Qt::QueuedConnection, Q_ARG(QJsonObject, request)); m_avoidanceTimeout->start(m_config.requestTimeoutMs); } void VisionService::finishAvoidance(int code, const QJsonObject &result) { m_avoidanceTimeout->stop(); const QString requestId = m_avoidanceRequestId; m_avoidanceRequestId.clear(); if (!m_avoidanceDiscarded && m_avoidanceEnabled && !m_shuttingDown && !m_releasing) { const double distance = result.value("distance_mm").toDouble(); const bool warning = distance < m_config.warningDistanceMm; if (code == HVG_DISTANCE_UNKNOWN) { m_avoidanceLogKey.clear(); } else if (code != HVG_OK) { logAvoidance("ERROR", QString("避障检测失败(code=%1):%2") .arg(code).arg(result.value("message").toString())); if (code == HVG_NOT_READY || code == HVG_DEVICE_ERROR || code == HVG_CONFIG_ERROR) m_ready = false; } else if (result.value("schema_version").toDouble() != 1 || !result.value("success").toBool() || !result.value("code").isDouble() || result.value("code").toDouble() != HVG_OK || result.value("request_id").toString() != requestId || !result.value("distance_valid").toBool() || !result.value("distance_mm").isDouble() || !std::isfinite(distance) || distance <= 0 || result.value("threshold_mm").toDouble() != m_config.warningDistanceMm || !result.value("warning").isBool() || result.value("warning").toBool() != warning || result.value("state").toString() != (warning ? "WARNING" : "SAFE")) { logAvoidance("ERROR", "避障检测结果无效,距离或状态不可用"); } else { logAvoidance(warning ? "WARNING" : "SAFE", QString("%1:距离 %2 mm,预警阈值 %3 mm") .arg(warning ? "避障距离预警" : "避障距离安全") .arg(distance, 0, 'f', 2).arg(m_config.warningDistanceMm, 0, 'f', 2)); } } publishAvailability(); if (m_avoidanceEnabled && !m_shuttingDown && !m_releasing) m_avoidanceTimer->start(isReady() ? 500 : 5000); } void VisionService::logAvoidance(const QString &state, const QString &message) { const QString key = state == "ERROR" ? state + message : state; if (key == m_avoidanceLogKey) return; m_avoidanceLogKey = key; emit avoidanceLog(state == "WARNING" ? "预警" : state == "SAFE" ? "安全" : "失败", message); } void VisionService::measure(const QJsonObject &request) { requestOperation(request, false); } void VisionService::recognizeGrooves(const QJsonObject &request) { requestOperation(request, false, true); } void VisionService::diagnose(const QJsonObject &request) { requestOperation(request, true); } void VisionService::requestOperation(QJsonObject request, bool diagnostic, bool grooves) { const QString type = grooves ? "recognize_grooves" : "measure"; const QString id = request.value("request_id").toString(); if (!id.isEmpty() && (m_completedRequests.contains(id) || m_activeWorkerRequest == id)) { emit logMessage("忽略重复的视觉请求编号:" + id); return; } QString error; const QJsonObject context = request.value("context").toObject(); const double tray = context.value("tray_no").toDouble(); if (!isEnabled()) error = "视觉功能未启用(vision/enabled=false)"; else if (id.isEmpty() || m_cancelledRequests.contains(id)) error = "请求编号为空或已取消"; else if (!isReady() || m_loading || m_initializing || m_releasing || m_workerBusy) error = "视觉 DLL 未就绪或仍在处理上一任务"; else if (m_maintenanceAllowed) error = "视觉维护期间只允许初始化"; else if (diagnostic == m_productionActive) error = "诊断与自动生产不可同时执行"; else if (!diagnostic && request.value("server_session_id").toString() != m_session) error = "生产请求会话已失效"; else if (!grooves && !validPose(request.value("robot_pose"))) error = "拍照位姿须为6个有限数值"; else if (context.value("workpiece_id").toString().isEmpty() || context.value("task_id").toString().isEmpty() || context.value("generation").toString().isEmpty() || tray < 1 || tray > 99 || std::floor(tray) != tray) error = "工件/任务/料盘上下文无效"; else if (!grooves && (request.value("parameters").toObject() != standardParameters() || request.value("convention").toObject() != standardConvention())) error = "测量参数或机器人坐标约定不匹配"; else if (!grooves && (!request.value("require_robot_pose").toBool() || request.value("pose_source").toString() != (diagnostic ? "diagnostic" : "plc_r40") || !QDateTime::fromString(request.value("pose_read_at").toString(), Qt::ISODate).isValid())) error = "拍照位姿来源或采集时间无效"; if (!error.isEmpty()) { if (!id.isEmpty()) m_completedRequests.insert(id); QJsonObject result = request; result.insert("type", type); result.insert("status", "error"); result.insert("error_code", "REQUEST_REJECTED"); result.insert("message", error); result.insert("camera_valid", false); result.insert("robot_valid", false); result.insert("occupancy_valid", false); if (grooves) emit groovesRecognized(result); else emit measurementFinished(result); emit logMessage(error); return; } request.insert("protocol_version", 1); request.insert("type", type); request.insert("server_session_id", m_session); m_request = request; m_diagnostic = diagnostic; m_workerBusy = true; m_activeWorkerRequest = id; // Foreground recognition takes priority; the worker queues it after the current DLL call. if (!m_avoidanceRequestId.isEmpty()) { m_avoidanceDiscarded = true; m_avoidanceTimeout->stop(); m_worker->cancelOperation(m_avoidanceRequestId); } if (!grooves) emit requestStarted(request); publishAvailability(); QMetaObject::invokeMethod(m_worker, grooves ? "recognizeGrooves" : "measure", Qt::QueuedConnection, Q_ARG(QJsonObject, dllRequest(request))); m_requestTimer->start(m_config.requestTimeoutMs); } QJsonObject VisionService::dllRequest(const QJsonObject &request) const { if (request.value("type").toString() == "recognize_grooves") { return {{"request_id", request.value("request_id")}, {"mode", "occupancy"}, {"input_source", "camera"}, {"camera", "left"}, {"confidence", 0.25}, {"nms", 0.45}, {"max_time_ms", m_config.requestTimeoutMs}}; } const QJsonObject parameters = request.value("parameters").toObject(); const QJsonObject context = request.value("context").toObject(); return { {"request_id", request.value("request_id")}, {"input_source", "camera"}, {"camera_mode", "left"}, {"max_time_ms", parameters.value("deadline_ms")}, {"refinement", "high"}, {"min_score", parameters.value("min_score")}, {"greediness", parameters.value("greediness")}, {"target_selection", 0}, {"grasp_offset_mm", parameters.value("grasp_offset_mm")}, {"tray_id", context.value("tray_no")}, {"z_strategy", "plane_xy"}, {"compute_grasp", true}, {"robot_pose_at_capture", QJsonObject{ {"XYZABC_mm_deg", request.value("robot_pose")}, {"frame_id", "world"}, {"tool_id", "TOOL1"}, {"capture_pose_confirmed", true}, {"source", request.value("pose_source")} }} }; } QJsonObject VisionService::adaptResult(int code, const QJsonObject &dllResult) const { QJsonObject result = m_request; result.insert("type", "measure"); result.insert("status", code == HVG_OK ? "ok" : code == HVG_TIMEOUT ? "timeout" : code == HVG_CANCELLED ? "cancelled" : "error"); result.insert("error_code", code); result.insert("message", dllResult.value("message")); result.insert("camera_valid", dllResult.value("vision_valid")); result.insert("robot_valid", dllResult.value("grasp_valid")); result.insert("camera_pose", dllResult.value("object_pose_camera").toObject() .value("XYZABC_mm_deg")); result.insert("robot_pose", dllResult.value("robot_grasp_pose").toObject() .value("XYZABC_mm_deg")); result.insert("source", dllResult.value("actual_source")); result.insert("score", dllResult.value("score")); result.insert("config_revision", dllResult.value("config_revision")); result.insert("run_id", dllResult.value("run_id")); result.insert("elapsed_ms", dllResult.value("elapsed_ms")); const QJsonObject capture = dllResult.value("capture").toObject(); result.insert("capture_begin", epochToIso(capture.value("begin_epoch_ms"))); result.insert("capture_end", epochToIso(capture.value("end_epoch_ms"))); return result; } void VisionService::onOperationCompleted(const QString &requestId, int code, const QJsonObject &dllResult) { if (!m_avoidanceRequestId.isEmpty() && requestId == m_avoidanceRequestId) { finishAvoidance(code, dllResult); return; } if (requestId != m_activeWorkerRequest) return; m_activeWorkerRequest.clear(); m_workerBusy = false; if (code == HVG_NOT_READY || code == HVG_DEVICE_ERROR || code == HVG_CONFIG_ERROR) m_ready = false; if (m_request.isEmpty() || m_cancelledRequests.contains(requestId)) { publishAvailability(); return; } if (m_request.value("type").toString() == "recognize_grooves") { if (code != HVG_OK) { failRequest(QString::number(code), "凹槽识别调用失败:" + dllResult.value("message").toString()); return; } if (dllResult.value("schema_version").toDouble() != 1 || !dllResult.value("success").toBool() || !dllResult.value("code").isDouble() || dllResult.value("code").toDouble() != HVG_OK || dllResult.value("request_id").toString() != requestId || !dllResult.value("vision_valid").toBool() || !dllResult.value("occupancy_valid").toBool() || !dllResult.value("grooves").isArray()) { failRequest("INVALID_RESULT", "凹槽识别结果无效,禁止放料"); return; } QJsonObject result = m_request; result.insert("status", "ok"); result.insert("occupancy_valid", true); result.insert("grooves", dllResult.value("grooves")); finishRequest(result); return; } const QJsonObject result = adaptResult(code, dllResult); if (code == HVG_OK) { QString error; if (!validateResult(result, &error)) { failRequest("INVALID_RESULT", error); return; } } finishRequest(result); } bool VisionService::validateResult(const QJsonObject &result, QString *error) const { const QDateTime begin = QDateTime::fromString(result.value("capture_begin").toString(), Qt::ISODate); const QDateTime end = QDateTime::fromString(result.value("capture_end").toString(), Qt::ISODate); const bool valid = result.value("context").toObject() == m_request.value("context").toObject() && result.value("camera_valid").toBool() && result.value("robot_valid").toBool() && validPose(result.value("camera_pose")) && validPose(result.value("robot_pose")) && result.value("source").toString() == "LEFT" && result.value("parameters").toObject() == m_request.value("parameters").toObject() && result.value("convention").toObject() == m_request.value("convention").toObject() && result.value("score").isDouble() && std::isfinite(result.value("score").toDouble()) && result.value("score").toDouble() >= 0.70 && result.value("score").toDouble() <= 1.0 && !result.value("config_revision").toString().isEmpty() && !result.value("run_id").toString().isEmpty() && begin.isValid() && end.isValid() && begin <= end && nonnegativeInteger(result.value("elapsed_ms")); if (!valid && error) *error = "视觉结果坐标、上下文、参数或有效性校验失败,禁止提交机器人"; return valid; } void VisionService::cancel(const QString &requestId) { if (requestId.isEmpty()) return; m_cancelledRequests.insert(requestId); if (m_activeWorkerRequest != requestId) return; QString error; m_worker->cancelOperation(requestId, &error); if (!error.isEmpty()) emit logMessage(error); failRequest("CANCELLED", "本次视觉结果已取消,等待 DLL 释放底层资源"); } void VisionService::setProductionActive(bool active, bool maintenanceAllowed) { m_productionActive = active; m_maintenanceAllowed = active && maintenanceAllowed; if (!m_request.isEmpty() && (m_maintenanceAllowed || (active && m_diagnostic) || (!active && !m_diagnostic))) cancel(m_request.value("request_id").toString()); publishAvailability(); } void VisionService::disconnectService() { if (m_productionActive && !m_maintenanceAllowed) { const QString message = "请先停止生产,再释放视觉资源"; emit logMessage(message); emit operationFeedback("disconnect", "error", message); return; } beginRelease("disconnect"); } void VisionService::beginRelease(const QString &operation) { m_avoidanceEnabled = false; m_avoidanceTimer->stop(); m_avoidanceTimeout->stop(); m_avoidanceLogKey.clear(); if (!m_avoidanceRequestId.isEmpty()) { m_avoidanceDiscarded = true; m_worker->cancelOperation(m_avoidanceRequestId); } if (m_releasing) { if (!operation.isEmpty()) emit operationFeedback(operation, "info", "视觉资源正在释放"); if (m_shuttingDown) QMetaObject::invokeMethod(m_worker, "release", Qt::QueuedConnection, Q_ARG(bool, true)); return; } if (!m_activeWorkerRequest.isEmpty()) cancel(m_activeWorkerRequest); else m_worker->cancelActiveOperation(); m_requestTimer->stop(); m_ready = false; m_session.clear(); m_releasing = true; m_releaseOperation = operation; publishAvailability(); QMetaObject::invokeMethod(m_worker, "release", Qt::QueuedConnection, Q_ARG(bool, m_shuttingDown)); } void VisionService::onReleased(bool success, const QString &message) { m_releasing = false; m_workerBusy = false; m_initializing = false; m_loading = false; m_activeWorkerRequest.clear(); m_request = QJsonObject(); m_ready = false; m_session.clear(); const QString operation = m_releaseOperation; m_releaseOperation.clear(); emit logMessage(message); if (!operation.isEmpty()) emit operationFeedback(operation, success ? "success" : "error", message); publishAvailability(); } void VisionService::shutdown() { if (m_shuttingDown) return; m_shuttingDown = true; if (!m_thread->isRunning()) return; beginRelease(QString()); } void VisionService::failRequest(const QString &code, const QString &message) { if (m_request.isEmpty()) return; QJsonObject result = m_request; result.insert("status", "error"); result.insert("error_code", code); result.insert("message", message); result.insert("camera_valid", false); result.insert("robot_valid", false); result.insert("occupancy_valid", false); finishRequest(result); } void VisionService::finishRequest(const QJsonObject &result) { m_requestTimer->stop(); const QString requestId = result.value("request_id").toString(); if (!requestId.isEmpty()) m_completedRequests.insert(requestId); m_request = QJsonObject(); if (result.value("type").toString() == "recognize_grooves") emit groovesRecognized(result); else emit measurementFinished(result); publishAvailability(); } void VisionService::publishAvailability() { emit availabilityChanged(isReady(), isBusy(), m_session); }