141 lines
4.9 KiB
C++
141 lines
4.9 KiB
C++
#include "HoleDetectionPresenter.h"
|
||
#include "VrLog.h"
|
||
#include "VrError.h"
|
||
|
||
// 初始化TCP服务器
|
||
int HoleDetectionPresenter::InitTCPServer()
|
||
{
|
||
LOG_DEBUG("Start initializing TCP server\n");
|
||
|
||
if (m_pTCPServer) {
|
||
LOG_WARNING("TCP server already initialized\n");
|
||
return 0;
|
||
}
|
||
|
||
m_pTCPServer = new TCPServerProtocol();
|
||
|
||
// 从配置获取TCP协议端口
|
||
ConfigResult configResult = m_pConfigManager->GetConfigResult();
|
||
int port = configResult.plcRobotServerConfig.tcpServerPort;
|
||
|
||
int nRet = m_pTCPServer->Initialize(port);
|
||
if (nRet != 0) {
|
||
LOG_ERROR("Failed to initialize TCP server, return code: %d\n", nRet);
|
||
delete m_pTCPServer;
|
||
m_pTCPServer = nullptr;
|
||
return nRet;
|
||
}
|
||
|
||
// 设置连接状态回调
|
||
m_pTCPServer->SetConnectionCallback([this](bool connected) {
|
||
this->OnTCPConnectionChanged(connected);
|
||
});
|
||
|
||
// 设置检测触发回调（文本协议：cameraIndex + robotPose）
|
||
m_pTCPServer->SetDetectionTriggerCallback([this](int cameraIndex, const RobotFlangePose& robotPose) {
|
||
return this->OnTCPDetectionTrigger(cameraIndex, robotPose);
|
||
});
|
||
|
||
LOG_INFO("TCP server protocol initialized successfully on port %d\n", port);
|
||
return 0;
|
||
}
|
||
|
||
// TCP连接状态改变回调
|
||
void HoleDetectionPresenter::OnTCPConnectionChanged(bool connected)
|
||
{
|
||
LOG_DEBUG("TCP connection status changed: %s\n", connected ? "connected" : "disconnected");
|
||
|
||
m_bTCPConnected = connected;
|
||
|
||
if (auto pStatus = GetStatusCallback<IYHoleDetectionStatus>()) {
|
||
if (connected) {
|
||
pStatus->OnStatusUpdate("TCP客户端已连接");
|
||
} else {
|
||
pStatus->OnStatusUpdate("TCP客户端已断开");
|
||
}
|
||
pStatus->OnRobotConnectionChanged(connected);
|
||
}
|
||
|
||
CheckAndUpdateWorkStatus();
|
||
}
|
||
|
||
// TCP检测触发回调
|
||
bool HoleDetectionPresenter::OnTCPDetectionTrigger(int cameraIndex, const RobotFlangePose& robotPose)
|
||
{
|
||
if (auto pStatus = GetStatusCallback<IYHoleDetectionStatus>()) {
|
||
pStatus->OnWorkStatusChanged(WorkStatus::Working);
|
||
}
|
||
|
||
LOG_DEBUG("TCP detection trigger: cameraIndex=%d, robotPose=(%.3f, %.3f, %.3f, %.3f, %.3f, %.3f)\n",
|
||
cameraIndex, robotPose.x, robotPose.y, robotPose.z,
|
||
robotPose.roll, robotPose.pitch, robotPose.yaw);
|
||
|
||
if (auto pStatus = GetStatusCallback<IYHoleDetectionStatus>()) {
|
||
pStatus->OnStatusUpdate("【TCP协议】接收到检测触发信号");
|
||
}
|
||
|
||
// 保存当前机器人法兰位姿
|
||
m_currentRobotPose = robotPose;
|
||
|
||
// 检查相机连接状态
|
||
if (!m_bCameraConnected) {
|
||
LOG_ERROR("Camera not connected, cannot start detection\n");
|
||
if (auto pStatus = GetStatusCallback<IYHoleDetectionStatus>()) {
|
||
pStatus->OnStatusUpdate("【TCP协议】相机未连接，无法启动检测");
|
||
}
|
||
return false;
|
||
}
|
||
|
||
// 启动检测
|
||
int result = StartDetection(cameraIndex, false);
|
||
bool success = (result == 0);
|
||
|
||
if (success) {
|
||
LOG_DEBUG("Detection started successfully via TCP trigger\n");
|
||
if (auto pStatus = GetStatusCallback<IYHoleDetectionStatus>()) {
|
||
pStatus->OnStatusUpdate("开始执行扫描检测...");
|
||
}
|
||
} else {
|
||
LOG_ERROR("Failed to start detection via TCP trigger, error code: %d\n", result);
|
||
if (auto pStatus = GetStatusCallback<IYHoleDetectionStatus>()) {
|
||
pStatus->OnStatusUpdate("检测启动失败");
|
||
}
|
||
}
|
||
|
||
return success;
|
||
}
|
||
|
||
// 发送检测结果给TCP客户端（文本格式）
|
||
void HoleDetectionPresenter::_SendDetectionResultToTCP(const HoleDetectionResult& detectionResult, int cameraIndex)
|
||
{
|
||
if (!m_pTCPServer || !m_bTCPConnected) {
|
||
LOG_DEBUG("TCP server not available, skipping result transmission\n");
|
||
return;
|
||
}
|
||
|
||
LOG_DEBUG("Sending detection result to TCP clients, camera index: %d\n", cameraIndex);
|
||
|
||
// 构建文本格式：PointNum_X1_Y1_Z1_Roll1_Pitch1_Yaw1/X2_Y2_Z2_Roll2_Pitch2_Yaw2/
|
||
int holeCount = static_cast<int>(detectionResult.positions.size());
|
||
std::string resultText = std::to_string(holeCount);
|
||
|
||
for (size_t i = 0; i < detectionResult.positions.size(); i++) {
|
||
const auto& pos = detectionResult.positions[i];
|
||
char buf[256];
|
||
snprintf(buf, sizeof(buf), "_%.2f_%.2f_%.2f_%.2f_%.2f_%.2f/", pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw);
|
||
resultText += buf;
|
||
}
|
||
|
||
LOG_INFO("TCP result text: %s\n", resultText.c_str());
|
||
|
||
int sendResult = m_pTCPServer->SendTextResult(resultText);
|
||
if (sendResult == 0) {
|
||
LOG_INFO("Detection result sent to TCP clients successfully\n");
|
||
if (auto pStatus = GetStatusCallback<IYHoleDetectionStatus>()) {
|
||
pStatus->OnStatusUpdate("【TCP协议】已发送检测结果");
|
||
}
|
||
} else {
|
||
LOG_ERROR("Failed to send detection result to TCP clients, error code: %d\n", sendResult);
|
||
}
|
||
}
|