zjk/moos-ivp-pi
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修复了任务管理器接受外部故障码重复上报的问题

Browse Source
This commit is contained in:
zhaojingkui
2023-11-28 16:06:38 +08:00
parent f47b198d6b
commit f5c34bd4b9
3 changed files with 74 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

87
src/pTaskManagement/TaskManger.cpp
View File

@@ -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<ps_cnt; i++)
{
string x = points["north"].asString();
string y = points["east"].asString();
if(safePolygon=="")
safePolygon = x + "," + y ;
else
safePolygon = safePolygon + ":" + x + "," + y ;
}
return 0;
}
int TaskManger::readWayConfig(string filename)
@@ -966,6 +1013,7 @@ bool TaskManger::buildReport()
}
m_msgs << "=========================================================" << endl;
RepList["Current Node"] = current_node;
RepList["Fault Number"] = faultNumber;
RepList["remaining number of nodes"] = (int)nodeList.size()+1;
string rep = Json::writeString(RepJsBuilder, RepList);
m_msgs << rep << endl;
@@ -992,7 +1040,7 @@ inline void TaskManger::InitConfig()
// Notify(MSG_START,"false");
Notify(MSG_RUN,"false");
//
readSafetyRules("q");
readSafetyRules();
setSafetyRules(maxTime, maxDepth, minAltitude, safePolygon);
readWayConfig("a");
setWayConfig(lead,lead_damper,capture_line,capture_radius,slip_radius);
@@ -1005,7 +1053,10 @@ void TaskManger::postReportToSSM()
msg["state"] = state;
msg["taskName"] = taskName;
msg["destName"] = getNodeName(current_node);
msg["errorCode"] = faultNumber;
if(faultNumber == 1111)
msg["errorCode"] = 0;
else
msg["errorCode"] = faultNumber;
// msg["progess"] =
// msg["eta"] =
RepList["toSSM"] = msg;

5
src/pTaskManagement/TaskManger.h
View File

@@ -2,7 +2,7 @@
* @Author: 1553836110 1553836110@qq.com
* @Date: 2023-09-28 15:45:17
* @LastEditors: zhaojingkui 1553836110@qq.com
* @LastEditTime: 2023-11-23 15:29:09
* @LastEditTime: 2023-11-28 15:38:44
* @FilePath: /moos-ivp-pi/src/pTaskManagement/TaskManger.h
* @Description:
*
@@ -65,7 +65,7 @@ class TaskManger : public AppCastingMOOSApp
TaskType getTaskTpye(const string node);
int readTaskFile(string taskName);
int readSafetyRules(string fileName);
int readSafetyRules();
int readWayConfig(string fileName);
void postReportToSSM();
@@ -141,6 +141,7 @@ private:
//任务文件参数
//TODO:动态配置任务文件等参数
string planConfigPath;
string safetyRulesPath;
vector<string> taskList;
int taskCount;
string taskName;

3
src/pTaskManagement/pTaskManger.moos
View File

@@ -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
}