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 }