436 lines
17 KiB
C++
436 lines
17 KiB
C++
#include "HolePitPositionPresenter.h"
|
||
#include "VrError.h"
|
||
#include "VrLog.h"
|
||
#include <QtCore/QCoreApplication>
|
||
#include <QtCore/QFileInfo>
|
||
#include <QtCore/QDir>
|
||
#include <QtCore/QString>
|
||
#include <QtCore/QStandardPaths>
|
||
#include <QtCore/QFile>
|
||
#include <QtCore/QDateTime>
|
||
#include <cmath>
|
||
#include <algorithm>
|
||
#include <cstdio>
|
||
#include <QImage>
|
||
#include <QThread>
|
||
#include <atomic>
|
||
|
||
#include "Version.h"
|
||
#include "VrTimeUtils.h"
|
||
#include "VrDateUtils.h"
|
||
#include "VrConvert.h"
|
||
#include "TCPServerProtocol.h"
|
||
#include "DetectPresenter.h"
|
||
#include "PathManager.h"
|
||
|
||
// 配置变化监听器代理实现
|
||
void ConfigChangeListenerProxy::OnSystemConfigChanged(const SystemConfig& config)
|
||
{
|
||
if (m_presenter) {
|
||
LOG_INFO("ConfigChangeListenerProxy: config changed, reloading algorithm parameters\n");
|
||
m_presenter->InitAlgoParams();
|
||
}
|
||
}
|
||
|
||
HolePitPositionPresenter::HolePitPositionPresenter(QObject *parent)
|
||
: BasePresenter(parent)
|
||
, m_pConfigManager(nullptr)
|
||
, m_pDetectPresenter(nullptr)
|
||
, m_pTCPServer(nullptr)
|
||
, m_bTCPConnected(false)
|
||
{
|
||
// 基类已经创建了相机重连定时器和检测数据缓存
|
||
}
|
||
|
||
HolePitPositionPresenter::~HolePitPositionPresenter()
|
||
{
|
||
// 基类会自动处理：相机重连定时器、算法检测线程、检测数据缓存、相机设备资源
|
||
|
||
// 释放ConfigManager
|
||
if (m_pConfigManager) {
|
||
m_pConfigManager->Shutdown();
|
||
delete m_pConfigManager;
|
||
m_pConfigManager = nullptr;
|
||
}
|
||
|
||
// 释放TCP服务器
|
||
if (m_pTCPServer) {
|
||
m_pTCPServer->Deinitialize();
|
||
delete m_pTCPServer;
|
||
m_pTCPServer = nullptr;
|
||
}
|
||
|
||
// 释放检测处理器
|
||
if (m_pDetectPresenter) {
|
||
delete m_pDetectPresenter;
|
||
m_pDetectPresenter = nullptr;
|
||
}
|
||
}
|
||
|
||
int HolePitPositionPresenter::InitApp()
|
||
{
|
||
LOG_DEBUG("Start APP Version: %s\n", HOLEPITPOSITION_FULL_VERSION_STRING);
|
||
fprintf(stderr, "[DIAG] InitApp started, version: %s\n", HOLEPITPOSITION_FULL_VERSION_STRING);
|
||
fflush(stderr);
|
||
|
||
// 初始化连接状态
|
||
SetWorkStatus(WorkStatus::InitIng);
|
||
|
||
fprintf(stderr, "[DIAG] Creating DetectPresenter...\n"); fflush(stderr);
|
||
m_pDetectPresenter = new DetectPresenter();
|
||
fprintf(stderr, "[DIAG] DetectPresenter created OK\n"); fflush(stderr);
|
||
|
||
int nRet = SUCCESS;
|
||
|
||
// 创建 ConfigManager 实例
|
||
fprintf(stderr, "[DIAG] Creating ConfigManager...\n"); fflush(stderr);
|
||
m_pConfigManager = new ConfigManager();
|
||
if (!m_pConfigManager) {
|
||
LOG_ERROR("Failed to create ConfigManager instance\n");
|
||
if (auto pStatus = GetStatusCallback<IYHolePitPositionStatus>()) pStatus->OnStatusUpdate("配置管理器创建失败");
|
||
return ERR_CODE(DEV_CONFIG_ERR);
|
||
}
|
||
fprintf(stderr, "[DIAG] ConfigManager created OK\n"); fflush(stderr);
|
||
|
||
// 初始化 ConfigManager
|
||
fprintf(stderr, "[DIAG] Initializing ConfigManager...\n"); fflush(stderr);
|
||
if (!m_pConfigManager->Initialize()) {
|
||
LOG_ERROR("Failed to initialize ConfigManager\n");
|
||
if (auto pStatus = GetStatusCallback<IYHolePitPositionStatus>()) pStatus->OnStatusUpdate("配置管理器初始化失败");
|
||
return ERR_CODE(DEV_CONFIG_ERR);
|
||
}
|
||
fprintf(stderr, "[DIAG] ConfigManager initialized OK\n"); fflush(stderr);
|
||
|
||
// 注册配置变化监听器
|
||
m_pConfigListener = std::make_shared<ConfigChangeListenerProxy>(this);
|
||
m_pConfigManager->AddConfigChangeListener(m_pConfigListener);
|
||
|
||
LOG_INFO("Configuration loaded successfully\n");
|
||
fprintf(stderr, "[DIAG] Configuration loaded OK\n"); fflush(stderr);
|
||
|
||
// 获取配置结果
|
||
ConfigResult configResult = m_pConfigManager->GetConfigResult();
|
||
fprintf(stderr, "[DIAG] Camera count from config: %zu\n", configResult.cameraList.size()); fflush(stderr);
|
||
|
||
// 调用基类InitCamera进行相机初始化（bRGB=false, bSwing=true）
|
||
fprintf(stderr, "[DIAG] Calling InitCamera...\n"); fflush(stderr);
|
||
InitCamera(configResult.cameraList, false, true);
|
||
fprintf(stderr, "[DIAG] InitCamera completed OK\n"); fflush(stderr);
|
||
|
||
LOG_INFO("Camera initialization completed. Connected cameras: %zu, default camera index: %d\n",
|
||
m_vrEyeDeviceList.size(), m_currentCameraIndex);
|
||
|
||
// 初始化TCP服务器
|
||
fprintf(stderr, "[DIAG] Initializing TCP server...\n"); fflush(stderr);
|
||
nRet = InitTCPServer();
|
||
if (nRet != 0) {
|
||
if (auto pStatus = GetStatusCallback<IYHolePitPositionStatus>()) pStatus->OnStatusUpdate("TCP服务器初始化失败");
|
||
m_bTCPConnected = false;
|
||
} else {
|
||
if (auto pStatus = GetStatusCallback<IYHolePitPositionStatus>()) pStatus->OnStatusUpdate("TCP服务器初始化成功");
|
||
}
|
||
fprintf(stderr, "[DIAG] TCP server init result: %d\n", nRet); fflush(stderr);
|
||
|
||
if (auto pStatus = GetStatusCallback<IYHolePitPositionStatus>()) pStatus->OnStatusUpdate("设备初始化完成");
|
||
|
||
CheckAndUpdateWorkStatus();
|
||
|
||
if (auto pStatus = GetStatusCallback<IYHolePitPositionStatus>()) pStatus->OnStatusUpdate("配置初始化成功");
|
||
|
||
fprintf(stderr, "[DIAG] InitApp completed successfully\n"); fflush(stderr);
|
||
return SUCCESS;
|
||
}
|
||
|
||
// 初始化算法参数（实现BasePresenter纯虚函数）
|
||
int HolePitPositionPresenter::InitAlgoParams()
|
||
{
|
||
fprintf(stderr, "[DIAG] InitAlgoParams started\n"); fflush(stderr);
|
||
LOG_DEBUG("initializing algorithm parameters\n");
|
||
QString exePath = QCoreApplication::applicationFilePath();
|
||
|
||
// 清空现有的手眼标定矩阵列表
|
||
m_clibMatrixList.clear();
|
||
|
||
// 从 ConfigManager 获取配置结果（包含多相机手眼标定矩阵列表）
|
||
ConfigResult configResult = m_pConfigManager->GetConfigResult();
|
||
|
||
// 从 config.xml 加载所有相机的手眼标定矩阵
|
||
for (size_t i = 0; i < configResult.handEyeCalibMatrixList.size(); i++) {
|
||
CalibMatrix calibMatrix;
|
||
memcpy(calibMatrix.clibMatrix, configResult.handEyeCalibMatrixList[i].matrix, sizeof(double) * 16);
|
||
m_clibMatrixList.push_back(calibMatrix);
|
||
|
||
LOG_INFO("Loaded hand-eye calibration matrix for camera %zu from config.xml:\n", i + 1);
|
||
QString clibMatrixStr;
|
||
for (int r = 0; r < 4; ++r) {
|
||
clibMatrixStr.clear();
|
||
for (int c = 0; c < 4; ++c) {
|
||
clibMatrixStr += QString::asprintf("%8.4f ", calibMatrix.clibMatrix[r * 4 + c]);
|
||
}
|
||
LOG_INFO(" %s\n", clibMatrixStr.toStdString().c_str());
|
||
}
|
||
}
|
||
|
||
// 如果没有配置任何矩阵，添加一个默认单位矩阵
|
||
if (m_clibMatrixList.empty()) {
|
||
CalibMatrix defaultMatrix;
|
||
double initClibMatrix[16] = {
|
||
1.0, 0.0, 0.0, 0.0,
|
||
0.0, 1.0, 0.0, 0.0,
|
||
0.0, 0.0, 1.0, 0.0,
|
||
0.0, 0.0, 0.0, 1.0
|
||
};
|
||
memcpy(defaultMatrix.clibMatrix, initClibMatrix, sizeof(initClibMatrix));
|
||
m_clibMatrixList.push_back(defaultMatrix);
|
||
LOG_WARNING("No hand-eye calibration matrices found, using identity matrix\n");
|
||
}
|
||
|
||
LOG_INFO("Total loaded %zu hand-eye calibration matrices\n", m_clibMatrixList.size());
|
||
|
||
const VrAlgorithmParams& xmlParams = configResult.algorithmParams;
|
||
|
||
// 打印线段参数
|
||
LOG_INFO("Loaded XML params - LineSeg: distScale=%.1f, segGapTh_y=%.1f, segGapTh_z=%.1f\n",
|
||
xmlParams.lineSegParam.distScale,
|
||
xmlParams.lineSegParam.segGapTh_y,
|
||
xmlParams.lineSegParam.segGapTh_z);
|
||
|
||
// 打印角点参数
|
||
LOG_INFO("Loaded XML params - Corner: cornerTh=%.1f, scale=%.1f, minEndingGap=%.1f, minEndingGap_z=%.1f, jumpCornerTh_1=%.1f, jumpCornerTh_2=%.1f\n",
|
||
xmlParams.cornerParam.cornerTh,
|
||
xmlParams.cornerParam.scale,
|
||
xmlParams.cornerParam.minEndingGap,
|
||
xmlParams.cornerParam.minEndingGap_z,
|
||
xmlParams.cornerParam.jumpCornerTh_1,
|
||
xmlParams.cornerParam.jumpCornerTh_2);
|
||
|
||
// 打印噪声滤除参数
|
||
LOG_INFO("Loaded XML params - OutlierFilter: continuityTh=%.1f, outlierTh=%.1f\n",
|
||
xmlParams.outlierFilterParam.continuityTh,
|
||
xmlParams.outlierFilterParam.outlierTh);
|
||
|
||
// 打印树生长参数
|
||
LOG_INFO("Loaded XML params - TreeGrow: maxLineSkipNum=%d, yDeviation_max=%.1f, maxSkipDistance=%.1f, zDeviation_max=%.1f, minLTypeTreeLen=%.1f, minVTypeTreeLen=%.1f\n",
|
||
xmlParams.treeGrowParam.maxLineSkipNum,
|
||
xmlParams.treeGrowParam.yDeviation_max,
|
||
xmlParams.treeGrowParam.maxSkipDistance,
|
||
xmlParams.treeGrowParam.zDeviation_max,
|
||
xmlParams.treeGrowParam.minLTypeTreeLen,
|
||
xmlParams.treeGrowParam.minVTypeTreeLen);
|
||
|
||
LOG_INFO("Algorithm parameters initialized successfully\n");
|
||
|
||
fprintf(stderr, "[DIAG] InitAlgoParams completed OK\n"); fflush(stderr);
|
||
return SUCCESS;
|
||
}
|
||
|
||
// 手眼标定矩阵管理方法实现
|
||
CalibMatrix HolePitPositionPresenter::GetClibMatrix(int index) const
|
||
{
|
||
CalibMatrix clibMatrix;
|
||
|
||
double initClibMatrix[16] = {
|
||
1.0, 0.0, 0.0, 0.0,
|
||
0.0, 1.0, 0.0, 0.0,
|
||
0.0, 0.0, 1.0, 0.0,
|
||
0.0, 0.0, 0.0, 1.0
|
||
};
|
||
memcpy(clibMatrix.clibMatrix, initClibMatrix, sizeof(initClibMatrix));
|
||
|
||
if (index >= 0 && index < static_cast<int>(m_clibMatrixList.size())) {
|
||
clibMatrix = m_clibMatrixList[index];
|
||
memcpy(clibMatrix.clibMatrix, m_clibMatrixList[index].clibMatrix, sizeof(initClibMatrix));
|
||
} else {
|
||
LOG_WARNING("Invalid hand-eye calibration matrix\n");
|
||
}
|
||
|
||
return clibMatrix;
|
||
}
|
||
|
||
std::string HolePitPositionPresenter::GetAlgoVersion() const
|
||
{
|
||
if (m_pDetectPresenter) {
|
||
return m_pDetectPresenter->GetAlgoVersion();
|
||
}
|
||
return "unknown";
|
||
}
|
||
|
||
|
||
void HolePitPositionPresenter::CheckAndUpdateWorkStatus()
|
||
{
|
||
if (m_bCameraConnected) {
|
||
SetWorkStatus(WorkStatus::Ready);
|
||
} else {
|
||
SetWorkStatus(WorkStatus::Error);
|
||
}
|
||
}
|
||
|
||
// 实现BasePresenter纯虚函数：执行算法检测
|
||
int HolePitPositionPresenter::ProcessAlgoDetection(std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& detectionDataCache)
|
||
{
|
||
LOG_INFO("[Algo Thread] Start real detection task using algorithm\n");
|
||
|
||
// 1. 获取缓存的点云数据（已由基类验证非空）
|
||
unsigned int lineNum = detectionDataCache.size();
|
||
if (auto pStatus = GetStatusCallback<IYHolePitPositionStatus>()) {
|
||
pStatus->OnStatusUpdate("扫描线数:" + std::to_string(lineNum) + "，正在算法检测...");
|
||
}
|
||
|
||
// 检查检测处理器是否已初始化
|
||
if (!m_pDetectPresenter) {
|
||
LOG_ERROR("DetectPresenter is null, cannot proceed with detection\n");
|
||
if (auto pStatus = GetStatusCallback<IYHolePitPositionStatus>()) {
|
||
pStatus->OnStatusUpdate("检测处理器未初始化");
|
||
}
|
||
return ERR_CODE(DEV_NOT_FIND);
|
||
}
|
||
|
||
CVrTimeUtils oTimeUtils;
|
||
|
||
// 获取当前使用的手眼标定矩阵
|
||
const CalibMatrix currentClibMatrix = GetClibMatrix(m_currentCameraIndex - 1);
|
||
|
||
// 打印手眼标定矩阵
|
||
LOG_INFO("[Algo Thread] Hand-Eye Calibration Matrix (Camera %d):\n", m_currentCameraIndex);
|
||
LOG_INFO(" [%.4f, %.4f, %.4f, %.4f]\n", currentClibMatrix.clibMatrix[0], currentClibMatrix.clibMatrix[1], currentClibMatrix.clibMatrix[2], currentClibMatrix.clibMatrix[3]);
|
||
LOG_INFO(" [%.4f, %.4f, %.4f, %.4f]\n", currentClibMatrix.clibMatrix[4], currentClibMatrix.clibMatrix[5], currentClibMatrix.clibMatrix[6], currentClibMatrix.clibMatrix[7]);
|
||
LOG_INFO(" [%.4f, %.4f, %.4f, %.4f]\n", currentClibMatrix.clibMatrix[8], currentClibMatrix.clibMatrix[9], currentClibMatrix.clibMatrix[10], currentClibMatrix.clibMatrix[11]);
|
||
LOG_INFO(" [%.4f, %.4f, %.4f, %.4f]\n", currentClibMatrix.clibMatrix[12], currentClibMatrix.clibMatrix[13], currentClibMatrix.clibMatrix[14], currentClibMatrix.clibMatrix[15]);
|
||
|
||
// 从 ConfigManager 获取算法参数和调试参数
|
||
VrAlgorithmParams algorithmParams = m_pConfigManager->GetAlgorithmParams();
|
||
ConfigResult configResult = m_pConfigManager->GetConfigResult();
|
||
VrDebugParam debugParam = configResult.debugParam;
|
||
|
||
// 获取旋转顺序配置（从对应相机的标定矩阵中获取）
|
||
int eulerOrder = 11; // 默认外旋ZYX
|
||
int calibIdx = m_currentCameraIndex - 1;
|
||
if (calibIdx >= 0 && calibIdx < static_cast<int>(configResult.handEyeCalibMatrixList.size())) {
|
||
eulerOrder = configResult.handEyeCalibMatrixList[calibIdx].eulerOrder;
|
||
}
|
||
LOG_INFO("[Algo Thread] Using euler order: %d (camera %d)\n", eulerOrder, m_currentCameraIndex);
|
||
|
||
// 获取方向向量反向配置
|
||
int dirVectorInvert = configResult.plcRobotServerConfig.dirVectorInvert;
|
||
LOG_INFO("[Algo Thread] Using dir vector invert: %d\n", dirVectorInvert);
|
||
|
||
HoleDetectionResult detectionResult;
|
||
|
||
int nRet = m_pDetectPresenter->DetectHoles(m_currentCameraIndex, detectionDataCache,
|
||
algorithmParams, debugParam, m_dataLoader,
|
||
currentClibMatrix.clibMatrix, eulerOrder, dirVectorInvert,
|
||
m_currentRobotPose, detectionResult);
|
||
|
||
// 根据项目类型选择处理方式
|
||
if (auto pStatus = GetStatusCallback<IYHolePitPositionStatus>()) {
|
||
QString err = QString("错误:%1").arg(nRet);
|
||
pStatus->OnStatusUpdate(QString("检测%1").arg(SUCCESS == nRet ? "成功": err).toStdString());
|
||
}
|
||
|
||
LOG_INFO("[Algo Thread] DetectHoles detected %zu holes (candidates=%d, filtered=%d) time : %.2f ms\n",
|
||
detectionResult.positions.size(), detectionResult.totalCandidates, detectionResult.filteredCount,
|
||
oTimeUtils.GetElapsedTimeInMilliSec());
|
||
ERR_CODE_RETURN(nRet);
|
||
|
||
// 8. 通知UI检测结果
|
||
detectionResult.cameraIndex = m_currentCameraIndex;
|
||
if (auto pStatus = GetStatusCallback<IYHolePitPositionStatus>()) {
|
||
pStatus->OnDetectionResult(detectionResult);
|
||
}
|
||
|
||
// 更新状态
|
||
QString statusMsg = QString("检测完成，发现%1个孔").arg(detectionResult.positions.size());
|
||
if (auto pStatus = GetStatusCallback<IYHolePitPositionStatus>()) pStatus->OnStatusUpdate(statusMsg.toStdString());
|
||
|
||
// 发送检测结果给TCP客户端
|
||
_SendDetectionResultToTCP(detectionResult, m_currentCameraIndex);
|
||
|
||
// 9. 检测完成后，将工作状态更新为"完成"
|
||
SetWorkStatus(WorkStatus::Completed);
|
||
|
||
// 恢复到就绪状态
|
||
SetWorkStatus(WorkStatus::Ready);
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
|
||
// 实现配置改变通知接口
|
||
void HolePitPositionPresenter::OnConfigChanged(const ConfigResult& configResult)
|
||
{
|
||
LOG_INFO("Configuration changed notification received, reloading algorithm parameters\n");
|
||
|
||
// 重新初始化算法参数
|
||
int result = InitAlgoParams();
|
||
if (result == SUCCESS) {
|
||
LOG_INFO("Algorithm parameters reloaded successfully after config change\n");
|
||
if (auto pStatus = GetStatusCallback<IYHolePitPositionStatus>()) {
|
||
pStatus->OnStatusUpdate("配置已更新，算法参数重新加载成功");
|
||
}
|
||
} else {
|
||
LOG_ERROR("Failed to reload algorithm parameters after config change, error: %d\n", result);
|
||
if (auto pStatus = GetStatusCallback<IYHolePitPositionStatus>()) {
|
||
pStatus->OnStatusUpdate("配置更新后算法参数重新加载失败");
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
// 实现BasePresenter纯虚函数：相机状态变化通知
|
||
void HolePitPositionPresenter::OnCameraStatusChanged(int cameraIndex, bool isConnected)
|
||
{
|
||
LOG_INFO("Camera %d status changed: %s\n", cameraIndex, isConnected ? "connected" : "disconnected");
|
||
|
||
// 通知UI更新相机状态
|
||
if (GetStatusCallback<IYHolePitPositionStatus>()) {
|
||
if (cameraIndex == 1) {
|
||
if (auto pStatus = GetStatusCallback<IYHolePitPositionStatus>()) pStatus->OnCamera1StatusChanged(isConnected);
|
||
} else if (cameraIndex == 2) {
|
||
if (auto pStatus = GetStatusCallback<IYHolePitPositionStatus>()) pStatus->OnCamera2StatusChanged(isConnected);
|
||
}
|
||
|
||
// 获取相机名称用于状态消息
|
||
QString cameraName;
|
||
int arrayIndex = cameraIndex - 1;
|
||
if (arrayIndex >= 0 && arrayIndex < static_cast<int>(m_vrEyeDeviceList.size())) {
|
||
cameraName = QString::fromStdString(m_vrEyeDeviceList[arrayIndex].first);
|
||
} else {
|
||
cameraName = QString("相机%1").arg(cameraIndex);
|
||
}
|
||
|
||
QString statusMsg = QString("%1%2").arg(cameraName).arg(isConnected ? "已连接" : "已断开");
|
||
if (auto pStatus = GetStatusCallback<IYHolePitPositionStatus>()) pStatus->OnStatusUpdate(statusMsg.toStdString());
|
||
}
|
||
|
||
// 检查并更新工作状态
|
||
CheckAndUpdateWorkStatus();
|
||
}
|
||
|
||
// 实现BasePresenter虚函数：工作状态变化通知
|
||
void HolePitPositionPresenter::OnWorkStatusChanged(WorkStatus status)
|
||
{
|
||
auto pStatus = GetStatusCallback<IYHolePitPositionStatus>();
|
||
if (pStatus) {
|
||
pStatus->OnWorkStatusChanged(status);
|
||
}
|
||
}
|
||
|
||
|
||
// 实现BasePresenter虚函数：相机数量变化通知
|
||
void HolePitPositionPresenter::OnCameraCountChanged(int count)
|
||
{
|
||
auto pStatus = GetStatusCallback<IYHolePitPositionStatus>();
|
||
if (pStatus) {
|
||
pStatus->OnCameraCountChanged(count);
|
||
}
|
||
}
|
||
|
||
// 实现BasePresenter虚函数：状态文字更新通知
|
||
void HolePitPositionPresenter::OnStatusUpdate(const std::string& statusMessage)
|
||
{
|
||
auto pStatus = GetStatusCallback<IYHolePitPositionStatus>();
|
||
if (pStatus) {
|
||
pStatus->OnStatusUpdate(statusMessage);
|
||
}
|
||
}
|