迁移pi上的master分支

src/CMakeLists.txt

@@ -0,0 +1,29 @@
+##############################################################################
+# FILE:        moos-ivp-extend/src/CMakeLists.txt
+# DATE:        2010/09/07
+#              2020/05/09 minor mods
+# DESCRIPTION: CMakeLists.txt file for the moos-ivp-extend source directory
+##############################################################################
+
+
+#============================================================================
+# Add the libraries in the current directory to the include path
+#============================================================================
+FILE(GLOB LOCAL_LIBRARY_DIRS ./lib_*)
+INCLUDE_DIRECTORIES(${LOCAL_LIBRARY_DIRS}/* ${LOCAL_LIBRARY_DIRS}/DUNE/)
+
+#============================================================================
+# List the subdirectories to build...
+#============================================================================
+ADD_SUBDIRECTORY(pBoardSupportComm)
+ADD_SUBDIRECTORY(pSurfaceSupportComm)
+ADD_SUBDIRECTORY(pClientViewer)
+ADD_SUBDIRECTORY(pDataManagement)
+ADD_SUBDIRECTORY(pTaskManger)
+ADD_SUBDIRECTORY(pTaskSend)
+ADD_SUBDIRECTORY(pMotionControler)
+ADD_SUBDIRECTORY(pEmulator)
+##############################################################################
+#                           END of CMakeLists.txt
+##############################################################################
+

src/pBoardSupportComm/BoardSupportComm.cpp

@@ -0,0 +1,785 @@
+/************************************************************/
+/*    NAME: Xiaobin Zeng                                              */
+/*    ORGN: MIT                                             */
+/*    FILE: BoardSupportComm.cpp                                        */
+/*    DATE:                                                 */
+/************************************************************/
+
+#include <iterator>
+#include "BoardSupportComm.h"
+#include <cmath>
+
+
+#define TCP_RECEIVE_PORT 8001
+#define TCP_SERVER_ADDRESS  "127.0.0.1"
+
+// #define MOOS_AUV_SIM
+#define MATLAB_AUV_SIM
+// #ifdef TRUE_AUV
+
+using namespace std;
+
+BoardSupportComm::BoardSupportComm()
+{
+  // uMotion_posX_fb = 0;
+  // uMotion_posY_fb = 0;
+  // uMotion_posZ_fb = 0;
+  // uMotion_roll_fb = 0;
+  // uMotion_pitch_fb = 0;
+  // uMotion_yaw_fb = 0;
+  // uMotion_longitude_fb = 0;
+  // uMotion_latitude_fb = 0;
+  // uMotion_velocityX_fb = 0;
+  // uMotion_velocityY_fb = 0;
+  // uMotion_velocityZ_fb = 0;
+  // uMotion_deep_fb = 0;
+
+  estimatedState.info = "lauv-150";
+  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;
+
+  executeCommand.header = 0xEBA2;               //1:[0,1]
+  executeCommand.count = 16;                    //2:[2,3]
+  executeCommand.size = 21;                     //3:[4]
+  executeCommand.drive_mode = 0xFF;            //4:[5]
+  executeCommand.thrust = 0;                 //5:[6]
+  executeCommand.yaw = 0;                       //6:[7,8]
+  executeCommand.depth = 0;                     //7:[9,10]
+  executeCommand.helm_top_angle = 0;         //8:[11]
+  executeCommand.helm_bottom_angle = 0;      //9:[12]
+  executeCommand.helm_left_angle = 0;          //10:[13]
+  executeCommand.helm_right_angle = 0;       //11:[14]
+  executeCommand.light_enable = 0;             //12:[15]
+  executeCommand.dvl_enable = 0;               //13:[16]
+  executeCommand.throwing_load_enable = 0;    //14:[17]
+  executeCommand.crc = 0;                     //15:[18]
+  executeCommand.footer = 0xEE2A;             //16:[19,20]
+  executeCommand.manual_mode = false;
+  tcpSockFD = -1;
+  tcpConnectRet = -1;
+}
+
+//---------------------------------------------------------
+// Destructor
+
+BoardSupportComm::~BoardSupportComm()
+{
+  // delete tcpReceiveBuffer;
+  close(tcpSockFD);
+
+}
+
+//---------------------------------------------------------
+// Procedure: OnNewMail
+
+bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
+{
+  // AppCastingMOOSApp::OnNewMail(NewMail);
+  MOOSMSG_LIST::iterator p;
+
+  for(p=NewMail.begin(); p!=NewMail.end(); p++)
+  {
+    CMOOSMsg &msg = *p;
+    string key   = msg.GetKey();
+    string comm  = msg.GetCommunity();
+    double dval  = msg.GetDouble();
+    string sval  = msg.GetString(); 
+    string msrc  = msg.GetSource();
+    double mtime = msg.GetTime();
+    bool   mdbl  = msg.IsDouble();
+    bool   mstr  = msg.IsString();
+
+#ifdef MOOS_AUV_SIM
+    if(key == "NAV_X")
+    {
+   	  //E->N
+      estimatedState.offsetNorth = msg.GetDouble();      
+    }
+    if(key == "NAV_Y")
+	  {
+   	  //N->E
+      estimatedState.offsetEast = msg.GetDouble();       
+	  }
+    if(key == "NAV_Z")
+	  {
+   	  //U->D
+      estimatedState.offsetDown = -msg.GetDouble();
+	  }
+    if(key == "NAV_YAW")
+	  {
+      estimatedState.yaw = -msg.GetDouble();
+	  }
+    if(key == "NAV_PITCH")
+	  {
+      estimatedState.pitch = msg.GetDouble();
+	  }
+    if(key == "NAV_LAT")
+	  {
+      estimatedState.currentLat = msg.GetDouble();
+	  }
+    if(key == "NAV_LONG")
+	  {
+      estimatedState.currentLon = msg.GetDouble();
+	  }
+    if(key == "NAV_SPEED")
+	  {
+      estimatedState.linearVelocityNorth = msg.GetDouble() * cos(estimatedState.yaw);
+      estimatedState.linearVelocityEast = -msg.GetDouble() * sin(estimatedState.yaw);
+      estimatedState.linearVelocityDown = 0;
+	  }
+    if(key == "NAV_DEPTH")
+	  {
+      estimatedState.depth = msg.GetDouble();
+	  }
+#endif
+
+
+    if(key == "uManual_enable_cmd")
+    {
+      if (msg.GetDouble() == 1.0)
+      {
+        executeCommand.drive_mode = 0x02;
+        executeCommand.manual_mode = true;
+      }
+      else
+      {
+        executeCommand.drive_mode = 0xFF;
+        executeCommand.manual_mode = false;
+
+      }
+
+    }
+
+    if(key == "uManual_drive_cmd")
+	  {
+      if (executeCommand.manual_mode)
+      {
+        std::string err;
+        Json::Value recvCommand;
+        std::istringstream iss(msg.GetString());  
+        Json::CharReaderBuilder builder;  
+        bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err);  
+        if (!parsingResult) 
+        {  
+            std::cerr << "Failed to parse JSON string." << std::endl;  
+            return false;  
+        }
+        int thrust = recvCommand["Thrust"].asInt();  
+        if ((thrust < -100) or (thrust > 100))
+        {
+          return false;
+        }
+        executeCommand.thrust = convertIntToUchar(recvCommand["Thrust"].asInt(), -100, 100);
+        float heading = recvCommand["Heading"].asFloat();
+        if ((heading >= 0) && (heading <= 180))
+        {
+          executeCommand.yaw = heading * 10;
+        }
+
+        if ((heading < 0) && (heading >= -180))
+        {
+          executeCommand.yaw = (360 + heading) * 10;
+        }
+        if ((heading > 180) or (heading < -180))
+        {
+          executeCommand.yaw = 180 * 10;          
+        }           
+        executeCommand.depth = 0;
+        executeCommand.helm_top_angle = 0;
+        executeCommand.helm_bottom_angle = 0;
+        executeCommand.helm_left_angle = 0;
+        executeCommand.helm_right_angle = 0;
+        int serializeResult = serializeMessage(tcpSendBuffer);
+        if ((serializeResult == 0) && (tcpSockFD != -1))
+        {
+          try
+          {
+            write(tcpSockFD, tcpSendBuffer, executeCommand.size);
+          }
+          catch(const std::exception& e)
+          {
+            std::cerr << e.what() << '\n';
+          }
+        }
+      }
+	  }
+    if(key == "uMotion_control_cmd")
+	  {
+      if (executeCommand.manual_mode)
+      {
+        return false;
+      }      
+      // if (executeCommand.drive_mode != 0x02)
+      // {
+      //   return false;
+      // } 
+      std::string err;
+      Json::Value recvCommand;
+      std::istringstream iss(msg.GetString());  
+      Json::CharReaderBuilder builder;  
+      bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err);  
+      if (!parsingResult) 
+      {  
+          std::cerr << "Failed to parse JSON string." << std::endl;  
+          return false;  
+      }
+      executeCommand.thrust = convertIntToUchar(recvCommand["thrust"].asInt(), -100, 100);
+      executeCommand.helm_top_angle = convertIntToUchar(recvCommand["rudder"].asInt(), -30, 30);
+      executeCommand.helm_bottom_angle = convertIntToUchar(recvCommand["rudder"].asInt(), -30, 30);
+      executeCommand.helm_left_angle = convertIntToUchar(recvCommand["elevator"].asInt(), -30, 30);
+      executeCommand.helm_right_angle = convertIntToUchar(recvCommand["elevator"].asInt(), -30, 30);
+
+      int serializeResult = serializeMessage(tcpSendBuffer);
+      // std::cout << "serializeResult: " << serializeResult << std::endl;
+      // printf("header: %u, %u\n", tcpSendBuffer[0], tcpSendBuffer[1]);
+      // printf("count: %u\n", tcpSendBuffer[2], tcpSendBuffer[3]);
+      // printf("size: %u\n", tcpSendBuffer[4]);
+      // printf("drive_mode: %u\n", tcpSendBuffer[5]);
+      // printf("thrust: %u\n", tcpSendBuffer[6]);
+      // printf("yaw: %u, %u\n", tcpSendBuffer[7], tcpSendBuffer[8]);
+      // printf("depth: %u, %u\n", tcpSendBuffer[9], tcpSendBuffer[10]); 
+      // printf("helm_top_angle: %u\n", tcpSendBuffer[11]);
+      // printf("helm_bottom_angle: %u\n", tcpSendBuffer[12]);
+      // printf("helm_left_angle: %u\n", tcpSendBuffer[13]);
+      // printf("helm_right_angle: %u\n", tcpSendBuffer[14]);
+      // printf("light_enable: %u\n", tcpSendBuffer[15]);
+      // printf("dvl_enable: %u\n", tcpSendBuffer[16]);
+      // printf("throwing_load_enable: %u\n", tcpSendBuffer[17]);
+      // printf("crc: %u\n", tcpSendBuffer[18]);
+      // printf("footer: %u, %u\n", tcpSendBuffer[19], tcpSendBuffer[20]);
+      if ((serializeResult == 0) && (tcpSockFD != -1))
+      {
+        try
+        {
+          write(tcpSockFD, tcpSendBuffer, executeCommand.size);
+        }
+        catch(const std::exception& e)
+        {
+          std::cerr << e.what() << '\n';
+        }
+      }      
+    }
+    if(key == "uMission_origin_cmd")
+	  {
+      std::string err;
+      Json::Value recvCommand;
+      std::istringstream iss(msg.GetString());  
+      Json::CharReaderBuilder builder;  
+      bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err);  
+      if (!parsingResult) 
+      {  
+          std::cerr << "Failed to parse JSON string." << std::endl;  
+          return false;  
+      }
+      estimatedState.referenceAltitude = recvCommand["altitude"].asFloat();
+      estimatedState.referenceLat = recvCommand["lat"].asFloat();
+      estimatedState.referenceLon = recvCommand["lon"].asFloat();
+    }
+
+
+  }
+
+  return(true);
+}
+
+uint8_t BoardSupportComm::convertIntToUchar(int src, int min, int max)
+{
+  uint8_t dst;
+  if (src < 0)
+  {
+    if (src < min)
+    {
+      dst  = std::abs(min) + 0B10000000;      
+    }
+    else
+    {
+      dst  = std::abs(src) + 0B10000000;
+    }
+  }
+  else
+  {
+    if (src > max)
+    {
+      dst  = std::abs(max);
+    }
+    else
+    {
+      dst  = std::abs(src);
+    }
+
+  }
+  return dst; 
+}
+
+bool BoardSupportComm::OnConnectToServer()
+{
+  RegisterVariables();
+  return(true);
+}
+
+std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState)
+{
+
+
+#ifdef MOOS_AUV_SIM
+  embeddedState["driveMode"] = 0xFF;
+  embeddedState["height"] = 0;
+  embeddedState["depth"] = 0;
+  embeddedState["yaw"] = estimatedState.yaw;
+  embeddedState["pitch"] = 0;
+  embeddedState["roll"] = 0;
+  embeddedState["insVX"] = estimatedState.linearVelocityNorth * 0.01;
+  embeddedState["insVY"] = estimatedState.linearVelocityEast * 0.01;
+  embeddedState["insVZ"] = estimatedState.linearVelocityDown * 0.01;
+  embeddedState["currentLon"] = estimatedState.currentLon * 0.000001;
+  embeddedState["currentLat"] = estimatedState.currentLat * 0.000001;
+  embeddedState["currentAltitude"] = 0;
+  embeddedState["dvlVX"] = 0;
+  embeddedState["dvlVY"] = 0;
+  embeddedState["dvlVZ"] = 0;
+  embeddedState["rpm"] = 0;
+  embeddedState["lightEnable"] = 0;
+  embeddedState["batteryVoltage"] = 0;
+  embeddedState["batteryLevel"] = 0;
+  embeddedState["batteryTemp"] = 0;
+  embeddedState["faultLeakSensor"] = 0;
+  embeddedState["faultBattery"] = 0;
+  embeddedState["faultEmergencyBattery"] = 0;
+  embeddedState["faultThrust"] = 0;
+  embeddedState["iridium"] = 0xFF;
+  embeddedState["throwingLoadEnable"] = 0xFF;
+  embeddedState["dvlStatus"] = 0;
+#endif
+
+#ifdef MATLAB_AUV_SIM
+  embeddedState["driveMode"] = embeddedInfo.drive_mode;
+  embeddedState["height"] = embeddedInfo.height * 0.01;
+  embeddedState["depth"] = embeddedInfo.depth * 0.01;
+  embeddedState["roll"] = embeddedInfo.roll * 0.01 * M_PI / 180; //N->N
+  embeddedState["pitch"] = embeddedInfo.pitch * 0.01 * M_PI / 180; //E->E    
+  embeddedState["yaw"] = embeddedInfo.yaw * 0.01 * M_PI / 180;    //D->D
+  embeddedState["insVX"] = embeddedInfo.ins_vx * 0.01; //N->N
+  embeddedState["insVY"] = embeddedInfo.ins_vy * 0.01; //E->E
+  embeddedState["insVZ"] = embeddedInfo.ins_vz * 0.01; //D->D  
+  embeddedState["currentLon"] = embeddedInfo.lon * 0.000001;
+  embeddedState["currentLat"] = embeddedInfo.lat * 0.000001;
+  embeddedState["currentAltitude"] = embeddedInfo.alt * 0.01;
+  embeddedState["dvlVX"] = embeddedInfo.dvl_vx * 0.01;    //N->N
+  embeddedState["dvlVY"] = embeddedInfo.dvl_vy * 0.01;    //E->E
+  embeddedState["dvlVZ"] = embeddedInfo.dvl_vz * 0.01;   //D->D
+  embeddedState["rpm"] = embeddedInfo.rpm;
+  embeddedState["lightEnable"] = embeddedInfo.light_enable;
+  embeddedState["batteryVoltage"] = embeddedInfo.battery_voltage;
+  embeddedState["batteryLevel"] = embeddedInfo.battery_level;
+  embeddedState["batteryTemp"] = embeddedInfo.battery_temp * 0.1;
+  embeddedState["faultLeakSensor"] = embeddedInfo.fault_leakSensor;
+  embeddedState["faultBattery"] = embeddedInfo.fault_battery;
+  embeddedState["faultEmergencyBattery"] = embeddedInfo.fault_emergencyBattery;
+  embeddedState["faultThrust"] = embeddedInfo.fault_thrust;
+  embeddedState["iridium"] = embeddedInfo.iridium;
+  embeddedState["throwingLoadEnable"] = embeddedInfo.throwing_load;
+  embeddedState["dvlStatus"] = embeddedInfo.dvl_status;
+#endif
+
+#ifdef TRUE_AUV
+  embeddedState["driveMode"] = embeddedInfo.drive_mode;
+  embeddedState["height"] = embeddedInfo.height * 0.01;
+  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["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
+  embeddedState["currentLon"] = embeddedInfo.lon * 0.000001;
+  embeddedState["currentLat"] = embeddedInfo.lat * 0.000001;
+  embeddedState["currentAltitude"] = embeddedInfo.alt * 0.01;
+  embeddedState["dvlVX"] = embeddedInfo.dvl_vy * 0.01;    //E->N
+  embeddedState["dvlVY"] = embeddedInfo.dvl_vx * 0.01;    //N->E
+  embeddedState["dvlVZ"] = -embeddedInfo.dvl_vz * 0.01;   //U->D
+  embeddedState["rpm"] = embeddedInfo.rpm;
+  embeddedState["lightEnable"] = embeddedInfo.light_enable;
+  embeddedState["batteryVoltage"] = embeddedInfo.battery_voltage;
+  embeddedState["batteryLevel"] = embeddedInfo.battery_level;
+  embeddedState["batteryTemp"] = embeddedInfo.battery_temp * 0.1;
+  embeddedState["faultLeakSensor"] = embeddedInfo.fault_leakSensor;
+  embeddedState["faultBattery"] = embeddedInfo.fault_battery;
+  embeddedState["faultEmergencyBattery"] = embeddedInfo.fault_emergencyBattery;
+  embeddedState["faultThrust"] = embeddedInfo.fault_thrust;
+  embeddedState["iridium"] = embeddedInfo.iridium;
+  embeddedState["throwingLoadEnable"] = embeddedInfo.throwing_load;
+  embeddedState["dvlStatus"] = embeddedInfo.dvl_status;
+#endif
+
+  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> 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);
+
+
+  Json::StreamWriterBuilder builder;
+  std::string embeddedStateString = Json::writeString(builder, embeddedState);
+  return embeddedStateString;
+}
+
+
+void BoardSupportComm::tcpProcessThread()
+{
+  while(1)
+  {
+    //bzero(tcpReceiveBuffer, 0);
+    //int lens = read(tcpSockFD, (unsigned char* )tcpReceiveBuffer, sizeof(tcpReceiveBuffer));
+    int lens = read(tcpSockFD, tcpReceiveBuffer, sizeof(tcpReceiveBuffer));
+    // std::cout << MOOS::Time() << " recv:" << lens << std::endl;
+    if(lens>0)
+    {
+      parseMessage((unsigned char* )tcpReceiveBuffer, lens);
+      // parseMessage(tcpReceiveBuffer, lens);
+
+      Json::Value embeddedState;
+      std::string embeddedStateString = convertEmbeddedFormat(embeddedState);
+      // std::cout << std::fixed << std::setprecision(6) << MOOS::Time() << embeddedStateString << std::endl;
+      Notify("uDevice_monitor_fb", embeddedStateString);
+      Notify("NAV_X", embeddedState["north"].asDouble());
+      Notify("NAV_Y", embeddedState["east"].asDouble());
+      Notify("NAV_Z", embeddedState["depth"].asDouble()); 
+      Notify("NAV_LAT", embeddedState["currentLat"].asDouble());
+      Notify("NAV_LONG", embeddedState["currentLon"].asDouble());
+      Notify("NAV_HEADING", embeddedState["yaw"].asDouble() * 180 / M_PI);
+      double driveSpeed = std::sqrt(std::pow((double)embeddedState["dvlVX"].asDouble(), 2) 
+                          + std::pow((double)embeddedState["dvlVY"].asDouble(), 2)
+                          + std::pow((double)embeddedState["dvlVZ"].asDouble(), 2));
+      Notify("NAV_SPEED", driveSpeed);
+      Notify("NAV_DEPTH", embeddedState["depth"].asDouble());
+      Notify("NAV_ROLL", embeddedState["roll"].asDouble() * 180 / M_PI); 
+      Notify("NAV_PITCH", embeddedState["pitch"].asDouble() * 180 / M_PI);
+      Notify("NAV_YAW", embeddedState["yaw"].asDouble() * 180 / M_PI);
+    }
+  }
+}
+
+// bool BoardSupportComm::buildReport()
+// {
+//   m_msgs << "buildReport:" << embeddedStateString << endl;
+//   return true;
+// }
+
+bool BoardSupportComm::Iterate()
+{
+  // AppCastingMOOSApp::Iterate();
+
+  if(tcpSockFD == -1)
+  {
+    tcpSockFD = socket(AF_INET, SOCK_STREAM, 0);
+    if(tcpSockFD == -1)
+    {
+      return false;
+    }
+  }
+
+  if(tcpConnectRet == -1)
+  { 
+    struct sockaddr_in saddr;
+    //inet_pton(AF_INET, TCP_SERVER_ADDRESS, &saddr.sin_addr.s_addr);
+    saddr.sin_addr.s_addr = inet_addr(TCP_SERVER_ADDRESS);
+    saddr.sin_family = AF_INET;
+    saddr.sin_port = htons(TCP_RECEIVE_PORT);
+    tcpConnectRet = connect(tcpSockFD, (struct sockaddr *)&saddr, sizeof(saddr));
+    if(tcpConnectRet == -1)
+    {
+      return false;
+    }
+  }
+  if ((tcpSockFD != -1) && (tcpConnectRet != -1))
+  {
+    std::thread t1(&BoardSupportComm::tcpProcessThread, this);
+    t1.detach();
+  }
+
+
+#if 0
+  printf("header: %hx\n", embeddedInfo.header);
+  printf("count: %hu\n", embeddedInfo.count);
+  printf("size: %u\n", embeddedInfo.size);
+  printf("drive_mode: %u\n", embeddedInfo.drive_mode);
+  printf("height: %hu\n", embeddedInfo.height);
+  printf("depth: %hu\n", embeddedInfo.depth);
+  printf("yaw: %hu\n", embeddedInfo.yaw);
+  printf("pitch: %hd\n", embeddedInfo.pitch);
+  printf("roll: %hd\n", embeddedInfo.roll);
+  printf("ins_vx: %hd\n", embeddedInfo.ins_vx);
+  printf("ins_vy: %hd\n", embeddedInfo.ins_vy);
+  printf("ins_vz: %hd\n", embeddedInfo.ins_vz);
+  printf("lon: %d\n", embeddedInfo.lon);
+  printf("lat: %d\n", embeddedInfo.lat);
+  printf("alt: %hd\n", embeddedInfo.alt);
+  printf("dvl_vx: %hd\n", embeddedInfo.dvl_vx);
+  printf("dvl_vy: %hd\n", embeddedInfo.dvl_vy);
+  printf("dvl_vz: %hd\n", embeddedInfo.dvl_vz);
+  printf("rpm: %hd\n", embeddedInfo.rpm);
+  printf("light_enable: %u\n", embeddedInfo.light_enable);
+  printf("battery_voltage: %u\n", embeddedInfo.battery_voltage);
+  printf("battery_level: %u\n", embeddedInfo.battery_level);
+  printf("battery_temp: %u\n", embeddedInfo.battery_temp);
+  printf("fault_leakSensor: %hd\n", embeddedInfo.fault_leakSensor);
+  printf("fault_battery: %u\n", embeddedInfo.fault_battery);
+  printf("fault_emergencyBattery: %u\n", embeddedInfo.fault_emergencyBattery);
+  printf("fault_thrust: %u\n", embeddedInfo.fault_thrust);
+  printf("iridium: %u\n", embeddedInfo.iridium);
+  printf("throwing_load: %u\n", embeddedInfo.throwing_load);
+  printf("dvl_status: %u\n", embeddedInfo.dvl_status);
+  printf("crc: %u\n", embeddedInfo.crc);
+  printf("footer: %u\n", embeddedInfo.footer);
+#endif
+
+#if 0
+  executeCommand.header = 0xEBA2;               //1:[0,1]
+  executeCommand.count = 16;                    //2:[2,3]
+  executeCommand.size = 21;                     //3:[4]
+  executeCommand.drive_mode = 0x02;            //4:[5]
+  executeCommand.thrust = 178;                 //5:[6],赋值-50
+  executeCommand.yaw = 45.3 * 10;             //6:[7,8],赋值45.3
+  executeCommand.depth = 298.6 * 10;          //7:[9,10],赋值298.6
+  executeCommand.helm_top_angle = 145;         //8:[11],赋值-17
+  executeCommand.helm_bottom_angle = 154;      //9:[12],赋值-26
+  executeCommand.helm_left_angle = 9;          //10:[13],赋值9
+  executeCommand.helm_right_angle = 158;       //11:[14],赋值-30
+  executeCommand.light_enable = 1;             //12:[15],赋值1
+  executeCommand.dvl_enable = 1;               //13:[16],赋值1
+  executeCommand.throwing_load_enable = 1;    //14:[17],赋值1
+  executeCommand.crc = 0;                     //15:[18]
+  executeCommand.footer = 0xEE2A;             //16:[19,20]
+
+  int serializeResult = serializeMessage(tcpSendBuffer);
+  if (serializeResult == 0)
+  {
+    try
+    {
+      write(tcpSockFD, tcpSendBuffer, executeCommand.size);
+      // sock_tcp_send.write(tcpSendBuffer, executeCommand.size);
+    }
+    catch(const std::exception& e)
+    {
+      std::cerr << e.what() << '\n';
+    }
+  }
+#endif
+
+  //AppCastingMOOSApp::PostReport();
+
+  return true;
+}
+
+uint16_t BoardSupportComm::imcPollTCP(char* buffer, int bufferLength)
+{
+  if (m_poll_1.poll(0))
+	{
+    bzero(buffer, 0);
+    uint16_t rv = sock_tcp_send.read(buffer, bufferLength);
+    return rv;
+  }
+
+  return 0;
+}
+
+template <typename Type>
+inline uint16_t BoardSupportComm::serialize(const Type t, uint8_t* bfr, uint16_t& sumSize)
+{
+  uint16_t size = sizeof(Type);
+  std::memcpy(bfr, &t, size);
+  sumSize += size;
+  return size;
+}
+
+int BoardSupportComm::serializeMessage(unsigned char* buffer)
+{
+  uint8_t* ptr = buffer;
+  uint16_t sumSize = 0;
+  ptr += serialize(executeCommand.header, ptr, sumSize);
+  ptr += serialize(executeCommand.count, ptr, sumSize);
+  ptr += serialize(executeCommand.size, ptr, sumSize);
+  ptr += serialize(executeCommand.drive_mode, ptr, sumSize);  
+  ptr += serialize(executeCommand.thrust, ptr, sumSize);
+  ptr += serialize(executeCommand.yaw, ptr, sumSize);
+  ptr += serialize(executeCommand.depth, ptr, sumSize);
+  ptr += serialize(executeCommand.helm_top_angle, ptr, sumSize);
+  ptr += serialize(executeCommand.helm_bottom_angle, ptr, sumSize);
+  ptr += serialize(executeCommand.helm_left_angle, ptr, sumSize);
+  ptr += serialize(executeCommand.helm_right_angle, ptr, sumSize);
+  ptr += serialize(executeCommand.light_enable, ptr, sumSize);
+  ptr += serialize(executeCommand.dvl_enable, ptr, sumSize);
+  ptr += serialize(executeCommand.throwing_load_enable, ptr, sumSize);
+  DUNE::Algorithms::CRC8 crc(0x07);
+  crc.putArray(buffer+CRC_COMPUTE_START_POS, CRC_COMPUTE_SUM_SIZE);
+  executeCommand.crc = crc.get();
+  ptr += serialize(executeCommand.crc, ptr, sumSize);
+  ptr += serialize(executeCommand.footer, ptr, sumSize);
+  if (sumSize != (uint16_t)executeCommand.size)
+  {
+    return -1;
+  }
+  return 0;
+}
+
+template <typename Type>
+inline uint16_t BoardSupportComm::deserialize(Type& t, const uint8_t* bfr, uint16_t& length)
+{
+  uint16_t size = sizeof(Type);
+
+  std::memcpy(&t, bfr, size);
+  length -= size;
+
+  return size;
+}
+
+int BoardSupportComm::parseMessage(unsigned char* buffer, int size)
+{
+  const uint8_t* ptr = buffer;
+  uint16_t size__ = size;
+  uint16_t header;
+  ptr += deserialize(embeddedInfo.header, ptr, size__);
+  ptr += deserialize(embeddedInfo.count, ptr, size__);
+  ptr += deserialize(embeddedInfo.size, ptr, size__);
+  ptr += deserialize(embeddedInfo.drive_mode, ptr, size__);
+  ptr += deserialize(embeddedInfo.height, ptr, size__);
+  ptr += deserialize(embeddedInfo.depth, ptr, size__);
+  ptr += deserialize(embeddedInfo.yaw, ptr, size__);
+  ptr += deserialize(embeddedInfo.pitch, ptr, size__);
+  ptr += deserialize(embeddedInfo.roll, ptr, size__);
+  ptr += deserialize(embeddedInfo.ins_vx, ptr, size__);
+  ptr += deserialize(embeddedInfo.ins_vy, ptr, size__);
+  ptr += deserialize(embeddedInfo.ins_vz, ptr, size__);
+  ptr += deserialize(embeddedInfo.lon, ptr, size__);
+  ptr += deserialize(embeddedInfo.lat, ptr, size__);
+  ptr += deserialize(embeddedInfo.alt, ptr, size__);
+  ptr += deserialize(embeddedInfo.dvl_vx, ptr, size__);
+  ptr += deserialize(embeddedInfo.dvl_vy, ptr, size__);
+  ptr += deserialize(embeddedInfo.dvl_vz, ptr, size__);
+  ptr += deserialize(embeddedInfo.rpm, ptr, size__);
+  ptr += deserialize(embeddedInfo.light_enable, ptr, size__);
+  ptr += deserialize(embeddedInfo.battery_voltage, ptr, size__);
+  ptr += deserialize(embeddedInfo.battery_level, ptr, size__);
+  ptr += deserialize(embeddedInfo.battery_temp, ptr, size__);
+  ptr += deserialize(embeddedInfo.fault_leakSensor, ptr, size__);
+  ptr += deserialize(embeddedInfo.fault_battery, ptr, size__);
+  ptr += deserialize(embeddedInfo.fault_emergencyBattery, ptr, size__);
+  ptr += deserialize(embeddedInfo.fault_thrust, ptr, size__);
+  ptr += deserialize(embeddedInfo.iridium, ptr, size__);
+  ptr += deserialize(embeddedInfo.throwing_load, ptr, size__);
+  ptr += deserialize(embeddedInfo.dvl_status, ptr, size__);
+  ptr += deserialize(embeddedInfo.crc, ptr, size__);
+  ptr += deserialize(embeddedInfo.footer, ptr, size__);
+
+  return 0;
+}
+
+bool BoardSupportComm::OnStartUp()
+{
+  // AppCastingMOOSApp::OnStartUp();
+
+  m_MissionReader.GetValue("LongOrigin", estimatedState.referenceLon);
+  m_MissionReader.GetValue("LatOrigin", estimatedState.referenceLat);
+  m_MissionReader.GetValue("AltOrigin", estimatedState.referenceAltitude);
+
+  std::cout << "BoardSupportComm OnStartUp: " << estimatedState.referenceLon << ", "
+                                               << estimatedState.referenceLat << ", "
+                                               << estimatedState.referenceAltitude << std::endl;
+
+  RegisterVariables();
+  return(true);
+}
+
+void BoardSupportComm::RegisterVariables()
+{
+  // AppCastingMOOSApp::RegisterVariables();
+
+#ifdef MOOS_AUV_SIM
+  Register("NAV_X", 0);
+  Register("NAV_Y", 0);
+  Register("NAV_Z", 0);
+  Register("NAV_LAT", 0);
+  Register("NAV_LONG", 0);
+  Register("NAV_SPEED", 0);
+  Register("NAV_DEPTH", 0);
+  Register("NAV_YAW", 0);
+  Register("NAV_PITCH", 0);
+#endif
+  Register("uManual_drive_cmd", 0);
+  Register("uMotion_control_cmd", 0);
+  Register("uManual_enable_cmd", 0);
+  Register("uMission_origin_cmd", 0); 
+}
+
+void BoardSupportComm::ConvertLLAToENU(std::vector<double> init_lla,
+                            std::vector<double> point_lla,
+                            std::vector<double>& point_enu)
+{
+  static GeographicLib::LocalCartesian local_cartesian;
+  local_cartesian.Reset(init_lla.at(1), init_lla.at(0), init_lla.at(2));
+  local_cartesian.Forward(point_lla.at(1), point_lla.at(0), point_lla.at(2),
+                          point_enu.at(0), point_enu.at(1), point_enu.at(2));
+}
+
+void BoardSupportComm::ConvertENUToLLA(std::vector<double> init_lla,
+                            std::vector<double> point_enu,
+                            std::vector<double> &point_lla)
+{
+  static GeographicLib::LocalCartesian local_cartesian;
+  local_cartesian.Reset(init_lla.at(1), init_lla.at(0), init_lla.at(2));
+  local_cartesian.Reverse(point_enu.at(0), point_enu.at(1), point_enu.at(2),
+                          point_lla.at(1), point_lla.at(0), point_lla.at(2));
+}
+
+
+void BoardSupportComm::ConvertLLAToNED(std::vector<double> init_lla,
+                            std::vector<double> point_lla,
+                            std::vector<double>& point_ned)
+{
+  static GeographicLib::LocalCartesian local_cartesian;
+  local_cartesian.Reset(init_lla.at(1), init_lla.at(0), init_lla.at(2));
+
+  double point_e = 0;
+  double point_n = 0;
+  double point_u = 0;
+  local_cartesian.Forward(point_lla.at(1), point_lla.at(0), point_lla.at(2),
+                          point_e, point_n, point_u);
+  point_ned.at(0) = point_n;
+  point_ned.at(1) = point_e;
+  point_ned.at(2) = -point_u;
+}
+
+
+void BoardSupportComm::ConvertNEDToLLA(std::vector<double> init_lla,
+                            std::vector<double> point_ned,
+                            std::vector<double> &point_lla)
+{
+  static GeographicLib::LocalCartesian local_cartesian;
+  local_cartesian.Reset(init_lla.at(1), init_lla.at(0), init_lla.at(2));
+
+  double point_e = point_ned.at(1);
+  double point_n = point_ned.at(0);
+  double point_u = -point_ned.at(2);
+
+  local_cartesian.Reverse(point_e, point_n, point_u,
+                          point_lla.at(1), point_lla.at(0), point_lla.at(2));
+}

src/pBoardSupportComm/BoardSupportComm.h

@@ -0,0 +1,171 @@
+/************************************************************/
+/*    NAME: Xiaobin Zeng                                              */
+/*    ORGN: MIT                                             */
+/*    FILE: BoardSupportComm.h                                          */
+/*    DATE:                                                 */
+/************************************************************/
+
+#ifndef BoardSupportComm_HEADER
+#define BoardSupportComm_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>
+
+#define CRC_COMPUTE_START_POS 5
+#define CRC_COMPUTE_SUM_SIZE 13
+
+struct AUVEmbedded
+{
+  uint16_t header;    //1:[0,1]
+  uint16_t count;     //2:[2,3]
+  uint8_t size;       //3:[4]
+  uint8_t drive_mode;       //4:[5]
+  uint16_t height;       //5:[6,7]
+  uint16_t depth;       //6:[8,9]
+  uint16_t yaw;       //7:[10,11]
+  int16_t pitch;       //8:[12,13]
+  int16_t roll;       //9:[14,15]
+  int16_t ins_vx;       //10:[16,17]
+  int16_t ins_vy;       //11:[18,19]
+  int16_t ins_vz;       //12:[20,21]
+  int32_t lon;       //13:[22,23,24,25]
+  int32_t lat;       //14:[26,27,28,29]
+  int16_t alt;       //15:[30,31]
+  int16_t dvl_vx;       //16:[32,33]
+  int16_t dvl_vy;       //17:[34,35]
+  int16_t dvl_vz;       //18:[36,37]
+  int16_t rpm;       //19:[38,39]
+  uint8_t light_enable;       //20:[40]
+  uint16_t battery_voltage;       //21:[41,42]
+  uint8_t battery_level;       //22:[43]
+  uint16_t battery_temp;       //23:[44,45]
+  uint32_t fault_leakSensor;       //24:[46,47,48,49]
+  uint8_t fault_battery;       //25:[50] 
+  uint8_t fault_emergencyBattery;       //26:[51]
+  uint8_t fault_thrust;       //27:[52]
+  uint8_t iridium;       //28:[53]
+  uint8_t throwing_load;       //29:[54]
+  uint8_t dvl_status;       //30:[55]
+  uint8_t crc;       //31:[56]
+  uint16_t footer;       //32:[57,58]
+};
+
+struct AUVExecuteCommand
+{
+  uint16_t header;    //1:[0,1]
+  uint16_t count;     //2:[2,3]
+  uint8_t size;       //3:[4]
+  uint8_t drive_mode;  //4:[5]
+  uint8_t thrust;       //5:[6]
+  uint16_t yaw;       //6:[7,8]
+  uint16_t depth;       //7:[9,10]    
+  uint8_t helm_top_angle;   //8:[11]  
+  uint8_t helm_bottom_angle;//9:[12]
+  uint8_t helm_left_angle;  //10:[13]
+  uint8_t helm_right_angle;//11:[14]
+  uint8_t light_enable;  //12:[15]
+  uint8_t dvl_enable;     //13:[16]
+  uint8_t throwing_load_enable;    //14:[17]
+  uint8_t crc;                     //15:[18]
+  uint16_t footer;       //16:[19,20]
+  bool manual_mode;
+};
+
+struct EstimatedState {
+  std::string info;
+  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  
+  bool OnNewMail(MOOSMSG_LIST &NewMail);
+  bool Iterate();
+  bool OnConnectToServer();
+  bool OnStartUp();
+  void RegisterVariables();
+  // bool buildReport();
+
+private:
+  // double uMotion_posX_fb;
+  // double uMotion_posY_fb;
+  // double uMotion_posZ_fb;
+  // double uMotion_roll_fb;
+  // double uMotion_pitch_fb;
+  // double uMotion_yaw_fb;    
+  // double uMotion_longitude_fb;
+  // double uMotion_latitude_fb;
+  // double uMotion_velocityX_fb;
+  // double uMotion_velocityY_fb;
+  // double uMotion_velocityZ_fb;
+  // double uMotion_deep_fb;
+
+  struct EstimatedState estimatedState;
+
+  void ConvertLLAToENU(std::vector<double> init_lla, 
+                            std::vector<double> point_lla, 
+                            std::vector<double>& point_enu);
+  void ConvertENUToLLA(std::vector<double> init_lla, 
+                              std::vector<double> point_enu,
+                              std::vector<double> &point_lla);
+  void ConvertLLAToNED(std::vector<double> init_lla, 
+                            std::vector<double> point_lla, 
+                            std::vector<double>& point_ned);
+  void ConvertNEDToLLA(std::vector<double> init_lla, 
+                            std::vector<double> point_ned,
+                            std::vector<double> &point_lla);
+
+  void tcpProcessThread();
+  // std::string convertEmbeddedFormat();
+  std::string convertEmbeddedFormat(Json::Value &embeddedState);  
+  int tcpSockFD;
+  int tcpConnectRet;
+  // unsigned char tcpReceiveBuffer[65535];
+  char tcpReceiveBuffer[65535];  
+  unsigned char tcpSendBuffer[65535];   
+  DUNE::IO::Poll m_poll_1;
+  DUNE::Network::TCPSocket sock_tcp_send;
+  uint16_t imcPollTCP(char* buffer, int bufferLength);
+  int parseMessage(unsigned char* buffer, int size);
+  int serializeMessage(unsigned char* buffer);
+
+  template <typename Type> inline uint16_t deserialize(Type& t, const uint8_t* bfr, uint16_t& length);
+  template <typename Type> inline uint16_t serialize(const Type t, uint8_t* bfr, uint16_t& sumSize);
+  uint8_t convertIntToUchar(int src, int min, int max);
+
+  struct AUVEmbedded embeddedInfo;
+  struct AUVExecuteCommand executeCommand;
+  int testFlag = 0;
+  bool testCount = false;
+	int sockfd = -1;
+	int connectFlg = -1;
+
+};
+
+#endif 

src/pBoardSupportComm/BoardSupportComm_Info.cpp

@@ -0,0 +1,115 @@
+/****************************************************************/
+/*   NAME: Xiaobin Zeng                                             */
+/*   ORGN: MIT Cambridge MA                                     */
+/*   FILE: BoardSupportComm_Info.cpp                               */
+/*   DATE: Dec 29th 1963                                        */
+/****************************************************************/
+
+#include <cstdlib>
+#include <iostream>
+#include "BoardSupportComm_Info.h"
+#include "ColorParse.h"
+#include "ReleaseInfo.h"
+
+using namespace std;
+
+//----------------------------------------------------------------
+// Procedure: showSynopsis
+
+void showSynopsis()
+{
+  blk("SYNOPSIS:                                                       ");
+  blk("------------------------------------                            ");
+  blk("  The pBoardSupportComm application is used for               ");
+  blk("                                                                ");
+  blk("                                                                ");
+  blk("                                                                ");
+  blk("                                                                ");
+}
+
+//----------------------------------------------------------------
+// Procedure: showHelpAndExit
+
+void showHelpAndExit()
+{
+  blk("                                                                ");
+  blu("=============================================================== ");
+  blu("Usage: pBoardSupportComm file.moos [OPTIONS]                   ");
+  blu("=============================================================== ");
+  blk("                                                                ");
+  showSynopsis();
+  blk("                                                                ");
+  blk("Options:                                                        ");
+  mag("  --alias","=<ProcessName>                                      ");
+  blk("      Launch pBoardSupportComm with the given process name         ");
+  blk("      rather than pBoardSupportComm.                           ");
+  mag("  --example, -e                                                 ");
+  blk("      Display example MOOS configuration block.                 ");
+  mag("  --help, -h                                                    ");
+  blk("      Display this help message.                                ");
+  mag("  --interface, -i                                               ");
+  blk("      Display MOOS publications and subscriptions.              ");
+  mag("  --version,-v                                                  ");
+  blk("      Display the release version of pBoardSupportComm.        ");
+  blk("                                                                ");
+  blk("Note: If argv[2] does not otherwise match a known option,       ");
+  blk("      then it will be interpreted as a run alias. This is       ");
+  blk("      to support pAntler launching conventions.                 ");
+  blk("                                                                ");
+  exit(0);
+}
+
+//----------------------------------------------------------------
+// Procedure: showExampleConfigAndExit
+
+void showExampleConfigAndExit()
+{
+  blk("                                                                ");
+  blu("=============================================================== ");
+  blu("pBoardSupportComm Example MOOS Configuration                   ");
+  blu("=============================================================== ");
+  blk("                                                                ");
+  blk("ProcessConfig = pBoardSupportComm                              ");
+  blk("{                                                               ");
+  blk("  AppTick   = 4                                                 ");
+  blk("  CommsTick = 4                                                 ");
+  blk("                                                                ");
+  blk("}                                                               ");
+  blk("                                                                ");
+  exit(0);
+}
+
+
+//----------------------------------------------------------------
+// Procedure: showInterfaceAndExit
+
+void showInterfaceAndExit()
+{
+  blk("                                                                ");
+  blu("=============================================================== ");
+  blu("pBoardSupportComm INTERFACE                                    ");
+  blu("=============================================================== ");
+  blk("                                                                ");
+  showSynopsis();
+  blk("                                                                ");
+  blk("SUBSCRIPTIONS:                                                  ");
+  blk("------------------------------------                            ");
+  blk("  NODE_MESSAGE = src_node=alpha,dest_node=bravo,var_name=FOO,   ");
+  blk("                 string_val=BAR                                 ");
+  blk("                                                                ");
+  blk("PUBLICATIONS:                                                   ");
+  blk("------------------------------------                            ");
+  blk("  Publications are determined by the node message content.      ");
+  blk("                                                                ");
+  exit(0);
+}
+
+//----------------------------------------------------------------
+// Procedure: showReleaseInfoAndExit
+
+void showReleaseInfoAndExit()
+{
+  showReleaseInfo("pBoardSupportComm", "gpl");
+  exit(0);
+}
+

src/pBoardSupportComm/BoardSupportComm_Info.h

@@ -0,0 +1,18 @@
+/****************************************************************/
+/*   NAME: Xiaobin Zeng                                             */
+/*   ORGN: MIT Cambridge MA                                     */
+/*   FILE: BoardSupportComm_Info.h                                      */
+/*   DATE: Dec 29th 1963                                        */
+/****************************************************************/
+
+#ifndef BoardSupportComm_INFO_HEADER
+#define BoardSupportComm_INFO_HEADER
+
+void showSynopsis();
+void showHelpAndExit();
+void showExampleConfigAndExit();
+void showInterfaceAndExit();
+void showReleaseInfoAndExit();
+
+#endif
+

src/pBoardSupportComm/CMakeLists.txt

@@ -0,0 +1,47 @@
+#--------------------------------------------------------
+# The CMakeLists.txt for:                       pBoardSupportComm
+# Author(s):                              Xiaobin Zeng
+#--------------------------------------------------------
+
+SET(SRC
+  BoardSupportComm.cpp
+  BoardSupportComm_Info.cpp
+  main.cpp
+)
+
+FIND_LIBRARY(DUNE_LIB dune-core /usr/local/lib /usr/local/lib/DUNE)
+FIND_PATH(DUNE_INCLUDE DUNE/IMC.hpp /usr/local/include /usr/local/include/DUNE)
+include_directories(${DUNE_INCLUDE})
+
+# include(FindProtobuf)
+# find_package(Protobuf REQUIRED)
+# include_directories(${Protobuf_INCLUDE_DIR})
+
+include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+
+#find_package (jsoncpp REQUIRED)
+
+find_package (GeographicLib REQUIRED)
+include_directories(${GeographicLib_INCLUDE_DIRS})
+
+ADD_EXECUTABLE(pBoardSupportComm ${SRC})
+
+include_directories(/usr/include/jsoncpp/)
+link_directories(/usr/local/lib/)
+
+TARGET_LINK_LIBRARIES(pBoardSupportComm
+   ${MOOS_LIBRARIES}
+   ${DUNE_LIB}
+   ${GeographicLib_LIBRARIES} 
+   mbutil
+   m
+   pthread
+   jsoncpp
+  #  jsoncpp_lib_static
+  #  protobuf
+  #  protoc
+  #  proto
+  #  ${PROTOBUF_LIBRARY}      
+)
+
+

src/pBoardSupportComm/main.cpp

@@ -0,0 +1,53 @@
+/************************************************************/
+/*    NAME: Xiaobin Zeng                                              */
+/*    ORGN: MIT                                             */
+/*    FILE: main.cpp                                        */
+/*    DATE:                                                 */
+/************************************************************/
+
+#include <string>
+#include "MBUtils.h"
+#include "ColorParse.h"
+#include "BoardSupportComm.h"
+#include "BoardSupportComm_Info.h"
+
+
+using namespace std;
+
+int main(int argc, char *argv[])
+{
+  string mission_file;
+  string run_command = argv[0];
+
+  for(int i=1; i<argc; i++) {
+    string argi = argv[i];
+    if((argi=="-v") || (argi=="--version") || (argi=="-version"))
+      showReleaseInfoAndExit();
+    else if((argi=="-e") || (argi=="--example") || (argi=="-example"))
+      showExampleConfigAndExit();
+    else if((argi == "-h") || (argi == "--help") || (argi=="-help"))
+      showHelpAndExit();
+    else if((argi == "-i") || (argi == "--interface"))
+      showInterfaceAndExit();
+    else if(strEnds(argi, ".moos") || strEnds(argi, ".moos++"))
+      mission_file = argv[i];
+    else if(strBegins(argi, "--alias="))
+      run_command = argi.substr(8);
+    else if(i==2)
+      run_command = argi;
+  }
+  
+  if(mission_file == "")
+    showHelpAndExit();
+
+  cout << termColor("green");
+  cout << "pBoardSupportComm launching as " << run_command << endl;
+  cout << termColor() << endl;
+
+  BoardSupportComm BoardSupportComm;
+
+  BoardSupportComm.Run(run_command.c_str(), mission_file.c_str());
+  
+  return(0);
+}
+

src/pBoardSupportComm/pBoardSupportComm.moos

@@ -0,0 +1,9 @@
+//------------------------------------------------
+// pBoardSupportComm config block
+
+ProcessConfig = pBoardSupportComm
+{
+   AppTick   = 4
+   CommsTick = 4
+}
+

src/pClientViewer/Behavior.pb.h

@@ -0,0 +1,927 @@
+// Generated by the protocol buffer compiler.  DO NOT EDIT!
+// source: Behavior.proto
+
+#ifndef GOOGLE_PROTOBUF_INCLUDED_Behavior_2eproto
+#define GOOGLE_PROTOBUF_INCLUDED_Behavior_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_Behavior_2eproto
+PROTOBUF_NAMESPACE_OPEN
+namespace internal {
+class AnyMetadata;
+}  // namespace internal
+PROTOBUF_NAMESPACE_CLOSE
+
+// Internal implementation detail -- do not use these members.
+struct TableStruct_Behavior_2eproto {
+  static const uint32_t offsets[];
+};
+extern const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_Behavior_2eproto;
+namespace Behavior {
+class Coordinate;
+struct CoordinateDefaultTypeInternal;
+extern CoordinateDefaultTypeInternal _Coordinate_default_instance_;
+class WayPointBehavior;
+struct WayPointBehaviorDefaultTypeInternal;
+extern WayPointBehaviorDefaultTypeInternal _WayPointBehavior_default_instance_;
+}  // namespace Behavior
+PROTOBUF_NAMESPACE_OPEN
+template<> ::Behavior::Coordinate* Arena::CreateMaybeMessage<::Behavior::Coordinate>(Arena*);
+template<> ::Behavior::WayPointBehavior* Arena::CreateMaybeMessage<::Behavior::WayPointBehavior>(Arena*);
+PROTOBUF_NAMESPACE_CLOSE
+namespace Behavior {
+
+// ===================================================================
+
+class Coordinate final :
+    public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:Behavior.Coordinate) */ {
+ public:
+  inline Coordinate() : Coordinate(nullptr) {}
+  ~Coordinate() override;
+  explicit PROTOBUF_CONSTEXPR Coordinate(::PROTOBUF_NAMESPACE_ID::internal::ConstantInitialized);
+
+  Coordinate(const Coordinate& from);
+  Coordinate(Coordinate&& from) noexcept
+    : Coordinate() {
+    *this = ::std::move(from);
+  }
+
+  inline Coordinate& operator=(const Coordinate& from) {
+    CopyFrom(from);
+    return *this;
+  }
+  inline Coordinate& operator=(Coordinate&& 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 Coordinate& default_instance() {
+    return *internal_default_instance();
+  }
+  static inline const Coordinate* internal_default_instance() {
+    return reinterpret_cast<const Coordinate*>(
+               &_Coordinate_default_instance_);
+  }
+  static constexpr int kIndexInFileMessages =
+    0;
+
+  friend void swap(Coordinate& a, Coordinate& b) {
+    a.Swap(&b);
+  }
+  inline void Swap(Coordinate* 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(Coordinate* other) {
+    if (other == this) return;
+    GOOGLE_DCHECK(GetOwningArena() == other->GetOwningArena());
+    InternalSwap(other);
+  }
+
+  // implements Message ----------------------------------------------
+
+  Coordinate* New(::PROTOBUF_NAMESPACE_ID::Arena* arena = nullptr) const final {
+    return CreateMaybeMessage<Coordinate>(arena);
+  }
+  using ::PROTOBUF_NAMESPACE_ID::Message::CopyFrom;
+  void CopyFrom(const Coordinate& from);
+  using ::PROTOBUF_NAMESPACE_ID::Message::MergeFrom;
+  void MergeFrom( const Coordinate& from) {
+    Coordinate::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(Coordinate* other);
+
+  private:
+  friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata;
+  static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() {
+    return "Behavior.Coordinate";
+  }
+  protected:
+  explicit Coordinate(::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 {
+    kNameFieldNumber = 1,
+    kLonFieldNumber = 2,
+    kLatFieldNumber = 3,
+    kDepthFieldNumber = 4,
+    kSpeedFieldNumber = 5,
+  };
+  // string name = 1;
+  void clear_name();
+  const std::string& name() const;
+  template <typename ArgT0 = const std::string&, typename... ArgT>
+  void set_name(ArgT0&& arg0, ArgT... args);
+  std::string* mutable_name();
+  PROTOBUF_NODISCARD std::string* release_name();
+  void set_allocated_name(std::string* name);
+  private:
+  const std::string& _internal_name() const;
+  inline PROTOBUF_ALWAYS_INLINE void _internal_set_name(const std::string& value);
+  std::string* _internal_mutable_name();
+  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 depth = 4;
+  void clear_depth();
+  float depth() const;
+  void set_depth(float value);
+  private:
+  float _internal_depth() const;
+  void _internal_set_depth(float value);
+  public:
+
+  // float speed = 5;
+  void clear_speed();
+  float speed() const;
+  void set_speed(float value);
+  private:
+  float _internal_speed() const;
+  void _internal_set_speed(float value);
+  public:
+
+  // @@protoc_insertion_point(class_scope:Behavior.Coordinate)
+ 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 name_;
+    float lon_;
+    float lat_;
+    float depth_;
+    float speed_;
+    mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
+  };
+  union { Impl_ _impl_; };
+  friend struct ::TableStruct_Behavior_2eproto;
+};
+// -------------------------------------------------------------------
+
+class WayPointBehavior final :
+    public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:Behavior.WayPointBehavior) */ {
+ public:
+  inline WayPointBehavior() : WayPointBehavior(nullptr) {}
+  ~WayPointBehavior() override;
+  explicit PROTOBUF_CONSTEXPR WayPointBehavior(::PROTOBUF_NAMESPACE_ID::internal::ConstantInitialized);
+
+  WayPointBehavior(const WayPointBehavior& from);
+  WayPointBehavior(WayPointBehavior&& from) noexcept
+    : WayPointBehavior() {
+    *this = ::std::move(from);
+  }
+
+  inline WayPointBehavior& operator=(const WayPointBehavior& from) {
+    CopyFrom(from);
+    return *this;
+  }
+  inline WayPointBehavior& operator=(WayPointBehavior&& 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 WayPointBehavior& default_instance() {
+    return *internal_default_instance();
+  }
+  static inline const WayPointBehavior* internal_default_instance() {
+    return reinterpret_cast<const WayPointBehavior*>(
+               &_WayPointBehavior_default_instance_);
+  }
+  static constexpr int kIndexInFileMessages =
+    1;
+
+  friend void swap(WayPointBehavior& a, WayPointBehavior& b) {
+    a.Swap(&b);
+  }
+  inline void Swap(WayPointBehavior* 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(WayPointBehavior* other) {
+    if (other == this) return;
+    GOOGLE_DCHECK(GetOwningArena() == other->GetOwningArena());
+    InternalSwap(other);
+  }
+
+  // implements Message ----------------------------------------------
+
+  WayPointBehavior* New(::PROTOBUF_NAMESPACE_ID::Arena* arena = nullptr) const final {
+    return CreateMaybeMessage<WayPointBehavior>(arena);
+  }
+  using ::PROTOBUF_NAMESPACE_ID::Message::CopyFrom;
+  void CopyFrom(const WayPointBehavior& from);
+  using ::PROTOBUF_NAMESPACE_ID::Message::MergeFrom;
+  void MergeFrom( const WayPointBehavior& from) {
+    WayPointBehavior::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(WayPointBehavior* other);
+
+  private:
+  friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata;
+  static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() {
+    return "Behavior.WayPointBehavior";
+  }
+  protected:
+  explicit WayPointBehavior(::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 {
+    kPointsFieldNumber = 3,
+    kNameFieldNumber = 1,
+    kPriorityFieldNumber = 2,
+    kDurationFieldNumber = 4,
+    kConstSpeedFieldNumber = 6,
+    kClosedLoopFieldNumber = 5,
+    kPerpetualFieldNumber = 8,
+    kRepeateFieldNumber = 7,
+    kMinDepthFieldNumber = 9,
+    kMaxDepthFieldNumber = 10,
+  };
+  // repeated .Behavior.Coordinate points = 3;
+  int points_size() const;
+  private:
+  int _internal_points_size() const;
+  public:
+  void clear_points();
+  ::Behavior::Coordinate* mutable_points(int index);
+  ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::Behavior::Coordinate >*
+      mutable_points();
+  private:
+  const ::Behavior::Coordinate& _internal_points(int index) const;
+  ::Behavior::Coordinate* _internal_add_points();
+  public:
+  const ::Behavior::Coordinate& points(int index) const;
+  ::Behavior::Coordinate* add_points();
+  const ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::Behavior::Coordinate >&
+      points() const;
+
+  // string name = 1;
+  void clear_name();
+  const std::string& name() const;
+  template <typename ArgT0 = const std::string&, typename... ArgT>
+  void set_name(ArgT0&& arg0, ArgT... args);
+  std::string* mutable_name();
+  PROTOBUF_NODISCARD std::string* release_name();
+  void set_allocated_name(std::string* name);
+  private:
+  const std::string& _internal_name() const;
+  inline PROTOBUF_ALWAYS_INLINE void _internal_set_name(const std::string& value);
+  std::string* _internal_mutable_name();
+  public:
+
+  // int32 priority = 2;
+  void clear_priority();
+  int32_t priority() const;
+  void set_priority(int32_t value);
+  private:
+  int32_t _internal_priority() const;
+  void _internal_set_priority(int32_t value);
+  public:
+
+  // float duration = 4;
+  void clear_duration();
+  float duration() const;
+  void set_duration(float value);
+  private:
+  float _internal_duration() const;
+  void _internal_set_duration(float value);
+  public:
+
+  // float constSpeed = 6;
+  void clear_constspeed();
+  float constspeed() const;
+  void set_constspeed(float value);
+  private:
+  float _internal_constspeed() const;
+  void _internal_set_constspeed(float value);
+  public:
+
+  // bool closedLoop = 5;
+  void clear_closedloop();
+  bool closedloop() const;
+  void set_closedloop(bool value);
+  private:
+  bool _internal_closedloop() const;
+  void _internal_set_closedloop(bool value);
+  public:
+
+  // bool perpetual = 8;
+  void clear_perpetual();
+  bool perpetual() const;
+  void set_perpetual(bool value);
+  private:
+  bool _internal_perpetual() const;
+  void _internal_set_perpetual(bool value);
+  public:
+
+  // int32 repeate = 7;
+  void clear_repeate();
+  int32_t repeate() const;
+  void set_repeate(int32_t value);
+  private:
+  int32_t _internal_repeate() const;
+  void _internal_set_repeate(int32_t value);
+  public:
+
+  // float minDepth = 9;
+  void clear_mindepth();
+  float mindepth() const;
+  void set_mindepth(float value);
+  private:
+  float _internal_mindepth() const;
+  void _internal_set_mindepth(float value);
+  public:
+
+  // float maxDepth = 10;
+  void clear_maxdepth();
+  float maxdepth() const;
+  void set_maxdepth(float value);
+  private:
+  float _internal_maxdepth() const;
+  void _internal_set_maxdepth(float value);
+  public:
+
+  // @@protoc_insertion_point(class_scope:Behavior.WayPointBehavior)
+ private:
+  class _Internal;
+
+  template <typename T> friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper;
+  typedef void InternalArenaConstructable_;
+  typedef void DestructorSkippable_;
+  struct Impl_ {
+    ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::Behavior::Coordinate > points_;
+    ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr name_;
+    int32_t priority_;
+    float duration_;
+    float constspeed_;
+    bool closedloop_;
+    bool perpetual_;
+    int32_t repeate_;
+    float mindepth_;
+    float maxdepth_;
+    mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
+  };
+  union { Impl_ _impl_; };
+  friend struct ::TableStruct_Behavior_2eproto;
+};
+// ===================================================================
+
+
+// ===================================================================
+
+#ifdef __GNUC__
+  #pragma GCC diagnostic push
+  #pragma GCC diagnostic ignored "-Wstrict-aliasing"
+#endif  // __GNUC__
+// Coordinate
+
+// string name = 1;
+inline void Coordinate::clear_name() {
+  _impl_.name_.ClearToEmpty();
+}
+inline const std::string& Coordinate::name() const {
+  // @@protoc_insertion_point(field_get:Behavior.Coordinate.name)
+  return _internal_name();
+}
+template <typename ArgT0, typename... ArgT>
+inline PROTOBUF_ALWAYS_INLINE
+void Coordinate::set_name(ArgT0&& arg0, ArgT... args) {
+ 
+ _impl_.name_.Set(static_cast<ArgT0 &&>(arg0), args..., GetArenaForAllocation());
+  // @@protoc_insertion_point(field_set:Behavior.Coordinate.name)
+}
+inline std::string* Coordinate::mutable_name() {
+  std::string* _s = _internal_mutable_name();
+  // @@protoc_insertion_point(field_mutable:Behavior.Coordinate.name)
+  return _s;
+}
+inline const std::string& Coordinate::_internal_name() const {
+  return _impl_.name_.Get();
+}
+inline void Coordinate::_internal_set_name(const std::string& value) {
+  
+  _impl_.name_.Set(value, GetArenaForAllocation());
+}
+inline std::string* Coordinate::_internal_mutable_name() {
+  
+  return _impl_.name_.Mutable(GetArenaForAllocation());
+}
+inline std::string* Coordinate::release_name() {
+  // @@protoc_insertion_point(field_release:Behavior.Coordinate.name)
+  return _impl_.name_.Release();
+}
+inline void Coordinate::set_allocated_name(std::string* name) {
+  if (name != nullptr) {
+    
+  } else {
+    
+  }
+  _impl_.name_.SetAllocated(name, GetArenaForAllocation());
+#ifdef PROTOBUF_FORCE_COPY_DEFAULT_STRING
+  if (_impl_.name_.IsDefault()) {
+    _impl_.name_.Set("", GetArenaForAllocation());
+  }
+#endif // PROTOBUF_FORCE_COPY_DEFAULT_STRING
+  // @@protoc_insertion_point(field_set_allocated:Behavior.Coordinate.name)
+}
+
+// float lon = 2;
+inline void Coordinate::clear_lon() {
+  _impl_.lon_ = 0;
+}
+inline float Coordinate::_internal_lon() const {
+  return _impl_.lon_;
+}
+inline float Coordinate::lon() const {
+  // @@protoc_insertion_point(field_get:Behavior.Coordinate.lon)
+  return _internal_lon();
+}
+inline void Coordinate::_internal_set_lon(float value) {
+  
+  _impl_.lon_ = value;
+}
+inline void Coordinate::set_lon(float value) {
+  _internal_set_lon(value);
+  // @@protoc_insertion_point(field_set:Behavior.Coordinate.lon)
+}
+
+// float lat = 3;
+inline void Coordinate::clear_lat() {
+  _impl_.lat_ = 0;
+}
+inline float Coordinate::_internal_lat() const {
+  return _impl_.lat_;
+}
+inline float Coordinate::lat() const {
+  // @@protoc_insertion_point(field_get:Behavior.Coordinate.lat)
+  return _internal_lat();
+}
+inline void Coordinate::_internal_set_lat(float value) {
+  
+  _impl_.lat_ = value;
+}
+inline void Coordinate::set_lat(float value) {
+  _internal_set_lat(value);
+  // @@protoc_insertion_point(field_set:Behavior.Coordinate.lat)
+}
+
+// float depth = 4;
+inline void Coordinate::clear_depth() {
+  _impl_.depth_ = 0;
+}
+inline float Coordinate::_internal_depth() const {
+  return _impl_.depth_;
+}
+inline float Coordinate::depth() const {
+  // @@protoc_insertion_point(field_get:Behavior.Coordinate.depth)
+  return _internal_depth();
+}
+inline void Coordinate::_internal_set_depth(float value) {
+  
+  _impl_.depth_ = value;
+}
+inline void Coordinate::set_depth(float value) {
+  _internal_set_depth(value);
+  // @@protoc_insertion_point(field_set:Behavior.Coordinate.depth)
+}
+
+// float speed = 5;
+inline void Coordinate::clear_speed() {
+  _impl_.speed_ = 0;
+}
+inline float Coordinate::_internal_speed() const {
+  return _impl_.speed_;
+}
+inline float Coordinate::speed() const {
+  // @@protoc_insertion_point(field_get:Behavior.Coordinate.speed)
+  return _internal_speed();
+}
+inline void Coordinate::_internal_set_speed(float value) {
+  
+  _impl_.speed_ = value;
+}
+inline void Coordinate::set_speed(float value) {
+  _internal_set_speed(value);
+  // @@protoc_insertion_point(field_set:Behavior.Coordinate.speed)
+}
+
+// -------------------------------------------------------------------
+
+// WayPointBehavior
+
+// string name = 1;
+inline void WayPointBehavior::clear_name() {
+  _impl_.name_.ClearToEmpty();
+}
+inline const std::string& WayPointBehavior::name() const {
+  // @@protoc_insertion_point(field_get:Behavior.WayPointBehavior.name)
+  return _internal_name();
+}
+template <typename ArgT0, typename... ArgT>
+inline PROTOBUF_ALWAYS_INLINE
+void WayPointBehavior::set_name(ArgT0&& arg0, ArgT... args) {
+ 
+ _impl_.name_.Set(static_cast<ArgT0 &&>(arg0), args..., GetArenaForAllocation());
+  // @@protoc_insertion_point(field_set:Behavior.WayPointBehavior.name)
+}
+inline std::string* WayPointBehavior::mutable_name() {
+  std::string* _s = _internal_mutable_name();
+  // @@protoc_insertion_point(field_mutable:Behavior.WayPointBehavior.name)
+  return _s;
+}
+inline const std::string& WayPointBehavior::_internal_name() const {
+  return _impl_.name_.Get();
+}
+inline void WayPointBehavior::_internal_set_name(const std::string& value) {
+  
+  _impl_.name_.Set(value, GetArenaForAllocation());
+}
+inline std::string* WayPointBehavior::_internal_mutable_name() {
+  
+  return _impl_.name_.Mutable(GetArenaForAllocation());
+}
+inline std::string* WayPointBehavior::release_name() {
+  // @@protoc_insertion_point(field_release:Behavior.WayPointBehavior.name)
+  return _impl_.name_.Release();
+}
+inline void WayPointBehavior::set_allocated_name(std::string* name) {
+  if (name != nullptr) {
+    
+  } else {
+    
+  }
+  _impl_.name_.SetAllocated(name, GetArenaForAllocation());
+#ifdef PROTOBUF_FORCE_COPY_DEFAULT_STRING
+  if (_impl_.name_.IsDefault()) {
+    _impl_.name_.Set("", GetArenaForAllocation());
+  }
+#endif // PROTOBUF_FORCE_COPY_DEFAULT_STRING
+  // @@protoc_insertion_point(field_set_allocated:Behavior.WayPointBehavior.name)
+}
+
+// int32 priority = 2;
+inline void WayPointBehavior::clear_priority() {
+  _impl_.priority_ = 0;
+}
+inline int32_t WayPointBehavior::_internal_priority() const {
+  return _impl_.priority_;
+}
+inline int32_t WayPointBehavior::priority() const {
+  // @@protoc_insertion_point(field_get:Behavior.WayPointBehavior.priority)
+  return _internal_priority();
+}
+inline void WayPointBehavior::_internal_set_priority(int32_t value) {
+  
+  _impl_.priority_ = value;
+}
+inline void WayPointBehavior::set_priority(int32_t value) {
+  _internal_set_priority(value);
+  // @@protoc_insertion_point(field_set:Behavior.WayPointBehavior.priority)
+}
+
+// repeated .Behavior.Coordinate points = 3;
+inline int WayPointBehavior::_internal_points_size() const {
+  return _impl_.points_.size();
+}
+inline int WayPointBehavior::points_size() const {
+  return _internal_points_size();
+}
+inline void WayPointBehavior::clear_points() {
+  _impl_.points_.Clear();
+}
+inline ::Behavior::Coordinate* WayPointBehavior::mutable_points(int index) {
+  // @@protoc_insertion_point(field_mutable:Behavior.WayPointBehavior.points)
+  return _impl_.points_.Mutable(index);
+}
+inline ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::Behavior::Coordinate >*
+WayPointBehavior::mutable_points() {
+  // @@protoc_insertion_point(field_mutable_list:Behavior.WayPointBehavior.points)
+  return &_impl_.points_;
+}
+inline const ::Behavior::Coordinate& WayPointBehavior::_internal_points(int index) const {
+  return _impl_.points_.Get(index);
+}
+inline const ::Behavior::Coordinate& WayPointBehavior::points(int index) const {
+  // @@protoc_insertion_point(field_get:Behavior.WayPointBehavior.points)
+  return _internal_points(index);
+}
+inline ::Behavior::Coordinate* WayPointBehavior::_internal_add_points() {
+  return _impl_.points_.Add();
+}
+inline ::Behavior::Coordinate* WayPointBehavior::add_points() {
+  ::Behavior::Coordinate* _add = _internal_add_points();
+  // @@protoc_insertion_point(field_add:Behavior.WayPointBehavior.points)
+  return _add;
+}
+inline const ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::Behavior::Coordinate >&
+WayPointBehavior::points() const {
+  // @@protoc_insertion_point(field_list:Behavior.WayPointBehavior.points)
+  return _impl_.points_;
+}
+
+// float duration = 4;
+inline void WayPointBehavior::clear_duration() {
+  _impl_.duration_ = 0;
+}
+inline float WayPointBehavior::_internal_duration() const {
+  return _impl_.duration_;
+}
+inline float WayPointBehavior::duration() const {
+  // @@protoc_insertion_point(field_get:Behavior.WayPointBehavior.duration)
+  return _internal_duration();
+}
+inline void WayPointBehavior::_internal_set_duration(float value) {
+  
+  _impl_.duration_ = value;
+}
+inline void WayPointBehavior::set_duration(float value) {
+  _internal_set_duration(value);
+  // @@protoc_insertion_point(field_set:Behavior.WayPointBehavior.duration)
+}
+
+// bool closedLoop = 5;
+inline void WayPointBehavior::clear_closedloop() {
+  _impl_.closedloop_ = false;
+}
+inline bool WayPointBehavior::_internal_closedloop() const {
+  return _impl_.closedloop_;
+}
+inline bool WayPointBehavior::closedloop() const {
+  // @@protoc_insertion_point(field_get:Behavior.WayPointBehavior.closedLoop)
+  return _internal_closedloop();
+}
+inline void WayPointBehavior::_internal_set_closedloop(bool value) {
+  
+  _impl_.closedloop_ = value;
+}
+inline void WayPointBehavior::set_closedloop(bool value) {
+  _internal_set_closedloop(value);
+  // @@protoc_insertion_point(field_set:Behavior.WayPointBehavior.closedLoop)
+}
+
+// float constSpeed = 6;
+inline void WayPointBehavior::clear_constspeed() {
+  _impl_.constspeed_ = 0;
+}
+inline float WayPointBehavior::_internal_constspeed() const {
+  return _impl_.constspeed_;
+}
+inline float WayPointBehavior::constspeed() const {
+  // @@protoc_insertion_point(field_get:Behavior.WayPointBehavior.constSpeed)
+  return _internal_constspeed();
+}
+inline void WayPointBehavior::_internal_set_constspeed(float value) {
+  
+  _impl_.constspeed_ = value;
+}
+inline void WayPointBehavior::set_constspeed(float value) {
+  _internal_set_constspeed(value);
+  // @@protoc_insertion_point(field_set:Behavior.WayPointBehavior.constSpeed)
+}
+
+// int32 repeate = 7;
+inline void WayPointBehavior::clear_repeate() {
+  _impl_.repeate_ = 0;
+}
+inline int32_t WayPointBehavior::_internal_repeate() const {
+  return _impl_.repeate_;
+}
+inline int32_t WayPointBehavior::repeate() const {
+  // @@protoc_insertion_point(field_get:Behavior.WayPointBehavior.repeate)
+  return _internal_repeate();
+}
+inline void WayPointBehavior::_internal_set_repeate(int32_t value) {
+  
+  _impl_.repeate_ = value;
+}
+inline void WayPointBehavior::set_repeate(int32_t value) {
+  _internal_set_repeate(value);
+  // @@protoc_insertion_point(field_set:Behavior.WayPointBehavior.repeate)
+}
+
+// bool perpetual = 8;
+inline void WayPointBehavior::clear_perpetual() {
+  _impl_.perpetual_ = false;
+}
+inline bool WayPointBehavior::_internal_perpetual() const {
+  return _impl_.perpetual_;
+}
+inline bool WayPointBehavior::perpetual() const {
+  // @@protoc_insertion_point(field_get:Behavior.WayPointBehavior.perpetual)
+  return _internal_perpetual();
+}
+inline void WayPointBehavior::_internal_set_perpetual(bool value) {
+  
+  _impl_.perpetual_ = value;
+}
+inline void WayPointBehavior::set_perpetual(bool value) {
+  _internal_set_perpetual(value);
+  // @@protoc_insertion_point(field_set:Behavior.WayPointBehavior.perpetual)
+}
+
+// float minDepth = 9;
+inline void WayPointBehavior::clear_mindepth() {
+  _impl_.mindepth_ = 0;
+}
+inline float WayPointBehavior::_internal_mindepth() const {
+  return _impl_.mindepth_;
+}
+inline float WayPointBehavior::mindepth() const {
+  // @@protoc_insertion_point(field_get:Behavior.WayPointBehavior.minDepth)
+  return _internal_mindepth();
+}
+inline void WayPointBehavior::_internal_set_mindepth(float value) {
+  
+  _impl_.mindepth_ = value;
+}
+inline void WayPointBehavior::set_mindepth(float value) {
+  _internal_set_mindepth(value);
+  // @@protoc_insertion_point(field_set:Behavior.WayPointBehavior.minDepth)
+}
+
+// float maxDepth = 10;
+inline void WayPointBehavior::clear_maxdepth() {
+  _impl_.maxdepth_ = 0;
+}
+inline float WayPointBehavior::_internal_maxdepth() const {
+  return _impl_.maxdepth_;
+}
+inline float WayPointBehavior::maxdepth() const {
+  // @@protoc_insertion_point(field_get:Behavior.WayPointBehavior.maxDepth)
+  return _internal_maxdepth();
+}
+inline void WayPointBehavior::_internal_set_maxdepth(float value) {
+  
+  _impl_.maxdepth_ = value;
+}
+inline void WayPointBehavior::set_maxdepth(float value) {
+  _internal_set_maxdepth(value);
+  // @@protoc_insertion_point(field_set:Behavior.WayPointBehavior.maxDepth)
+}
+
+#ifdef __GNUC__
+  #pragma GCC diagnostic pop
+#endif  // __GNUC__
+// -------------------------------------------------------------------
+
+
+// @@protoc_insertion_point(namespace_scope)
+
+}  // namespace Behavior
+
+// @@protoc_insertion_point(global_scope)
+
+#include <google/protobuf/port_undef.inc>
+#endif  // GOOGLE_PROTOBUF_INCLUDED_GOOGLE_PROTOBUF_INCLUDED_Behavior_2eproto

src/pClientViewer/CMakeLists.txt

@@ -0,0 +1,52 @@
+#--------------------------------------------------------
+# The CMakeLists.txt for:                       pClientViewer
+# Author(s):                              Xiaobin Zeng
+#--------------------------------------------------------
+
+SET(SRC
+  ClientViewer.cpp
+  ClientViewer_Info.cpp
+  # UDPCommunicationEvent.cpp
+  # TCPCommunicationEvent.cpp  
+  main.cpp
+)
+
+SET(CMAKE_CXX_STANDARD 11)
+
+FIND_LIBRARY(DUNE_LIB dune-core /usr/local/lib /usr/local/lib/DUNE)
+FIND_PATH(DUNE_INCLUDE DUNE/IMC.hpp /usr/local/include /usr/local/include/DUNE)
+include_directories(${DUNE_INCLUDE})
+
+# include(FindProtobuf)
+# find_package(Protobuf REQUIRED)
+# include_directories(${Protobuf_INCLUDE_DIR})
+
+# protobuf_generate_cpp(PROTO_SRC PROTO_HEADER Behavior.proto)
+# add_library(proto ${PROTO_HEADER} ${PROTO_SRC})
+
+include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+
+#find_package (jsoncpp NO_MODULE REQUIRED)
+
+include_directories(/usr/include/jsoncpp/)
+link_directories(/usr/local/lib/)
+
+ADD_EXECUTABLE(pClientViewer ${SRC})
+
+TARGET_LINK_LIBRARIES(pClientViewer
+  ${MOOS_LIBRARIES}
+  ${CMAKE_DL_LIBS}
+  ${SYSTEM_LIBS} 
+  ${DUNE_LIB}      
+  mbutil
+  m
+  pthread
+  jsoncpp
+  # jsoncpp_lib_static  
+  # protobuf
+  # protoc
+  # proto
+  # ${PROTOBUF_LIBRARY}
+)
+
+

src/pClientViewer/ClientViewer.cpp

@@ -0,0 +1,812 @@
+/************************************************************/
+/*    NAME: Xiaobin Zeng                                              */
+/*    ORGN: MIT                                             */
+/*    FILE: ClientViewer.cpp                                        */
+/*    DATE:                                                 */
+/************************************************************/
+
+#include <iterator>
+#include "ClientViewer.h"
+//#include "Behavior.pb.h"
+#include <json/json.h>
+
+using namespace std;
+
+#define UDP_RECEIVE_PORT 6001
+#define TCP_SEND_PORT 8000
+//#define TCP_SERVER_ADDRESS "10.25.0.230" //树莓派
+#define TCP_SERVER_ADDRESS  "127.0.0.1"
+// #define TCP_SERVER_ADDRESS  "10.25.0.163"
+// #define TCP_SERVER_ADDRESS  "10.25.0.160"
+
+//---------------------------------------------------------
+// Constructor
+
+ClientViewer::ClientViewer()
+{
+  udpReceiveBuffer = new uint8_t[65535];
+  tcpReceiveBuffer = new uint8_t[65535];
+
+}
+
+//---------------------------------------------------------
+// Destructor
+
+ClientViewer::~ClientViewer()
+{
+  // udpCommEvent.Stop();
+  // tcpCommEvent.Stop();  
+  delete udpReceiveBuffer;
+  delete tcpReceiveBuffer;
+
+  // if (sock_tcp_send)
+  // {
+  //   delete sock_tcp_send;
+  //   sock_tcp_send = NULL;
+  // }
+
+  // if (sock_udp_receive)
+  // {
+  //   delete sock_udp_receive;
+  //   sock_udp_receive = NULL;
+  // }
+  
+}
+
+//---------------------------------------------------------
+// Procedure: OnNewMail
+
+bool ClientViewer::OnNewMail(MOOSMSG_LIST &NewMail)
+{
+  MOOSMSG_LIST::iterator p;
+   
+  for(p=NewMail.begin(); p!=NewMail.end(); p++) 
+  {
+    CMOOSMsg &msg = *p;
+    string key   = msg.GetKey();
+    string comm  = msg.GetCommunity();
+    double dval  = msg.GetDouble();
+    string sval  = msg.GetString(); 
+    string msrc  = msg.GetSource();
+    double mtime = msg.GetTime();
+    bool   mdbl  = msg.IsDouble();
+    bool   mstr  = msg.IsString();
+    std::cout << key << " : " << sval << std::endl;    
+    if(key == "Command")
+    {
+      if (sval == "SetPlan1")     //PlanDB
+      {
+        std::string systemName = "CCU Neptus 0_163";
+        std::string plan_1_Spec = SetPlan1(systemName, MOOS::Time());
+        std::cout << "send task" << std::endl;  
+        //std::cout << plan_1_Spec << std::endl;
+        DUNE::IMC::PlanDB msg;
+        msg.setTimeStamp();
+        msg.type = DUNE::IMC::PlanDB::TypeEnum::DBT_REQUEST;
+        msg.op = DUNE::IMC::PlanDB::OperationEnum::DBOP_SET;
+        msg.plan_id.assign("BHV_Waypoint");
+        msg.info = plan_1_Spec;
+        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
+
+      }
+      if (sval == "SetPlan2")  //PlanDB
+      {
+        std::string systemName = "CCU Neptus 0_163";
+        std::string plan_2_Spec = SetPlan2(systemName, MOOS::Time());
+        std::cout << "send task" << std::endl;    
+        //std::cout << plan_2_Spec << std::endl;
+        DUNE::IMC::PlanDB msg;
+        msg.setTimeStamp();
+        msg.type = DUNE::IMC::PlanDB::TypeEnum::DBT_REQUEST;
+        msg.op = DUNE::IMC::PlanDB::OperationEnum::DBOP_SET;
+        msg.plan_id.assign("BHV_Waypoint");
+        msg.info = plan_2_Spec;
+        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
+      }
+      if (sval == "ModifyPlan1")  //PlanDB
+      {
+        std::string systemName = "CCU Neptus 0_163";  
+        std::string plan_1_Spec = ModifyPlan1(systemName, MOOS::Time());
+        std::cout << "modify task" << std::endl;    
+        //std::cout << plan_1_Spec << std::endl;
+        DUNE::IMC::PlanDB msg;
+        msg.setTimeStamp();
+        msg.type = DUNE::IMC::PlanDB::TypeEnum::DBT_REQUEST;
+        msg.op = DUNE::IMC::PlanDB::OperationEnum::DBOP_SET;
+        msg.plan_id.assign("BHV_Waypoint");
+        msg.info = plan_1_Spec;
+        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
+      }
+      if (sval == "GetPlanList")  //PlanDB
+      {
+        DUNE::IMC::PlanDB msg;
+        msg.setTimeStamp();
+        msg.type = DUNE::IMC::PlanDB::TypeEnum::DBT_REQUEST;
+        msg.op = DUNE::IMC::PlanDB::OperationEnum::DBOP_GET_STATE;
+        msg.plan_id.assign("Test");  
+        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
+      }      
+      if (sval == "SetThrust")  //SetEntityParameters
+      {
+        double timeStamp = MOOS::Time();
+        DUNE::IMC::SetEntityParameters msg;
+        msg.setTimeStamp(timeStamp);      
+        msg.name = "Thrust";
+        DUNE::IMC::EntityParameter subMsg;
+        subMsg.setTimeStamp(timeStamp);
+        SetEntityStatus(subMsg, "Pwm", generateEntityValue("30", "int"));
+        msg.params.push_back(subMsg);
+        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);        
+      }
+      if (sval == "SetIndicatorLight")  //SetEntityParameters
+      {
+        double timeStamp = MOOS::Time();
+        DUNE::IMC::SetEntityParameters msg;
+        msg.setTimeStamp(timeStamp);      
+        msg.name = "IndicatorLight";
+        DUNE::IMC::EntityParameter subMsg;
+        subMsg.setTimeStamp(timeStamp);
+        SetEntityStatus(subMsg, "Status", generateEntityValue("1", "int"));
+        msg.params.push_back(subMsg);
+        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
+      }
+      if (sval == "StartPlan1")  //PlanControl
+      {
+        DUNE::IMC::PlanControl msg;
+        msg.setTimeStamp();
+        msg.type = DUNE::IMC::PlanControl::TypeEnum::PC_REQUEST;  
+        msg.op = DUNE::IMC::PlanControl::OperationEnum::PC_START;
+        msg.plan_id = "east_waypt_survey";
+        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
+      }
+      if (sval == "StartPlan2")  //PlanControl
+      {
+        DUNE::IMC::PlanControl msg;
+        msg.setTimeStamp();
+        msg.type = DUNE::IMC::PlanControl::TypeEnum::PC_REQUEST;  
+        msg.op = DUNE::IMC::PlanControl::OperationEnum::PC_START;
+        msg.plan_id = "west_waypt_survey";
+        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
+      }      
+      if (sval == "StopPlan1")  //PlanControl
+      {
+        DUNE::IMC::PlanControl msg;
+        msg.setTimeStamp();
+        msg.type = DUNE::IMC::PlanControl::TypeEnum::PC_REQUEST;  
+        msg.op = DUNE::IMC::PlanControl::OperationEnum::PC_STOP;
+        msg.plan_id = "east_waypt_survey";
+        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
+      }
+      if (sval == "StopPlan2")  //PlanControl
+      {
+        DUNE::IMC::PlanControl msg;
+        msg.setTimeStamp();
+        msg.type = DUNE::IMC::PlanControl::TypeEnum::PC_REQUEST;  
+        msg.op = DUNE::IMC::PlanControl::OperationEnum::PC_STOP;
+        msg.plan_id = "west_waypt_survey";
+        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
+      }
+      if (sval == "RemoteActions")  //PlanControl
+      {
+        DUNE::IMC::RemoteActions msg;
+        msg.setTimeStamp();
+        Json::Value executeCommand;
+        executeCommand["Thrust"] = -100;
+        executeCommand["Heading"] = -27.5;
+        Json::StreamWriterBuilder builder;
+        msg.actions = Json::writeString(builder, executeCommand);        
+        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
+      }
+      if (sval == "VehicleCommand")  //PlanControl
+      {
+        DUNE::IMC::VehicleCommand msg;
+        msg.setTimeStamp();
+        msg.type = DUNE::IMC::VehicleCommand::TypeEnum::VC_REQUEST;
+        msg.command = 1;    
+        tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
+      }                  
+    }    
+  }
+	
+   return(true);
+}
+
+//---------------------------------------------------------
+// Procedure: OnConnectToServer
+
+bool ClientViewer::OnConnectToServer()
+{
+  RegisterVariables();
+  return(true);
+}
+
+bool ClientViewer::Iterate()
+{
+#if 1
+  DUNE::IMC::Message * receiveUDPMessage;
+
+  while ((receiveUDPMessage = imcPollUDP()) != NULL) 
+  {
+      processMessage(receiveUDPMessage);
+      free(receiveUDPMessage);
+  }
+#endif
+
+# if 1    
+  DUNE::IMC::Message * receiveTCPMessage;  
+  while ((receiveTCPMessage = imcPollTCP()) != NULL) 
+  {
+      processMessage(receiveTCPMessage);
+      free(receiveTCPMessage);
+  }
+#endif
+
+  return(true);
+}
+
+bool processMessageCallback(DUNE::IMC::Message * message)
+{
+  int type = message->getId(); 
+#if 1
+  if (type == DUNE::IMC::Announce::getIdStatic()) 
+	{
+    DUNE::IMC::Announce * msg = dynamic_cast<DUNE::IMC::Announce *>(message);
+    // printf("server receive %s: %lf, %lf, %lf, %lf\n",   \ 
+    //   msg->getName(), msg->getTimeStamp(), msg->lat, msg->lon, msg->height);
+    printf("server receive %s: %lf, %lf, %lf, %lf\n",   \ 
+      msg->getName(), msg->getTimeStamp(), msg->lat*180/M_PI, msg->lon*180/M_PI, msg->height);      
+  }
+#endif
+  if (type == DUNE::IMC::PlanDB::getIdStatic()) 
+	{
+    DUNE::IMC::PlanDB * msg = dynamic_cast<DUNE::IMC::PlanDB *>(message);
+    printf("server receive %s: %lf, %d, %d\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->op);
+    printf("%s\n", msg->info.c_str());
+  }
+#if 1
+  if (type == DUNE::IMC::PlanControlState::getIdStatic()) 
+	{
+    DUNE::IMC::PlanControlState * msg = dynamic_cast<DUNE::IMC::PlanControlState *>(message);
+
+    printf("server receive %s: %lf, %s, %s, %u\n",   \ 
+      msg->getName(), msg->getTimeStamp(), msg->plan_id.c_str(), msg->man_id.c_str(), msg->state);
+  }
+  if (type == DUNE::IMC::MsgList::getIdStatic()) 
+	{
+    DUNE::IMC::MsgList * msgList = dynamic_cast<DUNE::IMC::MsgList *>(message);
+    printf("server receive %s: %lf\n", msgList->getName(), msgList->getTimeStamp());
+    DUNE::IMC::MessageList<DUNE::IMC::Message>::const_iterator iter1 = msgList->msgs.begin();
+    for (; iter1 != msgList->msgs.end(); ++iter1)
+    {
+      DUNE::IMC::EntityParameters *entityParameter = static_cast<DUNE::IMC::EntityParameters *>(*iter1);
+      DUNE::IMC::MessageList<DUNE::IMC::EntityParameter>::const_iterator iter2 = entityParameter->params.begin();
+      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;        
+      }
+    }
+  }
+  if (type == DUNE::IMC::EstimatedState::getIdStatic()) 
+	{
+    DUNE::IMC::EstimatedState * msg = dynamic_cast<DUNE::IMC::EstimatedState *>(message);
+    // printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n", 
+    //         msg->getName(), msg->getTimeStamp(), 
+    //         msg->lat, msg->lon, msg->depth,
+    //         msg->phi, msg->theta, msg->psi);
+    printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n", 
+            msg->getName(), msg->getTimeStamp(), 
+            msg->lat*180/M_PI, msg->lon*180/M_PI, msg->depth*180/M_PI,
+            msg->phi*180/M_PI, msg->theta*180/M_PI, msg->psi*180/M_PI);            
+  }
+  if (type == DUNE::IMC::VehicleState::getIdStatic()) 
+	{
+    DUNE::IMC::VehicleState * msg = dynamic_cast<DUNE::IMC::VehicleState *>(message);
+    printf("server receive %s: %lf, %u\n", 
+            msg->getName(), msg->getTimeStamp(), msg->op_mode);            
+  }
+  if (type == DUNE::IMC::PlanControl::getIdStatic()) 
+	{
+    DUNE::IMC::PlanControl * msg = dynamic_cast<DUNE::IMC::PlanControl *>(message);
+    printf("server receive %s: %lf, %u,\n", 
+            msg->getName(), msg->getTimeStamp(), msg->type, msg->plan_id.c_str());            
+  }
+#endif
+  return true;
+}
+
+//---------------------------------------------------------
+// Procedure: OnStartUp()
+//            happens before connection is open
+
+bool ClientViewer::OnStartUp()
+{
+  initLon = -70.330400;
+  initLat = 43.825300;
+  initAlt = 0;
+
+  // udpCommEvent.SetPeriod(1);
+  // udpCommEvent.SetCallback(processMessageCallback,NULL);
+  // udpCommEvent.Start();
+
+  // tcpCommEvent.SetPeriod(1);
+  // tcpCommEvent.SetCallback(processMessageCallback,NULL);
+  // tcpCommEvent.Start();  
+
+#if 1
+  sock_udp_receive.bind(UDP_RECEIVE_PORT, DUNE::Network::Address::Any, false);
+  m_poll_0.add(sock_udp_receive);
+#endif
+
+#if 1
+  try
+  {
+    // sock_tcp_send.setSendTimeout(5);
+    // sock_tcp_send.setReceiveTimeout(5); 
+    sock_tcp_send.connect(TCP_SERVER_ADDRESS, TCP_SEND_PORT);
+    sock_tcp_send.setKeepAlive(true);
+    m_poll_1.add(sock_tcp_send);
+  }
+  catch(const std::exception& e)
+  {
+    std::cerr << e.what() << '\n';
+  }
+#endif
+
+
+#if 1
+  char local_ip2[INET_ADDRSTRLEN] = {0};
+  get_local_ip_using_create_socket(local_ip2);
+  ethernetIP = local_ip2;
+
+#endif
+
+  RegisterVariables();
+
+  return(true);
+}
+
+std::string ClientViewer::SetPlan1(std::string sourceName, double stamp)
+{
+  struct WayPointBehavior behavior;
+  behavior.source = sourceName;
+  behavior.priority = -1;
+  behavior.points.clear();
+  behavior.name = "east_waypt_survey";
+  behavior.priority = 10;
+  struct Landmark station_1 = {"station_1", -70.328891,43.824429, 10, 3, -1, -1, "point"};
+  struct Landmark station_2 = {"station_2", -70.327885,43.824676, 8, 5, -1, -1, "point"};
+  struct Landmark station_3 = {"station_3", -70.327867,43.823622, 6, 7, -1, -1, "point"};
+  struct Landmark station_4 = {"station_4", -70.328765,43.823622, 4, 9, -1, -1, "point"};
+  behavior.points.push_back(station_1);
+  behavior.points.push_back(station_2);
+  behavior.points.push_back(station_3);
+  behavior.points.push_back(station_4);
+  behavior.duration = -1;
+  behavior.constSpeed = -1;
+  behavior.repeat = 1;     
+  behavior.closedLoop = true;
+  behavior.perpetual = false;
+  behavior.minDepth = -1;
+  behavior.maxDepth = -1;
+
+  Json::Value behaviorConfig;
+  behaviorConfig["taskName"] = behavior.name;
+  behaviorConfig["taskId"] = "1";  
+  behaviorConfig["sourceName"] = behavior.source;
+  behaviorConfig["sourceAddress"] = ethernetIP;
+  behaviorConfig["clientStamp"] = stamp;
+  behaviorConfig["boardStamp"] = -1;     
+  behaviorConfig["priority"] = behavior.priority;
+  behaviorConfig["duration"] = behavior.duration;
+  behaviorConfig["closedLoop"] = behavior.closedLoop;
+  behaviorConfig["constSpeed"] = behavior.constSpeed;
+  behaviorConfig["repeat"] = behavior.repeat;
+  behaviorConfig["perpetual"] = behavior.perpetual;
+  behaviorConfig["minDepth"] = behavior.minDepth;
+  behaviorConfig["maxDepth"] = behavior.maxDepth;
+  behaviorConfig["origin"]["lon"] = initLon;
+  behaviorConfig["origin"]["lat"] = initLat;  
+  behaviorConfig["origin"]["altitude"] = initAlt;
+
+  Json::Value station;
+  station["name"] = station_1.name;  
+  station["lon"] = station_1.lon;
+  station["lat"] = station_1.lat;  
+  station["depth"] = station_1.depth;
+  station["speed"] = station_1.speed;
+  station["north"] = station_1.north;
+  station["east"] = station_1.east;
+  station["type"] = station_1.type;
+  behaviorConfig["points"].append(station);
+  station["name"] = station_2.name;    
+  station["lon"] = station_2.lon;
+  station["lat"] = station_2.lat;  
+  station["depth"] = station_2.depth;
+  station["speed"] = station_2.speed;
+  station["north"] = station_2.north;
+  station["east"] = station_2.east;
+  station["type"] = station_2.type;    
+  behaviorConfig["points"].append(station);
+  station["name"] = station_3.name;    
+  station["lon"] = station_3.lon;
+  station["lat"] = station_3.lat;  
+  station["depth"] = station_3.depth;
+  station["speed"] = station_3.speed;
+  station["north"] = station_3.north;
+  station["east"] = station_3.east;
+  station["type"] = station_3.type;    
+  behaviorConfig["points"].append(station);
+  station["name"] = station_4.name;    
+  station["lon"] = station_4.lon;
+  station["lat"] = station_4.lat;  
+  station["depth"] = station_4.depth;
+  station["speed"] = station_4.speed;
+  station["north"] = station_4.north;
+  station["east"] = station_4.east;
+  station["type"] = station_4.type;  
+  behaviorConfig["points"].append(station);
+  Json::StreamWriterBuilder builder;
+  std::string behaviorSpecString = Json::writeString(builder, behaviorConfig);
+
+  return behaviorSpecString;
+}
+
+std::string ClientViewer::SetPlan2(std::string sourceName, double stamp)
+{
+  struct WayPointBehavior behavior;
+  behavior.source = sourceName;
+  behavior.priority = -1;
+  behavior.points.clear();
+  behavior.name = "west_waypt_survey";
+  behavior.priority = 10;
+  struct Landmark station_1 = {"station_1", -70.331532,43.824194, 9, 4, -1, -1, "track"};
+  struct Landmark station_2 = {"station_2", -70.330328,43.824299, 7, 6, -1, -1, "track"};
+  struct Landmark station_3 = {"station_3", -70.330346,43.823518, 5, 8, -1, -1, "track"};
+  struct Landmark station_4 = {"station_4", -70.331406,43.823206, 3, 10, -1, -1, "track"};
+  behavior.points.push_back(station_1);
+  behavior.points.push_back(station_2);
+  behavior.points.push_back(station_3);
+  behavior.points.push_back(station_4);
+  behavior.duration = -1;
+  behavior.constSpeed = -1;
+  behavior.repeat = -1;     
+  behavior.closedLoop = true;
+  behavior.perpetual = true;
+  behavior.minDepth = -1;
+  behavior.maxDepth = -1;
+
+  Json::Value behaviorConfig;
+  behaviorConfig["taskName"] = behavior.name;
+  behaviorConfig["taskId"] = "2";    
+  behaviorConfig["sourceName"] = behavior.source;
+  behaviorConfig["sourceAddress"] = ethernetIP;
+  behaviorConfig["clientStamp"] = stamp;
+  behaviorConfig["boardStamp"] = -1;
+  behaviorConfig["priority"] = behavior.priority;
+  behaviorConfig["duration"] = behavior.duration;
+  behaviorConfig["closedLoop"] = behavior.closedLoop;
+  behaviorConfig["constSpeed"] = behavior.constSpeed;
+  behaviorConfig["repeat"] = behavior.repeat;
+  behaviorConfig["perpetual"] = behavior.perpetual;
+  behaviorConfig["minDepth"] = behavior.minDepth;
+  behaviorConfig["maxDepth"] = behavior.maxDepth;
+  behaviorConfig["origin"]["lon"] = initLon;
+  behaviorConfig["origin"]["lat"] = initLat;  
+  behaviorConfig["origin"]["altitude"] = initAlt;
+  Json::Value station;
+  station["name"] = station_1.name;  
+  station["lon"] = station_1.lon;
+  station["lat"] = station_1.lat;  
+  station["depth"] = station_1.depth;
+  station["speed"] = station_1.speed;
+  station["north"] = station_1.north;
+  station["east"] = station_1.east;
+  station["type"] = station_1.type;   
+  behaviorConfig["points"].append(station);
+  station["name"] = station_2.name;  
+  station["lon"] = station_2.lon;
+  station["lat"] = station_2.lat;  
+  station["depth"] = station_2.depth;
+  station["speed"] = station_2.speed;
+  station["north"] = station_2.north;
+  station["east"] = station_2.east;
+  station["type"] = station_2.type;  
+  behaviorConfig["points"].append(station);
+  station["name"] = station_3.name;
+  station["lon"] = station_3.lon;
+  station["lat"] = station_3.lat;  
+  station["depth"] = station_3.depth;
+  station["speed"] = station_3.speed;
+  station["north"] = station_3.north;
+  station["east"] = station_3.east;
+  station["type"] = station_3.type;  
+  behaviorConfig["points"].append(station);
+  station["name"] = station_4.name;
+  station["lon"] = station_4.lon;
+  station["lat"] = station_4.lat;  
+  station["depth"] = station_4.depth;
+  station["speed"] = station_4.speed;
+  station["north"] = station_4.north;
+  station["east"] = station_4.east;
+  station["type"] = station_4.type;  
+  behaviorConfig["points"].append(station);
+  Json::StreamWriterBuilder builder;
+  std::string behaviorSpecString = Json::writeString(builder, behaviorConfig);
+
+  return behaviorSpecString;
+}
+
+std::string ClientViewer::ModifyPlan1(std::string sourceName, double stamp)
+{
+  struct WayPointBehavior behavior;
+  behavior.source = sourceName;
+  behavior.priority = -1;
+  behavior.points.clear();
+  behavior.name = "east_waypt_survey";
+  behavior.priority = 10;
+  struct Landmark station_1 = {"station_1", -70.328891,43.824429, 9, 2, -1, -1, "track"};
+  struct Landmark station_2 = {"station_2", -70.327885,43.824676, 7, 4, -1, -1, "point"};
+  struct Landmark station_3 = {"station_3", -70.327867,43.823622, 5, 6, -1, -1, "track"};
+  struct Landmark station_4 = {"station_4", -70.328765,43.823622, 3, 8, -1, -1, "point"};
+  behavior.points.push_back(station_1);
+  behavior.points.push_back(station_2);
+  behavior.points.push_back(station_3);
+  behavior.points.push_back(station_4);
+  behavior.duration = -1;
+  behavior.constSpeed = -1;
+  behavior.repeat = 3;     
+  behavior.closedLoop = true;
+  behavior.perpetual = false;
+  behavior.minDepth = -1;
+  behavior.maxDepth = -1;
+
+  Json::Value behaviorConfig;
+  behaviorConfig["taskName"] = behavior.name;
+  behaviorConfig["taskId"] = "1";  
+  behaviorConfig["sourceName"] = behavior.source;
+  behaviorConfig["sourceAddress"] = ethernetIP;
+  behaviorConfig["clientStamp"] = stamp;
+  behaviorConfig["boardStamp"] = -1;  
+  behaviorConfig["priority"] = behavior.priority;
+  behaviorConfig["duration"] = behavior.duration;
+  behaviorConfig["closedLoop"] = behavior.closedLoop;
+  behaviorConfig["constSpeed"] = behavior.constSpeed;
+  behaviorConfig["repeat"] = behavior.repeat;
+  behaviorConfig["perpetual"] = behavior.perpetual;
+  behaviorConfig["minDepth"] = behavior.minDepth;
+  behaviorConfig["maxDepth"] = behavior.maxDepth;
+  behaviorConfig["origin"]["lon"] = initLon;
+  behaviorConfig["origin"]["lat"] = initLat;  
+  behaviorConfig["origin"]["altitude"] = initAlt;  
+  Json::Value station;
+  station["name"] = station_1.name;
+  station["lon"] = station_1.lon;
+  station["lat"] = station_1.lat;  
+  station["depth"] = station_1.depth;
+  station["speed"] = station_1.speed;
+  station["north"] = station_1.north;
+  station["east"] = station_1.east;
+  station["type"] = station_1.type;   
+  behaviorConfig["points"].append(station);
+  station["name"] = station_2.name;    
+  station["lon"] = station_2.lon;
+  station["lat"] = station_2.lat;  
+  station["depth"] = station_2.depth;
+  station["speed"] = station_2.speed;
+  station["north"] = station_2.north;
+  station["east"] = station_2.east;
+  station["type"] = station_2.type;    
+  behaviorConfig["points"].append(station);
+  station["name"] = station_3.name;      
+  station["lon"] = station_3.lon;
+  station["lat"] = station_3.lat;  
+  station["depth"] = station_3.depth;
+  station["speed"] = station_3.speed;
+  station["north"] = station_3.north;
+  station["east"] = station_3.east;
+  station["type"] = station_3.type;  
+  behaviorConfig["points"].append(station);
+  station["name"] = station_4.name;   
+  station["lon"] = station_4.lon;
+  station["lat"] = station_4.lat;  
+  station["depth"] = station_4.depth;
+  station["speed"] = station_4.speed;
+  station["north"] = station_4.north;
+  station["east"] = station_4.east;
+  station["type"] = station_4.type;
+  behaviorConfig["points"].append(station);
+  Json::StreamWriterBuilder builder;
+  std::string behaviorSpecString = Json::writeString(builder, behaviorConfig);
+
+  return behaviorSpecString;
+}
+
+DUNE::IMC::Message * ClientViewer::imcPollUDP() 
+{
+  if (m_poll_0.poll(0)) 
+	{
+    DUNE::Network::Address addr;
+    uint16_t rv = sock_udp_receive.read(udpReceiveBuffer, 65535, &addr);
+    DUNE::IMC::Message * msg = DUNE::IMC::Packet::deserialize(udpReceiveBuffer, rv);
+    return msg;
+  }
+
+  return NULL;
+}
+
+DUNE::IMC::Message * ClientViewer::imcPollTCP() 
+{
+  if (m_poll_1.poll(0)) 
+	{
+    uint16_t rv = sock_tcp_send.read(tcpReceiveBuffer, 65535);
+    DUNE::IMC::Message * msg = DUNE::IMC::Packet::deserialize(tcpReceiveBuffer, rv);
+    return msg;
+  }
+
+  return NULL;
+}
+
+bool ClientViewer::tcpSendToServer(DUNE::IMC::Message * msg, std::string addr, int port) 
+{
+
+  DUNE::Utils::ByteBuffer bb;
+  try {
+      DUNE::IMC::Packet::serialize(msg, bb);
+      return sock_tcp_send.write(bb.getBuffer(), msg->getSerializationSize());
+  }
+  catch (std::runtime_error& e)
+  {
+      MOOSTrace ("ERROR sending %s to %s:%d: %s\n", msg->getName(), addr.c_str(), port, e.what());
+      return false;
+  }
+
+  return true;
+}
+
+
+void ClientViewer::processMessage(DUNE::IMC::Message * message) {
+
+  int type = message->getId();
+  if (type == DUNE::IMC::Announce::getIdStatic()) 
+	{
+    DUNE::IMC::Announce * msg = dynamic_cast<DUNE::IMC::Announce *>(message);
+    // printf("server receive %s: %lf, %lf, %lf, %lf\n",   \ 
+    //   msg->getName(), msg->getTimeStamp(), msg->lat, msg->lon, msg->height);
+    printf("server receive %s: %lf, %lf, %lf, %lf\n",   \ 
+      msg->getName(), msg->getTimeStamp(), msg->lat*180/M_PI, msg->lon*180/M_PI, msg->height);      
+  }
+  else if (type == DUNE::IMC::PlanDB::getIdStatic()) 
+	{
+    DUNE::IMC::PlanDB * msg = dynamic_cast<DUNE::IMC::PlanDB *>(message);
+    printf("server receive %s: %lf, %d, %d\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->op);
+    printf("%s\n", msg->info.c_str());
+  }
+  else if (type == DUNE::IMC::PlanControlState::getIdStatic()) 
+	{
+    DUNE::IMC::PlanControlState * msg = dynamic_cast<DUNE::IMC::PlanControlState *>(message);
+
+    printf("server receive %s: %lf, %s, %s, %u\n",   \ 
+      msg->getName(), msg->getTimeStamp(), msg->plan_id.c_str(), msg->man_id.c_str(), msg->state);
+  }
+  else if (type == DUNE::IMC::MsgList::getIdStatic()) 
+	{
+    DUNE::IMC::MsgList * msgList = dynamic_cast<DUNE::IMC::MsgList *>(message);
+    printf("server receive %s: %lf\n", msgList->getName(), msgList->getTimeStamp());
+    DUNE::IMC::MessageList<DUNE::IMC::Message>::const_iterator iter1 = msgList->msgs.begin();
+    for (; iter1 != msgList->msgs.end(); ++iter1)
+    {
+      DUNE::IMC::EntityParameters *entityParameter = static_cast<DUNE::IMC::EntityParameters *>(*iter1);
+      DUNE::IMC::MessageList<DUNE::IMC::EntityParameter>::const_iterator iter2 = entityParameter->params.begin();
+      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;        
+      }
+    }
+  }
+  else if (type == DUNE::IMC::EstimatedState::getIdStatic()) 
+	{
+    DUNE::IMC::EstimatedState * msg = dynamic_cast<DUNE::IMC::EstimatedState *>(message);
+    // printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n", 
+    //         msg->getName(), msg->getTimeStamp(), 
+    //         msg->lat, msg->lon, msg->depth,
+    //         msg->phi, msg->theta, msg->psi);
+    printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n", 
+            msg->getName(), msg->getTimeStamp(), 
+            msg->lat*180/M_PI, msg->lon*180/M_PI, msg->depth*180/M_PI,
+            msg->phi*180/M_PI, msg->theta*180/M_PI, msg->psi*180/M_PI);            
+  }
+  else if (type == DUNE::IMC::VehicleState::getIdStatic()) 
+	{
+    DUNE::IMC::VehicleState * msg = dynamic_cast<DUNE::IMC::VehicleState *>(message);
+    printf("server receive %s: %lf, %u\n", 
+            msg->getName(), msg->getTimeStamp(), msg->op_mode);            
+  }
+  else if (type == DUNE::IMC::PlanControl::getIdStatic()) 
+	{
+    DUNE::IMC::PlanControl * msg = dynamic_cast<DUNE::IMC::PlanControl *>(message);
+    printf("server receive %s: %lf, %u,\n", 
+            msg->getName(), msg->getTimeStamp(), msg->type, msg->plan_id.c_str());            
+  }
+  else
+  {
+
+  }  
+}
+
+//---------------------------------------------------------
+// Procedure: RegisterVariables
+
+void ClientViewer::RegisterVariables()
+{
+  Register("Command", 0);
+  Register("Key", 0);
+  Register("Value", 0);
+}
+
+int ClientViewer::SetEntityStatus(DUNE::IMC::EntityParameter& subMsg, std::string name, std::string value)
+{
+  int result = -1;
+
+  subMsg.name = name;
+  subMsg.value = value;
+
+  result = 0;
+  return result;
+}
+
+std::string ClientViewer::generateEntityName(std::string parentName, std::string childName)
+{
+  Json::Value outputJsonValue;
+  outputJsonValue["parent"] = parentName;
+  outputJsonValue["child"] = childName;
+  Json::StreamWriterBuilder builder;       
+  std::string outputJsonString = Json::writeString(builder, outputJsonValue);
+  std::cout << outputJsonString << std::endl;
+  return outputJsonString;
+}
+
+std::string ClientViewer::generateEntityValue(std::string value, std::string type)
+{
+  Json::Value outputJsonValue;
+  outputJsonValue["value"] = value;
+  outputJsonValue["type"] = type;
+  Json::StreamWriterBuilder builder;       
+  std::string outputJsonString = Json::writeString(builder, outputJsonValue);
+  std::cout << outputJsonString << std::endl;
+  return outputJsonString;
+}
+
+
+int ClientViewer::get_local_ip_using_create_socket(char* str_ip) 
+{
+    int status = -1;
+    int af = AF_INET;
+    int sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
+    struct sockaddr_in remote_addr;
+    struct sockaddr_in local_addr;
+    char *local_ip = NULL;
+    socklen_t len = 0;
+
+    remote_addr.sin_family = AF_INET;
+    remote_addr.sin_port = htons(53);
+    remote_addr.sin_addr.s_addr = inet_addr("1.1.1.1");
+
+    len =  sizeof(struct sockaddr_in);
+    status = connect(sock_fd, (struct sockaddr*)&remote_addr, len);
+    if(status != 0 ){
+        printf("connect err \n");
+    }
+
+    len =  sizeof(struct sockaddr_in);
+    getsockname(sock_fd, (struct sockaddr*)&local_addr, &len);
+
+    local_ip = inet_ntoa(local_addr.sin_addr);
+    if(local_ip)
+    {
+        strcpy(str_ip, local_ip);
+        status = 0;
+    }
+    return status;
+}

src/pClientViewer/ClientViewer.h

@@ -0,0 +1,100 @@
+/************************************************************/
+/*    NAME: Xiaobin Zeng                                              */
+/*    ORGN: MIT                                             */
+/*    FILE: ClientViewer.h                                          */
+/*    DATE:                                                 */
+/************************************************************/
+
+#ifndef ClientViewer_HEADER
+#define ClientViewer_HEADER
+
+#include <stdio.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <unistd.h>
+#include <errno.h>
+#include <string.h>
+#include <stdlib.h>
+#include <arpa/inet.h>
+
+#include "MOOS/libMOOS/MOOSLib.h"
+#include "MOOS/libMOOS/Utils/MOOSThread.h"
+#include "MOOS/libMOOS/Utils/PeriodicEvent.h"
+#include <DUNE/DUNE.hpp>
+// #include "UDPCommunicationEvent.h"
+// #include "TCPCommunicationEvent.h"
+
+struct Landmark {
+  std::string name;
+	float lon;
+	float lat;
+	float depth;
+	float speed;
+	float north;
+  float east;
+  std::string type;  
+};
+
+struct WayPointBehavior
+{
+	std::string name;
+  std::string source;  
+	int priority;
+	std::vector<Landmark> points;
+	float duration;
+	bool closedLoop;
+	float constSpeed;
+	int repeat;
+	bool perpetual;
+	float minDepth;
+	float maxDepth;
+};
+
+class ClientViewer : public CMOOSApp
+{
+ public:
+   ClientViewer();
+   ~ClientViewer();
+
+ protected: // Standard MOOSApp functions to overload  
+  bool OnNewMail(MOOSMSG_LIST &NewMail);
+  bool Iterate();
+  bool OnConnectToServer();
+  bool OnStartUp();
+  void RegisterVariables();
+  void processMessage(DUNE::IMC::Message * message);
+  
+ private:
+  DUNE::IMC::Message * imcPollTCP();
+  DUNE::IMC::Message * imcPollUDP();  
+  DUNE::Network::TCPSocket sock_tcp_send;
+  DUNE::Network::UDPSocket sock_udp_receive;
+
+  bool tcpSendToServer(DUNE::IMC::Message * msg, std::string addr, int port);
+  int get_local_ip_using_create_socket(char* str_ip);
+  std::string SetPlan1(std::string sourceName, double stamp);
+  std::string SetPlan2(std::string sourceName, double stamp);
+  std::string ModifyPlan1(std::string sourceName, double stamp);
+  int SetEntityStatus(DUNE::IMC::EntityParameter& subMsg, std::string name, std::string value);
+  std::string generateEntityName(std::string parentName, std::string childName);
+  std::string generateEntityValue(std::string value, std::string type);
+
+  DUNE::IO::Poll m_poll_0;
+  DUNE::IO::Poll m_poll_1;  
+  uint8_t* udpReceiveBuffer;
+  uint8_t* tcpReceiveBuffer;
+  std::string SetPlan1();
+  std::string SetPlan2();
+  std::string ethernetIP;
+  float initLon;
+  float initLat;
+  float initAlt; 
+
+  CMOOSThread udpThread;
+
+  // UDPCommunicationEvent udpCommEvent;
+  // TCPCommunicationEvent tcpCommEvent;
+};
+
+#endif 

src/pClientViewer/ClientViewer_Info.cpp

@@ -0,0 +1,115 @@
+/****************************************************************/
+/*   NAME: Xiaobin Zeng                                             */
+/*   ORGN: MIT Cambridge MA                                     */
+/*   FILE: ClientViewer_Info.cpp                               */
+/*   DATE: Dec 29th 1963                                        */
+/****************************************************************/
+
+#include <cstdlib>
+#include <iostream>
+#include "ClientViewer_Info.h"
+#include "ColorParse.h"
+#include "ReleaseInfo.h"
+
+using namespace std;
+
+//----------------------------------------------------------------
+// Procedure: showSynopsis
+
+void showSynopsis()
+{
+  blk("SYNOPSIS:                                                       ");
+  blk("------------------------------------                            ");
+  blk("  The pClientViewer application is used for               ");
+  blk("                                                                ");
+  blk("                                                                ");
+  blk("                                                                ");
+  blk("                                                                ");
+}
+
+//----------------------------------------------------------------
+// Procedure: showHelpAndExit
+
+void showHelpAndExit()
+{
+  blk("                                                                ");
+  blu("=============================================================== ");
+  blu("Usage: pClientViewer file.moos [OPTIONS]                   ");
+  blu("=============================================================== ");
+  blk("                                                                ");
+  showSynopsis();
+  blk("                                                                ");
+  blk("Options:                                                        ");
+  mag("  --alias","=<ProcessName>                                      ");
+  blk("      Launch pClientViewer with the given process name         ");
+  blk("      rather than pClientViewer.                           ");
+  mag("  --example, -e                                                 ");
+  blk("      Display example MOOS configuration block.                 ");
+  mag("  --help, -h                                                    ");
+  blk("      Display this help message.                                ");
+  mag("  --interface, -i                                               ");
+  blk("      Display MOOS publications and subscriptions.              ");
+  mag("  --version,-v                                                  ");
+  blk("      Display the release version of pClientViewer.        ");
+  blk("                                                                ");
+  blk("Note: If argv[2] does not otherwise match a known option,       ");
+  blk("      then it will be interpreted as a run alias. This is       ");
+  blk("      to support pAntler launching conventions.                 ");
+  blk("                                                                ");
+  exit(0);
+}
+
+//----------------------------------------------------------------
+// Procedure: showExampleConfigAndExit
+
+void showExampleConfigAndExit()
+{
+  blk("                                                                ");
+  blu("=============================================================== ");
+  blu("pClientViewer Example MOOS Configuration                   ");
+  blu("=============================================================== ");
+  blk("                                                                ");
+  blk("ProcessConfig = pClientViewer                              ");
+  blk("{                                                               ");
+  blk("  AppTick   = 4                                                 ");
+  blk("  CommsTick = 4                                                 ");
+  blk("                                                                ");
+  blk("}                                                               ");
+  blk("                                                                ");
+  exit(0);
+}
+
+
+//----------------------------------------------------------------
+// Procedure: showInterfaceAndExit
+
+void showInterfaceAndExit()
+{
+  blk("                                                                ");
+  blu("=============================================================== ");
+  blu("pClientViewer INTERFACE                                    ");
+  blu("=============================================================== ");
+  blk("                                                                ");
+  showSynopsis();
+  blk("                                                                ");
+  blk("SUBSCRIPTIONS:                                                  ");
+  blk("------------------------------------                            ");
+  blk("  NODE_MESSAGE = src_node=alpha,dest_node=bravo,var_name=FOO,   ");
+  blk("                 string_val=BAR                                 ");
+  blk("                                                                ");
+  blk("PUBLICATIONS:                                                   ");
+  blk("------------------------------------                            ");
+  blk("  Publications are determined by the node message content.      ");
+  blk("                                                                ");
+  exit(0);
+}
+
+//----------------------------------------------------------------
+// Procedure: showReleaseInfoAndExit
+
+void showReleaseInfoAndExit()
+{
+  showReleaseInfo("pClientViewer", "gpl");
+  exit(0);
+}
+

src/pClientViewer/ClientViewer_Info.h

@@ -0,0 +1,18 @@
+/****************************************************************/
+/*   NAME: Xiaobin Zeng                                             */
+/*   ORGN: MIT Cambridge MA                                     */
+/*   FILE: ClientViewer_Info.h                                      */
+/*   DATE: Dec 29th 1963                                        */
+/****************************************************************/
+
+#ifndef ClientViewer_INFO_HEADER
+#define ClientViewer_INFO_HEADER
+
+void showSynopsis();
+void showHelpAndExit();
+void showExampleConfigAndExit();
+void showInterfaceAndExit();
+void showReleaseInfoAndExit();
+
+#endif
+

src/pClientViewer/main.cpp

@@ -0,0 +1,52 @@
+/************************************************************/
+/*    NAME: Xiaobin Zeng                                              */
+/*    ORGN: MIT                                             */
+/*    FILE: main.cpp                                        */
+/*    DATE:                                                 */
+/************************************************************/
+
+#include <string>
+#include "MBUtils.h"
+#include "ColorParse.h"
+#include "ClientViewer.h"
+#include "ClientViewer_Info.h"
+
+using namespace std;
+
+int main(int argc, char *argv[])
+{
+  string mission_file;
+  string run_command = argv[0];
+
+  for(int i=1; i<argc; i++) {
+    string argi = argv[i];
+    if((argi=="-v") || (argi=="--version") || (argi=="-version"))
+      showReleaseInfoAndExit();
+    else if((argi=="-e") || (argi=="--example") || (argi=="-example"))
+      showExampleConfigAndExit();
+    else if((argi == "-h") || (argi == "--help") || (argi=="-help"))
+      showHelpAndExit();
+    else if((argi == "-i") || (argi == "--interface"))
+      showInterfaceAndExit();
+    else if(strEnds(argi, ".moos") || strEnds(argi, ".moos++"))
+      mission_file = argv[i];
+    else if(strBegins(argi, "--alias="))
+      run_command = argi.substr(8);
+    else if(i==2)
+      run_command = argi;
+  }
+  
+  if(mission_file == "")
+    showHelpAndExit();
+
+  cout << termColor("green");
+  cout << "pClientViewer launching as " << run_command << endl;
+  cout << termColor() << endl;
+
+  ClientViewer ClientViewer;
+
+  ClientViewer.Run(run_command.c_str(), mission_file.c_str());
+  
+  return(0);
+}
+

src/pClientViewer/pClientViewer.moos

@@ -0,0 +1,9 @@
+//------------------------------------------------
+// pClientViewer config block
+
+ProcessConfig = pClientViewer
+{
+   AppTick   = 4
+   CommsTick = 4
+}
+

src/pDataManagement/CMakeLists.txt

@@ -0,0 +1,44 @@
+#--------------------------------------------------------
+# The CMakeLists.txt for:                       pDataManagement
+# Author(s):                              Xiaobin Zeng
+#--------------------------------------------------------
+
+SET(SRC
+  DataManagement.cpp
+  DataManagement_Info.cpp
+  main.cpp
+)
+
+FIND_LIBRARY(DUNE_LIB dune-core /usr/local/lib /usr/local/lib/DUNE)
+FIND_PATH(DUNE_INCLUDE DUNE/IMC.hpp /usr/local/include /usr/local/include/DUNE)
+include_directories(${DUNE_INCLUDE})
+
+# include(FindProtobuf)
+# find_package(Protobuf REQUIRED)
+# include_directories(${Protobuf_INCLUDE_DIR})
+
+include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+
+#find_package (jsoncpp NO_MODULE REQUIRED)
+
+include_directories(/usr/include/jsoncpp/)
+link_directories(/usr/local/lib/)
+
+ADD_EXECUTABLE(pDataManagement ${SRC})
+TARGET_LINK_LIBRARIES(pDataManagement
+   ${MOOS_LIBRARIES}
+   ${CMAKE_DL_LIBS}
+   ${SYSTEM_LIBS} 
+   ${DUNE_LIB}       
+   mbutil
+   m
+   pthread
+   jsoncpp   
+  #  jsoncpp_lib_static
+  #  ${rosbag_LIBRARIES} 
+  #  protobuf
+  #  protoc
+  #  proto   
+  #  ${PROTOBUF_LIBRARY}
+)   
+

src/pDataManagement/DataManagement.cpp

@@ -0,0 +1,194 @@
+/************************************************************/
+/*    NAME: Xiaobin Zeng                                              */
+/*    ORGN: MIT                                             */
+/*    FILE: DataManagement.cpp                                        */
+/*    DATE:                                                 */
+/************************************************************/
+
+#include <iterator>
+#include "MBUtils.h"
+#include "DataManagement.h"
+//#include "NavigationInfo.pb.h"
+#include <json/json.h>
+
+using namespace std;
+
+//---------------------------------------------------------
+// Constructor
+DataManagement::DataManagement()
+{
+}
+
+//---------------------------------------------------------
+// Destructor
+DataManagement::~DataManagement()
+{
+}
+
+//---------------------------------------------------------
+// Procedure: OnNewMail
+bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail)
+{
+  MOOSMSG_LIST::iterator p;
+   
+  for(p=NewMail.begin(); p!=NewMail.end(); p++) 
+  {
+    CMOOSMsg &msg = *p;
+
+    string key   = msg.GetKey();
+    string comm  = msg.GetCommunity();
+    double dval  = msg.GetDouble();
+    string sval  = msg.GetString(); 
+    string msrc  = msg.GetSource();
+    double mtime = msg.GetTime();
+    bool   mdbl  = msg.IsDouble();
+    bool   mstr  = msg.IsString();
+
+    std::string estimatedStateString;
+    if(key == "uMotion_pose_fb")
+    {
+      estimatedStateString = msg.GetString();
+      std::string err;
+      Json::Value estimatedStateData;
+      Json::CharReaderBuilder builder;
+      const std::unique_ptr<Json::CharReader> reader(builder.newCharReader());
+      if (!reader->parse(estimatedStateString.c_str(), 
+                        estimatedStateString.c_str() + static_cast<int>(estimatedStateString.length()), 
+                        &estimatedStateData,
+                        &err)) 
+      {
+        std::cout << "error" << std::endl;
+        return EXIT_FAILURE;    
+    
+      }
+      std::string info = estimatedStateData["info"].asString();
+      float currentLon = estimatedStateData["currentLon"].asFloat();
+      float currentLat = estimatedStateData["currentLat"].asFloat();
+      float currentAltitude = estimatedStateData["currentAltitude"].asFloat();
+      float referenceLon = estimatedStateData["referenceLon"].asFloat();
+      float referenceLat = estimatedStateData["referenceLat"].asFloat();
+      float referenceAltitude = estimatedStateData["referenceAltitude"].asFloat();  
+      float offsetNorth = estimatedStateData["offsetNorth"].asFloat();
+      float offsetEast = estimatedStateData["offsetEast"].asFloat();
+      float offsetDown = estimatedStateData["offsetDown"].asFloat();
+      float roll = estimatedStateData["roll"].asFloat();
+      float pitch = estimatedStateData["pitch"].asFloat();
+      float yaw = estimatedStateData["yaw"].asFloat();
+      float linearVelocityNorth = estimatedStateData["linearVelocityNorth"].asFloat();
+      float linearVelocityEast = estimatedStateData["linearVelocityEast"].asFloat();
+      float linearVelocityDown = estimatedStateData["linearVelocityDown"].asFloat();
+      float angularVelocityNorth = estimatedStateData["angularVelocityNorth"].asFloat();
+      float angularVelocityEast = estimatedStateData["angularVelocityEast"].asFloat();
+      float angularVelocityDown = estimatedStateData["angularVelocityDown"].asFloat();
+      float height = estimatedStateData["height"].asFloat();	
+      float depth = estimatedStateData["depth"].asFloat();
+
+      std::cout << "info: " << info << std::endl;
+      std::cout << "referenceLon: " << referenceLon << std::endl;
+      std::cout << "referenceLat: " << referenceLat << std::endl;
+      std::cout << "referenceAltitude: " << referenceAltitude << std::endl;
+      std::cout << "currentLon: " << currentLon << std::endl;
+      std::cout << "currentLat: " << currentLat << std::endl;
+      std::cout << "altitude: " << currentAltitude << std::endl;
+      std::cout << "offsetNorth: " << offsetNorth << std::endl;
+      std::cout << "offsetEast: " << offsetEast << std::endl;
+      std::cout << "offsetDown: " << offsetDown << std::endl;
+      std::cout << "roll: " << roll << std::endl;
+      std::cout << "pitch: " << pitch << std::endl;
+      std::cout << "yaw: " << yaw << std::endl;
+      std::cout << "linearVelocityNorth: " << linearVelocityNorth << std::endl;
+      std::cout << "linearVelocityEast: " << linearVelocityEast << std::endl;
+      std::cout << "linearVelocityDown: " << linearVelocityDown << std::endl;
+      std::cout << "angularVelocityNorth: " << angularVelocityNorth << std::endl;
+      std::cout << "angularVelocityEast: " << angularVelocityEast << std::endl;
+      std::cout << "angularVelocityDown: " << angularVelocityDown << std::endl;      
+      std::cout << "height: " << height << std::endl;
+      std::cout << "depth: " << depth << std::endl;
+      
+      std::stringstream ss;
+      ss << std::fixed << std::setprecision(6) << getTimeStamp() << ","
+         << info << "," 
+         << referenceLon << ","
+         << referenceLat << ","
+         << referenceAltitude << ","
+         << currentLon << ","
+         << currentLat << ","
+         << currentAltitude << ","
+         << offsetNorth << ","
+         << offsetEast << ","
+         << offsetDown << ","
+         << roll << ","
+         << pitch << ","
+         << yaw << ","
+         << height << ","
+         << depth << std::endl;
+      Notify("uMotion_pose_log", ss.str());
+    }
+   }
+	
+   return(true);
+}
+
+//---------------------------------------------------------
+// Procedure: OnConnectToServer
+
+bool DataManagement::OnConnectToServer()
+{
+   RegisterVariables();
+   return(true);
+}
+
+//---------------------------------------------------------
+// Procedure: Iterate()
+//            happens AppTick times per second
+
+bool DataManagement::Iterate()
+{
+  return(true);
+}
+
+//---------------------------------------------------------
+// Procedure: OnStartUp()
+//            happens before connection is open
+
+bool DataManagement::OnStartUp()
+{
+  list<string> sParams;
+  m_MissionReader.EnableVerbatimQuoting(false);
+  if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) {
+    list<string>::iterator p;
+    for(p=sParams.begin(); p!=sParams.end(); p++) {
+      string line  = *p;
+      string param = tolower(biteStringX(line, '='));
+      string value = line;
+      
+      if(param == "foo") {
+        //handled
+      }
+      else if(param == "bar") {
+        //handled
+      }
+    }
+  }
+
+  RegisterVariables();	
+  return(true);
+}
+
+//---------------------------------------------------------
+// Procedure: RegisterVariables
+
+void DataManagement::RegisterVariables()
+{
+  // Register("FOOBAR", 0);
+  Register("uMotion_pose_fb", 0);
+  //Register("uMotion_pose_log", 0);    
+}
+
+double DataManagement::getTimeStamp()
+{
+  struct timeval tv;
+  gettimeofday(&tv,NULL);
+  double stamp = double(tv.tv_sec*1000000 + tv.tv_usec) / 1000000;
+  return stamp;  
+}

src/pDataManagement/DataManagement.h

@@ -0,0 +1,32 @@
+/************************************************************/
+/*    NAME: Xiaobin Zeng                                              */
+/*    ORGN: MIT                                             */
+/*    FILE: DataManagement.h                                          */
+/*    DATE:                                                 */
+/************************************************************/
+
+#ifndef DataManagement_HEADER
+#define DataManagement_HEADER
+
+#include "MOOS/libMOOS/MOOSLib.h"
+
+class DataManagement : public CMOOSApp
+{
+ public:
+   DataManagement();
+   ~DataManagement();
+
+ protected: // Standard MOOSApp functions to overload  
+   bool OnNewMail(MOOSMSG_LIST &NewMail);
+   bool Iterate();
+   bool OnConnectToServer();
+   bool OnStartUp();
+
+ protected:
+   void RegisterVariables();
+
+ private:
+  double getTimeStamp();
+};
+
+#endif 

src/pDataManagement/DataManagement_Info.cpp

@@ -0,0 +1,115 @@
+/****************************************************************/
+/*   NAME: Xiaobin Zeng                                             */
+/*   ORGN: MIT Cambridge MA                                     */
+/*   FILE: DataManagement_Info.cpp                               */
+/*   DATE: Dec 29th 1963                                        */
+/****************************************************************/
+
+#include <cstdlib>
+#include <iostream>
+#include "DataManagement_Info.h"
+#include "ColorParse.h"
+#include "ReleaseInfo.h"
+
+using namespace std;
+
+//----------------------------------------------------------------
+// Procedure: showSynopsis
+
+void showSynopsis()
+{
+  blk("SYNOPSIS:                                                       ");
+  blk("------------------------------------                            ");
+  blk("  The pDataManagement application is used for               ");
+  blk("                                                                ");
+  blk("                                                                ");
+  blk("                                                                ");
+  blk("                                                                ");
+}
+
+//----------------------------------------------------------------
+// Procedure: showHelpAndExit
+
+void showHelpAndExit()
+{
+  blk("                                                                ");
+  blu("=============================================================== ");
+  blu("Usage: pDataManagement file.moos [OPTIONS]                   ");
+  blu("=============================================================== ");
+  blk("                                                                ");
+  showSynopsis();
+  blk("                                                                ");
+  blk("Options:                                                        ");
+  mag("  --alias","=<ProcessName>                                      ");
+  blk("      Launch pDataManagement with the given process name         ");
+  blk("      rather than pDataManagement.                           ");
+  mag("  --example, -e                                                 ");
+  blk("      Display example MOOS configuration block.                 ");
+  mag("  --help, -h                                                    ");
+  blk("      Display this help message.                                ");
+  mag("  --interface, -i                                               ");
+  blk("      Display MOOS publications and subscriptions.              ");
+  mag("  --version,-v                                                  ");
+  blk("      Display the release version of pDataManagement.        ");
+  blk("                                                                ");
+  blk("Note: If argv[2] does not otherwise match a known option,       ");
+  blk("      then it will be interpreted as a run alias. This is       ");
+  blk("      to support pAntler launching conventions.                 ");
+  blk("                                                                ");
+  exit(0);
+}
+
+//----------------------------------------------------------------
+// Procedure: showExampleConfigAndExit
+
+void showExampleConfigAndExit()
+{
+  blk("                                                                ");
+  blu("=============================================================== ");
+  blu("pDataManagement Example MOOS Configuration                   ");
+  blu("=============================================================== ");
+  blk("                                                                ");
+  blk("ProcessConfig = pDataManagement                              ");
+  blk("{                                                               ");
+  blk("  AppTick   = 4                                                 ");
+  blk("  CommsTick = 4                                                 ");
+  blk("                                                                ");
+  blk("}                                                               ");
+  blk("                                                                ");
+  exit(0);
+}
+
+
+//----------------------------------------------------------------
+// Procedure: showInterfaceAndExit
+
+void showInterfaceAndExit()
+{
+  blk("                                                                ");
+  blu("=============================================================== ");
+  blu("pDataManagement INTERFACE                                    ");
+  blu("=============================================================== ");
+  blk("                                                                ");
+  showSynopsis();
+  blk("                                                                ");
+  blk("SUBSCRIPTIONS:                                                  ");
+  blk("------------------------------------                            ");
+  blk("  NODE_MESSAGE = src_node=alpha,dest_node=bravo,var_name=FOO,   ");
+  blk("                 string_val=BAR                                 ");
+  blk("                                                                ");
+  blk("PUBLICATIONS:                                                   ");
+  blk("------------------------------------                            ");
+  blk("  Publications are determined by the node message content.      ");
+  blk("                                                                ");
+  exit(0);
+}
+
+//----------------------------------------------------------------
+// Procedure: showReleaseInfoAndExit
+
+void showReleaseInfoAndExit()
+{
+  showReleaseInfo("pDataManagement", "gpl");
+  exit(0);
+}
+

src/pDataManagement/DataManagement_Info.h

@@ -0,0 +1,18 @@
+/****************************************************************/
+/*   NAME: Xiaobin Zeng                                             */
+/*   ORGN: MIT Cambridge MA                                     */
+/*   FILE: DataManagement_Info.h                                      */
+/*   DATE: Dec 29th 1963                                        */
+/****************************************************************/
+
+#ifndef DataManagement_INFO_HEADER
+#define DataManagement_INFO_HEADER
+
+void showSynopsis();
+void showHelpAndExit();
+void showExampleConfigAndExit();
+void showInterfaceAndExit();
+void showReleaseInfoAndExit();
+
+#endif
+

src/pDataManagement/NavigationInfo.pb.h

@@ -0,0 +1,809 @@
+// 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/pDataManagement/main.cpp

@@ -0,0 +1,58 @@
+/************************************************************/
+/*    NAME: Xiaobin Zeng                                              */
+/*    ORGN: MIT                                             */
+/*    FILE: main.cpp                                        */
+/*    DATE:                                                 */
+/************************************************************/
+
+#include <string>
+#include "MBUtils.h"
+#include "ColorParse.h"
+#include "DataManagement.h"
+#include "DataManagement_Info.h"
+
+// #include <ros/ros.h>
+// #include <rosbag/bag.h>
+
+using namespace std;
+
+int main(int argc, char *argv[])
+{
+  string mission_file;
+  string run_command = argv[0];
+
+  for(int i=1; i<argc; i++) {
+    string argi = argv[i];
+    if((argi=="-v") || (argi=="--version") || (argi=="-version"))
+      showReleaseInfoAndExit();
+    else if((argi=="-e") || (argi=="--example") || (argi=="-example"))
+      showExampleConfigAndExit();
+    else if((argi == "-h") || (argi == "--help") || (argi=="-help"))
+      showHelpAndExit();
+    else if((argi == "-i") || (argi == "--interface"))
+      showInterfaceAndExit();
+    else if(strEnds(argi, ".moos") || strEnds(argi, ".moos++"))
+      mission_file = argv[i];
+    else if(strBegins(argi, "--alias="))
+      run_command = argi.substr(8);
+    else if(i==2)
+      run_command = argi;
+  }
+  
+  if(mission_file == "")
+    showHelpAndExit();
+
+  cout << termColor("green");
+  cout << "pDataManagement launching as " << run_command << endl;
+  cout << termColor() << endl;
+
+  // ros::init(argc, argv, "log2rosbag");
+  // ros::NodeHandle nh;
+
+  DataManagement DataManagement;
+
+  DataManagement.Run(run_command.c_str(), mission_file.c_str());
+  
+  return(0);
+}
+

src/pDataManagement/pDataManagement.moos

@@ -0,0 +1,9 @@
+//------------------------------------------------
+// pDataManagement config block
+
+ProcessConfig = pDataManagement
+{
+   AppTick   = 4
+   CommsTick = 4
+}
+

src/pEmulator/.LastOpenedMOOSLogDirectory

@@ -0,0 +1 @@
+LastOpenedLoggingDirectory=./MOOSLog_24_10_2023_____11_46_34

src/pEmulator/CMakeLists.txt

@@ -0,0 +1,21 @@
+#--------------------------------------------------------
+# The CMakeLists.txt for:                       pTaskManger
+# Author(s):                              zjk
+#--------------------------------------------------------
+# set(INCLUDE_IVP /home/zjk/Software/moos-ivp/include/ivp)
+
+FILE(GLOB SRC *.cpp *.hpp)
+
+ADD_EXECUTABLE(pEmulator ${SRC})
+#需要把被依赖的库放到依赖库的后面
+TARGET_LINK_LIBRARIES(pEmulator
+   ${MOOS_LIBRARIES}
+   ${MOOSGeodesy_LIBRARIES}
+   contacts
+   geometry
+   apputil
+   mbutil
+   m
+   pthread
+   )
+

src/pEmulator/Emulator.cpp

@@ -0,0 +1,416 @@
+/*
+ * @Author: zjk 1553836110@qq.com
+ * @Date: 2023-10-12 09:52:27
+ * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditTime: 2023-11-08 17:33:41
+ * @FilePath: /moos-ivp-pi/src/pEmulator/Emulator.cpp
+ * @Description: 
+ * 
+ * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
+ */
+// #define DEBUG
+#include"Emulator.hpp"
+#include <exception>
+#include"ACTable.h"
+
+bool _ListenCB(void * pParam)
+{
+  Emulator* pMe = (Emulator* )pParam;
+  return pMe->receiveUdpDate();    
+}
+
+bool _ListenBSC(void* pParam)
+{
+  _150server* pMe = (_150server*)pParam;
+  return pMe->listenInfo();
+}
+
+bool _Connect(void* pParam)
+{
+  Emulator* pMe = (Emulator* )pParam;
+  return pMe->_150Connect();
+}
+
+bool Emulator::OnNewMail(MOOSMSG_LIST &NewMail)
+{
+  AppCastingMOOSApp::OnNewMail(NewMail);
+  MOOSMSG_LIST::iterator p;
+  for(p=NewMail.begin(); p!=NewMail.end(); p++)
+  {
+    CMOOSMsg &msg = *p;
+    string key   = msg.m_sKey;
+    string sval = msg.m_sVal;
+    double dval = msg.m_dfVal;
+    double dfT;
+    msg.IsSkewed(m_curr_time, &dfT);
+    // if(key == "DESIRED_RUDDER")
+    // {
+    //   sendBuf[0] = dval;
+    //   remus100.rudder = dval;
+    // }
+    // else if(key == "DESIRED_ELEVATOR")
+    // {
+    //   sendBuf[1] = dval;
+    //   remus100.elevator = dval;
+    // }
+    // else if(key == "DESIRED_THRUST")
+    // {
+    //   sendBuf[2] = dval;
+    //   remus100.thrust = dval;
+    // }
+    // else 
+    if(key == "DESIRED_DEPTH")
+      sendBuf[3] = dval;
+    else if(key == "DESIRED_HEADING")
+      sendBuf[4] = dval;
+    else if(key == "DESIRED_SPEED")
+      sendBuf[5] = dval;
+    //   reportRunWarning("UnKown var:" + key);
+  }
+  
+  return true;
+}
+bool Emulator::Iterate()
+{
+  AppCastingMOOSApp::Iterate();
+  if(_150ServerThread.IsThreadRunning())
+  { // 这里需要转换数据
+    if(p_150server_1._150cmd.helm_top_angle > 128)
+      sendBuf[0] = -(p_150server_1._150cmd.helm_top_angle - 128);      
+    else
+      sendBuf[0] = p_150server_1._150cmd.helm_top_angle;   
+    if(p_150server_1._150cmd.helm_right_angle > 128) 
+      sendBuf[1] = -(p_150server_1._150cmd.helm_right_angle - 128);
+    else
+      sendBuf[1] = p_150server_1._150cmd.helm_right_angle;
+    sendBuf[2] = (uint8_t)p_150server_1._150cmd.thrust * 1525 / 100;
+    set150Info();
+    p_150server_1.postInfo();
+    isConnect = " 150 Server...";
+  }
+  else
+    isConnect = "No 150 Connect...";
+  udp->iSendMessageTo((void*)sendBuf, sendBufSize, matalb_port, matlab_host);
+  //postNodeRecordUpdate(prefix, record);
+  AppCastingMOOSApp::PostReport(); 
+  return true;
+}
+
+bool Emulator::OnConnectToServer()
+{
+    registerVariables();
+    return true;
+}
+
+bool Emulator::OnStartUp()
+{
+  AppCastingMOOSApp::OnStartUp();
+//TODO: 添加初始状态设置
+  STRING_LIST sParams;
+  m_MissionReader.GetConfiguration(GetAppName(), sParams);
+  STRING_LIST::iterator p;
+  
+  for(p=sParams.begin(); p!=sParams.end(); p++) 
+  {
+      bool unhandled = false;
+      string orig  = *p;
+      string line  = *p;
+      string param = toupper(biteStringX(line, '='));
+      string value = line;
+      double dval  = atof(value.c_str());
+      cout << param << endl;
+      if(param == "MATLAB_HOST")
+      {
+        matlab_host = value;
+      }
+      else if(param == "MATLAB_PORT")
+      {
+        matalb_port = (long int)dval;
+      }
+      else if(param == "LOCAL_PORT")
+      {
+        local_port = (long int)dval;
+      }
+      else if(param == "PREFIX")
+      {
+        setNonWhiteVarOnString(prefix, value);
+      }  
+      else if(param == "START_X")
+      {
+        start_x = dval;
+      }
+      else if(param == "START_Y")
+      {
+        start_y = dval;
+      }
+      else if(param == "START_Z")
+      {
+        start_z = dval;
+      }
+      else if(param == "START_HEADING")
+      {
+        start_heading = dval;
+      }
+      else
+        unhandled = true;
+      if(unhandled)
+        reportUnhandledConfigWarning(orig);
+  }
+  //添加UDP 设置
+  //TODO：添加try
+  try
+  {
+    udp = new XPCUdpSocket(local_port);
+    udp->vSetBroadcast(true);
+    udp->vBindSocket();
+  }
+  catch(const std::exception& e)
+  {
+    std::cerr << e.what() << '\n';
+    reportRunWarning("Local port Open Field");
+  }
+  //初始化数据
+  receiveBufSize = receiveDateSize*sizeof(double);
+  receiveBuf = new char[receiveBufSize];
+  sendBufSize = sendDataSize*sizeof(double);
+  sendBuf = new double[sendDataSize];
+  for(int i=0; i<sendDataSize; i++)
+    sendBuf[i] = 0;
+  for(int i=0; i<receiveBufSize; i++)
+    receiveBuf[i] = 0;
+  // sendBuf[0] = 0.1;
+  // sendBuf[1] = 0.1;
+  // sendBuf[2] = 1525;
+  
+  record.setX(start_x);
+  record.setY(start_y);
+  record.setDepth(start_z);
+  record.setHeading(start_heading);
+
+  receiveThread.Initialise(_ListenCB,this);
+  receiveThread.Start();
+
+  _150ConnectThread.Initialise(_Connect, this);
+  _150ConnectThread.Start();
+  //启动150 server
+  // 
+  // 
+  return true;
+  
+}
+bool Emulator::_150Connect()
+{
+  cout << "Connect ... " << endl;
+  isConnect = "Connect...";
+  p_150server_1._150_startServer();
+  _150ServerThread.Initialise(_ListenBSC,&(this->p_150server_1));
+  _150ServerThread.Start();
+  _150ConnectThread.RequestQuit();
+  isConnect = "Connected and start server";
+}
+void Emulator::registerVariables()
+{
+  AppCastingMOOSApp::RegisterVariables();
+  Register("DESIRED_RUDDER",0);
+  Register("DESIRED_THRUST",0);
+  Register("DESIRED_ELEVATOR",0);
+  Register("DESIRED_DEPTH");
+  Register("DESIRED_HEADING");
+  Register("DESIRED_SPEED");
+}
+bool Emulator::buildReport()
+{
+  // for(int i=0; i<sendDataSize; i++)
+  // {
+  //   m_msgs << sendBuf[i] << "!#";
+  // }
+  m_msgs << "MATLAB      : " << matlab_host << " : " << matalb_port << endl;
+  m_msgs << "LOCAL       : " << local_port << endl;
+  m_msgs << "_150 Connect: " << isConnect << endl;
+  m_msgs << "_150 Cmd Recive: " << p_150server_1._150_recive << endl;
+  m_msgs << "=============================================" << endl;
+  double *s = (double*)receiveBuf;
+  m_msgs << "DESIRED_RUDDER   : " << sendBuf[0] << " (deg)" << endl;
+  m_msgs << "DESIRED_ELEVATOR : " << sendBuf[1] << " (deg)" << endl;
+  m_msgs << "DESIRED_THRUST   : " << sendBuf[2] << " (rpm)" << endl;
+  
+  m_msgs << "Pose  : " << "[ "  <<  doubleToStringX(s[6],1) << " , " <<  doubleToStringX(s[7],1) << " ]" << endl;
+  m_msgs << "Depth : " <<  doubleToStringX(s[8],1) << " (m)" << endl;
+
+  m_msgs << "u     : " << doubleToStringX(s[0],2) << " (m/s)" << endl;
+  m_msgs << "v     : " << doubleToStringX(s[1],2) << " (m/s)" << endl;
+  m_msgs << "w     : " << doubleToStringX(s[2],2) << " (m/s)" << endl;
+
+  m_msgs << "p     : " << doubleToStringX(s[3],1) << " (deg/s)" << endl;
+  m_msgs << "q     : " << doubleToStringX(s[4],1) << " (deg/s)" << endl;
+  m_msgs << "r     : " << doubleToStringX(s[5],1) << " (deg/s)" << endl;
+
+  m_msgs << "roll  : " <<  doubleToStringX(s[9],1) << " (deg)" << endl;
+  m_msgs << "pitch : " <<  doubleToStringX(s[10],1) << " (deg)" << endl;
+  m_msgs << "yaw   : " <<  doubleToStringX(s[11],1) << " (deg)" << endl;
+  
+  m_msgs << "Lat   : " <<  doubleToStringX(s[12],5) << " (du)" << endl;
+  m_msgs << "Lon   : " <<  doubleToStringX(s[13],5) << " (du)" << endl;
+  m_msgs << "Alt   : " <<  doubleToStringX(s[14],5) << " (m)" << endl;
+  
+
+  return(true);
+}
+
+void Emulator::postNodeRecordUpdate(std::string, const NodeRecord& record)
+{
+  double nav_x = record.getX();
+  double nav_y = record.getY();
+
+  Notify(prefix+"_X", nav_x, m_curr_time);
+  Notify(prefix+"_Y", nav_y, m_curr_time);
+
+  if(geoOk) {
+    double nav_lat = record.getLat();
+    double nav_lon = record.getLon();
+    Notify(prefix+"_LAT", nav_lat, m_curr_time);
+    Notify(prefix+"_LONG", nav_lon, m_curr_time);
+  }
+
+  double new_speed = record.getSpeed();
+  new_speed = snapToStep(new_speed, 0.01);
+
+  Notify(prefix+"_HEADING", record.getHeading(), m_curr_time);
+  Notify(prefix+"_SPEED", new_speed, m_curr_time);
+  Notify(prefix+"_DEPTH", record.getDepth(), m_curr_time);
+
+  // Added by HS 120124 to make it work ok with iHuxley
+  // Notify("SIMULATION_MODE","TRUE", m_curr_time);
+  Notify(prefix+"_Z", -record.getDepth(), m_curr_time);
+  Notify(prefix+"_PITCH", record.getPitch(), m_curr_time);
+  Notify(prefix+"_YAW", record.getYaw(), m_curr_time);
+  Notify("TRUE_X", nav_x, m_curr_time);
+  Notify("TRUE_Y", nav_y, m_curr_time);
+
+  double hog = angle360(record.getHeadingOG());
+  double sog = record.getSpeedOG();
+
+  Notify(prefix+"_HEADING_OVER_GROUND", hog, m_curr_time);
+  Notify(prefix+"_SPEED_OVER_GROUND", sog, m_curr_time);
+  
+  if(record.isSetAltitude()) 
+    Notify(prefix+"_ALTITUDE", record.getAltitude(), m_curr_time);
+}
+
+bool Emulator::receiveUdpDate()
+{
+  double u,v,w;
+  double U;
+  while(!receiveThread.IsQuitRequested())
+  {
+    int iRx = udp->iRecieveMessage(receiveBuf,receiveBufSize);
+    if(iRx)
+    {
+      for(int i=0; i<receiveDateSize; i++)
+      {
+        // MOOSTrace(doubleToString(*((double*)receiveBuf+i))+"\n");
+        double data = *((double*)receiveBuf+i);
+        switch (i)
+        {
+        case 0: //u
+          u = data;
+          remus100.u = u*100;
+          break;
+        case 1: //v
+          // record.set
+          v = data;
+          remus100.v = v*100;
+          break;
+        case 2: //w
+          // record
+          w = data;
+          remus100.w = w*100;
+          U = sqrt(u*u + v*v + w*w);
+          record.setSpeed(U);
+          break;
+        case 3: //p
+          remus100.p = data*100;
+          break;
+        case 4: //q
+          remus100.q = data*100;
+          break;
+        case 5: //r
+          remus100.r = data*100;
+          break;
+        case 6: //x
+          remus100.x = data*100;
+          record.setY(data);
+          break;
+        case 7: //y
+          remus100.y = data*100;
+          record.setX(data);
+          break;
+        case 8: //z
+          remus100.z = data*100;
+          record.setDepth(data);
+          break;
+        case 9: //phi
+          remus100.roll = data*100;
+          break;
+        case 10: //theta
+          remus100.pitch = data*100;
+          record.setPitch(data);
+          break;
+        case 11: //psi
+          remus100.yaw = data*100;
+          record.setHeading(data);
+          break;
+        case 12: 
+          remus100.lat = data;
+          record.setLat(data);
+          break;
+        case 13:
+          remus100.lon = data;
+          record.setLon(data);
+          break;
+        case 14:
+          remus100.alt = data;
+          break;
+        default:
+          break;
+        }
+      } 
+    }  
+  }
+}
+
+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.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]
+   p_150server_1.embeddedInfoSrc.pitch = (int16_t)remus100.pitch;       //8:[12,13]
+   p_150server_1.embeddedInfoSrc.roll = (int16_t)remus100.roll;       //9:[14,15]
+   p_150server_1.embeddedInfoSrc.ins_vx = (int16_t)remus100.u;       //10:[16,17]
+   p_150server_1.embeddedInfoSrc.ins_vy = (int16_t)remus100.v;       //11:[18,19]
+   p_150server_1.embeddedInfoSrc.ins_vz = (int16_t)remus100.w;       //12:[20,21]
+   p_150server_1.embeddedInfoSrc.lon = (int32_t)(remus100.lon * 1000000);       //13:[22,23,24,25]
+   p_150server_1.embeddedInfoSrc.lat = (int32_t)(remus100.lat * 1000000)       ;       //14:[26,27,28,29]
+   p_150server_1.embeddedInfoSrc.alt = (int16_t)(remus100.alt * 100);//15:[30,31]
+   p_150server_1.embeddedInfoSrc.dvl_vx = (int16_t)remus100.u;       //16:[32,33]
+   p_150server_1.embeddedInfoSrc.dvl_vy = (int16_t)remus100.v;       //17:[34,35]
+   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_level = 60;       //22:[43]
+   p_150server_1.embeddedInfoSrc.battery_temp = 21.3 / 0.1;       //23:[44,45]
+   p_150server_1.embeddedInfoSrc.fault_leakSensor = 0x80;       //24:[46,47,48,49]
+   p_150server_1.embeddedInfoSrc.fault_battery = 0x2E;       //25:[50] 
+   p_150server_1.embeddedInfoSrc.fault_emergencyBattery = 0xAA;       //26:[51]
+   p_150server_1.embeddedInfoSrc.fault_thrust = 0x76;       //27:[52]
+   p_150server_1.embeddedInfoSrc.iridium = 0xFF;       //28:[53]
+   p_150server_1.embeddedInfoSrc.throwing_load = 0xFF;       //29:[54]
+   p_150server_1.embeddedInfoSrc.dvl_status = 0x00;       //30:[55]
+   p_150server_1.embeddedInfoSrc.crc = 0XFF;       //31:[56]
+   p_150server_1.embeddedInfoSrc.footer = 0XEE1A;       //32:[57,58]
+}

src/pEmulator/Emulator.hpp

@@ -0,0 +1,97 @@
+/*
+ * @Author: zjk 1553836110@qq.com
+ * @Date: 2023-10-12 15:57:27
+ * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditTime: 2023-11-08 17:24:35
+ * @FilePath: /moos-ivp-pi/src/pEmulator/Emulator.hpp
+ * @Description: 
+ * 
+ * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
+ */
+#ifndef __EMULATOR_H
+#define __EMULATOR_H
+#define UNIX
+#include"MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
+#include"MOOS/libMOOS/Comms/MulticastNode.h"
+#include <iostream>
+#include<vector>
+#include "VarDataPair.h"
+#include "MBUtils.h"
+#include "AngleUtils.h"
+#include <list>
+#include"MOOS/libMOOS/Comms/XPCUdpSocket.h"
+#include "NodeRecord.h"
+#include "MOOS/libMOOS/Utils/MOOSThread.h"
+#include "_150server.hpp"
+
+using namespace std;
+typedef struct uuv
+{
+  double u;
+  double v;
+  double w;
+  double p;
+  double q;
+  double r;
+  double x;
+  double y;
+  double z;
+  double roll;
+  double pitch;
+  double yaw;
+  double thrust;
+  double rudder;
+  double elevator;
+  double lon;
+  double lat;
+  double alt;
+};
+
+class Emulator : public AppCastingMOOSApp
+{
+  public:
+  Emulator(){};
+  ~Emulator(){delete receiveBuf;delete sendBuf;};
+  bool OnNewMail(MOOSMSG_LIST &NewMail);
+  bool Iterate();
+  bool OnConnectToServer();
+  bool OnStartUp();
+  void registerVariables();
+  bool buildReport();
+  void postNodeRecordUpdate(std::string, const NodeRecord&);
+  bool receiveUdpDate();
+  void set150Info();
+  bool _150Connect();
+
+  private:
+  long int local_port = 8080;
+  long int matalb_port = 8085;
+  string matlab_host = "127.0.0.1";
+  XPCUdpSocket* udp;
+  unsigned int receiveDateSize = 15;
+  unsigned int sendDataSize = 6;
+  unsigned int receiveBufSize;
+  unsigned int sendBufSize;
+  char *receiveBuf;
+  double *sendBuf;
+  NodeRecord record;
+  string prefix="NAV";
+  bool geoOk = false;
+  
+  
+  //UUV init state
+  double start_x = 0;
+  double start_y = 0;
+  double start_z = 0;
+  double start_heading = 0;
+  uuv remus100;
+  string isConnect = "";
+  //
+  CMOOSThread receiveThread;
+  CMOOSThread _150ServerThread;
+  CMOOSThread _150ConnectThread;
+  //150 Server
+  _150server p_150server_1;
+};
+
+#endif

src/pEmulator/_150server.cpp

@@ -0,0 +1,206 @@
+/*
+ * @Author: zjk 1553836110@qq.com
+ * @Date: 2023-11-07 14:59:47
+ * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditTime: 2023-11-08 09:19:46
+ * @FilePath: /moos-ivp-pi/src/pEmulator/_150server.cpp
+ * @Description: 
+ * 
+ * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
+ */
+#include "_150server.hpp"
+
+uint16_t _150server::serializeFields(AUVEmbedded &embeddedInfo, uint8_t* bfr)
+{
+    memcpy(bfr, &(embeddedInfo.header), sizeof(embeddedInfo.header));
+    bfr += sizeof(embeddedInfo.header);
+
+    memcpy(bfr, &(embeddedInfo.count), sizeof(embeddedInfo.count));
+    bfr += sizeof(embeddedInfo.count);
+
+    memcpy(bfr, &(embeddedInfo.size), sizeof(embeddedInfo.size));
+    bfr += sizeof(embeddedInfo.size);    
+
+    memcpy(bfr, &(embeddedInfo.drive_mode), sizeof(embeddedInfo.drive_mode));
+    bfr += sizeof(embeddedInfo.drive_mode);
+
+    memcpy(bfr, &(embeddedInfo.height), sizeof(embeddedInfo.height));
+    bfr += sizeof(embeddedInfo.height);
+
+    memcpy(bfr, &(embeddedInfo.depth), sizeof(embeddedInfo.depth));
+    bfr += sizeof(embeddedInfo.depth);    
+
+    memcpy(bfr, &(embeddedInfo.yaw), sizeof(embeddedInfo.yaw));
+    bfr += sizeof(embeddedInfo.yaw);
+
+    memcpy(bfr, &(embeddedInfo.pitch), sizeof(embeddedInfo.pitch));
+    bfr += sizeof(embeddedInfo.pitch);
+
+    memcpy(bfr, &(embeddedInfo.roll), sizeof(embeddedInfo.roll));
+    bfr += sizeof(embeddedInfo.roll);    
+
+    memcpy(bfr, &(embeddedInfo.ins_vx), sizeof(embeddedInfo.ins_vx));
+    bfr += sizeof(embeddedInfo.ins_vx);
+
+    memcpy(bfr, &(embeddedInfo.ins_vy), sizeof(embeddedInfo.ins_vy));
+    bfr += sizeof(embeddedInfo.ins_vy);
+
+    memcpy(bfr, &(embeddedInfo.ins_vz), sizeof(embeddedInfo.ins_vz));
+    bfr += sizeof(embeddedInfo.ins_vz);            
+
+    memcpy(bfr, &(embeddedInfo.lon), sizeof(embeddedInfo.lon));
+    bfr += sizeof(embeddedInfo.lon);
+
+    memcpy(bfr, &(embeddedInfo.lat), sizeof(embeddedInfo.lat));
+    bfr += sizeof(embeddedInfo.lat);    
+
+    memcpy(bfr, &(embeddedInfo.alt), sizeof(embeddedInfo.alt));
+    bfr += sizeof(embeddedInfo.alt);
+
+    memcpy(bfr, &(embeddedInfo.dvl_vx), sizeof(embeddedInfo.dvl_vx));
+    bfr += sizeof(embeddedInfo.dvl_vx);
+
+    memcpy(bfr, &(embeddedInfo.dvl_vy), sizeof(embeddedInfo.dvl_vy));
+    bfr += sizeof(embeddedInfo.dvl_vy);    
+
+    memcpy(bfr, &(embeddedInfo.dvl_vz), sizeof(embeddedInfo.dvl_vz));
+    bfr += sizeof(embeddedInfo.dvl_vz);
+
+    memcpy(bfr, &(embeddedInfo.rpm), sizeof(embeddedInfo.rpm));
+    bfr += sizeof(embeddedInfo.rpm);
+
+    memcpy(bfr, &(embeddedInfo.lightEnable), sizeof(embeddedInfo.lightEnable));
+    bfr += sizeof(embeddedInfo.lightEnable); 
+
+    memcpy(bfr, &(embeddedInfo.battery_voltage), sizeof(embeddedInfo.battery_voltage));
+    bfr += sizeof(embeddedInfo.battery_voltage);
+
+    memcpy(bfr, &(embeddedInfo.battery_level), sizeof(embeddedInfo.battery_level));
+    bfr += sizeof(embeddedInfo.battery_level);    
+
+    memcpy(bfr, &(embeddedInfo.battery_temp), sizeof(embeddedInfo.battery_temp));
+    bfr += sizeof(embeddedInfo.battery_temp);
+
+    memcpy(bfr, &(embeddedInfo.fault_leakSensor), sizeof(embeddedInfo.fault_leakSensor));
+    bfr += sizeof(embeddedInfo.fault_leakSensor);
+
+    memcpy(bfr, &(embeddedInfo.fault_battery), sizeof(embeddedInfo.fault_battery));
+    bfr += sizeof(embeddedInfo.fault_battery); 
+
+    memcpy(bfr, &(embeddedInfo.fault_emergencyBattery), sizeof(embeddedInfo.fault_emergencyBattery));
+    bfr += sizeof(embeddedInfo.fault_emergencyBattery); 
+
+    memcpy(bfr, &(embeddedInfo.fault_thrust), sizeof(embeddedInfo.fault_thrust));
+    bfr += sizeof(embeddedInfo.fault_thrust);
+
+    memcpy(bfr, &(embeddedInfo.iridium), sizeof(embeddedInfo.iridium));
+    bfr += sizeof(embeddedInfo.iridium);    
+
+    memcpy(bfr, &(embeddedInfo.throwing_load), sizeof(embeddedInfo.throwing_load));
+    bfr += sizeof(embeddedInfo.throwing_load);
+
+    memcpy(bfr, &(embeddedInfo.dvl_status), sizeof(embeddedInfo.dvl_status));
+    bfr += sizeof(embeddedInfo.dvl_status);
+
+    memcpy(bfr, &(embeddedInfo.crc), sizeof(embeddedInfo.crc));
+    bfr += sizeof(embeddedInfo.crc); 
+
+    memcpy(bfr, &(embeddedInfo.footer), sizeof(embeddedInfo.footer));
+    bfr += sizeof(embeddedInfo.footer); 
+}
+void _150server::postInfo()
+{     
+    bzero(embeddedBuffer, 0);
+    serializeFields(embeddedInfoSrc, embeddedBuffer);
+    write(cfd, embeddedBuffer, 59);
+};
+bool _150server::listenInfo()
+{   
+    while (1)
+    {
+        int lens = read(cfd, recvbuf, sizeof(recvbuf));
+        if(lens>0)
+        {
+            _150_recive++;
+            uint16_t* buf16;
+            printf("header: %u, %u\n", recvbuf[0], recvbuf[1]);
+            printf("count: %u\n", recvbuf[2], recvbuf[3]);
+            printf("size: %u\n", recvbuf[4]);
+            printf("drive_mode: %u\n", recvbuf[5]);
+            printf("thrust: %u\n", recvbuf[6]);
+            _150cmd.thrust = (double)recvbuf[6];
+            printf("yaw: %u, %u\n", recvbuf[7], recvbuf[8]);
+            memcpy(buf16, &recvbuf[7], 2*sizeof(recvbuf[7]));
+            _150cmd.yaw = *((double*)buf16);
+            printf("depth: %u, %u\n", recvbuf[9], recvbuf[10]); 
+            memcpy(buf16, &recvbuf[9], 2*sizeof(recvbuf[9]));
+            _150cmd.depth = *((double*)buf16);
+            printf("helm_top_angle: %u\n", recvbuf[11]);
+            _150cmd.helm_top_angle = (double)recvbuf[11];
+            printf("helm_bottom_angle: %u\n", recvbuf[12]);
+            _150cmd.helm_bottom_angle = (double)recvbuf[12];
+            _150cmd.helm_left_angle = (double)recvbuf[13];
+            printf("helm_left_angle: %u\n", recvbuf[13]);
+            _150cmd.helm_right_angle = (double)recvbuf[14];
+            printf("helm_right_angle: %u\n", recvbuf[14]);
+            _150cmd.light_enable = (double)recvbuf[15];
+            printf("light_enable: %u\n", recvbuf[15]);
+            printf("dvl_enable: %u\n", recvbuf[16]);
+            _150cmd.dvl_enable = (double)recvbuf[16];
+            printf("throwing_load_enable: %u\n", recvbuf[17]);
+            _150cmd.throwing_load_enable = (double)recvbuf[16];
+            printf("crc: %u\n", recvbuf[18]);
+            _150cmd.crc = (double)recvbuf[18];
+            printf("footer: %u, %u\n", recvbuf[19], recvbuf[20]);
+            memcpy(buf16, &recvbuf[19], 2*sizeof(recvbuf[19]));
+            _150cmd.footer = *((double*)buf16);
+        }
+    }
+    return true;
+}
+void _150server::_150_startServer()
+{
+    std::cout << "--------------------" << std::endl;
+    lfd = socket(AF_INET, SOCK_STREAM, 0);
+    if(lfd==-1)
+    {
+        perror("socket");
+        exit(-1);
+    }
+    //2.绑定
+    struct sockaddr_in saddr;
+    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));
+
+    if(ret == -1)
+    {
+        perror("bind");
+        exit(-1);
+    }
+
+    //3.监听
+    listen(lfd, 5);
+    if(ret==-1)
+    {
+        perror("listen");
+        exit(-1);
+    }
+    //4.接受客户端连接
+    struct sockaddr_in caddr;
+    socklen_t len = sizeof(caddr);
+    cfd = accept(lfd, (struct sockaddr *)&caddr, &len);
+
+    if(cfd==-1)
+    {
+        perror("accept");
+        exit(-1);
+    }
+    std::cout << "--------------------" << std::endl;
+    //输出客户端的信息
+    char cip[16];
+    inet_ntop(AF_INET, &caddr.sin_addr.s_addr, cip, sizeof(cip));
+    unsigned short cport = ntohs(caddr.sin_port);
+    printf("client ip is %s,port is %d\n", cip, cport);
+}

src/pEmulator/_150server.hpp

@@ -0,0 +1,95 @@
+/*
+ * @Author: zjk 1553836110@qq.com
+ * @Date: 2023-11-07 14:59:36
+ * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditTime: 2023-11-07 17:12:30
+ * @FilePath: /moos-ivp-pi/src/pEmulator/_150server.hpp
+ * @Description: 
+ * 
+ * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
+ */
+#ifndef __150SERVER_H
+#define __150SERVER_H
+#include <stdio.h>
+#include <arpa/inet.h>
+#include <unistd.h>
+#include <string.h>
+#include <stdlib.h>
+#include <sstream>
+#include <iostream>
+
+struct AUVEmbedded
+{
+  uint16_t header;    //1:[0,1] hx
+  uint16_t count;     //2:[2,3] hu
+  uint8_t size;       //3:[4] u
+  uint8_t drive_mode;       //4:[5] u
+  uint16_t height;       //5:[6,7] hu
+  uint16_t depth;       //6:[8,9] hu
+  uint16_t yaw;       //7:[10,11] hu
+  int16_t pitch;       //8:[12,13] hd
+  int16_t roll;       //9:[14,15] hd
+  int16_t ins_vx;       //10:[16,17] hd
+  int16_t ins_vy;       //11:[18,19] hd
+  int16_t ins_vz;       //12:[20,21] hd
+  int32_t lon;       //13:[22,23,24,25] d
+  int32_t lat;       //14:[26,27,28,29] d
+  int16_t alt;       //15:[30,31] hd
+  int16_t dvl_vx;       //16:[32,33] hd
+  int16_t dvl_vy;       //17:[34,35] hd
+  int16_t dvl_vz;       //18:[36,37] hd
+  int16_t rpm;       //19:[38,39] hd
+  uint8_t lightEnable;       //20:[40] u
+  uint16_t battery_voltage;       //21:[41,42] hu
+  uint8_t battery_level;       //22:[43] u
+  uint16_t battery_temp;       //23:[44,45] hu
+  uint32_t fault_leakSensor;       //24:[46,47,48,49] u
+  uint8_t fault_battery;       //25:[50] u 
+  uint8_t fault_emergencyBattery;       //26:[51] u
+  uint8_t fault_thrust;       //27:[52] u
+  uint8_t iridium;       //28:[53] u
+  uint8_t throwing_load;       //29:[54] u
+  uint8_t dvl_status;       //30:[55] u
+  uint8_t crc;       //31:[56] u
+  uint16_t footer;       //32:[57,58] hu
+};
+struct AUVCmd
+{
+    uint16_t header = 0xEBA2;
+    uint16_t count;
+    uint8_t size;
+    uint8_t mode = 0XFF;
+    uint8_t thrust;
+    uint16_t yaw;
+    uint16_t depth;
+    uint8_t helm_top_angle;
+    uint8_t helm_bottom_angle;
+    uint8_t helm_left_angle;
+    uint8_t helm_right_angle;
+    uint8_t light_enable;
+    uint8_t dvl_enable = 0XFF;
+    uint8_t throwing_load_enable = 0XFF;
+    uint8_t crc;
+    uint16_t footer = 0XEE2A;
+};
+
+class _150server
+{
+private:
+    /* data */
+    int lfd;
+    int cfd;
+public:
+    _150server(/* args */){};
+    ~_150server(){};
+
+    uint16_t serializeFields(AUVEmbedded &embeddedInfo, uint8_t* bfr);
+    bool listenInfo();
+    void postInfo();
+    void _150_startServer();
+    AUVEmbedded embeddedInfoSrc;
+    AUVCmd _150cmd;
+    unsigned char embeddedBuffer[59];
+    unsigned char recvbuf[65535] = {0};
+};
+#endif

src/pEmulator/a.moos

@@ -0,0 +1,249 @@
+//-------------------------------------------------
+// NAME: M. Benjamin, MIT CSAIL
+// FILE: alpha.moos
+//-------------------------------------------------
+
+ServerHost   = localhost
+ServerPort   = 9000
+Community    = alpha
+MOOSTimeWarp = 1
+
+// Forest Lake
+LatOrigin  = 43.825300 
+LongOrigin = -70.330400 
+
+// MIT Sailing Pavilion (use this one)
+// LatOrigin  = 42.358456 
+// LongOrigin = -71.087589
+
+//------------------------------------------
+// Antler configuration  block
+ProcessConfig = ANTLER
+{
+  MSBetweenLaunches = 200
+
+  Run = MOOSDB        @ NewConsole = false
+  Run = pMarineViewer	@ NewConsole = false
+  Run = uProcessWatch	@ NewConsole = false
+  Run = pNodeReporter	@ NewConsole = false
+  Run = pEmulator @ NewConsole = true
+  //Run = pLogger       @ NewConsole = false
+  Run = pMotionControler   @ NewConsole = false
+  Run = pTaskManger   @ NewConsole = false
+  Run = pHelmIvP      @ NewConsole = false
+}
+ProcessConfig = pTaskManger
+{
+  AppTick   = 8
+  CommsTick = 8
+
+  planConfigPath  = /home/zjk/Desktop/project/moos-ivp-extend/PlanConfigure.json
+}
+ProcessConfig = pEmulator
+{
+  AppTick   = 5
+  CommsTick = 5
+  matlab_host = 192.168.0.11
+  matlab_port = 8085
+  local_port = 8080
+  prefix = NAV
+
+  start_x = 10
+  start_y = 9
+  start_z = 1
+  start_heading = 30
+}
+
+ProcessConfig = pLogger
+{
+  AppTick   = 8
+  CommsTick = 8
+
+  AsyncLog = true
+
+  // For variables that are published in a bundle on their first post,
+  // explicitly declare their logging request
+  //Log = IVPHELM_LIFE_EVENT @ 0 NOSYNC
+  //Log = REPORT @ 0 NOSYNC
+  //Log = BHV_SETTINGS @ 0 NOSYNC
+  Log = OPREGION_RESET @ 0 NOSYNC
+
+  LogAuxSrc = true
+  WildCardLogging = true
+  WildCardOmitPattern = *_STATUS
+  WildCardOmitPattern = DB_VARSUMMARY
+  WildCardOmitPattern = DB_RWSUMMARY
+  WildCardExclusionLog = true
+}
+
+//------------------------------------------
+// uProcessWatch config block
+
+ProcessConfig = uProcessWatch
+{
+  AppTick   = 4
+  CommsTick = 4
+
+  watch_all = true
+	nowatch   = uPokeDB*
+	nowatch   = uQueryDB*
+	nowatch   = uXMS*
+	nowatch   = uMAC*
+}
+
+//------------------------------------------
+// uSimMarineV22 config block
+//------------------------------------------
+// pHelmIvP config block
+
+ProcessConfig = pHelmIvP
+{
+  AppTick    = 4
+  CommsTick  = 4
+
+  behaviors  = alpha.bhv
+  domain     = course:0:359:360
+  domain     = speed:0:10:101
+  domain     = depth:0:100:101
+
+  park_on_allstop = false
+  //park_on_allstop = true
+  
+  
+}
+
+//------------------------------------------
+// pMarinePID config block
+
+ProcessConfig = pMotionControler
+{
+  AppTick    = 10
+  CommsTick  = 10
+
+  verbose       = true
+  depth_control = true
+
+  // SIM_INSTABILITY = 20
+
+  // Yaw PID controller
+  yaw_pid_kp		 = 10
+  yaw_pid_kd		 = 0.0
+  yaw_pid_ki		 = 0.01
+  yaw_pid_integral_limit = 10
+
+  // Speed PID controller
+  speed_pid_kp		 = 20.0
+  speed_pid_kd		 = 0.0
+  speed_pid_ki		 = 1
+  speed_pid_integral_limit = 100
+
+  maxpitch = 15
+  maxelevator = 30
+
+  pitch_pid_kp = 1.5
+  pitch_pid_kd = 0
+  pitch_pid_ki = 1.0
+  pitch_pid_integral_limit = 0
+
+  z_to_pitch_pid_kp = 0.12
+  z_to_pitch_pid_kd = 0
+  z_to_pitch_pid_ki = 0.004
+  z_to_pitch_pid_integral_limit = 0.05
+  
+
+  //MAXIMUMS MAXRUDDER
+  maxrudder    = 30
+  maxthrust    = 1525
+
+  // A non-zero SPEED_FACTOR overrides use of SPEED_PID
+  // Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
+  speed_factor = 0
+}
+
+//------------------------------------------
+// pMarineViewer config block
+
+ProcessConfig = pMarineViewer
+{
+  AppTick    = 4
+  CommsTick  = 4
+
+  tiff_file            = forrest19.tif
+	//tiff_file            = MIT_SP.tif
+  vehicles_name_mode = names+depth //+shortmode
+
+
+  set_pan_x            = -90
+  set_pan_y            = -280
+  zoom                 = 0.65
+  vehicle_shape_scale  = 1.5
+  hash_delta           = 50
+  hash_shade           = 0.22
+  hash_viewable        = true
+
+  trails_point_size   = 1
+
+	//op_vertex = x=-83, y=-47,    lwidth=1,lcolor=yellow,looped=true,group=moa
+  //op_vertex = x=-46.4, y=-129.2, lwidth=1,lcolor=yellow,looped=true,group=moa
+  //op_vertex = x=94.6, y=-62.2,   lwidth=1,lcolor=yellow,looped=true,group=moa
+  //op_vertex = x=58,   y=20,    lwidth=1,lcolor=yellow,looped=true,group=moa
+
+  // Appcast configuration
+  appcast_height       = 75
+  appcast_width        = 30
+  appcast_viewable     = true
+  appcast_color_scheme = indigo
+  nodes_font_size      = xlarge
+  procs_font_size      = xlarge
+  appcast_font_size    = large
+  
+  // datum_viewable = true
+  // datum_size     = 18
+  // gui_size = small
+
+  // left_context[survey-point] = DEPLOY=true
+  // left_context[survey-point] = MOOS_MANUAL_OVERRIDE=false
+  // left_context[survey-point] = RETURN=false
+
+  right_context[return] = DEPLOY=true
+  right_context[return] = MOOS_MANUAL_OVERRIDE=false
+  right_context[return] = RETURN=false
+
+  scope  = RETURN
+  scope  = WPT_STAT
+  scope  = VIEW_SEGLIST
+  scope  = VIEW_POINT
+  scope  = VIEW_POLYGON
+  scope  = MVIEWER_LCLICK
+  scope  = MVIEWER_RCLICK
+
+  //button_one = START # START=true
+  //button_one = MOOS_MANUAL_OVERRIDE=false
+  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_two = MOOS_MANUAL_OVERRIDE=true
+  button_three = FaultClear # ClearFalut = true
+  button_four = SendSecurityZone # SendSaftRules = true
+
+
+  action  = MENU_KEY=deploy # DEPLOY = true # RETURN = false
+  action+ = MENU_KEY=deploy # MOOS_MANUAL_OVERRIDE=false
+  action  = RETURN=true
+  action  = UPDATES_RETURN=speed=1.4
+}
+
+//------------------------------------------
+// pNodeReporter config block
+
+ProcessConfig = pNodeReporter
+{
+  AppTick    = 2
+  CommsTick	 = 2
+
+  //platform_type   = kayak
+  //更改显示形状为uuv
+  platform_type = UUV 
+  platform_color  = red
+  platform_length = 4
+}

src/pEmulator/alpha.bhv

@@ -0,0 +1,181 @@
+//--------    FILE: alpha.bhv   -------------
+initialize RUN = false
+initialize TaskNum=t1
+initialize SendTask=false
+//--------------模式判断------------------------
+set MODE = ACTIVE{
+  RUN=true
+} INACTIVE
+
+set MODE = T1{
+  MODE=ACTIVE
+  TaskNum = t1
+}
+
+
+//----------路径点任务----------------------------
+Behavior = BHV_Waypoint
+{
+  name      = waypt_survey
+  pwt       = 100 //优先权重
+  condition = MODE==T1
+
+  //endflag   = START=false
+  endflag = END_WayPoint=true
+
+	configflag = CRUISE_SPD = $[SPEED]
+	//configflag = OSPOS = $[OSX],$[OSY]
+
+	activeflag = INFO=$[OWNSHIP]
+	activeflag = INFO=$[BHVNAME]
+	activeflag = INFO=$[BHVTYPE]
+
+	//cycleflag = CINFO=$[OSX],$[OSY]
+
+  wptflag = CurrentPointComplete=true
+	wptflag = PREV=$(PX),$(PY)
+	wptflag = NEXT=$(NX),$(NY)
+	wptflag = TEST=$(X),$(Y)
+	wptflag = OSPOS=$(OSX),$(OSY)
+	//wptflag_on_start = true
+  
+
+  updates    = WPT_UPDATE
+  //perpetual  = true
+
+  templating = spawn
+
+	//    speed_alt = 1.2
+  //use_alt_speed = true
+                lead = 8
+         lead_damper = 1
+       lead_to_start = false
+               speed = 1   // meters per second
+        capture_line = true
+      capture_radius = 5.0
+         slip_radius = 15.0
+	efficiency_measure = all
+
+               polygon = 60,-40
+               order = normal
+             //repeat  = 3
+
+    visual_hints = nextpt_color=yellow
+    visual_hints = nextpt_vertex_size=8
+    visual_hints = nextpt_lcolor=gray70
+    visual_hints = vertex_color=dodger_blue, edge_color=white
+    visual_hints = vertex_size=5, edge_size=1
+}
+
+
+//--------------定深任务------------------
+Behavior=BHV_ConstantDepth
+{
+  name = const_depth
+  pwt = 100
+  //condition = DEPLOY = true
+  condition = MODE==T1
+  duration = no-time-limit
+  updates = DEPTH_UPDATE
+  depth = 0
+}
+
+//--------------定向任务--------------------
+
+Behavior=BHV_ConstantHeading
+{
+  name       = const_heading
+  pwt        = 100
+  //condition  = START_TURN = true
+  //condition  = DEPLOY = true
+  condition = MODE==T3
+  perpetual  = true
+
+  activeflag = TURN = started
+
+  //endflag    = TURN = done
+  //endflag    = RETURN = true
+  //endflag    = START_TURN = false
+  endflag    = START=false
+
+          heading = 225
+  complete_thresh = 5
+       duration = no-time-limit
+ }
+
+//--------------定速任务--------------------
+Behavior=BHV_ConstantSpeed
+{
+  name       = const_speed
+  pwt        = 1000
+  condition = MODE==T1
+  perpetual  = true
+  updates    = SPEED_UPDATE
+  //endflag    = START=false
+
+  speed = 5
+
+  duration = no-time-limit
+  //peakwidth = 0.5
+  //basewidth = 0.5
+
+}
+//----------------安全模式-----------------------
+//----------------计时器---------------------
+Behavior = BHV_Timer
+{ 
+  name      = mtime
+  condition = MODE==T1
+  pwt = 100
+  templating = spawn
+	//duration_status = MSTATUS
+	//duration = 10
+  endflag = TIME_OUT=true
+
+  updates      = TIMER_UPDATES        
+  
+  //perpetual  = true
+}
+//-------------最大深度限制--------------------------
+Behavior = BHV_MaxDepth
+{
+  name = maxdepth
+  pwt  = 200
+  condition = MODE==ACTIVE
+  updates = MAXDEEP_UPDATES
+  max_depth = 20
+  tolerance = 0
+  duration = no-time-limit
+}
+//--------------安全区域设置-----------------------
+
+ Behavior = BHV_OpRegion
+ {
+   // General Behavior Parameters
+   // ---------------------------
+   name         = op_region                 // example
+   pwt          = 300                       // default
+   condition = MODE==TN
+   updates      = OPREGION_UPDATES          // example
+
+   // Parameters specific to this behavior
+   // ------------------------------------
+                  max_time = 20              // default (seconds)
+                 max_depth = 25              // default (meters)
+              min_altitude = 0              // default (meters)
+                 reset_var = OPREGION_RESET // example
+        trigger_entry_time = 1              // default (seconds)
+         trigger_exit_time = 0.5            // default (seconds) 
+
+                   polygon = pts={-80,-00:-30,-175:150,-100:95,25}
+
+    breached_altitude_flag = TaskFault = AltitudeOut
+       breached_depth_flag = TaskFault = DepthOut
+        breached_poly_flag = TaskFault = RegionOut
+        breached_time_flag = TaskFault = TimeOut
+
+              visual_hints = vertex_color = brown  // default
+              visual_hints = vertex_size  = 3      // default
+              visual_hints = edge_color   = aqua   // default
+              visual_hints = edge_size    = 1      // default
+ }

src/pEmulator/main.cpp

@@ -0,0 +1,63 @@
+/*
+ * @Author: zjk 1553836110@qq.com
+ * @Date: 2023-10-12 09:52:06
+ * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditTime: 2023-10-20 16:52:52
+ * @FilePath: /moos-ivp-extend/src/pEmulator/main.cpp
+ * @Description: 
+ * 
+ * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
+ */
+#include<iostream>
+using namespace std;
+#include"MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
+// #include "VarDataPair.h"
+#include"Emulator.hpp"
+#include <iostream>
+#include<string>
+#include "MBUtils.h"
+#include "ColorParse.h"
+// #include "MBUtils.h"
+// #include "ColorParse.h"
+
+int main(int argc, char *argv[])
+{
+  string mission_file;
+  string run_command = argv[0];
+
+  for(int i=1; i<argc; i++) {
+    string argi = argv[i];
+    if((argi=="-v") || (argi=="--version") || (argi=="-version"))
+      // showReleaseInfoAndExit();
+      cout << " Version : 0.01" << endl;
+    else if((argi=="-e") || (argi=="--example") || (argi=="-example"))
+      // showExampleConfigAndExit();
+      cout << " example : NULL" << endl;
+    else if((argi=="-h") || (argi == "--help") || (argi=="-help"))
+      // showHelpAndExit();
+      cout << " Low leave Control for UUV " << endl;
+    else if((argi=="-i") || (argi == "--interface"))
+      // showInterfaceAndExit();
+      cout << " UUV Motion Control " << endl;
+    else if(strEnds(argi, ".moos") || strEnds(argi, ".moos++"))
+      mission_file = argv[i];
+    else if(strBegins(argi, "--alias="))
+      run_command = argi.substr(8);
+    else if(i == 2)
+      run_command = argi;
+  }
+  
+  if(mission_file == "")
+      // showHelpAndExit();
+
+  cout << termColor("green");
+  cout << "Emulator launching as " << run_command << endl;
+  cout << termColor() << endl;
+
+  Emulator s;
+  s.Run(run_command.c_str(), mission_file.c_str(), argc, argv);
+
+  return(0);
+
+  return(0);
+}

src/pEmulator/pEmulator.moos

@@ -0,0 +1,15 @@
+ProcessConfig = pEmulator
+{
+  AppTick   = 5
+  CommsTick = 5
+  //matlab_host = 192.168.0.11
+  matlab_host = 10.25.0.206
+  matlab_port = 8085
+  local_port = 8080
+  prefix = NAV
+
+  start_x = 0
+  start_y = 0
+  start_z = 0
+  start_heading = 30
+}

src/pMotionControler/CMakeLists.txt

@@ -0,0 +1,22 @@
+#--------------------------------------------------------
+# The CMakeLists.txt for:                       pTaskManger
+# Author(s):                              zjk
+#--------------------------------------------------------
+
+FILE(GLOB SRC *.cpp *.hpp)
+
+include_directories(/usr/include/jsoncpp/)
+link_directories(/usr/local/lib/)
+
+ADD_EXECUTABLE(pMotionControler ${SRC})
+
+#需要把被依赖的库放到依赖库的后面
+TARGET_LINK_LIBRARIES(pMotionControler
+   ${MOOS_LIBRARIES}
+   geometry
+   mbutil
+   m
+   pthread
+   jsoncpp
+   )
+

src/pMotionControler/ControlParam.json

@@ -0,0 +1,43 @@
+{
+	"speed" : 
+	{
+		"Kp" : 10.0,
+		"Ki" : 5.0,
+		"Kd" : 0.0,
+		"LimitDelta" : 50,
+		"MaxOut" : 100,
+		"MinOut" : 0
+	},
+	"heading" :
+	{
+		"Kp" : 0.8,
+		"Ki" : 0.05,
+		"Kd" : 2.2,
+		"LimitDelta" : 5,
+		"MaxOut" : 30,
+		"MinOut" : -30
+	},
+	"depth" : 
+	{
+		"Kp" : 10.0,
+		"Ki" : 0.3,
+		"Kd" : 2.0,
+		"LimitDelta" : 5,
+		"MaxOut" : 10,    
+		"MinOut" : -10
+	},
+	"pitch" : 
+	{
+		"Kp" : 0.6,
+		"Ki" : 0.03,
+		"Kd" : 1.5,
+		"LimitDelta" : 5,
+		"MaxOut" : 30,
+		"MinOut" : -30
+	},
+	"const_thrust" : 0,
+	"dead_zone" : 10,
+	"speedCol" : true,
+	"depthCol" : true,
+	"HeadingCol" : true
+}

src/pMotionControler/Controler.cpp

@@ -0,0 +1,424 @@
+/*
+ * @Author: zjk 1553836110@qq.com
+ * @Date: 2023-10-16 14:04:53
+ * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditTime: 2023-11-02 15:17:38
+ * @FilePath: /moos-ivp-pi/src/pMotionControler/Controler.cpp
+ * @Description: 
+ * 
+ * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
+ */
+#include"Controler.hpp"
+
+Controler::Controler()
+{
+    // Error_capacity = 10;
+    initVariable();
+}
+
+void Controler::initVariable()
+{
+    ClearValTim(desired_speed);
+    ClearValTim(desired_heading);
+    ClearValTim(desired_depth);
+    ClearValTim(desired_pitch);
+
+    ClearValTim(current_speed);
+    ClearValTim(current_heading);
+    ClearValTim(current_pitch);
+    ClearValTim(current_depth);
+    current_time = 0;
+    start_time = 0;
+
+    ClearValTim(desired_thrust);
+    ClearValTim(desired_rudder);
+    ClearValTim(desired_elevator);
+
+    iterations = 0;
+    start_time = 0.0;
+
+    tardy_helm_thresh = 2.0;
+    tardy_nav_thresh  = 2.0;
+    // current_error = 0;
+    // last_error = 0;
+    // Error.clear();
+}
+
+bool Controler::setDesSpeed(double spd, double tim)
+{
+    desired_speed.value = spd;
+    desired_speed.time = tim;
+    return true;
+}
+bool Controler::setDesDepth(double dph, double tim)
+{
+    desired_depth.value = dph;
+    desired_depth.time = tim;
+    return true;
+}
+bool Controler::setDesPitch(double pth, double tim)
+{
+    desired_pitch.value = pth;
+    desired_pitch.time = tim;
+    return true;
+}
+bool Controler::setDesHeading(double hdg, double tim)
+{
+    if(is_rad)
+        hdg = radToDegrees(hdg);
+
+    desired_heading.value = angle180(hdg);
+    desired_heading.time = tim;
+    return true;
+}
+bool Controler::setCurSpeed(double spd, double tim)
+{
+    current_speed.value = spd;
+    current_speed.time = tim;
+    return true;
+}
+bool Controler::setCurDepth(double dph, double tim)
+{
+    current_depth.value = dph;
+    current_depth.time = tim;
+    return true;
+}
+bool Controler::setCurPitch(double pth, double tim)
+{
+    current_pitch.value = pth;
+    current_pitch.time = tim;
+    return true;
+}
+bool Controler::setCurHeading(double hdg, double tim)
+{
+    current_heading.value = hdg;
+    current_heading.time = tim;
+    return true;
+}
+bool Controler::setDesiredValues()
+{
+    iterations++;
+
+    desired_thrust.value = 0;
+    desired_rudder.value = 0;
+    desired_elevator.value = 0;
+
+    //=============================================================
+    // Part 1: Ensure all nav and helm messages have been received 
+    // for first time. If not, we simply wait, without declaring an
+    // override. After all messages have been received at least
+    // once, staleness is checked below.
+    //=============================================================
+    runMsg.push_back("desired_heading-time:" + doubleToString(desired_heading.time));
+    runMsg.push_back("desired_speed-time:" + doubleToString(desired_speed.time));
+    runMsg.push_back("current_heading-time:" + doubleToString(current_heading.time));
+    runMsg.push_back("current_speed-time:" + doubleToString(current_speed.time));
+    runMsg.push_back("desired_depth-time:" + doubleToString(desired_depth.time));
+    runMsg.push_back("current_depth-time:" + doubleToString(current_depth.time));
+    runMsg.push_back("current_pitch-time:" + doubleToString(current_pitch.time));
+    // runMsg.push_back("current_depth-time:" + doubleToString(current_depth.time));
+
+    if(has_speedCtrl)
+    {
+        if((desired_heading.time == 0) || (desired_speed.time == 0))
+            return true;
+    }
+    if(has_headCtrl)
+    {
+        if((current_heading.time == 0) || (current_speed.time == 0))
+            return true;
+    }
+    if(has_depthCtrl) 
+    {
+        if(desired_depth.time == 0)
+            return true;
+        if((current_depth.time == 0) || (current_pitch.time == 0))
+            return true;
+    }
+    //=============================================================
+    // Part 2: Critical info staleness (if not in simulation mode)
+    //=============================================================
+    //TODO: 验证信息过时操作
+    if(cheakStalensee) 
+    {
+        bool is_stale = checkForStaleness();
+        if(is_stale) 
+        {
+            has_override = true;
+            return false;
+        }
+    }
+
+    //=============================================================
+    // Part 3: Set the actuator values. Note: If desired thrust is 
+    // zero others desired values will automatically be zero too.
+    //=============================================================
+    
+    // m_desired_thrust = setDesiredThrust();
+
+    // if(m_desired_thrust > 0)
+    // m_desired_rudder = setDesiredRudder();
+
+    // if(m_desired_thrust > 0 && m_depth_control) 
+    // m_desired_elevator = setDesiredElevator();
+    return true;
+}
+//TODO： 添加操控权判断
+bool Controler::overrived(string sval)
+{
+    if(tolower(sval) == "true")
+    {
+        if(has_override == false)
+            addPosting("HAS_CONTROL", "false");    
+        has_override = true;
+    }
+    else if(tolower(sval) == "false") 
+    {
+    // Upon lifting an override, the timestamps are reset. New values
+    // for all will need to be received before desired_* outputs will
+    // be produced.  If we do not reset, it's possible we may
+    // interpret older pubs as being stale but they may have been
+    // paused also during an override.
+        if(has_override == true) 
+        {
+            desired_speed.time = 0;
+            desired_heading.time = 0;
+            desired_depth.time = 0;
+            desired_pitch.time = 0;
+
+            current_speed.time = 0;
+            current_heading.time = 0;
+            current_pitch.time = 0;
+            current_depth.time = 0;
+        }
+        has_override = false;
+        addPosting("HAS_CONTROL", "true");
+    }
+    return true;
+}
+
+bool Controler::checkForStaleness()
+{
+    bool is_stale = false;
+
+    // =========================================================
+    // Part 1: Check for Helm staleness
+    // =========================================================
+    double hdg_delta = (current_time - desired_heading.time);
+    if(hdg_delta > tardy_helm_thresh) {
+    string staleness = doubleToStringX(hdg_delta,3);
+    addPosting("PID_STALE", "Stale DesHdg:" + staleness);
+    is_stale = true;
+    }
+    double spd_delta = (current_time - desired_speed.time);
+    if(spd_delta > tardy_helm_thresh) {
+    string staleness = doubleToStringX(spd_delta,3);
+    addPosting("PID_STALE", "Stale DesSpd:" + staleness);
+    is_stale = true;
+    }
+
+    // =========================================================
+    // Part 2B: Check for Nav staleness
+    // =========================================================
+    double nav_hdg_delta = (current_time - current_heading.time);
+    if(nav_hdg_delta > tardy_nav_thresh) {
+    string staleness = doubleToStringX(nav_hdg_delta,3);
+    addPosting("PID_STALE", "Stale NavHdg:" + staleness);
+    is_stale = true;
+    }
+    double nav_spd_delta = (current_time - current_speed.time);
+    if(nav_spd_delta > tardy_nav_thresh) {
+    string staleness = doubleToStringX(nav_spd_delta,3);
+    addPosting("PID_STALE", "Stale NavSpd:" + staleness);
+    is_stale = true;
+    }
+    // =========================================================
+    // Part 2C: If depth control, check for Helm Depth staleness 
+    // =========================================================
+    if(has_depthCtrl) {
+    double dep_delta = (current_time - desired_depth.time);
+    if(dep_delta > tardy_helm_thresh) {
+        string staleness = doubleToStringX(dep_delta,3);
+        addPosting("PID_STALE", "Stale DesDepth:" + staleness);
+        is_stale = true;
+    }
+    }
+
+    // =========================================================
+    // Part 2D: If depth control, check for Nav Depth staleness 
+    // =========================================================
+    if(has_depthCtrl) {    
+    double nav_dep_delta = (current_time - current_depth.time);
+    if(nav_dep_delta > tardy_nav_thresh) {
+        string staleness = doubleToStringX(nav_dep_delta,3);
+        addPosting("PID_STALE", "Stale NavDep:" + staleness);
+        is_stale = true;
+    }
+    double nav_pit_delta = (current_time - current_pitch.time);
+    if(nav_pit_delta > tardy_nav_thresh) {
+        string staleness = doubleToStringX(nav_pit_delta,3);
+        addPosting("PID_STALE", "Stale NavPitch:" + staleness);
+        is_stale = true;
+    }
+    }
+    return(is_stale);
+}
+
+// bool Controler::setLimit(double max_thrush, double min_thrush, double max_rudder, double min_rubber, double max_elevator, double min_elevator)
+// {
+//     limit_thrust[0] = min_thrush;
+//     limit_thrust[1] = max_thrush;
+
+//     limit_rudder[0] = min_rubber;
+//     limit_rudder[1] = max_rudder;
+
+//     limit_elevator[0] = min_elevator;
+//     limit_elevator[1] = max_elevator;
+// }
+
+bool Controler::Limit(double &a, double max, double min)
+{
+    a = (a < min) ? min : a;
+    a = (a > max) ? max : a;
+    return true;
+}
+
+bool Controler::setCtrl(bool speed, bool heading, bool depth)
+{
+    has_speedCtrl = speed;
+    has_headCtrl = heading;
+    has_depthCtrl = depth;
+    return true;
+}
+
+void Controler::addPosting(std::string var, std::string sval)
+{
+    VarDataPair pair(var, sval);
+    postings.push_back(pair);
+}
+
+void Controler::addPosting(std::string var, double val)
+{
+    VarDataPair pair(var, val);
+    postings.push_back(pair);
+}
+
+double Controler::getFrequency() const
+{
+    double elapsed = current_time - start_time;
+
+    double frequency = 0;
+    if(elapsed > 0) 
+    frequency = ((double)(iterations)) / elapsed;
+
+    return(frequency);
+}
+
+bool Controler::ClearValTim(vlaTim &a)
+{
+    if(&a == NULL)
+        return false;
+    else
+    {
+        a.value = 0;
+        a.time = 0;
+    }
+
+}
+//TODO: 控制器参数配置函数
+list<std::string> Controler::setConfigParams(std::list<std::string>)
+{
+  list<string> unhandled_params;
+  list<string>::iterator p;
+//   for(p=param_lines.begin(); p!=param_lines.end(); p++) {
+//     string orig  = *p;
+//     string line  = tolower(orig);
+//     string param = biteStringX(line, '=');
+//     if(param == "speed_factor")
+//       m_config_params.push_front(orig);
+//     else if(param == "simulation")
+//       m_config_params.push_front(orig);
+//     else if(param == "sim_instability")
+//       m_config_params.push_front(orig);
+//     else if(param == "tardy_helm_threshold")
+//       m_config_params.push_front(orig);
+//     else if(param == "tardy_nav_threshold")
+//       m_config_params.push_front(orig);
+//     else if(param == "yaw_pid_kp")
+//       m_config_params.push_front(orig);
+//     else if(param == "yaw_pid_kd")
+//       m_config_params.push_front(orig);
+//     else if(param == "yaw_pid_ki")
+//       m_config_params.push_front(orig);
+//     else if(param == "yaw_pid_integral_limit")
+//       m_config_params.push_front(orig);
+//     else if(param == "yaw_pid_ki_limit")
+//       m_config_params.push_front(orig);
+//     else if(param == "maxrudder")
+//       m_config_params.push_front(orig);
+//     else if(param == "heading_debug")
+//       m_config_params.push_front(orig);
+
+//     else if(param == "speed_pid_kp")
+//       m_config_params.push_front(orig);
+//     else if(param == "speed_pid_kd")
+//       m_config_params.push_front(orig);
+//     else if(param == "speed_pid_ki")
+//       m_config_params.push_front(orig);
+//     else if(param == "speed_pid_integral_limit")
+//       m_config_params.push_front(orig);
+//     else if(param == "maxthrust")
+//       m_config_params.push_front(orig);
+//     else if(param == "speed_debug")
+//       m_config_params.push_front(orig);
+
+//     else if(param == "depth_control")
+//       m_config_params.push_front(orig);
+//     else if(param == "z_to_pitch_pid_kp")
+//       m_config_params.push_front(orig);
+//     else if(param == "z_to_pitch_pid_kd")
+//       m_config_params.push_front(orig);
+//     else if(param == "z_to_pitch_pid_ki")
+//       m_config_params.push_front(orig);
+//     else if(param == "z_to_pitch_pid_integral_limit")
+//       m_config_params.push_front(orig);
+//     else if(param == "maxpitch")
+//       m_config_params.push_front(orig);
+//     else if(param == "depth_debug")
+//       m_config_params.push_front(orig);
+
+//     else if(param == "pitch_pid_kp")
+//       m_config_params.push_front(orig);
+//     else if(param == "pitch_pid_kd")
+//       m_config_params.push_front(orig);
+//     else if(param == "pitch_pid_ki")
+//       m_config_params.push_front(orig);
+//     else if(param == "pitch_pid_integral_limit")
+//       m_config_params.push_front(orig);
+//     else if(param == "maxelevator")
+//       m_config_params.push_front(orig);
+
+//     else
+//       unhandled_params.push_front(orig);    
+//   }
+  return(unhandled_params);
+}
+
+Json::Value Controler::getConfig(string path)
+{  
+    ifstream ifs;
+    ifs.open(path, ios::in);
+    Json::Reader taskConfigureReader;
+    Json::Value inputJsonValue;
+    taskConfigureReader.parse(ifs, inputJsonValue);
+    ifs.close();
+    return inputJsonValue;
+}
+
+void Controler::setCheakStalensee(string s)
+{
+    if(s == "true")
+        cheakStalensee = true;
+    else
+        cheakStalensee = false;
+}

src/pMotionControler/Controler.hpp

@@ -0,0 +1,155 @@
+/*
+ * @Author: zjk 1553836110@qq.com
+ * @Date: 2023-10-16 14:05:16
+ * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditTime: 2023-11-02 14:31:16
+ * @FilePath: /moos-ivp-pi/src/pMotionControler/Controler.hpp
+ * @Description: 
+ * 
+ * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
+ */
+#ifndef __COBTROLLER_H
+#define __CONTROLLER_H
+#include<iostream>
+#include<vector>
+#include "VarDataPair.h"
+#include "MBUtils.h"
+#include "AngleUtils.h"
+#include <list>
+#include "json/json.h"
+#include <fstream>
+#include <map>
+
+// #include "VarDataPair.h"
+
+using namespace std;
+
+typedef struct
+{
+    double value;
+    double time;
+} vlaTim;
+
+class Controler
+{
+    public:
+    Controler();
+    ~Controler(){};
+
+    virtual int step(){return 0;}
+    virtual bool setConfigParams(){return false;}
+    virtual bool overrived(string sval);
+
+    virtual bool handleYawSettings(){return false;}
+    virtual bool handleSpeedSettings(){return false;};
+    virtual bool handleDepthSettings(){return false;};
+    virtual bool hasConfigSettings() const{return false;};
+    // bool setError(double err);
+
+    bool setDesSpeed(double spd, double tim);
+    bool setDesDepth(double dph, double tim);
+    bool setDesPitch(double pth, double tim);
+    bool setDesHeading(double hdg, double tim);
+
+    bool setCurSpeed(double spd, double tim);
+    bool setCurDepth(double dph, double tim);
+    bool setCurPitch(double pth, double tim);
+    bool setCurHeading(double hdg, double tim);
+    // bool setCurTime(double tim){current_time = tim;}
+    void updateTime(double tim){current_time = tim;}
+    double getCurTime(){return current_time;}
+    void setStartTime(double tim){start_time = tim;}
+    void setOverride(bool v){has_override = v;}
+    void setTardyHelm(double t){ tardy_helm_thresh = t;}
+    void setTardyNav(double t){ tardy_nav_thresh = t;}
+    void setCheakStalensee(string s);
+    void setConstThrust(double v){const_thrust = v;}
+    void setDeadZone(double v){dead_zone=v;}
+    void setDepthControl(bool v){has_depthCtrl=v;}
+    void setSpeedControl(bool v){has_speedCtrl=v;}
+    void setHeadingControl(bool v){has_headCtrl=v;}
+    bool setDesiredValues();
+
+    // bool setLimit(double max_thrush, double min_thrush, double max_rudder, double min_rubber, double max_elevator, double min_elevator);
+    bool setCtrl(bool speed, bool heading, bool depth);
+    bool Limit(double &a, double max, double min);
+
+    bool checkForStaleness();
+    void addPosting(std::string var, std::string sval);
+    void addPosting(std::string var, double val);
+    vector<VarDataPair> getPostings() {return(postings);}
+    void clearPostings() {postings.clear();}
+    bool ClearValTim(vlaTim &a);
+    list<std::string> setConfigParams(std::list<std::string>); //使用MOOS风格配置参数函数
+    Json::Value getConfig(string path);
+
+    double getFrequency() const;
+    bool hasControl(){return(!has_override);}
+    bool hasSpdCtrl(){return has_speedCtrl;}
+    bool hasDphCtrl(){return has_depthCtrl;}
+    bool hasHdgCtrl(){return has_headCtrl;}
+
+    double getDesiredRudder() const   {return(desired_rudder.value);}
+    double getDesiredThrust() const   {return(desired_thrust.value);}
+    double getDesiredElevator() const {return(desired_elevator.value);}
+    // bool setErrorCap(int c){Error_capacity = c;}
+
+    inline void initVariable();
+    list<string> getConfigParams(){return config_params;}
+    vector<string> getConfigErrors(){return config_errors;}
+    vector<string> getConfigInfo(){return config_info;}
+    vector<string> getRunMsg(){return runMsg;}
+    Json::Value getReport(){return RepList;}
+    void ClearRunMsg(){runMsg.clear();}
+
+    protected:
+    vlaTim desired_speed;
+    vlaTim desired_heading;
+    vlaTim desired_depth;
+    vlaTim desired_pitch;
+
+    vlaTim current_speed;
+    vlaTim current_heading;
+    vlaTim current_pitch;
+    vlaTim current_depth;
+    double current_time;
+    double start_time;
+
+    vlaTim desired_thrust;
+    vlaTim desired_rudder;
+    vlaTim desired_elevator;
+
+    double limit_thrust[2];
+    double limit_rudder[2];
+    double limit_elevator[2];
+    double max_rudder;
+    double max_thrust;
+    double max_pitch;
+    double max_elevator;
+
+    bool has_speedCtrl = true;
+    bool has_headCtrl = true;
+    bool has_depthCtrl = true;
+    bool has_override = true;
+    bool cheakStalensee = true; 
+
+    vector<VarDataPair> postings;
+    unsigned int iterations;
+
+    double tardy_helm_thresh;
+    double tardy_nav_thresh;
+    double const_thrust;
+    double dead_zone;
+    
+
+
+    double is_rad = false;
+
+    list<string> config_params;
+    vector<string> config_errors;
+    vector<string> config_info;
+    vector<string> runMsg;
+    Json::Value RepList;  
+};
+
+#endif

src/pMotionControler/MotionControler.cpp

@@ -0,0 +1,242 @@
+/*
+ * @Author: zjk 1553836110@qq.com
+ * @Date: 2023-10-12 09:52:27
+ * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditTime: 2023-11-07 12:01:57
+ * @FilePath: /moos-ivp-pi/src/pMotionControler/MotionControler.cpp
+ * @Description: 
+ * 
+ * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
+ */
+// #define DEBUG
+#include"MotionControler.hpp"
+//TODO：增加启用哪个控制器功能
+bool MotionControler::OnNewMail(MOOSMSG_LIST &NewMail)
+{
+  AppCastingMOOSApp::OnNewMail(NewMail);
+  MOOSMSG_LIST::iterator p;
+  for(p=NewMail.begin(); p!=NewMail.end(); p++)
+  {
+    CMOOSMsg &msg = *p;
+
+    string key  = msg.m_sKey;
+    string sval = msg.m_sVal;
+    double dval = msg.m_dfVal;
+    double dfT;
+    msg.IsSkewed(m_curr_time, &dfT);
+    if(fabs(dfT) < ok_skew) 
+    {
+      if((key == "MOOS_MANUAL_OVERIDE") || (key == "MOOS_MANUAL_OVERRIDE"))
+        pengine.overrived(sval);
+      else if(key == "DESIRED_HEADING")
+        pengine.setDesHeading(dval, MOOSTime());
+      else if(key == "DESIRED_SPEED")
+        pengine.setDesSpeed(dval, MOOSTime());
+      else if(key == "DESIRED_DEPTH") 
+        pengine.setDesDepth(dval, MOOSTime());
+      else if(key == "NAV_HEADING")
+        pengine.setCurHeading(angle360(dval), MOOSTime());
+      else if(key == "NAV_SPEED")
+        pengine.setCurSpeed(dval, MOOSTime());
+      else if(key == "NAV_DEPTH")
+        pengine.setCurDepth(dval, MOOSTime());
+      else if(key == "NAV_PITCH")
+        pengine.setCurPitch(dval, MOOSTime());      
+      else if(key == MSG_ReadConfig) //重新读取配置参数可以清除故障码
+      {
+        int e = pengine.setParam(configFilePath);
+        pengine.setOverride(true);
+        if(e != 0)
+          faultCode = 10 + e;
+        else
+          faultCode = 0;
+      }
+    }
+  }
+    return true;
+}
+bool MotionControler::Iterate()
+{
+    bool a;
+    AppCastingMOOSApp::Iterate();
+    pengine.updateTime(m_curr_time);
+    int i = pengine.step();
+    switch (i)
+    {
+      case 0:
+        RepList["State : "] = "Run";
+        break;
+      case 1:
+        RepList["State : "] = "Ready";
+        break;      
+      case -1:
+        RepList["State : "] = "Fault";
+        faultCode = 1;//信息超时
+        break;  
+      default:
+        break;
+    }
+    postPengineResults();
+    postPenginePostings();
+    postCharStatus();
+    Notify(MSG_FALUT,faultCode);
+    AppCastingMOOSApp::PostReport(); 
+    return true;
+}
+bool MotionControler::OnConnectToServer()
+{
+    registerVariables();
+    return true;
+}
+bool MotionControler::OnStartUp()
+{
+  AppCastingMOOSApp::OnStartUp();
+  pengine.setStartTime(MOOSTime());
+  STRING_LIST sParams;
+  m_MissionReader.GetConfiguration(GetAppName(), sParams);
+  //pengine.setConfigParams(sParams);
+  //bool handled = true;
+  STRING_LIST::iterator p;
+  for(p=sParams.begin(); p!=sParams.end(); p++) {
+      string orig  = *p;
+      string line  = (orig); //识别大小写
+      string param = biteStringX(line, '=');
+      string value = line;
+      double dval  = atof(value.c_str());
+      if(param == "config_file")
+        configFilePath = value;
+      else if(param == "tardy_helm_thresh")
+        pengine.setTardyHelm(dval);
+      else if(param == "tardy_nav_thresh")
+        pengine.setTardyNav(dval);
+      else if(param == "cheak_stalensee")
+        pengine.setCheakStalensee(value);
+      else if(param == "AppTick")
+        setFrequency = dval;
+      else if(param == "delta_freqency")
+        frequency_delta = dval;
+      else
+        reportUnhandledConfigWarning(orig);
+  }
+  int e = pengine.setParam(configFilePath);
+  if(e != 0)
+  {
+    faultCode = 10 + e;
+    return false;
+  }
+  return true;
+}
+
+void MotionControler::registerVariables()
+{
+      AppCastingMOOSApp::RegisterVariables();
+
+      Register("NAV_HEADING", 0);
+      Register("NAV_SPEED", 0);
+      Register("NAV_DEPTH", 0);
+      Register("NAV_PITCH", 0);
+      Register("DESIRED_HEADING", 0);
+      Register("DESIRED_SPEED", 0);
+      Register("DESIRED_DEPTH", 0);
+      Register("PID_VERBOSE", 0);
+      Register("SPEED_FACTOR", 0);
+      Register("MOOS_MANUAL_OVERIDE", 0);
+      Register("MOOS_MANUAL_OVERRIDE", 0);
+      Register(MSG_ReadConfig,0);
+}
+bool MotionControler::buildReport()
+{
+  double frequency = pengine.getFrequency();
+  double delta_freq =  100.0*(setFrequency - frequency) / setFrequency;
+  if(abs(delta_freq) > frequency_delta)
+    faultCode = 2;
+
+  m_msgs << "Frequency Delta  : " << frequency  << endl;
+  m_msgs << "PID has_control  : " << boolToString(pengine.hasControl()) << endl;
+  m_msgs << "Config File Path : " << configFilePath << endl;
+  m_msgs << "S : H : D        : | " << intToString(pengine.hasSpdCtrl())+ " | " 
+  << intToString(pengine.hasHdgCtrl())+" | " 
+  << intToString(pengine.hasDphCtrl())+" |" << endl;
+
+  RepList["to BS"] = colVar;
+  RepList["PID"] = pengine.getReport()["W"];
+  string rep = Json::writeString(RepJsBuilder, RepList);
+  m_msgs << rep << endl;
+
+  return(true);
+}
+
+void MotionControler::postPenginePostings()
+{
+    vector<VarDataPair> m_postings = pengine.getPostings();
+    for(unsigned int i=0; i<m_postings.size(); i++)
+    {
+        VarDataPair pair = m_postings[i];
+        string var = pair.get_var();
+        if(pair.is_string() && pair.get_sdata_set())
+        Notify(var, pair.get_sdata());
+        else if(!pair.is_string() && pair.get_ddata_set())
+        Notify(var, pair.get_ddata());
+    }
+    pengine.clearPostings();
+}
+void MotionControler::postPengineResults()
+{
+  bool all_stop = true;
+  if(pengine.hasControl())
+      all_stop = false;
+
+  if(all_stop) 
+  {
+    if(allstop_posted)
+      return;
+    Notify("DESIRED_RUDDER", 0.0);
+    Notify("DESIRED_THRUST", 0.0);
+    // if(pengine.hasDphCtrl())
+    Notify("DESIRED_ELEVATOR", 0.0);
+    postColVarToBS(0,0,0);
+    allstop_posted = true;
+  }
+  else 
+  {
+    int T=0,S=0,R=0;
+    if(pengine.hasHdgCtrl())
+    {
+      Notify("DESIRED_RUDDER", pengine.getDesiredRudder());
+      R = (int)pengine.getDesiredRudder();
+    }
+    if(pengine.hasSpdCtrl())
+    {
+      Notify("DESIRED_THRUST", pengine.getDesiredThrust());
+      T = (int)pengine.getDesiredThrust();
+    }
+      
+    if(pengine.hasDphCtrl())
+    {
+      Notify("DESIRED_ELEVATOR", pengine.getDesiredElevator());
+      S = pengine.getDesiredElevator();
+    }
+    postColVarToBS(T,S,R);
+    allstop_posted = false;
+  }
+}
+void MotionControler::postCharStatus()
+{
+  if(!verbose)
+    return;
+
+  if(pengine.hasControl())
+    cout << "$" << flush;
+  else 
+    cout << "*" << flush;
+}
+void MotionControler::postColVarToBS(int T, int S, int R)
+{
+  string s_msg;
+  colVar["mode"] = 2;
+  colVar["thrust"] = T;
+  colVar["rudder"] = R;
+  colVar["elevator"] = S;
+  s_msg = Json::writeString(RepJsBuilder,colVar);
+  Notify(MSG_TO_BS, s_msg);
+}

src/pMotionControler/MotionControler.hpp

@@ -0,0 +1,66 @@
+/*
+ * @Author: zjk 1553836110@qq.com
+ * @Date: 2023-10-12 15:57:27
+ * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditTime: 2023-11-07 11:33:04
+ * @FilePath: /moos-ivp-pi/src/pMotionControler/MotionControler.hpp
+ * @Description: 
+ * 
+ * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
+ */
+#ifndef TESTAPP
+#define TESEAPP
+#include"MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
+#include <iostream>
+#include"pidControl.hpp"
+#include<vector>
+// #include"MOOS/libMOOS/Comms/XPCUdpSocket.h"
+using namespace std;
+class MotionControler : public AppCastingMOOSApp
+{
+  public:
+  MotionControler(){};
+  ~MotionControler(){};
+  bool OnNewMail(MOOSMSG_LIST &NewMail);
+  bool Iterate();
+  bool OnConnectToServer();
+  bool OnStartUp();
+  void registerVariables();
+  bool buildReport();
+  
+  void postPenginePostings();
+  void postPengineResults();
+  void postColVarToBS(int T, int S, int R);
+  void postCharStatus();
+
+  const string MSG_FALUT = "uMotion_fault_fb";
+  const string MSG_ReadConfig = "uMotion_config_cmd";
+  const string MSG_TO_BS = "uMotion_control_cmd";
+
+  private:
+  bool ignore_nav_yaw;
+  bool allstop_posted;
+  bool verbose;
+  bool override;
+  double ok_skew = 2;
+  int faultCode = 0;
+  double setFrequency;
+  double frequency_delta;
+  
+  Json::Value RepList;
+  Json::StreamWriterBuilder RepJsBuilder;
+  Json::Value colVar;
+
+  pidControl pengine;
+  string configFilePath;
+  int e;
+
+
+  
+};
+
+#endif
+
+//1.读取参数配置错误 faultCode = 10~
+//2.信息过时错误 faultCode=1
+//3.频率相差过大错误 faultCode=2

src/pMotionControler/a.moos

@@ -0,0 +1,46 @@
+ServerHost = localhost
+ServerPort = 9000
+
+ProcessConfig = pMotionControler
+{
+    AppTick   = 20
+    CommsTick = 20
+    verbose       = true
+    depth_control = true
+
+    // SIM_INSTABILITY = 20
+
+    // Yaw PID controller
+    yaw_pid_kp		 = 1.2
+    yaw_pid_kd		 = 0.0
+    yaw_pid_ki		 = 0.3
+    yaw_pid_integral_limit = 0.07
+
+    // Speed PID controller
+    speed_pid_kp		 = 1.0
+    speed_pid_kd		 = 0.0
+    speed_pid_ki		 = 0.1
+    speed_pid_integral_limit = 0.07
+
+    maxpitch = 15
+    maxelevator = 13
+
+    pitch_pid_kp = 1.5
+    pitch_pid_kd = 0
+    pitch_pid_ki = 1.0
+    pitch_pid_integral_limit = 0
+
+    z_to_pitch_pid_kp = 0.12
+    z_to_pitch_pid_kd = 0
+    z_to_pitch_pid_ki = 0.004
+    z_to_pitch_pid_integral_limit = 0.05
+
+
+    //MAXIMUMS
+    maxrudder    = 100
+    maxthrust    = 100
+
+    // A non-zero SPEED_FACTOR overrides use of SPEED_PID
+    // Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
+    speed_factor = 0
+}

src/pMotionControler/alpha.bhv

@@ -0,0 +1,181 @@
+//--------    FILE: alpha.bhv   -------------
+initialize RUN = false
+initialize TaskNum=t1
+initialize SendTask=false
+//--------------模式判断------------------------
+set MODE = ACTIVE{
+  RUN=true
+} INACTIVE
+
+set MODE = T1{
+  MODE=ACTIVE
+  TaskNum = t1
+}
+
+
+//----------路径点任务----------------------------
+Behavior = BHV_Waypoint
+{
+  name      = waypt_survey
+  pwt       = 100 //优先权重
+  condition = MODE==T1
+
+  //endflag   = START=false
+  endflag = END_WayPoint=true
+
+	configflag = CRUISE_SPD = $[SPEED]
+	//configflag = OSPOS = $[OSX],$[OSY]
+
+	activeflag = INFO=$[OWNSHIP]
+	activeflag = INFO=$[BHVNAME]
+	activeflag = INFO=$[BHVTYPE]
+
+	//cycleflag = CINFO=$[OSX],$[OSY]
+
+  wptflag = CurrentPointComplete=true
+	wptflag = PREV=$(PX),$(PY)
+	wptflag = NEXT=$(NX),$(NY)
+	wptflag = TEST=$(X),$(Y)
+	wptflag = OSPOS=$(OSX),$(OSY)
+	//wptflag_on_start = true
+  
+
+  updates    = WPT_UPDATE
+  //perpetual  = true
+
+  templating = spawn
+
+	//    speed_alt = 1.2
+  //use_alt_speed = true
+                lead = 8
+         lead_damper = 1
+       lead_to_start = false
+               speed = 1   // meters per second
+        capture_line = true
+      capture_radius = 5.0
+         slip_radius = 15.0
+	efficiency_measure = all
+
+               polygon = 60,-40
+               order = normal
+             //repeat  = 3
+
+    visual_hints = nextpt_color=yellow
+    visual_hints = nextpt_vertex_size=8
+    visual_hints = nextpt_lcolor=gray70
+    visual_hints = vertex_color=dodger_blue, edge_color=white
+    visual_hints = vertex_size=5, edge_size=1
+}
+
+
+//--------------定深任务------------------
+Behavior=BHV_ConstantDepth
+{
+  name = const_depth
+  pwt = 100
+  //condition = DEPLOY = true
+  condition = MODE==T1
+  duration = no-time-limit
+  updates = DEPTH_UPDATE
+  depth = 0
+}
+
+//--------------定向任务--------------------
+
+Behavior=BHV_ConstantHeading
+{
+  name       = const_heading
+  pwt        = 100
+  //condition  = START_TURN = true
+  //condition  = DEPLOY = true
+  condition = MODE==T3
+  perpetual  = true
+
+  activeflag = TURN = started
+
+  //endflag    = TURN = done
+  //endflag    = RETURN = true
+  //endflag    = START_TURN = false
+  endflag    = START=false
+
+          heading = 225
+  complete_thresh = 5
+       duration = no-time-limit
+ }
+
+//--------------定速任务--------------------
+Behavior=BHV_ConstantSpeed
+{
+  name       = const_speed
+  pwt        = 1000
+  condition = MODE==T1
+  perpetual  = true
+  updates    = SPEED_UPDATE
+  //endflag    = START=false
+
+  speed = 5
+
+  duration = no-time-limit
+  //peakwidth = 0.5
+  //basewidth = 0.5
+
+}
+//----------------安全模式-----------------------
+//----------------计时器---------------------
+Behavior = BHV_Timer
+{ 
+  name      = mtime
+  condition = MODE==T1
+  pwt = 100
+  templating = spawn
+	//duration_status = MSTATUS
+	//duration = 10
+  endflag = TIME_OUT=true
+
+  updates      = TIMER_UPDATES        
+  
+  //perpetual  = true
+}
+//-------------最大深度限制--------------------------
+Behavior = BHV_MaxDepth
+{
+  name = maxdepth
+  pwt  = 200
+  condition = MODE==ACTIVE
+  updates = MAXDEEP_UPDATES
+  max_depth = 20
+  tolerance = 0
+  duration = no-time-limit
+}
+//--------------安全区域设置-----------------------
+
+ Behavior = BHV_OpRegion
+ {
+   // General Behavior Parameters
+   // ---------------------------
+   name         = op_region                 // example
+   pwt          = 300                       // default
+   condition = MODE==T5
+   updates      = OPREGION_UPDATES          // example
+
+   // Parameters specific to this behavior
+   // ------------------------------------
+                  max_time = 20              // default (seconds)
+                 max_depth = 25              // default (meters)
+              min_altitude = 0              // default (meters)
+                 reset_var = OPREGION_RESET // example
+        trigger_entry_time = 1              // default (seconds)
+         trigger_exit_time = 0.5            // default (seconds) 
+
+                   polygon = pts={-80,-00:-30,-175:150,-100:95,25}
+
+    breached_altitude_flag = TaskFault = AltitudeOut
+       breached_depth_flag = TaskFault = DepthOut
+        breached_poly_flag = TaskFault = RegionOut
+        breached_time_flag = TaskFault = TimeOut
+
+              visual_hints = vertex_color = brown  // default
+              visual_hints = vertex_size  = 3      // default
+              visual_hints = edge_color   = aqua   // default
+              visual_hints = edge_size    = 1      // default
+ }

src/pMotionControler/alpha.moos

@@ -0,0 +1,285 @@
+//-------------------------------------------------
+// NAME: M. Benjamin, MIT CSAIL
+// FILE: alpha.moos
+//-------------------------------------------------
+
+ServerHost   = localhost
+ServerPort   = 9000
+Community    = alpha
+MOOSTimeWarp = 1
+
+// Forest Lake
+LatOrigin  = 43.825300 
+LongOrigin = -70.330400 
+
+// MIT Sailing Pavilion (use this one)
+// LatOrigin  = 42.358456 
+// LongOrigin = -71.087589
+
+//------------------------------------------
+// Antler configuration  block
+ProcessConfig = ANTLER
+{
+  MSBetweenLaunches = 200
+
+  Run = MOOSDB        @ NewConsole = false
+  Run = pLogger       @ NewConsole = false
+  //Run = uSimMarineV22 @ NewConsole = false
+  //Run = pMarinePIDV22 @ NewConsole = false
+  Run = pHelmIvP      @ NewConsole = false
+  Run = pMarineViewer	@ NewConsole = false
+  Run = uProcessWatch	@ NewConsole = false
+  Run = pNodeReporter	@ NewConsole = false
+  Run = pRealm	      @ NewConsole = false
+  Run = pTaskManger   @ NewConsole = false
+  Run = pMotionControler   @ NewConsole = false
+  Run = pEmulator @ NewConsole = true
+  //Run = uTimerScript  @ NewConsole = false
+}
+ProcessConfig = pTaskManger
+{
+  AppTick   = 8
+  CommsTick = 8
+
+  planConfigPath  = /home/zjk/Desktop/project/moos-ivp-extend/PlanConfigure.json
+}
+ProcessConfig = pEmulator
+{
+  AppTick   = 5
+  CommsTick = 5
+  matlab_host = 192.168.0.11
+  matlab_port = 8085
+  local_port = 8080
+  prefix = NAV
+
+  start_x = 10
+  start_y = 9
+  start_z = 1
+  start_heading = 30
+}
+
+//------------------------------------------
+// pLogger config block
+
+ProcessConfig = pLogger
+{
+  AppTick   = 8
+  CommsTick = 8
+
+  AsyncLog = true
+
+  // For variables that are published in a bundle on their first post,
+  // explicitly declare their logging request
+  //Log = IVPHELM_LIFE_EVENT @ 0 NOSYNC
+  //Log = REPORT @ 0 NOSYNC
+  //Log = BHV_SETTINGS @ 0 NOSYNC
+  Log = OPREGION_RESET @ 0 NOSYNC
+
+  LogAuxSrc = true
+  WildCardLogging = true
+  WildCardOmitPattern = *_STATUS
+  WildCardOmitPattern = DB_VARSUMMARY
+  WildCardOmitPattern = DB_RWSUMMARY
+  WildCardExclusionLog = true
+}
+
+//------------------------------------------
+// uProcessWatch config block
+
+ProcessConfig = uProcessWatch
+{
+  AppTick   = 4
+  CommsTick = 4
+
+  watch_all = true
+	nowatch   = uPokeDB*
+	nowatch   = uQueryDB*
+	nowatch   = uXMS*
+	nowatch   = uMAC*
+}
+
+//------------------------------------------
+// uSimMarineV22 config block
+
+ProcessConfig = uSimMarineV22
+{
+  AppTick  	= 4
+  CommsTick	= 4
+
+  start_pos = x=0, y=-20, heading=180, speed=5
+
+  prefix        = NAV
+
+  turn_rate     = 40
+  thrust_map    = 0:0, 20:1, 40:2, 60:3, 80:4, 100:5
+  //thrust_reflect = true
+
+  buoyancy_rate = 0.075
+  max_depth_rate = 5
+  max_depth_rate_speed = 2.0
+  default_water_depth = 400
+}
+
+//------------------------------------------
+// pHelmIvP config block
+
+ProcessConfig = pHelmIvP
+{
+  AppTick    = 4
+  CommsTick  = 4
+
+  behaviors  = alpha.bhv
+  domain     = course:0:359:360
+  domain     = speed:0:10:101
+  domain     = depth:0:100:101
+
+  park_on_allstop = false
+  //park_on_allstop = true
+  
+  
+}
+
+//------------------------------------------
+// pMarinePID config block
+
+ProcessConfig = pMotionControler
+{
+  AppTick    = 10
+  CommsTick  = 10
+
+  verbose       = true
+  depth_control = true
+
+  // SIM_INSTABILITY = 20
+
+  // Yaw PID controller
+  yaw_pid_kp		 = 3.0
+  yaw_pid_kd		 = 0.0
+  yaw_pid_ki		 = 0.01
+  yaw_pid_integral_limit = 5.0
+
+  // Speed PID controller
+  speed_pid_kp		 = 50.0
+  speed_pid_kd		 = 0.0
+  speed_pid_ki		 = 10.0
+  speed_pid_integral_limit = 500.0
+
+  maxpitch = 10
+  maxelevator = 30
+
+  pitch_pid_kp = 10.0
+  pitch_pid_kd = 0
+  pitch_pid_ki = 0.1
+  pitch_pid_integral_limit = 5.0
+
+  z_to_pitch_pid_kp = 10.0
+  z_to_pitch_pid_kd = 0
+  z_to_pitch_pid_ki = 0.1
+  z_to_pitch_pid_integral_limit = 1.0
+  
+
+  //MAXIMUMS MAXRUDDER
+  maxrudder    = 35
+  maxthrust    = 1525
+
+  // A non-zero SPEED_FACTOR overrides use of SPEED_PID
+  // Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
+  speed_factor = 0
+}
+
+//------------------------------------------
+// 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_one = MOOS_MANUAL_OVERRIDE=false
+  button_two = STOP # START=false
+  //button_two = MOOS_MANUAL_OVERRIDE=true
+  button_three = FaultClear # ClearFalut = true
+  button_four = SendSecurityZone # SendSaftRules = true
+
+
+  action  = MENU_KEY=deploy # DEPLOY = true # RETURN = false
+  action+ = MENU_KEY=deploy # MOOS_MANUAL_OVERRIDE=false
+  action  = RETURN=true
+  action  = UPDATES_RETURN=speed=1.4
+}
+
+//------------------------------------------
+// pNodeReporter config block
+
+ProcessConfig = pNodeReporter
+{
+  AppTick    = 2
+  CommsTick	 = 2
+
+  //platform_type   = kayak
+  //更改显示形状为uuv
+  platform_type = UUV 
+  platform_color  = red
+  platform_length = 4
+}
+
+ProcessConfig = uTimerScript
+{
+  AppTick = 4
+  CommsTick = 4
+
+  condition = DEPLOY = true
+  randvar = varname = RND_DEPTH, min=20, max=80, key=at_reset
+  event = var = DEPTH_UPDATE, val=depth=$[RND_DEPTH], time=120
+  reset_max = nolimit reset_time = all-posted
+}

src/pMotionControler/main.cpp

@@ -0,0 +1,61 @@
+/*
+ * @Author: zjk 1553836110@qq.com
+ * @Date: 2023-10-12 09:52:06
+ * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditTime: 2023-10-24 15:11:21
+ * @FilePath: /moos-ivp-extend/src/pMotionControler/main.cpp
+ * @Description: 
+ * 
+ * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
+ */
+#include<iostream>
+using namespace std;
+#include"MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
+// #include "VarDataPair.h"
+#include"MotionControler.hpp"
+#include <iostream>
+#include<string>
+#include "MBUtils.h"
+#include "ColorParse.h"
+// #include "MBUtils.h"
+// #include "ColorParse.h"
+
+int main(int argc, char *argv[])
+{
+  string mission_file;
+  string run_command = argv[0];
+
+  for(int i=1; i<argc; i++) {
+    string argi = argv[i];
+    if((argi=="-v") || (argi=="--version") || (argi=="-version"))
+      // showReleaseInfoAndExit();
+      cout << " Version : 0.01" << endl;
+    else if((argi=="-e") || (argi=="--example") || (argi=="-example"))
+      // showExampleConfigAndExit();
+      cout << " example : NULL" << endl;
+    else if((argi=="-h") || (argi == "--help") || (argi=="-help"))
+      // showHelpAndExit();
+      cout << " Low leave Control for UUV " << endl;
+    else if((argi=="-i") || (argi == "--interface"))
+      // showInterfaceAndExit();
+      cout << " UUV Motion Control " << endl;
+    else if(strEnds(argi, ".moos") || strEnds(argi, ".moos++"))
+      mission_file = argv[i];
+    else if(strBegins(argi, "--alias="))
+      run_command = argi.substr(8);
+    else if(i == 2)
+      run_command = argi;
+  }
+  
+  if(mission_file == "")
+      // showHelpAndExit();
+
+  cout << termColor("green");
+  cout << "pMarinePIDV22 launching as " << run_command << endl;
+  cout << termColor() << endl;
+
+  MotionControler marinePID;
+  marinePID.Run(run_command.c_str(), mission_file.c_str(), argc, argv);
+
+  return(0);
+}

src/pMotionControler/pidControl.cpp

@@ -0,0 +1,305 @@
+/*
+ * @Author: zjk 1553836110@qq.com
+ * @Date: 2023-10-16 15:14:15
+ * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditTime: 2023-11-03 11:30:38
+ * @FilePath: /moos-ivp-pi/src/pMotionControler/pidControl.cpp
+ * @Description: 
+ * 
+ * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
+ */
+#include"pidControl.hpp"
+pidControl::pidControl()
+{
+    pidClear(pid_depth);
+    pidClear(pid_heading);
+    pidClear(pid_pitch);
+    pidClear(pid_speed);
+    setCurDepth(0,0);
+    setDesDepth(0,0);
+    setCurHeading(0,0);
+    setDesHeading(0,0);
+    setCurPitch(0,0);
+    setDesPitch(0,0);
+    setCurSpeed(0,0);
+    setDesSpeed(0,0);
+    const_thrust = 0;
+}
+int pidControl::step()
+{
+    if(!setDesiredValues())
+    {
+        //这里不对时间进行重置，保持故障状态
+        pidClear(pid_depth);
+        pidClear(pid_heading);
+        pidClear(pid_pitch);
+        pidClear(pid_speed);
+        return -1; //Falut
+    }
+    if(has_override)
+    {
+        //由手动移交的控制权，需要对时间进行重置，防止进入故障状态
+        pidClear(pid_depth);
+        pidClear(pid_heading);
+        pidClear(pid_pitch);
+        pidClear(pid_speed);
+        setCurDepth(0,0);
+        setDesDepth(0,0);
+        setCurHeading(0,0);
+        setDesHeading(0,0);
+        setCurPitch(0,0);
+        setDesPitch(0,0);
+        setCurSpeed(0,0);
+        setDesSpeed(0,0);
+        RepList["Waring"] = int(has_override);
+        return 1; //Ready
+    }
+    RepList["Waring"] = int(has_override);
+    string rpt = "";
+    if(has_speedCtrl)
+    {
+        if(const_thrust == 0)
+        {
+            double speed_error = desired_speed.value - current_speed.value;
+            desired_thrust.value = pidStep(speed_error, pid_speed);
+            desired_thrust.time = current_time;
+            //Limit(desired_thrust.value, max_thrust, 0);
+
+            rpt = "PID_SPEED: ";
+            rpt += " (Want):" + doubleToString(desired_speed.value);
+            rpt += " (Curr):" + doubleToString(current_speed.value);
+            rpt += " (Diff):" + doubleToString(speed_error);
+            rpt += " (Delt):" + doubleToString(pid_speed.delta_output);
+            rpt += " THRUST:" + doubleToString(desired_thrust.value);
+            addPosting("PID_SPD_DEBUG", rpt);
+            RepList["spd_pid"]["Want"] = doubleToString(desired_speed.value,2);
+            RepList["spd_pid"]["Curr"] = doubleToString(current_speed.value,2);
+            RepList["spd_pid"]["Diff"] = doubleToString(speed_error,2);
+            RepList["spd_pid"]["Delt"] = doubleToString(pid_speed.delta_output,2);
+            RepList["spd_pid"]["THRUST"] = doubleToString(desired_thrust.value,2);
+        }
+        else
+        {
+            desired_thrust.value = const_thrust;
+            desired_thrust.time = current_time;
+        }
+
+    }
+    if(desired_thrust.value <= dead_zone)
+    {
+        desired_rudder.value = 0;
+        desired_rudder.time = current_time;
+        desired_elevator.value = 0;
+        desired_elevator.time = current_time;
+        return true;
+    }
+
+    if(has_headCtrl)
+    {   
+        double heading_error = desired_heading.value - current_heading.value;
+        heading_error = angle180(heading_error);
+        desired_rudder.value = pidStep(heading_error, pid_heading);
+        desired_rudder.time = current_time;
+        
+        //Limit(desired_rudder.value, max_rudder, -max_rudder);
+        // Limit();
+
+        rpt = "PID_COURSE: ";
+        rpt += " (Want):" + doubleToString(desired_heading.value);
+        rpt += " (Curr):" + doubleToString(current_heading.value);
+        rpt += " (Diff):" + doubleToString(heading_error);
+        rpt += " (Delt):" + doubleToString(pid_heading.delta_output);
+        rpt += " RUDDER:" + doubleToString(desired_rudder.value);
+        addPosting("PID_HDG_DEBUG", rpt);
+        RepList["hdg_pid"]["Want"] = doubleToString(desired_heading.value,2);
+        RepList["hdg_pid"]["Curr"] = doubleToString(current_heading.value,2);
+        RepList["hdg_pid"]["Diff"] = doubleToString(heading_error,2);
+        RepList["hdg_pid"]["Delt"] = doubleToString(pid_heading.delta_output,2);
+        RepList["hdg_pid"]["RUDDER"] = doubleToString(desired_rudder.value,2);
+    }
+    if(has_depthCtrl)
+    {
+        double depth_error = desired_depth.value - current_depth.value;
+        desired_pitch.value = -pidStep(depth_error, pid_depth);
+        desired_pitch.time = current_time;
+        //Limit(desired_pitch.value, max_pitch, -max_pitch);
+        double pitch_error = desired_pitch.value - current_pitch.value;
+        desired_elevator.value = pidStep(pitch_error, pid_pitch);
+        desired_elevator.time = current_time;
+        //Limit(desired_elevator.value, max_elevator, -max_elevator);
+        
+        rpt = "PID_DEPTH: ";
+        rpt += " (Want):" + doubleToString(desired_depth.value);
+        rpt += " (Curr):" + doubleToString(current_depth.value);
+        rpt += " (Diff):" + doubleToString(depth_error);
+        rpt += " ELEVATOR:" + doubleToString(desired_elevator.value);
+        addPosting("PID_DEP_DEBUG", rpt);
+        RepList["dep_pid"]["Want"] = doubleToString(desired_depth.value,2);
+        RepList["dep_pid"]["Curr"] = doubleToString(current_depth.value,2);
+        RepList["dep_pid"]["Diff"] = doubleToString(depth_error,2);
+        RepList["dep_pid"]["Delt"] = doubleToString(pid_depth.delta_output,2);
+
+        RepList["pth_pid"]["Want"] = doubleToString(desired_pitch.value,2);
+        RepList["pth_pid"]["Curr"] = doubleToString(current_pitch.value,2);
+        RepList["pth_pid"]["Diff"] = doubleToString(pitch_error,2);
+        RepList["pth_pid"]["Delt"] = doubleToString(pid_pitch.delta_output,2);
+        RepList["pth_pid"]["ELEVATOR"] = doubleToString(desired_elevator.value,2);
+    }
+    // Limit();
+    return 0;
+}
+
+bool pidControl::setParam(double p, double i, double d, double limitDelta, double max, double min, pidInc &pid)
+{
+    // pid.kp = p + i + d;
+    // pid.ki = -p - 2*d;
+    // pid.kd = d;
+    pid.kp = p;
+    pid.ki = i;
+    pid.kd = d;
+    pid.limit_delta = limitDelta;
+    pid.output = 0;
+    pid.max_out = max;
+    pid.min_out = min;
+    return true;
+}
+
+double pidControl::pidStep(double error, pidInc &pid)
+{
+    double deltaOutPut = 0;
+    pid.error_0 = error;
+    deltaOutPut 
+    = pid.kp * (pid.error_0 - pid.error_1                 ) 
+    + pid.ki * (pid.error_0                               ) 
+    + pid.kd * (pid.error_0 -2.0*pid.error_1 + pid.error_2);
+    // RepList["W"]["d"] = deltaOutPut;
+    // RepList["W"]["e1"] = pid.kp;
+    // RepList["W"]["e2"] = pid.ki;
+    // RepList["W"]["e3"] = pid.kd;
+    Limit(deltaOutPut, pid.limit_delta, -pid.limit_delta);
+    pid.output += deltaOutPut;
+    Limit(pid.output, pid.max_out, pid.min_out);
+    pid.delta_output = deltaOutPut;
+    pid.error_2 = pid.error_1;
+    pid.error_1 = pid.error_0;
+    return pid.output;
+}
+// double  pidControl::picStep_p(double error, pidInc &pid)
+// {
+//     pid.delta_output = 
+// }
+bool pidControl::overrived(string svar)
+{
+    Controler::overrived(svar);
+    return true;
+}
+//TODO: 快速检测当前参数是具有与当前控制器相关
+bool pidControl::hasConfigSettings() const
+{
+    if(config_params.size() == 0)
+        return(false);
+
+    list<string>::const_iterator p;
+    for(p=config_params.begin(); p!=config_params.end(); p++) 
+    {
+        string line = *p;
+        string param = tolower(biteStringX(line, '='));
+
+        // if(param == "yaw_pid_kp")
+        //     return(true);
+        // else if(param == "yaw_pid_kd")
+        //     return(true);
+        // else if(param == "yaw_pid_ki")
+        //     return(true);
+        // else if(param == "yaw_pid_integral_limit")
+        //     return(true);
+        // else if(param == "yaw_pid_ki_limit")
+        //     return(true);
+        return true;
+    }
+
+    return(false);
+}
+
+bool pidControl::setParam(char n, double param, pidInc *pid_t)
+{
+    if(pid_t == NULL)
+        return false;
+    
+    switch (n)
+    {
+    case 'p':
+        pid_t->kp = param;
+        break;
+    case 'i':
+        pid_t->ki = param;
+        break;
+    case 'd':
+        pid_t->kd = param;
+        break;
+    default:
+        return false;
+        break;
+    }
+
+    return true;
+
+}
+int pidControl::setParam(string filePath)
+{
+    Json::Value paramList = getConfig(filePath);
+    if(paramList.empty())
+        return 1;
+    if(paramList["speed"].empty()       || paramList["pitch"].empty()
+       || paramList["depth"].empty()    || paramList["pitch"].empty()
+       || paramList["speedCol"].empty() ||  paramList["depthCol"].empty()
+       || paramList["HeadingCol"].empty())
+        return 2; //重要参数不全
+    Json::Value param = paramList["speed"];
+    if(!setParam(param, pid_speed))
+        return 3;
+    RepList["speed-param"] = param;
+    param = paramList["heading"];
+    if(!setParam(param, pid_heading))
+        return 4;
+    RepList["heading-param"] = param;
+    param = paramList["pitch"];
+    if(!setParam(param, pid_pitch))
+        return 5;
+    RepList["pitch-param"] = param;
+    param = paramList["depth"];
+    if(!setParam(param, pid_depth))
+        return 6;
+    RepList["depth-param"] = param;
+    setSpeedControl(paramList["speedCol"].asBool());
+    setDepthControl(paramList["depthCol"].asBool());
+    setHeadingControl(paramList["HeadingCol"].asBool());
+    //次要参数
+    if(!paramList["dead_zone"].empty())
+    {
+        dead_zone = paramList["dead_zone"].asDouble();
+        RepList["dead_zone"] = dead_zone;
+    }
+    if(!paramList["const_thrust"].empty())
+    {
+        const_thrust = paramList["const_thrust"].asDouble();
+        RepList["const_thrust"] = const_thrust;
+    }
+    return 0;
+}
+bool pidControl::setParam(Json::Value param, pidInc &pid)
+{
+    if(param.empty())
+        return false;
+    if(param["Kp"].empty() || param["Ki"].empty() || param["Kd"].empty())
+        return false;
+    pid.kp = param["Kp"].asDouble();
+    pid.ki = param["Ki"].asDouble();
+    pid.kd = param["Kd"].asDouble();
+    if(param["LimitDelta"].empty() || param["MaxOut"].empty() || param["MinOut"].empty())
+        return false;
+    pid.limit_delta = param["LimitDelta"].asDouble();
+    pid.max_out = param["MaxOut"].asDouble();
+    pid.min_out = param["MinOut"].asDouble();
+    return true;
+}

src/pMotionControler/pidControl.hpp

@@ -0,0 +1,77 @@
+/*
+ * @Author: zjk 1553836110@qq.com
+ * @Date: 2023-10-16 15:14:26
+ * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditTime: 2023-11-02 18:03:14
+ * @FilePath: /moos-ivp-pi/src/pMotionControler/pidControl.hpp
+ * @Description: 
+ * 
+ * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
+ */
+#ifndef _PIDCONTROL_H
+#define _PIDCONTROL_H
+#include"Controler.hpp"
+
+typedef struct
+{
+    double kp;
+    double ki;
+    double kd;
+
+    double error_0;
+    double error_1;
+    double error_2;
+
+    double limit_delta;
+    double max_out;
+    double min_out;
+
+    double delta_output;
+    double output;
+} pidInc;
+
+
+class pidControl : public Controler
+{
+private:
+    /* data */
+    pidInc pid_speed;
+    pidInc pid_heading;
+    pidInc pid_pitch;
+    pidInc pid_depth;
+
+    double current_error;
+    double last_error;
+    vector<double> Error;
+    int Error_capacity;
+
+public:
+    pidControl(/* args */);
+    ~pidControl(){};
+
+    int step();
+    
+    bool setParam(double p, double i, double d, double limitDelta, double max, double min, pidInc &pid);
+    bool setParam(char n, double param, pidInc *pid_t);
+    int setParam(string filePath);
+    bool setParam(Json::Value param, pidInc &pid);
+
+    bool hasConfigSettings() const;
+    
+    double pidStep(double error, pidInc &pid);
+    //double picStep_p(double error, pidInc &pid);
+
+    bool overrived(string svar);
+
+    inline void pidClear(pidInc &pid)
+    {
+        pid.error_0 = 0;
+        pid.error_1 = 0;
+        pid.error_2 = 0;
+        pid.delta_output = 0;
+        pid.output = 0;
+    }
+
+};
+
+#endif

src/pMotionControler/simMat.moos

@@ -0,0 +1,260 @@
+//-------------------------------------------------
+// NAME: M. Benjamin, MIT CSAIL
+// FILE: alpha.moos
+//-------------------------------------------------
+
+ServerHost   = localhost
+ServerPort   = 9000
+Community    = alpha
+MOOSTimeWarp = 1
+
+// Forest Lake
+LatOrigin  = 43.825300 
+LongOrigin = -70.330400 
+
+// MIT Sailing Pavilion (use this one)
+// LatOrigin  = 42.358456 
+// LongOrigin = -71.087589
+
+//------------------------------------------
+// Antler configuration  block
+ProcessConfig = ANTLER
+{
+  MSBetweenLaunches = 200
+
+  Run = MOOSDB        @ NewConsole = false
+  Run = pMarineViewer	@ NewConsole = false
+  Run = uProcessWatch	@ NewConsole = false
+  Run = pNodeReporter	@ NewConsole = false
+  Run = pEmulator @ NewConsole = false
+  //Run = pLogger       @ NewConsole = false
+  Run = pMotionControler   @ NewConsole = false
+  Run = pTaskManger   @ NewConsole = false
+  Run = pHelmIvP      @ NewConsole = false
+}
+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 = pTaskManger
+{
+  AppTick   = 8
+  CommsTick = 8
+}
+ProcessConfig = pEmulator
+{
+  AppTick   = 5
+  CommsTick = 5
+  matlab_host = 192.168.0.11
+  matlab_port = 8085
+  local_port = 8080
+  prefix = NAV
+
+  start_x = 10
+  start_y = 9
+  start_z = 1
+  start_heading = 30
+}
+
+ProcessConfig = pLogger
+{
+  AppTick   = 8
+  CommsTick = 8
+
+  AsyncLog = true
+
+  // For variables that are published in a bundle on their first post,
+  // explicitly declare their logging request
+  //Log = IVPHELM_LIFE_EVENT @ 0 NOSYNC
+  //Log = REPORT @ 0 NOSYNC
+  //Log = BHV_SETTINGS @ 0 NOSYNC
+  Log = OPREGION_RESET @ 0 NOSYNC
+
+  LogAuxSrc = true
+  WildCardLogging = true
+  WildCardOmitPattern = *_STATUS
+  WildCardOmitPattern = DB_VARSUMMARY
+  WildCardOmitPattern = DB_RWSUMMARY
+  WildCardExclusionLog = true
+}
+
+//------------------------------------------
+// uProcessWatch config block
+
+ProcessConfig = uProcessWatch
+{
+  AppTick   = 4
+  CommsTick = 4
+
+  watch_all = true
+	nowatch   = uPokeDB*
+	nowatch   = uQueryDB*
+	nowatch   = uXMS*
+	nowatch   = uMAC*
+}
+
+//------------------------------------------
+// uSimMarineV22 config block
+//------------------------------------------
+// pHelmIvP config block
+
+ProcessConfig = pHelmIvP
+{
+  AppTick    = 4
+  CommsTick  = 4
+
+  behaviors  = alpha.bhv
+  domain     = course:0:359:360
+  domain     = speed:0:10:101
+  domain     = depth:0:100:101
+
+  park_on_allstop = false
+  //park_on_allstop = true
+  
+  
+}
+
+//------------------------------------------
+// pMarinePID config block
+
+ProcessConfig = pMotionControler
+{
+  AppTick    = 5
+  CommsTick  = 5
+
+  verbose       = true
+  depth_control = true
+
+  // SIM_INSTABILITY = 20
+
+  // Yaw PID controller
+  yaw_pid_kp		 = 10
+  yaw_pid_kd		 = 0.0
+  yaw_pid_ki		 = 0.01
+  yaw_pid_integral_limit = 10
+
+  // Speed PID controller
+  speed_pid_kp		 = 20.0
+  speed_pid_kd		 = 0.0
+  speed_pid_ki		 = 1
+  speed_pid_integral_limit = 100
+
+  maxpitch = 15
+  maxelevator = 30
+
+  pitch_pid_kp = 1.5
+  pitch_pid_kd = 0
+  pitch_pid_ki = 1.0
+  pitch_pid_integral_limit = 0
+
+  z_to_pitch_pid_kp = 0.12
+  z_to_pitch_pid_kd = 0
+  z_to_pitch_pid_ki = 0.004
+  z_to_pitch_pid_integral_limit = 0.05
+  
+
+  //MAXIMUMS MAXRUDDER
+  maxrudder    = 30
+  maxthrust    = 1525
+
+  // A non-zero SPEED_FACTOR overrides use of SPEED_PID
+  // Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
+  speed_factor = 0
+}
+
+//------------------------------------------
+// pMarineViewer config block
+
+ProcessConfig = pMarineViewer
+{
+  AppTick    = 4
+  CommsTick  = 4
+
+  tiff_file            = forrest19.tif
+	//tiff_file            = MIT_SP.tif
+  vehicles_name_mode = names+depth //+shortmode
+
+
+  set_pan_x            = -90
+  set_pan_y            = -280
+  zoom                 = 0.65
+  vehicle_shape_scale  = 1.5
+  hash_delta           = 50
+  hash_shade           = 0.22
+  hash_viewable        = true
+
+  trails_point_size   = 1
+
+	//op_vertex = x=-83, y=-47,    lwidth=1,lcolor=yellow,looped=true,group=moa
+  //op_vertex = x=-46.4, y=-129.2, lwidth=1,lcolor=yellow,looped=true,group=moa
+  //op_vertex = x=94.6, y=-62.2,   lwidth=1,lcolor=yellow,looped=true,group=moa
+  //op_vertex = x=58,   y=20,    lwidth=1,lcolor=yellow,looped=true,group=moa
+
+  // Appcast configuration
+  appcast_height       = 75
+  appcast_width        = 30
+  appcast_viewable     = true
+  appcast_color_scheme = indigo
+  nodes_font_size      = xlarge
+  procs_font_size      = xlarge
+  appcast_font_size    = large
+  
+  // datum_viewable = true
+  // datum_size     = 18
+  // gui_size = small
+
+  // left_context[survey-point] = DEPLOY=true
+  // left_context[survey-point] = MOOS_MANUAL_OVERRIDE=false
+  // left_context[survey-point] = RETURN=false
+
+  right_context[return] = DEPLOY=true
+  right_context[return] = MOOS_MANUAL_OVERRIDE=false
+  right_context[return] = RETURN=false
+
+  scope  = RETURN
+  scope  = WPT_STAT
+  scope  = VIEW_SEGLIST
+  scope  = VIEW_POINT
+  scope  = VIEW_POLYGON
+  scope  = MVIEWER_LCLICK
+  scope  = MVIEWER_RCLICK
+
+  button_one = START # START=true
+  //button_one = MOOS_MANUAL_OVERRIDE=false
+  button_two = STOP # START=false
+  //button_two = MOOS_MANUAL_OVERRIDE=true
+  button_three = FaultClear # ClearFalut = true
+  button_four = SendSecurityZone # SendSaftRules = true
+
+
+  action  = MENU_KEY=deploy # DEPLOY = true # RETURN = false
+  action+ = MENU_KEY=deploy # MOOS_MANUAL_OVERRIDE=false
+  action  = RETURN=true
+  action  = UPDATES_RETURN=speed=1.4
+}
+
+//------------------------------------------
+// pNodeReporter config block
+
+ProcessConfig = pNodeReporter
+{
+  AppTick    = 2
+  CommsTick	 = 2
+
+  //platform_type   = kayak
+  //更改显示形状为uuv
+  platform_type = UUV 
+  platform_color  = red
+  platform_length = 4
+}

src/pSurfaceSupportComm/CMakeLists.txt

@@ -0,0 +1,53 @@
+#--------------------------------------------------------
+# The CMakeLists.txt for:                       pSurfaceSupportComm
+# Author(s):                              Xiaobin Zeng
+#--------------------------------------------------------
+
+SET(SRC
+  SurfaceSupportComm.cpp
+  SurfaceSupportComm_Info.cpp
+  PeriodicUDPEvent.cpp 
+
+  main.cpp
+)
+
+FIND_LIBRARY(DUNE_LIB dune-core /usr/local/lib /usr/local/lib/DUNE)
+FIND_PATH(DUNE_INCLUDE DUNE/IMC.hpp /usr/local/include /usr/local/include/DUNE)
+include_directories(${DUNE_INCLUDE})
+
+# include(FindProtobuf)
+# find_package(Protobuf REQUIRED)
+# include_directories(${Protobuf_INCLUDE_DIR})
+
+# protobuf_generate_cpp(PROTO_SRC PROTO_HEADER Behavior.proto NavigationInfo.proto)
+# add_library(proto ${PROTO_HEADER} ${PROTO_SRC})
+
+# include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+
+# find_package (jsoncpp NO_MODULE REQUIRED)
+
+include_directories(/usr/include/jsoncpp/)
+link_directories(/usr/local/lib/)
+
+find_package (GeographicLib REQUIRED)
+include_directories(${GeographicLib_INCLUDE_DIRS})
+
+ADD_EXECUTABLE(pSurfaceSupportComm ${SRC})
+
+TARGET_LINK_LIBRARIES(pSurfaceSupportComm
+  ${MOOS_LIBRARIES}
+  ${CMAKE_DL_LIBS}
+  ${SYSTEM_LIBS} 
+  ${DUNE_LIB}
+  ${GeographicLib_LIBRARIES}        
+  mbutil
+  m
+  pthread
+  jsoncpp  
+  # jsoncpp_lib_static
+  # protobuf
+  # protoc
+  # proto
+  # ${PROTOBUF_LIBRARY}   
+)
+

src/pSurfaceSupportComm/PeriodicTCPEvent.cpp

@@ -0,0 +1,142 @@
+
+#include "PeriodicTCPEvent.h"
+
+#define TCP_RECEIVE_PORT 8000
+
+class PeriodicTCPEvent::Impl
+{
+public:
+
+        Impl()
+        {
+		pfn_ = DefaultCallback;                
+                pParamCaller_= NULL;
+                Period_ = 1.0;
+
+
+        }
+
+        static bool DefaultCallback(DUNE::IMC::Message * msg)
+        {
+// UNUSED_PARAMETER(pParamCaller);
+
+                // std::cout.setf(std::ios::fixed);
+
+                // std::cout<<std::setprecision(4);
+
+                // std::cout<<"Timer Callback \n";
+                // std::cout<<"  TimeNow       "<<TimeNow<<"\n";
+                // std::cout<<"  TimeScheduled "<<TimeScheduled<<"\n";
+                // std::cout<<"  TimeLastRun   "<<TimeLastRun<<"\n";
+
+                return true;
+        }
+
+        static bool PeriodicTCPEventDispatch(void * pParam)
+        {
+                PeriodicTCPEvent::Impl* pMe = static_cast<PeriodicTCPEvent::Impl*> (pParam);
+                return pMe->DoWork();
+        }
+
+        void SetCallback(bool (*pfn)(DUNE::IMC::Message * msg), void * pCallerParam)
+        {
+UNUSED_PARAMETER(pCallerParam);
+                pfn_ = pfn;
+                //pParamCaller_ = pCallerParam;
+
+        }
+
+        bool SetPeriod(double PeriodSeconds)
+        {
+                if(PeriodSeconds<0)
+                        return false;
+                Period_ = PeriodSeconds;
+
+                return true;
+        }
+
+        bool Stop()
+        {
+                Thread_.Stop();
+                return true;
+        }
+
+
+        bool Start()
+        {
+                if(Thread_.IsThreadRunning())
+                        return false;
+                else
+                {
+                        Thread_.Initialise(PeriodicTCPEventDispatch,this);
+                        return Thread_.Start();
+                }
+        }
+
+        bool DoWork()
+        {
+
+                double TimeLast = MOOS::Time();
+                MOOS::BoostThisThread();
+                sock_tcp_receive.bind(TCP_RECEIVE_PORT);
+                sock_tcp_receive.listen(5);
+                while(!Thread_.IsQuitRequested())
+                {
+                        double TimeScheduled=TimeLast+Period_;                    
+                        size_t size = sock_tcp_receive.read(tcpReceiveBuffer, sizeof(tcpReceiveBuffer)/sizeof(uint8_t));
+                        if (size > 0)
+                        {
+                                DUNE::IMC::Message * msg = DUNE::IMC::Packet::deserialize(tcpReceiveBuffer, size);
+
+                                if(!(*pfn_)(msg))
+                                {
+                                        break;
+                                }
+                        }
+
+                }
+
+                return true;
+        }
+
+        CMOOSThread Thread_;
+        bool (*pfn_)(DUNE::IMC::Message * msg);
+        void * pParamCaller_;
+        double Period_;
+        // DUNE::Network::UDPSocket sock_udp_send;
+        DUNE::Network::TCPSocket sock_tcp_receive;        
+        uint8_t tcpReceiveBuffer[65535];
+	CMOOSLock m_Lock;
+        // std::stack<DUNE::IMC::Message *> msgBuffer;        
+};
+
+void PeriodicTCPEvent::SetCallback(bool (*pfn)(DUNE::IMC::Message * msg), void * pCallerParam)
+{
+        Impl_->SetCallback(pfn,pCallerParam);
+}
+
+PeriodicTCPEvent::PeriodicTCPEvent(): Impl_(new PeriodicTCPEvent::Impl  )
+{
+
+}
+
+bool PeriodicTCPEvent::SetPeriod(double PeriodSeconds)
+{
+        return Impl_->SetPeriod(PeriodSeconds);
+}
+
+bool PeriodicTCPEvent::Start()
+{
+        return Impl_->Start();
+}
+
+bool PeriodicTCPEvent::Stop()
+{
+        return Impl_->Stop();
+}
+
+
+// bool PeriodicTCPEvent::Push(DUNE::IMC::Message* msg)
+// {
+//         Impl_->msgBuffer.push(msg);        
+// }

src/pSurfaceSupportComm/PeriodicTCPEvent.h

@@ -0,0 +1,39 @@
+#include "MOOS/libMOOS/Utils/MOOSUtilityFunctions.h"
+#include "MOOS/libMOOS/Utils/ThreadPriority.h"
+
+#include "MOOS/libMOOS/Utils/MOOSThread.h"
+
+#include <DUNE/DUNE.hpp>
+
+
+#include <iostream>
+#include <iomanip>
+#include <chrono>
+#include <thread>
+
+class PeriodicTCPEvent
+{
+public:
+        PeriodicTCPEvent();
+	/**
+	 * this sets the callback you wish to have called
+	 */
+	void SetCallback(bool (*pfn)(DUNE::IMC::Message * msg), void * pCallerParam);
+	
+        /**
+	 * Set the period of the event
+	 */
+	bool SetPeriod(double PeriodSeconds);
+
+	/** start the service*/
+	bool Start();
+
+	/** stop the service */
+	bool Stop();
+
+private:
+	class Impl;
+	Impl * Impl_;
+
+	
+};

src/pSurfaceSupportComm/PeriodicUDPEvent.cpp

@@ -0,0 +1,156 @@
+
+#include "PeriodicUDPEvent.h"
+
+#define UDP_SEND_PORT 6001
+#define DEST_IP_ADDRESS  "127.0.0.1"
+
+class PeriodicUDPEvent::Impl
+{
+public:
+
+        Impl()
+        {
+		pfn_ = DefaultCallback;                
+                pParamCaller_= NULL;
+                Period_ = 1.0;
+
+
+        }
+
+        static bool DefaultCallback(double TimeNow,double TimeLastRun,double TimeScheduled, void * pParamCaller)
+        {
+UNUSED_PARAMETER(pParamCaller);
+
+                std::cout.setf(std::ios::fixed);
+
+                std::cout<<std::setprecision(4);
+
+                std::cout<<"Timer Callback \n";
+                std::cout<<"  TimeNow       "<<TimeNow<<"\n";
+                std::cout<<"  TimeScheduled "<<TimeScheduled<<"\n";
+                std::cout<<"  TimeLastRun   "<<TimeLastRun<<"\n";
+
+                return true;
+        }
+
+        static bool PeriodicUDPEventDispatch(void * pParam)
+        {
+                PeriodicUDPEvent::Impl* pMe = static_cast<PeriodicUDPEvent::Impl*> (pParam);
+                return pMe->DoWork();
+        }
+
+        void SetCallback(bool (*pfn)(double TimeNow,double TimeLastRun,double TimeScheduled, void * pParamCaller), void * pCallerParam)
+        {
+UNUSED_PARAMETER(pCallerParam);
+                pfn_ = pfn;
+                //pParamCaller_ = pCallerParam;
+
+        }
+
+        bool SetPeriod(double PeriodSeconds)
+        {
+                if(PeriodSeconds<0)
+                        return false;
+                Period_ = PeriodSeconds;
+
+                return true;
+        }
+
+        bool Stop()
+        {
+                Thread_.Stop();
+                return true;
+        }
+
+
+        bool Start()
+        {
+                if(Thread_.IsThreadRunning())
+                        return false;
+                else
+                {
+                        Thread_.Initialise(PeriodicUDPEventDispatch,this);
+                        return Thread_.Start();
+                }
+        }
+
+        bool DoWork()
+        {
+
+                double TimeLast = MOOS::Time();
+                MOOS::BoostThisThread();
+
+                while(!Thread_.IsQuitRequested())
+                {
+                        double TimeScheduled=TimeLast+Period_;
+                        std::string addr = DEST_IP_ADDRESS;
+                        int size = -1;
+                        int type = -1;
+                        m_Lock.Lock();
+                        if (!msgBuffer.empty())
+                        {
+                                DUNE::Utils::ByteBuffer bb;
+                                DUNE::IMC::Message *msg = msgBuffer.top();                                                             
+                                DUNE::IMC::Packet::serialize(msg, bb);                              
+                                size = sock_udp_send.write(bb.getBuffer(), msg->getSerializationSize(),
+                                DUNE::Network::Address(addr.c_str()), UDP_SEND_PORT);                               
+                                type = msg->getId();
+                                // delete msg;
+                                msgBuffer.pop();
+                        }
+                        m_Lock.UnLock();
+
+                        double TimeNow = MOOS::Time();
+
+                        if(!(*pfn_)(MOOS::Time(), TimeLast,TimeScheduled,pParamCaller_))
+                        {
+                                break;
+                        }
+
+                        TimeLast = TimeNow;
+
+                }
+
+                return true;
+        }
+
+        CMOOSThread Thread_;
+        bool (*pfn_)(double TimeNow,double TimeLastRun,double TimeScheduled, void* pParam);
+        void * pParamCaller_;
+        double Period_;
+        DUNE::Network::UDPSocket sock_udp_send;
+        uint8_t udpReceiveBuffer[65535];
+	CMOOSLock m_Lock;
+        std::stack<DUNE::IMC::Message *> msgBuffer;        
+};
+
+void PeriodicUDPEvent::SetCallback(bool (*pfn)(double Now,double LastRun,double Scheduled, void * pParamCaller), void * pCallerParam)
+{
+        Impl_->SetCallback(pfn,pCallerParam);
+}
+
+PeriodicUDPEvent::PeriodicUDPEvent(): Impl_(new PeriodicUDPEvent::Impl  )
+{
+
+}
+
+bool PeriodicUDPEvent::SetPeriod(double PeriodSeconds)
+{
+        return Impl_->SetPeriod(PeriodSeconds);
+}
+
+bool PeriodicUDPEvent::Start()
+{
+        return Impl_->Start();
+}
+
+bool PeriodicUDPEvent::Stop()
+{
+        return Impl_->Stop();
+}
+
+
+bool PeriodicUDPEvent::Push(DUNE::IMC::Message* msg)
+{
+        Impl_->msgBuffer.push(msg);        
+}

src/pSurfaceSupportComm/PeriodicUDPEvent.h

@@ -0,0 +1,42 @@
+#include "MOOS/libMOOS/Utils/MOOSUtilityFunctions.h"
+#include "MOOS/libMOOS/Utils/ThreadPriority.h"
+
+#include "MOOS/libMOOS/Utils/MOOSThread.h"
+
+#include <DUNE/DUNE.hpp>
+
+
+#include <iostream>
+#include <iomanip>
+#include <chrono>
+#include <thread>
+
+class PeriodicUDPEvent
+{
+public:
+        PeriodicUDPEvent();
+	/**
+	 * this sets the callback you wish to have called
+	 */
+        // void SetCallback(bool (*pfn)(int type, int size), void * pCallerParam);
+	void SetCallback(bool (*pfn)(double Now,double LastRun,double Scheduled, void * pParamCaller), void * pCallerParam);
+	
+        /**
+	 * Set the period of the event
+	 */
+	bool SetPeriod(double PeriodSeconds);
+
+	/** start the service*/
+	bool Start();
+
+	/** stop the service */
+	bool Stop();
+
+	bool Push(DUNE::IMC::Message* msg);
+
+private:
+	class Impl;
+	Impl * Impl_;
+
+	
+};

src/pSurfaceSupportComm/SurfaceSupportComm.h

@@ -0,0 +1,140 @@
+/************************************************************/
+/*    NAME: Xiaobin Zeng                                              */
+/*    ORGN: MIT                                             */
+/*    FILE: SurfaceSupportComm.h                                          */
+/*    DATE:                                                 */
+/************************************************************/
+
+#ifndef SurfaceSupportComm_HEADER
+#define SurfaceSupportComm_HEADER
+
+#include <stdio.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <unistd.h>
+#include <errno.h>
+#include <string.h>
+#include <stdlib.h>
+#include <arpa/inet.h>
+
+
+#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 "PeriodicUDPEvent.h"
+
+struct Landmark {
+	float lon;
+	float lat;
+	float depth;
+	float speed;
+};
+
+struct EstimatedState {
+  float referenceLon;
+  float referenceLat;
+  float referenceAltitude;  
+  float currentLon;
+  float currentLat;
+  float currentAltitude;
+  float offsetNorth;
+  float offsetEast;
+  float offsetDown;	
+  float roll;
+  float pitch;
+  float yaw;
+  float linearVelocityNorth;
+  float linearVelocityEast;
+  float linearVelocityDown;
+  float height;	        
+  float depth;
+};
+
+struct DeviceStatus {
+  unsigned int batteryVoltage; 
+  unsigned int batteryLevel;
+  float batteryTemp;
+  unsigned int controllerTemp;
+  int thrustRpm;
+  unsigned int lightEnable;
+  unsigned int throwingLoadEnable;
+  unsigned int dvlStatus;
+};
+
+
+class SurfaceSupportComm : public CMOOSApp
+// class SurfaceSupportComm : public AppCastingMOOSApp
+{
+public:
+  SurfaceSupportComm();
+  ~SurfaceSupportComm();
+
+protected: // Standard MOOSApp functions to overload  
+  bool OnNewMail(MOOSMSG_LIST &NewMail);
+  bool Iterate();
+  bool OnConnectToServer();
+  bool OnStartUp();
+  void RegisterVariables();
+  // bool buildReport();
+
+private:
+  char recv_buf[2048];
+  DUNE::Network::UDPSocket sock_udp_send;
+  DUNE::Network::TCPSocket sock_tcp_receive;
+  DUNE::IO::Poll m_poll;
+  uint16_t header_src, header_dst;
+  uint8_t header_src_ent, header_dst_ent;
+  std::string planConfigPath;
+  struct EstimatedState estimatedState;
+  struct DeviceStatus deviceStatus;
+  struct Client
+  {
+    DUNE::Network::TCPSocket* socket; // Socket handle.
+    DUNE::Network::Address address; // Client address.
+    uint16_t port; // Client port.
+    DUNE::IMC::Parser parser; // Parser handle
+  };
+
+  typedef std::list<Client> ClientList;
+  ClientList m_clients; 
+
+  bool udpSendToServer(DUNE::IMC::Message * msg, std::string addr, int port);
+  void tcpReceiveFromClient(char* buf, unsigned int cap, double timeout);
+  void tcpSendToClient(char* buf, unsigned int cap);  
+  void acceptNewClient(void);
+  void handleClients(char* buf, unsigned int cap);
+  void closeConnection(Client& c, std::exception& e);
+  void processMessage(DUNE::IMC::Message * message);
+  void ConvertLLAToNED(std::vector<double> init_lla, 
+                            std::vector<double> point_lla, 
+                            std::vector<double>& point_ned);
+  void ConvertNEDToLLA(std::vector<double> init_lla, 
+                            std::vector<double> point_ned,
+                            std::vector<double> &point_lla);
+  void ConvertLLAToENU(std::vector<double> init_lla, 
+                              std::vector<double> point_lla, 
+                              std::vector<double>& point_enu);
+  void ConvertENUToLLA(std::vector<double> init_lla, 
+                              std::vector<double> point_enu,
+                              std::vector<double> &point_lla);
+
+  void BatchConvertCoordinate(Json::Value &object);
+  void ParsePlanConfig(Json::Value inputJsonValue);
+  int simulateGetEntityStatus(DUNE::IMC::EntityParameters& entityParameter, std::string name, std::string value);   
+  int retrieveEntityStatus(DUNE::IMC::MsgList& equipmentMsgList);
+  template <typename Type> 
+  inline int getEntityStatus(DUNE::IMC::EntityParameters& entityParameter, std::string name, Type& value);
+
+  int testFlag = 0;
+  double TimeLast;
+  int sendCount = 0;
+
+  PeriodicUDPEvent udpEvent;
+  // PeriodicTCPEvent tcpEvent;
+  std::string missionStatusString; 
+};
+
+#endif 

src/pSurfaceSupportComm/SurfaceSupportComm_Info.cpp

@@ -0,0 +1,115 @@
+/****************************************************************/
+/*   NAME: Xiaobin Zeng                                             */
+/*   ORGN: MIT Cambridge MA                                     */
+/*   FILE: SurfaceSupportComm_Info.cpp                               */
+/*   DATE: Dec 29th 1963                                        */
+/****************************************************************/
+
+#include <cstdlib>
+#include <iostream>
+#include "SurfaceSupportComm_Info.h"
+#include "ColorParse.h"
+#include "ReleaseInfo.h"
+
+using namespace std;
+
+//----------------------------------------------------------------
+// Procedure: showSynopsis
+
+void showSynopsis()
+{
+  blk("SYNOPSIS:                                                       ");
+  blk("------------------------------------                            ");
+  blk("  The pSurfaceSupportComm application is used for               ");
+  blk("                                                                ");
+  blk("                                                                ");
+  blk("                                                                ");
+  blk("                                                                ");
+}
+
+//----------------------------------------------------------------
+// Procedure: showHelpAndExit
+
+void showHelpAndExit()
+{
+  blk("                                                                ");
+  blu("=============================================================== ");
+  blu("Usage: pSurfaceSupportComm file.moos [OPTIONS]                   ");
+  blu("=============================================================== ");
+  blk("                                                                ");
+  showSynopsis();
+  blk("                                                                ");
+  blk("Options:                                                        ");
+  mag("  --alias","=<ProcessName>                                      ");
+  blk("      Launch pSurfaceSupportComm with the given process name         ");
+  blk("      rather than pSurfaceSupportComm.                           ");
+  mag("  --example, -e                                                 ");
+  blk("      Display example MOOS configuration block.                 ");
+  mag("  --help, -h                                                    ");
+  blk("      Display this help message.                                ");
+  mag("  --interface, -i                                               ");
+  blk("      Display MOOS publications and subscriptions.              ");
+  mag("  --version,-v                                                  ");
+  blk("      Display the release version of pSurfaceSupportComm.        ");
+  blk("                                                                ");
+  blk("Note: If argv[2] does not otherwise match a known option,       ");
+  blk("      then it will be interpreted as a run alias. This is       ");
+  blk("      to support pAntler launching conventions.                 ");
+  blk("                                                                ");
+  exit(0);
+}
+
+//----------------------------------------------------------------
+// Procedure: showExampleConfigAndExit
+
+void showExampleConfigAndExit()
+{
+  blk("                                                                ");
+  blu("=============================================================== ");
+  blu("pSurfaceSupportComm Example MOOS Configuration                   ");
+  blu("=============================================================== ");
+  blk("                                                                ");
+  blk("ProcessConfig = pSurfaceSupportComm                              ");
+  blk("{                                                               ");
+  blk("  AppTick   = 4                                                 ");
+  blk("  CommsTick = 4                                                 ");
+  blk("                                                                ");
+  blk("}                                                               ");
+  blk("                                                                ");
+  exit(0);
+}
+
+
+//----------------------------------------------------------------
+// Procedure: showInterfaceAndExit
+
+void showInterfaceAndExit()
+{
+  blk("                                                                ");
+  blu("=============================================================== ");
+  blu("pSurfaceSupportComm INTERFACE                                    ");
+  blu("=============================================================== ");
+  blk("                                                                ");
+  showSynopsis();
+  blk("                                                                ");
+  blk("SUBSCRIPTIONS:                                                  ");
+  blk("------------------------------------                            ");
+  blk("  NODE_MESSAGE = src_node=alpha,dest_node=bravo,var_name=FOO,   ");
+  blk("                 string_val=BAR                                 ");
+  blk("                                                                ");
+  blk("PUBLICATIONS:                                                   ");
+  blk("------------------------------------                            ");
+  blk("  Publications are determined by the node message content.      ");
+  blk("                                                                ");
+  exit(0);
+}
+
+//----------------------------------------------------------------
+// Procedure: showReleaseInfoAndExit
+
+void showReleaseInfoAndExit()
+{
+  showReleaseInfo("pSurfaceSupportComm", "gpl");
+  exit(0);
+}
+

src/pSurfaceSupportComm/SurfaceSupportComm_Info.h

@@ -0,0 +1,18 @@
+/****************************************************************/
+/*   NAME: Xiaobin Zeng                                             */
+/*   ORGN: MIT Cambridge MA                                     */
+/*   FILE: SurfaceSupportComm_Info.h                                      */
+/*   DATE: Dec 29th 1963                                        */
+/****************************************************************/
+
+#ifndef SurfaceSupportComm_INFO_HEADER
+#define SurfaceSupportComm_INFO_HEADER
+
+void showSynopsis();
+void showHelpAndExit();
+void showExampleConfigAndExit();
+void showInterfaceAndExit();
+void showReleaseInfoAndExit();
+
+#endif
+

src/pSurfaceSupportComm/main.cpp

@@ -0,0 +1,52 @@
+/************************************************************/
+/*    NAME: Xiaobin Zeng                                              */
+/*    ORGN: MIT                                             */
+/*    FILE: main.cpp                                        */
+/*    DATE:                                                 */
+/************************************************************/
+
+#include <string>
+#include "MBUtils.h"
+#include "ColorParse.h"
+#include "SurfaceSupportComm.h"
+#include "SurfaceSupportComm_Info.h"
+
+using namespace std;
+
+int main(int argc, char *argv[])
+{
+  string mission_file;
+  string run_command = argv[0];
+
+  for(int i=1; i<argc; i++) {
+    string argi = argv[i];
+    if((argi=="-v") || (argi=="--version") || (argi=="-version"))
+      showReleaseInfoAndExit();
+    else if((argi=="-e") || (argi=="--example") || (argi=="-example"))
+      showExampleConfigAndExit();
+    else if((argi == "-h") || (argi == "--help") || (argi=="-help"))
+      showHelpAndExit();
+    else if((argi == "-i") || (argi == "--interface"))
+      showInterfaceAndExit();
+    else if(strEnds(argi, ".moos") || strEnds(argi, ".moos++"))
+      mission_file = argv[i];
+    else if(strBegins(argi, "--alias="))
+      run_command = argi.substr(8);
+    else if(i==2)
+      run_command = argi;
+  }
+  
+  if(mission_file == "")
+    showHelpAndExit();
+
+  cout << termColor("green");
+  cout << "pSurfaceSupportComm launching as " << run_command << endl;
+  cout << termColor() << endl;
+
+  SurfaceSupportComm SurfaceSupportComm;
+
+  SurfaceSupportComm.Run(run_command.c_str(), mission_file.c_str());
+  
+  return(0);
+}
+

src/pSurfaceSupportComm/pSurfaceSupportComm.moos

@@ -0,0 +1,9 @@
+//------------------------------------------------
+// pSurfaceSupportComm config block
+
+ProcessConfig = pSurfaceSupportComm
+{
+   AppTick   = 4
+   CommsTick = 4
+}
+

src/pTaskManger/CMakeLists.txt

@@ -0,0 +1,27 @@
+#--------------------------------------------------------
+# The CMakeLists.txt for:                       pTaskManger
+# Author(s):                              zjk
+#--------------------------------------------------------
+
+
+FILE(GLOB SRC *.cpp)
+
+
+#include_directories(/usr/local/include/jsoncpp)
+#link_directories(/usr/local/lib/)
+
+include_directories(/usr/include/jsoncpp/)
+link_directories(/usr/local/lib/)
+
+
+ADD_EXECUTABLE(pTaskManger ${SRC})
+
+
+
+TARGET_LINK_LIBRARIES(pTaskManger
+   ${MOOS_LIBRARIES}
+   mbutil
+   m
+   pthread
+   jsoncpp)
+

src/pTaskManger/TaskManger.cpp

@@ -0,0 +1,999 @@
+
+/************************************************************/
+/*    NAME: zjk                                              */
+/*    ORGN: MIT                                             */
+/*    FILE: TaskManger.cpp                                        */
+/*    DATE:                                                 */
+/************************************************************/
+//TODO: 增加故障处理
+#include <list>
+#include <iterator>
+#include "MBUtils.h"
+#include "TaskManger.h"
+
+using namespace std;
+
+#define DEBUG
+
+
+//---------------------------------------------------------
+// Constructor
+
+TaskManger::TaskManger()
+{
+
+  cout << "Task Manger process staring ... " << endl;
+}
+
+//---------------------------------------------------------
+// Destructor
+
+TaskManger::~TaskManger()
+{
+  Notify("RUN","false");
+  Notify("MOOS_MANUAL_OVERRIDE","true");
+}
+
+//---------------------------------------------------------
+// Procedure: OnNewMail
+
+bool TaskManger::OnNewMail(MOOSMSG_LIST &NewMail)
+{
+  AppCastingMOOSApp::OnNewMail(NewMail);
+  MOOSMSG_LIST::iterator p;
+  for(p=NewMail.begin(); p!=NewMail.end(); p++) 
+  {
+    CMOOSMsg &msg   = *p;
+    string msg_name = msg.GetName();
+    string msg_str  =  msg.GetString();
+    double msg_dval = msg.GetDouble();
+    bool msg_bval   = msg.GetBinaryData();
+    // cout << msg_name + ": " << msg_str << endl;
+    if(msg_name == MSG_ENDFLAG)
+    {
+      if(msg_str == "true")
+        current_node_complete = true;
+      else 
+        current_node_complete = false;
+    }
+
+    if(msg_name == MSG_WPTFLAG)
+    {
+      if(msg_str == "true")
+        current_pol_complete = true;
+      else
+        current_pol_complete = false;
+      clearHelmFlag();
+    }
+
+    if(msg_name == MSG_FALUT)
+    {
+      state = FAULT;
+      faultMsg = msg_str;
+      if(faultMsg == "AltitudeOut")
+        faultNumber = 4;
+      else if(faultMsg == "DepthOut")
+        faultNumber = 1;
+      else if(faultMsg == "RegionOut")
+        faultNumber = 2;
+      else if(faultMsg == "TimeOut")
+        faultNumber = 2;
+      else 
+        faultNumber = 99;
+    }
+
+    if(msg_name == MSG_SENDSAFTRULES && msg_str == "true")
+    {
+      state = CONFIG;
+      st = 40;
+    }
+    if(msg_name ==  MSG_CLEARFAULT)
+    {
+      st = 39;
+      state = FAULT;
+    }
+
+    if(msg_name == MSG_IN_SSM)
+    {
+      Json::Value j;
+      Json::Reader a;
+      a.parse(msg_str,j);
+      RepList["FormSSM"] = j;
+      if(j["action"].asString() == "start")
+      {
+        taskName = j["taskName"].asString();
+        state = RUN;
+        cout << "Task Manager Status is Run!!!" << endl;
+        start = true;
+        st = 10;
+      }
+      else if (j["action"].asString() == "stop")
+      {
+        taskName = "";
+        state = UNRUN;
+        cout << "Task Manager Status is UnRun!!!" << endl;
+        st = 20;
+      }
+      else
+      {
+        RepList["RunWaring"] = "form SSM have unhandle action : " + j["action"].asString();
+      }
+    }
+
+    if(msg_name == MSG_IN_MAN)
+    {
+      if(msg_dval == 1)
+        state = MANUAL;
+      else
+        state = UNRUN;
+    }
+  
+  }
+  return(true);
+}
+
+//---------------------------------------------------------
+// Procedure: OnConnectToServer
+
+bool TaskManger::OnConnectToServer()
+{
+   Notify(MSG_ENDFLAG,"false");
+   Notify(MSG_WPTFLAG,"false");
+  //  Notify(MSG_START,"false");
+   Notify(MSG_RUN,"false");
+   RegisterVariables();
+   InitConfig();
+   return(true);
+}
+
+//---------------------------------------------------------
+// Procedure: Iterate()
+bool TaskManger::Iterate()
+{
+   // happens AppTick times per second
+  AppCastingMOOSApp::Iterate();
+  switch (state)
+  {
+  case RUN: //状态0～10
+  {
+    switch(st)
+    {
+      case 0://发送任务开始、超时和第一个节点
+      {
+        
+        //1. send first node
+        current_node = getNode(nodeList);
+        if(current_node == "")
+          st = 9;
+        else
+        {
+          sendNode(current_node, nodeList);
+          st = 1;
+        }
+        //2. send start
+        taskStart();
+        current_node_complete = false;
+        current_pol_complete = false;
+        break;
+      }
+      case 1://等待节点完成信息
+      {
+        if(current_node_complete || current_pol_complete)
+        {
+          cout << "Current WayPoint Node complete!" << endl;
+          if(current_node == "")
+            st = 9;
+          else
+            st = 2;
+        }
+        break;
+      }
+      case 2://读取当前节点信息
+      {
+        current_node = getNode(nodeList);
+        if(current_node != "")
+          st = 3;
+        else
+          st = 1;
+        break;
+      }
+      case 3://发送任务节点
+      {
+        sendNode(current_node, nodeList);
+        current_node_complete = false;
+        current_pol_complete = false;
+        st = 1;
+        break;
+      }
+      case 9://task complete
+      {
+        taskFinish();
+        st = 10;
+        start = false;
+        break; 
+      }
+      case 10://等待任务开始
+      {
+        if(start)
+          st = 11;
+        break;
+      }
+      case 11://读取任务文件
+      {
+        faultMsg = "";
+        readTask = readTaskFile(taskName);
+        if( readTask != 0)
+        {
+          state = FAULT;
+          faultNumber = 10 + readTask;
+          st =30;
+          faultMsg = "TaskFileError : " + intToString(readTask);
+        }
+        st = 0;
+        break;
+      }
+      default:
+      {
+        st = 10;
+        //cout << "State reset ..." << endl;
+        //taskFinish();
+        break;
+      }
+    }
+    break;
+  }
+  case UNRUN: //状态20～29
+  {
+    switch(st)
+    {
+      case 20://进入UNRUN，初始化状态变量
+      {
+        current_node_complete = false;
+        current_pol_complete = false;
+        current_node="";
+        start = false;
+        Notify(MSG_ENDFLAG,"false");
+        Notify(MSG_WPTFLAG,"false");
+        // Notify(MSG_START,"false");
+        Notify(MSG_RUN,"false");
+        Notify("MOOS_MANUAL_OVERRIDE","true");
+        cout << "Wating task start......" << endl;
+        st = 21;
+        break;
+      }
+      case 21://等待任务开始
+      {
+        break;
+      }
+      default:
+      {
+        st = 20;
+        break;
+      }
+    }
+    break;
+  }
+  case FAULT: //状态30～39
+  {
+    switch(st)
+    {
+      case 30: //判断故障信息
+      {
+        if(faultNumber>0 && faultNumber<10) 
+          st = 31;
+        else if(faultNumber>10 && faultNumber<20)
+          st = 32;
+        else
+          st = 38;
+        break;
+      }
+      case 31: //任务错误
+      {
+        postFaultNumber(faultNumber);
+        state = UNRUN;
+        break;
+      }
+      case 32: //任务文本错误
+      {
+        postFaultNumber(faultNumber);
+        state = UNRUN;
+        break;
+      }
+      case 38: //未知故障
+      {
+        postFaultNumber(99);
+        break;
+      }
+      case 39:
+      {
+        FaultFlagClear();
+        state = UNRUN;
+        break;
+      }
+      default:
+      {
+        st = 30;
+        break;
+      }
+    }
+    break;
+  }
+  case CONFIG: //状态40～49
+  {
+    switch (st)
+    {
+      case 40: //配置安全规则
+      {
+        readSafetyRules("a");
+        setSafetyRules(maxTime, maxDepth, minAltitude, safePolygon);
+        // setMaxDepth(maxDepth);
+        setMaxDepth("2");
+        st = 49;
+        break;
+      }
+      case 41: //路径参数配置
+      {
+        readWayConfig("a");
+        setWayConfig(lead,lead_damper,capture_line,capture_radius,slip_radius);
+        st = 49;
+        //setWayConfig();
+      }
+      case 49: //等待
+      {
+        state = UNRUN;
+        break; 
+      }
+      default:
+      {
+        st = 49;
+        break;
+      } 
+    }
+
+    break;
+  }
+  case MANUAL: // 手操状态
+  {
+    switch (st)
+    {
+    case 50: // 清除变量
+      current_node_complete = false;
+      current_pol_complete = false;
+      current_node="";
+      start = false;
+      Notify(MSG_ENDFLAG,"false");
+      Notify(MSG_WPTFLAG,"false");
+      Notify(MSG_RUN,"false");
+      Notify("MOOS_MANUAL_OVERRIDE","true");
+      cout << "Wating task start......" << endl;
+      st = 51;
+      break;
+    case 51: // 等待
+      break;
+    default:
+      st = 50;
+      break;
+    }
+    break;
+  }
+  default:
+  {
+    st = 30;
+    break;
+  }
+  }
+  postReportToSSM();
+  AppCastingMOOSApp::PostReport();
+  return(true);
+}
+
+//---------------------------------------------------------
+// Procedure: OnStartUp()
+// happens before connection is open
+
+bool TaskManger::OnStartUp()
+{
+  AppCastingMOOSApp::OnStartUp();
+  list<string> sParams;                                                                                         
+  if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) 
+  {         
+    list<string>::iterator p;                                           
+    for(p=sParams.begin(); p!=sParams.end(); p++) {                 
+      string original_line = *p;                                    
+      string line = *p;                                    
+      string param = stripBlankEnds(toupper(biteString(line, '=')));  
+      string value = stripBlankEnds(line);
+      
+      if(param == "PLANCONFIGPATH")
+      {
+        planConfigPath = value;
+        RepList["CueenPath"] = planConfigPath;
+      }                           
+    }
+  } 
+  if(planConfigPath == "")
+    reportConfigWarning("NO TASK FILE PATH");
+  // readTaskFile("a");
+  RegisterVariables();	
+  return(true);
+}
+
+//---------------------------------------------------------
+// Procedure: RegisterVariables
+/**
+ * @description: 
+ * @param {double} depth
+ * @return {*}
+ */
+bool TaskManger::setMaxDepth(string depth)
+{
+  string msgContent = "";
+  msgContent = "max_depth=";
+  msgContent += depth;
+  Notify(UPDATE_MAXDEP, msgContent);
+  cout << "set max depth is " + depth << "m!" << endl;
+  return true;
+}
+bool TaskManger::setWayPol(string polygon, string speed, string depth)
+{
+  string msgContent="";
+  if(polygon=="#")
+  {
+    msgContent = "speed=";
+    msgContent += speed;
+    Notify(UPDATE_SPD, msgContent);
+
+    msgContent = "depth=" + depth;
+    Notify(UPDATE_DEP, msgContent);
+  }
+  else
+  {
+    msgContent = "polygon=";
+    msgContent += polygon;
+    msgContent += "#";
+    msgContent += "speed=";
+    msgContent += speed;
+    Notify(UPDATE_WPT, msgContent);
+
+    //使用模版参数生成一个新的行为r
+    Notify(UPDATE_WPT,"name=way_track");
+    // msgContent = "polygon=";
+    // msgContent += polygon;
+    // msgContent += "#";
+    Notify(UPDATE_WPT, msgContent);
+    msgContent = "speed=";
+    msgContent += speed;
+    Notify(UPDATE_SPD, msgContent);
+
+    msgContent = "depth=" + depth;
+    Notify(UPDATE_DEP, msgContent);
+  };
+  return true;
+}
+/**
+ * @description: 
+ * @param {string} wpt
+ * @return {*}
+ */
+bool TaskManger::setWpt(std::string wpt, string speed, string depth)
+{
+  string msgContent="";
+
+  msgContent += "points=";
+  msgContent += wpt;
+  msgContent += "#";
+  msgContent += "speed=";
+  msgContent += speed;
+  // msgContent += "#";
+  Notify(UPDATE_WPT,msgContent);
+  Notify(UPDATE_WPT,"name=way_topoint");
+  
+
+  msgContent = "depth=" + depth;
+  Notify(UPDATE_DEP, msgContent);
+
+  msgContent = "speed=";
+  msgContent += speed;
+  Notify(UPDATE_SPD, msgContent);
+  
+
+  cout << "MSG is : "  << msgContent + "#depth=" << depth << endl;
+  // Notify("WPT_UPDATE", wpt);
+  return true;
+}
+
+/**
+ * @description: 
+ * @param {double} speed
+ * @return {*}
+ */
+bool TaskManger::setSpeed(double speed)
+{ 
+  Notify("SPEED_UPDATE", speed);
+  return true;
+}
+
+bool TaskManger::setTaskTimer(string timeCount)
+{
+  cout << "Set task timer is " << timeCount  << "seconds!" << endl;
+  string msgContent="";
+  msgContent += "name=taskTimer";
+  msgContent += "#";
+  msgContent += "duration=";
+  msgContent += timeCount;
+  Notify(UPDATE_TIMER,msgContent);
+}
+
+bool TaskManger::setSafetyRules(string maxTime, string maxDepth, string minAltitude, string polygon)
+{
+  string msgContent="";
+  msgContent += "max_time=";
+  msgContent += maxTime;
+  msgContent += "#";
+  msgContent += "max_depth=";
+  msgContent += maxDepth;
+  msgContent += "#";
+  msgContent += "min_altitude=";
+  msgContent += minAltitude;
+  msgContent += "#";
+  msgContent += "polygon=";
+  msgContent += polygon;
+
+  cout << " The Op_Region parm is : " << msgContent << endl;
+  Notify(UPDATE_OPREGION, msgContent);
+  return true;
+}
+bool TaskManger::setWayConfig(string lead, string lead_damper, string capture_line, string capture_radius,string slip_radius)
+{
+  string msgContent="";
+
+  msgContent = "lead=";
+  msgContent += lead;
+  msgContent += "#";
+  msgContent += "lead_damper=";
+  msgContent += lead_damper;
+  msgContent += "#";
+  msgContent += "capture_line=";
+  msgContent += capture_line;
+  msgContent += "#";
+  msgContent += "capture_radius=";
+  msgContent += capture_radius;
+  msgContent += "#";
+  msgContent += "slip_radius=";
+  msgContent += slip_radius;
+
+  Notify(UPDATE_WPT, msgContent);
+  cout << "Config waypoint parm is :" << msgContent << endl;
+}
+
+/**
+ * @description: 
+ * @return {*}
+ */
+void TaskManger::RegisterVariables()
+{
+  AppCastingMOOSApp::RegisterVariables();
+  Register(MSG_ENDFLAG, 0);
+  // Register(MSG_START, 0);
+  Register(MSG_WPTFLAG,0);
+  Register(MSG_SENDSAFTRULES,0);
+  Register(MSG_FALUT,0);
+  Register(MSG_CLEARFAULT,0);
+  Register(MSG_IN_SSM,0);
+  Register(MSG_IN_MAN,0);
+}
+/**
+ * @description: read task File to nodeList
+ * @param {string} fileName
+ * @return {*} 0 if true else >0
+ */
+int  TaskManger::readTaskFile(string taskName)
+{
+  int faultNubmer = 0;
+  if(!nodeList.empty())
+    nodeList.clear();
+  ifstream ifs;
+  ifs.open(planConfigPath, ios::in);
+  Json::Reader taskConfigureReader;
+  Json::Value inputJsonValue;
+  Json::Value currentTask;
+  taskConfigureReader.parse(ifs, inputJsonValue);
+  ifs.close();
+  int taskCount = inputJsonValue.size();
+  taskList = inputJsonValue.getMemberNames(); 
+  taskCount = inputJsonValue.size();
+  //part1 ： 判断是否存在这个任务
+  if (!inputJsonValue.isMember(taskName))
+  {
+    RepList["Task in File"] = "False";
+    return faultNubmer=1;
+  }
+  else
+  {
+    //part2 : 读取任务
+    string node="";
+    currentTask = inputJsonValue[taskName];
+    if(currentTask["taskName"].asString() != taskName)
+      return faultNubmer=2;
+    
+    double currentTask_maxTime = currentTask["duration"].asDouble();
+    double repeat = currentTask["repeat"].asDouble();
+    
+    if(!currentTask["points"].isArray())
+      return faultNubmer=3;
+
+    Json::Value currentTask_Points = currentTask["points"]; 
+    int ps_cnt = currentTask_Points.size();
+    for(int i=0; i<ps_cnt; i++)
+    {
+      string node_name  = currentTask_Points[i]["name"].asString();
+      string node_x     = currentTask_Points[i]["north"].asString();
+      string node_y     = currentTask_Points[i]["east"].asString();
+      string node_depth = currentTask_Points[i]["depth"].asString();
+      string node_speed = currentTask_Points[i]["speed"].asString();
+      string node_type  = currentTask_Points[i]["type"].asString();
+
+      if(node_type == "point")
+        node_type = "@1";
+      else if(node_type == "track")
+        node_type = "@2";
+      else
+        return faultNubmer=4;
+      node = node_type;
+      node += ",";
+      node += node_x;
+      node += ",";
+      node += node_y;
+      node += ",";
+      node += node_depth;
+      node += ",";
+      node += node_speed;
+      node += ",";
+      node += node_name;
+      nodeList.push_back(node);
+    }
+  }
+  auto i = nodeList.begin();
+  cout << "-------current task node--------" << endl;
+  while (i != nodeList.end())
+  {
+    cout << *i << endl;
+    i++;
+  }
+  cout << "--------------------------------" << endl;
+  return 0;
+}
+
+int TaskManger::readSafetyRules(string fileName)
+{
+  maxDepth = "30";
+  maxTime  = "1000";
+  safePolygon = "pts={-80,-00:-30,-175:150,-100:95,25}";
+  return 0;
+}
+
+int TaskManger::readWayConfig(string filename)
+{
+  lead = "8";
+  lead_damper = "1";
+  lead_to_start = "false";
+  capture_line = "true";
+  capture_radius = "5";
+  slip_radius = "15";
+  efficiency_measure = "all";
+}
+
+/**
+ * @description: get Depth form
+ * @param {string} node
+ * @return {*}
+ */
+string TaskManger::getDepth(string node)
+{
+  string depth;
+  if(getWord(node, 3, depth))
+  {
+    // cout << "Current Node Depth is " << depth <<endl;
+    return depth;
+  }
+  else
+    cout << "Warming: read Depth Parm Filed" << endl;
+  return depth;
+}
+/**
+ * @description: 
+ * @param {string} node
+ * @return {*}
+ */
+string TaskManger::getSpeed(string node)
+{
+  string speed;
+  if(getWord(node, 4, speed))
+  {
+    // cout << "Current Node Speed is " << speed <<endl;
+    return speed;
+  }
+  else
+    cout << "Warming: read Speed Parm Filed" << endl;
+  // getWord(node, 3, depth);
+  return speed;
+}
+/**
+ * @description: 
+ * @param {string} node
+ * @return {*}
+ */
+string TaskManger::getWayPoint(string node)
+{
+  string x,y;
+  bool fr;
+  fr = getWord(node, 1, x);
+  fr &= getWord(node, 2, y);
+  if(fr)
+  {
+    // cout << "Current WayPoint is: " << x << "," << y << endl;
+    return x+","+y;
+  }
+  else
+    cout << "Warming: read WayPoint Parm Filed"  << endl;
+}
+
+string TaskManger::getWayPolygon(const list<string> nodeList)
+{
+  auto i = nodeList.begin();
+  string polygon="";
+  TaskType t;
+  while(i != nodeList.end())
+  {
+    if(getTaskTpye(*i)==WAYPOLYGON)
+    {
+      polygon += getWayPoint(*i);
+      polygon += ":";
+    }
+    else
+      break;
+    i++;
+  }
+  if(polygon.size()>1)
+    polygon.erase(polygon.end()-1);
+  // cout << "Current way polygon is: " << polygon << endl;
+  return polygon;
+}
+/**
+ * @description: 
+ * @param {string} node
+ * @return {*}
+ */
+TaskType TaskManger::getTaskTpye(string node)
+{
+  string type;
+  TaskType nodetype;
+  if(getWord(node, 0, type))
+  {
+    // cout << "Current task type is: " << type << endl;
+    // return type;
+    if(type=="@1")
+      return nodetype=WAYPOINTS;
+    else if(type=="@2")
+      return nodetype=WAYPOLYGON;
+    else if(type=="@3")
+      return nodetype=CONSTHEADING;
+    else
+      return nodetype=NOTYPE;
+  }
+  else
+    cout << "Warming: read type of task Parm Filed"  << endl;
+}
+/**
+ * @description: 
+ * @param {string} str
+ * @param {int} c
+ * @return {*}
+ */
+bool TaskManger::getWord(const string str, int ct, string &word)
+{
+  string cword;
+  int count=0;
+  for(auto c:str)
+  {
+    if(c==',')
+    {
+      if(count==ct)
+      {
+        word = cword;
+        return true;
+      }
+      cword="";
+      count++;
+    }
+    else
+      cword += c;
+  }
+  word = cword;
+  if(ct==0)
+    return true;
+  else
+    return false;
+}
+
+string TaskManger::getNode(list<string> &nodelist)
+{
+  string current_node;
+  if(!nodelist.empty())
+  {
+    cout << "The remaining number of nodes is: " << nodelist.size() << endl;
+    current_node = nodelist.front();
+    nodelist.pop_front();
+    cout << "Current Node is " << current_node << endl;
+  }
+  else
+    current_node="";
+  return current_node;
+}
+
+string TaskManger::getTimeCount()
+{
+  return "1000";
+}
+
+string TaskManger::getMaxDepth()
+{
+  return "20";
+}
+string  TaskManger::getNodeName(const string node)
+{
+  string name="";
+  if(getWord(node, 5, name))
+  {
+    return name;
+  }
+  else
+    cout << "Warming: read Speed Parm Filed" << endl;
+  // getWord(node, 3, depth);
+  return name;
+}
+bool TaskManger::sendNode(const string current_node, list<string> &nodeList)
+{
+  // cout << "Current WayPoint Node complete" << endl;
+  string node_pol;
+  nodeType = getTaskTpye(current_node);
+  switch(nodeType)
+  {
+    case WAYPOINTS:
+    {
+      node_wpt = getWayPoint(current_node);
+      node_depth = getDepth(current_node);
+      node_speed = getSpeed(current_node);
+      setWpt(node_wpt, node_speed, node_depth);
+      current_node_complete = false;
+      break;
+    }
+    case WAYPOLYGON:
+    {
+      node_depth = getDepth(current_node);
+      node_speed = getSpeed(current_node);
+      if (current_pol_complete && !current_node_complete) //wayflag发布但是endflag没有发布时代表不是首个节点，不发布路径信息
+      {
+        node_pol = "#";
+      }
+      else //if(current_pol_complete && current_node_complete)
+      {
+        nodeList.push_front(current_node);
+        node_pol = getWayPolygon(nodeList);
+        nodeList.pop_front();
+      }
+      setWayPol(node_pol, node_speed, node_depth);
+      cout << "MSG is :Pol=" + node_pol + "#speed=" + node_speed + "#depth=" + node_depth<< endl;
+      break;
+    }
+    default:
+    break;
+  }
+  current_node_complete = false;
+  current_pol_complete = false;
+  
+  return true;
+}
+
+void TaskManger::FaultFlagClear()
+{
+  faultNumber = 0;
+  if(faultMsg == "AltitudeOut")
+    Notify(UPDATE_RESETFAULT,"altittude");
+  else if(faultMsg == "DepthOut")
+    Notify(UPDATE_RESETFAULT,"depth");
+  else if(faultMsg == "RegionOut")
+    Notify(UPDATE_RESETFAULT,"poly");  
+  else if(faultMsg == "TimeOut")
+    Notify(UPDATE_RESETFAULT,"time");
+  else
+    cout << "Can not Clear Unknow Fault Flag!!!" << endl;
+  cout << "Clear Fault : " + faultMsg << endl;  
+}
+
+bool TaskManger::buildReport()
+{
+  switch (state)
+  {
+  case UNRUN:
+  {
+    m_msgs << "$Task Manager Status$:" << "UNRUN" << endl;
+    m_msgs << "$Idel Task Number$:" << taskCount << endl;
+    break;
+  }
+  case RUN:
+  {
+    m_msgs << "$Task Manager Status$:" << "RUN" << endl;
+    m_msgs << "$Current Task Point$ :" << current_node << endl;
+    m_msgs << "Currnet Task List    :" << endl;
+    for(string i : nodeList)
+      m_msgs << "  :  " << i << endl;  
+    break;
+  }
+  case FAULT:
+  {
+    m_msgs << "$Task Manager Status$:" << "FAULT" << endl;
+    break;
+  }
+
+  default:
+    break;
+  }
+  m_msgs << "=========================================================" << endl;
+  RepList["Current Node"] = current_node;
+  RepList["remaining number of nodes"] = nodeList.size()+1;
+  string rep = Json::writeString(RepJsBuilder, RepList);
+  m_msgs << rep << endl;
+  return true;
+}
+
+inline void TaskManger::taskStart()
+{
+  Notify("RUN","true");
+  Notify("MOOS_MANUAL_OVERRIDE","false");
+}
+inline void TaskManger::taskFinish()
+{    
+    current_node="";
+    Notify("RUN","false");
+    state = UNRUN;
+    cout << "The task node is all complete!!!" << endl;
+    Notify("MOOS_MANUAL_OVERRIDE","true");
+}
+inline void TaskManger::InitConfig()
+{
+  Notify(MSG_ENDFLAG,"false");
+  Notify(MSG_WPTFLAG,"false");
+  // Notify(MSG_START,"false");
+  Notify(MSG_RUN,"false");
+  //
+  readSafetyRules("q");
+  setSafetyRules(maxTime, maxDepth, minAltitude, safePolygon);
+  readWayConfig("a");
+  setWayConfig(lead,lead_damper,capture_line,capture_radius,slip_radius);
+}
+
+void TaskManger::postReportToSSM()
+{
+  string s_msg;
+  Json::Value msg;
+  msg["state"] = state;
+  msg["taskName"] = taskName;
+  msg["destName"] = getNodeName(current_node);
+  msg["errorCode"] = faultNumber;
+  // msg["progess"] = 
+  // msg["eta"] = 
+  RepList["toSSM"] = msg;
+  s_msg = Json::writeString(RepJsBuilder,msg);
+  Notify(MSG_TO_SSM, s_msg);
+}
+//1:超深
+//2.超时
+//3.超出区域
+//4.低于最小高度
+//11:任务文本
+//99.未知故障
+void TaskManger::postFaultNumber(int n)
+{
+  Notify(MSG_TO_FH,n);
+}

src/pTaskManger/TaskManger.h

@@ -0,0 +1,150 @@
+/*
+ * @Author: 1553836110 1553836110@qq.com
+ * @Date: 2023-09-28 15:45:17
+ * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditTime: 2023-11-08 14:53:05
+ * @FilePath: /moos-ivp-pi/src/pTaskManger/TaskManger.h
+ * @Description: 
+ * 
+ * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
+ */
+/************************************************************/
+/*    NAME: zjk                                              */
+/*    ORGN: MIT                                             */
+/*    FILE: TaskManger.h                                          */
+/*    DATE:                                                 */
+/************************************************************/
+
+#ifndef TaskManger_HEADER
+#define TaskManger_HEADER
+
+#include "MOOS/libMOOS/MOOSLib.h"
+#include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
+#include <queue>
+#include <list>
+#include "json/json.h"
+#include "VarDataPair.h"
+
+using namespace std;
+enum TaskType{WAYPOINTS, CONSTDEPTH, CONSTSPEED, CONSTHEADING, WAYPOLYGON, NOTYPE};
+enum Status{FAULT=0, UNRUN=1, MANUAL=2 ,RUN=3,  CONFIG=5};
+// class TaskManger : public CMOOSApp
+class TaskManger : public AppCastingMOOSApp
+{
+ public:
+  TaskManger();
+  virtual ~TaskManger();
+
+  bool OnNewMail(MOOSMSG_LIST &NewMail);
+  bool Iterate();
+  bool OnConnectToServer();
+  bool OnStartUp();
+  void RegisterVariables();
+  bool buildReport();
+  void postFaultNumber(int n);
+
+  bool setSpeed(double speed);
+  bool setMaxDepth(string depth);
+  bool setWpt(std::string wpt, string speed, string depth);
+  bool setWayPol(string polygon, string speed, string depth);
+  bool setTaskTimer(string timerCount);
+  bool setSafetyRules(string maxTime, string maxDepth, string minAltitude, string polygon);
+  bool setWayConfig(string lead, string lead_damper, string capture_line, string capture_radius,string slip_radius);
+  
+  bool sendNode(const string current_node, list<string> &nodeList);
+
+  bool getWord(const string str, int ct, string &word);
+  string getSpeed(const string node);
+  string getDepth(const string node);
+  string getWayPoint(const string node);
+  string getWayPolygon(const list<string> nodeList);
+  string getNodeName(const string node);
+  string getNode(list<string> &nodelist);
+  string getTimeCount();
+  string getMaxDepth();
+  TaskType getTaskTpye(const string node);
+
+  int readTaskFile(string taskName);
+  int readSafetyRules(string fileName);
+  int readWayConfig(string fileName);
+  void postReportToSSM();
+  
+  inline void clearHelmFlag(){Notify("HELM_MAP_CLEAR",0.0);}
+  inline void taskStart();
+  inline void taskFinish();
+  void FaultFlagClear();
+  inline void InitConfig();
+
+  const string UPDATE_WPT = "WPT_UPDATE";
+  const string UPDATE_SPD = "SPEED_UPDATE";
+  const string UPDATE_DEP = "DEPTH_UPDATE";
+  const string UPDATE_TIMER = "TIMER_UPDATES";
+  const string UPDATE_MAXDEP = "MAXDEEP_UPDATES";
+  const string UPDATE_OPREGION = "OPREGION_UPDATES";
+  const string UPDATE_RESETFAULT = "OPREGION_RESET";
+
+  const string MSG_WPTFLAG = "CurrentPointComplete";
+  const string MSG_ENDFLAG = "END_WayPoint";
+  const string MSG_START = "START";
+  const string MSG_SENDSAFTRULES = "SendSaftRules";
+  const string MSG_FALUT = "TaskFault";
+  const string MSG_RUN = "RUN";
+  const string MSG_CLEARFAULT = "ClearFalut";
+
+  const string MSG_IN_SSM = "uMission_action_cmd";
+  const string MSG_TO_SSM = "uMission_task_fb";
+  const string MSG_TO_FH = "uMission_fault_fb";
+  const string MSG_IN_FH = "uDrive_fault_fb";
+  const string MSG_IN_MAN = "uManual_enable_cmd";  //TODO: 增加手操状态
+
+protected:
+// 任务参数
+  string node_wpt;
+  string node_depth;
+  string node_speed;
+// 安全规则
+  string maxDepth;
+  string maxTime;
+  string safePolygon;
+  string minAltitude="0";
+// 路径参数
+  string lead;
+  string lead_damper;
+  string lead_to_start;
+  string capture_line;
+  string capture_radius;
+  string slip_radius;
+  string efficiency_measure;
+
+  string faultMsg;
+
+  TaskType nodeType;
+  list<string> nodeList;
+
+private:
+  bool current_node_complete = false;
+  bool current_pol_complete = false;
+  bool start=false;
+  bool time_out=false;
+  int st=10;
+  int faultNumber = 0;
+  int readTask = 0;
+  string msg;
+  string current_node;
+  Status state = UNRUN;
+
+  Json::Value RepList;
+  Json::StreamWriterBuilder RepJsBuilder;
+
+
+//任务文件参数
+//TODO:动态配置任务文件等参数
+  string planConfigPath;
+  vector<string> taskList;
+  int taskCount;
+  string taskName;
+//发送节点信息
+  Json::Value node_rpt;   
+};
+
+#endif 

src/pTaskManger/TaskMangerMain.cpp

@@ -0,0 +1,65 @@
+/*
+ * @Author: zjk 1553836110@qq.com
+ * @Date: 2023-09-28 15:45:17
+ * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditTime: 2023-10-24 15:13:22
+ * @FilePath: /moos-ivp-extend/src/pTaskManger/TaskMangerMain.cpp
+ * @Description: 
+ * 
+ * Copyright (c) 2023 by ${git_name_email}, All Rights Reserved. 
+ */
+/************************************************************/
+/*    NAME: zjk                                              */
+/*    ORGN: MIT                                             */
+/*    FILE: TaskMangerMain.cpp                                    */
+/*    DATE:                                                 */
+/************************************************************/
+
+#include <string>
+#include "TaskManger.h"
+#include "ColorParse.h"
+#include "MBUtils.h"
+
+using namespace std;
+
+int main(int argc, char *argv[])
+{
+  string mission_file;
+  string run_command = argv[0];
+
+  for(int i=1; i<argc; i++) {
+    string argi = argv[i];
+    if((argi=="-v") || (argi=="--version") || (argi=="-version"))
+      // showReleaseInfoAndExit();
+      cout << " Version : 0.01" << endl;
+    else if((argi=="-e") || (argi=="--example") || (argi=="-example"))
+      // showExampleConfigAndExit();
+      cout << " example : NULL" << endl;
+    else if((argi=="-h") || (argi == "--help") || (argi=="-help"))
+      // showHelpAndExit();
+      cout << " Low leave Control for UUV " << endl;
+    else if((argi=="-i") || (argi == "--interface"))
+      // showInterfaceAndExit();
+      cout << " UUV Motion Control " << endl;
+    else if(strEnds(argi, ".moos") || strEnds(argi, ".moos++"))
+      mission_file = argv[i];
+    else if(strBegins(argi, "--alias="))
+      run_command = argi.substr(8);
+    else if(i == 2)
+      run_command = argi;
+  }
+  
+  if(mission_file == "")
+      // showHelpAndExit();
+
+  cout << termColor("green");
+  cout << "pTaskManger launching as " << run_command << endl;
+  cout << termColor() << endl;
+
+  TaskManger TaskMangerApp;
+  TaskMangerApp.Run(run_command.c_str(), mission_file.c_str(), argc, argv);
+
+  
+  return(0);
+}
+

src/pTaskManger/alpha.bhv

@@ -0,0 +1,181 @@
+//--------    FILE: alpha.bhv   -------------
+initialize RUN = false
+initialize TaskNum=t1
+initialize SendTask=false
+//--------------模式判断------------------------
+set MODE = ACTIVE{
+  RUN=true
+} INACTIVE
+
+set MODE = T1{
+  MODE=ACTIVE
+  TaskNum = t1
+}
+
+
+//----------路径点任务----------------------------
+Behavior = BHV_Waypoint
+{
+  name      = waypt_survey
+  pwt       = 100 //优先权重
+  condition = MODE==T1
+
+  //endflag   = START=false
+  endflag = END_WayPoint=true
+
+	configflag = CRUISE_SPD = $[SPEED]
+	//configflag = OSPOS = $[OSX],$[OSY]
+
+	activeflag = INFO=$[OWNSHIP]
+	activeflag = INFO=$[BHVNAME]
+	activeflag = INFO=$[BHVTYPE]
+
+	//cycleflag = CINFO=$[OSX],$[OSY]
+
+  wptflag = CurrentPointComplete=true
+	wptflag = PREV=$(PX),$(PY)
+	wptflag = NEXT=$(NX),$(NY)
+	wptflag = TEST=$(X),$(Y)
+	wptflag = OSPOS=$(OSX),$(OSY)
+	//wptflag_on_start = true
+  
+
+  updates    = WPT_UPDATE
+  //perpetual  = true
+
+  templating = spawn
+
+	//    speed_alt = 1.2
+  //use_alt_speed = true
+                lead = 8
+         lead_damper = 1
+       lead_to_start = false
+               speed = 1   // meters per second
+        capture_line = true
+      capture_radius = 5.0
+         slip_radius = 15.0
+	efficiency_measure = all
+
+               polygon = 60,-40
+               order = normal
+             //repeat  = 3
+
+    visual_hints = nextpt_color=yellow
+    visual_hints = nextpt_vertex_size=8
+    visual_hints = nextpt_lcolor=gray70
+    visual_hints = vertex_color=dodger_blue, edge_color=white
+    visual_hints = vertex_size=5, edge_size=1
+}
+
+
+//--------------定深任务------------------
+Behavior=BHV_ConstantDepth
+{
+  name = const_depth
+  pwt = 100
+  //condition = DEPLOY = true
+  condition = MODE==T1
+  duration = no-time-limit
+  updates = DEPTH_UPDATE
+  depth = 0
+}
+
+//--------------定向任务--------------------
+
+Behavior=BHV_ConstantHeading
+{
+  name       = const_heading
+  pwt        = 100
+  //condition  = START_TURN = true
+  //condition  = DEPLOY = true
+  condition = MODE==T3
+  perpetual  = true
+
+  activeflag = TURN = started
+
+  //endflag    = TURN = done
+  //endflag    = RETURN = true
+  //endflag    = START_TURN = false
+  endflag    = START=false
+
+          heading = 225
+  complete_thresh = 5
+       duration = no-time-limit
+ }
+
+//--------------定速任务--------------------
+Behavior=BHV_ConstantSpeed
+{
+  name       = const_speed
+  pwt        = 1000
+  condition = MODE==T1
+  perpetual  = true
+  updates    = SPEED_UPDATE
+  //endflag    = START=false
+
+  speed = 5
+
+  duration = no-time-limit
+  //peakwidth = 0.5
+  //basewidth = 0.5
+
+}
+//----------------安全模式-----------------------
+//----------------计时器---------------------
+Behavior = BHV_Timer
+{ 
+  name      = mtime
+  condition = MODE==T1
+  pwt = 100
+  templating = spawn
+	//duration_status = MSTATUS
+	//duration = 10
+  endflag = TIME_OUT=true
+
+  updates      = TIMER_UPDATES        
+  
+  //perpetual  = true
+}
+//-------------最大深度限制--------------------------
+Behavior = BHV_MaxDepth
+{
+  name = maxdepth
+  pwt  = 200
+  condition = MODE==ACTIVE
+  updates = MAXDEEP_UPDATES
+  max_depth = 20
+  tolerance = 0
+  duration = no-time-limit
+}
+//--------------安全区域设置-----------------------
+
+ Behavior = BHV_OpRegion
+ {
+   // General Behavior Parameters
+   // ---------------------------
+   name         = op_region                 // example
+   pwt          = 300                       // default
+   condition = MODE==TN
+   updates      = OPREGION_UPDATES          // example
+
+   // Parameters specific to this behavior
+   // ------------------------------------
+                  max_time = 20              // default (seconds)
+                 max_depth = 25              // default (meters)
+              min_altitude = 0              // default (meters)
+                 reset_var = OPREGION_RESET // example
+        trigger_entry_time = 1              // default (seconds)
+         trigger_exit_time = 0.5            // default (seconds) 
+
+                   polygon = pts={-80,-00:-30,-175:150,-100:95,25}
+
+    breached_altitude_flag = TaskFault = AltitudeOut
+       breached_depth_flag = TaskFault = DepthOut
+        breached_poly_flag = TaskFault = RegionOut
+        breached_time_flag = TaskFault = TimeOut
+
+              visual_hints = vertex_color = brown  // default
+              visual_hints = vertex_size  = 3      // default
+              visual_hints = edge_color   = aqua   // default
+              visual_hints = edge_size    = 1      // default
+ }

src/pTaskManger/alpha.moos

@@ -0,0 +1,269 @@
+//-------------------------------------------------
+// NAME: M. Benjamin, MIT CSAIL
+// FILE: alpha.moos
+//-------------------------------------------------
+
+ServerHost   = localhost
+ServerPort   = 9000
+Community    = pi
+MOOSTimeWarp = 1
+
+// Forest Lake
+LatOrigin  = 43.825300 
+LongOrigin = -70.330400 
+
+// MIT Sailing Pavilion (use this one)
+// LatOrigin  = 42.358456 
+// LongOrigin = -71.087589
+
+//------------------------------------------
+// Antler configuration  block
+ProcessConfig = ANTLER
+{
+  MSBetweenLaunches = 200
+
+  Run = MOOSDB        @ NewConsole = false
+  //Run = pLogger       @ NewConsole = false
+  Run = uSimMarineV22 @ NewConsole = false
+  Run = pMarinePIDV22 @ NewConsole = false
+  Run = pHelmIvP      @ NewConsole = false
+  Run = pMarineViewer	@ NewConsole = false
+  Run = uProcessWatch	@ NewConsole = false
+  Run = pNodeReporter	@ NewConsole = false
+  Run = pRealm	      @ NewConsole = false
+  Run = pTaskManger   @ NewConsole = false
+  //Run = uTimerScript  @ NewConsole = false
+}
+
+//------------------------------------------
+// pLogger config block
+ProcessConfig = pTaskManger
+{
+  AppTick   = 8
+  CommsTick = 8
+
+  planConfigPath  = /home/zjk/Desktop/project/moos-ivp-extend/PlanConfigure.json
+}
+ProcessConfig = pLogger
+{
+  AppTick   = 8
+  CommsTick = 8
+
+  AsyncLog = true
+
+  // For variables that are published in a bundle on their first post,
+  // explicitly declare their logging request
+  //Log = IVPHELM_LIFE_EVENT @ 0 NOSYNC
+  //Log = REPORT @ 0 NOSYNC
+  //Log = BHV_SETTINGS @ 0 NOSYNC
+  Log = OPREGION_RESET @ 0 NOSYNC
+
+  LogAuxSrc = true
+  WildCardLogging = true
+  WildCardOmitPattern = *_STATUS
+  WildCardOmitPattern = DB_VARSUMMARY
+  WildCardOmitPattern = DB_RWSUMMARY
+  WildCardExclusionLog = true
+}
+
+//------------------------------------------
+// uProcessWatch config block
+
+ProcessConfig = uProcessWatch
+{
+  AppTick   = 4
+  CommsTick = 4
+
+  watch_all = true
+	nowatch   = uPokeDB*
+	nowatch   = uQueryDB*
+	nowatch   = uXMS*
+	nowatch   = uMAC*
+}
+
+//------------------------------------------
+// uSimMarineV22 config block
+
+ProcessConfig = uSimMarineV22
+{
+  AppTick  	= 4
+  CommsTick	= 4
+
+  start_pos = x=0, y=-20, heading=180, speed=5
+
+  prefix        = NAV
+
+  turn_rate     = 40
+  thrust_map    = 0:0, 20:1, 40:2, 60:3, 80:4, 100:5
+  //thrust_reflect = true
+
+  buoyancy_rate = 0.075
+  max_depth_rate = 5
+  max_depth_rate_speed = 2.0
+  default_water_depth = 400
+}
+
+//------------------------------------------
+// pHelmIvP config block
+
+ProcessConfig = pHelmIvP
+{
+  AppTick    = 4
+  CommsTick  = 4
+
+  behaviors  = alpha.bhv
+  domain     = course:0:359:360
+  domain     = speed:0:10:101
+  domain     = depth:0:100:101
+
+  park_on_allstop = false
+  //park_on_allstop = true
+  
+}
+
+//------------------------------------------
+// pMarinePID config block
+
+ProcessConfig = pMarinePIDV22
+{
+  AppTick    = 20
+  CommsTick  = 20
+
+  verbose       = true
+  depth_control = true
+
+  // SIM_INSTABILITY = 20
+
+  // Yaw PID controller
+  yaw_pid_kp		 = 1.2
+  yaw_pid_kd		 = 0.0
+  yaw_pid_ki		 = 0.3
+  yaw_pid_integral_limit = 0.07
+
+  // Speed PID controller
+  speed_pid_kp		 = 1.0
+  speed_pid_kd		 = 0.0
+  speed_pid_ki		 = 0.1
+  speed_pid_integral_limit = 0.07
+
+  maxpitch = 15
+  maxelevator = 13
+
+  pitch_pid_kp = 1.5
+  pitch_pid_kd = 0
+  pitch_pid_ki = 1.0
+  pitch_pid_integral_limit = 0
+
+  z_to_pitch_pid_kp = 0.12
+  z_to_pitch_pid_kd = 0
+  z_to_pitch_pid_ki = 0.004
+  z_to_pitch_pid_integral_limit = 0.05
+  
+
+  //MAXIMUMS
+  maxrudder    = 100
+  maxthrust    = 100
+
+  // A non-zero SPEED_FACTOR overrides use of SPEED_PID
+  // Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
+  speed_factor = 0
+  simulation = true
+}
+
+//------------------------------------------
+// pMarineViewer config block
+
+ProcessConfig = pMarineViewer
+{
+  AppTick    = 4
+  CommsTick  = 4
+
+  tiff_file            = forrest19.tif
+	//tiff_file            = MIT_SP.tif
+  vehicles_name_mode = names+depth //+shortmode
+
+
+  set_pan_x            = -90
+  set_pan_y            = -280
+  zoom                 = 0.65
+  vehicle_shape_scale  = 1.5
+  hash_delta           = 50
+  hash_shade           = 0.22
+  hash_viewable        = true
+
+  trails_point_size   = 1
+
+	//op_vertex = x=-83, y=-47,    lwidth=1,lcolor=yellow,looped=true,group=moa
+  //op_vertex = x=-46.4, y=-129.2, lwidth=1,lcolor=yellow,looped=true,group=moa
+  //op_vertex = x=94.6, y=-62.2,   lwidth=1,lcolor=yellow,looped=true,group=moa
+  //op_vertex = x=58,   y=20,    lwidth=1,lcolor=yellow,looped=true,group=moa
+
+  // Appcast configuration
+  appcast_height       = 75
+  appcast_width        = 30
+  appcast_viewable     = true
+  appcast_color_scheme = indigo
+  nodes_font_size      = xlarge
+  procs_font_size      = xlarge
+  appcast_font_size    = large
+  
+  // datum_viewable = true
+  // datum_size     = 18
+  // gui_size = small
+
+  // left_context[survey-point] = DEPLOY=true
+  // left_context[survey-point] = MOOS_MANUAL_OVERRIDE=false
+  // left_context[survey-point] = RETURN=false
+
+  right_context[return] = DEPLOY=true
+  right_context[return] = MOOS_MANUAL_OVERRIDE=false
+  right_context[return] = RETURN=false
+
+  scope  = RETURN
+  scope  = WPT_STAT
+  scope  = VIEW_SEGLIST
+  scope  = VIEW_POINT
+  scope  = VIEW_POLYGON
+  scope  = MVIEWER_LCLICK
+  scope  = MVIEWER_RCLICK
+
+  //button_one = START # START=true
+  button_one = START # uMission_action_cmd={"taskName":"east_waypt_survey","action":"start"}
+  //button_one = MOOS_MANUAL_OVERRIDE=false
+  button_two = STOP # START=false
+  //button_two = MOOS_MANUAL_OVERRIDE=true
+  button_three = FaultClear # ClearFalut = true
+  button_four = SendSecurityZone # SendSaftRules = true
+
+
+  action  = MENU_KEY=deploy # DEPLOY = true # RETURN = false
+  action+ = MENU_KEY=deploy # MOOS_MANUAL_OVERRIDE=false
+  action  = RETURN=true
+  action  = UPDATES_RETURN=speed=1.4
+}
+
+//------------------------------------------
+// pNodeReporter config block
+
+ProcessConfig = pNodeReporter
+{
+  AppTick    = 2
+  CommsTick	 = 2
+
+  //platform_type   = kayak
+  //更改显示形状为uuv
+  platform_type = UUV 
+  platform_color  = red
+  platform_length = 4
+}
+
+ProcessConfig = uTimerScript
+{
+  AppTick = 4
+  CommsTick = 4
+
+  condition = DEPLOY = true
+  randvar = varname = RND_DEPTH, min=20, max=80, key=at_reset
+  event = var = DEPTH_UPDATE, val=depth=$[RND_DEPTH], time=120
+  reset_max = nolimit reset_time = all-posted
+}

src/pTaskManger/clean.sh

@@ -0,0 +1,37 @@
+#!/bin/bash 
+#--------------------------------------------------------------
+#   Script: clean.sh                                    
+#   Author: Michael Benjamin  
+#     Date: June 2020     
+#----------------------------------------------------------
+#  Part 1: Declare global var defaults
+#----------------------------------------------------------
+VERBOSE=""
+
+#-------------------------------------------------------
+#  Part 2: Check for and handle command-line arguments
+#-------------------------------------------------------
+for ARGI; do
+    if [ "${ARGI}" = "--help" -o "${ARGI}" = "-h" ] ; then
+	echo "clean.sh [SWITCHES]        "
+	echo "  --verbose                " 
+	echo "  --help, -h               " 
+	exit 0;	
+    elif [ "${ARGI}" = "--verbose" -o "${ARGI}" = "-v" ] ; then
+	VERBOSE="-v"
+    else 
+	echo "clean.sh: Bad Arg:" $ARGI
+	exit 1
+    fi
+done
+
+#-------------------------------------------------------
+#  Part 2: Do the cleaning!
+#-------------------------------------------------------
+if [ "${VERBOSE}" = "-v" ]; then
+    echo "Cleaning: $PWD"
+fi
+rm -rf  $VERBOSE   MOOSLog_*  XLOG_* LOG_* 
+rm -f   $VERBOSE   *~  *.moos++
+rm -f   $VERBOSE   targ_*
+rm -f   $VERBOSE   .LastOpenedMOOSLogDirectory

src/pTaskManger/launch.sh

@@ -0,0 +1,39 @@
+#!/bin/bash -e
+#----------------------------------------------------------
+#  Script: launch.sh
+#  Author: Michael Benjamin
+#  LastEd: May 20th 2019
+#----------------------------------------------------------
+#  Part 1: Set Exit actions and declare global var defaults
+#----------------------------------------------------------
+TIME_WARP=1
+COMMUNITY="alpha"
+GUI="yes"
+
+#----------------------------------------------------------
+#  Part 2: Check for and handle command-line arguments
+#----------------------------------------------------------
+for ARGI; do
+    if [ "${ARGI}" = "--help" -o "${ARGI}" = "-h" ] ; then
+	echo "launch.sh [SWITCHES] [time_warp]   "
+	echo "  --help, -h           Show this help message            " 
+	exit 0;
+    elif [ "${ARGI}" = "--nogui" ] ; then
+	GUI="no"
+    elif [ "${ARGI//[^0-9]/}" = "$ARGI" -a "$TIME_WARP" = 1 ]; then 
+        TIME_WARP=$ARGI
+    else 
+        echo "launch.sh Bad arg:" $ARGI " Exiting with code: 1"
+        exit 1
+    fi
+done
+
+
+#----------------------------------------------------------
+#  Part 3: Launch the processes
+#----------------------------------------------------------
+echo "Launching $COMMUNITY MOOS Community with WARP:" $TIME_WARP
+pAntler $COMMUNITY.moos --MOOSTimeWarp=$TIME_WARP >& /dev/null &
+
+uMAC -t $COMMUNITY.moos
+kill -- -$$

src/pTaskManger/pTaskManger.moos

@@ -0,0 +1,12 @@
+// MOOS file
+
+ServerHost = localhost
+ServerPort = 9000
+
+ProcessConfig = pTaskManger
+{
+   AppTick   = 10
+   CommsTick = 10
+   planConfigPath  = /home/zjk/Desktop/project/moos-ivp-extend/PlanConfigure.json
+}
+

src/pTaskSend/CMakeLists.txt

@@ -0,0 +1,38 @@
+#--------------------------------------------------------
+# The CMakeLists.txt for:                       pTaskSend
+# Author(s):                              zjk
+#--------------------------------------------------------
+
+FILE(GLOB SRC *.cpp)
+
+SET(CMAKE_CXX_STANDARD 11)
+
+FIND_LIBRARY(DUNE_LIB dune-core /usr/local/lib /usr/local/lib/DUNE)
+FIND_PATH(DUNE_INCLUDE DUNE/IMC.hpp /usr/local/include /usr/local/include/DUNE)
+include_directories(${DUNE_INCLUDE})
+
+include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+
+# find_package (jsoncpp NO_MODULE REQUIRED)
+
+include_directories(/usr/include/jsoncpp/)
+link_directories(/usr/local/lib/)
+
+ADD_EXECUTABLE(pTaskSend ${SRC})
+
+TARGET_LINK_LIBRARIES(pTaskSend
+   ${MOOS_LIBRARIES}
+   ${CMAKE_DL_LIBS}
+   ${SYSTEM_LIBS}    
+   ${DUNE_LIB}   
+   mbutil
+   m
+   pthread
+   fltk # Standard libraries used by this project's FLTK apps...
+   fltk_gl 
+   dl
+   tiff
+   jsoncpp 
+   # jsoncpp_lib_static 
+)
+

src/pTaskSend/TaskSend.cpp

@@ -0,0 +1,545 @@
+/*
+ * @Author: error: error: git config user.name & please set dead value or install git && error: git config user.email & please set dead value or install git & please set dead value or install git
+ * @Date: 2023-09-21 11:04:00
+ * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditTime: 2023-10-07 10:37:30
+ * @FilePath: /moos-ivp-extend/src/pTaskSend/TaskSend.cpp
+ * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
+ */
+/************************************************************/
+/*    NAME: zjk                                              */
+/*    ORGN: MIT                                             */
+/*    FILE: TaskSend.cpp                                        */
+/*    DATE:                                                 */
+/************************************************************/
+
+#include <list>
+#include <iterator>
+#include "MBUtils.h"
+#include "TaskSend.h"
+#include <iostream>
+
+#include <json/json.h>
+
+using namespace std;
+
+#define UDP_RECEIVE_PORT 8000
+#define TCP_SEND_PORT 6000 
+#define TCP_SERVER_ADDRESS "127.0.0.1" 
+
+using namespace std;
+
+//---------------------------------------------------------
+// Constructor
+
+TaskSend::TaskSend()
+{
+}
+
+//---------------------------------------------------------
+// Destructor
+
+TaskSend::~TaskSend()
+{
+}
+
+//---------------------------------------------------------
+// Procedure: OnNewMail
+
+bool TaskSend::OnNewMail(MOOSMSG_LIST &NewMail)
+{
+   MOOSMSG_LIST::iterator p;
+   
+   for(p=NewMail.begin(); p!=NewMail.end(); p++) 
+   {
+      CMOOSMsg &msg = *p;
+      p->Trace();
+      // current_msg = msg;
+      if(p->GetName()== "SendTask")
+        if(p->GetString() == "true")
+        {
+          editView=true;
+        }
+      
+      if(p->GetName()=="BHV_WARNING")
+        if(p->GetString() != "")
+        {
+          error = true;
+        }
+        
+   }
+   
+   return(true);
+}
+
+//---------------------------------------------------------
+// Procedure: OnConnectToServer
+
+bool TaskSend::OnConnectToServer()
+{
+   Notify("SendTask","false");
+   RegisterVariables();
+   return(true);
+}
+
+//---------------------------------------------------------
+// Procedure: Iterate()
+
+bool TaskSend::Iterate()
+{
+   // happens AppTick times per second
+   if(editView)
+   {
+      error=false;
+      view = taskEditView();
+      view->show();
+      // delete view;
+      // Fl::grab();
+      Fl::run();                                                   /* 6. 运行FLTK主循环 */
+      cout << "editView = true" << endl;
+      editView=false;
+   }
+   
+   if(error)
+   {
+      error=true;
+      disp1->value("parameter incorrect");
+
+   }
+
+  cout << "error=" << error << "editView" << editView << endl;
+      
+
+  return(true);
+}
+
+//---------------------------------------------------------
+// Procedure: OnStartUp()
+// happens before connection is open
+
+bool TaskSend::OnStartUp()
+{
+  list<string> sParams;                                                                                         
+  if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) {         
+    list<string>::iterator p;                                           
+    for(p=sParams.begin(); p!=sParams.end(); p++) {                 
+      string original_line = *p;                                    
+      string line = *p;                                    
+      string param = stripBlankEnds(toupper(biteString(line, '=')));  
+      string value = stripBlankEnds(line);
+                                                           
+      if(param == "FOO") {                                      
+        //handled                                               
+      }                                                             
+      else if(param == "BAR") {                                  
+        //handled                                     
+      }                            
+    }
+  }
+
+  RegisterVariables();
+
+#if 1
+  sock_tcp_send.connect(TCP_SERVER_ADDRESS, TCP_SEND_PORT);
+  sock_tcp_send.setKeepAlive(true);
+#endif
+
+  return(true);
+}
+
+std::string TaskSend::SetPlan1(std::string sourceName, double stamp)
+{
+  struct WayPointBehavior behavior;
+  behavior.source = sourceName;
+  behavior.priority = -1;
+  behavior.points.clear();
+  behavior.name = "east_waypt_survey";
+  behavior.priority = 10;
+  struct Landmark station_1 = {-70.328891,43.824429, 10, 3};
+  struct Landmark station_2 = {-70.327885,43.824676, 8, 5};
+  struct Landmark station_3 = {-70.327867,43.823622, 6, 7};
+  struct Landmark station_4 = {-70.328765,43.823622, 4, 9};
+  behavior.points.push_back(station_1);
+  behavior.points.push_back(station_2);
+  behavior.points.push_back(station_3);
+  behavior.points.push_back(station_4);
+  behavior.duration = -1;
+  behavior.constSpeed = -1;
+  behavior.repeate = -1;     
+  behavior.closedLoop = true;
+  behavior.perpetual = true;
+  behavior.minDepth = -1;
+  behavior.maxDepth = -1;
+
+  Json::Value behaviorConfig;
+  behaviorConfig["name"] = behavior.name;
+  behaviorConfig["source"] = behavior.source;
+  behaviorConfig["client stamp"] = stamp;
+  behaviorConfig["board stamp"] = -1;     
+  behaviorConfig["priority"] = behavior.priority;
+  behaviorConfig["duration"] = behavior.duration;
+  behaviorConfig["closedLoop"] = behavior.closedLoop;
+  behaviorConfig["constSpeed"] = behavior.constSpeed;
+  behaviorConfig["repeate"] = behavior.repeate;
+  behaviorConfig["perpetual"] = behavior.perpetual;
+  behaviorConfig["minDepth"] = behavior.minDepth;
+  behaviorConfig["maxDepth"] = behavior.maxDepth;
+  Json::Value station;
+  station["lon"] = station_1.lon;
+  station["lat"] = station_1.lat;  
+  station["depth"] = station_1.depth;
+  station["speed"] = station_1.speed;
+  behaviorConfig["points"].append(station);
+  station["lon"] = station_2.lon;
+  station["lat"] = station_2.lat;  
+  station["depth"] = station_2.depth;
+  station["speed"] = station_2.speed;
+  behaviorConfig["points"].append(station);
+  station["lon"] = station_3.lon;
+  station["lat"] = station_3.lat;  
+  station["depth"] = station_3.depth;
+  station["speed"] = station_3.speed;
+  behaviorConfig["points"].append(station);
+  station["lon"] = station_4.lon;
+  station["lat"] = station_4.lat;  
+  station["depth"] = station_4.depth;
+  station["speed"] = station_4.speed;
+  behaviorConfig["points"].append(station);
+  behaviorConfig["client stamp"] = stamp;
+  Json::StreamWriterBuilder builder;
+  std::string behaviorSpecString = Json::writeString(builder, behaviorConfig);
+
+  return behaviorSpecString;
+}
+
+std::string TaskSend::SetPlan2(std::string sourceName, double stamp)
+{
+  struct WayPointBehavior behavior;
+  behavior.source = sourceName;
+  behavior.priority = -1;
+  behavior.points.clear();
+  behavior.name = "west_waypt_survey";
+  behavior.priority = 10;
+  struct Landmark station_1 = {-70.331532,43.824194, 9, 4};
+  struct Landmark station_2 = {-70.330328,43.824299, 7, 6};
+  struct Landmark station_3 = {-70.330346,43.823518, 5, 8};
+  struct Landmark station_4 = {-70.331406,43.823206, 3, 10};
+  behavior.points.push_back(station_1);
+  behavior.points.push_back(station_2);
+  behavior.points.push_back(station_3);
+  behavior.points.push_back(station_4);
+  behavior.duration = -1;
+  behavior.constSpeed = -1;
+  behavior.repeate = -1;     
+  behavior.closedLoop = true;
+  behavior.perpetual = true;
+  behavior.minDepth = -1;
+  behavior.maxDepth = -1;
+
+  Json::Value behaviorConfig;
+  behaviorConfig["name"] = behavior.name;
+  behaviorConfig["source"] = behavior.source;
+  behaviorConfig["client stamp"] = stamp;
+  behaviorConfig["board stamp"] = -1;
+  behaviorConfig["priority"] = behavior.priority;
+  behaviorConfig["duration"] = behavior.duration;
+  behaviorConfig["closedLoop"] = behavior.closedLoop;
+  behaviorConfig["constSpeed"] = behavior.constSpeed;
+  behaviorConfig["repeate"] = behavior.repeate;
+  behaviorConfig["perpetual"] = behavior.perpetual;
+  behaviorConfig["minDepth"] = behavior.minDepth;
+  behaviorConfig["maxDepth"] = behavior.maxDepth;
+  Json::Value station;
+  station["lon"] = station_1.lon;
+  station["lat"] = station_1.lat;  
+  station["depth"] = station_1.depth;
+  station["speed"] = station_1.speed;
+  behaviorConfig["points"].append(station);
+  station["lon"] = station_2.lon;
+  station["lat"] = station_2.lat;  
+  station["depth"] = station_2.depth;
+  station["speed"] = station_2.speed;
+  behaviorConfig["points"].append(station);
+  station["lon"] = station_3.lon;
+  station["lat"] = station_3.lat;  
+  station["depth"] = station_3.depth;
+  station["speed"] = station_3.speed;
+  behaviorConfig["points"].append(station);
+  station["lon"] = station_4.lon;
+  station["lat"] = station_4.lat;  
+  station["depth"] = station_4.depth;
+  station["speed"] = station_4.speed;
+  behaviorConfig["points"].append(station);
+
+  Json::StreamWriterBuilder builder;
+  std::string behaviorSpecString = Json::writeString(builder, behaviorConfig);
+
+  return behaviorSpecString;
+}
+
+std::string TaskSend::ModifyPlan1(std::string sourceName, double stamp)
+{
+  struct WayPointBehavior behavior;
+  behavior.source = sourceName;
+  behavior.priority = -1;
+  behavior.points.clear();
+  behavior.name = "east_waypt_survey";
+  behavior.priority = 10;
+  struct Landmark station_1 = {-70.328891,43.824429, 9, 2};
+  struct Landmark station_2 = {-70.327885,43.824676, 7, 4};
+  struct Landmark station_3 = {-70.327867,43.823622, 5, 6};
+  struct Landmark station_4 = {-70.328765,43.823622, 3, 8};
+  behavior.points.push_back(station_1);
+  behavior.points.push_back(station_2);
+  behavior.points.push_back(station_3);
+  behavior.points.push_back(station_4);
+  behavior.duration = -1;
+  behavior.constSpeed = -1;
+  behavior.repeate = -1;     
+  behavior.closedLoop = true;
+  behavior.perpetual = true;
+  behavior.minDepth = -1;
+  behavior.maxDepth = -1;
+
+  Json::Value behaviorConfig;
+  behaviorConfig["name"] = behavior.name;
+  behaviorConfig["source"] = behavior.source;
+  behaviorConfig["client stamp"] = stamp;
+  behaviorConfig["board stamp"] = -1;  
+  behaviorConfig["priority"] = behavior.priority;
+  behaviorConfig["duration"] = behavior.duration;
+  behaviorConfig["closedLoop"] = behavior.closedLoop;
+  behaviorConfig["constSpeed"] = behavior.constSpeed;
+  behaviorConfig["repeate"] = behavior.repeate;
+  behaviorConfig["perpetual"] = behavior.perpetual;
+  behaviorConfig["minDepth"] = behavior.minDepth;
+  behaviorConfig["maxDepth"] = behavior.maxDepth;
+  Json::Value station;
+  station["lon"] = station_1.lon;
+  station["lat"] = station_1.lat;  
+  station["depth"] = station_1.depth;
+  station["speed"] = station_1.speed;
+  behaviorConfig["points"].append(station);
+  station["lon"] = station_2.lon;
+  station["lat"] = station_2.lat;  
+  station["depth"] = station_2.depth;
+  station["speed"] = station_2.speed;
+  behaviorConfig["points"].append(station);
+  station["lon"] = station_3.lon;
+  station["lat"] = station_3.lat;  
+  station["depth"] = station_3.depth;
+  station["speed"] = station_3.speed;
+  behaviorConfig["points"].append(station);
+  station["lon"] = station_4.lon;
+  station["lat"] = station_4.lat;  
+  station["depth"] = station_4.depth;
+  station["speed"] = station_4.speed;
+  behaviorConfig["points"].append(station);
+
+  Json::StreamWriterBuilder builder;
+  std::string behaviorSpecString = Json::writeString(builder, behaviorConfig);
+
+  return behaviorSpecString;
+}
+
+//---------------------------------------------------------
+// Procedure: RegisterVariables
+
+void TaskSend::RegisterVariables()
+{
+  Register("SendTask",0);
+  Register("BHV_WARNING",0);
+  // Register("FOOBAR", 0);
+}
+
+Fl_Window * TaskSend::taskEditView()
+{
+    Fl_Window *window = new Fl_Window(405, 600, "task config");
+    // Fl_Double_Window *window(450, 350, "Simple Table");               /* 1. 创建一个窗口 */
+    Fl_Group* pGroup = new Fl_Group(0, 0, 400, 70);                  /* 2. 创建一个分组 */
+    pGroup->box(FL_GTK_UP_BOX);
+
+    Fl_Button* Button_1 = new Fl_Button(5, 5, 90, 30, "send task 1");   //发送任务
+    Button_1->callback(st_sendTask1Callback, (void*) this);
+    
+    Fl_Button* Button_2 = new Fl_Button(105, 5, 90, 30, "send task 2");   //发送任务
+    Button_2->callback(st_sendTask2Callback, (void*) this);
+
+    Fl_Button* Button_3 = new Fl_Button(205, 5, 90, 30, "modify task 1");   //发送任务
+    Button_3->callback(st_ModifyTask1Callback, (void*) this);    
+
+    Fl_Button* Button_4 = new Fl_Button(295, 5, 90, 30, "load task");
+    Button_4->callback(st_FeedbackTaskCallback, (void*) this);  
+
+    Fl_Output* TaskMsgDisp = new Fl_Output(5,35,390,30);
+    TaskMsgDisp->box(FL_FLAT_BOX);
+    disp1 = TaskMsgDisp;
+    pGroup->end();                                                      /* 4. 结束上个容器的创建 */
+    
+    Fl_Group* pGroup1 = new Fl_Group(0, 90, 400, 70,"task parameter");                  /* 2. 创建一个分组 */
+    pGroup1->box(FL_GTK_UP_BOX);
+    
+    Fl_Menu_Button* taskType =new Fl_Menu_Button(5,95,100,25,"task type");
+    taskType->add("path tracking");
+    taskType->add("fixed yaw");
+    taskType->add("fixed depth");
+    taskType->add("fixed speed");
+    // taskType->add("");
+    Fl_Input* pwt = new Fl_Input(200,90,100,30,"task priority");
+    pwt->value("100");
+    Fl_Input* duration = new Fl_Input(200,35+90,100,30,"task duration");
+    duration->value("no-time-limit");
+    // pGroup1->add(taskType);
+    pGroup1->end();
+
+    Fl_Group* pGroup2 = new Fl_Group(0, 180, 400, 300,"path tracking parameter");                 
+    pGroup2->box(FL_GTK_UP_BOX);
+    pGroup2->add(capture_radius);
+    capture_radius->value("5");
+    pGroup2->add(capture_line);
+    capture_line->value("true");
+    pGroup2->add(slip_radius);
+    slip_radius->value("15.0");
+    pGroup2->add(lead);
+    lead->value("8");
+    pGroup2->add(lead_damper);
+    lead_damper->value("1");
+    pGroup2->add(speed);
+    speed->value("10.0");
+    pGroup2->add(repeat);
+    repeat->value("3");
+    pGroup2->add(polygon);
+
+
+    pGroup2->end();
+    window->end();                                                       /* 4. 结束上个容器的创建 */
+    // window->show();                                                      /* 5. 显示窗口 */
+    return window;
+    // delete window;
+}
+
+inline int TaskSend::sendTaskCallback(Fl_Widget *w)
+{
+    disp1->value("Task Sending...");
+    Notify("TaskNum","t1");
+    string taskMsg = "WPT_UPDATE";
+    string taskMsgconit="";
+    // if(polygon->value()=="")
+    // disp1->value(polygon->value());
+    taskMsgconit = "capture_radius=";
+    taskMsgconit += capture_radius->value();
+    Notify(taskMsg,taskMsgconit);
+
+    taskMsgconit = "capture_line=";
+    taskMsgconit += capture_line->value();
+    Notify(taskMsg,taskMsgconit);
+
+    taskMsgconit = "slip_radius=";
+    taskMsgconit += slip_radius->value();
+    Notify(taskMsg,taskMsgconit);
+
+    taskMsgconit = "lead=";
+    taskMsgconit += lead->value();
+    Notify(taskMsg,taskMsgconit);
+
+    taskMsgconit = "lead_damper=";
+    taskMsgconit += lead_damper->value();
+    Notify(taskMsg,taskMsgconit);
+
+    taskMsgconit = "speed=";
+    taskMsgconit += speed->value();
+    Notify(taskMsg,taskMsgconit);
+
+    taskMsgconit = "repeat=";
+    taskMsgconit += repeat->value();
+    Notify(taskMsg,taskMsgconit);
+
+    taskMsgconit = "polygon=";
+    taskMsgconit += polygon->value();    
+    Notify(taskMsg,taskMsgconit);
+    
+    if(error)
+      disp1->value("configure incorrect");
+    else
+      disp1->value("Task Send complete");
+    // disp1->value("Task Send complete");
+    
+}
+
+inline int TaskSend::sendTask1Callback(Fl_Widget *w)
+{
+  std::string systemName = "neptus-client-1";
+  std::string plan_1_Spec = SetPlan1(systemName, getTimeStamp());
+  std::cout << plan_1_Spec << std::endl;
+  DUNE::IMC::PlanDB msg;
+  msg.setTimeStamp();
+  msg.type = DUNE::IMC::PlanDB::TypeEnum::DBT_REQUEST;
+  msg.op = DUNE::IMC::PlanDB::OperationEnum::DBOP_SET;
+  msg.plan_id.assign("BHV_Waypoint");
+  msg.info = plan_1_Spec;
+  tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
+
+}
+
+inline int TaskSend::sendTask2Callback(Fl_Widget *w)
+{
+  std::string systemName = "neptus-client-1";
+  std::string plan_2_Spec = SetPlan2(systemName, getTimeStamp());
+  std::cout << plan_2_Spec << std::endl;
+  DUNE::IMC::PlanDB msg;
+  msg.setTimeStamp();
+  msg.type = DUNE::IMC::PlanDB::TypeEnum::DBT_REQUEST;
+  msg.op = DUNE::IMC::PlanDB::OperationEnum::DBOP_SET;
+  msg.plan_id.assign("BHV_Waypoint");
+  msg.info = plan_2_Spec;
+  tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
+}
+
+inline int TaskSend::ModifyTask1Callback(Fl_Widget *w)
+{
+  std::string systemName = "neptus-client-1";  
+  std::string plan_1_Spec = ModifyPlan1(systemName, getTimeStamp());
+  std::cout << plan_1_Spec << std::endl;
+  DUNE::IMC::PlanDB msg;
+  msg.setTimeStamp();
+  msg.type = DUNE::IMC::PlanDB::TypeEnum::DBT_REQUEST;
+  msg.op = DUNE::IMC::PlanDB::OperationEnum::DBOP_SET;
+  msg.plan_id.assign("BHV_Waypoint");
+  msg.info = plan_1_Spec;
+  tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
+}
+
+inline int TaskSend::FeedbackTaskCallback(Fl_Widget *w)
+{
+  DUNE::IMC::PlanDB msg;
+  msg.setTimeStamp();
+  msg.type = DUNE::IMC::PlanDB::TypeEnum::DBT_REQUEST;
+  msg.op = DUNE::IMC::PlanDB::OperationEnum::DBOP_GET_STATE;
+  tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
+}
+
+
+bool TaskSend::tcpSendToServer(DUNE::IMC::Message * msg, std::string addr, int port) 
+{
+
+  DUNE::Utils::ByteBuffer bb;
+  try {
+      DUNE::IMC::Packet::serialize(msg, bb);
+      return sock_tcp_send.write(bb.getBuffer(), msg->getSerializationSize());
+  }
+  catch (std::runtime_error& e)
+  {
+      MOOSTrace ("ERROR sending %s to %s:%d: %s\n", msg->getName(), addr.c_str(), port, e.what());
+      return false;
+  }
+
+  return true;
+}
+
+double TaskSend::getTimeStamp()
+{
+  struct timeval tv;
+  gettimeofday(&tv,NULL);
+  double stamp = double(tv.tv_sec*1000000 + tv.tv_usec) / 1000000;
+  return stamp;  
+}

src/pTaskSend/TaskSend.h

@@ -0,0 +1,135 @@
+/*
+ * @Author: error: error: git config user.name & please set dead value or install git && error: git config user.email & please set dead value or install git & please set dead value or install git
+ * @Date: 2023-09-21 11:04:00
+ * @LastEditors: zjk 1553836110@qq.com
+ * @LastEditTime: 2023-10-07 09:09:54
+ * @FilePath: /moos-ivp-extend/src/pTaskSend/TaskSend.h
+ * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
+ */
+/************************************************************/
+/*    NAME: zjk                                              */
+/*    ORGN: MIT                                             */
+/*    FILE: TaskSend.h                                          */
+/*    DATE:                                                 */
+/************************************************************/
+
+#ifndef TaskSend_HEADER
+#define TaskSend_HEADER
+
+#include "MOOS/libMOOS/MOOSLib.h"
+#include <DUNE/DUNE.hpp>
+
+#include <FL/Fl.H>
+#include <FL/Fl_Double_Window.H>
+#include <FL/Fl_Button.H>
+#include <FL/Fl_Check_Button.H>
+#include <FL/Fl_Return_Button.H>
+#include <FL/Fl_Group.H>
+#include <FL/Fl_Text_Editor.H>
+#include <FL/Fl_Radio_Round_Button.H>
+#include <Fl/Fl_Output.H>
+#include <Fl/Fl_Input.H>
+#include <Fl/Fl_Menu_Button.H>
+#include <Fl/Fl_Multiline_Input.H>
+
+struct Landmark {
+	float lon;
+	float lat;
+	float depth;
+	float speed;
+};
+
+struct WayPointBehavior
+{
+	std::string name;
+  std::string source;  
+	int priority;
+	std::vector<Landmark> points;
+	float duration;
+	bool closedLoop;
+	float constSpeed;
+	int repeate;
+	bool perpetual;
+	float minDepth;
+	float maxDepth;
+};
+
+class TaskSend : public CMOOSApp
+{
+ public:
+  TaskSend();
+  virtual ~TaskSend();
+
+  bool OnNewMail(MOOSMSG_LIST &NewMail);
+  bool Iterate();
+  bool OnConnectToServer();
+  bool OnStartUp();
+  void RegisterVariables();
+
+protected:
+  // insert local vars here
+  Fl_Window * taskEditView();
+  inline int sendTaskCallback(Fl_Widget *w);
+  inline int sendTask1Callback(Fl_Widget *w);
+  inline int sendTask2Callback(Fl_Widget *w);
+  inline int ModifyTask1Callback(Fl_Widget *w);
+  inline int FeedbackTaskCallback(Fl_Widget *w);  
+  static void st_sendTask1Callback(Fl_Widget *w, void *f) { ((TaskSend *)f)->sendTask1Callback(w);}
+  static void st_sendTask2Callback(Fl_Widget *w, void *f) { ((TaskSend *)f)->sendTask2Callback(w);}
+  static void st_ModifyTask1Callback(Fl_Widget *w, void *f) { ((TaskSend *)f)->ModifyTask1Callback(w);}
+  static void st_FeedbackTaskCallback(Fl_Widget *w, void *f) { ((TaskSend *)f)->FeedbackTaskCallback(w);}  
+
+  int SecurityZoneEdit();
+
+  // static void st_taskTypeCallback(Fl_Widget *w, void *f) { ((TaskSend *)f)->sendTaskCallback(w);}
+  // int SecurityZoneEdit();
+
+protected:
+  typedef struct task
+  {
+    std::string type;
+    std::string duration;//时间设置 no-time-limit / 10/20/300...
+    std::string pwt;
+  } task;
+
+  typedef struct waypointTask
+  {
+    task taskParam;
+    std::string capture_radius;//捕获半径
+    std::string capture_line;//捕获线
+    std::string slip_radius; //滑移半径
+    std::string lead;        //number 引导点距离
+    std::string lead_damper; //与轨迹的距离
+    std::string lead_to_start="true";
+    std::string speed="12";
+    std::string repeat;
+    std::string polygon;
+  } waypointTask;
+  
+private:
+  CMOOSMsg current_msg;
+  bool error=false;
+  bool editView=false;
+  Fl_Output* disp1=nullptr;
+  Fl_Window* view=nullptr;
+
+  Fl_Input* capture_radius = new Fl_Input(280,175+30,100,30,"capture radius");
+  Fl_Input* capture_line   = new Fl_Input(80,175+30,100,30,"capture line");
+  Fl_Input* slip_radius    = new Fl_Input(280,175+30*2,100,30,"slip radius");
+  Fl_Input* lead           = new Fl_Input(80,175+30*2,100,30,"lead");
+  Fl_Input* lead_damper    = new Fl_Input(280,175+30*3,100,30,"lead damper");
+  Fl_Input* speed          = new Fl_Input(80,175+30*3,100,30,"speed");
+  Fl_Input* repeat         = new Fl_Input(80,175+30*4,100,30,"repeat");
+  Fl_Multiline_Input* polygon = new Fl_Multiline_Input(80,175+30*5,300,100,"polygon");
+  
+  std::string SetPlan1(std::string sourceName, double stamp);
+  std::string SetPlan2(std::string sourceName, double stamp);
+  std::string ModifyPlan1(std::string sourceName, double stamp);
+
+  DUNE::Network::TCPSocket sock_tcp_send;
+  DUNE::Network::UDPSocket sock_udp_receive;
+  bool tcpSendToServer(DUNE::IMC::Message * msg, std::string addr, int port);
+  double getTimeStamp();
+};
+
+#endif 

src/pTaskSend/TaskSendMain.cpp

@@ -0,0 +1,50 @@
+/*
+ * @Author: error: error: git config user.name & please set dead value or install git && error: git config user.email & please set dead value or install git & please set dead value or install git
+ * @Date: 2023-09-21 11:04:00
+ * @LastEditors: error: error: git config user.name & please set dead value or install git && error: git config user.email & please set dead value or install git & please set dead value or install git
+ * @LastEditTime: 2023-09-21 22:03:48
+ * @FilePath: /moos-ivp-extend/src/pTaskSend/TaskSendMain.cpp
+ * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
+ */
+/************************************************************/
+/*    NAME: zjk                                              */
+/*    ORGN: MIT                                             */
+/*    FILE: TaskSendMain.cpp                                    */
+/*    DATE:                                                 */
+/************************************************************/
+
+#include <string>
+#include "TaskSend.h"
+#include "display.h"
+// #include <FL/Fl.H>
+// #include <FL/Fl_Window.H>
+// #include <FL/Fl_Box.H>
+
+using namespace std;
+
+int main(int argc, char *argv[])
+{
+  // default parameters file
+  string sMissionFile = "TaskSend.moos";
+  //under what name shoud the application register with the MOOSDB?
+  string sMOOSName = "pTaskSend";
+  
+  switch(argc)
+    {
+    case 3:
+      //command line says don't register with default name              
+      sMOOSName = argv[2];
+    case 2:
+      //command line says don't use default config file
+      sMissionFile = argv[1];
+    }
+  
+  //make an application
+  TaskSend TaskSendApp;
+
+  //run it
+  TaskSendApp.Run(sMOOSName.c_str(), sMissionFile.c_str());
+  
+  return(0);
+}
+

src/pTaskSend/build/.cmake/api/v1/query/client-vscode/query.json

@@ -0,0 +1 @@
+{"requests":[{"kind":"cache","version":2},{"kind":"codemodel","version":2},{"kind":"toolchains","version":1},{"kind":"cmakeFiles","version":1}]}

src/pTaskSend/build/.cmake/api/v1/reply/cmakeFiles-v1-1b864ed9ef170e655086.json

@@ -0,0 +1,754 @@
+{
+	"inputs" : 
+	[
+		{
+			"path" : "CMakeLists.txt"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeDetermineSystem.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeSystem.cmake.in"
+		},
+		{
+			"isGenerated" : true,
+			"path" : "build/CMakeFiles/3.25.1/CMakeSystem.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeUnixFindMake.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeSystemSpecificInitialize.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeDetermineCCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeDetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeDetermineCompilerId.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeCompilerIdDetection.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/ADSP-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/ARMCC-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/ARMClang-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/AppleClang-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Clang-DetermineCompilerInternal.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Borland-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Bruce-C-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Clang-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Clang-DetermineCompilerInternal.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Compaq-C-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Cray-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Embarcadero-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Fujitsu-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/FujitsuClang-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/GHS-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/GNU-C-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/HP-C-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/IAR-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/IBMClang-C-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Intel-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/IntelLLVM-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/LCC-C-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/MSVC-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/NVHPC-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/NVIDIA-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/OpenWatcom-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/PGI-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/PathScale-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/SCO-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/SDCC-C-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/SunPro-C-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/TI-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Tasking-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/TinyCC-C-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/VisualAge-C-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/IBMCPP-C-DetermineVersionInternal.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Watcom-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/XL-C-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/IBMCPP-C-DetermineVersionInternal.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/XLClang-C-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/zOS-C-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/IBMCPP-C-DetermineVersionInternal.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeFindBinUtils.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/GNU-FindBinUtils.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeCCompiler.cmake.in"
+		},
+		{
+			"isGenerated" : true,
+			"path" : "build/CMakeFiles/3.25.1/CMakeCCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeDetermineCXXCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeDetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Platform/Linux-Determine-CXX.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeDetermineCompilerId.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeCompilerIdDetection.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/ADSP-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/ARMCC-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/ARMClang-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/AppleClang-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Clang-DetermineCompilerInternal.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Borland-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Clang-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Clang-DetermineCompilerInternal.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Comeau-CXX-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Compaq-CXX-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Cray-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Embarcadero-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Fujitsu-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/FujitsuClang-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/GHS-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/GNU-CXX-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/HP-CXX-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/IAR-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/IBMClang-CXX-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Intel-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/IntelLLVM-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/LCC-CXX-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/MSVC-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/NVHPC-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/NVIDIA-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/OpenWatcom-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/PGI-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/PathScale-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/SCO-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/SunPro-CXX-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/TI-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Tasking-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/VisualAge-CXX-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/IBMCPP-CXX-DetermineVersionInternal.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/Watcom-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/XL-CXX-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/IBMCPP-CXX-DetermineVersionInternal.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/XLClang-CXX-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/zOS-CXX-DetermineCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/IBMCPP-CXX-DetermineVersionInternal.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeFindBinUtils.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/GNU-FindBinUtils.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeCXXCompiler.cmake.in"
+		},
+		{
+			"isGenerated" : true,
+			"path" : "build/CMakeFiles/3.25.1/CMakeCXXCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeSystemSpecificInformation.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeGenericSystem.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeInitializeConfigs.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Platform/Linux.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Platform/UnixPaths.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeCInformation.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeLanguageInformation.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/GNU-C.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/GNU.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/CMakeCommonCompilerMacros.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Platform/Linux-GNU-C.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Platform/Linux-GNU.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeCommonLanguageInclude.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeTestCCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeTestCompilerCommon.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeDetermineCompilerABI.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeParseImplicitIncludeInfo.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeParseImplicitLinkInfo.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeParseLibraryArchitecture.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeTestCompilerCommon.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeCCompilerABI.c"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeDetermineCompileFeatures.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Internal/FeatureTesting.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeCCompiler.cmake.in"
+		},
+		{
+			"isGenerated" : true,
+			"path" : "build/CMakeFiles/3.25.1/CMakeCCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeCXXInformation.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeLanguageInformation.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/GNU-CXX.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Compiler/GNU.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Platform/Linux-GNU-CXX.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Platform/Linux-GNU.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeCommonLanguageInclude.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeTestCXXCompiler.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeTestCompilerCommon.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeDetermineCompilerABI.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeParseImplicitIncludeInfo.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeParseImplicitLinkInfo.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeParseLibraryArchitecture.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeTestCompilerCommon.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeCXXCompilerABI.cpp"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeDetermineCompileFeatures.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/Internal/FeatureTesting.cmake"
+		},
+		{
+			"isCMake" : true,
+			"isExternal" : true,
+			"path" : "/usr/share/cmake-3.25/Modules/CMakeCXXCompiler.cmake.in"
+		},
+		{
+			"isGenerated" : true,
+			"path" : "build/CMakeFiles/3.25.1/CMakeCXXCompiler.cmake"
+		}
+	],
+	"kind" : "cmakeFiles",
+	"paths" : 
+	{
+		"build" : "/home/zjk/Desktop/project/moos-ivp-extend/src/pTaskSend/build",
+		"source" : "/home/zjk/Desktop/project/moos-ivp-extend/src/pTaskSend"
+	},
+	"version" : 
+	{
+		"major" : 1,
+		"minor" : 0
+	}
+}

src/pTaskSend/build/.cmake/api/v1/reply/codemodel-v2-8cafde00da2f94adfddf.json

@@ -0,0 +1,56 @@
+{
+	"configurations" : 
+	[
+		{
+			"directories" : 
+			[
+				{
+					"build" : ".",
+					"jsonFile" : "directory-.-Debug-f5ebdc15457944623624.json",
+					"projectIndex" : 0,
+					"source" : ".",
+					"targetIndexes" : 
+					[
+						0
+					]
+				}
+			],
+			"name" : "Debug",
+			"projects" : 
+			[
+				{
+					"directoryIndexes" : 
+					[
+						0
+					],
+					"name" : "Project",
+					"targetIndexes" : 
+					[
+						0
+					]
+				}
+			],
+			"targets" : 
+			[
+				{
+					"directoryIndex" : 0,
+					"id" : "pTaskSend::@6890427a1f51a3e7e1df",
+					"jsonFile" : "target-pTaskSend-Debug-fe070126a6f1fe44e26b.json",
+					"name" : "pTaskSend",
+					"projectIndex" : 0
+				}
+			]
+		}
+	],
+	"kind" : "codemodel",
+	"paths" : 
+	{
+		"build" : "/home/zjk/Desktop/project/moos-ivp-extend/src/pTaskSend/build",
+		"source" : "/home/zjk/Desktop/project/moos-ivp-extend/src/pTaskSend"
+	},
+	"version" : 
+	{
+		"major" : 2,
+		"minor" : 4
+	}
+}

src/pTaskSend/build/.cmake/api/v1/reply/directory-.-Debug-f5ebdc15457944623624.json

@@ -0,0 +1,14 @@
+{
+	"backtraceGraph" : 
+	{
+		"commands" : [],
+		"files" : [],
+		"nodes" : []
+	},
+	"installers" : [],
+	"paths" : 
+	{
+		"build" : ".",
+		"source" : "."
+	}
+}

src/pTaskSend/build/.cmake/api/v1/reply/index-2023-09-21T07-09-11-0928.json

@@ -0,0 +1,132 @@
+{
+	"cmake" : 
+	{
+		"generator" : 
+		{
+			"multiConfig" : false,
+			"name" : "Unix Makefiles"
+		},
+		"paths" : 
+		{
+			"cmake" : "/usr/bin/cmake",
+			"cpack" : "/usr/bin/cpack",
+			"ctest" : "/usr/bin/ctest",
+			"root" : "/usr/share/cmake-3.25"
+		},
+		"version" : 
+		{
+			"isDirty" : false,
+			"major" : 3,
+			"minor" : 25,
+			"patch" : 1,
+			"string" : "3.25.1",
+			"suffix" : ""
+		}
+	},
+	"objects" : 
+	[
+		{
+			"jsonFile" : "codemodel-v2-8cafde00da2f94adfddf.json",
+			"kind" : "codemodel",
+			"version" : 
+			{
+				"major" : 2,
+				"minor" : 4
+			}
+		},
+		{
+			"jsonFile" : "cache-v2-d6d942c4138e4121aadf.json",
+			"kind" : "cache",
+			"version" : 
+			{
+				"major" : 2,
+				"minor" : 0
+			}
+		},
+		{
+			"jsonFile" : "cmakeFiles-v1-1b864ed9ef170e655086.json",
+			"kind" : "cmakeFiles",
+			"version" : 
+			{
+				"major" : 1,
+				"minor" : 0
+			}
+		},
+		{
+			"jsonFile" : "toolchains-v1-2781238ec0fa0b9996f9.json",
+			"kind" : "toolchains",
+			"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-d6d942c4138e4121aadf.json",
+						"kind" : "cache",
+						"version" : 
+						{
+							"major" : 2,
+							"minor" : 0
+						}
+					},
+					{
+						"jsonFile" : "codemodel-v2-8cafde00da2f94adfddf.json",
+						"kind" : "codemodel",
+						"version" : 
+						{
+							"major" : 2,
+							"minor" : 4
+						}
+					},
+					{
+						"jsonFile" : "toolchains-v1-2781238ec0fa0b9996f9.json",
+						"kind" : "toolchains",
+						"version" : 
+						{
+							"major" : 1,
+							"minor" : 0
+						}
+					},
+					{
+						"jsonFile" : "cmakeFiles-v1-1b864ed9ef170e655086.json",
+						"kind" : "cmakeFiles",
+						"version" : 
+						{
+							"major" : 1,
+							"minor" : 0
+						}
+					}
+				]
+			}
+		}
+	}
+}

src/pTaskSend/build/.cmake/api/v1/reply/target-pTaskSend-Debug-fe070126a6f1fe44e26b.json

@@ -0,0 +1,129 @@
+{
+	"artifacts" : 
+	[
+		{
+			"path" : "pTaskSend"
+		}
+	],
+	"backtrace" : 1,
+	"backtraceGraph" : 
+	{
+		"commands" : 
+		[
+			"ADD_EXECUTABLE",
+			"TARGET_LINK_LIBRARIES"
+		],
+		"files" : 
+		[
+			"CMakeLists.txt"
+		],
+		"nodes" : 
+		[
+			{
+				"file" : 0
+			},
+			{
+				"command" : 0,
+				"file" : 0,
+				"line" : 8,
+				"parent" : 0
+			},
+			{
+				"command" : 1,
+				"file" : 0,
+				"line" : 10,
+				"parent" : 0
+			}
+		]
+	},
+	"compileGroups" : 
+	[
+		{
+			"compileCommandFragments" : 
+			[
+				{
+					"fragment" : "-g"
+				}
+			],
+			"language" : "CXX",
+			"sourceIndexes" : 
+			[
+				0,
+				1,
+				2
+			]
+		}
+	],
+	"id" : "pTaskSend::@6890427a1f51a3e7e1df",
+	"link" : 
+	{
+		"commandFragments" : 
+		[
+			{
+				"fragment" : "-g",
+				"role" : "flags"
+			},
+			{
+				"fragment" : "-rdynamic",
+				"role" : "flags"
+			},
+			{
+				"backtrace" : 2,
+				"fragment" : "-lmbutil",
+				"role" : "libraries"
+			},
+			{
+				"backtrace" : 2,
+				"fragment" : "-lm",
+				"role" : "libraries"
+			},
+			{
+				"backtrace" : 2,
+				"fragment" : "-lpthread",
+				"role" : "libraries"
+			}
+		],
+		"language" : "CXX"
+	},
+	"name" : "pTaskSend",
+	"nameOnDisk" : "pTaskSend",
+	"paths" : 
+	{
+		"build" : ".",
+		"source" : "."
+	},
+	"sourceGroups" : 
+	[
+		{
+			"name" : "Source Files",
+			"sourceIndexes" : 
+			[
+				0,
+				1,
+				2
+			]
+		}
+	],
+	"sources" : 
+	[
+		{
+			"backtrace" : 1,
+			"compileGroupIndex" : 0,
+			"path" : "TaskSend.cpp",
+			"sourceGroupIndex" : 0
+		},
+		{
+			"backtrace" : 1,
+			"compileGroupIndex" : 0,
+			"path" : "TaskSendMain.cpp",
+			"sourceGroupIndex" : 0
+		},
+		{
+			"backtrace" : 1,
+			"compileGroupIndex" : 0,
+			"path" : "display.cpp",
+			"sourceGroupIndex" : 0
+		}
+	],
+	"type" : "EXECUTABLE"
+}

src/pTaskSend/build/.cmake/api/v1/reply/toolchains-v1-2781238ec0fa0b9996f9.json

@@ -0,0 +1,107 @@
+{
+	"kind" : "toolchains",
+	"toolchains" : 
+	[
+		{
+			"compiler" : 
+			{
+				"id" : "GNU",
+				"implicit" : 
+				{
+					"includeDirectories" : 
+					[
+						"/usr/lib/gcc/x86_64-linux-gnu/9/include",
+						"/usr/local/include",
+						"/usr/include/x86_64-linux-gnu",
+						"/usr/include"
+					],
+					"linkDirectories" : 
+					[
+						"/usr/lib/gcc/x86_64-linux-gnu/9",
+						"/usr/lib/x86_64-linux-gnu",
+						"/usr/lib",
+						"/lib/x86_64-linux-gnu",
+						"/lib"
+					],
+					"linkFrameworkDirectories" : [],
+					"linkLibraries" : 
+					[
+						"gcc",
+						"gcc_s",
+						"c",
+						"gcc",
+						"gcc_s"
+					]
+				},
+				"path" : "/usr/bin/gcc",
+				"version" : "9.4.0"
+			},
+			"language" : "C",
+			"sourceFileExtensions" : 
+			[
+				"c",
+				"m"
+			]
+		},
+		{
+			"compiler" : 
+			{
+				"id" : "GNU",
+				"implicit" : 
+				{
+					"includeDirectories" : 
+					[
+						"/usr/include/c++/9",
+						"/usr/include/x86_64-linux-gnu/c++/9",
+						"/usr/include/c++/9/backward",
+						"/usr/lib/gcc/x86_64-linux-gnu/9/include",
+						"/usr/local/include",
+						"/usr/include/x86_64-linux-gnu",
+						"/usr/include"
+					],
+					"linkDirectories" : 
+					[
+						"/usr/lib/gcc/x86_64-linux-gnu/9",
+						"/usr/lib/x86_64-linux-gnu",
+						"/usr/lib",
+						"/lib/x86_64-linux-gnu",
+						"/lib"
+					],
+					"linkFrameworkDirectories" : [],
+					"linkLibraries" : 
+					[
+						"stdc++",
+						"m",
+						"gcc_s",
+						"gcc",
+						"c",
+						"gcc_s",
+						"gcc"
+					]
+				},
+				"path" : "/usr/bin/g++",
+				"version" : "9.4.0"
+			},
+			"language" : "CXX",
+			"sourceFileExtensions" : 
+			[
+				"C",
+				"M",
+				"c++",
+				"cc",
+				"cpp",
+				"cxx",
+				"mm",
+				"mpp",
+				"CPP",
+				"ixx",
+				"cppm"
+			]
+		}
+	],
+	"version" : 
+	{
+		"major" : 1,
+		"minor" : 0
+	}
+}

src/pTaskSend/display.cpp

@@ -0,0 +1,58 @@
+/*
+ * @Author: error: error: git config user.name & please set dead value or install git && error: git config user.email & please set dead value or install git & please set dead value or install git
+ * @Date: 2023-09-21 12:50:21
+ * @LastEditors: error: error: git config user.name & please set dead value or install git && error: git config user.email & please set dead value or install git & please set dead value or install git
+ * @LastEditTime: 2023-09-21 22:04:13
+ * @FilePath: /moos-ivp-extend/src/pTaskSend/display.cpp
+ * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
+ */
+#include "display.h"
+
+display::display(/* args */)
+{
+    // Fl_Window *window = new Fl_Window(340, 180, "hello");
+    // Fl_Box *box = new Fl_Box(20, 40, 300, 100, "Hello");
+    // box->box(FL_UP_BOX);
+    // box->labelfont(FL_BOLD + FL_ITALIC);
+    // box->labelsize(36);
+    // box->labeltype(FL_SHADOW_LABEL);
+    // window->end();
+    // window->show();
+    // Fl::run();
+    TaskEdit();
+}
+
+int display::TaskEdit()
+{
+    Fl_Window *window = new Fl_Window(450, 350, "hello");
+    // Fl_Double_Window *window(450, 350, "Simple Table");               /* 1. 创建一个窗口 */
+    Fl_Group* pGroup = new Fl_Group(50, 50, 400, 150);                  /* 2. 创建一个分组 */
+    Fl_Button* pButton = new Fl_Button(70, 50, 150, 30, "Fl_Button");   /* 3. 创建控件 */
+
+    int xyz;
+    pButton->callback(sendTask, &xyz);
+
+    Fl_Check_Button* pChkButton = new Fl_Check_Button(230, 50, 150, 30, "Fl_Check_Button");
+    Fl_Return_Button* pRetButton = new Fl_Return_Button(70, 100, 150, 30, "Fl_Return_Button");
+    Fl_Radio_Round_Button* pRndButton = new Fl_Radio_Round_Button(230, 100, 150, 30, "Fl_Round_Button");
+    pGroup->end();                                                      /* 4. 结束上个容器的创建 */
+    Fl_Text_Editor* pText = new Fl_Text_Editor(50, 150, 350, 150);
+    Fl_Text_Buffer* pBuff = new Fl_Text_Buffer();
+    pText->buffer(pBuff);                                               /* pBuff->text()中的内容就是Fl_Text_Buffer中显示的内容 */
+    pBuff->text("示例文字");
+    pText->end();
+    window->end();                                                       /* 4. 结束上个容器的创建 */
+    window->show();                                                      /* 5. 显示窗口 */
+    Fl::run();                                                   /* 6. 运行FLTK主循环 */
+    // return true;
+}
+
+void display::sendTask(Fl_Widget *w, void *data)
+{
+    
+}
+
+
+display::~display()
+{
+}

src/pTaskSend/display.h

@@ -0,0 +1,39 @@
+/*
+ * @Author: error: error: git config user.name & please set dead value or install git && error: git config user.email & please set dead value or install git & please set dead value or install git
+ * @Date: 2023-09-21 12:50:12
+ * @LastEditors: error: error: git config user.name & please set dead value or install git && error: git config user.email & please set dead value or install git & please set dead value or install git
+ * @LastEditTime: 2023-09-21 21:45:28
+ * @FilePath: /moos-ivp-extend/src/pTaskSend/display.h
+ * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
+ */
+#ifndef DISPLAY_HEADER
+#define DISPLAY_HEADER
+
+#include <FL/Fl.H>
+#include <FL/Fl_Double_Window.H>
+#include <FL/Fl_Button.H>
+#include <FL/Fl_Check_Button.H>
+#include <FL/Fl_Return_Button.H>
+#include <FL/Fl_Group.H>
+#include <FL/Fl_Text_Editor.H>
+#include <FL/Fl_Radio_Round_Button.H>
+#include "MOOS/libMOOS/App/MOOSApp.h"
+
+
+class display
+{
+private:
+    /* data */
+public:
+    display(/* args */);
+    ~display();
+    int TaskEdit();
+    int SecurityZoneEdit();
+protected:
+    static void sendTask(Fl_Widget *w, void *data);
+};
+
+
+
+
+#endif 

src/pTaskSend/pTaskSend.moos

@@ -0,0 +1,11 @@
+// MOOS file
+
+ServerHost = localhost
+ServerPort = 9000
+
+ProcessConfig = pTaskSend
+{
+   AppTick   = 4
+   CommsTick = 4
+}
+