409 lines
12 KiB
C++
409 lines
12 KiB
C++
#include "BinocularMarkReceiver.h"
|
||
#include <QDebug>
|
||
|
||
// 静态工厂方法实现
|
||
int IBinocularMarkReceiver::CreateInstance(IBinocularMarkReceiver** ppReceiver)
|
||
{
|
||
if (!ppReceiver) {
|
||
return -1;
|
||
}
|
||
|
||
*ppReceiver = new BinocularMarkReceiver();
|
||
return 0;
|
||
}
|
||
|
||
BinocularMarkReceiver::BinocularMarkReceiver(QObject *parent)
|
||
: QObject(parent)
|
||
, m_pTcpSocket(new QTcpSocket(this))
|
||
, m_pHeartbeatTimer(new QTimer(this))
|
||
, m_nHeartbeatInterval(30)
|
||
, m_serverPort(0)
|
||
, m_bConnected(false)
|
||
{
|
||
// 连接TCP套接字信号
|
||
connect(m_pTcpSocket, &QTcpSocket::connected, this, &BinocularMarkReceiver::onConnected);
|
||
connect(m_pTcpSocket, &QTcpSocket::disconnected, this, &BinocularMarkReceiver::onDisconnected);
|
||
connect(m_pTcpSocket, QOverload<QAbstractSocket::SocketError>::of(&QAbstractSocket::error),
|
||
this, &BinocularMarkReceiver::onError);
|
||
connect(m_pTcpSocket, &QTcpSocket::readyRead, this, &BinocularMarkReceiver::onReadyRead);
|
||
|
||
// 连接心跳定时器
|
||
connect(m_pHeartbeatTimer, &QTimer::timeout, this, &BinocularMarkReceiver::onHeartbeatTimeout);
|
||
}
|
||
|
||
BinocularMarkReceiver::~BinocularMarkReceiver()
|
||
{
|
||
Disconnect();
|
||
}
|
||
|
||
int BinocularMarkReceiver::Connect(const std::string& serverIp, quint16 serverPort)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_mutex);
|
||
|
||
if (m_bConnected) {
|
||
qDebug() << "已经连接到服务器";
|
||
return 0;
|
||
}
|
||
|
||
m_serverIp = serverIp;
|
||
m_serverPort = serverPort;
|
||
|
||
// 连接到服务器
|
||
m_pTcpSocket->connectToHost(QString::fromStdString(serverIp), serverPort);
|
||
|
||
// 等待连接（超时5秒）
|
||
if (!m_pTcpSocket->waitForConnected(5000)) {
|
||
qDebug() << "连接BinocularMarkApp服务器失败:" << m_pTcpSocket->errorString();
|
||
return -1;
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
int BinocularMarkReceiver::Disconnect()
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_mutex);
|
||
|
||
if (!m_bConnected) {
|
||
return 0;
|
||
}
|
||
|
||
// 停止心跳
|
||
StopHeartbeat();
|
||
|
||
// 断开连接
|
||
m_pTcpSocket->disconnectFromHost();
|
||
|
||
// 等待断开连接（超时3秒）
|
||
if (m_pTcpSocket->state() != QAbstractSocket::UnconnectedState) {
|
||
m_pTcpSocket->waitForDisconnected(3000);
|
||
}
|
||
|
||
m_bConnected = false;
|
||
m_dataBuffer.clear();
|
||
|
||
return 0;
|
||
}
|
||
|
||
bool BinocularMarkReceiver::IsConnected() const
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_mutex);
|
||
return m_bConnected;
|
||
}
|
||
|
||
int BinocularMarkReceiver::SendHeartbeat()
|
||
{
|
||
QJsonObject jsonObj;
|
||
jsonObj["timestamp"] = QDateTime::currentMSecsSinceEpoch();
|
||
|
||
return sendJsonMessage("HEARTBEAT", jsonObj);
|
||
}
|
||
|
||
int BinocularMarkReceiver::TriggerDetection()
|
||
{
|
||
QJsonObject jsonObj;
|
||
jsonObj["timestamp"] = QDateTime::currentMSecsSinceEpoch();
|
||
|
||
return sendJsonMessage("CMD_TRIGGER", jsonObj);
|
||
}
|
||
|
||
void BinocularMarkReceiver::SetMarkResultCallback(MarkResultCallback callback)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_mutex);
|
||
m_markResultCallback = callback;
|
||
}
|
||
|
||
void BinocularMarkReceiver::SetConnectionStatusCallback(ConnectionStatusCallback callback)
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_mutex);
|
||
m_connectionCallback = callback;
|
||
}
|
||
|
||
void BinocularMarkReceiver::StartHeartbeat(int intervalSeconds)
|
||
{
|
||
m_nHeartbeatInterval = intervalSeconds;
|
||
m_pHeartbeatTimer->start(intervalSeconds * 1000);
|
||
}
|
||
|
||
void BinocularMarkReceiver::StopHeartbeat()
|
||
{
|
||
m_pHeartbeatTimer->stop();
|
||
}
|
||
|
||
void BinocularMarkReceiver::onConnected()
|
||
{
|
||
qDebug() << "已连接到BinocularMarkApp服务器";
|
||
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_mutex);
|
||
m_bConnected = true;
|
||
}
|
||
|
||
// 通知连接状态改变
|
||
if (m_connectionCallback) {
|
||
m_connectionCallback(true);
|
||
}
|
||
|
||
// 启动心跳
|
||
StartHeartbeat(m_nHeartbeatInterval);
|
||
}
|
||
|
||
void BinocularMarkReceiver::onDisconnected()
|
||
{
|
||
qDebug() << "与BinocularMarkApp服务器断开连接";
|
||
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_mutex);
|
||
m_bConnected = false;
|
||
}
|
||
|
||
// 停止心跳
|
||
StopHeartbeat();
|
||
|
||
// 通知连接状态改变
|
||
if (m_connectionCallback) {
|
||
m_connectionCallback(false);
|
||
}
|
||
}
|
||
|
||
void BinocularMarkReceiver::onError(QAbstractSocket::SocketError socketError)
|
||
{
|
||
qDebug() << "TCP套接字错误:" << socketError << m_pTcpSocket->errorString();
|
||
}
|
||
|
||
void BinocularMarkReceiver::onReadyRead()
|
||
{
|
||
// 读取所有可用数据
|
||
QByteArray data = m_pTcpSocket->readAll();
|
||
|
||
// 追加到缓冲区
|
||
m_dataBuffer.append(data);
|
||
|
||
// 解析数据帧
|
||
parseFrames(m_dataBuffer);
|
||
}
|
||
|
||
void BinocularMarkReceiver::onHeartbeatTimeout()
|
||
{
|
||
// 发送心跳消息
|
||
SendHeartbeat();
|
||
}
|
||
|
||
void BinocularMarkReceiver::parseFrames(const QByteArray& data)
|
||
{
|
||
while (true) {
|
||
// 查找帧头
|
||
int headerIndex = m_dataBuffer.indexOf(FRAME_HEADER);
|
||
if (headerIndex == -1) {
|
||
// 没有找到帧头，清空缓冲区
|
||
m_dataBuffer.clear();
|
||
break;
|
||
}
|
||
|
||
// 丢弃帧头之前的数据
|
||
if (headerIndex > 0) {
|
||
m_dataBuffer.remove(0, headerIndex);
|
||
}
|
||
|
||
// 检查是否有足够的数据来读取长度字段
|
||
if (m_dataBuffer.size() < FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE) {
|
||
// 数据不够，等待更多数据
|
||
break;
|
||
}
|
||
|
||
// 读取数据长度（8字节ASCII字符串格式，如 "00001234"）
|
||
QByteArray lengthStr = m_dataBuffer.mid(FRAME_HEADER_SIZE, FRAME_LENGTH_SIZE);
|
||
bool ok = false;
|
||
qint64 dataLength = lengthStr.toLongLong(&ok);
|
||
|
||
if (!ok) {
|
||
qDebug() << "无效的长度字符串:" << lengthStr;
|
||
m_dataBuffer.remove(0, FRAME_HEADER_SIZE);
|
||
continue;
|
||
}
|
||
|
||
// 检查数据长度是否合理（最大10MB）
|
||
if (dataLength < 0 || dataLength > 10 * 1024 * 1024) {
|
||
qDebug() << "无效的数据长度:" << dataLength;
|
||
m_dataBuffer.remove(0, FRAME_HEADER_SIZE);
|
||
continue;
|
||
}
|
||
|
||
// 计算完整帧的总长度
|
||
int totalFrameLength = FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE + dataLength + FRAME_TAIL_SIZE;
|
||
|
||
// 检查是否有完整的帧
|
||
if (m_dataBuffer.size() < totalFrameLength) {
|
||
// 数据不够，等待更多数据
|
||
break;
|
||
}
|
||
|
||
// 提取JSON数据
|
||
QByteArray jsonData = m_dataBuffer.mid(FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE, dataLength);
|
||
|
||
// 验证帧尾
|
||
QByteArray tail = m_dataBuffer.mid(FRAME_HEADER_SIZE + FRAME_LENGTH_SIZE + dataLength, FRAME_TAIL_SIZE);
|
||
if (tail != FRAME_TAIL) {
|
||
qDebug() << "无效的帧尾";
|
||
m_dataBuffer.remove(0, FRAME_HEADER_SIZE);
|
||
continue;
|
||
}
|
||
|
||
// 处理JSON消息
|
||
handleJsonMessage(jsonData);
|
||
|
||
// 移除已处理的帧
|
||
m_dataBuffer.remove(0, totalFrameLength);
|
||
}
|
||
}
|
||
|
||
void BinocularMarkReceiver::handleJsonMessage(const QByteArray& jsonData)
|
||
{
|
||
// 解析JSON
|
||
QJsonParseError parseError;
|
||
QJsonDocument jsonDoc = QJsonDocument::fromJson(jsonData, &parseError);
|
||
|
||
if (parseError.error != QJsonParseError::NoError) {
|
||
qDebug() << "JSON解析错误:" << parseError.errorString();
|
||
return;
|
||
}
|
||
|
||
if (!jsonDoc.isObject()) {
|
||
qDebug() << "JSON不是对象";
|
||
return;
|
||
}
|
||
|
||
QJsonObject jsonObj = jsonDoc.object();
|
||
|
||
// 获取消息类型
|
||
QString messageType = jsonObj["message_type"].toString();
|
||
|
||
if (messageType == "MARK_RESULT") {
|
||
handleMarkResult(jsonObj);
|
||
} else if (messageType == "HEARTBEAT_ACK") {
|
||
handleHeartbeatAck(jsonObj);
|
||
} else if (messageType == "CMD_RESPONSE") {
|
||
handleCommandResponse(jsonObj);
|
||
} else {
|
||
qDebug() << "未知的消息类型:" << messageType;
|
||
}
|
||
}
|
||
|
||
void BinocularMarkReceiver::handleMarkResult(const QJsonObject& jsonObj)
|
||
{
|
||
// 解析Mark结果
|
||
qint64 timestamp = jsonObj["timestamp"].toVariant().toLongLong();
|
||
int errorCode = jsonObj["error_code"].toInt();
|
||
|
||
qDebug() << "========== BinocularMark 接收数据 ==========";
|
||
qDebug() << "时间戳 (timestamp):" << timestamp;
|
||
qDebug() << "错误码 (error_code):" << errorCode;
|
||
|
||
std::vector<VrMark3D> marks;
|
||
|
||
if (errorCode == 0) {
|
||
QJsonArray marksArray = jsonObj["marks"].toArray();
|
||
qDebug() << "Mark 点数量:" << marksArray.size();
|
||
|
||
int index = 0;
|
||
for (const QJsonValue& markValue : marksArray) {
|
||
QJsonObject markObj = markValue.toObject();
|
||
|
||
VrMark3D mark;
|
||
mark.markID = markObj["mark_id"].toInt();
|
||
mark.x = markObj["x"].toDouble();
|
||
mark.y = markObj["y"].toDouble();
|
||
mark.z = markObj["z"].toDouble();
|
||
|
||
qDebug() << QString(" Mark[%1]: ID=%2, X=%3, Y=%4, Z=%5")
|
||
.arg(index)
|
||
.arg(mark.markID)
|
||
.arg(mark.x, 0, 'f', 3)
|
||
.arg(mark.y, 0, 'f', 3)
|
||
.arg(mark.z, 0, 'f', 3);
|
||
|
||
marks.push_back(mark);
|
||
index++;
|
||
}
|
||
} else {
|
||
qDebug() << "检测失败，错误码:" << errorCode;
|
||
}
|
||
qDebug() << "==========================================";
|
||
|
||
// 调用回调函数
|
||
if (m_markResultCallback) {
|
||
m_markResultCallback(marks, timestamp, errorCode);
|
||
}
|
||
}
|
||
|
||
void BinocularMarkReceiver::handleHeartbeatAck(const QJsonObject& jsonObj)
|
||
{
|
||
// 心跳应答，暂时不处理
|
||
qDebug() << "收到心跳应答";
|
||
}
|
||
|
||
void BinocularMarkReceiver::handleCommandResponse(const QJsonObject& jsonObj)
|
||
{
|
||
QString cmdType = jsonObj["cmd_type"].toString();
|
||
bool result = jsonObj["result"].toBool();
|
||
int errorCode = jsonObj["error_code"].toInt();
|
||
QString errorMsg = jsonObj["error_msg"].toString();
|
||
|
||
qDebug() << "命令应答:" << cmdType << "结果:" << result << "错误码:" << errorCode << "错误消息:" << errorMsg;
|
||
}
|
||
|
||
QByteArray BinocularMarkReceiver::buildFrame(const QByteArray& jsonData)
|
||
{
|
||
QByteArray frame;
|
||
|
||
// 添加帧头
|
||
frame.append(FRAME_HEADER, FRAME_HEADER_SIZE);
|
||
|
||
// 添加数据长度（8字节ASCII字符串格式，如 "00001234"）
|
||
quint64 dataLength = jsonData.size();
|
||
char lengthStr[9]; // 8位数字 + '\0'
|
||
#ifdef _WIN32
|
||
sprintf_s(lengthStr, "%08llu", dataLength);
|
||
#else
|
||
sprintf(lengthStr, "%08llu", dataLength);
|
||
#endif
|
||
frame.append(lengthStr, FRAME_LENGTH_SIZE);
|
||
|
||
// 添加JSON数据
|
||
frame.append(jsonData);
|
||
|
||
// 添加帧尾
|
||
frame.append(FRAME_TAIL, FRAME_TAIL_SIZE);
|
||
|
||
return frame;
|
||
}
|
||
|
||
int BinocularMarkReceiver::sendJsonMessage(const QString& messageType, const QJsonObject& jsonObj)
|
||
{
|
||
if (!m_bConnected) {
|
||
qDebug() << "未连接到服务器";
|
||
return -1;
|
||
}
|
||
|
||
// 构造JSON对象
|
||
QJsonObject msgObj = jsonObj;
|
||
msgObj["message_type"] = messageType;
|
||
|
||
// 转换为JSON字符串
|
||
QJsonDocument jsonDoc(msgObj);
|
||
QByteArray jsonData = jsonDoc.toJson(QJsonDocument::Compact);
|
||
|
||
// 构造数据帧
|
||
QByteArray frame = buildFrame(jsonData);
|
||
|
||
// 发送数据
|
||
qint64 bytesWritten = m_pTcpSocket->write(frame);
|
||
if (bytesWritten == -1) {
|
||
qDebug() << "发送数据失败:" << m_pTcpSocket->errorString();
|
||
return -1;
|
||
}
|
||
|
||
m_pTcpSocket->flush();
|
||
return 0;
|
||
}
|