YangOne/GrabBag
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
GrabBag/App/StatorPosition/StatorPositionApp/Presenter/Src/StatorPositionPresenter.cpp

983 lines
39 KiB
C++
Raw Normal View History

定子定位软件
2026-02-23 18:22:38 +08:00
#include "StatorPositionPresenter.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 <cstring>
#include <algorithm>
#include <QImage>
#include <QThread>
#include <atomic>
#include <QJsonObject>
#include <QJsonArray>
#include "Version.h"
#include "VrTimeUtils.h"
#include "VrDateUtils.h"
#include "SG_baseDataType.h"
#include "VrConvert.h"
#include "TCPServerProtocol.h"
#include "DetectPresenter.h"
#include "PathManager.h"
#include "motorStatorPosition_Export.h" // 调平算法接口
// 配置变化监听器代理实现
void ConfigChangeListenerProxy::OnSystemConfigChanged(const SystemConfig& config)
{
if (m_presenter) {
LOG_INFO("ConfigChangeListenerProxy: config changed, reloading algorithm parameters\n");
m_presenter->InitAlgoParams();
}
}
StatorPositionPresenter::StatorPositionPresenter(QObject *parent)
: BasePresenter(parent)
, m_pConfigManager(nullptr)
, m_pDetectPresenter(nullptr)
, m_pTCPServer(nullptr)
, m_bTCPConnected(false)
, m_pPLCModbusClient(nullptr)
, m_bPLCConnected(false)
, m_bRobotConnected(false)
{
// 基类已经创建了相机重连定时器和检测数据缓存
}
StatorPositionPresenter::~StatorPositionPresenter()
{
// 基类会自动处理：相机重连定时器、算法检测线程、检测数据缓存、相机设备资源
// 释放 PLC Modbus 客户端
if (m_pPLCModbusClient) {
m_pPLCModbusClient->Shutdown();
delete m_pPLCModbusClient;
m_pPLCModbusClient = nullptr;
}
// 释放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 StatorPositionPresenter::InitApp()
{
LOG_DEBUG("Start APP Version: %s\n", STATORPOSITION_FULL_VERSION_STRING);
// 初始化连接状态
SetWorkStatus(WorkStatus::InitIng);
m_pDetectPresenter = new DetectPresenter();
int nRet = SUCCESS;
// 创建 ConfigManager 实例
m_pConfigManager = new ConfigManager();
if (!m_pConfigManager) {
LOG_ERROR("Failed to create ConfigManager instance\n");
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) pStatus->OnStatusUpdate("配置管理器创建失败");
return ERR_CODE(DEV_CONFIG_ERR);
}
// 初始化 ConfigManager
if (!m_pConfigManager->Initialize()) {
LOG_ERROR("Failed to initialize ConfigManager\n");
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) pStatus->OnStatusUpdate("配置管理器初始化失败");
return ERR_CODE(DEV_CONFIG_ERR);
}
// 注册配置变化监听器
m_pConfigListener = std::make_shared<ConfigChangeListenerProxy>(this);
m_pConfigManager->AddConfigChangeListener(m_pConfigListener);
LOG_INFO("Configuration loaded successfully\n");
// 获取配置结果
ConfigResult configResult = m_pConfigManager->GetConfigResult();
// 调用基类InitCamera进行相机初始化（bRGB=false, bSwing=true）
InitCamera(configResult.cameraList, false, true);
LOG_INFO("Camera initialization completed. Connected cameras: %zu, default camera index: %d\n",
m_vrEyeDeviceList.size(), m_currentCameraIndex);
// 初始化TCP服务器
nRet = InitTCPServer();
if (nRet != 0) {
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) pStatus->OnStatusUpdate("TCP服务器初始化失败");
m_bTCPConnected = false;
} else {
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) pStatus->OnStatusUpdate("TCP服务器初始化成功");
}
#if 1
// 初始化 PLC Modbus 客户端
nRet = InitPLCModbus();
if (nRet != 0) {
LOG_WARNING("PLC Modbus initialization failed, continuing without PLC communication\n");
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) pStatus->OnStatusUpdate("PLC通信初始化失败");
} else {
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) pStatus->OnStatusUpdate("PLC通信初始化成功");
}
#endif
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) pStatus->OnStatusUpdate("设备初始化完成");
CheckAndUpdateWorkStatus();
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) pStatus->OnStatusUpdate("配置初始化成功");
return SUCCESS;
}
// 初始化算法参数（实现BasePresenter纯虚函数）
int StatorPositionPresenter::InitAlgoParams()
{
LOG_DEBUG("initializing algorithm parameters\n");
QString exePath = QCoreApplication::applicationFilePath();
// 清空现有的手眼标定矩阵列表
m_clibMatrixList.clear();
// 从 ConfigManager 获取配置结果（包含手眼标定矩阵）
ConfigResult configResult = m_pConfigManager->GetConfigResult();
// 从 config.xml 加载手眼标定矩阵
CalibMatrix calibMatrix;
memcpy(calibMatrix.clibMatrix, configResult.handEyeCalibMatrix.matrix, sizeof(double) * 16);
m_clibMatrixList.push_back(calibMatrix);
LOG_INFO("Loaded hand-eye calibration matrix from config.xml:\n");
QString clibMatrixStr;
for (int i = 0; i < 4; ++i) {
clibMatrixStr.clear();
for (int j = 0; j < 4; ++j) {
clibMatrixStr += QString::asprintf("%8.4f ", calibMatrix.clibMatrix[i * 4 + j]);
}
LOG_INFO(" %s\n", clibMatrixStr.toStdString().c_str());
}
LOG_INFO("Total loaded %zu hand-eye calibration matrices\n", m_clibMatrixList.size());
const VrAlgorithmParams& xmlParams = configResult.algorithmParams;
LOG_INFO("Loaded XML params - Filter: continuityTh=%.1f, outlierTh=%.1f\n",
xmlParams.filterParam.continuityTh, xmlParams.filterParam.outlierTh);
LOG_INFO("Loaded XML params - Corner: cornerTh=%.1f, scale=%.1f, minEndingGap=%.1f, minEndingGap_z=%.1f\n",
xmlParams.cornerParam.cornerTh, xmlParams.cornerParam.scale,
xmlParams.cornerParam.minEndingGap, xmlParams.cornerParam.minEndingGap_z);
LOG_INFO("Algorithm parameters initialized successfully\n");
// 循环打印所有相机的调平参数（添加安全检查）
LOG_INFO("Loading plane calibration parameters for all cameras:\n");
if (!xmlParams.planeCalibParam.cameraCalibParams.empty()) {
for (const auto& cameraParam : xmlParams.planeCalibParam.cameraCalibParams) {
try {
LOG_INFO("Camera %d (%s) calibration parameters:\n",
cameraParam.cameraIndex, cameraParam.cameraName.c_str());
LOG_INFO(" Is calibrated: %s\n", cameraParam.isCalibrated ? "YES" : "NO");
LOG_INFO(" Plane height: %.3f\n", cameraParam.planeHeight);
LOG_INFO(" Plane calibration matrix:\n");
LOG_INFO(" [%.3f, %.3f, %.3f]\n", cameraParam.planeCalib[0], cameraParam.planeCalib[1], cameraParam.planeCalib[2]);
LOG_INFO(" [%.3f, %.3f, %.3f]\n", cameraParam.planeCalib[3], cameraParam.planeCalib[4], cameraParam.planeCalib[5]);
LOG_INFO(" [%.3f, %.3f, %.3f]\n", cameraParam.planeCalib[6], cameraParam.planeCalib[7], cameraParam.planeCalib[8]);
LOG_INFO(" Inverse rotation matrix:\n");
LOG_INFO(" [%.3f, %.3f, %.3f]\n", cameraParam.invRMatrix[0], cameraParam.invRMatrix[1], cameraParam.invRMatrix[2]);
LOG_INFO(" [%.3f, %.3f, %.3f]\n", cameraParam.invRMatrix[3], cameraParam.invRMatrix[4], cameraParam.invRMatrix[5]);
LOG_INFO(" [%.3f, %.3f, %.3f]\n", cameraParam.invRMatrix[6], cameraParam.invRMatrix[7], cameraParam.invRMatrix[8]);
LOG_INFO(" --------------------------------\n");
} catch (const std::exception& e) {
LOG_ERROR("Exception while printing camera calibration parameters: %s\n", e.what());
} catch (...) {
LOG_ERROR("Unknown exception while printing camera calibration parameters\n");
}
}
} else {
LOG_WARNING("No plane calibration parameters found in configuration\n");
}
return SUCCESS;
}
// 手眼标定矩阵管理方法实现
CalibMatrix StatorPositionPresenter::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;
}
void StatorPositionPresenter::CheckAndUpdateWorkStatus()
{
if (m_bCameraConnected) {
SetWorkStatus(WorkStatus::Ready);
} else {
SetWorkStatus(WorkStatus::Error);
}
}
// 实现BasePresenter纯虚函数：执行算法检测
int StatorPositionPresenter::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(GetStatusCallback<IYStatorPositionStatus>()){
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) pStatus->OnStatusUpdate("扫描线数:" + std::to_string(lineNum) + "，正在算法检测...");
}
// 检查检测处理器是否已初始化
if (!m_pDetectPresenter) {
LOG_ERROR("DetectPresenter is null, cannot proceed with detection\n");
if (GetStatusCallback<IYStatorPositionStatus>()) {
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) 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 = configResult.handEyeCalibMatrix.eulerOrder;
LOG_INFO("[Algo Thread] Using euler order: %d\n", eulerOrder);
// 获取方向向量反向配置
int dirVectorInvert = configResult.plcRobotServerConfig.dirVectorInvert;
LOG_INFO("[Algo Thread] Using dir vector invert: %d\n", dirVectorInvert);
StatorPositionDetectionResult detectionResult;
int nRet = m_pDetectPresenter->DetectStatorPosition(m_currentCameraIndex, detectionDataCache,
algorithmParams, debugParam, m_dataLoader,
currentClibMatrix.clibMatrix, eulerOrder, dirVectorInvert, detectionResult);
// 根据项目类型选择处理方式
if (GetStatusCallback<IYStatorPositionStatus>()) {
QString err = QString("错误:%1").arg(nRet);
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) pStatus->OnStatusUpdate(QString("检测%1").arg(SUCCESS == nRet ? "成功": err).toStdString());
}
LOG_INFO("[Algo Thread] wd_particleSegVersion detected %zu workpieces time : %.2f ms\n", detectionResult.positions.size(), oTimeUtils.GetElapsedTimeInMilliSec());
if (nRet != SUCCESS) {
LOG_ERROR("[Algo Thread] Detection failed, error: %d\n", nRet);
// 检测失败，两个通道都写错误状态
ConfigResult errConfigResult = m_pConfigManager->GetConfigResult();
// 通道1: PLC Client
if (m_pPLCModbusClient) {
if (m_pPLCModbusClient->IsPLCConnected()) {
m_pPLCModbusClient->WriteWorkStatus(PLCModbusClient::WORK_STATUS_ERROR);
}
m_pPLCModbusClient->ResumeTriggerPolling();
}
// 通道2: Modbus Server
if (IsModbusServerRunning()) {
uint16_t errorStatus = PLCModbusClient::WORK_STATUS_ERROR;
WriteModbusRegisters(errConfigResult.plcRobotServerConfig.registerConfig.addrWorkStatus, &errorStatus, 1);
}
return nRet;
}
// 8. 通知UI检测结果
detectionResult.cameraIndex = m_currentCameraIndex;
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) {
pStatus->OnDetectionResult(detectionResult);
}
// 更新状态
QString statusMsg = QString("检测完成，发现%1个工件").arg(detectionResult.positions.size());
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) pStatus->OnStatusUpdate(statusMsg.toStdString());
// 发送检测结果给TCP客户端
_SendDetectionResultToTCP(detectionResult, m_currentCameraIndex);
// 写检测结果到所有通道（PLC Client + Modbus Server）
WriteDetectionResultToAllChannels(detectionResult);
// 9. 检测完成后，将工作状态更新为"完成"
SetWorkStatus(WorkStatus::Completed);
// 恢复到就绪状态
SetWorkStatus(WorkStatus::Ready);
return SUCCESS;
}
// 实现配置改变通知接口
void StatorPositionPresenter::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 (GetStatusCallback<IYStatorPositionStatus>()) {
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) pStatus->OnStatusUpdate("配置已更新，算法参数重新加载成功");
}
} else {
LOG_ERROR("Failed to reload algorithm parameters after config change, error: %d\n", result);
if (GetStatusCallback<IYStatorPositionStatus>()) {
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) pStatus->OnStatusUpdate("配置更新后算法参数重新加载失败");
}
}
}
// 根据相机索引获取调平参数
SSG_planeCalibPara StatorPositionPresenter::_GetCameraCalibParam(int cameraIndex)
{
// 查找指定相机索引的调平参数
SSG_planeCalibPara calibParam;
// 使用单位矩阵（未校准状态）
double identityMatrix[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
for (int i = 0; i < 9; i++) {
calibParam.planeCalib[i] = identityMatrix[i];
calibParam.invRMatrix[i] = identityMatrix[i];
}
calibParam.planeHeight = -1.0; // 使用默认高度
// 从 ConfigManager 获取算法参数
VrAlgorithmParams algorithmParams = m_pConfigManager->GetAlgorithmParams();
for (const auto& cameraParam : algorithmParams.planeCalibParam.cameraCalibParams) {
if (cameraParam.cameraIndex == cameraIndex) {
// 根据isCalibrated标志决定使用标定矩阵还是单位矩阵
if (cameraParam.isCalibrated) {
// 使用实际的标定矩阵
for (int i = 0; i < 9; i++) {
calibParam.planeCalib[i] = cameraParam.planeCalib[i];
calibParam.invRMatrix[i] = cameraParam.invRMatrix[i];
}
calibParam.planeHeight = cameraParam.planeHeight;
}
}
}
return calibParam;
}
// 实现BasePresenter纯虚函数：相机状态变化通知
void StatorPositionPresenter::OnCameraStatusChanged(int cameraIndex, bool isConnected)
{
LOG_INFO("Camera %d status changed: %s\n", cameraIndex, isConnected ? "connected" : "disconnected");
// 通知UI更新相机状态
if (GetStatusCallback<IYStatorPositionStatus>()) {
if (cameraIndex == 1) {
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) pStatus->OnCamera1StatusChanged(isConnected);
} else if (cameraIndex == 2) {
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) 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<IYStatorPositionStatus>()) pStatus->OnStatusUpdate(statusMsg.toStdString());
}
// 检查并更新工作状态
CheckAndUpdateWorkStatus();
}
// 实现BasePresenter虚函数：工作状态变化通知
void StatorPositionPresenter::OnWorkStatusChanged(WorkStatus status)
{
auto pStatus = GetStatusCallback<IYStatorPositionStatus>();
if (pStatus) {
pStatus->OnWorkStatusChanged(status);
}
}
// 实现BasePresenter虚函数：相机数量变化通知
void StatorPositionPresenter::OnCameraCountChanged(int count)
{
auto pStatus = GetStatusCallback<IYStatorPositionStatus>();
if (pStatus) {
pStatus->OnCameraCountChanged(count);
}
}
// 实现BasePresenter虚函数：状态文字更新通知
void StatorPositionPresenter::OnStatusUpdate(const std::string& statusMessage)
{
auto pStatus = GetStatusCallback<IYStatorPositionStatus>();
if (pStatus) {
pStatus->OnStatusUpdate(statusMessage);
}
}
// ============ 实现 ICameraLevelCalculator 接口 ============
bool StatorPositionPresenter::CalculatePlaneCalibration(
const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& scanData,
double planeCalib[9],
double& planeHeight,
double invRMatrix[9])
{
try {
// 检查是否有足够的扫描数据
if (scanData.empty()) {
LOG_ERROR("No scan data available for plane calibration\n");
return false;
}
LOG_INFO("Calculating plane calibration from %zu scan lines\n", scanData.size());
// 转换为算法需要的XYZ格式
LaserDataLoader dataLoader;
std::vector<std::vector<SVzNL3DPosition>> xyzData;
int convertResult = dataLoader.ConvertToSVzNL3DPosition(scanData, xyzData);
if (convertResult != SUCCESS || xyzData.empty()) {
LOG_WARNING("Failed to convert data to XYZ format or no XYZ data available\n");
return false;
}
// 调用定子定位项目的调平算法
SSG_planeCalibPara calibResult = wd_getBaseCalibPara(xyzData);
// 打印算法返回结果
LOG_INFO("=== wd_getBaseCalibPara 算法结果 ===\n");
LOG_INFO("planeHeight: %.6f\n", calibResult.planeHeight);
LOG_INFO("planeCalib (调平旋转矩阵):\n");
LOG_INFO(" [%.6f, %.6f, %.6f]\n", calibResult.planeCalib[0], calibResult.planeCalib[1], calibResult.planeCalib[2]);
LOG_INFO(" [%.6f, %.6f, %.6f]\n", calibResult.planeCalib[3], calibResult.planeCalib[4], calibResult.planeCalib[5]);
LOG_INFO(" [%.6f, %.6f, %.6f]\n", calibResult.planeCalib[6], calibResult.planeCalib[7], calibResult.planeCalib[8]);
LOG_INFO("invRMatrix (逆旋转矩阵):\n");
LOG_INFO(" [%.6f, %.6f, %.6f]\n", calibResult.invRMatrix[0], calibResult.invRMatrix[1], calibResult.invRMatrix[2]);
LOG_INFO(" [%.6f, %.6f, %.6f]\n", calibResult.invRMatrix[3], calibResult.invRMatrix[4], calibResult.invRMatrix[5]);
LOG_INFO(" [%.6f, %.6f, %.6f]\n", calibResult.invRMatrix[6], calibResult.invRMatrix[7], calibResult.invRMatrix[8]);
LOG_INFO("========================================\n");
// 将结构体中的数据复制到输出参数
for (int i = 0; i < 9; i++) {
planeCalib[i] = calibResult.planeCalib[i];
invRMatrix[i] = calibResult.invRMatrix[i];
}
planeHeight = calibResult.planeHeight;
LOG_INFO("Plane calibration calculated successfully: height=%.3f\n", planeHeight);
return true;
} catch (const std::exception& e) {
LOG_ERROR("Exception in CalculatePlaneCalibration: %s\n", e.what());
return false;
} catch (...) {
LOG_ERROR("Unknown exception in CalculatePlaneCalibration\n");
return false;
}
}
// ============ 实现 ICameraLevelResultSaver 接口 ============
bool StatorPositionPresenter::SaveLevelingResults(double planeCalib[9], double planeHeight, double invRMatrix[9],
int cameraIndex, const QString& cameraName)
{
try {
if (!m_pConfigManager) {
LOG_ERROR("ConfigManager is null, cannot save leveling results\n");
return false;
}
// 验证传入的相机参数
if (cameraIndex <= 0) {
LOG_ERROR("Invalid camera index: %d\n", cameraIndex);
return false;
}
if (cameraName.isEmpty()) {
LOG_ERROR("Camera name is empty\n");
return false;
}
// 获取当前配置
QString configPath = PathManager::GetInstance().GetConfigFilePath();
LOG_INFO("Config path: %s\n", configPath.toUtf8().constData());
SystemConfig systemConfig = m_pConfigManager->GetConfig();
// 创建或更新指定相机的调平参数
VrCameraPlaneCalibParam cameraParam;
cameraParam.cameraIndex = cameraIndex;
cameraParam.cameraName = cameraName.toStdString();
cameraParam.planeHeight = planeHeight;
cameraParam.isCalibrated = true;
// 复制校准矩阵
for (int i = 0; i < 9; i++) {
cameraParam.planeCalib[i] = planeCalib[i];
cameraParam.invRMatrix[i] = invRMatrix[i];
}
// 更新配置中的相机校准参数
systemConfig.configResult.algorithmParams.planeCalibParam.SetCameraCalibParam(cameraParam);
// 更新并保存配置
if (!m_pConfigManager->UpdateFullConfig(systemConfig)) {
LOG_ERROR("Failed to update config with leveling results\n");
return false;
}
if (!m_pConfigManager->SaveConfigToFile(configPath.toStdString())) {
LOG_ERROR("Failed to save config file with leveling results\n");
return false;
}
LOG_INFO("Leveling results saved successfully for camera %d (%s)\n", cameraIndex, cameraName.toUtf8().constData());
LOG_INFO("Plane height: %.3f\n", planeHeight);
LOG_INFO("Calibration marked as completed\n");
return true;
} catch (const std::exception& e) {
LOG_ERROR("Exception in SaveLevelingResults: %s\n", e.what());
return false;
}
}
bool StatorPositionPresenter::LoadLevelingResults(int cameraIndex, const QString& cameraName,
double planeCalib[9], double& planeHeight, double invRMatrix[9])
{
try {
if (!m_pConfigManager) {
LOG_ERROR("ConfigManager is null, cannot load calibration data\n");
return false;
}
// 从ConfigManager获取配置结果
ConfigResult configResult = m_pConfigManager->GetConfigResult();
// 获取指定相机的标定参数
VrCameraPlaneCalibParam cameraParamValue;
if (!configResult.algorithmParams.planeCalibParam.GetCameraCalibParam(cameraIndex, cameraParamValue) || !cameraParamValue.isCalibrated) {
LOG_INFO("No calibration data found for camera %d (%s)\n", cameraIndex, cameraName.toUtf8().constData());
return false;
}
// 复制标定数据
for (int i = 0; i < 9; i++) {
planeCalib[i] = cameraParamValue.planeCalib[i];
invRMatrix[i] = cameraParamValue.invRMatrix[i];
}
planeHeight = cameraParamValue.planeHeight;
LOG_INFO("Calibration data loaded successfully for camera %d (%s)\n", cameraIndex, cameraName.toUtf8().constData());
LOG_INFO("Plane height: %.3f\n", planeHeight);
return true;
} catch (const std::exception& e) {
LOG_ERROR("Exception in LoadLevelingResults: %s\n", e.what());
return false;
}
}
// ============ PLC Modbus 相关实现 ============
int StatorPositionPresenter::InitPLCModbus()
{
LOG_INFO("Initializing PLC Modbus client\n");
// 从配置获取 PLC 和机械臂的 IP 地址
ConfigResult configResult = m_pConfigManager->GetConfigResult();
// 从配置文件获取 PLC 配置
std::string plcIP = configResult.plcRobotServerConfig.plcServerIp;
int plcPort = configResult.plcRobotServerConfig.plcServerPort;
// 检查是否配置了 PLC 通信（如果为空则跳过初始化）
if (plcIP.empty()) {
LOG_INFO("PLC IP not configured, skipping PLC Modbus initialization\n");
return SUCCESS;
}
// 创建 PLCModbusClient 实例
m_pPLCModbusClient = new PLCModbusClient();
// 设置回调函数
m_pPLCModbusClient->SetPhotoTriggerCallback([this](int cameraIndex) {
OnPLCPhotoRequested(cameraIndex);
});
m_pPLCModbusClient->SetConnectionStateCallback([this](bool plcConnected) {
m_bPLCConnected = plcConnected;
LOG_INFO("PLC connection state changed: PLC=%s\n",
plcConnected ? "connected" : "disconnected");
// 通知 UI 更新连接状态（PLC连接状态关联到机械臂状态指示器）
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) {
pStatus->OnRobotConnectionChanged(plcConnected);
std::string statusMsg = std::string("PLC:") + (plcConnected ? "已连接" : "断开");
pStatus->OnStatusUpdate(statusMsg);
}
});
m_pPLCModbusClient->SetErrorCallback([this](const std::string& errorMsg) {
LOG_ERROR("PLC Modbus error: %s\n", errorMsg.c_str());
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) {
pStatus->OnStatusUpdate(std::string("PLC错误: ") + errorMsg);
}
});
// 设置重连回调，通知用户正在尝试重连
m_pPLCModbusClient->SetReconnectingCallback([this](const std::string& device, int attempt) {
LOG_INFO("Attempting to reconnect %s (attempt %d)\n", device.c_str(), attempt);
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) {
std::string statusMsg = device + "正在重连...";
pStatus->OnStatusUpdate(statusMsg);
}
});
// 构建寄存器地址配置
PLCModbusClient::RegisterConfig regConfig;
regConfig.addrTrigger = configResult.plcRobotServerConfig.registerConfig.addrTrigger;
regConfig.addrWorkStatus = configResult.plcRobotServerConfig.registerConfig.addrWorkStatus;
regConfig.addrCoordDataStart = configResult.plcRobotServerConfig.registerConfig.addrCoordDataStart;
regConfig.byteOrder = configResult.plcRobotServerConfig.byteOrder;
// 初始化连接
bool initResult = m_pPLCModbusClient->Initialize(plcIP, plcPort, regConfig);
if (!initResult) {
LOG_ERROR("Failed to initialize PLC Modbus client\n");
return ERR_CODE(DEV_OPEN_ERR);
}
// 启动轮询（100ms 间隔）
m_pPLCModbusClient->StartPolling(100);
LOG_INFO("PLC Modbus client initialized successfully\n");
return SUCCESS;
}
void StatorPositionPresenter::OnPLCPhotoRequested(int cameraIndex)
{
LOG_INFO("PLC trigger received for camera %d\n", cameraIndex);
// 暂停触发寄存器轮询（检测期间不再读取地址0）
if (m_pPLCModbusClient) {
m_pPLCModbusClient->PauseTriggerPolling();
}
// 清除触发寄存器（地址0 = 0）
if (m_pPLCModbusClient) {
m_pPLCModbusClient->ClearTrigger();
}
// 写工作状态 = 正在工作（地址1 = 0）
if (m_pPLCModbusClient) {
m_pPLCModbusClient->WriteWorkStatus(PLCModbusClient::WORK_STATUS_WORKING);
}
// 设置当前相机索引
SetDefaultCameraIndex(cameraIndex);
// 触发检测（使用基类的 StartDetection 方法）
int nRet = StartDetection(cameraIndex, false); // 非自动模式
if (nRet != SUCCESS) {
LOG_ERROR("Failed to trigger detection from PLC request, error: %d\n", nRet);
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) {
pStatus->OnStatusUpdate("PLC触发检测失败");
}
// 检测失败时写错误状态并恢复轮询
if (m_pPLCModbusClient) {
m_pPLCModbusClient->WriteWorkStatus(PLCModbusClient::WORK_STATUS_ERROR);
m_pPLCModbusClient->ResumeTriggerPolling();
}
}
}
void StatorPositionPresenter::WriteDetectionResultToAllChannels(const StatorPositionDetectionResult& result)
{
// 获取姿态输出顺序和字节序配置
ConfigResult configResult = m_pConfigManager->GetConfigResult();
int poseOutputOrder = configResult.plcRobotServerConfig.poseOutputOrder;
int byteOrder = configResult.plcRobotServerConfig.byteOrder;
LOG_INFO("Using pose output order: %d\n", poseOutputOrder);
// 检查检测结果（errorCode == 0 表示成功）
if (result.errorCode != 0 || result.positions.empty()) {
LOG_WARNING("Invalid detection result or no positions, writing error status to all channels\n");
// 通道1: PLC Modbus Client - 写错误状态
if (m_pPLCModbusClient) {
if (m_pPLCModbusClient->IsPLCConnected()) {
m_pPLCModbusClient->WriteWorkStatus(PLCModbusClient::WORK_STATUS_ERROR);
}
m_pPLCModbusClient->ResumeTriggerPolling();
}
// 通道2: Modbus Server - 写错误状态
if (IsModbusServerRunning()) {
uint16_t errorStatus = PLCModbusClient::WORK_STATUS_ERROR;
WriteModbusRegisters(configResult.plcRobotServerConfig.registerConfig.addrWorkStatus, &errorStatus, 1);
}
return;
}
// 准备坐标数据
const auto& pos = result.positions[0];
PLCModbusClient::CoordinateData coord;
coord.x = static_cast<float>(pos.x);
coord.y = static_cast<float>(pos.y);
coord.z = static_cast<float>(pos.z);
// 根据姿态输出顺序配置调整输出
switch (poseOutputOrder) {
case POSE_ORDER_RPY: // Roll, Pitch, Yaw（默认）
coord.roll = static_cast<float>(pos.roll);
coord.pitch = static_cast<float>(pos.pitch);
coord.yaw = static_cast<float>(pos.yaw);
break;
case POSE_ORDER_RYP: // Roll, Yaw, Pitch
coord.roll = static_cast<float>(pos.roll);
coord.pitch = static_cast<float>(pos.yaw);
coord.yaw = static_cast<float>(pos.pitch);
break;
case POSE_ORDER_PRY: // Pitch, Roll, Yaw
coord.roll = static_cast<float>(pos.pitch);
coord.pitch = static_cast<float>(pos.roll);
coord.yaw = static_cast<float>(pos.yaw);
break;
case POSE_ORDER_PYR: // Pitch, Yaw, Roll
coord.roll = static_cast<float>(pos.pitch);
coord.pitch = static_cast<float>(pos.yaw);
coord.yaw = static_cast<float>(pos.roll);
break;
case POSE_ORDER_YRP: // Yaw, Roll, Pitch
coord.roll = static_cast<float>(pos.yaw);
coord.pitch = static_cast<float>(pos.roll);
coord.yaw = static_cast<float>(pos.pitch);
break;
case POSE_ORDER_YPR: // Yaw, Pitch, Roll
coord.roll = static_cast<float>(pos.yaw);
coord.pitch = static_cast<float>(pos.pitch);
coord.yaw = static_cast<float>(pos.roll);
break;
default: // 默认 RPY
coord.roll = static_cast<float>(pos.roll);
coord.pitch = static_cast<float>(pos.pitch);
coord.yaw = static_cast<float>(pos.yaw);
break;
}
LOG_INFO("Workpiece: X=%.5f, Y=%.5f, Z=%.5f, R=%.5f, P=%.5f, Y=%.5f (order=%d)\n",
coord.x, coord.y, coord.z, coord.roll, coord.pitch, coord.yaw, poseOutputOrder);
// ========== 通道1: PLC Modbus Client ==========
if (m_pPLCModbusClient && m_pPLCModbusClient->IsPLCConnected()) {
LOG_INFO("Sending workpiece position to PLC (Channel 1: Client)\n");
bool sendResult = m_pPLCModbusClient->SendCoordinateToPLC(coord);
if (sendResult) {
m_pPLCModbusClient->WriteWorkStatus(PLCModbusClient::WORK_STATUS_COMPLETE_OK);
} else {
LOG_ERROR("Failed to send coordinate data to PLC\n");
m_pPLCModbusClient->WriteWorkStatus(PLCModbusClient::WORK_STATUS_ERROR);
}
m_pPLCModbusClient->ResumeTriggerPolling();
} else if (m_pPLCModbusClient) {
LOG_WARNING("PLC not connected, skipping Channel 1\n");
m_pPLCModbusClient->ResumeTriggerPolling();
}
// ========== 通道2: Modbus Server ==========
if (IsModbusServerRunning()) {
LOG_INFO("Sending workpiece position to Modbus Server (Channel 2: Server)\n");
int addrCoordStart = configResult.plcRobotServerConfig.registerConfig.addrCoordDataStart;
int addrWorkStatus = configResult.plcRobotServerConfig.registerConfig.addrWorkStatus;
// 构建坐标寄存器数据
uint16_t regs[PLCModbusClient::REGS_PER_POINT];
floatToRegisters(coord.x, regs[0], regs[1], byteOrder);
floatToRegisters(coord.y, regs[2], regs[3], byteOrder);
floatToRegisters(coord.z, regs[4], regs[5], byteOrder);
floatToRegisters(coord.roll, regs[6], regs[7], byteOrder);
floatToRegisters(coord.pitch, regs[8], regs[9], byteOrder);
floatToRegisters(coord.yaw, regs[10], regs[11], byteOrder);
// 写坐标数据
int writeResult = WriteModbusRegisters(addrCoordStart, regs, PLCModbusClient::REGS_PER_POINT);
if (writeResult == 0) {
// 写完成状态
uint16_t completeStatus = PLCModbusClient::WORK_STATUS_COMPLETE_OK;
WriteModbusRegisters(addrWorkStatus, &completeStatus, 1);
} else {
LOG_ERROR("Failed to write coordinate data to Modbus Server\n");
uint16_t errorStatus = PLCModbusClient::WORK_STATUS_ERROR;
WriteModbusRegisters(addrWorkStatus, &errorStatus, 1);
}
}
LOG_INFO("Detection result written to all channels\n");
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) {
pStatus->OnStatusUpdate("已发送工件坐标到PLC");
}
}
// ============ Modbus Server 回调实现 ============
void StatorPositionPresenter::OnModbusWriteCallback(uint16_t startAddress, const uint16_t* data, uint16_t count)
{
if (!data || count == 0) return;
// 获取触发寄存器地址
ConfigResult configResult = m_pConfigManager->GetConfigResult();
int addrTrigger = configResult.plcRobotServerConfig.registerConfig.addrTrigger;
// 检测地址0（触发寄存器）的写入
if (startAddress <= addrTrigger && (startAddress + count) > addrTrigger) {
int offset = addrTrigger - startAddress;
uint16_t triggerValue = data[offset];
bool currentState = (triggerValue == 1);
// 边沿检测：只在 0→1 变化时触发
if (currentState && !m_bModbusServerTriggerLastState) {
LOG_INFO("Modbus Server trigger detected (addr %d = 1)\n", addrTrigger);
// 通过 QueuedConnection 切换到主线程执行
QMetaObject::invokeMethod(this, [this]() {
OnModbusServerTrigger(1);
}, Qt::QueuedConnection);
}
m_bModbusServerTriggerLastState = currentState;
}
}
void StatorPositionPresenter::OnModbusServerStatusChanged(bool isConnected)
{
m_bModbusServerConnected = isConnected;
LOG_INFO("Modbus Server client connection state changed: %s\n", isConnected ? "connected" : "disconnected");
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) {
std::string statusMsg = std::string("Modbus Server:") + (isConnected ? "客户端已连接" : "客户端已断开");
pStatus->OnStatusUpdate(statusMsg);
}
}
void StatorPositionPresenter::OnModbusServerTrigger(int cameraIndex)
{
LOG_INFO("Modbus Server trigger received for camera %d\n", cameraIndex);
// 获取配置中的地址
ConfigResult configResult = m_pConfigManager->GetConfigResult();
int addrTrigger = configResult.plcRobotServerConfig.registerConfig.addrTrigger;
int addrWorkStatus = configResult.plcRobotServerConfig.registerConfig.addrWorkStatus;
// 清除服务端地址0（触发寄存器 = 0）
uint16_t zero = 0;
WriteModbusRegisters(addrTrigger, &zero, 1);
// 写地址1 = 0（工作中）
uint16_t workingStatus = PLCModbusClient::WORK_STATUS_WORKING;
WriteModbusRegisters(addrWorkStatus, &workingStatus, 1);
// 检查当前工作状态，若正在工作则忽略
if (GetCurrentWorkStatus() == WorkStatus::Working) {
LOG_WARNING("Already working, ignoring Modbus Server trigger\n");
// 写错误状态
uint16_t errorStatus = PLCModbusClient::WORK_STATUS_ERROR;
WriteModbusRegisters(addrWorkStatus, &errorStatus, 1);
return;
}
// 设置当前相机索引
SetDefaultCameraIndex(cameraIndex);
// 触发检测
int nRet = StartDetection(cameraIndex, false);
if (nRet != SUCCESS) {
LOG_ERROR("Failed to trigger detection from Modbus Server, error: %d\n", nRet);
if (auto pStatus = GetStatusCallback<IYStatorPositionStatus>()) {
pStatus->OnStatusUpdate("Modbus Server触发检测失败");
}
// 检测失败，写错误状态
uint16_t errorStatus = PLCModbusClient::WORK_STATUS_ERROR;
WriteModbusRegisters(addrWorkStatus, &errorStatus, 1);
}
}
void StatorPositionPresenter::floatToRegisters(float value, uint16_t& high, uint16_t& low, int byteOrder)
{
uint32_t bits;
std::memcpy(&bits, &value, sizeof(float));
if (byteOrder == BYTE_ORDER_LITTLE_ENDIAN) {
// 小端序 (DCBA) - 低字节在前
low = static_cast<uint16_t>((bits >> 16) & 0xFFFF);
high = static_cast<uint16_t>(bits & 0xFFFF);
} else {
// 大端序 (ABCD) - 高字节在前（默认）
high = static_cast<uint16_t>((bits >> 16) & 0xFFFF);
low = static_cast<uint16_t>(bits & 0xFFFF);
}
}
Reference in New Issue Copy Permalink