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);
|
|||
|
|
}
|
|||
|
|
}
|