From bee3fea02df75f0953525b9e74bbdad6f8d6b80c Mon Sep 17 00:00:00 2001 From: chenlizhi Date: Tue, 28 Nov 2023 09:52:36 +0800 Subject: [PATCH 1/3] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=BA=86=E7=8A=B6?= =?UTF-8?q?=E6=80=81=E7=AE=A1=E7=90=86=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/pStateManagement/CMakeLists.txt | 6 +++++- src/pStateManagement/StateManagement.cpp | 23 +++++++++++------------ src/pStateManagement/StateManagement.h | 6 ++++++ 3 files changed, 22 insertions(+), 13 deletions(-) diff --git a/src/pStateManagement/CMakeLists.txt b/src/pStateManagement/CMakeLists.txt index bde6d92..0e5676b 100644 --- a/src/pStateManagement/CMakeLists.txt +++ b/src/pStateManagement/CMakeLists.txt @@ -17,6 +17,9 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR}) #find_package (jsoncpp NO_MODULE REQUIRED) +find_package (GeographicLib REQUIRED) +include_directories(${GeographicLib_INCLUDE_DIRS}) + include_directories(/usr/include/jsoncpp/) link_directories(/usr/local/lib/) @@ -26,7 +29,8 @@ TARGET_LINK_LIBRARIES(pStateManagement ${MOOS_LIBRARIES} ${CMAKE_DL_LIBS} ${SYSTEM_LIBS} - ${DUNE_LIB} + ${DUNE_LIB} + ${GeographicLib_LIBRARIES} mbutil m pthread diff --git a/src/pStateManagement/StateManagement.cpp b/src/pStateManagement/StateManagement.cpp index 787cf75..5dbdb31 100644 --- a/src/pStateManagement/StateManagement.cpp +++ b/src/pStateManagement/StateManagement.cpp @@ -9,6 +9,7 @@ #include "MBUtils.h" #include "StateManagement.h" #include +#include using namespace std; @@ -36,7 +37,6 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail) for(p=NewMail.begin(); p!=NewMail.end(); p++) { CMOOSMsg &msg = *p; -#if 1 // Keep these around just for template string key = msg.GetKey(); string comm = msg.GetCommunity(); double dval = msg.GetDouble(); @@ -45,11 +45,10 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail) double mtime = msg.GetTime(); bool mdbl = msg.IsDouble(); bool mstr = msg.IsString(); -#endif Json::Value deviceState; - double manualState; - double missionState; + double manualState; + int missionState; if(key == "uManual_enable_cmd") { @@ -75,18 +74,18 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail) deviceState["opMode"] = opModeLists.external; } else if (fabs(manualState - 0) < 1e-6) //manualState=0 - { - if(missionState == 0) + { + switch (missionState) { + case 0: deviceState["opMode"] = opModeLists.error; - } - if(missionState == 1) - { + break; + case 1: deviceState["opMode"] = opModeLists.service; - } - else if((missionState == 3) ) - { + break; + case 3: deviceState["opMode"] = opModeLists.maneuver; + break; } } diff --git a/src/pStateManagement/StateManagement.h b/src/pStateManagement/StateManagement.h index 5da5f97..6635540 100644 --- a/src/pStateManagement/StateManagement.h +++ b/src/pStateManagement/StateManagement.h @@ -9,6 +9,12 @@ #define StateManagement_HEADER #include "MOOS/libMOOS/MOOSLib.h" +#include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h" +#include +#include +#include +#include +#include class StateManagement : public CMOOSApp { From dcf08e089aa5672e3318db7a84d5290cb4bbd54a Mon Sep 17 00:00:00 2001 From: zhaojingkui <1553836110@qq.com> Date: Tue, 28 Nov 2023 10:49:40 +0800 Subject: [PATCH 2/3] =?UTF-8?q?=E5=8F=96=E6=B6=88=E4=BA=86=E5=AF=B9launch?= =?UTF-8?q?=5Flocal.moos=E6=96=87=E4=BB=B6=E7=9A=84=E8=BF=BD=E8=B8=AA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- launch/launch_local.moos | 328 --------------------------------------- 1 file changed, 328 deletions(-) delete mode 100644 launch/launch_local.moos diff --git a/launch/launch_local.moos b/launch/launch_local.moos deleted file mode 100644 index 2d914e1..0000000 --- a/launch/launch_local.moos +++ /dev/null @@ -1,328 +0,0 @@ -//====================================== -//1. 在phare配置块里面添加需要的变量和调试端端口配置 -//2. Our define process中加入自定义程序 -//3. For test process 中加入配合调试的程序 - -ServerHost = localhost -ServerPort = 9000 -Simulator = true - -Community = pi - -MOOSTimeWarp = 1 - -LatOrigin = 43.825300 -LongOrigin = -70.330400 -AltOrigin = 0 - -VehicleName = lauv-150 - -LogEnable = true -//LogDir = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/log/ -LogDir = /home/ben/project/moos-ivp-pi/log/ -AuvDataLog = auvData.mdat -MissionHistoryLog = missionHistory.txt -ClientCommandLog = clientCommand.txt -FaultLog = faultLog.txt -MotionControlLog = motionControl.txt - -//llaOriginPath = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/setting/Origin.json -llaOriginPath = /home/ben/project/moos-ivp-pi/setting/Origin.json - -//------------------------------------------ -// Antler configuration block -ProcessConfig = ANTLER -{ - MSBetweenLaunches = 200 - //============MOOS process========================= - Run = MOOSDB @ NewConsole = false - Run = pHelmIvP @ NewConsole = false - Run = pNodeReporter @ NewConsole = false - Run = uProcessWatch @ NewConsole = false - Run = pRealm @ NewConsole = false - //Run = pShare @ NewConsole = false - Run = pMarineViewer @ NewConsole = false - //===========Our define process==================== - Run = pBoardSupportComm @ NewConsole = false - Run = pTaskManagement @ NewConsole = false - //Run = pMotionControler @ NewConsole = false - Run = pSurfaceSupportComm @ NewConsole = false - Run = pDataManagement @ NewConsole = false - Run = pFaultHandle @ NewConsole = false - Run = pStateManagement @ NewConsole = false - //===============For test process=================== - //Run = pEmulator @ NewConsole = false - Run = uSimMarine @ NewConsole = false - Run = pMarinePID @ NewConsole = false - -} - -ProcessConfig = pMarineViewer -{ - AppTick = 4 - CommsTick = 4 - - tiff_file = forrest19.tif - //tiff_file = MIT_SP.tif - vehicles_name_mode = names+depth //+shortmode - - - set_pan_x = -90 - set_pan_y = -280 - zoom = 0.65 - vehicle_shape_scale = 1.5 - hash_delta = 50 - hash_shade = 0.22 - hash_viewable = true - - trails_point_size = 1 - - //op_vertex = x=-83, y=-47, lwidth=1,lcolor=yellow,looped=true,group=moa - //op_vertex = x=-46.4, y=-129.2, lwidth=1,lcolor=yellow,looped=true,group=moa - //op_vertex = x=94.6, y=-62.2, lwidth=1,lcolor=yellow,looped=true,group=moa - //op_vertex = x=58, y=20, lwidth=1,lcolor=yellow,looped=true,group=moa - - // Appcast configuration - appcast_height = 75 - appcast_width = 30 - appcast_viewable = true - appcast_color_scheme = indigo - nodes_font_size = xlarge - procs_font_size = xlarge - appcast_font_size = large - - // datum_viewable = true - // datum_size = 18 - // gui_size = small - - // left_context[survey-point] = DEPLOY=true - // left_context[survey-point] = MOOS_MANUAL_OVERRIDE=false - // left_context[survey-point] = RETURN=false - - right_context[return] = DEPLOY=true - right_context[return] = MOOS_MANUAL_OVERRIDE=false - right_context[return] = RETURN=false - - scope = RETURN - scope = WPT_STAT - scope = VIEW_SEGLIST - scope = VIEW_POINT - scope = VIEW_POLYGON - scope = MVIEWER_LCLICK - scope = MVIEWER_RCLICK - - //button_one = START # START=true - button_one = START # uMission_action_cmd={"taskName":"east_waypt_survey","action":"start"} - //button_one = MOOS_MANUAL_OVERRIDE=false - button_two = STOP # uMission_action_cmd={"taskName":"east_waypt_survey","action":"stop"} - //button_two = MOOS_MANUAL_OVERRIDE=true - //button_three = FaultClear # ClearFalut = true - //button_four = SendSecurityZone # SendSaftRules = true - - - action = MENU_KEY=deploy # DEPLOY = true # RETURN = false - action+ = MENU_KEY=deploy # MOOS_MANUAL_OVERRIDE=false - action = RETURN=true - action = UPDATES_RETURN=speed=1.4 - cmd = label=DEPLOY, var=DEPLOY, sval=true, receivers=all:$(VNAMES) - cmd = label=DEPLOY, var=MOOS_MANUAL_OVERRIDE, sval=false, receivers=all:$(VNAMES) - cmd = label=DEPLOY, var=AVOID, sval=true, receivers=all:$(VNAMES) - cmd = label=DEPLOY, var=RETURN, sval=false, receivers=all:$(VNAMES) - cmd = label=DEPLOY, var=STATION_KEEP, sval=false, receivers=all:$(VNAMES) - - cmd = label=RETURN, var=RETURN, sval=true, receivers=all:$(VNAMES) - cmd = label=RETURN, var=STATION_KEEP, sval=false, receivers=all:$(VNAMES) - - cmd = label=PERMUTE, var=UTS_FORWARD, dval=0, receivers=shore - - cmd = label=STATION, var=STATION_KEEP, sval=true, receivers=all:$(VNAMES), color=pink - - cmd = label=LOITER-FAST, var=UP_LOITER, sval=speed=2.8, receivers=all:$(VNAMES) - cmd = label=LOITER-SLOW, var=UP_LOITER, sval=speed=1.4, receivers=all:$(VNAMES) - - cmd = label=LOITER-CLOCKWISE-TRUE, var=UP_LOITER, sval=clockwise=true, receivers=all:$(VNAMES) - cmd = label=LOITER-CLOCKWISE-FALSE, var=UP_LOITER, sval=clockwise=false, receivers=all:$(VNAMES) - cmd = label=LOITER-CLOCKWISE-BEST, var=UP_LOITER, sval=clockwise=false, receivers=all:$(VNAMES) -} - -ProcessConfig = pHelmIvP -{ - AppTick = 4 - CommsTick = 4 - - behaviors = alpha.bhv - domain = course:0:359:360 - domain = speed:0:10:101 - domain = depth:0:100:101 - - park_on_allstop = false - //park_on_allstop = true - -} -ProcessConfig = uProcessWatch -{ - AppTick = 4 - CommsTick = 4 - - watch_all = true - nowatch = uPokeDB* - nowatch = uQueryDB* - nowatch = uXMS* - nowatch = uMAC* - nowatch = pShare* - nowatch = pRealm* - nowatch = pNodeReporter* -} - -ProcessConfig = pNodeReporter -{ - AppTick = 2 - CommsTick = 2 - - platform_type = UUV - platform_color = red - platform_length = 4 -} - - -ProcessConfig = pShare -{ - AppTick = 2 - CommsTick = 2 - //UUV 信息 - output = src_name = NODE_REPORT*, route = 10.25.0.160:8085 - output = src_name = NODE_REPORT*, route = 10.25.0.248:8085 - output = src_name = NODE_REPORT*, route = 10.25.0.165:8085 - //App 信息 - output = src_name = APPCAST*, route = 10.25.0.160:8085 - output = src_name = APPCAST*, route = 10.25.0.165:8085 - output = src_name = APPCAST*, route = 10.25.0.248:8085 - //路径点信息 - output = src_name = VIEW*, route = 10.25.0.160:8085 - output = src_name = VIEW*, route = 10.25.0.165:8085 - output = src_name = VIEW*, route = 10.25.0.248:8085 - //调试端输入端口 - input = route = localhost:8081 - input = route = localhost:8082 - input = route = localhost:8083 -} - -ProcessConfig = pTaskManagement -{ - AppTick = 8 - CommsTick = 8 - //planConfigPath = /home/zjk/Desktop/project/moos-ivp-pi/setting/PlanConfigure.json - planConfigPath = /home/ben/project/moos-ivp-pi/setting/PlanConfigure.json - -} - -ProcessConfig = pBoardSupportComm -{ - AppTick = 5 - CommsTick = 5 -} - -ProcessConfig = uSimMarine -{ - AppTick = 10 - CommsTick = 10 - - START_X = 0 - START_Y = 0 - START_SPEED = 0 - START_HEADING = 180 - - PREFIX = NAV -} - -ProcessConfig = pMarinePID -{ - AppTick = 20 - CommsTick = 20 - - VERBOSE = true - DEPTH_CONTROL = false - - // Yaw PID controller - YAW_PID_KP = 0.5 - YAW_PID_KD = 0.0 - YAW_PID_KI = 0.0 - YAW_PID_INTEGRAL_LIMIT = 0.07 - - // Speed PID controller - SPEED_PID_KP = 1.0 - SPEED_PID_KD = 0.0 - SPEED_PID_KI = 0.0 - SPEED_PID_INTEGRAL_LIMIT = 0.07 - - //MAXIMUMS - MAXRUDDER = 100 - MAXTHRUST = 100 - - // A non-zero SPEED_FACTOR overrides use of SPEED_PID - // Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR - SPEED_FACTOR = 20 - -} - -ProcessConfig = pMotionControler -{ - AppTick = 5 - CommsTick = 5 - tardy_nav_thresh = 2.0 - tardy_helm_thresh = 2.0 - - cheak_stalensee = true - delta_freqency = 5 - - //config_file = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/setting/ControlParam.json - config_file = /home/ben/project/moos-ivp-pi/setting/ControlParam.json -} - -ProcessConfig = pEmulator -{ - AppTick = 5 - CommsTick = 5 - matlab_host = 10.25.0.137 - matlab_port = 8085 - local_port = 8080 - prefix = NAV - - start_x = 10 - start_y = 9 - start_z = 1 - start_heading = 30 -} - -ProcessConfig = pDataManagement -{ - AppTick = 4 - CommsTick = 4 -} - -ProcessConfig = pLogger -{ - AppTick = 8 - CommsTick = 8 - - AsyncLog = true - FileTimeStamp = true - - // For variables that are published in a bundle on their first post, - // explicitly declare their logging request - Log = IVPHELM_LIFE_EVENT @ 0 NOSYNC - Log = REPORT @ 0 NOSYNC - - LogAuxSrc = true - WildCardLogging = true - WildCardOmitPattern = *_STATUS - WildCardOmitPattern = DB_VARSUMMARY - WildCardOmitPattern = DB_RWSUMMARY - WildCardExclusionLog = true -} -ProcessConfig = pFaultHandle -{ - AppTick = 4 - CommsTick = 4 -} - From 5e042bfb62914eca8eeef947886fda91fa58720e Mon Sep 17 00:00:00 2001 From: zhaojingkui <1553836110@qq.com> Date: Tue, 28 Nov 2023 11:07:50 +0800 Subject: [PATCH 3/3] =?UTF-8?q?=E5=88=A0=E9=99=A4=E4=BA=86=E4=B8=8D?= =?UTF-8?q?=E9=9C=80=E8=A6=81=E8=B7=9F=E8=B8=AA=E7=9A=84=E6=96=87=E4=BB=B6?= =?UTF-8?q?,=E5=B9=B6=E5=A2=9E=E5=8A=A0=E4=BA=86REAMME=E7=9A=84=E8=AF=B4?= =?UTF-8?q?=E6=98=8E?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 4 +- PlanConfigure.json | 175 --------------------------------------------- README.md | 16 ++++- 3 files changed, 16 insertions(+), 179 deletions(-) delete mode 100644 PlanConfigure.json diff --git a/.gitignore b/.gitignore index ad97f86..19b6a37 100644 --- a/.gitignore +++ b/.gitignore @@ -101,7 +101,7 @@ dkms.conf /log /launch/launch_local.moos -bin/ -build/ +/bin/ +/build/ /scripts /.vscode diff --git a/PlanConfigure.json b/PlanConfigure.json deleted file mode 100644 index 861e03d..0000000 --- a/PlanConfigure.json +++ /dev/null @@ -1,175 +0,0 @@ -{ - "east_waypt_survey" : - { - "boardStamp" : 1698135269.1985879, - "clientStamp" : 1698135269.1812179, - "closedLoop" : true, - "constSpeed" : -1.0, - "duration" : -1.0, - "maxDepth" : -1.0, - "minDepth" : -1.0, - "origin" : - { - "altitude" : 0.0, - "lat" : 43.825298309326172, - "lon" : -70.330398559570312 - }, - "perpetual" : false, - "points" : - [ - { - "depth" : 9.0, - "east" : 121.51780491942634, - "lat" : 43.824428558349609, - "lon" : -70.328887939453125, - "name" : "station_1", - "north" : -96.635898081838207, - "speed" : 2.0, - "type" : "point" - }, - { - "depth" : 7.0, - "east" : 201.91511278899367, - "lat" : 43.824676513671875, - "lon" : -70.327888488769531, - "name" : "station_2", - "north" : -69.083922179977421, - "speed" : 2.5, - "type" : "point" - } - ], - "priority" : 10, - "repeat" : 3, - "sourceAddress" : "10.25.0.160", - "sourceName" : "CCU Neptus 0_163", - "taskId" : "1", - "taskName" : "east_waypt_survey" - }, - "plan1_toMoos" : - { - "boardStamp" : 1699602762.2845099, - "clientStamp" : 1699602762.7520001, - "closedLoop" : false, - "duration" : -1.0, - "maxDepth" : -1.0, - "minDepth" : -1.0, - "origin" : - { - "altitude" : 0.0, - "lat" : 43.825299999999999, - "lon" : -70.330399999999997 - }, - "perpetual" : false, - "points" : - [ - { - "depth" : 2.0, - "east" : 117.83291847226671, - "lat" : 43.825713999999998, - "lon" : -70.32893, - "name" : "Goto1", - "north" : 46.200319317940647, - "speed" : 2.0, - "type" : "point" - }, - { - "depth" : 2.0, - "east" : -17.18366087421261, - "lat" : 43.826782000000001, - "lon" : -70.330609999999993, - "name" : "Goto2", - "north" : 164.87635389378988, - "speed" : 2.0, - "type" : "point" - }, - { - "depth" : 2.0, - "east" : -241.19025325837993, - "lat" : 43.825465999999999, - "lon" : -70.333399999999997, - "name" : "Goto3", - "north" : 18.653618776002617, - "speed" : 2.0, - "type" : "point" - }, - { - "depth" : 2.0, - "east" : -203.76118848802312, - "lat" : 43.823234999999997, - "lon" : -70.332930000000005, - "name" : "Goto4", - "north" : -229.29782627916489, - "speed" : 2.0, - "type" : "point" - } - ], - "priority" : 10, - "repeat" : 1, - "sourceAddress" : "10.25.0.163", - "sourceName" : "CCU JHL 0_163", - "taskId" : "0,106,3,-96,8,-103,13,6,9,32,50,-13,47,-71,61,1", - "taskName" : "plan1_toMoos" - }, - "west_waypt_survey" : - { - "boardStamp" : 1698135268.3958621, - "clientStamp" : 1698135268.2057669, - "closedLoop" : true, - "constSpeed" : -1.0, - "duration" : -1.0, - "maxDepth" : -1.0, - "minDepth" : -1.0, - "origin" : - { - "altitude" : 0.0, - "lat" : 43.825298309326172, - "lon" : -70.330398559570312 - }, - "perpetual" : true, - "points" : - [ - { - "depth" : 9.0, - "east" : -91.445572043530944, - "lat" : 43.824195861816406, - "lon" : -70.331535339355469, - "name" : "station_1", - "north" : -122.49101460421512, - "speed" : 4.0 - }, - { - "depth" : 7.0, - "east" : 5.5235485468483718, - "lat" : 43.824298858642578, - "lon" : -70.330329895019531, - "name" : "station_2", - "north" : -111.04778559533926, - "speed" : 6.0 - }, - { - "depth" : 5.0, - "east" : 4.2961493948725868, - "lat" : 43.823516845703125, - "lon" : -70.330345153808594, - "name" : "station_3", - "north" : -197.93630920628678, - "speed" : 8.0 - }, - { - "depth" : 3.0, - "east" : -81.013520711457318, - "lat" : 43.823207855224609, - "lon" : -70.331405639648438, - "name" : "station_4", - "north" : -232.26737690334403, - "speed" : 10.0 - } - ], - "priority" : 10, - "repeat" : -1, - "sourceAddress" : "10.25.0.160", - "sourceName" : "CCU Neptus 0_163", - "taskId" : "2", - "taskName" : "west_waypt_survey" - } -} diff --git a/README.md b/README.md index 923b3b3..51d2d47 100644 --- a/README.md +++ b/README.md @@ -19,8 +19,20 @@ bin/ : 程序执行文件存放位置 # 文件说明 -./build.sh : 编译脚本 +build.sh : 编译脚本 -./clean.sh : 清除编译生成文件 +clean.sh : 清除编译生成文件 + +launch/launch.moos : Pi上的moos-ivp-pi启动脚本 + +alpha.moos : moos-ivp调试端启动脚本，在pi以外的调试计算机上使用，可以监控pi的各个状态。 + +.gitignore : git的配置文件，编辑这个可以文件以设置忽略哪些不需要跟踪的文件，比如编译生成文件等。 + +setting/PlanConfigure.json : 使命文本 + +setting/ControlParam.json ： 运动控制器参数配置文件 + +setting/Origin.json ：经纬度原点配置文件