243 lines
7.3 KiB
C++
243 lines
7.3 KiB
C++
#include "TCPServerProtocol.h"
|
||
#include "VrLog.h"
|
||
#include <QString>
|
||
#include <QStringList>
|
||
|
||
TCPServerProtocol::TCPServerProtocol()
|
||
: m_pTCPServer(nullptr)
|
||
, m_bServerRunning(false)
|
||
, m_nPort(5020)
|
||
, m_connectionStatus(CONNECTION_DISCONNECTED)
|
||
{
|
||
}
|
||
|
||
TCPServerProtocol::~TCPServerProtocol()
|
||
{
|
||
Deinitialize();
|
||
}
|
||
|
||
int TCPServerProtocol::Initialize(uint16_t port)
|
||
{
|
||
LOG_DEBUG("Initializing TCP server protocol on port %d\n", port);
|
||
|
||
m_nPort = port;
|
||
|
||
if (!VrCreatYTCPServer(&m_pTCPServer)) {
|
||
LOG_ERROR("Failed to create TCP server instance\n");
|
||
return -1;
|
||
}
|
||
|
||
if (!m_pTCPServer->Init(port)) {
|
||
LOG_ERROR("Failed to initialize TCP server on port %d\n", port);
|
||
delete m_pTCPServer;
|
||
m_pTCPServer = nullptr;
|
||
return -2;
|
||
}
|
||
|
||
m_pTCPServer->SetEventCallback([this](const TCPClient* pClient, TCPServerEventType eventType) {
|
||
this->OnTCPEvent(pClient, eventType);
|
||
});
|
||
|
||
if (!m_pTCPServer->Start([this](const TCPClient* pClient, const char* pData, const unsigned int nLen) {
|
||
this->OnTCPDataReceived(pClient, pData, nLen);
|
||
})) {
|
||
LOG_ERROR("Failed to start TCP server\n");
|
||
m_pTCPServer->Close();
|
||
delete m_pTCPServer;
|
||
m_pTCPServer = nullptr;
|
||
return -3;
|
||
}
|
||
|
||
m_bServerRunning = true;
|
||
m_connectionStatus = CONNECTION_CONNECTED;
|
||
|
||
LOG_DEBUG("TCP server protocol initialized successfully on port %d\n", port);
|
||
return 0;
|
||
}
|
||
|
||
void TCPServerProtocol::Deinitialize()
|
||
{
|
||
if (m_pTCPServer) {
|
||
LOG_DEBUG("Stopping TCP server protocol\n");
|
||
|
||
m_bServerRunning = false;
|
||
m_connectionStatus = CONNECTION_DISCONNECTED;
|
||
|
||
m_pTCPServer->Stop();
|
||
m_pTCPServer->Close();
|
||
delete m_pTCPServer;
|
||
m_pTCPServer = nullptr;
|
||
|
||
m_clientBuffers.clear();
|
||
|
||
LOG_DEBUG("TCP server protocol stopped\n");
|
||
}
|
||
}
|
||
|
||
int TCPServerProtocol::SendTextResult(const std::string& text, const TCPClient* pClient)
|
||
{
|
||
if (!m_pTCPServer || !m_bServerRunning) {
|
||
LOG_ERROR("TCP server is not running\n");
|
||
return -1;
|
||
}
|
||
|
||
// 添加换行符作为消息分隔
|
||
std::string sendData = text + "\n";
|
||
|
||
bool success = false;
|
||
if (pClient) {
|
||
success = m_pTCPServer->SendData(pClient, sendData.c_str(), sendData.size());
|
||
LOG_DEBUG("Sent text result to specific client, size: %zu bytes\n", sendData.size());
|
||
} else {
|
||
success = m_pTCPServer->SendAllData(sendData.c_str(), sendData.size());
|
||
LOG_DEBUG("Sent text result to all clients, size: %zu bytes\n", sendData.size());
|
||
}
|
||
|
||
if (!success) {
|
||
LOG_ERROR("Failed to send text result\n");
|
||
return -2;
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
TCPServerProtocol::TCPStatus TCPServerProtocol::GetConnectionStatus() const
|
||
{
|
||
return m_connectionStatus;
|
||
}
|
||
|
||
void TCPServerProtocol::SetConnectionCallback(const ConnectionCallback& callback)
|
||
{
|
||
m_connectionCallback = callback;
|
||
}
|
||
|
||
void TCPServerProtocol::SetDetectionTriggerCallback(const DetectionTriggerCallback& callback)
|
||
{
|
||
m_detectionTriggerCallback = callback;
|
||
}
|
||
|
||
bool TCPServerProtocol::IsRunning() const
|
||
{
|
||
return m_bServerRunning;
|
||
}
|
||
|
||
void TCPServerProtocol::OnTCPEvent(const TCPClient* pClient, TCPServerEventType eventType)
|
||
{
|
||
switch (eventType) {
|
||
case TCP_EVENT_CLIENT_CONNECTED:
|
||
LOG_DEBUG("TCP client connected: %p\n", pClient);
|
||
m_connectionStatus = CONNECTION_CONNECTED;
|
||
if (m_connectionCallback) {
|
||
m_connectionCallback(true);
|
||
}
|
||
break;
|
||
|
||
case TCP_EVENT_CLIENT_DISCONNECTED:
|
||
LOG_DEBUG("TCP client disconnected: %p\n", pClient);
|
||
// 清理该客户端的缓冲区
|
||
m_clientBuffers.erase(pClient);
|
||
if (m_connectionCallback) {
|
||
m_connectionCallback(false);
|
||
}
|
||
break;
|
||
|
||
case TCP_EVENT_CLIENT_EXCEPTION:
|
||
LOG_WARNING("TCP client exception: %p\n", pClient);
|
||
m_clientBuffers.erase(pClient);
|
||
if (m_connectionCallback) {
|
||
m_connectionCallback(false);
|
||
}
|
||
break;
|
||
}
|
||
}
|
||
|
||
void TCPServerProtocol::OnTCPDataReceived(const TCPClient* pClient, const char* pData, unsigned int nLen)
|
||
{
|
||
if (!pData || nLen == 0) {
|
||
LOG_WARNING("Received empty data from client %p\n", pClient);
|
||
return;
|
||
}
|
||
|
||
LOG_DEBUG("Received TCP data from client %p, size: %u bytes\n", pClient, nLen);
|
||
|
||
// 追加数据到该客户端的缓冲区
|
||
m_clientBuffers[pClient].append(pData, nLen);
|
||
|
||
// 按换行符分割完整行
|
||
QByteArray& buffer = m_clientBuffers[pClient];
|
||
while (true) {
|
||
int idx = buffer.indexOf('\n');
|
||
if (idx < 0) break;
|
||
|
||
QByteArray line = buffer.left(idx).trimmed();
|
||
buffer.remove(0, idx + 1);
|
||
|
||
if (!line.isEmpty()) {
|
||
ParseTextCommand(pClient, line);
|
||
}
|
||
}
|
||
}
|
||
|
||
void TCPServerProtocol::ParseTextCommand(const TCPClient* pClient, const QByteArray& line)
|
||
{
|
||
// 协议格式：X1 RbtX-23.45 RbtY-23.45 RbtZ-23.45 RbtRoll-23.45 RbtPitch-123.45 RbtYaw-23.45
|
||
QString strLine = QString::fromUtf8(line);
|
||
QStringList tokens = strLine.split(' ', QString::SkipEmptyParts);
|
||
|
||
LOG_DEBUG("ParseTextCommand: line='%s', tokens=%d\n", strLine.toStdString().c_str(), tokens.size());
|
||
|
||
if (tokens.size() < 7) {
|
||
LOG_ERROR("Invalid command format, expected 7 tokens, got %d: %s\n",
|
||
tokens.size(), strLine.toStdString().c_str());
|
||
return;
|
||
}
|
||
|
||
// 解析相机索引：X1 或 X2
|
||
QString cameraToken = tokens[0];
|
||
if (!cameraToken.startsWith('X') && !cameraToken.startsWith('x')) {
|
||
LOG_ERROR("Invalid camera token: %s\n", cameraToken.toStdString().c_str());
|
||
return;
|
||
}
|
||
int cameraIndex = cameraToken.mid(1).toInt();
|
||
if (cameraIndex < 1 || cameraIndex > 2) {
|
||
LOG_ERROR("Invalid camera index: %d\n", cameraIndex);
|
||
return;
|
||
}
|
||
|
||
// 解析机器人法兰位姿
|
||
// 每个 token 格式为 "RbtX-23.45"，前缀为 RbtX/RbtY/RbtZ/RbtRoll/RbtPitch/RbtYaw
|
||
RobotFlangePose robotPose;
|
||
const QString prefixes[] = {"RbtX", "RbtY", "RbtZ", "RbtRoll", "RbtPitch", "RbtYaw"};
|
||
double* values[] = {&robotPose.x, &robotPose.y, &robotPose.z,
|
||
&robotPose.roll, &robotPose.pitch, &robotPose.yaw};
|
||
|
||
for (int i = 0; i < 6; i++) {
|
||
QString token = tokens[i + 1];
|
||
if (!token.startsWith(prefixes[i])) {
|
||
LOG_ERROR("Invalid token prefix at position %d: expected '%s', got '%s'\n",
|
||
i + 1, prefixes[i].toStdString().c_str(), token.toStdString().c_str());
|
||
return;
|
||
}
|
||
QString valueStr = token.mid(prefixes[i].length());
|
||
bool ok = false;
|
||
double val = valueStr.toDouble(&ok);
|
||
if (!ok) {
|
||
LOG_ERROR("Invalid numeric value at position %d: '%s'\n",
|
||
i + 1, valueStr.toStdString().c_str());
|
||
return;
|
||
}
|
||
*values[i] = val;
|
||
}
|
||
|
||
LOG_INFO("Parsed trigger: camera=%d, pose=(%.3f, %.3f, %.3f, %.3f, %.3f, %.3f)\n",
|
||
cameraIndex, robotPose.x, robotPose.y, robotPose.z,
|
||
robotPose.roll, robotPose.pitch, robotPose.yaw);
|
||
|
||
// 触发检测回调
|
||
if (m_detectionTriggerCallback) {
|
||
m_detectionTriggerCallback(cameraIndex, robotPose);
|
||
} else {
|
||
LOG_WARNING("Detection trigger callback not set\n");
|
||
}
|
||
}
|