Files
ssp/services/visionservice.cpp
T
2026-11-12 09:26:26 +08:00

605 lines
24 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
#include "visionservice.h"
#include "visiondllworker.h"
#include <QDateTime>
#include <QJsonArray>
#include <QMetaObject>
#include <QThread>
#include <QTimer>
#include <QUuid>
#include <cmath>
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>("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_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);
}