input.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2007 Austin Robot Technology, Patrick Beeson
00003  *  Copyright (C) 2009, 2010 Austin Robot Technology, Jack O'Quin
00004  *
00005  *  License: Modified BSD Software License Agreement
00006  *
00007  *  $Id$
00008  */
00009 
00024 #include <unistd.h>
00025 #include <sys/socket.h>
00026 #include <arpa/inet.h>
00027 #include <poll.h>
00028 #include <errno.h>
00029 #include <fcntl.h>
00030 #include <sys/file.h>
00031 #include <velodyne_driver/input.h>
00032 
00033 namespace velodyne_driver
00034 {
00035   static const size_t packet_size = sizeof(velodyne_msgs::VelodynePacket::data);
00036 
00038   // InputSocket class implementation
00040 
00046   InputSocket::InputSocket(ros::NodeHandle private_nh, uint16_t udp_port):
00047     Input()
00048   {
00049     sockfd_ = -1;
00050 
00051     // connect to Velodyne UDP port
00052     ROS_INFO_STREAM("Opening UDP socket: port " << udp_port);
00053     sockfd_ = socket(PF_INET, SOCK_DGRAM, 0);
00054     if (sockfd_ == -1)
00055       {
00056         perror("socket");               // TODO: ROS_ERROR errno
00057         return;
00058       }
00059   
00060     sockaddr_in my_addr;                     // my address information
00061     memset(&my_addr, 0, sizeof(my_addr));    // initialize to zeros
00062     my_addr.sin_family = AF_INET;            // host byte order
00063     my_addr.sin_port = htons(udp_port);      // short, in network byte order
00064     my_addr.sin_addr.s_addr = INADDR_ANY;    // automatically fill in my IP
00065   
00066     if (bind(sockfd_, (sockaddr *)&my_addr, sizeof(sockaddr)) == -1)
00067       {
00068         perror("bind");                 // TODO: ROS_ERROR errno
00069         return;
00070       }
00071   
00072     if (fcntl(sockfd_,F_SETFL, O_NONBLOCK|FASYNC) < 0)
00073       {
00074         perror("non-block");
00075         return;
00076       }
00077 
00078     ROS_DEBUG("Velodyne socket fd is %d\n", sockfd_);
00079   }
00080 
00082   InputSocket::~InputSocket(void)
00083   {
00084     (void) close(sockfd_);
00085   }
00086 
00088   int InputSocket::getPacket(velodyne_msgs::VelodynePacket *pkt)
00089   {
00090     double time1 = ros::Time::now().toSec();
00091 
00092     struct pollfd fds[1];
00093     fds[0].fd = sockfd_;
00094     fds[0].events = POLLIN;
00095     static const int POLL_TIMEOUT = 1000; // one second (in msec)
00096 
00097     while (true)
00098       {
00099         // Unfortunately, the Linux kernel recvfrom() implementation
00100         // uses a non-interruptible sleep() when waiting for data,
00101         // which would cause this method to hang if the device is not
00102         // providing data.  We poll() the device first to make sure
00103         // the recvfrom() will not block.
00104         //
00105         // Note, however, that there is a known Linux kernel bug:
00106         //
00107         //   Under Linux, select() may report a socket file descriptor
00108         //   as "ready for reading", while nevertheless a subsequent
00109         //   read blocks.  This could for example happen when data has
00110         //   arrived but upon examination has wrong checksum and is
00111         //   discarded.  There may be other circumstances in which a
00112         //   file descriptor is spuriously reported as ready.  Thus it
00113         //   may be safer to use O_NONBLOCK on sockets that should not
00114         //   block.
00115 
00116         // poll() until input available
00117         do
00118           {
00119             int retval = poll(fds, 1, POLL_TIMEOUT);
00120             if (retval < 0)             // poll() error?
00121               {
00122                 if (errno != EINTR)
00123                   ROS_ERROR("poll() error: %s", strerror(errno));
00124                 return 1;
00125               }
00126             if (retval == 0)            // poll() timeout?
00127               {
00128                 ROS_WARN("Velodyne poll() timeout");
00129                 return 1;
00130               }
00131             if ((fds[0].revents & POLLERR)
00132                 || (fds[0].revents & POLLHUP)
00133                 || (fds[0].revents & POLLNVAL)) // device error?
00134               {
00135                 ROS_ERROR("poll() reports Velodyne error");
00136                 return 1;
00137               }
00138           } while ((fds[0].revents & POLLIN) == 0);
00139 
00140         // Receive packets that should now be available from the
00141         // socket using a blocking read.
00142         ssize_t nbytes = recvfrom(sockfd_, &pkt->data[0],
00143                                   packet_size,  0, NULL, NULL);
00144 
00145         if (nbytes < 0)
00146           {
00147             if (errno != EWOULDBLOCK)
00148               {
00149                 perror("recvfail");
00150                 ROS_INFO("recvfail");
00151                 return 1;
00152               }
00153           }
00154         else if ((size_t) nbytes == packet_size)
00155           {
00156             // read successful, done now
00157             break;
00158           }
00159 
00160         ROS_DEBUG_STREAM("incomplete Velodyne packet read: "
00161                          << nbytes << " bytes");
00162       }
00163 
00164     // Average the times at which we begin and end reading.  Use that to
00165     // estimate when the scan occurred.
00166     double time2 = ros::Time::now().toSec();
00167     pkt->stamp = ros::Time((time2 + time1) / 2.0);
00168 
00169     return 0;
00170   }
00171 
00173   // InputPCAP class implementation
00175 
00185   InputPCAP::InputPCAP(ros::NodeHandle private_nh,
00186                        double packet_rate,
00187                        std::string filename,
00188                        bool read_once,
00189                        bool read_fast,
00190                        double repeat_delay):
00191     Input(),
00192     packet_rate_(packet_rate)
00193   {
00194     filename_ = filename;
00195     fp_ = NULL;  
00196     pcap_ = NULL;  
00197     empty_ = true;
00198 
00199     // get parameters using private node handle
00200     private_nh.param("read_once", read_once_, read_once);
00201     private_nh.param("read_fast", read_fast_, read_fast);
00202     private_nh.param("repeat_delay", repeat_delay_, repeat_delay);
00203 
00204     if (read_once_)
00205       ROS_INFO("Read input file only once.");
00206     if (read_fast_)
00207       ROS_INFO("Read input file as quickly as possible.");
00208     if (repeat_delay_ > 0.0)
00209       ROS_INFO("Delay %.3f seconds before repeating input file.",
00210                repeat_delay_);
00211 
00212     // Open the PCAP dump file
00213     ROS_INFO("Opening PCAP file \"%s\"", filename_.c_str());
00214     if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_) ) == NULL)
00215       {
00216         ROS_FATAL("Error opening Velodyne socket dump file.");
00217         return;
00218       }
00219   }
00220 
00221 
00223   InputPCAP::~InputPCAP(void)
00224   {
00225     pcap_close(pcap_);
00226   }
00227 
00228 
00230   int InputPCAP::getPacket(velodyne_msgs::VelodynePacket *pkt)
00231   {
00232     struct pcap_pkthdr *header;
00233     const u_char *pkt_data;
00234 
00235     while (true)
00236       {
00237         int res;
00238         if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0)
00239           {
00240             // Keep the reader from blowing through the file.  The
00241             // actual device generates either 2600 (64E) or 1808 (32E)
00242             // packets per second at 600 RPM.
00243             if (read_fast_ == false)
00244               packet_rate_.sleep();
00245             
00246             memcpy(&pkt->data[0], pkt_data+42, packet_size);
00247             pkt->stamp = ros::Time::now();
00248             empty_ = false;
00249             return 0;                   // success
00250           }
00251 
00252         if (empty_)                 // no data in file?
00253           {
00254             ROS_WARN("Error %d reading Velodyne packet: %s", 
00255                      res, pcap_geterr(pcap_));
00256             return -1;
00257           }
00258 
00259         if (read_once_)
00260           {
00261             ROS_INFO("end of file reached -- done reading.");
00262             return -1;
00263           }
00264         
00265         if (repeat_delay_ > 0.0)
00266           {
00267             ROS_INFO("end of file reached -- delaying %.3f seconds.",
00268                      repeat_delay_);
00269             usleep(rint(repeat_delay_ * 1000000.0));
00270           }
00271 
00272         ROS_DEBUG("replaying Velodyne dump file");
00273 
00274         // I can't figure out how to rewind the file, because it
00275         // starts with some kind of header.  So, close the file
00276         // and reopen it with pcap.
00277         pcap_close(pcap_);
00278         pcap_ = pcap_open_offline(filename_.c_str(), errbuf_);
00279         empty_ = true;              // maybe the file disappeared?
00280       } // loop back and try again
00281   }
00282 
00283 } // velodyne namespace


velodyne_driver
Author(s): Jack O'Quin, Patrick Beeson, Michael Quinlan, Yaxin Liu
autogenerated on Fri Jan 3 2014 12:11:08