From f5c34bd4b9eca20592cc02980edf3e4e87eb933b Mon Sep 17 00:00:00 2001 From: zhaojingkui <1553836110@qq.com> Date: Tue, 28 Nov 2023 16:06:38 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E4=BA=86=E4=BB=BB=E5=8A=A1?= =?UTF-8?q?=E7=AE=A1=E7=90=86=E5=99=A8=E6=8E=A5=E5=8F=97=E5=A4=96=E9=83=A8?= =?UTF-8?q?=E6=95=85=E9=9A=9C=E7=A0=81=E9=87=8D=E5=A4=8D=E4=B8=8A=E6=8A=A5?= =?UTF-8?q?=E7=9A=84=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/pTaskManagement/TaskManger.cpp | 87 ++++++++++++++++++++++------ src/pTaskManagement/TaskManger.h | 5 +- src/pTaskManagement/pTaskManger.moos | 3 +- 3 files changed, 74 insertions(+), 21 deletions(-) diff --git a/src/pTaskManagement/TaskManger.cpp b/src/pTaskManagement/TaskManger.cpp index e99ccd7..58e9bec 100644 --- a/src/pTaskManagement/TaskManger.cpp +++ b/src/pTaskManagement/TaskManger.cpp @@ -13,7 +13,7 @@ using namespace std; -#define DEBUG +// #define DEBUG //--------------------------------------------------------- @@ -138,7 +138,8 @@ bool TaskManger::OnNewMail(MOOSMSG_LIST &NewMail) if(fault_level != 0) { state = FAULT; - faultNumber = 1111; + if(!j["FaultMsgs"].isMember("uMission_status_fault")) + faultNumber = 1111; } } @@ -243,6 +244,16 @@ bool TaskManger::Iterate() st =30; faultMsg = "TaskFileError : " + intToString(readTask); } + readTask = readSafetyRules(); + if( readTask != 0 ) + { + state = FAULT; + faultNumber = 14 + readTask; + st = 30; + faultMsg = "safetyFileError : " + intToString(readTask); + } + //发送安全规则设置 + setSafetyRules(maxTime, maxDepth, minAltitude, safePolygon); st = 0; break; } @@ -346,7 +357,7 @@ bool TaskManger::Iterate() { case 40: //配置安全规则 { - readSafetyRules("a"); + readSafetyRules(); setSafetyRules(maxTime, maxDepth, minAltitude, safePolygon); // setMaxDepth(maxDepth); setMaxDepth("2"); @@ -429,13 +440,19 @@ bool TaskManger::OnStartUp() if(param == "PLANCONFIGPATH") { planConfigPath = value; - RepList["CueenPath"] = planConfigPath; - } + //RepList["CueenPath"] = planConfigPath; + } + if(param == "SAFETYRULESPATH") + { + safetyRulesPath = value; + //RepList["RulesPath"] = safetyRulesPath; + } } } if(planConfigPath == "") reportConfigWarning("NO TASK FILE PATH"); - // readTaskFile("a"); + if(planConfigPath == "") + reportConfigWarning("NO TASK SAFETY RULES CONFIG"); RegisterVariables(); return(true); } @@ -614,7 +631,7 @@ void TaskManger::RegisterVariables() */ int TaskManger::readTaskFile(string taskName) { - int faultNubmer = 0; + int fault = 0; if(!nodeList.empty()) nodeList.clear(); ifstream ifs; @@ -631,7 +648,7 @@ int TaskManger::readTaskFile(string taskName) if (!inputJsonValue.isMember(taskName)) { RepList["Task in File"] = "False"; - return faultNubmer=1; + return fault=1; } else { @@ -639,13 +656,13 @@ int TaskManger::readTaskFile(string taskName) string node=""; currentTask = inputJsonValue[taskName]; if(currentTask["taskName"].asString() != taskName) - return faultNubmer=2; + return fault=2; double currentTask_maxTime = currentTask["duration"].asDouble(); double repeat = currentTask["repeat"].asDouble(); if(!currentTask["points"].isArray()) - return faultNubmer=3; + return fault=3; Json::Value currentTask_Points = currentTask["points"]; int ps_cnt = currentTask_Points.size(); @@ -663,7 +680,7 @@ int TaskManger::readTaskFile(string taskName) else if(node_type == "track") node_type = "@2"; else - return faultNubmer=4; + return fault=4; node = node_type; node += ","; node += node_x; @@ -688,13 +705,43 @@ int TaskManger::readTaskFile(string taskName) cout << "--------------------------------" << endl; return 0; } - -int TaskManger::readSafetyRules(string fileName) +//maxTime, maxDepth, minAltitude, safePolygon +int TaskManger::readSafetyRules() { - maxDepth = "30"; - maxTime = "1000"; - safePolygon = "pts={-80,-00:-30,-175:150,-100:95,25}"; + ifstream ifs; + ifs.open(planConfigPath, ios::in); + Json::Reader reader; + Json::Value inputJsonValue; + reader.parse(ifs, inputJsonValue); + ifs.close(); + if(!inputJsonValue.isMember("maxTime") + || !inputJsonValue.isMember("maxDepth") + || !inputJsonValue.isMember("minAltitude") + || !inputJsonValue.isMember("points")) + return 1; + if(!inputJsonValue["points"].isArray()) + return 2; + double time = inputJsonValue["maxTime"].asDouble() * 60; + double depth = inputJsonValue["maxDepth"].asDouble(); + double altitude = inputJsonValue["minAltitude"].asDouble(); + maxTime = doubleToString(time); + maxDepth = doubleToString(depth); + minAltitude = doubleToString(altitude); + + safePolygon = ""; + Json::Value points = inputJsonValue["points"]; + int ps_cnt = points.size(); + for(int i=0; i taskList; int taskCount; string taskName; diff --git a/src/pTaskManagement/pTaskManger.moos b/src/pTaskManagement/pTaskManger.moos index 1e790d3..a9d1c58 100644 --- a/src/pTaskManagement/pTaskManger.moos +++ b/src/pTaskManagement/pTaskManger.moos @@ -7,6 +7,7 @@ ProcessConfig = pTaskManger { AppTick = 10 CommsTick = 10 - planConfigPath = /home/zjk/Desktop/project/moos-ivp-extend/PlanConfigure.json + planConfigPath = /home/zjk/project/work/moos-ivp-pi/setting/PlanConfigure.json + planConfigPath = /home/zjk/project/work/moos-ivp-pi/setting/SafetyRules.json }