273 lines
13 KiB
C++
273 lines
13 KiB
C++
#include "VrConfig.h"
|
||
#include <iostream>
|
||
#include <vector>
|
||
#include <string>
|
||
|
||
#include "VrLog.h"
|
||
#include "ConfigXmlUtils.h"
|
||
|
||
using namespace tinyxml2;
|
||
|
||
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("Failed to open config file: %s\n", filePath.c_str());
|
||
return LOAD_CONFIG_FILE_NOT_FOUND;
|
||
}
|
||
|
||
// 获取根元素
|
||
XMLElement* root = doc.RootElement();
|
||
if (!root || std::string(root->Name()) != "RodWeldSeamConfig")
|
||
{
|
||
LOG_ERR("Config file format error: root element is not RodWeldSeamConfig\n");
|
||
return LOAD_CONFIG_INVALID_FORMAT;
|
||
}
|
||
|
||
// 1. 解析相机设备列表
|
||
ConfigXmlUtils::LoadCameraList(root, configResult.cameraList);
|
||
|
||
// 2. 解析算法参数
|
||
XMLElement* algoElement = root->FirstChildElement("AlgorithmParams");
|
||
if (algoElement)
|
||
{
|
||
// 2.1 解析钢筋参数 (RodParam)
|
||
XMLElement* rodElement = algoElement->FirstChildElement("RodParam");
|
||
if (rodElement)
|
||
{
|
||
rodElement->QueryDoubleAttribute("rodDiameter", &configResult.algorithmParams.rodParam.rodDiameter);
|
||
rodElement->QueryDoubleAttribute("rodLen", &configResult.algorithmParams.rodParam.rodLen);
|
||
|
||
LOG_INFO("RodParam: rodDiameter=%.1f, rodLen=%.1f\n",
|
||
configResult.algorithmParams.rodParam.rodDiameter,
|
||
configResult.algorithmParams.rodParam.rodLen);
|
||
}
|
||
|
||
// 2.2 解析离群点滤波参数 (FilterParam)
|
||
XMLElement* filterElement = algoElement->FirstChildElement("FilterParam");
|
||
if (filterElement)
|
||
{
|
||
filterElement->QueryDoubleAttribute("continuityTh", &configResult.algorithmParams.filterParam.continuityTh);
|
||
filterElement->QueryDoubleAttribute("outlierTh", &configResult.algorithmParams.filterParam.outlierTh);
|
||
|
||
LOG_INFO("FilterParam: continuityTh=%.1f, outlierTh=%.1f\n",
|
||
configResult.algorithmParams.filterParam.continuityTh,
|
||
configResult.algorithmParams.filterParam.outlierTh);
|
||
}
|
||
|
||
// 2.3 解析角点检测参数 (CornerParam)
|
||
XMLElement* cornerElement = algoElement->FirstChildElement("CornerParam");
|
||
if (cornerElement)
|
||
{
|
||
cornerElement->QueryDoubleAttribute("minEndingGap", &configResult.algorithmParams.cornerParam.minEndingGap);
|
||
cornerElement->QueryDoubleAttribute("minEndingGap_z", &configResult.algorithmParams.cornerParam.minEndingGap_z);
|
||
cornerElement->QueryDoubleAttribute("scale", &configResult.algorithmParams.cornerParam.scale);
|
||
cornerElement->QueryDoubleAttribute("cornerTh", &configResult.algorithmParams.cornerParam.cornerTh);
|
||
cornerElement->QueryDoubleAttribute("jumpCornerTh_1", &configResult.algorithmParams.cornerParam.jumpCornerTh_1);
|
||
cornerElement->QueryDoubleAttribute("jumpCornerTh_2", &configResult.algorithmParams.cornerParam.jumpCornerTh_2);
|
||
|
||
LOG_INFO("CornerParam: minEndingGap=%.1f, cornerTh=%.1f\n",
|
||
configResult.algorithmParams.cornerParam.minEndingGap,
|
||
configResult.algorithmParams.cornerParam.cornerTh);
|
||
}
|
||
|
||
// 2.4 解析树生长参数 (GrowParam)
|
||
XMLElement* growElement = algoElement->FirstChildElement("GrowParam");
|
||
if (growElement)
|
||
{
|
||
growElement->QueryDoubleAttribute("yDeviation_max", &configResult.algorithmParams.growParam.yDeviation_max);
|
||
growElement->QueryDoubleAttribute("zDeviation_max", &configResult.algorithmParams.growParam.zDeviation_max);
|
||
growElement->QueryIntAttribute("maxLineSkipNum", &configResult.algorithmParams.growParam.maxLineSkipNum);
|
||
growElement->QueryDoubleAttribute("maxSkipDistance", &configResult.algorithmParams.growParam.maxSkipDistance);
|
||
growElement->QueryDoubleAttribute("minLTypeTreeLen", &configResult.algorithmParams.growParam.minLTypeTreeLen);
|
||
growElement->QueryDoubleAttribute("minVTypeTreeLen", &configResult.algorithmParams.growParam.minVTypeTreeLen);
|
||
|
||
LOG_INFO("GrowParam: yDeviation_max=%.1f, zDeviation_max=%.1f\n",
|
||
configResult.algorithmParams.growParam.yDeviation_max,
|
||
configResult.algorithmParams.growParam.zDeviation_max);
|
||
}
|
||
|
||
// 2.5 解析焊缝搜索参数 (WeldSeamParam)
|
||
XMLElement* weldSeamElement = algoElement->FirstChildElement("WeldSeamParam");
|
||
if (weldSeamElement)
|
||
{
|
||
VrWeldSeamParam& weldSeamParam = configResult.algorithmParams.weldSeamParam;
|
||
weldSeamElement->QueryDoubleAttribute("weldSeamRangeMin", &weldSeamParam.weldSeamRangeMin);
|
||
if (weldSeamElement->QueryDoubleAttribute("weldSeamRangeMax", &weldSeamParam.weldSeamRangeMax) != XML_SUCCESS) {
|
||
// 兼容旧配置：weldSeanRange 原本仅表示范围最大值。
|
||
weldSeamElement->QueryDoubleAttribute("weldSeanRange", &weldSeamParam.weldSeamRangeMax);
|
||
}
|
||
weldSeamElement->QueryIntAttribute("weldingCategory", &weldSeamParam.weldingCategory);
|
||
if (weldSeamParam.weldingCategory != 1 && weldSeamParam.weldingCategory != 2) {
|
||
LOG_WARNING("Invalid weldingCategory=%d, fallback to category II\n",
|
||
weldSeamParam.weldingCategory);
|
||
weldSeamParam.weldingCategory = 2;
|
||
}
|
||
LOG_INFO("WeldSeamParam: range=[%.1f, %.1f], weldingCategory=%d\n",
|
||
weldSeamParam.weldSeamRangeMin,
|
||
weldSeamParam.weldSeamRangeMax,
|
||
weldSeamParam.weldingCategory);
|
||
}
|
||
|
||
// 2.6 解析平面标定参数 (PlaneCalibParams)
|
||
ConfigXmlUtils::LoadPlaneCalibParams(algoElement, configResult.algorithmParams.planeCalibParam);
|
||
}
|
||
|
||
// 3. 解析调试参数
|
||
ConfigXmlUtils::LoadDebugParam(root, configResult.debugParam);
|
||
|
||
// 4. 解析串口配置
|
||
ConfigXmlUtils::LoadSerialConfig(root, configResult.serialConfig);
|
||
|
||
// 5. 解析网络参数
|
||
XMLElement* networkElement = root->FirstChildElement("NetworkConfig");
|
||
if (networkElement)
|
||
{
|
||
networkElement->QueryIntAttribute("eulerOrder", &configResult.eulerOrder);
|
||
networkElement->QueryIntAttribute("dirVectorInvert", &configResult.dirVectorInvert);
|
||
networkElement->QueryIntAttribute("byteOrder", &configResult.byteOrder);
|
||
networkElement->QueryIntAttribute("longAxisDir", &configResult.longAxisDir);
|
||
networkElement->QueryIntAttribute("poseOutputOrder", &configResult.poseOutputOrder);
|
||
networkElement->QueryIntAttribute("tcpServerPort", &configResult.tcpPort);
|
||
if (configResult.tcpPort <= 0 || configResult.tcpPort > 65535) {
|
||
LOG_WARNING("Invalid TCP server port %d, fallback to 7800\n", configResult.tcpPort);
|
||
configResult.tcpPort = 7800;
|
||
}
|
||
|
||
LOG_INFO("Network config: eulerOrder=%d, dirVectorInvert=%d, byteOrder=%d, longAxisDir=%d, poseOutputOrder=%d, tcpPort=%d\n",
|
||
configResult.eulerOrder, configResult.dirVectorInvert,
|
||
configResult.byteOrder, configResult.longAxisDir,
|
||
configResult.poseOutputOrder, configResult.tcpPort);
|
||
}
|
||
|
||
// 6. 解析手眼标定矩阵列表（支持多相机）
|
||
ConfigXmlUtils::LoadHandEyeCalibMatrixs(root, configResult.handEyeCalibMatrixList,
|
||
configResult.eulerOrder, 1);
|
||
|
||
LOG_INFO("Config loaded successfully from: %s\n", filePath.c_str());
|
||
return LOAD_CONFIG_SUCCESS;
|
||
}
|
||
|
||
bool CVrConfig::SaveConfig(const std::string& filePath, ConfigResult& configResult)
|
||
{
|
||
XMLDocument doc;
|
||
|
||
// 创建 XML 声明
|
||
XMLDeclaration* declaration = doc.NewDeclaration();
|
||
doc.InsertFirstChild(declaration);
|
||
|
||
// 创建根元素
|
||
XMLElement* root = doc.NewElement("RodWeldSeamConfig");
|
||
doc.InsertEndChild(root);
|
||
|
||
// 1. 保存相机设备列表
|
||
ConfigXmlUtils::SaveCameraList(doc, root, configResult.cameraList);
|
||
|
||
// 2. 保存算法参数
|
||
XMLElement* algoElement = doc.NewElement("AlgorithmParams");
|
||
|
||
// 2.1 保存钢筋参数
|
||
XMLElement* rodElement = doc.NewElement("RodParam");
|
||
rodElement->SetAttribute("rodDiameter", configResult.algorithmParams.rodParam.rodDiameter);
|
||
rodElement->SetAttribute("rodLen", configResult.algorithmParams.rodParam.rodLen);
|
||
algoElement->InsertEndChild(rodElement);
|
||
|
||
// 2.2 保存离群点滤波参数
|
||
XMLElement* filterElement = doc.NewElement("FilterParam");
|
||
filterElement->SetAttribute("continuityTh", configResult.algorithmParams.filterParam.continuityTh);
|
||
filterElement->SetAttribute("outlierTh", configResult.algorithmParams.filterParam.outlierTh);
|
||
algoElement->InsertEndChild(filterElement);
|
||
|
||
// 2.3 保存角点检测参数
|
||
XMLElement* cornerElement = doc.NewElement("CornerParam");
|
||
cornerElement->SetAttribute("cornerTh", configResult.algorithmParams.cornerParam.cornerTh);
|
||
cornerElement->SetAttribute("scale", configResult.algorithmParams.cornerParam.scale);
|
||
cornerElement->SetAttribute("minEndingGap", configResult.algorithmParams.cornerParam.minEndingGap);
|
||
cornerElement->SetAttribute("minEndingGap_z", configResult.algorithmParams.cornerParam.minEndingGap_z);
|
||
cornerElement->SetAttribute("jumpCornerTh_1", configResult.algorithmParams.cornerParam.jumpCornerTh_1);
|
||
cornerElement->SetAttribute("jumpCornerTh_2", configResult.algorithmParams.cornerParam.jumpCornerTh_2);
|
||
algoElement->InsertEndChild(cornerElement);
|
||
|
||
// 2.4 保存树生长参数
|
||
XMLElement* growElement = doc.NewElement("GrowParam");
|
||
growElement->SetAttribute("maxLineSkipNum", configResult.algorithmParams.growParam.maxLineSkipNum);
|
||
growElement->SetAttribute("yDeviation_max", configResult.algorithmParams.growParam.yDeviation_max);
|
||
growElement->SetAttribute("maxSkipDistance", configResult.algorithmParams.growParam.maxSkipDistance);
|
||
growElement->SetAttribute("zDeviation_max", configResult.algorithmParams.growParam.zDeviation_max);
|
||
growElement->SetAttribute("minLTypeTreeLen", configResult.algorithmParams.growParam.minLTypeTreeLen);
|
||
growElement->SetAttribute("minVTypeTreeLen", configResult.algorithmParams.growParam.minVTypeTreeLen);
|
||
algoElement->InsertEndChild(growElement);
|
||
|
||
// 2.5 保存焊缝搜索参数
|
||
XMLElement* weldSeamElement = doc.NewElement("WeldSeamParam");
|
||
weldSeamElement->SetAttribute("weldSeamRangeMin", configResult.algorithmParams.weldSeamParam.weldSeamRangeMin);
|
||
weldSeamElement->SetAttribute("weldSeamRangeMax", configResult.algorithmParams.weldSeamParam.weldSeamRangeMax);
|
||
weldSeamElement->SetAttribute("weldingCategory", configResult.algorithmParams.weldSeamParam.weldingCategory);
|
||
algoElement->InsertEndChild(weldSeamElement);
|
||
|
||
// 2.6 保存平面标定参数
|
||
ConfigXmlUtils::SavePlaneCalibParams(doc, algoElement, configResult.algorithmParams.planeCalibParam);
|
||
|
||
root->InsertEndChild(algoElement);
|
||
|
||
// 3. 保存调试参数
|
||
ConfigXmlUtils::SaveDebugParam(doc, root, configResult.debugParam);
|
||
|
||
// 4. 保存串口配置
|
||
ConfigXmlUtils::SaveSerialConfig(doc, root, configResult.serialConfig);
|
||
|
||
// 5. 保存网络参数
|
||
XMLElement* networkElement = doc.NewElement("NetworkConfig");
|
||
networkElement->SetAttribute("eulerOrder", configResult.eulerOrder);
|
||
networkElement->SetAttribute("dirVectorInvert", configResult.dirVectorInvert);
|
||
networkElement->SetAttribute("byteOrder", configResult.byteOrder);
|
||
networkElement->SetAttribute("longAxisDir", configResult.longAxisDir);
|
||
networkElement->SetAttribute("poseOutputOrder", configResult.poseOutputOrder);
|
||
networkElement->SetAttribute("tcpServerPort", configResult.tcpPort);
|
||
root->InsertEndChild(networkElement);
|
||
|
||
// 6. 保存手眼标定矩阵列表（支持多相机）
|
||
ConfigXmlUtils::SaveHandEyeCalibMatrixs(doc, root, configResult.handEyeCalibMatrixList);
|
||
|
||
// 保存到文件
|
||
XMLError err = doc.SaveFile(filePath.c_str());
|
||
if (err != XML_SUCCESS)
|
||
{
|
||
LOG_ERR("Failed to save config file: %s\n", filePath.c_str());
|
||
return false;
|
||
}
|
||
|
||
LOG_INFO("Config saved successfully to: %s\n", filePath.c_str());
|
||
return true;
|
||
}
|
||
|
||
// 设置配置改变通知回调
|
||
void CVrConfig::SetConfigChangeNotify(IVrConfigChangeNotify* notify)
|
||
{
|
||
m_pNotify = notify;
|
||
}
|
||
|
||
|
||
/**
|
||
* @brief 创建实例
|
||
* @return 实例
|
||
*/
|
||
bool IVrConfig::CreateInstance(IVrConfig** ppVrConfig)
|
||
{
|
||
*ppVrConfig = new CVrConfig() ;
|
||
return *ppVrConfig != nullptr;
|
||
}
|