00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00047 #include <unistd.h>
00048 #include <string>
00049 #include <sstream>
00050 #include <sys/socket.h>
00051 #include <arpa/inet.h>
00052 #include <poll.h>
00053 #include <errno.h>
00054 #include <fcntl.h>
00055 #include <sys/file.h>
00056 #include <velodyne_driver/input.h>
00057 #include <velodyne_driver/time_conversion.hpp>
00058
00059 namespace velodyne_driver
00060 {
00061 static const size_t packet_size =
00062 sizeof(velodyne_msgs::VelodynePacket().data);
00063
00065
00067
00073 Input::Input(ros::NodeHandle private_nh, uint16_t port):
00074 private_nh_(private_nh),
00075 port_(port)
00076 {
00077 private_nh.param("device_ip", devip_str_, std::string(""));
00078 private_nh.param("gps_time", gps_time_, false);
00079 if (!devip_str_.empty())
00080 ROS_INFO_STREAM("Only accepting packets from IP address: "
00081 << devip_str_);
00082 }
00083
00085
00087
00093 InputSocket::InputSocket(ros::NodeHandle private_nh, uint16_t port):
00094 Input(private_nh, port)
00095 {
00096 sockfd_ = -1;
00097
00098 if (!devip_str_.empty()) {
00099 inet_aton(devip_str_.c_str(),&devip_);
00100 }
00101
00102
00103 ROS_INFO_STREAM("Opening UDP socket: port " << port);
00104 sockfd_ = socket(PF_INET, SOCK_DGRAM, 0);
00105 if (sockfd_ == -1)
00106 {
00107 perror("socket");
00108 return;
00109 }
00110
00111 sockaddr_in my_addr;
00112 memset(&my_addr, 0, sizeof(my_addr));
00113 my_addr.sin_family = AF_INET;
00114 my_addr.sin_port = htons(port);
00115 my_addr.sin_addr.s_addr = INADDR_ANY;
00116
00117 if (bind(sockfd_, (sockaddr *)&my_addr, sizeof(sockaddr)) == -1)
00118 {
00119 perror("bind");
00120 return;
00121 }
00122
00123 if (fcntl(sockfd_,F_SETFL, O_NONBLOCK|FASYNC) < 0)
00124 {
00125 perror("non-block");
00126 return;
00127 }
00128
00129 ROS_DEBUG("Velodyne socket fd is %d\n", sockfd_);
00130 }
00131
00133 InputSocket::~InputSocket(void)
00134 {
00135 (void) close(sockfd_);
00136 }
00137
00139 int InputSocket::getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset)
00140 {
00141 double time1 = ros::Time::now().toSec();
00142
00143 struct pollfd fds[1];
00144 fds[0].fd = sockfd_;
00145 fds[0].events = POLLIN;
00146 static const int POLL_TIMEOUT = 1000;
00147
00148 sockaddr_in sender_address;
00149 socklen_t sender_address_len = sizeof(sender_address);
00150
00151 while (true)
00152 {
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171 do
00172 {
00173 int retval = poll(fds, 1, POLL_TIMEOUT);
00174 if (retval < 0)
00175 {
00176 if (errno != EINTR)
00177 ROS_ERROR("poll() error: %s", strerror(errno));
00178 return -1;
00179 }
00180 if (retval == 0)
00181 {
00182 ROS_WARN("Velodyne poll() timeout");
00183 return -1;
00184 }
00185 if ((fds[0].revents & POLLERR)
00186 || (fds[0].revents & POLLHUP)
00187 || (fds[0].revents & POLLNVAL))
00188 {
00189 ROS_ERROR("poll() reports Velodyne error");
00190 return -1;
00191 }
00192 } while ((fds[0].revents & POLLIN) == 0);
00193
00194
00195
00196 ssize_t nbytes = recvfrom(sockfd_, &pkt->data[0],
00197 packet_size, 0,
00198 (sockaddr*) &sender_address,
00199 &sender_address_len);
00200
00201 if (nbytes < 0)
00202 {
00203 if (errno != EWOULDBLOCK)
00204 {
00205 perror("recvfail");
00206 ROS_INFO("recvfail");
00207 return -1;
00208 }
00209 }
00210 else if ((size_t) nbytes == packet_size)
00211 {
00212
00213
00214
00215 if(devip_str_ != ""
00216 && sender_address.sin_addr.s_addr != devip_.s_addr)
00217 continue;
00218 else
00219 break;
00220 }
00221
00222 ROS_DEBUG_STREAM("incomplete Velodyne packet read: "
00223 << nbytes << " bytes");
00224 }
00225
00226 if (!gps_time_) {
00227
00228
00229 double time2 = ros::Time::now().toSec();
00230 pkt->stamp = ros::Time((time2 + time1) / 2.0 + time_offset);
00231 } else {
00232
00233
00234 pkt->stamp = rosTimeFromGpsTimestamp(&(pkt->data[1200]));
00235 }
00236
00237 return 0;
00238 }
00239
00241
00243
00251 InputPCAP::InputPCAP(ros::NodeHandle private_nh, uint16_t port,
00252 double packet_rate, std::string filename,
00253 bool read_once, bool read_fast, double repeat_delay):
00254 Input(private_nh, port),
00255 packet_rate_(packet_rate),
00256 filename_(filename)
00257 {
00258 pcap_ = NULL;
00259 empty_ = true;
00260
00261
00262 private_nh.param("read_once", read_once_, false);
00263 private_nh.param("read_fast", read_fast_, false);
00264 private_nh.param("repeat_delay", repeat_delay_, 0.0);
00265
00266 if (read_once_)
00267 ROS_INFO("Read input file only once.");
00268 if (read_fast_)
00269 ROS_INFO("Read input file as quickly as possible.");
00270 if (repeat_delay_ > 0.0)
00271 ROS_INFO("Delay %.3f seconds before repeating input file.",
00272 repeat_delay_);
00273
00274
00275 ROS_INFO("Opening PCAP file \"%s\"", filename_.c_str());
00276 if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_) ) == NULL)
00277 {
00278 ROS_FATAL("Error opening Velodyne socket dump file.");
00279 return;
00280 }
00281
00282 std::stringstream filter;
00283 if( devip_str_ != "" )
00284 {
00285 filter << "src host " << devip_str_ << " && ";
00286 }
00287 filter << "udp dst port " << port;
00288 pcap_compile(pcap_, &pcap_packet_filter_,
00289 filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN);
00290 }
00291
00293 InputPCAP::~InputPCAP(void)
00294 {
00295 pcap_close(pcap_);
00296 }
00297
00299 int InputPCAP::getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset)
00300 {
00301 struct pcap_pkthdr *header;
00302 const u_char *pkt_data;
00303
00304 while (true)
00305 {
00306 int res;
00307 if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0)
00308 {
00309
00310
00311 if (0 == pcap_offline_filter(&pcap_packet_filter_,
00312 header, pkt_data))
00313 continue;
00314
00315
00316 if (read_fast_ == false)
00317 packet_rate_.sleep();
00318
00319 memcpy(&pkt->data[0], pkt_data+42, packet_size);
00320 pkt->stamp = ros::Time::now();
00321 empty_ = false;
00322 return 0;
00323 }
00324
00325 if (empty_)
00326 {
00327 ROS_WARN("Error %d reading Velodyne packet: %s",
00328 res, pcap_geterr(pcap_));
00329 return -1;
00330 }
00331
00332 if (read_once_)
00333 {
00334 ROS_INFO("end of file reached -- done reading.");
00335 return -1;
00336 }
00337
00338 if (repeat_delay_ > 0.0)
00339 {
00340 ROS_INFO("end of file reached -- delaying %.3f seconds.",
00341 repeat_delay_);
00342 usleep(rint(repeat_delay_ * 1000000.0));
00343 }
00344
00345 ROS_DEBUG("replaying Velodyne dump file");
00346
00347
00348
00349
00350 pcap_close(pcap_);
00351 pcap_ = pcap_open_offline(filename_.c_str(), errbuf_);
00352 empty_ = true;
00353 }
00354 }
00355
00356 }