497 lines
18 KiB
C++
497 lines
18 KiB
C++
#include "WeldSeamPresenter.h"
|
||
|
||
#include "DetectPresenter.h"
|
||
#include "PathManager.h"
|
||
#include "SX_lapWeldDetection_Export.h"
|
||
#include "Version.h"
|
||
#include "VrError.h"
|
||
#include "VrLog.h"
|
||
#include "VrTimeUtils.h"
|
||
|
||
#include <QDateTime>
|
||
#include <QFile>
|
||
#include <QMetaObject>
|
||
#include <cstring>
|
||
|
||
WeldSeamPresenter::WeldSeamPresenter(QObject* parent)
|
||
: BasePresenter(parent)
|
||
{
|
||
}
|
||
|
||
WeldSeamPresenter::~WeldSeamPresenter()
|
||
{
|
||
DeinitApp();
|
||
}
|
||
|
||
int WeldSeamPresenter::InitApp()
|
||
{
|
||
LOG_INFO("Start WeldSeamApp version: %s\n", WELDSEAM_VERSION_STRING);
|
||
SetWorkStatus(WorkStatus::InitIng);
|
||
|
||
m_pDetectPresenter = new DetectPresenter();
|
||
|
||
// 将随程序发布的默认配置只在首次启动时落到用户配置目录。
|
||
const QString configPath = PathManager::GetInstance().GetConfigFilePath();
|
||
if (!QFile::exists(configPath) &&
|
||
!QFile::copy(QStringLiteral(":/app/default_config.xml"), configPath)) {
|
||
LOG_WARNING("Failed to install default WeldSeam config: %s\n",
|
||
configPath.toUtf8().constData());
|
||
}
|
||
m_pConfigManager = new ConfigManager();
|
||
if (!m_pConfigManager || !m_pConfigManager->Initialize()) {
|
||
LOG_ERROR("WeldSeam configuration initialization failed\n");
|
||
return ERR_CODE(DEV_CONFIG_ERR);
|
||
}
|
||
|
||
const ConfigResult config = m_pConfigManager->GetConfigResult();
|
||
SetDebugParam(config.debugParam);
|
||
std::vector<DeviceInfo> cameras = config.cameraList;
|
||
|
||
// 动态扫描：使用 VzNLSDK，但明确关闭相机摆动电机。
|
||
InitCamera(cameras, false, false);
|
||
|
||
if (InitTCPServer() != SUCCESS) {
|
||
LOG_ERROR("WeldSeam TCP server initialization failed\n");
|
||
}
|
||
CheckAndUpdateWorkStatus();
|
||
return SUCCESS;
|
||
}
|
||
|
||
void WeldSeamPresenter::DeinitApp()
|
||
{
|
||
if (m_deinitialized.exchange(true)) return;
|
||
|
||
CancelDynamicScan();
|
||
BasePresenter::StopDetection();
|
||
StopTCPServer();
|
||
|
||
delete m_pDetectPresenter;
|
||
m_pDetectPresenter = nullptr;
|
||
delete m_pConfigManager;
|
||
m_pConfigManager = nullptr;
|
||
}
|
||
|
||
int WeldSeamPresenter::InitAlgoParams()
|
||
{
|
||
if (!m_pConfigManager) return ERR_CODE(DEV_CONFIG_ERR);
|
||
const ConfigResult config = m_pConfigManager->GetConfigResult();
|
||
SetDebugParam(config.debugParam);
|
||
const auto& p = config.algorithmParams;
|
||
LOG_INFO("WeldSeam BQ params loaded: valleyWindow=%.3f valleyMaxWidth=%.3f "
|
||
"valleyMinHeight=%.3f refs=%d skipLines=%d yDev=%.3f skipDist=%.3f "
|
||
"zDev=%.3f minL=%.3f minV=%.3f\n",
|
||
p.valleyParam.chkWinWidth, p.valleyParam.maxWidth,
|
||
p.valleyParam.minHeight, p.weldSeamParam.weldRefPoints,
|
||
p.treeGrowParam.maxLineSkipNum, p.treeGrowParam.yDeviationMax,
|
||
p.treeGrowParam.maxSkipDistance, p.treeGrowParam.zDeviationMax,
|
||
p.treeGrowParam.minLTypeTreeLen, p.treeGrowParam.minVTypeTreeLen);
|
||
LOG_INFO("WeldSeam flaw type=%d\n",
|
||
static_cast<int>(p.weldSeamParam.detectFlawType));
|
||
return SUCCESS;
|
||
}
|
||
|
||
VzNL_OnNotifyStatusCBEx WeldSeamPresenter::GetCameraStatusCallback()
|
||
{
|
||
return &WeldSeamPresenter::DynamicCameraStatusCallback;
|
||
}
|
||
|
||
void WeldSeamPresenter::DynamicCameraStatusCallback(
|
||
EVzDeviceWorkStatus status, void* extData, unsigned int dataLength, void* userData)
|
||
{
|
||
Q_UNUSED(extData);
|
||
Q_UNUSED(dataLength);
|
||
auto* context = static_cast<CameraCallbackContext*>(userData);
|
||
if (!context || !context->presenter) {
|
||
LOG_ERROR("WeldSeam dynamic camera status callback has invalid context\n");
|
||
return;
|
||
}
|
||
|
||
auto* presenter = static_cast<WeldSeamPresenter*>(context->presenter);
|
||
switch (status) {
|
||
case keDeviceWorkStatus_Offline:
|
||
presenter->m_bCameraConnected = false;
|
||
presenter->OnCameraStatusChanged(context->cameraIndex, false);
|
||
break;
|
||
case keDeviceWorkStatus_Eye_Reconnect:
|
||
presenter->m_bCameraConnected = true;
|
||
presenter->OnCameraStatusChanged(context->cameraIndex, true);
|
||
break;
|
||
case keDeviceWorkStatus_Device_Swing_Finish:
|
||
// 动态相机的扫描结束只能由 STOP 命令确认；设备完成通知不启动算法。
|
||
LOG_DEBUG("WeldSeam camera %d reported scan finish; waiting for STOP command\n",
|
||
context->cameraIndex);
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
QString WeldSeamPresenter::StartDynamicScan(int cameraIndex)
|
||
{
|
||
ScanState expected = ScanState::Ready;
|
||
if (!m_scanState.compare_exchange_strong(expected, ScanState::Scanning)) {
|
||
return QStringLiteral("ERROR_BUSY");
|
||
}
|
||
if (!m_bCameraConnected) {
|
||
m_scanState = ScanState::Ready;
|
||
UpdateModbusStatus(4);
|
||
return QStringLiteral("ERROR_NOT_READY");
|
||
}
|
||
|
||
ResetModbusResults();
|
||
UpdateModbusStatus(1);
|
||
const int result = BasePresenter::StartDetection(cameraIndex, false);
|
||
if (result != SUCCESS) {
|
||
m_scanState = ScanState::Ready;
|
||
UpdateModbusStatus(4);
|
||
SetWorkStatus(WorkStatus::Error);
|
||
return QStringLiteral("ERROR_NOT_READY");
|
||
}
|
||
|
||
OnStatusUpdate("动态扫描已开始，等待外部运动结束");
|
||
return QStringLiteral("START_ACK");
|
||
}
|
||
|
||
QString WeldSeamPresenter::StopDynamicScanAndDetect()
|
||
{
|
||
ScanState expected = ScanState::Scanning;
|
||
if (!m_scanState.compare_exchange_strong(expected, ScanState::Calculating)) {
|
||
return expected == ScanState::Calculating
|
||
? QStringLiteral("ERROR_BUSY") : QStringLiteral("ERROR_NOT_READY");
|
||
}
|
||
|
||
const int arrayIndex = m_currentCameraIndex - 1;
|
||
if (arrayIndex < 0 || arrayIndex >= static_cast<int>(m_vrEyeDeviceList.size()) ||
|
||
!m_vrEyeDeviceList[arrayIndex].second) {
|
||
m_scanState = ScanState::Ready;
|
||
UpdateModbusStatus(4);
|
||
SetWorkStatus(WorkStatus::Error);
|
||
return QStringLiteral("ERROR_NOT_READY");
|
||
}
|
||
|
||
const int stopResult = m_vrEyeDeviceList[arrayIndex].second->StopDetect();
|
||
if (stopResult != SUCCESS) {
|
||
LOG_ERROR("VzNL dynamic scan stop failed: %d\n", stopResult);
|
||
// 设备未确认停止前不允许开始新任务；保留 Scanning 以便重试 STOP。
|
||
m_scanState = ScanState::Scanning;
|
||
UpdateModbusStatus(4);
|
||
SetWorkStatus(WorkStatus::Error);
|
||
return QStringLiteral("ERROR_INTERNAL_ERROR");
|
||
}
|
||
|
||
// StopDetect 成功后才检查缓存。空扫描仍属于已受理的 STOP 请求：
|
||
// 先让调用方返回 STOP_ACK，再在 Presenter 线程异步发布 RESULT_NG。
|
||
if (!HasPendingDetectionData()) {
|
||
UpdateModbusStatus(2);
|
||
OnStatusUpdate("动态扫描已结束，未采集到有效数据");
|
||
QMetaObject::invokeMethod(this, [this]() {
|
||
CompleteFailedScan(QStringLiteral("扫描数据为空"), ERR_CODE(DEV_DATA_INVALID));
|
||
}, Qt::QueuedConnection);
|
||
return QStringLiteral("STOP_ACK");
|
||
}
|
||
|
||
// 非摆动模式不会产生 Swing_Finish；显式唤醒基类算法线程。
|
||
UpdateModbusStatus(2);
|
||
m_algoDetectTaskPending.store(true);
|
||
m_algoDetectCondition.notify_one();
|
||
OnStatusUpdate("动态扫描已结束，正在计算焊缝");
|
||
return QStringLiteral("STOP_ACK");
|
||
}
|
||
|
||
void WeldSeamPresenter::CancelDynamicScan()
|
||
{
|
||
ScanState expected = ScanState::Scanning;
|
||
if (!m_scanState.compare_exchange_strong(expected, ScanState::Ready)) return;
|
||
|
||
const int arrayIndex = m_currentCameraIndex - 1;
|
||
if (arrayIndex >= 0 && arrayIndex < static_cast<int>(m_vrEyeDeviceList.size()) &&
|
||
m_vrEyeDeviceList[arrayIndex].second) {
|
||
m_vrEyeDeviceList[arrayIndex].second->StopDetect();
|
||
}
|
||
ClearDetectionDataCache();
|
||
UpdateModbusStatus(m_bCameraConnected ? 0 : 4);
|
||
SetWorkStatus(m_bCameraConnected ? WorkStatus::Ready : WorkStatus::Error);
|
||
}
|
||
|
||
void WeldSeamPresenter::CompleteFailedScan(const QString& message, int errorCode)
|
||
{
|
||
ScanState expected = ScanState::Calculating;
|
||
if (!m_scanState.compare_exchange_strong(expected, ScanState::Ready)) return;
|
||
|
||
DetectionResult result;
|
||
result.cameraIndex = m_currentCameraIndex;
|
||
result.success = false;
|
||
result.errorCode = errorCode;
|
||
result.message = message;
|
||
|
||
if (auto* status = GetStatusCallback<IYWeldSeamStatus>()) {
|
||
status->OnDetectionResult(result);
|
||
status->OnStatusUpdate(result.message.toStdString());
|
||
}
|
||
SendTCPResult(result);
|
||
PublishModbusResult(result);
|
||
SetWorkStatus(WorkStatus::Error);
|
||
}
|
||
|
||
int WeldSeamPresenter::ProcessAlgoDetection(
|
||
std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& detectionDataCache)
|
||
{
|
||
DetectionResult result;
|
||
result.cameraIndex = m_currentCameraIndex;
|
||
|
||
int code = ERR_CODE(DEV_NOT_FIND);
|
||
if (m_pDetectPresenter && m_pConfigManager) {
|
||
const ConfigResult config = m_pConfigManager->GetConfigResult();
|
||
// Live WeldSeam scans are preprocessed line-by-line. The base cache is
|
||
// only populated in debug mode for persistence, so release that copy
|
||
// before asking DetectPresenter to consume its incremental data.
|
||
if (m_pDetectPresenter->HasIncrementalData() && !detectionDataCache.empty()) {
|
||
m_dataLoader.FreeLaserScanData(detectionDataCache);
|
||
detectionDataCache.clear();
|
||
}
|
||
code = m_pDetectPresenter->DetectWeldSeam(
|
||
m_currentCameraIndex, detectionDataCache, config.algorithmParams,
|
||
GetCameraCalibParam(m_currentCameraIndex), config.debugParam, m_dataLoader, result);
|
||
} else {
|
||
result.success = false;
|
||
result.message = QStringLiteral("算法或配置未初始化");
|
||
}
|
||
|
||
if (code != SUCCESS) {
|
||
result.success = false;
|
||
if (result.errorCode == 0) result.errorCode = code;
|
||
}
|
||
|
||
if (auto* status = GetStatusCallback<IYWeldSeamStatus>()) {
|
||
status->OnDetectionResult(result);
|
||
status->OnStatusUpdate(result.message.toStdString());
|
||
}
|
||
|
||
SendTCPResult(result);
|
||
PublishModbusResult(result);
|
||
m_scanState = ScanState::Ready;
|
||
return code;
|
||
}
|
||
|
||
bool WeldSeamPresenter::OnDetectionData(EVzResultDataType dataType,
|
||
SVzLaserLineData* laserData)
|
||
{
|
||
if (!laserData || !m_pDetectPresenter || !m_pConfigManager ||
|
||
dataType != keResultDataType_Position) {
|
||
return false;
|
||
}
|
||
|
||
const ConfigResult config = m_pConfigManager->GetConfigResult();
|
||
const bool processed = m_pDetectPresenter->ProcessDetectionLine(
|
||
dataType, *laserData, config.algorithmParams,
|
||
GetCameraCalibParam(m_currentCameraIndex), m_dataLoader);
|
||
if (!processed) {
|
||
LOG_WARNING("WeldSeam failed to preprocess one point-cloud line\n");
|
||
return true;
|
||
}
|
||
|
||
// Keep the original line only when debug persistence is enabled. In the
|
||
// normal path BasePresenter immediately drops its temporary callback data.
|
||
return !config.debugParam.enableDebug;
|
||
}
|
||
|
||
bool WeldSeamPresenter::HasPendingDetectionData() const
|
||
{
|
||
return m_pDetectPresenter && m_pDetectPresenter->HasIncrementalData();
|
||
}
|
||
|
||
void WeldSeamPresenter::ClearDetectionDataCache()
|
||
{
|
||
BasePresenter::ClearDetectionDataCache();
|
||
if (m_pDetectPresenter) m_pDetectPresenter->ResetIncrementalData();
|
||
}
|
||
|
||
int WeldSeamPresenter::LoadAndDetect(const QString& fileName)
|
||
{
|
||
ScanState expected = ScanState::Ready;
|
||
if (!m_scanState.compare_exchange_strong(expected, ScanState::Calculating)) {
|
||
return ERR_CODE(DEV_BUSY);
|
||
}
|
||
const int result = LoadDebugDataAndDetect(fileName.toStdString());
|
||
if (result != SUCCESS) {
|
||
m_scanState = ScanState::Ready;
|
||
SetWorkStatus(WorkStatus::Error);
|
||
}
|
||
return result;
|
||
}
|
||
|
||
void WeldSeamPresenter::ReconnectCamera()
|
||
{
|
||
TryReconnectCameras();
|
||
}
|
||
|
||
VrAlgorithmParams WeldSeamPresenter::GetAlgorithmParams() const
|
||
{
|
||
return m_pConfigManager ? m_pConfigManager->GetConfigResult().algorithmParams
|
||
: VrAlgorithmParams{};
|
||
}
|
||
|
||
void WeldSeamPresenter::SetAlgorithmParams(const VrAlgorithmParams& params)
|
||
{
|
||
if (!m_pConfigManager) return;
|
||
SystemConfig config = m_pConfigManager->GetConfig();
|
||
config.configResult.algorithmParams = params;
|
||
m_pConfigManager->UpdateFullConfig(config);
|
||
}
|
||
|
||
QString WeldSeamPresenter::GetAlgoVersion() const
|
||
{
|
||
return DetectPresenter::GetAlgoVersion();
|
||
}
|
||
|
||
void WeldSeamPresenter::OnConfigChanged(const ConfigResult& configResult)
|
||
{
|
||
SetDebugParam(configResult.debugParam);
|
||
InitAlgoParams();
|
||
if (configResult.tcpPort != m_tcpPort) {
|
||
StopTCPServer();
|
||
if (InitTCPServer() != SUCCESS) {
|
||
OnStatusUpdate("TCP服务端口更新失败");
|
||
}
|
||
}
|
||
OnStatusUpdate("配置已更新");
|
||
}
|
||
|
||
SSG_planeCalibPara WeldSeamPresenter::GetCameraCalibParam(int cameraIndex) const
|
||
{
|
||
SSG_planeCalibPara result{};
|
||
const double identity[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
|
||
std::memcpy(result.planeCalib, identity, sizeof(identity));
|
||
std::memcpy(result.invRMatrix, identity, sizeof(identity));
|
||
result.planeHeight = -1.0;
|
||
if (!m_pConfigManager) return result;
|
||
|
||
const auto& list = m_pConfigManager->GetConfigResult().algorithmParams.planeCalibParam.cameraCalibParams;
|
||
for (const auto& item : list) {
|
||
if (item.cameraIndex == cameraIndex && item.isCalibrated) {
|
||
std::memcpy(result.planeCalib, item.planeCalib, sizeof(result.planeCalib));
|
||
std::memcpy(result.invRMatrix, item.invRMatrix, sizeof(result.invRMatrix));
|
||
result.planeHeight = item.planeHeight;
|
||
break;
|
||
}
|
||
}
|
||
return result;
|
||
}
|
||
|
||
bool WeldSeamPresenter::CalculatePlaneCalibration(
|
||
const std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& scanData,
|
||
double planeCalib[9], double& planeHeight, double invRMatrix[9])
|
||
{
|
||
LaserDataLoader loader;
|
||
std::vector<std::vector<SVzNL3DPosition>> xyzData;
|
||
if (loader.ConvertToSVzNL3DPosition(scanData, xyzData) != SUCCESS || xyzData.empty()) return false;
|
||
const SSG_planeCalibPara result = sx_getBaseCalibPara(xyzData);
|
||
std::memcpy(planeCalib, result.planeCalib, sizeof(result.planeCalib));
|
||
std::memcpy(invRMatrix, result.invRMatrix, sizeof(result.invRMatrix));
|
||
planeHeight = result.planeHeight;
|
||
return true;
|
||
}
|
||
|
||
bool WeldSeamPresenter::SaveLevelingResults(double planeCalib[9], double planeHeight,
|
||
double invRMatrix[9], int cameraIndex,
|
||
const QString& cameraName)
|
||
{
|
||
if (!m_pConfigManager || cameraIndex <= 0) return false;
|
||
SystemConfig config = m_pConfigManager->GetConfig();
|
||
VrCameraPlaneCalibParam item;
|
||
item.cameraIndex = cameraIndex;
|
||
item.cameraName = cameraName.toStdString();
|
||
item.planeHeight = planeHeight;
|
||
item.isCalibrated = true;
|
||
std::memcpy(item.planeCalib, planeCalib, sizeof(item.planeCalib));
|
||
std::memcpy(item.invRMatrix, invRMatrix, sizeof(item.invRMatrix));
|
||
config.configResult.algorithmParams.planeCalibParam.SetCameraCalibParam(item);
|
||
return m_pConfigManager->UpdateFullConfig(config) &&
|
||
m_pConfigManager->SaveConfigToFile(PathManager::GetInstance().GetConfigFilePath().toStdString());
|
||
}
|
||
|
||
bool WeldSeamPresenter::LoadLevelingResults(int cameraIndex, const QString& cameraName,
|
||
double planeCalib[9], double& planeHeight,
|
||
double invRMatrix[9])
|
||
{
|
||
Q_UNUSED(cameraName);
|
||
if (!m_pConfigManager) return false;
|
||
VrCameraPlaneCalibParam item;
|
||
if (!m_pConfigManager->GetConfigResult().algorithmParams.planeCalibParam
|
||
.GetCameraCalibParam(cameraIndex, item) || !item.isCalibrated) return false;
|
||
std::memcpy(planeCalib, item.planeCalib, sizeof(item.planeCalib));
|
||
std::memcpy(invRMatrix, item.invRMatrix, sizeof(item.invRMatrix));
|
||
planeHeight = item.planeHeight;
|
||
return true;
|
||
}
|
||
|
||
void WeldSeamPresenter::CheckAndUpdateWorkStatus()
|
||
{
|
||
if (!m_bCameraConnected) {
|
||
SetWorkStatus(WorkStatus::Error);
|
||
} else if (m_scanState.load() != ScanState::Ready) {
|
||
SetWorkStatus(WorkStatus::Working);
|
||
} else {
|
||
SetWorkStatus(WorkStatus::Ready);
|
||
}
|
||
}
|
||
|
||
void WeldSeamPresenter::OnCameraStatusChanged(int cameraIndex, bool connected)
|
||
{
|
||
if (auto* status = GetStatusCallback<IYWeldSeamStatus>()) {
|
||
if (cameraIndex == 1) status->OnCamera1StatusChanged(connected);
|
||
else if (cameraIndex == 2) status->OnCamera2StatusChanged(connected);
|
||
}
|
||
CheckAndUpdateWorkStatus();
|
||
if (!connected && cameraIndex == m_currentCameraIndex &&
|
||
m_scanState.load() == ScanState::Scanning) {
|
||
QMetaObject::invokeMethod(this, [this]() { CancelDynamicScan(); }, Qt::QueuedConnection);
|
||
}
|
||
}
|
||
|
||
void WeldSeamPresenter::OnWorkStatusChanged(WorkStatus statusValue)
|
||
{
|
||
if (!m_modbusRegistersInitialized && statusValue == WorkStatus::Ready &&
|
||
IsModbusServerRunning()) {
|
||
const uint16_t zero = 0;
|
||
WriteModbusRegisters(0, &zero, 1);
|
||
ResetModbusResults();
|
||
UpdateModbusStatus(0);
|
||
m_modbusRegistersInitialized = true;
|
||
}
|
||
if (auto* status = GetStatusCallback<IYWeldSeamStatus>()) status->OnWorkStatusChanged(statusValue);
|
||
}
|
||
|
||
void WeldSeamPresenter::OnCameraCountChanged(int count)
|
||
{
|
||
if (auto* status = GetStatusCallback<IYWeldSeamStatus>()) status->OnCameraCountChanged(count);
|
||
}
|
||
|
||
void WeldSeamPresenter::OnStatusUpdate(const std::string& message)
|
||
{
|
||
if (auto* status = GetStatusCallback<IYWeldSeamStatus>()) status->OnStatusUpdate(message);
|
||
}
|
||
|
||
void WeldSeamPresenter::OnModbusServerStatusChanged(bool connected)
|
||
{
|
||
if (auto* status = GetStatusCallback<IYWeldSeamStatus>()) status->OnRobotConnectionChanged(connected);
|
||
}
|
||
|
||
void WeldSeamPresenter::OnModbusWriteCallback(uint16_t startAddress,
|
||
const uint16_t* data, uint16_t count)
|
||
{
|
||
if (!data || count == 0 || startAddress != 0) return;
|
||
const uint16_t command = data[0];
|
||
const uint16_t reset = 0;
|
||
WriteModbusRegisters(0, &reset, 1);
|
||
if (command == 1) {
|
||
const QString response = StartDynamicScan(-1);
|
||
if (response != QStringLiteral("START_ACK")) UpdateModbusStatus(4);
|
||
} else if (command == 2) {
|
||
const QString response = StopDynamicScanAndDetect();
|
||
if (response != QStringLiteral("STOP_ACK")) UpdateModbusStatus(4);
|
||
} else if (command != 0) {
|
||
UpdateModbusStatus(4);
|
||
}
|
||
}
|