/************************************************************/ /* NAME: zjk */ /* ORGN: MIT */ /* FILE: TaskManger.cpp */ /* DATE: */ /************************************************************/ //TODO: 增加故障处理 #include #include #include "MBUtils.h" #include "TaskManger.h" using namespace std; #define DEBUG //--------------------------------------------------------- // Constructor TaskManger::TaskManger() { cout << "Task Manger process staring ... " << endl; } //--------------------------------------------------------- // Destructor TaskManger::~TaskManger() { Notify("RUN","false"); Notify("MOOS_MANUAL_OVERRIDE","true"); } //--------------------------------------------------------- // Procedure: OnNewMail bool TaskManger::OnNewMail(MOOSMSG_LIST &NewMail) { AppCastingMOOSApp::OnNewMail(NewMail); MOOSMSG_LIST::iterator p; for(p=NewMail.begin(); p!=NewMail.end(); p++) { CMOOSMsg &msg = *p; string msg_name = msg.GetName(); string msg_str = msg.GetString(); double msg_dval = msg.GetDouble(); bool msg_bval = msg.GetBinaryData(); // cout << msg_name + ": " << msg_str << endl; if(msg_name == MSG_ENDFLAG) { if(msg_str == "true") current_node_complete = true; else current_node_complete = false; } if(msg_name == MSG_WPTFLAG) { if(msg_str == "true") current_pol_complete = true; else current_pol_complete = false; clearHelmFlag(); } if(msg_name == MSG_FALUT) { state = FAULT; faultMsg = msg_str; if(faultMsg == "AltitudeOut") faultNumber = 4; else if(faultMsg == "DepthOut") faultNumber = 1; else if(faultMsg == "RegionOut") faultNumber = 2; else if(faultMsg == "TimeOut") faultNumber = 2; else faultNumber = 99; } //CONFIG 状态和手动清除任务故障的功能 if(msg_name == MSG_SENDSAFTRULES && msg_str == "true") { state = CONFIG; st = 40; } if(msg_name == MSG_CLEARFAULT) { st = 39; state = FAULT; } if(msg_name == MSG_IN_SSM) { Json::Value j; Json::Reader a; a.parse(msg_str,j); RepList["FormSSM"] = j; if(j["action"].asString() == "start") { taskName = j["taskName"].asString(); state = RUN; cout << "Task Manager Status is Run!!!" << endl; start = true; st = 10; } else if (j["action"].asString() == "stop") { taskName = ""; state = UNRUN; cout << "Task Manager Status is UnRun!!!" << endl; st = 20; } else { RepList["RunWaring"] = "form SSM have unhandle action : " + j["action"].asString(); } } if(msg_name == MSG_IN_MAN) { if(msg_str == "true") state = MANUAL; else state = UNRUN; } if((msg_name == MSG_FH_LEV2) || (msg_name == MSG_FH_LEV3)) { Json::Value j; Json::Reader a; a.parse(msg_str,j); int fault_level = j["FaultLevel"].asInt(); if(fault_level != 0) { state = FAULT; faultNumber = 1111; } } } return(true); } //--------------------------------------------------------- // Procedure: OnConnectToServer bool TaskManger::OnConnectToServer() { Notify(MSG_ENDFLAG,"false"); Notify(MSG_WPTFLAG,"false"); // Notify(MSG_START,"false"); Notify(MSG_RUN,"false"); RegisterVariables(); InitConfig(); return(true); } //--------------------------------------------------------- // Procedure: Iterate() bool TaskManger::Iterate() { // happens AppTick times per second AppCastingMOOSApp::Iterate(); switch (state) { case RUN: //状态0~10 { switch(st) { case 0://发送任务开始、超时和第一个节点 { //1. send first node current_node = getNode(nodeList); if(current_node == "") st = 9; else { sendNode(current_node, nodeList); st = 1; } //2. send start taskStart(); current_node_complete = false; current_pol_complete = false; break; } case 1://等待节点完成信息 { if(current_node_complete || current_pol_complete) { cout << "Current WayPoint Node complete!" << endl; if(current_node == "") st = 9; else st = 2; } break; } case 2://读取当前节点信息 { current_node = getNode(nodeList); if(current_node != "") st = 3; else st = 1; break; } case 3://发送任务节点 { sendNode(current_node, nodeList); current_node_complete = false; current_pol_complete = false; st = 1; break; } case 9://task complete { taskFinish(); st = 10; start = false; break; } case 10://等待任务开始 { if(start) st = 11; break; } case 11://读取任务文件 { faultMsg = ""; readTask = readTaskFile(taskName); if( readTask != 0) { state = FAULT; faultNumber = 10 + readTask; st =30; faultMsg = "TaskFileError : " + intToString(readTask); } st = 0; break; } default: { st = 10; //cout << "State reset ..." << endl; //taskFinish(); break; } } break; } case UNRUN: //状态20~29 { switch(st) { case 20://进入UNRUN,初始化状态变量 { current_node_complete = false; current_pol_complete = false; current_node=""; start = false; Notify(MSG_ENDFLAG,"false"); Notify(MSG_WPTFLAG,"false"); // Notify(MSG_START,"false"); Notify(MSG_RUN,"false"); Notify("MOOS_MANUAL_OVERRIDE","true"); cout << "Wating task start......" << endl; st = 21; break; } case 21://等待任务开始 { break; } default: { st = 20; break; } } break; } case FAULT: //状态30~39 { switch(st) { case 30: //判断故障信息 { if(faultNumber>0 && faultNumber<10) st = 31; else if(faultNumber>10 && faultNumber<20) st = 32; else if(faultNumber == 1111) st = 33; else st = 38; break; } case 31: //任务错误 { postFaultNumber(faultNumber); state = UNRUN; break; } case 32: //任务文本错误 { postFaultNumber(faultNumber); state = UNRUN; break; } case 33: //外部故障 { state = UNRUN; break; } case 38: //未知故障 { postFaultNumber(99); break; } case 39: // 清除任务故障 { FaultFlagClear(); faultNumber = 0; state = UNRUN; break; } default: { st = 30; break; } } break; } case CONFIG: //状态40~49 { switch (st) { case 40: //配置安全规则 { readSafetyRules("a"); setSafetyRules(maxTime, maxDepth, minAltitude, safePolygon); // setMaxDepth(maxDepth); setMaxDepth("2"); st = 49; break; } case 41: //路径参数配置 { readWayConfig("a"); setWayConfig(lead,lead_damper,capture_line,capture_radius,slip_radius); st = 49; //setWayConfig(); } case 49: //等待 { state = UNRUN; break; } default: { st = 49; break; } } break; } case MANUAL: // 手操状态 { switch (st) { case 50: // 清除变量 current_node_complete = false; current_pol_complete = false; current_node=""; start = false; Notify(MSG_ENDFLAG,"false"); Notify(MSG_WPTFLAG,"false"); Notify(MSG_RUN,"false"); Notify("MOOS_MANUAL_OVERRIDE","true"); cout << "Wating task start......" << endl; st = 51; break; case 51: // 等待 break; default: st = 50; break; } break; } default: { st = 30; break; } } postReportToSSM(); AppCastingMOOSApp::PostReport(); return(true); } //--------------------------------------------------------- // Procedure: OnStartUp() // happens before connection is open bool TaskManger::OnStartUp() { AppCastingMOOSApp::OnStartUp(); list sParams; if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) { list::iterator p; for(p=sParams.begin(); p!=sParams.end(); p++) { string original_line = *p; string line = *p; string param = stripBlankEnds(toupper(biteString(line, '='))); string value = stripBlankEnds(line); if(param == "PLANCONFIGPATH") { planConfigPath = value; RepList["CueenPath"] = planConfigPath; } } } if(planConfigPath == "") reportConfigWarning("NO TASK FILE PATH"); // readTaskFile("a"); RegisterVariables(); return(true); } //--------------------------------------------------------- // Procedure: RegisterVariables /** * @description: * @param {double} depth * @return {*} */ bool TaskManger::setMaxDepth(string depth) { string msgContent = ""; msgContent = "max_depth="; msgContent += depth; Notify(UPDATE_MAXDEP, msgContent); cout << "set max depth is " + depth << "m!" << endl; return true; } bool TaskManger::setWayPol(string polygon, string speed, string depth) { string msgContent=""; if(polygon=="#") { msgContent = "speed="; msgContent += speed; Notify(UPDATE_SPD, msgContent); msgContent = "depth=" + depth; Notify(UPDATE_DEP, msgContent); } else { msgContent = "polygon="; msgContent += polygon; msgContent += "#"; msgContent += "speed="; msgContent += speed; Notify(UPDATE_WPT, msgContent); //使用模版参数生成一个新的行为r Notify(UPDATE_WPT,"name=way_track"); // msgContent = "polygon="; // msgContent += polygon; // msgContent += "#"; Notify(UPDATE_WPT, msgContent); msgContent = "speed="; msgContent += speed; Notify(UPDATE_SPD, msgContent); msgContent = "depth=" + depth; Notify(UPDATE_DEP, msgContent); }; return true; } /** * @description: * @param {string} wpt * @return {*} */ bool TaskManger::setWpt(std::string wpt, string speed, string depth) { string msgContent=""; msgContent += "points="; msgContent += wpt; msgContent += "#"; msgContent += "speed="; msgContent += speed; // msgContent += "#"; Notify(UPDATE_WPT,msgContent); Notify(UPDATE_WPT,"name=way_topoint"); msgContent = "depth=" + depth; Notify(UPDATE_DEP, msgContent); msgContent = "speed="; msgContent += speed; Notify(UPDATE_SPD, msgContent); cout << "MSG is : " << msgContent + "#depth=" << depth << endl; // Notify("WPT_UPDATE", wpt); return true; } /** * @description: * @param {double} speed * @return {*} */ bool TaskManger::setSpeed(double speed) { Notify("SPEED_UPDATE", speed); return true; } bool TaskManger::setTaskTimer(string timeCount) { cout << "Set task timer is " << timeCount << "seconds!" << endl; string msgContent=""; msgContent += "name=taskTimer"; msgContent += "#"; msgContent += "duration="; msgContent += timeCount; Notify(UPDATE_TIMER,msgContent); } bool TaskManger::setSafetyRules(string maxTime, string maxDepth, string minAltitude, string polygon) { string msgContent=""; msgContent += "max_time="; msgContent += maxTime; msgContent += "#"; msgContent += "max_depth="; msgContent += maxDepth; msgContent += "#"; msgContent += "min_altitude="; msgContent += minAltitude; msgContent += "#"; msgContent += "polygon="; msgContent += polygon; cout << " The Op_Region parm is : " << msgContent << endl; Notify(UPDATE_OPREGION, msgContent); return true; } bool TaskManger::setWayConfig(string lead, string lead_damper, string capture_line, string capture_radius,string slip_radius) { string msgContent=""; msgContent = "lead="; msgContent += lead; msgContent += "#"; msgContent += "lead_damper="; msgContent += lead_damper; msgContent += "#"; msgContent += "capture_line="; msgContent += capture_line; msgContent += "#"; msgContent += "capture_radius="; msgContent += capture_radius; msgContent += "#"; msgContent += "slip_radius="; msgContent += slip_radius; Notify(UPDATE_WPT, msgContent); cout << "Config waypoint parm is :" << msgContent << endl; } /** * @description: * @return {*} */ void TaskManger::RegisterVariables() { AppCastingMOOSApp::RegisterVariables(); Register(MSG_ENDFLAG, 0); // Register(MSG_START, 0); Register(MSG_WPTFLAG,0); Register(MSG_SENDSAFTRULES,0); Register(MSG_FALUT,0); Register(MSG_CLEARFAULT,0); Register(MSG_IN_SSM,0); Register(MSG_IN_MAN,0); Register(MSG_FH_LEV2,0); Register(MSG_FH_LEV3,0); } /** * @description: read task File to nodeList * @param {string} fileName * @return {*} 0 if true else >0 */ int TaskManger::readTaskFile(string taskName) { int faultNubmer = 0; if(!nodeList.empty()) nodeList.clear(); ifstream ifs; ifs.open(planConfigPath, ios::in); Json::Reader taskConfigureReader; Json::Value inputJsonValue; Json::Value currentTask; taskConfigureReader.parse(ifs, inputJsonValue); ifs.close(); int taskCount = inputJsonValue.size(); taskList = inputJsonValue.getMemberNames(); taskCount = inputJsonValue.size(); //part1 : 判断是否存在这个任务 if (!inputJsonValue.isMember(taskName)) { RepList["Task in File"] = "False"; return faultNubmer=1; } else { //part2 : 读取任务 string node=""; currentTask = inputJsonValue[taskName]; if(currentTask["taskName"].asString() != taskName) return faultNubmer=2; double currentTask_maxTime = currentTask["duration"].asDouble(); double repeat = currentTask["repeat"].asDouble(); if(!currentTask["points"].isArray()) return faultNubmer=3; Json::Value currentTask_Points = currentTask["points"]; int ps_cnt = currentTask_Points.size(); for(int i=0; i nodeList) { auto i = nodeList.begin(); string polygon=""; TaskType t; while(i != nodeList.end()) { if(getTaskTpye(*i)==WAYPOLYGON) { polygon += getWayPoint(*i); polygon += ":"; } else break; i++; } if(polygon.size()>1) polygon.erase(polygon.end()-1); // cout << "Current way polygon is: " << polygon << endl; return polygon; } /** * @description: * @param {string} node * @return {*} */ TaskType TaskManger::getTaskTpye(string node) { string type; TaskType nodetype; if(getWord(node, 0, type)) { // cout << "Current task type is: " << type << endl; // return type; if(type=="@1") return nodetype=WAYPOINTS; else if(type=="@2") return nodetype=WAYPOLYGON; else if(type=="@3") return nodetype=CONSTHEADING; else return nodetype=NOTYPE; } else cout << "Warming: read type of task Parm Filed" << endl; } /** * @description: * @param {string} str * @param {int} c * @return {*} */ bool TaskManger::getWord(const string str, int ct, string &word) { string cword; int count=0; for(auto c:str) { if(c==',') { if(count==ct) { word = cword; return true; } cword=""; count++; } else cword += c; } word = cword; if(ct==0) return true; else return false; } string TaskManger::getNode(list &nodelist) { string current_node; if(!nodelist.empty()) { cout << "The remaining number of nodes is: " << nodelist.size() << endl; current_node = nodelist.front(); nodelist.pop_front(); cout << "Current Node is " << current_node << endl; } else current_node=""; return current_node; } string TaskManger::getTimeCount() { return "1000"; } string TaskManger::getMaxDepth() { return "20"; } string TaskManger::getNodeName(const string node) { string name=""; if(getWord(node, 5, name)) { return name; } else cout << "Warming: read Speed Parm Filed" << endl; // getWord(node, 3, depth); return name; } bool TaskManger::sendNode(const string current_node, list &nodeList) { // cout << "Current WayPoint Node complete" << endl; string node_pol; nodeType = getTaskTpye(current_node); switch(nodeType) { case WAYPOINTS: { node_wpt = getWayPoint(current_node); node_depth = getDepth(current_node); node_speed = getSpeed(current_node); setWpt(node_wpt, node_speed, node_depth); current_node_complete = false; break; } case WAYPOLYGON: { node_depth = getDepth(current_node); node_speed = getSpeed(current_node); if (current_pol_complete && !current_node_complete) //wayflag发布但是endflag没有发布时代表不是首个节点,不发布路径信息 { node_pol = "#"; } else //if(current_pol_complete && current_node_complete) { nodeList.push_front(current_node); node_pol = getWayPolygon(nodeList); nodeList.pop_front(); } setWayPol(node_pol, node_speed, node_depth); cout << "MSG is :Pol=" + node_pol + "#speed=" + node_speed + "#depth=" + node_depth<< endl; break; } default: break; } current_node_complete = false; current_pol_complete = false; return true; } void TaskManger::FaultFlagClear() { faultNumber = 0; if(faultMsg == "AltitudeOut") Notify(UPDATE_RESETFAULT,"altittude"); else if(faultMsg == "DepthOut") Notify(UPDATE_RESETFAULT,"depth"); else if(faultMsg == "RegionOut") Notify(UPDATE_RESETFAULT,"poly"); else if(faultMsg == "TimeOut") Notify(UPDATE_RESETFAULT,"time"); else cout << "Can not Clear Unknow Fault Flag!!!" << endl; cout << "Clear Fault : " + faultMsg << endl; } bool TaskManger::buildReport() { switch (state) { case UNRUN: { m_msgs << "$Task Manager Status$:" << "UNRUN" << endl; m_msgs << "$Idel Task Number$:" << taskCount << endl; break; } case RUN: { m_msgs << "$Task Manager Status$:" << "RUN" << endl; m_msgs << "$Current Task Point$ :" << current_node << endl; m_msgs << "Currnet Task List :" << endl; for(string i : nodeList) m_msgs << " : " << i << endl; break; } case FAULT: { m_msgs << "$Task Manager Status$:" << "FAULT" << endl; break; } default: break; } m_msgs << "=========================================================" << endl; RepList["Current Node"] = current_node; RepList["remaining number of nodes"] = (int)nodeList.size()+1; string rep = Json::writeString(RepJsBuilder, RepList); m_msgs << rep << endl; return true; } inline void TaskManger::taskStart() { Notify("RUN","true"); Notify("MOOS_MANUAL_OVERRIDE","false"); } inline void TaskManger::taskFinish() { current_node=""; Notify("RUN","false"); state = UNRUN; cout << "The task node is all complete!!!" << endl; Notify("MOOS_MANUAL_OVERRIDE","true"); } inline void TaskManger::InitConfig() { Notify(MSG_ENDFLAG,"false"); Notify(MSG_WPTFLAG,"false"); // Notify(MSG_START,"false"); Notify(MSG_RUN,"false"); // readSafetyRules("q"); setSafetyRules(maxTime, maxDepth, minAltitude, safePolygon); readWayConfig("a"); setWayConfig(lead,lead_damper,capture_line,capture_radius,slip_radius); } void TaskManger::postReportToSSM() { string s_msg; Json::Value msg; msg["state"] = state; msg["taskName"] = taskName; msg["destName"] = getNodeName(current_node); msg["errorCode"] = faultNumber; // msg["progess"] = // msg["eta"] = RepList["toSSM"] = msg; s_msg = Json::writeString(RepJsBuilder,msg); Notify(MSG_TO_SSM, s_msg); } //1:超深 //2.超时 //3.超出区域 //4.低于最小高度 //11:任务文本 //99.未知故障 void TaskManger::postFaultNumber(int n) { Notify(MSG_TO_FH,n); }