279 lines
8.5 KiB
C++
279 lines
8.5 KiB
C++
|
#include "TireHolePoseTCPProtocol.h"
|
|||
|
|
|
|||
|
|
#include "IYTCPServer.h"
|
|||
|
|
#include "IVrConfig.h"
|
|||
|
|
#include "VrLog.h"
|
|||
|
|
|
|||
|
|
#include <QString>
|
|||
|
|
#include <QStringList>
|
|||
|
|
#include <QMetaObject>
|
|||
|
|
#include <map>
|
|||
|
|
|
|||
|
|
// ============================================================
|
|||
|
|
// Impl —— 持有 IYTCPServer 生命周期与行缓冲，通过回调驱动 QObject 信号
|
|||
|
|
// ============================================================
|
|||
|
|
class TireHolePoseTCPProtocol::Impl
|
|||
|
|
{
|
|||
|
|
public:
|
|||
|
|
TireHolePoseTCPProtocol* q = nullptr;
|
|||
|
|
|
|||
|
|
IYTCPServer* m_pTCPServer = nullptr;
|
|||
|
|
bool m_bServerRunning = false;
|
|||
|
|
uint16_t m_nPort = 0;
|
|||
|
|
bool m_bConnected = false;
|
|||
|
|
|
|||
|
|
DetectionTriggerCallback m_detectionTriggerCallback;
|
|||
|
|
|
|||
|
|
// 每个客户端的行缓冲（基于 TCPClient 指针作为 key）
|
|||
|
|
std::map<const TCPClient*, QByteArray> m_clientBuffers;
|
|||
|
|
|
|||
|
|
// ============ 生命周期 ============
|
|||
|
|
|
|||
|
|
bool StartServer(uint16_t port)
|
|||
|
|
{
|
|||
|
|
m_nPort = port;
|
|||
|
|
|
|||
|
|
if (!VrCreatYTCPServer(&m_pTCPServer)) {
|
|||
|
|
LOG_ERROR("TireHolePose: Failed to create TCP server instance\n");
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (!m_pTCPServer->Init(port)) {
|
|||
|
|
LOG_ERROR("TireHolePose: Failed to initialize TCP server on port %d\n", port);
|
|||
|
|
delete m_pTCPServer;
|
|||
|
|
m_pTCPServer = nullptr;
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
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("TireHolePose: Failed to start TCP server\n");
|
|||
|
|
m_pTCPServer->Close();
|
|||
|
|
delete m_pTCPServer;
|
|||
|
|
m_pTCPServer = nullptr;
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
m_bServerRunning = true;
|
|||
|
|
LOG_DEBUG("TireHolePose TCP protocol started on port %d\n", port);
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void StopServer()
|
|||
|
|
{
|
|||
|
|
if (m_pTCPServer) {
|
|||
|
|
m_bServerRunning = false;
|
|||
|
|
m_bConnected = false;
|
|||
|
|
m_pTCPServer->Stop();
|
|||
|
|
m_pTCPServer->Close();
|
|||
|
|
delete m_pTCPServer;
|
|||
|
|
m_pTCPServer = nullptr;
|
|||
|
|
m_clientBuffers.clear();
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
bool IsConnected() const
|
|||
|
|
{
|
|||
|
|
return m_bConnected;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// ============ 发送 ============
|
|||
|
|
|
|||
|
|
int SendResult(const QString& resultText, const TCPClient* pClient = nullptr)
|
|||
|
|
{
|
|||
|
|
if (!m_pTCPServer || !m_bServerRunning) {
|
|||
|
|
return -1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
const QByteArray sendData = resultText.toUtf8() + "\r\n";
|
|||
|
|
bool success = false;
|
|||
|
|
if (pClient) {
|
|||
|
|
success = m_pTCPServer->SendData(pClient, sendData.constData(),
|
|||
|
|
static_cast<unsigned int>(sendData.size()));
|
|||
|
|
} else {
|
|||
|
|
success = m_pTCPServer->SendAllData(sendData.constData(),
|
|||
|
|
static_cast<unsigned int>(sendData.size()));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return success ? 0 : -2;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// ============ TCP 事件回调（在 IYTCPServer 工作线程中）============
|
|||
|
|
|
|||
|
|
void OnTCPEvent(const TCPClient* pClient, TCPServerEventType eventType)
|
|||
|
|
{
|
|||
|
|
switch (eventType) {
|
|||
|
|
case TCP_EVENT_CLIENT_CONNECTED:
|
|||
|
|
LOG_DEBUG("TireHolePose: TCP client connected: %p\n", pClient);
|
|||
|
|
m_bConnected = true;
|
|||
|
|
QMetaObject::invokeMethod(q, [this]() {
|
|||
|
|
emit q->ConnectionChanged(true);
|
|||
|
|
}, Qt::QueuedConnection);
|
|||
|
|
break;
|
|||
|
|
case TCP_EVENT_CLIENT_DISCONNECTED:
|
|||
|
|
LOG_DEBUG("TireHolePose: TCP client disconnected: %p\n", pClient);
|
|||
|
|
m_clientBuffers.erase(pClient);
|
|||
|
|
m_bConnected = false;
|
|||
|
|
QMetaObject::invokeMethod(q, [this]() {
|
|||
|
|
emit q->ConnectionChanged(false);
|
|||
|
|
}, Qt::QueuedConnection);
|
|||
|
|
break;
|
|||
|
|
case TCP_EVENT_CLIENT_EXCEPTION:
|
|||
|
|
LOG_WARNING("TireHolePose: TCP client exception: %p\n", pClient);
|
|||
|
|
m_clientBuffers.erase(pClient);
|
|||
|
|
m_bConnected = false;
|
|||
|
|
QMetaObject::invokeMethod(q, [this]() {
|
|||
|
|
emit q->ConnectionChanged(false);
|
|||
|
|
}, Qt::QueuedConnection);
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void OnTCPDataReceived(const TCPClient* pClient, const char* pData, unsigned int nLen)
|
|||
|
|
{
|
|||
|
|
if (!pData || nLen == 0) {
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
m_clientBuffers[pClient].append(pData, static_cast<int>(nLen));
|
|||
|
|
QByteArray& buffer = m_clientBuffers[pClient];
|
|||
|
|
|
|||
|
|
while (true) {
|
|||
|
|
int idx = buffer.indexOf("\r\n");
|
|||
|
|
int terminatorLen = 2;
|
|||
|
|
if (idx < 0) {
|
|||
|
|
idx = buffer.indexOf('\n');
|
|||
|
|
terminatorLen = 1;
|
|||
|
|
}
|
|||
|
|
if (idx < 0) {
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
QByteArray line = buffer.left(idx).trimmed();
|
|||
|
|
buffer.remove(0, idx + terminatorLen);
|
|||
|
|
|
|||
|
|
if (!line.isEmpty()) {
|
|||
|
|
const QString strLine = QString::fromUtf8(line);
|
|||
|
|
|
|||
|
|
// Emit DataReceived signal on main thread
|
|||
|
|
QMetaObject::invokeMethod(q, [this, strLine]() {
|
|||
|
|
emit q->DataReceived(strLine);
|
|||
|
|
}, Qt::QueuedConnection);
|
|||
|
|
|
|||
|
|
ProcessCommand(strLine);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// ============ 命令解析 ============
|
|||
|
|
|
|||
|
|
void ProcessCommand(const QString& command)
|
|||
|
|
{
|
|||
|
|
// 眼在手外协议：D1 触发相机1检测（无需机械臂位姿）
|
|||
|
|
// D = 轮胎检测
|
|||
|
|
// 1 = 相机索引（从 1 开始）
|
|||
|
|
const QString typeToken = command.trimmed().toUpper();
|
|||
|
|
|
|||
|
|
LOG_DEBUG("TireHolePose ParseCommand: '%s'\n",
|
|||
|
|
command.toStdString().c_str());
|
|||
|
|
|
|||
|
|
if (!typeToken.startsWith('D')) {
|
|||
|
|
LOG_ERROR("TireHolePose: Invalid type token: %s (expected D)\n",
|
|||
|
|
typeToken.toStdString().c_str());
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 解析相机索引
|
|||
|
|
bool ok = false;
|
|||
|
|
int cameraIndex = typeToken.mid(1).toInt(&ok);
|
|||
|
|
if (!ok || cameraIndex < 1) {
|
|||
|
|
cameraIndex = 1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 眼在手外：机械臂位姿不参与触发，使用零位姿
|
|||
|
|
const RobotPose6D robotPose;
|
|||
|
|
|
|||
|
|
LOG_INFO("TireHolePose TCP trigger: camera=%d (Eye-to-Hand)\n", cameraIndex);
|
|||
|
|
|
|||
|
|
// Emit DetectionTriggered signal on main thread
|
|||
|
|
QMetaObject::invokeMethod(q, [this, cameraIndex, robotPose]() {
|
|||
|
|
emit q->DetectionTriggered(cameraIndex, robotPose);
|
|||
|
|
}, Qt::QueuedConnection);
|
|||
|
|
|
|||
|
|
if (m_detectionTriggerCallback) {
|
|||
|
|
m_detectionTriggerCallback(cameraIndex, robotPose);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
// ============================================================
|
|||
|
|
// TireHolePoseTCPProtocol 公开接口（委托给 Impl）
|
|||
|
|
// ============================================================
|
|||
|
|
|
|||
|
|
TireHolePoseTCPProtocol::TireHolePoseTCPProtocol(QObject* parent)
|
|||
|
|
: QObject(parent)
|
|||
|
|
, m_pImpl(std::make_unique<Impl>())
|
|||
|
|
{
|
|||
|
|
m_pImpl->q = this;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
TireHolePoseTCPProtocol::~TireHolePoseTCPProtocol()
|
|||
|
|
{
|
|||
|
|
StopServer();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
bool TireHolePoseTCPProtocol::StartServer(uint16_t port)
|
|||
|
|
{
|
|||
|
|
return m_pImpl->StartServer(port);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void TireHolePoseTCPProtocol::StopServer()
|
|||
|
|
{
|
|||
|
|
m_pImpl->StopServer();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
bool TireHolePoseTCPProtocol::IsConnected() const
|
|||
|
|
{
|
|||
|
|
return m_pImpl->IsConnected();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void TireHolePoseTCPProtocol::SetDetectionTriggerCallback(DetectionTriggerCallback callback)
|
|||
|
|
{
|
|||
|
|
m_pImpl->m_detectionTriggerCallback = std::move(callback);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void TireHolePoseTCPProtocol::SendResult(const QString& resultText)
|
|||
|
|
{
|
|||
|
|
m_pImpl->SendResult(resultText);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Private slots —— 这些槽在当前设计中由 Impl 的回调通过 QMetaObject::invokeMethod
|
|||
|
|
// 驱动信号，因此这里保留空实现（实际逻辑在 Impl::OnTCPEvent / OnTCPDataReceived 内完成）。
|
|||
|
|
// 如果将来需要外部主动调用这些槽，可以补充转发逻辑。
|
|||
|
|
void TireHolePoseTCPProtocol::OnNewConnection()
|
|||
|
|
{
|
|||
|
|
// 当前由 Impl 内部 TCP_EVENT_CLIENT_CONNECTED 直接 emit ConnectionChanged
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void TireHolePoseTCPProtocol::OnDisconnected()
|
|||
|
|
{
|
|||
|
|
// 当前由 Impl 内部 TCP_EVENT_CLIENT_DISCONNECTED 直接 emit ConnectionChanged
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void TireHolePoseTCPProtocol::OnReadyRead(const QByteArray& data)
|
|||
|
|
{
|
|||
|
|
// 当前数据接收在 Impl::OnTCPDataReceived 中完成，
|
|||
|
|
// 此槽保留供将来 Qt 信号驱动的接收路径使用
|
|||
|
|
Q_UNUSED(data);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void TireHolePoseTCPProtocol::ProcessCommand(const QString& command)
|
|||
|
|
{
|
|||
|
|
m_pImpl->ProcessCommand(command);
|
|||
|
|
}
|