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

.gitignore

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

PlanConfigure.json

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

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

alpha.moos

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

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

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

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_time = 0.1              // default (seconds)
-                 max_depth = 25              // default (meters)
+                 max_depth = 2              // default (meters)
-              min_altitude = 0              // default (meters)
+              //min_altitude = 0              // default (meters)
                  reset_var = OPREGION_RESET // example
         trigger_entry_time = 1              // default (seconds)
          trigger_exit_time = 0.5            // default (seconds) 
 
                    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

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/project/moos-ivp-pi/setting/Origin.json
-llaOriginPath = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/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

launch/launch_local.moos

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

setting/Origin.json

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

setting/PlanConfigure.json

@@ -67,8 +67,8 @@
 	},
 	"plan1_toMoos" : 
 	{
-		"boardStamp" : 1699602762.2845099,
+		"boardStamp" : 1700965801.2335429,
-		"clientStamp" : 1699602762.7520001,
+		"clientStamp" : 1700965795.187,
 		"closedLoop" : false,
 		"duration" : -1.0,
 		"maxDepth" : -1.0,
@@ -76,49 +76,49 @@
 		"origin" : 
 		{
 			"altitude" : 0.0,
-			"lat" : 50.825299999999999,
+			"lat" : 43.825299999999999,
-			"lon" : -90.330399999999997
+			"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" : 

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

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

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 MOOS_AUV_SIM
-// #define MATLAB_AUV_SIM
+#define MATLAB_AUV_SIM
-// #ifdef TRUE_AUV
+//#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);
+      float tempYaw = (float)(embeddedInfo.yaw) / 100.0;
-      estimatedState.linearVelocityEast = -msg.GetDouble() * sin(estimatedState.yaw);
+      embeddedInfo.ins_vx = dval * cos(tempYaw) * 100;
-      estimatedState.linearVelocityDown = 0;
+      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();
+        uint8_t helm_top_bottom_angle = convertIntToUchar((int)(recvCommand["Heading"].asFloat()), -30, 30);        
-        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;          
-        }           
         executeCommand.depth = 0;
-        executeCommand.helm_top_angle = 0;
+        executeCommand.helm_top_angle = helm_top_bottom_angle;
-        executeCommand.helm_bottom_angle = 0;
+        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();
+      embeddedInfo.refLon = recvCommand["lon"].asFloat();
-      estimatedState.referenceLat = recvCommand["lat"].asFloat();
+      embeddedInfo.refLat = recvCommand["lat"].asFloat();
-      estimatedState.referenceLon = recvCommand["lon"].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["insVX"] = embeddedInfo.ins_vx * 0.01;
-  embeddedState["insVY"] = estimatedState.linearVelocityEast * 0.01;
+  embeddedState["insVY"] = embeddedInfo.ins_vy * 0.01;
-  embeddedState["insVZ"] = estimatedState.linearVelocityDown * 0.01;
+  embeddedState["insVZ"] = embeddedInfo.ins_vz * 0.01;
-  embeddedState["currentLon"] = estimatedState.currentLon;
+  embeddedState["currentLon"] = embeddedInfo.lon * 0.000001;
-  embeddedState["currentLat"] = estimatedState.currentLat;
+  embeddedState["currentLat"] = embeddedInfo.lat * 0.000001;
-  embeddedState["currentAltitude"] = 0;
+  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["referenceLon"]= embeddedInfo.refLon;  
-  embeddedState["referenceLat"]= estimatedState.referenceLat;  
+  embeddedState["referenceLat"]= embeddedInfo.refLat;  
-  embeddedState["referenceAltitude"]= estimatedState.referenceAltitude;    
+  embeddedState["referenceAltitude"]= embeddedInfo.refAlt;    
 
   Json::StreamWriterBuilder builder;
-  std::string embeddedStateString = Json::writeString(builder, embeddedState);
+  return Json::writeString(builder, embeddedState);
-  return embeddedStateString;
 }
 
 
@@ -530,15 +500,15 @@ void BoardSupportComm::tcpProcessThread()
   while(1)
   {
     bzero(tcpReceiveBuffer, 0);
-    int lens = read(tcpSockFD, tcpReceiveBuffer, sizeof(tcpReceiveBuffer));
+    recvLen = read(tcpSockFD, tcpReceiveBuffer, sizeof(tcpReceiveBuffer));
-    if(lens>0)
+    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);
+      recvContent = convertEmbeddedFormat(embeddedState);
-      Notify("uDevice_monitor_fb", embeddedStateString);
+      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()
+bool BoardSupportComm::buildReport()
-// {
+{
-//   m_msgs << "buildReport:" << embeddedStateString << endl;
+  m_msgs << std::fixed << std::setprecision(6) << MOOS::Time() << ", recvLen:" << recvLen << endl;
-//   return true;
+  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();
+        embeddedInfo.refLon = originJsonValue["LongOrigin"].asFloat();
-        estimatedState.referenceLat = originJsonValue["LatOrigin"].asFloat();
+        embeddedInfo.refLat = originJsonValue["LatOrigin"].asFloat();
-        estimatedState.referenceAltitude = originJsonValue["AltOrigin"].asFloat();        
+        embeddedInfo.refAlt = originJsonValue["AltOrigin"].asFloat();        
       }
       else
       {
@@ -798,22 +772,18 @@ bool BoardSupportComm::OnStartUp()
   }
   catch(...)
   {
-    m_MissionReader.GetValue("LongOrigin", estimatedState.referenceLon);
+    m_MissionReader.GetValue("LongOrigin", embeddedInfo.refLon);
-    m_MissionReader.GetValue("LatOrigin", estimatedState.referenceLat);
+    m_MissionReader.GetValue("LatOrigin", embeddedInfo.refLat);
-    m_MissionReader.GetValue("AltOrigin", estimatedState.referenceAltitude);
+    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);

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 {
+class BoardSupportComm : public AppCastingMOOSApp
-  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
 {
  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;
 
 };
 

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<DUNE::IMC::EntityParameter *>(*iter2);
-        std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl;        
+        //std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl;        
       }
     }
   }

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())
+  CloseOutputStream();   
-  {
-    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();
-  }    
 }
 
 //---------------------------------------------------------
@@ -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,7 +290,7 @@ 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 << ","
@@ -270,6 +298,8 @@ bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail)
         << motionControlInfo.desiredDepth;
       Notify("uMotion_desired_log", ss.str());        
     }
+ 
+
   }
 	
   return(true);
@@ -372,92 +402,55 @@ bool DataManagement::OnStartUp()
 {
   AppCastingMOOSApp::OnStartUp();
 
-  list<string> sParams;
+  // list<string> sParams;
-  m_MissionReader.EnableVerbatimQuoting(false);
+  // m_MissionReader.EnableVerbatimQuoting(false);
-  if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) {
+  // if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) {
-    list<string>::iterator p;
+  //   list<string>::iterator p;
-    for(p=sParams.begin(); p!=sParams.end(); p++) {
+  //   for(p=sParams.begin(); p!=sParams.end(); p++) {
-      string line  = *p;
+  //     string line  = *p;
-      string param = tolower(biteStringX(line, '='));
+  //     string param = tolower(biteStringX(line, '='));
-      string value = line;
+  //     string value = line;
+      
+  //     if(param == "foo") {
+  //       //handled
+  //     }
+  //     else if(param == "bar") {
+  //       //handled
+  //     }
+  //   }
+  // }
 
-      if(param == "foo") {
-        //handled
-      }
-      else if(param == "bar") {
-        //handled
-      }
-    }
-  }
-  std::string saveLogDir;
   m_MissionReader.GetValue("LogDir", saveLogDir);
   std::string vehicleName;
   m_MissionReader.GetValue("VehicleName", vehicleName);
-  bool logEnable = false;
   m_MissionReader.GetValue("LogEnable", logEnable);
 
-  std::string auvDataFile;
+  // std::string auvDataFile;
-  m_MissionReader.GetValue("AuvDataLog", auvDataFile);  
+  // m_MissionReader.GetValue("AuvDataLog", auvDataFile);  
-  std::string missionHistoryFile;
+  // std::string missionHistoryFile;
-  m_MissionReader.GetValue("MissionHistoryLog", missionHistoryFile);    
+  // m_MissionReader.GetValue("MissionHistoryLog", missionHistoryFile);    
-  std::string clientCommandFile;
+  // std::string clientCommandFile;
-  m_MissionReader.GetValue("ClientCommandLog", clientCommandFile);  
+  // m_MissionReader.GetValue("ClientCommandLog", clientCommandFile);  
-  std::string faultLogFile;
+  // std::string faultLogFile;
-  m_MissionReader.GetValue("FaultLog", faultLogFile);  
+  // m_MissionReader.GetValue("FaultLog", faultLogFile);  
-  std::string motionControlFile;
+  // std::string motionControlFile;
-  m_MissionReader.GetValue("MotionControlLog", 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 )
+    OpenOutputStream("");
-    {
-      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);    
   }
 
   RegisterVariables();
@@ -489,6 +482,8 @@ void DataManagement::RegisterVariables()
   Register("DESIRED_SPEED", 0);
   Register("DESIRED_DEPTH", 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();
+  }
+}

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

src/pDataManagement/NavigationInfo.pb.h

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

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
+ * @LastEditors: zhaojingkui 1553836110@qq.com
- * @LastEditTime: 2023-11-21 15:28:32
+ * @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_leakSensor = 0x0;       //24:[46,47,48,49]
-   p_150server_1.embeddedInfoSrc.fault_battery = 0x2E;       //25:[50] 
+   p_150server_1.embeddedInfoSrc.fault_battery = 0x0;       //25:[50] 
-   p_150server_1.embeddedInfoSrc.fault_emergencyBattery = 0xAA;       //26:[51]
+   p_150server_1.embeddedInfoSrc.fault_emergencyBattery = 0x0;       //26:[51]
-   p_150server_1.embeddedInfoSrc.fault_thrust = 0x76;       //27:[52]
+   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]

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
+ * @LastEditors: zhaojingkui 1553836110@qq.com
- * @LastEditTime: 2023-11-10 08:34:14
+ * @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
 {

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
+ * @LastEditors: zhaojingkui 1553836110@qq.com
- * @LastEditTime: 2023-11-08 09:19:46
+ * @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");

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
+ * @LastEditors: zhaojingkui 1553836110@qq.com
- * @LastEditTime: 2023-11-07 17:12:30
+ * @LastEditTime: 2023-11-28 18:19:10
  * @FilePath: /moos-ivp-pi/src/pEmulator/_150server.hpp
  * @Description: 
  * 

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/)
 
@@ -27,6 +30,7 @@ TARGET_LINK_LIBRARIES(pStateManagement
    ${CMAKE_DL_LIBS}
    ${SYSTEM_LIBS} 
    ${DUNE_LIB} 
+   ${GeographicLib_LIBRARIES}       
    mbutil
    m
    pthread

src/pStateManagement/StateManagement.cpp

@@ -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;
-
+    std::string manualState;
-	  Json::Value deviceState;
+    int missionState;
-    double manualState;
-    double 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;
       }

src/pStateManagement/StateManagement.h

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