zjk/moos-ivp-pi
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'master' of http://10.25.0.206:3002/UUV/moos-ivp-pi

Browse Source
This commit is contained in:
wade
2023-11-30 09:47:31 +08:00
parent 1a10bbedaa 6e35606305
commit f75e622d96
137 changed files with 807 additions and 2615 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
.gitignore vendored
View File

@@ -101,7 +101,7 @@ dkms.conf
/log
/launch/launch_local.moos
bin/
build/
bin/*
/build/
/scripts
/.vscode

0
CMakeLists.txt Normal file → Executable file
View File

175
PlanConfigure.json
View File

@@ -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"
}
}

0
README Normal file → Executable file
View File

19
README.md Normal file → Executable file
View File

@@ -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 : 路径参数配置文件

147
alpha.moos
View File

@@ -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)
}

0
bin/placeholder → bin/placehold
View File

0
build/.cmake/api/v1/query/client-vscode/query.json Normal file → Executable file
View File

0
build/.cmake/api/v1/reply/cache-v2-dd2c937f009d9a652f28.json Normal file → Executable file
View File

0
build/.cmake/api/v1/reply/cmakeFiles-v1-a2b0be97f994224a37d6.json Normal file → Executable file
View File

0
build/.cmake/api/v1/reply/codemodel-v2-e5f65b12d81cb1175a33.json Normal file → Executable file
View File

116
build/.cmake/api/v1/reply/index-2023-11-24T08-55-39-0072.json
View File

@@ -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
}
}
]
}
}
}
}

0
build/.cmake/api/v1/reply/target-pBoardSupportComm-None-6b6714a772e692ce7bda.json Normal file → Executable file
View File

0
build/.cmake/api/v1/reply/target-pClientViewer-None-637c837c32b7f1c4a3f8.json Normal file → Executable file
View File

0
build/.cmake/api/v1/reply/target-pDataManagement-None-3fb8b0228e67220a29b1.json Normal file → Executable file
View File

0
build/.cmake/api/v1/reply/target-pEmulator-None-e8b09174b873ffc7a0a4.json Normal file → Executable file
View File

0
build/.cmake/api/v1/reply/target-pFaultHandle-None-96d2d6ef9203623ec32c.json Normal file → Executable file
View File

0
build/.cmake/api/v1/reply/target-pMotionControler-None-3f183e81cc43357193c0.json Normal file → Executable file
View File

0
build/.cmake/api/v1/reply/target-pStateManagement-None-12c0626db4f6cdfc3816.json Normal file → Executable file
View File

0
build/.cmake/api/v1/reply/target-pSurfaceSupportComm-None-5c1b066d62accb72956a.json Normal file → Executable file
View File

0
build/.cmake/api/v1/reply/target-pTaskManagement-None-97473a05e92e62137b49.json Normal file → Executable file
View File

0
build/.cmake/api/v1/reply/target-pTaskSend-None-3a4f500e73d7f1e47a5a.json Normal file → Executable file
View File

0
build/read.txt Normal file → Executable file
View File

0
launch/.LastOpenedMOOSLogDirectory Normal file → Executable file
View File

14
launch/alpha.bhv Normal file → Executable file
View File

@@ -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

0
launch/alpha.moos Normal file → Executable file
View File

11
launch/launch.moos Normal file → Executable file
View File

@@ -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

328
launch/launch_local.moos
View File

@@ -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
}

0
missions/alder/README Normal file → Executable file
View File

0
missions/alder/alder.bhv Normal file → Executable file
View File

0
missions/alder/alder.moos Normal file → Executable file
View File

0
missions/alder/alder_orig.bhv Normal file → Executable file
View File

0
missions/s1_alpha/alpha.bhv Normal file → Executable file
View File

0
missions/s1_alpha/alpha.moos Normal file → Executable file
View File

0
missions/xrelay/xrelay.moos Normal file → Executable file
View File

0
scripts/README Normal file → Executable file
View File

0
setting/ControlParam.json Normal file → Executable file
View File

2
setting/Origin.json Normal file → Executable file
View File

@@ -1,5 +1,5 @@
{
"AltOrigin" : 0,
"AltOrigin" : 0.0,
"LatOrigin" : 50.825298309326172,
"LongOrigin" : -90.330398559570312,
"TaskName" : "east_waypt_survey"

34
setting/PlanConfigure.json Normal file → Executable file
View File

@@ -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,

24
setting/SafetyRules.json Normal file
View File

@@ -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
}
]
}

9
setting/WayConfigParam.json Normal file
View File

@@ -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"
}

0
src/CMakeLists.txt Normal file → Executable file
View File

206
src/pBoardSupportComm/BoardSupportComm.cpp Normal file → Executable file
View File

@@ -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<double> reference = {estimatedState.referenceLon, estimatedState.referenceLat, estimatedState.referenceAltitude};
std::vector<double> reference = {embeddedInfo.refLon, embeddedInfo.refLat, embeddedInfo.refAlt};
std::vector<double> current = {currentLon, currentLat, currentAlt};
std::vector<double> 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);

45
src/pBoardSupportComm/BoardSupportComm.h Normal file → Executable file
View File

@@ -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<double> init_lla,
std::vector<double> point_lla,
std::vector<double>& point_enu);
@@ -129,7 +112,6 @@ private:
std::vector<double> &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;
};

0
src/pBoardSupportComm/BoardSupportComm_Info.cpp Normal file → Executable file
View File

0
src/pBoardSupportComm/BoardSupportComm_Info.h Normal file → Executable file
View File

0
src/pBoardSupportComm/CMakeLists.txt Normal file → Executable file
View File

0
src/pBoardSupportComm/main.cpp Normal file → Executable file
View File

0
src/pBoardSupportComm/pBoardSupportComm.moos Normal file → Executable file
View File

0
src/pClientViewer/Behavior.pb.h Normal file → Executable file
View File

0
src/pClientViewer/CMakeLists.txt Normal file → Executable file
View File

2
src/pClientViewer/ClientViewer.cpp Normal file → Executable file
View File

@@ -765,7 +765,7 @@ void ClientViewer::processMessage(DUNE::IMC::Message * message) {
for (; iter2 != entityParameter->params.end(); ++iter2)
{
DUNE::IMC::EntityParameter *subEntityParameter = static_cast<DUNE::IMC::EntityParameter *>(*iter2);
std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl;
//std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl;
}
}
}

0
src/pClientViewer/ClientViewer.h Normal file → Executable file
View File

0
src/pClientViewer/ClientViewer_Info.cpp Normal file → Executable file
View File

0
src/pClientViewer/ClientViewer_Info.h Normal file → Executable file
View File

0
src/pClientViewer/main.cpp Normal file → Executable file
View File

0
src/pClientViewer/pClientViewer.moos Normal file → Executable file
View File

0
src/pDataManagement/CMakeLists.txt Normal file → Executable file
View File

291
src/pDataManagement/DataManagement.cpp Normal file → Executable file
View File

@@ -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<string> sParams;
m_MissionReader.EnableVerbatimQuoting(false);
if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) {
list<string>::iterator p;
for(p=sParams.begin(); p!=sParams.end(); p++) {
string line = *p;
string param = tolower(biteStringX(line, '='));
string value = line;
// list<string> sParams;
// m_MissionReader.EnableVerbatimQuoting(false);
// if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) {
// list<string>::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();
}
}

4
src/pDataManagement/DataManagement.h Normal file → Executable file
View File

@@ -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<std::string, int> logVarList;
std::string contentFromStream;
struct MotionControlInfo motionControlInfo;
bool logEnable;
std::string saveLogDir;
};
#endif

0
src/pDataManagement/DataManagement_Info.cpp Normal file → Executable file
View File

0
src/pDataManagement/DataManagement_Info.h Normal file → Executable file
View File

809
src/pDataManagement/NavigationInfo.pb.h
View File

@@ -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 <limits>
#include <string>
#include <google/protobuf/port_def.inc>
#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 <google/protobuf/port_undef.inc>
#include <google/protobuf/io/coded_stream.h>
#include <google/protobuf/arena.h>
#include <google/protobuf/arenastring.h>
#include <google/protobuf/generated_message_util.h>
#include <google/protobuf/metadata_lite.h>
#include <google/protobuf/generated_message_reflection.h>
#include <google/protobuf/message.h>
#include <google/protobuf/repeated_field.h> // IWYU pragma: export
#include <google/protobuf/extension_set.h> // IWYU pragma: export
#include <google/protobuf/unknown_field_set.h>
// @@protoc_insertion_point(includes)
#include <google/protobuf/port_def.inc>
#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<const EstimatedState*>(
&_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<EstimatedState>(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 <typename ArgT0 = const std::string&, typename... ArgT>
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 <typename T> 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 <typename ArgT0, typename... ArgT>
inline PROTOBUF_ALWAYS_INLINE
void EstimatedState::set_info(ArgT0&& arg0, ArgT... args) {
_impl_.info_.Set(static_cast<ArgT0 &&>(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 <google/protobuf/port_undef.inc>
#endif // GOOGLE_PROTOBUF_INCLUDED_GOOGLE_PROTOBUF_INCLUDED_NavigationInfo_2eproto

0
src/pDataManagement/main.cpp Normal file → Executable file
View File

0
src/pDataManagement/pDataManagement.moos Normal file → Executable file
View File

0
src/pEmulator/.LastOpenedMOOSLogDirectory Normal file → Executable file
View File

0
src/pEmulator/CMakeLists.txt Normal file → Executable file
View File

18
src/pEmulator/Emulator.cpp Normal file → Executable file
View File

@@ -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]

6
src/pEmulator/Emulator.hpp Normal file → Executable file
View File

@@ -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
{

21
src/pEmulator/_150server.cpp Normal file → Executable file
View File

@@ -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 <unistd.h>
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");

4
src/pEmulator/_150server.hpp Normal file → Executable file
View File

@@ -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:
*

0
src/pEmulator/a.moos Normal file → Executable file
View File

0
src/pEmulator/alpha.bhv Normal file → Executable file
View File

0
src/pEmulator/main.cpp Normal file → Executable file
View File

0
src/pEmulator/pEmulator.moos Normal file → Executable file
View File

0
src/pFaultHandle/CMakeLists.txt Normal file → Executable file
View File

0
src/pFaultHandle/FaultHandle.cpp Normal file → Executable file
View File

0
src/pFaultHandle/FaultHandle.h Normal file → Executable file
View File

0
src/pFaultHandle/FaultHandle_Info.cpp Normal file → Executable file
View File

0
src/pFaultHandle/FaultHandle_Info.h Normal file → Executable file
View File

0
src/pFaultHandle/main.cpp Normal file → Executable file
View File

0
src/pFaultHandle/pFaultHandle.moos Normal file → Executable file
View File

0
src/pMotionControler/CMakeLists.txt Normal file → Executable file
View File

0
src/pMotionControler/ControlParam.json Normal file → Executable file
View File

0
src/pMotionControler/Controler.cpp Normal file → Executable file
View File

0
src/pMotionControler/Controler.hpp Normal file → Executable file
View File

0
src/pMotionControler/MotionControler.cpp Normal file → Executable file
View File

0
src/pMotionControler/MotionControler.hpp Normal file → Executable file
View File

0
src/pMotionControler/a.moos Normal file → Executable file
View File

0
src/pMotionControler/alpha.bhv Normal file → Executable file
View File

0
src/pMotionControler/alpha.moos Normal file → Executable file
View File

0
src/pMotionControler/main.cpp Normal file → Executable file
View File

0
src/pMotionControler/pidControl.cpp Normal file → Executable file
View File

0
src/pMotionControler/pidControl.hpp Normal file → Executable file
View File

0
src/pMotionControler/simMat.moos Normal file → Executable file
View File

6
src/pStateManagement/CMakeLists.txt Normal file → Executable file
View File

@@ -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

18
src/pStateManagement/StateManagement.cpp Normal file → Executable file
View File

@@ -9,6 +9,7 @@
#include "MBUtils.h"
#include "StateManagement.h"
#include <json/json.h>
#include <cmath>
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;
}

6
src/pStateManagement/StateManagement.h Normal file → Executable file
View File

@@ -9,6 +9,12 @@
#define StateManagement_HEADER
#include "MOOS/libMOOS/MOOSLib.h"
#include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
#include <DUNE/DUNE.hpp>
#include <GeographicLib/LocalCartesian.hpp>
#include <json/json.h>
#include <thread>
#include <chrono>
class StateManagement : public CMOOSApp
{

0
src/pStateManagement/StateManagement_Info.cpp Normal file → Executable file
View File

0
src/pStateManagement/StateManagement_Info.h Normal file → Executable file
View File

Some files were not shown because too many files have changed in this diff Show More