修改了状态管理代码

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;
-    double manualState;
-    double missionState;
+    double      manualState;
+    int         missionState;
 
     if(key == "uManual_enable_cmd")
     {
@@ -76,17 +75,17 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail)
     }
     else if (fabs(manualState - 0) < 1e-6) //manualState=0
     {     
-      if(missionState == 0)
+      switch (missionState)
       {
+      case 0:
         deviceState["opMode"] = opModeLists.error;
-      }
-      if(missionState == 1)
-      {
+        break;
+      case 1:
         deviceState["opMode"] = opModeLists.service;
-      }
-      else if((missionState == 3) )
-      {
+        break;
+      case 3:
         deviceState["opMode"] = opModeLists.maneuver;
+        break;
       }
     }
 

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
 {