1820 lines
66 KiB
C++
1820 lines
66 KiB
C++
#include "BeltTearingPresenter.h"
|
||
#include "PathManager.h"
|
||
#include "version.h"
|
||
#include "PointCloudImageUtils.h"
|
||
#include <QUuid>
|
||
#include <QDataStream>
|
||
#include <QJsonDocument>
|
||
#include <QJsonObject>
|
||
#include <QJsonArray>
|
||
#include <mutex>
|
||
#include <deque>
|
||
#include <QDateTime>
|
||
#include <QBuffer>
|
||
#include <QMutexLocker>
|
||
#include <QFile>
|
||
#include <QDir>
|
||
#include <iostream>
|
||
#include <thread>
|
||
#include <future>
|
||
#include <QPainter>
|
||
#include <QPainterPath>
|
||
#include <QFont>
|
||
#include <cmath>
|
||
|
||
#include "IVrUtils.h"
|
||
#include "VrError.h"
|
||
#include <algorithm>
|
||
|
||
// 静态实例指针
|
||
BeltTearingPresenter* BeltTearingPresenter::s_instance = nullptr;
|
||
|
||
BeltTearingPresenter::BeltTearingPresenter(QObject *parent)
|
||
: QObject(parent)
|
||
, m_tcpServer(nullptr)
|
||
, m_tearingProtocol(nullptr)
|
||
, m_config(nullptr)
|
||
, m_eyeDevice(nullptr)
|
||
, m_cameraInitTimer(new QTimer(this))
|
||
, m_cameraInitialized(false)
|
||
, m_cameraDetecting(false)
|
||
, m_lineCounter(0)
|
||
, m_maxQueueSize(300) // 默认值
|
||
, m_generationInterval(100) // 默认值
|
||
, m_bAlgoDetectThreadRunning(false)
|
||
, m_pRobotProtocol(nullptr) // 初始化RobotProtocol指针
|
||
, m_pModbusRTUMaster(nullptr) // 初始化ModbusRTUMaster指针
|
||
, m_bRobotConnected(false) // 初始化连接状态
|
||
{
|
||
// 设置静态实例
|
||
s_instance = this;
|
||
|
||
QString versionWithBuildTime = QString("%1.%2_%3%4%5%6%7%8")
|
||
.arg(BELT_TEARING_SERVER_VERSION_STRING)
|
||
.arg(BELT_TEARING_SERVER_VERSION_BUILD)
|
||
.arg(YEAR)
|
||
.arg(MONTH, 2, 10, QChar('0'))
|
||
.arg(DAY, 2, 10, QChar('0'))
|
||
.arg(HOUR, 2, 10, QChar('0'))
|
||
.arg(MINUTE, 2, 10, QChar('0'))
|
||
.arg(SECOND, 2, 10, QChar('0'));
|
||
// 打印版本信息
|
||
LOG_INFO("Initializing %s\n", versionWithBuildTime.toStdString().c_str());
|
||
|
||
// 连接定时器信号
|
||
connect(m_cameraInitTimer, &QTimer::timeout, this, &BeltTearingPresenter::onCameraInitTimer);
|
||
|
||
// 启动算法检测线程
|
||
m_bAlgoDetectThreadRunning = true;
|
||
m_algoDetectThread = std::thread(&BeltTearingPresenter::_AlgoDetectThread, this);
|
||
m_algoDetectThread.detach();
|
||
|
||
// 创建TCP服务器实例
|
||
if (!VrCreatYTCPServer(&m_tcpServer)) {
|
||
LOG_ERROR("Failed to create TCP server\n");
|
||
m_tcpServer = nullptr;
|
||
}
|
||
|
||
// 创建配置实例
|
||
IVrBeltTearingConfig::CreateInstance(&m_config);
|
||
if (m_config) {
|
||
m_config->SetConfigChangeNotify(this);
|
||
}
|
||
|
||
// 初始化SDK算法参数 - 使用配置系统的默认值
|
||
m_algorithmParam = configToSDKParam(BeltTearingConfigResult());
|
||
|
||
// 初始化相机
|
||
initializeCamera();
|
||
|
||
// 初始化机械臂协议
|
||
int nRet = InitRobotProtocol();
|
||
if (nRet != 0) {
|
||
LOG_WARNING("Robot protocol initialization failed\n");
|
||
m_bRobotConnected = false;
|
||
} else {
|
||
LOG_INFO("Robot protocol initialization successful\n");
|
||
m_bRobotConnected = true;
|
||
}
|
||
|
||
|
||
InitModbusRTUMaster();
|
||
}
|
||
|
||
BeltTearingPresenter::~BeltTearingPresenter()
|
||
{
|
||
// 清除静态实例
|
||
s_instance = nullptr;
|
||
|
||
stopServer();
|
||
stopCamera(); // 忽略返回值，因为在析构函数中无法处理
|
||
|
||
// 停止算法检测线程
|
||
m_bAlgoDetectThreadRunning = false;
|
||
m_algoDetectCondition.notify_all();
|
||
|
||
// 等待算法检测线程结束
|
||
if (m_algoDetectThread.joinable()) {
|
||
m_algoDetectThread.join();
|
||
}
|
||
|
||
// 清理激光线队列中的内存
|
||
std::lock_guard<std::mutex> lock(m_queueMutex);
|
||
for (auto& line : m_laserLineQueue) {
|
||
if (line.p3DPoint) {
|
||
free(line.p3DPoint);
|
||
line.p3DPoint = nullptr;
|
||
}
|
||
}
|
||
m_laserLineQueue.clear();
|
||
|
||
// 释放TearingTcpProtocol
|
||
if (m_tearingProtocol) {
|
||
delete m_tearingProtocol;
|
||
m_tearingProtocol = nullptr;
|
||
}
|
||
|
||
if (m_tcpServer) {
|
||
delete m_tcpServer;
|
||
m_tcpServer = nullptr;
|
||
}
|
||
|
||
if (m_config) {
|
||
delete m_config;
|
||
m_config = nullptr;
|
||
}
|
||
|
||
// 释放RobotProtocol资源
|
||
if (m_pRobotProtocol) {
|
||
m_pRobotProtocol->Deinitialize();
|
||
delete m_pRobotProtocol;
|
||
m_pRobotProtocol = nullptr;
|
||
}
|
||
|
||
// 释放RobotProtocolSimplified资源
|
||
if (m_pRobotProtocolSimplified) {
|
||
m_pRobotProtocolSimplified->Deinitialize();
|
||
delete m_pRobotProtocolSimplified;
|
||
m_pRobotProtocolSimplified = nullptr;
|
||
}
|
||
|
||
// 释放ModbusRTUMaster资源
|
||
if (m_pModbusRTUMaster) {
|
||
m_pModbusRTUMaster->Deinitialize();
|
||
delete m_pModbusRTUMaster;
|
||
m_pModbusRTUMaster = nullptr;
|
||
}
|
||
}
|
||
|
||
bool BeltTearingPresenter::loadConfiguration(const QString& configFilePath)
|
||
{
|
||
if (!m_config) {
|
||
LOG_WARNING("Config instance not created\n");
|
||
return false;
|
||
}
|
||
|
||
m_configFilePath = configFilePath;
|
||
|
||
try {
|
||
// 加载配置文件
|
||
m_configResult = m_config->LoadConfig(configFilePath.toStdString());
|
||
|
||
LOG_INFO("Configuration loaded successfully:\n");
|
||
LOG_INFO(" Server Port: %d\n", m_configResult.serverPort);
|
||
LOG_INFO(" Project Type: %d\n", static_cast<int>(m_configResult.projectType));
|
||
LOG_INFO(" Servers count: %d\n", m_configResult.servers.size());
|
||
|
||
// 应用队列处理参数（如果配置文件中没有，将使用构造函数中的默认值300和100）
|
||
if (m_configResult.queueProcessParam.maxQueueSize > 0) {
|
||
m_maxQueueSize = m_configResult.queueProcessParam.maxQueueSize;
|
||
}
|
||
if (m_configResult.queueProcessParam.generationInterval > 0) {
|
||
m_generationInterval = m_configResult.queueProcessParam.generationInterval;
|
||
}
|
||
LOG_INFO(" Max Queue Size: %d\n", m_maxQueueSize);
|
||
LOG_INFO(" Generation Interval: %d\n", m_generationInterval);
|
||
|
||
// 应用算法参数
|
||
applyAlgorithmParameters(m_configResult.algorithmParams);
|
||
|
||
return true;
|
||
|
||
} catch (const std::exception& e) {
|
||
LOG_ERROR("Error loading configuration: %s\n", e.what());
|
||
return false;
|
||
}
|
||
}
|
||
|
||
void BeltTearingPresenter::applyAlgorithmParameters(const BeltTearingAlgorithmParams& params)
|
||
{
|
||
// 使用配置结构转换为SDK参数
|
||
BeltTearingConfigResult tempConfig;
|
||
tempConfig.algorithmParams = params;
|
||
m_algorithmParam = configToSDKParam(tempConfig);
|
||
|
||
// 应用算法参数
|
||
LOG_DEBUG("Applying SDK algorithm parameters...\n");
|
||
LOG_DEBUG(" Scan X Scale: %f\n", m_algorithmParam.scanXScale);
|
||
LOG_DEBUG(" Scan Y Scale: %f\n", m_algorithmParam.scanYScale);
|
||
LOG_DEBUG(" Difference Bin Threshold: %f\n", m_algorithmParam.differnceBinTh);
|
||
LOG_DEBUG(" Min Tear Length: %f\n", m_algorithmParam.tearingMinLen);
|
||
LOG_DEBUG(" Min Tear Gap: %f\n", m_algorithmParam.tearingMinGap);
|
||
LOG_DEBUG(" Same Gap Threshold: %f\n", m_algorithmParam.extractPara.sameGapTh);
|
||
LOG_DEBUG(" Gap Check Window: %d\n", m_algorithmParam.extractPara.gapChkWin);
|
||
|
||
// 监控参数
|
||
LOG_DEBUG(" Check Interval: %d ms\n", params.monitoringParam.checkInterval);
|
||
LOG_DEBUG(" Alert Threshold: %f\n", params.monitoringParam.alertThreshold);
|
||
|
||
// 调试参数
|
||
if (m_configResult.debugParam.enableDebug) {
|
||
LOG_DEBUG("Debug mode enabled:\n");
|
||
LOG_DEBUG(" Save debug images: %s\n", (m_configResult.debugParam.saveDebugImage ? "Yes" : "No"));
|
||
LOG_DEBUG(" Print detail log: %s\n", (m_configResult.debugParam.printDetailLog ? "Yes" : "No"));
|
||
}
|
||
}
|
||
|
||
void BeltTearingPresenter::OnConfigChanged(const BeltTearingConfigResult& configResult)
|
||
{
|
||
LOG_INFO("Configuration changed notification received\n");
|
||
|
||
if (configResult.serverPort != m_configResult.serverPort)
|
||
{
|
||
// 如果服务器端口改变，可能需要重启服务器
|
||
LOG_INFO("Server port changed, restarting server...\n");
|
||
stopServer();
|
||
startServer(configResult.serverPort);
|
||
}
|
||
|
||
// 更新配置结果
|
||
m_configResult = configResult;
|
||
|
||
// 重新应用算法参数
|
||
applyAlgorithmParameters(configResult.algorithmParams);
|
||
}
|
||
|
||
void BeltTearingPresenter::sendTestData(std::string fileName){
|
||
if (!m_tcpServer) {
|
||
LOG_WARNING("TCP server not initialized, cannot send test data\n");
|
||
return;
|
||
}
|
||
|
||
if(m_pRobotProtocol){
|
||
m_pRobotProtocol->SetWorkStatus(RobotProtocol::WORK_STATUS_WORKING);
|
||
}
|
||
|
||
// 判断文件类型
|
||
std::ifstream inputFile(fileName);
|
||
if (!inputFile.is_open()) {
|
||
LOG_WARN("UN open file \n");
|
||
return;
|
||
} else {
|
||
LOG_DEBUG("------------------------ \n");
|
||
}
|
||
|
||
std::string line;
|
||
std::vector<SVzNL3DPosition> sVzNLPostion;
|
||
sVzNLPostion.clear();
|
||
|
||
int nIndex = 0;
|
||
SVzLaserLineData pLaserLine;
|
||
|
||
while (std::getline(inputFile, line)) {
|
||
if (line.find("Line_") == 0) {
|
||
|
||
if(!sVzNLPostion.empty()){
|
||
pLaserLine.p3DPoint = sVzNLPostion.data();
|
||
pLaserLine.nPointCount = sVzNLPostion.size();
|
||
addLaserLineToQueue(&pLaserLine);
|
||
}
|
||
sscanf(line.c_str(), "Line_%lld_%lld", &pLaserLine.llFrameIdx, &pLaserLine.llTimeStamp);
|
||
sVzNLPostion.clear();
|
||
|
||
nIndex++;
|
||
|
||
} else if (line.find("{") == 0) {
|
||
float lx, ly, rx, ry;
|
||
SVzNL3DPosition pos;
|
||
sscanf(line.c_str(), "{ %lf, %lf, %lf }-{ %f, %f}-{ %f, %f }",
|
||
&pos.pt3D.x, &pos.pt3D.y, &pos.pt3D.z, &lx, &ly, &rx, &ry);
|
||
|
||
sVzNLPostion.push_back(pos);
|
||
}
|
||
}
|
||
|
||
inputFile.close();
|
||
}
|
||
|
||
// 发送模拟撕裂结果数据
|
||
void BeltTearingPresenter::sendSimulationData()
|
||
{
|
||
LOG_INFO("Sending simulation data\n");
|
||
|
||
if(m_pRobotProtocol){
|
||
m_pRobotProtocol->SetWorkStatus(RobotProtocol::WORK_STATUS_WORKING);
|
||
}
|
||
|
||
// 创建模拟的撕裂结果数据
|
||
std::vector<SSG_beltTearingInfo> simulationResults;
|
||
|
||
// 创建第一个模拟撕裂结果
|
||
SSG_beltTearingInfo tear1;
|
||
tear1.tearID = 11;
|
||
tear1.tearStatus = keSG_tearStatus_New; // 假设1表示有效撕裂
|
||
tear1.tearWidth = 15.5f; // 撕裂宽度 15.5mm
|
||
tear1.tearDepth = 8.2f; // 撕裂深度 8.2mm
|
||
tear1.roi.left = 0.0f;
|
||
tear1.roi.top = 0.0f;
|
||
tear1.roi.right = 60.0f;
|
||
tear1.roi.bottom = 1.0f;
|
||
// 其他字段由于结构定义不明确，暂时不填充
|
||
|
||
simulationResults.push_back(tear1);
|
||
|
||
// 创建第二个模拟撕裂结果
|
||
SSG_beltTearingInfo tear2;
|
||
tear2.tearID = 12;
|
||
tear2.tearStatus = keSG_tearStatus_Growing; // 假设2表示无效撕裂
|
||
tear2.tearWidth = 22.3f; // 撕裂宽度 22.3mm
|
||
tear2.tearDepth = 12.7f; // 撕裂深度 12.7mm
|
||
tear2.roi.left = 20.0f;
|
||
tear2.roi.top = 0.0f;
|
||
tear2.roi.right = 50.0f;
|
||
tear2.roi.bottom = 1.0f;
|
||
// 其他字段由于结构定义不明确，暂时不填充
|
||
|
||
simulationResults.push_back(tear2);
|
||
// 创建一个模拟的图像
|
||
QImage simulationImage(800, 600, QImage::Format_RGB888);
|
||
|
||
// 填充背景色
|
||
simulationImage.fill(Qt::white);
|
||
|
||
// 获取当前时间
|
||
QDateTime currentTime = QDateTime::currentDateTime();
|
||
QString timeString = currentTime.toString("yyyy-MM-dd hh:mm:ss");
|
||
|
||
// 在图像上绘制时间信息
|
||
QPainter painter(&simulationImage);
|
||
if (painter.isActive()) {
|
||
QFont font("Arial", 16, QFont::Bold);
|
||
painter.setFont(font);
|
||
painter.setPen(Qt::black);
|
||
|
||
// 在图像顶部绘制时间
|
||
painter.drawText(QRect(10, 10, 780, 40), Qt::AlignLeft | Qt::AlignVCenter, QString("时间: %1").arg(timeString));
|
||
painter.end();
|
||
}
|
||
|
||
// 发送撕裂结果到所有客户端
|
||
sendTearingResults(simulationResults);
|
||
SendDetectionResultToRobot(simulationResults); // 发送检测结果到机械臂
|
||
sendImageToClients(simulationImage);
|
||
|
||
m_tearingProtocol->sendDetectResult(simulationResults, simulationImage);
|
||
|
||
LOG_INFO("Simulation image data sent successfully (timestamp: %s)\n", timeString.toStdString().c_str());
|
||
}
|
||
|
||
bool BeltTearingPresenter::initializeCamera()
|
||
{
|
||
if (m_eyeDevice) {
|
||
return true; // 已经初始化过
|
||
}
|
||
|
||
int result = IVrEyeDevice::CreateObject(&m_eyeDevice);
|
||
if (result != 0 || !m_eyeDevice) {
|
||
LOG_ERROR("Failed to create VrEyeDevice object, error code: %d\n", result);
|
||
return false;
|
||
}
|
||
|
||
result = m_eyeDevice->InitDevice();
|
||
if (result != 0) {
|
||
LOG_ERROR("Failed to initialize VrEyeDevice, error code: %d\n", result);
|
||
delete m_eyeDevice;
|
||
m_eyeDevice = nullptr;
|
||
return false;
|
||
}
|
||
|
||
// 设置状态回调
|
||
result = m_eyeDevice->SetStatusCallback(OnStatusCallback, this);
|
||
if (result != 0) {
|
||
LOG_WARNING("Failed to set status callback, error code: %d\n", result);
|
||
}
|
||
|
||
LOG_INFO("VrEyeDevice initialized successfully\n");
|
||
return true;
|
||
}
|
||
|
||
int BeltTearingPresenter::startCamera()
|
||
{
|
||
if (!initializeCamera()) {
|
||
// 启动定时器持续重试初始化
|
||
LOG_WARNING("Camera initialization failed, starting retry timer...\n");
|
||
m_cameraInitTimer->start(5000); // 每5秒重试一次
|
||
return ERR_CODE(DEV_NO_OPEN); // 初始化失败
|
||
}
|
||
|
||
// 准备相机IP参数（使用配置中的第一个相机）
|
||
const char* pCameraIP = nullptr;
|
||
if (!m_configResult.cameras.empty() && !m_configResult.cameras[0].cameraIP.empty()) {
|
||
pCameraIP = m_configResult.cameras[0].cameraIP.c_str();
|
||
LOG_INFO("Using configured camera [%s] IP: %s\n",
|
||
m_configResult.cameras[0].name.c_str(), pCameraIP);
|
||
} else {
|
||
LOG_INFO("No camera IP configured, using auto-discovery\n");
|
||
}
|
||
|
||
// 尝试打开设备
|
||
int result = m_eyeDevice->OpenDevice(pCameraIP, false, false, true);
|
||
if (result != 0) {
|
||
LOG_WARNING("Failed to open camera device, error code: %d, retrying...\n", result);
|
||
|
||
// 启动定时器持续重试连接
|
||
m_cameraInitTimer->start(3000); // 每3秒重试一次
|
||
return result; // 返回错误码
|
||
}
|
||
|
||
LOG_INFO("Camera opened successfully\n");
|
||
|
||
// 停止重试定时器
|
||
m_cameraInitTimer->stop();
|
||
m_cameraInitialized = true;
|
||
|
||
// 开始检测
|
||
result = m_eyeDevice->StartDetect(OnPointCloudCallback, keResultDataType_Position, this);
|
||
LOG_DEBUG("Camera detection started, result: %d\n", result);
|
||
if (result != 0) {
|
||
LOG_ERROR("Failed to start detection, error code: %d\n", result);
|
||
return result; // 返回错误码
|
||
}
|
||
|
||
m_cameraDetecting = true;
|
||
|
||
// 设置机械臂工作状态为工作中
|
||
if (m_pRobotProtocol) {
|
||
int robotResult = m_pRobotProtocol->SetWorkStatus(RobotProtocol::WORK_STATUS_WORKING);
|
||
if (robotResult != 0) {
|
||
LOG_WARNING("Failed to set robot work status to working, error code: %d\n", robotResult);
|
||
return robotResult; // 返回错误码
|
||
} else {
|
||
LOG_DEBUG("Robot work status set to working\n");
|
||
}
|
||
}
|
||
|
||
return SUCCESS; // 成功
|
||
}
|
||
|
||
int BeltTearingPresenter::stopCamera()
|
||
{
|
||
m_cameraInitTimer->stop();
|
||
|
||
if (m_eyeDevice) {
|
||
if (m_cameraDetecting) {
|
||
m_eyeDevice->StopDetect();
|
||
m_cameraDetecting = false;
|
||
}
|
||
|
||
if (m_cameraInitialized) {
|
||
m_eyeDevice->CloseDevice();
|
||
m_cameraInitialized = false;
|
||
}
|
||
|
||
delete m_eyeDevice;
|
||
m_eyeDevice = nullptr;
|
||
}
|
||
|
||
// 设置机械臂工作状态为空闲
|
||
if (m_pRobotProtocol) {
|
||
int robotResult = m_pRobotProtocol->SetWorkStatus(RobotProtocol::WORK_STATUS_IDLE);
|
||
if (robotResult != 0) {
|
||
LOG_WARNING("Failed to set robot work status to idle, error code: %d\n", robotResult);
|
||
return ERR_CODE(DEV_CTRL_ERR); // 返回设备控制错误码
|
||
} else {
|
||
LOG_DEBUG("Robot work status set to idle\n");
|
||
}
|
||
}
|
||
|
||
LOG_INFO("Camera stopped\n");
|
||
return SUCCESS; // 成功
|
||
}
|
||
|
||
void BeltTearingPresenter::onCameraInitTimer()
|
||
{
|
||
// 尝试重新初始化和连接相机
|
||
if (!m_eyeDevice) {
|
||
if (!initializeCamera()) {
|
||
return; // 继续重试
|
||
}
|
||
}
|
||
|
||
// 准备相机IP参数（使用配置中的第一个相机）
|
||
const char* pCameraIP = nullptr;
|
||
if (!m_configResult.cameras.empty() && !m_configResult.cameras[0].cameraIP.empty()) {
|
||
pCameraIP = m_configResult.cameras[0].cameraIP.c_str();
|
||
}
|
||
|
||
// 尝试连接相机
|
||
int result = m_eyeDevice->OpenDevice(pCameraIP, false, false, true);
|
||
if (result == 0) {
|
||
// 连接成功，停止定时器
|
||
m_cameraInitTimer->stop();
|
||
m_cameraInitialized = true;
|
||
|
||
// 开始检测
|
||
result = m_eyeDevice->StartDetect(OnPointCloudCallback, keResultDataType_Position, this);
|
||
if (result == 0) {
|
||
m_cameraDetecting = true;
|
||
LOG_INFO("Camera connection restored successfully!\n");
|
||
|
||
// 设置机械臂工作状态为工作中
|
||
if (m_pRobotProtocol) {
|
||
int robotResult = m_pRobotProtocol->SetWorkStatus(RobotProtocol::WORK_STATUS_WORKING);
|
||
if (robotResult != 0) {
|
||
LOG_WARNING("Failed to set robot work status to working when camera reconnected, error code: %d\n", robotResult);
|
||
} else {
|
||
LOG_DEBUG("Robot work status set to working when camera reconnected\n");
|
||
}
|
||
}
|
||
} else {
|
||
LOG_ERROR("Failed to start detection after reconnection, error code: %d\n", result);
|
||
}
|
||
} else {
|
||
LOG_WARNING("Still unable to connect to camera, continuing retry...\n");
|
||
}
|
||
}
|
||
|
||
void BeltTearingPresenter::OnStatusCallback(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pInfoParam)
|
||
{
|
||
BeltTearingPresenter* presenter = static_cast<BeltTearingPresenter*>(pInfoParam);
|
||
if (!presenter) return;
|
||
|
||
LOG_DEBUG("Camera status changed: %d\n", static_cast<int>(eStatus));
|
||
|
||
// 处理相机状态变化
|
||
switch (eStatus) {
|
||
case keDeviceWorkStatus_Eye_Comming:
|
||
LOG_INFO("Camera connected\n");
|
||
break;
|
||
case keDeviceWorkStatus_Offline:
|
||
LOG_WARNING("Camera disconnected, attempting to reconnect...\n");
|
||
presenter->m_cameraInitialized = false;
|
||
presenter->m_cameraDetecting = false;
|
||
|
||
// 设置机械臂工作状态为空闲
|
||
if (presenter->m_pRobotProtocol) {
|
||
int robotResult = presenter->m_pRobotProtocol->SetWorkStatus(RobotProtocol::WORK_STATUS_IDLE);
|
||
if (robotResult != 0) {
|
||
LOG_WARNING("Failed to set robot work status to idle when camera disconnected, error code: %d\n", robotResult);
|
||
} else {
|
||
LOG_DEBUG("Robot work status set to idle when camera disconnected\n");
|
||
}
|
||
}
|
||
|
||
// 开始重连尝试
|
||
if (!presenter->m_cameraInitTimer->isActive()) {
|
||
presenter->m_cameraInitTimer->start(3000);
|
||
}
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
void BeltTearingPresenter::OnPointCloudCallback(EVzResultDataType eDataType, SVzLaserLineData* pLaserLinePoint, void* pParam)
|
||
{
|
||
if(keResultDataType_Position != eDataType) return;
|
||
|
||
BeltTearingPresenter* presenter = static_cast<BeltTearingPresenter*>(pParam);
|
||
if (!presenter || !pLaserLinePoint) return;
|
||
|
||
// 处理点云数据
|
||
// 将激光线数据添加到队列（用于图像生成和算法检测）
|
||
presenter->addLaserLineToQueue(pLaserLinePoint);
|
||
}
|
||
|
||
void BeltTearingPresenter::sendTearingResults(const std::vector<SSG_beltTearingInfo>& results)
|
||
{
|
||
if (results.empty() || m_clients.isEmpty() || !m_tcpServer) {
|
||
return;
|
||
}
|
||
|
||
// 将检测结果转换为JSON格式
|
||
QJsonArray tearingArray;
|
||
|
||
for (const auto& result : results) {
|
||
QJsonObject tearingObj;
|
||
// tearingObj["level"] = QString::number(result.level);
|
||
tearingObj["id"] = QString::number(result.tearID);
|
||
tearingObj["tearStatus"] = QString::number(static_cast<int>(result.tearStatus));
|
||
tearingObj["tearType"] = QString::number(result.tearType);
|
||
tearingObj["statLineIdx"] = QString::number(result.statLineIdx);
|
||
tearingObj["endLineIdx"] = QString::number(result.endLineIdx);
|
||
tearingObj["tearDepth"] = QString::number(result.tearDepth);
|
||
tearingObj["tearWidth"] = QString::number(result.tearWidth);
|
||
double dWidth = result.roi.right - result.roi.left;
|
||
double dLength = result.roi.bottom - result.roi.top;
|
||
tearingObj["tearLength"] = QString::number(dWidth > dLength ? dWidth : dLength);
|
||
tearingObj["roiLeft"] = QString::number(result.roi.left);
|
||
tearingObj["roiRight"] = QString::number(result.roi.right);
|
||
tearingObj["roiTop"] = QString::number(result.roi.top);
|
||
tearingObj["roiBottom"] = QString::number(result.roi.bottom);
|
||
// tearingObj["imageFile"] = QString::fromStdString(result.imageFile);
|
||
// tearingObj["older"] = QString::fromStdString(result.older);
|
||
tearingObj["timestamp"] = QDateTime::currentDateTime().toString(Qt::ISODate);
|
||
|
||
tearingArray.append(tearingObj);
|
||
}
|
||
|
||
// 转换为JSON字符串
|
||
QJsonDocument doc(tearingArray);
|
||
QByteArray message = doc.toJson(QJsonDocument::Compact);
|
||
|
||
// 创建数据包
|
||
QByteArray package;
|
||
QDataStream stream(&package, QIODevice::WriteOnly);
|
||
stream.setByteOrder(QDataStream::BigEndian);
|
||
stream << quint8(static_cast<quint8>(ByteDataType::Text)) << quint32(message.size());
|
||
stream.writeRawData(message.constData(), message.size());
|
||
package.append("___END___\r\n");
|
||
|
||
// 发送到所有连接的客户端
|
||
bool success = m_tcpServer->SendAllData(package.constData(), package.size());
|
||
if (!success) {
|
||
LOG_WARNING("Failed to send tearing results to all clients\n");
|
||
}
|
||
}
|
||
|
||
bool BeltTearingPresenter::startServer(quint16 port)
|
||
{
|
||
if (!m_tcpServer) {
|
||
LOG_ERROR("TCP server not created\n");
|
||
return false;
|
||
}
|
||
|
||
// 先停止现有服务器
|
||
stopServer();
|
||
|
||
m_port = port;
|
||
m_tcpPort = m_configResult.tcpPort; // 从配置读取新协议端口
|
||
|
||
// 初始化旧协议TCP服务器
|
||
if (!m_tcpServer->Init(port, true)) { // 启用Nagle算法优化
|
||
LOG_ERROR("Failed to initialize TCP server on port %d\n", port);
|
||
return false;
|
||
}
|
||
|
||
// 设置事件回调
|
||
m_tcpServer->SetEventCallback(OnServerEvent);
|
||
|
||
// 启动旧协议TCP服务器
|
||
if (!m_tcpServer->Start(OnServerRecv, false)) { // 不使用自定义协议
|
||
LOG_ERROR("Failed to start TCP server on port %d\n", port);
|
||
return false;
|
||
}
|
||
|
||
LOG_INFO("TCP server (old protocol) started on port %d\n", port);
|
||
|
||
// 创建并启动TearingTcpProtocol
|
||
if (!m_tearingProtocol) {
|
||
m_tearingProtocol = new TearingTcpProtocol(this);
|
||
|
||
// 设置TCP端口
|
||
m_tearingProtocol->setTcpPort(m_tcpPort);
|
||
|
||
// 设置速度控制回调
|
||
m_tearingProtocol->setSpeedCallback([this](int speed) -> int {
|
||
LOG_INFO("Speed callback triggered: %d mm/s\n", speed);
|
||
|
||
// 调用相机接口设置速度
|
||
if (m_eyeDevice) {
|
||
// 将速度从 mm/s 转换为相机摆动速度参数（根据实际需要调整转换公式）
|
||
// 这里假设直接使用速度值，实际可能需要根据相机文档进行单位转换
|
||
float fSpeed = static_cast<float>(speed);
|
||
|
||
int result = m_eyeDevice->SetSwingSpeed(fSpeed);
|
||
if (result == 0) {
|
||
LOG_INFO("Camera swing speed set successfully: %.2f\n", fSpeed);
|
||
return SUCCESS; // 成功
|
||
} else {
|
||
LOG_ERROR("Failed to set camera swing speed, error code: %d\n", result);
|
||
return result; // 返回错误码
|
||
}
|
||
} else {
|
||
LOG_WARNING("Camera device not initialized, cannot set speed\n");
|
||
return ERR_CODE(DEV_NO_OPEN); // 设备未初始化错误
|
||
}
|
||
});
|
||
|
||
// 设置启停控制回调
|
||
m_tearingProtocol->setControlCallback([this](bool control) -> int {
|
||
LOG_INFO("Control callback triggered: %s\n", control ? "start" : "stop");
|
||
int result = 0;
|
||
if (control) {
|
||
result = StartWork();
|
||
} else {
|
||
result = StopWork();
|
||
}
|
||
return result;
|
||
});
|
||
|
||
// 启动协议处理
|
||
m_tearingProtocol->start(30); // 30秒心跳间隔
|
||
}
|
||
|
||
LOG_INFO("TearingTcpProtocol started on port %d\n", m_tcpPort);
|
||
|
||
return true;
|
||
}
|
||
|
||
void BeltTearingPresenter::stopServer()
|
||
{
|
||
// 停止TearingTcpProtocol
|
||
if (m_tearingProtocol) {
|
||
m_tearingProtocol->stop();
|
||
}
|
||
|
||
// 停止旧协议TCP服务器
|
||
if (m_tcpServer) {
|
||
// 清空客户端映射
|
||
m_clients.clear();
|
||
|
||
m_tcpServer->Stop();
|
||
m_tcpServer->Close();
|
||
LOG_INFO("TCP server stopped\n");
|
||
}
|
||
|
||
m_port = 0;
|
||
m_tcpPort = 0;
|
||
}
|
||
|
||
// 静态回调函数实现
|
||
void BeltTearingPresenter::OnServerRecv(const TCPClient* pClient, const char* pData, const unsigned int nLen)
|
||
{
|
||
if (s_instance) {
|
||
s_instance->handleServerRecv(pClient, pData, nLen);
|
||
}
|
||
}
|
||
|
||
void BeltTearingPresenter::OnServerEvent(const TCPClient* pClient, TCPServerEventType eventType)
|
||
{
|
||
if (s_instance) {
|
||
s_instance->handleServerEvent(pClient, eventType);
|
||
}
|
||
}
|
||
|
||
// 实例方法实现
|
||
void BeltTearingPresenter::handleServerRecv(const TCPClient* pClient, const char* pData, const unsigned int nLen)
|
||
{
|
||
QString clientId = generateClientId(pClient);
|
||
LOG_DEBUG("Received %d bytes from client %s\n", nLen, clientId.toStdString().c_str());
|
||
|
||
// 解析数据包协议头
|
||
if (nLen < 5) {
|
||
LOG_ERROR("Packet too small: %d bytes\n", nLen);
|
||
return;
|
||
}
|
||
|
||
quint8 dataType;
|
||
quint32 dataSize;
|
||
|
||
QDataStream stream(QByteArray(pData, nLen));
|
||
stream.setByteOrder(QDataStream::BigEndian);
|
||
stream >> dataType >> dataSize;
|
||
|
||
// 验证数据包完整性
|
||
if (dataSize + 5 > nLen) {
|
||
LOG_ERROR("Incomplete packet: expected %d+5, got %d bytes\n", dataSize, nLen);
|
||
return;
|
||
}
|
||
|
||
QByteArray payloadData(pData + 5, dataSize);
|
||
|
||
// LOG_DEBUG("Parsed packet: dataType=%d, dataSize=%d, payload_size=%d\n", dataType, dataSize, payloadData.size());
|
||
|
||
switch (static_cast<ByteDataType>(dataType)) {
|
||
case ByteDataType::Text:
|
||
// 处理文本数据
|
||
handleAlgorithmParameterUpdate(payloadData);
|
||
break;
|
||
|
||
case ByteDataType::ReadConfig:
|
||
// 处理读取配置请求
|
||
handleReadConfig(pClient);
|
||
break;
|
||
|
||
case ByteDataType::WriteConfig:
|
||
// 处理写入配置请求
|
||
handleWriteConfig(payloadData);
|
||
break;
|
||
|
||
default:
|
||
LOG_ERROR("Unknown data type %d\n", dataType);
|
||
break;
|
||
}
|
||
}
|
||
|
||
void BeltTearingPresenter::handleServerEvent(const TCPClient* pClient, TCPServerEventType eventType)
|
||
{
|
||
QString clientId = generateClientId(pClient);
|
||
|
||
switch (eventType) {
|
||
case TCP_EVENT_CLIENT_CONNECTED:
|
||
m_clients[clientId] = pClient;
|
||
LOG_INFO("Client connected: %s\n", clientId.toStdString().c_str());
|
||
break;
|
||
|
||
case TCP_EVENT_CLIENT_DISCONNECTED:
|
||
m_clients.remove(clientId);
|
||
LOG_INFO("Client disconnected: %s\n", clientId.toStdString().c_str());
|
||
break;
|
||
|
||
case TCP_EVENT_CLIENT_EXCEPTION:
|
||
m_clients.remove(clientId);
|
||
LOG_WARNING("Client exception: %s\n", clientId.toStdString().c_str());
|
||
break;
|
||
}
|
||
}
|
||
|
||
QString BeltTearingPresenter::generateClientId(const TCPClient* client)
|
||
{
|
||
return QString("Client_%1").arg(client->m_nFD);
|
||
}
|
||
|
||
// 激光线队列管理方法实现
|
||
void BeltTearingPresenter::addLaserLineToQueue(const SVzLaserLineData* laserLine)
|
||
{
|
||
if(laserLine == nullptr || laserLine->nPointCount <= 0){
|
||
return;
|
||
}
|
||
|
||
// 创建激光线数据的深拷贝
|
||
SVzLaserLineData copiedLine = *laserLine;
|
||
|
||
// 深拷贝点云数据
|
||
size_t pointDataSize = laserLine->nPointCount * sizeof(SVzNL3DPosition);
|
||
copiedLine.p3DPoint = static_cast<SVzNL3DPosition*>(malloc(pointDataSize));
|
||
if (copiedLine.p3DPoint) {
|
||
memcpy(copiedLine.p3DPoint, laserLine->p3DPoint, pointDataSize);
|
||
}
|
||
|
||
// 添加数据到队列的锁作用域
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_queueMutex);
|
||
// 添加到队列
|
||
m_laserLineQueue.push_back(copiedLine);
|
||
m_lineCounter++;
|
||
|
||
// 管理队列大小
|
||
while (m_laserLineQueue.size() > static_cast<size_t>(m_maxQueueSize)) {
|
||
SVzLaserLineData oldLine = m_laserLineQueue.front();
|
||
m_laserLineQueue.pop_front();
|
||
|
||
// 释放深拷贝的点云数据内存
|
||
if (oldLine.p3DPoint) {
|
||
free(oldLine.p3DPoint);
|
||
oldLine.p3DPoint = nullptr;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 每到图像生成间隔，提交图像生成任务给工作线程
|
||
if (m_lineCounter % m_generationInterval == 0) {
|
||
m_algoDetectCondition.notify_one();
|
||
}
|
||
}
|
||
|
||
void BeltTearingPresenter::sendImageToClients(const QImage& image)
|
||
{
|
||
if (image.isNull() || m_clients.isEmpty() || !m_tcpServer) {
|
||
return;
|
||
}
|
||
|
||
try {
|
||
// 将图像转换为字节数组
|
||
QByteArray imageData;
|
||
QBuffer buffer(&imageData);
|
||
buffer.open(QIODevice::WriteOnly);
|
||
|
||
// 保存为JPEG格式以减少数据大小
|
||
if (!image.save(&buffer, "JPEG", 50)) {
|
||
LOG_ERROR("Failed to convert image to JPEG format\n");
|
||
return;
|
||
}
|
||
|
||
// 构造数据包
|
||
QByteArray packet;
|
||
QDataStream stream(&packet, QIODevice::WriteOnly);
|
||
stream.setByteOrder(QDataStream::BigEndian);
|
||
|
||
// 写入数据类型标识（图像类型）
|
||
stream << static_cast<quint8>(ByteDataType::Image);
|
||
|
||
// 写入图像数据大小
|
||
stream << static_cast<quint32>(imageData.size());
|
||
|
||
// 写入图像数据
|
||
stream.writeRawData(imageData.constData(), imageData.size());
|
||
packet.append("___END___\r\n");
|
||
|
||
|
||
// 发送给所有连接的客户端
|
||
bool success = m_tcpServer->SendAllData(packet.constData(), packet.size());
|
||
if (!success) {
|
||
LOG_WARNING("Failed to send image data to all clients\n");
|
||
}
|
||
// LOG_DEBUG("Sent image data to %d clients datalen %d\n", m_clients.size(), imageData.size());
|
||
|
||
} catch (const std::exception& e) {
|
||
LOG_ERROR("Error sending image to clients: %s\n", e.what());
|
||
}
|
||
}
|
||
|
||
SSG_beltTearingParam BeltTearingPresenter::configToSDKParam(const BeltTearingConfigResult& config) const
|
||
{
|
||
SSG_beltTearingParam sdkParam;
|
||
|
||
// 基本参数
|
||
sdkParam.scanXScale = config.algorithmParams.beltTearingParam.scanXScale;
|
||
sdkParam.scanYScale = config.algorithmParams.beltTearingParam.scanYScale;
|
||
sdkParam.differnceBinTh = config.algorithmParams.beltTearingParam.differnceBinTh;
|
||
sdkParam.tearingMinLen = config.algorithmParams.beltTearingParam.tearingMinLen;
|
||
sdkParam.tearingMinGap = config.algorithmParams.beltTearingParam.tearingMinGap;
|
||
|
||
// 特征提取参数
|
||
sdkParam.extractPara.sameGapTh = config.algorithmParams.beltTearingParam.sameGapTh;
|
||
sdkParam.extractPara.gapChkWin = config.algorithmParams.beltTearingParam.gapChkWin;
|
||
|
||
return sdkParam;
|
||
}
|
||
|
||
BeltTearingConfigResult BeltTearingPresenter::sdkToConfigParam(const SSG_beltTearingParam& sdkParam) const
|
||
{
|
||
BeltTearingConfigResult config;
|
||
|
||
// 基本参数
|
||
config.algorithmParams.beltTearingParam.scanXScale = sdkParam.scanXScale;
|
||
config.algorithmParams.beltTearingParam.scanYScale = sdkParam.scanYScale;
|
||
config.algorithmParams.beltTearingParam.differnceBinTh = sdkParam.differnceBinTh;
|
||
config.algorithmParams.beltTearingParam.tearingMinLen = sdkParam.tearingMinLen;
|
||
config.algorithmParams.beltTearingParam.tearingMinGap = sdkParam.tearingMinGap;
|
||
|
||
// 特征提取参数
|
||
config.algorithmParams.beltTearingParam.sameGapTh = sdkParam.extractPara.sameGapTh;
|
||
config.algorithmParams.beltTearingParam.gapChkWin = sdkParam.extractPara.gapChkWin;
|
||
|
||
return config;
|
||
}
|
||
|
||
void BeltTearingPresenter::handleAlgorithmParameterUpdate(const QByteArray& paramData)
|
||
{
|
||
try {
|
||
// 解析JSON数据
|
||
QJsonDocument doc = QJsonDocument::fromJson(paramData);
|
||
if (!doc.isObject()) {
|
||
LOG_WARNING("Invalid JSON format in parameter update\n");
|
||
return;
|
||
}
|
||
|
||
QJsonObject paramObj = doc.object();
|
||
QString command = paramObj["command"].toString();
|
||
|
||
if (command == "setAlgorithmParams") {
|
||
// 处理算法参数设置
|
||
handleSetAlgorithmParams(paramObj);
|
||
} else if (command == "getServerInfo") {
|
||
// 处理服务器信息获取请求
|
||
handleGetServerInfo(paramObj);
|
||
} else if (command == "resetDetect") {
|
||
// 处理重新检测请求
|
||
LOG_INFO("Received reset detect command from client\n");
|
||
ResetDetect();
|
||
|
||
// 发送响应给客户端
|
||
QJsonObject responseObj;
|
||
responseObj["command"] = "resetDetectResponse";
|
||
responseObj["status"] = "success";
|
||
responseObj["message"] = "Detection reset completed";
|
||
responseObj["timestamp"] = QDateTime::currentDateTime().toString(Qt::ISODate);
|
||
|
||
QJsonDocument responseDoc(responseObj);
|
||
QByteArray responseData = responseDoc.toJson();
|
||
|
||
// 构建数据包并发送给所有客户端
|
||
if (!m_clients.isEmpty() && m_tcpServer) {
|
||
QByteArray package;
|
||
QDataStream stream(&package, QIODevice::WriteOnly);
|
||
stream.setByteOrder(QDataStream::BigEndian);
|
||
stream << quint8(static_cast<quint8>(ByteDataType::Text)) << quint32(responseData.size());
|
||
stream.writeRawData(responseData.constData(), responseData.size());
|
||
package.append("___END___\r\n");
|
||
|
||
bool success = m_tcpServer->SendAllData(package.constData(), package.size());
|
||
if (success) {
|
||
LOG_INFO("Reset detect response sent to clients\n");
|
||
} else {
|
||
LOG_WARNING("Failed to send reset detect response to clients\n");
|
||
}
|
||
}
|
||
} else {
|
||
LOG_WARNING("Unknown command: %s\n", command.toStdString().c_str());
|
||
return;
|
||
}
|
||
|
||
} catch (const std::exception& e) {
|
||
LOG_ERROR("Error processing algorithm parameter update: %s\n", e.what());
|
||
}
|
||
}
|
||
|
||
void BeltTearingPresenter::handleSetAlgorithmParams(const QJsonObject& paramObj)
|
||
{
|
||
// 提取算法参数
|
||
double scanXScale = paramObj["scanXScale"].toDouble();
|
||
double scanYScale = paramObj["scanYScale"].toDouble();
|
||
double differnceBinTh = paramObj["differnceBinTh"].toDouble();
|
||
double tearingMinLen = paramObj["tearingMinLen"].toDouble();
|
||
double tearingMinGap = paramObj["tearingMinGap"].toDouble();
|
||
|
||
// 更新特征提取参数
|
||
m_algorithmParam.extractPara.sameGapTh = paramObj["sameGapTh"].toDouble();
|
||
m_algorithmParam.extractPara.gapChkWin = paramObj["gapChkWin"].toInt();
|
||
|
||
// 更新SDK算法参数
|
||
m_algorithmParam.scanXScale = scanXScale;
|
||
m_algorithmParam.scanYScale = scanYScale;
|
||
m_algorithmParam.differnceBinTh = differnceBinTh;
|
||
m_algorithmParam.tearingMinLen = tearingMinLen;
|
||
m_algorithmParam.tearingMinGap = tearingMinGap;
|
||
|
||
LOG_INFO("Algorithm parameters updated: scanXScale=%f, scanYScale=%f, differnceBinTh=%f, tearingMinLen=%f, tearingMinGap=%f\n",
|
||
scanXScale, scanYScale, differnceBinTh, tearingMinLen, tearingMinGap);
|
||
|
||
// 处理队列处理参数
|
||
if (paramObj.contains("maxQueueSize")) {
|
||
int newMaxQueueSize = paramObj["maxQueueSize"].toInt();
|
||
if (newMaxQueueSize > 0) {
|
||
m_maxQueueSize = newMaxQueueSize;
|
||
LOG_INFO("Max queue size updated to: %d\n", m_maxQueueSize);
|
||
}
|
||
}
|
||
|
||
if (paramObj.contains("generationInterval")) {
|
||
int newGenerationInterval = paramObj["generationInterval"].toInt();
|
||
if (newGenerationInterval > 0) {
|
||
m_generationInterval = newGenerationInterval;
|
||
LOG_INFO("Generation interval updated to: %d\n", m_generationInterval);
|
||
}
|
||
}
|
||
|
||
// 处理相机IP配置
|
||
if (paramObj.contains("cameraIP")) {
|
||
QString newCameraIP = paramObj["cameraIP"].toString();
|
||
|
||
// 更新配置中的相机IP
|
||
if (!m_configResult.cameras.empty()) {
|
||
std::string oldIP = m_configResult.cameras[0].cameraIP;
|
||
m_configResult.cameras[0].cameraIP = newCameraIP.toStdString();
|
||
|
||
if (oldIP != m_configResult.cameras[0].cameraIP) {
|
||
LOG_INFO("Camera IP updated from [%s] to [%s]\n", oldIP.c_str(), m_configResult.cameras[0].cameraIP.c_str());
|
||
LOG_INFO("Camera IP change requires restart to take effect\n");
|
||
}
|
||
} else {
|
||
// 如果相机列表为空，创建新的相机配置
|
||
CameraParam newCamera;
|
||
newCamera.name = "摄像头1";
|
||
newCamera.cameraIP = newCameraIP.toStdString();
|
||
m_configResult.cameras.push_back(newCamera);
|
||
LOG_INFO("Camera IP configured: [%s]\n", newCameraIP.toStdString().c_str());
|
||
}
|
||
}
|
||
|
||
// 保存参数到配置文件
|
||
if (m_config) {
|
||
BeltTearingConfigResult currentConfig = sdkToConfigParam(m_algorithmParam);
|
||
// 保留相机配置
|
||
currentConfig.cameras = m_configResult.cameras;
|
||
currentConfig.servers = m_configResult.servers;
|
||
currentConfig.debugParam = m_configResult.debugParam;
|
||
currentConfig.projectType = m_configResult.projectType;
|
||
currentConfig.serverPort = m_configResult.serverPort;
|
||
// 保存队列处理参数
|
||
currentConfig.queueProcessParam.maxQueueSize = m_maxQueueSize;
|
||
currentConfig.queueProcessParam.generationInterval = m_generationInterval;
|
||
|
||
QString configPath = PathManager::GetConfigFilePath();
|
||
|
||
if (m_config->SaveConfig(configPath.toStdString(), currentConfig)) {
|
||
LOG_INFO("Algorithm parameters saved to config file: %s\n", configPath.toStdString().c_str());
|
||
// 更新内存中的配置
|
||
m_configResult = currentConfig;
|
||
} else {
|
||
LOG_ERROR("Failed to save algorithm parameters to config file\n");
|
||
}
|
||
}
|
||
}
|
||
|
||
void BeltTearingPresenter::handleGetServerInfo(const QJsonObject& requestObj)
|
||
{
|
||
// 创建服务器信息响应
|
||
QJsonObject responseObj;
|
||
responseObj["command"] = "serverInfoResponse";
|
||
responseObj["requestId"] = requestObj["requestId"].toString();
|
||
|
||
// 服务器基本信息
|
||
QJsonObject serverInfo;
|
||
serverInfo["name"] = BELT_TEARING_SERVER_PRODUCT_NAME;
|
||
serverInfo["version"] = BELT_TEARING_SERVER_VERSION_STRING;
|
||
serverInfo["buildInfo"] = BELT_TEARING_SERVER_PLATFORM;
|
||
serverInfo["status"] = "running";
|
||
serverInfo["port"] = static_cast<int>(getServerPort());
|
||
|
||
// 当前算法参数
|
||
QJsonObject algorithmParams;
|
||
algorithmParams["scanXScale"] = m_algorithmParam.scanXScale;
|
||
algorithmParams["scanYScale"] = m_algorithmParam.scanYScale;
|
||
algorithmParams["differnceBinTh"] = m_algorithmParam.differnceBinTh;
|
||
algorithmParams["tearingMinLen"] = m_algorithmParam.tearingMinLen;
|
||
algorithmParams["tearingMinGap"] = m_algorithmParam.tearingMinGap;
|
||
|
||
responseObj["serverInfo"] = serverInfo;
|
||
responseObj["algorithmParams"] = algorithmParams;
|
||
responseObj["timestamp"] = QDateTime::currentDateTime().toString(Qt::ISODate);
|
||
|
||
// 将响应发送给所有客户端
|
||
QJsonDocument responseDoc(responseObj);
|
||
QByteArray responseData = responseDoc.toJson();
|
||
|
||
// 使用现有的sendTearingResults传输机制发送响应
|
||
if (!m_clients.isEmpty() && m_tcpServer) {
|
||
// 创建数据包
|
||
QByteArray package;
|
||
QDataStream stream(&package, QIODevice::WriteOnly);
|
||
stream.setByteOrder(QDataStream::BigEndian);
|
||
stream << quint8(static_cast<quint8>(ByteDataType::Text)) << quint32(responseData.size());
|
||
stream.writeRawData(responseData.constData(), responseData.size());
|
||
package.append("___END___\r\n");
|
||
|
||
// 发送到所有连接的客户端
|
||
bool success = m_tcpServer->SendAllData(package.constData(), package.size());
|
||
if (success) {
|
||
LOG_INFO("Server info response sent to %d clients\n", m_clients.size());
|
||
} else {
|
||
LOG_WARNING("Failed to send server info response to clients\n");
|
||
}
|
||
}
|
||
}
|
||
|
||
void BeltTearingPresenter::handleReadConfig(const TCPClient* pClient)
|
||
{
|
||
try {
|
||
// 直接使用当前内存中的配置参数
|
||
BeltTearingConfigResult configResult = sdkToConfigParam(m_algorithmParam);
|
||
|
||
// 将配置转换为JSON格式
|
||
QJsonObject configObj;
|
||
|
||
// 服务器配置
|
||
QJsonArray serversArray;
|
||
for (const auto& server : m_configResult.servers) {
|
||
QJsonObject serverObj;
|
||
serverObj["name"] = QString::fromStdString(server.name);
|
||
serverObj["ip"] = QString::fromStdString(server.ip);
|
||
serverObj["port"] = server.port;
|
||
serversArray.append(serverObj);
|
||
}
|
||
configObj["servers"] = serversArray;
|
||
|
||
// 算法参数
|
||
QJsonObject algorithmParams;
|
||
algorithmParams["scanXScale"] = configResult.algorithmParams.beltTearingParam.scanXScale;
|
||
algorithmParams["scanYScale"] = configResult.algorithmParams.beltTearingParam.scanYScale;
|
||
algorithmParams["differnceBinTh"] = configResult.algorithmParams.beltTearingParam.differnceBinTh;
|
||
algorithmParams["tearingMinLen"] = configResult.algorithmParams.beltTearingParam.tearingMinLen;
|
||
algorithmParams["tearingMinGap"] = configResult.algorithmParams.beltTearingParam.tearingMinGap;
|
||
algorithmParams["sameGapTh"] = configResult.algorithmParams.beltTearingParam.sameGapTh;
|
||
algorithmParams["gapChkWin"] = configResult.algorithmParams.beltTearingParam.gapChkWin;
|
||
configObj["algorithmParams"] = algorithmParams;
|
||
|
||
// 相机配置
|
||
QJsonObject cameraConfig;
|
||
if (!m_configResult.cameras.empty()) {
|
||
cameraConfig["cameraIP"] = QString::fromStdString(m_configResult.cameras[0].cameraIP);
|
||
cameraConfig["cameraName"] = QString::fromStdString(m_configResult.cameras[0].name);
|
||
}
|
||
configObj["cameraConfig"] = cameraConfig;
|
||
|
||
// 队列处理参数
|
||
QJsonObject queueConfig;
|
||
queueConfig["maxQueueSize"] = m_maxQueueSize;
|
||
queueConfig["generationInterval"] = m_generationInterval;
|
||
configObj["queueProcessParam"] = queueConfig;
|
||
|
||
// 调试参数
|
||
QJsonObject debugParams;
|
||
debugParams["enableDebug"] = m_configResult.debugParam.enableDebug;
|
||
debugParams["saveDebugImage"] = m_configResult.debugParam.saveDebugImage;
|
||
debugParams["printDetailLog"] = m_configResult.debugParam.printDetailLog;
|
||
debugParams["debugOutputPath"] = QString::fromStdString(m_configResult.debugParam.debugOutputPath);
|
||
configObj["debugParams"] = debugParams;
|
||
|
||
// 项目类型
|
||
configObj["projectType"] = static_cast<int>(m_configResult.projectType);
|
||
|
||
// 服务端端口
|
||
configObj["serverPort"] = m_configResult.serverPort;
|
||
|
||
QString versionText = QString("%1_%2_%3")
|
||
.arg(BELT_TEARING_SERVER_VERSION_STRING)
|
||
.arg(BELT_TEARING_SERVER_VERSION_BUILD)
|
||
.arg(BUILD_TIME.c_str());
|
||
|
||
// 创建响应对象
|
||
QJsonObject responseObj;
|
||
responseObj["command"] = "configResponse";
|
||
responseObj["version"] = versionText; // 添加版本信息
|
||
responseObj["config"] = configObj;
|
||
|
||
// 转换为JSON数据
|
||
QJsonDocument responseDoc(responseObj);
|
||
QByteArray responseData = responseDoc.toJson();
|
||
|
||
// 构建数据包
|
||
QByteArray packet;
|
||
QDataStream stream(&packet, QIODevice::WriteOnly);
|
||
stream.setByteOrder(QDataStream::BigEndian);
|
||
stream << static_cast<quint8>(ByteDataType::ReadConfig) << static_cast<quint32>(responseData.size());
|
||
stream.writeRawData(responseData.constData(), responseData.size());
|
||
packet.append("___END___\r\n");
|
||
|
||
// 发送响应
|
||
if (m_tcpServer && pClient) {
|
||
bool success = m_tcpServer->SendData(pClient, packet.constData(), packet.size());
|
||
if (success) {
|
||
LOG_INFO("Configuration sent to client successfully\n");
|
||
} else {
|
||
LOG_ERROR("Failed to send configuration to client\n");
|
||
}
|
||
}
|
||
} catch (const std::exception& e) {
|
||
LOG_ERROR("Error handling ReadConfig: %s\n", e.what());
|
||
}
|
||
}
|
||
|
||
void BeltTearingPresenter::handleWriteConfig(const QByteArray& paramData)
|
||
{
|
||
try {
|
||
// 解析JSON数据
|
||
QJsonDocument doc = QJsonDocument::fromJson(paramData);
|
||
if (!doc.isObject()) {
|
||
LOG_WARNING("Invalid JSON format in WriteConfig\n");
|
||
return;
|
||
}
|
||
|
||
QJsonObject paramObj = doc.object();
|
||
|
||
// 检查是否是来自 dialogalgoarg 的简单参数格式
|
||
if (paramObj.contains("command") && paramObj["command"].toString() == "setAlgorithmParams") {
|
||
// 处理来自 dialogalgoarg 的算法参数更新
|
||
handleSetAlgorithmParams(paramObj);
|
||
}
|
||
|
||
|
||
} catch (const std::exception& e) {
|
||
LOG_ERROR("Error handling WriteConfig: %s\n", e.what());
|
||
}
|
||
}
|
||
|
||
void BeltTearingPresenter::ResetDetect()
|
||
{
|
||
LOG_INFO("Resetting detection system\n");
|
||
|
||
int result = stopCamera();
|
||
if (result != 0) {
|
||
LOG_WARNING("Failed to stop camera during reset, error code: %d\n", result);
|
||
}
|
||
|
||
m_hLineWorkers.clear();
|
||
m_beltTearings_new.clear();
|
||
m_beltTearings_growing.clear();
|
||
m_beltTearings_ended.clear();
|
||
m_beltTearings_unknown.clear();
|
||
|
||
m_bInitAlgo = false;
|
||
|
||
std::lock_guard<std::mutex> lock(m_queueMutex);
|
||
for (auto& line : m_laserLineQueue) {
|
||
if (line.p3DPoint) {
|
||
free(line.p3DPoint);
|
||
line.p3DPoint = nullptr;
|
||
}
|
||
}
|
||
m_laserLineQueue.clear();
|
||
|
||
// 清空Modbus检测结果数据
|
||
if (m_pRobotProtocol) {
|
||
int ret = m_pRobotProtocol->ClearDetectionData();
|
||
if (ret != 0) {
|
||
LOG_WARNING("Failed to clear Modbus detection data during reset, error code: %d\n", ret);
|
||
} else {
|
||
LOG_INFO("Modbus detection data cleared during reset\n");
|
||
}
|
||
}
|
||
|
||
// 清空简化协议的报警数据
|
||
if (m_pRobotProtocolSimplified) {
|
||
int ret = m_pRobotProtocolSimplified->ClearAlarmData();
|
||
if (ret != 0) {
|
||
LOG_WARNING("Failed to clear simplified protocol alarm data during reset, error code: %d\n", ret);
|
||
} else {
|
||
LOG_INFO("Simplified protocol alarm data cleared during reset\n");
|
||
}
|
||
}
|
||
|
||
result = startCamera();
|
||
if (result != 0) {
|
||
LOG_WARNING("Failed to start camera after reset, error code: %d\n", result);
|
||
}
|
||
|
||
LOG_INFO("Detection system reset completed\n");
|
||
}
|
||
|
||
int BeltTearingPresenter::StopWork()
|
||
{
|
||
LOG_INFO("Stopping work - camera and detection\n");
|
||
|
||
// 停止相机
|
||
int cameraResult = stopCamera();
|
||
if (cameraResult != SUCCESS) {
|
||
LOG_ERROR("Failed to stop camera, error code: %d\n", cameraResult);
|
||
// 继续执行其他停止操作，但返回相机错误码
|
||
}
|
||
|
||
// 设置机械臂工作状态为空闲
|
||
if (m_pRobotProtocol) {
|
||
m_pRobotProtocol->SetWorkStatus(RobotProtocol::WORK_STATUS_IDLE);
|
||
}
|
||
|
||
LOG_INFO("Work stopped successfully\n");
|
||
return cameraResult; // 返回相机操作结果，成功时为SUCCESS
|
||
}
|
||
|
||
int BeltTearingPresenter::StartWork()
|
||
{
|
||
LOG_INFO("Starting work - camera and detection\n");
|
||
|
||
// 启动相机
|
||
int cameraResult = startCamera();
|
||
if (cameraResult != SUCCESS) {
|
||
LOG_ERROR("Failed to start camera, error code: %d\n", cameraResult);
|
||
return cameraResult; // 返回相机错误码
|
||
}
|
||
|
||
// 设置机械臂工作状态为工作中（在startCamera中已经设置）
|
||
|
||
LOG_INFO("Work started successfully\n");
|
||
return SUCCESS; // 成功
|
||
}
|
||
|
||
void BeltTearingPresenter::_AlgoDetectThread()
|
||
{
|
||
while (m_bAlgoDetectThreadRunning) {
|
||
// 等待条件变量通知
|
||
std::unique_lock<std::mutex> lock(m_algoDetectMutex);
|
||
m_algoDetectCondition.wait(lock);
|
||
|
||
// 如果线程停止运行，退出循环
|
||
if (!m_bAlgoDetectThreadRunning) {
|
||
break;
|
||
}
|
||
|
||
// 执行检测任务
|
||
_DetectTask();
|
||
}
|
||
}
|
||
|
||
int BeltTearingPresenter::_DetectTask()
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_detectionDataMutex);
|
||
|
||
// 临时存储从队列中取出的数据，避免重复处理
|
||
std::vector<SVzNL3DLaserLine> algorithmDataCache;
|
||
std::vector<std::vector<SVzNL3DPosition>> imageScanLines;
|
||
|
||
// 从队列中获取数据
|
||
{
|
||
std::lock_guard<std::mutex> queueLock(m_queueMutex);
|
||
if (m_laserLineQueue.empty()) {
|
||
return SUCCESS; // 没有数据可处理
|
||
}
|
||
|
||
// 只保留最后的m_generationInterval大小的数据进行检测
|
||
algorithmDataCache.reserve(m_generationInterval);
|
||
int cacheSize = static_cast<int>(m_laserLineQueue.size()) - m_generationInterval;
|
||
int startIndex = 0 > cacheSize ? 0 : cacheSize;
|
||
for (auto it = m_laserLineQueue.begin() + startIndex; it != m_laserLineQueue.end(); ++it) {
|
||
const auto& laserLine = *it;
|
||
SVzNL3DLaserLine algorithmData;
|
||
algorithmData.nTimeStamp = laserLine.llTimeStamp;
|
||
algorithmData.nPositionCnt = laserLine.nPointCount;
|
||
|
||
// 深拷贝点云数据
|
||
if (laserLine.nPointCount > 0 && laserLine.p3DPoint) {
|
||
size_t pointDataSize = laserLine.nPointCount * sizeof(SVzNL3DPosition);
|
||
algorithmData.p3DPosition = static_cast<SVzNL3DPosition*>(malloc(pointDataSize));
|
||
if (algorithmData.p3DPosition) {
|
||
memcpy(algorithmData.p3DPosition, laserLine.p3DPoint, pointDataSize);
|
||
}
|
||
} else {
|
||
algorithmData.p3DPosition = nullptr;
|
||
}
|
||
|
||
algorithmDataCache.push_back(algorithmData);
|
||
}
|
||
|
||
// 准备扫描线数据用于图像生成
|
||
imageScanLines.reserve(m_laserLineQueue.size());
|
||
for (const auto& line : m_laserLineQueue) {
|
||
// 增加安全检查，确保指针有效
|
||
if (line.p3DPoint && line.nPointCount > 0) {
|
||
std::vector<SVzNL3DPosition> linePoints;
|
||
linePoints.reserve(line.nPointCount); // 预分配内存提高效率
|
||
|
||
SVzNL3DPosition* p3DPoints = static_cast<SVzNL3DPosition*>(line.p3DPoint);
|
||
|
||
// 使用更高效的批量复制
|
||
linePoints.assign(p3DPoints, p3DPoints + line.nPointCount);
|
||
imageScanLines.emplace_back(std::move(linePoints)); // 使用move避免拷贝
|
||
}
|
||
}
|
||
}
|
||
|
||
// 调用SDK算法进行皮带撕裂检测
|
||
int errorCode = 0;
|
||
std::vector<SSG_beltTearingInfo> allTearings;
|
||
|
||
// 初始化算法（如果尚未初始化）
|
||
if (!m_bInitAlgo) {
|
||
const SVzNL3DLaserLine& firstLine = algorithmDataCache.front();
|
||
m_hLineWorkers.resize(firstLine.nPositionCnt);
|
||
|
||
m_beltTearings_new.clear();
|
||
m_beltTearings_new.shrink_to_fit();
|
||
m_beltTearings_growing.clear();
|
||
m_beltTearings_growing.shrink_to_fit();
|
||
m_beltTearings_ended.clear();
|
||
m_beltTearings_ended.shrink_to_fit();
|
||
m_beltTearings_unknown.clear();
|
||
m_beltTearings_unknown.shrink_to_fit();
|
||
|
||
// 复位内部静态变量
|
||
sg_detectBeltTearing(nullptr, 0, 0, &errorCode,
|
||
m_hLineWorkers,
|
||
m_beltTearings_new, m_beltTearings_growing, m_beltTearings_ended, m_beltTearings_unknown,
|
||
m_algorithmParam
|
||
);
|
||
m_bInitAlgo = true;
|
||
}
|
||
|
||
// 对缓存中的每一帧数据进行检测
|
||
for (auto& algorithmData : algorithmDataCache) {
|
||
// 直接调用皮带撕裂检测算法
|
||
sg_detectBeltTearing(&algorithmData, static_cast<int>(algorithmData.nTimeStamp),
|
||
algorithmData.nPositionCnt, &errorCode,
|
||
m_hLineWorkers,
|
||
m_beltTearings_new, m_beltTearings_growing, m_beltTearings_ended, m_beltTearings_unknown,
|
||
m_algorithmParam
|
||
);
|
||
|
||
// 合并各个撕裂结果容器
|
||
#if 1
|
||
_MergeAndReplace(allTearings, m_beltTearings_new);
|
||
_MergeAndReplace(allTearings, m_beltTearings_growing);
|
||
_MergeAndReplace(allTearings, m_beltTearings_ended);
|
||
// _MergeAndReplace(allTearings, m_beltTearings_unknown);
|
||
#else
|
||
allTearings.insert(allTearings.end(), m_beltTearings_new.begin(), m_beltTearings_new.end());
|
||
allTearings.insert(allTearings.end(), m_beltTearings_growing.begin(), m_beltTearings_growing.end());
|
||
allTearings.insert(allTearings.end(), m_beltTearings_ended.begin(), m_beltTearings_ended.end());
|
||
// allTearings.insert(allTearings.end(), m_beltTearings_unknown.begin(), m_beltTearings_unknown.end());
|
||
#endif
|
||
}
|
||
|
||
// 发送检测结果
|
||
if (!allTearings.empty()) {
|
||
sendTearingResults(allTearings);
|
||
SendDetectionResultToRobot(allTearings); // 发送检测结果到机械臂
|
||
}
|
||
|
||
// 释放深拷贝的点云数据
|
||
for (auto& algorithmData : algorithmDataCache) {
|
||
if (algorithmData.p3DPosition) {
|
||
free(algorithmData.p3DPosition);
|
||
algorithmData.p3DPosition = nullptr;
|
||
}
|
||
}
|
||
|
||
// 在算法处理完成后，生成带撕裂检测结果的图像
|
||
QImage image = PointCloudImageUtils::GeneratePointCloudImage(imageScanLines, allTearings, 800, 600);
|
||
sendImageToClients(image);
|
||
|
||
// 使用TearingTcpProtocol发送DETECT_RESULT消息（新协议）
|
||
if (m_tearingProtocol && !allTearings.empty()) {
|
||
m_tearingProtocol->sendDetectResult(allTearings, image);
|
||
}
|
||
|
||
return SUCCESS; // 成功完成检测任务
|
||
}
|
||
|
||
void BeltTearingPresenter::_MergeAndReplace(std::vector<SSG_beltTearingInfo>& allTearings, const std::vector<SSG_beltTearingInfo>& source)
|
||
{
|
||
for (const auto& newItem : source)
|
||
{
|
||
bool found = false;
|
||
// 检查是否已存在相同ID的项
|
||
for (auto& existingItem : allTearings) {
|
||
if (existingItem.tearID == newItem.tearID) {
|
||
// 如果存在，用新数据替换旧数据
|
||
existingItem = newItem;
|
||
found = true;
|
||
break;
|
||
}
|
||
}
|
||
// 如果不存在相同ID的项，则添加新项
|
||
if (!found) {
|
||
allTearings.push_back(newItem);
|
||
}
|
||
}
|
||
}
|
||
|
||
// 初始化Modbus-RTU主端
|
||
int BeltTearingPresenter::InitModbusRTUMaster()
|
||
{
|
||
// 创建ModbusRTUMaster实例
|
||
m_pModbusRTUMaster = new ModbusRTUMaster();
|
||
|
||
// 从配置中获取串口参数
|
||
const SerialPortParam& serialParam = m_configResult.serialPortParam;
|
||
|
||
// 如果配置中的串口名为空，使用平台默认值
|
||
std::string portName = serialParam.portName;
|
||
|
||
// 初始化Modbus-RTU主端，使用配置的参数
|
||
int nRet = m_pModbusRTUMaster->Initialize(portName.c_str(), serialParam.baudRate, serialParam.parity,
|
||
serialParam.dataBits, serialParam.stopBits);
|
||
|
||
if (nRet != 0) {
|
||
LOG_ERROR("Failed to initialize Modbus RTU master, error code: %d\n", nRet);
|
||
return nRet;
|
||
}
|
||
|
||
// 设置温度回调函数
|
||
m_pModbusRTUMaster->SetTemperatureCallback([this](float temperature) {
|
||
this->SendTemperatureData(temperature);
|
||
});
|
||
|
||
// 启动定时器读取温度数据
|
||
m_pModbusRTUMaster->StartReading();
|
||
|
||
m_bModbusRTUConnected = true;
|
||
LOG_INFO("Modbus RTU master initialized successfully\n");
|
||
return SUCCESS;
|
||
}
|
||
|
||
|
||
// 发送温度数据给所有客户端
|
||
void BeltTearingPresenter::SendTemperatureData(float temperature)
|
||
{
|
||
// 检查TCP服务器是否已初始化
|
||
if (!m_tcpServer) {
|
||
LOG_WARNING("TCP server not initialized, cannot send temperature data\n");
|
||
return;
|
||
}
|
||
|
||
// 将温度数据转换为JSON格式
|
||
QJsonObject temperatureObj;
|
||
temperatureObj["id"] = QString::number(0); // 温度目标ID为0
|
||
temperatureObj["status"] = QString::number(1); // 状态1表示有效数据
|
||
temperatureObj["value"] = QString::number(temperature); // 宽度字段存储温度值
|
||
temperatureObj["type"] = QString("temperature"); // 数据类型标识
|
||
|
||
QJsonArray temperatureArray;
|
||
temperatureArray.append(temperatureObj);
|
||
|
||
// 转换为JSON字符串
|
||
QJsonDocument doc(temperatureArray);
|
||
QByteArray message = doc.toJson(QJsonDocument::Compact);
|
||
|
||
// 创建数据包
|
||
QByteArray package;
|
||
QDataStream stream(&package, QIODevice::WriteOnly);
|
||
stream.setByteOrder(QDataStream::BigEndian);
|
||
stream << quint8(static_cast<quint8>(ByteDataType::Text)) << quint32(message.size());
|
||
stream.writeRawData(message.constData(), message.size());
|
||
package.append("___END___\r\n");
|
||
|
||
// 通过TCP服务器发送到所有连接的客户端
|
||
bool success = m_tcpServer->SendAllData(package.constData(), package.size());
|
||
if (!success) {
|
||
LOG_WARNING("Failed to send temperature data to all clients\n");
|
||
} else {
|
||
LOG_DEBUG("Temperature data sent successfully: %.2f°C\n", temperature);
|
||
}
|
||
}
|
||
|
||
|
||
// 初始化机械臂协议
|
||
int BeltTearingPresenter::InitRobotProtocol()
|
||
{
|
||
LOG_DEBUG("Start initializing robot protocol\n");
|
||
|
||
// 根据配置选择协议类型
|
||
m_modbusTCPProtocolType = m_configResult.modbusTCPProtocol;
|
||
|
||
int nRet = 0;
|
||
|
||
if (m_modbusTCPProtocolType == ModbusTCPProtocolType::Simplified) {
|
||
// 使用简化协议
|
||
LOG_INFO("Using simplified ModbusTCP protocol\n");
|
||
|
||
// 创建RobotProtocolSimplified实例
|
||
if (nullptr == m_pRobotProtocolSimplified) {
|
||
m_pRobotProtocolSimplified = new RobotProtocolSimplified();
|
||
}
|
||
|
||
// 初始化协议服务（使用端口502）
|
||
nRet = m_pRobotProtocolSimplified->Initialize(502);
|
||
|
||
// 设置连接状态回调
|
||
m_pRobotProtocolSimplified->SetConnectionCallback([this](bool connected) {
|
||
this->OnRobotConnectionChanged(connected);
|
||
});
|
||
|
||
// 设置复位回调
|
||
m_pRobotProtocolSimplified->SetResetCallback([this]() {
|
||
LOG_INFO("ModbusTCP simplified protocol: Reset command received\n");
|
||
this->ResetDetect();
|
||
});
|
||
|
||
} else {
|
||
// 使用标准协议
|
||
LOG_INFO("Using standard ModbusTCP protocol\n");
|
||
|
||
// 创建RobotProtocol实例
|
||
if (nullptr == m_pRobotProtocol) {
|
||
m_pRobotProtocol = new RobotProtocol();
|
||
}
|
||
|
||
// 初始化协议服务（使用端口502）
|
||
nRet = m_pRobotProtocol->Initialize(502);
|
||
|
||
// 设置连接状态回调
|
||
m_pRobotProtocol->SetConnectionCallback([this](bool connected) {
|
||
this->OnRobotConnectionChanged(connected);
|
||
});
|
||
|
||
// 设置工作信号回调
|
||
m_pRobotProtocol->SetWorkSignalCallback([this](bool startWork, int cameraIndex) {
|
||
return this->OnRobotWorkSignal(startWork, cameraIndex);
|
||
});
|
||
|
||
// 设置系统控制回调
|
||
m_pRobotProtocol->SetSystemControlCallback([this](uint16_t command) {
|
||
switch(command) {
|
||
case 0: // 停止工作
|
||
LOG_INFO("ModbusTCP command: Stop work\n");
|
||
this->StopWork();
|
||
break;
|
||
case 1: // 开始工作
|
||
LOG_INFO("ModbusTCP command: Start work\n");
|
||
this->StartWork();
|
||
break;
|
||
case 2: // 复位检测
|
||
LOG_INFO("ModbusTCP command: Reset detect\n");
|
||
this->ResetDetect();
|
||
break;
|
||
default:
|
||
LOG_WARNING("Unknown ModbusTCP command: %d\n", command);
|
||
break;
|
||
}
|
||
});
|
||
}
|
||
|
||
LOG_INFO("Robot protocol initialization completed successfully (type: %s)\n",
|
||
m_modbusTCPProtocolType == ModbusTCPProtocolType::Simplified ? "Simplified" : "Standard");
|
||
return nRet;
|
||
}
|
||
|
||
// 机械臂连接状态改变回调
|
||
void BeltTearingPresenter::OnRobotConnectionChanged(bool connected)
|
||
{
|
||
LOG_INFO("Robot connection status changed: %s\n", connected ? "Connected" : "Disconnected");
|
||
m_bRobotConnected = connected;
|
||
|
||
// 可以在这里添加其他连接状态改变时需要处理的逻辑
|
||
if (connected) {
|
||
LOG_INFO("Robot connected successfully\n");
|
||
} else {
|
||
LOG_WARNING("Robot disconnected\n");
|
||
}
|
||
}
|
||
|
||
|
||
// 机械臂工作信号回调
|
||
bool BeltTearingPresenter::OnRobotWorkSignal(bool startWork, int cameraIndex)
|
||
{
|
||
return true; // 返回处理结果
|
||
}
|
||
|
||
// 发送检测结果到机械臂
|
||
/**
|
||
* @brief 发送检测结果到机械臂
|
||
* @param detectionResults 检测到的所有撕裂信息
|
||
*
|
||
* 功能说明：
|
||
* 1. 根据协议类型发送数据（简化协议或标准协议）
|
||
* 2. 在简化协议中，使用ROI的宽度和高度中的较大值找出最大的撕裂区域
|
||
* 3. 使用对角线长度作为撕裂大小的更准确表示
|
||
*/
|
||
void BeltTearingPresenter::SendDetectionResultToRobot(const std::vector<SSG_beltTearingInfo>& detectionResults)
|
||
{
|
||
// 根据协议类型发送数据
|
||
if (m_modbusTCPProtocolType == ModbusTCPProtocolType::Simplified) {
|
||
// 简化协议：只发送最大撕裂信息
|
||
if (!m_pRobotProtocolSimplified) {
|
||
LOG_WARNING("Simplified robot protocol not initialized, cannot send detection results\n");
|
||
return;
|
||
}
|
||
|
||
RobotProtocolSimplified::TearingAlarmData alarmData;
|
||
|
||
if (!detectionResults.empty()) {
|
||
// 找出最大撕裂（使用宽度和高度中的较大值）
|
||
auto maxTearIt = std::max_element(detectionResults.begin(), detectionResults.end(),
|
||
[](const SSG_beltTearingInfo& a, const SSG_beltTearingInfo& b) {
|
||
// 计算撕裂区域的宽度和高度，取较大值
|
||
double widthA = a.roi.right - a.roi.left;
|
||
double lengthA = a.roi.bottom - a.roi.top;
|
||
double maxSizeA = widthA > lengthA ? widthA : lengthA;
|
||
|
||
double widthB = b.roi.right - b.roi.left;
|
||
double lengthB = b.roi.bottom - b.roi.top;
|
||
double maxSizeB = widthB > lengthB ? widthB : lengthB;
|
||
|
||
// 返回true表示a应该排在b之前，即a的最大边长比b小
|
||
return maxSizeA < maxSizeB;
|
||
});
|
||
|
||
// 设置报警标志
|
||
alarmData.alarmFlag = 1; // 有撕裂报警
|
||
|
||
// 计算撕裂区域的宽度和长度
|
||
double dWidth = maxTearIt->roi.right - maxTearIt->roi.left;
|
||
double dLength = maxTearIt->roi.bottom - maxTearIt->roi.top;
|
||
// 计算撕裂区域的对角线长度，作为撕裂大小的更准确表示
|
||
double diagonalLength = std::sqrt(dWidth * dWidth + dLength * dLength);
|
||
alarmData.maxLength = static_cast<uint16_t>(diagonalLength); // 最大长度（毫米）
|
||
alarmData.maxWidth = static_cast<uint16_t>(maxTearIt->tearWidth); // 最大宽度（毫米）
|
||
alarmData.maxId = maxTearIt->tearID; // 最大撕裂ID
|
||
} else {
|
||
// 没有撕裂，清空报警
|
||
alarmData.alarmFlag = 0;
|
||
alarmData.maxLength = 0;
|
||
alarmData.maxWidth = 0;
|
||
alarmData.maxId = 0;
|
||
|
||
LOG_DEBUG("No tearing detected, clearing alarm\n");
|
||
}
|
||
|
||
// 发送报警数据
|
||
m_pRobotProtocolSimplified->SetAlarmData(alarmData);
|
||
|
||
} else {
|
||
// 标准协议：发送完整的撕裂信息列表
|
||
if (!m_pRobotProtocol || !m_bRobotConnected) {
|
||
LOG_WARNING("Robot protocol not initialized or not connected, cannot send detection results\n");
|
||
return;
|
||
}
|
||
|
||
// 准备发送给机械臂的数据结构
|
||
MultiTargetData multiTargetData;
|
||
|
||
// 将皮带撕裂检测结果转换为机械臂可识别的数据
|
||
for (const auto& result : detectionResults) {
|
||
// 创建目标结果
|
||
TargetResult target;
|
||
target.id = result.tearID;
|
||
target.status = static_cast<uint16_t>(result.tearStatus);
|
||
target.width = static_cast<float>(result.tearWidth);
|
||
target.depth = static_cast<float>(result.tearDepth);
|
||
|
||
// 添加到目标列表
|
||
multiTargetData.targets.push_back(target);
|
||
}
|
||
|
||
// 调用RobotProtocol接口发送数据
|
||
m_pRobotProtocol->SetMultiTargetData(multiTargetData);
|
||
}
|
||
}
|
||
|