删除了多余文件

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

launch/launch_local.moos

@@ -0,0 +1,328 @@
+//======================================
+//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 = false
+//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
+}
+

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,11 +45,10 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail)
     double mtime = msg.GetTime();
     bool   mdbl  = msg.IsDouble();
     bool   mstr  = msg.IsString();
-#endif
 
-	  Json::Value deviceState;
+    Json::Value deviceState;
     std::string manualState;
-    double missionState;
+    int missionState;
 
     if(key == "uManual_enable_cmd")
     {
@@ -84,7 +83,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
 {