653 lines
22 KiB
C++
653 lines
22 KiB
C++
#include "TireHolePosePresenter.h"
|
||
|
||
#include "DetectPresenter.h"
|
||
#include "PathManager.h"
|
||
#include "TireHolePoseTCPProtocol.h"
|
||
#include "Version.h"
|
||
#include "VrConvert.h"
|
||
#include "VrError.h"
|
||
#include "VrLog.h"
|
||
#include "VrTimeUtils.h"
|
||
|
||
#include <QtCore/QCoreApplication>
|
||
#include <QtCore/QDateTime>
|
||
#include <QtCore/QString>
|
||
#include <algorithm>
|
||
#include <cstdio>
|
||
#include <cstring>
|
||
|
||
TireHolePosePresenter::TireHolePosePresenter(QObject *parent)
|
||
: BasePresenter(parent)
|
||
{
|
||
}
|
||
|
||
TireHolePosePresenter::~TireHolePosePresenter()
|
||
{
|
||
if (m_pConfigManager) {
|
||
delete m_pConfigManager;
|
||
m_pConfigManager = nullptr;
|
||
}
|
||
|
||
if (m_pTCPServer) {
|
||
m_pTCPServer->StopServer();
|
||
delete m_pTCPServer;
|
||
m_pTCPServer = nullptr;
|
||
}
|
||
|
||
if (m_pDetectPresenter) {
|
||
delete m_pDetectPresenter;
|
||
m_pDetectPresenter = nullptr;
|
||
}
|
||
}
|
||
|
||
int TireHolePosePresenter::InitApp()
|
||
{
|
||
LOG_DEBUG("Start APP Version: %s\n", TIREHOLEPOSE_VERSION_STRING);
|
||
SetWorkStatus(WorkStatus::InitIng);
|
||
|
||
m_pDetectPresenter = new DetectPresenter();
|
||
|
||
m_pConfigManager = new ConfigManager();
|
||
if (!m_pConfigManager) {
|
||
LOG_ERROR("Failed to create ConfigManager instance\n");
|
||
if (auto pStatus = GetStatusCallback<IYTireHolePoseStatus>()) {
|
||
pStatus->OnStatusUpdate("配置管理器创建失败");
|
||
}
|
||
return ERR_CODE(DEV_CONFIG_ERR);
|
||
}
|
||
|
||
if (!m_pConfigManager->Initialize()) {
|
||
LOG_ERROR("Failed to initialize ConfigManager\n");
|
||
if (auto pStatus = GetStatusCallback<IYTireHolePoseStatus>()) {
|
||
pStatus->OnStatusUpdate("配置管理器初始化失败");
|
||
}
|
||
return ERR_CODE(DEV_CONFIG_ERR);
|
||
}
|
||
|
||
const ConfigResult configResult = m_pConfigManager->GetConfigResult();
|
||
SetDebugParam(configResult.debugParam);
|
||
m_modbusRegistersInitialized = false;
|
||
m_modbusWorkStatus = 0;
|
||
|
||
std::vector<DeviceInfo> cameraList = configResult.cameraList;
|
||
InitCamera(cameraList, false, true);
|
||
|
||
LOG_INFO("Camera initialization completed. Connected cameras: %zu, default camera index: %d\n",
|
||
m_vrEyeDeviceList.size(), m_currentCameraIndex);
|
||
|
||
int nRet = InitTCPServer();
|
||
if (nRet != SUCCESS) {
|
||
if (auto pStatus = GetStatusCallback<IYTireHolePoseStatus>()) {
|
||
pStatus->OnStatusUpdate("TCP服务器初始化失败");
|
||
}
|
||
m_bTCPConnected = false;
|
||
} else {
|
||
if (auto pStatus = GetStatusCallback<IYTireHolePoseStatus>()) {
|
||
pStatus->OnStatusUpdate("TCP服务器初始化成功");
|
||
}
|
||
}
|
||
|
||
if (auto pStatus = GetStatusCallback<IYTireHolePoseStatus>()) {
|
||
pStatus->OnStatusUpdate("设备初始化完成");
|
||
}
|
||
|
||
CheckAndUpdateWorkStatus();
|
||
|
||
if (auto pStatus = GetStatusCallback<IYTireHolePoseStatus>()) {
|
||
pStatus->OnStatusUpdate("配置初始化成功");
|
||
}
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
int TireHolePosePresenter::InitAlgoParams()
|
||
{
|
||
LOG_DEBUG("initializing algorithm parameters\n");
|
||
|
||
const ConfigResult configResult = m_pConfigManager->GetConfigResult();
|
||
SetDebugParam(configResult.debugParam);
|
||
|
||
LOG_INFO("Algorithm parameters: cornerTh=%.1f scale=%.1f, tireDiam=%.1f tireThick=%.1f\n",
|
||
configResult.algorithmParams.cornerParam.cornerTh,
|
||
configResult.algorithmParams.cornerParam.scale,
|
||
configResult.algorithmParams.tireParam.diameter,
|
||
configResult.algorithmParams.tireParam.thickness);
|
||
|
||
LOG_INFO("Algorithm parameters initialized successfully\n");
|
||
return SUCCESS;
|
||
}
|
||
|
||
CalibMatrix TireHolePosePresenter::GetClibMatrix(int cameraIndex) const
|
||
{
|
||
CalibMatrix clibMatrix;
|
||
const double identity[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
|
||
};
|
||
std::memcpy(clibMatrix.clibMatrix, identity, sizeof(identity));
|
||
|
||
if (!m_pConfigManager) {
|
||
LOG_WARNING("ConfigManager not initialized while requesting clib matrix\n");
|
||
return clibMatrix;
|
||
}
|
||
|
||
const ConfigResult configResult = m_pConfigManager->GetConfigResult();
|
||
if (const auto* item = configResult.FindHandEyeMatrix(cameraIndex)) {
|
||
std::memcpy(clibMatrix.clibMatrix, item->matrix, sizeof(identity));
|
||
} else {
|
||
LOG_WARNING("No clib matrix configured for camera=%d, using identity\n", cameraIndex);
|
||
}
|
||
|
||
return clibMatrix;
|
||
}
|
||
|
||
void TireHolePosePresenter::CheckAndUpdateWorkStatus()
|
||
{
|
||
SetWorkStatus(m_bCameraConnected ? WorkStatus::Ready : WorkStatus::Error);
|
||
}
|
||
|
||
int TireHolePosePresenter::ProcessAlgoDetection(std::vector<std::pair<EVzResultDataType, SVzLaserLineData>>& detectionDataCache)
|
||
{
|
||
LOG_INFO("[Algo Thread] Start real detection task using algorithm\n");
|
||
|
||
const unsigned int lineNum = detectionDataCache.size();
|
||
if (auto pStatus = GetStatusCallback<IYTireHolePoseStatus>()) {
|
||
pStatus->OnStatusUpdate(QString("扫描线数:%1，正在算法检测...").arg(lineNum).toStdString());
|
||
}
|
||
|
||
if (!m_pDetectPresenter) {
|
||
LOG_ERROR("DetectPresenter is null, cannot proceed with detection\n");
|
||
if (auto pStatus = GetStatusCallback<IYTireHolePoseStatus>()) {
|
||
pStatus->OnStatusUpdate("检测处理器未初始化");
|
||
}
|
||
return ERR_CODE(DEV_NOT_FIND);
|
||
}
|
||
|
||
CVrTimeUtils oTimeUtils;
|
||
const CalibMatrix currentClibMatrix = GetClibMatrix(m_currentCameraIndex);
|
||
const SSG_planeCalibPara groundCalibParam = _GetCameraCalibParam(m_currentCameraIndex);
|
||
const ConfigResult configResult = m_pConfigManager->GetConfigResult();
|
||
const VrDebugParam debugParam = configResult.debugParam;
|
||
const int poseOutputOrder = configResult.poseOutputOrder;
|
||
|
||
// 使用共享算法参数（类比 WorkpieceHole 风格）
|
||
VrAlgorithmParams algorithmParams = configResult.algorithmParams;
|
||
|
||
// 从全局配置获取欧拉角输入/输出顺序（网络配置页设置）
|
||
HandEyeExtrinsic extrinsic;
|
||
extrinsic.eulerOrder = configResult.eulerOrder;
|
||
|
||
// 从手眼标定矩阵列表获取当前相机的外参补偿值
|
||
if (const auto* heItem = configResult.FindHandEyeMatrix(m_currentCameraIndex)) {
|
||
extrinsic.rotX = heItem->rotX;
|
||
extrinsic.rotY = heItem->rotY;
|
||
extrinsic.rotZ = heItem->rotZ;
|
||
extrinsic.offsetX = heItem->offsetX;
|
||
extrinsic.offsetY = heItem->offsetY;
|
||
extrinsic.offsetZ = heItem->offsetZ;
|
||
extrinsic.outRotX = heItem->outRotX;
|
||
extrinsic.outRotY = heItem->outRotY;
|
||
extrinsic.outRotZ = heItem->outRotZ;
|
||
}
|
||
|
||
LOG_INFO("[Algo Thread] camera=%d eulerOrder=%d(outputEuler=%d) poseOutputOrder=%d rot=(%.3f, %.3f, %.3f) outRot=(%.3f, %.3f, %.3f) offset=(%.3f, %.3f, %.3f)\n",
|
||
m_currentCameraIndex,
|
||
extrinsic.eulerOrder, configResult.outputEulerOrder, poseOutputOrder,
|
||
extrinsic.rotX, extrinsic.rotY, extrinsic.rotZ,
|
||
extrinsic.outRotX, extrinsic.outRotY, extrinsic.outRotZ,
|
||
extrinsic.offsetX, extrinsic.offsetY, extrinsic.offsetZ);
|
||
|
||
DetectionResult detectionResult;
|
||
detectionResult.cameraIndex = m_currentCameraIndex;
|
||
|
||
int nRet = m_pDetectPresenter->DetectTireHole(
|
||
m_currentCameraIndex,
|
||
detectionDataCache,
|
||
algorithmParams,
|
||
groundCalibParam,
|
||
debugParam,
|
||
m_dataLoader,
|
||
currentClibMatrix.clibMatrix,
|
||
m_currentRobotPose,
|
||
extrinsic,
|
||
poseOutputOrder,
|
||
detectionResult);
|
||
|
||
if (nRet != SUCCESS) {
|
||
detectionResult.success = false;
|
||
if (detectionResult.errorCode == 0) {
|
||
detectionResult.errorCode = nRet;
|
||
}
|
||
if (detectionResult.message.isEmpty() || detectionResult.message == QStringLiteral("检测成功")) {
|
||
detectionResult.message = QString("检测失败:%1").arg(nRet);
|
||
}
|
||
}
|
||
|
||
LOG_INFO("[Algo Thread] objects=%zu time : %.2f ms\n",
|
||
detectionResult.positions.size(),
|
||
oTimeUtils.GetElapsedTimeInMilliSec());
|
||
|
||
if (auto pStatus = GetStatusCallback<IYTireHolePoseStatus>()) {
|
||
pStatus->OnDetectionResult(detectionResult);
|
||
}
|
||
|
||
QString statusMsg;
|
||
if (!detectionResult.success) {
|
||
statusMsg = detectionResult.message;
|
||
} else {
|
||
statusMsg = QString("轮胎检测完成，发现%1个轮胎").arg(detectionResult.positions.size());
|
||
}
|
||
|
||
if (auto pStatus = GetStatusCallback<IYTireHolePoseStatus>()) {
|
||
pStatus->OnStatusUpdate(statusMsg.toStdString());
|
||
}
|
||
|
||
_SendDetectionResultToTCP(detectionResult, m_currentCameraIndex);
|
||
_PublishDetectionResultToModbus(detectionResult);
|
||
return nRet;
|
||
}
|
||
|
||
void TireHolePosePresenter::OnConfigChanged(const ConfigResult& configResult)
|
||
{
|
||
LOG_INFO("Configuration changed notification received, reloading algorithm parameters\n");
|
||
SetDebugParam(configResult.debugParam);
|
||
|
||
int result = InitAlgoParams();
|
||
stopServer();
|
||
const int tcpResult = InitTCPServer();
|
||
if (result == SUCCESS) {
|
||
LOG_INFO("Algorithm parameters reloaded successfully after config change\n");
|
||
if (auto pStatus = GetStatusCallback<IYTireHolePoseStatus>()) {
|
||
pStatus->OnStatusUpdate("配置已更新，算法参数重新加载成功");
|
||
}
|
||
} else {
|
||
LOG_ERROR("Failed to reload algorithm parameters after config change, error: %d\n", result);
|
||
if (auto pStatus = GetStatusCallback<IYTireHolePoseStatus>()) {
|
||
pStatus->OnStatusUpdate("配置更新后算法参数重新加载失败");
|
||
}
|
||
}
|
||
|
||
if (tcpResult != SUCCESS) {
|
||
LOG_ERROR("Failed to restart TCP server after config change, error: %d\n", tcpResult);
|
||
if (auto pStatus = GetStatusCallback<IYTireHolePoseStatus>()) {
|
||
pStatus->OnStatusUpdate("TCP server restart failed");
|
||
}
|
||
}
|
||
}
|
||
|
||
SSG_planeCalibPara TireHolePosePresenter::_GetCameraCalibParam(int cameraIndex)
|
||
{
|
||
SSG_planeCalibPara calibParam;
|
||
const 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;
|
||
|
||
const VrAlgorithmParams algorithmParams = m_pConfigManager->GetAlgorithmParams();
|
||
for (const auto& cameraParam : algorithmParams.planeCalibParam.cameraCalibParams) {
|
||
if (cameraParam.cameraIndex == cameraIndex && 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;
|
||
}
|
||
|
||
void TireHolePosePresenter::OnCameraStatusChanged(int cameraIndex, bool isConnected)
|
||
{
|
||
LOG_INFO("Camera %d status changed: %s\n", cameraIndex, isConnected ? "connected" : "disconnected");
|
||
|
||
if (auto pStatus = GetStatusCallback<IYTireHolePoseStatus>()) {
|
||
if (cameraIndex == 1) {
|
||
pStatus->OnCamera1StatusChanged(isConnected);
|
||
} else if (cameraIndex == 2) {
|
||
pStatus->OnCamera2StatusChanged(isConnected);
|
||
}
|
||
|
||
QString cameraName;
|
||
const 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);
|
||
}
|
||
|
||
const QString statusMsg = QString("%1%2").arg(cameraName).arg(isConnected ? "已连接" : "已断开");
|
||
pStatus->OnStatusUpdate(statusMsg.toStdString());
|
||
}
|
||
|
||
CheckAndUpdateWorkStatus();
|
||
}
|
||
|
||
void TireHolePosePresenter::OnWorkStatusChanged(WorkStatus status)
|
||
{
|
||
if (auto pStatus = GetStatusCallback<IYTireHolePoseStatus>()) {
|
||
pStatus->OnWorkStatusChanged(status);
|
||
}
|
||
|
||
if (!m_modbusRegistersInitialized && status == WorkStatus::Ready) {
|
||
_InitializeModbusRegisters();
|
||
}
|
||
|
||
if (status == WorkStatus::Working) {
|
||
_UpdateModbusWorkStatus(1);
|
||
} else if (status == WorkStatus::Error) {
|
||
_UpdateModbusWorkStatus(3);
|
||
}
|
||
}
|
||
|
||
void TireHolePosePresenter::OnCameraCountChanged(int count)
|
||
{
|
||
if (auto pStatus = GetStatusCallback<IYTireHolePoseStatus>()) {
|
||
pStatus->OnCameraCountChanged(count);
|
||
}
|
||
}
|
||
|
||
void TireHolePosePresenter::OnStatusUpdate(const std::string& statusMessage)
|
||
{
|
||
if (auto pStatus = GetStatusCallback<IYTireHolePoseStatus>()) {
|
||
pStatus->OnStatusUpdate(statusMessage);
|
||
}
|
||
}
|
||
|
||
void TireHolePosePresenter::OnModbusServerStatusChanged(bool isConnected)
|
||
{
|
||
if (auto pStatus = GetStatusCallback<IYTireHolePoseStatus>()) {
|
||
pStatus->OnRobotConnectionChanged(isConnected);
|
||
}
|
||
}
|
||
|
||
void TireHolePosePresenter::OnModbusWriteCallback(uint16_t startAddress, const uint16_t* data, uint16_t count)
|
||
{
|
||
if (!data || count == 0) {
|
||
return;
|
||
}
|
||
|
||
// Modbus 地址分配（与 TCPServerMethods.cpp 一致）：
|
||
// 地址 0 : 触发寄存器
|
||
// 地址 2~13 : 机械臂位姿（6 个 float32 = 12 个 uint16）
|
||
constexpr uint16_t kTriggerAddress = 0;
|
||
constexpr uint16_t kRobotPoseAddress = 2;
|
||
constexpr uint16_t kRobotPoseRegCount = 12;
|
||
|
||
// 缓存机械臂位姿写入（地址2~13，6个float32）
|
||
for (uint16_t i = 0; i < count; ++i) {
|
||
uint16_t addr = startAddress + i;
|
||
if (addr >= kRobotPoseAddress && addr < kRobotPoseAddress + kRobotPoseRegCount) {
|
||
m_modbusRobotPoseRegs[addr - kRobotPoseAddress] = data[i];
|
||
}
|
||
}
|
||
|
||
// 检查触发
|
||
uint16_t triggerValue = 0;
|
||
bool hasTrigger = false;
|
||
|
||
for (uint16_t i = 0; i < count; ++i) {
|
||
if (startAddress + i == kTriggerAddress) {
|
||
triggerValue = data[i];
|
||
hasTrigger = true;
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (!hasTrigger) {
|
||
return;
|
||
}
|
||
|
||
const uint16_t resetValue = 0;
|
||
WriteModbusRegisters(kTriggerAddress, &resetValue, 1);
|
||
|
||
if (triggerValue == 1) { // TireHole detection type
|
||
// 从缓存的寄存器中解析机械臂位姿
|
||
RobotPose6D robotPose;
|
||
auto regToFloat = [this](int offset) -> float {
|
||
uint32_t raw = (static_cast<uint32_t>(m_modbusRobotPoseRegs[offset]) << 16) | static_cast<uint32_t>(m_modbusRobotPoseRegs[offset + 1]);
|
||
float val = 0;
|
||
std::memcpy(&val, &raw, sizeof(val));
|
||
return val;
|
||
};
|
||
robotPose.x = regToFloat(0);
|
||
robotPose.y = regToFloat(2);
|
||
robotPose.z = regToFloat(4);
|
||
robotPose.a = regToFloat(6);
|
||
robotPose.b = regToFloat(8);
|
||
robotPose.c = regToFloat(10);
|
||
|
||
LOG_INFO("Modbus trigger: type=%u, robotPose=(%.3f, %.3f, %.3f, %.3f, %.3f, %.3f)\n",
|
||
triggerValue, robotPose.x, robotPose.y, robotPose.z, robotPose.a, robotPose.b, robotPose.c);
|
||
|
||
TriggerDetection(-1, robotPose);
|
||
return;
|
||
}
|
||
|
||
LOG_WARNING("Unsupported Modbus trigger value: %u\n", triggerValue);
|
||
_UpdateModbusWorkStatus(3);
|
||
if (auto pStatus = GetStatusCallback<IYTireHolePoseStatus>()) {
|
||
pStatus->OnStatusUpdate(QString("Modbus触发值无效:%1").arg(triggerValue).toStdString());
|
||
}
|
||
}
|
||
|
||
bool TireHolePosePresenter::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;
|
||
}
|
||
|
||
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_getGroundCalibPara(xyzData);
|
||
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;
|
||
}
|
||
}
|
||
|
||
bool TireHolePosePresenter::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 || cameraName.isEmpty()) {
|
||
LOG_ERROR("Invalid camera info when saving leveling results\n");
|
||
return false;
|
||
}
|
||
|
||
const QString configPath = PathManager::GetInstance().GetConfigFilePath();
|
||
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());
|
||
return true;
|
||
} catch (const std::exception& e) {
|
||
LOG_ERROR("Exception in SaveLevelingResults: %s\n", e.what());
|
||
return false;
|
||
}
|
||
}
|
||
|
||
bool TireHolePosePresenter::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;
|
||
}
|
||
|
||
const 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;
|
||
return true;
|
||
} catch (const std::exception& e) {
|
||
LOG_ERROR("Exception in LoadLevelingResults: %s\n", e.what());
|
||
return false;
|
||
}
|
||
}
|
||
|
||
void TireHolePosePresenter::DeinitApp()
|
||
{
|
||
LOG_DEBUG("Deinitializing TireHolePosePresenter\n");
|
||
|
||
StopDetection();
|
||
|
||
if (m_pTCPServer) {
|
||
m_pTCPServer->StopServer();
|
||
delete m_pTCPServer;
|
||
m_pTCPServer = nullptr;
|
||
}
|
||
|
||
if (m_pConfigManager) {
|
||
delete m_pConfigManager;
|
||
m_pConfigManager = nullptr;
|
||
}
|
||
|
||
if (m_pDetectPresenter) {
|
||
delete m_pDetectPresenter;
|
||
m_pDetectPresenter = nullptr;
|
||
}
|
||
}
|
||
|
||
bool TireHolePosePresenter::TriggerDetection(int cameraIndex, const RobotPose6D& robotPose)
|
||
{
|
||
if (cameraIndex > 0) {
|
||
SetDefaultCameraIndex(cameraIndex);
|
||
}
|
||
|
||
m_currentRobotPose = robotPose;
|
||
m_requestTimestamp = QDateTime::currentMSecsSinceEpoch();
|
||
|
||
if (!m_bCameraConnected) {
|
||
LOG_WARNING("Camera not connected, cannot trigger detection\n");
|
||
_UpdateModbusWorkStatus(3);
|
||
return false;
|
||
}
|
||
|
||
if (GetCurrentWorkStatus() == WorkStatus::Working) {
|
||
LOG_WARNING("Detection is already running, skip duplicated trigger\n");
|
||
_UpdateModbusWorkStatus(1);
|
||
return false;
|
||
}
|
||
|
||
_InitializeModbusRegisters();
|
||
_ResetModbusResultRegisters();
|
||
_UpdateModbusWorkStatus(1);
|
||
|
||
int ret = StartDetection(cameraIndex, false);
|
||
if (ret != SUCCESS) {
|
||
LOG_ERROR("Failed to trigger detection, error: %d\n", ret);
|
||
_UpdateModbusWorkStatus(3);
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
int TireHolePosePresenter::LoadAndDetect(const QString& fileName)
|
||
{
|
||
LOG_INFO("Loading data from file: %s\n", fileName.toStdString().c_str());
|
||
return LoadDebugDataAndDetect(fileName.toStdString());
|
||
}
|
||
|
||
void TireHolePosePresenter::ReconnectCamera()
|
||
{
|
||
LOG_INFO("Attempting to reconnect cameras\n");
|
||
TryReconnectCameras();
|
||
}
|
||
|
||
VrCornerParam TireHolePosePresenter::GetCornerParams() const
|
||
{
|
||
VrCornerParam params;
|
||
if (m_pConfigManager) {
|
||
const ConfigResult configResult = m_pConfigManager->GetConfigResult();
|
||
params = configResult.algorithmParams.cornerParam;
|
||
return params;
|
||
}
|
||
// 默认值由 VrCornerParam 结构体默认初始化提供
|
||
return params;
|
||
}
|
||
|
||
QString TireHolePosePresenter::GetAlgoVersion() const
|
||
{
|
||
return DetectPresenter::GetAlgoVersion();
|
||
}
|
||
|
||
void TireHolePosePresenter::SetCornerParams(const VrCornerParam& params)
|
||
{
|
||
if (!m_pConfigManager) {
|
||
LOG_WARNING("ConfigManager not initialized, cannot set algorithm params\n");
|
||
return;
|
||
}
|
||
|
||
// 将角点参数写入共享算法参数（类比 WorkpieceHole 风格）
|
||
SystemConfig systemConfig = m_pConfigManager->GetConfig();
|
||
systemConfig.configResult.algorithmParams.cornerParam = params;
|
||
m_pConfigManager->UpdateFullConfig(systemConfig);
|
||
|
||
LOG_INFO("Corner parameters updated\n");
|
||
}
|