292 lines
17 KiB
C++
292 lines
17 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)
|
||
{
|
||
// 读取 HoleDetection 专用配置文件。
|
||
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;
|
||
}
|
||
|
||
// 根节点固定为 HoleDetectionConfig，便于和其它应用配置区分。
|
||
XMLElement* root = doc.RootElement();
|
||
if (!root || std::string(root->Name()) != "HoleDetectionConfig")
|
||
{
|
||
std::cerr << "config file format error: root element is not HoleDetectionConfig" << std::endl;
|
||
return LOAD_CONFIG_INVALID_FORMAT;
|
||
}
|
||
|
||
// 通用配置仍复用公共工具读取，例如相机列表、调试参数、串口等。
|
||
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");
|
||
}
|
||
}
|
||
|
||
// 算法参数分为 RANSAC / Detection / Filter 三组，分别与算法头文件一一对应。
|
||
XMLElement* algoParamsElement = root->FirstChildElement("AlgorithmParams");
|
||
if (algoParamsElement)
|
||
{
|
||
XMLElement* ransacParamElement = algoParamsElement->FirstChildElement("RansacParam");
|
||
if (ransacParamElement)
|
||
{
|
||
// 读取 RANSAC 平面分割参数。
|
||
if (ransacParamElement->Attribute("distanceThreshold"))
|
||
configResult.algorithmParams.ransacParam.distanceThreshold = ransacParamElement->DoubleAttribute("distanceThreshold");
|
||
if (ransacParamElement->Attribute("maxIterations"))
|
||
configResult.algorithmParams.ransacParam.maxIterations = ransacParamElement->IntAttribute("maxIterations");
|
||
if (ransacParamElement->Attribute("minPlanePoints"))
|
||
configResult.algorithmParams.ransacParam.minPlanePoints = ransacParamElement->IntAttribute("minPlanePoints");
|
||
if (ransacParamElement->Attribute("maxPlanes"))
|
||
configResult.algorithmParams.ransacParam.maxPlanes = ransacParamElement->IntAttribute("maxPlanes");
|
||
if (ransacParamElement->Attribute("growthZThreshold"))
|
||
configResult.algorithmParams.ransacParam.growthZThreshold = ransacParamElement->DoubleAttribute("growthZThreshold");
|
||
if (ransacParamElement->Attribute("minPlaneRatio"))
|
||
configResult.algorithmParams.ransacParam.minPlaneRatio = ransacParamElement->DoubleAttribute("minPlaneRatio");
|
||
if (ransacParamElement->Attribute("maxNormalAngleDeg"))
|
||
configResult.algorithmParams.ransacParam.maxNormalAngleDeg = ransacParamElement->DoubleAttribute("maxNormalAngleDeg");
|
||
}
|
||
|
||
XMLElement* detectionParamElement = algoParamsElement->FirstChildElement("DetectionParam");
|
||
if (detectionParamElement)
|
||
{
|
||
// 孔检测参数，需要完整覆盖 SHoleDetectionParams。
|
||
if (detectionParamElement->Attribute("angleThresholdPos"))
|
||
configResult.algorithmParams.detectionParam.angleThresholdPos = detectionParamElement->DoubleAttribute("angleThresholdPos");
|
||
if (detectionParamElement->Attribute("angleThresholdNeg"))
|
||
configResult.algorithmParams.detectionParam.angleThresholdNeg = detectionParamElement->DoubleAttribute("angleThresholdNeg");
|
||
if (detectionParamElement->Attribute("angleSearchDistance"))
|
||
configResult.algorithmParams.detectionParam.angleSearchDistance = detectionParamElement->DoubleAttribute("angleSearchDistance");
|
||
if (detectionParamElement->Attribute("minPitDepth"))
|
||
configResult.algorithmParams.detectionParam.minPitDepth = detectionParamElement->DoubleAttribute("minPitDepth");
|
||
if (detectionParamElement->Attribute("minRadius"))
|
||
configResult.algorithmParams.detectionParam.minRadius = detectionParamElement->DoubleAttribute("minRadius");
|
||
if (detectionParamElement->Attribute("maxRadius"))
|
||
configResult.algorithmParams.detectionParam.maxRadius = detectionParamElement->DoubleAttribute("maxRadius");
|
||
if (detectionParamElement->Attribute("expansionSize1"))
|
||
configResult.algorithmParams.detectionParam.expansionSize1 = detectionParamElement->IntAttribute("expansionSize1");
|
||
if (detectionParamElement->Attribute("expansionSize2"))
|
||
configResult.algorithmParams.detectionParam.expansionSize2 = detectionParamElement->IntAttribute("expansionSize2");
|
||
if (detectionParamElement->Attribute("minVTransitionPoints"))
|
||
configResult.algorithmParams.detectionParam.minVTransitionPoints = detectionParamElement->IntAttribute("minVTransitionPoints");
|
||
if (detectionParamElement->Attribute("jumpThresholdResidual"))
|
||
configResult.algorithmParams.detectionParam.jumpThresholdResidual = detectionParamElement->DoubleAttribute("jumpThresholdResidual");
|
||
if (detectionParamElement->Attribute("gapJumpThresholdResidual"))
|
||
configResult.algorithmParams.detectionParam.gapJumpThresholdResidual = detectionParamElement->DoubleAttribute("gapJumpThresholdResidual");
|
||
if (detectionParamElement->Attribute("maxGapPointsInLine"))
|
||
configResult.algorithmParams.detectionParam.maxGapPointsInLine = detectionParamElement->IntAttribute("maxGapPointsInLine");
|
||
if (detectionParamElement->Attribute("minFeatureSpan"))
|
||
configResult.algorithmParams.detectionParam.minFeatureSpan = detectionParamElement->DoubleAttribute("minFeatureSpan");
|
||
if (detectionParamElement->Attribute("residualSmoothWindow"))
|
||
configResult.algorithmParams.detectionParam.residualSmoothWindow = detectionParamElement->IntAttribute("residualSmoothWindow");
|
||
if (detectionParamElement->Attribute("slopeAngleThreshold"))
|
||
configResult.algorithmParams.detectionParam.slopeAngleThreshold = detectionParamElement->DoubleAttribute("slopeAngleThreshold");
|
||
if (detectionParamElement->Attribute("edgeBoundaryFilterDist"))
|
||
configResult.algorithmParams.detectionParam.edgeBoundaryFilterDist = detectionParamElement->DoubleAttribute("edgeBoundaryFilterDist");
|
||
}
|
||
|
||
XMLElement* filterParamElement = algoParamsElement->FirstChildElement("FilterParam");
|
||
if (filterParamElement)
|
||
{
|
||
// 孔过滤参数，需要完整覆盖 SHoleFilterParams。
|
||
if (filterParamElement->Attribute("maxEccentricity"))
|
||
configResult.algorithmParams.filterParam.maxEccentricity = filterParamElement->DoubleAttribute("maxEccentricity");
|
||
if (filterParamElement->Attribute("minAngularCoverage"))
|
||
configResult.algorithmParams.filterParam.minAngularCoverage = filterParamElement->DoubleAttribute("minAngularCoverage");
|
||
if (filterParamElement->Attribute("maxRadiusFitRatio"))
|
||
configResult.algorithmParams.filterParam.maxRadiusFitRatio = filterParamElement->DoubleAttribute("maxRadiusFitRatio");
|
||
if (filterParamElement->Attribute("minQualityScore"))
|
||
configResult.algorithmParams.filterParam.minQualityScore = filterParamElement->DoubleAttribute("minQualityScore");
|
||
if (filterParamElement->Attribute("maxPlaneResidual"))
|
||
configResult.algorithmParams.filterParam.maxPlaneResidual = filterParamElement->DoubleAttribute("maxPlaneResidual");
|
||
if (filterParamElement->Attribute("maxAngularGap"))
|
||
configResult.algorithmParams.filterParam.maxAngularGap = filterParamElement->DoubleAttribute("maxAngularGap");
|
||
if (filterParamElement->Attribute("minInlierRatio"))
|
||
configResult.algorithmParams.filterParam.minInlierRatio = filterParamElement->DoubleAttribute("minInlierRatio");
|
||
if (filterParamElement->Attribute("minHoleDepth"))
|
||
configResult.algorithmParams.filterParam.minHoleDepth = filterParamElement->DoubleAttribute("minHoleDepth");
|
||
}
|
||
|
||
// 排序模式 sortMode 属于应用层选项，但仍跟随算法参数一起持久化。
|
||
XMLElement* sortModeElement = algoParamsElement->FirstChildElement("SortMode");
|
||
if (sortModeElement && sortModeElement->Attribute("value"))
|
||
configResult.algorithmParams.sortMode = sortModeElement->IntAttribute("value");
|
||
|
||
// 平面标定参数继续复用通用读写逻辑。
|
||
ConfigXmlUtils::LoadPlaneCalibParams(algoParamsElement, configResult.algorithmParams.planeCalibParam);
|
||
}
|
||
|
||
ConfigXmlUtils::LoadDebugParam(root, configResult.debugParam);
|
||
ConfigXmlUtils::LoadSerialConfig(root, configResult.serialConfig);
|
||
ConfigXmlUtils::LoadHandEyeCalibMatrixs(root, configResult.handEyeCalibMatrixList);
|
||
|
||
// TCP 服务配置由当前应用消费，用于姿态输出与网络通讯。
|
||
XMLElement* tcpServerConfigElement = root->FirstChildElement("TcpServerConfig");
|
||
if (tcpServerConfigElement)
|
||
{
|
||
if (tcpServerConfigElement->Attribute("tcpServerPort"))
|
||
configResult.plcRobotServerConfig.tcpServerPort = tcpServerConfigElement->IntAttribute("tcpServerPort");
|
||
else
|
||
configResult.plcRobotServerConfig.tcpServerPort = 7800;
|
||
|
||
if (tcpServerConfigElement->Attribute("poseOutputOrder"))
|
||
configResult.plcRobotServerConfig.poseOutputOrder = tcpServerConfigElement->IntAttribute("poseOutputOrder");
|
||
else
|
||
configResult.plcRobotServerConfig.poseOutputOrder = POSE_ORDER_RPY;
|
||
|
||
if (tcpServerConfigElement->Attribute("dirVectorInvert"))
|
||
configResult.plcRobotServerConfig.dirVectorInvert = tcpServerConfigElement->IntAttribute("dirVectorInvert");
|
||
else
|
||
configResult.plcRobotServerConfig.dirVectorInvert = DIR_INVERT_YZ;
|
||
}
|
||
|
||
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("HoleDetectionConfig");
|
||
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);
|
||
|
||
// 先写 RANSAC，再写 Detection / Filter，顺序与 LoadConfig 保持一致。
|
||
XMLElement* ransacParamElement = doc.NewElement("RansacParam");
|
||
ransacParamElement->SetAttribute("distanceThreshold", configResult.algorithmParams.ransacParam.distanceThreshold);
|
||
ransacParamElement->SetAttribute("maxIterations", configResult.algorithmParams.ransacParam.maxIterations);
|
||
ransacParamElement->SetAttribute("minPlanePoints", configResult.algorithmParams.ransacParam.minPlanePoints);
|
||
ransacParamElement->SetAttribute("maxPlanes", configResult.algorithmParams.ransacParam.maxPlanes);
|
||
ransacParamElement->SetAttribute("growthZThreshold", configResult.algorithmParams.ransacParam.growthZThreshold);
|
||
ransacParamElement->SetAttribute("minPlaneRatio", configResult.algorithmParams.ransacParam.minPlaneRatio);
|
||
ransacParamElement->SetAttribute("maxNormalAngleDeg", configResult.algorithmParams.ransacParam.maxNormalAngleDeg);
|
||
algoParamsElement->InsertEndChild(ransacParamElement);
|
||
|
||
// 检测参数 DetectionParam 需要覆盖算法库当前全部检测字段。
|
||
XMLElement* detectionParamElement = doc.NewElement("DetectionParam");
|
||
detectionParamElement->SetAttribute("angleThresholdPos", configResult.algorithmParams.detectionParam.angleThresholdPos);
|
||
detectionParamElement->SetAttribute("angleThresholdNeg", configResult.algorithmParams.detectionParam.angleThresholdNeg);
|
||
detectionParamElement->SetAttribute("angleSearchDistance", configResult.algorithmParams.detectionParam.angleSearchDistance);
|
||
detectionParamElement->SetAttribute("minPitDepth", configResult.algorithmParams.detectionParam.minPitDepth);
|
||
detectionParamElement->SetAttribute("minRadius", configResult.algorithmParams.detectionParam.minRadius);
|
||
detectionParamElement->SetAttribute("maxRadius", configResult.algorithmParams.detectionParam.maxRadius);
|
||
detectionParamElement->SetAttribute("expansionSize1", configResult.algorithmParams.detectionParam.expansionSize1);
|
||
detectionParamElement->SetAttribute("expansionSize2", configResult.algorithmParams.detectionParam.expansionSize2);
|
||
detectionParamElement->SetAttribute("minVTransitionPoints", configResult.algorithmParams.detectionParam.minVTransitionPoints);
|
||
detectionParamElement->SetAttribute("jumpThresholdResidual", configResult.algorithmParams.detectionParam.jumpThresholdResidual);
|
||
detectionParamElement->SetAttribute("gapJumpThresholdResidual", configResult.algorithmParams.detectionParam.gapJumpThresholdResidual);
|
||
detectionParamElement->SetAttribute("maxGapPointsInLine", configResult.algorithmParams.detectionParam.maxGapPointsInLine);
|
||
detectionParamElement->SetAttribute("minFeatureSpan", configResult.algorithmParams.detectionParam.minFeatureSpan);
|
||
detectionParamElement->SetAttribute("residualSmoothWindow", configResult.algorithmParams.detectionParam.residualSmoothWindow);
|
||
detectionParamElement->SetAttribute("slopeAngleThreshold", configResult.algorithmParams.detectionParam.slopeAngleThreshold);
|
||
detectionParamElement->SetAttribute("edgeBoundaryFilterDist", configResult.algorithmParams.detectionParam.edgeBoundaryFilterDist);
|
||
algoParamsElement->InsertEndChild(detectionParamElement);
|
||
|
||
// 过滤参数 FilterParam 需要覆盖算法库当前全部过滤字段。
|
||
XMLElement* filterParamElement = doc.NewElement("FilterParam");
|
||
filterParamElement->SetAttribute("maxEccentricity", configResult.algorithmParams.filterParam.maxEccentricity);
|
||
filterParamElement->SetAttribute("minAngularCoverage", configResult.algorithmParams.filterParam.minAngularCoverage);
|
||
filterParamElement->SetAttribute("maxRadiusFitRatio", configResult.algorithmParams.filterParam.maxRadiusFitRatio);
|
||
filterParamElement->SetAttribute("minQualityScore", configResult.algorithmParams.filterParam.minQualityScore);
|
||
filterParamElement->SetAttribute("maxPlaneResidual", configResult.algorithmParams.filterParam.maxPlaneResidual);
|
||
filterParamElement->SetAttribute("maxAngularGap", configResult.algorithmParams.filterParam.maxAngularGap);
|
||
filterParamElement->SetAttribute("minInlierRatio", configResult.algorithmParams.filterParam.minInlierRatio);
|
||
filterParamElement->SetAttribute("minHoleDepth", configResult.algorithmParams.filterParam.minHoleDepth);
|
||
algoParamsElement->InsertEndChild(filterParamElement);
|
||
|
||
// 排序模式 sortMode 仍由应用层维护，因此单独保存。
|
||
XMLElement* sortModeElement = doc.NewElement("SortMode");
|
||
sortModeElement->SetAttribute("value", configResult.algorithmParams.sortMode);
|
||
algoParamsElement->InsertEndChild(sortModeElement);
|
||
|
||
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);
|
||
|
||
// 网络输出配置单独放在根节点下，和算法参数解耦。
|
||
XMLElement* tcpServerConfigElement = doc.NewElement("TcpServerConfig");
|
||
tcpServerConfigElement->SetAttribute("tcpServerPort", configResult.plcRobotServerConfig.tcpServerPort);
|
||
tcpServerConfigElement->SetAttribute("poseOutputOrder", configResult.plcRobotServerConfig.poseOutputOrder);
|
||
tcpServerConfigElement->SetAttribute("dirVectorInvert", configResult.plcRobotServerConfig.dirVectorInvert);
|
||
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;
|
||
}
|
||
|
||
bool IVrConfig::CreateInstance(IVrConfig** ppVrConfig)
|
||
{
|
||
*ppVrConfig = new CVrConfig();
|
||
return *ppVrConfig != nullptr;
|
||
}
|