899 lines
34 KiB
C++
899 lines
34 KiB
C++
#include "WheelMeasurePresenter.h"
|
||
#include "PathManager.h"
|
||
#include "VrLog.h"
|
||
#include "VrError.h"
|
||
|
||
// wheelArchHeigthMeasure SDK
|
||
#include "wheelArchHeigthMeasure_Export.h"
|
||
|
||
// PointCloudImageUtils
|
||
#include "PointCloudImageUtils.h"
|
||
|
||
#include <QThread>
|
||
#include <QApplication>
|
||
#include <QCoreApplication>
|
||
#include <QDateTime>
|
||
#include <cmath>
|
||
|
||
WheelMeasurePresenter::WheelMeasurePresenter(QObject* parent)
|
||
: BasePresenter(parent)
|
||
{
|
||
// 创建配置接口
|
||
IVrWheelMeasureConfig::CreateInstance(&m_config);
|
||
LOG_INFO("ALGO_VERSION: %s \n", wd_wheelArchHeigthMeasureVersion());
|
||
}
|
||
|
||
WheelMeasurePresenter::~WheelMeasurePresenter()
|
||
{
|
||
// 清除状态回调，防止后续回调访问已销毁对象
|
||
m_statusUpdate = nullptr;
|
||
|
||
// 停止顺序检测
|
||
m_stopSequentialRequested = true;
|
||
m_sequentialDetecting = false;
|
||
|
||
// 处理待处理的 Qt 事件，确保 QueuedConnection 的回调不会访问已销毁对象
|
||
QCoreApplication::processEvents();
|
||
|
||
if (m_config) {
|
||
delete m_config;
|
||
m_config = nullptr;
|
||
}
|
||
}
|
||
|
||
int WheelMeasurePresenter::InitApp()
|
||
{
|
||
LOG_INFO("WheelMeasurePresenter::InitApp() called\n");
|
||
|
||
SetWorkStatus(WorkStatus::InitIng);
|
||
|
||
// 加载配置
|
||
QString configPath = PathManager::GetInstance().GetConfigFilePath();
|
||
if (!initializeConfig(configPath)) {
|
||
LOG_ERROR("Failed to initialize config from: %s\n", configPath.toStdString().c_str());
|
||
if (m_statusUpdate) {
|
||
m_statusUpdate->OnErrorOccurred("配置文件加载失败");
|
||
}
|
||
return ERR_CODE(DEV_CONFIG_ERR);
|
||
}
|
||
|
||
// 初始化相机
|
||
if (!initializeCameras()) {
|
||
LOG_ERROR("Failed to initialize cameras\n");
|
||
if (m_statusUpdate) {
|
||
m_statusUpdate->OnErrorOccurred("相机初始化失败");
|
||
}
|
||
}
|
||
|
||
LOG_INFO("WheelMeasurePresenter::InitApp() completed\n");
|
||
return SUCCESS;
|
||
}
|
||
|
||
int WheelMeasurePresenter::InitAlgoParams()
|
||
{
|
||
LOG_DEBUG("Initializing algorithm parameters\n");
|
||
// 算法参数已在配置加载时初始化
|
||
return SUCCESS;
|
||
}
|
||
|
||
int WheelMeasurePresenter::ProcessAlgoDetection(std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& detectionDataCache)
|
||
{
|
||
LOG_INFO("ProcessAlgoDetection called, data lines: %zu\n", detectionDataCache.size());
|
||
|
||
// 处理检测数据
|
||
processScanData(detectionDataCache);
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
void WheelMeasurePresenter::OnCameraStatusChanged(int cameraIndex, bool isConnected)
|
||
{
|
||
LOG_INFO("Camera[%d] status changed: %s\n", cameraIndex, isConnected ? "connected" : "disconnected");
|
||
|
||
if (!m_statusUpdate) return;
|
||
|
||
// 从配置中获取相机名称（cameraIndex从1开始）
|
||
QString cameraName;
|
||
int enabledIndex = 0;
|
||
for (const auto& cameraConfig : m_configResult.cameras) {
|
||
if (cameraConfig.enabled) {
|
||
enabledIndex++;
|
||
if (enabledIndex == cameraIndex) {
|
||
cameraName = QString::fromStdString(cameraConfig.name);
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
if (cameraName.isEmpty()) {
|
||
cameraName = QString("Camera%1").arg(cameraIndex);
|
||
}
|
||
|
||
// 切换到主线程更新UI
|
||
QMetaObject::invokeMethod(this, [this, cameraName, isConnected]() {
|
||
if (!m_statusUpdate) return;
|
||
if (isConnected) {
|
||
m_statusUpdate->OnCameraConnected(cameraName);
|
||
m_statusUpdate->OnDeviceStatusChanged(cameraName, static_cast<int>(DeviceStatus::Online));
|
||
} else {
|
||
m_statusUpdate->OnCameraDisconnected(cameraName);
|
||
m_statusUpdate->OnDeviceStatusChanged(cameraName, static_cast<int>(DeviceStatus::Offline));
|
||
}
|
||
}, Qt::QueuedConnection);
|
||
}
|
||
|
||
void WheelMeasurePresenter::OnWorkStatusChanged(WorkStatus status)
|
||
{
|
||
// 写入Modbus工作状态 (地址1)
|
||
uint16_t statusValue = 0;
|
||
switch (status) {
|
||
case WorkStatus::InitIng: statusValue = 1; break;
|
||
case WorkStatus::Ready: statusValue = 2; break;
|
||
case WorkStatus::Working: statusValue = 3; break;
|
||
case WorkStatus::Detecting: statusValue = 3; break;
|
||
case WorkStatus::Completed: statusValue = 4; break;
|
||
case WorkStatus::Error: statusValue = 5; break;
|
||
default: statusValue = 0; break;
|
||
}
|
||
WriteModbusRegisters(1, &statusValue, 1);
|
||
|
||
// 切换到主线程更新UI
|
||
QMetaObject::invokeMethod(this, [this, status]() {
|
||
if (m_statusUpdate) {
|
||
m_statusUpdate->OnWorkStatusChanged(status);
|
||
}
|
||
}, Qt::QueuedConnection);
|
||
}
|
||
|
||
void WheelMeasurePresenter::OnCameraCountChanged(int count)
|
||
{
|
||
if (!m_statusUpdate) return;
|
||
|
||
QStringList cameraNames;
|
||
for (const auto& cameraConfig : m_configResult.cameras) {
|
||
if (cameraConfig.enabled) {
|
||
cameraNames.append(QString::fromStdString(cameraConfig.name));
|
||
}
|
||
}
|
||
LOG_INFO("OnCameraCountChanged: count=%d, cameraNames=%d\n", count, cameraNames.size());
|
||
|
||
// 切换到主线程更新UI
|
||
QMetaObject::invokeMethod(this, [this, cameraNames]() {
|
||
if (m_statusUpdate) {
|
||
m_statusUpdate->OnNeedShowImageCount(cameraNames);
|
||
}
|
||
}, Qt::QueuedConnection);
|
||
}
|
||
|
||
void WheelMeasurePresenter::OnStatusUpdate(const std::string& statusMessage)
|
||
{
|
||
if (!m_statusUpdate) return;
|
||
|
||
QString msg = QString::fromStdString(statusMessage);
|
||
// 切换到主线程更新UI
|
||
QMetaObject::invokeMethod(this, [this, msg]() {
|
||
if (m_statusUpdate) {
|
||
m_statusUpdate->OnStatusUpdate(msg);
|
||
}
|
||
}, Qt::QueuedConnection);
|
||
}
|
||
|
||
bool WheelMeasurePresenter::initializeConfig(const QString& configPath)
|
||
{
|
||
if (!m_config) {
|
||
LOG_ERROR("Config interface is null\n");
|
||
return false;
|
||
}
|
||
|
||
m_configResult = m_config->LoadConfig(configPath.toStdString());
|
||
|
||
// 设置调试参数到基类
|
||
SetDebugParam(m_configResult.debugParam);
|
||
|
||
// 设置配置改变通知
|
||
m_config->SetConfigChangeNotify(this);
|
||
|
||
LOG_INFO("Config loaded successfully, cameras: %zu\n", m_configResult.cameras.size());
|
||
return true;
|
||
}
|
||
|
||
bool WheelMeasurePresenter::initializeCameras()
|
||
{
|
||
// 转换相机配置为DeviceInfo列表
|
||
std::vector<DeviceInfo> cameraList;
|
||
for (const auto& cameraConfig : m_configResult.cameras) {
|
||
if (!cameraConfig.enabled) {
|
||
LOG_INFO("Camera %s is disabled, skipping\n", cameraConfig.name.c_str());
|
||
continue;
|
||
}
|
||
|
||
DeviceInfo deviceInfo;
|
||
deviceInfo.index = cameraConfig.cameraIndex;
|
||
deviceInfo.name = cameraConfig.name;
|
||
deviceInfo.ip = cameraConfig.cameraIP;
|
||
cameraList.push_back(deviceInfo);
|
||
}
|
||
|
||
// 调用基类InitCamera进行相机初始化（bRGB=false, bSwing=true）
|
||
int result = InitCamera(cameraList, false, true);
|
||
|
||
LOG_INFO("Camera initialization completed. Connected cameras: %zu\n", m_vrEyeDeviceList.size());
|
||
|
||
return result == SUCCESS;
|
||
}
|
||
|
||
QStringList WheelMeasurePresenter::getCameraNames() const
|
||
{
|
||
QStringList names;
|
||
for (const auto& camera : m_vrEyeDeviceList) {
|
||
names.append(QString::fromStdString(camera.first));
|
||
}
|
||
return names;
|
||
}
|
||
|
||
void WheelMeasurePresenter::ResetDetect(int cameraIndex)
|
||
{
|
||
StopDetection();
|
||
|
||
// 设置当前相机索引（从0-based转换为1-based）
|
||
m_currentCameraIndex = cameraIndex + 1;
|
||
LOG_INFO("ResetDetect: cameraIndex=%d, m_currentCameraIndex=%d\n", cameraIndex, m_currentCameraIndex);
|
||
|
||
// 设置工作状态为检测中
|
||
SetWorkStatus(WorkStatus::Working);
|
||
|
||
// 清空数据
|
||
ClearDetectionDataCache();
|
||
|
||
// 注意：不调用 OnClearMeasureData()，因为 mainwindow 中的 onDeviceClicked
|
||
// 已经调用了 clearDeviceResult(deviceName) 来清除指定设备的结果
|
||
|
||
// 重新开始检测（BasePresenter::StartDetection 期望 1-based 索引）
|
||
StartDetection(m_currentCameraIndex);
|
||
}
|
||
|
||
void WheelMeasurePresenter::StartAllDetection()
|
||
{
|
||
LOG_INFO("Starting sequential detection for all cameras\n");
|
||
|
||
// 计算启用的相机数量
|
||
m_sequentialTotalCount = 0;
|
||
for (const auto& cameraConfig : m_configResult.cameras) {
|
||
if (cameraConfig.enabled) {
|
||
m_sequentialTotalCount++;
|
||
}
|
||
}
|
||
|
||
if (m_sequentialTotalCount == 0) {
|
||
LOG_WARNING("No enabled cameras to detect\n");
|
||
if (m_statusUpdate) {
|
||
m_statusUpdate->OnStatusUpdate(QString("没有可用的相机设备"));
|
||
}
|
||
return;
|
||
}
|
||
|
||
// 初始化顺序检测状态
|
||
m_sequentialDetecting = true;
|
||
m_stopSequentialRequested = false;
|
||
m_sequentialCurrentIndex = 0;
|
||
|
||
// 清空之前的检测结果
|
||
if (m_statusUpdate) {
|
||
m_statusUpdate->OnClearMeasureData();
|
||
}
|
||
|
||
LOG_INFO("Sequential detection started, total cameras: %d\n", m_sequentialTotalCount);
|
||
|
||
// 开始检测第一个设备
|
||
continueSequentialDetection();
|
||
}
|
||
|
||
void WheelMeasurePresenter::StopAllDetection()
|
||
{
|
||
LOG_INFO("Stop sequential detection requested\n");
|
||
|
||
if (m_sequentialDetecting) {
|
||
// 设置停止标志，等待当前设备检测完成后停止
|
||
m_stopSequentialRequested = true;
|
||
if (m_statusUpdate) {
|
||
m_statusUpdate->OnStatusUpdate(QString("正在完成当前设备检测，之后将停止..."));
|
||
}
|
||
} else {
|
||
// 如果不是顺序检测模式，直接停止
|
||
StopDetection();
|
||
}
|
||
}
|
||
|
||
void WheelMeasurePresenter::continueSequentialDetection()
|
||
{
|
||
// 检查是否应该停止
|
||
if (m_stopSequentialRequested) {
|
||
LOG_INFO("Sequential detection stopped by user request\n");
|
||
m_sequentialDetecting = false;
|
||
m_stopSequentialRequested = false;
|
||
if (m_statusUpdate) {
|
||
m_statusUpdate->OnStatusUpdate(QString("顺序检测已停止"));
|
||
}
|
||
SetWorkStatus(WorkStatus::Ready);
|
||
return;
|
||
}
|
||
|
||
// 检查是否还有设备需要检测
|
||
if (m_sequentialCurrentIndex >= m_sequentialTotalCount) {
|
||
LOG_INFO("Sequential detection completed, all %d cameras processed\n", m_sequentialTotalCount);
|
||
m_sequentialDetecting = false;
|
||
if (m_statusUpdate) {
|
||
m_statusUpdate->OnStatusUpdate(QString("所有 %1 个设备检测完成").arg(m_sequentialTotalCount));
|
||
}
|
||
SetWorkStatus(WorkStatus::Completed);
|
||
return;
|
||
}
|
||
|
||
// 获取当前要检测的相机索引（1-based）
|
||
int cameraIndex = m_sequentialCurrentIndex + 1;
|
||
m_currentCameraIndex = cameraIndex;
|
||
|
||
// 获取相机名称
|
||
QString cameraName;
|
||
int enabledIndex = 0;
|
||
for (const auto& cameraConfig : m_configResult.cameras) {
|
||
if (cameraConfig.enabled) {
|
||
enabledIndex++;
|
||
if (enabledIndex == cameraIndex) {
|
||
cameraName = QString::fromStdString(cameraConfig.name);
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
LOG_INFO("Starting detection for camera %d/%d: %s\n",
|
||
m_sequentialCurrentIndex + 1, m_sequentialTotalCount,
|
||
cameraName.toStdString().c_str());
|
||
|
||
if (m_statusUpdate) {
|
||
m_statusUpdate->OnStatusUpdate(QString("正在检测设备 %1/%2: %3")
|
||
.arg(m_sequentialCurrentIndex + 1)
|
||
.arg(m_sequentialTotalCount)
|
||
.arg(cameraName));
|
||
}
|
||
|
||
// 清空数据缓存
|
||
ClearDetectionDataCache();
|
||
|
||
// 开始检测当前相机
|
||
StartDetection(cameraIndex);
|
||
}
|
||
|
||
void WheelMeasurePresenter::OnConfigChanged(const WheelMeasureConfigResult& configResult)
|
||
{
|
||
LOG_INFO("Config changed notification received\n");
|
||
m_configResult = configResult;
|
||
|
||
// 更新基类调试参数
|
||
SetDebugParam(m_configResult.debugParam);
|
||
|
||
emit configUpdated();
|
||
}
|
||
|
||
void WheelMeasurePresenter::processScanData(std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& detectionDataCache)
|
||
{
|
||
LOG_INFO("Processing scan data, lines: %zu\n", detectionDataCache.size());
|
||
|
||
// 打印扫描数据统计信息
|
||
int totalPoints = 0;
|
||
for (const auto& linePair : detectionDataCache) {
|
||
totalPoints += linePair.second.nPointCount;
|
||
}
|
||
LOG_INFO("Scan data statistics: %zu lines, %d total points\n", detectionDataCache.size(), totalPoints);
|
||
|
||
// 打印调平参数
|
||
LOG_INFO("========== PlaneCalib Parameters ==========\n");
|
||
if (!m_configResult.planeCalibParams.empty()) {
|
||
for (const auto& calibParam : m_configResult.planeCalibParams) {
|
||
LOG_INFO("Camera[%d] %s: isCalibrated=%d, planeHeight=%.2f\n",
|
||
calibParam.cameraIndex, calibParam.cameraName.c_str(),
|
||
calibParam.isCalibrated ? 1 : 0, calibParam.planeHeight);
|
||
LOG_INFO(" planeCalib: [%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f]\n",
|
||
calibParam.planeCalib[0], calibParam.planeCalib[1], calibParam.planeCalib[2],
|
||
calibParam.planeCalib[3], calibParam.planeCalib[4], calibParam.planeCalib[5],
|
||
calibParam.planeCalib[6], calibParam.planeCalib[7], calibParam.planeCalib[8]);
|
||
LOG_INFO(" invRMatrix: [%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f]\n",
|
||
calibParam.invRMatrix[0], calibParam.invRMatrix[1], calibParam.invRMatrix[2],
|
||
calibParam.invRMatrix[3], calibParam.invRMatrix[4], calibParam.invRMatrix[5],
|
||
calibParam.invRMatrix[6], calibParam.invRMatrix[7], calibParam.invRMatrix[8]);
|
||
}
|
||
} else {
|
||
LOG_INFO("No PlaneCalib parameters loaded!\n");
|
||
}
|
||
|
||
// 打印算法参数
|
||
LOG_INFO("========== Algorithm Parameters ==========\n");
|
||
LOG_INFO("CornerParam: minEndingGap=%.2f, minEndingGap_z=%.2f, scale=%.2f, cornerTh=%.2f, jumpCornerTh_1=%.2f, jumpCornerTh_2=%.2f\n",
|
||
m_configResult.algorithmParams.cornerParam.minEndingGap,
|
||
m_configResult.algorithmParams.cornerParam.minEndingGap_z,
|
||
m_configResult.algorithmParams.cornerParam.scale,
|
||
m_configResult.algorithmParams.cornerParam.cornerTh,
|
||
m_configResult.algorithmParams.cornerParam.jumpCornerTh_1,
|
||
m_configResult.algorithmParams.cornerParam.jumpCornerTh_2);
|
||
|
||
LOG_INFO("LineSegParam: segGapTh_y=%.2f, segGapTh_z=%.2f, maxDist=%.2f\n",
|
||
m_configResult.algorithmParams.lineSegParam.segGapTh_y,
|
||
m_configResult.algorithmParams.lineSegParam.segGapTh_z,
|
||
m_configResult.algorithmParams.lineSegParam.maxDist);
|
||
|
||
LOG_INFO("OutlierFilterParam: continuityTh=%.2f, outlierTh=%.2f\n",
|
||
m_configResult.algorithmParams.filterParam.continuityTh,
|
||
m_configResult.algorithmParams.filterParam.outlierTh);
|
||
|
||
LOG_INFO("TreeGrowParam: yDeviation_max=%.2f, zDeviation_max=%.2f, maxLineSkipNum=%d, maxSkipDistance=%.2f, minLTypeTreeLen=%.2f, minVTypeTreeLen=%.2f\n",
|
||
m_configResult.algorithmParams.growParam.yDeviation_max,
|
||
m_configResult.algorithmParams.growParam.zDeviation_max,
|
||
m_configResult.algorithmParams.growParam.maxLineSkipNum,
|
||
m_configResult.algorithmParams.growParam.maxSkipDistance,
|
||
m_configResult.algorithmParams.growParam.minLTypeTreeLen,
|
||
m_configResult.algorithmParams.growParam.minVTypeTreeLen);
|
||
|
||
LOG_INFO("DebugParam: enableDebug=%d, saveDebugImage=%d, printDetailLog=%d\n",
|
||
m_configResult.debugParam.enableDebug,
|
||
m_configResult.debugParam.saveDebugImage,
|
||
m_configResult.debugParam.printDetailLog);
|
||
|
||
LOG_INFO("============================================\n");
|
||
|
||
// ========== 调用 wheelArchHeigthMeasure SDK ==========
|
||
|
||
// 1. 转换检测数据为SDK所需格式
|
||
std::vector<std::vector<SVzNL3DPosition>> scanLines;
|
||
size_t validPointCount = 0;
|
||
int convertResult = m_dataLoader.ConvertToSVzNL3DPosition(detectionDataCache, scanLines, &validPointCount);
|
||
|
||
// 检查数据有效性
|
||
if (convertResult != SUCCESS || scanLines.empty()) {
|
||
LOG_WARNING("Failed to convert data to XYZ format or no XYZ data available\n");
|
||
return;
|
||
}
|
||
|
||
// 检查线束数量和有效点数量
|
||
const size_t MIN_SCAN_LINES = 200;
|
||
const size_t MIN_VALID_POINTS = 1000;
|
||
|
||
if (scanLines.size() < MIN_SCAN_LINES || validPointCount < MIN_VALID_POINTS) {
|
||
LOG_ERROR("Insufficient data for detection: scan lines=%zu (min=%zu), valid points=%zu (min=%zu)\n",
|
||
scanLines.size(), MIN_SCAN_LINES, validPointCount, MIN_VALID_POINTS);
|
||
|
||
if (m_statusUpdate) {
|
||
QString errorMsg = QString("数据不足: 线束=%1(最少%2), 有效点=%3(最少%4)")
|
||
.arg(scanLines.size()).arg(MIN_SCAN_LINES)
|
||
.arg(validPointCount).arg(MIN_VALID_POINTS);
|
||
m_statusUpdate->OnStatusUpdate(errorMsg);
|
||
m_statusUpdate->OnErrorOccurred(errorMsg);
|
||
}
|
||
|
||
SetWorkStatus(WorkStatus::Error);
|
||
|
||
// 如果正在进行顺序检测，继续检测下一个设备
|
||
if (m_sequentialDetecting) {
|
||
m_sequentialCurrentIndex++;
|
||
QMetaObject::invokeMethod(this, [this]() {
|
||
continueSequentialDetection();
|
||
}, Qt::QueuedConnection);
|
||
}
|
||
return;
|
||
}
|
||
|
||
LOG_INFO("Data validation passed: scan lines=%zu, valid points=%zu\n", scanLines.size(), validPointCount);
|
||
|
||
// 2. 准备算法参数
|
||
SSG_cornerParam cornerParam;
|
||
cornerParam.minEndingGap = m_configResult.algorithmParams.cornerParam.minEndingGap;
|
||
cornerParam.minEndingGap_z = m_configResult.algorithmParams.cornerParam.minEndingGap_z;
|
||
cornerParam.scale = m_configResult.algorithmParams.cornerParam.scale;
|
||
cornerParam.cornerTh = m_configResult.algorithmParams.cornerParam.cornerTh;
|
||
cornerParam.jumpCornerTh_1 = m_configResult.algorithmParams.cornerParam.jumpCornerTh_1;
|
||
cornerParam.jumpCornerTh_2 = m_configResult.algorithmParams.cornerParam.jumpCornerTh_2;
|
||
|
||
SSG_lineSegParam lineSegParam;
|
||
lineSegParam.segGapTh_y = m_configResult.algorithmParams.lineSegParam.segGapTh_y;
|
||
lineSegParam.segGapTh_z = m_configResult.algorithmParams.lineSegParam.segGapTh_z;
|
||
lineSegParam.maxDist = m_configResult.algorithmParams.lineSegParam.maxDist;
|
||
|
||
SSG_outlierFilterParam filterParam;
|
||
filterParam.continuityTh = m_configResult.algorithmParams.filterParam.continuityTh;
|
||
filterParam.outlierTh = m_configResult.algorithmParams.filterParam.outlierTh;
|
||
|
||
SSG_treeGrowParam growParam;
|
||
growParam.yDeviation_max = m_configResult.algorithmParams.growParam.yDeviation_max;
|
||
growParam.zDeviation_max = m_configResult.algorithmParams.growParam.zDeviation_max;
|
||
growParam.maxLineSkipNum = m_configResult.algorithmParams.growParam.maxLineSkipNum;
|
||
growParam.maxSkipDistance = m_configResult.algorithmParams.growParam.maxSkipDistance;
|
||
growParam.minLTypeTreeLen = m_configResult.algorithmParams.growParam.minLTypeTreeLen;
|
||
growParam.minVTypeTreeLen = m_configResult.algorithmParams.growParam.minVTypeTreeLen;
|
||
|
||
// 3. 准备调平参数（使用当前相机的调平参数）
|
||
SSG_planeCalibPara groundCalibPara;
|
||
memset(&groundCalibPara, 0, sizeof(groundCalibPara));
|
||
|
||
// 初始化为单位矩阵
|
||
groundCalibPara.planeCalib[0] = 1.0;
|
||
groundCalibPara.planeCalib[4] = 1.0;
|
||
groundCalibPara.planeCalib[8] = 1.0;
|
||
groundCalibPara.invRMatrix[0] = 1.0;
|
||
groundCalibPara.invRMatrix[4] = 1.0;
|
||
groundCalibPara.invRMatrix[8] = 1.0;
|
||
groundCalibPara.planeHeight = 0.0;
|
||
|
||
// 查找当前相机的调平参数
|
||
WheelCameraPlaneCalibParam* calibParam = getPlaneCalibParam(m_currentCameraIndex);
|
||
if (calibParam && calibParam->isCalibrated) {
|
||
for (int i = 0; i < 9; ++i) {
|
||
groundCalibPara.planeCalib[i] = calibParam->planeCalib[i];
|
||
groundCalibPara.invRMatrix[i] = calibParam->invRMatrix[i];
|
||
}
|
||
groundCalibPara.planeHeight = calibParam->planeHeight;
|
||
LOG_INFO("Using calibrated plane parameters for camera %d\n", m_currentCameraIndex);
|
||
LOG_INFO(" planeHeight: %.3f, errorCompensation: %.2f\n", calibParam->planeHeight, calibParam->errorCompensation);
|
||
|
||
if(calibParam){
|
||
// 计算调平使用的地面高度（加上该相机的误差补偿）
|
||
double adjustedPlaneHeight = groundCalibPara.planeHeight + calibParam->errorCompensation;
|
||
LOG_INFO(" adjustedPlaneHeight (with compensation): %.3f\n", adjustedPlaneHeight);
|
||
|
||
for(size_t i = 0; i < scanLines.size(); i++){
|
||
wd_horizonCamera_lineDataR(scanLines[i], calibParam->planeCalib, adjustedPlaneHeight);
|
||
}
|
||
}
|
||
} else {
|
||
LOG_WARN("No calibration data for camera %d, using default parameters\n", m_currentCameraIndex);
|
||
}
|
||
|
||
|
||
// 4. 调用算法
|
||
int errCode = 0;
|
||
LOG_INFO("Calling wd_wheelArchHeigthMeasure...\n");
|
||
|
||
WD_wheelArchInfo wheelArchResult = wd_wheelArchHeigthMeasure(
|
||
scanLines,
|
||
cornerParam,
|
||
lineSegParam,
|
||
filterParam,
|
||
growParam,
|
||
groundCalibPara,
|
||
&errCode);
|
||
|
||
LOG_INFO("Algorithm completed with errCode=%d\n", errCode);
|
||
|
||
// 5. 处理算法结果
|
||
WheelMeasureResult result;
|
||
|
||
// 从配置中获取当前相机名称
|
||
QString cameraName;
|
||
int enabledIndex = 0;
|
||
for (const auto& cameraConfig : m_configResult.cameras) {
|
||
if (cameraConfig.enabled) {
|
||
enabledIndex++;
|
||
if (enabledIndex == m_currentCameraIndex) {
|
||
cameraName = QString::fromStdString(cameraConfig.name);
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
if (cameraName.isEmpty() && !m_vrEyeDeviceList.empty()) {
|
||
cameraName = QString::fromStdString(m_vrEyeDeviceList[0].first);
|
||
}
|
||
if (cameraName.isEmpty()) {
|
||
cameraName = QString("Camera%1").arg(m_currentCameraIndex);
|
||
}
|
||
|
||
result.cameraName = cameraName;
|
||
result.aliasName = cameraName;
|
||
result.bResultValid = (errCode == 0);
|
||
|
||
if (errCode == 0) {
|
||
LOG_INFO("========== Wheel Arch Measurement Result ==========\n");
|
||
LOG_INFO("wheelArchPos: (%.3f, %.3f, %.3f)\n",
|
||
wheelArchResult.wheelArchPos.x,
|
||
wheelArchResult.wheelArchPos.y,
|
||
wheelArchResult.wheelArchPos.z);
|
||
LOG_INFO("wheelUpPos: (%.3f, %.3f, %.3f)\n",
|
||
wheelArchResult.wheelUpPos.x,
|
||
wheelArchResult.wheelUpPos.y,
|
||
wheelArchResult.wheelUpPos.z);
|
||
LOG_INFO("wheelDownPos: (%.3f, %.3f, %.3f)\n",
|
||
wheelArchResult.wheelDownPos.x,
|
||
wheelArchResult.wheelDownPos.y,
|
||
wheelArchResult.wheelDownPos.z);
|
||
LOG_INFO("archToCenterHeigth: %.3f archToGroundHeigth: %.3f mm\n", wheelArchResult.archToCenterHeigth, wheelArchResult.archToGroundHeigth);
|
||
LOG_INFO("==================================================\n");
|
||
|
||
if (m_statusUpdate) {
|
||
QString statusMsg = QString("轮眉高度: %1 mm, 到地面高度: %2 mm").arg(wheelArchResult.archToCenterHeigth).arg(wheelArchResult.archToGroundHeigth);
|
||
m_statusUpdate->OnStatusUpdate(statusMsg);
|
||
}
|
||
|
||
// 填充测量结果数据
|
||
WheelMeasureData measureData;
|
||
measureData.archToCenterHeight = wheelArchResult.archToCenterHeigth;
|
||
measureData.archToGroundHeight = wheelArchResult.archToGroundHeigth;
|
||
measureData.wheelArchPosX = wheelArchResult.wheelArchPos.x;
|
||
measureData.wheelArchPosY = wheelArchResult.wheelArchPos.y;
|
||
measureData.wheelArchPosZ = wheelArchResult.wheelArchPos.z;
|
||
measureData.wheelUpPosX = wheelArchResult.wheelUpPos.x;
|
||
measureData.wheelUpPosY = wheelArchResult.wheelUpPos.y;
|
||
measureData.wheelUpPosZ = wheelArchResult.wheelUpPos.z;
|
||
measureData.wheelDownPosX = wheelArchResult.wheelDownPos.x;
|
||
measureData.wheelDownPosY = wheelArchResult.wheelDownPos.y;
|
||
measureData.wheelDownPosZ = wheelArchResult.wheelDownPos.z;
|
||
measureData.timestamp = QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss");
|
||
result.result.push_back(measureData);
|
||
|
||
// 使用 PointCloudImageUtils 生成带检测结果的图像
|
||
result.image = PointCloudImageUtils::GenerateWheelArchImage(
|
||
scanLines,
|
||
wheelArchResult.wheelArchPos,
|
||
wheelArchResult.wheelUpPos,
|
||
wheelArchResult.wheelDownPos,
|
||
wheelArchResult.archToCenterHeigth,
|
||
true);
|
||
} else {
|
||
LOG_ERROR("Algorithm failed with errCode=%d\n", errCode);
|
||
if (m_statusUpdate) {
|
||
m_statusUpdate->OnStatusUpdate(QString("算法检测失败，错误码: %1").arg(errCode));
|
||
}
|
||
|
||
// 生成仅有点云的图像（无检测结果）
|
||
SVzNL3DPoint emptyPoint = {0.0, 0.0, 0.0};
|
||
result.image = PointCloudImageUtils::GenerateWheelArchImage(
|
||
scanLines,
|
||
emptyPoint,
|
||
emptyPoint,
|
||
emptyPoint,
|
||
0.0,
|
||
false);
|
||
}
|
||
|
||
result.bImageValid = !result.image.isNull();
|
||
|
||
// ========== 写入检测结果到 Modbus (地址2-25) ==========
|
||
// 辅助lambda: float转两个uint16_t (大端模式)
|
||
auto floatToUint16 = [](float value, uint16_t& high, uint16_t& low) {
|
||
uint32_t bits;
|
||
memcpy(&bits, &value, sizeof(float));
|
||
high = static_cast<uint16_t>((bits >> 16) & 0xFFFF);
|
||
low = static_cast<uint16_t>(bits & 0xFFFF);
|
||
};
|
||
|
||
uint16_t modbusData[24];
|
||
memset(modbusData, 0, sizeof(modbusData));
|
||
|
||
// 地址2: 设备序号
|
||
modbusData[0] = static_cast<uint16_t>(m_currentCameraIndex);
|
||
// 地址3: 结果有效
|
||
modbusData[1] = (errCode == 0) ? 1 : 0;
|
||
|
||
if (errCode == 0) {
|
||
// 地址4-5: 轮眉高度（到中心）
|
||
floatToUint16(static_cast<float>(wheelArchResult.archToCenterHeigth), modbusData[2], modbusData[3]);
|
||
// 地址6-7: 轮眉X
|
||
floatToUint16(static_cast<float>(wheelArchResult.wheelArchPos.x), modbusData[4], modbusData[5]);
|
||
// 地址8-9: 轮眉Y
|
||
floatToUint16(static_cast<float>(wheelArchResult.wheelArchPos.y), modbusData[6], modbusData[7]);
|
||
// 地址10-11: 轮眉Z
|
||
floatToUint16(static_cast<float>(wheelArchResult.wheelArchPos.z), modbusData[8], modbusData[9]);
|
||
// 地址12-13: 上点X
|
||
floatToUint16(static_cast<float>(wheelArchResult.wheelUpPos.x), modbusData[10], modbusData[11]);
|
||
// 地址14-15: 上点Y
|
||
floatToUint16(static_cast<float>(wheelArchResult.wheelUpPos.y), modbusData[12], modbusData[13]);
|
||
// 地址16-17: 上点Z
|
||
floatToUint16(static_cast<float>(wheelArchResult.wheelUpPos.z), modbusData[14], modbusData[15]);
|
||
// 地址18-19: 下点X
|
||
floatToUint16(static_cast<float>(wheelArchResult.wheelDownPos.x), modbusData[16], modbusData[17]);
|
||
// 地址20-21: 下点Y
|
||
floatToUint16(static_cast<float>(wheelArchResult.wheelDownPos.y), modbusData[18], modbusData[19]);
|
||
// 地址22-23: 下点Z
|
||
floatToUint16(static_cast<float>(wheelArchResult.wheelDownPos.z), modbusData[20], modbusData[21]);
|
||
// 地址24-25: 到地面高度
|
||
floatToUint16(static_cast<float>(wheelArchResult.archToGroundHeigth), modbusData[22], modbusData[23]);
|
||
}
|
||
|
||
WriteModbusRegisters(2, modbusData, 24);
|
||
LOG_INFO("Modbus: 写入检测结果到地址2-25, 设备=%d, 有效=%d\n", m_currentCameraIndex, modbusData[1]);
|
||
|
||
// 检测完成后清零"检测控制"（地址0）
|
||
uint16_t zero = 0;
|
||
WriteModbusRegisters(0, &zero, 1);
|
||
|
||
// 直接调用回调，不使用信号槽
|
||
if (m_statusUpdate) {
|
||
m_statusUpdate->OnMeasureResult(result);
|
||
}
|
||
|
||
SetWorkStatus(WorkStatus::Completed);
|
||
|
||
// 如果正在进行顺序检测，继续检测下一个设备
|
||
if (m_sequentialDetecting) {
|
||
m_sequentialCurrentIndex++;
|
||
QMetaObject::invokeMethod(this, [this]() {
|
||
continueSequentialDetection();
|
||
}, Qt::QueuedConnection);
|
||
}
|
||
}
|
||
|
||
WheelCameraPlaneCalibParam* WheelMeasurePresenter::getPlaneCalibParam(int cameraIndex)
|
||
{
|
||
for (auto& param : m_configResult.planeCalibParams) {
|
||
if (param.cameraIndex == cameraIndex) {
|
||
return ¶m;
|
||
}
|
||
}
|
||
return nullptr;
|
||
}
|
||
|
||
// ============ ICameraLevelCalculator 接口实现 ============
|
||
|
||
bool WheelMeasurePresenter::CalculatePlaneCalibration(
|
||
const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& scanData,
|
||
double planeCalib[9],
|
||
double& planeHeight,
|
||
double invRMatrix[9])
|
||
{
|
||
LOG_INFO("CalculatePlaneCalibration called, scan lines: %zu\n", scanData.size());
|
||
|
||
// TODO: 调用调平算法库计算调平参数
|
||
// 暂时返回单位矩阵
|
||
planeCalib[0] = 1.0; planeCalib[1] = 0.0; planeCalib[2] = 0.0;
|
||
planeCalib[3] = 0.0; planeCalib[4] = 1.0; planeCalib[5] = 0.0;
|
||
planeCalib[6] = 0.0; planeCalib[7] = 0.0; planeCalib[8] = 1.0;
|
||
|
||
invRMatrix[0] = 1.0; invRMatrix[1] = 0.0; invRMatrix[2] = 0.0;
|
||
invRMatrix[3] = 0.0; invRMatrix[4] = 1.0; invRMatrix[5] = 0.0;
|
||
invRMatrix[6] = 0.0; invRMatrix[7] = 0.0; invRMatrix[8] = 1.0;
|
||
|
||
planeHeight = 0.0;
|
||
|
||
return true;
|
||
}
|
||
|
||
// ============ ICameraLevelResultSaver 接口实现 ============
|
||
|
||
bool WheelMeasurePresenter::SaveLevelingResults(double planeCalib[9], double planeHeight, double invRMatrix[9],
|
||
int cameraIndex, const QString& cameraName)
|
||
{
|
||
LOG_INFO("SaveLevelingResults: Camera[%d] %s, planeHeight=%.2f\n",
|
||
cameraIndex, cameraName.toStdString().c_str(), planeHeight);
|
||
|
||
// 查找或创建调平参数
|
||
WheelCameraPlaneCalibParam* param = getPlaneCalibParam(cameraIndex);
|
||
if (!param) {
|
||
WheelCameraPlaneCalibParam newParam;
|
||
newParam.cameraIndex = cameraIndex;
|
||
newParam.cameraName = cameraName.toStdString();
|
||
m_configResult.planeCalibParams.push_back(newParam);
|
||
param = &m_configResult.planeCalibParams.back();
|
||
}
|
||
|
||
// 更新调平参数
|
||
param->isCalibrated = true;
|
||
param->planeHeight = planeHeight;
|
||
for (int i = 0; i < 9; ++i) {
|
||
param->planeCalib[i] = planeCalib[i];
|
||
param->invRMatrix[i] = invRMatrix[i];
|
||
}
|
||
|
||
// 保存配置到文件
|
||
QString configPath = PathManager::GetInstance().GetConfigFilePath();
|
||
bool success = m_config->SaveConfig(configPath.toStdString(), m_configResult);
|
||
|
||
if (success) {
|
||
LOG_INFO("Leveling results saved successfully\n");
|
||
} else {
|
||
LOG_ERROR("Failed to save leveling results\n");
|
||
}
|
||
|
||
return success;
|
||
}
|
||
|
||
bool WheelMeasurePresenter::LoadLevelingResults(int cameraIndex, const QString& cameraName,
|
||
double planeCalib[9], double& planeHeight, double invRMatrix[9])
|
||
{
|
||
LOG_INFO("LoadLevelingResults: Camera[%d] %s\n", cameraIndex, cameraName.toStdString().c_str());
|
||
|
||
WheelCameraPlaneCalibParam* param = getPlaneCalibParam(cameraIndex);
|
||
if (!param || !param->isCalibrated) {
|
||
LOG_WARN("No calibration data found for camera %d\n", cameraIndex);
|
||
return false;
|
||
}
|
||
|
||
planeHeight = param->planeHeight;
|
||
for (int i = 0; i < 9; ++i) {
|
||
planeCalib[i] = param->planeCalib[i];
|
||
invRMatrix[i] = param->invRMatrix[i];
|
||
}
|
||
|
||
LOG_INFO("Leveling results loaded: planeHeight=%.2f\n", planeHeight);
|
||
return true;
|
||
}
|
||
|
||
// ============ 静态相机状态回调函数 ============
|
||
|
||
void WheelMeasurePresenter::_StaticCameraNotify(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pInfoParam)
|
||
{
|
||
// 从pInfoParam获取this指针，转换回WheelMeasurePresenter*类型
|
||
WheelMeasurePresenter* pThis = reinterpret_cast<WheelMeasurePresenter*>(pInfoParam);
|
||
if (pThis) {
|
||
// 调用实例的非静态成员函数
|
||
pThis->_CameraNotify(eStatus, pExtData, nDataLength, pInfoParam);
|
||
}
|
||
}
|
||
|
||
void WheelMeasurePresenter::_CameraNotify(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pInfoParam)
|
||
{
|
||
LOG_DEBUG("[Camera Notify] received: status=%d\n", (int)eStatus);
|
||
|
||
switch (eStatus) {
|
||
case EVzDeviceWorkStatus::keDeviceWorkStatus_Offline:
|
||
{
|
||
LOG_WARNING("[Camera Notify] Camera device offline/disconnected\n");
|
||
|
||
// 通知UI相机状态变更
|
||
if (m_statusUpdate) {
|
||
m_statusUpdate->OnCameraDisconnected(QString("Camera"));
|
||
m_statusUpdate->OnStatusUpdate(QString("相机设备离线"));
|
||
}
|
||
break;
|
||
}
|
||
|
||
case EVzDeviceWorkStatus::keDeviceWorkStatus_Eye_Reconnect:
|
||
{
|
||
LOG_INFO("[Camera Notify] Camera device reconnecting\n");
|
||
|
||
if (m_statusUpdate) {
|
||
m_statusUpdate->OnStatusUpdate(QString("相机设备重连中..."));
|
||
}
|
||
break;
|
||
}
|
||
|
||
case EVzDeviceWorkStatus::keDeviceWorkStatus_Eye_Comming:
|
||
{
|
||
LOG_INFO("[Camera Notify] Camera device connected\n");
|
||
|
||
if (m_statusUpdate) {
|
||
m_statusUpdate->OnCameraConnected(QString("Camera"));
|
||
m_statusUpdate->OnStatusUpdate(QString("相机设备已连接"));
|
||
}
|
||
break;
|
||
}
|
||
|
||
default:
|
||
LOG_DEBUG("[Camera Notify] Unhandled status: %d\n", (int)eStatus);
|
||
break;
|
||
}
|
||
}
|
||
|
||
// ============ Modbus写寄存器回调处理 ============
|
||
|
||
void WheelMeasurePresenter::OnModbusWriteCallback(uint16_t startAddress, const uint16_t* data, uint16_t count)
|
||
{
|
||
LOG_INFO("OnModbusWriteCallback: address=%d, count=%d\n", startAddress, count);
|
||
|
||
if (!data || count == 0) {
|
||
return;
|
||
}
|
||
|
||
// 地址0: 写1-4直接开始检测对应设备
|
||
if (startAddress == 0) {
|
||
int deviceIndex = data[0];
|
||
|
||
// 检查设备索引是否有效 (1-4)
|
||
if (deviceIndex >= 1 && deviceIndex <= 4 &&
|
||
deviceIndex <= static_cast<int>(m_vrEyeDeviceList.size())) {
|
||
LOG_INFO("Modbus: 开始检测设备 %d\n", deviceIndex);
|
||
// 使用 QMetaObject::invokeMethod 在主线程执行
|
||
QMetaObject::invokeMethod(this, [this, deviceIndex]() {
|
||
ResetDetect(deviceIndex - 1); // ResetDetect 期望 0-based 索引
|
||
}, Qt::QueuedConnection);
|
||
} else {
|
||
LOG_WARNING("Modbus: 无效的设备索引: %d (有效范围: 1-%d)\n",
|
||
deviceIndex, static_cast<int>(m_vrEyeDeviceList.size()));
|
||
}
|
||
}
|
||
}
|