1350 lines
47 KiB
C++
1350 lines
47 KiB
C++
#include "WDRemoteReceiver.h"
|
||
|
||
#include "IVrZeroMQClient.h"
|
||
#include "IVrZeroMQPubSub.h"
|
||
#include "VrLog.h"
|
||
#include "VrError.h"
|
||
#include "AuthManager.h"
|
||
|
||
#include "json/json.h"
|
||
|
||
#include <QAbstractSocket>
|
||
#include <QElapsedTimer>
|
||
#include <QHostAddress>
|
||
#include <QNetworkInterface>
|
||
#include <QUdpSocket>
|
||
|
||
#include <opencv2/core.hpp>
|
||
#include <opencv2/imgcodecs.hpp>
|
||
|
||
|
||
#include <cstring>
|
||
#include <cstdlib>
|
||
#include <exception>
|
||
#include <new>
|
||
#include <set>
|
||
#include <string>
|
||
#include <utility>
|
||
#include <vector>
|
||
#include <zmq.hpp>
|
||
|
||
using namespace VA; // 项目内 jsoncpp 包在 VA 命名空间内
|
||
|
||
namespace
|
||
{
|
||
constexpr int kDefaultZmqControlPort = 15555;
|
||
constexpr int kDefaultZmqResultPort = 15556;
|
||
constexpr int kDefaultZmqRawImagePort = 15557;
|
||
|
||
std::string logPreview(const std::string& text, size_t maxLen = 512)
|
||
{
|
||
if (text.size() <= maxLen)
|
||
return text;
|
||
return text.substr(0, maxLen) + "...";
|
||
}
|
||
|
||
int sendZmqJsonRequest(const std::string& ip,
|
||
int port,
|
||
const std::string& jsonReq,
|
||
std::string& jsonResp,
|
||
int timeoutMs)
|
||
{
|
||
if (ip.empty() || port <= 0) return ERR_CODE(NET_ERR_ARG);
|
||
if (jsonReq.find("\"cmd\":\"discover\"") != std::string::npos ||
|
||
jsonReq.find("\"cmd\" : \"discover\"") != std::string::npos)
|
||
{
|
||
LOG_ERROR("[ZMQ-REQ] discover over ZMQ is disabled; use UDP discovery port=%u\n",
|
||
static_cast<unsigned>(WD_REMOTE_DISCOVERY_PORT));
|
||
return ERR_CODE(DRONESCREW_ERR_UDP_DISCOVERY);
|
||
}
|
||
|
||
try
|
||
{
|
||
zmq::context_t ctx(1);
|
||
zmq::socket_t sock(ctx, ZMQ_REQ);
|
||
const int linger = 0;
|
||
sock.set(zmq::sockopt::linger, linger);
|
||
sock.set(zmq::sockopt::rcvtimeo, timeoutMs);
|
||
sock.set(zmq::sockopt::sndtimeo, timeoutMs);
|
||
sock.set(zmq::sockopt::connect_timeout, timeoutMs);
|
||
|
||
const std::string addr = "tcp://" + ip + ":" + std::to_string(port);
|
||
sock.connect(addr);
|
||
|
||
zmq::message_t req(jsonReq.data(), jsonReq.size());
|
||
const std::string reqPreview = logPreview(jsonReq);
|
||
LOG_DEBUG("[ZMQ-REQ] send %s:%d bytes=%zu preview=%s\n",
|
||
ip.c_str(), port, jsonReq.size(), reqPreview.c_str());
|
||
auto sent = sock.send(req, zmq::send_flags::none);
|
||
if (!sent.has_value())
|
||
{
|
||
LOG_DEBUG("[ZMQ-REQ] send failed %s:%d\n", ip.c_str(), port);
|
||
return ERR_CODE(NET_ERR_SEND_DATA);
|
||
}
|
||
|
||
zmq::message_t reply;
|
||
auto recved = sock.recv(reply, zmq::recv_flags::none);
|
||
if (!recved.has_value())
|
||
{
|
||
LOG_DEBUG("[ZMQ-REQ] recv timeout %s:%d timeout=%d\n",
|
||
ip.c_str(), port, timeoutMs);
|
||
return ERR_CODE(NET_ERR_RECV_DATA);
|
||
}
|
||
|
||
jsonResp.assign(static_cast<const char*>(reply.data()), reply.size());
|
||
const std::string respPreview = logPreview(jsonResp);
|
||
LOG_DEBUG("[ZMQ-REQ] recv %s:%d bytes=%zu preview=%s\n",
|
||
ip.c_str(), port, jsonResp.size(), respPreview.c_str());
|
||
return 0;
|
||
}
|
||
catch (const std::exception& e)
|
||
{
|
||
LOG_DEBUG("[ZMQ-REQ] exception %s:%d %s\n", ip.c_str(), port, e.what());
|
||
return ERR_CODE(NET_ERR_CONNECT);
|
||
}
|
||
}
|
||
|
||
std::vector<QHostAddress> buildBroadcastAddresses()
|
||
{
|
||
std::vector<QHostAddress> addrs;
|
||
std::set<QString> seen;
|
||
|
||
auto addAddr = [&](const QHostAddress& addr) {
|
||
if (addr.isNull()) return;
|
||
const QString key = addr.toString();
|
||
if (key.isEmpty()) return;
|
||
if (seen.insert(key).second)
|
||
addrs.push_back(addr);
|
||
};
|
||
|
||
addAddr(QHostAddress::Broadcast);
|
||
|
||
const QList<QNetworkInterface> ifaces = QNetworkInterface::allInterfaces();
|
||
for (const QNetworkInterface& iface : ifaces)
|
||
{
|
||
const QNetworkInterface::InterfaceFlags flags = iface.flags();
|
||
if (!(flags & QNetworkInterface::IsUp) ||
|
||
!(flags & QNetworkInterface::CanBroadcast) ||
|
||
(flags & QNetworkInterface::IsLoopBack))
|
||
continue;
|
||
|
||
const QList<QNetworkAddressEntry> entries = iface.addressEntries();
|
||
for (const QNetworkAddressEntry& entry : entries)
|
||
{
|
||
if (entry.ip().protocol() != QAbstractSocket::IPv4Protocol)
|
||
continue;
|
||
addAddr(entry.broadcast());
|
||
}
|
||
}
|
||
|
||
return addrs;
|
||
}
|
||
|
||
std::string hostAddressToIpv4String(const QHostAddress& addr)
|
||
{
|
||
bool ok = false;
|
||
const quint32 ipv4 = addr.toIPv4Address(&ok);
|
||
if (ok)
|
||
return QHostAddress(ipv4).toString().toStdString();
|
||
return addr.toString().toStdString();
|
||
}
|
||
|
||
bool decodePngImage(const uint8_t* data,
|
||
size_t dataSize,
|
||
int expectedWidth,
|
||
int expectedHeight,
|
||
WDRemotePixelFormat& pixelFormat,
|
||
int& stride,
|
||
std::vector<uint8_t>& decodedBytes)
|
||
{
|
||
if (!data || dataSize == 0) return false;
|
||
|
||
std::vector<uchar> encoded(data, data + dataSize);
|
||
cv::Mat decoded = cv::imdecode(encoded, cv::IMREAD_UNCHANGED);
|
||
if (decoded.empty()) return false;
|
||
if (decoded.depth() != CV_8U) return false;
|
||
if (expectedWidth > 0 && decoded.cols != expectedWidth) return false;
|
||
if (expectedHeight > 0 && decoded.rows != expectedHeight) return false;
|
||
|
||
const int channels = decoded.channels();
|
||
if (channels == 1)
|
||
{
|
||
pixelFormat = WDRemotePixelFormat::MONO8;
|
||
stride = decoded.cols;
|
||
}
|
||
else if (channels == 3)
|
||
{
|
||
pixelFormat = WDRemotePixelFormat::BGR8;
|
||
stride = decoded.cols * 3;
|
||
}
|
||
else
|
||
{
|
||
return false;
|
||
}
|
||
|
||
if (!decoded.isContinuous())
|
||
decoded = decoded.clone();
|
||
|
||
const size_t bytes = decoded.total() * decoded.elemSize();
|
||
decodedBytes.assign(decoded.data, decoded.data + bytes);
|
||
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";
|
||
}
|
||
}
|
||
|
||
int responseErrorCode(const std::string& jsonResp)
|
||
{
|
||
if (jsonResp.empty())
|
||
return ERR_CODE(DRONESCREW_ERR_RESPONSE_NOT_OK);
|
||
|
||
Json::Value root;
|
||
Json::Reader reader;
|
||
if (!reader.parse(jsonResp, root))
|
||
return ERR_CODE(DRONESCREW_ERR_INVALID_JSON);
|
||
|
||
if (root.get("ok", false).asBool())
|
||
return 0;
|
||
|
||
const int code = root.get("code", 0).asInt();
|
||
return code != 0 ? code : ERR_CODE(DRONESCREW_ERR_RESPONSE_NOT_OK);
|
||
}
|
||
|
||
void fillMatrix(const Json::Value& root, WDRemoteMatrix& matrix)
|
||
{
|
||
matrix.rows = root.get("rows", 0).asInt();
|
||
matrix.cols = root.get("cols", 0).asInt();
|
||
matrix.data.clear();
|
||
|
||
const Json::Value& data = root["data"];
|
||
if (!data.isArray())
|
||
return;
|
||
|
||
matrix.data.reserve(data.size());
|
||
for (Json::ArrayIndex i = 0; i < data.size(); ++i)
|
||
matrix.data.push_back(data[i].asDouble());
|
||
}
|
||
|
||
void fillCameraCalibration(const Json::Value& root,
|
||
WDRemoteCameraCalibration& calibration)
|
||
{
|
||
if (!root.isObject())
|
||
return;
|
||
|
||
calibration.valid = root.get("valid", false).asBool();
|
||
calibration.imageWidth = root.get("imageWidth", 0).asInt();
|
||
calibration.imageHeight = root.get("imageHeight", 0).asInt();
|
||
calibration.rms = root.get("rms", 0.0).asDouble();
|
||
fillMatrix(root["cameraMatrix"], calibration.cameraMatrix);
|
||
fillMatrix(root["distCoeffs"], calibration.distCoeffs);
|
||
}
|
||
|
||
void fillStereoCalibration(const Json::Value& root,
|
||
WDRemoteStereoCalibration& calibration)
|
||
{
|
||
if (!root.isObject())
|
||
return;
|
||
|
||
calibration.valid = root.get("valid", false).asBool();
|
||
calibration.rms = root.get("rms", 0.0).asDouble();
|
||
calibration.baselineMm = root.get("baselineMm", 0.0).asDouble();
|
||
fillMatrix(root["R"], calibration.R);
|
||
fillMatrix(root["T"], calibration.T);
|
||
fillMatrix(root["E"], calibration.E);
|
||
fillMatrix(root["F"], calibration.F);
|
||
fillMatrix(root["R1"], calibration.R1);
|
||
fillMatrix(root["R2"], calibration.R2);
|
||
fillMatrix(root["P1"], calibration.P1);
|
||
fillMatrix(root["P2"], calibration.P2);
|
||
fillMatrix(root["Q"], calibration.Q);
|
||
}
|
||
|
||
void fillCalibrationInfo(const Json::Value& root,
|
||
WDRemoteCalibrationInfo& calibration)
|
||
{
|
||
const Json::Value& node = root["calibration"];
|
||
if (!node.isObject())
|
||
return;
|
||
|
||
calibration.valid = node.get("valid", false).asBool();
|
||
calibration.sourcePath = node.get("sourcePath", "").asString();
|
||
calibration.algoConfigPath = node.get("algoConfigPath", "").asString();
|
||
calibration.message = node.get("message", "").asString();
|
||
fillCameraCalibration(node["left"], calibration.left);
|
||
fillCameraCalibration(node["right"], calibration.right);
|
||
fillStereoCalibration(node["stereo"], calibration.stereo);
|
||
}
|
||
|
||
void fillRuntimeFields(const Json::Value& root, WDRemoteServerInfo& info)
|
||
{
|
||
info.detectFps = root.get("detectFps", 0.0).asDouble();
|
||
info.streamFps = root.get("streamFps", 0.0).asDouble();
|
||
info.leftExposure = root.get("leftExposure", 0.0).asDouble();
|
||
info.rightExposure = root.get("rightExposure", 0.0).asDouble();
|
||
info.leftGain = root.get("leftGain", 0.0).asDouble();
|
||
info.rightGain = root.get("rightGain", 0.0).asDouble();
|
||
info.leftCameraIndex = root.get("leftCameraIndex", 0).asInt();
|
||
info.rightCameraIndex = root.get("rightCameraIndex", 1).asInt();
|
||
info.leftCameraSerial = root.get("leftCameraSerial", "").asString();
|
||
info.rightCameraSerial = root.get("rightCameraSerial", "").asString();
|
||
info.streamWidth = root.get("streamWidth", 0).asInt();
|
||
info.streamHeight = root.get("streamHeight", 0).asInt();
|
||
info.streamCamera = root.get("streamCamera", "").asString();
|
||
info.binningHorizontal = root.get("binningHorizontal", 0).asInt();
|
||
info.binningVertical = root.get("binningVertical", 0).asInt();
|
||
info.mode = root.get("mode", "").asString();
|
||
info.detectMode = root.get("detectMode", "").asString();
|
||
// 算法参数
|
||
info.algoScore = static_cast<float>(root.get("algoScore", 0.5).asDouble());
|
||
info.algoNms = static_cast<float>(root.get("algoNms", 0.45).asDouble());
|
||
info.algoWidth = root.get("algoWidth", 640).asInt();
|
||
info.algoHeight = root.get("algoHeight", 640).asInt();
|
||
info.algoModel = root.get("algoModel", "").asString();
|
||
info.algoModelType = root.get("algoModelType", 0).asInt();
|
||
info.algoExpectedBoltCount = root.get("algoExpectedBoltCount", 8).asInt();
|
||
fillCalibrationInfo(root, info.calibration);
|
||
}
|
||
|
||
void fillRuntimeFields(const Json::Value& root, WDRemoteDeviceInfo& info)
|
||
{
|
||
info.detectFps = root.get("detectFps", 0.0).asDouble();
|
||
info.streamFps = root.get("streamFps", 0.0).asDouble();
|
||
info.leftExposure = root.get("leftExposure", 0.0).asDouble();
|
||
info.rightExposure = root.get("rightExposure", 0.0).asDouble();
|
||
info.leftGain = root.get("leftGain", 0.0).asDouble();
|
||
info.rightGain = root.get("rightGain", 0.0).asDouble();
|
||
info.leftCameraIndex = root.get("leftCameraIndex", 0).asInt();
|
||
info.rightCameraIndex = root.get("rightCameraIndex", 1).asInt();
|
||
info.leftCameraSerial = root.get("leftCameraSerial", "").asString();
|
||
info.rightCameraSerial = root.get("rightCameraSerial", "").asString();
|
||
info.streamWidth = root.get("streamWidth", 0).asInt();
|
||
info.streamHeight = root.get("streamHeight", 0).asInt();
|
||
info.streamCamera = root.get("streamCamera", "").asString();
|
||
info.binningHorizontal = root.get("binningHorizontal", 0).asInt();
|
||
info.binningVertical = root.get("binningVertical", 0).asInt();
|
||
info.mode = root.get("mode", "").asString();
|
||
info.detectMode = root.get("detectMode", "").asString();
|
||
}
|
||
}
|
||
|
||
// =====================================================================
|
||
// 工厂
|
||
// =====================================================================
|
||
int IWDRemoteReceiver::CreateInstance(IWDRemoteReceiver** ppReceiver)
|
||
{
|
||
if (!ppReceiver) return ERR_CODE(DEV_ARG_INVAILD);
|
||
*ppReceiver = new (std::nothrow) WDRemoteReceiver();
|
||
return *ppReceiver ? 0 : ERR_CODE(DATA_ERR_MEM);
|
||
}
|
||
|
||
// =====================================================================
|
||
// ctor / dtor
|
||
// =====================================================================
|
||
WDRemoteReceiver::WDRemoteReceiver()
|
||
{
|
||
}
|
||
|
||
WDRemoteReceiver::~WDRemoteReceiver()
|
||
{
|
||
Disconnect();
|
||
}
|
||
|
||
// =====================================================================
|
||
// 连接 / 断开
|
||
// =====================================================================
|
||
int WDRemoteReceiver::Connect(const ConnectConfig& cfg)
|
||
{
|
||
if (m_bConnected.load())
|
||
{
|
||
if (m_cfg.serverIp == cfg.serverIp &&
|
||
m_cfg.controlPort == cfg.controlPort &&
|
||
m_cfg.resultPort == cfg.resultPort &&
|
||
m_cfg.rawImagePort == cfg.rawImagePort)
|
||
return 0;
|
||
Disconnect();
|
||
}
|
||
|
||
if (cfg.serverIp.empty() || cfg.controlPort <= 0)
|
||
return ERR_CODE(NET_ERR_ARG);
|
||
|
||
Json::Value probeReq;
|
||
probeReq["cmd"] = "get_info";
|
||
Json::FastWriter probeWriter;
|
||
std::string probeResp;
|
||
const int probeRet = sendZmqJsonRequest(cfg.serverIp, cfg.controlPort,
|
||
probeWriter.write(probeReq),
|
||
probeResp, 1000);
|
||
if (probeRet != 0)
|
||
{
|
||
LOG_DEBUG("[CLIENT] Connect probe failed: ctrl=%s:%d ret=%d\n",
|
||
cfg.serverIp.c_str(), cfg.controlPort, probeRet);
|
||
emitEvent(WDRemoteEventType::CONNECTION_ERROR, "server probe failed");
|
||
return probeRet;
|
||
}
|
||
|
||
Json::Value probeRoot;
|
||
Json::Reader probeReader;
|
||
if (!probeReader.parse(probeResp, probeRoot) ||
|
||
!probeRoot.get("ok", false).asBool())
|
||
{
|
||
LOG_DEBUG("[CLIENT] Connect probe invalid response: ctrl=%s:%d resp=%s\n",
|
||
cfg.serverIp.c_str(), cfg.controlPort, probeResp.c_str());
|
||
emitEvent(WDRemoteEventType::CONNECTION_ERROR, "server probe invalid response");
|
||
return ERR_CODE(DRONESCREW_ERR_RESPONSE_NOT_OK);
|
||
}
|
||
|
||
m_cfg = cfg;
|
||
|
||
// 1) 控制通道
|
||
if (!IVrZeroMQClient::CreateObject(&m_pCtrlClient) || !m_pCtrlClient)
|
||
{
|
||
emitEvent(WDRemoteEventType::CONNECTION_ERROR,
|
||
"create ZeroMQ client failed");
|
||
return ERR_CODE(NET_ERR_CREAT_INIT);
|
||
}
|
||
|
||
int r = m_pCtrlClient->Init(m_cfg.serverIp.c_str(), m_cfg.controlPort);
|
||
if (r != 0)
|
||
{
|
||
emitEvent(WDRemoteEventType::CONNECTION_ERROR,
|
||
"ZeroMQ client init failed");
|
||
delete m_pCtrlClient;
|
||
m_pCtrlClient = nullptr;
|
||
return r;
|
||
}
|
||
|
||
LOG_DEBUG("[CLIENT] Connect OK: ctrl=%s:%d result=%d raw=%d\n",
|
||
m_cfg.serverIp.c_str(), m_cfg.controlPort,
|
||
m_cfg.resultPort, m_cfg.rawImagePort);
|
||
|
||
m_bConnected = true;
|
||
emitEvent(WDRemoteEventType::CONNECTED, "");
|
||
|
||
return 0;
|
||
}
|
||
|
||
bool WDRemoteReceiver::parseDeviceInfoJson(const std::string& jsonStr,
|
||
const std::string& serverIp,
|
||
WDRemoteDeviceInfo& outDevice)
|
||
{
|
||
Json::Value root;
|
||
Json::Reader reader;
|
||
if (!reader.parse(jsonStr, root)) return false;
|
||
if (!root.get("ok", false).asBool()) return false;
|
||
|
||
outDevice.machineCode = root.get("machineCode", "").asString();
|
||
outDevice.clientMachineCode = root.get("clientMachineCode", "").asString();
|
||
outDevice.serverIp = serverIp.empty()
|
||
? root.get("serverIp", "").asString()
|
||
: serverIp;
|
||
outDevice.deviceName = root.get("deviceName", "DroneScrewServer").asString();
|
||
outDevice.controlPort = root.get("controlPort", kDefaultZmqControlPort).asInt();
|
||
outDevice.resultPort = root.get("resultPort", kDefaultZmqResultPort).asInt();
|
||
outDevice.rawImagePort = root.get("rawImagePort", kDefaultZmqRawImagePort).asInt();
|
||
outDevice.rtspUrl = root.get("rtsp", "").asString();
|
||
fillRuntimeFields(root, outDevice);
|
||
outDevice.rawJson = jsonStr;
|
||
return !outDevice.machineCode.empty();
|
||
}
|
||
|
||
int WDRemoteReceiver::SearchDevices(std::vector<WDRemoteDeviceInfo>& devices,
|
||
int timeoutMs)
|
||
{
|
||
devices.clear();
|
||
m_lastDevices.clear();
|
||
LOG_DEBUG("[CLIENT-DISCOVER] UDP discovery begin port=%u timeout=%d\n",
|
||
static_cast<unsigned>(WD_REMOTE_DISCOVERY_PORT), timeoutMs);
|
||
|
||
const std::string clientMachineCode = AuthManager::GetMachineCode();
|
||
Json::Value req;
|
||
req["cmd"] = "discover";
|
||
req["machineCode"] = clientMachineCode;
|
||
Json::FastWriter writer;
|
||
const std::string jsonReq = writer.write(req);
|
||
|
||
QUdpSocket socket;
|
||
if (!socket.bind(QHostAddress::AnyIPv4, 0,
|
||
QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint))
|
||
{
|
||
LOG_DEBUG("[CLIENT-DISCOVER] UDP bind failed: %s\n",
|
||
socket.errorString().toStdString().c_str());
|
||
return ERR_CODE(NET_ERR_CREAT_BIND);
|
||
}
|
||
|
||
const QByteArray datagram(jsonReq.data(), static_cast<int>(jsonReq.size()));
|
||
const std::vector<QHostAddress> broadcasts = buildBroadcastAddresses();
|
||
for (const QHostAddress& addr : broadcasts)
|
||
{
|
||
const qint64 sent = socket.writeDatagram(
|
||
datagram, addr, static_cast<quint16>(WD_REMOTE_DISCOVERY_PORT));
|
||
LOG_DEBUG("[CLIENT-DISCOVER] send udp %s:%u bytes=%lld\n",
|
||
addr.toString().toStdString().c_str(),
|
||
static_cast<unsigned>(WD_REMOTE_DISCOVERY_PORT),
|
||
static_cast<long long>(sent));
|
||
}
|
||
|
||
std::set<std::string> seenMachineCodes;
|
||
QElapsedTimer timer;
|
||
timer.start();
|
||
while (timer.elapsed() < timeoutMs)
|
||
{
|
||
const int remain = timeoutMs - static_cast<int>(timer.elapsed());
|
||
if (remain <= 0) break;
|
||
|
||
if (!socket.waitForReadyRead(remain))
|
||
break;
|
||
|
||
while (socket.hasPendingDatagrams())
|
||
{
|
||
QByteArray resp;
|
||
resp.resize(static_cast<int>(socket.pendingDatagramSize()));
|
||
|
||
QHostAddress sender;
|
||
quint16 senderPort = 0;
|
||
const qint64 readBytes =
|
||
socket.readDatagram(resp.data(), resp.size(), &sender, &senderPort);
|
||
if (readBytes <= 0)
|
||
continue;
|
||
resp.resize(static_cast<int>(readBytes));
|
||
|
||
const std::string senderIp = hostAddressToIpv4String(sender);
|
||
const std::string respJson(resp.constData(), static_cast<size_t>(resp.size()));
|
||
|
||
WDRemoteDeviceInfo info;
|
||
if (!parseDeviceInfoJson(respJson, senderIp, info))
|
||
continue;
|
||
if (!seenMachineCodes.insert(info.machineCode).second)
|
||
continue;
|
||
|
||
devices.push_back(info);
|
||
m_lastDevices.push_back(info);
|
||
LOG_DEBUG("[CLIENT-DISCOVER] recv device machine=%s ip=%s ctrl=%d result=%d raw=%d from=%s:%u\n",
|
||
info.machineCode.c_str(), info.serverIp.c_str(),
|
||
info.controlPort, info.resultPort, info.rawImagePort,
|
||
senderIp.c_str(), static_cast<unsigned>(senderPort));
|
||
}
|
||
}
|
||
|
||
return devices.empty() ? ERR_CODE(NET_DEV_NOT_FIND) : 0;
|
||
}
|
||
|
||
int WDRemoteReceiver::Open(const std::string& machineCode, int timeoutMs)
|
||
{
|
||
if (machineCode.empty()) return ERR_CODE(DEV_ARG_INVAILD);
|
||
|
||
std::vector<WDRemoteDeviceInfo> devices = m_lastDevices;
|
||
if (devices.empty())
|
||
SearchDevices(devices, timeoutMs);
|
||
|
||
for (const WDRemoteDeviceInfo& info : devices)
|
||
{
|
||
if (info.machineCode != machineCode)
|
||
continue;
|
||
|
||
Json::Value req;
|
||
req["cmd"] = "open";
|
||
req["machineCode"] = AuthManager::GetMachineCode();
|
||
req["targetMachineCode"] = machineCode;
|
||
Json::FastWriter writer;
|
||
|
||
std::string resp;
|
||
const int r = sendZmqJsonRequest(info.serverIp, info.controlPort,
|
||
writer.write(req), resp, timeoutMs);
|
||
if (r != 0)
|
||
return r;
|
||
|
||
WDRemoteDeviceInfo confirmed;
|
||
if (!parseDeviceInfoJson(resp, info.serverIp, confirmed))
|
||
return ERR_CODE(DATA_ERR_INVALID);
|
||
if (confirmed.machineCode != machineCode)
|
||
return ERR_CODE(DRONESCREW_ERR_MACHINE_CODE_MISMATCH);
|
||
|
||
ConnectConfig cfg;
|
||
cfg.serverIp = confirmed.serverIp;
|
||
cfg.controlPort = confirmed.controlPort;
|
||
cfg.resultPort = confirmed.resultPort;
|
||
cfg.rawImagePort = confirmed.rawImagePort;
|
||
return Connect(cfg);
|
||
}
|
||
|
||
return ERR_CODE(NET_DEV_NOT_FIND);
|
||
}
|
||
|
||
int WDRemoteReceiver::Disconnect()
|
||
{
|
||
if (!m_bConnected.load() && !m_pCtrlClient && !m_pSubResult && !m_pSubRaw)
|
||
return 0;
|
||
|
||
StopSubscribeDetection();
|
||
StopSubscribeRawImage();
|
||
{
|
||
std::lock_guard<std::mutex> stateLock(m_mtxWorkState);
|
||
m_workState = WorkState::Idle;
|
||
}
|
||
|
||
if (m_pCtrlClient)
|
||
{
|
||
m_pCtrlClient->UnInit();
|
||
delete m_pCtrlClient;
|
||
m_pCtrlClient = nullptr;
|
||
}
|
||
|
||
bool was = m_bConnected.exchange(false);
|
||
if (was) emitEvent(WDRemoteEventType::DISCONNECTED, "");
|
||
return 0;
|
||
}
|
||
|
||
bool WDRemoteReceiver::IsConnected() const
|
||
{
|
||
return m_bConnected.load();
|
||
}
|
||
|
||
// =====================================================================
|
||
// 回调注入
|
||
// =====================================================================
|
||
void WDRemoteReceiver::SetDetectionCallback(DetectionCallback cb)
|
||
{
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_mtxCallback);
|
||
m_cbDetection = std::move(cb);
|
||
}
|
||
}
|
||
|
||
void WDRemoteReceiver::SetRawImageCallback(RawImageCallback cb)
|
||
{
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_mtxCallback);
|
||
m_cbRawImage = std::move(cb);
|
||
}
|
||
}
|
||
|
||
void WDRemoteReceiver::SetEventCallback(EventCallback cb)
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_mtxCallback);
|
||
m_cbEvent = std::move(cb);
|
||
}
|
||
|
||
void WDRemoteReceiver::emitEvent(WDRemoteEventType ev, const std::string& msg)
|
||
{
|
||
EventCallback cb;
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_mtxCallback);
|
||
cb = m_cbEvent;
|
||
}
|
||
if (cb) cb(ev, msg);
|
||
}
|
||
|
||
// =====================================================================
|
||
// 控制：REQ-REP 通用发送
|
||
// =====================================================================
|
||
int WDRemoteReceiver::sendCommandAndParse(const std::string& jsonReq,
|
||
std::string& jsonResp,
|
||
int timeoutMs)
|
||
{
|
||
if (!m_bConnected.load())
|
||
{
|
||
LOG_DEBUG("[CLIENT] sendCommand: not connected\n");
|
||
return ERR_CODE(NET_ERR_NOTINIT);
|
||
}
|
||
|
||
const std::string reqPreview = logPreview(jsonReq);
|
||
LOG_DEBUG("[CLIENT] sendCommand bytes=%zu preview=%s\n",
|
||
jsonReq.size(), reqPreview.c_str());
|
||
|
||
std::lock_guard<std::mutex> lk(m_mtxCtrl);
|
||
|
||
int r = sendZmqJsonRequest(m_cfg.serverIp, m_cfg.controlPort,
|
||
jsonReq, jsonResp, timeoutMs);
|
||
if (r != 0)
|
||
{
|
||
LOG_DEBUG("[CLIENT] sendCommand FAIL r=%d\n", r);
|
||
emitEvent(WDRemoteEventType::REQUEST_TIMEOUT, "SendAndWaitBack failed");
|
||
return r;
|
||
}
|
||
const std::string respPreview = logPreview(jsonResp);
|
||
LOG_DEBUG("[CLIENT] sendCommand recv bytes=%zu preview=%s\n",
|
||
jsonResp.size(), respPreview.c_str());
|
||
return 0;
|
||
}
|
||
|
||
int WDRemoteReceiver::SendCustomCommand(const std::string& jsonReq,
|
||
std::string& jsonResp,
|
||
int timeoutMs)
|
||
{
|
||
return sendCommandAndParse(jsonReq, jsonResp, timeoutMs);
|
||
}
|
||
|
||
int WDRemoteReceiver::StartWork(int timeoutMs)
|
||
{
|
||
return StartWork(WDRemoteWorkMode::Detection, timeoutMs);
|
||
}
|
||
|
||
int WDRemoteReceiver::StartWork(WDRemoteWorkMode mode, int timeoutMs)
|
||
{
|
||
std::lock_guard<std::mutex> stateLock(m_mtxWorkState);
|
||
|
||
if (mode == WDRemoteWorkMode::LiveStream)
|
||
{
|
||
Json::Value stopReq;
|
||
stopReq["cmd"] = "stop";
|
||
Json::FastWriter w;
|
||
std::string stopResp;
|
||
const int stopRet = sendCommandAndParse(w.write(stopReq), stopResp, timeoutMs);
|
||
if (stopRet != 0)
|
||
return stopRet;
|
||
const int stopCode = responseErrorCode(stopResp);
|
||
if (stopCode != 0)
|
||
return stopCode;
|
||
|
||
StopSubscribeRawImage();
|
||
|
||
const int subRet = StartSubscribeDetection();
|
||
if (subRet != 0)
|
||
{
|
||
m_workState = WorkState::Idle;
|
||
return subRet;
|
||
}
|
||
|
||
Json::Value req;
|
||
req["cmd"] = "start_stream";
|
||
std::string resp;
|
||
const int r = sendCommandAndParse(w.write(req), resp, timeoutMs);
|
||
if (r != 0)
|
||
{
|
||
StopSubscribeDetection();
|
||
m_workState = WorkState::Idle;
|
||
return r;
|
||
}
|
||
const int respCode = responseErrorCode(resp);
|
||
if (respCode != 0)
|
||
{
|
||
StopSubscribeDetection();
|
||
m_workState = WorkState::Idle;
|
||
return respCode;
|
||
}
|
||
m_workState = WorkState::LiveStream;
|
||
return 0;
|
||
}
|
||
|
||
Json::Value stopStreamReq;
|
||
stopStreamReq["cmd"] = "stop_stream";
|
||
Json::FastWriter w;
|
||
std::string stopStreamResp;
|
||
const int stopStreamRet = sendCommandAndParse(w.write(stopStreamReq), stopStreamResp, timeoutMs);
|
||
if (stopStreamRet != 0)
|
||
return stopStreamRet;
|
||
const int stopStreamCode = responseErrorCode(stopStreamResp);
|
||
if (stopStreamCode != 0)
|
||
return stopStreamCode;
|
||
|
||
int subRet = StartSubscribeDetection();
|
||
if (subRet != 0)
|
||
{
|
||
m_workState = WorkState::Idle;
|
||
return subRet;
|
||
}
|
||
|
||
subRet = StartSubscribeRawImage();
|
||
if (subRet != 0)
|
||
{
|
||
StopSubscribeDetection();
|
||
m_workState = WorkState::Idle;
|
||
return subRet;
|
||
}
|
||
|
||
Json::Value req;
|
||
req["cmd"] = "start";
|
||
req["mode"] = "detection";
|
||
std::string detectMode;
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_mtxDetectMode);
|
||
detectMode = m_detectMode;
|
||
}
|
||
req["detectMode"] = detectMode;
|
||
std::string resp;
|
||
int r = sendCommandAndParse(w.write(req), resp, timeoutMs);
|
||
if (r != 0)
|
||
{
|
||
StopSubscribeRawImage();
|
||
StopSubscribeDetection();
|
||
m_workState = WorkState::Idle;
|
||
return r;
|
||
}
|
||
const int respCode = responseErrorCode(resp);
|
||
if (respCode != 0)
|
||
{
|
||
StopSubscribeRawImage();
|
||
StopSubscribeDetection();
|
||
m_workState = WorkState::Idle;
|
||
return respCode;
|
||
}
|
||
m_workState = WorkState::Detection;
|
||
return 0;
|
||
}
|
||
|
||
int WDRemoteReceiver::StopWork(int timeoutMs)
|
||
{
|
||
const int streamRet = StopWork(WDRemoteWorkMode::LiveStream, timeoutMs);
|
||
const int detectRet = StopWork(WDRemoteWorkMode::Detection, timeoutMs);
|
||
if (streamRet != 0 && detectRet != 0)
|
||
return detectRet;
|
||
if (detectRet != 0) return detectRet;
|
||
if (streamRet != 0) return streamRet;
|
||
return 0;
|
||
}
|
||
|
||
int WDRemoteReceiver::StopWork(WDRemoteWorkMode mode, int timeoutMs)
|
||
{
|
||
std::lock_guard<std::mutex> stateLock(m_mtxWorkState);
|
||
|
||
Json::FastWriter w;
|
||
if (mode == WDRemoteWorkMode::LiveStream)
|
||
{
|
||
Json::Value streamReq;
|
||
streamReq["cmd"] = "stop_stream";
|
||
std::string streamResp;
|
||
const int streamRet = sendCommandAndParse(w.write(streamReq), streamResp, timeoutMs);
|
||
if (streamRet != 0) return streamRet;
|
||
const int streamCode = responseErrorCode(streamResp);
|
||
if (streamCode != 0) return streamCode;
|
||
|
||
if (m_workState == WorkState::LiveStream)
|
||
{
|
||
StopSubscribeDetection();
|
||
StopSubscribeRawImage();
|
||
m_workState = WorkState::Idle;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
Json::Value detectReq;
|
||
detectReq["cmd"] = "stop";
|
||
std::string detectResp;
|
||
const int detectRet = sendCommandAndParse(w.write(detectReq), detectResp, timeoutMs);
|
||
if (detectRet != 0) return detectRet;
|
||
const int detectCode = responseErrorCode(detectResp);
|
||
if (detectCode != 0) return detectCode;
|
||
|
||
WDRemoteDetectionFrame finalFrame;
|
||
if (parseDetectionJson(detectResp, finalFrame) && finalFrame.isFinalResult)
|
||
{
|
||
DetectionCallback cb;
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_mtxCallback);
|
||
cb = m_cbDetection;
|
||
}
|
||
if (cb) cb(finalFrame);
|
||
}
|
||
|
||
if (m_workState == WorkState::Detection)
|
||
{
|
||
StopSubscribeRawImage();
|
||
StopSubscribeDetection();
|
||
m_workState = WorkState::Idle;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
int WDRemoteReceiver::RequestSingleDetection(int timeoutMs)
|
||
{
|
||
Json::Value req;
|
||
req["cmd"] = "single";
|
||
Json::FastWriter w;
|
||
std::string resp;
|
||
int r = sendCommandAndParse(w.write(req), resp, timeoutMs);
|
||
if (r != 0) return r;
|
||
return responseErrorCode(resp);
|
||
}
|
||
|
||
int WDRemoteReceiver::SetExposure(double exposureTime, int timeoutMs)
|
||
{
|
||
Json::Value req;
|
||
req["cmd"] = "set_exposure";
|
||
req["value"] = exposureTime;
|
||
Json::FastWriter w;
|
||
std::string resp;
|
||
int r = sendCommandAndParse(w.write(req), resp, timeoutMs);
|
||
if (r != 0) return r;
|
||
return responseErrorCode(resp);
|
||
}
|
||
|
||
int WDRemoteReceiver::SetGain(double gain, int timeoutMs)
|
||
{
|
||
Json::Value req;
|
||
req["cmd"] = "set_gain";
|
||
req["value"] = gain;
|
||
Json::FastWriter w;
|
||
std::string resp;
|
||
int r = sendCommandAndParse(w.write(req), resp, timeoutMs);
|
||
if (r != 0) return r;
|
||
return responseErrorCode(resp);
|
||
}
|
||
|
||
int WDRemoteReceiver::SetAlgoParams(const WDRemoteAlgoParams& params, int timeoutMs)
|
||
{
|
||
Json::Value req;
|
||
req["cmd"] = "set_algo_params";
|
||
req["score"] = params.scoreThreshold;
|
||
req["nms"] = params.nmsThreshold;
|
||
req["width"] = params.inputWidth;
|
||
req["height"] = params.inputHeight;
|
||
req["modelType"] = params.modelType;
|
||
req["expectedBoltCount"] = params.expectedBoltCount;
|
||
Json::FastWriter w;
|
||
std::string resp;
|
||
int r = sendCommandAndParse(w.write(req), resp, timeoutMs);
|
||
if (r != 0) return r;
|
||
return responseErrorCode(resp);
|
||
}
|
||
|
||
void WDRemoteReceiver::SetDetectMode(const std::string& mode)
|
||
{
|
||
if (mode == "mono" || mode == "binocular")
|
||
{
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_mtxDetectMode);
|
||
m_detectMode = mode;
|
||
}
|
||
LOG_DEBUG("[CLIENT] detectMode set to: %s\n", mode.c_str());
|
||
if (m_bConnected.load())
|
||
{
|
||
Json::Value req;
|
||
req["cmd"] = "set_detect_mode";
|
||
req["detectMode"] = mode;
|
||
Json::FastWriter w;
|
||
std::string resp;
|
||
const int r = sendCommandAndParse(w.write(req), resp, 1000);
|
||
if (r != 0)
|
||
{
|
||
LOG_DEBUG("[CLIENT] set_detect_mode sync failed ret=%d mode=%s\n",
|
||
r, m_detectMode.c_str());
|
||
}
|
||
else
|
||
{
|
||
const int code = responseErrorCode(resp);
|
||
if (code != 0)
|
||
LOG_DEBUG("[CLIENT] set_detect_mode rejected code=%d mode=%s\n",
|
||
code, m_detectMode.c_str());
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
std::string current;
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_mtxDetectMode);
|
||
current = m_detectMode;
|
||
}
|
||
LOG_WARN("[CLIENT] unknown detectMode: %s, keep: %s\n",
|
||
mode.c_str(), current.c_str());
|
||
}
|
||
}
|
||
|
||
WDRemoteServerInfo WDRemoteReceiver::GetServerInfo(int timeoutMs)
|
||
{
|
||
WDRemoteServerInfo info;
|
||
Json::Value req;
|
||
req["cmd"] = "get_info";
|
||
Json::FastWriter w;
|
||
std::string resp;
|
||
int r = sendCommandAndParse(w.write(req), resp, timeoutMs);
|
||
info.rawJson = resp;
|
||
if (r != 0) return info;
|
||
|
||
Json::Value root;
|
||
Json::Reader reader;
|
||
if (!reader.parse(resp, root)) return info;
|
||
if (!root.get("ok", false).asBool()) return info;
|
||
|
||
info.ok = true;
|
||
info.rtspUrl = root.get("rtsp", "").asString();
|
||
info.version = root.get("version", "").asString();
|
||
info.controlPort = root.get("controlPort", 0 ).asInt();
|
||
info.resultPort = root.get("resultPort", 0 ).asInt();
|
||
info.rawImagePort = root.get("rawImagePort", 0 ).asInt();
|
||
fillRuntimeFields(root, info);
|
||
return info;
|
||
}
|
||
|
||
// =====================================================================
|
||
// PUB-SUB 订阅启停
|
||
// =====================================================================
|
||
int WDRemoteReceiver::StartSubscribeDetection()
|
||
{
|
||
if (m_pSubResult) return 0;
|
||
if (!IVrZeroMQSubscriber::CreateObject(&m_pSubResult) || !m_pSubResult)
|
||
return ERR_CODE(NET_ERR_CREAT_INIT);
|
||
|
||
auto onMsg = [this](const char* topic, const size_t topicLen,
|
||
const char* data, const size_t dataLen) {
|
||
this->onResultMessage(topic, topicLen, data, dataLen);
|
||
};
|
||
|
||
int r = m_pSubResult->Init(m_cfg.serverIp.c_str(), m_cfg.resultPort,
|
||
"result", onMsg);
|
||
if (r != 0)
|
||
{
|
||
delete m_pSubResult;
|
||
m_pSubResult = nullptr;
|
||
return r;
|
||
}
|
||
LOG_DEBUG("[CLIENT-SUB] result subscribe tcp://%s:%d topic=result\n",
|
||
m_cfg.serverIp.c_str(), m_cfg.resultPort);
|
||
return 0;
|
||
}
|
||
|
||
int WDRemoteReceiver::StopSubscribeDetection()
|
||
{
|
||
if (!m_pSubResult) return 0;
|
||
m_pSubResult->UnInit();
|
||
delete m_pSubResult;
|
||
m_pSubResult = nullptr;
|
||
return 0;
|
||
}
|
||
|
||
int WDRemoteReceiver::StartSubscribeRawImage()
|
||
{
|
||
if (m_pSubRaw) return 0;
|
||
if (m_cfg.rawImagePort <= 0) return ERR_CODE(NET_ERR_ARG);
|
||
if (!IVrZeroMQSubscriber::CreateObject(&m_pSubRaw) || !m_pSubRaw)
|
||
return ERR_CODE(NET_ERR_CREAT_INIT);
|
||
|
||
auto onMsg = [this](const char* topic, const size_t topicLen,
|
||
const char* data, const size_t dataLen) {
|
||
this->onRawImageMessage(topic, topicLen, data, dataLen);
|
||
};
|
||
|
||
int r = m_pSubRaw->Init(m_cfg.serverIp.c_str(), m_cfg.rawImagePort,
|
||
"raw", onMsg);
|
||
if (r != 0)
|
||
{
|
||
delete m_pSubRaw;
|
||
m_pSubRaw = nullptr;
|
||
return r;
|
||
}
|
||
LOG_DEBUG("[CLIENT-SUB] raw subscribe tcp://%s:%d topic=raw\n",
|
||
m_cfg.serverIp.c_str(), m_cfg.rawImagePort);
|
||
return 0;
|
||
}
|
||
|
||
int WDRemoteReceiver::StopSubscribeRawImage()
|
||
{
|
||
if (!m_pSubRaw) return 0;
|
||
m_pSubRaw->UnInit();
|
||
delete m_pSubRaw;
|
||
m_pSubRaw = nullptr;
|
||
return 0;
|
||
}
|
||
|
||
// =====================================================================
|
||
// 订阅消息分发
|
||
// =====================================================================
|
||
void WDRemoteReceiver::onResultMessage(const char* /*topic*/, size_t /*topicLen*/,
|
||
const char* data, size_t dataLen)
|
||
{
|
||
if (!data || dataLen == 0) return;
|
||
static int resultCnt = 0;
|
||
++resultCnt;
|
||
if (resultCnt == 1 || resultCnt % 100 == 0)
|
||
LOG_DEBUG("[CLIENT-SUB] result #%d size=%zu\n", resultCnt, dataLen);
|
||
std::string json(data, dataLen);
|
||
|
||
WDRemoteDetectionFrame frame;
|
||
if (!parseDetectionJson(json, frame)) return;
|
||
|
||
DetectionCallback cb;
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_mtxCallback);
|
||
cb = m_cbDetection;
|
||
}
|
||
if (cb) cb(frame);
|
||
}
|
||
|
||
void WDRemoteReceiver::onRawImageMessage(const char* /*topic*/, size_t /*topicLen*/,
|
||
const char* data, size_t dataLen)
|
||
{
|
||
if (!data || dataLen < sizeof(WDRemoteRawImageHeader)) return;
|
||
static int rawCnt = 0;
|
||
++rawCnt;
|
||
if (rawCnt == 1 || rawCnt % 100 == 0)
|
||
LOG_DEBUG("[CLIENT-SUB] raw #%d size=%zu\n", rawCnt, dataLen);
|
||
|
||
// 先尝试解析为双目图像
|
||
if (dataLen >= sizeof(WDRemoteBinocularRawImageHeader))
|
||
{
|
||
WDRemoteBinocularRawImageHeader binocHdr{};
|
||
std::memcpy(&binocHdr, data, sizeof(binocHdr));
|
||
|
||
if (binocHdr.magic == WD_REMOTE_BINOCULAR_RAW_IMAGE_MAGIC &&
|
||
binocHdr.version == WD_REMOTE_BINOCULAR_RAW_IMAGE_VERSION)
|
||
{
|
||
// 双目图像
|
||
size_t expectedSize = sizeof(binocHdr) + binocHdr.leftDataSize + binocHdr.rightDataSize;
|
||
if (dataLen < expectedSize) return;
|
||
|
||
const uint8_t* leftPayload =
|
||
reinterpret_cast<const uint8_t*>(data + sizeof(binocHdr));
|
||
const uint8_t* rightPayload =
|
||
reinterpret_cast<const uint8_t*>(data + sizeof(binocHdr) + binocHdr.leftDataSize);
|
||
const uint32_t compression = binocHdr.reserved[0];
|
||
std::string detectMode;
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_mtxDetectMode);
|
||
detectMode = m_detectMode;
|
||
}
|
||
const bool localMono = (detectMode == "mono");
|
||
std::vector<uint8_t> leftDecoded;
|
||
std::vector<uint8_t> rightDecoded;
|
||
|
||
WDRemoteBinocularRawImage img;
|
||
img.frameId = binocHdr.frameId;
|
||
img.timestampUs = binocHdr.timestampUs;
|
||
|
||
img.leftWidth = static_cast<int>(binocHdr.leftWidth);
|
||
img.leftHeight = static_cast<int>(binocHdr.leftHeight);
|
||
img.leftSourceWidth = binocHdr.reserved[3] > 0
|
||
? static_cast<int>(binocHdr.reserved[3])
|
||
: 0;
|
||
img.leftSourceHeight = binocHdr.reserved[4] > 0
|
||
? static_cast<int>(binocHdr.reserved[4])
|
||
: 0;
|
||
img.leftStride = static_cast<int>(binocHdr.leftStride);
|
||
img.leftPixelFormat = static_cast<WDRemotePixelFormat>(binocHdr.leftPixelFormat);
|
||
img.leftData = leftPayload;
|
||
img.leftDataSize = binocHdr.leftDataSize;
|
||
|
||
img.rightWidth = static_cast<int>(binocHdr.rightWidth);
|
||
img.rightHeight = static_cast<int>(binocHdr.rightHeight);
|
||
img.rightSourceWidth = img.rightWidth;
|
||
img.rightSourceHeight = img.rightHeight;
|
||
img.rightStride = static_cast<int>(binocHdr.rightStride);
|
||
img.rightPixelFormat = static_cast<WDRemotePixelFormat>(binocHdr.rightPixelFormat);
|
||
img.rightData = binocHdr.rightDataSize > 0 ? rightPayload : nullptr;
|
||
img.rightDataSize = binocHdr.rightDataSize;
|
||
|
||
if (compression == WD_REMOTE_IMAGE_COMPRESSION_PNG ||
|
||
compression == WD_REMOTE_IMAGE_COMPRESSION_MJPEG)
|
||
{
|
||
// PNG 与 MJPEG/JPEG 都用 cv::imdecode 解码（IMREAD_UNCHANGED 按内容自动识别格式）
|
||
if (!decodePngImage(leftPayload, binocHdr.leftDataSize,
|
||
img.leftWidth, img.leftHeight,
|
||
img.leftPixelFormat, img.leftStride,
|
||
leftDecoded))
|
||
{
|
||
LOG_DEBUG("[CLIENT-SUB] raw decode left failed comp=%u frame=%llu size=%u\n",
|
||
compression,
|
||
static_cast<unsigned long long>(binocHdr.frameId),
|
||
binocHdr.leftDataSize);
|
||
return;
|
||
}
|
||
img.leftData = leftDecoded.data();
|
||
img.leftDataSize = leftDecoded.size();
|
||
|
||
if (!localMono && binocHdr.rightDataSize > 0)
|
||
{
|
||
if (!decodePngImage(rightPayload, binocHdr.rightDataSize,
|
||
img.rightWidth, img.rightHeight,
|
||
img.rightPixelFormat, img.rightStride,
|
||
rightDecoded))
|
||
{
|
||
LOG_DEBUG("[CLIENT-SUB] raw decode right failed comp=%u frame=%llu size=%u\n",
|
||
compression,
|
||
static_cast<unsigned long long>(binocHdr.frameId),
|
||
binocHdr.rightDataSize);
|
||
return;
|
||
}
|
||
img.rightData = rightDecoded.data();
|
||
img.rightDataSize = rightDecoded.size();
|
||
}
|
||
}
|
||
else if (compression == WD_REMOTE_IMAGE_COMPRESSION_ZLIB)
|
||
{
|
||
// 真·无损：Qt qUncompress 解回原始字节（与服务端 qCompress 配对，跨平台无额外依赖）。
|
||
// 图像格式/尺寸/stride 沿用包头（解出的就是原始裸帧）。
|
||
QByteArray leftRaw = qUncompress(reinterpret_cast<const uchar*>(leftPayload),
|
||
static_cast<int>(binocHdr.leftDataSize));
|
||
if (leftRaw.isEmpty())
|
||
{
|
||
LOG_DEBUG("[CLIENT-SUB] zlib inflate left failed frame=%llu size=%u\n",
|
||
static_cast<unsigned long long>(binocHdr.frameId),
|
||
binocHdr.leftDataSize);
|
||
return;
|
||
}
|
||
leftDecoded.assign(reinterpret_cast<const uint8_t*>(leftRaw.constData()),
|
||
reinterpret_cast<const uint8_t*>(leftRaw.constData()) + leftRaw.size());
|
||
img.leftData = leftDecoded.data();
|
||
img.leftDataSize = leftDecoded.size();
|
||
|
||
if (!localMono && binocHdr.rightDataSize > 0)
|
||
{
|
||
QByteArray rightRaw = qUncompress(reinterpret_cast<const uchar*>(rightPayload),
|
||
static_cast<int>(binocHdr.rightDataSize));
|
||
if (rightRaw.isEmpty())
|
||
{
|
||
LOG_DEBUG("[CLIENT-SUB] zlib inflate right failed frame=%llu size=%u\n",
|
||
static_cast<unsigned long long>(binocHdr.frameId),
|
||
binocHdr.rightDataSize);
|
||
return;
|
||
}
|
||
rightDecoded.assign(reinterpret_cast<const uint8_t*>(rightRaw.constData()),
|
||
reinterpret_cast<const uint8_t*>(rightRaw.constData()) + rightRaw.size());
|
||
img.rightData = rightDecoded.data();
|
||
img.rightDataSize = rightDecoded.size();
|
||
}
|
||
}
|
||
else if (compression != WD_REMOTE_IMAGE_COMPRESSION_RAW)
|
||
{
|
||
LOG_DEBUG("[CLIENT-SUB] raw unknown compression=%u frame=%llu\n",
|
||
compression,
|
||
static_cast<unsigned long long>(binocHdr.frameId));
|
||
return;
|
||
}
|
||
|
||
if (localMono)
|
||
{
|
||
img.rightWidth = 0;
|
||
img.rightHeight = 0;
|
||
img.rightStride = 0;
|
||
img.rightPixelFormat = WDRemotePixelFormat::UNKNOWN;
|
||
img.rightData = nullptr;
|
||
img.rightDataSize = 0;
|
||
}
|
||
|
||
if (rawCnt == 1 || rawCnt % 100 == 0)
|
||
{
|
||
LOG_DEBUG("[CLIENT-SUB] raw #%d compression=%s left=%dx%d size=%zu right=%dx%d size=%zu\n",
|
||
rawCnt,
|
||
rawCompressionName(compression),
|
||
img.leftWidth, img.leftHeight, img.leftDataSize,
|
||
img.rightWidth, img.rightHeight, img.rightDataSize);
|
||
}
|
||
|
||
RawImageCallback cb;
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_mtxCallback);
|
||
cb = m_cbRawImage;
|
||
}
|
||
if (cb) cb(img);
|
||
return;
|
||
}
|
||
}
|
||
|
||
// 单目图像 → 包装为双目（仅左眼有效）
|
||
WDRemoteRawImageHeader hdr{};
|
||
std::memcpy(&hdr, data, sizeof(hdr));
|
||
|
||
if (hdr.magic != WD_REMOTE_RAW_IMAGE_MAGIC) return;
|
||
if (hdr.version != WD_REMOTE_RAW_IMAGE_VERSION) return;
|
||
|
||
if (dataLen < sizeof(hdr) + hdr.dataSize) return;
|
||
|
||
WDRemoteBinocularRawImage img;
|
||
img.frameId = hdr.frameId;
|
||
img.timestampUs = hdr.timestampUs;
|
||
|
||
img.leftWidth = static_cast<int>(hdr.width);
|
||
img.leftHeight = static_cast<int>(hdr.height);
|
||
img.leftSourceWidth = img.leftWidth;
|
||
img.leftSourceHeight = img.leftHeight;
|
||
img.leftStride = static_cast<int>(hdr.stride);
|
||
img.leftPixelFormat = static_cast<WDRemotePixelFormat>(hdr.pixelFormat);
|
||
img.leftData = reinterpret_cast<const uint8_t*>(data + sizeof(hdr));
|
||
img.leftDataSize = hdr.dataSize;
|
||
|
||
// 右眼置空
|
||
img.rightWidth = 0;
|
||
img.rightHeight = 0;
|
||
img.rightStride = 0;
|
||
img.rightPixelFormat = WDRemotePixelFormat::UNKNOWN;
|
||
img.rightData = nullptr;
|
||
img.rightDataSize = 0;
|
||
|
||
{
|
||
RawImageCallback cb;
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_mtxCallback);
|
||
cb = m_cbRawImage;
|
||
}
|
||
if (cb) cb(img);
|
||
}
|
||
}
|
||
|
||
// =====================================================================
|
||
// JSON → WDRemoteDetectionFrame
|
||
// =====================================================================
|
||
bool WDRemoteReceiver::parseDetectionJson(const std::string& jsonStr,
|
||
WDRemoteDetectionFrame& outFrame)
|
||
{
|
||
Json::Value root;
|
||
Json::Reader reader;
|
||
if (!reader.parse(jsonStr, root)) return false;
|
||
|
||
outFrame.frameId = static_cast<uint64_t>(root.get("frameId", 0).asInt64());
|
||
outFrame.timestampUs = static_cast<int64_t> (root.get("ts", 0).asInt64());
|
||
outFrame.imageWidth = root.get("width", 0).asInt();
|
||
outFrame.imageHeight = root.get("height", 0).asInt();
|
||
outFrame.success = root.get("success", true).asBool();
|
||
outFrame.errorCode = root.get("errorCode", 0).asInt();
|
||
outFrame.message = root.get("msg", "").asString();
|
||
outFrame.isFinalResult = root.get("isFinalResult",
|
||
root.get("resultFinal", false)).asBool();
|
||
outFrame.rankScore = root.get("rankScore", 0.0).asDouble();
|
||
outFrame.rankSampleCount = root.get("rankSampleCount", 0).asInt();
|
||
outFrame.saveIndex = static_cast<uint64_t>(root.get("saveIndex", 0).asInt64());
|
||
|
||
outFrame.boxes.clear();
|
||
outFrame.distances.clear();
|
||
|
||
const Json::Value& bs = root["boxes"];
|
||
if (bs.isArray())
|
||
{
|
||
outFrame.boxes.reserve(bs.size());
|
||
for (unsigned i = 0; i < bs.size(); ++i)
|
||
{
|
||
const Json::Value& b = bs[i];
|
||
WDRemoteDetectionBox box;
|
||
box.classId = b.get("cls", 0 ).asInt();
|
||
box.score = static_cast<float>(b.get("score", 0.0).asDouble());
|
||
box.x = b.get("x", 0 ).asInt();
|
||
box.y = b.get("y", 0 ).asInt();
|
||
box.width = b.get("w", 0 ).asInt();
|
||
box.height = b.get("h", 0 ).asInt();
|
||
box.hasPhysicalHeight = b.isMember("height_mm");
|
||
box.physicalHeightMm = static_cast<float>(b.get("height_mm", 0.0).asDouble());
|
||
box.hasStereoMatch = b.get("hasStereoMatch", false).asBool();
|
||
box.trusted = b.get("trusted", false).asBool();
|
||
box.matchConfidence = b.get("matchConfidence", 1).asInt();
|
||
box.matchStatus = b.get("matchStatus", "").asString();
|
||
box.pairId = b.get("pairId", -1).asInt();
|
||
box.leftIndex = b.get("leftIndex", -1).asInt();
|
||
box.rightIndex = b.get("rightIndex", -1).asInt();
|
||
outFrame.boxes.push_back(box);
|
||
}
|
||
}
|
||
|
||
const Json::Value& ds = root["distances"];
|
||
if (ds.isArray())
|
||
{
|
||
outFrame.distances.reserve(ds.size());
|
||
for (unsigned i = 0; i < ds.size(); ++i)
|
||
{
|
||
const Json::Value& d = ds[i];
|
||
WDRemoteDetectionDistance distance;
|
||
distance.fromId = d.get("fromId", 0).asInt();
|
||
distance.toId = d.get("toId", 0).asInt();
|
||
distance.distanceMm = static_cast<float>(d.get("distanceMm", 0.0).asDouble());
|
||
outFrame.distances.push_back(distance);
|
||
}
|
||
}
|
||
return true;
|
||
}
|