891 lines
33 KiB
C++
891 lines
33 KiB
C++
#include "DroneScrewZmqProtocol.h"
|
||
#include "MjpegEncoder.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 <cstdint>
|
||
#include <cstring>
|
||
#include <exception>
|
||
#include <mutex>
|
||
#include <new>
|
||
#include <vector>
|
||
|
||
namespace
|
||
{
|
||
constexpr int kRawImageZlibLevel = 9;
|
||
constexpr bool kUseFixedPrecisionMonoRawMjpeg = true;
|
||
constexpr int kFixedPrecisionMonoRawMjpegQuality = 25;
|
||
constexpr int kFixedPrecisionMonoRawScaleDiv = 4;
|
||
|
||
struct MjpegPayload
|
||
{
|
||
std::vector<uint8_t> jpeg;
|
||
uint32_t width{0};
|
||
uint32_t height{0};
|
||
uint32_t stride{0};
|
||
uint32_t decodedSize{0};
|
||
int quality{0};
|
||
int scaleDiv{1};
|
||
};
|
||
|
||
void downscaleMono8Nearest(const uint8_t* src,
|
||
int srcWidth,
|
||
int srcHeight,
|
||
int scaleDiv,
|
||
std::vector<uint8_t>& dst,
|
||
int& dstWidth,
|
||
int& dstHeight)
|
||
{
|
||
dstWidth = std::max(1, srcWidth / scaleDiv);
|
||
dstHeight = std::max(1, srcHeight / scaleDiv);
|
||
dst.resize(static_cast<size_t>(dstWidth) * static_cast<size_t>(dstHeight));
|
||
for (int y = 0; y < dstHeight; ++y)
|
||
{
|
||
const int sy = std::min(srcHeight - 1, y * scaleDiv);
|
||
const uint8_t* srcRow = src + static_cast<size_t>(sy) * static_cast<size_t>(srcWidth);
|
||
uint8_t* dstRow = dst.data() + static_cast<size_t>(y) * static_cast<size_t>(dstWidth);
|
||
for (int x = 0; x < dstWidth; ++x)
|
||
{
|
||
const int sx = std::min(srcWidth - 1, x * scaleDiv);
|
||
dstRow[x] = srcRow[sx];
|
||
}
|
||
}
|
||
}
|
||
|
||
bool encodeMonoMjpegFixedRatio(const MvsImageData& left, MjpegPayload& out)
|
||
{
|
||
if (!left.pData || left.width <= 0 || left.height <= 0 || left.dataSize == 0)
|
||
return false;
|
||
|
||
static MppMjpegEncoder s_mjpegEncoder;
|
||
|
||
const int srcWidth = static_cast<int>(left.width);
|
||
const int srcHeight = static_cast<int>(left.height);
|
||
const int scaleDiv = kFixedPrecisionMonoRawScaleDiv;
|
||
std::vector<uint8_t> scaled;
|
||
const uint8_t* encodeData = left.pData;
|
||
int encodeWidth = srcWidth;
|
||
int encodeHeight = srcHeight;
|
||
|
||
if (scaleDiv > 1)
|
||
{
|
||
downscaleMono8Nearest(left.pData, srcWidth, srcHeight,
|
||
scaleDiv, scaled, encodeWidth, encodeHeight);
|
||
encodeData = scaled.data();
|
||
}
|
||
|
||
std::vector<uint8_t> jpeg;
|
||
s_mjpegEncoder.SetQuality(kFixedPrecisionMonoRawMjpegQuality);
|
||
const bool ok = s_mjpegEncoder.EncodeMono8(
|
||
encodeData,
|
||
static_cast<size_t>(encodeWidth) * static_cast<size_t>(encodeHeight),
|
||
encodeWidth,
|
||
encodeHeight,
|
||
jpeg);
|
||
|
||
LOG_DEBUG("[PUB-raw] mjpeg fixed frame=%llu ok=%d q=%d scale=1/%d size=%zu raw=%u\n",
|
||
left.frameID, ok ? 1 : 0, kFixedPrecisionMonoRawMjpegQuality,
|
||
scaleDiv, jpeg.size(), left.dataSize);
|
||
if (!ok)
|
||
{
|
||
LOG_WARN("[PUB-raw] mjpeg fixed failed frame=%llu q=%d scale=1/%d encode=%dx%d raw=%u fallback=zlib\n",
|
||
left.frameID, kFixedPrecisionMonoRawMjpegQuality,
|
||
scaleDiv, encodeWidth, encodeHeight, left.dataSize);
|
||
return false;
|
||
}
|
||
|
||
static int s_mjpegOkLogCnt = 0;
|
||
++s_mjpegOkLogCnt;
|
||
if (s_mjpegOkLogCnt <= 5 || s_mjpegOkLogCnt % 30 == 0)
|
||
{
|
||
const double ratio = left.dataSize > 0
|
||
? static_cast<double>(jpeg.size()) / static_cast<double>(left.dataSize)
|
||
: 0.0;
|
||
const double mbpsAt2Fps = static_cast<double>(jpeg.size()) * 8.0 * 2.0 / 1000000.0;
|
||
LOG_INFO("[PUB-raw] mjpeg fixed ok #%d frame=%llu q=%d scale=1/%d out=%dx%d jpeg=%zu raw=%u ratio=%.4f approx=%.2fMbps@2fps\n",
|
||
s_mjpegOkLogCnt, left.frameID, kFixedPrecisionMonoRawMjpegQuality,
|
||
scaleDiv, encodeWidth, encodeHeight, jpeg.size(), left.dataSize,
|
||
ratio, mbpsAt2Fps);
|
||
}
|
||
|
||
out.jpeg.swap(jpeg);
|
||
out.width = static_cast<uint32_t>(encodeWidth);
|
||
out.height = static_cast<uint32_t>(encodeHeight);
|
||
out.stride = static_cast<uint32_t>(encodeWidth);
|
||
out.decodedSize = static_cast<uint32_t>(
|
||
static_cast<size_t>(encodeWidth) * static_cast<size_t>(encodeHeight));
|
||
out.quality = kFixedPrecisionMonoRawMjpegQuality;
|
||
out.scaleDiv = scaleDiv;
|
||
return true;
|
||
}
|
||
|
||
const char* rawCompressionName(uint32_t compression)
|
||
{
|
||
switch (compression)
|
||
{
|
||
case WD_REMOTE_IMAGE_COMPRESSION_RAW: return "raw";
|
||
case WD_REMOTE_IMAGE_COMPRESSION_PNG: return "png";
|
||
case WD_REMOTE_IMAGE_COMPRESSION_MJPEG: return "mjpeg";
|
||
case WD_REMOTE_IMAGE_COMPRESSION_ZLIB: return "zlib";
|
||
default: return "unknown";
|
||
}
|
||
}
|
||
|
||
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);
|
||
obj["isFinalResult"] = result.isFinalResult;
|
||
obj["rankScore"] = result.rankScore;
|
||
obj["rankSampleCount"] = result.rankSampleCount;
|
||
obj["saveIndex"] = static_cast<qint64>(result.saveIndex);
|
||
|
||
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;
|
||
if (b.hasPhysicalHeight)
|
||
{
|
||
const double heightMm = static_cast<double>(b.physicalHeightMm);
|
||
jb["height_mm"] = heightMm;
|
||
jb["hasStereoMatch"] = b.hasStereoMatch;
|
||
jb["trusted"] = b.trusted;
|
||
jb["matchConfidence"] = b.matchConfidence;
|
||
jb["matchStatus"] = QString::fromStdString(b.matchStatus);
|
||
jb["pairId"] = b.pairId;
|
||
jb["leftIndex"] = b.leftIndex;
|
||
jb["rightIndex"] = b.rightIndex;
|
||
}
|
||
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 (m_finalDetectionResultProvider)
|
||
{
|
||
DroneScrewResult finalResult;
|
||
if (m_finalDetectionResultProvider(finalResult))
|
||
{
|
||
finalResult.isFinalResult = true;
|
||
fillDetectionJson(finalResult, resp);
|
||
resp["hasFinalResult"] = true;
|
||
publishDetectionResult(finalResult);
|
||
}
|
||
else
|
||
{
|
||
resp["hasFinalResult"] = false;
|
||
}
|
||
}
|
||
}
|
||
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);
|
||
hdr.reserved[3] = static_cast<uint32_t>(left.width);
|
||
hdr.reserved[4] = static_cast<uint32_t>(left.height);
|
||
hdr.reserved[5] = 1u;
|
||
|
||
// 右目（无效时保持 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);
|
||
}
|
||
|
||
MjpegPayload leftMjpeg;
|
||
bool useMjpeg = false;
|
||
const bool singleRawFrame = !hasRight;
|
||
if (kUseFixedPrecisionMonoRawMjpeg &&
|
||
singleRawFrame &&
|
||
hdr.leftPixelFormat == static_cast<uint32_t>(WDRemotePixelFormat::MONO8))
|
||
{
|
||
useMjpeg = encodeMonoMjpegFixedRatio(left, leftMjpeg);
|
||
}
|
||
|
||
QByteArray leftZ;
|
||
QByteArray rightZ;
|
||
bool useZlib = false;
|
||
if (!useMjpeg)
|
||
{
|
||
LOG_DEBUG("[PUB-raw] before zlib encode frame=%llu level=%d\n",
|
||
left.frameID, kRawImageZlibLevel);
|
||
|
||
leftZ = qCompress(reinterpret_cast<const uchar*>(left.pData),
|
||
static_cast<int>(left.dataSize), kRawImageZlibLevel);
|
||
useZlib = !leftZ.isEmpty();
|
||
if (useZlib && hasRight)
|
||
{
|
||
rightZ = qCompress(reinterpret_cast<const uchar*>(right.pData),
|
||
static_cast<int>(right.dataSize), kRawImageZlibLevel);
|
||
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 (useMjpeg)
|
||
{
|
||
hdr.reserved[0] = WD_REMOTE_IMAGE_COMPRESSION_MJPEG;
|
||
hdr.reserved[1] = leftMjpeg.decodedSize;
|
||
hdr.reserved[2] = 0u;
|
||
hdr.leftWidth = leftMjpeg.width;
|
||
hdr.leftHeight = leftMjpeg.height;
|
||
hdr.leftStride = leftMjpeg.stride;
|
||
hdr.leftDataSize = static_cast<uint32_t>(leftMjpeg.jpeg.size());
|
||
hdr.rightDataSize = 0u;
|
||
hdr.reserved[5] = static_cast<uint32_t>(leftMjpeg.scaleDiv);
|
||
}
|
||
else 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;
|
||
}
|
||
|
||
#if 0
|
||
// Precision raw image publishing must be lossless; mono frames use zlib/raw too.
|
||
|
||
LOG_DEBUG("[PUB-raw] before zlib encode frame=%llu level=%d\n",
|
||
left.frameID, kRawImageZlibLevel);
|
||
|
||
// 真·无损：Qt qCompress（zlib/deflate 封装，bit-exact，跨平台、无需额外依赖）。
|
||
// level=9 高压缩：优先降低精确检测全分辨率 raw 图像带宽，代价是 CPU 时间增加。
|
||
QByteArray leftZ = qCompress(reinterpret_cast<const uchar*>(left.pData),
|
||
static_cast<int>(left.dataSize), kRawImageZlibLevel);
|
||
QByteArray rightZ;
|
||
bool useZlib = !leftZ.isEmpty();
|
||
if (useZlib && hasRight)
|
||
{
|
||
rightZ = qCompress(reinterpret_cast<const uchar*>(right.pData),
|
||
static_cast<int>(right.dataSize), kRawImageZlibLevel);
|
||
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;
|
||
}
|
||
|
||
#endif
|
||
|
||
QByteArray payload;
|
||
payload.reserve(static_cast<int>(sizeof(hdr) + hdr.leftDataSize + hdr.rightDataSize));
|
||
payload.append(reinterpret_cast<const char*>(&hdr), sizeof(hdr));
|
||
if (useMjpeg)
|
||
{
|
||
payload.append(reinterpret_cast<const char*>(leftMjpeg.jpeg.data()),
|
||
static_cast<int>(leftMjpeg.jpeg.size()));
|
||
}
|
||
else 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,
|
||
rawCompressionName(hdr.reserved[0]), 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,
|
||
rawCompressionName(hdr.reserved[0]),
|
||
hdr.leftWidth, hdr.leftHeight, hdr.leftPixelFormat, hdr.leftDataSize,
|
||
hdr.reserved[0] != WD_REMOTE_IMAGE_COMPRESSION_RAW ? hdr.reserved[1] : hdr.leftDataSize,
|
||
hdr.rightWidth, hdr.rightHeight, hdr.rightPixelFormat, hdr.rightDataSize,
|
||
hdr.reserved[0] != WD_REMOTE_IMAGE_COMPRESSION_RAW ? 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, rawCompressionName(hdr.reserved[0]), 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);
|
||
}
|
||
}
|