applanix.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2005, 2009 Austin Robot Technology, Jack O'Quin
00003  *
00004  *  License: Modified BSD Software License Agreement
00005  *
00006  *  $Id: applanix.cc 710 2010-10-29 03:39:34Z austinrobot $
00007  */
00008 
00016 #include <math.h>
00017 #include <net/ethernet.h>
00018 #include <netinet/ip.h>
00019 #include <netinet/tcp.h>
00020 #include <netinet/udp.h>
00021 
00022 #include "applanix.h"
00023 
00024 #define DEVICE "Applanix POS-LV"
00025 
00026 void DevApplanix::unpack_grp1(applanix_data_t *adata, GRP1_MSG *msg) 
00027 {
00028   ROS_DEBUG(DEVICE " Group1: Speed %.3f Lat %.8f Lon %.8f Alt "
00029             "%.2f Heading %.2f, Alignment %d",
00030             msg->data.speed, msg->data.lat, msg->data.lon,
00031             msg->data.alt, msg->data.heading, msg->data.alignment);
00032   ROS_DEBUG(DEVICE " Group1: roll %.3f pitch %.3f",
00033             msg->data.roll, msg->data.pitch);
00034 
00035   adata->grp1 = msg->data;
00036 
00037   // As long as Omnistar differential GPS is active, the POS-LV
00038   // returns NAD-83 coordinates.  Without DGPS, it gives WGS-84
00039   // instead, which is what DARPA uses for the Urban Challenge.  In
00040   // the area of Victorville, CA we measured a difference between the
00041   // two coordinate systems is about 1.5m (North) and -1.333m (West).
00042   // Add these offsets to the NAD-83 values to get equivalent WGS-84
00043   // coordinates.
00044   //
00045   // log a warning loudly, if DGPS is lost
00046   if (have_DGPS)
00047     {
00048       if (msg->data.alignment > ApplStatusFull
00049           && msg->data.alignment < ApplStatusInitial)
00050         {
00051           ROS_WARN("Differential GPS lost!  Alignment status %d",
00052                    msg->data.alignment);
00053           have_DGPS = false;            // only warn once
00054         }
00055     }
00056   else                                  // DGPS was lost
00057     {
00058       if (msg->data.alignment == ApplStatusFull)
00059         {
00060           ROS_WARN("Differential GPS regained!");
00061           have_DGPS = true;
00062         }
00063     }
00064 }
00065 
00066 void DevApplanix::unpack_grp4(applanix_data_t *adata, GRP4_MSG *msg) 
00067 {
00068   //ROS_DEBUG("Group 4 message %d b %d\n",sizeof(GRP4_MSG),sizeof(ushort));
00069   ROS_DEBUG("  Status Data 0x%x IMU 0x%x IMU type 0x%x",
00070             msg->data.datastatus, msg->data.imustatus, msg->data.imutype);
00071 #if 0 // need to figure out how to interpret these fields
00072   ROS_DEBUG("  X, Y, Z velocities: (%d, %d, %d)",
00073             msg->data.vel_x, msg->data.vel_y, msg->data.vel_z);
00074   ROS_DEBUG("  roll, pitch and yaw angular velocities (raw): (%d, %d, %d)",
00075             msg->data.ang_x, msg->data.ang_y, msg->data.ang_z);
00076   ROS_DEBUG("  roll, pitch and yaw angular velocities (deg): (%.3f, %.3f, %.3f)",
00077             APPLANIX_NMEA_DEG_PER_BIT * msg->data.ang_x,
00078             APPLANIX_NMEA_DEG_PER_BIT * msg->data.ang_y,
00079             APPLANIX_NMEA_DEG_PER_BIT * msg->data.ang_z);
00080 #endif
00081   adata->grp4 = msg->data;
00082 }
00083 
00084 int DevApplanix::connect_socket(void) 
00085 {
00086   int rc;
00087 
00088   ROS_DEBUG(DEVICE " creating socket");
00089   sockfd = socket(AF_INET, SOCK_STREAM, 0);
00090   if (sockfd < 0)
00091     {
00092       ROS_ERROR("Unable to create socket for " DEVICE " (%s)",
00093                 strerror(errno));
00094       return errno;
00095     }
00096  
00097   memset(&serveraddr, 0, sizeof(struct sockaddr_in));
00098   serveraddr.sin_family = AF_INET;
00099  
00100   serverhostp = gethostbyname(serverhost);
00101   if (serverhostp == NULL)
00102     {
00103       ROS_ERROR("gethostbyname() failed for " DEVICE " (%s)",
00104                 strerror(errno));
00105       return errno;
00106     }
00107 
00108   bcopy(serverhostp->h_addr,
00109         (char *)&(serveraddr.sin_addr.s_addr),
00110         serverhostp->h_length);
00111  
00112   serveraddr.sin_port = htons(APPLANIX_RTDATA_PORT);       
00113  
00114   // Now connect to the server
00115   ROS_DEBUG(DEVICE " connecting to socket");
00116   rc = connect(sockfd, (struct sockaddr *)&serveraddr, sizeof(serveraddr));
00117   if (rc < 0)
00118     {
00119       ROS_ERROR("Unable to connect to " DEVICE " socket (%s)",
00120                 strerror(errno));
00121       return errno;
00122     }
00123 
00124   rc = fcntl(sockfd, F_SETFL, O_NONBLOCK);
00125   if (rc < 0)
00126     {
00127       ROS_ERROR("Unable to set O_NONBLOCK for " DEVICE " socket (%s)",
00128                 strerror(errno));
00129       return errno;
00130     }
00131 
00132   return 0;
00133 }
00134  
00135 // read the Applanix input socket
00136 //
00137 // returns 0 if any data received;
00138 //         EAGAIN if no data received;
00139 //         errno value if error
00140 int DevApplanix::read_packet(ros::Time *time)
00141 {
00142   int nbytes;
00143 
00144   // try to complete the packet in our buffer
00145   do
00146     {
00147       ROS_DEBUG(DEVICE " reading socket");
00148       nbytes = recv(sockfd, packet_buffer+buffer_length,
00149                     sizeof(packet_buffer)-buffer_length, 0);
00150     }
00151   while ((nbytes < 0) && (errno == EINTR));
00152 
00153   if (nbytes == 0)                      // nothing received?
00154     {
00155       ROS_DEBUG(DEVICE " nothing received");
00156       return EAGAIN;
00157     }
00158   else if (nbytes < 0)
00159     {
00160       if (errno != EAGAIN)
00161         ROS_WARN(DEVICE " socket recv() error (%s)", strerror(errno));
00162       return errno;
00163     }
00164 
00165   *time = ros::Time::now();
00166   buffer_length += nbytes;
00167 
00168   ROS_DEBUG("Got packet with %d bytes, time %.6f", nbytes, time->toSec());
00169 
00170   return 0;
00171 }
00172 
00173 // get next Applanix packet
00174 //
00175 // return the packet in the applanix data struct.  Update adata->time
00176 // only when a new navigation solution (GRP1) packet arrives.
00177 //
00178 // returns 0 if full packet received;
00179 //         EAGAIN if no packet (or incomplete);
00180 //         errno value if error
00181 int DevApplanix::get_packet(applanix_data_t *adata)
00182 {
00183   GRPHDR_MSG *hdr = (GRPHDR_MSG *) packet_buffer;
00184   ros::Time packet_time = adata->time;
00185 
00186   // have we got a complete packet?
00187   while ((buffer_length < sizeof(GRPHDR_MSG))
00188          || (buffer_length < hdr->bytecount + sizeof(GRPHDR_MSG)))
00189     {
00190       int rc = read_packet(&packet_time);
00191       if (rc != 0)                      // no data received?
00192         {
00193           ROS_DEBUG_STREAM(DEVICE " partial packet received ("
00194                            << buffer_length << " bytes)");
00195           return rc;
00196         }
00197     }
00198 
00199   // have a full packet in the buffer
00200   // \todo fix for 64-bit
00201   ROS_DEBUG(DEVICE " %*.*s %d packet, size %d",
00202             (int) sizeof(hdr->grpstart), (int) sizeof(hdr->grpstart),
00203             hdr->grpstart, hdr->groupnum, hdr->bytecount);
00204 
00205   // copy packet data to applanix data struct
00206   switch(hdr->groupnum)
00207   {
00208   case 1:
00209     unpack_grp1(adata, (GRP1_MSG *) packet_buffer);
00210     adata->time = packet_time;
00211     break;
00212   case 4:
00213     unpack_grp4(adata, (GRP4_MSG *) packet_buffer);
00214     break;
00215   default:
00216     break;
00217   }
00218 
00219   // shift packet_buffer to the left
00220   size_t packet_length = hdr->bytecount + sizeof(GRPHDR_MSG);
00221   buffer_length -= packet_length;
00222   if (buffer_length > 0)
00223     memmove(packet_buffer, packet_buffer+packet_length, buffer_length);
00224 
00225   return 0;
00226 }
00227 
00228 
00229 int DevApplanixPCAP::connect_socket(void)
00230 {
00231   ROS_INFO("Opening input file \"%s\"", filename_.c_str());
00232   
00233   /* Open the capture file */
00234   if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_) ) == NULL)
00235     {
00237       ROS_FATAL("Error opening Applanix socket dump file.");
00238       return -1;
00239     }
00240   return 0;
00241 }
00242 
00249 int DevApplanixPCAP::read_packet(ros::Time *time)
00250 {
00251   struct pcap_pkthdr *header;
00252   const u_char *pkt_data;
00253   int res;
00254 
00255   // return EAGAIN after successful calls, so the driver does not blow
00256   // through the entire file
00257   if (got_data_)
00258     {
00259       got_data_ = false;
00260       return EAGAIN;
00261     }
00262 
00263   // read the next packet from the PCAP dump
00264   if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0)
00265     {
00266       if (header->len > sizeof(packet_buffer)-buffer_length)
00267         {
00268           // TODO Save this packet for next time?
00269           ROS_WARN_STREAM("PCAP packet (size " << header->len
00270                           << ") overflows buffer (" <<
00271                           sizeof(packet_buffer)-buffer_length <<" left)");
00272           return EAGAIN;                // buffer too full
00273         }
00274 
00275       size_t pkt_offset = ETH_HLEN;     // IP header offset in packet
00276       struct iphdr *ip = (struct iphdr *) (pkt_data + pkt_offset);
00277       size_t size_ip = ip->ihl * 4;
00278       if (size_ip < 20)
00279         {
00280           ROS_WARN("invalid IP header size: %d", ip->ihl);
00281           return EAGAIN;                // ignore this packet
00282         }
00283       pkt_offset += size_ip;            // next header offset in packet
00284 
00285       if (ip->protocol == IPPROTO_TCP)  // got a TCP packet?
00286         {
00287           struct tcphdr *tcp = (struct tcphdr *) (pkt_data + pkt_offset);
00288           size_t size_tcp = tcp->doff * 4;
00289           if (size_tcp < 20)
00290             {
00291               ROS_WARN("invalid TCP header offset: %d", tcp->doff);
00292               return EAGAIN;            // ignore this packet
00293             }
00294           uint16_t dport = ntohs(tcp->dest);
00295           if (dport != APPLANIX_RTDATA_PORT)
00296             {
00297               ROS_INFO("wrong TCP data port: %u", dport);
00298               return EAGAIN;            // ignore this packet
00299             }
00300           ROS_DEBUG_STREAM("have TCP packet, port " << dport);
00301           pkt_offset += size_tcp;           // message offset in packet
00302         }
00303       else if (ip->protocol == IPPROTO_UDP) // got a UDP packet?
00304         {
00305           // I have not figured out how to use tcpdump to dump a TCP
00306           // connection with the RTDATA port.  But, the low-bandwidth
00307           // UDP display port is easy.  So, use that, instead.
00308           struct udphdr *udp = (struct udphdr *) (pkt_data + pkt_offset);
00309           uint16_t dport = ntohs(udp->dest);
00310           if (dport != APPLANIX_DISPLAY_PORT)
00311             {
00312               ROS_DEBUG("other UDP data port: %u", dport);
00313               return EAGAIN;            // ignore this packet
00314             }
00315           ROS_DEBUG_STREAM("have UDP packet, port " << dport);
00316           pkt_offset += sizeof(struct udphdr); // message offset in packet
00317         }
00318       else
00319         {
00320           ROS_DEBUG_STREAM("non-Applanix packet, protocol " << ip->protocol);
00321           return EAGAIN;                // ignore other packets
00322         }
00323 
00324 
00325         // copy all packet data after headers to buffer
00326         size_t pkt_length = header->len - pkt_offset;
00327         memcpy(packet_buffer + buffer_length, pkt_data + pkt_offset, pkt_length);
00328         buffer_length += pkt_length;
00329         *time = ros::Time::now();
00330         got_data_ = true;
00331         empty_ = false;
00332     }
00333   else                                  // end of file or error?
00334     {
00335       if (empty_)                       // no data in file?
00336         {
00337           if (res == -1)
00338             {
00339               ROS_WARN("Error reading Applanix packet: %s",
00340                        pcap_geterr(pcap_));
00341               return EIO;
00342             }
00343           ROS_FATAL("No useable Applanix packets in PCAP file");
00344           ros::shutdown();              // terminate this node
00345           return EIO;
00346         }
00347 
00348       if (read_once_)
00349         {
00350           //ROS_INFO("end of file reached -- done reading.");
00351           // stop returning data from now on
00352           return EAGAIN;
00353         }
00354 
00355       if (repeat_delay_ > 0.0)
00356         {
00357           ROS_INFO("end of file reached -- delaying %.3f seconds.",
00358                    repeat_delay_);
00359           usleep(rint(repeat_delay_ * 1000000.0));
00360         }
00361 
00362       ROS_DEBUG("replaying Applanix dump file");
00363 
00364       // I can't figure out how to rewind the file, because it
00365       // starts with some kind of header.  So, close the file
00366       // and reopen it with pcap.
00367       pcap_close(pcap_);
00368       pcap_ = pcap_open_offline(filename_.c_str(), errbuf_);
00369       empty_ = true;                    // maybe the file disappeared?
00370 
00371       return EAGAIN;                    // provide nothing this call
00372     }
00373 
00374   return 0;
00375 }
00376 
00377 int DevApplanixTest::connect_socket(void)
00378 {
00379   fp_ = fopen(testfile_.c_str(), "r");
00380   if (fp_ == NULL)
00381     {
00382       int rc = errno;
00383       ROS_ERROR("failed to open \"%s\" (%s)",
00384                 testfile_.c_str(), strerror(rc));
00385       return rc;
00386     }
00387 
00388   ROS_INFO("unit test connection: running with fake data");
00389   return 0;
00390 }
00391 
00393 int DevApplanixTest::get_packet(applanix_data_t *adata)
00394 {
00395   applanix_data_t test_data;
00396   memset(&test_data, 0, sizeof(test_data));
00397 
00398   int rc = fscanf(fp_, " %d %lf %lf %lf %f %f %f %lf %lf %lf %f",
00399                   (int *) &test_data.grp1.alignment,
00400                   &test_data.grp1.lat,
00401                   &test_data.grp1.lon,
00402                   &test_data.grp1.alt,
00403                   &test_data.grp1.vel_north,
00404                   &test_data.grp1.vel_east,
00405                   &test_data.grp1.vel_down,
00406                   &test_data.grp1.roll,
00407                   &test_data.grp1.pitch,
00408                   &test_data.grp1.heading,
00409                   &test_data.grp1.speed);
00410 
00411   if (feof(fp_))                        // EOF?
00412     {
00413       clearerr(fp_);                    // clear EOF
00414       rewind(fp_);                      // start over
00415       return EAGAIN;                    //   but, not this time
00416     }
00417   if (ferror(fp_))                      // I/O error?
00418     {
00419       clearerr(fp_);
00420       return EIO;                       // failure
00421     }
00422   else if (rc < 11 || test_data.grp1.alignment == ApplStatusInvalid)
00423     return EAGAIN;                      // empty or incomplete packet
00424 
00425   test_data.time = ros::Time::now();
00426   *adata = test_data;
00427 
00428   return 0;
00429 }


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