223 lines
10 KiB
C++
223 lines
10 KiB
C++
#include "VrConfig.h"
|
|
|
|
#include <iostream>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
#include "ConfigXmlUtils.h"
|
|
#include "VrLog.h"
|
|
|
|
using namespace tinyxml2;
|
|
|
|
CVrConfig::CVrConfig() : m_pNotify(nullptr)
|
|
{
|
|
}
|
|
|
|
CVrConfig::~CVrConfig()
|
|
{
|
|
}
|
|
|
|
int CVrConfig::LoadConfig(const std::string& filePath, ConfigResult& configResult)
|
|
{
|
|
XMLDocument doc;
|
|
const 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()) != "ScrewPositionConfig")
|
|
{
|
|
LOG_ERR("Config file format error: root element is not ScrewPositionConfig\n");
|
|
return LOAD_CONFIG_INVALID_FORMAT;
|
|
}
|
|
|
|
ConfigXmlUtils::LoadCameraList(root, configResult.cameraList);
|
|
|
|
XMLElement* algoElement = root->FirstChildElement("AlgorithmParams");
|
|
if (algoElement)
|
|
{
|
|
XMLElement* screwElement = algoElement->FirstChildElement("ScrewParam");
|
|
if (screwElement)
|
|
{
|
|
if (screwElement->Attribute("rodDiameter"))
|
|
configResult.algorithmParams.screwParam.rodDiameter = screwElement->DoubleAttribute("rodDiameter");
|
|
if (screwElement->Attribute("isHorizonScan"))
|
|
configResult.algorithmParams.screwParam.isHorizonScan = screwElement->BoolAttribute("isHorizonScan");
|
|
}
|
|
|
|
XMLElement* filterElement = algoElement->FirstChildElement("FilterParam");
|
|
if (filterElement)
|
|
{
|
|
if (filterElement->Attribute("continuityTh"))
|
|
configResult.algorithmParams.filterParam.continuityTh = filterElement->DoubleAttribute("continuityTh");
|
|
if (filterElement->Attribute("outlierTh"))
|
|
configResult.algorithmParams.filterParam.outlierTh = filterElement->DoubleAttribute("outlierTh");
|
|
}
|
|
|
|
XMLElement* cornerElement = algoElement->FirstChildElement("CornerParam");
|
|
if (cornerElement)
|
|
{
|
|
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("scale"))
|
|
configResult.algorithmParams.cornerParam.scale = cornerElement->DoubleAttribute("scale");
|
|
if (cornerElement->Attribute("cornerTh"))
|
|
configResult.algorithmParams.cornerParam.cornerTh = cornerElement->DoubleAttribute("cornerTh");
|
|
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* growElement = algoElement->FirstChildElement("GrowParam");
|
|
if (growElement)
|
|
{
|
|
if (growElement->Attribute("yDeviation_max"))
|
|
configResult.algorithmParams.growParam.yDeviation_max = growElement->DoubleAttribute("yDeviation_max");
|
|
if (growElement->Attribute("zDeviation_max"))
|
|
configResult.algorithmParams.growParam.zDeviation_max = growElement->DoubleAttribute("zDeviation_max");
|
|
if (growElement->Attribute("maxLineSkipNum"))
|
|
configResult.algorithmParams.growParam.maxLineSkipNum = growElement->IntAttribute("maxLineSkipNum");
|
|
if (growElement->Attribute("maxSkipDistance"))
|
|
configResult.algorithmParams.growParam.maxSkipDistance = growElement->DoubleAttribute("maxSkipDistance");
|
|
if (growElement->Attribute("minLTypeTreeLen"))
|
|
configResult.algorithmParams.growParam.minLTypeTreeLen = growElement->DoubleAttribute("minLTypeTreeLen");
|
|
if (growElement->Attribute("minVTypeTreeLen"))
|
|
configResult.algorithmParams.growParam.minVTypeTreeLen = growElement->DoubleAttribute("minVTypeTreeLen");
|
|
}
|
|
|
|
ConfigXmlUtils::LoadPlaneCalibParams(algoElement, configResult.algorithmParams.planeCalibParam);
|
|
}
|
|
|
|
ConfigXmlUtils::LoadDebugParam(root, configResult.debugParam);
|
|
ConfigXmlUtils::LoadSerialConfig(root, configResult.serialConfig);
|
|
|
|
XMLElement* networkElement = root->FirstChildElement("NetworkConfig");
|
|
if (networkElement)
|
|
{
|
|
if (networkElement->Attribute("tcpServerPort"))
|
|
configResult.tcpPort = static_cast<uint16_t>(networkElement->IntAttribute("tcpServerPort"));
|
|
if (networkElement->Attribute("eulerOrder"))
|
|
configResult.eulerOrder = networkElement->IntAttribute("eulerOrder");
|
|
if (networkElement->Attribute("dirVectorInvert"))
|
|
configResult.dirVectorInvert = networkElement->IntAttribute("dirVectorInvert");
|
|
if (networkElement->Attribute("byteOrder"))
|
|
configResult.byteOrder = networkElement->IntAttribute("byteOrder");
|
|
if (networkElement->Attribute("longAxisDir"))
|
|
configResult.longAxisDir = networkElement->IntAttribute("longAxisDir");
|
|
}
|
|
|
|
XMLElement* tcpElement = root->FirstChildElement("TCP");
|
|
if (tcpElement && tcpElement->Attribute("port"))
|
|
{
|
|
configResult.tcpPort = static_cast<uint16_t>(tcpElement->IntAttribute("port"));
|
|
}
|
|
|
|
ConfigXmlUtils::LoadHandEyeCalibMatrixs(root,
|
|
configResult.handEyeCalibMatrixList,
|
|
configResult.eulerOrder);
|
|
if (!configResult.handEyeCalibMatrixList.empty()) {
|
|
configResult.eulerOrder = configResult.handEyeCalibMatrixList.front().eulerOrder;
|
|
}
|
|
|
|
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;
|
|
|
|
XMLDeclaration* declaration = doc.NewDeclaration("xml version=\"1.0\" encoding=\"UTF-8\"");
|
|
doc.InsertFirstChild(declaration);
|
|
|
|
XMLElement* root = doc.NewElement("ScrewPositionConfig");
|
|
doc.InsertEndChild(root);
|
|
|
|
ConfigXmlUtils::SaveCameraList(doc, root, configResult.cameraList);
|
|
|
|
XMLElement* algoElement = doc.NewElement("AlgorithmParams");
|
|
root->InsertEndChild(algoElement);
|
|
|
|
XMLElement* screwElement = doc.NewElement("ScrewParam");
|
|
screwElement->SetAttribute("rodDiameter", configResult.algorithmParams.screwParam.rodDiameter);
|
|
screwElement->SetAttribute("isHorizonScan", configResult.algorithmParams.screwParam.isHorizonScan);
|
|
algoElement->InsertEndChild(screwElement);
|
|
|
|
XMLElement* filterElement = doc.NewElement("FilterParam");
|
|
filterElement->SetAttribute("continuityTh", configResult.algorithmParams.filterParam.continuityTh);
|
|
filterElement->SetAttribute("outlierTh", configResult.algorithmParams.filterParam.outlierTh);
|
|
algoElement->InsertEndChild(filterElement);
|
|
|
|
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);
|
|
|
|
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);
|
|
|
|
ConfigXmlUtils::SavePlaneCalibParams(doc, algoElement, configResult.algorithmParams.planeCalibParam);
|
|
ConfigXmlUtils::SaveDebugParam(doc, root, configResult.debugParam);
|
|
ConfigXmlUtils::SaveSerialConfig(doc, root, configResult.serialConfig);
|
|
|
|
for (auto& handEyeMatrix : configResult.handEyeCalibMatrixList) {
|
|
handEyeMatrix.eulerOrder = configResult.eulerOrder;
|
|
}
|
|
ConfigXmlUtils::SaveHandEyeCalibMatrixs(doc, root, configResult.handEyeCalibMatrixList);
|
|
|
|
XMLElement* networkElement = doc.NewElement("NetworkConfig");
|
|
networkElement->SetAttribute("tcpServerPort", configResult.tcpPort);
|
|
networkElement->SetAttribute("eulerOrder", configResult.eulerOrder);
|
|
networkElement->SetAttribute("dirVectorInvert", configResult.dirVectorInvert);
|
|
networkElement->SetAttribute("byteOrder", configResult.byteOrder);
|
|
networkElement->SetAttribute("longAxisDir", configResult.longAxisDir);
|
|
root->InsertEndChild(networkElement);
|
|
|
|
// Keep the legacy TCP node for backward compatibility.
|
|
XMLElement* tcpElement = doc.NewElement("TCP");
|
|
tcpElement->SetAttribute("port", configResult.tcpPort);
|
|
root->InsertEndChild(tcpElement);
|
|
|
|
const 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;
|
|
}
|
|
|
|
if (m_pNotify)
|
|
{
|
|
m_pNotify->OnConfigChanged(configResult);
|
|
}
|
|
|
|
LOG_INFO("Config saved successfully to: %s\n", filePath.c_str());
|
|
return true;
|
|
}
|
|
|
|
void CVrConfig::SetConfigChangeNotify(IVrConfigChangeNotify* notify)
|
|
{
|
|
m_pNotify = notify;
|
|
}
|
|
|
|
bool IVrConfig::CreateInstance(IVrConfig** ppVrConfig)
|
|
{
|
|
*ppVrConfig = new CVrConfig();
|
|
return *ppVrConfig != nullptr;
|
|
}
|