259 lines
12 KiB
C++
259 lines
12 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)
|
|
{
|
|
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()) != "HolePitPositionConfig")
|
|
{
|
|
std::cerr << "config file format error: root element is not HolePitPositionConfig" << 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");
|
|
}
|
|
}
|
|
|
|
// 解析算法参数
|
|
XMLElement* algoParamsElement = root->FirstChildElement("AlgorithmParams");
|
|
if (algoParamsElement)
|
|
{
|
|
// 解析线段参数
|
|
XMLElement* lineSegElement = algoParamsElement->FirstChildElement("LineSegParam");
|
|
if (lineSegElement)
|
|
{
|
|
if (lineSegElement->Attribute("distScale"))
|
|
configResult.algorithmParams.lineSegParam.distScale = lineSegElement->DoubleAttribute("distScale");
|
|
if (lineSegElement->Attribute("segGapTh_y"))
|
|
configResult.algorithmParams.lineSegParam.segGapTh_y = lineSegElement->DoubleAttribute("segGapTh_y");
|
|
if (lineSegElement->Attribute("segGapTh_z"))
|
|
configResult.algorithmParams.lineSegParam.segGapTh_z = lineSegElement->DoubleAttribute("segGapTh_z");
|
|
}
|
|
|
|
// 解析角点参数
|
|
XMLElement* cornerElement = algoParamsElement->FirstChildElement("CornerParam");
|
|
if (cornerElement)
|
|
{
|
|
if (cornerElement->Attribute("cornerTh"))
|
|
configResult.algorithmParams.cornerParam.cornerTh = cornerElement->DoubleAttribute("cornerTh");
|
|
if (cornerElement->Attribute("scale"))
|
|
configResult.algorithmParams.cornerParam.scale = cornerElement->DoubleAttribute("scale");
|
|
if (cornerElement->Attribute("minEndingGap"))
|
|
configResult.algorithmParams.cornerParam.minEndingGap = cornerElement->DoubleAttribute("minEndingGap");
|
|
if (cornerElement->Attribute("minEndingGap_z"))
|
|
configResult.algorithmParams.cornerParam.minEndingGap_z = cornerElement->DoubleAttribute("minEndingGap_z");
|
|
if (cornerElement->Attribute("jumpCornerTh_1"))
|
|
configResult.algorithmParams.cornerParam.jumpCornerTh_1 = cornerElement->DoubleAttribute("jumpCornerTh_1");
|
|
if (cornerElement->Attribute("jumpCornerTh_2"))
|
|
configResult.algorithmParams.cornerParam.jumpCornerTh_2 = cornerElement->DoubleAttribute("jumpCornerTh_2");
|
|
}
|
|
|
|
// 解析噪声滤除参数
|
|
XMLElement* outlierElement = algoParamsElement->FirstChildElement("OutlierFilterParam");
|
|
if (outlierElement)
|
|
{
|
|
if (outlierElement->Attribute("continuityTh"))
|
|
configResult.algorithmParams.outlierFilterParam.continuityTh = outlierElement->DoubleAttribute("continuityTh");
|
|
if (outlierElement->Attribute("outlierTh"))
|
|
configResult.algorithmParams.outlierFilterParam.outlierTh = outlierElement->DoubleAttribute("outlierTh");
|
|
}
|
|
|
|
// 解析树生长参数
|
|
XMLElement* treeGrowElement = algoParamsElement->FirstChildElement("TreeGrowParam");
|
|
if (treeGrowElement)
|
|
{
|
|
if (treeGrowElement->Attribute("maxLineSkipNum"))
|
|
configResult.algorithmParams.treeGrowParam.maxLineSkipNum = treeGrowElement->IntAttribute("maxLineSkipNum");
|
|
if (treeGrowElement->Attribute("yDeviation_max"))
|
|
configResult.algorithmParams.treeGrowParam.yDeviation_max = treeGrowElement->DoubleAttribute("yDeviation_max");
|
|
if (treeGrowElement->Attribute("maxSkipDistance"))
|
|
configResult.algorithmParams.treeGrowParam.maxSkipDistance = treeGrowElement->DoubleAttribute("maxSkipDistance");
|
|
if (treeGrowElement->Attribute("zDeviation_max"))
|
|
configResult.algorithmParams.treeGrowParam.zDeviation_max = treeGrowElement->DoubleAttribute("zDeviation_max");
|
|
if (treeGrowElement->Attribute("minLTypeTreeLen"))
|
|
configResult.algorithmParams.treeGrowParam.minLTypeTreeLen = treeGrowElement->DoubleAttribute("minLTypeTreeLen");
|
|
if (treeGrowElement->Attribute("minVTypeTreeLen"))
|
|
configResult.algorithmParams.treeGrowParam.minVTypeTreeLen = treeGrowElement->DoubleAttribute("minVTypeTreeLen");
|
|
}
|
|
|
|
// 解析多相机平面校准参数
|
|
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)
|
|
{
|
|
XMLDocument doc;
|
|
|
|
XMLDeclaration* declaration = doc.NewDeclaration("xml version=\"1.0\" encoding=\"UTF-8\"");
|
|
doc.InsertFirstChild(declaration);
|
|
|
|
XMLElement* root = doc.NewElement("HolePitPositionConfig");
|
|
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);
|
|
|
|
// 线段参数
|
|
XMLElement* lineSegElement = doc.NewElement("LineSegParam");
|
|
lineSegElement->SetAttribute("distScale", configResult.algorithmParams.lineSegParam.distScale);
|
|
lineSegElement->SetAttribute("segGapTh_y", configResult.algorithmParams.lineSegParam.segGapTh_y);
|
|
lineSegElement->SetAttribute("segGapTh_z", configResult.algorithmParams.lineSegParam.segGapTh_z);
|
|
algoParamsElement->InsertEndChild(lineSegElement);
|
|
|
|
// 角点参数
|
|
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);
|
|
algoParamsElement->InsertEndChild(cornerElement);
|
|
|
|
// 噪声滤除参数
|
|
XMLElement* outlierElement = doc.NewElement("OutlierFilterParam");
|
|
outlierElement->SetAttribute("continuityTh", configResult.algorithmParams.outlierFilterParam.continuityTh);
|
|
outlierElement->SetAttribute("outlierTh", configResult.algorithmParams.outlierFilterParam.outlierTh);
|
|
algoParamsElement->InsertEndChild(outlierElement);
|
|
|
|
// 树生长参数
|
|
XMLElement* treeGrowElement = doc.NewElement("TreeGrowParam");
|
|
treeGrowElement->SetAttribute("maxLineSkipNum", configResult.algorithmParams.treeGrowParam.maxLineSkipNum);
|
|
treeGrowElement->SetAttribute("yDeviation_max", configResult.algorithmParams.treeGrowParam.yDeviation_max);
|
|
treeGrowElement->SetAttribute("maxSkipDistance", configResult.algorithmParams.treeGrowParam.maxSkipDistance);
|
|
treeGrowElement->SetAttribute("zDeviation_max", configResult.algorithmParams.treeGrowParam.zDeviation_max);
|
|
treeGrowElement->SetAttribute("minLTypeTreeLen", configResult.algorithmParams.treeGrowParam.minLTypeTreeLen);
|
|
treeGrowElement->SetAttribute("minVTypeTreeLen", configResult.algorithmParams.treeGrowParam.minVTypeTreeLen);
|
|
algoParamsElement->InsertEndChild(treeGrowElement);
|
|
|
|
// 平面校准参数
|
|
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服务端配置
|
|
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;
|
|
}
|