251 lines
9.3 KiB
C++
251 lines
9.3 KiB
C++
#include "VrError.h"
|
||
#include "WorkpiecePositionPresenter.h"
|
||
#include "VrLog.h"
|
||
#include <QDebug>
|
||
#include <QDateTime>
|
||
|
||
WorkpiecePositionPresenter::WorkpiecePositionPresenter(QObject *parent)
|
||
: QObject(parent), m_pStatusCallback(nullptr), m_pConfig(nullptr)
|
||
, m_pMarkReceiver(nullptr), m_pEpicEyeDevice(nullptr)
|
||
, m_binocularMarkPort(0), m_bMarkConnected(false), m_bEpicEyeConnected(false)
|
||
, m_pMarkReconnectTimer(new QTimer(this)), m_pEpicEyeReconnectTimer(new QTimer(this))
|
||
, m_bDetecting(false)
|
||
{
|
||
connect(m_pMarkReconnectTimer, &QTimer::timeout, this, &WorkpiecePositionPresenter::onMarkReconnectTimeout);
|
||
connect(m_pEpicEyeReconnectTimer, &QTimer::timeout, this, &WorkpiecePositionPresenter::onEpicEyeReconnectTimeout);
|
||
}
|
||
|
||
WorkpiecePositionPresenter::~WorkpiecePositionPresenter()
|
||
{
|
||
if (m_pMarkReconnectTimer) m_pMarkReconnectTimer->stop();
|
||
if (m_pEpicEyeReconnectTimer) m_pEpicEyeReconnectTimer->stop();
|
||
if (m_pMarkReceiver) { m_pMarkReceiver->Disconnect(); delete m_pMarkReceiver; }
|
||
if (m_pEpicEyeDevice) { m_pEpicEyeDevice->Disconnect(); delete m_pEpicEyeDevice; }
|
||
if (m_pConfig) delete m_pConfig;
|
||
}
|
||
|
||
int WorkpiecePositionPresenter::Init()
|
||
{
|
||
LOG_INFO("WorkpiecePositionPresenter初始化\n");
|
||
UpdateWorkStatus(WorkStatus::InitIng);
|
||
|
||
int ret = InitConfig();
|
||
if (ret != SUCCESS) { LOG_ERR("初始化配置失败: %d\n", ret); UpdateWorkStatus(WorkStatus::Error); return ret; }
|
||
|
||
ret = InitBinocularMarkReceiver();
|
||
if (ret != SUCCESS) { LOG_ERR("初始化BinocularMarkReceiver失败: %d\n", ret); m_pMarkReconnectTimer->start(5000); }
|
||
|
||
ret = InitEpicEyeDevice();
|
||
if (ret != SUCCESS) { LOG_ERR("初始化EpicEyeDevice失败: %d\n", ret); m_pEpicEyeReconnectTimer->start(5000); }
|
||
|
||
UpdateWorkStatus(WorkStatus::Ready);
|
||
LOG_INFO("WorkpiecePositionPresenter初始化完成\n");
|
||
return SUCCESS;
|
||
}
|
||
|
||
void WorkpiecePositionPresenter::SetStatusCallback(IYWorkpiecePositionStatus* pStatus)
|
||
{
|
||
m_pStatusCallback = pStatus;
|
||
}
|
||
|
||
int WorkpiecePositionPresenter::StartDetection()
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_mutex);
|
||
if (m_bDetecting) { LOG_WARN("正在检测中\n"); return ERR_CODE(APP_ERR_EXEC); }
|
||
if (!m_bMarkConnected) { LOG_ERR("BinocularMarkApp未连接\n"); UpdateStatusMessage("BinocularMarkApp未连接"); return -2; }
|
||
if (!m_bEpicEyeConnected) { LOG_ERR("EpicEye设备未连接\n"); UpdateStatusMessage("EpicEye设备未连接"); return -3; }
|
||
|
||
m_bDetecting = true;
|
||
UpdateWorkStatus(WorkStatus::Working);
|
||
LOG_INFO("开始检测\n");
|
||
|
||
int ret = m_pMarkReceiver->TriggerDetection();
|
||
if (ret != SUCCESS) { LOG_ERR("触发BinocularMarkApp检测失败\n"); m_bDetecting = false; UpdateWorkStatus(WorkStatus::Error); return -4; }
|
||
return SUCCESS;
|
||
}
|
||
|
||
int WorkpiecePositionPresenter::StopDetection()
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_mutex);
|
||
if (!m_bDetecting) return SUCCESS;
|
||
m_bDetecting = false;
|
||
UpdateWorkStatus(WorkStatus::Ready);
|
||
LOG_INFO("停止检测\n");
|
||
return SUCCESS;
|
||
}
|
||
|
||
int WorkpiecePositionPresenter::InitConfig()
|
||
{
|
||
if (!IVrConfig::CreateInstance((IVrConfig**)&m_pConfig)) { LOG_ERR("创建配置管理器失败\n"); return ERR_CODE(APP_ERR_EXEC); }
|
||
|
||
std::string configPath = "./config/config.xml";
|
||
int ret = m_pConfig->LoadConfig(configPath, m_configResult);
|
||
if (ret != SUCCESS) { LOG_ERR("加载配置文件失败: %s\n", configPath.c_str()); return -2; }
|
||
|
||
// 从 binocularMarkConfig 获取 BinocularMark 连接信息
|
||
m_binocularMarkIp = m_configResult.binocularMarkConfig.serverIP;
|
||
m_binocularMarkPort = m_configResult.binocularMarkConfig.serverPort;
|
||
|
||
// 从 epicEyeDeviceList 获取 EpicEye 设备信息
|
||
if (m_configResult.epicEyeDeviceList.size() > 0) {
|
||
m_epicEyeIp = m_configResult.epicEyeDeviceList[0].ip;
|
||
} else {
|
||
LOG_WARN("配置文件中未找到EpicEye设备，使用默认IP: 192.168.1.100\n");
|
||
m_epicEyeIp = "192.168.1.100";
|
||
}
|
||
|
||
return SUCCESS;
|
||
}
|
||
|
||
int WorkpiecePositionPresenter::InitBinocularMarkReceiver()
|
||
{
|
||
int ret = IBinocularMarkReceiver::CreateInstance(&m_pMarkReceiver);
|
||
if (ret != SUCCESS) { LOG_ERR("创建BinocularMarkReceiver实例失败\n"); return ERR_CODE(APP_ERR_EXEC); }
|
||
|
||
m_pMarkReceiver->SetMarkResultCallback([this](const std::vector<VrMark3D>& marks, qint64 timestamp, int errorCode) {
|
||
this->OnMarkResult(marks, timestamp, errorCode);
|
||
});
|
||
m_pMarkReceiver->SetConnectionStatusCallback([this](bool connected) { this->OnMarkConnectionChanged(connected); });
|
||
|
||
return ConnectToBinocularMark();
|
||
}
|
||
|
||
int WorkpiecePositionPresenter::InitEpicEyeDevice()
|
||
{
|
||
int ret = IEpicEyeDevice::CreateObject(&m_pEpicEyeDevice);
|
||
if (ret != SUCCESS) { LOG_ERR("创建EpicEyeDevice实例失败\n"); return ERR_CODE(APP_ERR_EXEC); }
|
||
return ConnectToEpicEye();
|
||
}
|
||
|
||
void WorkpiecePositionPresenter::OnMarkResult(const std::vector<VrMark3D>& marks, qint64 timestamp, int errorCode)
|
||
{
|
||
LOG_INFO("收到BinocularMark检测结果，Mark数量: %zu, 错误码: %d\n", marks.size(), errorCode);
|
||
|
||
if (errorCode != 0) {
|
||
LOG_ERR("BinocularMark检测失败: %d\n", errorCode);
|
||
UpdateWorkStatus(WorkStatus::Error);
|
||
UpdateStatusMessage("BinocularMark检测失败");
|
||
m_bDetecting = false;
|
||
return;
|
||
}
|
||
|
||
{ std::lock_guard<std::mutex> lock(m_mutex); m_lastMarks = marks; }
|
||
|
||
PointCloudData pointCloud;
|
||
int ret = m_pEpicEyeDevice->CapturePointCloud(pointCloud, 5000);
|
||
if (ret != SUCCESS) {
|
||
LOG_ERR("获取EpicEye点云数据失败: %d\n", ret);
|
||
UpdateWorkStatus(WorkStatus::Error);
|
||
UpdateStatusMessage("获取EpicEye点云数据失败");
|
||
m_bDetecting = false;
|
||
return;
|
||
}
|
||
|
||
WorkpieceCenterPosition centerPosition;
|
||
ret = CalculateWorkpieceCenter(marks, pointCloud, centerPosition);
|
||
if (ret != SUCCESS) {
|
||
LOG_ERR("计算工件中心点失败: %d\n", ret);
|
||
UpdateWorkStatus(WorkStatus::Error);
|
||
UpdateStatusMessage("计算工件中心点失败");
|
||
m_bDetecting = false;
|
||
if (pointCloud.pData) delete[] pointCloud.pData;
|
||
return;
|
||
}
|
||
|
||
if (pointCloud.pData) delete[] pointCloud.pData;
|
||
|
||
WorkpiecePositionDetectionResult result;
|
||
result.positions.push_back(centerPosition);
|
||
result.marks = marks;
|
||
|
||
if (m_pStatusCallback) m_pStatusCallback->OnDetectionResult(result);
|
||
UpdateWorkStatus(WorkStatus::Completed);
|
||
LOG_INFO("工件定位检测完成，中心点: (%.2f, %.2f, %.2f)\n", centerPosition.x, centerPosition.y, centerPosition.z);
|
||
m_bDetecting = false;
|
||
}
|
||
|
||
void WorkpiecePositionPresenter::OnMarkConnectionChanged(bool connected)
|
||
{
|
||
LOG_INFO("BinocularMark连接状态改变: %s\n", connected ? "已连接" : "断开连接");
|
||
m_bMarkConnected = connected;
|
||
if (m_pStatusCallback) m_pStatusCallback->OnBinocularMarkConnectionChanged(connected);
|
||
|
||
if (connected) {
|
||
m_pMarkReconnectTimer->stop();
|
||
UpdateStatusMessage("BinocularMarkApp已连接");
|
||
} else {
|
||
m_pMarkReconnectTimer->start(5000);
|
||
UpdateStatusMessage("BinocularMarkApp连接断开，正在重连...");
|
||
}
|
||
}
|
||
|
||
int WorkpiecePositionPresenter::CalculateWorkpieceCenter(const std::vector<VrMark3D>& marks, const PointCloudData& pointCloud, WorkpieceCenterPosition& centerPosition)
|
||
{
|
||
// TODO: 实现工件中心点计算算法（由用户后续填充）
|
||
// 示例实现：简单地将所有Mark点的平均值作为中心点
|
||
if (marks.empty()) { LOG_ERR("Mark数据为空\n"); return ERR_CODE(APP_ERR_EXEC); }
|
||
|
||
double sumX = 0.0, sumY = 0.0, sumZ = 0.0;
|
||
for (const auto& mark : marks) {
|
||
sumX += mark.x;
|
||
sumY += mark.y;
|
||
sumZ += mark.z;
|
||
}
|
||
|
||
centerPosition.x = sumX / marks.size();
|
||
centerPosition.y = sumY / marks.size();
|
||
centerPosition.z = sumZ / marks.size();
|
||
centerPosition.roll = 0.0;
|
||
centerPosition.pitch = 0.0;
|
||
centerPosition.yaw = 0.0;
|
||
|
||
LOG_INFO("计算得到工件中心点: (%.2f, %.2f, %.2f)\n", centerPosition.x, centerPosition.y, centerPosition.z);
|
||
return SUCCESS;
|
||
}
|
||
|
||
void WorkpiecePositionPresenter::UpdateWorkStatus(WorkStatus status)
|
||
{
|
||
if (m_pStatusCallback) m_pStatusCallback->OnWorkStatusChanged(status);
|
||
}
|
||
|
||
void WorkpiecePositionPresenter::UpdateStatusMessage(const std::string& message)
|
||
{
|
||
if (m_pStatusCallback) m_pStatusCallback->OnStatusUpdate(message);
|
||
}
|
||
|
||
int WorkpiecePositionPresenter::ConnectToBinocularMark()
|
||
{
|
||
LOG_INFO("连接BinocularMarkApp: %s:%d\n", m_binocularMarkIp.c_str(), m_binocularMarkPort);
|
||
return m_pMarkReceiver->Connect(m_binocularMarkIp, m_binocularMarkPort);
|
||
}
|
||
|
||
int WorkpiecePositionPresenter::ConnectToEpicEye()
|
||
{
|
||
LOG_INFO("连接EpicEye设备: %s\n", m_epicEyeIp.c_str());
|
||
int ret = m_pEpicEyeDevice->Connect(m_epicEyeIp);
|
||
if (ret != SUCCESS) {
|
||
LOG_ERR("连接EpicEye设备失败\n");
|
||
m_bEpicEyeConnected = false;
|
||
if (m_pStatusCallback) m_pStatusCallback->OnEpicEyeConnectionChanged(false);
|
||
return ERR_CODE(APP_ERR_EXEC);
|
||
}
|
||
|
||
m_bEpicEyeConnected = true;
|
||
if (m_pStatusCallback) m_pStatusCallback->OnEpicEyeConnectionChanged(true);
|
||
m_pEpicEyeReconnectTimer->stop();
|
||
UpdateStatusMessage("EpicEye设备已连接");
|
||
return SUCCESS;
|
||
}
|
||
|
||
void WorkpiecePositionPresenter::onMarkReconnectTimeout()
|
||
{
|
||
LOG_INFO("尝试重连BinocularMarkApp\n");
|
||
ConnectToBinocularMark();
|
||
}
|
||
|
||
void WorkpiecePositionPresenter::onEpicEyeReconnectTimeout()
|
||
{
|
||
LOG_INFO("尝试重连EpicEye设备\n");
|
||
ConnectToEpicEye();
|
||
}
|