452 lines
13 KiB
C++
452 lines
13 KiB
C++
#include "BinocularMarkTcpProtocol.h"
|
||
#include "VrLog.h"
|
||
#include <QDateTime>
|
||
#include <QBuffer>
|
||
#include <cstring>
|
||
|
||
// 静态实例指针
|
||
BinocularMarkTcpProtocol* BinocularMarkTcpProtocol::s_pInstance = nullptr;
|
||
|
||
BinocularMarkTcpProtocol::BinocularMarkTcpProtocol(QObject *parent)
|
||
: QObject(parent)
|
||
, m_pTcpServer(nullptr)
|
||
, m_pHeartbeatTimer(nullptr)
|
||
, m_nHeartbeatInterval(30)
|
||
, m_nTcpPort(5901)
|
||
{
|
||
s_pInstance = this;
|
||
|
||
// 创建TCP服务器
|
||
VrCreatYTCPServer(&m_pTcpServer);
|
||
|
||
// 创建心跳定时器
|
||
m_pHeartbeatTimer = new QTimer(this);
|
||
connect(m_pHeartbeatTimer, &QTimer::timeout, this, &BinocularMarkTcpProtocol::onHeartbeatTimeout);
|
||
|
||
LOG_INFO("BinocularMarkTcpProtocol created\n");
|
||
}
|
||
|
||
BinocularMarkTcpProtocol::~BinocularMarkTcpProtocol()
|
||
{
|
||
stopHeartbeat();
|
||
stopServer();
|
||
|
||
if (m_pTcpServer != nullptr)
|
||
{
|
||
delete m_pTcpServer;
|
||
m_pTcpServer = nullptr;
|
||
}
|
||
|
||
s_pInstance = nullptr;
|
||
LOG_INFO("BinocularMarkTcpProtocol destroyed\n");
|
||
}
|
||
|
||
bool BinocularMarkTcpProtocol::startServer(quint16 port)
|
||
{
|
||
if (m_pTcpServer == nullptr)
|
||
{
|
||
LOG_ERROR("TCP server not created\n");
|
||
return false;
|
||
}
|
||
|
||
m_nTcpPort = port;
|
||
|
||
// 初始化TCP服务器
|
||
if (!m_pTcpServer->Init(port, false))
|
||
{
|
||
LOG_ERROR("Failed to init TCP server on port %d\n", port);
|
||
return false;
|
||
}
|
||
|
||
// 设置事件回调
|
||
m_pTcpServer->SetEventCallback(tcpEventCallback);
|
||
|
||
// 启动TCP服务器
|
||
if (!m_pTcpServer->Start(tcpRecvCallback, false))
|
||
{
|
||
LOG_ERROR("Failed to start TCP server\n");
|
||
return false;
|
||
}
|
||
|
||
LOG_INFO("TCP server started on port %d\n", port);
|
||
return true;
|
||
}
|
||
|
||
void BinocularMarkTcpProtocol::stopServer()
|
||
{
|
||
if (m_pTcpServer != nullptr)
|
||
{
|
||
m_pTcpServer->Stop();
|
||
m_pTcpServer->Close();
|
||
LOG_INFO("TCP server stopped\n");
|
||
}
|
||
|
||
// 清空客户端缓冲区
|
||
m_clientBuffers.clear();
|
||
}
|
||
|
||
void BinocularMarkTcpProtocol::startHeartbeat(int heartbeatInterval)
|
||
{
|
||
m_nHeartbeatInterval = heartbeatInterval;
|
||
m_pHeartbeatTimer->start(heartbeatInterval * 1000);
|
||
LOG_INFO("Heartbeat started, interval: %d seconds\n", heartbeatInterval);
|
||
}
|
||
|
||
void BinocularMarkTcpProtocol::stopHeartbeat()
|
||
{
|
||
if (m_pHeartbeatTimer != nullptr)
|
||
{
|
||
m_pHeartbeatTimer->stop();
|
||
}
|
||
LOG_INFO("Heartbeat stopped\n");
|
||
}
|
||
|
||
void BinocularMarkTcpProtocol::onHeartbeatTimeout()
|
||
{
|
||
// 发送心跳消息给所有客户端
|
||
QJsonObject heartbeatObj;
|
||
heartbeatObj["msg_type"] = "heartbeat";
|
||
heartbeatObj["timestamp"] = QDateTime::currentMSecsSinceEpoch();
|
||
|
||
QJsonDocument doc(heartbeatObj);
|
||
QByteArray jsonData = doc.toJson(QJsonDocument::Compact);
|
||
QByteArray frameData = buildFrame(jsonData);
|
||
|
||
if (m_pTcpServer != nullptr)
|
||
{
|
||
m_pTcpServer->SendAllData(frameData.data(), frameData.size());
|
||
}
|
||
}
|
||
|
||
void BinocularMarkTcpProtocol::tcpRecvCallback(const TCPClient* pClient, const char* pData, const unsigned int nLen)
|
||
{
|
||
if (s_pInstance != nullptr)
|
||
{
|
||
s_pInstance->handleReceivedData(pClient, pData, nLen);
|
||
}
|
||
}
|
||
|
||
void BinocularMarkTcpProtocol::tcpEventCallback(const TCPClient* pClient, TCPServerEventType eventType)
|
||
{
|
||
if (s_pInstance == nullptr)
|
||
return;
|
||
|
||
QString clientId = s_pInstance->generateClientId(pClient);
|
||
|
||
switch (eventType)
|
||
{
|
||
case TCP_EVENT_CLIENT_CONNECTED:
|
||
LOG_INFO("Client connected: %s\n", clientId.toStdString().c_str());
|
||
s_pInstance->m_clientBuffers[clientId].clear();
|
||
break;
|
||
|
||
case TCP_EVENT_CLIENT_DISCONNECTED:
|
||
LOG_INFO("Client disconnected: %s\n", clientId.toStdString().c_str());
|
||
s_pInstance->m_clientBuffers.remove(clientId);
|
||
break;
|
||
|
||
case TCP_EVENT_CLIENT_EXCEPTION:
|
||
LOG_WARN("Client exception: %s\n", clientId.toStdString().c_str());
|
||
s_pInstance->m_clientBuffers.remove(clientId);
|
||
break;
|
||
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
void BinocularMarkTcpProtocol::handleReceivedData(const TCPClient* pClient, const char* pData, unsigned int nLen)
|
||
{
|
||
QString clientId = generateClientId(pClient);
|
||
|
||
// 将接收到的数据追加到客户端缓冲区
|
||
QByteArray receivedData(pData, nLen);
|
||
m_clientBuffers[clientId].append(receivedData);
|
||
|
||
// 解析数据帧（处理粘包）
|
||
QList<QByteArray> jsonDataList;
|
||
int frameCount = parseFrames(clientId, m_clientBuffers[clientId], jsonDataList);
|
||
|
||
// 处理每个完整的JSON消息
|
||
for (const QByteArray& jsonData : jsonDataList)
|
||
{
|
||
handleJsonMessage(pClient, jsonData);
|
||
}
|
||
|
||
LOG_DEBUG("Received %u bytes from %s, parsed %d frames\n",
|
||
nLen, clientId.toStdString().c_str(), frameCount);
|
||
}
|
||
|
||
QByteArray BinocularMarkTcpProtocol::buildFrame(const QByteArray& jsonData)
|
||
{
|
||
QByteArray frame;
|
||
|
||
// 帧头（8字节）
|
||
frame.append(FRAME_HEADER, FRAME_HEADER_SIZE);
|
||
|
||
// 写入数据长度（8位字符串格式）
|
||
quint64 dataLength = jsonData.size();
|
||
char lengthStr[9]; // 8位数字 + '\0'
|
||
#ifdef _WIN32
|
||
sprintf_s(lengthStr, "%08u", dataLength);
|
||
#else
|
||
sprintf(lengthStr, "%08u", dataLength);
|
||
#endif
|
||
frame.append(lengthStr, FRAME_LENGTH_SIZE);
|
||
|
||
// JSON数据
|
||
frame.append(jsonData);
|
||
|
||
// 帧尾（4字节）
|
||
frame.append(FRAME_TAIL, FRAME_TAIL_SIZE);
|
||
|
||
return frame;
|
||
}
|
||
|
||
int BinocularMarkTcpProtocol::parseFrames(const QString& clientId, const QByteArray& data, QList<QByteArray>& outJsonData)
|
||
{
|
||
QByteArray& buffer = m_clientBuffers[clientId];
|
||
int frameCount = 0;
|
||
|
||
while (true)
|
||
{
|
||
// 检查缓冲区是否有足够数据（至少包含帧头+长度字段）
|
||
if (buffer.size() < FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE)
|
||
break;
|
||
|
||
// 查找帧头
|
||
int headerPos = buffer.indexOf(FRAME_HEADER, 0);
|
||
if (headerPos < 0)
|
||
{
|
||
// 没有找到帧头，清空缓冲区
|
||
buffer.clear();
|
||
break;
|
||
}
|
||
|
||
// 如果帧头不在起始位置，丢弃之前的数据
|
||
if (headerPos > 0)
|
||
{
|
||
buffer.remove(0, headerPos);
|
||
}
|
||
|
||
// 检查是否有完整的长度字段
|
||
if (buffer.size() < FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE)
|
||
break;
|
||
|
||
// 读取数据长度（8字节ASCII字符串格式，如 "00001234"）
|
||
QByteArray lengthStr = buffer.mid(FRAME_HEADER_SIZE, FRAME_LENGTH_SIZE);
|
||
bool ok = false;
|
||
quint64 dataLength = lengthStr.toULongLong(&ok);
|
||
|
||
if (!ok)
|
||
{
|
||
LOG_ERROR("Invalid length string: %s\n", lengthStr.toStdString().c_str());
|
||
buffer.remove(0, FRAME_HEADER_SIZE);
|
||
continue;
|
||
}
|
||
|
||
// 计算完整帧的长度
|
||
quint64 frameLength = FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE + dataLength + FRAME_TAIL_SIZE;
|
||
|
||
// 检查是否接收到完整的帧
|
||
if (buffer.size() < static_cast<int>(frameLength))
|
||
break;
|
||
|
||
// 检查帧尾
|
||
int tailPos = FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE + dataLength;
|
||
if (buffer.mid(tailPos, FRAME_TAIL_SIZE) != QByteArray(FRAME_TAIL, FRAME_TAIL_SIZE))
|
||
{
|
||
// 帧尾不匹配，丢弃当前帧头，继续查找下一个帧头
|
||
buffer.remove(0, FRAME_HEADER_SIZE);
|
||
continue;
|
||
}
|
||
|
||
// 提取JSON数据
|
||
QByteArray jsonData = buffer.mid(FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE, dataLength);
|
||
outJsonData.append(jsonData);
|
||
|
||
// 从缓冲区移除已处理的帧
|
||
buffer.remove(0, frameLength);
|
||
|
||
frameCount++;
|
||
}
|
||
|
||
return frameCount;
|
||
}
|
||
|
||
MarkMessageType BinocularMarkTcpProtocol::parseMessageType(const QString& msgTypeStr)
|
||
{
|
||
if (msgTypeStr == "mark_result")
|
||
return MarkMessageType::MARK_RESULT;
|
||
else if (msgTypeStr == "heartbeat")
|
||
return MarkMessageType::HEARTBEAT;
|
||
else if (msgTypeStr == "heartbeat_ack")
|
||
return MarkMessageType::HEARTBEAT_ACK;
|
||
else if (msgTypeStr == "trigger")
|
||
return MarkMessageType::CMD_TRIGGER;
|
||
else if (msgTypeStr == "cmd_response")
|
||
return MarkMessageType::CMD_RESPONSE;
|
||
else
|
||
return MarkMessageType::UNKNOWN;
|
||
}
|
||
|
||
void BinocularMarkTcpProtocol::handleJsonMessage(const TCPClient* pClient, const QByteArray& jsonData)
|
||
{
|
||
QJsonParseError parseError;
|
||
QJsonDocument doc = QJsonDocument::fromJson(jsonData, &parseError);
|
||
|
||
if (parseError.error != QJsonParseError::NoError)
|
||
{
|
||
LOG_ERROR("Failed to parse JSON: %s\n", parseError.errorString().toStdString().c_str());
|
||
return;
|
||
}
|
||
|
||
if (!doc.isObject())
|
||
{
|
||
LOG_ERROR("JSON is not an object\n");
|
||
return;
|
||
}
|
||
|
||
QJsonObject jsonObj = doc.object();
|
||
|
||
// 解析消息类型
|
||
QString msgTypeStr = jsonObj["msg_type"].toString();
|
||
MarkMessageType msgType = parseMessageType(msgTypeStr);
|
||
|
||
// 根据消息类型处理
|
||
switch (msgType)
|
||
{
|
||
case MarkMessageType::HEARTBEAT:
|
||
handleHeartbeat(pClient, jsonObj);
|
||
break;
|
||
|
||
case MarkMessageType::CMD_TRIGGER:
|
||
handleTriggerCommand(pClient, jsonObj);
|
||
break;
|
||
|
||
case MarkMessageType::HEARTBEAT_ACK:
|
||
// 心跳应答，不做处理
|
||
break;
|
||
|
||
default:
|
||
LOG_WARN("Unknown message type: %s\n", msgTypeStr.toStdString().c_str());
|
||
break;
|
||
}
|
||
}
|
||
|
||
void BinocularMarkTcpProtocol::handleHeartbeat(const TCPClient* pClient, const QJsonObject& jsonObj)
|
||
{
|
||
// 发送心跳应答
|
||
sendHeartbeatAck(pClient);
|
||
}
|
||
|
||
void BinocularMarkTcpProtocol::handleTriggerCommand(const TCPClient* pClient, const QJsonObject& jsonObj)
|
||
{
|
||
// 触发检测
|
||
emit triggerDetection();
|
||
|
||
// 发送命令应答
|
||
sendCommandResponse(pClient, "trigger", true, 0, "OK");
|
||
}
|
||
|
||
void BinocularMarkTcpProtocol::sendHeartbeatAck(const TCPClient* pClient)
|
||
{
|
||
QJsonObject ackObj;
|
||
ackObj["msg_type"] = "heartbeat_ack";
|
||
ackObj["timestamp"] = QDateTime::currentMSecsSinceEpoch();
|
||
|
||
QJsonDocument doc(ackObj);
|
||
QByteArray jsonData = doc.toJson(QJsonDocument::Compact);
|
||
QByteArray frameData = buildFrame(jsonData);
|
||
|
||
if (m_pTcpServer != nullptr)
|
||
{
|
||
m_pTcpServer->SendData(pClient, frameData.data(), frameData.size());
|
||
}
|
||
}
|
||
|
||
void BinocularMarkTcpProtocol::sendCommandResponse(const TCPClient* pClient, const QString& cmdType,
|
||
bool result, int errorCode, const QString& errorMsg)
|
||
{
|
||
QJsonObject responseObj;
|
||
responseObj["msg_type"] = "cmd_response";
|
||
responseObj["cmd_type"] = cmdType;
|
||
responseObj["result"] = result;
|
||
responseObj["error_code"] = errorCode;
|
||
responseObj["error_msg"] = errorMsg;
|
||
responseObj["timestamp"] = QDateTime::currentMSecsSinceEpoch();
|
||
|
||
QJsonDocument doc(responseObj);
|
||
QByteArray jsonData = doc.toJson(QJsonDocument::Compact);
|
||
QByteArray frameData = buildFrame(jsonData);
|
||
|
||
if (m_pTcpServer != nullptr)
|
||
{
|
||
m_pTcpServer->SendData(pClient, frameData.data(), frameData.size());
|
||
}
|
||
}
|
||
|
||
void BinocularMarkTcpProtocol::sendMarkResult(const std::vector<SWD_charuco3DMark>& marks,
|
||
const cv::Mat& leftImage,
|
||
const cv::Mat& rightImage,
|
||
int errorCode)
|
||
{
|
||
QJsonObject resultObj;
|
||
resultObj["msg_type"] = "mark_result";
|
||
resultObj["timestamp"] = QDateTime::currentMSecsSinceEpoch();
|
||
resultObj["error_code"] = errorCode;
|
||
resultObj["mark_count"] = static_cast<int>(marks.size());
|
||
|
||
// 添加标记数据
|
||
QJsonArray marksArray;
|
||
for (const auto& mark : marks)
|
||
{
|
||
QJsonObject markObj;
|
||
markObj["mark_id"] = mark.markID;
|
||
markObj["x"] = mark.mark3D.x;
|
||
markObj["y"] = mark.mark3D.y;
|
||
markObj["z"] = mark.mark3D.z;
|
||
marksArray.append(markObj);
|
||
}
|
||
resultObj["marks"] = marksArray;
|
||
|
||
#if 0
|
||
// 添加图像（可选，Base64编码）
|
||
if (!leftImage.empty())
|
||
{
|
||
resultObj["left_image"] = imageToBase64(leftImage);
|
||
}
|
||
if (!rightImage.empty())
|
||
{
|
||
resultObj["right_image"] = imageToBase64(rightImage);
|
||
}
|
||
#endif
|
||
QJsonDocument doc(resultObj);
|
||
QByteArray jsonData = doc.toJson(QJsonDocument::Compact);
|
||
QByteArray frameData = buildFrame(jsonData);
|
||
|
||
// 发送给所有客户端
|
||
if (m_pTcpServer != nullptr)
|
||
{
|
||
m_pTcpServer->SendAllData(frameData.data(), frameData.size());
|
||
}
|
||
|
||
LOG_INFO("Sent mark result, mark_count: %zu, error_code: %d\n", marks.size(), errorCode);
|
||
}
|
||
|
||
QString BinocularMarkTcpProtocol::imageToBase64(const cv::Mat& image)
|
||
{
|
||
// 将cv::Mat编码为JPEG格式
|
||
std::vector<uchar> buf;
|
||
cv::imencode(".jpg", image, buf);
|
||
|
||
// 转换为Base64
|
||
QByteArray ba(reinterpret_cast<const char*>(buf.data()), buf.size());
|
||
return ba.toBase64();
|
||
}
|
||
|
||
QString BinocularMarkTcpProtocol::generateClientId(const TCPClient* pClient)
|
||
{
|
||
return QString::number(reinterpret_cast<qintptr>(pClient));
|
||
}
|