From 0bd499588751fcc0655ccbee2b114bd0beb93d25 Mon Sep 17 00:00:00 2001 From: zhaojingkui <1553836110@qq.com> Date: Wed, 29 Nov 2023 12:41:28 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=BA=86=E5=8A=A8=E6=80=81?= =?UTF-8?q?=E9=85=8D=E7=BD=AE=E5=AE=89=E5=85=A8=E5=8C=BA=E5=92=8C=E8=B7=AF?= =?UTF-8?q?=E5=BE=84=E5=8F=82=E6=95=B0=E7=9A=84=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- launch/alpha.bhv | 8 ++-- src/pTaskManagement/TaskManger.cpp | 71 +++++++++++++++++------------- src/pTaskManagement/TaskManger.h | 3 +- 3 files changed, 47 insertions(+), 35 deletions(-) diff --git a/launch/alpha.bhv b/launch/alpha.bhv index e0164ef..6f406a0 100644 --- a/launch/alpha.bhv +++ b/launch/alpha.bhv @@ -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 + 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) diff --git a/src/pTaskManagement/TaskManger.cpp b/src/pTaskManagement/TaskManger.cpp index c4a16c9..0150b11 100644 --- a/src/pTaskManagement/TaskManger.cpp +++ b/src/pTaskManagement/TaskManger.cpp @@ -45,7 +45,7 @@ bool TaskManger::OnNewMail(MOOSMSG_LIST &NewMail) { CMOOSMsg &msg = *p; string msg_name = msg.GetName(); - string msg_str = msg.GetString(); + string msg_str = msg.GetString(); double msg_dval = msg.GetDouble(); bool msg_bval = msg.GetBinaryData(); // cout << msg_name + ": " << msg_str << endl; @@ -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() diff --git a/src/pTaskManagement/TaskManger.h b/src/pTaskManagement/TaskManger.h index 7a84d32..d2d379b 100644 --- a/src/pTaskManagement/TaskManger.h +++ b/src/pTaskManagement/TaskManger.h @@ -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";