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");
|
|||
|
|
}
|
|||
|
|
}
|