419 lines
19 KiB
C++
419 lines
19 KiB
C++
#include "VrConfig.h"
|
||
#include <iostream>
|
||
#include <vector>
|
||
#include <string>
|
||
|
||
#include "VrLog.h"
|
||
#include "ConfigXmlUtils.h"
|
||
|
||
using namespace tinyxml2;
|
||
|
||
namespace {
|
||
|
||
constexpr int kSoftwareProductNumberingVersion = 2;
|
||
|
||
int ToSoftwareWorkpieceNumber(int storedNumber, bool usesSoftwareProductNumbering)
|
||
{
|
||
if (usesSoftwareProductNumbering) {
|
||
return storedNumber;
|
||
}
|
||
|
||
// 旧配置按算法 test 编号保存；运行时统一转换为软件产品编号。
|
||
switch (storedNumber) {
|
||
case 3: return 4;
|
||
case 4: return 5;
|
||
case 5: return 3;
|
||
default: return storedNumber;
|
||
}
|
||
}
|
||
|
||
void LoadWorkpieceProfileParams(
|
||
XMLElement* parentElement,
|
||
VrWorkpieceAlgorithmProfile& profile)
|
||
{
|
||
if (!parentElement) {
|
||
return;
|
||
}
|
||
|
||
XMLElement* workpieceHoleParamElement = parentElement->FirstChildElement("WorkpieceHoleParam");
|
||
if (workpieceHoleParamElement)
|
||
{
|
||
if (workpieceHoleParamElement->Attribute("workpieceType"))
|
||
profile.workpieceHoleParam.workpieceType = workpieceHoleParamElement->IntAttribute("workpieceType");
|
||
if (workpieceHoleParamElement->Attribute("holeDiameter"))
|
||
profile.workpieceHoleParam.holeDiameter = workpieceHoleParamElement->DoubleAttribute("holeDiameter");
|
||
if (workpieceHoleParamElement->Attribute("bigHoleDiameter"))
|
||
profile.workpieceHoleParam.bigHoleDiameter = workpieceHoleParamElement->DoubleAttribute("bigHoleDiameter");
|
||
if (workpieceHoleParamElement->Attribute("holeDist_L"))
|
||
profile.workpieceHoleParam.holeDist_L = workpieceHoleParamElement->DoubleAttribute("holeDist_L");
|
||
if (workpieceHoleParamElement->Attribute("holeDist_W"))
|
||
profile.workpieceHoleParam.holeDist_W = workpieceHoleParamElement->DoubleAttribute("holeDist_W");
|
||
if (workpieceHoleParamElement->Attribute("xLen"))
|
||
profile.workpieceHoleParam.xLen = workpieceHoleParamElement->DoubleAttribute("xLen");
|
||
if (workpieceHoleParamElement->Attribute("yLen"))
|
||
profile.workpieceHoleParam.yLen = workpieceHoleParamElement->DoubleAttribute("yLen");
|
||
if (workpieceHoleParamElement->Attribute("H"))
|
||
profile.workpieceHoleParam.H = workpieceHoleParamElement->DoubleAttribute("H");
|
||
}
|
||
|
||
XMLElement* filterParamElement = parentElement->FirstChildElement("FilterParam");
|
||
if (filterParamElement)
|
||
{
|
||
if (filterParamElement->Attribute("continuityTh"))
|
||
profile.filterParam.continuityTh = filterParamElement->DoubleAttribute("continuityTh");
|
||
if (filterParamElement->Attribute("outlierTh"))
|
||
profile.filterParam.outlierTh = filterParamElement->DoubleAttribute("outlierTh");
|
||
}
|
||
|
||
XMLElement* lineSegParamElement = parentElement->FirstChildElement("LineSegParam");
|
||
if (lineSegParamElement)
|
||
{
|
||
if (lineSegParamElement->Attribute("distScale"))
|
||
profile.lineSegParam.distScale = lineSegParamElement->DoubleAttribute("distScale");
|
||
if (lineSegParamElement->Attribute("segGapTh_y"))
|
||
profile.lineSegParam.segGapTh_y = lineSegParamElement->DoubleAttribute("segGapTh_y");
|
||
if (lineSegParamElement->Attribute("segGapTh_z"))
|
||
profile.lineSegParam.segGapTh_z = lineSegParamElement->DoubleAttribute("segGapTh_z");
|
||
}
|
||
|
||
XMLElement* growParamElement = parentElement->FirstChildElement("GrowParam");
|
||
if (growParamElement)
|
||
{
|
||
if (growParamElement->Attribute("maxLineSkipNum"))
|
||
profile.growParam.maxLineSkipNum = growParamElement->IntAttribute("maxLineSkipNum");
|
||
if (growParamElement->Attribute("yDeviation_max"))
|
||
profile.growParam.yDeviation_max = growParamElement->DoubleAttribute("yDeviation_max");
|
||
if (growParamElement->Attribute("maxSkipDistance"))
|
||
profile.growParam.maxSkipDistance = growParamElement->DoubleAttribute("maxSkipDistance");
|
||
if (growParamElement->Attribute("zDeviation_max"))
|
||
profile.growParam.zDeviation_max = growParamElement->DoubleAttribute("zDeviation_max");
|
||
if (growParamElement->Attribute("minLTypeTreeLen"))
|
||
profile.growParam.minLTypeTreeLen = growParamElement->DoubleAttribute("minLTypeTreeLen");
|
||
if (growParamElement->Attribute("minVTypeTreeLen"))
|
||
profile.growParam.minVTypeTreeLen = growParamElement->DoubleAttribute("minVTypeTreeLen");
|
||
}
|
||
|
||
XMLElement* toolParamElement = parentElement->FirstChildElement("ToolParam");
|
||
if (toolParamElement)
|
||
{
|
||
if (toolParamElement->Attribute("eulerOrder"))
|
||
profile.toolParam.eulerOrder = toolParamElement->IntAttribute("eulerOrder");
|
||
if (toolParamElement->Attribute("rotX"))
|
||
profile.toolParam.rotX = toolParamElement->DoubleAttribute("rotX");
|
||
if (toolParamElement->Attribute("rotY"))
|
||
profile.toolParam.rotY = toolParamElement->DoubleAttribute("rotY");
|
||
if (toolParamElement->Attribute("rotZ"))
|
||
profile.toolParam.rotZ = toolParamElement->DoubleAttribute("rotZ");
|
||
if (toolParamElement->Attribute("offsetX"))
|
||
profile.toolParam.offsetX = toolParamElement->DoubleAttribute("offsetX");
|
||
if (toolParamElement->Attribute("offsetY"))
|
||
profile.toolParam.offsetY = toolParamElement->DoubleAttribute("offsetY");
|
||
if (toolParamElement->Attribute("offsetZ"))
|
||
profile.toolParam.offsetZ = toolParamElement->DoubleAttribute("offsetZ");
|
||
}
|
||
}
|
||
|
||
void SaveWorkpieceProfileParams(
|
||
XMLDocument& doc,
|
||
XMLElement* algoParamsElement,
|
||
const VrWorkpieceAlgorithmProfile& profile)
|
||
{
|
||
XMLElement* profileElement = doc.NewElement("WorkpieceParamGroup");
|
||
profileElement->SetAttribute("workpieceNumber", profile.workpieceNumber);
|
||
algoParamsElement->InsertEndChild(profileElement);
|
||
|
||
XMLElement* workpieceHoleParamElement = doc.NewElement("WorkpieceHoleParam");
|
||
workpieceHoleParamElement->SetAttribute("workpieceType", profile.workpieceHoleParam.workpieceType);
|
||
workpieceHoleParamElement->SetAttribute("holeDiameter", profile.workpieceHoleParam.holeDiameter);
|
||
workpieceHoleParamElement->SetAttribute("bigHoleDiameter", profile.workpieceHoleParam.bigHoleDiameter);
|
||
workpieceHoleParamElement->SetAttribute("holeDist_L", profile.workpieceHoleParam.holeDist_L);
|
||
workpieceHoleParamElement->SetAttribute("holeDist_W", profile.workpieceHoleParam.holeDist_W);
|
||
workpieceHoleParamElement->SetAttribute("xLen", profile.workpieceHoleParam.xLen);
|
||
workpieceHoleParamElement->SetAttribute("yLen", profile.workpieceHoleParam.yLen);
|
||
workpieceHoleParamElement->SetAttribute("H", profile.workpieceHoleParam.H);
|
||
profileElement->InsertEndChild(workpieceHoleParamElement);
|
||
|
||
XMLElement* filterParamElement = doc.NewElement("FilterParam");
|
||
filterParamElement->SetAttribute("continuityTh", profile.filterParam.continuityTh);
|
||
filterParamElement->SetAttribute("outlierTh", profile.filterParam.outlierTh);
|
||
profileElement->InsertEndChild(filterParamElement);
|
||
|
||
XMLElement* lineSegParamElement = doc.NewElement("LineSegParam");
|
||
lineSegParamElement->SetAttribute("distScale", profile.lineSegParam.distScale);
|
||
lineSegParamElement->SetAttribute("segGapTh_y", profile.lineSegParam.segGapTh_y);
|
||
lineSegParamElement->SetAttribute("segGapTh_z", profile.lineSegParam.segGapTh_z);
|
||
profileElement->InsertEndChild(lineSegParamElement);
|
||
|
||
XMLElement* growParamElement = doc.NewElement("GrowParam");
|
||
growParamElement->SetAttribute("maxLineSkipNum", profile.growParam.maxLineSkipNum);
|
||
growParamElement->SetAttribute("yDeviation_max", profile.growParam.yDeviation_max);
|
||
growParamElement->SetAttribute("maxSkipDistance", profile.growParam.maxSkipDistance);
|
||
growParamElement->SetAttribute("zDeviation_max", profile.growParam.zDeviation_max);
|
||
growParamElement->SetAttribute("minLTypeTreeLen", profile.growParam.minLTypeTreeLen);
|
||
growParamElement->SetAttribute("minVTypeTreeLen", profile.growParam.minVTypeTreeLen);
|
||
profileElement->InsertEndChild(growParamElement);
|
||
|
||
XMLElement* toolParamElement = doc.NewElement("ToolParam");
|
||
toolParamElement->SetAttribute("eulerOrder", profile.toolParam.eulerOrder);
|
||
toolParamElement->SetAttribute("rotX", profile.toolParam.rotX);
|
||
toolParamElement->SetAttribute("rotY", profile.toolParam.rotY);
|
||
toolParamElement->SetAttribute("rotZ", profile.toolParam.rotZ);
|
||
toolParamElement->SetAttribute("offsetX", profile.toolParam.offsetX);
|
||
toolParamElement->SetAttribute("offsetY", profile.toolParam.offsetY);
|
||
toolParamElement->SetAttribute("offsetZ", profile.toolParam.offsetZ);
|
||
profileElement->InsertEndChild(toolParamElement);
|
||
}
|
||
|
||
} // namespace
|
||
|
||
CVrConfig::CVrConfig() : m_pNotify(nullptr)
|
||
{
|
||
// 构造函数
|
||
}
|
||
|
||
CVrConfig::~CVrConfig()
|
||
{
|
||
// 析构函数
|
||
}
|
||
|
||
int CVrConfig::LoadConfig(const std::string& filePath, ConfigResult& configResult)
|
||
{
|
||
// 使用tinyxml2库加载XML文件
|
||
XMLDocument doc;
|
||
XMLError err = doc.LoadFile(filePath.c_str());
|
||
if (err != XML_SUCCESS)
|
||
{
|
||
LOG_ERR("open config file failed: %s\n", filePath.c_str());
|
||
return LOAD_CONFIG_FILE_NOT_FOUND;
|
||
}
|
||
|
||
// 获取根元素
|
||
XMLElement* root = doc.RootElement();
|
||
if (!root || std::string(root->Name()) != "WorkpieceHoleConfig")
|
||
{
|
||
std::cerr << "config file format error: root element is not WorkpieceHoleConfig" << std::endl;
|
||
return LOAD_CONFIG_INVALID_FORMAT;
|
||
}
|
||
|
||
const int productNumberingVersion =
|
||
root->IntAttribute("productNumberingVersion", 1);
|
||
const bool usesSoftwareProductNumbering =
|
||
productNumberingVersion >= kSoftwareProductNumberingVersion;
|
||
if (!usesSoftwareProductNumbering) {
|
||
LOG_INFO("Migrating legacy workpiece profiles to software product numbering\n");
|
||
}
|
||
|
||
// 解析摄像头列表
|
||
ConfigXmlUtils::LoadCameraList(root, configResult.cameraList);
|
||
|
||
// 解析设备列表
|
||
XMLElement* devicesElement = root->FirstChildElement("Devices");
|
||
if (devicesElement)
|
||
{
|
||
XMLElement* deviceElement = devicesElement->FirstChildElement("Device");
|
||
while (deviceElement)
|
||
{
|
||
DeviceInfo device;
|
||
if (deviceElement->Attribute("name"))
|
||
device.name = deviceElement->Attribute("name");
|
||
|
||
if (deviceElement->Attribute("ip"))
|
||
device.ip = deviceElement->Attribute("ip");
|
||
|
||
configResult.deviceList.push_back(device);
|
||
deviceElement = deviceElement->NextSiblingElement("Device");
|
||
}
|
||
}
|
||
|
||
// 每次加载都从完整的工件默认参数组开始，避免复用对象时残留旧数据。
|
||
configResult.algorithmParams = VrAlgorithmParams();
|
||
|
||
// 解析算法参数
|
||
XMLElement* algoParamsElement = root->FirstChildElement("AlgorithmParams");
|
||
if (algoParamsElement)
|
||
{
|
||
XMLElement* profileElement = algoParamsElement->FirstChildElement("WorkpieceParamGroup");
|
||
if (profileElement) {
|
||
while (profileElement) {
|
||
const int storedWorkpieceNumber =
|
||
profileElement->IntAttribute("workpieceNumber", 0);
|
||
const int workpieceNumber = ToSoftwareWorkpieceNumber(
|
||
storedWorkpieceNumber, usesSoftwareProductNumbering);
|
||
VrWorkpieceAlgorithmProfile* workpieceProfile =
|
||
configResult.algorithmParams.FindWorkpieceProfile(workpieceNumber);
|
||
if (workpieceProfile) {
|
||
LoadWorkpieceProfileParams(profileElement, *workpieceProfile);
|
||
}
|
||
profileElement = profileElement->NextSiblingElement("WorkpieceParamGroup");
|
||
}
|
||
} else {
|
||
// 兼容旧版单套参数：旧参数归入工件1，其余工件保留 test 默认值。
|
||
VrWorkpieceAlgorithmProfile* workpieceProfile =
|
||
configResult.algorithmParams.FindWorkpieceProfile(1);
|
||
if (workpieceProfile) {
|
||
LoadWorkpieceProfileParams(algoParamsElement, *workpieceProfile);
|
||
}
|
||
}
|
||
|
||
// 解析多相机平面校准参数
|
||
ConfigXmlUtils::LoadPlaneCalibParams(algoParamsElement, configResult.algorithmParams.planeCalibParam);
|
||
}
|
||
|
||
// 解析调试参数
|
||
ConfigXmlUtils::LoadDebugParam(root, configResult.debugParam);
|
||
|
||
// 解析串口配置
|
||
ConfigXmlUtils::LoadSerialConfig(root, configResult.serialConfig);
|
||
|
||
// 解析手眼标定矩阵列表（支持多相机）
|
||
ConfigXmlUtils::LoadHandEyeCalibMatrixs(root, configResult.handEyeCalibMatrixList);
|
||
|
||
// 解析TCP服务端配置（PLC和机械臂）
|
||
XMLElement* tcpServerConfigElement = root->FirstChildElement("TcpServerConfig");
|
||
if (tcpServerConfigElement)
|
||
{
|
||
if (tcpServerConfigElement->Attribute("plcServerIp"))
|
||
configResult.plcRobotServerConfig.plcServerIp = tcpServerConfigElement->Attribute("plcServerIp");
|
||
if (tcpServerConfigElement->Attribute("plcServerPort"))
|
||
configResult.plcRobotServerConfig.plcServerPort = tcpServerConfigElement->IntAttribute("plcServerPort");
|
||
if (tcpServerConfigElement->Attribute("robotServerIp"))
|
||
configResult.plcRobotServerConfig.robotServerIp = tcpServerConfigElement->Attribute("robotServerIp");
|
||
if (tcpServerConfigElement->Attribute("robotServerPort"))
|
||
configResult.plcRobotServerConfig.robotServerPort = tcpServerConfigElement->IntAttribute("robotServerPort");
|
||
|
||
// 解析PLC寄存器地址配置
|
||
if (tcpServerConfigElement->Attribute("addrPhotoRequest"))
|
||
configResult.plcRobotServerConfig.registerConfig.addrPhotoRequest = tcpServerConfigElement->IntAttribute("addrPhotoRequest");
|
||
if (tcpServerConfigElement->Attribute("addrDataComplete"))
|
||
configResult.plcRobotServerConfig.registerConfig.addrDataComplete = tcpServerConfigElement->IntAttribute("addrDataComplete");
|
||
if (tcpServerConfigElement->Attribute("addrCoordDataStart"))
|
||
configResult.plcRobotServerConfig.registerConfig.addrCoordDataStart = tcpServerConfigElement->IntAttribute("addrCoordDataStart");
|
||
|
||
// 解析姿态输出顺序配置
|
||
if (tcpServerConfigElement->Attribute("poseOutputOrder"))
|
||
configResult.plcRobotServerConfig.poseOutputOrder = tcpServerConfigElement->IntAttribute("poseOutputOrder");
|
||
else
|
||
configResult.plcRobotServerConfig.poseOutputOrder = POSE_ORDER_RPY; // 默认RPY
|
||
|
||
// 解析方向向量反向配置
|
||
if (tcpServerConfigElement->Attribute("dirVectorInvert"))
|
||
configResult.plcRobotServerConfig.dirVectorInvert = tcpServerConfigElement->IntAttribute("dirVectorInvert");
|
||
else
|
||
configResult.plcRobotServerConfig.dirVectorInvert = DIR_INVERT_YZ; // 默认YZ反向（兼容原有行为）
|
||
|
||
// 解析数据字节序配置
|
||
if (tcpServerConfigElement->Attribute("byteOrder"))
|
||
configResult.plcRobotServerConfig.byteOrder = tcpServerConfigElement->IntAttribute("byteOrder");
|
||
else
|
||
configResult.plcRobotServerConfig.byteOrder = BYTE_ORDER_BIG_ENDIAN; // 默认大端序
|
||
}
|
||
|
||
return LOAD_CONFIG_SUCCESS;
|
||
}
|
||
|
||
bool CVrConfig::SaveConfig(const std::string& filePath, ConfigResult& configResult)
|
||
{
|
||
// 创建XML文档
|
||
XMLDocument doc;
|
||
|
||
// 添加声明
|
||
XMLDeclaration* declaration = doc.NewDeclaration("xml version=\"1.0\" encoding=\"UTF-8\"");
|
||
doc.InsertFirstChild(declaration);
|
||
|
||
// 创建根元素
|
||
XMLElement* root = doc.NewElement("WorkpieceHoleConfig");
|
||
root->SetAttribute("productNumberingVersion", kSoftwareProductNumberingVersion);
|
||
doc.InsertEndChild(root);
|
||
|
||
// 添加摄像头列表
|
||
ConfigXmlUtils::SaveCameraList(doc, root, configResult.cameraList);
|
||
|
||
// 添加设备列表
|
||
XMLElement* devicesElement = doc.NewElement("Devices");
|
||
root->InsertEndChild(devicesElement);
|
||
|
||
for (const auto& device : configResult.deviceList)
|
||
{
|
||
XMLElement* deviceElement = doc.NewElement("Device");
|
||
deviceElement->SetAttribute("name", device.name.c_str());
|
||
deviceElement->SetAttribute("ip", device.ip.c_str());
|
||
devicesElement->InsertEndChild(deviceElement);
|
||
}
|
||
|
||
// 添加算法参数
|
||
XMLElement* algoParamsElement = doc.NewElement("AlgorithmParams");
|
||
root->InsertEndChild(algoParamsElement);
|
||
|
||
// 每个工件保存一套独立算法参数。
|
||
for (int workpieceNumber = 1; workpieceNumber <= 5; ++workpieceNumber) {
|
||
const VrWorkpieceAlgorithmProfile* workpieceProfile =
|
||
configResult.algorithmParams.FindWorkpieceProfile(workpieceNumber);
|
||
if (workpieceProfile) {
|
||
SaveWorkpieceProfileParams(doc, algoParamsElement, *workpieceProfile);
|
||
}
|
||
}
|
||
|
||
// 添加多相机平面校准参数
|
||
ConfigXmlUtils::SavePlaneCalibParams(doc, algoParamsElement, configResult.algorithmParams.planeCalibParam);
|
||
|
||
// 添加调试参数
|
||
ConfigXmlUtils::SaveDebugParam(doc, root, configResult.debugParam);
|
||
|
||
// 添加串口配置
|
||
ConfigXmlUtils::SaveSerialConfig(doc, root, configResult.serialConfig);
|
||
|
||
// 添加手眼标定矩阵列表（支持多相机）
|
||
ConfigXmlUtils::SaveHandEyeCalibMatrixs(doc, root, configResult.handEyeCalibMatrixList);
|
||
|
||
// 添加TCP服务端配置（PLC和机械臂）
|
||
XMLElement* tcpServerConfigElement = doc.NewElement("TcpServerConfig");
|
||
tcpServerConfigElement->SetAttribute("plcServerIp", configResult.plcRobotServerConfig.plcServerIp.c_str());
|
||
tcpServerConfigElement->SetAttribute("plcServerPort", configResult.plcRobotServerConfig.plcServerPort);
|
||
tcpServerConfigElement->SetAttribute("robotServerIp", configResult.plcRobotServerConfig.robotServerIp.c_str());
|
||
tcpServerConfigElement->SetAttribute("robotServerPort", configResult.plcRobotServerConfig.robotServerPort);
|
||
// 保存PLC寄存器地址配置
|
||
tcpServerConfigElement->SetAttribute("addrPhotoRequest", configResult.plcRobotServerConfig.registerConfig.addrPhotoRequest);
|
||
tcpServerConfigElement->SetAttribute("addrDataComplete", configResult.plcRobotServerConfig.registerConfig.addrDataComplete);
|
||
tcpServerConfigElement->SetAttribute("addrCoordDataStart", configResult.plcRobotServerConfig.registerConfig.addrCoordDataStart);
|
||
// 保存姿态输出顺序配置
|
||
tcpServerConfigElement->SetAttribute("poseOutputOrder", configResult.plcRobotServerConfig.poseOutputOrder);
|
||
// 保存方向向量反向配置
|
||
tcpServerConfigElement->SetAttribute("dirVectorInvert", configResult.plcRobotServerConfig.dirVectorInvert);
|
||
// 保存数据字节序配置
|
||
tcpServerConfigElement->SetAttribute("byteOrder", configResult.plcRobotServerConfig.byteOrder);
|
||
root->InsertEndChild(tcpServerConfigElement);
|
||
|
||
// 保存到文件
|
||
XMLError err = doc.SaveFile(filePath.c_str());
|
||
if (err != XML_SUCCESS)
|
||
{
|
||
std::cerr << "无法保存配置文件: " << filePath << std::endl;
|
||
return false;
|
||
}
|
||
|
||
// 触发配置改变通知
|
||
if (m_pNotify)
|
||
{
|
||
m_pNotify->OnConfigChanged(configResult);
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
// 设置配置改变通知回调
|
||
void CVrConfig::SetConfigChangeNotify(IVrConfigChangeNotify* notify)
|
||
{
|
||
m_pNotify = notify;
|
||
}
|
||
|
||
|
||
/**
|
||
* @brief 创建实例
|
||
* @return 实例
|
||
*/
|
||
bool IVrConfig::CreateInstance(IVrConfig** ppVrConfig)
|
||
{
|
||
*ppVrConfig = new CVrConfig();
|
||
return *ppVrConfig != nullptr;
|
||
}
|