diff --git a/.gitignore b/.gitignore index ad97f86..7fbfaf4 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/CMakeLists.txt b/CMakeLists.txt old mode 100644 new mode 100755 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 b/README old mode 100644 new mode 100755 diff --git a/README.md b/README.md old mode 100644 new mode 100755 index 923b3b3..ef04b78 --- a/README.md +++ b/README.md @@ -1,3 +1,4 @@ + # moos-ivp-pi 基于MOOS-ivp框架下150UUV本体控制软件 @@ -19,8 +20,22 @@ 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 ：经纬度原点配置文件 + +setting/SafetyRules.json : 安全规则配置文件 + +setting/WayConfigParam.json : 路径参数配置文件 diff --git a/alpha.moos b/alpha.moos deleted file mode 100644 index d65fb50..0000000 --- a/alpha.moos +++ /dev/null @@ -1,147 +0,0 @@ - -ServerHost = localhost -ServerPort = 9000 -Community = zjk -MOOSTimeWarp = 1 - -// Forest Lake -//这两个参数没有pMarineViewer就会闪退 -LatOrigin = 43.825300 -LongOrigin = -70.330400 - -//------------------------------------------ -// Antler configuration block -ProcessConfig = ANTLER -{ - MSBetweenLaunches = 200 - - Run = MOOSDB @ NewConsole = false - //Run = pLogger @ NewConsole = false - Run = pMarineViewer @ NewConsole = false - Run = pRealm @ NewConsole = false - Run = pShare @ NewConsole = false -} -ProcessConfig = pShare -{ - AppTick = 2 - CommsTick = 2 - - input = route = localhost:8085 - output = src_name=APPCAST_REQ, route=10.25.0.230:8081 - - //输出有两个端口，8081和8082，选择用1或者2 - //发送消息看以下格式 - //output = src_name=Y, dest_name=B, route=host:port - - output = src_name=uMission_action_cmd,route=10.25.0.230:8081 - output = src_name=uMotion_config_cmd,route=10.25.0.230:8081 -} - -ProcessConfig = pLogger -{ - AppTick = 8 - CommsTick = 8 - - AsyncLog = 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 - //Log = BHV_SETTINGS @ 0 NOSYNC - Log = OPREGION_RESET @ 0 NOSYNC - - LogAuxSrc = true - WildCardLogging = true - WildCardOmitPattern = *_STATUS - WildCardOmitPattern = DB_VARSUMMARY - WildCardOmitPattern = DB_RWSUMMARY - WildCardExclusionLog = true -} -//------------------------------------------ -// pMarineViewer config block - -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 # uMission_action_cmd={"taskName":"east_waypt_survey","action":"start"} - button_two = STOP # uMission_action_cmd={"taskName":"east_waypt_survey","action":"stop"} - button_three = ReConfig # uMotion_config_cmd = 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) -} \ No newline at end of file diff --git a/bin/placeholder b/bin/placehold similarity index 100% rename from bin/placeholder rename to bin/placehold diff --git a/build/.cmake/api/v1/query/client-vscode/query.json b/build/.cmake/api/v1/query/client-vscode/query.json old mode 100644 new mode 100755 diff --git a/build/.cmake/api/v1/reply/cache-v2-dd2c937f009d9a652f28.json b/build/.cmake/api/v1/reply/cache-v2-dd2c937f009d9a652f28.json old mode 100644 new mode 100755 diff --git a/build/.cmake/api/v1/reply/cmakeFiles-v1-a2b0be97f994224a37d6.json b/build/.cmake/api/v1/reply/cmakeFiles-v1-a2b0be97f994224a37d6.json old mode 100644 new mode 100755 diff --git a/build/.cmake/api/v1/reply/codemodel-v2-e5f65b12d81cb1175a33.json b/build/.cmake/api/v1/reply/codemodel-v2-e5f65b12d81cb1175a33.json old mode 100644 new mode 100755 diff --git a/build/.cmake/api/v1/reply/index-2023-11-24T08-55-39-0072.json b/build/.cmake/api/v1/reply/index-2023-11-24T08-55-39-0072.json deleted file mode 100644 index 27a9d91..0000000 --- a/build/.cmake/api/v1/reply/index-2023-11-24T08-55-39-0072.json +++ /dev/null @@ -1,116 +0,0 @@ -{ - "cmake" : - { - "generator" : - { - "name" : "Unix Makefiles" - }, - "paths" : - { - "cmake" : "/usr/bin/cmake", - "cpack" : "/usr/bin/cpack", - "ctest" : "/usr/bin/ctest", - "root" : "/usr/share/cmake-3.16" - }, - "version" : - { - "isDirty" : false, - "major" : 3, - "minor" : 16, - "patch" : 3, - "string" : "3.16.3", - "suffix" : "" - } - }, - "objects" : - [ - { - "jsonFile" : "codemodel-v2-e5f65b12d81cb1175a33.json", - "kind" : "codemodel", - "version" : - { - "major" : 2, - "minor" : 0 - } - }, - { - "jsonFile" : "cache-v2-dd2c937f009d9a652f28.json", - "kind" : "cache", - "version" : - { - "major" : 2, - "minor" : 0 - } - }, - { - "jsonFile" : "cmakeFiles-v1-a2b0be97f994224a37d6.json", - "kind" : "cmakeFiles", - "version" : - { - "major" : 1, - "minor" : 0 - } - } - ], - "reply" : - { - "client-vscode" : - { - "query.json" : - { - "requests" : - [ - { - "kind" : "cache", - "version" : 2 - }, - { - "kind" : "codemodel", - "version" : 2 - }, - { - "kind" : "toolchains", - "version" : 1 - }, - { - "kind" : "cmakeFiles", - "version" : 1 - } - ], - "responses" : - [ - { - "jsonFile" : "cache-v2-dd2c937f009d9a652f28.json", - "kind" : "cache", - "version" : - { - "major" : 2, - "minor" : 0 - } - }, - { - "jsonFile" : "codemodel-v2-e5f65b12d81cb1175a33.json", - "kind" : "codemodel", - "version" : - { - "major" : 2, - "minor" : 0 - } - }, - { - "error" : "unknown request kind 'toolchains'" - }, - { - "jsonFile" : "cmakeFiles-v1-a2b0be97f994224a37d6.json", - "kind" : "cmakeFiles", - "version" : - { - "major" : 1, - "minor" : 0 - } - } - ] - } - } - } -} diff --git a/build/.cmake/api/v1/reply/target-pBoardSupportComm-None-6b6714a772e692ce7bda.json b/build/.cmake/api/v1/reply/target-pBoardSupportComm-None-6b6714a772e692ce7bda.json old mode 100644 new mode 100755 diff --git a/build/.cmake/api/v1/reply/target-pClientViewer-None-637c837c32b7f1c4a3f8.json b/build/.cmake/api/v1/reply/target-pClientViewer-None-637c837c32b7f1c4a3f8.json old mode 100644 new mode 100755 diff --git a/build/.cmake/api/v1/reply/target-pDataManagement-None-3fb8b0228e67220a29b1.json b/build/.cmake/api/v1/reply/target-pDataManagement-None-3fb8b0228e67220a29b1.json old mode 100644 new mode 100755 diff --git a/build/.cmake/api/v1/reply/target-pEmulator-None-e8b09174b873ffc7a0a4.json b/build/.cmake/api/v1/reply/target-pEmulator-None-e8b09174b873ffc7a0a4.json old mode 100644 new mode 100755 diff --git a/build/.cmake/api/v1/reply/target-pFaultHandle-None-96d2d6ef9203623ec32c.json b/build/.cmake/api/v1/reply/target-pFaultHandle-None-96d2d6ef9203623ec32c.json old mode 100644 new mode 100755 diff --git a/build/.cmake/api/v1/reply/target-pMotionControler-None-3f183e81cc43357193c0.json b/build/.cmake/api/v1/reply/target-pMotionControler-None-3f183e81cc43357193c0.json old mode 100644 new mode 100755 diff --git a/build/.cmake/api/v1/reply/target-pStateManagement-None-12c0626db4f6cdfc3816.json b/build/.cmake/api/v1/reply/target-pStateManagement-None-12c0626db4f6cdfc3816.json old mode 100644 new mode 100755 diff --git a/build/.cmake/api/v1/reply/target-pSurfaceSupportComm-None-5c1b066d62accb72956a.json b/build/.cmake/api/v1/reply/target-pSurfaceSupportComm-None-5c1b066d62accb72956a.json old mode 100644 new mode 100755 diff --git a/build/.cmake/api/v1/reply/target-pTaskManagement-None-97473a05e92e62137b49.json b/build/.cmake/api/v1/reply/target-pTaskManagement-None-97473a05e92e62137b49.json old mode 100644 new mode 100755 diff --git a/build/.cmake/api/v1/reply/target-pTaskSend-None-3a4f500e73d7f1e47a5a.json b/build/.cmake/api/v1/reply/target-pTaskSend-None-3a4f500e73d7f1e47a5a.json old mode 100644 new mode 100755 diff --git a/build/read.txt b/build/read.txt old mode 100644 new mode 100755 diff --git a/launch/.LastOpenedMOOSLogDirectory b/launch/.LastOpenedMOOSLogDirectory old mode 100644 new mode 100755 diff --git a/launch/alpha.bhv b/launch/alpha.bhv old mode 100644 new mode 100755 index 7a727f1..6f406a0 --- a/launch/alpha.bhv +++ b/launch/alpha.bhv @@ -12,7 +12,6 @@ set MODE = T1{ TaskNum = t1 } - //----------路径点任务---------------------------- Behavior = BHV_Waypoint { @@ -153,21 +152,22 @@ Behavior = BHV_MaxDepth { // General Behavior Parameters // --------------------------- - name = op_region // example + name = op_region_uuv // example pwt = 300 // default - condition = MODE==TN + condition = MODE == ACTIVE updates = OPREGION_UPDATES // example - + //templating = spawn // Parameters specific to this behavior // ------------------------------------ - max_time = 20 // default (seconds) - max_depth = 25 // default (meters) - min_altitude = 0 // default (meters) + max_time = 0.1 // default (seconds) + max_depth = 2 // 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) polygon = pts={-80,-00:-30,-175:150,-100:95,25} + //polygon = -160,90:275,90:275,-360:-160,-360 breached_altitude_flag = TaskFault = AltitudeOut breached_depth_flag = TaskFault = DepthOut diff --git a/launch/alpha.moos b/launch/alpha.moos old mode 100644 new mode 100755 diff --git a/launch/launch.moos b/launch/launch.moos old mode 100644 new mode 100755 index 5d5f7e8..08f6d21 --- a/launch/launch.moos +++ b/launch/launch.moos @@ -18,15 +18,14 @@ AltOrigin = 0 VehicleName = lauv-150 LogEnable = false -LogDir = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/log/ +LogDir = /home/jhl/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/jhl/project/moos-ivp-pi/setting/Origin.json //------------------------------------------ // Antler configuration block @@ -210,7 +209,9 @@ ProcessConfig = pTaskManagement { AppTick = 8 CommsTick = 8 - planConfigPath = /home/zjk/Desktop/project/moos-ivp-pi/setting/PlanConfigure.json + planConfigPath = /home/jhl/project/moos-ivp-pi/setting/PlanConfigure.json + safetyRulesPath = /home/jhl/project/moos-ivp-pi/setting/SafetyRules.json + wayConfigParamPath = /home/jhl/project/moos-ivp-pi/setting/WayConfigParam.json } ProcessConfig = pBoardSupportComm @@ -272,7 +273,7 @@ ProcessConfig = pMotionControler cheak_stalensee = true delta_freqency = 5 - config_file = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/setting/ControlParam.json + config_file = /home/jhl/project/moos-ivp-pi/setting/ControlParam.json } ProcessConfig = pEmulator 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 -} - diff --git a/missions/alder/README b/missions/alder/README old mode 100644 new mode 100755 diff --git a/missions/alder/alder.bhv b/missions/alder/alder.bhv old mode 100644 new mode 100755 diff --git a/missions/alder/alder.moos b/missions/alder/alder.moos old mode 100644 new mode 100755 diff --git a/missions/alder/alder_orig.bhv b/missions/alder/alder_orig.bhv old mode 100644 new mode 100755 diff --git a/missions/s1_alpha/alpha.bhv b/missions/s1_alpha/alpha.bhv old mode 100644 new mode 100755 diff --git a/missions/s1_alpha/alpha.moos b/missions/s1_alpha/alpha.moos old mode 100644 new mode 100755 diff --git a/missions/xrelay/xrelay.moos b/missions/xrelay/xrelay.moos old mode 100644 new mode 100755 diff --git a/scripts/README b/scripts/README old mode 100644 new mode 100755 diff --git a/setting/ControlParam.json b/setting/ControlParam.json old mode 100644 new mode 100755 diff --git a/setting/Origin.json b/setting/Origin.json old mode 100644 new mode 100755 index 5822f5c..1cf6e0d --- a/setting/Origin.json +++ b/setting/Origin.json @@ -1,5 +1,5 @@ { - "AltOrigin" : 0, + "AltOrigin" : 0.0, "LatOrigin" : 50.825298309326172, "LongOrigin" : -90.330398559570312, "TaskName" : "east_waypt_survey" diff --git a/setting/PlanConfigure.json b/setting/PlanConfigure.json old mode 100644 new mode 100755 index 8737b95..545935b --- a/setting/PlanConfigure.json +++ b/setting/PlanConfigure.json @@ -67,8 +67,8 @@ }, "plan1_toMoos" : { - "boardStamp" : 1699602762.2845099, - "clientStamp" : 1699602762.7520001, + "boardStamp" : 1700965801.2335429, + "clientStamp" : 1700965795.187, "closedLoop" : false, "duration" : -1.0, "maxDepth" : -1.0, @@ -76,49 +76,49 @@ "origin" : { "altitude" : 0.0, - "lat" : 50.825299999999999, - "lon" : -90.330399999999997 + "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" + }, + { + "depth" : 2.0, + "lat" : 43.82414, + "lon" : -70.328389999999999, + "name" : "Goto5", "speed" : 2.0, "type" : "point" } @@ -127,7 +127,7 @@ "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", + "taskId" : "-104,-48,78,56,26,-62,60,-67,-16,20,-117,108,100,79,-81,-97", "taskName" : "plan1_toMoos" }, "west_waypt_survey" : @@ -156,7 +156,7 @@ "name" : "station_1", "north" : -122.49101460421512, "speed" : 4.0, - "type" : "point" + "type" : "point" }, { "depth" : 7.0, @@ -166,7 +166,7 @@ "name" : "station_2", "north" : -111.04778559533926, "speed" : 6.0, - "type" : "point" + "type" : "point" }, { "depth" : 5.0, @@ -176,7 +176,7 @@ "name" : "station_3", "north" : -197.93630920628678, "speed" : 8.0, - "type" : "track" + "type" : "track" }, { "depth" : 3.0, @@ -186,7 +186,7 @@ "name" : "station_4", "north" : -232.26737690334403, "speed" : 10.0, - "type" : "track" + "type" : "track" } ], "priority" : 10, diff --git a/setting/SafetyRules.json b/setting/SafetyRules.json new file mode 100644 index 0000000..f2505b5 --- /dev/null +++ b/setting/SafetyRules.json @@ -0,0 +1,24 @@ +{ + "maxTime" : 100, + "maxDepth" : 100, + "minAltitude" : 1, + "points" : + [ + { + "east" : 90, + "north" : -160 + }, + { + "east" : 90, + "north" : 275 + }, + { + "east" : -360, + "north" : 275 + }, + { + "east" : -360, + "north" : -160 + } + ] +} \ No newline at end of file diff --git a/setting/WayConfigParam.json b/setting/WayConfigParam.json new file mode 100644 index 0000000..3760740 --- /dev/null +++ b/setting/WayConfigParam.json @@ -0,0 +1,9 @@ +{ + "lead" : 8, + "lead_damper" : 1, + "lead_to_start" : false, + "capture_line" : true, + "capture_radius" : 5, + "slip_radius" : 15, + "efficiency_measure" :"all" +} diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/src/pBoardSupportComm/BoardSupportComm.cpp b/src/pBoardSupportComm/BoardSupportComm.cpp old mode 100644 new mode 100755 index 5e882ab..eab01b6 --- a/src/pBoardSupportComm/BoardSupportComm.cpp +++ b/src/pBoardSupportComm/BoardSupportComm.cpp @@ -13,31 +13,14 @@ #define TCP_RECEIVE_PORT 8001 #define TCP_SERVER_ADDRESS "127.0.0.1" -#define MOOS_AUV_SIM -// #define MATLAB_AUV_SIM -// #ifdef TRUE_AUV +//#define MOOS_AUV_SIM +#define MATLAB_AUV_SIM +//#define TRUE_AUV using namespace std; BoardSupportComm::BoardSupportComm() { - estimatedState.currentLon = 0; - estimatedState.currentLat = 0; - estimatedState.currentAltitude = 0; - estimatedState.referenceLon = 0; - estimatedState.referenceLat = 0; - estimatedState.referenceAltitude = 0; - estimatedState.offsetNorth = 0; - estimatedState.offsetEast = 0; - estimatedState.offsetDown = 0; - estimatedState.roll = 0; - estimatedState.pitch = 0; - estimatedState.yaw = 0; - estimatedState.linearVelocityNorth = 0; - estimatedState.linearVelocityEast = 0; - estimatedState.linearVelocityDown = 0; - estimatedState.height = 0; - estimatedState.depth = 0; embeddedInfo.header = 0xEBA1; embeddedInfo.count = 0; @@ -74,7 +57,7 @@ BoardSupportComm::BoardSupportComm() executeCommand.header = 0xEBA2; //1:[0,1] executeCommand.count = 16; //2:[2,3] executeCommand.size = 21; //3:[4] - executeCommand.drive_mode = 0xFF; //4:[5] + executeCommand.drive_mode = 0x02; //4:[5] executeCommand.thrust = 0; //5:[6] executeCommand.yaw = 0; //6:[7,8] executeCommand.depth = 0; //7:[9,10] @@ -89,26 +72,23 @@ BoardSupportComm::BoardSupportComm() executeCommand.footer = 0xEE2A; //16:[19,20] executeCommand.manual_mode = false; - tcpSockFD = -1; + embeddedInfo.header = 0xEBA1; + embeddedInfo.count = 0; tcpConnectRet = -1; + tcpSockFD = -1; + recvLen = -1; } //--------------------------------------------------------- // Destructor - BoardSupportComm::~BoardSupportComm() { - // delete tcpReceiveBuffer; close(tcpSockFD); - } -//--------------------------------------------------------- -// Procedure: OnNewMail - bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) { - // AppCastingMOOSApp::OnNewMail(NewMail); + AppCastingMOOSApp::OnNewMail(NewMail); MOOSMSG_LIST::iterator p; for(p=NewMail.begin(); p!=NewMail.end(); p++) @@ -127,84 +107,83 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) if(key == "NAV_X") { //E->N - estimatedState.offsetEast = msg.GetDouble(); + embeddedInfo.east = (int)dval; } if(key == "NAV_Y") { //N->E - estimatedState.offsetNorth = msg.GetDouble(); + embeddedInfo.north = (int)dval; } if(key == "NAV_Z") { //U->D - estimatedState.offsetDown = -msg.GetDouble(); + embeddedInfo.depth = -1 * dval * 100; } if(key == "NAV_YAW") { - double yawTemp = msg.GetDouble(); + double yawTemp = dval; //radian if (std::abs(yawTemp) <= M_PI) { if (yawTemp <= 0) { - estimatedState.yaw = -yawTemp * 180 / M_PI; + embeddedInfo.yaw = -yawTemp * 180 / M_PI * 100; } else { - estimatedState.yaw = (2 * M_PI - yawTemp) * 180 / M_PI; + embeddedInfo.yaw = (2 * M_PI - yawTemp) * 180 / M_PI * 100; } } } if(key == "NAV_PITCH") { - estimatedState.pitch = msg.GetDouble(); + embeddedInfo.pitch = dval * 100; } if(key == "NAV_LAT") { - estimatedState.currentLat = msg.GetDouble(); + embeddedInfo.lat = dval * 1000000; } if(key == "NAV_LONG") { - estimatedState.currentLon = msg.GetDouble(); + embeddedInfo.lon = dval * 1000000; } if(key == "NAV_SPEED") { - estimatedState.linearVelocityNorth = msg.GetDouble() * cos(estimatedState.yaw); - estimatedState.linearVelocityEast = -msg.GetDouble() * sin(estimatedState.yaw); - estimatedState.linearVelocityDown = 0; + float tempYaw = (float)(embeddedInfo.yaw) / 100.0; + embeddedInfo.ins_vx = dval * cos(tempYaw) * 100; + embeddedInfo.ins_vy = -dval * sin(tempYaw) * 100; + embeddedInfo.ins_vz = 0; + } if(key == "NAV_DEPTH") { - estimatedState.depth = msg.GetDouble(); + embeddedInfo.depth = dval * 100; } +#endif if(key == "Fault_LeakSensor") { - embeddedInfo.fault_leakSensor = (uint32_t)msg.GetDouble(); + embeddedInfo.fault_leakSensor = (uint32_t)dval; } if(key == "Fault_Battery") { - embeddedInfo.fault_battery = (uint8_t)msg.GetDouble(); + embeddedInfo.fault_battery = (uint8_t)dval; } if(key == "Fault_EmergencyBattery") { - embeddedInfo.fault_emergencyBattery = (uint8_t)(msg.GetDouble()); + embeddedInfo.fault_emergencyBattery = (uint8_t)dval; } if(key == "Fault_Thrust") { - embeddedInfo.fault_thrust = (uint8_t)(msg.GetDouble()); + embeddedInfo.fault_thrust = (uint8_t)dval; } -#endif - if(key == "uManual_enable_cmd") { - if (msg.GetDouble() == 1.0) + if (sval == "true") { - executeCommand.drive_mode = 0x02; executeCommand.manual_mode = true; } else { - executeCommand.drive_mode = 0xFF; executeCommand.manual_mode = false; } @@ -212,11 +191,15 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) if(key == "uManual_drive_cmd") { +#ifdef MOOS_AUV_SIM if (executeCommand.manual_mode) +#else + if (embeddedInfo.drive_mode == 0x02) +#endif { std::string err; Json::Value recvCommand; - std::istringstream iss(msg.GetString()); + std::istringstream iss(sval); Json::CharReaderBuilder builder; bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err); if (!parsingResult) @@ -230,26 +213,21 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) return false; } executeCommand.thrust = convertIntToUchar(recvCommand["Thrust"].asInt(), -100, 100); - float heading = recvCommand["Heading"].asFloat(); - if ((heading >= 0) && (heading <= 180)) - { - executeCommand.yaw = heading * 10; - } - - if ((heading < 0) && (heading >= -180)) - { - executeCommand.yaw = (360 + heading) * 10; - } - if ((heading > 180) or (heading < -180)) - { - executeCommand.yaw = 180 * 10; - } + uint8_t helm_top_bottom_angle = convertIntToUchar((int)(recvCommand["Heading"].asFloat()), -30, 30); executeCommand.depth = 0; - executeCommand.helm_top_angle = 0; - executeCommand.helm_bottom_angle = 0; + executeCommand.helm_top_angle = helm_top_bottom_angle; + executeCommand.helm_bottom_angle = helm_top_bottom_angle; executeCommand.helm_left_angle = 0; executeCommand.helm_right_angle = 0; + executeCommand.yaw = 0; + int serializeResult = serializeMessage(tcpSendBuffer); + std::stringstream ss; + ss << tcpSockFD; + ss << ", "; + ss << serializeResult; + castLogStream = ss.str(); + if ((serializeResult == 0) && (tcpSockFD != -1)) { try @@ -271,7 +249,7 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) } std::string err; Json::Value recvCommand; - std::istringstream iss(msg.GetString()); + std::istringstream iss(sval); Json::CharReaderBuilder builder; bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err); if (!parsingResult) @@ -319,7 +297,7 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) { std::string err; Json::Value recvCommand; - std::istringstream iss(msg.GetString()); + std::istringstream iss(sval); Json::CharReaderBuilder builder; bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err); if (!parsingResult) @@ -327,9 +305,9 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) std::cerr << "Failed to parse JSON string." << std::endl; return false; } - estimatedState.referenceAltitude = recvCommand["alt"].asFloat(); - estimatedState.referenceLat = recvCommand["lat"].asFloat(); - estimatedState.referenceLon = recvCommand["lon"].asFloat(); + embeddedInfo.refLon = recvCommand["lon"].asFloat(); + embeddedInfo.refLat = recvCommand["lat"].asFloat(); + embeddedInfo.refAlt = recvCommand["alt"].asFloat(); struct stat info; if (stat(llaOriginPath.c_str(), &info) == 0) @@ -366,9 +344,6 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) ofs.close(); } } - - - } return(true); @@ -415,15 +390,15 @@ std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState) embeddedState["driveMode"] = 0xFF; embeddedState["height"] = 0; embeddedState["depth"] = 0; - embeddedState["yaw"] = estimatedState.yaw; + embeddedState["yaw"] = embeddedInfo.yaw * 0.01 * M_PI / 180; embeddedState["pitch"] = 0; embeddedState["roll"] = 0; - embeddedState["insVX"] = estimatedState.linearVelocityNorth * 0.01; - embeddedState["insVY"] = estimatedState.linearVelocityEast * 0.01; - embeddedState["insVZ"] = estimatedState.linearVelocityDown * 0.01; - embeddedState["currentLon"] = estimatedState.currentLon; - embeddedState["currentLat"] = estimatedState.currentLat; - embeddedState["currentAltitude"] = 0; + embeddedState["insVX"] = embeddedInfo.ins_vx * 0.01; + embeddedState["insVY"] = embeddedInfo.ins_vy * 0.01; + embeddedState["insVZ"] = embeddedInfo.ins_vz * 0.01; + embeddedState["currentLon"] = embeddedInfo.lon * 0.000001; + embeddedState["currentLat"] = embeddedInfo.lat * 0.000001; + embeddedState["currentAltitude"] = embeddedInfo.alt * 0.01; embeddedState["dvlVX"] = 0; embeddedState["dvlVY"] = 0; embeddedState["dvlVZ"] = 0; @@ -432,10 +407,6 @@ std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState) embeddedState["batteryVoltage"] = 15; embeddedState["batteryLevel"] = 60; embeddedState["batteryTemp"] = 25; - // embeddedState["faultLeakSensor"] = 0; - // embeddedState["faultBattery"] = 0; - // embeddedState["faultEmergencyBattery"] = 0; - // embeddedState["faultThrust"] = 0; embeddedState["faultLeakSensor"] = embeddedInfo.fault_leakSensor; embeddedState["faultBattery"] = embeddedInfo.fault_battery; embeddedState["faultEmergencyBattery"] = embeddedInfo.fault_emergencyBattery; @@ -482,7 +453,7 @@ std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState) embeddedState["depth"] = embeddedInfo.depth * 0.01; embeddedState["roll"] = embeddedInfo.pitch * 0.01 * M_PI / 180; //E->N embeddedState["pitch"] = embeddedInfo.roll * 0.01 * M_PI / 180; //N->E - embeddedState["yaw"] = embeddedInfo.yaw * 0.01 * M_PI / 180; //D->D + embeddedState["yaw"] = embeddedInfo.yaw * 0.01 * M_PI / 180; //D->D radian<-degree embeddedState["insVX"] = embeddedInfo.ins_vy * 0.01; //E->N embeddedState["insVY"] = embeddedInfo.ins_vx * 0.01; //N->E embeddedState["insVZ"] = -embeddedInfo.ins_vz * 0.01; //U->D @@ -509,19 +480,18 @@ std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState) double currentLon = embeddedInfo.lon * 0.000001; double currentLat = embeddedInfo.lat * 0.000001; double currentAlt = embeddedInfo.alt * 0.01; - std::vector reference = {estimatedState.referenceLon, estimatedState.referenceLat, estimatedState.referenceAltitude}; + std::vector reference = {embeddedInfo.refLon, embeddedInfo.refLat, embeddedInfo.refAlt}; std::vector current = {currentLon, currentLat, currentAlt}; std::vector ned = {0, 0, 0}; ConvertLLAToNED(reference, current, ned); embeddedState["north"] = ned.at(0); embeddedState["east"]= ned.at(1); - embeddedState["referenceLon"]= estimatedState.referenceLon; - embeddedState["referenceLat"]= estimatedState.referenceLat; - embeddedState["referenceAltitude"]= estimatedState.referenceAltitude; + embeddedState["referenceLon"]= embeddedInfo.refLon; + embeddedState["referenceLat"]= embeddedInfo.refLat; + embeddedState["referenceAltitude"]= embeddedInfo.refAlt; Json::StreamWriterBuilder builder; - std::string embeddedStateString = Json::writeString(builder, embeddedState); - return embeddedStateString; + return Json::writeString(builder, embeddedState); } @@ -530,15 +500,15 @@ void BoardSupportComm::tcpProcessThread() while(1) { bzero(tcpReceiveBuffer, 0); - int lens = read(tcpSockFD, tcpReceiveBuffer, sizeof(tcpReceiveBuffer)); - if(lens>0) + recvLen = read(tcpSockFD, tcpReceiveBuffer, sizeof(tcpReceiveBuffer)); + if(recvLen > 0) { #ifndef MOOS_AUV_SIM - parseMessage((unsigned char* )tcpReceiveBuffer, lens); + parseMessage((unsigned char* )tcpReceiveBuffer, recvLen); #endif Json::Value embeddedState; - std::string embeddedStateString = convertEmbeddedFormat(embeddedState); - Notify("uDevice_monitor_fb", embeddedStateString); + recvContent = convertEmbeddedFormat(embeddedState); + Notify("uDevice_monitor_fb", recvContent); #ifndef MOOS_AUV_SIM Notify("NAV_X", embeddedState["north"].asDouble()); Notify("NAV_Y", embeddedState["east"].asDouble()); @@ -559,15 +529,17 @@ void BoardSupportComm::tcpProcessThread() } } -// bool BoardSupportComm::buildReport() -// { -// m_msgs << "buildReport:" << embeddedStateString << endl; -// return true; -// } +bool BoardSupportComm::buildReport() +{ + m_msgs << std::fixed << std::setprecision(6) << MOOS::Time() << ", recvLen:" << recvLen << endl; + m_msgs << std::fixed << std::setprecision(6) << MOOS::Time() << ", recvContent:" << recvContent << endl; + m_msgs << std::fixed << std::setprecision(6) << MOOS::Time() << ", sendContent:" << castLogStream << endl; + return true; +} bool BoardSupportComm::Iterate() { - // AppCastingMOOSApp::Iterate(); + AppCastingMOOSApp::Iterate(); if(tcpSockFD == -1) { @@ -591,8 +563,10 @@ bool BoardSupportComm::Iterate() return false; } } + if ((tcpSockFD != -1) && (tcpConnectRet != -1)) { + std::thread t1(&BoardSupportComm::tcpProcessThread, this); t1.detach(); } @@ -665,7 +639,7 @@ bool BoardSupportComm::Iterate() } #endif - //AppCastingMOOSApp::PostReport(); + AppCastingMOOSApp::PostReport(); return true; } @@ -763,7 +737,7 @@ int BoardSupportComm::parseMessage(unsigned char* buffer, int size) bool BoardSupportComm::OnStartUp() { - // AppCastingMOOSApp::OnStartUp(); + AppCastingMOOSApp::OnStartUp(); m_MissionReader.GetValue("llaOriginPath", llaOriginPath); try { @@ -782,9 +756,9 @@ bool BoardSupportComm::OnStartUp() originJsonValue.isMember("LatOrigin") && originJsonValue.isMember("AltOrigin")) { - estimatedState.referenceLon = originJsonValue["LongOrigin"].asFloat(); - estimatedState.referenceLat = originJsonValue["LatOrigin"].asFloat(); - estimatedState.referenceAltitude = originJsonValue["AltOrigin"].asFloat(); + embeddedInfo.refLon = originJsonValue["LongOrigin"].asFloat(); + embeddedInfo.refLat = originJsonValue["LatOrigin"].asFloat(); + embeddedInfo.refAlt = originJsonValue["AltOrigin"].asFloat(); } else { @@ -798,22 +772,18 @@ bool BoardSupportComm::OnStartUp() } catch(...) { - m_MissionReader.GetValue("LongOrigin", estimatedState.referenceLon); - m_MissionReader.GetValue("LatOrigin", estimatedState.referenceLat); - m_MissionReader.GetValue("AltOrigin", estimatedState.referenceAltitude); + m_MissionReader.GetValue("LongOrigin", embeddedInfo.refLon); + m_MissionReader.GetValue("LatOrigin", embeddedInfo.refLat); + m_MissionReader.GetValue("AltOrigin", embeddedInfo.refAlt); } - // std::cout << "BoardSupportComm OnStartUp: " << estimatedState.referenceLon << ", " - // << estimatedState.referenceLat << ", " - // << estimatedState.referenceAltitude << std::endl; - RegisterVariables(); return(true); } void BoardSupportComm::RegisterVariables() { - // AppCastingMOOSApp::RegisterVariables(); + AppCastingMOOSApp::RegisterVariables(); #ifdef MOOS_AUV_SIM Register("NAV_X", 0); diff --git a/src/pBoardSupportComm/BoardSupportComm.h b/src/pBoardSupportComm/BoardSupportComm.h old mode 100644 new mode 100755 index 974524b..255fab1 --- a/src/pBoardSupportComm/BoardSupportComm.h +++ b/src/pBoardSupportComm/BoardSupportComm.h @@ -53,6 +53,12 @@ struct AUVEmbedded uint8_t dvl_status; //30:[55] uint8_t crc; //31:[56] uint16_t footer; //32:[57,58] + + float refLon; + float refLat; + float refAlt; + float north; + float east; }; struct AUVExecuteCommand @@ -76,45 +82,22 @@ struct AUVExecuteCommand bool manual_mode; }; -struct EstimatedState { - float referenceLon; - float referenceLat; - float referenceAltitude; - float currentLon; - float currentLat; - float currentAltitude; - float offsetNorth; - float offsetEast; - float offsetDown; - float roll; - float pitch; - float yaw; - float linearVelocityNorth; - float linearVelocityEast; - float linearVelocityDown; - float height; - float depth; -}; - -class BoardSupportComm : public CMOOSApp -// class BoardSupportComm : public AppCastingMOOSApp +class BoardSupportComm : public AppCastingMOOSApp { public: BoardSupportComm(); ~BoardSupportComm(); -protected: // Standard MOOSApp functions to overload +protected: bool OnNewMail(MOOSMSG_LIST &NewMail); bool Iterate(); bool OnConnectToServer(); bool OnStartUp(); void RegisterVariables(); - // bool buildReport(); + bool buildReport(); private: - struct EstimatedState estimatedState; - void ConvertLLAToENU(std::vector init_lla, std::vector point_lla, std::vector& point_enu); @@ -129,7 +112,6 @@ private: std::vector &point_lla); void tcpProcessThread(); - // std::string convertEmbeddedFormat(); std::string convertEmbeddedFormat(Json::Value &embeddedState); int tcpSockFD; int tcpConnectRet; @@ -144,12 +126,11 @@ private: struct AUVEmbedded embeddedInfo; struct AUVExecuteCommand executeCommand; - int testFlag = 0; - bool testCount = false; - int sockfd = -1; - int connectFlg = -1; - std::string llaOriginPath; + std::string llaOriginPath; + int recvLen; + std::string recvContent; + std::string castLogStream; }; diff --git a/src/pBoardSupportComm/BoardSupportComm_Info.cpp b/src/pBoardSupportComm/BoardSupportComm_Info.cpp old mode 100644 new mode 100755 diff --git a/src/pBoardSupportComm/BoardSupportComm_Info.h b/src/pBoardSupportComm/BoardSupportComm_Info.h old mode 100644 new mode 100755 diff --git a/src/pBoardSupportComm/CMakeLists.txt b/src/pBoardSupportComm/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/src/pBoardSupportComm/main.cpp b/src/pBoardSupportComm/main.cpp old mode 100644 new mode 100755 diff --git a/src/pBoardSupportComm/pBoardSupportComm.moos b/src/pBoardSupportComm/pBoardSupportComm.moos old mode 100644 new mode 100755 diff --git a/src/pClientViewer/Behavior.pb.h b/src/pClientViewer/Behavior.pb.h old mode 100644 new mode 100755 diff --git a/src/pClientViewer/CMakeLists.txt b/src/pClientViewer/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/src/pClientViewer/ClientViewer.cpp b/src/pClientViewer/ClientViewer.cpp old mode 100644 new mode 100755 index 8d5954b..d2d69e9 --- a/src/pClientViewer/ClientViewer.cpp +++ b/src/pClientViewer/ClientViewer.cpp @@ -765,7 +765,7 @@ void ClientViewer::processMessage(DUNE::IMC::Message * message) { for (; iter2 != entityParameter->params.end(); ++iter2) { DUNE::IMC::EntityParameter *subEntityParameter = static_cast(*iter2); - std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl; + //std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl; } } } diff --git a/src/pClientViewer/ClientViewer.h b/src/pClientViewer/ClientViewer.h old mode 100644 new mode 100755 diff --git a/src/pClientViewer/ClientViewer_Info.cpp b/src/pClientViewer/ClientViewer_Info.cpp old mode 100644 new mode 100755 diff --git a/src/pClientViewer/ClientViewer_Info.h b/src/pClientViewer/ClientViewer_Info.h old mode 100644 new mode 100755 diff --git a/src/pClientViewer/main.cpp b/src/pClientViewer/main.cpp old mode 100644 new mode 100755 diff --git a/src/pClientViewer/pClientViewer.moos b/src/pClientViewer/pClientViewer.moos old mode 100644 new mode 100755 diff --git a/src/pDataManagement/CMakeLists.txt b/src/pDataManagement/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/src/pDataManagement/DataManagement.cpp b/src/pDataManagement/DataManagement.cpp old mode 100644 new mode 100755 index c0a6ccb..0872b9f --- a/src/pDataManagement/DataManagement.cpp +++ b/src/pDataManagement/DataManagement.cpp @@ -17,6 +17,7 @@ using namespace std; // Constructor DataManagement::DataManagement() { + logEnable = false; motionControlInfo.desiredHeading = 0; motionControlInfo.desiredSpeed = 0; @@ -44,26 +45,7 @@ DataManagement::DataManagement() // Destructor DataManagement::~DataManagement() { - if(auvDataStream.is_open()) - { - auvDataStream.close(); - } - if(missionHistoryStream.is_open()) - { - missionHistoryStream.close(); - } - if(clientCommandStream.is_open()) - { - clientCommandStream.close(); - } - if(faultLogStream.is_open()) - { - faultLogStream.close(); - } - if(motionControlStream.is_open()) - { - motionControlStream.close(); - } + CloseOutputStream(); } //--------------------------------------------------------- @@ -89,11 +71,58 @@ bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail) bool mdbl = msg.IsDouble(); bool mstr = msg.IsString(); + if (key == "uClient_logEnable_cmd") + { + if (sval == "true") + { + logEnable = true; + OpenOutputStream(""); + } + else + { + logEnable = false; + CloseOutputStream(); + } + } + if(key == "uMission_action_cmd") + { + if (logEnable) + { + std::string err; + Json::Value recvCommand; + std::istringstream iss(sval); + Json::CharReaderBuilder builder; + try + { + bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err); + if (!parsingResult) + { + throw ("uMission_action_cmd parse error"); + } + + if (recvCommand["action"].asString() == "start") + { + CloseOutputStream(); + OpenOutputStream(recvCommand["taskName"].asString()); + } + else + { + CloseOutputStream(); + OpenOutputStream(""); + } + } + catch (std::string s) + { + std::cout << s << std::endl; + } + } + } + if(key == "uDevice_monitor_fb") { std::string err; Json::Value estimatedStateData; - std::istringstream iss(msg.GetString()); + std::istringstream iss(sval); Json::CharReaderBuilder builder; bool parsingResult = Json::parseFromStream(builder, iss, &estimatedStateData, &err); if (!parsingResult) @@ -138,7 +167,7 @@ bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail) { std::string err; Json::Value missionStatusObject; - std::istringstream iss(msg.GetString()); + std::istringstream iss(sval); Json::CharReaderBuilder builder; bool parsingResult = Json::parseFromStream(builder, iss, &missionStatusObject, &err); try @@ -168,10 +197,9 @@ bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail) { std::string err; Json::Value errorStatus; - std::istringstream iss(msg.GetString()); + std::istringstream iss(sval); Json::CharReaderBuilder builder; bool parsingResult = Json::parseFromStream(builder, iss, &errorStatus, &err); - // std::cout << "uFH_errorMsg: " << msg.GetString() << std::endl; try { if (!parsingResult) @@ -242,7 +270,7 @@ bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail) } if(key == "DESIRED_HEADING") { - motionControlInfo.desiredHeading = msg.GetDouble(); + motionControlInfo.desiredHeading = dval; std::stringstream ss; ss << std::fixed << std::setprecision(6) << MOOS::Time() << ","; ss << motionControlInfo.desiredHeading << "," @@ -252,7 +280,7 @@ bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail) } if(key == "DESIRED_SPEED") { - motionControlInfo.desiredSpeed = msg.GetDouble(); + motionControlInfo.desiredSpeed = dval; std::stringstream ss; ss << std::fixed << std::setprecision(6) << MOOS::Time() << ","; ss << motionControlInfo.desiredHeading << "," @@ -262,14 +290,16 @@ bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail) } if(key == "DESIRED_DEPTH") { - motionControlInfo.desiredDepth = msg.GetDouble(); + motionControlInfo.desiredDepth = dval; std::stringstream ss; ss << std::fixed << std::setprecision(6) << MOOS::Time() << ","; ss << motionControlInfo.desiredHeading << "," << motionControlInfo.desiredSpeed << "," << motionControlInfo.desiredDepth; Notify("uMotion_desired_log", ss.str()); - } + } + + } return(true); @@ -372,92 +402,55 @@ bool DataManagement::OnStartUp() { AppCastingMOOSApp::OnStartUp(); - list sParams; - m_MissionReader.EnableVerbatimQuoting(false); - if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) { - list::iterator p; - for(p=sParams.begin(); p!=sParams.end(); p++) { - string line = *p; - string param = tolower(biteStringX(line, '=')); - string value = line; + // list sParams; + // m_MissionReader.EnableVerbatimQuoting(false); + // if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) { + // list::iterator p; + // for(p=sParams.begin(); p!=sParams.end(); p++) { + // string line = *p; + // string param = tolower(biteStringX(line, '=')); + // string value = line; - if(param == "foo") { - //handled - } - else if(param == "bar") { - //handled - } - } - } - std::string saveLogDir; + // if(param == "foo") { + // //handled + // } + // else if(param == "bar") { + // //handled + // } + // } + // } + m_MissionReader.GetValue("LogDir", saveLogDir); std::string vehicleName; m_MissionReader.GetValue("VehicleName", vehicleName); - bool logEnable = false; m_MissionReader.GetValue("LogEnable", logEnable); - std::string auvDataFile; - m_MissionReader.GetValue("AuvDataLog", auvDataFile); - std::string missionHistoryFile; - m_MissionReader.GetValue("MissionHistoryLog", missionHistoryFile); - std::string clientCommandFile; - m_MissionReader.GetValue("ClientCommandLog", clientCommandFile); - std::string faultLogFile; - m_MissionReader.GetValue("FaultLog", faultLogFile); - std::string motionControlFile; - m_MissionReader.GetValue("MotionControlLog", motionControlFile); + // std::string auvDataFile; + // m_MissionReader.GetValue("AuvDataLog", auvDataFile); + // std::string missionHistoryFile; + // m_MissionReader.GetValue("MissionHistoryLog", missionHistoryFile); + // std::string clientCommandFile; + // m_MissionReader.GetValue("ClientCommandLog", clientCommandFile); + // std::string faultLogFile; + // m_MissionReader.GetValue("FaultLog", faultLogFile); + // std::string motionControlFile; + // m_MissionReader.GetValue("MotionControlLog", motionControlFile); + if (access(saveLogDir.c_str(), F_OK) == -1 ) + { + mode_t mode = 0775; + mkdir(saveLogDir.c_str(), mode); + } + saveLogDir += "/" + vehicleName; + if (access(saveLogDir.c_str(), F_OK) == -1 ) + { + mode_t mode = 0775; + mkdir(saveLogDir.c_str(), mode); + } if (logEnable) { - if (access(saveLogDir.c_str(), F_OK) == -1 ) - { - mode_t mode = 0775; - mkdir(saveLogDir.c_str(), mode); - } - saveLogDir += "/" + vehicleName; - if (access(saveLogDir.c_str(), F_OK) == -1 ) - { - mode_t mode = 0775; - mkdir(saveLogDir.c_str(), mode); - } - - std::string subDir; - std::string localTime; - GenerateFileName(subDir, localTime); - saveLogDir += "/" + subDir; - if (access(saveLogDir.c_str(), F_OK) == -1 ) - { - mode_t mode = 0775; - mkdir(saveLogDir.c_str(), mode); - } - saveLogDir += "/" + localTime; - if (access(saveLogDir.c_str(), F_OK) == -1 ) - { - mode_t mode = 0775; - mkdir(saveLogDir.c_str(), mode); - } - std::string auvDataSavePath = saveLogDir + "/" + auvDataFile; - std::string missionHistorySavePath = saveLogDir + "/" + missionHistoryFile; - std::string clientCommandSavePath = saveLogDir + "/" + clientCommandFile; - std::string faultLogSavePath = saveLogDir + "/" + faultLogFile; - std::string motionControlSavePath = saveLogDir + "/" + motionControlFile; - - if(!OpenFile(auvDataStream, auvDataSavePath)) - return MOOSFail("Failed to Open auvData file"); - if(!OpenFile(missionHistoryStream, missionHistorySavePath)) - return MOOSFail("Failed to Open missionHistory file"); - if(!OpenFile(clientCommandStream, clientCommandSavePath)) - return MOOSFail("Failed to Open clientCommand file"); - if(!OpenFile(faultLogStream, faultLogSavePath)) - return MOOSFail("Failed to Open faultLog file"); - if(!OpenFile(motionControlStream, motionControlSavePath)) - return MOOSFail("Failed to Open faultLog file"); - - DoAuvDataLogBanner(auvDataStream); - DoMissionHistoryBanner(missionHistoryStream); - DoFaultHandleBanner(faultLogStream); - DoMotionControlBanner(motionControlStream); + OpenOutputStream(""); } RegisterVariables(); @@ -488,7 +481,9 @@ void DataManagement::RegisterVariables() Register("DESIRED_HEADING", 0); Register("DESIRED_SPEED", 0); Register("DESIRED_DEPTH", 0); - Register("uMotion_desired_log", 0); + Register("uMotion_desired_log", 0); + Register("uClient_logEnable_cmd", 0); + Register("uMission_action_cmd", 0); } bool DataManagement::buildReport() @@ -638,3 +633,89 @@ void DataManagement::GenerateFileName(std::string &fileDir, std::string &fileNam ss << hour << minute << second; fileName = ss.str(); } + +bool DataManagement::OpenOutputStream(std::string extraName) +{ + std::string subDir; + std::string localTime; + GenerateFileName(subDir, localTime); + std::string saveSubLogDir = saveLogDir + "/" + subDir; + if (access(saveSubLogDir.c_str(), F_OK) == -1 ) + { + mode_t mode = 0775; + mkdir(saveSubLogDir.c_str(), mode); + } + if (extraName.size() == 0) + { + saveSubLogDir += "/" + localTime; + } + else + { + saveSubLogDir += "/" + localTime + "_" + extraName; + } + if (access(saveSubLogDir.c_str(), F_OK) == -1 ) + { + mode_t mode = 0775; + mkdir(saveSubLogDir.c_str(), mode); + } + + std::string auvDataFile; + m_MissionReader.GetValue("AuvDataLog", auvDataFile); + std::string missionHistoryFile; + m_MissionReader.GetValue("MissionHistoryLog", missionHistoryFile); + std::string clientCommandFile; + m_MissionReader.GetValue("ClientCommandLog", clientCommandFile); + std::string faultLogFile; + m_MissionReader.GetValue("FaultLog", faultLogFile); + std::string motionControlFile; + m_MissionReader.GetValue("MotionControlLog", motionControlFile); + + std::string auvDataSavePath = saveSubLogDir + "/" + auvDataFile; + std::string missionHistorySavePath = saveSubLogDir + "/" + missionHistoryFile; + std::string clientCommandSavePath = saveSubLogDir + "/" + clientCommandFile; + std::string faultLogSavePath = saveSubLogDir + "/" + faultLogFile; + std::string motionControlSavePath = saveSubLogDir + "/" + motionControlFile; + + if(!OpenFile(auvDataStream, auvDataSavePath)) + return MOOSFail("Failed to Open auvData file"); + if(!OpenFile(missionHistoryStream, missionHistorySavePath)) + return MOOSFail("Failed to Open missionHistory file"); + if(!OpenFile(clientCommandStream, clientCommandSavePath)) + return MOOSFail("Failed to Open clientCommand file"); + if(!OpenFile(faultLogStream, faultLogSavePath)) + return MOOSFail("Failed to Open faultLog file"); + if(!OpenFile(motionControlStream, motionControlSavePath)) + return MOOSFail("Failed to Open faultLog file"); + + DoAuvDataLogBanner(auvDataStream); + DoMissionHistoryBanner(missionHistoryStream); + DoFaultHandleBanner(faultLogStream); + DoMotionControlBanner(motionControlStream); + + return true; +} + + +void DataManagement::CloseOutputStream() +{ + if(auvDataStream.is_open()) + { + auvDataStream.close(); + } + if(missionHistoryStream.is_open()) + { + missionHistoryStream.close(); + } + if(clientCommandStream.is_open()) + { + clientCommandStream.close(); + } + if(faultLogStream.is_open()) + { + faultLogStream.close(); + } + if(motionControlStream.is_open()) + { + motionControlStream.close(); + } +} diff --git a/src/pDataManagement/DataManagement.h b/src/pDataManagement/DataManagement.h old mode 100644 new mode 100755 index f2b57f3..1950cad --- a/src/pDataManagement/DataManagement.h +++ b/src/pDataManagement/DataManagement.h @@ -44,6 +44,8 @@ protected: void DoMotionControlBanner(std::ofstream &os); void GenerateFileName(std::string &fileDir, std::string &fileName); + bool OpenOutputStream(std::string extraName); + void CloseOutputStream(); double getTimeStamp(); int nDoublePrecision; std::ofstream auvDataStream; @@ -54,6 +56,8 @@ protected: std::map logVarList; std::string contentFromStream; struct MotionControlInfo motionControlInfo; + bool logEnable; + std::string saveLogDir; }; #endif diff --git a/src/pDataManagement/DataManagement_Info.cpp b/src/pDataManagement/DataManagement_Info.cpp old mode 100644 new mode 100755 diff --git a/src/pDataManagement/DataManagement_Info.h b/src/pDataManagement/DataManagement_Info.h old mode 100644 new mode 100755 diff --git a/src/pDataManagement/NavigationInfo.pb.h b/src/pDataManagement/NavigationInfo.pb.h deleted file mode 100644 index b7e4a5d..0000000 --- a/src/pDataManagement/NavigationInfo.pb.h +++ /dev/null @@ -1,809 +0,0 @@ -// Generated by the protocol buffer compiler. DO NOT EDIT! -// source: NavigationInfo.proto - -#ifndef GOOGLE_PROTOBUF_INCLUDED_NavigationInfo_2eproto -#define GOOGLE_PROTOBUF_INCLUDED_NavigationInfo_2eproto - -#include -#include - -#include -#if PROTOBUF_VERSION < 3021000 -#error This file was generated by a newer version of protoc which is -#error incompatible with your Protocol Buffer headers. Please update -#error your headers. -#endif -#if 3021012 < PROTOBUF_MIN_PROTOC_VERSION -#error This file was generated by an older version of protoc which is -#error incompatible with your Protocol Buffer headers. Please -#error regenerate this file with a newer version of protoc. -#endif - -#include -#include -#include -#include -#include -#include -#include -#include -#include // IWYU pragma: export -#include // IWYU pragma: export -#include -// @@protoc_insertion_point(includes) -#include -#define PROTOBUF_INTERNAL_EXPORT_NavigationInfo_2eproto -PROTOBUF_NAMESPACE_OPEN -namespace internal { -class AnyMetadata; -} // namespace internal -PROTOBUF_NAMESPACE_CLOSE - -// Internal implementation detail -- do not use these members. -struct TableStruct_NavigationInfo_2eproto { - static const uint32_t offsets[]; -}; -extern const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_NavigationInfo_2eproto; -namespace NavigationInfo { -class EstimatedState; -struct EstimatedStateDefaultTypeInternal; -extern EstimatedStateDefaultTypeInternal _EstimatedState_default_instance_; -} // namespace NavigationInfo -PROTOBUF_NAMESPACE_OPEN -template<> ::NavigationInfo::EstimatedState* Arena::CreateMaybeMessage<::NavigationInfo::EstimatedState>(Arena*); -PROTOBUF_NAMESPACE_CLOSE -namespace NavigationInfo { - -// =================================================================== - -class EstimatedState final : - public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:NavigationInfo.EstimatedState) */ { - public: - inline EstimatedState() : EstimatedState(nullptr) {} - ~EstimatedState() override; - explicit PROTOBUF_CONSTEXPR EstimatedState(::PROTOBUF_NAMESPACE_ID::internal::ConstantInitialized); - - EstimatedState(const EstimatedState& from); - EstimatedState(EstimatedState&& from) noexcept - : EstimatedState() { - *this = ::std::move(from); - } - - inline EstimatedState& operator=(const EstimatedState& from) { - CopyFrom(from); - return *this; - } - inline EstimatedState& operator=(EstimatedState&& from) noexcept { - if (this == &from) return *this; - if (GetOwningArena() == from.GetOwningArena() - #ifdef PROTOBUF_FORCE_COPY_IN_MOVE - && GetOwningArena() != nullptr - #endif // !PROTOBUF_FORCE_COPY_IN_MOVE - ) { - InternalSwap(&from); - } else { - CopyFrom(from); - } - return *this; - } - - static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() { - return GetDescriptor(); - } - static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() { - return default_instance().GetMetadata().descriptor; - } - static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() { - return default_instance().GetMetadata().reflection; - } - static const EstimatedState& default_instance() { - return *internal_default_instance(); - } - static inline const EstimatedState* internal_default_instance() { - return reinterpret_cast( - &_EstimatedState_default_instance_); - } - static constexpr int kIndexInFileMessages = - 0; - - friend void swap(EstimatedState& a, EstimatedState& b) { - a.Swap(&b); - } - inline void Swap(EstimatedState* other) { - if (other == this) return; - #ifdef PROTOBUF_FORCE_COPY_IN_SWAP - if (GetOwningArena() != nullptr && - GetOwningArena() == other->GetOwningArena()) { - #else // PROTOBUF_FORCE_COPY_IN_SWAP - if (GetOwningArena() == other->GetOwningArena()) { - #endif // !PROTOBUF_FORCE_COPY_IN_SWAP - InternalSwap(other); - } else { - ::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other); - } - } - void UnsafeArenaSwap(EstimatedState* other) { - if (other == this) return; - GOOGLE_DCHECK(GetOwningArena() == other->GetOwningArena()); - InternalSwap(other); - } - - // implements Message ---------------------------------------------- - - EstimatedState* New(::PROTOBUF_NAMESPACE_ID::Arena* arena = nullptr) const final { - return CreateMaybeMessage(arena); - } - using ::PROTOBUF_NAMESPACE_ID::Message::CopyFrom; - void CopyFrom(const EstimatedState& from); - using ::PROTOBUF_NAMESPACE_ID::Message::MergeFrom; - void MergeFrom( const EstimatedState& from) { - EstimatedState::MergeImpl(*this, from); - } - private: - static void MergeImpl(::PROTOBUF_NAMESPACE_ID::Message& to_msg, const ::PROTOBUF_NAMESPACE_ID::Message& from_msg); - public: - PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final; - bool IsInitialized() const final; - - size_t ByteSizeLong() const final; - const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final; - uint8_t* _InternalSerialize( - uint8_t* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final; - int GetCachedSize() const final { return _impl_._cached_size_.Get(); } - - private: - void SharedCtor(::PROTOBUF_NAMESPACE_ID::Arena* arena, bool is_message_owned); - void SharedDtor(); - void SetCachedSize(int size) const final; - void InternalSwap(EstimatedState* other); - - private: - friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata; - static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() { - return "NavigationInfo.EstimatedState"; - } - protected: - explicit EstimatedState(::PROTOBUF_NAMESPACE_ID::Arena* arena, - bool is_message_owned = false); - public: - - static const ClassData _class_data_; - const ::PROTOBUF_NAMESPACE_ID::Message::ClassData*GetClassData() const final; - - ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - enum : int { - kInfoFieldNumber = 1, - kLonFieldNumber = 2, - kLatFieldNumber = 3, - kHeightFieldNumber = 4, - kOffsetNorthFieldNumber = 5, - kOffsetEastFieldNumber = 6, - kOffsetDownFieldNumber = 7, - kRollFieldNumber = 8, - kPitchFieldNumber = 9, - kYawFieldNumber = 10, - kLinearVelocityNorthFieldNumber = 11, - kLinearVelocityEastFieldNumber = 12, - kLinearVelocityDownFieldNumber = 13, - kAngularVelocityNorthFieldNumber = 14, - kAngularVelocityEastFieldNumber = 15, - kAngularVelocityDownFieldNumber = 16, - kDepthFieldNumber = 17, - kAltitudeFieldNumber = 18, - }; - // string info = 1; - void clear_info(); - const std::string& info() const; - template - void set_info(ArgT0&& arg0, ArgT... args); - std::string* mutable_info(); - PROTOBUF_NODISCARD std::string* release_info(); - void set_allocated_info(std::string* info); - private: - const std::string& _internal_info() const; - inline PROTOBUF_ALWAYS_INLINE void _internal_set_info(const std::string& value); - std::string* _internal_mutable_info(); - public: - - // float lon = 2; - void clear_lon(); - float lon() const; - void set_lon(float value); - private: - float _internal_lon() const; - void _internal_set_lon(float value); - public: - - // float lat = 3; - void clear_lat(); - float lat() const; - void set_lat(float value); - private: - float _internal_lat() const; - void _internal_set_lat(float value); - public: - - // float height = 4; - void clear_height(); - float height() const; - void set_height(float value); - private: - float _internal_height() const; - void _internal_set_height(float value); - public: - - // float offsetNorth = 5; - void clear_offsetnorth(); - float offsetnorth() const; - void set_offsetnorth(float value); - private: - float _internal_offsetnorth() const; - void _internal_set_offsetnorth(float value); - public: - - // float offsetEast = 6; - void clear_offseteast(); - float offseteast() const; - void set_offseteast(float value); - private: - float _internal_offseteast() const; - void _internal_set_offseteast(float value); - public: - - // float offsetDown = 7; - void clear_offsetdown(); - float offsetdown() const; - void set_offsetdown(float value); - private: - float _internal_offsetdown() const; - void _internal_set_offsetdown(float value); - public: - - // float roll = 8; - void clear_roll(); - float roll() const; - void set_roll(float value); - private: - float _internal_roll() const; - void _internal_set_roll(float value); - public: - - // float pitch = 9; - void clear_pitch(); - float pitch() const; - void set_pitch(float value); - private: - float _internal_pitch() const; - void _internal_set_pitch(float value); - public: - - // float yaw = 10; - void clear_yaw(); - float yaw() const; - void set_yaw(float value); - private: - float _internal_yaw() const; - void _internal_set_yaw(float value); - public: - - // float linearVelocityNorth = 11; - void clear_linearvelocitynorth(); - float linearvelocitynorth() const; - void set_linearvelocitynorth(float value); - private: - float _internal_linearvelocitynorth() const; - void _internal_set_linearvelocitynorth(float value); - public: - - // float linearVelocityEast = 12; - void clear_linearvelocityeast(); - float linearvelocityeast() const; - void set_linearvelocityeast(float value); - private: - float _internal_linearvelocityeast() const; - void _internal_set_linearvelocityeast(float value); - public: - - // float linearVelocityDown = 13; - void clear_linearvelocitydown(); - float linearvelocitydown() const; - void set_linearvelocitydown(float value); - private: - float _internal_linearvelocitydown() const; - void _internal_set_linearvelocitydown(float value); - public: - - // float angularVelocityNorth = 14; - void clear_angularvelocitynorth(); - float angularvelocitynorth() const; - void set_angularvelocitynorth(float value); - private: - float _internal_angularvelocitynorth() const; - void _internal_set_angularvelocitynorth(float value); - public: - - // float angularVelocityEast = 15; - void clear_angularvelocityeast(); - float angularvelocityeast() const; - void set_angularvelocityeast(float value); - private: - float _internal_angularvelocityeast() const; - void _internal_set_angularvelocityeast(float value); - public: - - // float angularVelocityDown = 16; - void clear_angularvelocitydown(); - float angularvelocitydown() const; - void set_angularvelocitydown(float value); - private: - float _internal_angularvelocitydown() const; - void _internal_set_angularvelocitydown(float value); - public: - - // float depth = 17; - void clear_depth(); - float depth() const; - void set_depth(float value); - private: - float _internal_depth() const; - void _internal_set_depth(float value); - public: - - // float altitude = 18; - void clear_altitude(); - float altitude() const; - void set_altitude(float value); - private: - float _internal_altitude() const; - void _internal_set_altitude(float value); - public: - - // @@protoc_insertion_point(class_scope:NavigationInfo.EstimatedState) - private: - class _Internal; - - template friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper; - typedef void InternalArenaConstructable_; - typedef void DestructorSkippable_; - struct Impl_ { - ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr info_; - float lon_; - float lat_; - float height_; - float offsetnorth_; - float offseteast_; - float offsetdown_; - float roll_; - float pitch_; - float yaw_; - float linearvelocitynorth_; - float linearvelocityeast_; - float linearvelocitydown_; - float angularvelocitynorth_; - float angularvelocityeast_; - float angularvelocitydown_; - float depth_; - float altitude_; - mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_; - }; - union { Impl_ _impl_; }; - friend struct ::TableStruct_NavigationInfo_2eproto; -}; -// =================================================================== - - -// =================================================================== - -#ifdef __GNUC__ - #pragma GCC diagnostic push - #pragma GCC diagnostic ignored "-Wstrict-aliasing" -#endif // __GNUC__ -// EstimatedState - -// string info = 1; -inline void EstimatedState::clear_info() { - _impl_.info_.ClearToEmpty(); -} -inline const std::string& EstimatedState::info() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.info) - return _internal_info(); -} -template -inline PROTOBUF_ALWAYS_INLINE -void EstimatedState::set_info(ArgT0&& arg0, ArgT... args) { - - _impl_.info_.Set(static_cast(arg0), args..., GetArenaForAllocation()); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.info) -} -inline std::string* EstimatedState::mutable_info() { - std::string* _s = _internal_mutable_info(); - // @@protoc_insertion_point(field_mutable:NavigationInfo.EstimatedState.info) - return _s; -} -inline const std::string& EstimatedState::_internal_info() const { - return _impl_.info_.Get(); -} -inline void EstimatedState::_internal_set_info(const std::string& value) { - - _impl_.info_.Set(value, GetArenaForAllocation()); -} -inline std::string* EstimatedState::_internal_mutable_info() { - - return _impl_.info_.Mutable(GetArenaForAllocation()); -} -inline std::string* EstimatedState::release_info() { - // @@protoc_insertion_point(field_release:NavigationInfo.EstimatedState.info) - return _impl_.info_.Release(); -} -inline void EstimatedState::set_allocated_info(std::string* info) { - if (info != nullptr) { - - } else { - - } - _impl_.info_.SetAllocated(info, GetArenaForAllocation()); -#ifdef PROTOBUF_FORCE_COPY_DEFAULT_STRING - if (_impl_.info_.IsDefault()) { - _impl_.info_.Set("", GetArenaForAllocation()); - } -#endif // PROTOBUF_FORCE_COPY_DEFAULT_STRING - // @@protoc_insertion_point(field_set_allocated:NavigationInfo.EstimatedState.info) -} - -// float lon = 2; -inline void EstimatedState::clear_lon() { - _impl_.lon_ = 0; -} -inline float EstimatedState::_internal_lon() const { - return _impl_.lon_; -} -inline float EstimatedState::lon() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.lon) - return _internal_lon(); -} -inline void EstimatedState::_internal_set_lon(float value) { - - _impl_.lon_ = value; -} -inline void EstimatedState::set_lon(float value) { - _internal_set_lon(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.lon) -} - -// float lat = 3; -inline void EstimatedState::clear_lat() { - _impl_.lat_ = 0; -} -inline float EstimatedState::_internal_lat() const { - return _impl_.lat_; -} -inline float EstimatedState::lat() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.lat) - return _internal_lat(); -} -inline void EstimatedState::_internal_set_lat(float value) { - - _impl_.lat_ = value; -} -inline void EstimatedState::set_lat(float value) { - _internal_set_lat(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.lat) -} - -// float height = 4; -inline void EstimatedState::clear_height() { - _impl_.height_ = 0; -} -inline float EstimatedState::_internal_height() const { - return _impl_.height_; -} -inline float EstimatedState::height() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.height) - return _internal_height(); -} -inline void EstimatedState::_internal_set_height(float value) { - - _impl_.height_ = value; -} -inline void EstimatedState::set_height(float value) { - _internal_set_height(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.height) -} - -// float offsetNorth = 5; -inline void EstimatedState::clear_offsetnorth() { - _impl_.offsetnorth_ = 0; -} -inline float EstimatedState::_internal_offsetnorth() const { - return _impl_.offsetnorth_; -} -inline float EstimatedState::offsetnorth() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.offsetNorth) - return _internal_offsetnorth(); -} -inline void EstimatedState::_internal_set_offsetnorth(float value) { - - _impl_.offsetnorth_ = value; -} -inline void EstimatedState::set_offsetnorth(float value) { - _internal_set_offsetnorth(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.offsetNorth) -} - -// float offsetEast = 6; -inline void EstimatedState::clear_offseteast() { - _impl_.offseteast_ = 0; -} -inline float EstimatedState::_internal_offseteast() const { - return _impl_.offseteast_; -} -inline float EstimatedState::offseteast() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.offsetEast) - return _internal_offseteast(); -} -inline void EstimatedState::_internal_set_offseteast(float value) { - - _impl_.offseteast_ = value; -} -inline void EstimatedState::set_offseteast(float value) { - _internal_set_offseteast(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.offsetEast) -} - -// float offsetDown = 7; -inline void EstimatedState::clear_offsetdown() { - _impl_.offsetdown_ = 0; -} -inline float EstimatedState::_internal_offsetdown() const { - return _impl_.offsetdown_; -} -inline float EstimatedState::offsetdown() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.offsetDown) - return _internal_offsetdown(); -} -inline void EstimatedState::_internal_set_offsetdown(float value) { - - _impl_.offsetdown_ = value; -} -inline void EstimatedState::set_offsetdown(float value) { - _internal_set_offsetdown(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.offsetDown) -} - -// float roll = 8; -inline void EstimatedState::clear_roll() { - _impl_.roll_ = 0; -} -inline float EstimatedState::_internal_roll() const { - return _impl_.roll_; -} -inline float EstimatedState::roll() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.roll) - return _internal_roll(); -} -inline void EstimatedState::_internal_set_roll(float value) { - - _impl_.roll_ = value; -} -inline void EstimatedState::set_roll(float value) { - _internal_set_roll(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.roll) -} - -// float pitch = 9; -inline void EstimatedState::clear_pitch() { - _impl_.pitch_ = 0; -} -inline float EstimatedState::_internal_pitch() const { - return _impl_.pitch_; -} -inline float EstimatedState::pitch() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.pitch) - return _internal_pitch(); -} -inline void EstimatedState::_internal_set_pitch(float value) { - - _impl_.pitch_ = value; -} -inline void EstimatedState::set_pitch(float value) { - _internal_set_pitch(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.pitch) -} - -// float yaw = 10; -inline void EstimatedState::clear_yaw() { - _impl_.yaw_ = 0; -} -inline float EstimatedState::_internal_yaw() const { - return _impl_.yaw_; -} -inline float EstimatedState::yaw() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.yaw) - return _internal_yaw(); -} -inline void EstimatedState::_internal_set_yaw(float value) { - - _impl_.yaw_ = value; -} -inline void EstimatedState::set_yaw(float value) { - _internal_set_yaw(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.yaw) -} - -// float linearVelocityNorth = 11; -inline void EstimatedState::clear_linearvelocitynorth() { - _impl_.linearvelocitynorth_ = 0; -} -inline float EstimatedState::_internal_linearvelocitynorth() const { - return _impl_.linearvelocitynorth_; -} -inline float EstimatedState::linearvelocitynorth() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.linearVelocityNorth) - return _internal_linearvelocitynorth(); -} -inline void EstimatedState::_internal_set_linearvelocitynorth(float value) { - - _impl_.linearvelocitynorth_ = value; -} -inline void EstimatedState::set_linearvelocitynorth(float value) { - _internal_set_linearvelocitynorth(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.linearVelocityNorth) -} - -// float linearVelocityEast = 12; -inline void EstimatedState::clear_linearvelocityeast() { - _impl_.linearvelocityeast_ = 0; -} -inline float EstimatedState::_internal_linearvelocityeast() const { - return _impl_.linearvelocityeast_; -} -inline float EstimatedState::linearvelocityeast() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.linearVelocityEast) - return _internal_linearvelocityeast(); -} -inline void EstimatedState::_internal_set_linearvelocityeast(float value) { - - _impl_.linearvelocityeast_ = value; -} -inline void EstimatedState::set_linearvelocityeast(float value) { - _internal_set_linearvelocityeast(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.linearVelocityEast) -} - -// float linearVelocityDown = 13; -inline void EstimatedState::clear_linearvelocitydown() { - _impl_.linearvelocitydown_ = 0; -} -inline float EstimatedState::_internal_linearvelocitydown() const { - return _impl_.linearvelocitydown_; -} -inline float EstimatedState::linearvelocitydown() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.linearVelocityDown) - return _internal_linearvelocitydown(); -} -inline void EstimatedState::_internal_set_linearvelocitydown(float value) { - - _impl_.linearvelocitydown_ = value; -} -inline void EstimatedState::set_linearvelocitydown(float value) { - _internal_set_linearvelocitydown(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.linearVelocityDown) -} - -// float angularVelocityNorth = 14; -inline void EstimatedState::clear_angularvelocitynorth() { - _impl_.angularvelocitynorth_ = 0; -} -inline float EstimatedState::_internal_angularvelocitynorth() const { - return _impl_.angularvelocitynorth_; -} -inline float EstimatedState::angularvelocitynorth() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.angularVelocityNorth) - return _internal_angularvelocitynorth(); -} -inline void EstimatedState::_internal_set_angularvelocitynorth(float value) { - - _impl_.angularvelocitynorth_ = value; -} -inline void EstimatedState::set_angularvelocitynorth(float value) { - _internal_set_angularvelocitynorth(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.angularVelocityNorth) -} - -// float angularVelocityEast = 15; -inline void EstimatedState::clear_angularvelocityeast() { - _impl_.angularvelocityeast_ = 0; -} -inline float EstimatedState::_internal_angularvelocityeast() const { - return _impl_.angularvelocityeast_; -} -inline float EstimatedState::angularvelocityeast() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.angularVelocityEast) - return _internal_angularvelocityeast(); -} -inline void EstimatedState::_internal_set_angularvelocityeast(float value) { - - _impl_.angularvelocityeast_ = value; -} -inline void EstimatedState::set_angularvelocityeast(float value) { - _internal_set_angularvelocityeast(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.angularVelocityEast) -} - -// float angularVelocityDown = 16; -inline void EstimatedState::clear_angularvelocitydown() { - _impl_.angularvelocitydown_ = 0; -} -inline float EstimatedState::_internal_angularvelocitydown() const { - return _impl_.angularvelocitydown_; -} -inline float EstimatedState::angularvelocitydown() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.angularVelocityDown) - return _internal_angularvelocitydown(); -} -inline void EstimatedState::_internal_set_angularvelocitydown(float value) { - - _impl_.angularvelocitydown_ = value; -} -inline void EstimatedState::set_angularvelocitydown(float value) { - _internal_set_angularvelocitydown(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.angularVelocityDown) -} - -// float depth = 17; -inline void EstimatedState::clear_depth() { - _impl_.depth_ = 0; -} -inline float EstimatedState::_internal_depth() const { - return _impl_.depth_; -} -inline float EstimatedState::depth() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.depth) - return _internal_depth(); -} -inline void EstimatedState::_internal_set_depth(float value) { - - _impl_.depth_ = value; -} -inline void EstimatedState::set_depth(float value) { - _internal_set_depth(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.depth) -} - -// float altitude = 18; -inline void EstimatedState::clear_altitude() { - _impl_.altitude_ = 0; -} -inline float EstimatedState::_internal_altitude() const { - return _impl_.altitude_; -} -inline float EstimatedState::altitude() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.altitude) - return _internal_altitude(); -} -inline void EstimatedState::_internal_set_altitude(float value) { - - _impl_.altitude_ = value; -} -inline void EstimatedState::set_altitude(float value) { - _internal_set_altitude(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.altitude) -} - -#ifdef __GNUC__ - #pragma GCC diagnostic pop -#endif // __GNUC__ - -// @@protoc_insertion_point(namespace_scope) - -} // namespace NavigationInfo - -// @@protoc_insertion_point(global_scope) - -#include -#endif // GOOGLE_PROTOBUF_INCLUDED_GOOGLE_PROTOBUF_INCLUDED_NavigationInfo_2eproto diff --git a/src/pDataManagement/main.cpp b/src/pDataManagement/main.cpp old mode 100644 new mode 100755 diff --git a/src/pDataManagement/pDataManagement.moos b/src/pDataManagement/pDataManagement.moos old mode 100644 new mode 100755 diff --git a/src/pEmulator/.LastOpenedMOOSLogDirectory b/src/pEmulator/.LastOpenedMOOSLogDirectory old mode 100644 new mode 100755 diff --git a/src/pEmulator/CMakeLists.txt b/src/pEmulator/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/src/pEmulator/Emulator.cpp b/src/pEmulator/Emulator.cpp old mode 100644 new mode 100755 index d1edd47..98ea2bc --- a/src/pEmulator/Emulator.cpp +++ b/src/pEmulator/Emulator.cpp @@ -1,8 +1,8 @@ /* * @Author: zjk 1553836110@qq.com * @Date: 2023-10-12 09:52:27 - * @LastEditors: zjk 1553836110@qq.com - * @LastEditTime: 2023-11-21 15:28:32 + * @LastEditors: zhaojingkui 1553836110@qq.com + * @LastEditTime: 2023-11-28 11:32:48 * @FilePath: /moos-ivp-pi/src/pEmulator/Emulator.cpp * @Description: * @@ -207,6 +207,7 @@ bool Emulator::_150Connect() _150ServerThread.Start(); _150ConnectThread.RequestQuit(); isConnect = "Connected and start server"; + return true; } void Emulator::registerVariables() { @@ -377,6 +378,7 @@ bool Emulator::receiveUdpDate() } } } + return true; } void Emulator::set150Info() @@ -384,7 +386,7 @@ void Emulator::set150Info() p_150server_1.embeddedInfoSrc.header = 0xEBA1; //0:[0,1] p_150server_1.embeddedInfoSrc.count = (uint16_t)m_iteration; //2:[2,3] p_150server_1.embeddedInfoSrc.size = (uint8_t)51; //3:[4] - p_150server_1.embeddedInfoSrc.drive_mode = (uint8_t)0xFF; //4:[5] + p_150server_1.embeddedInfoSrc.drive_mode = (uint8_t)0x02; //4:[5] p_150server_1.embeddedInfoSrc.height = (uint16_t)(150-remus100.z); //5:[6,7] p_150server_1.embeddedInfoSrc.depth = (uint16_t)remus100.z; //6:[8,9] p_150server_1.embeddedInfoSrc.yaw = (uint16_t)remus100.yaw; //7:[10,11] @@ -401,13 +403,13 @@ void Emulator::set150Info() p_150server_1.embeddedInfoSrc.dvl_vz = (int16_t)remus100.w; //18:[36,37] p_150server_1.embeddedInfoSrc.rpm = (int16_t)remus100.thrust; //19:[38,39] p_150server_1.embeddedInfoSrc.lightEnable = (uint8_t)0x01; //20:[40] - p_150server_1.embeddedInfoSrc.battery_voltage = (uint16_t)1024; //21:[41,42] + p_150server_1.embeddedInfoSrc.battery_voltage = (uint16_t)30; //21:[41,42] p_150server_1.embeddedInfoSrc.battery_level = 60; //22:[43] p_150server_1.embeddedInfoSrc.battery_temp = 21.3 / 0.1; //23:[44,45] - p_150server_1.embeddedInfoSrc.fault_leakSensor = 0x80; //24:[46,47,48,49] - p_150server_1.embeddedInfoSrc.fault_battery = 0x2E; //25:[50] - p_150server_1.embeddedInfoSrc.fault_emergencyBattery = 0xAA; //26:[51] - p_150server_1.embeddedInfoSrc.fault_thrust = 0x76; //27:[52] + p_150server_1.embeddedInfoSrc.fault_leakSensor = 0x0; //24:[46,47,48,49] + p_150server_1.embeddedInfoSrc.fault_battery = 0x0; //25:[50] + p_150server_1.embeddedInfoSrc.fault_emergencyBattery = 0x0; //26:[51] + p_150server_1.embeddedInfoSrc.fault_thrust = 0x0; //27:[52] p_150server_1.embeddedInfoSrc.iridium = 0xFF; //28:[53] p_150server_1.embeddedInfoSrc.throwing_load = 0xFF; //29:[54] p_150server_1.embeddedInfoSrc.dvl_status = 0x00; //30:[55] diff --git a/src/pEmulator/Emulator.hpp b/src/pEmulator/Emulator.hpp old mode 100644 new mode 100755 index fbd6b8c..bd7f386 --- a/src/pEmulator/Emulator.hpp +++ b/src/pEmulator/Emulator.hpp @@ -1,8 +1,8 @@ /* * @Author: zjk 1553836110@qq.com * @Date: 2023-10-12 15:57:27 - * @LastEditors: zjk 1553836110@qq.com - * @LastEditTime: 2023-11-10 08:34:14 + * @LastEditors: zhaojingkui 1553836110@qq.com + * @LastEditTime: 2023-11-28 11:32:15 * @FilePath: /moos-ivp-pi/src/pEmulator/Emulator.hpp * @Description: * @@ -45,7 +45,7 @@ typedef struct uuv double lon; double lat; double alt; -}; +}uuv; class Emulator : public AppCastingMOOSApp { diff --git a/src/pEmulator/_150server.cpp b/src/pEmulator/_150server.cpp old mode 100644 new mode 100755 index ec89abf..cb2ed1f --- a/src/pEmulator/_150server.cpp +++ b/src/pEmulator/_150server.cpp @@ -1,14 +1,15 @@ /* * @Author: zjk 1553836110@qq.com * @Date: 2023-11-07 14:59:47 - * @LastEditors: zjk 1553836110@qq.com - * @LastEditTime: 2023-11-08 09:19:46 + * @LastEditors: zhaojingkui 1553836110@qq.com + * @LastEditTime: 2023-11-30 09:26:27 * @FilePath: /moos-ivp-pi/src/pEmulator/_150server.cpp * @Description: * * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. */ #include "_150server.hpp" +#include uint16_t _150server::serializeFields(AUVEmbedded &embeddedInfo, uint8_t* bfr) { @@ -172,7 +173,13 @@ void _150server::_150_startServer() saddr.sin_family = PF_INET; saddr.sin_addr.s_addr = INADDR_ANY; //0.0.0.0 saddr.sin_port = htons(8001); - int ret = bind(lfd, (struct sockaddr *)&saddr, sizeof(saddr)); + int ret = -1; + do + { + ret = bind(lfd, (struct sockaddr *)&saddr, sizeof(saddr)); + std::cout << "Try bind adress..." << std::endl; + sleep(1); + } while (ret==-1); if(ret == -1) { @@ -181,7 +188,13 @@ void _150server::_150_startServer() } //3.监听 - listen(lfd, 5); + do + { + listen(lfd, 5); + std::cout << "Try to listening..." << std::endl; + sleep(1); + } while (ret==-1); + if(ret==-1) { perror("listen"); diff --git a/src/pEmulator/_150server.hpp b/src/pEmulator/_150server.hpp old mode 100644 new mode 100755 index f7049c5..3956800 --- a/src/pEmulator/_150server.hpp +++ b/src/pEmulator/_150server.hpp @@ -1,8 +1,8 @@ /* * @Author: zjk 1553836110@qq.com * @Date: 2023-11-07 14:59:36 - * @LastEditors: zjk 1553836110@qq.com - * @LastEditTime: 2023-11-07 17:12:30 + * @LastEditors: zhaojingkui 1553836110@qq.com + * @LastEditTime: 2023-11-28 18:19:10 * @FilePath: /moos-ivp-pi/src/pEmulator/_150server.hpp * @Description: * diff --git a/src/pEmulator/a.moos b/src/pEmulator/a.moos old mode 100644 new mode 100755 diff --git a/src/pEmulator/alpha.bhv b/src/pEmulator/alpha.bhv old mode 100644 new mode 100755 diff --git a/src/pEmulator/main.cpp b/src/pEmulator/main.cpp old mode 100644 new mode 100755 diff --git a/src/pEmulator/pEmulator.moos b/src/pEmulator/pEmulator.moos old mode 100644 new mode 100755 diff --git a/src/pFaultHandle/CMakeLists.txt b/src/pFaultHandle/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/src/pFaultHandle/FaultHandle.cpp b/src/pFaultHandle/FaultHandle.cpp old mode 100644 new mode 100755 diff --git a/src/pFaultHandle/FaultHandle.h b/src/pFaultHandle/FaultHandle.h old mode 100644 new mode 100755 diff --git a/src/pFaultHandle/FaultHandle_Info.cpp b/src/pFaultHandle/FaultHandle_Info.cpp old mode 100644 new mode 100755 diff --git a/src/pFaultHandle/FaultHandle_Info.h b/src/pFaultHandle/FaultHandle_Info.h old mode 100644 new mode 100755 diff --git a/src/pFaultHandle/main.cpp b/src/pFaultHandle/main.cpp old mode 100644 new mode 100755 diff --git a/src/pFaultHandle/pFaultHandle.moos b/src/pFaultHandle/pFaultHandle.moos old mode 100644 new mode 100755 diff --git a/src/pMotionControler/CMakeLists.txt b/src/pMotionControler/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/src/pMotionControler/ControlParam.json b/src/pMotionControler/ControlParam.json old mode 100644 new mode 100755 diff --git a/src/pMotionControler/Controler.cpp b/src/pMotionControler/Controler.cpp old mode 100644 new mode 100755 diff --git a/src/pMotionControler/Controler.hpp b/src/pMotionControler/Controler.hpp old mode 100644 new mode 100755 diff --git a/src/pMotionControler/MotionControler.cpp b/src/pMotionControler/MotionControler.cpp old mode 100644 new mode 100755 diff --git a/src/pMotionControler/MotionControler.hpp b/src/pMotionControler/MotionControler.hpp old mode 100644 new mode 100755 diff --git a/src/pMotionControler/a.moos b/src/pMotionControler/a.moos old mode 100644 new mode 100755 diff --git a/src/pMotionControler/alpha.bhv b/src/pMotionControler/alpha.bhv old mode 100644 new mode 100755 diff --git a/src/pMotionControler/alpha.moos b/src/pMotionControler/alpha.moos old mode 100644 new mode 100755 diff --git a/src/pMotionControler/main.cpp b/src/pMotionControler/main.cpp old mode 100644 new mode 100755 diff --git a/src/pMotionControler/pidControl.cpp b/src/pMotionControler/pidControl.cpp old mode 100644 new mode 100755 diff --git a/src/pMotionControler/pidControl.hpp b/src/pMotionControler/pidControl.hpp old mode 100644 new mode 100755 diff --git a/src/pMotionControler/simMat.moos b/src/pMotionControler/simMat.moos old mode 100644 new mode 100755 diff --git a/src/pStateManagement/CMakeLists.txt b/src/pStateManagement/CMakeLists.txt old mode 100644 new mode 100755 index bde6d92..0e5676b --- 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 old mode 100644 new mode 100755 index 787cf75..5707194 --- 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,15 +45,13 @@ 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; + Json::Value deviceState; + std::string manualState; + int missionState; if(key == "uManual_enable_cmd") { - manualState = msg.GetDouble(); + manualState = msg.GetString(); } if(key == "uMission_task_fb") { @@ -70,11 +68,11 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail) missionState = missionStateData["state"].asInt(); } - if(fabs(manualState - 1) < 1e-6) //manualState=1 + if(manualState == "true") { deviceState["opMode"] = opModeLists.external; } - else if (fabs(manualState - 0) < 1e-6) //manualState=0 + else if (manualState == "false") { if(missionState == 0) { @@ -84,7 +82,7 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail) { deviceState["opMode"] = opModeLists.service; } - else if((missionState == 3) ) + if(missionState == 3) { deviceState["opMode"] = opModeLists.maneuver; } diff --git a/src/pStateManagement/StateManagement.h b/src/pStateManagement/StateManagement.h old mode 100644 new mode 100755 index 5da5f97..6635540 --- 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 { diff --git a/src/pStateManagement/StateManagement_Info.cpp b/src/pStateManagement/StateManagement_Info.cpp old mode 100644 new mode 100755 diff --git a/src/pStateManagement/StateManagement_Info.h b/src/pStateManagement/StateManagement_Info.h old mode 100644 new mode 100755 diff --git a/src/pStateManagement/main.cpp b/src/pStateManagement/main.cpp old mode 100644 new mode 100755 diff --git a/src/pStateManagement/pStateManagement.moos b/src/pStateManagement/pStateManagement.moos old mode 100644 new mode 100755 diff --git a/src/pSurfaceSupportComm/CMakeLists.txt b/src/pSurfaceSupportComm/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/src/pSurfaceSupportComm/PeriodicTCPEvent.cpp b/src/pSurfaceSupportComm/PeriodicTCPEvent.cpp old mode 100644 new mode 100755 diff --git a/src/pSurfaceSupportComm/PeriodicTCPEvent.h b/src/pSurfaceSupportComm/PeriodicTCPEvent.h old mode 100644 new mode 100755 diff --git a/src/pSurfaceSupportComm/PeriodicUDPEvent.cpp b/src/pSurfaceSupportComm/PeriodicUDPEvent.cpp old mode 100644 new mode 100755 diff --git a/src/pSurfaceSupportComm/PeriodicUDPEvent.h b/src/pSurfaceSupportComm/PeriodicUDPEvent.h old mode 100644 new mode 100755 diff --git a/src/pSurfaceSupportComm/SurfaceSupportComm.cpp b/src/pSurfaceSupportComm/SurfaceSupportComm.cpp old mode 100644 new mode 100755 index 11d61dd..b15d5d5 --- a/src/pSurfaceSupportComm/SurfaceSupportComm.cpp +++ b/src/pSurfaceSupportComm/SurfaceSupportComm.cpp @@ -20,10 +20,10 @@ using namespace std; #define TCP_RECEIVE_PORT 8000 #define TCP_SEND_FILE_PORT 8002 -// #define DEST_IP_ADDRESS "10.25.0.230" //树莓派 -#define DEST_IP_ADDRESS "127.0.0.1" -//#define DEST_IP_ADDRESS "10.25.0.163" -//#define DEST_IP_ADDRESS "10.25.0.160" +#define SRC_IP_ADDRESS "127.0.0.1" + +// #define DEST_IP_ADDRESS "127.0.0.1" +#define DEST_IP_ADDRESS "10.25.0.163" //--------------------------------------------------------- // Constructor @@ -96,7 +96,7 @@ bool SurfaceSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) if(key == "uDevice_monitor_fb") { - std::string estimatedStateString = msg.GetString(); + std::string estimatedStateString = sval; std::string err; Json::Value estimatedStateData; std::istringstream iss(estimatedStateString); @@ -133,13 +133,13 @@ bool SurfaceSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) } if(key == "uMission_task_fb") { - missionStatusString = msg.GetString(); + missionStatusString = sval; } if(key == "CPUTemperature") { - if (msg.GetString() != "Failed") + if (sval != "Failed") { - deviceStatus.controllerTemp = atof(msg.GetString().c_str()); + deviceStatus.controllerTemp = atof(sval.c_str()); } } @@ -189,10 +189,8 @@ bool SurfaceSupportComm::Iterate() announceMsg.lat = estimatedState.currentLat * M_PI / 180; announceMsg.lon = estimatedState.currentLon * M_PI / 180; announceMsg.height = estimatedState.depth; - // announceMsg.services.assign("dune://0.0.0.0/version/2022.04.0;imc+info://0.0.0.0/version/5.4.30-8be592a;imc+udp://127.0.0.1:6001/;imc+tcp://127.0.0.1:8000/"); - announceMsg.services.assign("dune://0.0.0.0/version/2022.04.0;imc+info://0.0.0.0/version/5.4.30-8be592a;imc+tcp://10.25.0.230:8000/"); - //announceMsg.services.assign("dune://0.0.0.0/version/2022.04.0;imc+info://0.0.0.0/version/5.4.30-8be592a;imc+udp://10.25.0.160:6001/;imc+tcp://10.25.0.160:8000/"); - + // announceMsg.services.assign("dune://0.0.0.0/version/2022.04.0;imc+info://0.0.0.0/version/5.4.30-8be592a;imc+tcp://10.25.0.160:8000/"); + announceMsg.services.assign("dune://0.0.0.0/version/2022.04.0;imc+info://0.0.0.0/version/5.4.30-8be592a;imc+tcp://10.25.0.230:8000/"); DUNE::IMC::PlanControlState planControlStateMsg; planControlStateMsg.setTimeStamp(); @@ -219,23 +217,23 @@ bool SurfaceSupportComm::Iterate() { if (missionStatusObject["state"].asInt() == FAULT) { - planControlStateMsg.state == DUNE::IMC::VehicleState::OperationModeEnum::VS_ERROR; + planControlStateMsg.state = DUNE::IMC::VehicleState::OperationModeEnum::VS_ERROR; } else if (missionStatusObject["state"].asInt() == UNRUN) { - planControlStateMsg.state == DUNE::IMC::VehicleState::OperationModeEnum::VS_SERVICE; + planControlStateMsg.state = DUNE::IMC::VehicleState::OperationModeEnum::VS_SERVICE; } else if (missionStatusObject["state"].asInt() == MANUAL) { - planControlStateMsg.state == DUNE::IMC::VehicleState::OperationModeEnum::VS_EXTERNAL; + planControlStateMsg.state = DUNE::IMC::VehicleState::OperationModeEnum::VS_EXTERNAL; } else if (missionStatusObject["state"].asInt() == RUN) { - planControlStateMsg.state == DUNE::IMC::VehicleState::OperationModeEnum::VS_MANEUVER; + planControlStateMsg.state = DUNE::IMC::VehicleState::OperationModeEnum::VS_MANEUVER; } else { - planControlStateMsg.state == DUNE::IMC::VehicleState::OperationModeEnum::VS_ERROR; + planControlStateMsg.state = DUNE::IMC::VehicleState::OperationModeEnum::VS_ERROR; } planControlStateMsg.plan_id = missionStatusObject["taskName"].asString(); //子任务名称，对应PlanDB info的[taskName] planControlStateMsg.plan_eta = -1; //缺省值 @@ -245,7 +243,6 @@ bool SurfaceSupportComm::Iterate() planControlStateMsg.man_eta = -1; //缺省值 planControlStateMsg.last_outcome = missionStatusObject["errorCode"].asUInt(); - vehicleStateMsg.op_mode = planControlStateMsg.state; vehicleStateMsg.error_count = 0; vehicleStateMsg.error_ents = ""; @@ -553,8 +550,9 @@ void SurfaceSupportComm::acceptNewClient(void) c.socket = sock_tcp_receive.accept(&c.address, &c.port); c.socket->setKeepAlive(true); c.socket->setNoDelay(true); - c.socket->setReceiveTimeout(5); - c.socket->setSendTimeout(5); + // c.socket->setReceiveTimeout(5); + c.socket->setReceiveTimeout(1); + c.socket->setSendTimeout(5); m_poll.add(*c.socket); m_clients.push_back(c); } @@ -592,10 +590,13 @@ void SurfaceSupportComm::handleClients(char* buf, unsigned int cap) if (n > 0) { + mtx.lock(); DUNE::IMC::Message * m = DUNE::IMC::Packet::deserialize((uint8_t *)buf, cap); processMessage(m); free(m); + mtx.unlock(); } + ++itr; } } @@ -608,56 +609,74 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) DUNE::IMC::PlanDB * msg = dynamic_cast(message); printf("server receive %s: %lf, %u, %u, %s\n", \ msg->getName(), msg->getTimeStamp(), msg->type, msg->op, msg->plan_id.c_str()); + std::stringstream appCastStream; + appCastStream << std::fixed << std::setprecision(6) + << "server receive: " + << msg->getTimeStamp() << "," + << std::string(msg->getName()) << "," + << (int)msg->type << "," + << (int)msg->op << "," + << msg->plan_id; + appCastContent = appCastStream.str(); + if (msg->type == DUNE::IMC::PlanDB::DBT_REQUEST && msg->op == DUNE::IMC::PlanDB::DBOP_SET) { std::string behaviorSpecString = msg->info; std::string err; Json::Value behaviorSpecData; std::istringstream iss(behaviorSpecString); - Json::CharReaderBuilder builder; - bool parsingResult = Json::parseFromStream(builder, iss, &behaviorSpecData, &err); - if (!parsingResult) { - std::cerr << "Failed to parse JSON string." << std::endl; - return; - } + Json::CharReaderBuilder builder; - behaviorSpecData["boardStamp"] = MOOS::Time(); - BatchConvertCoordinate(behaviorSpecData); - - std::string queryMemberName = behaviorSpecData["taskName"].asString(); - struct stat info; - if (stat(planConfigPath.c_str(), &info) != 0) + try { - Json::Value saveJsonValue; - saveJsonValue[queryMemberName] = behaviorSpecData; - Json::StreamWriterBuilder builder; - std::ofstream ofs; - ofs.open(planConfigPath, std::ios::out); - ofs << Json::writeString(builder, saveJsonValue) << std::endl; - ofs.close(); - } - else - { - std::ifstream ifs; - ifs.open(planConfigPath, std::ios::in); - Json::Reader taskConfigureReader; - Json::Value tempJsonValue; - taskConfigureReader.parse(ifs, tempJsonValue); - ifs.close(); + bool parsingResult = Json::parseFromStream(builder, iss, &behaviorSpecData, &err); + if (!parsingResult) { + throw ("PlanDB DBOP_SET parse error"); + } - Json::StreamWriterBuilder builder; - tempJsonValue[queryMemberName] = behaviorSpecData; - std::ofstream ofs; - ofs.open(planConfigPath, std::ios::out); - ofs << Json::writeString(builder, tempJsonValue) << std::endl; - ofs.close(); + behaviorSpecData["boardStamp"] = MOOS::Time(); + BatchConvertCoordinate(behaviorSpecData); + + std::string queryMemberName = behaviorSpecData["taskName"].asString(); + struct stat info; + if (stat(planConfigPath.c_str(), &info) != 0) + { + Json::Value saveJsonValue; + saveJsonValue[queryMemberName] = behaviorSpecData; + Json::StreamWriterBuilder builder; + std::ofstream ofs; + ofs.open(planConfigPath, std::ios::out); + ofs << Json::writeString(builder, saveJsonValue) << std::endl; + ofs.close(); + } + else + { + std::ifstream ifs; + ifs.open(planConfigPath, std::ios::in); + Json::Reader taskConfigureReader; + Json::Value tempJsonValue; + taskConfigureReader.parse(ifs, tempJsonValue); + ifs.close(); + + Json::StreamWriterBuilder builder; + tempJsonValue[queryMemberName] = behaviorSpecData; + std::ofstream ofs; + ofs.open(planConfigPath, std::ios::out); + ofs << Json::writeString(builder, tempJsonValue) << std::endl; + ofs.close(); + } + std::stringstream ss; + ss << std::fixed << std::setprecision(6) + << "PlanDB Set" << ":" + << MOOS::Time() << "," + << behaviorSpecData["taskName"].asString(); + Notify("uClient_plandbSet_log", ss.str()); + } + catch(std::string s) + { + std::cout << s << std::endl; + appCastContent = s; } - std::stringstream ss; - ss << std::fixed << std::setprecision(6) - << "PlanDB Set" << ":" - << MOOS::Time() << "," - << behaviorSpecData["taskName"].asString(); - Notify("uClient_plandbSet_log", ss.str()); } if (msg->type == DUNE::IMC::PlanDB::DBT_REQUEST && msg->op == DUNE::IMC::PlanDB::DBOP_GET_STATE) { @@ -707,65 +726,98 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) DUNE::IMC::SetEntityParameters * msg = dynamic_cast(message); printf("server receive %s: %lf\n", msg->getName(), msg->getTimeStamp()); DUNE::IMC::MessageList::const_iterator iter = msg->params.begin(); - std::stringstream ss2; - ss2 << std::fixed << std::setprecision(6) + std::stringstream ss1; + ss1 << std::fixed << std::setprecision(6) << "SetEntityParameters" << ":" << MOOS::Time(); - for(; iter < msg->params.end(); iter++) + + + try { - DUNE::IMC::EntityParameter *subEntityParameter = static_cast(*iter); + Json::Value calibrationCommand; + for(; iter < msg->params.end(); iter++) + { + DUNE::IMC::EntityParameter *subEntityParameter = static_cast(*iter); - std::string err; + std::string err; - Json::Value parameterValue; - std::istringstream iss(subEntityParameter->value); - Json::CharReaderBuilder builder; - bool parsingResult = Json::parseFromStream(builder, iss, ¶meterValue, &err); - if (!parsingResult) { - std::cerr << "Failed to parse JSON string." << std::endl; - return; - } + Json::Value parameterValue; + std::istringstream iss(subEntityParameter->value); + Json::CharReaderBuilder builder; + bool parsingResult = Json::parseFromStream(builder, iss, ¶meterValue, &err); + if (!parsingResult) + { + throw ("SetEntityParameters parse error"); + } + std::string parentName = msg->name; + std::string childName = subEntityParameter->name; + std::string dataType = parameterValue["type"].asString(); + std::string dataValueTemp = parameterValue["value"].asString(); + calibrationCommand[parentName][childName] = subEntityParameter->value; +#if 0 + if (dataType == "float") + { + std::stringstream ss2(dataValueTemp); + float dataValue; + ss2 >> dataValue; + printf("%s, %s: %s, %f\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue); + } + else if (dataType == "bool") + { + std::stringstream ss2(dataValueTemp); + bool dataValue; + ss2 >> dataValue; + printf("%s, %s: %s, %d\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue); + } + else if (dataType == "int") + { + std::stringstream ss2(dataValueTemp); + int dataValue; + ss2 >> dataValue; + printf("%s, %s: %s, %d\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue); + } + else + { + std::stringstream ss2(dataValueTemp); + std::string dataValue; + ss2 >> dataValue; + printf("%s, %s: %s, %s\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue.c_str()); + } +#endif + ss1 << "," << parentName << "," << childName << "," << dataValueTemp; + } + std::stringstream appCastStream; + appCastStream << std::fixed << std::setprecision(6) + << "server receive: " + << msg->getTimeStamp() << "," + << std::string(msg->getName()) << "," + << ss1.str(); + appCastContent = appCastStream.str(); + Notify("uClient_parameterSet_log", ss1.str()); - std::string parentName = msg->name; - std::string childName = subEntityParameter->name; - std::string dataType = parameterValue["type"].asString(); - std::string dataValueTemp = parameterValue["value"].asString(); - if (dataType == "float") - { - std::stringstream ss(dataValueTemp); - float dataValue; - ss >> dataValue; - printf("%s, %s: %s, %f\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue); - } - else if (dataType == "bool") - { - std::stringstream ss(dataValueTemp); - bool dataValue; - ss >> dataValue; - printf("%s, %s: %s, %d\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue); - } - else if (dataType == "int") - { - std::stringstream ss(dataValueTemp); - int dataValue; - ss >> dataValue; - printf("%s, %s: %s, %d\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue); - } - else - { - std::stringstream ss(dataValueTemp); - std::string dataValue; - ss >> dataValue; - printf("%s, %s: %s, %s\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue.c_str()); - } - ss2 << "," << parentName << "," << childName << "," << dataValueTemp; + Json::StreamWriterBuilder builder2; + Notify("uClient_calibration_cmd", Json::writeString(builder2, calibrationCommand)); + } + catch(std::string s) + { + std::cout << s << std::endl; + appCastContent = s; } - Notify("uClient_parameterSet_log", ss2.str()); } if (type == DUNE::IMC::PlanControl::getIdStatic()) { DUNE::IMC::PlanControl * msg = dynamic_cast(message); printf("server receive %s: %lf, %s\n", msg->getName(), msg->getTimeStamp(), msg->plan_id.c_str()); + std::stringstream appCastStream; + appCastStream << std::fixed << std::setprecision(6) + << "server receive: " + << msg->getTimeStamp() << "," + << std::string(msg->getName()) << "," + << (int)msg->type << "," + << (int)msg->op << "," + << msg->plan_id; + appCastContent = appCastStream.str(); + if (msg->type == DUNE::IMC::PlanControl::TypeEnum::PC_REQUEST) { std::string action = ""; @@ -781,8 +833,8 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) Json::Value outputTaskValue; outputTaskValue["taskName"] = msg->plan_id; outputTaskValue["action"] = action; - Json::StreamWriterBuilder builder2; - std::string outputTaskString = Json::writeString(builder2, outputTaskValue); + Json::StreamWriterBuilder builder; + std::string outputTaskString = Json::writeString(builder, outputTaskValue); if (action == "start") { std::ifstream ifs; @@ -821,16 +873,24 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) if (type == DUNE::IMC::VehicleCommand::getIdStatic()) { DUNE::IMC::VehicleCommand * msg = dynamic_cast(message); - printf("server receive %s: %lf, %u, %u\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->command); + printf("server receive %s: %lf, %u, %u\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->command); + std::stringstream appCastStream; + appCastStream << std::fixed << std::setprecision(6) + << "server receive: " + << msg->getTimeStamp() << "," + << std::string(msg->getName()) << "," + << (int)msg->type << "," + << (int)msg->command; + appCastContent = appCastStream.str(); if (msg->type == DUNE::IMC::VehicleCommand::TypeEnum::VC_REQUEST) { - if (msg->command == DUNE::IMC::VehicleCommand::CommandEnum::VC_EXEC_MANEUVER) + if (msg->command == 1) { - Notify("uManual_enable_cmd", (double)msg->command); + Notify("uManual_enable_cmd", "true"); } - if (msg->command == DUNE::IMC::VehicleCommand::CommandEnum::VC_STOP_MANEUVER) - { - Notify("uManual_enable_cmd", (double)msg->command); + if (msg->command == 0) + { + Notify("uManual_enable_cmd", "false"); } } std::stringstream ss; @@ -838,27 +898,46 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) << "VehicleCommand" << ":" << MOOS::Time() << "," << (int)msg->command; - Notify("uClient_manualEnable_log", ss.str()); + Notify("uClient_manualEnable_log", ss.str()); } if (type == DUNE::IMC::RemoteActions::getIdStatic()) { DUNE::IMC::RemoteActions * msg = dynamic_cast(message); - printf("server receive %s: %lf, %s\n", msg->getName(), msg->getTimeStamp(), msg->actions); + printf("server receive %s: %lf, %s\n", msg->getName(), msg->getTimeStamp(), msg->actions.c_str()); + std::stringstream appCastStream; + appCastStream << std::fixed << std::setprecision(6) + << "server receive: " + << msg->getTimeStamp() << "," + << std::string(msg->getName()) << "," + << msg->actions; + appCastContent = appCastStream.str(); Notify("uManual_drive_cmd", msg->actions); std::string err; Json::Value recvCommand; std::istringstream iss(msg->actions); Json::CharReaderBuilder builder; - bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err); - std::stringstream ss; - ss << std::fixed << std::setprecision(6) - << "RemoteActions" << ":" - << MOOS::Time() << "," - << "Thrust" << "," - << recvCommand["Thrust"].asInt() << "," - << "Heading" << "," - << recvCommand["Heading"].asFloat(); - Notify("uClient_manualDrive_log", ss.str()); + try + { + bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err); + if (!parsingResult) + { + throw ("RemoteActions parse error"); + } + std::stringstream ss; + ss << std::fixed << std::setprecision(6) + << "RemoteActions" << ":" + << MOOS::Time() << "," + << "Thrust" << "," + << recvCommand["Thrust"].asInt() << "," + << "Heading" << "," + << recvCommand["Heading"].asFloat(); + Notify("uClient_manualDrive_log", ss.str()); + } + catch(std::string s) + { + std::cout << s << std::endl; + appCastContent = s; + } } if (type == DUNE::IMC::LogBookControl::getIdStatic()) { @@ -875,9 +954,18 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) logBookContext = msgLogBookEntry->context; logBookText = msgLogBookEntry->text; } - printf("server receive %s: %lf, %u, %u, %s\n", msg->getName(), msg->getTimeStamp(), msg->command, logBookType, logBookContext.c_str()); + std::stringstream appCastStream; + appCastStream << std::fixed << std::setprecision(6) + << "server receive: " + << msg->getTimeStamp() << "," + << std::string(msg->getName()) << "," + << (int)msg->command << "," + << (int)logBookType << "," + << logBookContext << "," + << logBookText; + appCastContent = appCastStream.str(); std::string saveLogDir; m_MissionReader.GetValue("LogDir", saveLogDir); @@ -973,14 +1061,13 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) Json::Value retrFileObject; std::istringstream iss(logBookText); Json::CharReaderBuilder builder; - // std::cout << logBookText << std::endl; try { bool parsingResult = Json::parseFromStream(builder, iss, &retrFileObject, &err); if (!parsingResult) { - throw std::string("parsing json failure"); + throw std::string("LogBookEntry RETR parsing json failure"); } std::vector dirList = retrFileObject.getMemberNames(); @@ -1014,7 +1101,8 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) } catch(std::string s) { - std::cerr << s << std::endl; + std::cout << s << std::endl; + appCastContent = s; } } } @@ -1031,7 +1119,6 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) if (parsingResult) { - std::vector dirList = inputObject.getMemberNames(); int dirCount = dirList.size(); // Json::Value outputJsonValue; @@ -1130,7 +1217,6 @@ void SurfaceSupportComm::tcpProcessThread(const std::string& downloadFile) while (remaining > 0) { long int sendSize = sendfile(tcpSockConnect, fid, &offset, c_block_size); - std::cout << "sendSize: " << sendSize << std::endl; if (sendSize == -1) { break; @@ -1142,6 +1228,7 @@ void SurfaceSupportComm::tcpProcessThread(const std::string& downloadFile) catch(std::string s) { std::cout << s << std::endl; + appCastContent = s; } } @@ -1253,7 +1340,7 @@ bool SurfaceSupportComm::OnStartUp() if(tcpBindRet == -1) { struct sockaddr_in saddr; - saddr.sin_addr.s_addr = inet_addr(DEST_IP_ADDRESS); + saddr.sin_addr.s_addr = inet_addr(SRC_IP_ADDRESS); // saddr.sin_addr.s_addr = INADDR_ANY; saddr.sin_family = AF_INET; saddr.sin_port = htons(TCP_SEND_FILE_PORT); @@ -1271,7 +1358,8 @@ bool SurfaceSupportComm::OnStartUp() } catch(std::string s) { - std::cout << s << '\n'; + std::cout << s << std::endl; + appCastContent = s; } TimeLast = MOOS::Time(); @@ -1279,11 +1367,11 @@ bool SurfaceSupportComm::OnStartUp() return(true); } -// bool SurfaceSupportComm::buildReport() -// { -// m_msgs << "buildReport:" << testFlag++ << endl; -// return true; -// } +bool SurfaceSupportComm::buildReport() +{ + // m_msgs << appCastContent << endl; + return true; +} void SurfaceSupportComm::RegisterVariables() { diff --git a/src/pSurfaceSupportComm/SurfaceSupportComm.h b/src/pSurfaceSupportComm/SurfaceSupportComm.h old mode 100644 new mode 100755 index 7da2788..69623fe --- a/src/pSurfaceSupportComm/SurfaceSupportComm.h +++ b/src/pSurfaceSupportComm/SurfaceSupportComm.h @@ -18,6 +18,7 @@ #include #include #include +#include #include "MOOS/libMOOS/MOOSLib.h" #include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h" @@ -78,7 +79,7 @@ protected: // Standard MOOSApp functions to overload bool OnConnectToServer(); bool OnStartUp(); void RegisterVariables(); - // bool buildReport(); + bool buildReport(); private: enum MissionStatus{FAULT=0, UNRUN=1, MANUAL=2 ,RUN=3, CONFIG=5}; @@ -137,8 +138,6 @@ private: void tcpProcessThread(const std::string& downloadFile); double getFileSize(std::string filePath); - - int testFlag = 0; double TimeLast; int sendCount = 0; @@ -148,7 +147,8 @@ private: std::string vehicleName; unsigned int c_block_size; - std::list m_sockets; + std::string appCastContent; + std::mutex mtx; }; diff --git a/src/pSurfaceSupportComm/SurfaceSupportComm_Info.cpp b/src/pSurfaceSupportComm/SurfaceSupportComm_Info.cpp old mode 100644 new mode 100755 diff --git a/src/pSurfaceSupportComm/SurfaceSupportComm_Info.h b/src/pSurfaceSupportComm/SurfaceSupportComm_Info.h old mode 100644 new mode 100755 diff --git a/src/pSurfaceSupportComm/main.cpp b/src/pSurfaceSupportComm/main.cpp old mode 100644 new mode 100755 diff --git a/src/pSurfaceSupportComm/pSurfaceSupportComm.moos b/src/pSurfaceSupportComm/pSurfaceSupportComm.moos old mode 100644 new mode 100755 diff --git a/src/pTaskManagement/CMakeLists.txt b/src/pTaskManagement/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/src/pTaskManagement/TaskManger.cpp b/src/pTaskManagement/TaskManger.cpp old mode 100644 new mode 100755 index 142553b..5411cd1 --- a/src/pTaskManagement/TaskManger.cpp +++ b/src/pTaskManagement/TaskManger.cpp @@ -13,7 +13,7 @@ using namespace std; -#define DEBUG +// #define DEBUG //--------------------------------------------------------- @@ -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; @@ -123,7 +128,7 @@ bool TaskManger::OnNewMail(MOOSMSG_LIST &NewMail) if(msg_name == MSG_IN_MAN) { - if(msg_dval == 1) + if(msg_str == "true") state = MANUAL; else state = UNRUN; @@ -134,14 +139,14 @@ bool TaskManger::OnNewMail(MOOSMSG_LIST &NewMail) Json::Value j; Json::Reader a; a.parse(msg_str,j); - int fault_level = j["FaultLevel"].asInt(); - if(fault_level != 0) + // int fault_level = j["FaultLevel"].asInt(); + if(!j["FaultLevel"].empty()) //TODO: 修改故障码支持 { state = FAULT; - faultNumber = 1111; + if(!j["FaultMsgs"].isMember("uMission_status_fault")) + faultNumber = 1111; } } - } return(true); } @@ -346,16 +351,30 @@ bool TaskManger::Iterate() { case 40: //配置安全规则 { - readSafetyRules("a"); + readTask = readSafetyRules(); + if( readTask != 0 ) + { + state = FAULT; + faultNumber = 14 + readTask; + st = 30; + faultMsg = "safetyFileError : " + intToString(readTask); + } + //发送安全规则设置 setSafetyRules(maxTime, maxDepth, minAltitude, safePolygon); - // setMaxDepth(maxDepth); - setMaxDepth("2"); + setMaxDepth(maxDepth); st = 49; break; } case 41: //路径参数配置 { - readWayConfig("a"); + 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(); @@ -379,6 +398,8 @@ bool TaskManger::Iterate() switch (st) { case 50: // 清除变量 + FaultFlagClear(); + faultNumber = 0; current_node_complete = false; current_pol_complete = false; current_node=""; @@ -429,13 +450,32 @@ bool TaskManger::OnStartUp() if(param == "PLANCONFIGPATH") { planConfigPath = value; - RepList["CueenPath"] = planConfigPath; - } + //RepList["CueenPath"] = planConfigPath; + } + if(param == "SAFETYRULESPATH") + { + safetyRulesPath = value; + //RepList["RulesPath"] = safetyRulesPath; + } + if(param == "WAYCONFIGPARAMPATH") + { + wayParamPath = value; + RepList["WayConfigPath"] = wayParamPath; + } } } if(planConfigPath == "") reportConfigWarning("NO TASK FILE PATH"); - // readTaskFile("a"); + if(planConfigPath == "") + 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); } @@ -549,20 +589,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) @@ -583,9 +624,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; } /** @@ -599,6 +639,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); @@ -614,7 +655,7 @@ void TaskManger::RegisterVariables() */ int TaskManger::readTaskFile(string taskName) { - int faultNubmer = 0; + int fault = 0; if(!nodeList.empty()) nodeList.clear(); ifstream ifs; @@ -631,7 +672,7 @@ int TaskManger::readTaskFile(string taskName) if (!inputJsonValue.isMember(taskName)) { RepList["Task in File"] = "False"; - return faultNubmer=1; + return fault=1; } else { @@ -639,13 +680,13 @@ int TaskManger::readTaskFile(string taskName) string node=""; currentTask = inputJsonValue[taskName]; if(currentTask["taskName"].asString() != taskName) - return faultNubmer=2; + return fault=2; double currentTask_maxTime = currentTask["duration"].asDouble(); double repeat = currentTask["repeat"].asDouble(); if(!currentTask["points"].isArray()) - return faultNubmer=3; + return fault=3; Json::Value currentTask_Points = currentTask["points"]; int ps_cnt = currentTask_Points.size(); @@ -663,7 +704,7 @@ int TaskManger::readTaskFile(string taskName) else if(node_type == "track") node_type = "@2"; else - return faultNubmer=4; + return fault=4; node = node_type; node += ","; node += node_x; @@ -688,24 +729,76 @@ int TaskManger::readTaskFile(string taskName) cout << "--------------------------------" << endl; return 0; } - -int TaskManger::readSafetyRules(string fileName) +//maxTime, maxDepth, minAltitude, safePolygon +int TaskManger::readSafetyRules() { - maxDepth = "30"; - maxTime = "1000"; - safePolygon = "pts={-80,-00:-30,-175:150,-100:95,25}"; + ifstream ifs; + ifs.open(safetyRulesPath, ios::in); + Json::Reader reader; + Json::Value inputJsonValue; + reader.parse(ifs, inputJsonValue); + ifs.close(); + if(!inputJsonValue.isMember("maxTime") + || !inputJsonValue.isMember("maxDepth") + || !inputJsonValue.isMember("minAltitude") + || !inputJsonValue.isMember("points")) + return 1; + if(!inputJsonValue["points"].isArray()) + return 2; + double time = inputJsonValue["maxTime"].asDouble() * 60; + double depth = inputJsonValue["maxDepth"].asDouble(); + double altitude = inputJsonValue["minAltitude"].asDouble(); + maxTime = doubleToString(time); + maxDepth = doubleToString(depth); + minAltitude = doubleToString(altitude); + + string pts = ""; + Json::Value points = inputJsonValue["points"]; + //"pts={-80,-00:-30,-175:150,-100:95,25}" + int ps_cnt = points.size(); + for(int i=0; i taskList; int taskCount; string taskName; diff --git a/src/pTaskManagement/TaskMangerMain.cpp b/src/pTaskManagement/TaskMangerMain.cpp old mode 100644 new mode 100755 diff --git a/src/pTaskManagement/alpha.bhv b/src/pTaskManagement/alpha.bhv deleted file mode 100644 index 7a727f1..0000000 --- a/src/pTaskManagement/alpha.bhv +++ /dev/null @@ -1,181 +0,0 @@ -//-------- FILE: alpha.bhv ------------- -initialize RUN = false -initialize TaskNum=t1 -initialize SendTask=false -//--------------模式判断------------------------ -set MODE = ACTIVE{ - RUN=true -} INACTIVE - -set MODE = T1{ - MODE=ACTIVE - TaskNum = t1 -} - - -//----------路径点任务---------------------------- -Behavior = BHV_Waypoint -{ - name = waypt_survey - pwt = 100 //优先权重 - condition = MODE==T1 - - //endflag = START=false - endflag = END_WayPoint=true - - configflag = CRUISE_SPD = $[SPEED] - //configflag = OSPOS = $[OSX],$[OSY] - - activeflag = INFO=$[OWNSHIP] - activeflag = INFO=$[BHVNAME] - activeflag = INFO=$[BHVTYPE] - - //cycleflag = CINFO=$[OSX],$[OSY] - - wptflag = CurrentPointComplete=true - wptflag = PREV=$(PX),$(PY) - wptflag = NEXT=$(NX),$(NY) - wptflag = TEST=$(X),$(Y) - wptflag = OSPOS=$(OSX),$(OSY) - //wptflag_on_start = true - - - updates = WPT_UPDATE - //perpetual = true - - templating = spawn - - // speed_alt = 1.2 - //use_alt_speed = true - lead = 8 - lead_damper = 1 - lead_to_start = false - speed = 1 // meters per second - capture_line = true - capture_radius = 5.0 - slip_radius = 15.0 - efficiency_measure = all - - polygon = 60,-40 - order = normal - //repeat = 3 - - visual_hints = nextpt_color=yellow - visual_hints = nextpt_vertex_size=8 - visual_hints = nextpt_lcolor=gray70 - visual_hints = vertex_color=dodger_blue, edge_color=white - visual_hints = vertex_size=5, edge_size=1 -} - - -//--------------定深任务------------------ -Behavior=BHV_ConstantDepth -{ - name = const_depth - pwt = 100 - //condition = DEPLOY = true - condition = MODE==T1 - duration = no-time-limit - updates = DEPTH_UPDATE - depth = 0 -} - -//--------------定向任务-------------------- - -Behavior=BHV_ConstantHeading -{ - name = const_heading - pwt = 100 - //condition = START_TURN = true - //condition = DEPLOY = true - condition = MODE==T3 - perpetual = true - - activeflag = TURN = started - - //endflag = TURN = done - //endflag = RETURN = true - //endflag = START_TURN = false - endflag = START=false - - heading = 225 - complete_thresh = 5 - duration = no-time-limit - } - -//--------------定速任务-------------------- -Behavior=BHV_ConstantSpeed -{ - name = const_speed - pwt = 1000 - condition = MODE==T1 - perpetual = true - updates = SPEED_UPDATE - //endflag = START=false - - speed = 5 - - duration = no-time-limit - //peakwidth = 0.5 - //basewidth = 0.5 - -} -//----------------安全模式----------------------- -//----------------计时器--------------------- -Behavior = BHV_Timer -{ - name = mtime - condition = MODE==T1 - pwt = 100 - templating = spawn - //duration_status = MSTATUS - //duration = 10 - endflag = TIME_OUT=true - - updates = TIMER_UPDATES - - //perpetual = true -} -//-------------最大深度限制-------------------------- -Behavior = BHV_MaxDepth -{ - name = maxdepth - pwt = 200 - condition = MODE==ACTIVE - updates = MAXDEEP_UPDATES - max_depth = 20 - tolerance = 0 - duration = no-time-limit -} -//--------------安全区域设置----------------------- - - Behavior = BHV_OpRegion - { - // General Behavior Parameters - // --------------------------- - name = op_region // example - pwt = 300 // default - condition = MODE==TN - updates = OPREGION_UPDATES // example - - // Parameters specific to this behavior - // ------------------------------------ - max_time = 20 // default (seconds) - max_depth = 25 // 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) - - polygon = pts={-80,-00:-30,-175:150,-100:95,25} - - breached_altitude_flag = TaskFault = AltitudeOut - breached_depth_flag = TaskFault = DepthOut - breached_poly_flag = TaskFault = RegionOut - breached_time_flag = TaskFault = TimeOut - - visual_hints = vertex_color = brown // default - visual_hints = vertex_size = 3 // default - visual_hints = edge_color = aqua // default - visual_hints = edge_size = 1 // default - } \ No newline at end of file diff --git a/src/pTaskManagement/alpha.moos b/src/pTaskManagement/alpha.moos deleted file mode 100644 index 7cb4788..0000000 --- a/src/pTaskManagement/alpha.moos +++ /dev/null @@ -1,269 +0,0 @@ -//------------------------------------------------- -// NAME: M. Benjamin, MIT CSAIL -// FILE: alpha.moos -//------------------------------------------------- - -ServerHost = localhost -ServerPort = 9000 -Community = pi -MOOSTimeWarp = 1 - -// Forest Lake -LatOrigin = 43.825300 -LongOrigin = -70.330400 - -// MIT Sailing Pavilion (use this one) -// LatOrigin = 42.358456 -// LongOrigin = -71.087589 - -//------------------------------------------ -// Antler configuration block -ProcessConfig = ANTLER -{ - MSBetweenLaunches = 200 - - Run = MOOSDB @ NewConsole = false - //Run = pLogger @ NewConsole = false - Run = uSimMarineV22 @ NewConsole = false - Run = pMarinePIDV22 @ NewConsole = false - Run = pHelmIvP @ NewConsole = false - Run = pMarineViewer @ NewConsole = false - Run = uProcessWatch @ NewConsole = false - Run = pNodeReporter @ NewConsole = false - Run = pRealm @ NewConsole = false - Run = pTaskManger @ NewConsole = false - //Run = uTimerScript @ NewConsole = false -} - -//------------------------------------------ -// pLogger config block -ProcessConfig = pTaskManger -{ - AppTick = 8 - CommsTick = 8 - - planConfigPath = /home/zjk/Desktop/project/moos-ivp-extend/PlanConfigure.json -} -ProcessConfig = pLogger -{ - AppTick = 8 - CommsTick = 8 - - AsyncLog = 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 - //Log = BHV_SETTINGS @ 0 NOSYNC - Log = OPREGION_RESET @ 0 NOSYNC - - LogAuxSrc = true - WildCardLogging = true - WildCardOmitPattern = *_STATUS - WildCardOmitPattern = DB_VARSUMMARY - WildCardOmitPattern = DB_RWSUMMARY - WildCardExclusionLog = true -} - -//------------------------------------------ -// uProcessWatch config block - -ProcessConfig = uProcessWatch -{ - AppTick = 4 - CommsTick = 4 - - watch_all = true - nowatch = uPokeDB* - nowatch = uQueryDB* - nowatch = uXMS* - nowatch = uMAC* -} - -//------------------------------------------ -// uSimMarineV22 config block - -ProcessConfig = uSimMarineV22 -{ - AppTick = 4 - CommsTick = 4 - - start_pos = x=0, y=-20, heading=180, speed=5 - - prefix = NAV - - turn_rate = 40 - thrust_map = 0:0, 20:1, 40:2, 60:3, 80:4, 100:5 - //thrust_reflect = true - - buoyancy_rate = 0.075 - max_depth_rate = 5 - max_depth_rate_speed = 2.0 - default_water_depth = 400 -} - -//------------------------------------------ -// pHelmIvP config block - -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 - -} - -//------------------------------------------ -// pMarinePID config block - -ProcessConfig = pMarinePIDV22 -{ - AppTick = 20 - CommsTick = 20 - - verbose = true - depth_control = true - - // SIM_INSTABILITY = 20 - - // Yaw PID controller - yaw_pid_kp = 1.2 - yaw_pid_kd = 0.0 - yaw_pid_ki = 0.3 - yaw_pid_integral_limit = 0.07 - - // Speed PID controller - speed_pid_kp = 1.0 - speed_pid_kd = 0.0 - speed_pid_ki = 0.1 - speed_pid_integral_limit = 0.07 - - maxpitch = 15 - maxelevator = 13 - - pitch_pid_kp = 1.5 - pitch_pid_kd = 0 - pitch_pid_ki = 1.0 - pitch_pid_integral_limit = 0 - - z_to_pitch_pid_kp = 0.12 - z_to_pitch_pid_kd = 0 - z_to_pitch_pid_ki = 0.004 - z_to_pitch_pid_integral_limit = 0.05 - - - //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 = 0 - simulation = true -} - -//------------------------------------------ -// pMarineViewer config block - -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 # START=false - //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 -} - -//------------------------------------------ -// pNodeReporter config block - -ProcessConfig = pNodeReporter -{ - AppTick = 2 - CommsTick = 2 - - //platform_type = kayak - //更改显示形状为uuv - platform_type = UUV - platform_color = red - platform_length = 4 -} - -ProcessConfig = uTimerScript -{ - AppTick = 4 - CommsTick = 4 - - condition = DEPLOY = true - randvar = varname = RND_DEPTH, min=20, max=80, key=at_reset - event = var = DEPTH_UPDATE, val=depth=$[RND_DEPTH], time=120 - reset_max = nolimit reset_time = all-posted -} \ No newline at end of file diff --git a/src/pTaskManagement/clean.sh b/src/pTaskManagement/clean.sh deleted file mode 100644 index 63baef7..0000000 --- a/src/pTaskManagement/clean.sh +++ /dev/null @@ -1,37 +0,0 @@ -#!/bin/bash -#-------------------------------------------------------------- -# Script: clean.sh -# Author: Michael Benjamin -# Date: June 2020 -#---------------------------------------------------------- -# Part 1: Declare global var defaults -#---------------------------------------------------------- -VERBOSE="" - -#------------------------------------------------------- -# Part 2: Check for and handle command-line arguments -#------------------------------------------------------- -for ARGI; do - if [ "${ARGI}" = "--help" -o "${ARGI}" = "-h" ] ; then - echo "clean.sh [SWITCHES] " - echo " --verbose " - echo " --help, -h " - exit 0; - elif [ "${ARGI}" = "--verbose" -o "${ARGI}" = "-v" ] ; then - VERBOSE="-v" - else - echo "clean.sh: Bad Arg:" $ARGI - exit 1 - fi -done - -#------------------------------------------------------- -# Part 2: Do the cleaning! -#------------------------------------------------------- -if [ "${VERBOSE}" = "-v" ]; then - echo "Cleaning: $PWD" -fi -rm -rf $VERBOSE MOOSLog_* XLOG_* LOG_* -rm -f $VERBOSE *~ *.moos++ -rm -f $VERBOSE targ_* -rm -f $VERBOSE .LastOpenedMOOSLogDirectory diff --git a/src/pTaskManagement/launch.sh b/src/pTaskManagement/launch.sh deleted file mode 100644 index 4c1b0a6..0000000 --- a/src/pTaskManagement/launch.sh +++ /dev/null @@ -1,39 +0,0 @@ -#!/bin/bash -e -#---------------------------------------------------------- -# Script: launch.sh -# Author: Michael Benjamin -# LastEd: May 20th 2019 -#---------------------------------------------------------- -# Part 1: Set Exit actions and declare global var defaults -#---------------------------------------------------------- -TIME_WARP=1 -COMMUNITY="alpha" -GUI="yes" - -#---------------------------------------------------------- -# Part 2: Check for and handle command-line arguments -#---------------------------------------------------------- -for ARGI; do - if [ "${ARGI}" = "--help" -o "${ARGI}" = "-h" ] ; then - echo "launch.sh [SWITCHES] [time_warp] " - echo " --help, -h Show this help message " - exit 0; - elif [ "${ARGI}" = "--nogui" ] ; then - GUI="no" - elif [ "${ARGI//[^0-9]/}" = "$ARGI" -a "$TIME_WARP" = 1 ]; then - TIME_WARP=$ARGI - else - echo "launch.sh Bad arg:" $ARGI " Exiting with code: 1" - exit 1 - fi -done - - -#---------------------------------------------------------- -# Part 3: Launch the processes -#---------------------------------------------------------- -echo "Launching $COMMUNITY MOOS Community with WARP:" $TIME_WARP -pAntler $COMMUNITY.moos --MOOSTimeWarp=$TIME_WARP >& /dev/null & - -uMAC -t $COMMUNITY.moos -kill -- -$$ diff --git a/src/pTaskManagement/pTaskManger.moos b/src/pTaskManagement/pTaskManger.moos old mode 100644 new mode 100755 index 1e790d3..092de1b --- a/src/pTaskManagement/pTaskManger.moos +++ b/src/pTaskManagement/pTaskManger.moos @@ -7,6 +7,8 @@ ProcessConfig = pTaskManger { AppTick = 10 CommsTick = 10 - planConfigPath = /home/zjk/Desktop/project/moos-ivp-extend/PlanConfigure.json + planConfigPath = /home/zjk/project/work/moos-ivp-pi/setting/PlanConfigure.json + safetyRulesPath = /home/zjk/project/work/moos-ivp-pi/setting/SafetyRules.json + wayConfigParamPath = /home/zjk/project/work/moos-ivp-pi/setting/WayConfigParam.json } diff --git a/src/pTaskSend/CMakeLists.txt b/src/pTaskSend/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/src/pTaskSend/TaskSend.cpp b/src/pTaskSend/TaskSend.cpp old mode 100644 new mode 100755 diff --git a/src/pTaskSend/TaskSend.h b/src/pTaskSend/TaskSend.h old mode 100644 new mode 100755 diff --git a/src/pTaskSend/TaskSendMain.cpp b/src/pTaskSend/TaskSendMain.cpp old mode 100644 new mode 100755 diff --git a/src/pTaskSend/build/.cmake/api/v1/query/client-vscode/query.json b/src/pTaskSend/build/.cmake/api/v1/query/client-vscode/query.json old mode 100644 new mode 100755 diff --git a/src/pTaskSend/build/.cmake/api/v1/reply/cache-v2-d6d942c4138e4121aadf.json b/src/pTaskSend/build/.cmake/api/v1/reply/cache-v2-d6d942c4138e4121aadf.json old mode 100644 new mode 100755 diff --git a/src/pTaskSend/build/.cmake/api/v1/reply/cmakeFiles-v1-1b864ed9ef170e655086.json b/src/pTaskSend/build/.cmake/api/v1/reply/cmakeFiles-v1-1b864ed9ef170e655086.json old mode 100644 new mode 100755 diff --git a/src/pTaskSend/build/.cmake/api/v1/reply/codemodel-v2-8cafde00da2f94adfddf.json b/src/pTaskSend/build/.cmake/api/v1/reply/codemodel-v2-8cafde00da2f94adfddf.json old mode 100644 new mode 100755 diff --git a/src/pTaskSend/build/.cmake/api/v1/reply/directory-.-Debug-f5ebdc15457944623624.json b/src/pTaskSend/build/.cmake/api/v1/reply/directory-.-Debug-f5ebdc15457944623624.json old mode 100644 new mode 100755 diff --git a/src/pTaskSend/build/.cmake/api/v1/reply/index-2023-09-21T07-09-11-0928.json b/src/pTaskSend/build/.cmake/api/v1/reply/index-2023-09-21T07-09-11-0928.json old mode 100644 new mode 100755 diff --git a/src/pTaskSend/build/.cmake/api/v1/reply/target-pTaskSend-Debug-fe070126a6f1fe44e26b.json b/src/pTaskSend/build/.cmake/api/v1/reply/target-pTaskSend-Debug-fe070126a6f1fe44e26b.json old mode 100644 new mode 100755 diff --git a/src/pTaskSend/build/.cmake/api/v1/reply/toolchains-v1-2781238ec0fa0b9996f9.json b/src/pTaskSend/build/.cmake/api/v1/reply/toolchains-v1-2781238ec0fa0b9996f9.json old mode 100644 new mode 100755 diff --git a/src/pTaskSend/display.cpp b/src/pTaskSend/display.cpp old mode 100644 new mode 100755 diff --git a/src/pTaskSend/display.h b/src/pTaskSend/display.h old mode 100644 new mode 100755 diff --git a/src/pTaskSend/pTaskSend.moos b/src/pTaskSend/pTaskSend.moos old mode 100644 new mode 100755