1353 lines
48 KiB
C++
1353 lines
48 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 "VrLog.h"
|
||
#include "VrSimpleLog.h"
|
||
#include "VrTimeUtils.h"
|
||
|
||
// 静态实例指针
|
||
BeltTearingPresenter* BeltTearingPresenter::s_instance = nullptr;
|
||
|
||
BeltTearingPresenter::BeltTearingPresenter(QObject *parent)
|
||
: QObject(parent)
|
||
, m_tcpServer(nullptr)
|
||
, m_config(nullptr)
|
||
, m_eyeDevice(nullptr)
|
||
, m_cameraInitTimer(new QTimer(this))
|
||
, m_cameraInitialized(false)
|
||
, m_cameraDetecting(false)
|
||
, m_lineCounter(0)
|
||
, m_bAlgoDetectThreadRunning(false)
|
||
, m_pRobotProtocol(nullptr) // 初始化RobotProtocol指针
|
||
, 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;
|
||
}
|
||
}
|
||
|
||
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();
|
||
|
||
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;
|
||
}
|
||
}
|
||
|
||
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());
|
||
|
||
// 应用算法参数
|
||
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");
|
||
|
||
// 更新配置结果
|
||
m_configResult = configResult;
|
||
|
||
// 重新应用算法参数
|
||
applyAlgorithmParameters(configResult.algorithmParams);
|
||
|
||
// 如果服务器端口改变，可能需要重启服务器
|
||
LOG_INFO("Server port changed, restarting server...\n");
|
||
stopServer();
|
||
startServer(configResult.serverPort);
|
||
}
|
||
|
||
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
|
||
// 其他字段由于结构定义不明确，暂时不填充
|
||
|
||
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
|
||
// 其他字段由于结构定义不明确，暂时不填充
|
||
|
||
simulationResults.push_back(tear2);
|
||
|
||
// 发送撕裂结果到所有客户端
|
||
sendTearingResults(simulationResults);
|
||
SendDetectionResultToRobot(simulationResults); // 发送检测结果到机械臂
|
||
|
||
LOG_INFO("Simulation tearing data sent successfully\n");
|
||
}
|
||
|
||
// 发送模拟图像数据
|
||
void BeltTearingPresenter::sendSimulationImageData()
|
||
{
|
||
LOG_INFO("Sending simulation image data\n");
|
||
|
||
// 创建一个模拟的图像
|
||
QImage simulationImage(800, 600, QImage::Format_RGB32);
|
||
|
||
// 填充背景色
|
||
simulationImage.fill(Qt::white);
|
||
|
||
// 创建 QPainter 对象用于绘制
|
||
QPainter painter(&simulationImage);
|
||
|
||
// 发送图像到所有客户端
|
||
sendImageToClients(simulationImage);
|
||
|
||
LOG_INFO("Simulation image data sent successfully\n");
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
void BeltTearingPresenter::startCamera()
|
||
{
|
||
if (!m_eyeDevice) {
|
||
if (!initializeCamera()) {
|
||
// 启动定时器持续重试初始化
|
||
LOG_WARNING("Camera initialization failed, starting retry timer...\n");
|
||
m_cameraInitTimer->start(5000); // 每5秒重试一次
|
||
return;
|
||
}
|
||
}
|
||
|
||
// 尝试打开设备
|
||
int result = m_eyeDevice->OpenDevice(nullptr, 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;
|
||
}
|
||
|
||
LOG_DEBUG("Camera started 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;
|
||
}
|
||
|
||
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);
|
||
} else {
|
||
LOG_DEBUG("Robot work status set to working\n");
|
||
}
|
||
}
|
||
}
|
||
|
||
void 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);
|
||
} else {
|
||
LOG_DEBUG("Robot work status set to idle\n");
|
||
}
|
||
}
|
||
|
||
LOG_INFO("Camera stopped\n");
|
||
}
|
||
|
||
void BeltTearingPresenter::onCameraInitTimer()
|
||
{
|
||
// 尝试重新初始化和连接相机
|
||
if (!m_eyeDevice) {
|
||
if (!initializeCamera()) {
|
||
return; // 继续重试
|
||
}
|
||
}
|
||
|
||
// 尝试连接相机
|
||
int result = m_eyeDevice->OpenDevice(nullptr, 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(std::max(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;
|
||
|
||
// 初始化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 started on port %d\n", port);
|
||
return true;
|
||
}
|
||
|
||
void BeltTearingPresenter::stopServer()
|
||
{
|
||
if (!m_tcpServer) {
|
||
return;
|
||
}
|
||
|
||
// 清空客户端映射
|
||
m_clients.clear();
|
||
|
||
// 停止TCP服务器
|
||
m_tcpServer->Stop();
|
||
m_tcpServer->Close();
|
||
|
||
m_port = 0;
|
||
LOG_INFO("TCP server stopped\n");
|
||
}
|
||
|
||
// 静态回调函数实现
|
||
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() > MAX_QUEUE_SIZE) {
|
||
SVzLaserLineData oldLine = m_laserLineQueue.front();
|
||
m_laserLineQueue.pop_front();
|
||
|
||
// 释放深拷贝的点云数据内存
|
||
if (oldLine.p3DPoint) {
|
||
free(oldLine.p3DPoint);
|
||
oldLine.p3DPoint = nullptr;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 每到图像生成间隔，提交图像生成任务给工作线程
|
||
if (m_lineCounter % GENERATION_INTERVAL == 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 (m_config) {
|
||
BeltTearingConfigResult currentConfig = sdkToConfigParam(m_algorithmParam);
|
||
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());
|
||
} 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 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;
|
||
|
||
// 创建响应对象
|
||
QJsonObject responseObj;
|
||
responseObj["command"] = "configResponse";
|
||
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()
|
||
{
|
||
stopCamera();
|
||
|
||
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();
|
||
|
||
startCamera();
|
||
}
|
||
|
||
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 0; // 没有数据可处理
|
||
}
|
||
|
||
// 只保留最后的GENERATION_INTERVAL大小的数据进行检测
|
||
algorithmDataCache.reserve(GENERATION_INTERVAL);
|
||
int startIndex = std::max(0, static_cast<int>(m_laserLineQueue.size()) - GENERATION_INTERVAL);
|
||
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);
|
||
|
||
return 0; // 成功完成检测任务
|
||
}
|
||
|
||
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);
|
||
}
|
||
}
|
||
}
|
||
|
||
// 初始化机械臂协议
|
||
int BeltTearingPresenter::InitRobotProtocol()
|
||
{
|
||
LOG_DEBUG("Start initializing robot protocol\n");
|
||
|
||
// 创建RobotProtocol实例
|
||
if(nullptr == m_pRobotProtocol){
|
||
m_pRobotProtocol = new RobotProtocol();
|
||
}
|
||
|
||
// 初始化协议服务（使用端口502）
|
||
int 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);
|
||
});
|
||
|
||
LOG_INFO("Robot protocol initialization completed successfully\n");
|
||
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; // 返回处理结果
|
||
}
|
||
|
||
// 发送检测结果到机械臂
|
||
void BeltTearingPresenter::SendDetectionResultToRobot(const std::vector<SSG_beltTearingInfo>& detectionResults)
|
||
{
|
||
// 检查RobotProtocol是否已初始化且连接正常
|
||
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);
|
||
}
|
||
|