665 lines
24 KiB
C++
665 lines
24 KiB
C++
#include "DroneScrewZmqProtocol.h"
|
||
#include "VrLog.h"
|
||
#include "VrError.h"
|
||
#include "IWDRemoteReceiver.h"
|
||
#include "AuthManager.h"
|
||
|
||
// 复用 WDRemoteReceiver 头部协议（48 字节）
|
||
#include "IWDRemoteReceiver.h"
|
||
|
||
#include <QJsonObject>
|
||
#include <QJsonArray>
|
||
#include <QJsonDocument>
|
||
#include <QHostAddress>
|
||
#include <QUdpSocket>
|
||
|
||
#include <algorithm>
|
||
#include <cstring>
|
||
#include <exception>
|
||
#include <mutex>
|
||
#include <new>
|
||
#include <vector>
|
||
|
||
namespace
|
||
{
|
||
void fillDetectionJson(const DroneScrewResult& result, QJsonObject& obj)
|
||
{
|
||
obj["frameId"] = static_cast<qint64>(result.frameId);
|
||
obj["ts"] = static_cast<qint64>(result.timestampUs);
|
||
obj["width"] = result.imageWidth;
|
||
obj["height"] = result.imageHeight;
|
||
obj["success"] = result.success;
|
||
obj["errorCode"] = result.errorCode;
|
||
obj["msg"] = QString::fromStdString(result.message);
|
||
|
||
QJsonArray boxes;
|
||
for (const auto& b : result.boxes)
|
||
{
|
||
QJsonObject jb;
|
||
jb["cls"] = b.classId;
|
||
jb["score"] = b.score;
|
||
jb["x"] = b.x;
|
||
jb["y"] = b.y;
|
||
jb["w"] = b.width;
|
||
jb["h"] = b.height;
|
||
boxes.append(jb);
|
||
}
|
||
obj["boxes"] = boxes;
|
||
|
||
QJsonArray distances;
|
||
for (const auto& d : result.distances)
|
||
{
|
||
QJsonObject jd;
|
||
jd["fromId"] = d.fromId;
|
||
jd["toId"] = d.toId;
|
||
jd["distanceMm"] = static_cast<double>(d.distanceMm);
|
||
distances.append(jd);
|
||
}
|
||
obj["distances"] = distances;
|
||
}
|
||
|
||
void setErrorResp(QJsonObject& resp, int code, const QString& msg)
|
||
{
|
||
resp["ok"] = false;
|
||
resp["code"] = code;
|
||
resp["msg"] = msg;
|
||
}
|
||
|
||
void appendObject(QJsonObject& dst, const QJsonObject& src)
|
||
{
|
||
for (auto it = src.constBegin(); it != src.constEnd(); ++it)
|
||
dst[it.key()] = it.value();
|
||
}
|
||
|
||
QString localMachineCode()
|
||
{
|
||
static QString code = QString::fromStdString(AuthManager::GetMachineCode());
|
||
return code;
|
||
}
|
||
|
||
void fillServerInfoJson(const QString& cmd,
|
||
const QString& rtspUrl,
|
||
int controlPort,
|
||
int resultPort,
|
||
int rawImagePort,
|
||
const QJsonObject& req,
|
||
QJsonObject& resp)
|
||
{
|
||
resp["ok"] = true;
|
||
resp["code"] = 0;
|
||
resp["cmd"] = cmd;
|
||
resp["machineCode"] = localMachineCode();
|
||
resp["clientMachineCode"] = req["machineCode"].toString();
|
||
resp["deviceName"] = "DroneScrewServer";
|
||
resp["rtsp"] = rtspUrl;
|
||
resp["controlPort"] = controlPort;
|
||
resp["resultPort"] = resultPort;
|
||
resp["rawImagePort"] = rawImagePort;
|
||
}
|
||
|
||
int fallbackHandlerError()
|
||
{
|
||
return ERR_CODE(DRONESCREW_ERR_ZMQ_HANDLER_NOT_SET);
|
||
}
|
||
}
|
||
|
||
DroneScrewZmqProtocol::DroneScrewZmqProtocol(QObject* parent)
|
||
: QObject(parent)
|
||
{
|
||
}
|
||
|
||
DroneScrewZmqProtocol::~DroneScrewZmqProtocol()
|
||
{
|
||
stopServer();
|
||
}
|
||
|
||
bool DroneScrewZmqProtocol::startServer(int controlPort, int resultPort, int rawImagePort)
|
||
{
|
||
if (m_bRunning.load()) return true;
|
||
|
||
m_controlPort = controlPort;
|
||
m_resultPort = resultPort;
|
||
m_rawImagePort = rawImagePort;
|
||
|
||
// 1) REP 控制
|
||
if (!IVrZeroMQServer::CreateObject(&m_pZmqServer) || !m_pZmqServer)
|
||
{
|
||
LOG_ERROR("ZmqServer create fail\n");
|
||
return false;
|
||
}
|
||
|
||
auto onEvent = [](MQEvent ev) {
|
||
LOG_DEBUG("Zmq Event: %d\n", (int)ev);
|
||
};
|
||
auto onRecv = [this](const char* data, const size_t len) -> std::string {
|
||
QByteArray req(data, static_cast<int>(len));
|
||
QByteArray resp = this->handleControlMessage(req);
|
||
return std::string(resp.constData(), static_cast<size_t>(resp.size()));
|
||
};
|
||
|
||
int r = m_pZmqServer->Init(controlPort, onEvent, onRecv, false);
|
||
if (r != 0)
|
||
{
|
||
LOG_ERROR("ZmqServer Init fail: %d\n", r);
|
||
stopServer();
|
||
return false;
|
||
}
|
||
|
||
// 2) PUB 检测结果 (topic="result")
|
||
if (!IVrZeroMQPublisher::CreateObject(&m_pZmqPublisher) || !m_pZmqPublisher)
|
||
{
|
||
LOG_ERROR("ZmqPublisher create fail\n");
|
||
stopServer();
|
||
return false;
|
||
}
|
||
if (m_pZmqPublisher->Init(resultPort) != 0)
|
||
{
|
||
LOG_ERROR("ZmqPublisher init fail\n");
|
||
stopServer();
|
||
return false;
|
||
}
|
||
|
||
// 3) PUB 原始图像 (topic="raw")，可选
|
||
if (rawImagePort > 0)
|
||
{
|
||
if (!IVrZeroMQPublisher::CreateObject(&m_pZmqRawPublisher) || !m_pZmqRawPublisher)
|
||
{
|
||
LOG_ERROR("ZmqRawPublisher create fail\n");
|
||
stopServer();
|
||
return false;
|
||
}
|
||
if (m_pZmqRawPublisher->Init(rawImagePort) != 0)
|
||
{
|
||
LOG_ERROR("ZmqRawPublisher init fail\n");
|
||
stopServer();
|
||
return false;
|
||
}
|
||
}
|
||
|
||
if (!startDiscoveryServer())
|
||
{
|
||
LOG_ERROR("[UDP-DISCOVER] start fail port=%u\n",
|
||
static_cast<unsigned>(WD_REMOTE_DISCOVERY_PORT));
|
||
stopServer();
|
||
return false;
|
||
}
|
||
|
||
m_bRunning = true;
|
||
LOG_INFO("ZMQ control=%d, result=%d, rawImage=%d, discoverUdp=%u\n",
|
||
controlPort, resultPort, rawImagePort,
|
||
static_cast<unsigned>(WD_REMOTE_DISCOVERY_PORT));
|
||
return true;
|
||
}
|
||
|
||
void DroneScrewZmqProtocol::stopServer()
|
||
{
|
||
stopDiscoveryServer();
|
||
if (m_pZmqServer) { m_pZmqServer->UnInit(); delete m_pZmqServer; m_pZmqServer = nullptr; }
|
||
if (m_pZmqPublisher) { m_pZmqPublisher->UnInit(); delete m_pZmqPublisher; m_pZmqPublisher = nullptr; }
|
||
if (m_pZmqRawPublisher) { m_pZmqRawPublisher->UnInit(); delete m_pZmqRawPublisher; m_pZmqRawPublisher = nullptr; }
|
||
m_bRunning = false;
|
||
}
|
||
|
||
bool DroneScrewZmqProtocol::startDiscoveryServer()
|
||
{
|
||
if (m_pDiscoverySocket) return true;
|
||
|
||
m_pDiscoverySocket = new QUdpSocket(this);
|
||
const bool ok = m_pDiscoverySocket->bind(
|
||
QHostAddress::AnyIPv4,
|
||
static_cast<quint16>(WD_REMOTE_DISCOVERY_PORT),
|
||
QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint);
|
||
if (!ok)
|
||
{
|
||
LOG_ERROR("[UDP-DISCOVER] bind failed port=%u err=%s\n",
|
||
static_cast<unsigned>(WD_REMOTE_DISCOVERY_PORT),
|
||
m_pDiscoverySocket->errorString().toStdString().c_str());
|
||
delete m_pDiscoverySocket;
|
||
m_pDiscoverySocket = nullptr;
|
||
return false;
|
||
}
|
||
|
||
connect(m_pDiscoverySocket, &QUdpSocket::readyRead,
|
||
this, &DroneScrewZmqProtocol::handleDiscoveryDatagrams);
|
||
LOG_INFO("[UDP-DISCOVER] listen port=%u\n",
|
||
static_cast<unsigned>(WD_REMOTE_DISCOVERY_PORT));
|
||
return true;
|
||
}
|
||
|
||
void DroneScrewZmqProtocol::stopDiscoveryServer()
|
||
{
|
||
if (!m_pDiscoverySocket) return;
|
||
m_pDiscoverySocket->close();
|
||
delete m_pDiscoverySocket;
|
||
m_pDiscoverySocket = nullptr;
|
||
}
|
||
|
||
void DroneScrewZmqProtocol::handleDiscoveryDatagrams()
|
||
{
|
||
while (m_pDiscoverySocket && m_pDiscoverySocket->hasPendingDatagrams())
|
||
{
|
||
QByteArray req;
|
||
req.resize(static_cast<int>(m_pDiscoverySocket->pendingDatagramSize()));
|
||
|
||
QHostAddress sender;
|
||
quint16 senderPort = 0;
|
||
const qint64 readBytes =
|
||
m_pDiscoverySocket->readDatagram(req.data(), req.size(),
|
||
&sender, &senderPort);
|
||
if (readBytes <= 0)
|
||
continue;
|
||
req.resize(static_cast<int>(readBytes));
|
||
|
||
QJsonParseError err;
|
||
const QJsonDocument doc = QJsonDocument::fromJson(req, &err);
|
||
if (err.error != QJsonParseError::NoError || !doc.isObject())
|
||
{
|
||
LOG_DEBUG("[UDP-DISCOVER] ignore invalid json from=%s:%u err=%s\n",
|
||
sender.toString().toStdString().c_str(),
|
||
static_cast<unsigned>(senderPort),
|
||
err.errorString().toStdString().c_str());
|
||
continue;
|
||
}
|
||
|
||
const QJsonObject obj = doc.object();
|
||
const QString cmd = obj["cmd"].toString();
|
||
if (cmd != "discover")
|
||
continue;
|
||
|
||
QJsonObject resp;
|
||
fillServerInfoJson("discover", m_rtspUrl, m_controlPort, m_resultPort,
|
||
m_rawImagePort, obj, resp);
|
||
if (m_serverInfoProvider)
|
||
appendObject(resp, m_serverInfoProvider());
|
||
|
||
const QByteArray payload =
|
||
QJsonDocument(resp).toJson(QJsonDocument::Compact);
|
||
const qint64 sent =
|
||
m_pDiscoverySocket->writeDatagram(payload, sender, senderPort);
|
||
LOG_DEBUG("[UDP-DISCOVER] reply %s:%u bytes=%lld reqMachine=%s\n",
|
||
sender.toString().toStdString().c_str(),
|
||
static_cast<unsigned>(senderPort),
|
||
static_cast<long long>(sent),
|
||
obj["machineCode"].toString().toStdString().c_str());
|
||
}
|
||
}
|
||
|
||
QByteArray DroneScrewZmqProtocol::handleControlMessage(const QByteArray& reqData)
|
||
{
|
||
LOG_DEBUG("[ZMQ-REP] recv: %s\n", reqData.constData());
|
||
|
||
QJsonParseError err;
|
||
QJsonDocument doc = QJsonDocument::fromJson(reqData, &err);
|
||
if (err.error != QJsonParseError::NoError || !doc.isObject())
|
||
{
|
||
LOG_DEBUG("[ZMQ-REP] parse error: %s\n", err.errorString().toStdString().c_str());
|
||
QJsonObject o;
|
||
setErrorResp(o, ERR_CODE(DRONESCREW_ERR_INVALID_JSON), "invalid json");
|
||
return QJsonDocument(o).toJson(QJsonDocument::Compact);
|
||
}
|
||
QJsonObject obj = doc.object();
|
||
QString cmd = obj["cmd"].toString();
|
||
LOG_DEBUG("[ZMQ-REP] cmd=%s\n", cmd.toStdString().c_str());
|
||
|
||
QJsonObject resp;
|
||
resp["ok"] = true;
|
||
resp["code"] = 0;
|
||
resp["cmd"] = cmd;
|
||
|
||
if (cmd == "start")
|
||
{
|
||
// 解析检测模式（mono/binocular），设置后才启动检测
|
||
const std::string detectMode = obj["detectMode"].toString("binocular").toStdString();
|
||
emit detectModeRequested(detectMode);
|
||
|
||
const bool enableRaw = (m_rawImagePort > 0 &&
|
||
m_pZmqRawPublisher != nullptr);
|
||
emit rawImagePublishEnabledRequested(enableRaw);
|
||
const int ret = m_startWorkHandler ? m_startWorkHandler()
|
||
: fallbackHandlerError();
|
||
resp["mode"] = "detection";
|
||
resp["detectMode"] = QString::fromStdString(detectMode);
|
||
resp["rawImagePublish"] = enableRaw;
|
||
if (ret != 0)
|
||
{
|
||
emit rawImagePublishEnabledRequested(false);
|
||
setErrorResp(resp, ret, "start detection failed");
|
||
}
|
||
}
|
||
else if (cmd == "set_detect_mode")
|
||
{
|
||
const QString mode = obj["detectMode"].toString(obj["mode"].toString());
|
||
if (mode != "mono" && mode != "binocular")
|
||
{
|
||
setErrorResp(resp, ERR_CODE(DEV_ARG_INVAILD), "invalid detectMode");
|
||
}
|
||
else
|
||
{
|
||
emit detectModeRequested(mode.toStdString());
|
||
resp["detectMode"] = mode;
|
||
}
|
||
}
|
||
else if (cmd == "stop")
|
||
{
|
||
emit rawImagePublishEnabledRequested(false);
|
||
const int ret = m_stopWorkHandler ? m_stopWorkHandler()
|
||
: fallbackHandlerError();
|
||
if (ret != 0)
|
||
setErrorResp(resp, ret, "stop detection failed");
|
||
}
|
||
else if (cmd == "start_stream")
|
||
{
|
||
emit rawImagePublishEnabledRequested(false);
|
||
const int ret = m_startLiveStreamHandler ? m_startLiveStreamHandler()
|
||
: fallbackHandlerError();
|
||
resp["mode"] = "live_stream";
|
||
resp["rawImagePublish"] = false;
|
||
if (ret != 0)
|
||
setErrorResp(resp, ret, "start live stream failed");
|
||
}
|
||
else if (cmd == "stop_stream")
|
||
{
|
||
const int ret = m_stopLiveStreamHandler ? m_stopLiveStreamHandler()
|
||
: fallbackHandlerError();
|
||
resp["mode"] = "idle";
|
||
if (ret != 0)
|
||
setErrorResp(resp, ret, "stop live stream failed");
|
||
}
|
||
else if (cmd == "swap_cameras" || cmd == "swap_left_right")
|
||
{
|
||
const int ret = m_swapCameraRolesHandler ? m_swapCameraRolesHandler()
|
||
: fallbackHandlerError();
|
||
if (ret != 0)
|
||
{
|
||
setErrorResp(resp, ret, "swap camera roles failed");
|
||
}
|
||
else if (m_serverInfoProvider)
|
||
{
|
||
appendObject(resp, m_serverInfoProvider());
|
||
}
|
||
}
|
||
else if (cmd == "single")
|
||
{
|
||
if (!m_singleDetectionHandler)
|
||
{
|
||
setErrorResp(resp, ERR_CODE(DRONESCREW_ERR_SINGLE_HANDLER_NOT_SET),
|
||
"single handler not set");
|
||
}
|
||
else
|
||
{
|
||
std::unique_lock<std::mutex> singleLock(m_singleMutex, std::try_to_lock);
|
||
if (!singleLock.owns_lock())
|
||
{
|
||
setErrorResp(resp, ERR_CODE(DRONESCREW_ERR_SINGLE_BUSY),
|
||
"single detection busy");
|
||
}
|
||
else
|
||
{
|
||
DroneScrewResult result = m_singleDetectionHandler();
|
||
fillDetectionJson(result, resp);
|
||
resp["ok"] = result.success;
|
||
resp["code"] = result.success ? 0
|
||
: (result.errorCode != 0
|
||
? result.errorCode
|
||
: ERR_CODE(DRONESCREW_ERR_RESPONSE_NOT_OK));
|
||
}
|
||
}
|
||
}
|
||
else if (cmd == "open")
|
||
{
|
||
const QString targetMachineCode = obj["targetMachineCode"].toString();
|
||
const QString selfMachineCode = localMachineCode();
|
||
if (!targetMachineCode.isEmpty() && targetMachineCode != selfMachineCode)
|
||
{
|
||
setErrorResp(resp, ERR_CODE(DRONESCREW_ERR_MACHINE_CODE_MISMATCH),
|
||
"machine code mismatch");
|
||
resp["machineCode"] = selfMachineCode;
|
||
}
|
||
else
|
||
{
|
||
fillServerInfoJson(cmd, m_rtspUrl, m_controlPort, m_resultPort,
|
||
m_rawImagePort, obj, resp);
|
||
if (m_serverInfoProvider)
|
||
appendObject(resp, m_serverInfoProvider());
|
||
}
|
||
}
|
||
else if (cmd == "set_exposure")
|
||
{
|
||
emit setExposureRequested(obj["value"].toDouble());
|
||
}
|
||
else if (cmd == "set_gain")
|
||
{
|
||
emit setGainRequested(obj["value"].toDouble());
|
||
}
|
||
else if (cmd == "set_algo_params")
|
||
{
|
||
DroneScrewAlgoParams p;
|
||
p.scoreThreshold = static_cast<float>(obj["score"].toDouble(0.5));
|
||
p.nmsThreshold = static_cast<float>(obj["nms"].toDouble(0.45));
|
||
p.inputWidth = obj["width"].toInt(4096);
|
||
p.inputHeight = obj["height"].toInt(3000);
|
||
const QString configPath = obj.contains("configPath")
|
||
? obj["configPath"].toString()
|
||
: (obj.contains("modelPath")
|
||
? obj["modelPath"].toString()
|
||
: obj["model"].toString());
|
||
p.modelPath = configPath.toStdString();
|
||
p.modelType = obj["modelType"].toInt(1);
|
||
p.expectedBoltCount = obj.contains("expected")
|
||
? obj["expected"].toInt(8)
|
||
: obj["expectedBoltCount"].toInt(8);
|
||
emit updateAlgoParamsRequested(p);
|
||
}
|
||
else if (cmd == "get_info")
|
||
{
|
||
fillServerInfoJson(cmd, m_rtspUrl, m_controlPort, m_resultPort,
|
||
m_rawImagePort, obj, resp);
|
||
if (m_serverInfoProvider)
|
||
appendObject(resp, m_serverInfoProvider());
|
||
if (m_calibrationInfoProvider)
|
||
resp["calibration"] = m_calibrationInfoProvider();
|
||
}
|
||
else
|
||
{
|
||
setErrorResp(resp, ERR_CODE(DRONESCREW_ERR_UNKNOWN_CMD), "unknown cmd");
|
||
}
|
||
QByteArray rspData = QJsonDocument(resp).toJson(QJsonDocument::Compact);
|
||
const int previewSize = std::min(rspData.size(), 512);
|
||
LOG_DEBUG("[ZMQ-REP] send resp bytes=%d preview=%.*s%s\n",
|
||
rspData.size(), previewSize, rspData.constData(),
|
||
rspData.size() > previewSize ? "..." : "");
|
||
return rspData;
|
||
}
|
||
|
||
void DroneScrewZmqProtocol::publishDetectionResult(const DroneScrewResult& result)
|
||
{
|
||
try
|
||
{
|
||
if (!m_pZmqPublisher) { LOG_DEBUG("[PUB-result] no publisher\n"); return; }
|
||
|
||
QJsonObject obj;
|
||
fillDetectionJson(result, obj);
|
||
|
||
QByteArray payload = QJsonDocument(obj).toJson(QJsonDocument::Compact);
|
||
|
||
std::lock_guard<std::mutex> lk(m_pubMutex);
|
||
const int ret = m_pZmqPublisher->Publish("result", payload.constData(),
|
||
static_cast<size_t>(payload.size()));
|
||
static int pubCnt = 0;
|
||
static int pubFailCnt = 0;
|
||
if (ret != 0)
|
||
{
|
||
++pubFailCnt;
|
||
LOG_ERROR("[PUB-result] publish failed ret=%d fail=%d frame=%llu payload=%d\n",
|
||
ret, pubFailCnt, result.frameId, payload.size());
|
||
return;
|
||
}
|
||
if (++pubCnt % 100 == 0)
|
||
LOG_DEBUG("[PUB-result] #%d frame=%llu boxes=%zu\n", pubCnt, result.frameId, result.boxes.size());
|
||
}
|
||
catch (const std::exception& e)
|
||
{
|
||
LOG_ERROR("[PUB-result] exception frame=%llu boxes=%zu: %s\n",
|
||
result.frameId, result.boxes.size(), e.what());
|
||
}
|
||
catch (...)
|
||
{
|
||
LOG_ERROR("[PUB-result] unknown exception frame=%llu boxes=%zu\n",
|
||
result.frameId, result.boxes.size());
|
||
}
|
||
}
|
||
|
||
// MvsImageData.pixelFormat (Hik 原值，enMvGvspPixelType 透传) → WDRemotePixelFormat
|
||
// 常见映射，未知归 UNKNOWN
|
||
static uint32_t mvsPixelFormatToWDRemote(int mvsPixelFormat)
|
||
{
|
||
switch (mvsPixelFormat)
|
||
{
|
||
case 0x01080001: return static_cast<uint32_t>(WDRemotePixelFormat::MONO8); // PixelType_Gvsp_Mono8
|
||
case 0x02180014: return static_cast<uint32_t>(WDRemotePixelFormat::BGR8); // PixelType_Gvsp_BGR8_Packed
|
||
case 0x02180015: return static_cast<uint32_t>(WDRemotePixelFormat::RGB8); // PixelType_Gvsp_RGB8_Packed
|
||
case 0x01080009: return static_cast<uint32_t>(WDRemotePixelFormat::BAYERRG8); // PixelType_Gvsp_BayerRG8
|
||
default: return static_cast<uint32_t>(WDRemotePixelFormat::UNKNOWN);
|
||
}
|
||
}
|
||
|
||
void DroneScrewZmqProtocol::publishRawImage(const MvsImageData& left, const MvsImageData& right)
|
||
{
|
||
try
|
||
{
|
||
if (!m_pZmqRawPublisher) return;
|
||
// 左目必须有效；右目允许缺失（缺失时右目字段保持 0，接收端按空右目处理）
|
||
if (!left.pData || left.dataSize == 0) return;
|
||
|
||
LOG_DEBUG("[PUB-raw] enter frame=%llu left=%ux%u right=%ux%u\n",
|
||
left.frameID, left.width, left.height, right.width, right.height);
|
||
|
||
WDRemoteBinocularRawImageHeader hdr{};
|
||
hdr.magic = WD_REMOTE_BINOCULAR_RAW_IMAGE_MAGIC;
|
||
hdr.version = WD_REMOTE_BINOCULAR_RAW_IMAGE_VERSION;
|
||
hdr.frameId = left.frameID;
|
||
hdr.timestampUs = static_cast<int64_t>(left.timestamp);
|
||
|
||
// 左目（多数 GVSP 单字节像素 stride==width）
|
||
hdr.leftWidth = static_cast<uint32_t>(left.width);
|
||
hdr.leftHeight = static_cast<uint32_t>(left.height);
|
||
hdr.leftStride = static_cast<uint32_t>(left.width);
|
||
hdr.leftPixelFormat = mvsPixelFormatToWDRemote(left.pixelFormat);
|
||
|
||
// 右目（无效时保持 0）
|
||
const bool hasRight = (right.pData != nullptr && right.dataSize > 0);
|
||
if (hasRight)
|
||
{
|
||
hdr.rightWidth = static_cast<uint32_t>(right.width);
|
||
hdr.rightHeight = static_cast<uint32_t>(right.height);
|
||
hdr.rightStride = static_cast<uint32_t>(right.width);
|
||
hdr.rightPixelFormat = mvsPixelFormatToWDRemote(right.pixelFormat);
|
||
}
|
||
|
||
// Precision raw image publishing must be lossless; mono frames use zlib/raw too.
|
||
|
||
LOG_DEBUG("[PUB-raw] before zlib encode frame=%llu\n", left.frameID);
|
||
|
||
// 真·无损：Qt qCompress（zlib/deflate 封装，bit-exact，跨平台、无需额外依赖）。
|
||
// level=1 快速档：不在意压缩比，优先低 CPU 占用。下游算法可直接用还原字节。
|
||
QByteArray leftZ = qCompress(reinterpret_cast<const uchar*>(left.pData),
|
||
static_cast<int>(left.dataSize), 1);
|
||
QByteArray rightZ;
|
||
bool useZlib = !leftZ.isEmpty();
|
||
if (useZlib && hasRight)
|
||
{
|
||
rightZ = qCompress(reinterpret_cast<const uchar*>(right.pData),
|
||
static_cast<int>(right.dataSize), 1);
|
||
useZlib = !rightZ.isEmpty();
|
||
}
|
||
|
||
LOG_DEBUG("[PUB-raw] after zlib encode frame=%llu useZlib=%d leftSize=%d rightSize=%d\n",
|
||
left.frameID, useZlib ? 1 : 0, leftZ.size(), rightZ.size());
|
||
|
||
if (useZlib)
|
||
{
|
||
hdr.reserved[0] = WD_REMOTE_IMAGE_COMPRESSION_ZLIB;
|
||
hdr.reserved[1] = static_cast<uint32_t>(left.dataSize); // 解压后大小（左）
|
||
hdr.reserved[2] = hasRight ? static_cast<uint32_t>(right.dataSize) : 0u; // 解压后大小（右）
|
||
hdr.leftDataSize = static_cast<uint32_t>(leftZ.size());
|
||
hdr.rightDataSize = hasRight ? static_cast<uint32_t>(rightZ.size()) : 0u;
|
||
}
|
||
else
|
||
{
|
||
hdr.reserved[0] = WD_REMOTE_IMAGE_COMPRESSION_RAW;
|
||
hdr.leftDataSize = static_cast<uint32_t>(left.dataSize);
|
||
hdr.rightDataSize = hasRight ? static_cast<uint32_t>(right.dataSize) : 0u;
|
||
}
|
||
|
||
QByteArray payload;
|
||
payload.reserve(static_cast<int>(sizeof(hdr) + hdr.leftDataSize + hdr.rightDataSize));
|
||
payload.append(reinterpret_cast<const char*>(&hdr), sizeof(hdr));
|
||
if (useZlib)
|
||
{
|
||
payload.append(leftZ);
|
||
if (hasRight)
|
||
payload.append(rightZ);
|
||
}
|
||
else
|
||
{
|
||
payload.append(reinterpret_cast<const char*>(left.pData), static_cast<int>(left.dataSize));
|
||
if (hasRight)
|
||
payload.append(reinterpret_cast<const char*>(right.pData), static_cast<int>(right.dataSize));
|
||
}
|
||
|
||
LOG_DEBUG("[PUB-raw] before ZMQ publish frame=%llu payloadSize=%d\n",
|
||
left.frameID, payload.size());
|
||
|
||
std::lock_guard<std::mutex> lk(m_rawMutex);
|
||
const int ret = m_pZmqRawPublisher->Publish("raw", payload.constData(),
|
||
static_cast<size_t>(payload.size()));
|
||
|
||
LOG_DEBUG("[PUB-raw] after ZMQ publish frame=%llu ret=%d\n", left.frameID, ret);
|
||
|
||
static int rawPubCnt = 0;
|
||
static int rawFailCnt = 0;
|
||
if (ret != 0)
|
||
{
|
||
++rawFailCnt;
|
||
LOG_ERROR("[PUB-raw] publish failed ret=%d fail=%d frame=%llu compression=%s payload=%d\n",
|
||
ret, rawFailCnt, left.frameID, useZlib ? "zlib" : "raw", payload.size());
|
||
return;
|
||
}
|
||
++rawPubCnt;
|
||
if (rawPubCnt == 1)
|
||
LOG_INFO("[PUB-raw] first frame=%llu compression=%s left=%ux%u fmt=%u size=%u raw=%u right=%ux%u fmt=%u size=%u raw=%u payload=%d\n",
|
||
left.frameID,
|
||
useZlib ? "zlib" : "raw",
|
||
hdr.leftWidth, hdr.leftHeight, hdr.leftPixelFormat, hdr.leftDataSize,
|
||
useZlib ? hdr.reserved[1] : hdr.leftDataSize,
|
||
hdr.rightWidth, hdr.rightHeight, hdr.rightPixelFormat, hdr.rightDataSize,
|
||
useZlib ? hdr.reserved[2] : hdr.rightDataSize,
|
||
payload.size());
|
||
else if (rawPubCnt % 100 == 0)
|
||
LOG_DEBUG("[PUB-raw] #%d frame=%llu compression=%s left=%ux%u right=%ux%u payload=%d\n",
|
||
rawPubCnt, left.frameID, useZlib ? "zlib" : "raw", left.width, left.height,
|
||
hasRight ? right.width : 0, hasRight ? right.height : 0,
|
||
payload.size());
|
||
|
||
LOG_DEBUG("[PUB-raw] exit frame=%llu\n", left.frameID);
|
||
}
|
||
catch (const std::bad_alloc& e)
|
||
{
|
||
LOG_ERROR("[PUB-raw] memory allocation failed frame=%llu left=%ux%u leftSize=%u right=%ux%u rightSize=%u: %s\n",
|
||
left.frameID, left.width, left.height, left.dataSize,
|
||
right.width, right.height, right.dataSize, e.what());
|
||
}
|
||
catch (const std::exception& e)
|
||
{
|
||
LOG_ERROR("[PUB-raw] exception frame=%llu left=%ux%u leftSize=%u right=%ux%u rightSize=%u: %s\n",
|
||
left.frameID, left.width, left.height, left.dataSize,
|
||
right.width, right.height, right.dataSize, e.what());
|
||
}
|
||
catch (...)
|
||
{
|
||
LOG_ERROR("[PUB-raw] unknown exception frame=%llu left=%ux%u leftSize=%u right=%ux%u rightSize=%u\n",
|
||
left.frameID, left.width, left.height, left.dataSize,
|
||
right.width, right.height, right.dataSize);
|
||
}
|
||
}
|