413 lines
12 KiB
C++
413 lines
12 KiB
C++
#include "RodWeldSeamTCPProtocol.h"
|
||
|
||
#include "VrLog.h"
|
||
|
||
#include <QString>
|
||
#include <QStringList>
|
||
|
||
#include <cmath>
|
||
#include <exception>
|
||
#include <utility>
|
||
#include <vector>
|
||
|
||
namespace {
|
||
|
||
std::string FrameText(const std::string& text)
|
||
{
|
||
std::string framed = text;
|
||
while (!framed.empty() && (framed.back() == '\r' || framed.back() == '\n')) {
|
||
framed.pop_back();
|
||
}
|
||
framed += "\r\n";
|
||
return framed;
|
||
}
|
||
|
||
} // namespace
|
||
|
||
RodWeldSeamTCPProtocol::RodWeldSeamTCPProtocol() = default;
|
||
|
||
RodWeldSeamTCPProtocol::~RodWeldSeamTCPProtocol()
|
||
{
|
||
Deinitialize();
|
||
}
|
||
|
||
int RodWeldSeamTCPProtocol::Initialize(uint16_t port)
|
||
{
|
||
std::lock_guard<std::mutex> lifecycleLock(m_lifecycleMutex);
|
||
|
||
if (m_serverRunning.load()) {
|
||
LOG_WARNING("RodWeldSeam TCP server is already running on port %u\n",
|
||
static_cast<unsigned int>(m_port));
|
||
return 0;
|
||
}
|
||
|
||
IYTCPServer* server = nullptr;
|
||
if (!VrCreatYTCPServer(&server) || !server) {
|
||
LOG_ERROR("Failed to create RodWeldSeam TCP server instance\n");
|
||
return -1;
|
||
}
|
||
|
||
if (!server->Init(port)) {
|
||
LOG_ERROR("Failed to initialize RodWeldSeam TCP server on port %u\n",
|
||
static_cast<unsigned int>(port));
|
||
delete server;
|
||
return -2;
|
||
}
|
||
|
||
server->SetEventCallback(
|
||
[this](const TCPClient* client, TCPServerEventType eventType) {
|
||
OnTCPEvent(client, eventType);
|
||
});
|
||
|
||
{
|
||
std::lock_guard<std::mutex> sendLock(m_sendMutex);
|
||
m_tcpServer = server;
|
||
m_port = port;
|
||
// Start() 启动接收线程后客户端即可立即发包，先发布运行状态供错误应答使用。
|
||
m_serverRunning.store(true);
|
||
}
|
||
|
||
if (!server->Start(
|
||
[this](const TCPClient* client, const char* data, const unsigned int length) {
|
||
OnTCPDataReceived(client, data, length);
|
||
})) {
|
||
LOG_ERROR("Failed to start RodWeldSeam TCP server on port %u\n",
|
||
static_cast<unsigned int>(port));
|
||
|
||
{
|
||
std::lock_guard<std::mutex> sendLock(m_sendMutex);
|
||
m_serverRunning.store(false);
|
||
m_tcpServer = nullptr;
|
||
}
|
||
server->Close();
|
||
delete server;
|
||
return -3;
|
||
}
|
||
|
||
LOG_INFO("RodWeldSeam TCP server initialized on port %u\n",
|
||
static_cast<unsigned int>(port));
|
||
return 0;
|
||
}
|
||
|
||
void RodWeldSeamTCPProtocol::Deinitialize()
|
||
{
|
||
std::lock_guard<std::mutex> lifecycleLock(m_lifecycleMutex);
|
||
|
||
IYTCPServer* server = nullptr;
|
||
{
|
||
// 等待已经开始的发送结束，并阻止后续发送再访问 server。
|
||
std::lock_guard<std::mutex> sendLock(m_sendMutex);
|
||
m_serverRunning.store(false);
|
||
server = m_tcpServer;
|
||
}
|
||
|
||
if (server) {
|
||
server->Stop();
|
||
server->Close();
|
||
|
||
{
|
||
std::lock_guard<std::mutex> sendLock(m_sendMutex);
|
||
if (m_tcpServer == server) {
|
||
m_tcpServer = nullptr;
|
||
}
|
||
}
|
||
delete server;
|
||
}
|
||
|
||
bool notifyDisconnected = false;
|
||
{
|
||
std::lock_guard<std::mutex> clientLock(m_clientMutex);
|
||
notifyDisconnected = !m_clients.empty();
|
||
m_clients.clear();
|
||
m_clientBuffers.clear();
|
||
m_clientCount.store(0);
|
||
}
|
||
|
||
if (notifyDisconnected) {
|
||
NotifyConnectionChanged(false);
|
||
}
|
||
}
|
||
|
||
bool RodWeldSeamTCPProtocol::IsRunning() const
|
||
{
|
||
return m_serverRunning.load();
|
||
}
|
||
|
||
std::size_t RodWeldSeamTCPProtocol::ClientCount() const
|
||
{
|
||
return m_clientCount.load();
|
||
}
|
||
|
||
void RodWeldSeamTCPProtocol::SetConnectionCallback(const ConnectionCallback& callback)
|
||
{
|
||
std::lock_guard<std::mutex> callbackLock(m_callbackMutex);
|
||
m_connectionCallback = callback;
|
||
}
|
||
|
||
void RodWeldSeamTCPProtocol::SetDetectionTriggerCallback(
|
||
const DetectionTriggerCallback& callback)
|
||
{
|
||
std::lock_guard<std::mutex> callbackLock(m_callbackMutex);
|
||
m_detectionTriggerCallback = callback;
|
||
}
|
||
|
||
int RodWeldSeamTCPProtocol::SendTextResult(const std::string& text)
|
||
{
|
||
const std::string framed = FrameText(text);
|
||
|
||
std::lock_guard<std::mutex> sendLock(m_sendMutex);
|
||
if (!m_serverRunning.load() || !m_tcpServer) {
|
||
return -1;
|
||
}
|
||
|
||
const bool sent = m_tcpServer->SendAllData(
|
||
framed.c_str(), static_cast<int>(framed.size()));
|
||
return sent ? 0 : -2;
|
||
}
|
||
|
||
int RodWeldSeamTCPProtocol::SendTextToClient(
|
||
const TCPClient* client, const std::string& text)
|
||
{
|
||
if (!client) {
|
||
return -2;
|
||
}
|
||
|
||
const std::string framed = FrameText(text);
|
||
|
||
std::lock_guard<std::mutex> sendLock(m_sendMutex);
|
||
if (!m_serverRunning.load() || !m_tcpServer) {
|
||
return -1;
|
||
}
|
||
|
||
const bool sent = m_tcpServer->SendData(
|
||
client, framed.c_str(), static_cast<int>(framed.size()));
|
||
return sent ? 0 : -2;
|
||
}
|
||
|
||
void RodWeldSeamTCPProtocol::OnTCPEvent(
|
||
const TCPClient* client, TCPServerEventType eventType)
|
||
{
|
||
bool notify = false;
|
||
bool connected = false;
|
||
std::size_t count = 0;
|
||
|
||
{
|
||
std::lock_guard<std::mutex> clientLock(m_clientMutex);
|
||
const std::size_t previousCount = m_clients.size();
|
||
|
||
switch (eventType) {
|
||
case TCP_EVENT_CLIENT_CONNECTED:
|
||
if (client) {
|
||
m_clients.insert(client);
|
||
m_clientBuffers.emplace(client, QByteArray());
|
||
}
|
||
break;
|
||
case TCP_EVENT_CLIENT_DISCONNECTED:
|
||
case TCP_EVENT_CLIENT_EXCEPTION:
|
||
m_clients.erase(client);
|
||
m_clientBuffers.erase(client);
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
|
||
count = m_clients.size();
|
||
m_clientCount.store(count);
|
||
notify = (previousCount == 0 && count > 0) ||
|
||
(previousCount > 0 && count == 0);
|
||
connected = count > 0;
|
||
}
|
||
|
||
switch (eventType) {
|
||
case TCP_EVENT_CLIENT_CONNECTED:
|
||
LOG_INFO("RodWeldSeam TCP client connected: %p, clients=%zu\n", client, count);
|
||
break;
|
||
case TCP_EVENT_CLIENT_DISCONNECTED:
|
||
LOG_INFO("RodWeldSeam TCP client disconnected: %p, clients=%zu\n", client, count);
|
||
break;
|
||
case TCP_EVENT_CLIENT_EXCEPTION:
|
||
LOG_WARNING("RodWeldSeam TCP client exception: %p, clients=%zu\n", client, count);
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
|
||
// CYTCPServer 在异常路径会先发 EXCEPTION、随后再发 DISCONNECTED。使用集合和
|
||
// 0<->非0 状态转换可确保聚合连接状态只通知一次。
|
||
if (notify) {
|
||
NotifyConnectionChanged(connected);
|
||
}
|
||
}
|
||
|
||
void RodWeldSeamTCPProtocol::OnTCPDataReceived(
|
||
const TCPClient* client, const char* data, unsigned int length)
|
||
{
|
||
if (!client || !data || length == 0 || !m_serverRunning.load()) {
|
||
return;
|
||
}
|
||
|
||
std::vector<QByteArray> lines;
|
||
bool bufferOverflow = false;
|
||
|
||
{
|
||
std::lock_guard<std::mutex> clientLock(m_clientMutex);
|
||
|
||
// 只接受已登记的连接，避免断开事件与收包回调交错时重新创建缓存。
|
||
if (m_clients.find(client) == m_clients.end()) {
|
||
return;
|
||
}
|
||
|
||
QByteArray& buffer = m_clientBuffers[client];
|
||
if (length > kMaxClientBufferBytes ||
|
||
static_cast<std::size_t>(buffer.size()) + length > kMaxClientBufferBytes) {
|
||
buffer.clear();
|
||
bufferOverflow = true;
|
||
} else {
|
||
buffer.append(data, static_cast<int>(length));
|
||
|
||
while (true) {
|
||
int terminatorIndex = buffer.indexOf("\r\n");
|
||
int terminatorLength = 2;
|
||
|
||
// 与参考项目一致，兼容只发送 LF 的客户端；正式协议仍规定 CRLF。
|
||
if (terminatorIndex < 0) {
|
||
terminatorIndex = buffer.indexOf('\n');
|
||
terminatorLength = 1;
|
||
}
|
||
if (terminatorIndex < 0) {
|
||
break;
|
||
}
|
||
|
||
lines.push_back(buffer.left(terminatorIndex).trimmed());
|
||
buffer.remove(0, terminatorIndex + terminatorLength);
|
||
}
|
||
}
|
||
}
|
||
|
||
if (bufferOverflow) {
|
||
LOG_WARNING("RodWeldSeam TCP client buffer exceeded %zu bytes: %p\n",
|
||
kMaxClientBufferBytes, client);
|
||
SendTextToClient(client, "ERR");
|
||
return;
|
||
}
|
||
|
||
for (const QByteArray& line : lines) {
|
||
if (line.isEmpty()) {
|
||
LOG_WARNING("RodWeldSeam TCP received an empty command\n");
|
||
SendTextToClient(client, "ERR");
|
||
continue;
|
||
}
|
||
ParseTextCommand(client, line);
|
||
}
|
||
}
|
||
|
||
void RodWeldSeamTCPProtocol::ParseTextCommand(
|
||
const TCPClient* client, const QByteArray& line)
|
||
{
|
||
const QString commandLine = QString::fromUtf8(line);
|
||
const QStringList tokens = commandLine.split('_', QString::KeepEmptyParts);
|
||
|
||
// 严格格式：W{camera}_X_Y_Z_A_B_C，共 7 段且不允许空字段。
|
||
if (tokens.size() != 7) {
|
||
LOG_WARNING("Invalid RodWeldSeam TCP command segment count: %d\n", tokens.size());
|
||
SendTextToClient(client, "ERR");
|
||
return;
|
||
}
|
||
for (const QString& token : tokens) {
|
||
if (token.trimmed().isEmpty()) {
|
||
LOG_WARNING("Invalid RodWeldSeam TCP command: empty segment\n");
|
||
SendTextToClient(client, "ERR");
|
||
return;
|
||
}
|
||
}
|
||
|
||
const QString typeToken = tokens.at(0).trimmed();
|
||
if (typeToken.size() < 2 || typeToken.at(0).toUpper() != QLatin1Char('W')) {
|
||
LOG_WARNING("Invalid RodWeldSeam TCP command prefix\n");
|
||
SendTextToClient(client, "ERR");
|
||
return;
|
||
}
|
||
|
||
const QString cameraToken = typeToken.mid(1);
|
||
for (const QChar ch : cameraToken) {
|
||
if (!ch.isDigit()) {
|
||
LOG_WARNING("Invalid RodWeldSeam TCP camera token\n");
|
||
SendTextToClient(client, "ERR");
|
||
return;
|
||
}
|
||
}
|
||
|
||
bool cameraOk = false;
|
||
const int cameraIndex = cameraToken.toInt(&cameraOk);
|
||
if (!cameraOk || cameraIndex < 1) {
|
||
LOG_WARNING("Invalid RodWeldSeam TCP camera index\n");
|
||
SendTextToClient(client, "ERR");
|
||
return;
|
||
}
|
||
|
||
RobotPose6D robotPose;
|
||
double* values[6] = {
|
||
&robotPose.x, &robotPose.y, &robotPose.z,
|
||
&robotPose.a, &robotPose.b, &robotPose.c
|
||
};
|
||
|
||
for (int i = 0; i < 6; ++i) {
|
||
bool valueOk = false;
|
||
const double value = tokens.at(i + 1).trimmed().toDouble(&valueOk);
|
||
if (!valueOk || !std::isfinite(value)) {
|
||
LOG_WARNING("Invalid RodWeldSeam TCP pose value at segment %d\n", i + 1);
|
||
SendTextToClient(client, "ERR");
|
||
return;
|
||
}
|
||
*values[i] = value;
|
||
}
|
||
|
||
DetectionTriggerCallback triggerCallback;
|
||
{
|
||
std::lock_guard<std::mutex> callbackLock(m_callbackMutex);
|
||
triggerCallback = m_detectionTriggerCallback;
|
||
}
|
||
|
||
bool accepted = false;
|
||
if (triggerCallback) {
|
||
try {
|
||
accepted = triggerCallback(cameraIndex, robotPose);
|
||
} catch (const std::exception& exception) {
|
||
LOG_ERROR("RodWeldSeam TCP trigger callback exception: %s\n", exception.what());
|
||
} catch (...) {
|
||
LOG_ERROR("RodWeldSeam TCP trigger callback unknown exception\n");
|
||
}
|
||
}
|
||
|
||
if (!accepted) {
|
||
SendTextToClient(client, "BUSY");
|
||
return;
|
||
}
|
||
|
||
LOG_INFO("RodWeldSeam TCP trigger accepted: camera=%d, pose=(%.3f, %.3f, %.3f, %.3f, %.3f, %.3f)\n",
|
||
cameraIndex,
|
||
robotPose.x, robotPose.y, robotPose.z,
|
||
robotPose.a, robotPose.b, robotPose.c);
|
||
}
|
||
|
||
void RodWeldSeamTCPProtocol::NotifyConnectionChanged(bool connected)
|
||
{
|
||
ConnectionCallback connectionCallback;
|
||
{
|
||
std::lock_guard<std::mutex> callbackLock(m_callbackMutex);
|
||
connectionCallback = m_connectionCallback;
|
||
}
|
||
|
||
if (!connectionCallback) {
|
||
return;
|
||
}
|
||
|
||
try {
|
||
connectionCallback(connected);
|
||
} catch (const std::exception& exception) {
|
||
LOG_ERROR("RodWeldSeam TCP connection callback exception: %s\n", exception.what());
|
||
} catch (...) {
|
||
LOG_ERROR("RodWeldSeam TCP connection callback unknown exception\n");
|
||
}
|
||
}
|