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-29 12:41:28 +08:00
parent c85bcf5ad5
commit 0bd4995887
3 changed files with 47 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
launch/alpha.bhv
View File

@@ -152,16 +152,16 @@ Behavior = BHV_MaxDepth
{ {
// General Behavior Parameters // General Behavior Parameters
// --------------------------- // ---------------------------
name = op_region // example name = op_region_uuv // example
pwt = 300 // default pwt = 300 // default
condition = MODE==ACTIVE condition = MODE == ACTIVE
updates = OPREGION_UPDATES // example updates = OPREGION_UPDATES // example
//templating = spawn
// Parameters specific to this behavior // Parameters specific to this behavior
// ------------------------------------ // ------------------------------------
max_time = 0.1 // default (seconds) max_time = 0.1 // default (seconds)
max_depth = 2 // default (meters) max_depth = 2 // default (meters)
min_altitude = 0 // default (meters) //min_altitude = 0 // default (meters)
reset_var = OPREGION_RESET // example reset_var = OPREGION_RESET // example
trigger_entry_time = 1 // default (seconds) trigger_entry_time = 1 // default (seconds)
trigger_exit_time = 0.5 // default (seconds) trigger_exit_time = 0.5 // default (seconds)

71
src/pTaskManagement/TaskManger.cpp
View File

@@ -45,7 +45,7 @@ bool TaskManger::OnNewMail(MOOSMSG_LIST &NewMail)
{ {
CMOOSMsg &msg = *p; CMOOSMsg &msg = *p;
string msg_name = msg.GetName(); string msg_name = msg.GetName();
string msg_str = msg.GetString(); string msg_str = msg.GetString();
double msg_dval = msg.GetDouble(); double msg_dval = msg.GetDouble();
bool msg_bval = msg.GetBinaryData(); bool msg_bval = msg.GetBinaryData();
// cout << msg_name + ": " << msg_str << endl; // cout << msg_name + ": " << msg_str << endl;
@@ -93,7 +93,12 @@ bool TaskManger::OnNewMail(MOOSMSG_LIST &NewMail)
st = 39; st = 39;
state = FAULT; state = FAULT;
} }
if(msg_name == MGS_SENDWAYPARAM && msg_str == "true")
{
state = CONFIG;
st = 41;
}
// 控制消息
if(msg_name == MSG_IN_SSM) if(msg_name == MSG_IN_SSM)
{ {
Json::Value j; Json::Value j;
@@ -243,16 +248,6 @@ bool TaskManger::Iterate()
st =30; st =30;
faultMsg = "TaskFileError : " + intToString(readTask); 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; st = 0;
break; break;
} }
@@ -356,16 +351,30 @@ bool TaskManger::Iterate()
{ {
case 40: //配置安全规则 case 40: //配置安全规则
{ {
readSafetyRules(); readTask = readSafetyRules();
if( readTask != 0 )
{
state = FAULT;
faultNumber = 14 + readTask;
st = 30;
faultMsg = "safetyFileError : " + intToString(readTask);
}
//发送安全规则设置
setSafetyRules(maxTime, maxDepth, minAltitude, safePolygon); setSafetyRules(maxTime, maxDepth, minAltitude, safePolygon);
// setMaxDepth(maxDepth);
setMaxDepth(maxDepth); setMaxDepth(maxDepth);
st = 49; st = 49;
break; break;
} }
case 41: //路径参数配置 case 41: //路径参数配置
{ {
readWayConfig(); readTask = readWayConfig();
if( readTask != 0 )
{
state = FAULT;
faultNumber = 14 + readTask;
st = 30;
faultMsg = "safetyFileError : " + intToString(readTask);
}
setWayConfig(lead,lead_damper,capture_line,capture_radius,slip_radius); setWayConfig(lead,lead_damper,capture_line,capture_radius,slip_radius);
st = 49; st = 49;
//setWayConfig(); //setWayConfig();
@@ -415,7 +424,7 @@ bool TaskManger::Iterate()
} }
} }
postReportToSSM(); postReportToSSM();
AppCastingMOOSApp::PostReport(); // AppCastingMOOSApp::PostReport();
return(true); return(true);
} }
@@ -459,6 +468,12 @@ bool TaskManger::OnStartUp()
reportConfigWarning("NO TASK SAFETY RULES CONFIG"); reportConfigWarning("NO TASK SAFETY RULES CONFIG");
if(wayParamPath == "") if(wayParamPath == "")
reportConfigWarning("NO WAY PARAM CONFIG"); reportConfigWarning("NO WAY PARAM CONFIG");
readSafetyRules();
setSafetyRules(maxTime, maxDepth, minAltitude, safePolygon);
readWayConfig();
setWayConfig(lead,lead_damper,capture_line,capture_radius,slip_radius);
RegisterVariables(); RegisterVariables();
return(true); return(true);
} }
@@ -572,20 +587,21 @@ bool TaskManger::setTaskTimer(string timeCount)
bool TaskManger::setSafetyRules(string maxTime, string maxDepth, string minAltitude, string polygon) bool TaskManger::setSafetyRules(string maxTime, string maxDepth, string minAltitude, string polygon)
{ {
string msgContent=""; string msgContent="";
// msgContent += "name=op_region#";
msgContent += "max_time="; msgContent += "max_time=";
msgContent += maxTime; msgContent += maxTime;
msgContent += "#"; msgContent += "#";
msgContent += "max_depth="; msgContent += "max_depth=";
msgContent += maxDepth; msgContent += maxDepth;
msgContent += "#"; // msgContent += "#";
msgContent += "min_altitude="; // msgContent += "min_altitude=";
msgContent += minAltitude; // msgContent += minAltitude;
msgContent += "#"; msgContent += "#";
msgContent += "polygon="; msgContent += "polygon=";
msgContent += polygon; msgContent += polygon;
cout << " The Op_Region parm is : " << msgContent << endl;
Notify(UPDATE_OPREGION, msgContent); Notify(UPDATE_OPREGION, msgContent);
cout << msgContent << endl;
// Notify(UPDATE_OPREGION, msgContent);
return true; return true;
} }
bool TaskManger::setWayConfig(string lead, string lead_damper, string capture_line, string capture_radius,string slip_radius) bool TaskManger::setWayConfig(string lead, string lead_damper, string capture_line, string capture_radius,string slip_radius)
@@ -606,9 +622,8 @@ bool TaskManger::setWayConfig(string lead, string lead_damper, string capture_li
msgContent += "#"; msgContent += "#";
msgContent += "slip_radius="; msgContent += "slip_radius=";
msgContent += slip_radius; msgContent += slip_radius;
Notify(UPDATE_WPT, msgContent); Notify(UPDATE_WPT, msgContent);
cout << "Config waypoint parm is :" << msgContent << endl; cout << msgContent << endl;
} }
/** /**
@@ -622,6 +637,7 @@ void TaskManger::RegisterVariables()
// Register(MSG_START, 0); // Register(MSG_START, 0);
Register(MSG_WPTFLAG,0); Register(MSG_WPTFLAG,0);
Register(MSG_SENDSAFTRULES,0); Register(MSG_SENDSAFTRULES,0);
Register(MGS_SENDWAYPARAM,0);
Register(MSG_FALUT,0); Register(MSG_FALUT,0);
Register(MSG_CLEARFAULT,0); Register(MSG_CLEARFAULT,0);
Register(MSG_IN_SSM,0); Register(MSG_IN_SSM,0);
@@ -947,8 +963,8 @@ string TaskManger::getNodeName(const string node)
{ {
return name; return name;
} }
else // else
cout << "Warming: read Speed Parm Filed" << endl; // cout << "Warming: read Speed Parm Filed" << endl;
// getWord(node, 3, depth); // getWord(node, 3, depth);
return name; return name;
} }
@@ -1067,11 +1083,6 @@ inline void TaskManger::InitConfig()
Notify(MSG_WPTFLAG,"false"); Notify(MSG_WPTFLAG,"false");
// Notify(MSG_START,"false"); // Notify(MSG_START,"false");
Notify(MSG_RUN,"false"); Notify(MSG_RUN,"false");
//
readSafetyRules();
setSafetyRules(maxTime, maxDepth, minAltitude, safePolygon);
readWayConfig();
setWayConfig(lead,lead_damper,capture_line,capture_radius,slip_radius);
} }
void TaskManger::postReportToSSM() void TaskManger::postReportToSSM()

3
src/pTaskManagement/TaskManger.h
View File

@@ -2,7 +2,7 @@
* @Author: 1553836110 1553836110@qq.com * @Author: 1553836110 1553836110@qq.com
* @Date: 2023-09-28 15:45:17 * @Date: 2023-09-28 15:45:17
* @LastEditors: zhaojingkui 1553836110@qq.com * @LastEditors: zhaojingkui 1553836110@qq.com
* @LastEditTime: 2023-11-29 10:08:55 * @LastEditTime: 2023-11-29 11:51:56
* @FilePath: /moos-ivp-pi/src/pTaskManagement/TaskManger.h * @FilePath: /moos-ivp-pi/src/pTaskManagement/TaskManger.h
* @Description: * @Description:
* *
@@ -90,6 +90,7 @@ class TaskManger : public AppCastingMOOSApp
const string MSG_FALUT = "TaskFault"; const string MSG_FALUT = "TaskFault";
const string MSG_RUN = "RUN"; const string MSG_RUN = "RUN";
const string MSG_CLEARFAULT = "ClearFalut"; const string MSG_CLEARFAULT = "ClearFalut";
const string MGS_SENDWAYPARAM = "SendWatParam"; //TODO:增加路径参数配置
const string MSG_IN_SSM = "uMission_action_cmd"; const string MSG_IN_SSM = "uMission_action_cmd";
const string MSG_TO_SSM = "uMission_task_fb"; const string MSG_TO_SSM = "uMission_task_fb";