applanix.h
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2005 Austin Robot Technology
00003  *    by Alberto Alonso, Jack O'Quin
00004  *  Copyright (C) 2009 Austin Robot Technology, Jack O'Quin
00005  *
00006  *  License: Modified BSD Software License Agreement
00007  *
00008  *  $Id: applanix.h 2 2010-01-17 01:54:03Z jack.oquin $
00009  */
00010 
00019 #ifndef _APPLANIX_H_
00020 #define _APPLANIX_H_ 1
00021 
00022 #include <ros/ros.h>
00023 
00024 #include <sys/types.h>
00025 #include <sys/socket.h>
00026 #include <sys/errno.h>
00027 #include <netinet/in.h>
00028 #include <netdb.h>
00029 #include <string.h>
00030 #include <stdio.h>
00031 #include <signal.h>
00032 #include <stdlib.h>
00033 #include <fcntl.h>
00034 #include <pcap.h>
00035 
00036 #define APPLANIX_DISPLAY_PORT 5600
00037 #define APPLANIX_CONTROL_PORT 5601
00038 #define APPLANIX_RTDATA_PORT 5602
00039 #define APPLANIX_LOGDATA_PORT 5603
00040 
00041 #define APPLANIX_MAXMSGSIZE 6000
00042 
00043 #define APPLANIX_DEFAULT_IP "192.168.1.25"
00044 
00045 #define APPLANIX_NMEA_DEG_PER_BIT 0.0054931640625
00046 
00047 #pragma pack(1)
00048 typedef struct GRPHDR_MSG_ {
00049   char grpstart[4];
00050   uint16_t groupnum;
00051   uint16_t bytecount;                   /* size includes footer, not header */
00052 } GRPHDR_MSG;
00053 
00054 #pragma pack(1)
00055 typedef struct GRPFTR_MSG_ {
00056   uint16_t crc;
00057   char grpend[2];  
00058 } GRPFTR_MSG;
00059 
00060 typedef enum {
00061   ApplStatusFull =      0,
00062   ApplStatusFine =      1,
00063   ApplStatusGcChi2 =    2,
00064   ApplStatusPcChi2 =    3,
00065   ApplStatusGcChi1 =    4,
00066   ApplStatusPcChi1 =    5,
00067   ApplStatusCoarse =    6,
00068   ApplStatusInitial =   7,
00069   ApplStatusInvalid =   8
00070 } appl_alignment_status_t;
00071 
00072 #pragma pack(1)
00073 typedef struct GRP1DATA_MSG_ {
00074   char timedist[26];
00075   double lat;
00076   double lon;
00077   double alt;
00078   float vel_north;
00079   float vel_east;
00080   float vel_down;
00081   double roll;
00082   double pitch;
00083   double heading;
00084   double wander;
00085   float track;
00086   float speed;
00087   float arate_lon;
00088   float arate_trans;
00089   float arate_down;
00090   float accel_lon;
00091   float accel_trans;
00092   float accel_down;
00093   char alignment;
00094   char padding;
00095 } GRP1DATA_MSG;
00096 
00097 #pragma pack(1)
00098 typedef struct GRP1_MSG_ {
00099   GRPHDR_MSG hdr;
00100   GRP1DATA_MSG data;
00101   GRPFTR_MSG ftr;
00102 } GRP1_MSG;
00103 
00104 #pragma pack(1)
00105 typedef struct GRP4DATA_MSG_ {
00106   char timedist[26];
00107   int32_t vel_x;
00108   int32_t vel_y;
00109   int32_t vel_z;
00110   int32_t ang_x;
00111   int32_t ang_y;
00112   int32_t ang_z;
00113   char datastatus;
00114   char imutype;
00115   char imurate;
00116   uint16_t imustatus;
00117   char padding;
00118 } GRP4DATA_MSG;
00119 
00120 #pragma pack(1)
00121 typedef struct GRP4_MSG_ {
00122   GRPHDR_MSG hdr;
00123   GRP4DATA_MSG data;
00124   GRPFTR_MSG ftr;
00125 } GRP4_MSG;
00126 
00127 #pragma pack(1)
00128 typedef struct GRP_MSG_ {
00129   union {
00130     GRPHDR_MSG hdr;
00131     GRP1_MSG grp1;
00132     GRP4_MSG grp4;
00133   };
00134 } GRP_MSG;
00135 
00136 typedef struct {
00137   GRP1DATA_MSG  grp1;
00138   GRP4DATA_MSG  grp4;
00139   ros::Time time;
00140 } applanix_data_t;
00141 
00142 class DevApplanix
00143 {
00144  public:
00145 
00146   DevApplanix(void)
00147     {
00148       serverhost = APPLANIX_DEFAULT_IP;
00149       buffer_length = 0;
00150       have_DGPS = true;
00151     };
00152   virtual ~DevApplanix() {};
00153 
00154   virtual int connect_socket(void);
00155   virtual int get_packet(applanix_data_t *adata);
00156 
00157  protected:
00158   bool have_DGPS;                       // have full DGPS nav solution
00159 
00160   // socket parameters
00161   int                     sockfd;
00162   struct sockaddr_in      serveraddr;
00163   const char                    *serverhost;
00164   struct hostent          *serverhostp;
00165 
00166   char packet_buffer[APPLANIX_MAXMSGSIZE];
00167   size_t buffer_length;
00168 
00169   virtual int  read_packet(ros::Time *time);
00170 
00171   // unpack specific message types
00172   virtual void unpack_grp1(applanix_data_t *adata, GRP1_MSG *msg);
00173   virtual void unpack_grp4(applanix_data_t *adata, GRP4_MSG *msg);
00174 };
00175 
00181 class DevApplanixPCAP: public DevApplanix
00182 {
00183  public:
00184 
00185  DevApplanixPCAP(std::string filename="",
00186               bool read_once=false,
00187               bool read_fast=false,
00188               double repeat_delay=0.0): DevApplanix()
00189     {
00190       filename_ = filename;
00191       fp_ = NULL;  
00192       pcap_ = NULL;  
00193       got_data_ = false;
00194       empty_ = true;
00195       read_once_ = read_once;
00196       if (read_once_)
00197         ROS_INFO("Read input file only once.");
00198       read_fast_ = read_fast;
00199       if (read_fast_)
00200         ROS_INFO("Read input file as quickly as possible.");
00201       repeat_delay_ = repeat_delay;
00202       if (repeat_delay_ > 0.0)
00203         ROS_INFO("Delay %.3f seconds before repeating input file.",
00204                  repeat_delay_);
00205     }
00206 
00207   virtual int connect_socket(void);
00208   virtual int  read_packet(ros::Time *time);
00209 
00210  private:
00211   std::string filename_;
00212   FILE *fp_;
00213   pcap_t *pcap_;
00214   char errbuf_[PCAP_ERRBUF_SIZE];
00215   bool got_data_;
00216   bool empty_;
00217   bool read_once_;
00218   bool read_fast_;
00219   double repeat_delay_;
00220 };
00221 
00223 class DevApplanixTest: public DevApplanix
00224 {
00225  public:
00226 
00227  DevApplanixTest(std::string testfile): DevApplanix()
00228     {
00229       fp_ = NULL;
00230       testfile_ = testfile;
00231       ROS_INFO("testing with GPS data from \"%s\"", testfile_.c_str());
00232     }
00233 
00234   virtual int connect_socket(void);
00235   virtual int get_packet(applanix_data_t *adata);
00236 
00237  private:
00238   std::string testfile_;
00239   FILE *fp_;
00240 };
00241 
00242 #endif // _APPLANIX_H_


applanix
Author(s): Jack O'Quin, Patrick Beeson, Alberto Alonso
autogenerated on Fri Jan 3 2014 11:09:02