迁移pi上的master分支
This commit is contained in:
999
src/pTaskManger/TaskManger.cpp
Normal file
999
src/pTaskManger/TaskManger.cpp
Normal file
@@ -0,0 +1,999 @@
|
||||
|
||||
/************************************************************/
|
||||
/* 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);
|
||||
}
|
||||
Reference in New Issue
Block a user