288 lines
8.7 KiB
C++
288 lines
8.7 KiB
C++
#include "GrabBagPresenter.h"
|
||
#include "VrError.h"
|
||
#include "VrSimpleLog.h"
|
||
#include <QtCore/QCoreApplication>
|
||
#include <QtCore/QFileInfo>
|
||
#include <QtCore/QDir>
|
||
|
||
GrabBagPresenter::GrabBagPresenter()
|
||
: m_vrConfig(nullptr)
|
||
, m_vrEyeDevice(nullptr)
|
||
, m_pStatus(nullptr)
|
||
, m_pRobotProtocol(nullptr)
|
||
{
|
||
|
||
}
|
||
|
||
GrabBagPresenter::~GrabBagPresenter()
|
||
{
|
||
// 释放机械臂协议
|
||
if (m_pRobotProtocol) {
|
||
m_pRobotProtocol->Deinitialize();
|
||
delete m_pRobotProtocol;
|
||
m_pRobotProtocol = nullptr;
|
||
}
|
||
|
||
// 释放其他资源
|
||
if (m_vrEyeDevice) {
|
||
// 关闭设备
|
||
m_vrEyeDevice->CloseDevice();
|
||
}
|
||
}
|
||
|
||
int GrabBagPresenter::Init()
|
||
{
|
||
// 初始化VrConfig模块
|
||
IVrConfig::CreateInstance(&m_vrConfig);
|
||
|
||
// 获取可执行文件路径
|
||
QString exePath = QCoreApplication::applicationFilePath();
|
||
|
||
// 获取可执行文件所在目录
|
||
QString configPath = QFileInfo(exePath).absoluteDir().path() + "/config.xml";
|
||
|
||
// 读取IP列表
|
||
ConfigResult configResult = m_vrConfig->LoadConfig(configPath.toStdString());
|
||
if (configResult.cameraList.size() == 0) {
|
||
m_pStatus->OnStatusUpdate("读取配置信息列表失败");
|
||
m_pStatus->OnCamera1StatusChanged(false);
|
||
m_pStatus->OnCamera2StatusChanged(false);
|
||
m_pStatus->OnCameraCountChanged(0);
|
||
return ERR_CODE(DEV_CONFIG_ERR);
|
||
}
|
||
|
||
// 初始化机械臂协议
|
||
int nRet = InitRobotProtocol();
|
||
if (nRet != 0) {
|
||
m_pStatus->OnStatusUpdate("机械臂服务初始化失败");
|
||
m_pStatus->OnWorkStatusChanged(WorkStatus::Error);
|
||
} else {
|
||
m_pStatus->OnStatusUpdate("机械臂服务初始化成功");
|
||
}
|
||
|
||
// 通知UI相机个数
|
||
int cameraCount = configResult.cameraList.size();
|
||
m_pStatus->OnCameraCountChanged(cameraCount);
|
||
|
||
// 初始化VrEyeDevice模块
|
||
IVrEyeDevice::CreateObject(&m_vrEyeDevice);
|
||
|
||
nRet = m_vrEyeDevice->InitDevice();
|
||
ERR_CODE_RETURN(nRet);
|
||
|
||
|
||
if (configResult.cameraList.size() >= 1) {
|
||
// 尝试打开相机1
|
||
nRet = m_vrEyeDevice->OpenDevice(configResult.cameraList[0].ip.c_str(), &GrabBagPresenter::_StaticCameraNotify, this);
|
||
// 通过回调更新相机1状态
|
||
m_pStatus->OnCamera1StatusChanged(SUCCESS == nRet);
|
||
m_pStatus->OnStatusUpdate(SUCCESS == nRet ? "相机1连接成功" : "相机1连接失败");
|
||
}
|
||
else {
|
||
m_pStatus->OnCamera1StatusChanged(false);
|
||
}
|
||
m_pStatus->OnWorkStatusChanged(SUCCESS == nRet ? WorkStatus::Ready : WorkStatus::Error);
|
||
ERR_CODE_RETURN(nRet);
|
||
|
||
if (configResult.cameraList.size() >= 2) {
|
||
// 尝试打开相机2
|
||
nRet = m_vrEyeDevice->OpenDevice(configResult.cameraList[1].ip.c_str(), &GrabBagPresenter::_StaticCameraNotify, this);
|
||
// 通过回调更新相机2状态
|
||
m_pStatus->OnCamera2StatusChanged(SUCCESS == nRet);
|
||
m_pStatus->OnStatusUpdate(SUCCESS == nRet ? "相机2连接成功" : "相机2连接失败");
|
||
}
|
||
else {
|
||
// 如果只有一个相机，则将相机2状态设为未连接
|
||
m_pStatus->OnCamera2StatusChanged(false);
|
||
}
|
||
ERR_CODE_RETURN(nRet);
|
||
|
||
m_pStatus->OnStatusUpdate("设备初始化完成");
|
||
m_pStatus->OnWorkStatusChanged(SUCCESS == nRet ? WorkStatus::Ready : WorkStatus::Error);
|
||
return SUCCESS;
|
||
}
|
||
|
||
// 初始化机械臂协议
|
||
int GrabBagPresenter::InitRobotProtocol()
|
||
{
|
||
LOG_DEBUG("GrabBagPresenter::InitRobotProtocol - 开始初始化机械臂协议\n");
|
||
|
||
// 创建RobotProtocol实例
|
||
if(nullptr == m_pRobotProtocol){
|
||
m_pRobotProtocol = new RobotProtocol();
|
||
}
|
||
|
||
// 设置连接状态回调
|
||
m_pRobotProtocol->SetConnectionCallback([this](bool connected) {
|
||
this->OnRobotConnectionChanged(connected);
|
||
});
|
||
|
||
// 设置工作信号回调
|
||
m_pRobotProtocol->SetWorkSignalCallback([this](bool startWork) {
|
||
this->OnRobotWorkSignal(startWork);
|
||
});
|
||
|
||
// 初始化协议服务（使用端口503，避免与原有ModbusTCP冲突）
|
||
int nRet = m_pRobotProtocol->Initialize();
|
||
ERR_CODE_RETURN(nRet);
|
||
|
||
LOG_INFO("GrabBagPresenter::InitRobotProtocol - 机械臂协议初始化成功\n");
|
||
return SUCCESS;
|
||
|
||
}
|
||
|
||
// 机械臂协议回调函数实现
|
||
void GrabBagPresenter::OnRobotConnectionChanged(bool connected)
|
||
{
|
||
LOG_INFO("GrabBagPresenter::OnRobotConnectionChanged - 机械臂连接状态变化:%s\n", (connected ? "已连接" : "已断开"));
|
||
|
||
if (m_pStatus) {
|
||
m_pStatus->OnRobotConnectionChanged(connected);
|
||
m_pStatus->OnStatusUpdate(connected ? "机械臂已连接" : "机械臂连接断开");
|
||
}
|
||
}
|
||
|
||
void GrabBagPresenter::OnRobotWorkSignal(bool startWork)
|
||
{
|
||
LOG_INFO("GrabBagPresenter::OnRobotWorkSignal - 收到机械臂工作信号:%s\n", (startWork ? "开始工作" : "停止工作"));
|
||
|
||
if (nullptr == m_pStatus) {
|
||
return;
|
||
}
|
||
if (startWork) {
|
||
m_pStatus->OnStatusUpdate("收到开始工作信号，启动检测");
|
||
StartDetection();
|
||
} else {
|
||
m_pStatus->OnStatusUpdate("收到停止工作信号，停止检测");
|
||
StopDetection();
|
||
}
|
||
}
|
||
|
||
void GrabBagPresenter::SetStatusCallback(IYGrabBagStatus* status)
|
||
{
|
||
m_pStatus = status;
|
||
}
|
||
|
||
// 模拟检测函数，用于演示
|
||
int GrabBagPresenter::StartDetection()
|
||
{
|
||
LOG_INFO("GrabBagPresenter::StartDetection - 开始检测\n");
|
||
|
||
// 通知UI工作状态变更为"正在工作"
|
||
if (m_pStatus) {
|
||
m_pStatus->OnWorkStatusChanged(WorkStatus::Working);
|
||
}
|
||
|
||
// 创建测试图像
|
||
QImage image(":/resource/result_image.jpg");
|
||
|
||
// 创建测试数据
|
||
DetectionResult testResult;
|
||
testResult.image = image;
|
||
|
||
// 创建第一层数据
|
||
GrabBagPositionLayout layer1;
|
||
layer1.layerIndex = 1;
|
||
|
||
GrabBagPosition pos1;
|
||
pos1.x = 100.5;
|
||
pos1.y = 200.3;
|
||
pos1.z = 50.22;
|
||
pos1.roll = 45.0;
|
||
pos1.pitch = 10.0;
|
||
pos1.yaw = 20.0;
|
||
|
||
GrabBagPosition pos2;
|
||
pos2.x = 110.8;
|
||
pos2.y = 220.5;
|
||
pos2.z = 52.1;
|
||
pos2.roll = 48.5;
|
||
pos2.pitch = 12.3;
|
||
pos2.yaw = 22.7;
|
||
|
||
layer1.position.push_back(pos1);
|
||
layer1.position.push_back(pos2);
|
||
|
||
// 创建第二层数据
|
||
GrabBagPositionLayout layer2;
|
||
layer2.layerIndex = 2;
|
||
|
||
GrabBagPosition pos3;
|
||
pos3.x = 150.2;
|
||
pos3.y = 180.7;
|
||
pos3.z = 55.0;
|
||
pos3.roll = 30.0;
|
||
pos3.pitch = 15.0;
|
||
pos3.yaw = 25.0;
|
||
|
||
layer2.position.push_back(pos3);
|
||
|
||
// 添加层数据到结果
|
||
testResult.positionLayout.push_back(layer1);
|
||
testResult.positionLayout.push_back(layer2);
|
||
|
||
// 调用检测结果回调函数
|
||
m_pStatus->OnDetectionResult(testResult);
|
||
|
||
// 更新状态
|
||
m_pStatus->OnStatusUpdate("检测完成，发现3个目标");
|
||
|
||
// 检测完成后，将工作状态更新为"完成"
|
||
if (m_pStatus) {
|
||
m_pStatus->OnWorkStatusChanged(WorkStatus::Completed);
|
||
}
|
||
|
||
// 更新机械臂协议状态
|
||
if (m_pRobotProtocol) {
|
||
|
||
// 更新检测结果坐标（以第一个检测到的目标为例）
|
||
if (!testResult.positionLayout.empty() && !testResult.positionLayout[0].position.empty()) {
|
||
const auto& firstPos = testResult.positionLayout[0].position[0];
|
||
RobotProtocol::RobotCoordinate detectedCoord;
|
||
detectedCoord.x = firstPos.x;
|
||
detectedCoord.y = firstPos.y;
|
||
detectedCoord.z = firstPos.z;
|
||
detectedCoord.rx = firstPos.roll;
|
||
detectedCoord.ry = firstPos.pitch;
|
||
detectedCoord.rz = firstPos.yaw;
|
||
m_pRobotProtocol->SetRobotCoordinate(detectedCoord);
|
||
|
||
LOG_INFO("GrabBagPresenter::StartDetection - 更新检测结果坐标: X=%.2f, Y=%.2f, Z=%.2f\n",
|
||
detectedCoord.x, detectedCoord.y, detectedCoord.z);
|
||
}
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
int GrabBagPresenter::StopDetection()
|
||
{
|
||
LOG_INFO("GrabBagPresenter::StopDetection - 停止检测\n");
|
||
|
||
// 通知UI工作状态变更为"就绪"
|
||
if (m_pStatus) {
|
||
m_pStatus->OnWorkStatusChanged(WorkStatus::Ready);
|
||
m_pStatus->OnStatusUpdate("检测已停止");
|
||
}
|
||
|
||
|
||
return 0;
|
||
}
|
||
|
||
// 静态回调函数实现
|
||
void GrabBagPresenter::_StaticCameraNotify(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pInfoParam)
|
||
{
|
||
// 从pInfoParam获取this指针，转换回GrabBagPresenter*类型
|
||
GrabBagPresenter* pThis = reinterpret_cast<GrabBagPresenter*>(pInfoParam);
|
||
if (pThis)
|
||
{
|
||
// 调用实例的非静态成员函数
|
||
pThis->_CameraNotify(eStatus, pExtData, nDataLength, pInfoParam);
|
||
}
|
||
}
|
||
|
||
void GrabBagPresenter::_CameraNotify(EVzDeviceWorkStatus eStatus, void *pExtData, unsigned int nDataLength, void *pInfoParam)
|
||
{
|
||
|
||
}
|