#include "VrConfig.h" #include #include #include #include "VrLog.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()) != "BagThreadPositionConfig") { LOG_ERR("Config file format error: root element is not BagThreadPositionConfig\n"); return LOAD_CONFIG_INVALID_FORMAT; } // 1. 解析相机设备列表 XMLElement* camerasElement = root->FirstChildElement("Cameras"); if (camerasElement) { configResult.cameraList.clear(); XMLElement* cameraElement = camerasElement->FirstChildElement("Camera"); while (cameraElement) { DeviceInfo camera; if (cameraElement->Attribute("name")) camera.name = cameraElement->Attribute("name"); if (cameraElement->Attribute("ip")) camera.ip = cameraElement->Attribute("ip"); cameraElement->QueryIntAttribute("index", &camera.index); cameraElement->QueryDoubleAttribute("baseDistance", &camera.baseDistance); configResult.cameraList.push_back(camera); LOG_INFO("Camera: name=%s, ip=%s, index=%d, baseDistance=%.1f\n", camera.name.c_str(), camera.ip.c_str(), camera.index, camera.baseDistance); cameraElement = cameraElement->NextSiblingElement("Camera"); } } // 2. 解析算法参数 XMLElement* algoElement = root->FirstChildElement("AlgorithmParams"); if (algoElement) { // 2.1 解析线头参数 (ThreadParam) XMLElement* threadElement = algoElement->FirstChildElement("ThreadParam"); if (threadElement) { threadElement->QueryBoolAttribute("isHorizonScan", &configResult.algorithmParams.threadParam.isHorizonScan); threadElement->QueryBoolAttribute("scanFromThreadHead", &configResult.algorithmParams.threadParam.scanFromThreadHead); threadElement->QueryDoubleAttribute("stitchWidth", &configResult.algorithmParams.threadParam.stitchWidth); threadElement->QueryDoubleAttribute("operateDist", &configResult.algorithmParams.threadParam.operateDist); threadElement->QueryDoubleAttribute("mark_diameter", &configResult.algorithmParams.threadParam.mark_diameter); threadElement->QueryDoubleAttribute("mark_height", &configResult.algorithmParams.threadParam.mark_height); threadElement->QueryDoubleAttribute("mark_distance", &configResult.algorithmParams.threadParam.mark_distance); LOG_INFO("ThreadParam: isHorizonScan=%d, stitchWidth=%.1f, operateDist=%.1f\n", configResult.algorithmParams.threadParam.isHorizonScan, configResult.algorithmParams.threadParam.stitchWidth, configResult.algorithmParams.threadParam.operateDist); LOG_INFO("ThreadParam Mark: diameter=%.1f, height=%.1f, distance=%.1f\n", configResult.algorithmParams.threadParam.mark_diameter, configResult.algorithmParams.threadParam.mark_height, configResult.algorithmParams.threadParam.mark_distance); } // 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 解析凸起特征参数 (RaisedFeatureParam) XMLElement* raisedElement = algoElement->FirstChildElement("RaisedFeatureParam"); if (raisedElement) { raisedElement->QueryDoubleAttribute("minJumpZ", &configResult.algorithmParams.raisedFeatureParam.minJumpZ); raisedElement->QueryDoubleAttribute("minK", &configResult.algorithmParams.raisedFeatureParam.minK); raisedElement->QueryDoubleAttribute("widthMin", &configResult.algorithmParams.raisedFeatureParam.widthMin); raisedElement->QueryDoubleAttribute("widthMax", &configResult.algorithmParams.raisedFeatureParam.widthMax); LOG_INFO("RaisedFeatureParam: minJumpZ=%.1f, minK=%.2f\n", configResult.algorithmParams.raisedFeatureParam.minJumpZ, configResult.algorithmParams.raisedFeatureParam.minK); } // 2.5 解析树生长参数 (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.6 解析平面标定参数 (PlaneCalibParams) XMLElement* calibElement = algoElement->FirstChildElement("PlaneCalibParams"); if (calibElement) { XMLElement* cameraCalibElement = calibElement->FirstChildElement("CameraCalibParam"); while (cameraCalibElement) { VrCameraPlaneCalibParam cameraParam; cameraCalibElement->QueryIntAttribute("cameraIndex", &cameraParam.cameraIndex); if (cameraCalibElement->Attribute("cameraName")) cameraParam.cameraName = cameraCalibElement->Attribute("cameraName"); cameraCalibElement->QueryBoolAttribute("isCalibrated", &cameraParam.isCalibrated); cameraCalibElement->QueryDoubleAttribute("planeHeight", &cameraParam.planeHeight); // 解析校准矩阵(展开的属性格式) cameraCalibElement->QueryDoubleAttribute("planeCalib_00", &cameraParam.planeCalib[0]); cameraCalibElement->QueryDoubleAttribute("planeCalib_01", &cameraParam.planeCalib[1]); cameraCalibElement->QueryDoubleAttribute("planeCalib_02", &cameraParam.planeCalib[2]); cameraCalibElement->QueryDoubleAttribute("planeCalib_10", &cameraParam.planeCalib[3]); cameraCalibElement->QueryDoubleAttribute("planeCalib_11", &cameraParam.planeCalib[4]); cameraCalibElement->QueryDoubleAttribute("planeCalib_12", &cameraParam.planeCalib[5]); cameraCalibElement->QueryDoubleAttribute("planeCalib_20", &cameraParam.planeCalib[6]); cameraCalibElement->QueryDoubleAttribute("planeCalib_21", &cameraParam.planeCalib[7]); cameraCalibElement->QueryDoubleAttribute("planeCalib_22", &cameraParam.planeCalib[8]); // 解析逆矩阵(展开的属性格式) cameraCalibElement->QueryDoubleAttribute("invRMatrix_00", &cameraParam.invRMatrix[0]); cameraCalibElement->QueryDoubleAttribute("invRMatrix_01", &cameraParam.invRMatrix[1]); cameraCalibElement->QueryDoubleAttribute("invRMatrix_02", &cameraParam.invRMatrix[2]); cameraCalibElement->QueryDoubleAttribute("invRMatrix_10", &cameraParam.invRMatrix[3]); cameraCalibElement->QueryDoubleAttribute("invRMatrix_11", &cameraParam.invRMatrix[4]); cameraCalibElement->QueryDoubleAttribute("invRMatrix_12", &cameraParam.invRMatrix[5]); cameraCalibElement->QueryDoubleAttribute("invRMatrix_20", &cameraParam.invRMatrix[6]); cameraCalibElement->QueryDoubleAttribute("invRMatrix_21", &cameraParam.invRMatrix[7]); cameraCalibElement->QueryDoubleAttribute("invRMatrix_22", &cameraParam.invRMatrix[8]); configResult.algorithmParams.planeCalibParam.SetCameraCalibParam(cameraParam); LOG_INFO("PlaneCalibParam: camera %d, isCalibrated=%d, planeHeight=%.3f\n", cameraParam.cameraIndex, cameraParam.isCalibrated, cameraParam.planeHeight); cameraCalibElement = cameraCalibElement->NextSiblingElement("CameraCalibParam"); } } } // 3. 解析调试参数 XMLElement* debugElement = root->FirstChildElement("DebugParam"); if (debugElement) { debugElement->QueryBoolAttribute("enableDebug", &configResult.debugParam.enableDebug); debugElement->QueryBoolAttribute("saveDebugImage", &configResult.debugParam.saveDebugImage); debugElement->QueryBoolAttribute("savePointCloud", &configResult.debugParam.savePointCloud); debugElement->QueryBoolAttribute("printDetailLog", &configResult.debugParam.printDetailLog); if (debugElement->Attribute("debugOutputPath")) configResult.debugParam.debugOutputPath = debugElement->Attribute("debugOutputPath"); LOG_INFO("Debug config: enableDebug=%d, saveDebugImage=%d, savePointCloud=%d, debugOutputPath=%s\n", configResult.debugParam.enableDebug, configResult.debugParam.saveDebugImage, configResult.debugParam.savePointCloud, configResult.debugParam.debugOutputPath.c_str()); } // 4. 解析串口配置 XMLElement* serialElement = root->FirstChildElement("SerialConfig"); if (serialElement) { if (serialElement->Attribute("portName")) configResult.serialConfig.portName = serialElement->Attribute("portName"); serialElement->QueryIntAttribute("baudRate", &configResult.serialConfig.baudRate); serialElement->QueryIntAttribute("dataBits", &configResult.serialConfig.dataBits); serialElement->QueryIntAttribute("stopBits", &configResult.serialConfig.stopBits); serialElement->QueryIntAttribute("parity", &configResult.serialConfig.parity); serialElement->QueryIntAttribute("flowControl", &configResult.serialConfig.flowControl); serialElement->QueryBoolAttribute("enabled", &configResult.serialConfig.enabled); LOG_INFO("Serial config: port=%s, baudRate=%d, enabled=%d\n", configResult.serialConfig.portName.c_str(), configResult.serialConfig.baudRate, configResult.serialConfig.enabled); } // 5. 解析TCP端口 XMLElement* tcpElement = root->FirstChildElement("TCP"); if (tcpElement) { int port = 5020; tcpElement->QueryIntAttribute("port", &port); configResult.tcpPort = static_cast(port); LOG_INFO("TCP port: %d\n", configResult.tcpPort); } // 6. 解析ModbusTCP配置 XMLElement* modbusElement = root->FirstChildElement("ModbusTCP"); if (modbusElement) { bool bigEndian = true; // 默认大端 modbusElement->QueryBoolAttribute("bigEndian", &bigEndian); configResult.modbusBigEndian = bigEndian; // 解析 Modbus Client 配置 if (modbusElement->Attribute("ip")) configResult.modbusIP = modbusElement->Attribute("ip"); int modbusPort = configResult.modbusPort; modbusElement->QueryIntAttribute("port", &modbusPort); configResult.modbusPort = static_cast(modbusPort); modbusElement->QueryIntAttribute("pollingInterval", &configResult.modbusPollingInterval); LOG_INFO("ModbusTCP bigEndian: %s, ip: %s, port: %d, pollingInterval: %d\n", bigEndian ? "true" : "false", configResult.modbusIP.c_str(), configResult.modbusPort, configResult.modbusPollingInterval); } 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("BagThreadPositionConfig"); doc.InsertEndChild(root); // 1. 保存相机设备列表 if (!configResult.cameraList.empty()) { XMLElement* camerasElement = doc.NewElement("Cameras"); for (const auto& camera : configResult.cameraList) { XMLElement* cameraElement = doc.NewElement("Camera"); cameraElement->SetAttribute("name", camera.name.c_str()); cameraElement->SetAttribute("ip", camera.ip.c_str()); cameraElement->SetAttribute("index", camera.index); cameraElement->SetAttribute("baseDistance", camera.baseDistance); camerasElement->InsertEndChild(cameraElement); } root->InsertEndChild(camerasElement); } // 2. 保存算法参数 XMLElement* algoElement = doc.NewElement("AlgorithmParams"); // 2.1 保存线头参数 XMLElement* threadElement = doc.NewElement("ThreadParam"); threadElement->SetAttribute("isHorizonScan", configResult.algorithmParams.threadParam.isHorizonScan); threadElement->SetAttribute("scanFromThreadHead", configResult.algorithmParams.threadParam.scanFromThreadHead); threadElement->SetAttribute("stitchWidth", configResult.algorithmParams.threadParam.stitchWidth); threadElement->SetAttribute("operateDist", configResult.algorithmParams.threadParam.operateDist); threadElement->SetAttribute("mark_diameter", configResult.algorithmParams.threadParam.mark_diameter); threadElement->SetAttribute("mark_height", configResult.algorithmParams.threadParam.mark_height); threadElement->SetAttribute("mark_distance", configResult.algorithmParams.threadParam.mark_distance); algoElement->InsertEndChild(threadElement); // 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* raisedElement = doc.NewElement("RaisedFeatureParam"); raisedElement->SetAttribute("minJumpZ", configResult.algorithmParams.raisedFeatureParam.minJumpZ); raisedElement->SetAttribute("minK", configResult.algorithmParams.raisedFeatureParam.minK); raisedElement->SetAttribute("widthMin", configResult.algorithmParams.raisedFeatureParam.widthMin); raisedElement->SetAttribute("widthMax", configResult.algorithmParams.raisedFeatureParam.widthMax); algoElement->InsertEndChild(raisedElement); // 2.5 保存树生长参数 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.6 保存平面标定参数 if (!configResult.algorithmParams.planeCalibParam.cameraCalibParams.empty()) { XMLElement* calibElement = doc.NewElement("PlaneCalibParams"); for (const auto& cameraParam : configResult.algorithmParams.planeCalibParam.cameraCalibParams) { XMLElement* cameraCalibElement = doc.NewElement("CameraCalibParam"); cameraCalibElement->SetAttribute("cameraIndex", cameraParam.cameraIndex); cameraCalibElement->SetAttribute("cameraName", cameraParam.cameraName.c_str()); cameraCalibElement->SetAttribute("isCalibrated", cameraParam.isCalibrated); cameraCalibElement->SetAttribute("planeHeight", cameraParam.planeHeight); // 保存校准矩阵(展开的属性格式) cameraCalibElement->SetAttribute("planeCalib_00", cameraParam.planeCalib[0]); cameraCalibElement->SetAttribute("planeCalib_01", cameraParam.planeCalib[1]); cameraCalibElement->SetAttribute("planeCalib_02", cameraParam.planeCalib[2]); cameraCalibElement->SetAttribute("planeCalib_10", cameraParam.planeCalib[3]); cameraCalibElement->SetAttribute("planeCalib_11", cameraParam.planeCalib[4]); cameraCalibElement->SetAttribute("planeCalib_12", cameraParam.planeCalib[5]); cameraCalibElement->SetAttribute("planeCalib_20", cameraParam.planeCalib[6]); cameraCalibElement->SetAttribute("planeCalib_21", cameraParam.planeCalib[7]); cameraCalibElement->SetAttribute("planeCalib_22", cameraParam.planeCalib[8]); // 保存逆矩阵(展开的属性格式) cameraCalibElement->SetAttribute("invRMatrix_00", cameraParam.invRMatrix[0]); cameraCalibElement->SetAttribute("invRMatrix_01", cameraParam.invRMatrix[1]); cameraCalibElement->SetAttribute("invRMatrix_02", cameraParam.invRMatrix[2]); cameraCalibElement->SetAttribute("invRMatrix_10", cameraParam.invRMatrix[3]); cameraCalibElement->SetAttribute("invRMatrix_11", cameraParam.invRMatrix[4]); cameraCalibElement->SetAttribute("invRMatrix_12", cameraParam.invRMatrix[5]); cameraCalibElement->SetAttribute("invRMatrix_20", cameraParam.invRMatrix[6]); cameraCalibElement->SetAttribute("invRMatrix_21", cameraParam.invRMatrix[7]); cameraCalibElement->SetAttribute("invRMatrix_22", cameraParam.invRMatrix[8]); calibElement->InsertEndChild(cameraCalibElement); } algoElement->InsertEndChild(calibElement); } root->InsertEndChild(algoElement); // 3. 保存调试参数 XMLElement* debugElement = doc.NewElement("DebugParam"); debugElement->SetAttribute("enableDebug", configResult.debugParam.enableDebug); debugElement->SetAttribute("savePointCloud", configResult.debugParam.savePointCloud); debugElement->SetAttribute("saveDebugImage", configResult.debugParam.saveDebugImage); debugElement->SetAttribute("printDetailLog", configResult.debugParam.printDetailLog); debugElement->SetAttribute("debugOutputPath", configResult.debugParam.debugOutputPath.c_str()); root->InsertEndChild(debugElement); // 4. 保存串口配置 XMLElement* serialElement = doc.NewElement("SerialConfig"); serialElement->SetAttribute("portName", configResult.serialConfig.portName.c_str()); serialElement->SetAttribute("baudRate", configResult.serialConfig.baudRate); serialElement->SetAttribute("dataBits", configResult.serialConfig.dataBits); serialElement->SetAttribute("stopBits", configResult.serialConfig.stopBits); serialElement->SetAttribute("parity", configResult.serialConfig.parity); serialElement->SetAttribute("flowControl", configResult.serialConfig.flowControl); serialElement->SetAttribute("enabled", configResult.serialConfig.enabled); root->InsertEndChild(serialElement); // 5. 保存TCP端口 XMLElement* tcpElement = doc.NewElement("TCP"); tcpElement->SetAttribute("port", configResult.tcpPort); root->InsertEndChild(tcpElement); // 6. 保存ModbusTCP配置 XMLElement* modbusElement = doc.NewElement("ModbusTCP"); modbusElement->SetAttribute("bigEndian", configResult.modbusBigEndian); modbusElement->SetAttribute("ip", configResult.modbusIP.c_str()); modbusElement->SetAttribute("port", configResult.modbusPort); modbusElement->SetAttribute("pollingInterval", configResult.modbusPollingInterval); root->InsertEndChild(modbusElement); // 保存到文件 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; }