$search
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: input.cc 2293 2012-04-22 12:55:15Z jack.oquin $ 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