迁移pi上的master分支

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