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

6
launch/alpha.bhv
View File

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

69
src/pTaskManagement/TaskManger.cpp
View File

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

3
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-29 10:08:55
* @LastEditTime: 2023-11-29 11:51:56
* @FilePath: /moos-ivp-pi/src/pTaskManagement/TaskManger.h
* @Description:
*
@@ -90,6 +90,7 @@ class TaskManger : public AppCastingMOOSApp
const string MSG_FALUT = "TaskFault";
const string MSG_RUN = "RUN";
const string MSG_CLEARFAULT = "ClearFalut";
const string MGS_SENDWAYPARAM = "SendWatParam"; //TODO:增加路径参数配置
const string MSG_IN_SSM = "uMission_action_cmd";
const string MSG_TO_SSM = "uMission_task_fb";