Files
VisionGuide/source/OpcUaComm.cpp
T
2026-10-26 10:56:56 +08:00

578 lines
22 KiB
C++

// ════════════════════════════════════════════════════════════════════════════
// OpcUaComm.cpp — OPC UA 通信模块实现 (基于 open62541)
//
// 编译要求:
// 1. 下载 open62541 amalgamated: open62541.h + open62541.c
// https://github.com/open62541/open62541/releases
// 选择 v1.3.x 的 open62541.c / open62541.h
// 2. 将 open62541.c 添加到项目源文件
// 3. Windows: 链接 ws2_32.lib (在.pro中: LIBS += -lws2_32)
// 4. 在 .pro 中: DEFINES += UA_ARCHITECTURE_WIN32
// ════════════════════════════════════════════════════════════════════════════
#include "OpcUaComm.h"
// open62541 头文件
#include "open62541.h"
#include <QDebug>
#include <QDateTime>
#include <QMutex>
// ════════════════════════════════════════════════════════════════════════════
// OpcUaServer::Impl
// ════════════════════════════════════════════════════════════════════════════
struct OpcUaServer::Impl
{
UA_Server* server = nullptr;
bool running = false;
QMutex mutex;
// 当前状态 (线程安全用 mutex 保护)
VisionSystemState sysState = VisionSystemState::Idle;
MeasureResultCode measResult = MeasureResultCode::None;
PoseData pose;
bool poseValid = false;
QString errorMsg;
// 节点ID缓存
UA_NodeId nodeSystemState;
UA_NodeId nodeMeasResult;
UA_NodeId nodePoseValid;
UA_NodeId nodeErrorMsg;
UA_NodeId nodePoseX, nodePoseY, nodePoseZ;
UA_NodeId nodePoseRx, nodePoseRy, nodePoseRz;
UA_NodeId nodePartType;
// 回调上下文 (指向 OpcUaServer)
OpcUaServer* owner = nullptr;
};
// ── Method 回调: StartMeasurement ────────────────────────────────────
static UA_StatusCode
startMeasurementCallback(UA_Server* /*server*/,
const UA_NodeId* /*sessionId*/, void* /*sessionContext*/,
const UA_NodeId* /*methodId*/, void* methodContext,
const UA_NodeId* /*objectId*/, void* /*objectContext*/,
size_t inputSize, const UA_Variant* input,
size_t /*outputSize*/, UA_Variant* output)
{
auto* srv = static_cast<OpcUaServer*>(methodContext);
if (!srv) return UA_STATUSCODE_BADINTERNALERROR;
uint32_t timeoutMs = 30000;
if (inputSize > 0 && UA_Variant_hasScalarType(&input[0], &UA_TYPES[UA_TYPES_UINT32])) {
timeoutMs = *static_cast<UA_UInt32*>(input[0].data);
}
const uint32_t statusCode = srv->handleStartMeasurementCommand(timeoutMs);
// 设置输出
UA_UInt32 sc = statusCode;
UA_Variant_setScalarCopy(&output[0], &sc, &UA_TYPES[UA_TYPES_UINT32]);
return UA_STATUSCODE_GOOD;
}
// ── Method 回调: ResetSystem ─────────────────────────────────────────
static UA_StatusCode
resetSystemCallback(UA_Server* /*server*/,
const UA_NodeId* /*sessionId*/, void* /*sessionContext*/,
const UA_NodeId* /*methodId*/, void* methodContext,
const UA_NodeId* /*objectId*/, void* /*objectContext*/,
size_t /*inputSize*/, const UA_Variant* /*input*/,
size_t /*outputSize*/, UA_Variant* output)
{
auto* srv = static_cast<OpcUaServer*>(methodContext);
if (!srv) return UA_STATUSCODE_BADINTERNALERROR;
UA_UInt32 sc = 0;
UA_Variant_setScalarCopy(&output[0], &sc, &UA_TYPES[UA_TYPES_UINT32]);
srv->handleResetCommand();
return UA_STATUSCODE_GOOD;
}
// ════════════════════════════════════════════════════════════════════════════
// OpcUaServer
// ════════════════════════════════════════════════════════════════════════════
OpcUaServer::OpcUaServer(QObject* parent)
: QObject(parent), d(new Impl)
{
d->owner = this;
m_timer = new QTimer(this);
m_timer->setInterval(10); // 10ms 迭代
connect(m_timer, &QTimer::timeout, this, &OpcUaServer::iterate);
}
OpcUaServer::~OpcUaServer()
{
stop();
delete d;
}
bool OpcUaServer::isRunning() const { return d->running; }
VisionSystemState OpcUaServer::systemState() const
{
QMutexLocker lock(&d->mutex);
return d->sysState;
}
uint32_t OpcUaServer::handleStartMeasurementCommand(uint32_t timeoutMs)
{
uint32_t statusCode = static_cast<uint32_t>(CmdStatusCode::OK);
{
QMutexLocker lock(&d->mutex);
if (d->sysState == VisionSystemState::Measuring)
statusCode = static_cast<uint32_t>(CmdStatusCode::Busy);
}
if (statusCode == static_cast<uint32_t>(CmdStatusCode::OK)) {
emit logMessage(QString::fromUtf8("[OPC UA] 收到StartMeasurement指令, timeout=%1ms").arg(timeoutMs));
emit startMeasurementRequested(timeoutMs);
}
return statusCode;
}
void OpcUaServer::handleResetCommand()
{
emit logMessage(QString::fromUtf8("[OPC UA] 收到ResetSystem指令"));
emit resetSystemRequested();
}
bool OpcUaServer::start(uint16_t port)
{
if (d->running) return true;
d->server = UA_Server_new();
UA_ServerConfig* config = UA_Server_getConfig(d->server);
UA_ServerConfig_setMinimal(config, port, nullptr);
// 设置服务端名称
config->applicationDescription.applicationName = UA_LOCALIZEDTEXT_ALLOC("zh-CN", "DualVisionSystem");
config->applicationDescription.applicationUri = UA_STRING_ALLOC("urn:DualVision:Server");
createNodes();
UA_StatusCode rc = UA_Server_run_startup(d->server);
if (rc != UA_STATUSCODE_GOOD) {
emit logMessage(QString::fromUtf8("[OPC UA Server] 启动失败: 0x%1").arg(rc, 8, 16, QChar('0')));
UA_Server_delete(d->server);
d->server = nullptr;
return false;
}
d->running = true;
m_timer->start();
emit logMessage(QString::fromUtf8("[OPC UA Server] 已启动, 端口: %1").arg(port));
return true;
}
void OpcUaServer::stop()
{
if (!d->running) return;
m_timer->stop();
d->running = false;
if (d->server) {
UA_Server_run_shutdown(d->server);
UA_Server_delete(d->server);
d->server = nullptr;
}
emit logMessage(QString::fromUtf8("[OPC UA Server] 已停止"));
}
void OpcUaServer::iterate()
{
if (d->server && d->running) {
UA_Server_run_iterate(d->server, false);
}
}
// ── 创建节点树 ──────────────────────────────────────────────────────
void OpcUaServer::createNodes()
{
if (!d->server) return;
// 创建文件夹: Objects/DualVision
UA_NodeId folderId = UA_NODEID_STRING_ALLOC(1, "DualVision");
UA_ObjectAttributes folderAttr = UA_ObjectAttributes_default;
folderAttr.displayName = UA_LOCALIZEDTEXT_ALLOC("", "DualVision");
UA_Server_addObjectNode(d->server, folderId,
UA_NODEID_NUMERIC(0, UA_NS0ID_OBJECTSFOLDER),
UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES),
UA_QUALIFIEDNAME_ALLOC(1, "DualVision"),
UA_NODEID_NUMERIC(0, UA_NS0ID_BASEOBJECTTYPE),
folderAttr, nullptr, nullptr);
// ── 变量节点 ──
auto addUInt32 = [&](const char* name, uint32_t initVal, UA_NodeId* outId) {
*outId = UA_NODEID_STRING_ALLOC(1, name);
UA_VariableAttributes attr = UA_VariableAttributes_default;
attr.displayName = UA_LOCALIZEDTEXT_ALLOC("", name);
attr.accessLevel = UA_ACCESSLEVELMASK_READ;
UA_UInt32 val = initVal;
UA_Variant_setScalar(&attr.value, &val, &UA_TYPES[UA_TYPES_UINT32]);
UA_Server_addVariableNode(d->server, *outId, folderId,
UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
UA_QUALIFIEDNAME_ALLOC(1, name),
UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
attr, nullptr, nullptr);
};
auto addFloat = [&](const char* name, float initVal, UA_NodeId parentId, UA_NodeId* outId) {
*outId = UA_NODEID_STRING_ALLOC(1, name);
UA_VariableAttributes attr = UA_VariableAttributes_default;
attr.displayName = UA_LOCALIZEDTEXT_ALLOC("", name);
attr.accessLevel = UA_ACCESSLEVELMASK_READ;
UA_Float val = initVal;
UA_Variant_setScalar(&attr.value, &val, &UA_TYPES[UA_TYPES_FLOAT]);
UA_Server_addVariableNode(d->server, *outId, parentId,
UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
UA_QUALIFIEDNAME_ALLOC(1, name),
UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
attr, nullptr, nullptr);
};
addUInt32("SystemState", 0, &d->nodeSystemState);
addUInt32("MeasurementResult", 0, &d->nodeMeasResult);
// PoseValid (Boolean)
{
d->nodePoseValid = UA_NODEID_STRING_ALLOC(1, "PoseValid");
UA_VariableAttributes attr = UA_VariableAttributes_default;
attr.displayName = UA_LOCALIZEDTEXT_ALLOC("", "PoseValid");
attr.accessLevel = UA_ACCESSLEVELMASK_READ;
UA_Boolean val = false;
UA_Variant_setScalar(&attr.value, &val, &UA_TYPES[UA_TYPES_BOOLEAN]);
UA_Server_addVariableNode(d->server, d->nodePoseValid, folderId,
UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
UA_QUALIFIEDNAME_ALLOC(1, "PoseValid"),
UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
attr, nullptr, nullptr);
}
// ErrorMessage (String)
{
d->nodeErrorMsg = UA_NODEID_STRING_ALLOC(1, "ErrorMessage");
UA_VariableAttributes attr = UA_VariableAttributes_default;
attr.displayName = UA_LOCALIZEDTEXT_ALLOC("", "ErrorMessage");
attr.accessLevel = UA_ACCESSLEVELMASK_READ;
UA_String val = UA_STRING_ALLOC("");
UA_Variant_setScalar(&attr.value, &val, &UA_TYPES[UA_TYPES_STRING]);
UA_Server_addVariableNode(d->server, d->nodeErrorMsg, folderId,
UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
UA_QUALIFIEDNAME_ALLOC(1, "ErrorMessage"),
UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE),
attr, nullptr, nullptr);
}
// TargetPose 文件夹
UA_NodeId poseFolderId = UA_NODEID_STRING_ALLOC(1, "TargetPose");
{
UA_ObjectAttributes pfa = UA_ObjectAttributes_default;
pfa.displayName = UA_LOCALIZEDTEXT_ALLOC("", "TargetPose");
UA_Server_addObjectNode(d->server, poseFolderId, folderId,
UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
UA_QUALIFIEDNAME_ALLOC(1, "TargetPose"),
UA_NODEID_NUMERIC(0, UA_NS0ID_BASEOBJECTTYPE),
pfa, nullptr, nullptr);
}
addFloat("Pose_X", 0, poseFolderId, &d->nodePoseX);
addFloat("Pose_Y", 0, poseFolderId, &d->nodePoseY);
addFloat("Pose_Z", 0, poseFolderId, &d->nodePoseZ);
addFloat("Pose_Rx", 0, poseFolderId, &d->nodePoseRx);
addFloat("Pose_Ry", 0, poseFolderId, &d->nodePoseRy);
addFloat("Pose_Rz", 0, poseFolderId, &d->nodePoseRz);
addUInt32("Pose_PartType", 0, &d->nodePartType);
// 把 PartType 移到 TargetPose 下 (重新添加)
// (简化处理: PartType 放在 DualVision 下也可以)
// ── Method: StartMeasurement ──
{
UA_Argument inputArg;
UA_Argument_init(&inputArg);
inputArg.name = UA_STRING_ALLOC("timeoutMs");
inputArg.dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
inputArg.valueRank = UA_VALUERANK_SCALAR;
UA_Argument outputArg;
UA_Argument_init(&outputArg);
outputArg.name = UA_STRING_ALLOC("statusCode");
outputArg.dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
outputArg.valueRank = UA_VALUERANK_SCALAR;
UA_MethodAttributes mAttr = UA_MethodAttributes_default;
mAttr.displayName = UA_LOCALIZEDTEXT_ALLOC("", "StartMeasurement");
mAttr.executable = true;
mAttr.userExecutable = true;
UA_Server_addMethodNode(d->server,
UA_NODEID_STRING_ALLOC(1, "StartMeasurement"),
folderId,
UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
UA_QUALIFIEDNAME_ALLOC(1, "StartMeasurement"),
mAttr, &startMeasurementCallback,
1, &inputArg, 1, &outputArg,
this, nullptr);
}
// ── Method: ResetSystem ──
{
UA_Argument outputArg;
UA_Argument_init(&outputArg);
outputArg.name = UA_STRING_ALLOC("statusCode");
outputArg.dataType = UA_TYPES[UA_TYPES_UINT32].typeId;
outputArg.valueRank = UA_VALUERANK_SCALAR;
UA_MethodAttributes mAttr = UA_MethodAttributes_default;
mAttr.displayName = UA_LOCALIZEDTEXT_ALLOC("", "ResetSystem");
mAttr.executable = true;
mAttr.userExecutable = true;
UA_Server_addMethodNode(d->server,
UA_NODEID_STRING_ALLOC(1, "ResetSystem"),
folderId,
UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT),
UA_QUALIFIEDNAME_ALLOC(1, "ResetSystem"),
mAttr, &resetSystemCallback,
0, nullptr, 1, &outputArg,
this, nullptr);
}
}
// ── 更新节点值 ──────────────────────────────────────────────────────
void OpcUaServer::setSystemState(VisionSystemState s)
{
QMutexLocker lock(&d->mutex);
d->sysState = s;
if (!d->server) return;
UA_UInt32 val = static_cast<uint32_t>(s);
UA_Variant v; UA_Variant_setScalar(&v, &val, &UA_TYPES[UA_TYPES_UINT32]);
UA_Server_writeValue(d->server, d->nodeSystemState, v);
}
void OpcUaServer::setMeasureResult(MeasureResultCode c)
{
QMutexLocker lock(&d->mutex);
d->measResult = c;
if (!d->server) return;
UA_UInt32 val = static_cast<uint32_t>(c);
UA_Variant v; UA_Variant_setScalar(&v, &val, &UA_TYPES[UA_TYPES_UINT32]);
UA_Server_writeValue(d->server, d->nodeMeasResult, v);
}
void OpcUaServer::setPoseData(const PoseData& p)
{
QMutexLocker lock(&d->mutex);
d->pose = p;
if (!d->server) return;
auto writeF = [&](UA_NodeId id, float val) {
UA_Float f = val;
UA_Variant v; UA_Variant_setScalar(&v, &f, &UA_TYPES[UA_TYPES_FLOAT]);
UA_Server_writeValue(d->server, id, v);
};
writeF(d->nodePoseX, p.X);
writeF(d->nodePoseY, p.Y);
writeF(d->nodePoseZ, p.Z);
writeF(d->nodePoseRx, p.Rx);
writeF(d->nodePoseRy, p.Ry);
writeF(d->nodePoseRz, p.Rz);
UA_UInt32 pt = p.partType;
UA_Variant pv; UA_Variant_setScalar(&pv, &pt, &UA_TYPES[UA_TYPES_UINT32]);
UA_Server_writeValue(d->server, d->nodePartType, pv);
}
void OpcUaServer::setPoseValid(bool v)
{
QMutexLocker lock(&d->mutex);
d->poseValid = v;
if (!d->server) return;
UA_Boolean val = v;
UA_Variant var; UA_Variant_setScalar(&var, &val, &UA_TYPES[UA_TYPES_BOOLEAN]);
UA_Server_writeValue(d->server, d->nodePoseValid, var);
}
void OpcUaServer::setErrorMessage(const QString& msg)
{
QMutexLocker lock(&d->mutex);
d->errorMsg = msg;
if (!d->server) return;
QByteArray utf8 = msg.toUtf8();
UA_String val = UA_STRING_ALLOC(utf8.constData());
UA_Variant v; UA_Variant_setScalar(&v, &val, &UA_TYPES[UA_TYPES_STRING]);
UA_Server_writeValue(d->server, d->nodeErrorMsg, v);
UA_String_clear(&val);
}
// ════════════════════════════════════════════════════════════════════════════
// OpcUaClient::Impl
// ════════════════════════════════════════════════════════════════════════════
struct OpcUaClient::Impl
{
UA_Client* client = nullptr;
bool connected = false;
// 机械臂位姿节点ID
QString nodeX = "ns=2;s=RobotPose.X";
QString nodeY = "ns=2;s=RobotPose.Y";
QString nodeZ = "ns=2;s=RobotPose.Z";
QString nodeRx = "ns=2;s=RobotPose.Rx";
QString nodeRy = "ns=2;s=RobotPose.Ry";
QString nodeRz = "ns=2;s=RobotPose.Rz";
};
OpcUaClient::OpcUaClient(QObject* parent)
: QObject(parent), d(new Impl)
{
}
OpcUaClient::~OpcUaClient()
{
disconnect();
delete d;
}
bool OpcUaClient::isConnected() const { return d->connected; }
void OpcUaClient::setRobotPoseNodeIds(const QString& x, const QString& y, const QString& z,
const QString& rx, const QString& ry, const QString& rz)
{
d->nodeX = x; d->nodeY = y; d->nodeZ = z;
d->nodeRx = rx; d->nodeRy = ry; d->nodeRz = rz;
}
bool OpcUaClient::connectToServer(const QString& url)
{
if (d->connected) disconnect();
d->client = UA_Client_new();
UA_ClientConfig_setDefault(UA_Client_getConfig(d->client));
QByteArray urlUtf8 = url.toUtf8();
UA_StatusCode rc = UA_Client_connect(d->client, urlUtf8.constData());
if (rc != UA_STATUSCODE_GOOD) {
emit logMessage(QString::fromUtf8("[OPC UA Client] 连接失败: %1 (0x%2)")
.arg(url).arg(rc, 8, 16, QChar('0')));
UA_Client_delete(d->client);
d->client = nullptr;
return false;
}
d->connected = true;
emit logMessage(QString::fromUtf8("[OPC UA Client] 已连接: %1").arg(url));
return true;
}
void OpcUaClient::disconnect()
{
if (d->client) {
if (d->connected) UA_Client_disconnect(d->client);
UA_Client_delete(d->client);
d->client = nullptr;
}
d->connected = false;
}
bool OpcUaClient::readRobotPose(PoseData& out)
{
if (!d->connected || !d->client) return false;
auto readFloat = [&](const QString& nodeIdStr, float& val) -> bool {
QByteArray ba = nodeIdStr.toUtf8();
UA_NodeId nodeId = UA_NODEID_STRING_ALLOC(2, ba.constData());
// 尝试解析 "ns=X;s=xxx" 格式
// 简化: 直接用 UA_NodeId_parse
UA_Variant v;
UA_Variant_init(&v);
UA_StatusCode rc = UA_Client_readValueAttribute(d->client, nodeId, &v);
if (rc != UA_STATUSCODE_GOOD) return false;
if (UA_Variant_hasScalarType(&v, &UA_TYPES[UA_TYPES_FLOAT])) {
val = *static_cast<UA_Float*>(v.data);
} else if (UA_Variant_hasScalarType(&v, &UA_TYPES[UA_TYPES_DOUBLE])) {
val = static_cast<float>(*static_cast<UA_Double*>(v.data));
} else {
UA_Variant_clear(&v);
return false;
}
UA_Variant_clear(&v);
return true;
};
out.clear();
bool ok = true;
ok &= readFloat(d->nodeX, out.X);
ok &= readFloat(d->nodeY, out.Y);
ok &= readFloat(d->nodeZ, out.Z);
ok &= readFloat(d->nodeRx, out.Rx);
ok &= readFloat(d->nodeRy, out.Ry);
ok &= readFloat(d->nodeRz, out.Rz);
out.valid = ok;
if (ok) {
emit logMessage(QString::fromUtf8("[OPC UA Client] 读取机械臂位姿: X=%1 Y=%2 Z=%3 Rx=%4 Ry=%5 Rz=%6")
.arg(out.X,0,'f',3).arg(out.Y,0,'f',3).arg(out.Z,0,'f',3)
.arg(out.Rx,0,'f',2).arg(out.Ry,0,'f',2).arg(out.Rz,0,'f',2));
} else {
emit logMessage(QString::fromUtf8("[OPC UA Client] 读取机械臂位姿失败"));
}
return ok;
}
// ════════════════════════════════════════════════════════════════════════════
// OpcUaCommManager
// ════════════════════════════════════════════════════════════════════════════
OpcUaCommManager::OpcUaCommManager(QObject* parent)
: QObject(parent)
, m_server(new OpcUaServer(this))
, m_client(new OpcUaClient(this))
{
connect(m_server, &OpcUaServer::startMeasurementRequested,
this, &OpcUaCommManager::measurementRequested);
connect(m_server, &OpcUaServer::resetSystemRequested,
this, &OpcUaCommManager::resetRequested);
connect(m_server, &OpcUaServer::logMessage, this, &OpcUaCommManager::logMessage);
connect(m_client, &OpcUaClient::logMessage, this, &OpcUaCommManager::logMessage);
}
OpcUaCommManager::~OpcUaCommManager()
{
stopServer();
disconnectFromRobot();
}
bool OpcUaCommManager::startServer(uint16_t port) { return m_server->start(port); }
void OpcUaCommManager::stopServer() { m_server->stop(); }
bool OpcUaCommManager::connectToRobot(const QString& url) { return m_client->connectToServer(url); }
void OpcUaCommManager::disconnectFromRobot() { m_client->disconnect(); }
void OpcUaCommManager::publishMeasureResult(MeasureResultCode code,
const PoseData& pose,
const QString& errMsg)
{
if (code == MeasureResultCode::Success) {
m_server->setSystemState(VisionSystemState::MeasureSuccess);
m_server->setPoseData(pose);
m_server->setPoseValid(true);
} else {
m_server->setSystemState(VisionSystemState::MeasureFailed);
m_server->setPoseValid(false);
}
m_server->setMeasureResult(code);
m_server->setErrorMessage(errMsg);
emit logMessage(QString::fromUtf8("[OPC UA] 已发布测量结果: code=%1 valid=%2")
.arg(static_cast<int>(code)).arg(code == MeasureResultCode::Success));
}
void OpcUaCommManager::resetToIdle()
{
m_server->setSystemState(VisionSystemState::Idle);
m_server->setMeasureResult(MeasureResultCode::None);
m_server->setPoseValid(false);
m_server->setErrorMessage("");
emit logMessage(QString::fromUtf8("[OPC UA] 系统已重置为Idle"));
}