250 lines
8.8 KiB
C++
250 lines
8.8 KiB
C++
#include "StatorWorkpiecePositionPresenter.h"
|
||
#include "VrLog.h"
|
||
#include "VrError.h"
|
||
#include <QStringList>
|
||
|
||
namespace {
|
||
|
||
QString NumberField(double value)
|
||
{
|
||
return QString::number(value, 'f', 3);
|
||
}
|
||
|
||
QString IntField(int value)
|
||
{
|
||
return QString::number(value);
|
||
}
|
||
|
||
void AppendPoseFields(QStringList& fields, const WorkpiecePosition& position, int poseOutputOrder)
|
||
{
|
||
switch (poseOutputOrder) {
|
||
case POSE_ORDER_RPY:
|
||
fields << NumberField(position.roll) << NumberField(position.pitch) << NumberField(position.yaw);
|
||
break;
|
||
case POSE_ORDER_RYP:
|
||
fields << NumberField(position.roll) << NumberField(position.yaw) << NumberField(position.pitch);
|
||
break;
|
||
case POSE_ORDER_PRY:
|
||
fields << NumberField(position.pitch) << NumberField(position.roll) << NumberField(position.yaw);
|
||
break;
|
||
case POSE_ORDER_PYR:
|
||
fields << NumberField(position.pitch) << NumberField(position.yaw) << NumberField(position.roll);
|
||
break;
|
||
case POSE_ORDER_YRP:
|
||
fields << NumberField(position.yaw) << NumberField(position.roll) << NumberField(position.pitch);
|
||
break;
|
||
case POSE_ORDER_YPR:
|
||
fields << NumberField(position.yaw) << NumberField(position.pitch) << NumberField(position.roll);
|
||
break;
|
||
default:
|
||
fields << NumberField(position.roll) << NumberField(position.pitch) << NumberField(position.yaw);
|
||
break;
|
||
}
|
||
}
|
||
|
||
} // namespace
|
||
|
||
// 初始化TCP服务器
|
||
int StatorWorkpiecePositionPresenter::InitTCPServer()
|
||
{
|
||
LOG_DEBUG("Start initializing TCP server\n");
|
||
|
||
if (m_pTCPServer) {
|
||
LOG_WARNING("TCP server already initialized\n");
|
||
return 0;
|
||
}
|
||
|
||
// 创建TCP服务器协议实例
|
||
m_pTCPServer = new TCPServerProtocol();
|
||
|
||
// Register callbacks before Initialize() starts the listening thread.
|
||
// Otherwise an immediate client connection/request can be lost.
|
||
m_pTCPServer->SetConnectionCallback([this](bool connected) {
|
||
this->OnTCPConnectionChanged(connected);
|
||
});
|
||
|
||
m_pTCPServer->SetDetectionTriggerCallback([this](bool startWork, int cameraIndex, qint64 timestamp) {
|
||
return this->OnTCPDetectionTrigger(startWork, cameraIndex, timestamp);
|
||
});
|
||
m_pTCPServer->SetBinDetectionTriggerCallback([this](int cameraIndex, qint64 timestamp) {
|
||
return this->OnTCPBinDetectionTrigger(cameraIndex, timestamp);
|
||
});
|
||
|
||
// 从配置获取 TCP/IP 协议端口，默认 7800。
|
||
// Modbus TCP Server 端口固定为 5020，不使用此配置项。
|
||
int port = 7800;
|
||
if (m_pConfigManager) {
|
||
const ConfigResult configResult = m_pConfigManager->GetConfigResult();
|
||
const int configuredPort = configResult.plcRobotServerConfig.robotServerPort;
|
||
if (configuredPort > 0 && configuredPort <= 65535) {
|
||
port = configuredPort;
|
||
}
|
||
}
|
||
|
||
// 初始化TCP服务器
|
||
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;
|
||
}
|
||
|
||
LOG_INFO("TCP server protocol initialized successfully on port %d\n", port);
|
||
return 0;
|
||
}
|
||
|
||
// TCP连接状态改变回调
|
||
void StatorWorkpiecePositionPresenter::OnTCPConnectionChanged(bool connected)
|
||
{
|
||
LOG_DEBUG("TCP connection status changed: %s\n", connected ? "connected" : "disconnected");
|
||
|
||
m_bTCPConnected.store(connected);
|
||
|
||
// 通知状态更新
|
||
if (auto pStatus = GetStatusCallback<IYStatorWorkpiecePositionStatus>()) {
|
||
if (connected) {
|
||
pStatus->OnStatusUpdate("TCP客户端已连接");
|
||
} else {
|
||
pStatus->OnStatusUpdate("TCP客户端已断开");
|
||
}
|
||
|
||
// 更新机械臂连接状态
|
||
pStatus->OnRobotConnectionChanged(connected);
|
||
}
|
||
|
||
// 更新工作状态
|
||
CheckAndUpdateWorkStatus();
|
||
}
|
||
|
||
// TCP检测触发回调
|
||
bool StatorWorkpiecePositionPresenter::OnTCPDetectionTrigger(bool startWork, int cameraIndex, qint64 timestamp)
|
||
{
|
||
LOG_DEBUG("TCP detection trigger: startWork=%s, cameraIndex=%d, timestamp=%lld\n",
|
||
startWork ? "true" : "false", cameraIndex, timestamp);
|
||
|
||
// 通知UI接收到TCP扫描请求
|
||
if (auto pStatus = GetStatusCallback<IYStatorWorkpiecePositionStatus>()) {
|
||
pStatus->OnStatusUpdate("【TCP协议】接收到工件检测请求");
|
||
}
|
||
|
||
if (!startWork) {
|
||
LOG_WARNING("Received stop work signal, ignoring\n");
|
||
return false;
|
||
}
|
||
|
||
// 检查相机连接状态
|
||
if (!m_bCameraConnected) {
|
||
LOG_ERROR("Camera not connected, cannot start detection\n");
|
||
if (auto pStatus = GetStatusCallback<IYStatorWorkpiecePositionStatus>()) {
|
||
pStatus->OnStatusUpdate("【TCP协议】相机未连接，无法启动检测");
|
||
}
|
||
return false;
|
||
}
|
||
|
||
m_pendingDetectionTask.store(DETECTION_TASK_WORKPIECE);
|
||
|
||
// 启动检测
|
||
int result = StartDetection(cameraIndex, false); // false表示手动触发
|
||
bool success = (result == 0);
|
||
|
||
if (success) {
|
||
LOG_DEBUG("Detection started successfully via TCP trigger\n");
|
||
if (auto pStatus = GetStatusCallback<IYStatorWorkpiecePositionStatus>()) {
|
||
pStatus->OnStatusUpdate("开始执行扫描检测...");
|
||
}
|
||
} else {
|
||
LOG_ERROR("Failed to start detection via TCP trigger, error code: %d\n", result);
|
||
if (auto pStatus = GetStatusCallback<IYStatorWorkpiecePositionStatus>()) {
|
||
pStatus->OnStatusUpdate("检测启动失败");
|
||
}
|
||
|
||
}
|
||
|
||
return success;
|
||
}
|
||
|
||
bool StatorWorkpiecePositionPresenter::OnTCPBinDetectionTrigger(int cameraIndex, qint64 timestamp)
|
||
{
|
||
LOG_DEBUG("TCP bin detection trigger: cameraIndex=%d, timestamp=%lld\n", cameraIndex, timestamp);
|
||
if (auto pStatus = GetStatusCallback<IYStatorWorkpiecePositionStatus>()) {
|
||
pStatus->OnStatusUpdate("【TCP协议】接收到料框检测请求");
|
||
}
|
||
if (!m_bCameraConnected) {
|
||
if (auto pStatus = GetStatusCallback<IYStatorWorkpiecePositionStatus>()) {
|
||
pStatus->OnStatusUpdate("【TCP协议】相机未连接，无法启动料框检测");
|
||
}
|
||
return false;
|
||
}
|
||
|
||
m_pendingDetectionTask.store(DETECTION_TASK_BIN);
|
||
const int result = StartDetection(cameraIndex, false);
|
||
if (result != SUCCESS) {
|
||
m_pendingDetectionTask.store(DETECTION_TASK_WORKPIECE);
|
||
}
|
||
return result == SUCCESS;
|
||
}
|
||
|
||
// 发送检测结果给TCP客户端
|
||
void StatorWorkpiecePositionPresenter::_SendDetectionResultToTCP(const StatorWorkpiecePositionDetectionResult& detectionResult, int cameraIndex)
|
||
{
|
||
if (!m_pTCPServer || !m_bTCPConnected.load()) {
|
||
LOG_DEBUG("TCP server not available, skipping result transmission\n");
|
||
return;
|
||
}
|
||
|
||
LOG_DEBUG("Sending detection result to TCP clients, camera index: %d\n", cameraIndex);
|
||
|
||
QStringList fields;
|
||
if (detectionResult.errorCode != SUCCESS || detectionResult.positions.empty()) {
|
||
fields << "0";
|
||
} else {
|
||
int poseOutputOrder = POSE_ORDER_RPY;
|
||
if (m_pConfigManager) {
|
||
poseOutputOrder = m_pConfigManager->GetConfigResult().plcRobotServerConfig.poseOutputOrder;
|
||
}
|
||
|
||
fields << IntField(static_cast<int>(detectionResult.positions.size()));
|
||
for (const WorkpiecePosition& position : detectionResult.positions) {
|
||
fields << IntField(position.workpieceType)
|
||
<< NumberField(position.x)
|
||
<< NumberField(position.y)
|
||
<< NumberField(position.z);
|
||
AppendPoseFields(fields, position, poseOutputOrder);
|
||
}
|
||
}
|
||
|
||
const int sendResult = m_pTCPServer->SendTextFrame(fields.join(QStringLiteral("_")).toUtf8());
|
||
if (sendResult == 0) {
|
||
LOG_INFO("Detection result sent to TCP clients successfully\n");
|
||
if (auto pStatus = GetStatusCallback<IYStatorWorkpiecePositionStatus>()) {
|
||
pStatus->OnStatusUpdate("【TCP协议】已发送工件检测结果");
|
||
}
|
||
} else {
|
||
LOG_ERROR("Failed to send detection result to TCP clients, error code: %d\n", sendResult);
|
||
}
|
||
}
|
||
|
||
void StatorWorkpiecePositionPresenter::_SendBinInfoToTCP(const WD_HRM_BinInfo& binInfo, int cameraIndex, int errorCode)
|
||
{
|
||
(void)binInfo;
|
||
(void)cameraIndex;
|
||
(void)errorCode;
|
||
LOG_DEBUG("Bin detection has no TCP result frame\n");
|
||
}
|
||
|
||
void StatorWorkpiecePositionPresenter::_SendDetectionErrorToTCP(int errorCode, int cameraIndex)
|
||
{
|
||
if (!m_pTCPServer || !m_bTCPConnected.load()) {
|
||
LOG_DEBUG("TCP server not available, skipping detection error transmission\n");
|
||
return;
|
||
}
|
||
|
||
(void)cameraIndex;
|
||
const QString frame = QString("Error_%1_检测失败").arg(errorCode);
|
||
const int sendResult = m_pTCPServer->SendTextFrame(frame.toUtf8());
|
||
if (sendResult != 0) {
|
||
LOG_ERROR("Failed to send detection error to TCP clients, error code: %d\n", sendResult);
|
||
}
|
||
}
|