Files
ssp/services/visionservice.cpp
T

540 lines
22 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 <QCoreApplication>
#include <QDateTime>
#include <QDir>
#include <QFileInfo>
#include <QJsonArray>
#include <QJsonDocument>
#include <QLocalSocket>
#include <QProcess>
#include <QProcessEnvironment>
#include <QTimer>
#include <QUuid>
#include <QtEndian>
#include <cmath>
namespace {
const quint32 MaxMessageSize = 1024 * 1024;
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();
}
}
VisionService::VisionService(const VisionConfig &config, QObject *parent)
: QObject(parent), m_config(config), m_socket(new QLocalSocket(this))
, m_process(new QProcess(this)), m_connectTimer(new QTimer(this))
, m_pollTimer(new QTimer(this)), m_requestTimer(new QTimer(this))
, m_initializeTimer(new QTimer(this)), m_ready(false), m_busy(false)
, m_productionActive(false), m_diagnostic(false), m_connecting(false)
, m_shuttingDown(false)
{
qRegisterMetaType<QJsonObject>("QJsonObject");
m_connectTimer->setInterval(200);
m_pollTimer->setInterval(1000);
m_requestTimer->setSingleShot(true);
m_initializeTimer->setSingleShot(true);
connect(m_connectTimer, &QTimer::timeout, this, &VisionService::connectAttempt);
connect(m_socket, &QLocalSocket::connected, this, [this]() {
m_connectTimer->stop();
m_buffer.clear();
m_session.clear();
m_helloId = sendCommand("hello");
m_lastReply.start();
m_pollTimer->start();
});
connect(m_socket, &QLocalSocket::readyRead, this, &VisionService::readMessages);
connect(m_socket, &QLocalSocket::disconnected, this, [this]() {
if (!m_connecting)
failConnection("视觉进程连接已断开");
});
// Qt 5.14 exposes error(...), not the newer errorOccurred socket signal.
connect(m_socket, QOverload<QLocalSocket::LocalSocketError>::of(&QLocalSocket::error),
this, [this](QLocalSocket::LocalSocketError) {
if (!m_connecting)
failConnection(m_socket->errorString());
});
connect(m_pollTimer, &QTimer::timeout, this, [this]() {
const int limit = m_session.isEmpty() ? m_config.startupTimeoutMs : 5000;
if (m_lastReply.isValid() && m_lastReply.elapsed() > limit) {
failConnection("视觉进程未回应状态查询");
return;
}
if (!m_session.isEmpty()) {
for (auto it = m_commands.constBegin(); it != m_commands.constEnd(); ++it) {
if (it.value() == "get_status")
return;
}
sendCommand("get_status");
}
});
connect(m_requestTimer, &QTimer::timeout, this, [this]() {
const QString id = m_request.value("request_id").toString();
if (!id.isEmpty()) {
m_cancelledRequests.insert(id);
sendCommand("cancel", QJsonObject{{"target_request_id", id}});
failMeasurement("CLIENT_TIMEOUT", "视觉测量及抓取计算等待超时,结果已作废");
}
});
connect(m_initializeTimer, &QTimer::timeout, this, [this]() {
failConnection("视觉初始化等待超时,请核对视觉程序状态后重新连接");
});
connect(m_process, &QProcess::errorOccurred, this, [this](QProcess::ProcessError) {
emit logMessage("视觉进程启动/运行失败:" + m_process->errorString());
if (m_process->state() == QProcess::NotRunning)
failConnection(m_process->errorString());
});
connect(m_process, QOverload<int, QProcess::ExitStatus>::of(&QProcess::finished),
this, [this](int code, QProcess::ExitStatus) {
emit logMessage(QString("视觉子进程已退出,退出码 %1").arg(code));
if (m_serverProcessId == QString::number(m_process->processId()) || m_connecting)
failConnection("视觉子进程已退出");
});
connect(m_process, &QProcess::readyReadStandardError, this, [this]() {
const QString text = QString::fromLocal8Bit(m_process->readAllStandardError()).trimmed();
if (!text.isEmpty()) emit logMessage(text.left(4096));
});
connect(m_process, &QProcess::readyReadStandardOutput, this, [this]() {
m_process->readAllStandardOutput();
});
}
VisionService::~VisionService()
{
QElapsedTimer deadline;
deadline.start();
shutdown();
// Destruction cannot rely on the GUI event loop to deliver shutdown or reap
// the child. Normal operation stays asynchronous; only teardown is bounded.
m_socket->disconnect(this);
m_process->disconnect(this);
while (m_socket->bytesToWrite() > 0 && deadline.elapsed() < 3000) {
if (!m_socket->waitForBytesWritten(int(3000 - deadline.elapsed())))
break;
}
// This QProcess only owns the child we spawned, never an attached instance.
if (m_process->state() != QProcess::NotRunning
&& !m_process->waitForFinished(qMax(0, int(3000 - deadline.elapsed())))) {
m_process->kill();
m_process->waitForFinished(1000);
}
}
QJsonObject VisionService::standardParameters()
{
return {{"camera_mode", "left_only"}, {"refinement", "least_squares_high"},
{"deadline_ms", 5000}, {"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::isReady() const { return m_ready && !m_session.isEmpty(); }
bool VisionService::isBusy() const { return m_busy || !m_request.isEmpty() || !m_initializeId.isEmpty(); }
QString VisionService::sessionId() const { return m_session; }
void VisionService::connectService()
{
if (m_connecting || m_socket->state() == QLocalSocket::ConnectedState)
return;
m_shuttingDown = false;
m_connecting = true;
m_connectElapsed.start();
m_connectTimer->start();
connectAttempt();
}
void VisionService::disconnectService()
{
if (m_productionActive) {
emit logMessage("请先停止生产,再断开视觉控制连接");
return;
}
if (!m_request.isEmpty()) cancel(m_request.value("request_id").toString());
m_connecting = false;
m_connectTimer->stop();
m_pollTimer->stop();
m_socket->disconnectFromServer();
if (m_socket->state() == QLocalSocket::UnconnectedState)
failConnection("视觉控制连接已断开,可在视觉窗口手动调试");
}
void VisionService::startService()
{
if (m_process->state() != QProcess::NotRunning || m_connecting
|| m_socket->state() == QLocalSocket::ConnectedState)
return;
const QFileInfo executable(m_config.executablePath);
if (!executable.isFile()) {
emit logMessage("请在 config.ini 配置有效的 VisionGuide executablePath");
return;
}
QProcessEnvironment environment = QProcessEnvironment::systemEnvironment();
environment.remove("QT_PLUGIN_PATH");
environment.remove("QT_QPA_PLATFORM_PLUGIN_PATH");
environment.remove("QML2_IMPORT_PATH");
environment.remove("QML_IMPORT_PATH");
QStringList cleanPath;
for (const QString &entry : environment.value("PATH").split(';', QString::SkipEmptyParts)) {
// Qt kits injected by Creator must not override the MSVC runtime beside the child.
if (!QFileInfo(QDir(entry).filePath("Qt5Core.dll")).exists()
&& !entry.contains("mingw", Qt::CaseInsensitive))
cleanPath.append(entry);
}
cleanPath.prepend(executable.absolutePath());
environment.insert("PATH", cleanPath.join(';'));
m_process->setProcessEnvironment(environment);
m_process->setWorkingDirectory(executable.absolutePath());
m_process->start(executable.absoluteFilePath(),
QStringList() << "--ipc-name" << m_config.ipcName << "--background");
connectService();
}
void VisionService::connectAttempt()
{
if (m_connectElapsed.elapsed() >= m_config.startupTimeoutMs) {
failConnection("连接视觉进程超时,请核对程序路径和 IPC 名称");
return;
}
if (m_socket->state() == QLocalSocket::UnconnectedState)
m_socket->connectToServer(m_config.ipcName);
}
QString VisionService::sendCommand(const QString &type, QJsonObject payload)
{
const QString id = newId();
payload.insert("protocol_version", 1);
payload.insert("type", type);
payload.insert("request_id", id);
payload.insert("server_session_id", m_session);
m_commands.insert(id, type);
if (!sendMessage(payload)) {
m_commands.remove(id);
return QString();
}
return id;
}
bool VisionService::sendMessage(const QJsonObject &message)
{
if (m_socket->state() != QLocalSocket::ConnectedState)
return false;
const QByteArray payload = QJsonDocument(message).toJson(QJsonDocument::Compact);
if (payload.isEmpty() || payload.size() > int(MaxMessageSize))
return false;
QByteArray frame(4, 0);
qToBigEndian<quint32>(quint32(payload.size()), reinterpret_cast<uchar *>(frame.data()));
frame.append(payload);
return m_socket->write(frame) == frame.size();
}
void VisionService::readMessages()
{
m_buffer.append(m_socket->readAll());
while (m_buffer.size() >= 4) {
const quint32 size = qFromBigEndian<quint32>(reinterpret_cast<const uchar *>(m_buffer.constData()));
if (size == 0 || size > MaxMessageSize) {
failConnection("视觉通信帧长度无效");
return;
}
if (m_buffer.size() < int(size) + 4)
return;
QJsonParseError error;
const QJsonDocument document = QJsonDocument::fromJson(m_buffer.mid(4, int(size)), &error);
m_buffer.remove(0, int(size) + 4);
if (error.error != QJsonParseError::NoError || !document.isObject()) {
failConnection("视觉通信 JSON 无效");
return;
}
handleMessage(document.object());
if (m_socket->state() != QLocalSocket::ConnectedState)
return;
}
}
void VisionService::handleMessage(const QJsonObject &message)
{
const QString type = message.value("type").toString();
const QString id = message.value("request_id").toString();
const QString session = message.value("server_session_id").toString();
const QString status = message.value("status").toString();
if (message.value("protocol_version").toDouble() != 1 || !message.value("request_id").isString()
|| session.isEmpty() || !message.value("ready").isBool()
|| !message.value("resource_busy").isBool()) {
failConnection("视觉通信版本或公共字段无效");
return;
}
if (type == "hello") {
if (id != m_helloId || m_helloId.isEmpty() || !m_session.isEmpty() || status != "ok") {
failConnection("视觉握手响应无效");
return;
}
m_session = session;
m_serverProcessId = message.value("server_process_id").toString();
m_commands.remove(id);
m_helloId.clear();
m_connecting = false;
m_lastReply.restart();
updateAvailability(message);
emit logMessage("视觉通信已连接,等待初始化/就绪");
return;
}
// The server may publish its initial state immediately after accepting the socket.
// Only hello establishes the session; an earlier status must not establish it.
if (m_session.isEmpty() && type == "status" && id.isEmpty())
return;
if (session != m_session || m_session.isEmpty()) {
failConnection("视觉进程会话已变化,本次结果不可用");
return;
}
if (type == "measure") {
if (m_request.isEmpty() || id != m_request.value("request_id").toString())
return; // Cancelled, timed-out and previous-cycle results cannot become current.
if (status == "accepted") {
m_busy = true;
publishAvailability();
return;
}
if (status != "ok" && status != "error" && status != "cancelled" && status != "timeout") {
failConnection("未知的视觉测量终态");
return;
}
m_lastReply.restart();
updateAvailability(message);
QString error;
if (status == "ok" && !validateResult(message, &error)) {
failMeasurement("INVALID_RESULT", error);
return;
}
finishMeasurement(message);
return;
}
if (type == "status") {
if (!id.isEmpty()) return;
} else {
if (!m_commands.contains(id) || m_commands.value(id) != type)
return;
if (status != "accepted") m_commands.remove(id);
}
m_lastReply.restart();
updateAvailability(message);
if (type == "initialize" && id == m_initializeId && status != "accepted") {
m_initializeTimer->stop();
m_initializeId.clear();
emit logMessage(status == "ok" ? "视觉初始化完成" : "视觉初始化失败:" + message.value("message").toString());
publishAvailability();
}
if (status == "error")
emit logMessage(message.value("error_code").toString() + ":" + message.value("message").toString());
}
void VisionService::initialize()
{
if (m_session.isEmpty() || isBusy() || m_productionActive) {
emit logMessage("初始化需要已连接、资源空闲且生产已停止");
return;
}
m_ready = false;
m_initializeId = sendCommand("initialize");
if (!m_initializeId.isEmpty()) m_initializeTimer->start(m_config.initializeTimeoutMs);
publishAvailability();
}
void VisionService::measure(const QJsonObject &request) { requestMeasurement(request, false); }
void VisionService::diagnose(const QJsonObject &request) { requestMeasurement(request, true); }
void VisionService::requestMeasurement(QJsonObject request, bool diagnostic)
{
const QString id = request.value("request_id").toString();
if (!id.isEmpty() && (m_completedRequests.contains(id)
|| (!m_request.isEmpty() && m_request.value("request_id").toString() == id))) {
emit logMessage("忽略重复的视觉请求编号:" + id);
return;
}
QString error;
const QJsonObject context = request.value("context").toObject();
const double tray = context.value("tray_no").toDouble();
if (id.isEmpty() || m_cancelledRequests.contains(id)) error = "请求编号为空或已取消";
else if (!isReady() || isBusy()) error = "视觉未就绪或仍在处理上一任务";
else if (diagnostic == m_productionActive) error = "诊断与自动生产不可同时执行";
else if (!diagnostic && request.value("server_session_id").toString() != m_session) error = "生产请求会话已失效";
else if (!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 (request.value("parameters").toObject() != standardParameters()
|| request.value("convention").toObject() != standardConvention()) error = "测量参数或机器人坐标约定不匹配";
else if (!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", "measure");
result.insert("status", "error");
result.insert("error_code", "REQUEST_REJECTED");
result.insert("message", error);
result.insert("robot_valid", false);
emit measurementFinished(result);
emit logMessage(error);
return;
}
request.insert("protocol_version", 1);
request.insert("type", "measure");
request.insert("server_session_id", m_session);
m_request = request;
m_diagnostic = diagnostic;
m_busy = true;
emit requestStarted(request);
publishAvailability();
if (!sendMessage(request)) {
failConnection("发送视觉测量请求失败");
return;
}
m_requestTimer->start(m_config.requestTimeoutMs);
}
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").isBool() && result.value("camera_valid").toBool()
&& result.value("robot_valid").isBool() && result.value("robot_valid").toBool()
&& validPose(result.value("camera_pose")) && validPose(result.value("robot_pose"))
&& result.value("source").toString() == "LEFT"
&& result.value("pose_type").toString() == "model_origin"
&& 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("calibration_fingerprint").toString().isEmpty()
&& !result.value("run_id").toString().isEmpty()
&& begin.isValid() && end.isValid() && begin <= end
&& nonnegativeInteger(result.value("measure_elapsed_ms"))
&& nonnegativeInteger(result.value("grasp_elapsed_ms"));
if (!valid && error) *error = "视觉结果坐标、上下文、参数或有效性校验失败,禁止提交机器人";
return valid;
}
void VisionService::cancel(const QString &requestId)
{
if (requestId.isEmpty()) return;
m_cancelledRequests.insert(requestId);
if (m_request.value("request_id").toString() != requestId) return;
sendCommand("cancel", QJsonObject{{"target_request_id", requestId}});
failMeasurement("CANCELLED", "本次视觉结果已取消,等待底层资源释放");
}
void VisionService::setProductionActive(bool active)
{
m_productionActive = active;
if (!m_request.isEmpty() && ((active && m_diagnostic) || (!active && !m_diagnostic)))
cancel(m_request.value("request_id").toString());
publishAvailability();
}
void VisionService::showWindow()
{
if (!m_session.isEmpty()) sendCommand("show_window");
}
void VisionService::shutdown()
{
if (m_shuttingDown) return;
m_shuttingDown = true;
if (!m_request.isEmpty()) cancel(m_request.value("request_id").toString());
if (m_process->state() != QProcess::NotRunning
&& m_serverProcessId == QString::number(m_process->processId()) && !m_session.isEmpty()) {
sendCommand("shutdown");
m_socket->flush();
QTimer::singleShot(3000, m_process, [this]() {
if (m_process->state() != QProcess::NotRunning) m_process->kill();
});
}
m_connecting = false;
m_connectTimer->stop();
m_pollTimer->stop();
m_initializeTimer->stop();
m_socket->disconnectFromServer();
m_ready = false;
m_session.clear();
publishAvailability();
}
void VisionService::failMeasurement(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("resource_busy", m_busy);
finishMeasurement(result);
}
void VisionService::finishMeasurement(const QJsonObject &result)
{
m_requestTimer->stop();
m_completedRequests.insert(result.value("request_id").toString());
m_request = QJsonObject();
emit measurementFinished(result);
publishAvailability();
}
void VisionService::failConnection(const QString &message)
{
const bool hadConnection = m_connecting || !m_session.isEmpty() || !m_request.isEmpty();
m_connecting = false;
m_connectTimer->stop();
m_pollTimer->stop();
m_initializeTimer->stop();
m_ready = false;
m_busy = false;
m_initializeId.clear();
m_commands.clear();
m_session.clear();
m_serverProcessId.clear();
m_buffer.clear();
failMeasurement("DISCONNECTED", message);
m_socket->abort();
publishAvailability();
if (hadConnection && !m_shuttingDown) emit logMessage(message);
}
void VisionService::updateAvailability(const QJsonObject &message)
{
m_ready = message.value("ready").toBool();
m_busy = message.value("resource_busy").toBool();
emit statusChanged(message);
publishAvailability();
}
void VisionService::publishAvailability()
{
emit availabilityChanged(isReady(), isBusy(), m_session);
}