input.cc
Go to the documentation of this file.
1 // Copyright (C) 2007, 2009, 2010, 2015 Austin Robot Technology, Patrick Beeson, Jack O'Quin
2 // All rights reserved.
3 //
4 // Software License Agreement (BSD License 2.0)
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions
8 // are met:
9 //
10 // * Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 // * Redistributions in binary form must reproduce the above
13 // copyright notice, this list of conditions and the following
14 // disclaimer in the documentation and/or other materials provided
15 // with the distribution.
16 // * Neither the name of {copyright_holder} nor the names of its
17 // contributors may be used to endorse or promote products derived
18 // from this software without specific prior written permission.
19 //
20 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 // COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 // INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 // BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 // LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 // POSSIBILITY OF SUCH DAMAGE.
32 
47 #include <unistd.h>
48 #include <string>
49 #include <sstream>
50 #include <sys/socket.h>
51 #include <arpa/inet.h>
52 #include <poll.h>
53 #include <errno.h>
54 #include <fcntl.h>
55 #include <sys/file.h>
56 #include <velodyne_driver/input.h>
58 
59 namespace velodyne_driver
60 {
61  static const size_t packet_size =
62  sizeof(velodyne_msgs::VelodynePacket().data);
63 
65  // Input base class implementation
67 
73  Input::Input(ros::NodeHandle private_nh, uint16_t port):
74  private_nh_(private_nh),
75  port_(port)
76  {
77  private_nh.param("device_ip", devip_str_, std::string(""));
78  private_nh.param("gps_time", gps_time_, false);
79  if (!devip_str_.empty())
80  ROS_INFO_STREAM("Only accepting packets from IP address: "
81  << devip_str_);
82  }
83 
85  // InputSocket class implementation
87 
93  InputSocket::InputSocket(ros::NodeHandle private_nh, uint16_t port):
94  Input(private_nh, port)
95  {
96  sockfd_ = -1;
97 
98  if (!devip_str_.empty()) {
99  inet_aton(devip_str_.c_str(),&devip_);
100  }
101 
102  // connect to Velodyne UDP port
103  ROS_INFO_STREAM("Opening UDP socket: port " << port);
104  sockfd_ = socket(PF_INET, SOCK_DGRAM, 0);
105  if (sockfd_ == -1)
106  {
107  perror("socket"); // TODO: ROS_ERROR errno
108  return;
109  }
110 
111  sockaddr_in my_addr; // my address information
112  memset(&my_addr, 0, sizeof(my_addr)); // initialize to zeros
113  my_addr.sin_family = AF_INET; // host byte order
114  my_addr.sin_port = htons(port); // port in network byte order
115  my_addr.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP
116 
117  if (bind(sockfd_, (sockaddr *)&my_addr, sizeof(sockaddr)) == -1)
118  {
119  perror("bind"); // TODO: ROS_ERROR errno
120  return;
121  }
122 
123  if (fcntl(sockfd_,F_SETFL, O_NONBLOCK|FASYNC) < 0)
124  {
125  perror("non-block");
126  return;
127  }
128 
129  ROS_DEBUG("Velodyne socket fd is %d\n", sockfd_);
130  }
131 
134  {
135  (void) close(sockfd_);
136  }
137 
139  int InputSocket::getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset)
140  {
141  double time1 = ros::Time::now().toSec();
142 
143  struct pollfd fds[1];
144  fds[0].fd = sockfd_;
145  fds[0].events = POLLIN;
146  static const int POLL_TIMEOUT = 1000; // one second (in msec)
147 
148  sockaddr_in sender_address;
149  socklen_t sender_address_len = sizeof(sender_address);
150 
151  while (true)
152  {
153  // Unfortunately, the Linux kernel recvfrom() implementation
154  // uses a non-interruptible sleep() when waiting for data,
155  // which would cause this method to hang if the device is not
156  // providing data. We poll() the device first to make sure
157  // the recvfrom() will not block.
158  //
159  // Note, however, that there is a known Linux kernel bug:
160  //
161  // Under Linux, select() may report a socket file descriptor
162  // as "ready for reading", while nevertheless a subsequent
163  // read blocks. This could for example happen when data has
164  // arrived but upon examination has wrong checksum and is
165  // discarded. There may be other circumstances in which a
166  // file descriptor is spuriously reported as ready. Thus it
167  // may be safer to use O_NONBLOCK on sockets that should not
168  // block.
169 
170  // poll() until input available
171  do
172  {
173  int retval = poll(fds, 1, POLL_TIMEOUT);
174  if (retval < 0) // poll() error?
175  {
176  if (errno != EINTR)
177  ROS_ERROR("poll() error: %s", strerror(errno));
178  return -1;
179  }
180  if (retval == 0) // poll() timeout?
181  {
182  ROS_WARN("Velodyne poll() timeout");
183  return -1;
184  }
185  if ((fds[0].revents & POLLERR)
186  || (fds[0].revents & POLLHUP)
187  || (fds[0].revents & POLLNVAL)) // device error?
188  {
189  ROS_ERROR("poll() reports Velodyne error");
190  return -1;
191  }
192  } while ((fds[0].revents & POLLIN) == 0);
193 
194  // Receive packets that should now be available from the
195  // socket using a blocking read.
196  ssize_t nbytes = recvfrom(sockfd_, &pkt->data[0],
197  packet_size, 0,
198  (sockaddr*) &sender_address,
199  &sender_address_len);
200 
201  if (nbytes < 0)
202  {
203  if (errno != EWOULDBLOCK)
204  {
205  perror("recvfail");
206  ROS_INFO("recvfail");
207  return -1;
208  }
209  }
210  else if ((size_t) nbytes == packet_size)
211  {
212  // read successful,
213  // if packet is not from the lidar scanner we selected by IP,
214  // continue otherwise we are done
215  if(devip_str_ != ""
216  && sender_address.sin_addr.s_addr != devip_.s_addr)
217  continue;
218  else
219  break; //done
220  }
221 
222  ROS_DEBUG_STREAM("incomplete Velodyne packet read: "
223  << nbytes << " bytes");
224  }
225 
226  if (!gps_time_) {
227  // Average the times at which we begin and end reading. Use that to
228  // estimate when the scan occurred. Add the time offset.
229  double time2 = ros::Time::now().toSec();
230  pkt->stamp = ros::Time((time2 + time1) / 2.0 + time_offset);
231  } else {
232  // time for each packet is a 4 byte uint located starting at offset 1200 in
233  // the data packet
234  pkt->stamp = rosTimeFromGpsTimestamp(&(pkt->data[1200]));
235  }
236 
237  return 0;
238  }
239 
241  // InputPCAP class implementation
243 
251  InputPCAP::InputPCAP(ros::NodeHandle private_nh, uint16_t port,
252  double packet_rate, std::string filename,
253  bool read_once, bool read_fast, double repeat_delay):
254  Input(private_nh, port),
255  packet_rate_(packet_rate),
256  filename_(filename)
257  {
258  pcap_ = NULL;
259  empty_ = true;
260 
261  // get parameters using private node handle
262  private_nh.param("read_once", read_once_, false);
263  private_nh.param("read_fast", read_fast_, false);
264  private_nh.param("repeat_delay", repeat_delay_, 0.0);
265 
266  if (read_once_)
267  ROS_INFO("Read input file only once.");
268  if (read_fast_)
269  ROS_INFO("Read input file as quickly as possible.");
270  if (repeat_delay_ > 0.0)
271  ROS_INFO("Delay %.3f seconds before repeating input file.",
272  repeat_delay_);
273 
274  // Open the PCAP dump file
275  ROS_INFO("Opening PCAP file \"%s\"", filename_.c_str());
276  if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_) ) == NULL)
277  {
278  ROS_FATAL("Error opening Velodyne socket dump file.");
279  return;
280  }
281 
282  std::stringstream filter;
283  if( devip_str_ != "" ) // using specific IP?
284  {
285  filter << "src host " << devip_str_ << " && ";
286  }
287  filter << "udp dst port " << port;
288  pcap_compile(pcap_, &pcap_packet_filter_,
289  filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN);
290  }
291 
294  {
295  pcap_close(pcap_);
296  }
297 
299  int InputPCAP::getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset)
300  {
301  struct pcap_pkthdr *header;
302  const u_char *pkt_data;
303 
304  while (true)
305  {
306  int res;
307  if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0)
308  {
309  // Skip packets not for the correct port and from the
310  // selected IP address.
311  if (0 == pcap_offline_filter(&pcap_packet_filter_,
312  header, pkt_data))
313  continue;
314 
315  // Keep the reader from blowing through the file.
316  if (read_fast_ == false)
318 
319  memcpy(&pkt->data[0], pkt_data+42, packet_size);
320  pkt->stamp = ros::Time::now(); // time_offset not considered here, as no synchronization required
321  empty_ = false;
322  return 0; // success
323  }
324 
325  if (empty_) // no data in file?
326  {
327  ROS_WARN("Error %d reading Velodyne packet: %s",
328  res, pcap_geterr(pcap_));
329  return -1;
330  }
331 
332  if (read_once_)
333  {
334  ROS_INFO("end of file reached -- done reading.");
335  return -1;
336  }
337 
338  if (repeat_delay_ > 0.0)
339  {
340  ROS_INFO("end of file reached -- delaying %.3f seconds.",
341  repeat_delay_);
342  usleep(rint(repeat_delay_ * 1000000.0));
343  }
344 
345  ROS_DEBUG("replaying Velodyne dump file");
346 
347  // I can't figure out how to rewind the file, because it
348  // starts with some kind of header. So, close the file
349  // and reopen it with pcap.
350  pcap_close(pcap_);
351  pcap_ = pcap_open_offline(filename_.c_str(), errbuf_);
352  empty_ = true; // maybe the file disappeared?
353  } // loop back and try again
354  }
355 
356 } // velodyne namespace
static const size_t packet_size
Definition: input.cc:61
#define ROS_FATAL(...)
std::string devip_str_
Definition: input.h:92
bpf_program pcap_packet_filter_
Definition: input.h:139
InputSocket(ros::NodeHandle private_nh, uint16_t port=DATA_PORT_NUMBER)
constructor
Definition: input.cc:93
Velodyne input base class.
Definition: input.h:72
InputPCAP(ros::NodeHandle private_nh, uint16_t port=DATA_PORT_NUMBER, double packet_rate=0.0, std::string filename="", bool read_once=false, bool read_fast=false, double repeat_delay=0.0)
constructor
Definition: input.cc:251
#define ROS_WARN(...)
char errbuf_[PCAP_ERRBUF_SIZE]
Definition: input.h:140
Input(ros::NodeHandle private_nh, uint16_t port)
constructor
Definition: input.cc:73
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
#define ROS_DEBUG_STREAM(args)
virtual int getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset)
Get one velodyne packet.
Definition: input.cc:299
bool sleep()
#define ROS_INFO_STREAM(args)
virtual int getPacket(velodyne_msgs::VelodynePacket *pkt, const double time_offset)
Get one velodyne packet.
Definition: input.cc:139
static Time now()
ros::Time rosTimeFromGpsTimestamp(const uint8_t *const data)
const std::string header
std::string filename_
Definition: input.h:137
#define ROS_ERROR(...)
virtual ~InputSocket()
destructor
Definition: input.cc:133
ros::Rate packet_rate_
Definition: input.h:136
#define ROS_DEBUG(...)


velodyne_driver
Author(s): Jack O'Quin, Patrick Beeson, Michael Quinlan, Yaxin Liu
autogenerated on Thu Jul 4 2019 19:09:28