999 lines
22 KiB
C++
999 lines
22 KiB
C++
|
||
/************************************************************/
|
||
/* NAME: zjk */
|
||
/* ORGN: MIT */
|
||
/* FILE: TaskManger.cpp */
|
||
/* DATE: */
|
||
/************************************************************/
|
||
//TODO: 增加故障处理
|
||
#include <list>
|
||
#include <iterator>
|
||
#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;
|
||
}
|
||
|
||
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_dval == 1)
|
||
state = MANUAL;
|
||
else
|
||
state = UNRUN;
|
||
}
|
||
|
||
}
|
||
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
|
||
st = 38;
|
||
break;
|
||
}
|
||
case 31: //任务错误
|
||
{
|
||
postFaultNumber(faultNumber);
|
||
state = UNRUN;
|
||
break;
|
||
}
|
||
case 32: //任务文本错误
|
||
{
|
||
postFaultNumber(faultNumber);
|
||
state = UNRUN;
|
||
break;
|
||
}
|
||
case 38: //未知故障
|
||
{
|
||
postFaultNumber(99);
|
||
break;
|
||
}
|
||
case 39:
|
||
{
|
||
FaultFlagClear();
|
||
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<string> sParams;
|
||
if(m_MissionReader.GetConfiguration(GetAppName(), sParams))
|
||
{
|
||
list<string>::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);
|
||
}
|
||
/**
|
||
* @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<ps_cnt; i++)
|
||
{
|
||
string node_name = currentTask_Points[i]["name"].asString();
|
||
string node_x = currentTask_Points[i]["north"].asString();
|
||
string node_y = currentTask_Points[i]["east"].asString();
|
||
string node_depth = currentTask_Points[i]["depth"].asString();
|
||
string node_speed = currentTask_Points[i]["speed"].asString();
|
||
string node_type = currentTask_Points[i]["type"].asString();
|
||
|
||
if(node_type == "point")
|
||
node_type = "@1";
|
||
else if(node_type == "track")
|
||
node_type = "@2";
|
||
else
|
||
return faultNubmer=4;
|
||
node = node_type;
|
||
node += ",";
|
||
node += node_x;
|
||
node += ",";
|
||
node += node_y;
|
||
node += ",";
|
||
node += node_depth;
|
||
node += ",";
|
||
node += node_speed;
|
||
node += ",";
|
||
node += node_name;
|
||
nodeList.push_back(node);
|
||
}
|
||
}
|
||
auto i = nodeList.begin();
|
||
cout << "-------current task node--------" << endl;
|
||
while (i != nodeList.end())
|
||
{
|
||
cout << *i << endl;
|
||
i++;
|
||
}
|
||
cout << "--------------------------------" << endl;
|
||
return 0;
|
||
}
|
||
|
||
int TaskManger::readSafetyRules(string fileName)
|
||
{
|
||
maxDepth = "30";
|
||
maxTime = "1000";
|
||
safePolygon = "pts={-80,-00:-30,-175:150,-100:95,25}";
|
||
return 0;
|
||
}
|
||
|
||
int TaskManger::readWayConfig(string filename)
|
||
{
|
||
lead = "8";
|
||
lead_damper = "1";
|
||
lead_to_start = "false";
|
||
capture_line = "true";
|
||
capture_radius = "5";
|
||
slip_radius = "15";
|
||
efficiency_measure = "all";
|
||
}
|
||
|
||
/**
|
||
* @description: get Depth form
|
||
* @param {string} node
|
||
* @return {*}
|
||
*/
|
||
string TaskManger::getDepth(string node)
|
||
{
|
||
string depth;
|
||
if(getWord(node, 3, depth))
|
||
{
|
||
// cout << "Current Node Depth is " << depth <<endl;
|
||
return depth;
|
||
}
|
||
else
|
||
cout << "Warming: read Depth Parm Filed" << endl;
|
||
return depth;
|
||
}
|
||
/**
|
||
* @description:
|
||
* @param {string} node
|
||
* @return {*}
|
||
*/
|
||
string TaskManger::getSpeed(string node)
|
||
{
|
||
string speed;
|
||
if(getWord(node, 4, speed))
|
||
{
|
||
// cout << "Current Node Speed is " << speed <<endl;
|
||
return speed;
|
||
}
|
||
else
|
||
cout << "Warming: read Speed Parm Filed" << endl;
|
||
// getWord(node, 3, depth);
|
||
return speed;
|
||
}
|
||
/**
|
||
* @description:
|
||
* @param {string} node
|
||
* @return {*}
|
||
*/
|
||
string TaskManger::getWayPoint(string node)
|
||
{
|
||
string x,y;
|
||
bool fr;
|
||
fr = getWord(node, 1, x);
|
||
fr &= getWord(node, 2, y);
|
||
if(fr)
|
||
{
|
||
// cout << "Current WayPoint is: " << x << "," << y << endl;
|
||
return x+","+y;
|
||
}
|
||
else
|
||
cout << "Warming: read WayPoint Parm Filed" << endl;
|
||
}
|
||
|
||
string TaskManger::getWayPolygon(const list<string> 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<string> &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<string> &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"] = 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);
|
||
} |